关于matlab地图规划代码的一些思路

关于matlab 地图规划代码的一些思路

可以做demo的用x代替

 

occupancyMatrix = readmatrix('navigation map x ');

 

map = binaryOccupancyMap(occupancyMatrix);

 

show(map)

 

 

 

 

robotRadius = x ;

 

mapInflated = copy(map);

inflate(mapInflated,robotRadius);

 

show(mapInflated);

 

prm = mobileRobotPRM;

prm.Map = mapInflated;

prm.NumNodes = x ;

prm.ConnectionDistance = x;

startLocation = [x x];

endLocation = [x x];

 

path=findpath(prm, startLocation, endLocation);

 

while isempty(path)

    % No feasible path found yet, increase the number of nodes

    prm.NumNodes = prm.NumNodes + x;

    

    % Use the |update| function to re-create the PRM roadmap with the changed

    % attribute

    update(prm);

    

    % Search for a feasible path with the updated PRM

    path = findpath(prm, startLocation, endLocation);

end

 

path

 

show(prm)

 

1. 1. The code for the kinetic vehicle

%define the condition

unicycle = unicycleKinematics("VehicleInputs","VehicleSpeedHeadingRate");

diffDrive = differentialDriveKinematics("VehicleInputs","VehicleSpeedHeadingRate");

diffDrive.WheelSpeedRange = [-10 10]*2*pi;

bicycle = bicycleKinematics("VehicleInputs","VehicleSpeedHeadingRate","MaxSteeringAngle",pi/8);

carLike = ackermannKinematics;

 

%set up simulation parameters

waypoints = [path];

% Define the total time and the sample rate

sampleTime = x;               % Sample time [s]

tVec = 0:sampleTime:x;          % Time array

 

initPose = [waypoints(1,:)'; 0]; % Initial pose (x y theta)

 

%create controller

% Define a controller. Each robot requires its own controller

controller1 = controllerPurePursuit("Waypoints",waypoints,"DesiredLinearVelocity",3,"MaxAngularVelocity",3*pi);

controller2 = controllerPurePursuit("Waypoints",waypoints,"DesiredLinearVelocity",3,"MaxAngularVelocity",3*pi);

controller3 = controllerPurePursuit("Waypoints",waypoints,"DesiredLinearVelocity",3,"MaxAngularVelocity",3*pi);

 

goalPoints = waypoints(end,:)';

goalRadius = 1;

 

% Compute trajectories for each kinematic model under motion control

[tUnicycle,unicyclePose] = ode45(@(t,y)derivative(unicycle,y,exampleHelperMobileRobotController(controller1,y,goalPoints,goalRadius)),tVec,initPose);

[tBicycle,bicyclePose] = ode45(@(t,y)derivative(bicycle,y,exampleHelperMobileRobotController(controller2,y,goalPoints,goalRadius)),tVec,initPose);

[tDiffDrive,diffDrivePose] = ode45(@(t,y)derivative(diffDrive,y,exampleHelperMobileRobotController(controller3,y,goalPoints,goalRadius)),tVec,initPose);

 

unicycleTranslations = [unicyclePose(:,1:2) zeros(length(unicyclePose),1)];

unicycleRot = axang2quat([repmat([0 0 1],length(unicyclePose),1) unicyclePose(:,3)]);

 

bicycleTranslations = [bicyclePose(:,1:2) zeros(length(bicyclePose),1)];

bicycleRot = axang2quat([repmat([0 0 1],length(bicyclePose),1) bicyclePose(:,3)]);

 

diffDriveTranslations = [diffDrivePose(:,1:2) zeros(length(diffDrivePose),1)];

diffDriveRot = axang2quat([repmat([0 0 1],length(diffDrivePose),1) diffDrivePose(:,3)]);

 

figure

plot(waypoints(:,1),waypoints(:,2),"kx-","MarkerSize",20);

hold all

plotTransforms(unicycleTranslations(1:10:end,:),unicycleRot(1:10:end,:),'MeshFilePath','groundvehicle.stl',"MeshColor","r");

plotTransforms(bicycleTranslations(1:10:end,:),bicycleRot(1:10:end,:),'MeshFilePath','groundvehicle.stl',"MeshColor","b");

plotTransforms(diffDriveTranslations(1:10:end,:),diffDriveRot(1:10:end,:),'MeshFilePath','groundvehicle.stl',"MeshColor","g");

view(0,90)

 

1. 1. The code for the RRT

ss = stateSpaceSE2;

sv = validatorOccupancyMap(ss);

 

occupancyMatrix = readmatrix('navigation map 2');

 

map = occupancyMap(occupancyMatrix,10);

sv.Map = map;

sv.ValidationDistance = x;

ss.StateBounds = [map.XWorldLimits;map.YWorldLimits; [-pi pi]];

planner = plannerRRT(ss,sv);

planner.MaxConnectionDistance = 0.3;

start = [x x];

goal = [x ,x ,0];

rng(100,'twister'); % for repeatable result

[pthObj,solnInfo] = plan(planner,start,goal);

show(map)

hold