Control-based RRT planner
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:
plannerControlRRT, requests a state from the state space.
Planner finds the nearest state in the search tree based on cost.
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.
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.
creates a kinodynamic RRT planner from a state propagator object and sets the
controlPlanner = plannerControlRRT(propagator)
specifies additional properties using name-value arguments. For example,
controlPlanner = plannerControlRRT(propagator,Name=Value)
to search for alternative paths after the tree first reaches the goal.
ContinueAfterGoalReached — Optimization after reaching goal
0 (default) |
Optimization after reaching the goal, specified as a logical
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.
MaxPlanningTime — Maximum time allowed for planning
Inf (default) | positive scalar in seconds
Maximum time allowed for planning, specified as a positive scalar in seconds.
MaxNumTreeNodes — Maximum number of nodes in search tree
1e4 (default) | positive integer
Maximum number of nodes in the search tree, excluding the root node, specified as a positive integer.
MaxNumIteration — Maximum number of iterations
1e4 (default) | positive integer
Maximum number of iterations, specified as a positive integer.
NumGoalExtension — Number of times to propagate towards goal
1 (default) | positive integer
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
To turn this behavior off, set the property to
0. Turning this
behavior off will result in propagating randomly instead of toward the goal.
GoalBias — Probability of choosing goal state during state sampling
0.1 (default) | real scalar in range [0, 1]
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
GoalReachedFcn — Callback function to determine whether goal is reached
@plannerControlRRT.GoalReachedDefault | function handle
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:
function isReached = myGoalReachedFcn(planner,currentState,goalState),
planner— is the created planner object, specified as a
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
|Plan path from start to goal state|
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];
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 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
[v psi] control inputs of forward velocity and steering angle.
plot(path.Controls) ylim([-1 1]) legend(["Velocity (m/s)","Steering Angle (rad)"])
 S.M. Lavalle, J.J. Kuffner, "Randomized kinodynamic planning", International Journal of Robotics Research, vol. 20, no. 5, pp. 378-400, May 2001
 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.