自动泊车算法中混合A*粗路径的MATLAB实现_motionprimitivelength

这里使用Matlab的navigation toolbox实现简单的混合A*粗路径。

Load the cost values of cells in the vehicle costmap of a parking lot.

load parkingLotCostVal.mat
1

Create a binaryOccupancyMap with cost values.

map = binaryOccupancyMap(costVal);
1

Create a stateValidator object for collision checking.

validator = validatorOccupancyMap;
1

Assign the map to the stateValidator object.

validator.Map = map;
1

Initialize the plannerHybridAStar object with the stateValidator object.
Specify the MinTurningRadius and MotionPrimitiveLength properties of the planner.

planner = plannerHybridAStar(validator,'MinTurningRadius'...
    ,8,'MotionPrimitiveLength',12,'AnalyticExpansionInterval',10);
1
2

Define start and goal poses for the vehicle as [x, y, theta] vectors.
x and y specify the position in meters, and theta specifies the orientation angle in radians.

startPose = [75 20 -pi/2]; % [meters, meters, radians]
goalPose = [90 54 pi/2];
1
2

Plan a path from the start pose to the goal pose.

   refpath = plan(planner,startPose,goalPose);
1

Visualize the path using show function

show(planner)
size(refpath.States)
1
2

figure
show(map)
hold on
plot(startPose(1),startPose(2),'g.','markersize',25)
plot(goalPose(1),goalPose(2),'r.','markersize',25)
for i = 1: size(refpath.States,1)
   plot(refpath.States(i,1),refpath.States(i,2),'b-*','markersize',2)
   drawnow
   hold on
end
1
2
3
4
5
6
7
8
9
10