Lateral Offset Follower with High-Fidelity Vehicle Dynamics

This example shows how to design and implement a lateral offset action follower in Simulink® and cosimulate it with RoadRunner Scenario. The lateral offset action 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 lateral offset action follower in Simulink. This figure shows an overview of the information exchanged between RoadRunner Scenario and the lateral offset action follower.

RoadRunner Scenario communicates with the lateral offset action follower using message-based communication. The lateral offset action follower gets the ego path from the path action message, uses the lateral offset action messages to add an offset to the ego path, and uses the speed action messages to update the speed of the ego vehicle. To navigate the ego vehicle along the path, the lateral offset action follower implements a controller and vehicle dynamics variant subsystem using 3DOF and 14DOF variants of vehicle dynamics and updates the ego pose using the self actor runtime message.

In this example, you:

• Set Up Environment — Open RoadRunner and copy the required files to the project folder.

• Explore Scenario — Explore how the scenario defines actions for actors.

• Simulate Scenario with Built-In Behavior — Simulate the scenario in which the ego vehicle uses the built-in behavior of RoadRunner Scenario.

• Design Lateral Offset Action Follower — Design a lateral offset action follower using Simulink.

• Simulate Model with 3DOF Vehicle Dynamics — Configure the lateral offset action follower to use 3DOF vehicle dynamics. Cosimulate the scenario and inspect simulation results.

• Simulate Model with 14DOF Vehicle Dynamics — Configure the lateral offset action follower to use 14DOF vehicle dynamics. Cosimulate the scenario and inspect simulation results.

Set Up Environment

This section shows how to set up the environment to cosimulate a lateral offset action follower with RoadRunner Scenario.

roadrunnerSetup function enables you to start the RoadRunner application interactively. For more information, see roadrunnerSetup. Open RoadRunner using roadrunnerSetup function.

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 this object, see roadrunner

This example uses these files, which you must add to the RoadRunner project.

• scenario_LOF_LeadBreakDown.rrscenario — Scenario file based on the ScenarioBasic.rrscene scene included with RoadRunner.

• LateralOffsetFollower.rrbehavior.rrmeta — Behavior file that associates the lateral offset action follower behavior, implemented using the Simulink model, to the ego vehicle in RoadRunner Scenario.

Copy these files to the RoadRunner project.

copyfile("scenario_LOF_LeadBreakDown.rrscenario",fullfile(rrApp.status.Project.Filename,"Scenarios"))
copyfile("LateralOffsetFollower.rrbehavior.rrmeta",fullfile(rrApp.status.Project.Filename,"Assets/Behaviors/"))

Explore Scenario

Open the scenario scenario_LOF_LeadBreakDown.rrscenario.

openScenario(rrApp,"scenario_LOF_LeadBreakDown.rrscenario")

The scenario contains three vehicles. The orange lead car and the red car follow the lane-following built-in behavior. The blue ego car travels on a specified path. The lead car initially travels at a speed of 10 m/s following the center of the ego lane. Due to a break down, the lead car shifts to the right in its lane by one meter and comes to rest. The red car travels at a speed of 4 m/s in the left adjacent lane of the ego vehicle. The ego vehicle initially travels at a speed of 0.5 m/s, and then it accelerates to 5 m/s. Because the left adjacent lane is blocked by the red car, the ego vehicle shifts laterally to the left by 1.5 meters to avoid collision with the lead car. Then, the ego vehicle returns to the original path after passing the lead car. RoadRunner Scenario implements this behavior by using a Change Lateral Offset action. For more information on the Change Lateral Offset action, see Change Lateral Offset Actions (RoadRunner Scenario).

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

In this example, you implement a lateral offset action following behavior for the ego vehicle using a Simulink model. In the Library Browser, open the Assets folder, then the Behaviors folder, and select the LateralOffsetFollower.rrbehavior.rrmeta file. The Attributes pane shows that the behavior file points to a Simulink model, LateralOffsetFollowerTestBench.slx.

Specify the behavior for the ego vehicle using the egoBehavior variable.

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

rrSim = createSimulation(rrApp);
set(rrSim,Logging="on")

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

Simulate Scenario with Built-In Behavior

When you do not specify egoBehavior, the ego vehicle uses the built-in behavior of the RoadRunner scenario. Clear the egoBehavior variable to use the built-in behavior.

setScenarioVariable(rrApp,"egoBehavior"," ")

Run the simulation and wait for the simulation to complete.

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

Use the helperPlotLOFResults helper function to visualize the ego vehicle trajectory. The helperPlotLOFResults function takes rrSim and the ego actor ID as input.

hFigPath = helperPlotLOFResults(rrSim,1);

The ego trajectory shows that the ego vehicle follows the lateral offset, specified using the Change Lateral Offset action, to avoid collision with the lead vehicle.

Set the visibility of the figure to off.

set(hFigPath,Visible="off")

Design Lateral Offset Action Follower

This example uses the LateralOffsetFollowerTestBench model to define the custom behavior of the ego vehicle. This model uses a Stanley controller and vehicle dynamics variant subsystem to control the ego vehicle. Open the test bench model.

open_system("LateralOffsetFollowerTestBench")

The test bench model contains RoadRunner Scenario blocks, which configure, read from, and write to RoadRunner Scenario, as well as these modules:

• Lateral Offset Action Adapter — System object™ that processes the Change Lateral Offset action specified in RoadRunner Scenario and updates the reference path of the ego vehicle.

• Speed Action Adapter — System object that processes the Change Speed action specified in RoadRunner Scenario and calculates the reference speed of the ego vehicle.

• Reference Pose on Path — System object that calculates the reference pose of the ego vehicle.

• Stanley Controller — Subsystem that uses a Lateral Controller Stanley block for steering angle control and Longitudinal Controller Stanley block for acceleration control.

• Vehicle Dynamics — Variant subsystem that specifies the vehicle dynamics of the ego vehicle.

• Pack Ego Pose — Subsystem that packs the ego vehicle runtime information to write to RoadRunner Scenario.

• ENU to NWD — Subsystem that transforms the ego vehicle pose from the RoadRunner Scenario coordinate system to the cuboid coordinate system.

• NWD to ENU — Subsystem that transforms the ego vehicle pose from the cuboid coordinate system to the RoadRunner Scenario coordinate system.

The Speed Action Adapter, Reference Pose on Path, Stanley Controller, Pack Ego Pose, ENU to NWD, and NWD to ENU subsystems are based on the modules used in the Trajectory Follower with RoadRunner Scenario example. This example focuses on the RoadRunner Scenario blocks, the Lateral Offset Action Adapter subsystem, and the Vehicle Dynamics subsystem.

RoadRunner Scenario Blocks

The model uses RoadRunner Scenario Reader blocks to read messages from RoadRunner Scenario and the RoadRunner Scenario Writer block to write the ego vehicle runtime message to RoadRunner Scenario.

This example uses these RoadRunner Scenario blocks:

• RoadRunner Scenario — Defines the interface for an actor model.

• Path Action — Reads the path of the ego vehicle.

• Lateral Offset Action — Reads the Change Lateral Offset actions of the ego vehicle.

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

• Self Vehicle Runtime — Reads ego vehicle runtime information.

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

• Self Actor Runtime Writer — Writes the ego vehicle runtime information to RoadRunner Scenario.

Lateral Offset Action Adapter

The lateral offset action adapter processes Change Lateral Offset action commands specified in RoadRunner Scenario. This example implements the lateral offset action adapter using the HelperLateralOffsetActionAdapter System object, which reads the ego path, ego vehicle runtime message, and Change Lateral Offset action message from RoadRunner Scenario. Then, the System object calculates the required lateral offset for the ego vehicle and updates the ego path using the calculated lateral offset. The System object uses this primary interface:

[UpdatedPath,UpdatedNumTrajPoints] =
stepImpl(obj,Trajectory,NumTrajPoints,LongitudinalVelocity,VehicleRuntime,LateralOffsetAction)

• Trajectory — Trajectory of the ego vehicle specified in RoadRunner Scenario.

• NumTrajPoints — Number of points in the trajectory of the ego vehicle.

• LongitudinalVelocity — Longitudinal velocity of the ego vehicle.

• VehicleRuntime — Runtime information of the ego vehicle in RoadRunner Scenario.

• LateralOffsetAction — Change Lateral Offset action command from RoadRunner Scenario.

• UpdatedPath — Updated trajectory of the ego vehicle with the lateral offset.

• UpdatedNumTrajPoints — Number of points in the updated trajectory.

Vehicle Dynamics

The Vehicle Dynamics variant subsystem contains two vehicle variants.

• Vehicle Dynamics 3DOF

• Vehicle Dynamics 14DOF (default)

Each of these variants takes the steering angle, acceleration command, deceleration command, and initial states as input and outputs the ego vehicle information and longitudinal velocity of the ego vehicle. Open the Vehicle Dynamics variant subsystem.

open_system("LateralOffsetFollowerTestBench/Vehicle Dynamics")

You can configure the desired variant using the helperSLLateralOffsetFollowerSetup helper function. The default variant is Vehicle Dynamics 14DOF.

Explore 3DOF Variant

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

helperSLLateralOffsetFollowerSetup(vehicleVariantName="3DOF")
pause(3)
open_system("LateralOffsetFollowerTestBench/Vehicle Dynamics/Vehicle Dynamics 3DOF")

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).

Explore 14DOF Variant

The Vehicle Dynamics 14DOF variant implements transmission controls, tires, load transfer, braking, and powertrain response for a vehicle. Open the Vehicle Dynamics 14DOF variant.

helperSLLateralOffsetFollowerSetup(vehicleVariantName="14DOF")
pause(3)
open_system("LateralOffsetFollowerTestBench/Vehicle Dynamics/Vehicle Dynamics 14DOF")

The Vehicle Dynamics 14DOF subsystem contains the VehDyn14DOF reference model, which has the Input Routing, Driver Commands, Controllers, and Environment subsystems. Open the VehDyn14DOF reference model.

open_system("VehDyn14DOF")

The VehDyn14DOF reference model has a Passenger Vehicle subsystem that contains the Engine, Steering and Driveline, Pedal cluster and Cabin, and Chassis and Tires subsystems. For more information, see the Autonomous Emergency Braking with High-Fidelity Vehicle Dynamics example.

Simulate Model with 3DOF Vehicle Dynamics

Set the scenario variable egoBehavior to use the Simulink model.

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

Configure the Vehicle Dynamics variant subsystem to use 3DOF vehicle dynamics, and initialize the parameters of the test bench model using the helperSLLateralOffsetFollowerSetup helper function.

helperSLLateralOffsetFollowerSetup(rrApp,rrSim,vehicleVariantName="3DOF")

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

set(rrSim,StepSize=Ts)

Run the simulation and wait for it to complete.

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

Plot the vehicle trajectory and simulation results using the helperPlotLOFResults function. The helperPlotLOFResults helper function also takes a log from the Simulink model, as an optional input, to plot the cosimulation results.

hFigPath = helperPlotLOFResults(rrSim,1,"3DOF",logsout);

Notice that the trajectory followed by the ego vehicle with 3DOF dynamics is similar to the trajectory generated by the built-in behavior. However, you can observe the deviation when lateral offset action is triggered.

Set the visibility of the figure to off.

set(hFigPath,Visible="off")

Simulate Model with 14DOF Vehicle Dynamics

Configure the Vehicle Dynamics variant subsystem to use 14DOF vehicle dynamics using the helperSLLateralOffsetFollowerSetup helper function.

helperSLLateralOffsetFollowerSetup(rrApp,rrSim,vehicleVariantName="14DOF")

This model may take a couple of minutes to update the diagram when you are compiling for the first time. Update the model before running the simulation.

set_param("LateralOffsetFollowerTestBench",SimulationCommand="update");

Run the simulation and wait for it to complete.

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

Plot the simulation results using the helperPlotLOFResults helper function, and compare the performances of the 3DOF and 14DOF vehicle dynamics.

helperPlotLOFResults(rrSim,1,"14DOF",logsout);

The Ego Trajectory and Lane Centers plot compares the ego vehicle trajectory using built-in behavior to that using the different vehicle dynamics.

• From the Longitudinal Results plots, you can observe the acceleration and deceleration input commands and corresponding outputs from the high-fidelity vehicle dynamics model, such as the ego car velocity and longitudinal acceleration. These plots enable you to analyze the longitudinal behavior of the vehicle.

• From the Lateral Results plots, you can observe the steering input commands and corresponding outputs, such as the yaw rate and lateral acceleration. These plots enable you to analyze the lateral behavior of the vehicle.

• From the Vertical Results plots, you can observe the suspension forces on wheels and angular velocity of the vehicle when the vehicle steers. These plots enable you to analyze the vertical behavior of the vehicle.