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

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

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

load parkingLotCostVal.mat

Create a binaryOccupancyMap with cost values.

map = binaryOccupancyMap(costVal);

Create a stateValidator object for collision checking.

validator = validatorOccupancyMap;

Assign the map to the stateValidator object.

validator.Map = map;

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);

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];

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

   refpath = plan(planner,startPose,goalPose);

Visualize the path using show function

show(planner)
size(refpath.States)

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