This example of Forward Collision Warning Using Sensor Fusion in the matlab autonomous driving toolbox shows how to track objects in front of the vehicle by fusing data from visual and radar sensors to achieve forward collision detection. Development and verification of collision warning systems.
1 Overview
Forward Collision Warning (FCW) is an important function in driver assistance systems and autonomous driving systems. Its goal is to provide the driver with correct, timely and reliable warnings before a collision with the vehicle in front. In order to achieve this goal, the vehicle is equipped with forward-facing visual and millimeter-wave radar sensors. The fusion of vision and millimeter-wave radar is one of the current mainstream solutions in the market. In order to make full use of the respective advantages of vision and millimeter-wave radar, increase the probability of accurate warnings, and minimize the probability of false warnings, sensor fusion is required.
In this example, a test vehicle (the controlled vehicle) is equipped with various sensors and their outputs are recorded. The sensors used in this example are:
1 Vision sensor, which provides a list of observed objects and their classification and information about lane boundaries. The object list is output 10 times per second. Lane boundaries are output 20 times per second;
2 mmWave radar sensor with medium to long range mode providing unclassified detected objects. The object list is output 20 times per second;
3 Inertial navigation unit IMU, outputs the speed and yaw rate of the controlled vehicle 20 times per second;
4 cameras, which record video footage of the scene in front of the car. Note: This video is not used by the tracker and is only used to display the tracking results on the video for verification.
The process of providing forward collision warning includes the following steps:
1 Obtain sensor data;
2 Fusion of sensor data to obtain a trajectory list, that is, the estimated position and speed of objects in front of the car;
3 Warnings are issued based on trajectory and FCW standards. The FCW standard is based on the Euro-NCAP AEB test procedures (the latest version is the 2023 version, which is one of the leading global AEB FCW regulations) and takes into account the relative distance and relative speed to the object in front of the vehicle.
The visualization in this example is done using monoCamera and birdsEyePlot. For brevity, the functions that create and update the display have been moved to functions outside of this example. Related documents will be discussed later.
This example is a script, shown here as the main body, which will be shown in the form of local functions in later chapters.
% Set display
[videoReader, videoDisplayHandle, bepPlotters, sensor] = ...
helperCreateFCWDemoDisplay('01_city_c2s_fcw_10s.mp4', 'SensorConfigurationData.mat');
% Read the recorded detection file
[visionObjects, radarObjects, inertialMeasurementUnit, laneReports, ...
timeStep, numSteps] = readSensorRecordingsFile('01_city_c2s_fcw_10s_sensor.mat');
% Calculate the initial controlled vehicle lane.
% If the recorded lane information is invalid, define the lane boundary as a straight line half a lane distance on both sides of the car
laneWidth = 3.6; % meters
egoLane = struct('left', [0 0 laneWidth/2], 'right', [0 0 -laneWidth/2]);
% Prepare some time variables
time = 0; % time since recording started
currentStep = 0; % current step size
snapTime = 9.3; % time to take screen snapshot
% Initialize tracking module
[tracker, positionSelector, velocitySelector] = setupTracker();
while currentStep < numSteps & amp; & ishghandle(videoDisplayHandle)
% Update scene counter
currentStep = currentStep + 1;
time = time + timeStep;
% Process sensor detection results as object detection input and send them to the tracking module
[detections, laneBoundaries, egoLane] = processDetections(...
visionObjects(currentStep), radarObjects(currentStep), ...
inertialMeasurementUnit(currentStep), laneReports(currentStep), ...
egoLane, time);
% Use the object detection list to return the trajectory updated as time
confirmedTracks = updateTracks(tracker, detections, time);
% Find the most likely target to collide with and calculate the forward collision warning time
mostImportantObject = findMostImportantObject(confirmedTracks, egoLane, positionSelector, velocitySelector);
% Update video and top view display
frame = readFrame(videoReader); % Read video frame
helperUpdateFCWDemoDisplay(frame, videoDisplayHandle, bepPlotters, ...
laneBoundaries, sensor, confirmedTracks, mostImportantObject, positionSelector, ...
velocitySelector, visionObjects(currentStep), radarObjects(currentStep));
% Grab snapshot
if time >= snapTime & amp; & amp; time < snapTime + timeStep
snapnow;
end
end
2 Create multi-object tracking module
multiObjectTracker tracks objects around the controlled vehicle based on the object list output by the vision and millimeter wave radar sensors. By fusing information from two sensors, the probability of false collision warnings is reduced.
The setupTracker function returns multiObjectTracker. When creating a multiObjectTracker, consider the following:
1 FilterInitializationFcn. Motion and measurement models. In this case, the object is expected to move with a constant acceleration. Although it is possible to configure a linear Kalman filter for this model, initConstantAccelerationFilter configures an extended Kalman filter.
2 AssignmentThreshold: The distance between the detected data and the trajectory. The default value of this parameter is 30. Increase this value if there are some detections that are not assigned to a trajectory but should be. Lower this value if detections are assigned to trajectories that are too far away. This example uses 35.
3DeletionThreshold. When a track is confirmed, it should not be deleted on the first update because no detections are assigned to it. Instead, it should be followed (predicted) until it is obvious that the trajectory is not getting any sensor information to update it. The logic is that if a trajectory misses P times out of Q times, it should be deleted. The default value for this parameter is 5 out of 5 times. In this case, the tracking module is called 20 times per second, and there are two sensors, so there is no need to modify the default values.
4ConfirmationThreshold. Confirm the parameters of the trajectory. Each unassigned detection initializes a new trajectory. Some of these detections may be false, so all trajectories are initialized to “tentative”. To confirm a trajectory, it must be detected at least M times in N trajectory updates. The choice of M and N depends on the visibility of the object. This example uses the default detection of 2 out of 3 updates.
The output of setupTracker is
– tracker – The multi-object tracking module configured for this example.
– positionSelector–a matrix that specifies which elements in the state vector are positions: position = positionSelector * State
– velocitySelector – a matrix that specifies which elements of the state vector are velocities: velocity = velocitySelector * State.
function [tracker, positionSelector, velocitySelector] = setupTracker()
tracker = multiObjectTracker(...
'FilterInitializationFcn', @initConstantAccelerationFilter, ...
'AssignmentThreshold', 35, 'ConfirmationThreshold', [2 3], ...
'DeletionThreshold', 5);
% The state vector is:
% In the constant speed state: State = [x;vx;y;vy]
% In acceleration state: State = [x;vx;ax;y;vy;ay]
% defines the position of the state. For example:
% In uniform speed state: [x;y] = [1 0 0 0; 0 0 1 0] * State
% In acceleration state: [x;y] = [1 0 0 0 0 0; 0 0 0 1 0 0] * State
positionSelector = [1 0 0 0 0 0; 0 0 0 1 0 0];
% Defines the speed of the state. For example:
% In uniform speed state: [x;y] = [0 1 0 0; 0 0 0 1] * State
% In acceleration state: [x;y] = [0 1 0 0 0 0; 0 0 0 0 1 0] * State
velocitySelector = [0 1 0 0 0 0; 0 0 0 0 1 0];
end
3 Define Kalman filter
The multiObjectTracker defined in the previous section uses the filter initialization function defined in this section to create a Kalman filter (linear, extended or invisible). This filter is then used to track each object around the controlled vehicle.
function filter = initConstantAccelerationFilter(detection)
% This function shows how to configure a constant acceleration filter. The input is the detection object, and the output is the tracking filter module.
%For clarity, this function shows how to configure trackingKF, trackingEKF or trackingUKF for constant acceleration.
% Steps to create a filter:
% 1. Define motion model and state
% 2. Define process noise
% 3. Define the measurement model
% 4. Initialize the state vector based on the measurement results
% 5. Initialize state covariance based on measurement noise
% 6. Create the correct filter
% Step 1: Define motion model and states
% This example uses a constant acceleration model, so:
STF = @constacc; % state transition function of EKF and UKF
STFJ = @constaccjac; % state transition function Jacob, only applicable to EKF
% The motion model means that the state is [x;vx;ax;y;vy;ay] .
% You can also use constvel and constveljac to establish a constant velocity model,
% Use constturn and constturnjac to build a constant rate model, or write your own. .
% Step 2: Define process noise
dt = 0.05; % time step
sigma = 1; % unknown magnitude of acceleration change rate
% Process noise along one dimension
Q1d = [dt^4/4, dt^3/2, dt^2/2; dt^3/2, dt^2, dt; dt^2/2, dt, 1] * sigma^2;
Q = blkdiag(Q1d, Q1d); % two-dimensional process noise
% Step 3 Define the measurement model
MF = @fcwmeas; % measurement function for EKF and UKF
MJF = @fcwmeasjac; % Measure Jacob's function, only available for EKF
% Step 4: Initialize the state vector based on the measurement results
% The measured value of the sensor is [x;vx;y;vy], and the state of the constant acceleration model is [x;vx;ax;y;vy;ay],
% Therefore the third and sixth elements of the state vector are initialized to zero.
state = [detection.Measurement(1); detection.Measurement(2); 0; detection.Measurement(3); detection.Measurement(4); 0];
% Step 5: Initialize state covariance based on measurement noise.
% For parts of the state that are not directly measured, a larger measurement noise value will be assigned to take this into account.
L = 100; % is a large number relative to the measurement noise
stateCov = blkdiag(detection.MeasurementNoise(1:2,1:2), L, detection.MeasurementNoise(3:4,3:4), L);
% Step 6: Create the correct filter.
% Use "KF " to track KF, use "EKF " to track EKF, or use "UKF " to track UKF
FilterType = 'EKF';
% Create filter:
switch FilterType
case 'EKF'
filter = trackingEKF(STF, MF, state,...
'StateCovariance', stateCov, ...
'MeasurementNoise', detection.MeasurementNoise(1:4,1:4), ...
'StateTransitionJacobianFcn', STFJ, ...
'MeasurementJacobianFcn', MJF, ...
'ProcessNoise', Q ...
);
case 'UKF'
filter = trackingUKF(STF, MF, state, ...
'StateCovariance', stateCov, ...
'MeasurementNoise', detection.MeasurementNoise(1:4,1:4), ...
'Alpha', 1e-1, ...
'ProcessNoise', Q ...
);
case 'KF' % The constant acceleration model is linear, you can use KF
% Define measurement model: measurement = H * state
% in this case
% measure = [x;vx;y;vy] = H * [x;vx;ax;y;vy;ay]
% Therefore, H = [1 0 0 0 0; 0 1 0 0 0; 0 0 0 1 0 0; 0 0 0 1 0]
% measurement model = H * state
% Note that the constant acceleration motion model will automatically calculate process noise (ProcessNoise)
H = [1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 0 1 0 0; 0 0 0 0 1 0];
filter = trackingKF('MotionModel', '2D Constant Acceleration', ...
'MeasurementModel', H, 'State', state, ...
'MeasurementNoise', detection.MeasurementNoise(1:4,1:4), ...
'StateCovariance', stateCov);
end
end
4 Processing and formatting detection
The logged information must be processed and formatted before it can be used by the tracking module. There are several steps to this.
1 Filter out unnecessary debris detected by millimeter wave radar. Millimeter wave radar reports many objects that correspond to fixed objects including: guardrails, road medians, traffic signs, etc. If these detections are used in tracking, they will produce spurious trajectories of stationary objects at the edges of the road, so they must be removed before calling the tracking module. Objects detected by millimeter-wave radar are considered non-clutter objects if they are stationary in front of the vehicle or moving near the vehicle.
2 Format the detection results as the input of the tracking module, that is, an array of objectDetection elements. See the processVideo and processRadar support functions at the end of this example.
function [detections,laneBoundaries, egoLane] = processDetections...
(visionFrame, radarFrame, IMUFrame, laneFrame, egoLane, time)
% input:
% visionFrame - the object output by the vision sensor during this time period
% radarFrame - the object output by the millimeter wave radar sensor during this time period
%IMUFrame - Inertial measurement unit data for this time frame
% laneFrame - lane line output for this time frame
% egoLane - Estimated lane of the controlled vehicle
% time - the time corresponding to the time frame
% Remove clutter objects detected by millimeter wave radar
[laneBoundaries, egoLane] = processLanes(laneFrame, egoLane);
realRadarObjects = findNonClutterRadarObjects(radarFrame.object,...
radarFrame.numObjects, IMUFrame.velocity, laneBoundaries);
% If there is no output object, return an empty list
% Calculate the total target quantity
detections = {};
if (visionFrame.numObjects + numel(realRadarObjects)) == 0
return;
end
% Process remaining millimeter wave radar targets
detections = processRadar(detections, realRadarObjects, time);
% Handle video target
detections = processVideo(detections, visionFrame, time);
end
5 Update Tracking
To update tracks, call the updateTracks method with the following input:
1 tracker – the previously configured multiObjectTracker;
2 detections – a list of objectDetection objects created by processDetections;
3 time – The time of the current scene.
The output of the tracker is an array of trajectory structures.
Find the most important objects and issue forward collision warning.
The most important object (MIO) is defined as the trajectory closest to the front of the car on the controlled lane, that is, with the smallest positive x value. To reduce the probability of false positives, only confirmed trajectories are considered.
Once the MIO is found, the relative speed between the car and the MIO is calculated. Relative distance and relative speed determine forward collision warning. There are 3 situations in FCW.
1 Safe (green): There is no car in the controlled lane (no MIO), the MIO is moving away from the car, or the distance to the MIO remains unchanged;
2 Caution (yellow): MIO is approaching the car, but the distance is still higher than the FCW distance. FCW distance is calculated using Euro NCAP AEB test protocol. Note that this distance varies with the relative speed between the MIO and the car, and is larger when the closing speed is higher;
3 Warning (red): MIO is approaching the car, and its distance is less than the FCW distance.
The Euro-NCAP AEB test procedures define the following distance calculations:
in:
function mostImportantObject = findMostImportantObject(confirmedTracks,egoLane,positionSelector,velocitySelector)
% Initialize output and parameters
MIO = []; % Default no target MIO
trackID = []; % By default, no trackID is assigned to MIO
FCW = 3; % No MIO by default, FCW is 'safe'
threatColor = 'green'; % The default threat color is green
maxX = 1000; % far enough forward to ensure that the trajectory does not exceed this distance
gAccel = 9.8; % acceleration due to gravity Unit: m/s^2
maxDeceleration = 0.4 * gAccel; % Euro NCAP AEB definition
delayTime = 1.2; % driver reaction time, unit is s
positions = getTrackPositions(confirmedTracks, positionSelector);
velocities = getTrackVelocities(confirmedTracks, velocitySelector);
for i = 1:numel(confirmedTracks)
x = positions(i,1);
y = positions(i,2);
relSpeed = velocities(i,1); % relative speed to the target in this lane
if x < maxX & amp; & amp; x > 0 % otherwise there is no need to monitor
yleftLane = polyval(egoLane.left, x);
yrightLane = polyval(egoLane.right, x);
if (yrightLane <= y) & amp; & amp; (y <= yleftLane)
maxX = x;
trackID = i;
MIO = confirmedTracks(i).TrackID;
if relSpeed < 0 % relative speed means the object is getting closer
% Calculation of expected braking distance according to Euro-NCAP AEB test procedures
d = abs(relSpeed) * delayTime + relSpeed^2 / 2 / maxDeceleration;
if x <= d % 'alarm'
FCW = 1;
threatColor = 'red';
else % 'warning'
FCW = 2;
threatColor = 'yellow';
end
end
end
end
end
mostImportantObject = struct('ObjectID', MIO, 'TrackIndex', trackID, 'Warning', FCW, 'ThreatColor', threatColor);
6 Summary
This example shows how to create a forward collision warning system for a vehicle equipped with vision, millimeter wave radar and IMU sensors. It uses the objectDetection object to pass the sensor reports to the multiObjectTracker object, which fuses them and tracks objects in front of the controlled vehicle.
You can try using different parameters for the tracking module and see how they affect tracking quality. Try modifying the tracking filter, using trackingKF or trackingUKF, or defining a different motion model, for example, constant speed or constant rotation. Finally, you can try to define your own motion model.