This example shows how to simulate an automated lane change maneuver system for highway driving scenario.
An automated lane change maneuver (LCM) system enables the ego vehicle to automatically move from one lane to another lane. The LCM system models the longitudinal and lateral control dynamics for automated lane change. An LCM system senses the environment for most important objects (MIOs) using on-board sensors, identifies an optimal trajectory that avoids these objects, and steers ego vehicle along this trajectory.
This example shows how to design and test the planner and controller components of an LCM system. In this example, the lane change planner uses ground truth information from the scenario to detect MIOs. It then generates a feasible trajectory to negotiate a lane change that is executed by the lane change controller. In this example, you:
Explore the test bench model: The model contains planning, controls, vehicle dynamics, sensors, and metrics to assess functionality.
Model the lane change planner subsystem: The subsystem contains a simple behavior layer and a motion planner. The behavior layer configures the motion planner to generate an optimal trajectory by considering MIO information.
Model the lane change controller subsystem: The subsystem generates control commands for the ego vehicle based on the generated trajectory.
Simulate and visualize system behavior: The test bench model is configured to test the integration of planning and controls to perform lane change maneuver on a straight road with multiple vehicles.
Explore other scenarios: These scenarios test the system under additional conditions.
You can apply the modeling patterns used in this example to test your own planner and controller components of an LCM system.
In this example, you use a system-level simulation test bench model to explore the behavior of the planner and controller components for a lane change maneuver system. Open the system-level simulation test bench model.
Opening this model runs the
helperSLHighwayLaneChangeSetup script that initializes the road scenario using the
drivingScenario object in the base workspace. It also configures the controller design parameters, vehicle model parameters, and the Simulink bus signals required for defining the inputs and outputs for the
The test bench model contains the following subsystems.
Sensors and Environment specifies the road, vehicles, and sensors used for simulation.
Lane Change Planner specifies behavior and trajectory planning for the ego vehicle.
Lane Change Controller specifies the path following controller that generates control commands to steer the ego vehicle along the generated trajectory.
Vehicle Dynamics models the ego-vehicle using a Bicycle Model and updates its state using commands received from the Lane Change Controller.
Metric Assessment specifies metrics to assess system level behavior.
The Vehicle Dynamics subsystem is based on the subsystem used in the Highway Lane Following (Automated Driving Toolbox) example. This example focuses on the Lane Change Planner and Lane Change Controller subsystems.
The Sensors and Environment subsystem uses Scenario Reader and Vision Detection Generator blocks to provide the road network, vehicle ground truth positions, and lane detections required for the lane change maneuver system. Open the Sensors and Environment subsystem. This subsystem also outputs the reference path required by the lane change planner.
open_system('HighwayLaneChangeTestBench/Sensors and Environment')
The Scenario Reader block is configured to read the
drivingScenario object from the base workspace. It uses this object to read the actor data and lane information. It takes in ego vehicle information to perform a closed-loop simulation. This block outputs ground truth information of lanes and actors in ego vehicle coordinates.
The Vision Detection Generator block provides lane detections with respect to the ego vehicle which help in identifying vehicles present in the ego lane and adjacent lanes.
The Lane Change Planner subsystem uses lane detections from the Vision Detection Generator block and ground-truth actor or vehicle poses from the Scenario Reader to perform trajectory planning for the automated lane change maneuver.
The Lane Change Planner subsystem uses a Simple Behavior Layer to configure the Motion Planner. Motion Planner is responsible for generating the trajectory for a lane change maneuver. Open the Lane Change Planner subsystem.
open_system('HighwayLaneChangeTestBench/Lane Change Planner')
The Find MIOs subsystem finds the most important objects with respect to the current state of the ego vehicle.
The Simple Behavior Layer subsystem specifies the planner behavior for the Motion planner.
The Motion Planner subsystem uses
trajectoryOptimalFrenet and MIO information to perform trajectory planning.
The Find MIOs subsystem uses ground-truth vehicle poses to compute MIO information with respect to the ego vehicle. The vehicles present in the front or rear of the ego vehicle are considered as MIOs. The MIOs can also be in adjacent lanes as shown in the following figure.
The Simple Behavior Layer subsystem configures planner behavior by computing the terminal states, weights, and deviation values using the current ego vehicle state information. Open the Simple Behavior Layer subsystem.
open_system('HighwayLaneChangeTestBench/Lane Change Planner/Simple Behavior Layer')
The EstimateCurrentEgoState block uses a System object™,
HelperEstimateCurrentEgoState, to compute the lateral and longitudinal positions of the ego vehicle. This computation is done in Frenet space using the current state of the ego vehicle and lane detections. It also computes information about adjacent lane availability (NoLeftLane, NoRightLane) with respect to ego vehicle position and deviation offset from the reference path.
The LongitudinalSampler block defines the longitudinal planning horizon, as shown in the following figure. It configures the possible longitudinal terminal state for defining the planner behavior based on the distance to the input longitudinal position. The input longitudinal position is calculated based on the distance to the goal point.
The LateralSampler block defines the lateral planning horizon, as shown in the following figure. It configures the possible lateral terminal states for defining the planner behavior based on adjacent lane availability (NoLeftLane, NoRightLane).
The VelocitySampler block configures the possible velocity terminal states for defining the planner behavior. It is set to a default configuration in this model.
The AccelerationSampler block configures the possible acceleration terminal states for defining the planner behavior. It is set to a default configuration in this model.
The Weights bus defines the attributes that can be configured to vary the trajectory profile generated by the planner. These are set to default values in this model.
The Motion Planner reference model generates a trajectory by using the planner behavior, scenario information, and MIO information. Open the Motion Planner reference model.
The Pulse Generator block defines a replan period for the Trajectory Generator block. The default value is set to 1 second.
The Trajectory Generator block generates an optimal trajectory based on MIOs, the reference path, and the specified planner behavior. This block uses a system object,
HelperTrajectoryGenerator, to generate the required trajectory. This System object is implemented using
trajectoryOptimalFrenet. It uses MIO information updated using a state validator. Multiple trajectory samples are validated for dynamic collision using the state validator and the optimal trajectory is identified.
The Extract States block extracts path information from the generated trajectory.
The Path Analyzer block estimates the heading angle and finds the appropriate point on the path to follow. The generated path must conform to the road shape. This reference point on the path is used by the Lane Change Controller subsystem.
The Lane Change Controller reference model simulates a path following control 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). Open the Lane Change Controller reference model.
The Virtual Lane Center subsystem creates a virtual lane from the path point. The virtual lane matches the format required by the Path Following Controller block.
The Preview Curvature subsystem converts trajectory to curvature input required by Path Following Controller.
The Path Following Controller block uses the Path Following Control System block from the Model Predictive Control Toolbox™.
The Path Following Controller block keeps the vehicle traveling within a marked lane of a highway while maintaining a user-set velocity. This controller includes combined longitudinal and lateral control of the ego vehicle:
Longitudinal control maintains a user-set velocity of the ego vehicle.
Lateral control keeps the ego vehicle traveling along the center line of its lane by adjusting the steering of the ego vehicle.
The Metric Assessment subsystem assesses system level behavior of the LCM system using the metrics mentioned below. Open the Metrics Assessment subsystem.
The DetectCollision block detects the collision of the ego vehicle with other vehicles and halts the simulation if a collision is detected.
The DetectLeadVehicle block computes the headway between the ego and lead vehicles, which is used for computing the TimeGap.
The TimeGap is calculated using the distance to the lead vehicle (headway) and the longitudinal velocity of the ego vehicle, and it is evaluated against prescribed limits.
The LongitudinalJerk is calculated using the longitudinal velocity and evaluated against prescribed limits.
The LateralJerk value is calculated using the lateral velocity evaluated against prescribed limits.
Set up and run the
HighwayLaneChangeTestBench simulation model to visualize the behavior of the system during a lane change. The 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. Configure the
HighwayLaneChangeTestBench model to use
Simulate the model for 5 seconds. To reduce command-window output, first turn off the MPC update messages.
Run the simulation for 8 seconds. A trajectory is calculated to navigate around a slower lead vehicle.
Run the simulation for 13 seconds. The vehicle continues straight ahead in the left lane.
In the previous section, you explored the system behavior for the
scenario_LC_06_DoubleLaneChange scenario. Below is a list of scenarios that are compatible with the
scenario_LC_01_SlowMoving scenario_LC_02_SlowMovingWithPassingCar scenario_LC_03_DisabledCar scenario_LC_04_CutInWithBrake scenario_LC_05_SingleLaneChange scenario_LC_06_DoubleLaneChange [Default] scenario_LC_07_RightLaneChange scenario_LC_08_SlowmovingCar_Curved scenario_LC_09_CutInWithBreak_Curved scenario_LC_10_SingleLaneChange_Curved
These scenarios are created using the Driving Scenario Designer and are exported to a scenario file. Examine the comments in each file for more details on the road and vehicles in each scenario. You can configure the
HighwayLaneChangeTestBench and workspace to simulate these scenarios using the
helperSLHighwayLaneChangeSetup function. For example, you can configure the simulation for a curved road scenario.
This example shows how to simulate a highway lane change maneuver using ideal vehicle positions and lane detections.
Enable the MPC update messages again.