Main Content

plan

Find obstacle-free path between two poses

Description

example

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

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 contains 8 objects of type image, line, scatter. These objects represent Forward Motion Primitives, Reverse 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.

Data Types: struct

Extended Capabilities

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

Version History

Introduced in R2019b

expand all