差动驱动机器人的路径跟踪

打开生活的脚本

这个例子演示了如何使用一个机器人模拟器来控制一个机器人沿着预定的路径前进。该示例使用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);%初始化图数字用plotTransforms确定最接近代表车辆的车辆框架大小。frameSize = robot.TrackWidth / 0.8;而(distanceToGoal)计算控制器输出，即机器人的输入[v, omega] = controller(robotCurrentPose);使用控制器输入获取机器人的速度。vel =导数(robot, robotCurrentPose， [v omega]);更新当前姿势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)计算控制器输出，即机器人的输入[v, omega] = controller(robotCurrentPose);使用控制器输入获取机器人的速度。vel =导数(robot, robotCurrentPose， [v omega]);更新当前姿势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);结束