vrep小车模型搭建-CSDN博客

本文链接：https://blog.csdn.net/a1233219/article/details/125753338

一、任务：智能小车平台建模
（1）参考⽂档中Tutorial - BubbleRob tutorial 部分，学习:
a、⻋⾝与轮⼦物理模型的设计
b、动⼒学模型等参数的设置
c、传感器的添加与使⽤
d、驱动关节的设计与控制
e 、控制脚本的编写

（2）实现要求:
a、搭建⼀个四轮⼩⻋，⼩⻋搭载⼀个单⽬彩⾊摄像头
b、尺⼨(如⻓宽⾼)及底盘参数(如速度限制)可参考DJI RoboMaster S1进⾏设计
https://www.dji.com/cn/robomaster-s1/specs
c、不采⽤⻨克纳姆轮
d、编写简单脚本使得⼩⻋能呈S型路线⾏⾛，当碰到障碍物后能够绕⾏

二、设计内容

1. 总体设计

机器人由长方体车身和四个轮子、传感器、彩色摄像头组成，两个前轮主要负责转向和
动力输出，两个后轮作为从动轮，传感器负责检测前方障碍物并绕行。
以下是 MyBot 机器人的总体设计层次图。其中 FLwheel_Steering 和 FRwheel_Steering 为
前轮； RLwheel_Motor 和 RRwheel_Motor 为后轮； Nose_Sensor 为传感器，搭载单目彩色摄
像头； AutoFittingCamera 为自适应摄像头，用于拍摄机器人运动时的姿态。

1 机器人总体设计

2. 车身构建

我们使用 Cuboid 作为车身主体，为避免主体与其他部件冲突，我们需要设置其 Object
属性，关闭 Collidable 等属性。

2 车身 Object 属性

同时，我们需要关闭 Local Respondable Mask 属性。

3 车身 Object 属性

3. 车轮构建

我们将机器人的车轮划分为两组：前轮组（Front）和后轮组（Rear）。前轮组负责转向
和动力，后轮组则为从动轮。
由于我们希望前轮组既负责转向功能，又负责动力输出。为实现这两个功能，我们需要
两个转动关节，其中一转动关节竖向放置，用于控制转向角度，另一转动关节横向放置，用
于输出动力。

4 转向轮示意图

以下是 MyBot 的各车轮的层次图。注意到，以前左轮（FLwheel）为例，其由竖向关节
（Steering）和横向关机（Motor）构成；与之不同的是，后左轮（RLwheel）则由横向关节
（Motor）构成。

以下是机器人的车轮拆解示意图（已设置各部件的可视状况）。拥有两个转动关节的即
为前轮组，仅拥有一个转动关节的为后轮组。

4. 直线运动

为实现直线运动，我们需要编写脚本。而在这里，我们使用 Threaded Child Script 进行
脚本的填写，其涉及到一个很重要的函数，名为 sysCall_threadmain，这是一个需要用户填
充自定义代码的函数模板，模板如下。

        在 sysCall_threadmain 函数模板中，我们需要将变量的初始化放置在主循环之外，机器
人的运动逻辑放置在主循环之中，这是需要注意的地方。
        现在，我们进行直线运动的脚本编写。我们知道，在前面的车轮构建部分中，我们添加
了两个前进电机。所以，我们可通过 setJointTargetVelocity 设置这两个前进电机同样的运行
速度，实现机器人的直线运动。
        为此，我们实现了 motor 工具函数，该函数用于两个前进电机的动力设置，其相关代码
如下。

在上述脚本中，我们通过 getObjectHandle 获取两个前进电机的实例。在 motor 工具函
数中，我们使用 setJointTargetVelocity 设置两个电机同样的运行速度。由于我们所添加的脚
本是 Threaded 类型，我们需要在 while 主循环内，执行了 motor(0.8) 命令。按下模拟按钮
后，我们即可观察到机器人进行直线运动。
下面是直线运动的全部脚本代码，后续由于篇幅限制，只贴出关键部分的代码。

5. S 型运动

在实现机器人的直线运动后，我们需要在其基础上，使得机器人能够以 S 型路线前进。
我们的思想很简单：在电机启动的情况下，控制转向角度，使其按照余弦函数变化，即可控
制机器人的前进路线为 S 型路线。
我们使用的是 Ackermann 转向运动学模型。根据该运动学模型，车辆在转弯时，其两个
转向轮的转向角度是不一致的，但其角度可由数理公式推导而出。为此，我们实现了 steer
转向工具函数，实现两个转向轮能够按照模型进行不同角度的转向。

在 steer 工具函数中，我们为其传入 d 和 l 参数，它们分别与左右轮间距和前后轮间距
有关，是转向运动学模型所必需的参数。我们通过 getObjectHandle 获取两个转向关节的实
例，通过 setJointTargetPosition 控制两个转向关节的角度，进而控制机器人的前进方向。

6. 传感器

我们在 Body 添加 Proximity Sensor，用于搭载单目彩色摄像头，用于避障算法的障碍物
检测。
下面是 Nose Sensor 的 Volume 的各项属性，我们将 Volume Type 设置为 Disc，并配置
其的角度和半径等参数。

在设置好 Nose Sensor 的属性后，我们为其添加单目彩色摄像头，并设置好摄像头属性，
最后添加 Floating View 用于显示摄像头的视野。

7. 简单避障

我们使用这样的避障算法：当机器人在往左边运动的过程中，探测到了障碍物，则将转
向角度设置为往右边运动的最大角度，该操作持续一段时间，直至无探测到障碍物或达到阈值时间，随后设置转向角度为往左边运动的最大角度，继续原有的运动。
我们使用 Nose_Sensor 作为障碍物的探测感应器，通过获取其探测结果，来决定是否采
取避障措施。

若探测到障碍物，设置避障操作的阈值时间，同时我们根据当前的运动方向，设置避障
所需要的最大转动角度和方向。

在超出阈值时间后，即机器人已远离障碍物后，我们重新设置机器人的转动方向为原有
的运动方向，且转动角度设置为最大转动角度。

若机器人在前进过程中，未探测到障碍物，则其转动角度一直按余弦函数变化。

三、代码

function sysCall_threadmain()
    -- Initialization

    -- Get FL Steering Motor.
    FLwheel_Steering = sim.getObjectHandle('MyBot_FLwheel_Steering')
    -- Get FR Steering Motor.
    FRwheel_Steering = sim.getObjectHandle('MyBot_FRwheel_Steering')
    -- Get FL Motor.
    FLwheel_Motor = sim.getObjectHandle('MyBot_FLwheel_Motor')
    -- Get FR Motor.
    FRwheel_Motor = sim.getObjectHandle('MyBot_FRwheel_Motor')
    -- Get Sensing Nose.
    Nose_Sensor = sim.getObjectHandle('MyBot_Nose_Sensor')
    -- d = 0.2 * distance (cm) between left and right wheels.
    -- l = 0.2 * distance (cm) between front and rear wheels.
    d = 28 * 0.2
    l = 30 * 0.2
    -- It represents the abs value of the max steering angle.
    steeringAngleAbs = 30
    -- It's used to generate cos-like angle sequence.
    tick = 0
    lock = 0
    -- Angle to steer.
    -- +: left
    -- -: right
    steeringAngle = -steeringAngleAbs
    -- It's used to control the total time of avoiding.
    avoidUntilTime = 0

    -- Steer Tool Function
    steer = function(angle)
        -- Conversion
        angle = angle * math.pi / 180
        leftAngle = math.atan(l / (-d + l / math.tan(angle)))
        rightAngle = math.atan(l / (d + l / math.tan(angle)))
        sim.setJointTargetPosition(FLwheel_Steering, leftAngle)
        sim.setJointTargetPosition(FRwheel_Steering, rightAngle)
    end

    -- Motor Tool Function
    motor = function(speed)
        sim.setJointTargetVelocity(FLwheel_Motor, speed)
        sim.setJointTargetVelocity(FRwheel_Motor, speed)
    end

    -- The Main Loop
    while sim.getSimulationState() ~= sim.simulation_advancing_abouttostop do
        result = sim.readProximitySensor(Nose_Sensor)

        if (result > 0) then
            avoidUntilTime = sim.getSimulationTime() + 5
        end

        if (avoidUntilTime < sim.getSimulationTime()) then
            if (lock == 1) then
                lock = 0
                if steeringAngle > 0 then
                    steeringAngle = steeringAngleAbs
                else
                    steeringAngle = -steeringAngleAbs
                end
                -- Restore the corresponding tick.
                tick = math.acos(steeringAngle / steeringAngleAbs) / 0.05
            end
            tick = tick + 0.1
            steeringAngle = steeringAngleAbs * math.cos(tick * 0.05)
            -- Normal speed.
            motor(0.5)
        else
            if (lock == 0) then
                lock = 1
                if steeringAngle > 0 then
                    -- When obstacle is in the left side, turn right.
                    steeringAngle = -steeringAngleAbs
                else
                    -- When obstacle is in the right side, turn left.
                    steeringAngle = steeringAngleAbs
                end
                -- Speed up when avoids obstacle.
                motor(1)
            end
        end
        
        -- Steer for specific angle.
        steer(steeringAngle)

        -- Since this script is threaded, don't waste time here.
        sim.switchThread()
    end
end