Lateral Offset Follower with High-Fidelity 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

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.

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:

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:

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)

Vehicle Dynamics

The Vehicle Dynamics variant subsystem contains two vehicle variants.

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.