Adaptive Cruise Control with Sensor Fusion within Matlab/Simulink

Recently I am trying to use Matlab/Simulink toolbox to running some advanced driving assistance system (ADAS) test benches. Since recent years, Matlab has published the Automated Driving toolbox , combining with the recent popular machine/deep learning techniques, makes the development of individual ADAS functions much more straightforward.

Before I go further, short summary of the related topics has to be placed first.

Object detection

There’re tons of tutorials and examples about this issue. Objects can be detected from the camera video stream using either Tensorflow API or Matlab/Simulink Computer Vision Toolbox with various types of sensor data, e.g. 3D Lidar cloud point and/or pictures taken from camera.

Even when the objects are possibly detected via the ML/DL techniques, it is still far away from an even simple ADAS function development. Since the ADAS or autonomous vehicle has to first correctly interpret the real world with the help of sensors data, then it should also possess the ability to thinking, planning and reaction, more specifically control of the car.

Model Predictive Control

Based on the simple Bicycle Model , the kinematic and dynamic control equations can be feeded into MPC algorithm.

Model Predictive Control is only one of the control techniques and becomes more and more popular for ADAS development due to the optimization ability with multiple input/output, as well as constraints. For the linear-time-invariant (LTI) control system, the continuum state-space model can be described using

continuum state-space model. A, B, C D are constant state space matrices. x is state vector, y the output, u the input/control variable.

Based on a simple Bicycle model, the state function can be written as

Vy, dot_Vy is used for lateral control, psi, dot_psi for steering control, and Vx, dot_Vx for longitudinal control. Detail information can be found from Matlab documentation page.

There is a short webinar from Matlab about MPC which can help to understand the algorithm behind. The main idea of MPC is to make predictions about the future plant output, and the optimizer finds the optimal sequence of control inputs that drives the plant output as close to the setpoint as possible.

For example, in the following picture show a typical scenario of turing a car in a cross road. The MPC will take into account of the curvature of the road map and minimize the error between the road map and the plant path. One of the major advantage of MPC is the multiple input and output with hard/soft constraints ability, which makes it quite fit for the control strategy in the ADAS function.

Since we have the input obtained from object detection techniques and MPC strategy, then we can start to build a toy model using Matlab/Simulink toolbox.

One important point concerning the aforementioned MPC and it only works for slow motion senario. When dealing with a high speed condition, discretized MPC technique has to be used.

Adaptive Cruise Control (ACC)

The example shown here demonstrates such a scenario: a car in front of the ego car are cutting from the right into the lane of ego car’s. The radar and camera sensors detects and confirms the front car. For safety reason the ego car has to estimate the relative distance w.r.t. the leading car, if the distance smaller than the permitted distance, then the ego car has to break and maintain the safety distance till full stop. Till/When the leading car is far away, then the ego car accelerates gradually until reaches the desired velocity.

ACC with sensor fusion function

In this test bench, the module of ACC with sensor fusion has such a function that it detects if there’re a leading car in the same lane (as well as in other lanes within the detection range of sensors), fuses the detections (remove redundancy), passes the detection to MPC; the MPC slows/accelerates the ego car accordingly.

The input parameters are vision and radar detection objects, simulation time, longitudinal velocity of the ego car and curvature of the road. The sensor fusion and tracking lead car submodule contains first radar detection clustering due to the noise from radar and then combines the detections from vision and radar passed to multi object tracker. It use Kalman filter to estimate the state of the detections precisely and fuses the detections. Then the confirmed tracks together with the road information are used to determine the relative distance and relative velocity between ego car and the lead car, which will be used for ACC.

Together with the relative distance and relative velocity, time_gap (reaction time of the driver?) longitudinal velocity and Driver-set velocity are feeded into the Adaptive Cruise Control System using MPC scheme. In this test bench, the prebuilt ACC control module is used. It is also possible to build the user-specific MPC module, here is an tutorial how to do so. The main function of this ACC block is tracking a driver-set velocity and maintains a safe distance from a lead vehicle by adjusting the longitudinal acceleration of an ego vehicle. The block computes optimal control actions while satisfying safe distance, velocity, and acceleration constraints using model predictive control (MPC). The detail of the algorithm structure is shown below. Then the user can modify the original ACC block from Matlab accordingly.

Adaptive Cruise Control using Model Predictive Control

Until now the main ACC control is almost done. However, when driving on the road, the driver has also to be kept in the lane all the time. Thus the lane follow function, in other words the steering control has also to be considered. Together with the MPC regulated longitudinal acceleration, the road (map) information have to be feeded into the Vehicle and Environment Simulink block. In the current test case, the road geometry is simply described via a constant curvature 1/R and has already be created in the workspace of Matlab, which can be directly used from the subsystem.

Using the MPC regulated longitudinal acceleration and curvature of the road to update the position and yaw angle of the ego car. The steering control is achieved via PID control.

Together with the MPC regulated acceleration of the ego car, the steering control is achieved via proportional–integral–derivative (PID) control scheme.

Position and yaw angle of the ego car is simulated based on the Bicycle Model with force input.

Until now we have all the necessary ingredients to run a ACC simulation. Click the run button and the results can be viewed from the Bird’s-Eye Scope as following, only one frame is shown.

The whole simulation results looks like

From the video we can see that when a lower speed car cut into the lane of the faster ego vehicle, as long as the sensors detect such leading car and with the help of MPC control, the ego vehicle firstly slows down to keep a safe distance. When the leading car leaves the same lane, the ego car accelerates again till to the driver set velocity. The velocity of the ego vehicle and the driver set velocity are shown in the following figure. Relative distance and acceleration profile of the ego vehicle are also shown.

Last but not least, such ADAS has to be deployed onto a specific ECU with C or C++ and Matlab has provide the Code Generator toolbox to easily do so. If further C Algorithm has to be added or modified, then the work can continue based on the generated C/C++ code.

Until now I have reviewed the ACC test bensch from the Matlab/Simulink implementation. The main function can be correctly recovered. For a more complicated or different driving scenarios, user can use similar schedule as described here to do further functional development.