Main Content

plannerHybridAStar

Hybrid A* path planner

Description

The Hybrid A* path planner object generates a smooth path in a given 2-D map for vehicles with nonholonomic constraints. The approach applies A* algorithm to 3D kinematic state space of the vehicle with state variables (x, y, theta). In addition, it uses analytic Reed-Shepp expansion to improve search speed.

You can modify the behavior of the connection by tuning properties like MinTurningRadius, ForwardCost, and ReverseCost. You can use the AnalyticExpansionInterval property to set the cycle to check for the Reeds-Shepp connection.

Note

The Hybrid A* planner checks for collisions in the map by interpolating the motion primitives and analytic expansion based on the ValidationDistance property of the stateValidator object. If the ValidationDistance property is set to Inf, the object interpolates based on the cell size of the map specified in the state validator. Inflate the occupancy map before assigning it to the planner to account for the vehicle size.

Creation

Description

planner = plannerHybridAStar(validator) creates a path planner object using the Hybrid A* algorithm. Specify the validator input as a validatorOccupancyMap or validatorVehicleCostmap object. The validator input sets the value of the StateValidator property.

example

planner = plannerHybridAStar(validator,Name,Value) sets Properties of the path planner by using one or more name-value pair arguments. Enclose each property name inside single quotes (' ').

Properties

expand all

State validator for planning, specified either as a validatorOccupancyMap or validatorVehicleCostmap object based on SE(2) state space.

Length of motion primitives to be generated, specified as the comma-separated pair consisting of 'MotionPrimitiveLength' and a positive scalar in meters. Increase the length for large maps or sparse environments. Decrease the length for dense environments.

Note

'MotionPrimitiveLength' cannot exceed one-fourth the length of the circumference of a circle based on the 'MinTurningRadius'.

Data Types: double

Minimum turning radius of vehicle, specified as the comma-separated pair consisting of 'MinTurningRadius' and a positive scalar in meters.

Note

The value of 'MinTurningRadius' is set such that the 'MotionPrimitiveLength' cannot exceed one-fourth the length of the circumference of a circle based on it.

Data Types: double

Number of motion primitives to be generated, specified as the comma-separated pair consisting of 'NumMotionPrimitives' and a positive odd integer scalar greater than or equal to 3.

Cost multiplier to travel in forward direction, specified as the comma-separated pair consisting of 'ForwardCost' and a positive scalar. Increase the cost value to penalize forward motion.

Data Types: double

Cost multiplier to travel in reverse direction, specified as the comma-separated pair consisting of 'ReverseCost' and a positive scalar. Increase the cost value to penalize reverse motion.

Data Types: double

Additive cost for switching direction of motion, specified as the comma-separated pair consisting of 'DirectionSwitchingCost' and a positive scalar. Increase the cost value to penalize direction switching.

Data Types: double

Interval for attempting analytic expansion from the lowest cost node available at that instance, specified as the comma-separated pair consisting of 'AnalyticExpansionInterval' and a positive integer scalar.

The Hybrid A* path planner expands the motion primitives from the nodes with the lowest cost available at that instance:

  • The number of nodes to be expanded depends upon the number of primitives to be generated in both the direction and their validity, the cycle repeats until 'AnalyticExpansionInterval' is reached.

  • The planner then attempts an analytic expansion to reach the goal pose from the tree using a Reeds-Shepp model. If the attempt fails, the planner repeats the cycle.

Improve the algorithm performance by reducing the interval to increase the number of checks for a Reeds-Shepp connection to the final goal.

Distance between interpolated poses in output path, specified as the comma-separated pair consisting of 'InterpolationDistance' and a positive scalar in meters.

Data Types: double

Since R2023b

Transition cost function, specified as a function handle. This value specifies the cost for transitioning from one state to another state. By default, the plannerHybridAStar function uses Euclidean distance as the transition cost function. You can also specify a custom transition cost function to compute the transition cost. The function handle for the custom cost function must have at least one input and return one output. The output can be a scalar or a vector specifying the transition cost for motion primitives in forward and reverse direction.

Example: planner = plannerHybridAStar(validator,TransitionCostFcn=@(param1)customFcnName(param1));

Data Types: function_handle

Since R2023b

Cost for analytic expansion, specified as a function handle. By default, the plannerHybridAStar function uses analytic expansions based on the Reeds-Shepp model. You can also specify a custom cost function for analytic expansion. The function handle for the custom cost function must have at least one input and return one output.

Example: planner = plannerHybridAStar(validator,AnalyticalExpansionCostFcn=@(param1)customFcnName(param1));

Data Types: function_handle

Object Functions

planFind obstacle-free path between two poses
showVisualize the planned path

Examples

collapse all

Plan a collision-free path for a vehicle through a parking lot by using the Hybrid A* algorithm.

Create and Assign Map to State Validator

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

load parkingLotCostVal.mat % costVal

Create a binaryOccupancyMap with cost values.

resolution = 3;
map = binaryOccupancyMap(costVal,resolution);

Create a state space.

ss = stateSpaceSE2;

Update state space bounds to be the same as map limits.

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

Create a state validator object for collision checking.

sv = validatorOccupancyMap(ss);

Assign the map to the state validator object.

sv.Map = map;

Plan and Visualize Path

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

planner = plannerHybridAStar(sv, ...
                             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 9 pi/2]; % [meters, meters, radians]
goalPose = [30 19 -pi/2];

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

refpath = plan(planner,startPose,goalPose,SearchMode='exhaustive');     

Visualize the path using show function.

show(planner)

Figure contains an axes object. The axes object with title Hybrid A* Path Planner, xlabel X [meters], ylabel Y [meters] contains 8 objects of type image, line, scatter. These objects represent Reverse Motion Primitives, Forward Motion Primitives, Forward Path, Path Points, Orientation, Start, Goal.

References

[1] Dolgov, Dmitri, Sebastian Thrun, Michael Montemerlo, and James Diebel. Practical Search Techniques in Path Planning for Autonomous Driving. American Association for Artificial Intelligence, 2008.

[2] Petereit, Janko, Thomas Emter, Christian W. Frey, Thomas Kopfstedt, and Andreas Beutel. "Application of Hybrid A* to an Autonomous Mobile Robot for Path Planning in Unstructured Outdoor Environments." ROBOTIK 2012: 7th German Conference on Robotics. 2012, pp. 1-6.

Extended Capabilities

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

Version History

Introduced in R2019b

expand all