??這個例子matlab自動駕駛工具箱中關(guān)于使用傳感器融合的前方碰撞預(yù)警-(Forward Collision Warning Using Sensor Fusion)例子,其展示了如何通過融合視覺和雷達(dá)傳感器的數(shù)據(jù)來跟蹤車輛前方的物體,從而實現(xiàn)前向碰撞預(yù)警系統(tǒng)的開發(fā)與驗證。
1 概述
前方碰撞預(yù)警(FCW)是駕駛輔助系統(tǒng)和自動駕駛系統(tǒng)中的一項重要功能,其目標(biāo)是在即將與前車發(fā)生碰撞之前,向駕駛員提供正確、及時、可靠的警告。為了實現(xiàn)這一目標(biāo),車輛配備了面向前方的視覺和毫米波雷達(dá)傳感器,基于視覺和毫米波雷達(dá)融合是目前市場主流方案之一。為了充分利用視覺和毫米波雷達(dá)的各自的優(yōu)點,提高準(zhǔn)確警告的概率,并將錯誤警告的概率降到最低,需要進(jìn)行傳感器融合。
在本例中,一輛測試車(被控車輛)裝備了各類傳感器,并記錄了它們的輸出。本例中使用的傳感器有:
1 視覺傳感器,它提供了觀察到的物體列表及其分類和車道邊界的信息。對象列表每秒輸出10次。車道邊界每秒輸出20次;
2 毫米波雷達(dá)傳感器,具有中遠(yuǎn)距離模式,提供未分類的探測到的物體。物體清單每秒輸出20次;
3 慣性導(dǎo)航單元IMU,每秒輸出被控車輛的速度和橫擺角速度20次;
4 攝像機,它記錄了車前場景的視頻片段。注:這段視頻不被跟蹤器使用,只用于在視頻上顯示跟蹤結(jié)果,以便驗證。
提供前方碰撞預(yù)警的過程包括以下步驟:
1 獲取傳感器的數(shù)據(jù);
2 融合傳感器數(shù)據(jù),得到軌跡列表,即汽車前方物體的估計位置和速度;
3 根據(jù)軌跡和FCW標(biāo)準(zhǔn)發(fā)出警告。FCW標(biāo)準(zhǔn)基于Euro-NCAP AEB測試規(guī)程(目前最新的為2023版,是目前全球AEB FCW主導(dǎo)法規(guī)之一),并考慮到與車前物體的相對距離和相對速度。
本例中的可視化是使用monoCamera和birdsEyePlot完成的。為了簡潔起見,創(chuàng)建和更新顯示的函數(shù)被移到本例之外的函數(shù)中。后續(xù)相關(guān)文檔會講述。
本例是一個腳本,這里顯示的是主體,在后面的章節(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');
??
% 計算出初始的被控車輛的車道。
% 如果記錄的車道信息無效,則將車道邊界定義為汽車兩側(cè)各半個車道距離的直線
laneWidth?=?3.6;?%?米?
egoLane = struct('left', [0 0 laneWidth/2], 'right', [0 0 -laneWidth/2]);
%?準(zhǔn)備一些時間變量?
time?=?0;???????????%?記錄開始后的時間?
currentStep?=?0;????%?當(dāng)前步長?
snapTime?=?9.3;?????%?截取顯示屏快照的時間
%?初始化跟蹤模塊?
[tracker, positionSelector, velocitySelector] = setupTracker();
while?currentStep?<?numSteps?&&?ishghandle(videoDisplayHandle)
????%?更新場景計數(shù)器?
????currentStep?=?currentStep?+?1;?
????time?=?time?+?timeStep;
?????
% 將傳感器檢測結(jié)果作為物體檢測輸入處理,并將其發(fā)送給跟蹤模塊
[detections, laneBoundaries, egoLane] = processDetections(...
visionObjects(currentStep), radarObjects(currentStep), ...
inertialMeasurementUnit(currentStep), laneReports(currentStep), ...
????????egoLane,?time);
% 使用對象檢測列表,返回更新為時間的軌跡
confirmedTracks = updateTracks(tracker, detections, time);
??
% 找到最可能發(fā)生碰撞的目標(biāo)并計算前向碰撞預(yù)警時間
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)建多對象跟蹤模塊
multiObjectTracker根據(jù)視覺和毫米波雷達(dá)傳感器輸出的物體列表,跟蹤被控車輛四周的物體。通過融合兩個傳感器的信息,降低了錯誤碰撞預(yù)警的概率。
setupTracker函數(shù)會返回multiObjectTracker。當(dāng)創(chuàng)建一個multiObjectTracker,考慮如下:
1 FilterInitializationFcn。運動和測量模型。在這種情況下,預(yù)計物體會有一個恒定的加速度運動。雖然可以為這個模型配置一個線性卡爾曼濾波器,但initConstantAccelerationFilter配置了一個擴(kuò)展的卡爾曼濾波器。
2 AssignmentThreshold: 檢測到的數(shù)據(jù)與軌跡的距離。這個參數(shù)的默認(rèn)值是30。如果有一些檢測結(jié)果沒有被分配到軌跡上,但應(yīng)該被分配到軌跡上,則增加該值。如果有檢測結(jié)果被分配到太遠(yuǎn)的軌跡上,則降低此值。本例使用35。
3 DeletionThreshold。當(dāng)一條軌跡被確認(rèn)后,不應(yīng)該在第一次更新時刪除,因為沒有檢測分配給它。相反,它應(yīng)該被沿用(預(yù)測),直到很明顯該軌跡沒有得到任何傳感器信息來更新它。其邏輯是,如果軌跡在 Q 次中漏掉 P 次,則應(yīng)將其刪除。該參數(shù)的默認(rèn)值是5次中的5次。在這種情況下,跟蹤模塊每秒被調(diào)用20次,而且有兩個傳感器,所以沒有必要修改默認(rèn)值。
4 ConfirmationThreshold。確認(rèn)軌跡的參數(shù)。每次未分配的檢測都會初始化一個新的軌跡。其中有些檢測可能是假的,所以所有的軌跡都初始化為 "暫定"。要確認(rèn)一條軌跡,必須在N次軌跡更新中至少檢測到M次。M和N的選擇取決于對象的可見度。本例使用默認(rèn)的3次更新中檢測到2次。
setupTracker的輸出是
- tracker - 為本例配置的多對象追蹤模塊。
- positionSelector--指定狀態(tài)向量中哪些元素是位置的矩陣:position = positionSelector * State
- velocitySelector - 一個指定狀態(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)建一個卡爾曼過濾器(線性、擴(kuò)展或無痕)。然后,這個濾波器被用于跟蹤被控車輛周圍的每個對象。
function filter = initConstantAccelerationFilter(detection)
% 該函數(shù)展示了如何配置恒定加速度濾波器。輸入是探測對象,輸出是跟蹤濾波模塊。
%為清楚起見,此函數(shù)展示了如何為恒加速度配置 trackingKF、trackingEKF 或 trackingUKF。
% 創(chuàng)建濾波器的步驟:
% 1. 定義運動模型和狀態(tài)
% 2. 定義過程噪聲
% 3. 定義測量模型
% 4. 根據(jù)測量結(jié)果初始化狀態(tài)向量
% 5. 根據(jù)測量噪聲初始化狀態(tài)協(xié)方差
% 6. 創(chuàng)建正確的濾波器
% 第 1 步:定義運動模型和狀態(tài)
% 本例使用恒定加速度模型,因此:
STF = @constacc; % EKF 和 UKF 的狀態(tài)轉(zhuǎn)換函數(shù)
STFJ = @constaccjac; % 狀態(tài)轉(zhuǎn)換函數(shù)雅各布,僅適用于 EKF
% 運動模型意味著狀態(tài)為 [x;vx;ax;y;vy;ay] 。
% 還可以使用 constvel 和 constveljac 建立恒定速度模型,
% 使用 constturn 和 constturnjac 建立恒定轉(zhuǎn)率模型,或者編寫自己的模型。。
% 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 的測量函數(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é)方差。
% 對于非直接測量的狀態(tài)部分,將賦予一個較大的測量噪聲值,以考慮到這一點。
L = 100; % 相對于測量噪音而言是個大數(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
% 注意,恒定加速度運動模型會自動計算過程噪聲(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 處理和格式化檢測
記錄的信息必須經(jīng)過處理和格式化,才能被跟蹤模塊使用。這有以下幾個步驟。
1 過濾掉不必要的毫米波雷達(dá)探測到的雜物。毫米波雷達(dá)會報告許多與固定物體相對應(yīng)的物體,這些物體包括:護(hù)欄、道路中間線、交通標(biāo)志等。如果在跟蹤中使用了這些檢測結(jié)果,就會在道路邊緣產(chǎn)生固定物體的虛假軌跡,因此必須在調(diào)用跟蹤模塊之前將其刪除。毫米波雷達(dá)探測到的物體如果在車前靜止或在車附近移動,則被認(rèn)為是非雜亂物體。
2 將探測結(jié)果格式化,作為跟蹤模塊的輸入,即objectDetection元素的數(shù)組。參見本例最后的processVideo和processRadar支持函數(shù)。
function [detections,laneBoundaries, egoLane] = processDetections...
(visionFrame, radarFrame, IMUFrame, laneFrame, egoLane, time)
% 輸入:
% visionFrame - 視覺傳感器在該時間段輸出的對象
% radarFrame - 該時間段毫米波雷達(dá)傳感器輸出的物體
% IMUFrame - 此時間幀的慣性測量單元數(shù)據(jù)
% laneFrame - 此時間幀的車道線輸出
% egoLane - 估計的被控車輛車道
% time - 與時間幀相對應(yīng)的時間
% 去除毫米波雷達(dá)探測的雜波對象
[laneBoundaries, egoLane] = processLanes(laneFrame, egoLane);
realRadarObjects = findNonClutterRadarObjects(radarFrame.object,...
radarFrame.numObjects, IMUFrame.velocity, laneBoundaries);
% 如果沒有輸出對象,則返回空列表
% 計算總的目標(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對象列表;
3 time - 當(dāng)前場景的時間。
跟蹤器的輸出是一個軌跡結(jié)構(gòu)數(shù)組。
找到最重要的對象并發(fā)出前向碰撞警告。
最重要的物體(MIO)被定義為在被控車道上且離車前最近的軌跡,即正x值最小。為了降低誤報的概率,只考慮確認(rèn)的軌跡。
一旦找到了MIO,就會計算汽車和MIO之間的相對速度。相對距離和相對速度決定了前撞預(yù)警。FCW有3種情況。
1 安全(綠色):被控車道上沒有車(沒有MIO),MIO正在遠(yuǎn)離汽車,或者與MIO的距離保持不變;
2 小心(黃色):MIO正在向汽車靠近,但距離仍高于FCW距離。FCW距離使用歐洲NCAP AEB測試協(xié)議計算。請注意,該距離隨MIO和汽車之間的相對速度而變化,當(dāng)關(guān)閉速度較高時,該距離較大;
3 警告(紅色):MIO正在向汽車靠近,其距離小于FCW距離。
Euro-NCAP AEB測試規(guī)程定義了以下距離計算:
其中:
是前方碰撞警告距離;
是兩車之間的相對速度;
是最大減速度,定義為重力加速度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; % 足夠靠前,以確保軌跡不會超過這個距離
gAccel = 9.8; % 重力加速度 單位:m/s^2
maxDeceleration = 0.4 * gAccel; % Euro NCAP AEB 定義
delayTime = 1.2; % 駕駛員反應(yīng)時間,單位為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)的相對速度
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 % 相對速度表示物體越來越近
% 根據(jù)Euro - NCAP AEB 測試規(guī)程計算預(yù)期制動距離
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);
6 總結(jié)
這個例子展示了如何為配備視覺、毫米波雷達(dá)和IMU傳感器的車輛創(chuàng)建一個前向碰撞預(yù)警系統(tǒng)。它使用objectDetection對象將傳感器報告?zhèn)鬟f給multiObjectTracker對象,multiObjectTracker對象將它們?nèi)诤?,并跟蹤被控車輛前方的物體。
大家可以嘗試為跟蹤模塊使用不同的參數(shù),看看它們?nèi)绾斡绊懜欃|(zhì)量。嘗試修改跟蹤過濾器,使用 trackingKF 或 trackingUKF,或者定義不同的運動模型,例如,恒速或恒轉(zhuǎn)。最后,可以嘗試定義自己的運動模型。
(歡迎申請加入智能駕駛交流學(xué)習(xí)群,加小編微信號zhijiashexiaoming)