This example shows how to simulate an automated lane change maneuver system for highway driving scenario.
In this example, you will:
Perform trajectory planning to automate lane change maneuver.
Deploy a path following controller to steer the vehicle along the generated trajectory.
Test the lane change maneuver in a closed-loop Simulink® model using prebuilt scenarios generated by the Automated Driving Toolbox™.
An automated lane change maneuver (LCM) system assists the ego vehicle to automatically move from one lane to another lane (with the same direction of travel) in a highway. The LCM system models the longitudinal and lateral control dynamics for automated lane change on a straight road segment. The high-level block diagram of the LCM system is as shown in the figure below
The LCM system dynamically generates an optimal trajectory for lane change while avoiding collision with other actors in the scenario. The LCM system steers the ego vehicle to travel along the optimal trajectory. The LCM system test bench model focuses on the planner and the controller aspects of a lane change system. The model comprises of three main subsystems
Planner and Controller subsystem generates the optimal trajectory for lane change and sends a control command to steer the ego vehicle.
Vehicle and Environment subsystem models the environment and the motion of the ego vehicle. The subsystem receives control command from the Planner and Controller subsystem and updates the state for the ego vehicle.
Metric Assessment subsystem validates the state of the ego vehicle for collision avoidance.
To open the Simulink test bench model, use the following command.
Opening this model runs the
exampleHelperLaneChangeSetup script, which initializes the vehicle model parameters, controller design parameters, and the road scenario. The script also initializes the buses required for defining the inputs into and outputs for the LCM Simulink model.
Open the planner and controller model under test.
The planner and the controller subsystem comprise of a Motion Planner and a Path Following Controller that simulates trajectory planning and motion control of the ego vehicle respectively.
A) Motion Planner
The Motion Planner performs trajectory planning by using the lane information, ego data, and the non-ego actors' data from the Vehicle and Environment subsystem block. The Motion Planner consists of:
Find MIOs function block
Update Global Plan and Deviation function block
Trajectory Generator subsystem
Trajectory to Path function block
Path Analyzer subsystem
The Find MIOs MATLAB function block finds the most important objects (MIOs) in the scenario. MIOs refer to the non-ego actors that lie in the vicinity of the ego vehicle. The MIOs can be located in front or rear of the ego-vehicle along its current lane and the adjacent lanes.
The Update Global Plan and Deviation MATLAB function block calculates the desired deviation from the reference path and updates the global plan to prevent frequent lane changes and maintain the lateral deviation. The global plan updates based on the deviation. A positive offset value means the ego vehicle is offset to the right of the reference path. A negative offset value means the ego vehicle is offset to the left of the reference path.
The Trajectory Generator subsystem generates an optimal trajectory based on MIOs, reference path, and the specified deviation. MIOs are used to update the state validator. The state validator object and the reference path waypoints are passed as inputs to the
trajectoryOptimalFrenet function. The
trajectoryOptimalFrenet function generates multiple trajectories. Sampled trajectories are validated for dynamic collision using the state validator. The subsystem identifies the optimal trajectory based on different weights and set properties of
The Trajectory to Path MATLAB function block extracts path information from the generated trajectory.
The Path Analyzer subsystem estimates the heading angle and finds the appropriate point on the path to follow. The generated path must conform to the road shape.
B) Path Following Controller
The Path Following Controller simulates a path-following control (PFC) mechanism that keeps the ego vehicle traveling along the generated trajectory while tracking a set velocity. To do so, the controller adjusts both the longitudinal acceleration and front steering angle of the ego vehicle. The controller computes optimal control actions while satisfying velocity, acceleration, and steering angle constraints using adaptive model predictive control (MPC). The controller comprises of:
A Virtual Lane Center subsystem - creates a virtual lane from the path point. The virtual lane matches the format required by the path following controller.
A Preview Curvature subsystem - converts trajectory to curvature input required by path following controller.
A Lane Following Controller subsystem - uses the Path Following Control System block from the Model Predictive Control Toolbox.
The Lane Following Controller keeps the vehicle traveling within a marked lane of a highway while maintaining a user-set velocity or safe distance from the preceding vehicle. The lane following controller includes combined longitudinal and lateral control of the ego vehicle:
Longitudinal control - Maintain a user-set velocity and keep a safe distance from the preceding car in the lane by adjusting the acceleration of the ego vehicle.
Lateral control - Keep the ego vehicle travelling along the centerline of its lane by adjusting the steering of the ego vehicle.
The Vehicle and Environment subsystem enables closed-loop simulation of the path following controller.
open_system('LaneChangeTestBench/Vehicle and Environment')
The System Latency blocks model the latency in the system between model inputs and outputs.
The Vehicle Dynamics subsystem models the vehicle dynamics using a Bicycle Model - Force Input block from the Vehicle Dynamics Blockset™. The lower-level dynamics are modeled by a first-order linear system with a time constant of seconds.
The SAE J670E to ISO 8855 subsystem converts the coordinates from Vehicle Dynamics, which uses SAE J670E, to Scenario Reader, which uses ISO 8855.
The Scenario Reader block reads data from a scenario file created using the Driving Scenario Designer app. To change the scenario file, double-click on the Scenario Reader block and open the block parameters tab.
Set Source of driving scenario parameter to
From File and click on Browse to select the desired scenario file. The Scenario Reader block reads the actor data and the lane information from the scenario file. The block converts the actor poses from the world coordinates of the scenario into ego vehicle coordinates.
The Vision Detection Generator block takes lane boundaries from the Scenario Reader block and generates detections from simulated actor poses. All detections are referenced to the coordinate system of the ego vehicle. The detection generator models the field of view of a monocular camera and determines the heading angle, curvature, curvature derivative, and valid length of each lane boundary, accounting for any other obstacles.
The Metric Assessment subsystem validates that the ego vehicle does not collide with other vehicles. The Metric Assessment subsystem halts lane change maneuvering and stops the simulation if the system detects a collision of ego vehicle with target vehicles.
Run the LCM simulation model system to visualize the behavior of the system during a lane change. The Visualization block in the model creates a bird’s eye plot that displays the lane information, ego vehicle, ego trajectory, and other vehicles in the scenario.
% Turn off messages from MPC. mpcverbosity('off'); % Run the model for 3 secs. sim("LaneChangeTestBench","StopTime","3");
Run the simulation for 9 seconds and notice that a trajectory is calculated to navigate around a slower lead vehicle.
% Turn off messages from MPC. mpcverbosity('off'); % Run the model for 3 secs. sim("LaneChangeTestBench","StopTime","9");
Run the simulation for 14 seconds and notice that the vehicle continues straight ahead in the left lane.
% Turn off messages from MPC. mpcverbosity('off'); % Run the model for 3 secs. sim("LaneChangeTestBench","StopTime","14");
Explore the other test scenarios in the example by changing the
scenarioId and running the
exampleHelperLaneChangeSetup script. This script updates the scenario reader block with the selected scenario and reinitializes the buses with respect to the selected scenario. Save and run the model to explore the behavior. Updating the scenario from scenario reader block needs updating the
scenarioId from the command line and re-run the
exampleHelperLaneChangeSetup script to reinitialize buses required by the model.
This example showed how to simulate a highway lane change maneuver using ideal vehicle positions and lane detections.