Main Content

plannerControlRRT

Control-based RRT planner

Since R2021b

expand all in page

    Description

    The plannerControlRRT object is a rapidly exploring random tree (RRT) planner for solving kinematic and dynamic (kinodynamic) planning problems using controls. The RRT algorithm is a tree-based motion planning routine that incrementally grows a search tree. In kinematic planners, the tree grows by randomly sampling states in system configuration space, and then attempts to propagate the nearest node toward that state. The state propagator samples controls for reaching the state based on the kinematic model and control policies. As the tree adds nodes, the sampled states span the search space and eventually connect the start and goal states.

    These are the control-based RRT algorithm steps:

    • Planner, plannerControlRRT, requests a state from the state space.

    • Planner finds the nearest state in the search tree based on cost.

    • State propagator, mobileRobotPropagator, samples control commands and durations to propagate toward the target state.

    • State propagator propagates toward the target state.

    • If the propagator returns a valid trajectory to the state, then add the state to the tree.

    • Optional: Attempt to direct trajectory toward final goal based on NumGoalExtension and GoalBias properties.

    • Continue searching until the search tree reaches the goal or satisfies other exit criteria.

    The benefit of a kinodynamic planner like plannerControlRRT is that it is guaranteed to return a sequence of states, controls, and references which comprise a kinematically or dynamically feasibly path. The drawback to a kinodynamic planner is that the kinematic propagations cannot guarantee that new states are exactly equal to the target states unless there exists and analytic representation for a sequence of controls that drive the system between two configurations with zero residual error. This means that kinodynamic planners are typically asymptotically complete and guarantee kinematic feasibility, but often can not guarantee asymptotic optimality.

    Creation

    Syntax

    controlPlanner = plannerControlRRT(propagator)
    controlPlanner = plannerControlRRT(propagator,Name=Value)

    Description

    controlPlanner = plannerControlRRT(propagator) creates a kinodynamic RRT planner from a state propagator object and sets the StatePropagator property.

    example

    controlPlanner = plannerControlRRT(propagator,Name=Value) specifies additional properties using name-value arguments. For example, plannerControlRRT(propagator,ContinueAfterGoalReached=1) continues to search for alternative paths after the tree first reaches the goal.

    Properties

    expand all

    Mobile robot state propagator, specified as a mobileRobotPropagator object or an object of a subclass of nav.StatePropagator.

    Optimization after reaching the goal, specified as a logical 0 (false) or 1 (true). If specified as true, the planner continues to search for alternative paths after it first reaches the goal. The planner terminates regardless of the value of this property if it reaches the maximum number of iterations or maximum number of tree nodes.

    Data Types: logical

    Maximum time allowed for planning, specified as a positive scalar in seconds.

    Data Types: single | double

    Maximum number of nodes in the search tree, excluding the root node, specified as a positive integer.

    Data Types: single | double

    Maximum number of iterations, specified as a positive integer.

    Data Types: single | double

    The maximum number of times the planner can propagate towards the goal, specified as a positive integer. After successfully adding a new node to the tree, the planner attempts to propagate the new node toward the goal using the propagateWhileValid object function of the state propagator. The planner continues propagating until the function returns an empty state vector indicating that no valid control is found, the planner reaches the goal, or the function has been called NumGoalExtension times.

    To turn this behavior off, set the property to 0. Turning this behavior off will result in propagating randomly instead of toward the goal.

    Data Types: single | double

    Probability of choosing the goal state during state sampling, specified as a real scalar in the range [0, 1]. This property determines the likelihood of the planner choosing the actual goal state when randomly selecting states from the state space. You can start by setting the probability to a small value, such as 0.05.

    Data Types: single | double

    Callback function to determine whether the goal is reached, specified as a function handle. You can create your own goal-reached function. The function must follow this syntax:

     isReached = myGoalReachedFcn(planner,currentState,goalState)

    where:

    • planner — is the created planner object, specified as a plannerControlRRT object.

    • currentState — is the current state, specified as a s-element real vector. s is the number of state variables in the state space.

    • goalState — is the goal state, specified as a s-element real vector. s is the number of state variables in the state space.

    • isReached — is a boolean that indicates whether the current state has reached the goal state, returned as true or false.

    Data Types: function handle

    Object Functions

    planPlan kinematically feasible path between two states
    copyCreates deep copy of planner object

    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).

    References

    [1] S.M. Lavalle, J.J. Kuffner, "Randomized kinodynamic planning", International Journal of Robotics Research, vol. 20, no. 5, pp. 378-400, May 2001

    [2] Kavraki, L. and S. LaValle. "Chapter 5 Motion Planning", 1st ed., B. Siciliano et O. Khatib, Ed. New York: Springer-Verlag Berlin Heidelberg, 2008, pp. 109-131.

    Extended Capabilities

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

    Version History

    Introduced in R2021b

    See Also

    Objects

    Functions

    Topics