此示例演示如何使用机器人模拟器控制机器人遵循所需路径。该示例使用纯粹的追踪路径跟随控制器沿预定路径驱动模拟机器人。期望的路径是一组明确定义的航点,使用路径规划器(参见)不同复杂性环境下的路径规划)。创建用于模拟差分驱动机器人的控制器后面的纯追踪路径,并计算控制命令以遵循给定路径。计算的控制命令用于沿着所需的轨迹驱动模拟机器人以遵循基于纯追踪控制器的所需路径。
注意:从R2016B开始,而不是使用step方法执行由System Object™定义的操作,您可以使用参数调用对象,就像它是一个函数一样。例如,y =步骤(obj,x)
和y = obj(x)
执行等效操作。
为机器人定义一组所需路径的路点
路径= [2.00 1.00;1.25 - 1.75;5.25 - 8.25;7.25 - 8.75;11.75 - 10.75;12.00 - 10.00);
根据路径的定义设置机器人的当前位置和目标位置。
: robotInitialLocation =路径(1);robotGoal =路径(结束:);
假设初始机器人取向(机器人取向是机器人标题和正X轴之间的角度,而是逆时针测量)。
initialOnientation = 0;
定义机器人的当前姿势[x y]
robotCurrentPose = [robotInitialLocation initialOrientation]';
初始化机器人模型并分配初始姿势。模拟机器人具有用于两轮差动驱动机器人的运动的运动方程。该模拟机器人的输入是线性和角速度。
机器人= differentialDriveKinematics (“trackwidth”,1,“VehicleInputs”那“车辆淘汰头标题”);
可视化所需的路径
图绘制(路径(:1),路径(:,2),“k - d ')XLIM([013])ylim([0 13])
基于上面定义的路径和机器人运动模型,您需要一个路径,然后控制器沿路径驱动机器人。使用使用的控制器创建路径controllerPurePursuit
对象。
控制器= controllerPurePursuit;
使用上面定义的路径为控制器设置所需的路径点
Controller.WayPoints = Path;
设置控制器参数后的路径。对于该示例,所需的线速设定为0.6米/秒。
控制器。DesiredLinearVelocity = 0.6;
最大角速度作为旋转速度的饱和限制,在本例中设置为2弧度/秒。
Controller.MaxangularVelocity = 2;
一般来说,对于光滑路径,前视距离应该大于期望的线速度。当前方距离较大时,机器人可能会抄近路。相反,小的前视距离会导致不稳定的路径跟随行为。本例选择0.3 m的值。
控制器。LookaheadDistance = 0.3;
路径跟踪控制器为机器人提供输入控制信号,机器人使用该信号沿着所需的路径驱动自己。
定义目标半径,即机器人最终位置与目标位置之间的期望距离阈值。一旦机器人到达目标距离内,它就会停止。同时,计算机器人位置和目标位置之间的当前距离。这个距离与目标半径进行连续检查,当这个距离小于目标半径时机器人停止。
注意,目标半径值过小可能会导致机器人错过目标,这可能会导致在目标附近出现意外行为。
goalRadius = 0.1;distancetogoal = norm(RobotInitialLocation - RobotGoal);
这controllerPurePursuit
对象为机器人计算控制命令。使用这些控制命令驱动机器人,直到它到达目标半径内。如果您正在使用外部模拟器或物理机器人,则控制器输出应应用于机器人,并可能需要一个定位系统来更新机器人的姿态。控制器以10 Hz运行。
初始化模拟循环sampletime = 0.1;Vizrate = RATECONTROL(1 / SAMPLETIME);%初始化图数字%确定车辆框架尺寸,以最密切地用plottransforms代表车辆框架= robot.trackwidth / 0.8;而(distancetogoal> Poalradius)%计算控制器输出,即对机器人的输入[v,omega] =控制器(RobotCurrentpose);%通过控制器输入获得机器人的速度vel =衍生物(机器人,robotcurrentoshes,[V omega]);更新当前姿势RobotCurrentospose = RobotCurrentospose + Vel * Sampletime;重新计算到目标的距离distancetogoal = norm(robotcurrentospose(1:2) - 机器人(:));%更新图抓住从绘制每个实例的路径,以便在机器人网格时它保持持久%的举措绘图(路径(:,1),路径(:,2),“k - d”)举行所有%绘制机器人的路径作为一组变换plotTrVec = [robotCurrentPose (1:2);0);plotRot = axang2quat([0 0 1 robotCurrentPose(3)]);plotRot plotTransforms (plotTrVec ',“meshfilepath”那“roundvehicle.stl”那“父”,GCA,“视图”那“2d”那“框架化”, frameSize);淡定;XLIM([013])YLIM([013])等待(vizrate);结尾
如果通过路径策划器计算所需的路点集,则可以以相同的方式使用以下控制器之后的路径。首先,可视化地图
加载exampleMaps地图= binaryOccupancyMap (simpleMap);图显示(图)
你可以计算小路
使用PRM路径规划算法。看不同复杂性环境下的路径规划有关详细信息。
MapInflated = COPY(MAP);充气(MapInflated,Robot.TrackWidth / 2);PRM = Robotics.prm(MapInflated);prm.numnodes = 100;prm.connectiondistance = 10;
找到开始和结束位置之间的路径。请注意,小路
由于PRM算法的概率性质,将是不同的。
startLocation = [4.0 2.0];endlocation = [24.0 20.0];path = findpath(prm,startlocation,endlocation)
路径=8×24.0000 2.0000 3.1703 2.7616 7.0797 11.2229 11.2229 8.1337 13.4835 14.0707 13.3248 16.8068 18.7834 24.4564 20.6514 24.0000 20.0000
显示膨胀的地图,道路地图和最终路径。
展示(PRM);
您定义了一个路径跟踪控制器,您可以在上面重用该控制器来计算此地图上机器人的控制命令。要重用控制器并重新定义路径点,同时保持其他信息不变,请使用释放
功能。
释放(控制器);Controller.WayPoints = Path;
设置初始位置和路径定义的机器人的目标
: robotInitialLocation =路径(1);robotGoal =路径(结束:);
假设机器人的初始方向
initialOnientation = 0;
定义机器人运动的电流姿势[x y theta]
robotCurrentPose = [robotInitialLocation initialOrientation]';
计算到目标位置的距离
distancetogoal = norm(RobotInitialLocation - RobotGoal);
定义目标半径
goalRadius = 0.1;
在给定地图上使用控制器输出驱动机器人,直到它到达目标。控制器以10 Hz运行。
重置(vizRate);%初始化图数字而(distancetogoal> Poalradius)%计算控制器输出,即对机器人的输入[v,omega] =控制器(RobotCurrentpose);%通过控制器输入获得机器人的速度vel =衍生物(机器人,robotcurrentoshes,[V omega]);更新当前姿势RobotCurrentospose = RobotCurrentospose + Vel * Sampletime;重新计算到目标的距离distancetogoal = norm(robotcurrentospose(1:2) - 机器人(:));%更新图抓住从显示地图);抓住所有绘制每个实例的路径,以便在机器人网格时它保持持久%的举措绘图(路径(:,1),路径(:,2),“k - d”)%绘制机器人的路径作为一组变换plotTrVec = [robotCurrentPose (1:2);0);plotRot = axang2quat([0 0 1 robotCurrentPose(3)]);plotRot plotTransforms (plotTrVec ',“MeshFilePath”那“groundvehicle.stl”那“父”,GCA,“视图”那“2d”那“框架化”, frameSize);淡定;xlim([0 27]) ylim([0 26]) waitfor(vizRate);结尾