该例程演示如何使用机器人运动模型控制机器人跟踪目标路径。该例程使用“Pure Persuit”路径跟踪控制器驱动仿真机器人沿着预先决定的路径运动。要求的路径时一系列的陆标，该陆标是明确定义的或者由路径规划器计算的得到。创建仿真差动机器人的“Pure Persuit”路径规划器，计算控制指令以跟踪给定路径。计算的控制指令用于驱动仿真机器人沿着要求的轨迹基于“Pure Persuit”控制器。

定义陆标

为机器人定义一系列要求路径的路标点。

path = [2.00 1.00;

1.25 1.75;

5.25 8.25;

7.25 8.75;

11.75 10.75;

12.00 10.00];

可视化要求的路径。

plot(path(:,1), path(:,2),'k--d')

xlim([0 13])

ylim([0 13])

定义机器人运动模型

当没有外部仿真器或者实际机器人可用的情况下，就需要机器人运动模型。在本例中使用的仿真运动模型，根据给定的输入，更新和返回机器人的位姿。外部仿真或真实机器人将需要定位机制以提供机器人更新的位姿。

设置当前的位置和机器人的目标位置，目标位置由路径定义。

robotCurrentLocation = path(1,:);

robotGoal = path(end,:);

假设机器人的初始方向(机器人的初始方向是机器人的航向和X轴正方向的夹角，以逆时针的方式测量)。

initialOrientation = 0;

定义机器人的当前位姿[x,y,theta]：

robotCurrentPose = [robotCurrentLocation initialOrientation];

初始化仿真机器人对象的当前位姿，仿真机器人具有两轮机器人的运动学方程。该仿真机器人对象能够基于这些运动方程和线性、角度速度输入。具备绘图的能力显示机器人的当前位置和绘制机器人的轨迹。

robot = ExampleHelperDifferentialDriveRobot(robotCurrentPose);

定义路径跟踪控制器

根据上文定义的路径和机器人运动模型，用户需要一个路径跟踪控制器以驱动机器人沿着路径运动。使用“robotics.PurePursuit”对象创建路径跟踪控制器。

controller = robotics.PurePursuit

controller =

System: robotics.PurePursuit

Properties:

Waypoints: []

MaxAngularVelocity: 1

LookaheadDistance: 1

DesiredLinearVelocity: 0.1

使用上面定义的路径为控制器设置要求的路标。

controller.Waypoints = path;

设置路径跟踪控制器的参数，本例中要求的线性速度设置为0.3m/s。

controller.DesiredLinearVelocity = 0.3;

最大角速度作为旋转速度的饱和限制，本例中设置为2rad/s。

controller.MaxAngularVelocity = 2;

作为一般规则，为了平滑的路径，向前的距离应该比要求的线性速度大。当向前距离大时，机器人可能抄近路。相反的，向前距离小可能时可能导致不稳定的路径跟踪表现。本例选择0.5m。

controller.LookaheadDistance = 0.5;

使用路径跟踪控制器，在需要的路标驱动机器人

路径跟踪控制器为机器人提供输入控制信号，机器人使用该信号驱动自己沿着要求的路径。

定义目标半径，作为机器人最终位置和要求距离阈值，一旦机器人与目标位置的距离小于该值，则停止。此外，计算机器人的当前位置和目标位置的距离，持续的比较该值与目标半径，当该距离小于目标半径的时候，机器人将会停止。注意，如果目标半径太小可能导致机器人错过目标，这可能会导致目标附近预料之外的表现。

goalRadius = 0.1;

distanceToGoal = norm(robotCurrentLocation - robotGoal);

“robotics.PurePursuit.step”函数计算控制命令给机器人，使用这些指令驱动机器人直到抵达目标半径之内。如果用户使用一个外部的仿真器或者真实机器人，那么控制器的输出需要应用到机器人，而且需要一个定位系统更新机器人的位姿。

while( distanceToGoal > goalRadius )

% Compute the controller outputs, i.e., the inputs to the robot

[v, omega] = step(controller, robot.CurrentPose);

% Simulate the robot using the controller outputs.

drive(robot, v, omega)

% Extract current location information ([X,Y]) from the current pose of the

% robot

robotCurrentLocation = robot.CurrentPose(1:2);

% Re-compute the distance to the goal

distanceToGoal = norm(robotCurrentLocation - robotGoal);

end

仿真机器人使用路径跟踪控制器沿着要求的路径已经到达目标位置。如果目标路标集由路径规划器计算，路径跟踪控制器应用的样式相同。导入一个简单的地图用于计算路径。

filePath = fullfile(fileparts(which('PathFollowingControllerExample')), 'data', 'exampleMaps.mat');

load(filePath)

map = robotics.BinaryOccupancyGrid(simpleMap, 2)

map =

BinaryOccupancyGrid with properties:

GridSize: [26 27]

Resolution: 2

XWorldLimits: [0 13.5000]

YWorldLimits: [0 13]

GridLocationInWorld: [0 0]

显示地图：

show(map)

用户可以使用PRM路径规划算法计算路径。

robotRadius = 0.2;

mapInflated = copy(map);

inflate(mapInflated,robotRadius);

prm = robotics.PRM(mapInflated);

prm.NumNodes = 100;

prm.ConnectionDistance = 10;

寻找开始位置和结束位置的路径，注意，由于PRM算法的随机性，路径将会有所不同。

startLocation = [2.0 1.0];

endLocation = [12.0 10.0];

path = findpath(prm, startLocation, endLocation)

path =

2.0000 1.0000

1.5851 1.3808

4.0668 6.7417

7.0353 8.6624

12.2282 10.3257

12.0000 10.0000

显示路径：

hold on

show(prm, 'Map', 'off', 'Roadmap', 'off');

hold off

在上文定义了一个路径跟踪控制器，用户可以在本地图重新使用以计算机器人的控制指令。为了重新使用控制器和重新定义路标点，同时保持其它信息不变，使用“robotics.PurePursuit.release”函数。

release(controller)

controller.Waypoints = path;

设置当前的位置和机器人的目标位置，正如路径所定义的。

robotCurrentLocation = path(1,:);

robotGoal = path(end,:);

假设机器人的初始方向：

initialOrientation = 0;

定义机器人运动的当前位姿[x,y,theta]

robotCurrentPose = [robotCurrentLocation initialOrientation];

使用当前位姿，重新创建仿真机器人对象。

robot = ExampleHelperDifferentialDriveRobot(robotCurrentPose);

计算与目标位置的距离：

distanceToGoal = norm(robotCurrentLocation - robotGoal);

定义目标半径：

goalRadius = 0.1;

在给定的地图上，使用控制器的输出驱动机器人，直到抵达目标位置。

while( distanceToGoal > goalRadius )

% Compute the controller outputs, i.e., the inputs to the robot

[v, omega] = step(controller, robot.CurrentPose);

% Simulate the robot using the controller outputs

drive(robot, v, omega)

% Extract current location information from the current pose

robotCurrentLocation = robot.CurrentPose(1:2);

% Re-compute the distance to the goal

distanceToGoal = norm(robotCurrentLocation - robotGoal);

end