Enable Vehicle Collision Checking for Path Planning Using Hybrid A*

This example shows how to use a Hybrid A* planner to plan a path to a narrow parking space, while accounting for the shape of a car-like robot.

Set Up Map and Vehicle Collision Checker

Load the parkingLotCostVal.mat file into the MATLAB® workspace. The MAT file includes a parking lot costmap, costVal.

load parkingLotCostVal.mat

Represent the vehicle size using a vehicleDimensions (Automated Driving Toolbox) object with length and width of 5 meters and 3 meters, respectively. Then, create a vehicleCostmap (Automated Driving Toolbox) object using the parking lot costmap, to represent the planning space around the vehicle. Plot the vehicle costmap.

vehicleDims = vehicleDimensions(5,3);
map = vehicleCostmap(costVal);
plot(map,Inflation="off")
title("Costmap of the parking lot")

To avoid obstacles and plan collision-free paths through the parking lot, inflate the sizes of the obstacles using an inflationCollisionChecker (Automated Driving Toolbox) object. Create the inflationCollisionChecker object that uses three circles to approximate the dimensions of the vehicle.

ccConfig = inflationCollisionChecker(vehicleDims,3);

The vehicle costmap inflates the sizes of obstacles based on the vehicle dimensions and the number of circles used to estimate the vehicle. Visualize the vehicle collision-checking configuration to see how it approximates the dimensions.

plot(ccConfig) % display the collision-checking configuration

Assign the collision check to the map object, and display the inflated costmap of the parking lot.

map.CollisionChecker = ccConfig;
plot(map)
title("Inflated costmap of the parking lot")

To validate the vehicle states with the inflated obstacles, create a validatorVehicleCostmap object using the vehicle costmap.

validator = validatorVehicleCostmap(stateSpaceSE2,Map=map);

Plan and Visualize Path

Initialize the plannerHybridAStar object using the state validator object. Specify the minimum turning radius and the motion primitive length using the MinTurningRadius and MotionPrimitiveLength properties of the planner, respectively. Real-world vehicles typically have a turning radius in the range of 4 to 7 meters, so specify MinTurningRadius as 4. Specify MotionPrimitiveLength as 6, providing offering a decent tradeoff between maneuverability and search efficiency, but note that other values may be better for different environments.

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

Define start and goal poses for the vehicle as vectors of the form [x y theta], where x and y specify the position in meters, and theta specifies the orientation angle in radians. The start pose is located near the entrance of the parking lot, and the goal pose is the desired parking space.

startPose = [6 10 pi/2]; % [meters, meters, radians]
goalPose = [90 57 -pi/2];

Plan a path from the start pose to the goal pose. To generate a smoother path, you can use the optimizePath function. For more information, see Optimization Based Path Smoothing for Autonomous Vehicles.

refpath = plan(planner,startPose,goalPose);

Visualize the path with the vehicle model.

vehiclePlotInterval = 10; % Draw a vehicle model every 10 states
exampleHelperVisualizePathWithVehicleModel(planner,refpath,ccConfig,vehiclePlotInterval)

Zoom in on the vehicle with the circle estimations, in the final pose. The vehicleCostmap estimates the underlying geometry of the vehicle well enough to plan a collision-free path to the goal position. Note that, even though the circle estimations overlap with the inflated area, the vehicle is collision-free as long as the centers of the circles are not in the inflated area.

Note that, for a vehicle with a much larger width or length, such as a large truck, the radius of the circle estimations is much larger, too. This means the inflated area is also enlarged, and the planner might be unable to find a feasible solution. To plan a path accurately for your vehicle, you must set up the vehicle collision checker correctly for the dimensions of your vehicle when enabling this functionality.