差速驱动机器人的路径跟踪——示例

此示例演示了如何使用机器人模拟器控制机器人按照所需路径运行。该示例使用Pure Pursuit路径跟随控制器沿着预定路径驱动模拟机器人。所需路径是明确定义或使用路径规划器计算的一组路点（请参阅不同复杂性环境中的路径规划）。建立了模拟差速驱动机器人的Pure Pursuit路径跟随控制器，并计算了跟随给定路径的控制命令。基于Pure Pursuit控制器，计算的控制命令用于沿着期望的轨迹驱动模拟机器人以遵循期望的路径。

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

%% 定义航路点

path = [2.00    1.00;   %定义机器人所需路径的一组路点
        1.25    1.75;
        2.5     6   ;
        2       10  ;
        3       12  ;
        4.5     11  ;
        5.25    8.25;
        7.25    8.75;
        10      12  ;
        11.75   10.75;
        12.00   10.00
        10      8   ;
        12      6   ;
        10      4   ;
        9       2   ;
        12      1   ;
        12.5    2   ];

robotInitialLocation = path(1,:);   %当前位置
robotGoal            = path(end,:); %目标位置
initialOrientation   = 0;           %机器人初始方向

robotCurrentPose=[robotInitialLocation...
                    initialOrientation]'; %当前位姿[x y theta]

%%  创建机器人运动学模型
% 
% 初始化机器人模型并指定初始姿势。 
% 模拟的机器人具有两轮差速驱动机器人的运动学方程。 
% 这个模拟机器人的输入是线速度和角速度。

robot = differentialDriveKinematics("WheelRadius",0.1525, ...
                                    "TrackWidth", 0.5940, ...
                                    "VehicleInputs", "VehicleSpeedHeadingRate");

figure
plot(path(:,1), path(:,2),'k--d')% 可视化路径规划的点
xlim([0 13])                     % 限制x轴范围
ylim([0 13])                     % 限制y轴范围

%% 定义路径跟踪控制器
% 
% 基于上面定义的路径和机器人运动模型，
% 您需要一个路径跟随控制器来沿路径驱动机器人。
% 使用 controllerPurePursuit对象创建路径跟随控制器。
controller = controllerPurePursuit;

controller.Waypoints             = path;% 使用上面定义的路径为控制器设置所需的路点
controller.DesiredLinearVelocity = 0.4; % 线速度设置为0.6米/秒。
controller.MaxAngularVelocity    = 2;   % 最大角速度设置为2弧度/秒。

% 对于平滑路径，前视距离应大于所需的线速度。当前方距离较大时，机器人可能会抄近路。
% 相反，很小的向前看距离会导致不稳定的路径跟随行为。本例选择的值为0.3 m。
controller.LookaheadDistance     = 0.3;

%% 使用路径跟踪控制器，驱动机器人越过所需的航路点
% 路径跟踪控制器为机器人提供输入控制信号，机器人使用这些信号驱动自己沿着所需的路径前进。
% 
% 定义目标半径，即机器人最终位置和目标位置之间所需的距离阈值。 
% 一旦机器人到达目标的这个距离内，它就会停下来。 
% 此外，还可以计算机器人位置和目标位置之间的当前距离。
% 根据目标半径连续检查该距离，当该距离小于目标半径时，机器人停止。
% 
% 请注意，目标半径值太小可能导致机器人错过目标，这可能导致目标附近出现意外行为。
goalRadius     = 0.1;                                    %距离目标的最小半径
distanceToGoal = norm(robotInitialLocation - robotGoal); %计算点前点到目标点的欧式距离

% controllerPurePursuit对象为机器人计算控制命令。 使用这些控制命令驱动机器人，
% 直到它到达目标半径内。 如果您使用的是外部模拟器或物理机器人，
% 则应将控制器输出应用于机器人，并且可能需要定位系统来更新机器人的姿势。
% 控制器以10HZ的频率运行。
% 初始化模拟循环
sampleTime = 0.1;
vizRate = rateControl(1/sampleTime);


fps = 25;% 设置帧数
myVideo = VideoWriter('test2','MPEG-4'); % 创建一个AVI视频文件
myVideo.Quality = 95;
myVideo.FrameRate = fps; % 设置帧数
open(myVideo); % 打开文件

figure% 初始化图形

frameSize = robot.TrackWidth/0.8;% 确定车架尺寸，以便用plotTransforms最接近地表示车辆


while( distanceToGoal > goalRadius )
    
    % 计算控制器输出，即机器人的输入
    [v, omega] = controller(robotCurrentPose);%给控制器当前位置，得到线速度和角速度
    
    % 使用控制器输出获取机器人的速度
    vel = derivative(robot, robotCurrentPose, [v omega]);
    
    % 重新计算到目标的距离
    robotCurrentPose = robotCurrentPose + vel*sampleTime; 
    
    % 重新计算到目标的距离
    distanceToGoal = norm(robotCurrentPose(1:2) - robotGoal(:));
    
    % 更新绘图
    hold off
    
    
    % 绘制每个实例的路径，使其在机器人网格移动时保持持久
    plot(path(:,1), path(:,2),"k--d")
    hold all
    
    % 将机器人的路径绘制成一组变换
    plotTrVec = [robotCurrentPose(1:2); 0];
    plotRot = axang2quat([0 0 1 robotCurrentPose(3)]);% 将轴角度旋转转换为四元数
    plotTransforms(plotTrVec', plotRot, "MeshFilePath", "groundvehicle.stl", "Parent", gca, "View","2D", "FrameSize",frameSize);
    light;%灯光
    xlim([0 13])
    ylim([0 13])

    frame = getframe(gcf);  % 获取figure窗口
    im = frame2im(frame);   % 转为彩色图像
    writeVideo(myVideo,im); % frame可以是灰度图或彩色图
    
    waitfor(vizRate);%阻止执行并等待条件
end

close(myVideo); % 关闭文件