使用传感器融合的前方碰撞预警-(Forward Collision Warning Using Sensor Fusion)

这个例子matlab自动驾驶工具箱中关于使用传感器融合的前方碰撞预警-(Forward Collision Warning Using Sensor Fusion)例子，其展示了如何通过融合视觉和雷达传感器的数据来跟踪车辆前方的物体，从而实现前向碰撞预警系统的开发与验证。

1 概述

前方碰撞预警(FCW)是驾驶辅助系统和自动驾驶系统中的一项重要功能，其目标是在即将与前车发生碰撞之前，向驾驶员提供正确、及时、可靠的警告。为了实现这一目标，车辆配备了面向前方的视觉和毫米波雷达传感器，基于视觉和毫米波雷达融合是目前市场主流方案之一。为了充分利用视觉和毫米波雷达的各自的优点，提高准确警告的概率，并将错误警告的概率降到最低，需要进行传感器融合。

在本例中，一辆测试车（被控车辆）装备了各类传感器，并记录了它们的输出。本例中使用的传感器有：

1 视觉传感器，它提供了观察到的物体列表及其分类和车道边界的信息。对象列表每秒输出10次。车道边界每秒输出20次；

2 毫米波雷达传感器，具有中远距离模式，提供未分类的探测到的物体。物体清单每秒输出20次；

3 惯性导航单元IMU，每秒输出被控车辆的速度和横摆角速度20次；

4 摄像机，它记录了车前场景的视频片段。注：这段视频不被跟踪器使用，只用于在视频上显示跟踪结果，以便验证。

提供前方碰撞预警的过程包括以下步骤：

1 获取传感器的数据；

2 融合传感器数据，得到轨迹列表，即汽车前方物体的估计位置和速度；

3 根据轨迹和FCW标准发出警告。FCW标准基于Euro-NCAP AEB测试规程(目前最新的为2023版，是目前全球AEB FCW主导法规之一)，并考虑到与车前物体的相对距离和相对速度。

本例中的可视化是使用monoCamera和birdsEyePlot完成的。为了简洁起见，创建和更新显示的函数被移到本例之外的函数中。后续相关文档会讲述。

本例是一个脚本，这里显示的是主体，在后面的章节中以局部函数的形式显示。

% 设置显示

[videoReader, videoDisplayHandle, bepPlotters, sensor] = ...
helperCreateFCWDemoDisplay('01_city_c2s_fcw_10s.mp4', 'SensorConfigurationData.mat');


% 读取记录的探测文件

[visionObjects, radarObjects, inertialMeasurementUnit, laneReports, ...

    timeStep, numSteps] = readSensorRecordingsFile('01_city_c2s_fcw_10s_sensor.mat');



% 计算出初始的被控车辆的车道。
% 如果记录的车道信息无效，则将车道边界定义为汽车两侧各半个车道距离的直线

laneWidth = 3.6; % 米

egoLane = struct('left', [0 0 laneWidth/2], 'right', [0 0 -laneWidth/2]);


% 准备一些时间变量

time = 0;           % 记录开始后的时间

currentStep = 0;    % 当前步长

snapTime = 9.3;     % 截取显示屏快照的时间



% 初始化跟踪模块

[tracker, positionSelector, velocitySelector] = setupTracker();

while currentStep < numSteps && ishghandle(videoDisplayHandle)

    % 更新场景计数器

    currentStep = currentStep + 1;

    time = time + timeStep;



    % 将传感器检测结果作为物体检测输入处理，并将其发送给跟踪模块

    [detections, laneBoundaries, egoLane] = processDetections(...

        visionObjects(currentStep), radarObjects(currentStep), ...

        inertialMeasurementUnit(currentStep), laneReports(currentStep), ...

        egoLane, time);



    % 使用对象检测列表，返回更新为时间的轨迹

    confirmedTracks = updateTracks(tracker, detections, time);


    % 找到最可能发生碰撞的目标并计算前向碰撞预警时间

    mostImportantObject = findMostImportantObject(confirmedTracks, egoLane, positionSelector, velocitySelector);


    % 更新视频和俯视图显示

    frame = readFrame(videoReader);     % 读取视频帧
    helperUpdateFCWDemoDisplay(frame, videoDisplayHandle, bepPlotters, ...
laneBoundaries, sensor, confirmedTracks, mostImportantObject, positionSelector, ...
velocitySelector, visionObjects(currentStep), radarObjects(currentStep));


    % 抓取快照

    if time >= snapTime && time < snapTime + timeStep

        snapnow;

    end

end

2 创建多对象跟踪模块

multiObjectTracker根据视觉和毫米波雷达传感器输出的物体列表，跟踪被控车辆四周的物体。通过融合两个传感器的信息，降低了错误碰撞预警的概率。

setupTracker函数会返回multiObjectTracker。当创建一个multiObjectTracker，考虑如下：

1 FilterInitializationFcn。运动和测量模型。在这种情况下，预计物体会有一个恒定的加速度运动。虽然可以为这个模型配置一个线性卡尔曼滤波器，但initConstantAccelerationFilter配置了一个扩展的卡尔曼滤波器。

2 AssignmentThreshold: 检测到的数据与轨迹的距离。这个参数的默认值是30。如果有一些检测结果没有被分配到轨迹上，但应该被分配到轨迹上，则增加该值。如果有检测结果被分配到太远的轨迹上，则降低此值。本例使用35。

3 DeletionThreshold。当一条轨迹被确认后，不应该在第一次更新时删除，因为没有检测分配给它。相反，它应该被沿用（预测），直到很明显该轨迹没有得到任何传感器信息来更新它。其逻辑是，如果轨迹在 Q 次中漏掉 P 次，则应将其删除。该参数的默认值是5次中的5次。在这种情况下，跟踪模块每秒被调用20次，而且有两个传感器，所以没有必要修改默认值。

4 ConfirmationThreshold。确认轨迹的参数。每次未分配的检测都会初始化一个新的轨迹。其中有些检测可能是假的，所以所有的轨迹都初始化为 "暂定"。要确认一条轨迹，必须在N次轨迹更新中至少检测到M次。M和N的选择取决于对象的可见度。本例使用默认的3次更新中检测到2次。

setupTracker的输出是

- tracker - 为本例配置的多对象追踪模块。

- positionSelector--指定状态向量中哪些元素是位置的矩阵：position = positionSelector * State

- velocitySelector - 一个指定状态向量中哪些元素为速度的矩阵：velocity = velocitySelector * State。

function [tracker, positionSelector, velocitySelector] = setupTracker()
        tracker = multiObjectTracker(...
            'FilterInitializationFcn', @initConstantAccelerationFilter, ...
            'AssignmentThreshold', 35, 'ConfirmationThreshold', [2 3], ...
            'DeletionThreshold', 5);

        % 状态向量为:
        %   在匀速状态下 : State = [x;vx;y;vy]
        %   在加速状态下: State = [x;vx;ax;y;vy;ay]

        % 定义状态的位置。例如:
        %  在匀速状态下 : [x;y] = [1 0 0 0; 0 0 1 0] * 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];

        % 定义状态的速度. 例如:
        %   在匀速状态下: [x;y] = [0 1 0 0; 0 0 0 1] * 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 定义卡尔曼滤波器

上一节中定义的multiObjectTracker使用本节中定义的过滤器初始化函数来创建一个卡尔曼过滤器（线性、扩展或无痕）。然后，这个滤波器被用于跟踪被控车辆周围的每个对象。

function filter = initConstantAccelerationFilter(detection)
% 该函数展示了如何配置恒定加速度滤波器。输入是探测对象，输出是跟踪滤波模块。
%为清楚起见，此函数展示了如何为恒加速度配置 trackingKF、trackingEKF 或 trackingUKF。

% 创建滤波器的步骤：
% 1. 定义运动模型和状态
% 2. 定义过程噪声
% 3. 定义测量模型
% 4. 根据测量结果初始化状态向量
% 5. 根据测量噪声初始化状态协方差
% 6. 创建正确的滤波器

% 第 1 步：定义运动模型和状态
% 本例使用恒定加速度模型，因此:
    STF = @constacc;     % EKF 和 UKF 的状态转换函数
    STFJ = @constaccjac; % 状态转换函数雅各布，仅适用于 EKF
    % 运动模型意味着状态为 [x;vx;ax;y;vy;ay] 。
    % 还可以使用 constvel 和 constveljac 建立恒定速度模型，
    % 使用 constturn 和 constturnjac 建立恒定转率模型，或者编写自己的模型。。

    % Step 2: 定义过程噪声
    dt = 0.05; % 时间步长
    sigma = 1; % 未知加速度变化率的大小
    % 沿一个维度的过程噪声
    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); % 两维度的过程噪声

    % 步骤3定义测量模型
    MF = @fcwmeas;       % EKF 和 UKF 的测量函数
    MJF = @fcwmeasjac;   % 测量雅各布函数，仅适用于 EKF

    % 步骤 4：根据测量结果初始化状态向量
    % 传感器的测量值为 [x;vx;y;vy]，恒定加速度模型的状态为 [x;vx;ax;y;vy;ay]，
    % 因此状态向量的第三和第六元素被初始化为零。
    state = [detection.Measurement(1); detection.Measurement(2); 0; detection.Measurement(3); detection.Measurement(4); 0];

    % 第 5 步：根据测量噪声初始化状态协方差。
    % 对于非直接测量的状态部分，将赋予一个较大的测量噪声值，以考虑到这一点。
    L = 100; % 相对于测量噪音而言是个大数字
    stateCov = blkdiag(detection.MeasurementNoise(1:2,1:2), L, detection.MeasurementNoise(3:4,3:4), L);

    % 第 6 步：创建正确的过滤器。
    % 使用 "KF "跟踪 KF，使用 "EKF "跟踪EKF，或使用 "UKF "跟踪UKF
    FilterType = 'EKF';

    % 创建滤波:
    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' % 恒定加速度模型是线性的，可以使用 KF
            % 定义测量模型：测量 = H * 状态
            % 在这种情况下
            % 测量 = [x;vx;y;vy] = H * [x;vx;ax;y;vy;ay]
            %  因此，H = [1 0 0 0 0; 0 1 0 0 0; 0 0 0 1 0 0; 0 0 0 1 0] 
            % 测量模型 = H * state
            %  注意，恒定加速度运动模型会自动计算过程噪声（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 处理和格式化检测

记录的信息必须经过处理和格式化，才能被跟踪模块使用。这有以下几个步骤。

1 过滤掉不必要的毫米波雷达探测到的杂物。毫米波雷达会报告许多与固定物体相对应的物体，这些物体包括：护栏、道路中间线、交通标志等。如果在跟踪中使用了这些检测结果，就会在道路边缘产生固定物体的虚假轨迹，因此必须在调用跟踪模块之前将其删除。毫米波雷达探测到的物体如果在车前静止或在车附近移动，则被认为是非杂乱物体。

2 将探测结果格式化，作为跟踪模块的输入，即objectDetection元素的数组。参见本例最后的processVideo和processRadar支持函数。

function [detections,laneBoundaries, egoLane] = processDetections...
            (visionFrame, radarFrame, IMUFrame, laneFrame, egoLane, time)
        % 输入：
        % visionFrame - 视觉传感器在该时间段输出的对象
        % radarFrame - 该时间段毫米波雷达传感器输出的物体
        % IMUFrame - 此时间帧的惯性测量单元数据
        % laneFrame - 此时间帧的车道线输出
        % egoLane - 估计的被控车辆车道
        % time - 与时间帧相对应的时间

        % 去除毫米波雷达探测的杂波对象
        [laneBoundaries, egoLane] = processLanes(laneFrame, egoLane);
        realRadarObjects = findNonClutterRadarObjects(radarFrame.object,...
            radarFrame.numObjects, IMUFrame.velocity, laneBoundaries);

        % 如果没有输出对象，则返回空列表

        % 计算总的目标数量
        detections = {};
        if (visionFrame.numObjects + numel(realRadarObjects)) == 0
            return;
        end

        % 处理剩余的毫米波雷达目标
        detections = processRadar(detections, realRadarObjects, time);

        % 处理视频目标
        detections = processVideo(detections, visionFrame, time);
    end

5 更新追踪

要更新跟踪，调用 updateTracks 方法，输入如下内容：

1 tracker - 之前配置的 multiObjectTracker；

2 detections - 由processDetections创建的objectDetection对象列表；

3 time - 当前场景的时间。

跟踪器的输出是一个轨迹结构数组。

找到最重要的对象并发出前向碰撞警告。

最重要的物体（MIO）被定义为在被控车道上且离车前最近的轨迹，即正x值最小。为了降低误报的概率，只考虑确认的轨迹。

一旦找到了MIO，就会计算汽车和MIO之间的相对速度。相对距离和相对速度决定了前撞预警。FCW有3种情况。

1 安全（绿色）：被控车道上没有车（没有MIO），MIO正在远离汽车，或者与MIO的距离保持不变；

2 小心（黄色）：MIO正在向汽车靠近，但距离仍高于FCW距离。FCW距离使用欧洲NCAP AEB测试协议计算。请注意，该距离随MIO和汽车之间的相对速度而变化，当关闭速度较高时，该距离较大；

3 警告（红色）：MIO正在向汽车靠近，其距离小于FCW距离。

Euro-NCAP AEB测试规程定义了以下距离计算：

其中：

$d{_{fcw}}$ 是前方碰撞警告距离;

$v{_{ref}}$ 是两车之间的相对速度;

$a{_{max}}$ 是最大减速度，定义为重力加速度g的40%。

function mostImportantObject = findMostImportantObject(confirmedTracks,egoLane,positionSelector,velocitySelector)

        % 初始化输出和参数
        MIO = [];               % 默认无目标MIO
        trackID = [];           % 默认无trackID分配给MIO
        FCW = 3;                % 默认无MIO, FCW 是'安全的'
        threatColor = 'green';  % 默认威胁颜色是绿色
        maxX = 1000;  % 足够靠前，以确保轨迹不会超过这个距离
        gAccel = 9.8; % 重力加速度 单位：m/s^2
        maxDeceleration = 0.4 * gAccel; % Euro NCAP AEB 定义
        delayTime = 1.2; % 驾驶员反应时间，单位为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); % 本车道内与目标的相对速度

            if x < maxX && x > 0 % 否则没有必要监测
                yleftLane  = polyval(egoLane.left,  x);
                yrightLane = polyval(egoLane.right, x);
                if (yrightLane <= y) && (y <= yleftLane)
                    maxX = x;
                    trackID = i;
                    MIO = confirmedTracks(i).TrackID;
                    if relSpeed < 0 % 相对速度表示物体越来越近
                        % 根据Euro - NCAP AEB 测试规程计算预期制动距离
                        d = abs(relSpeed) * delayTime + relSpeed^2 / 2 / maxDeceleration;
                        if x <= d  % '报警'
                            FCW = 1;
                            threatColor = 'red';
                        else % '警告'
                            FCW = 2;
                            threatColor = 'yellow';
                        end
                    end
                end
            end
        end
        mostImportantObject = struct('ObjectID', MIO, 'TrackIndex', trackID, 'Warning', FCW, 'ThreatColor', threatColor);