Trajectory Follower with RoadRunner Scenario

This example shows how to design a trajectory follower in Simulink® and cosimulate it with RoadRunner Scenario. The trajectory follower includes controls and vehicle dynamics.

Introduction

RoadRunner Scenario is an interactive editor that enables you to design scenarios for simulating and testing automated driving systems. You can place vehicles, define their paths and interactions in the scenario, and then simulate the scenario in the editor. RoadRunner Scenario supports in-editor playback for scenario visualization and connecting to other simulators such as MATLAB® and Simulink for cosimulation.

This example shows the steps for cosimulation of RoadRunner Scenario and Simulink. It shows how to design a trajectory follower in Simulink. It also shows how to visualize simulation data using MATLAB. This diagram shows an overview of the information exchanged between RoadRunner Scenario and the trajectory follower:

RoadRunner Scenario communicates with the Simulink trajectory follower using message-based communication. The trajectory follower gets the ego path from the path action message, uses the speed action messages to update the current speed of the ego vehicle, and reads the runtime information of all the actors in the scenario to support relative target speed. To navigate the ego vehicle along the specified trajectory, the trajectory follower implements a controller and vehicle dynamics subsystem and updates the ego pose using the self vehicle runtime message.

In this example, you:

1. Set up Environment — Configure MATLAB to interact with RoadRunner Scenario.

2. Explore scenario — Explore how the scenario defines actions for an actor.

3. Simulate scenario with built-in behavior — Simulate the scenario with the ego vehicle using the RoadRunner Scenario built-in behavior. Inspect the velocity profile using the Simulation Data Inspector (SDI).

4. Design trajectory follower using Simulink — Design a trajectory follower using Simulink.

5. Simulate scenario with trajectory follower — Associate the trajectory following behavior to the ego vehicle in RoadRunner Scenario. Simulate the scenario and inspect the ego velocity profile using the SDI.

Set Up Environment

This section shows how to set up the environment to cosimulate MATLAB with RoadRunner Scenario.

Start RoadRunner application interactively by using the roadrunnerSetup function. When the function opens a dialog box, specify the RoadRunner Project Folder and RoadRunner Installation Folder locations.

rrApp = roadrunnerSetup;

The rrApp RoadRunner object enables you to interact with RoadRunner from the MATLAB workspace. You can open the scenario and update scenario variables using this object. For more information on variables, see Generate Scenario Variations Using gRPC API (RoadRunner Scenario).

This example uses two files that you must add to the RoadRunner project.

• scenario_TF_EgoFollowsSpeed.rrscenario — Scenario file based on the ScenarioBasic.rrscene scene that ships with RoadRunner.

• TrajectoryFollower.rrbehavior.rrmeta — Behavior file that associates the trajectory follower behavior implemented using the Simulink model to the ego vehicle in RoadRunner Scenario.

Copy these files to the RoadRunner project. To learn more about the RoadRunner environment, see RoadRunner Project and Scene System (RoadRunner).

copyfile("scenario_TF_EgoFollowsSpeed.rrscenario",fullfile(rrApp.status.Project.Filename,"Scenarios"));
copyfile("TrajectoryFollower.rrbehavior.rrmeta",fullfile(rrApp.status.Project.Filename,"Assets","Behaviors"));

Explore Scenario

Open the scenario scenario_TF_EgoFollowsSpeed.rrscenario.

openScenario(rrApp,"scenario_TF_EgoFollowsSpeed.rrscenario");

The scenario contains a white ego vehicle that travels on the specified path. In this example, you must specify a path for the ego vehicle. The ego vehicle initially travels at a speed of 5 m/s, and then increases its speed to 15 m/s over a period of 10 seconds.

Visualize the scenario logic in the Logic editor pane. For more information, see Define Scenario Logic (RoadRunner Scenario).

In this example, you implement trajectory following behavior for the ego vehicle using a Simulink model. Select the TrajectoryFollower.rrbehavior.rrmeta file in the Library Browser. The Attributes pane shows that the behavior file points to a Simulink model, TrajectoryFollowerRRTestBench.slx.

Specify the behavior for the ego vehicle by adding the path to the TrajectoryFollower.rrbehavior.rrmeta file to the egoBehavior variable value. To do this, select the ego vehicle and, in the Library Borwser, drag the TrajectoryFollower.rrbehavior.rrmeta file to the behavior field in the Attributes pane. To learn more about variables in RoadRunner Scenario, see Scenario Variables (RoadRunner Scenario).

Connect to the RoadRunner Scenario server for cosimulation by using the createSimulation function, and enable data logging.

rrSim = rrApp.createSimulation;
rrSim.set('Logging','on');

Connection status: 1
Connected to RoadRunner Scenario server on localhost:54322, with client id {59b2ae36-9e89-43c0-ac97-3a9a5972211e}

rrSim is the ScenarioSimulation object. Use this object to set variables and to read scenario-related information.

Simulate RoadRunner Scenario

When egoBehavior is unspecified, the ego vehicle uses the built-in behavior of the RoadRunner scenario. Clear the egoBehavior variable to use the built-in behavior.

rrApp.setScenarioVariable("egoBehavior"," ");

Run the simulation and wait for the simulation to complete.

rrSim.set("SimulationCommand","Start");
while strcmp(rrSim.get("SimulationStatus"),"Running")
    pause(1);
end

Use the helperVisualizeVelocityProfile function to visualize the velocity profile using the SDI. The helperVisualizeVelocityProfile function also plots lane centers and the ego vehicle trajectory. The helperVisualizeVelocityProfile function takes rrSim, the ego actor ID, and the signal name for SDI as inputs.

helperVisualizeVelocityProfile(rrSim,1,"Built-in")
hFigSDI = Simulink.sdi.snapshot;

The velocity profile shows the change in velocity over time. Notice that the ego vehicle follows the Change Speed action specified in the scenario logic.

Close the figure.

close(hFigSDI)

Design Trajectory Follower Using Simulink

This example uses the TrajectoryFollowerRRTestBench model to define the custom behavior of the ego vehicle. This model uses a Stanley controller and 3DOF vehicle dynamics to control the ego speed. Open the test bench model.

open_system("TrajectoryFollowerRRTestBench");

The test bench model performs these tasks:

• Reads data from RoadRunner Scenario.

• Processes the input data.

• Writes the processed data to RoadRunner Scenario.

Read Data from RoadRunner Scenario

The test bench model reads the data from RoadRunner Scenario using these modules:

The model uses RoadRunner Scenario Reader blocks to read messages from RoadRunner Scenario.

This example uses these RoadRunner Scenario Reader blocks:

• Self Vehicle Runtime — Reads the ego vehicle runtime information.

• Speed Action — Reads the Change Speed actions of the ego vehicle.

• All Actor Runtime — Reads the actor runtime information of all vehicles.

The Path Action Reader System object™ reads the specified path of the ego vehicle from RoadRunner Scenario.

The ENU to NWD subsystem converts the RoadRunner Scenario coordinate system to the cuboid coordinate system. For more information on these coordinate systems, see Coordinate Space and Georeferencing (RoadRunner) and Coordinate Systems in Automated Driving Toolbox. Open the ENU to NWD subsystem.

open_system("TrajectoryFollowerRRTestBench/ENU to NWD");

The Speed Action Adapter block processes the speed action commands specified in RoadRunner Scenario and calculates the reference speed of the ego vehicle.

The Initialize State subsystem calculates the initial state of the ego vehicle for vehicle dynamics.

Process Input Data

The test bench model processes the RoadRunner Scenario data using these modules:

The Reference Pose on Path subsystem calculates the reference pose of the ego vehicle on the path using the previous pose of the ego vehicle. The subsystem also calculates the reference curvature of the trajectory using the smoothPathSpline function.

The Stanley Controller subsystem uses a Lateral Controller Stanley block for steering angle control and Longitudinal Controller Stanley block for acceleration control. Open the Stanley Controller subsystem.

open_system("TrajectoryFollowerRRTestBench/Stanley Controller");

To compute the steering angle command, the Lateral Controller Stanley block minimizes the position error and the angle error of the current pose with respect to the reference pose.

In this example, the Lateral Controller Stanley block uses the dynamic bicycle model as a vehicle model for the steering angle control. The dynamic bicycle model is suitable for trajectory following in high-speed environments such as highways, where inertial effects are more pronounced. For more details, see Lateral Controller Stanley.

The Lateral Controller Stanley block in this example supports a maximum steering angle of 30 degrees. Due to this, the ego vehicle may not be able to take some sharp turns.

To compute the acceleration and deceleration commands, the Longitudinal Controller Stanley block implements a discrete proportional-integral (PI) controller. For more details, see Longitudinal Controller Stanley.

The Longitudinal Controller Stanley block in this example supports a maximum acceleration of 2 m/s^2. To get the desired behavior, you must specify a Change Speed action in RoadRunner Scenario with a required acceleration that does not exceed 2 m/s^2.

The Vehicle Dynamics subsystem uses a 3DOF Bicycle Model block to model the ego vehicle. Open the Vehicle Dynamics subsystem.

open_system("TrajectoryFollowerRRTestBench/Vehicle Dynamics");

The Vehicle Body 3DOF block implements a rigid, two-axle, single-track vehicle body model to calculate longitudinal, lateral, and yaw motion. The block accounts for body mass, aerodynamic drag, and weight distribution between the axles due to acceleration and steering. For more details, see Vehicle Body 3DOF Three Axles (Vehicle Dynamics Blockset).

The Lateral Metrics subsystem calculates position error and relative heading to check the performance of the controller and the vehicle dynamics. Position error is the deviation of the ego vehicle from the reference pose. Relative heading is the ego vehicle heading angle relative to reference path.

Write Processed Data to RoadRunner Scenario

The test bench model writes the processed data to RoadRunner Scenario using these modules:

The NWD to ENU subsystem transforms the pose that you get from the cuboid coordinate system to the RoadRunner Scenario coordinate system. Open the NWD to ENU subsystem.

open_system("TrajectoryFollowerRRTestBench/NWD to ENU");

The Pack Actor Pose subsystem converts the pose into a 4-by-4 position matrix using the current pose of the ego vehicle and calculates the velocity components of the vehicle. Open the Pack Actor Pose subsystem.

open_system("TrajectoryFollowerRRTestBench/Pack Actor Pose");

The subsystem writes the vehicle pose to RoadRunner Scenario using the RoadRunner Scenario Writer block.

Simulate Scenario with Trajectory Follower

Set the scenario variable egoBehavior to use the Simulink model.

rrApp.setScenarioVariable("egoBehavior","<PROJECT>/Assets/Behaviors/TrajectoryFollower.rrbehavior");

Initialize parameters of the test bench model using the helperSLTrajectoryFollowerWithRRScenarioSetup function. The MaxPathPoints name-value argument of the function defines the upper limit on the number of path points to read from RoadRunner Scenario. For this example, set the value of MaxPathPoints argument to 5000.

helperSLTrajectoryFollowerWithRRScenarioSetup(MaxPathPoints=5000);

Set the step size of RoadRunner Scenario to match the step size of the TrajectoryFollowerRRTestBench model.

rrSim.set("StepSize",timeStep);

Run the simulation and wait for it to complete.

rrSim.set("SimulationCommand","Start");
while strcmp(rrSim.get("SimulationStatus"),"Running")
  pause(1);
end

Plot the simulation results.

helperVisualizeVelocityProfile(rrSim,1,"SimulinkBehavior");
Simulink.sdi.snapshot;

The first subplot compares the velocity profile of the built-in behavior with that of the trajectory follower. Notice the overshoot in the trajectory follower profile that increases when the ego vehicle travels on a turning road segment. You can adjust the controller parameters to reduce the overshoot. The second subplot plots the position error of the ego vehicle due to controller and vehicle dynamics.