plan

Find obstacle-free path between two poses

Syntax

path = plan(planner,start,goal)

[path,directions] = plan(planner,start,goal)

[path,directions,solutionInfo] = plan(planner,start,goal)

[___] = plan(___,"SearchMode",mode)

Description

path = plan(planner,start,goal) computes an obstacle-free path between start and goal poses, specified as [x y theta] vectors, using the input plannerHybridAStar object.

[path,directions] = plan(planner,start,goal) also returns the direction of motion for each pose along the path, directions, as a column vector. A value of 1 indicates forward direction and a value of -1 indicates reverse direction. The function returns an empty column vector when the planner is unable to find a path.

[path,directions,solutionInfo] = plan(planner,start,goal) also returns solutionInfo that contains the solution information of the path planning as a structure.

[___] = plan(___,"SearchMode",mode) specifies the search algorithm mode mode in addition to any combination of arguments from previous syntaxes.

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)

Input Arguments

planner — Hybrid A* path planner

plannerHybridAStar object

Hybrid A* path planner, specified as a plannerHybridAStar object.

start — Start location of path

three-element vector

Start location of path, specified as a 1-by-3 vector in the form [x y theta]. x and y specify the position in meters, and theta specifies the orientation angle in radians.

Example: [5 5 pi/2]

Data Types: double

goal — Final location of path

three-element vector

Final location of path, specified as a 1-by-3 vector in the form [x y theta]. x and y specify the position in meters, and theta specifies the orientation angle in radians.

Example: [45 45 pi/4]

Data Types: double

mode — Search algorithm mode

"greedy" (default) | "exhaustive"

Search algorithm mode, specified as one of these options:

• "greedy" — Prioritize finding a solution in the shortest time on average.

• "exhaustive" — Increase the number of nodes in the open set to optimize the solution.

Example: plan(phastar,start,goal,"SearchMode","greedy")

Data Types: string | char

Output Arguments

path — Obstacle-free path

navPath object

Obstacle-free path, returned as a navPath object.

directions — Directions of motion

column vector of 1s (forward) and –1s (reverse)

Direction of motion for each pose along the path, returned as a column vector of 1s (forward) and –1s (reverse).

Data Types: double

solutionInfo — Solution Information

structure

Solution Information, returned as a structure. The fields of the structure are:

Fields of solutionInfo

Fields	Description
IsPathFound	Indicates whether a path is found. It returns as 1 if a path is found. Otherwise, it returns 0.
NumNodes	Number of nodes in the search tree when the planner terminates (excluding the root node).
NumIterations	Number of planning iterations executed.
ExitFlag	Indicates the terminate status of the planner, returned as 1 — if the goal is reached 2 — if the heuristic cost is infinity 3 — if path is not found

Data Types: struct

Extended Capabilities

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

Version History

Introduced in R2019b

R2022b: Set search algorithm mode with name-value argument

Use the SearchMode name-value argument to set the search algorithm mode.