Main Content

plan

Plan kinematically feasible path between two states

Since R2021b

collapse all in page

    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.

    example

    [___,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

    collapse all

    Open Live Script

    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: [1x1 mobileRobotPropagator]
             States: [192x3 double]
           Controls: [191x2 double]
          Durations: [191x1 double]
       TargetStates: [191x3 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

    Figure contains an axes object. The axes object with title Occupancy Grid, xlabel X [meters], ylabel Y [meters] contains 4 objects of type image, line. One or more of the lines displays its values using only markers

    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)"])

    Figure contains an axes object. The axes object contains 2 objects of type line. These objects represent Velocity (m/s), Steering Angle (rad).

    Input Arguments

    collapse all

    Path planner, specified as a plannerControlRRT object.

    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

    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

    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

    collapse all

    Planned path information, returned as a navPathControl object.

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

    FieldDescription
    IsPathFoundIndicates 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

    NumTreeNodeNumber of nodes in the search tree when the planner terminates excluding the root node.
    NumIterationsNumber of target states propagated.
    PlanningTimeElapsed time while planning, returned as a scalar in seconds.
    TreeInfoCollection 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

    See Also

    Objects

    Functions