plan

Plan kinematically feasible path between two states

Since R2021b

Syntax

path = plan(planner,startState,goalState)

[___,solutionInfo] = plan(planner,startState,goalState)

[___] = plan(planner,startState,goalSampleFcn)

Description

path = plan(planner,startState,goalState) tries to find a valid path between startState and goalState.

The planning is carried out based on the state propagator, which leverages a kinematic model and controllers of the system to search the configuration space. The planner returns a navPathControl object, path, which contains the propagator used during planning and a sequence of states, controls, target states, and control durations.

[___,solutionInfo] = plan(planner,startState,goalState) also returns the solution information solutionInfo of the path planning.

[___] = plan(planner,startState,goalSampleFcn) takes in a function handle that produces a goal configuration when called. The function handle should take no inputs and generate a goal state whose size matches startState.

Examples

Plan Kinodynamic Path with Controls for Mobile Robot

Plan control paths for a bicycle kinematic model with the mobileRobotPropagator object. Specify a map for the environment, set state bounds, and define a start and goal location. Plan a path using the control-based RRT algorithm, which uses a state propagator for planning motion and the required control commands.

Set State and State Propagator Parameters

Load a ternary map matrix and create an occupancyMap object. Create the state propagator using the map. By default, the state propagator uses a bicycle kinematic model.

load('exampleMaps','ternaryMap')
map = occupancyMap(ternaryMap,10);

propagator = mobileRobotPropagator(Environment=map); % Bicycle model

Set the state bounds on the state space based on the map world limits.

propagator.StateSpace.StateBounds(1:2,:) = ...
                    [map.XWorldLimits; map.YWorldLimits];

Plan Path

Create the path planner from the state propagator.

planner = plannerControlRRT(propagator);

Specify the start and goal states.

start = [10 15 0];
goal  = [40 30 0];

Plan a path between the states. For repeatable results, reset the random number generator before planning. The plan function outputs a navPathControl object, which contains the states, control commands, and durations.

rng("default")
path = plan(planner,start,goal)

path = 
  navPathControl with properties:

    StatePropagator: [1×1 mobileRobotPropagator]
             States: [192×3 double]
           Controls: [191×2 double]
          Durations: [191×1 double]
       TargetStates: [191×3 double]
          NumStates: 192
        NumSegments: 191

Visualize Results

Visualize the map and plot the path states.

show(map)
hold on
plot(start(1),start(2),"rx")
plot(goal(1),goal(2),"go")
plot(path.States(:,1),path.States(:,2),"b")
hold off

Display the [v psi] control inputs of forward velocity and steering angle.

plot(path.Controls)
ylim([-1 1])
legend(["Velocity (m/s)","Steering Angle (rad)"])

Input Arguments

planner — Path planner

plannerControlRRT object

Path planner, specified as a plannerControlRRT object.

startState — Start state of path

s-element vector

Initial state of the path, specified as an s-element vector. s is the number of state variables in the state space. For example, a robot in the SE(2) space has a state vector of [x y theta].

Example: [1 1 pi/6]

Data Types: single | double

goalState — Goal state of path

s-element vector

Goal state of the path, specified as an s-element vector. s is the number of state variables in the state space. For example, a robot in the SE(2) space has a state vector of [x y theta].

Example: [2 2 pi/3]

Data Types: single | double

goalSampleFcn — Goal state sample function

function handle

Goal state sample function, specified as a function handle. The function handle should take no inputs and generate a goal state whose size matches startState.

Example:

Data Types: function_handle

Output Arguments

path — Planned path information

navPathControl object

Planned path information, returned as a navPathControl object.

solutionInfo — Solution Information

structure

Solution Information, returned as a structure. The structure contains these fields:

Field	Description
IsPathFound	Indicates whether a path is found. It returns as 1 if a path is found. Otherwise, it returns as 0.
ExitFlag	Indicates the terminate status of the planner, returned as one of these options: 1 — Goal successfully reached 2 — Exceeded maximum number of iterations 3 — Exceeded maximum number of nodes 4 — Exceeded maximum planning time
NumTreeNode	Number of nodes in the search tree when the planner terminates excluding the root node.
NumIterations	Number of target states propagated.
PlanningTime	Elapsed time while planning, returned as a scalar in seconds.
TreeInfo	Collection of explored states that reflects the status of the search tree when the planner terminates. Note that the planner inserts NaN values as delimiters to separate each individual edge.

Data Types: structure

Extended Capabilities

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

Version History

Introduced in R2021b