ROS与Matlab语言入门教程-差动机器人的路径跟踪

该例程演示如何使用机器人运动模型控制机器人跟踪目标路径。该例程使用“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