step
Compute velocity commands and optimal trajectory for subsequent time steps
Since R2023a
Description
[
computes the linear and angular velocity commands velcmds
,timestamps
,optPath
] = step(controller
,curState
,curVel
)velcmds
, with their
corresponding timestamps
and corresponding optimized path
optPath
, for the specified current pose curState
and current velocity curVel
of a robot.
[___,
returns extra information, extraInfo
] = step(___)extraInfo
, to evaluate the solution, in
addition to all arguments from the previous syntax.
Examples
Compute Velocity Commands and Optimal Trajectory for DifferentialDrive Robot Using Timed Elastic Band Algorithm
Set Up Parking Lot Environment
Create an occupancyMap
object from a parking lot map and set the map resolution to 3 cells per meter.
load parkingMap.mat;
resolution = 3;
map = occupancyMap(map,resolution);
Visualize the map. The map contains the floor plan of a parking lot with some parking slots already occupied.
show(map) title("Parking Lot Map") hold on
Set Up and Run Global Planner
Create a validatorOccupancyMap
state validator using the stateSpaceSE2
definition. Specify the map and the distance for interpolating and validating path segments.
validator = validatorOccupancyMap(stateSpaceSE2,Map=map); validator.ValidationDistance = 0.1;
Create an RRT* path planner. Increase the maximum connection distance.
rrtstar = plannerRRTStar(validator.StateSpace,validator); rrtstar.MaxConnectionDistance = 0.2;
Set the start and goal states.
start = [2 9 0]; goal = [27 18 pi/2];
Plan a path with default settings.
rng(42,"twister") % Set random number generator seed for repeatable result. route = plan(rrtstar,start,goal); refpath = route.States;
RRT* uses a random orientation, which can cause unnecessary turns.
headingToNextPose = headingFromXY(refpath(:,1:2));
Align the orientation to the path, except for at the start and goal states.
refpath(2:end1,3) = headingToNextPose(2:end1);
Visualize the path.
plot(refpath(:,1),refpath(:,2),"r",LineWidth=2) hold off
Set Up and Run Local Planner
Create a local occupancyMap
object with a width and height of 15 meters and the same resolution as the global map.
localmap = occupancyMap(15,15,map.Resolution);
Create a controllerTEB
object by using the reference path generated by the global planner and the local map.
teb = controllerTEB(refpath,localmap);
Specify the properties of the controllerTEB
object.
teb.LookAheadTime = 10; % sec teb.ObstacleSafetyMargin = 0.4; % meters % To generate timeoptimal trajectories, specify a larger weight value, % like 100, for the cost function, Time. To follow the reference path % closely, keep the weight to a smaller value like 1e3. teb.CostWeights.Time = 100;
Create a deep clone of the controllerTEB
object.
teb2 = clone(teb);
Initialize parameters.
curpose = refpath(1,:);
curvel = [0 0];
simtime = 0;
% Reducing timestep can lead to more accurate path tracking.
timestep = 0.1;
itr = 0;
goalReached = false;
Compute velocity commands and optimal trajectory.
while ~goalReached && simtime < 200 % Update map to keep robot in the center of the map. Also update the % map with new information from the global map or sensor measurements. moveMapBy = curpose(1:2)  localmap.XLocalLimits(end)/2; localmap.move(moveMapBy,FillValue=0.5) syncWith(localmap,map) if mod(itr,10) == 0 % every 1 sec % Generate new vel commands with teb [velcmds,tstamps,curpath,info] = step(teb,curpose,curvel); goalReached = info.HasReachedGoal; feasibleDriveDuration = tstamps(info.LastFeasibleIdx); % If robot is far from goal and only less than third of trajectory % is feasible, then an option is to replan the path to follow to % reach the goal. if info.ExitFlag == 1 && ... feasibleDriveDuration < (teb.LookAheadTime/3) route = plan(rrtstar,curpose,[27 18 pi/2]); refpath = route.States; headingToNextPose = headingFromXY(refpath(:,1:2)); refpath(2:end1,3) = headingToNextPose(2:end1); teb.ReferencePath = refpath; end timestamps = tstamps + simtime; % Show the updated information input to or output % from controllerTEB clf show(localmap) hold on plot(refpath(:,1),refpath(:,2),".",Color="#EDB120", ... DisplayName="Reference Path") quiver(curpath(:,1),curpath(:,2), ... cos(curpath(:,3)),sin(curpath(:,3)), ... 0.2,Color="#A2142F",DisplayName="Current Path") quiver(curpose(:,1),curpose(:,2), ... cos(curpose(:,3)),sin(curpose(:,3)), ... 0.5,"o",MarkerSize=20,ShowArrowHead="off", ... Color="#0072BD",DisplayName="Start Pose") end simtime = simtime+timestep; % Compute the instantaneous velocity to be sent to the robot from the % series of timestamped commands generated by controllerTEB velcmd = velocityCommand(velcmds,timestamps,simtime); % Very basic robot model, should be replaced by simulator. statedot = [velcmd(1)*cos(curpose(3)) ... velcmd(1)*sin(curpose(3)) ... velcmd(2)]; curpose = curpose + statedot*timestep; if exist("hndl","var") delete(hndl) end hndl = quiver(curpose(:,1),curpose(:,2), ... cos(curpose(:,3)),sin(curpose(:,3)), ... 0.5,"o",MarkerSize=20,ShowArrowHead="off", ... Color="#D95319",DisplayName="Current Robot Pose"); itr = itr + 1; drawnow end legend
Input Arguments
controller
— TEB controller
controllerTEB
object
TEB controller, specified as a controllerTEB
object.
curState
— Current pose of robot
threeelement vector of form [x
y
theta]
Current pose of the robot, specified as a threeelement vector of the form [x y theta]. x and y specify the robot position in meters. theta specifies the robot orientation in radians.
Data Types: single
 double
curVel
— Current velocity of robot
twoelement vector of form [v
w]
Current velocity of the robot, specified as a twoelement vector of the form [v w]. v specifies the linear velocity of the robot in meters per second. w specifies the angular velocity of the robot in radians per second.
Data Types: single
 double
Output Arguments
velcmds
— Velocity commands
Nby2 matrix
Velocity commands, returned as an Nby2 matrix. The first column is the linear velocity in meters per second, and the second column is the angular velocity in radians per second.
Data Types: double
timestamps
— Timestamps corresponding to velocity commands
Nelement column vector
Timestamps corresponding to velocity commands, returned as an Nelement column vector.
Data Types: double
optPath
— Optimized path
Nby3 matrix
Optimized path, returned as an Nby3 matrix. Each row is of the form [x y theta], which defines the xyposition and orientation angle theta at a point in the path.
N is affected by the ReferenceDeltaTime and LookAheadTime
properties of controller
. The algorithm tries to keep the
difference between any two consecutive timestamps
close to ReferenceDeltaTime
. If the gap between a pair of
consecutive timestamps is greater than ReferenceDeltaTime
, the
function adds poses and timestamps to the path. If the gap is less than
ReferenceDeltaTime
, the function removes poses and timestamps
from the path. In addition, the algorithm tries to keep the final value of
timestamps
close to LookAheadTime
, so
increasing LookAheadTime
increases N as
well.
Data Types: double
extraInfo
— Extra information
structure
Extra information, returned as a structure. The fields of the structure are:
Field  Description 

LastFeasibleIdx  Index specifying an element in the optimized path and timestamp
outputs until which the trajectory is feasible. Beyond this index, the value
of 
DistanceFromStartPose  Distance of each pose in 
HasReachedGoal  Indicates whether the robot has successfully reached the last pose
in the ReferencePath within a tolerance, and returns as

TrajectoryCost  Cost of optimized trajectory for cost functions in the Timed Elastic Band algorithm. 
ExitFlag  Scalar value indicating the exit condition of the

Data Types: struct
Version History
Introduced in R2023aR2023b: Output argument extraInfo
returns exit condition
The output argument extraInfo
now returns an addition field
ExitFlag
that specifies more information about the velocity commands,
timestamps, and path returned by the step
function.
MATLAB Command
You clicked a link that corresponds to this MATLAB command:
Run the command by entering it in the MATLAB Command Window. Web browsers do not support MATLAB commands.
Select a Web Site
Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .
You can also select a web site from the following list:
How to Get Best Site Performance
Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.
Americas
 América Latina (Español)
 Canada (English)
 United States (English)
Europe
 Belgium (English)
 Denmark (English)
 Deutschland (Deutsch)
 España (Español)
 Finland (English)
 France (Français)
 Ireland (English)
 Italia (Italiano)
 Luxembourg (English)
 Netherlands (English)
 Norway (English)
 Österreich (Deutsch)
 Portugal (English)
 Sweden (English)
 Switzerland
 United Kingdom (English)