路径跟随控制器与PRM算法——示例

首先，查看地图

所需的路点集合由路径规划器计算，则路径跟随控制器可以以相同的方式使用。显示膨胀的地图、道路地图和最终路径。

 

路径跟随控制器与PRM算法——示例

%% 将路径跟踪控制器与PRM一起使用

% 如果路径规划器计算出所需的路点集合，则可以以相同的方式使用路径跟随控制器
%% 首先，将地图可视化
load exampleMaps
map = binaryOccupancyMap(complexMap);
figure
show(map)

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

robot = differentialDriveKinematics("WheelRadius",0.1525, ...
                                    "TrackWidth", 0.5940, ...
                                    "VehicleInputs", "VehicleSpeedHeadingRate");
%% 使用PRM路径规划算法计算路径
mapInflated = copy(map);                  %复制副本
inflate(mapInflated, robot.TrackWidth/2); %膨胀地图
show(mapInflated)                         %显示膨胀后的地图

prm = robotics.PRM(mapInflated);%输入副本地图
prm.NumNodes = 1000;            %算法需要节点
prm.ConnectionDistance = 100;   %算法连接距离

% 查找起始位置和结束位置之间的路径
%请注意，由于PRM算法的概率性质，路径将不同
startLocation=[4.0 2.0];                  %起始位置
endLocation=[46 35];                      %结束位置
path=findpath(prm,startLocation,endLocation) %使用prm算法的路径

show(prm) % 显示展开的地图，路线图和最终路径

%% 定义路径跟踪控制器

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

controller.DesiredLinearVelocity = 0.4; % 线速度设置为0.6米/秒。
controller.MaxAngularVelocity    = 2;   % 最大角速度设置为2弧度/秒。

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

sampleTime = 0.1;                   %采样时间
vizRate = rateControl(1/sampleTime);%以固定频率执行循环

% 在此控制器之上，你可以重复使用该控制器来计算此
% 地图上机器人的控制命令，要重复使用控制器并重新定义路点，同时保持其他信息不变，
% 请使用release函数
release(controller);
controller.Waypoints=path;% 使用上面定义的路径为控制器设置所需的路点

%按照路径定义设置机器人的初始位置和目标位置
robotIntitialLocation=path(1,:);   %设置机器人的初始位置
robotGoal            =path(end,:); %设置机器人的目标位置

initialOrientation=0;              %设置机器人的初始方位

%定义机器人运动的当前姿势[x y theta]
robotCurrentPose=[robotIntitialLocation initialOrientation]';

distanceToGoal=norm(robotIntitialLocation-robotGoal); %计算距离

goalRadius =0.1;                %定义目标半径

% 使用给定地图上的控制器输出来驱动机器人，直到它到达目标。
% 控制器10赫兹的频率运行
reset(vizRate);
frameSize = robot.TrackWidth*4; %确定车架尺寸，以便用plotTransforms最接近地表示车辆

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

figure                          %初始化图形

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
    show(map);
    hold all

    % 绘制每个实例的路径，使其在机器人网格移动时保持持久
    plot(path(:,1),path(:,2),"k--d")

    % 将机器人的路径绘制成一组变换
    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; %灯光
   
    frame = getframe(gcf);% 获取figure窗口
    im = frame2im(frame); % 转为彩色图像
    writeVideo(myVideo,im); % frame可以是灰度图或彩色图

    waitfor(vizRate);%阻止执行并等待条件
end
close(myVideo); % 关闭文件