Main Content

plan

Find obstacle-free path between two poses

collapse all in page

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.

example

[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

collapse all

Open Live Script

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.

Input Arguments

collapse all

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

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

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

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

collapse all

Obstacle-free path, returned as a navPath object.

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

Data Types: double

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

Fields of solutionInfo

FieldsDescription
IsPathFoundIndicates whether a path is found. It returns as 1 if a path is found. Otherwise, it returns 0.
NumNodesNumber of nodes in the search tree when the planner terminates (excluding the root node).
NumIterationsNumber 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

expand all

See Also

| | |

Topics