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

Syntax

planner = plannerHybridAStar(validator)

planner = plannerHybridAStar(validator,Name,Value)

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.

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

StateValidator — State validator for planning

validatorOccupancyMap object | validatorVehicleCostmap object

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

MotionPrimitiveLength — Length of motion primitives to be generated

ceil(sqrt(2)*map_CellSize) (default) | positive scalar

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.

If you specify MotionPrimitiveLength greater than one-fourth the length of the circumference of a circle based on the MinTurningRadius, the planner can generate motion primitives that curve back on themselves, resulting in a less optimal path.

You must specify a MotionPrimitiveLength value greater than the diagonal length of the map cell (sqrt(2) times length of map cell). Otherwise, the planner returns an error.

Data Types: double

MinTurningRadius — Minimum turning radius of vehicle

2 (default) | positive scalar

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

Data Types: double

NumMotionPrimitives — Number of motion primitives to be generated

5 (default) | positive odd integer scalar greater than or equal to 3

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.

ForwardCost — Cost multiplier to travel in forward direction

1 (default) | positive scalar

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

ReverseCost — Cost multiplier to travel in reverse direction

3 (default) | positive scalar

Cost multiplier to travel in the reverse direction, specified as the comma-separated pair consisting of 'ReverseCost' and a positive scalar. Increase the cost value to penalize reverse motion. This property has no effect if you set the MotionDirection property to "forward".

Data Types: double

DirectionSwitchingCost — Additive cost for switching direction of motion

0 (default) | positive scalar

Additive cost for switching the direction of motion, specified as the comma-separated pair consisting of 'DirectionSwitchingCost' and a positive scalar. Increase the cost value to penalize switching directions. This property has no effect if you set the MotionDirection property to "forward".

Data Types: double

AnalyticExpansionInterval — Interval for attempting analytic expansion from lowest cost node available

5 (default) | positive integer scalar

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.

InterpolationDistance — Distance between interpolated poses in output path

1 (default) | positive scalar

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

TransitionCostFcn — Transition cost function

function handle

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.

This property is nontunable for code generation.

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

Data Types: function_handle

AnalyticalExpansionCostFcn — Cost for analytic expansion

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.

If you set the MotionDirection property ito "forward-reverse", planner uses analytic expansions based on a Reeds-Shepp path model. If you set the MotionDirection property to "forward", the planner uses analytic expansions based on a Dubins path model.

This property is nontunable for code generation.

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

Data Types: function_handle

MotionDirection — Allowed motion directions

"forward-reverse" (default) | "forward"

Since R2026a

Allowed motion directions, specified as one of these values:

• "forward-reverse" — Allows both forward and reverse motion. The planner uses analytic expansions based on a Reeds-Shepp path model.

• "forward" — Allows only forward motion. The planner uses analytic expansions based on a Dubins path model.

This property is nontunable for code generation.

MaxNumNodes — Maximum number of explored nodes

Inf | positive integer

Since R2026a

This property is read-only after object creation.

Maximum number of nodes explored to optimize the solution, specified as one of these values:

• inf — The maximum number of nodes to explore is resizeable. This is the default value in MATLAB simulation mode or when dynamic memory allocation is enabled in C/C++ code generation mode.

• Positive integer — Specifies the maximum number of nodes to explore. You can use this to limit the number of nodes explored when you are not using dynamic memory allocation during C/C++ code generation.

If using this object for code generation with dynamic memory allocation disabled, this property defaults to half the area of the map. For example, given a StateValidator property that contains a validatorOccupancyMap object with an occupancyMap object in its Map property, the default value for code generation without dynamic memory allocation is based on the values of the GridSize property of that occupancyMap object: 0.5*GridSize(1)*GridSize(2).

For information about enabling dynamic memory allocation in C/C++ code generation mode, see the coder.areUnboundedVariableSizedArraysSupported (MATLAB Coder) object.

MaxNumPathStates — Maximum number of path states

Inf | positive integer

Since R2026a

This property is read-only after object creation.

Maximum number of path states, specified as one of these values:

• inf — The maximum number of path states is resizeable. This is the default value in MATLAB simulation mode or when dynamic memory allocation is enabled in C/C++ code generation mode.

• Positive integer — Specifies the maximum number of path states. You can use this to limit the number of path states when you are not using dynamic memory allocation during C/C++ code generation.

If using this object for code generation with dynamic memory allocation disabled, this property defaults to twice the area of the map. For example, given a StateValidator property that contains a validatorOccupancyMap object with an occupancyMap object in its Map property, the default value for code generation without dynamic memory allocation is based on the values of the GridSize property of that occupancyMap object: 2*GridSize(1)*GridSize(2).

For information about enabling dynamic memory allocation in C/C++ code generation mode, see the coder.areUnboundedVariableSizedArraysSupported (MATLAB Coder) object.

Object Functions

plan	Find obstacle-free path between two poses
show	Visualize the planned path

Examples

Obstacle-Free Path Planning Using Hybrid A Star

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)

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

R2026a: Cost functions are tunable in MATLAB simulation mode

You can now tune the AnalyticalExpansionCostFcn and TransitionCostFcn properties after object creation in MATLAB simulation mode.

R2026a: Specify maximum number of nodes to explore and path states

You can now specify the maximum number of nodes to explore and path states using the MaxNumNodes and MaxNumPathStates properties, respectively. You can use these properties to limit the number of explored nodes and path states when dynamic memory allocation is not enabled during code generation.

R2026a: Default MinTurningRadius value has changed

The plannerHybridAStar object now uses 2 meters as the default value of the MinTurningRadius property, independent of map cell size. In previous releases, the default value is max(2,2*ceil(sqrt(2)*map_CellSize)/pi) meters, where map_CellSize is the cell size of the map. To use the previous default value of MinTurningRadius when the map cell size is greater than 2.122 meters (resolution of map less than 0.471 cells/meter), set MinTurningRadius based on the value of the StateValidator property.

• If StateValidator is a validatorOccupancyMap object, set MinTurningRadius to 2*ceil(sqrt(2)/planner.StateValidator.Resolution)/pi, where planner is your plannerHybridAStar object.

• If StateValidator is a validatorVehicleCostmap object, set MinTurningRadius to 2*ceil(sqrt(2)*planner.StateValidator.CellSize)/pi, where planner is your plannerHybridAStar object.

R2026a: Error about constraint between MotionPrimitiveLength and MinTurningRadius is now a warning

The error returned when MotionPrimitiveLength exceeds one-fourth the length of the circumference of a circle based on MinTurningRadius is now a warning.

For example, this code sets MotionPrimitiveLength to greater than one-fourth the length of the circumference of a circle based on MinTurningRadius.

load("exampleMaps.mat","simpleMap")
map = binaryOccupancyMap(simpleMap);
validator = validatorOccupancyMap(stateSpaceSE2,Map=map);
planner = plannerHybridAStar(validator);
planner.MotionPrimitiveLength = (pi)*planner.MinTurningRadius

This table compares the output of the code in previous releases to the output in R2026a.

Previous Releases

R2026a

MATLAB® displays an error. Error using plannerHybridAStar Expected MotionPrimitiveLength to be an array with all of the values <= 3.14159.

MATLAB now displays a warning and sets MotionPrimitiveLength to the specified value. Warning: Setting the "MotionPrimitiveLength" has resulted in "MotionPrimitiveLength" value (6.2832 meters) being greater than (pi/2) times "MinTurningRadius" (3.1416 meters), which can cause motion primitives to curve back on themselves and result in a less optimal path. To prevent this, set "MotionPrimitiveLength" to a value less than 3.1416 meters. planner = plannerHybridAStar with properties: StateValidator: [1×1 validatorOccupancyMap] MinTurningRadius: 2 MotionPrimitiveLength: 6.2832 NumMotionPrimitives: 5 ForwardCost: 1 ReverseCost: 3 DirectionSwitchingCost: 0 AnalyticExpansionInterval: 5 InterpolationDistance: 1 MotionDirection: "forward-reverse"

Similarly, this code sets MinTurningRadius such that MotionPrimitiveLength is greater than one-fourth the length of the circumference of a circle based on MinTurningRadius.

load("exampleMaps.mat","simpleMap")
map = binaryOccupancyMap(simpleMap);
validator = validatorOccupancyMap(stateSpaceSE2,Map=map);
planner = plannerHybridAStar(validator);
planner.MinTurningRadius = (1/pi)*planner.MotionPrimitiveLength

This table compares the output of the code in previous releases to the output in R2026a.

Previous Releases

R2026a

MATLAB displays an error. Error using plannerHybridAStar Expected MinTurningRadius to be an array with all of the values >= 1.27324.

MATLAB now displays a warning and sets MinTurningRadius to the specified value. Warning: Setting the "MinTurningRadius" has resulted in "MotionPrimitiveLength" value (2 meters) being greater than (pi/2) times "MinTurningRadius" (1 meters), which can cause motion primitives to curve back on themselves and result in a less optimal path. To prevent this, set "MotionPrimitiveLength" to a value less than 1 meters. planner = plannerHybridAStar with properties: StateValidator: [1×1 validatorOccupancyMap] MinTurningRadius: 0.6366 MotionPrimitiveLength: 2 NumMotionPrimitives: 5 ForwardCost: 1 ReverseCost: 3 DirectionSwitchingCost: 0 AnalyticExpansionInterval: 5 InterpolationDistance: 1 MotionDirection: "forward-reverse"

R2026a: Specify allowed motion directions

You can now specify the allowed motion directions for the planner by setting the MotionDirection property using the corresponding name-value argument. By default, the planner allows both forward and reverse motion directions. You can limit the planner to forward motion by specifying the motion direction as "forward".

R2023b: Specify custom cost function for g-cost computation

You can now use TransitionCostFcn and AnalyticalExpansionCostFcn name, value pair arguments to specify custom cost functions for g-cost computation in hybrid A* planning algorithm.