controllerTEB

Avoid unseen obstacles with time-optimal trajectories

Since R2023a

Description

The controllerTEB object creates a controller (local planner) using the Timed Elastic Band (TEB) algorithm. The controller enables a robot to follow a reference path typically generated by a global planner, such as RRT or Hybrid A*. Additionally, the planner avoids obstacles and smooths the path while optimizing travel time, and maintains a safe distance from obstacles known or unknown to the global planner. The object also computes velocity commands and an optimal trajectory using the current pose of the robot and its current linear and angular velocities.

Creation

Syntax

controller = controllerTEB(refpath)

controller = controllerTEB(refpath,map)

controller = controllerTEB(___,Name=Value)

Description

controller = controllerTEB(refpath) creates a TEB controller object, controller, that computes the linear and angular velocity commands for a car-like or differential-drive robot to follow the reference path refpath and travel for 5 seconds in an obstacle-free environment. The refpath input sets the value of the ReferencePath property.

controller = controllerTEB(refpath,map) attempts to avoid obstacles in the specified occupancy map map. The controller assumes the space outside the map boundary is free. The map input sets the value of the Map property.

example

controller = controllerTEB(___,Name=Value) specifies properties using one or more name-value arguments in addition to any combination of input arguments from the previous syntaxes.

Properties

expand all

`ReferencePath` — Reference path to follow
N-by-2 matrix | N-by-3 matrix | `navPath` object with SE(2) state space

Reference path to follow, specified as an N-by-2 matrix, N-by-3 matrix, or navPath object with an SE(2) state space. When specified as a matrix, each row represents a pose on the path. Use the LookAheadTime property to select a part of the ReferencePath for which to optimize the trajectory and generate velocity commands.

Note

If you specify the reference path as an N-by-2 matrix, then the object computes the orientation using the headingFromXY function and appends it as the third column.

Data Types: single | double

`Map` — Occupancy map representing environment
`binaryOccupancyMap()` (default) | `binaryOccupancyMap` object | `occupancyMap` object

Occupancy map representing the environment, specified as a binaryOccupancyMap object or occupancyMap object containing the obstacles in the vicinity of the robot. When optimizing the trajectory, the controller considers the space outside the boundary of the map to be free. Larger maps can lead to slower performance.

`CostWeights` — Weights for cost function optimization
`struct("Time",10,"Smoothness",1000,"Obstacle",50)` (default) | structure

Weights for cost function optimization, specified as a structure. The fields of the structure are:

Field	Description
`Time`	Cost function weight for time, specified as a positive scalar. To lower the travel time, increase this weight value.
`Smoothness`	Cost function weight for smooth motion, specified as a positive scalar. To obtain a smoother path, increase this weight value.
`Obstacles`	Cost function weight for maintaining a safe distance from obstacles, specified as a positive scalar. To prioritize maintaining a safe distance from obstacles, increase this weight value.

Data Types: struct

`RobotInformation` — Robot geometry information for collision checking
`struct("Dimension",[1 0.67],"Shape","Rectangle","FixedTransform",[1 0.5 0.5])` (default) | structure

Robot geometry information for collision checking, specified as a structure. The fields of the structure are:

Field Description

Field	Description
`Dimension`	Size of the robot, specified as a two-element positive vector of the form [length width], in meters.
`Shape`	Shape of the robot, specified as `"Rectangle"` or `"Point"`.
`FixedTransform`	Origin of the robot, specified as a three-element row vector of form [x, y, theta] or a `se2` object defining an SE(2) transformation matrix. When you specify the origin as [x, y, theta], the `controllerTEB` object computes the associated SE(2) transformation matrix and stores it in the `FixedTransform` field. The default origin is at the center of the rear end of the robot. You can use this field to specify a new origin for the robot relative to the default origin. This field is valid only if the robot is of rectangular shape.

Dimension

Size of the robot, specified as a two-element positive vector of the form [length width], in meters.

Shape

Shape of the robot, specified as "Rectangle" or "Point".

FixedTransform

Origin of the robot, specified as a three-element row vector of form [x, y, theta] or a se2 object defining an SE(2) transformation matrix. When you specify the origin as [x, y, theta], the controllerTEB object computes the associated SE(2) transformation matrix and stores it in the FixedTransform field.

The default origin is at the center of the rear end of the robot. You can use this field to specify a new origin for the robot relative to the default origin.

This field is valid only if the robot is of rectangular shape.

If you set Shape as "Point", the Dimension field is set to [0 0].
If you set Shape as "Rectangle", the function considers the center of the robot's rear edge as the robot's origin.

Data Types: struct

`MinTurningRadius` — Minimum turning radius for vehicle on optimized path
0 (default) | nonnegative scalar

Since R2023b

Minimum turning radius for the vehicle on the optimized path, specified as a nonnegative scalar. This value corresponds to the radius of the turning circle at the maximum steering angle of the vehicle. Units are in meters.

Decrease this value to allow sharp turns and in place rotations. Increase this value to limit sharp turns. When you increase the value, the vehicle will make more forward and reverse motions to turn in a restricted space.

`ObstacleSafetyMargin` — Safe distance between robot and obstacles
`0.5` (default) | positive scalar

Safe distance between the robot and the obstacles, specified as a positive scalar, in meters. Note that this is a soft constraint that the planner may ignore.

Data Types: single | double

`NumIteration` — Number of iterations to optimize trajectory
`2` (default) | positive integer

Number of iterations to optimize the trajectory, specified as a positive integer. This value is the number of times interpolation occurs and the controller calls the solver for trajectory optimization.

Data Types: single | double

`MaxVelocity` — Maximum limits of linear and angular velocity
`[0.8 1.6]` (default) | two-element positive vector

Maximum limits of linear and angular velocity for velocity commands, specified as a two-element positive vector. The first element is the linear velocity limit, in meters per second, and the second element is the angular velocity limit, in radians per second.

Data Types: single | double

`MaxReverseVelocity` — Maximum velocity for reverse motion
`NaN` (default) | positive scalar

Since R2023b

Maximum velocity of the vehicle while moving in reverse direction, specified as a positive scalar. The default value is NaN. When the property is set to NaN, the value of maximum reverse velocity is same as that of the maximum linear velocity.

`MaxAcceleration` — Maximum limits of linear and angular acceleration
`[2.4 4.8]` (default) | two-element positive vector

Maximum limits of linear and angular acceleration for velocity commands, specified as a two-element positive vector. The first element is the linear acceleration limit, in meters per second squared, and the second element is the angular acceleration limit, in radians per second squared.

Data Types: single | double

`ReferenceDeltaTime` — Reference travel time between consecutive poses
`0.3` (default) | positive scalar

Reference travel time between consecutive poses, specified as a positive scalar in seconds. This property affects the addition and deletion of poses for the optimized trajectory. Increase the value of this property to have fewer poses and reduce it to have more poses in the output path.

Data Types: single | double

`LookAheadTime` — Look-ahead time
`5` (default) | positive scalar

Look-ahead time, specified as a positive scalar in seconds. The controller generates velocity commands and optimizes the trajectory until the controller reaches the look-ahead time. A higher look-ahead time generates velocity commands further into the future. This enables the robot to react earlier to unseen obstacles, but increases the controller execution time. Conversely, a shorter look-ahead time reduces the available time to react to new, unknown obstacles, but enables the controller to run at a faster rate.

Note

This property impacts the number of velocity commands, timestamps, and poses in the path.

Data Types: single | double

`GoalTolerance` — Tolerance around goal pose
[`0.1 0.1 0.1`] (default) | three-element vector

Since R2023b

Tolerance around the goal pose, specified as a three-element vector of the form [x y θ]. x and y denote the position of the robot in x and y directions, respectively. Units are in meters. θ is the heading angle of the robot in radians. This goal tolerance value specifies the limit for determining whether the robot has reached the goal pose.

Object Functions

`step`	Compute velocity commands and optimal trajectory for subsequent time steps
`clone`	Create deep clone of `controllerTEB` object

Examples

collapse all

Compute Velocity Commands and Optimal Trajectory for Differential-Drive Robot Using Timed Elastic Band Algorithm

Open Live Script

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

Extended Capabilities

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

Version History

Introduced in R2023a

expand all

R2024b: Specify origin of robot

You can now specify the origin of the robot relative to the default origin by using the FixedTransform field of the RobotInformation property. You can specify the new origin as either a three-element row vector or an SE(2) transformation matrix. In either case, the FixedTransform field stores the specified origin as an SE(2) transformation matrix.

For an example, specify the origin [1.0, 0.5, pi/4] as a three-element row vector.

origin = [1.0, 0.5, pi/4];
info = struct(Dimension=[1 1],Shape="Rectangle",FixedTransform=origin);
teb = controllerTEB(refpath,RobotInformation=info);

Alternatively, specify the origin [1.0, 0.5, pi/4] using a SE(2) transformation matrix.

tform = se2(pi/4,"theta",[1.0 0.5]);
info = struct(Dimension=[1 1],Shape="Rectangle",FixedTransform=tform);
teb = controllerTEB(refpath,RobotInformation=info);

R2023b: Specify minimum turning radius, maximum reverse velocity, and goal tolerance

You can now specify

Minimum turning radius for a vehicle on the optimized path by using the MinTurningRadius property.
Maximum velocity for reverse motion by using the MaxReverseVelocity property.
The limit for determining whether the robot has reached the goal pose by using the GoalTolerance property.

controllerTEB

Description

Creation

Syntax

Description

Properties

`ReferencePath` — Reference path to follow
N-by-2 matrix | N-by-3 matrix | `navPath` object with SE(2) state space

`Map` — Occupancy map representing environment
`binaryOccupancyMap()` (default) | `binaryOccupancyMap` object | `occupancyMap` object

`CostWeights` — Weights for cost function optimization
`struct("Time",10,"Smoothness",1000,"Obstacle",50)` (default) | structure

`RobotInformation` — Robot geometry information for collision checking
`struct("Dimension",[1 0.67],"Shape","Rectangle","FixedTransform",[1 0.5 0.5])` (default) | structure

`MinTurningRadius` — Minimum turning radius for vehicle on optimized path
0 (default) | nonnegative scalar

`ObstacleSafetyMargin` — Safe distance between robot and obstacles
`0.5` (default) | positive scalar

`NumIteration` — Number of iterations to optimize trajectory
`2` (default) | positive integer

`MaxVelocity` — Maximum limits of linear and angular velocity
`[0.8 1.6]` (default) | two-element positive vector

`MaxReverseVelocity` — Maximum velocity for reverse motion
`NaN` (default) | positive scalar

`MaxAcceleration` — Maximum limits of linear and angular acceleration
`[2.4 4.8]` (default) | two-element positive vector

`ReferenceDeltaTime` — Reference travel time between consecutive poses
`0.3` (default) | positive scalar

`LookAheadTime` — Look-ahead time
`5` (default) | positive scalar

`GoalTolerance` — Tolerance around goal pose
[`0.1 0.1 0.1`] (default) | three-element vector

Object Functions

Examples

Compute Velocity Commands and Optimal Trajectory for Differential-Drive Robot Using Timed Elastic Band Algorithm

Extended Capabilities

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

Version History

R2024b: Specify origin of robot

R2023b: Specify minimum turning radius, maximum reverse velocity, and goal tolerance

See Also

Objects

Functions

controllerTEB

Description

Creation

Syntax

Description

Properties

ReferencePath — Reference path to follow N-by-2 matrix | N-by-3 matrix | navPath object with SE(2) state space

Map — Occupancy map representing environment binaryOccupancyMap() (default) | binaryOccupancyMap object | occupancyMap object

CostWeights — Weights for cost function optimization struct("Time",10,"Smoothness",1000,"Obstacle",50) (default) | structure

RobotInformation — Robot geometry information for collision checking struct("Dimension",[1 0.67],"Shape","Rectangle","FixedTransform",[1 0.5 0.5]) (default) | structure

MinTurningRadius — Minimum turning radius for vehicle on optimized path 0 (default) | nonnegative scalar

ObstacleSafetyMargin — Safe distance between robot and obstacles 0.5 (default) | positive scalar

NumIteration — Number of iterations to optimize trajectory 2 (default) | positive integer

MaxVelocity — Maximum limits of linear and angular velocity [0.8 1.6] (default) | two-element positive vector

MaxReverseVelocity — Maximum velocity for reverse motion NaN (default) | positive scalar

MaxAcceleration — Maximum limits of linear and angular acceleration [2.4 4.8] (default) | two-element positive vector

ReferenceDeltaTime — Reference travel time between consecutive poses 0.3 (default) | positive scalar

LookAheadTime — Look-ahead time 5 (default) | positive scalar

GoalTolerance — Tolerance around goal pose [0.1 0.1 0.1] (default) | three-element vector

Object Functions

Examples

Compute Velocity Commands and Optimal Trajectory for Differential-Drive Robot Using Timed Elastic Band Algorithm

Extended Capabilities

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

Version History

R2024b: Specify origin of robot

R2023b: Specify minimum turning radius, maximum reverse velocity, and goal tolerance

See Also

Objects

Functions

`ReferencePath` — Reference path to follow
N-by-2 matrix | N-by-3 matrix | `navPath` object with SE(2) state space

`Map` — Occupancy map representing environment
`binaryOccupancyMap()` (default) | `binaryOccupancyMap` object | `occupancyMap` object

`CostWeights` — Weights for cost function optimization
`struct("Time",10,"Smoothness",1000,"Obstacle",50)` (default) | structure

`RobotInformation` — Robot geometry information for collision checking
`struct("Dimension",[1 0.67],"Shape","Rectangle","FixedTransform",[1 0.5 0.5])` (default) | structure

`MinTurningRadius` — Minimum turning radius for vehicle on optimized path
0 (default) | nonnegative scalar

`ObstacleSafetyMargin` — Safe distance between robot and obstacles
`0.5` (default) | positive scalar

`NumIteration` — Number of iterations to optimize trajectory
`2` (default) | positive integer

`MaxVelocity` — Maximum limits of linear and angular velocity
`[0.8 1.6]` (default) | two-element positive vector

`MaxReverseVelocity` — Maximum velocity for reverse motion
`NaN` (default) | positive scalar

`MaxAcceleration` — Maximum limits of linear and angular acceleration
`[2.4 4.8]` (default) | two-element positive vector

`ReferenceDeltaTime` — Reference travel time between consecutive poses
`0.3` (default) | positive scalar

`LookAheadTime` — Look-ahead time
`5` (default) | positive scalar

`GoalTolerance` — Tolerance around goal pose
[`0.1 0.1 0.1`] (default) | three-element vector

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