差动驱动机器人的路径跟踪
这个例子演示了如何使用机器人模拟器来控制机器人沿着期望的路径前进。该示例使用Pure Pursuit路径跟踪控制器驱动模拟机器人沿预定路径前进。所需的路径是一组明确定义或使用路径规划器计算的路径点(参见不同复杂性环境下的路径规划）.建立了仿真差动驱动机器人的Pure Pursuit路径跟踪控制器，并计算出了跟踪给定路径的控制指令。基于Pure Pursuit控制器，利用计算得到的控制命令驱动仿真机器人沿期望的轨迹跟随期望的路径。
注意:从R2016b开始，不再使用step方法执行System对象™定义的操作，而是可以使用参数调用对象，就像它是一个函数一样。例如,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轴之间的角度，逆时针测量)。
initialOrientation = 0;
定义机器人的当前姿势[x y]
robotCurrentPose = [robotInitialLocation initialOrientation]';
创建一个运动学机器人模型
初始化机器人模型并分配初始姿态。该仿真机器人具有两轮差动驱动机器人的运动方程。这个模拟机器人的输入是线速度和角速度。
机器人= differentialDriveKinematics (“TrackWidth”, 1“VehicleInputs”，“VehicleSpeedHeadingRate”)；
想象想要的路径
图绘制(路径(:1),路径(:,2),“k - d ') xlim([0 13])
定义路径跟踪控制器
根据上面定义的路径和机器人的运动模型，需要一个路径跟随控制器来驱动机器人沿路径前进。属性创建路径跟随控制器controllerPurePursuit对象。
控制器= controllerPurePursuit;
使用上面定义的路径为控制器设置所需的路径点
控制器。路点=路径;
按照控制器参数设置路径。在本例中，所需的线速度设置为0.6米/秒。
控制器。DesiredLinearVelocity = 0.6;
最大角速度作为旋转速度的饱和限制，在本例中设置为2弧度/秒。
控制器。MaxAngularVelocity = 2;
一般来说，对于光滑路径，前视距离应该大于期望的线速度。当前方距离较大时，机器人可能会抄近路。相反，小的前视距离会导致不稳定的路径跟随行为。本例选择0.3 m的值。
控制器。LookaheadDistance = 0.3;
使用路径跟踪控制器，驱动机器人越过所需的路径点
路径跟踪控制器为机器人提供输入控制信号，机器人使用该信号沿着所需的路径驱动自己。
定义目标半径，即机器人最终位置与目标位置之间的期望距离阈值。一旦机器人到达目标距离内，它就会停止。同时，计算机器人位置和目标位置之间的当前距离。这个距离与目标半径进行连续检查，当这个距离小于目标半径时机器人停止。
注意，目标半径值过小可能会导致机器人错过目标，这可能会导致在目标附近出现意外行为。
goalRadius = 0.1;distanceToGoal = norm(robotInitialLocation - robotGoal);
的controllerPurePursuit对象为机器人计算控制命令。使用这些控制命令驱动机器人，直到它到达目标半径内。如果您正在使用外部模拟器或物理机器人，则控制器输出应应用于机器人，并可能需要一个定位系统来更新机器人的姿态。控制器运行频率为10hz。
初始化模拟循环sampleTime = 0.1;vizRate = rateControl (1 / sampleTime);初始化数字数字%确定车辆框架大小最接近的车辆与plot变换frameSize = robot.TrackWidth / 0.8;而(distanceToGoal > goalRadius)%计算控制器输出，即机器人的输入[v, omega] =控制器(robotCurrentPose);%通过控制器输入获得机器人的速度vel =导数(机器人，robotCurrentPose， [v ω]);更新当前姿势robotCurrentPose = robotCurrentPose + vel*sampleTime;重新计算到目标的距离distanceToGoal = norm(robotCurrentPose(1:2) - robotGoal(:)));%更新图持有从绘制每个实例的路径，以便在机器人网格时它保持持久%的举措情节(路径(:1),路径(:,2),“k - d”)举行所有%绘制机器人的路径作为一组变换plotTrVec = [robotCurrentPose (1:2);0);plotRot = axang2quat([0 0 1 robotCurrentPose(3)]);plotRot plotTransforms (plotTrVec ',“MeshFilePath”，“groundvehicle.stl”，“父”甘氨胆酸,,“视图”，“二维”，“FrameSize”, frameSize);淡定;xlim([0 13]) ylim([0 13]) waitfor(vizRate);结束
与PRM一起使用路径跟踪控制器
如果路径规划器计算所需的一组路径点，路径跟踪控制器可以以同样的方式使用。首先，可视化地图
负载exampleMaps地图= binaryOccupancyMap (simpleMap);图显示(图)
你可以计算路径使用PRM路径规划算法。看到不同复杂性环境下的路径规划获取详细信息。
mapInflated = (map)复印件;充气(mapInflated, robot.TrackWidth / 2);人口、难民和移民事务局= robotics.PRM (mapInflated);人口、难民和移民事务局。NumNodes = 100;人口、难民和移民事务局。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 8.1337 13.4835 14.0707 17.3248 16.8068 18.7834 24.4564 20.6514 24.0000 20.0000
显示膨胀的地图，道路地图和最终路径。
显示(人口、难民和移民事务局);
您定义了一个路径跟踪控制器，您可以在上面重用该控制器来计算此地图上机器人的控制命令。要重用控制器并重新定义路径点，同时保持其他信息不变，请使用释放函数。
释放(控制器);控制器。路点=路径;
根据路径定义机器人的初始位置和目标
: robotInitialLocation =路径(1);robotGoal =路径(结束:);
假设机器人的初始方向
initialOrientation = 0;
定义机器人运动的当前姿势[x y]
robotCurrentPose = [robotInitialLocation initialOrientation]';
计算到目标位置的距离
distanceToGoal = norm(robotInitialLocation - robotGoal);
定义目标半径
goalRadius = 0.1;
使用给定地图上的控制器输出驱动机器人，直到它到达目标。控制器运行频率为10hz。
重置(vizRate);初始化数字数字而(distanceToGoal > goalRadius)%计算控制器输出，即机器人的输入[v, omega] =控制器(robotCurrentPose);%通过控制器输入获得机器人的速度vel =导数(机器人，robotCurrentPose， [v ω]);更新当前姿势robotCurrentPose = robotCurrentPose + vel*sampleTime;重新计算到目标的距离distanceToGoal = norm(robotCurrentPose(1:2) - robotGoal(:)));%更新图持有从显示(地图);持有所有绘制每个实例的路径，以便在机器人网格时它保持持久%的举措情节(路径(:1),路径(:,2),“k - d”）%绘制机器人的路径作为一组变换plotTrVec = [robotCurrentPose (1:2);0);plotRot = axang2quat([0 0 1 robotCurrentPose(3)]);plotRot plotTransforms (plotTrVec ',“MeshFilePath”，“groundvehicle.stl”，“父”甘氨胆酸,,“视图”，“二维”，“FrameSize”, frameSize);淡定;xlim([0 27]) ylim([0 26]) waitfor(vizRate);结束
另请参阅
你也可以从以下列表中选择一个网站: