PRM路径规划算法仿真使用

摘要

小虎想分享一个简单的路径规划算法，因为仿真起来很直观。

结果

完整代码

clc,clear,close all 

robot = differentialDriveKinematics("TrackWidth", 1.5, "VehicleInputs", "VehicleSpeedHeadingRate");

%加载地图
load exampleMaps
map = binaryOccupancyMap(simpleMap);
figure(1)
show(map)
% PRM路径规划算法
mapInflated = copy(map);
inflate(mapInflated, robot.TrackWidth/2);
prm = robotics.PRM(mapInflated);
% way1
prm.NumNodes = 800;
prm.ConnectionDistance = 20;
%  prm.NumNodes = 500;
%  prm.ConnectionDistance = 10;

% prm.NumNodes = 3000;
% prm.ConnectionDistance = 30;
% way1
startLocation = [5 6];
endLocation = [22 20];
% % endLocation = [22 3.5];

path = findpath(prm, startLocation, endLocation);
show(prm);
% 
controller = controllerPurePursuit;
controller.Waypoints = path;%路径
controller.DesiredLinearVelocity = 3;%线速度
controller.MaxAngularVelocity = 2;%角速度
controller.LookaheadDistance = 0.3;

robotInitialLocation = path(1,:);
robotGoal = path(end,:);

initialOrientation = 0;

robotCurrentPose = [robotInitialLocation initialOrientation]';

distanceToGoal = norm(robotInitialLocation - robotGoal);

goalRadius = 0.1;

sampleTime = 0.1;
vizRate = rateControl(1/sampleTime);
frameSize = robot.TrackWidth/0.8;

reset(vizRate);

% Initialize the figure
figure(2)

while( distanceToGoal > goalRadius )
    
    % Compute the controller outputs, i.e., the inputs to the robot
    [v, omega] = controller(robotCurrentPose);
    
    % 速度计算
    vel = derivative(robot, robotCurrentPose, [v omega]);
    
    %更新位置
    robotCurrentPose = robotCurrentPose + vel*sampleTime; 
    
    % 目标距离
    distanceToGoal = norm(robotCurrentPose(1:2) - robotGoal(:));
    
    % Update the plot
    hold off
    show(map);
    hold all

    % Plot path each instance so that it stays persistent while robot mesh
    % moves
    plot(path(:,1), path(:,2),"k--d")
    
    % Plot the path of the robot as a set of transforms
    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 27])
%     ylim([0 26])
    
    waitfor(vizRate);
end