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.
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:
Set up MATLAB — Configure MATLAB settings to interact with RoadRunner Scenario.
Set up RoadRunner Scenario — Open a RoadRunner Scenario project and copy the required files to the project folder.
Explore scenario — Explore how the scenario defines actions for an actor.
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).
Design trajectory follower using Simulink — Design a trajectory follower using Simulink.
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 MATLAB
This section shows how to set up the environment to cosimulate MATLAB with RoadRunner Scenario.
Specify the path to your local RoadRunner installation folder. This code shows the path for the default installation location on Windows®.
rrAppPath = "C:\Program Files\RoadRunner " + matlabRelease.Release + "\bin\win64";
To update the path for the RoadRunner installation folder, get the root object within the settings hierarchical tree. For more information, see
s = settings; s.roadrunner.application.InstallationFolder.TemporaryValue = rrAppPath;
Set Up RoadRunner Scenario
Specify the path to your RoadRunner project. This code shows the path to a sample project folder on Windows.
rrProjectPath = "C:\RR\MyProjects";
Open RoadRunner using the specified path to your project.
rrApp = roadrunner(rrProjectPath);
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.rrscenescene 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).
Open the scenario
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,
Specify the behavior for the ego vehicle using the
Connect to the RoadRunner Scenario server for cosimulation by using the
createSimulation function, and enable data logging.
rrSim = rrApp.createSimulation; rrSim.set('Logging','on');
rrSim is the
Simulink.ScenarioSimulation object. Use this object to set variables and to read scenario-related information.
Simulate RoadRunner Scenario
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.
Run the simulation and wait for the simulation to complete.
rrSim.set("SimulationCommand","Start"); while strcmp(rrSim.get("SimulationStatus"),"Running") pause(1); end
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.
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.
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 Speedactions of the ego vehicle.
All Actor Runtime— Reads the actor runtime information of all vehicles.
Path Action Reader System object™ reads the specified path of the ego vehicle from RoadRunner Scenario.
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");
Speed Action Adapter block processes the speed action commands specified in RoadRunner Scenario and calculates the reference speed of the ego vehicle.
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:
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
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.
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.
Vehicle Dynamics subsystem uses a 3DOF Bicycle Model block to model the ego vehicle. Open the
Vehicle Dynamics subsystem.
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).
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:
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");
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.
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
Set the step size of RoadRunner Scenario to match the step size of the
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.
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.
- Speed Action Follower with RoadRunner Scenario
- Lateral Offset Follower with High-Fidelity Vehicle Dynamics
- Lane-Level Path Planning with RoadRunner Scenario
- Lane-Keeping System for Self-Driving Trucks
- Highway Lane Change Planner with RoadRunner Scenario
- Generate Scenario Variations Using gRPC API (RoadRunner Scenario)