使用傳感器融合的前方碰撞預(yù)警仿真

??這個(gè)例子matlab自動(dòng)駕駛工具箱中關(guān)于使用傳感器融合的前方碰撞預(yù)警-(Forward Collision Warning Using Sensor Fusion)例子，其展示了如何通過融合視覺和雷達(dá)傳感器的數(shù)據(jù)來跟蹤車輛前方的物體，從而實(shí)現(xiàn)前向碰撞預(yù)警系統(tǒng)的開發(fā)與驗(yàn)證。

1 概述

前方碰撞預(yù)警(FCW)是駕駛輔助系統(tǒng)和自動(dòng)駕駛系統(tǒng)中的一項(xiàng)重要功能，其目標(biāo)是在即將與前車發(fā)生碰撞之前，向駕駛員提供正確、及時(shí)、可靠的警告。為了實(shí)現(xiàn)這一目標(biāo)，車輛配備了面向前方的視覺和毫米波雷達(dá)傳感器，基于視覺和毫米波雷達(dá)融合是目前市場主流方案之一。為了充分利用視覺和毫米波雷達(dá)的各自的優(yōu)點(diǎn)，提高準(zhǔn)確警告的概率，并將錯(cuò)誤警告的概率降到最低，需要進(jìn)行傳感器融合。

在本例中，一輛測試車（被控車輛）裝備了各類傳感器，并記錄了它們的輸出。本例中使用的傳感器有：

1 視覺傳感器，它提供了觀察到的物體列表及其分類和車道邊界的信息。對(duì)象列表每秒輸出10次。車道邊界每秒輸出20次；

2 毫米波雷達(dá)傳感器，具有中遠(yuǎn)距離模式，提供未分類的探測到的物體。物體清單每秒輸出20次；

3 慣性導(dǎo)航單元IMU，每秒輸出被控車輛的速度和橫擺角速度20次；

4 攝像機(jī)，它記錄了車前場景的視頻片段。注：這段視頻不被跟蹤器使用，只用于在視頻上顯示跟蹤結(jié)果，以便驗(yàn)證。

提供前方碰撞預(yù)警的過程包括以下步驟：

1 獲取傳感器的數(shù)據(jù)；

2 融合傳感器數(shù)據(jù)，得到軌跡列表，即汽車前方物體的估計(jì)位置和速度；

3 根據(jù)軌跡和FCW標(biāo)準(zhǔn)發(fā)出警告。FCW標(biāo)準(zhǔn)基于Euro-NCAP AEB測試規(guī)程(目前最新的為2023版，是目前全球AEB FCW主導(dǎo)法規(guī)之一)，并考慮到與車前物體的相對(duì)距離和相對(duì)速度。

本例中的可視化是使用monoCamera和birdsEyePlot完成的。為了簡潔起見，創(chuàng)建和更新顯示的函數(shù)被移到本例之外的函數(shù)中。后續(xù)相關(guān)文檔會(huì)講述。

本例是一個(gè)腳本，這里顯示的是主體，在后面的章節(jié)中以局部函數(shù)的形式顯示。

% 設(shè)置顯示[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');??% 計(jì)算出初始的被控車輛的車道。% 如果記錄的車道信息無效，則將車道邊界定義為汽車兩側(cè)各半個(gè)車道距離的直線 laneWidth?=?3.6;?%?米?egoLane = struct('left', [0 0 laneWidth/2], 'right', [0 0 -laneWidth/2]);  %?準(zhǔn)備一些時(shí)間變量?time?=?0;???????????%?記錄開始后的時(shí)間?currentStep?=?0;????%?當(dāng)前步長?snapTime?=?9.3;?????%?截取顯示屏快照的時(shí)間  %?初始化跟蹤模塊?[tracker, positionSelector, velocitySelector] = setupTracker();
while?currentStep?<?numSteps?&&?ishghandle(videoDisplayHandle)????%?更新場景計(jì)數(shù)器?????currentStep?=?currentStep?+?1;?????time?=?time?+?timeStep;?????    % 將傳感器檢測結(jié)果作為物體檢測輸入處理，并將其發(fā)送給跟蹤模塊     [detections, laneBoundaries, egoLane] = processDetections(...         visionObjects(currentStep), radarObjects(currentStep), ...         inertialMeasurementUnit(currentStep), laneReports(currentStep), ... ????????egoLane,?time);      % 使用對(duì)象檢測列表，返回更新為時(shí)間的軌跡     confirmedTracks = updateTracks(tracker, detections, time);??    % 找到最可能發(fā)生碰撞的目標(biāo)并計(jì)算前向碰撞預(yù)警時(shí)間     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 創(chuàng)建多對(duì)象跟蹤模塊

multiObjectTracker根據(jù)視覺和毫米波雷達(dá)傳感器輸出的物體列表，跟蹤被控車輛四周的物體。通過融合兩個(gè)傳感器的信息，降低了錯(cuò)誤碰撞預(yù)警的概率。

setupTracker函數(shù)會(huì)返回multiObjectTracker。當(dāng)創(chuàng)建一個(gè)multiObjectTracker，考慮如下：

1 FilterInitializationFcn。運(yùn)動(dòng)和測量模型。在這種情況下，預(yù)計(jì)物體會(huì)有一個(gè)恒定的加速度運(yùn)動(dòng)。雖然可以為這個(gè)模型配置一個(gè)線性卡爾曼濾波器，但initConstantAccelerationFilter配置了一個(gè)擴(kuò)展的卡爾曼濾波器。

2 AssignmentThreshold: 檢測到的數(shù)據(jù)與軌跡的距離。這個(gè)參數(shù)的默認(rèn)值是30。如果有一些檢測結(jié)果沒有被分配到軌跡上，但應(yīng)該被分配到軌跡上，則增加該值。如果有檢測結(jié)果被分配到太遠(yuǎn)的軌跡上，則降低此值。本例使用35。

3 DeletionThreshold。當(dāng)一條軌跡被確認(rèn)后，不應(yīng)該在第一次更新時(shí)刪除，因?yàn)闆]有檢測分配給它。相反，它應(yīng)該被沿用（預(yù)測），直到很明顯該軌跡沒有得到任何傳感器信息來更新它。其邏輯是，如果軌跡在 Q 次中漏掉 P 次，則應(yīng)將其刪除。該參數(shù)的默認(rèn)值是5次中的5次。在這種情況下，跟蹤模塊每秒被調(diào)用20次，而且有兩個(gè)傳感器，所以沒有必要修改默認(rèn)值。

4 ConfirmationThreshold。確認(rèn)軌跡的參數(shù)。每次未分配的檢測都會(huì)初始化一個(gè)新的軌跡。其中有些檢測可能是假的，所以所有的軌跡都初始化為 "暫定"。要確認(rèn)一條軌跡，必須在N次軌跡更新中至少檢測到M次。M和N的選擇取決于對(duì)象的可見度。本例使用默認(rèn)的3次更新中檢測到2次。

setupTracker的輸出是

- tracker - 為本例配置的多對(duì)象追蹤模塊。

- positionSelector--指定狀態(tài)向量中哪些元素是位置的矩陣：position = positionSelector * State

- velocitySelector - 一個(gè)指定狀態(tài)向量中哪些元素為速度的矩陣：velocity = velocitySelector * State。

function [tracker, positionSelector, velocitySelector] = setupTracker()        tracker = multiObjectTracker(...            'FilterInitializationFcn', @initConstantAccelerationFilter, ...            'AssignmentThreshold', 35, 'ConfirmationThreshold', [2 3], ...            'DeletionThreshold', 5);         % 狀態(tài)向量為:        %   在勻速狀態(tài)下 : State = [x;vx;y;vy]        %   在加速狀態(tài)下: State = [x;vx;ax;y;vy;ay]         % 定義狀態(tài)的位置。例如:        %  在勻速狀態(tài)下 : [x;y] = [1 0 0 0; 0 0 1 0] * State        %   在加速狀態(tài)下: [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];         % 定義狀態(tài)的速度. 例如:        %   在勻速狀態(tài)下: [x;y] = [0 1 0 0; 0 0 0 1] * State        %   在加速狀態(tài)下: [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 定義卡爾曼濾波器

上一節(jié)中定義的multiObjectTracker使用本節(jié)中定義的過濾器初始化函數(shù)來創(chuàng)建一個(gè)卡爾曼過濾器（線性、擴(kuò)展或無痕）。然后，這個(gè)濾波器被用于跟蹤被控車輛周圍的每個(gè)對(duì)象。

function filter = initConstantAccelerationFilter(detection)% 該函數(shù)展示了如何配置恒定加速度濾波器。輸入是探測對(duì)象，輸出是跟蹤濾波模塊。%為清楚起見，此函數(shù)展示了如何為恒加速度配置 trackingKF、trackingEKF 或 trackingUKF。 % 創(chuàng)建濾波器的步驟：% 1. 定義運(yùn)動(dòng)模型和狀態(tài)% 2. 定義過程噪聲% 3. 定義測量模型% 4. 根據(jù)測量結(jié)果初始化狀態(tài)向量% 5. 根據(jù)測量噪聲初始化狀態(tài)協(xié)方差% 6. 創(chuàng)建正確的濾波器 % 第 1 步：定義運(yùn)動(dòng)模型和狀態(tài)% 本例使用恒定加速度模型，因此:    STF = @constacc;     % EKF 和 UKF 的狀態(tài)轉(zhuǎn)換函數(shù)    STFJ = @constaccjac; % 狀態(tài)轉(zhuǎn)換函數(shù)雅各布，僅適用于 EKF    % 運(yùn)動(dòng)模型意味著狀態(tài)為 [x;vx;ax;y;vy;ay] 。    % 還可以使用 constvel 和 constveljac 建立恒定速度模型，    % 使用 constturn 和 constturnjac 建立恒定轉(zhuǎn)率模型，或者編寫自己的模型。。     % Step 2: 定義過程噪聲    dt = 0.05; % 時(shí)間步長    sigma = 1; % 未知加速度變化率的大小    % 沿一個(gè)維度的過程噪聲    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 的測量函數(shù)    MJF = @fcwmeasjac;   % 測量雅各布函數(shù)，僅適用于 EKF     % 步驟 4：根據(jù)測量結(jié)果初始化狀態(tài)向量    % 傳感器的測量值為 [x;vx;y;vy]，恒定加速度模型的狀態(tài)為 [x;vx;ax;y;vy;ay]，    % 因此狀態(tài)向量的第三和第六元素被初始化為零。    state = [detection.Measurement(1); detection.Measurement(2); 0; detection.Measurement(3); detection.Measurement(4); 0];     % 第 5 步：根據(jù)測量噪聲初始化狀態(tài)協(xié)方差。    % 對(duì)于非直接測量的狀態(tài)部分，將賦予一個(gè)較大的測量噪聲值，以考慮到這一點(diǎn)。    L = 100; % 相對(duì)于測量噪音而言是個(gè)大數(shù)字    stateCov = blkdiag(detection.MeasurementNoise(1:2,1:2), L, detection.MeasurementNoise(3:4,3:4), L);     % 第 6 步：創(chuàng)建正確的過濾器。    % 使用 "KF "跟蹤 KF，使用 "EKF "跟蹤EKF，或使用 "UKF "跟蹤UKF    FilterType = 'EKF';     % 創(chuàng)建濾波:    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 * 狀態(tài)            % 在這種情況下            % 測量 = [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            %  注意，恒定加速度運(yùn)動(dòng)模型會(huì)自動(dòng)計(jì)算過程噪聲（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);    endend

4 處理和格式化檢測

記錄的信息必須經(jīng)過處理和格式化，才能被跟蹤模塊使用。這有以下幾個(gè)步驟。

1 過濾掉不必要的毫米波雷達(dá)探測到的雜物。毫米波雷達(dá)會(huì)報(bào)告許多與固定物體相對(duì)應(yīng)的物體，這些物體包括：護(hù)欄、道路中間線、交通標(biāo)志等。如果在跟蹤中使用了這些檢測結(jié)果，就會(huì)在道路邊緣產(chǎn)生固定物體的虛假軌跡，因此必須在調(diào)用跟蹤模塊之前將其刪除。毫米波雷達(dá)探測到的物體如果在車前靜止或在車附近移動(dòng)，則被認(rèn)為是非雜亂物體。

2 將探測結(jié)果格式化，作為跟蹤模塊的輸入，即objectDetection元素的數(shù)組。參見本例最后的processVideo和processRadar支持函數(shù)。

function [detections,laneBoundaries, egoLane] = processDetections...            (visionFrame, radarFrame, IMUFrame, laneFrame, egoLane, time)        % 輸入：        % visionFrame - 視覺傳感器在該時(shí)間段輸出的對(duì)象        % radarFrame - 該時(shí)間段毫米波雷達(dá)傳感器輸出的物體        % IMUFrame - 此時(shí)間幀的慣性測量單元數(shù)據(jù)        % laneFrame - 此時(shí)間幀的車道線輸出        % egoLane - 估計(jì)的被控車輛車道        % time - 與時(shí)間幀相對(duì)應(yīng)的時(shí)間         % 去除毫米波雷達(dá)探測的雜波對(duì)象        [laneBoundaries, egoLane] = processLanes(laneFrame, egoLane);        realRadarObjects = findNonClutterRadarObjects(radarFrame.object,...            radarFrame.numObjects, IMUFrame.velocity, laneBoundaries);         % 如果沒有輸出對(duì)象，則返回空列表         % 計(jì)算總的目標(biāo)數(shù)量        detections = {};        if (visionFrame.numObjects + numel(realRadarObjects)) == 0            return;        end         % 處理剩余的毫米波雷達(dá)目標(biāo)        detections = processRadar(detections, realRadarObjects, time);         % 處理視頻目標(biāo)        detections = processVideo(detections, visionFrame, time);    end

5 更新追蹤

要更新跟蹤，調(diào)用 updateTracks 方法，輸入如下內(nèi)容：

1 tracker - 之前配置的 multiObjectTracker；

2 detections - 由processDetections創(chuàng)建的objectDetection對(duì)象列表；

3 time - 當(dāng)前場景的時(shí)間。

跟蹤器的輸出是一個(gè)軌跡結(jié)構(gòu)數(shù)組。

找到最重要的對(duì)象并發(fā)出前向碰撞警告。

最重要的物體（MIO）被定義為在被控車道上且離車前最近的軌跡，即正x值最小。為了降低誤報(bào)的概率，只考慮確認(rèn)的軌跡。

一旦找到了MIO，就會(huì)計(jì)算汽車和MIO之間的相對(duì)速度。相對(duì)距離和相對(duì)速度決定了前撞預(yù)警。FCW有3種情況。

1 安全（綠色）：被控車道上沒有車（沒有MIO），MIO正在遠(yuǎn)離汽車，或者與MIO的距離保持不變；

2 小心（黃色）：MIO正在向汽車靠近，但距離仍高于FCW距離。FCW距離使用歐洲NCAP AEB測試協(xié)議計(jì)算。請(qǐng)注意，該距離隨MIO和汽車之間的相對(duì)速度而變化，當(dāng)關(guān)閉速度較高時(shí)，該距離較大；

3 警告（紅色）：MIO正在向汽車靠近，其距離小于FCW距離。

Euro-NCAP AEB測試規(guī)程定義了以下距離計(jì)算：

其中：

是前方碰撞警告距離;

是兩車之間的相對(duì)速度;

是最大減速度，定義為重力加速度g的40%。

function mostImportantObject = findMostImportantObject(confirmedTracks,egoLane,positionSelector,velocitySelector)         % 初始化輸出和參數(shù)        MIO = [];               % 默認(rèn)無目標(biāo)MIO        trackID = [];           % 默認(rèn)無trackID分配給MIO        FCW = 3;                % 默認(rèn)無MIO, FCW 是'安全的'        threatColor = 'green';  % 默認(rèn)威脅顏色是綠色        maxX = 1000;  % 足夠靠前，以確保軌跡不會(huì)超過這個(gè)距離        gAccel = 9.8; % 重力加速度 單位：m/s^2        maxDeceleration = 0.4 * gAccel; % Euro NCAP AEB 定義        delayTime = 1.2; % 駕駛員反應(yīng)時(shí)間，單位為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); % 本車道內(nèi)與目標(biāo)的相對(duì)速度             if x < maxX && x > 0 % 否則沒有必要監(jiān)測                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 % 相對(duì)速度表示物體越來越近                        % 根據(jù)Euro - NCAP AEB 測試規(guī)程計(jì)算預(yù)期制動(dòng)距離                        d = abs(relSpeed) * delayTime + relSpeed^2 / 2 / maxDeceleration;                        if x <= d  % '報(bào)警'                            FCW = 1;                            threatColor = 'red';                        else % '警告'                            FCW = 2;                            threatColor = 'yellow';                        end                    end                end            end        end        mostImportantObject = struct('ObjectID', MIO, 'TrackIndex', trackID, 'Warning', FCW, 'ThreatColor', threatColor);

6 總結(jié)

這個(gè)例子展示了如何為配備視覺、毫米波雷達(dá)和IMU傳感器的車輛創(chuàng)建一個(gè)前向碰撞預(yù)警系統(tǒng)。它使用objectDetection對(duì)象將傳感器報(bào)告?zhèn)鬟f給multiObjectTracker對(duì)象，multiObjectTracker對(duì)象將它們?nèi)诤?，并跟蹤被控車輛前方的物體。

大家可以嘗試為跟蹤模塊使用不同的參數(shù)，看看它們?nèi)绾斡绊懜欃|(zhì)量。嘗試修改跟蹤過濾器，使用 trackingKF 或 trackingUKF，或者定義不同的運(yùn)動(dòng)模型，例如，恒速或恒轉(zhuǎn)。最后，可以嘗試定義自己的運(yùn)動(dòng)模型。

（歡迎申請(qǐng)加入智能駕駛交流學(xué)習(xí)群，加小編微信號(hào)zhijiashexiaoming）