vrep小车模型搭建
vrep小车模型搭建
一、任务：智能小车平台建模
（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.2l = 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 = 0lock = 0-- Angle to steer.-- +: left-- -: rightsteeringAngle = -steeringAngleAbs-- It's used to control the total time of avoiding.avoidUntilTime = 0-- Steer Tool Functionsteer = function(angle)-- Conversionangle = angle * math.pi / 180leftAngle = 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 Functionmotor = function(speed)sim.setJointTargetVelocity(FLwheel_Motor, speed)sim.setJointTargetVelocity(FRwheel_Motor, speed)end-- The Main Loopwhile sim.getSimulationState() ~= sim.simulation_advancing_abouttostop doresult = sim.readProximitySensor(Nose_Sensor)if (result > 0) thenavoidUntilTime = sim.getSimulationTime() + 5endif (avoidUntilTime < sim.getSimulationTime()) thenif (lock == 1) thenlock = 0if steeringAngle > 0 thensteeringAngle = steeringAngleAbselsesteeringAngle = -steeringAngleAbsend-- Restore the corresponding tick.tick = math.acos(steeringAngle / steeringAngleAbs) / 0.05endtick = tick + 0.1steeringAngle = steeringAngleAbs * math.cos(tick * 0.05)-- Normal speed.motor(0.5)elseif (lock == 0) thenlock = 1if steeringAngle > 0 then-- When obstacle is in the left side, turn right.steeringAngle = -steeringAngleAbselse-- When obstacle is in the right side, turn left.steeringAngle = steeringAngleAbsend-- Speed up when avoids obstacle.motor(1)endend-- Steer for specific angle.steer(steeringAngle)-- Since this script is threaded, don't waste time here.sim.switchThread()end
end