Plan Path Using A-Star Path Planners

Plan the shortest vehicle path to a parking spot using the A* grid algorithm. Then impose nonholonomic constraints on the vehicle and replan the path using the Hybrid A* algorithm.

Create Occpancy Map

Load a costmap of a parking lot. Create an occupancyMap (Navigation Toolbox) object using the properties of the costmap object. Visualize the occupancy map.

data = load('parkingLotCostmapReducedInflation.mat');
costmapObj = data.parkingLotCostmapReducedInflation;
resolution = 1/costmapObj.CellSize;
oMap = occupancyMap(costmapObj.Costmap,resolution);
oMap.FreeThreshold = costmapObj.FreeThreshold;
oMap.OccupiedThreshold = costmapObj.OccupiedThreshold;
show(oMap)

Plan Path Using A* Grid Planner

Use the occupancy map to create a plannerAStarGrid (Navigation Toolbox) object.

gridPlanner = plannerAStarGrid(oMap);

Define the start and goal positions in world coordinate frame. The origin of this coordinate frame is at the bottom-left corner of the map.

startPos = [11,10];
goalPos  = [31.5,18];

Plan a path from the start point to the goal point in world coordinates.

path = plan(gridPlanner,startPos,goalPos,"world");

Visualize the path and the explored nodes using the show object function.

show(gridPlanner)

Impose Nonholonomic Constraints and Replan Using Hybrid A* Planner

Create a state validator object for validating planned path using collision checking. Assign the occupancy map to the state validator object.

validator = validatorOccupancyMap;
validator.Map = oMap;

Initialize a plannerHybridAStar (Navigation Toolbox) object with the state validator object. Impose the nonholonomic constraints of minimum turning radius and motion primitive length by specifying the MinTurningRadius and MotionPrimitiveLength properties of the planner.

hybridPlanner = plannerHybridAStar(validator,MinTurningRadius=4,MotionPrimitiveLength=6);

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 = [4 4 pi/2]; % [meters, meters, radians]
goalPose = [45 27 -pi/2];

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

refpath = plan(hybridPlanner,startPose,goalPose);

Visualize the path using show object function.

show(hybridPlanner)