trajectory

Create actor or vehicle trajectory in driving scenario

Syntax

trajectory(ac,waypoints)

trajectory(ac,waypoints,speed)

trajectory(ac,waypoints,speed,waittime)

trajectory(___,Name=Value)

trajectory(ac,waypoints,TimeOfArrival=timeOfArrival)

trajectory(ac,waypoints,TimeOfArrival=timeOfArrival,Name=Value)

Description

The trajectory function creates a trajectory based on either the speed values or arrival times of an actor at each waypoint. By default, the function creates a trajectory for a vehicle whose steering location is at its rear axle. To create a trajectory for a vehicle whose steering location is at its front axle, use the ReferenceSteerPosition name-value argument.

Trajectory from Actor Speed Information

trajectory(ac,waypoints) creates a trajectory for an actor or vehicle, ac, from a set of waypoints.

Note

The trajectory function generates trajectories that have discontinuities in acceleration between waypoints, resulting in high amounts of jerk. To generate a smooth, jerk-limited trajectory, use the smoothTrajectory function instead.

trajectory(ac,waypoints,speed) also specifies the speed with which the actor or vehicle travels along the trajectory, in either forward or reverse motion.

trajectory(ac,waypoints,speed,waittime) specifies the wait time for an actor or vehicle. Use this syntax to generate stop-and-go driving scenarios by pausing an actor or vehicle actors at specific waypoints.

trajectory(___,Name=Value) specifies options using one or more name-value arguments in addition to any combination of arguments from the previous syntaxes. For example, Course=course specifies the course angle course, which defines the steering direction of the actor or vehicle at each waypoint.

Trajectory from Actor Arrival Times

Since R2024b

trajectory(ac,waypoints,TimeOfArrival=timeOfArrival) creates a trajectory for an actor or vehicle, ac, from a set of waypoints, waypoints, using the arrival time of the actor at each waypoint timeOfArrival.

trajectory(ac,waypoints,TimeOfArrival=timeOfArrival,Name=Value) specifies options using one or more name-value arguments. For example, MotionDirection=motionDirection specifies the direction motionDirection for the actor to move at each waypoint.

Examples

Simulate Vehicle with Trajectory of Varying Speeds

Create a driving scenario and add a curved two-lane road to it.

scenario = drivingScenario('SampleTime',0.05);
roadcenters = [5 0; 30 10; 35 25];
lspec = lanespec(2);
road(scenario,roadcenters,'Lanes',lspec);

Add a vehicle to the scenario. Set a trajectory in which the vehicle drives around the curve at varying speeds.

v = vehicle(scenario,'ClassID',1);
waypoints = [6 2; 18 4; 25 7; 28 10; 31 15; 33 22];
speeds = [30 10 5 5 10 30];
trajectory(v,waypoints,speeds)

Plot the scenario and run the simulation. Observe how the vehicle slows down as it drives along the curve.

plot(scenario,'Waypoints','on','RoadCenters','on')
while advance(scenario)
    pause(0.1)
end

Generate Stop-and-Go Driving Scenario

Create a driving scenario consisting of two, two-lane roads that intersect at a right angle.

scenario = drivingScenario('StopTime',2.75);
roadCenters = [50 1 0; 2 0.9 0];
laneSpecification = lanespec(2,'Width',4);
road(scenario,roadCenters,'Lanes',laneSpecification);
roadCenters = [27 24 0; 27 -21 0];
road(scenario,roadCenters,'Lanes',laneSpecification);

Add an ego vehicle to the scenario. Specify the waypoints and the speed values for the vehicle at each waypoint. Set a wait time for the vehicle at the second waypoint. Generate a trajectory in which the ego vehicle travels through the specified waypoints at the specified speed.

egoVehicle = vehicle(scenario,'ClassID',1,'Position',[5 -1 0]);
waypoints = [5 -1 0; 16 -1 0; 40 -1 0];
speed = [30; 0; 30];
waittime = [0; 0.3; 0];
trajectory(egoVehicle,waypoints,speed,waittime);

Add a car to the scenario. Specify the waypoints and the speed values for the car at each waypoint. Set a wait time for the car at the second waypoint. Generate a trajectory in which the car travels through the specified waypoints at the specified speed.

car = vehicle(scenario,'ClassID',1,'Position',[48 4 0],'PlotColor',[0.494 0.184 0.556], 'Name','Car');
waypoints = [47 3 0; 38 3 0; 10 3 0];
speed = [30; 0; 30];
waittime = [0; 0.3; 0];
trajectory(car,waypoints,speed,waittime);

Add an ambulance to the scenario. Generate a trajectory in which the ambulance travels through the specified waypoints at a constant speed.

ambulance = vehicle(scenario,'ClassID',6,'Position',[25 22 0],'PlotColor',[0.466 0.674 0.188],'Name','Ambulance');
waypoints = [25 22 0; 25 13 0; 25 6 0; 26 2 0; 33 -1 0; 45 -1 0];
speed = 25;
trajectory(ambulance,waypoints,speed);

Create a custom figure window to plot the scenario.

fig = figure;
set(fig,'Position',[0,0,800,600]);
movegui(fig,'center');
hViewPnl = uipanel(fig,'Position',[0 0 1 1],'Title','Stop-and-Go Scenario');
hPlt = axes(hViewPnl);

Plot the scenario and run the simulation. The ego vehicle and the car pause for their specified wait times to avoid collision with the ambulance.

plot(scenario,'Waypoints','on','RoadCenters','on','Parent',hPlt)
while advance(scenario)
    pause(0.1)
end

Simulate Car Backing into Parking Space

Simulate a driving scenario in which a car drives in reverse to back into a parking space.

Create a driving scenario containing a parking lot.

scenario = drivingScenario;
vertices = [0 9; 18 9; 18 -9; 0 -9];
parkingLot(scenario,vertices,ParkingSpace=parkingSpace);

Create a car and define its trajectory. The car drives forward, stops, and then drives in reverse to back into the parking space. As the car enters the parking space, it has a yaw orientation angle that is 90 degrees counterclockwise from where it started.

car = vehicle(scenario,ClassID=1);
waypoints = [9 -5; 9 5; 6 -1.3; 2 -1.3];
speed = [3; 0; -2; 0];
yaw = [90 90 180 180];
smoothTrajectory(car,waypoints,speed,Yaw=yaw)

Plot the driving scenario and display the waypoints of the trajectory.

plot(scenario,Waypoints="on")
while advance(scenario)
    pause(scenario.SampleTime)
end

Define Trajectory of Pedestrian

Define the trajectory of a pedestrian who takes a sharp right turn at an intersection.

Create a driving scenario. Add road segments that define an intersection.

scenario = drivingScenario;
roadCenters = [0 10; 0 -10];
road(scenario,roadCenters);
road(scenario,flip(roadCenters,2));

Add a pedestrian actor to the scenario.

pedestrian = actor(scenario, ...
    'ClassID',4, ...
    'Length',0.24, ...
    'Width',0.45, ...
    'Height',1.7, ...
    'Position',[-9 0 0], ...
    'RCSPattern',[-8 -8; -8 -8], ...
    'Mesh', driving.scenario.pedestrianMesh, ...
    'Name','Pedestrian');

Define the trajectory of the pedestrian. The pedestrian approaches the intersection, pauses briefly, and then takes a sharp right turn at the intersection. To define the sharp right turn, specify two waypoints at the intersection that are close together. For these waypoints, specify the yaw orientation angle of the second waypoint at a 90-degree angle from the first waypoint.

waypoints = [-9 0; -0.25 0; 0 -0.25; 0 -9];
speed = [1.5; 0; 0.5; 1.5];
yaw =  [0; 0; -90; -90];
waittime = [0; 0.2; 0; 0];
trajectory(pedestrian,waypoints,speed,waittime,'Yaw', yaw);

Plot the driving scenario and display the waypoints of the pedestrian.

plot(scenario,'Waypoints','on')
while advance(scenario)
    pause(0.001)
end

Create Trajectory Using Actor Arrival Times

Since R2024b

Create a driving scenario object.

scenario = drivingScenario;

Add road and lane segments to the driving scenario.

roadCenters = [18.3 17.94 0;
              34.8 0.45 0;
              19.1 -21.74 0];
laneSpecification = lanespec(2,Width=5);
road(scenario,roadCenters,Lanes=laneSpecification,Name="Road")

Add a vehicle to the scenario with waypoints and the corresponding times of actor arrival. Create a trajectory using the actor arrival times.

egoVehicle = vehicle(scenario, ...
    ClassID=2, ...
    Length=8.2, ...
    Width=2.5, ...
    Height=3.5, ...
    Position=[21.4185032445924 18.4409098505372 0.01], ...
    RearOverhang=1, ...
    FrontOverhang=0.9, ...
    Mesh=driving.scenario.truckMesh, ...
    Name="Truck");
waypoints = [21.4185032445924 18.4409098505372 0.01;
            29.9 12.34 0.01;
            35.3 5.85 0.01;
            36.99 0.75 0.01;
            36.4 -6.25 0.01;
            33.2 -12.24 0.01;
            28.6 -17.14 0.01];
actorArrivalTime = [0 0.2 0.5 0.8 1 1.3 1.5]';
trajectory(egoVehicle,waypoints,TimeOfArrival=actorArrivalTime)

Plot the scenario, and run the simulation.

plot(scenario,Waypoints="on",RoadCenters="on")
while advance(scenario)
    pause(0.1)
end

Create Trajectory with Front-Axle Steering for Long Vehicle

Since R2024b

Create a driving scenario object.

scenario1 = drivingScenario(StopTime=2.75);

Add road and lane segments to the driving scenario.

laneSpecification = lanespec(2,Width=5);
roadCenters = [27 24 0; 
              27 -3 0;
              47 16 0];
road(scenario1,roadCenters,Lanes=laneSpecification);

Specify the vehicle parameters, along with its waypoints and relative speed values.

egoCar1 = vehicle(scenario1,ClassID=1,Position=[22 19 0], ...
    PlotColor=[0.494 0.184 0.556],Name="Car1",Length=10,FrontOverhang=1.8);
waypoints = [22 19 0; 
            19 13 0; 
            19 6 0; 
            20 2 0; 
            33 -6 0; 
            48 10 0];
speed = 25;

Create a trajectory for the vehicle, with reference to the front-axle for steering.

trajectory(egoCar1,waypoints,speed,ReferenceSteerPosition="front-axle")

Create an another driving scenario object.

scenario2 = drivingScenario(StopTime=2.75);

Add the same road and lane segments to the driving scenario which you added to the previous driving scenario.

road(scenario2,roadCenters,Lanes=laneSpecification);

Specify the same vehicle parameters, along with its waypoints and relative speed values which you added to the previous driving scenario.

egoCar2 = vehicle(scenario2,ClassID=1,Position=[22 19 0], ...
    PlotColor=[0.466 0.674 0.188],Name="Car2",Length=10,FrontOverhang=1.8);

Create a trajectory for the vehicle, with reference to the default rear-axle for steering.

trajectory(egoCar2,waypoints,speed)

Plot and simulate the scenarios, displaying the front-axle and rear-axle trajectories. Observe the first subplot contains a dashed line trajectory for the rear axle, created with reference to the front-axle steering.

fig = figure(Visible="on"); % Ensure the figure is visible
axesWidth  = 0.35;          % Relative width of each axes
axesHeight = 0.8;           % Relative height of each axes
margin     = 0.1;           % Margin from left/right and between axes

% Calculate axes positions of each subplot for equal spacing
pos1 = [margin 0.1 axesWidth axesHeight];
pos2 = [1-(margin+axesWidth) 0.1 axesWidth axesHeight];

% Specify title to the first axes in the figure
ax1 = axes(fig,Position=pos1);
title(ax1,"Trajectory with Front-Axle Steering")

% Specify title to the second axes in the figure
ax2 = axes(fig,Position=pos2);
title(ax2,"Trajectory with Rear-Axle Steering")

% Plot and simulate the scenarios
plot(scenario1,Waypoints="on",RoadCenters="on",ShowRearAxle="on",Parent=ax1)
plot(scenario2,Waypoints="on",RoadCenters="on",ShowRearAxle="on",Parent=ax2)
while advance(scenario1) && advance(scenario2)
    pause(0.1)
end

Input Arguments

ac — Actor

Actor object | Vehicle object

Actor belonging to a drivingScenario object, specified as an Actor or Vehicle object. To create these objects, use the actor and vehicle functions, respectively.

waypoints — Trajectory waypoints

real-valued N-by-2 matrix | real-valued N-by-3 matrix

Trajectory waypoints, specified as a real-valued N-by-2 or N-by-3 matrix, where N is the number of waypoints.

• If waypoints is an N-by-2 matrix, then each matrix row represents the (x, y) coordinates of a waypoint. The z-coordinate of each waypoint is zero.

• If waypoints is an N-by-3 matrix, then each matrix row represents the (x, y, z) coordinates of a waypoint.

Waypoints are in the world coordinate system. Units are in meters.

Example: [1 0 0; 2 7 7; 3 8 8]

Data Types: single | double

speed — Actor speed

30.0 | real-valued scalar | N-element real-valued vector

Actor speed at each waypoint in waypoints, specified as a real-valued scalar or N-element real-valued vector. N is the number of waypoints.

• When speed is a scalar, the speed is constant throughout the actor motion.

• When speed is a vector, the vector values specify the speed at each waypoint. For forward motion, specify positive speed values. For reverse motion, specify negative speed values. To change motion directions, separate the positive speeds and negative speeds by a waypoint with 0 speed.

Speeds are interpolated between waypoints. speed can be zero at any waypoint but cannot be zero at two consecutive waypoints. Units are in meters per second.

Example: [10 8 9] specifies speeds of 10 m/s, 8 m/s, and 9 m/s.

Example: [10 0 -10] specifies a speed of 10 m/s in forward motion, followed by a pause, followed by a speed of 10 m/s in reverse.

Data Types: single | double | int8 | int16 | int32 | int64 | uint8 | uint16 | uint32 | uint64

waittime — Pause time for actor

0 (default) | N-element vector of nonnegative values

Pause time for the actor, specified as an N-element vector of nonnegative values. N is the number of waypoints. When you specify a pause time for the actor at a particular waypoint, you must set the corresponding speed value to 0. You can set the waitime to 0 at any waypoint, but you cannot set waittime at two consecutive waypoints to non-zero values. Units are in seconds.

Data Types: single | double

timeOfArrival — Time of actor arrival

N-element real-valued column vector

Since R2024b

Time of actor arrival, specified as an N-element real-valued column vector. N is the number of waypoints. Units are in seconds.

Example: [1 1.2 1.5] specifies actor arrival times of 1 s, 1.2 s, and 1.5 s.

Data Types: single | double | int8 | int16 | int32 | int64 | uint8 | uint16 | uint32 | uint64

Name-Value Arguments

Specify optional pairs of arguments as Name1=Value1,...,NameN=ValueN, where Name is the argument name and Value is the corresponding value. Name-value arguments must appear after other arguments, but the order of the pairs does not matter.

Before R2021a, use commas to separate each name and value, and enclose Name in quotes.

Example: trajectory(vehicle,waypoints,Course=course,ReferenceSteerPosition="front-axle") creates a trajectory for a vehicle, vehicle, from a set of waypoints, waypoints, using course angles at each waypoint, course, with reference to the front-axle for steering.

Course — Course angle

N-element real-valued column vector

Since R2024b

Course angle specifying the steering direction of the actor, specified as an N-element real-valued column vector, where N is the number of waypoints. Units are in degrees, and angles are positive in the counterclockwise direction.

If you do not specify Course, then the course angle at each waypoint is NaN, meaning that the course has no constraints.

Example: [0 90] specifies an actor at a 0-degree angle at the first waypoint and a 90-degree angle at the second waypoint.

Example: [0 NaN] specifies an actor at a 0-degree angle at the first waypoint. The actor has no constraints on its course at the second waypoint.

Data Types: single | double

Yaw — Yaw orientation angle of actor

N-element real-valued vector

Yaw orientation angle of the actor at each waypoint, specified as an N-element real-valued vector, where N is the number of waypoints. Units are in degrees and angles are positive in the counterclockwise direction.

If you do not specify yaw, then the yaw at each waypoint is NaN, meaning that the yaw has no constraints.

Example: [0 90] specifies an actor at a 0-degree angle at the first waypoint and a 90-degree angle at the second waypoint.

Example: [0 NaN] specifies an actor at a 0-degree angle at the first waypoint. The actor has no constraints on its yaw at the second waypoint.

Data Types: single | double

InitialHeading — Heading angle of actor at first waypoint

scalar

Since R2024b

Heading angle of the actor at the first waypoint, specified as a scalar. Units are in degrees.

If you do not specify InitialHeading, the function sets this value to align with the direction of the initial waypoint.

Note

To use the InitialHeading name-value argument, you must specify the ReferenceSteerPosition name-value argument as "front-axle".

Data Types: single | double

MotionDirection — Direction in which actor is moving

N-element numeric column vector | N-element string array

Since R2024b

Direction in which the actor is moving, specified as an N-element numeric column vector or an N-element string array. N is the number of waypoints.

• If you specify a numeric vector, use 1, -1, and 0 to represent forward, reverse, and stop, respectively.

• If you specify a string array, use "Forward", "Reverse", and "Stopped" to represent forward, reverse, and stop, respectively.

• By default, the MotionDirection value for each waypoint is "Forward".

• For an actor to perform reverse movement, you must stop the actor before reversing its direction. For example, to perform reverse movement in the form forward-stop-reverse, you must specify [1 0 -1].

Note

To use the MotionDirection name-value argument, you must specify the timeOfArrival input argument.

Example: [1 0 -1] specifies actor directions of forward, stop, and reverse at the first, second, and third waypoints, respectively.

Example: ["Forward","Stopped","Reverse"] specifies actor directions of forward, stop, and reverse at the first, second, and third waypoints, respectively.

Data Types: single | double | int8 | int16 | int32 | int64 | string

ReferenceSteerPosition — Reference steering position

"rear-axle" (default) | "front-axle"

Since R2024b

Reference steering position of the vehicle, specified as "rear-axle" or "front-axle". The function creates a trajectory for the actor ac with the steering location specified by this argument.

If you specify "front-axle", the input actor ac must be a Vehicle object. To create a Vehicle object, use the vehicle function. To accurately model vehicle dynamics for front-axle steering during turns, you can specify an actor as a bicycle model or a long vehicle. For more information, see the Create Trajectory with Front-Axle Steering for Long Vehicle example.

This figure displays the steering locations on a vehicle for front-axle steering and rear-axle steering.

Example: "front-axle" creates a trajectory for a vehicle from a set of waypoints with reference to the front axle for steering.

Data Types: string

Tips

• To get the position of the vehicle with reference to a specific vehicle body location, use the getReferencePosition function.

• If a driving scenario contains a front-axle trajectory for at least one vehicle, you can get the front-axle position from the FrontAxlePosition field of the ActorPoses structure returned by the record function.

• If a driving scenario contains a front-axle trajectory, to plot both the rear-axle and front-axle trajectories, specify the ShowRearAxle name-value argument of the plot function as "on".

Algorithms

The trajectory function creates a trajectory for an actor to follow in a scenario. A trajectory consists of the path followed by an object and its speed along the path. You specify the path using N two-dimensional or three-dimensional waypoints. Each of the N – 1 segments between waypoints defines a curve whose curvature varies linearly with distance along the segment. The function fits a piecewise clothoid curve to the (x, y) coordinates of the waypoints by matching the curvature on both sides of the waypoint. For a nonclosed curve, the curvature at the first and last waypoint is zero. If the first and last waypoints coincide, then the curvatures before and after the endpoints are matched. The z-coordinates of the trajectory are interpolated using a shape-preserving piecewise cubic curve.

The generated trajectory results in a piecewise constant-acceleration profile for each segment between waypoints. These segments have acceleration discontinuities between them. To avoid discontinuities in acceleration, use the smoothTrajectory function to generate trajectories instead.

Version History

Introduced in R2018a

R2024b: Additional input arguments

You can now specify the timeOfArrival input argument to create a trajectory based on actor arrival times at each waypoint.

The trajectory function now supports these additional name-value arguments:

• MotionDirection

• ReferenceSteerPosition

• Course

• InitialHeading

R2024b: Yaw name-value argument to be removed

The Yaw name-value argument will be removed in a future release. Use the Course name-value argument instead.