step

Compute velocity commands and optimal trajectory for subsequent time steps

Since R2023a

Syntax

[velcmds,timestamps,optPath] = step(controller,curState,curVel)

[___,extraInfo] = step(___)

Description

[velcmds,timestamps,optPath] = step(controller,curState,curVel) computes the linear and angular velocity commands velcmds, with their corresponding timestamps and corresponding optimized path optPath, for the specified current pose curState and current velocity curVel of a robot.

[___,extraInfo] = step(___) returns extra information, extraInfo, to evaluate the solution, in addition to all arguments from the previous syntax.

Examples

Compute Velocity Commands and Optimal Trajectory for Differential-Drive 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:end-1,3) = headingToNextPose(2:end-1);

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 time-optimal 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 1e-3.
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 re-plan 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:end-1,3) = headingToNextPose(2:end-1);
            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

three-element vector of form [x y theta]

Current pose of the robot, specified as a three-element 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

two-element vector of form [v w]

Current velocity of the robot, specified as a two-element 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

N-by-2 matrix

Velocity commands, returned as an N-by-2 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

N-element column vector

Timestamps corresponding to velocity commands, returned as an N-element column vector.

Data Types: double

optPath — Optimized path

N-by-3 matrix

Optimized path, returned as an N-by-3 matrix. Each row is of the form [x y theta], which defines the xy-position 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 ExitFlag will be greater than zero. This implies one or more scenarios corresponding to ExitFlag > 0 has occurred.
DistanceFromStartPose	Distance of each pose in optPath from the first pose in optPath. The value of curState is always the first pose in optPath.
HasReachedGoal	Indicates whether the robot has successfully reached the last pose in the ReferencePath within a tolerance, and returns as true if successful. Otherwise, this value returns false.
TrajectoryCost	Cost of optimized trajectory for cost functions in the Timed Elastic Band algorithm.
ExitFlag	Scalar value indicating the exit condition of the step function. 0 — Indicates that the step function returned feasible velocity commands and trajectory. 1 — Indicates that the trajectory output returned by the step function is collision free only until the index value specified using LastFeasibleIdx. 2 — Indicates that the poses corresponding to indices after the LastFeasibleIdx violates the obstacle safety margin constraint of the TEB controller by more than 10%. 3 — Indicates that the turning radius of the vehicle while moving from a pose at LastFeasibleIdx to a subsequent pose violates the minimum turning radius constraint by more than 10%. 4 — Indicates that one or more time stamps returned by the step function are older than the previous time stamp. 5 — Indicates that the robot will not reach the reference path within a specified look-ahead time even if it travels at a maximum linear velocity from its current position.

Data Types: struct

Version History

Introduced in R2023a

R2023b: 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.