萬字聊聊ADAS多傳感器后融合

智駕最前沿 2023-02-22 08:30

告別射頻開關(guān)短板！MEMS 高速測試資料
一站式破解芯片研發(fā)全流程痛點，西門子 EDA Forum 2026等你來

--關(guān)注回復(fù)“SOA”--

↓↓領(lǐng)取：面向智能車輛開發(fā)的開放性SOA方案↓↓

前言

ADAS系統(tǒng)是一種高自動化的軟件應(yīng)用，對系統(tǒng)的魯棒性與可靠性要求很高，單一傳感器往往存在一定限制，此時便需要多傳感器融合。多傳感器融合會帶來如下收益：

可以在部分場景提升整體感知精度。
某一傳感器出現(xiàn)錯誤/失效時可以使用另一傳感器進(jìn)行補償。
多傳感器可以擴大FOV范圍。

在論文《A Review of Data Fusion Techniques》中對融合技術(shù)分類中對融合分成以下三類，可見與上面預(yù)期收益是對應(yīng)的：

協(xié)作型：多個輸入可以組成遠(yuǎn)比原始信息更復(fù)雜的信息。
冗余型：兩個或多個輸入提供同一個目標(biāo)信息用于增強可信度。
互補型：輸入為同一場景的不同部分用于形成完整的全局信息。
?

融合架構(gòu)

如下圖為不同論文介紹的融合架構(gòu)，雖命名方式有所不同，但基本可以分為以下兩類：

前融合：集中式架構(gòu)/low-level融合架構(gòu)。
后融合：分布式架構(gòu)/high-level融合架構(gòu)。

前融合的中心思想是將所有傳感器的原始信息(點云/圖像)全都傳給中心融合模塊，集中在融合模塊做跟蹤、關(guān)聯(lián)與估計。前融合可以使用深度學(xué)習(xí)訓(xùn)練網(wǎng)絡(luò)來實現(xiàn)，也可以對raw data進(jìn)行關(guān)聯(lián)與融合來跟蹤。后者對時間的同步性有較高的要求，否則會影響關(guān)聯(lián)的準(zhǔn)確性。

后融合的中心思想是每個傳感器使用其內(nèi)部的濾波與跟蹤算法，融合模塊做的就是對多個傳感器的濾波結(jié)果有效的結(jié)合起來。由于每個傳感器輸出已經(jīng)為目標(biāo)/track，因此此種融合也可以稱為目標(biāo)級別/track級別融合。

當(dāng)然我們也可以選擇一種折中的方法，稱為feature-level。中心思想為從raw data中提取特征信息后送入融合模塊進(jìn)行跟蹤融合。比如我們可以提取目標(biāo)的輪廓信息進(jìn)行擴展目標(biāo)跟蹤。主要缺點就是：接口不容易定義，不同傳感器可能提取的特征有差異，在有新增傳感器的情況下可能造成融合框架失效。

下圖為low-level、feature-level與high-level三種融合架構(gòu)的優(yōu)缺點對比：

可見后融合相比前融合有如下優(yōu)點：

解耦性好，接口可封裝，方便模塊化開發(fā)。
節(jié)省帶寬，可以降低成本。
對傳感器不敏感，可保證系統(tǒng)穩(wěn)定。
犧牲一定精度情況下，降低了算法復(fù)雜度。
方便問題定位回推，易于落地。

當(dāng)前業(yè)界主要采用的融合框架仍為后融合框架。Tier1一般不會放棄后融合架構(gòu)，因為在商業(yè)上來說后融合可以即插即用，單個產(chǎn)品可以賣給主機廠讓他們采購別的傳感器做拼接，主要還是得益于這個架構(gòu)的方便落地以及良好的可擴展性。主機廠則一般會在基于后融合框架的系統(tǒng)穩(wěn)定后，逐步向前融合探索，拿到更多的信息以提升精度。

RC后融合架構(gòu)一般可達(dá)成的效果(應(yīng)實現(xiàn)功能)：

目標(biāo)的位置、速度等信息精度不低于最好的傳感器。
大大增強系統(tǒng)魯棒性，如降低單camera傳感器的漏檢率/單Radar傳感器的虛警率；對傳感器模塊失效可容錯等。
?

接口

如上可知，后融合架構(gòu)的輸入為radar與camera跟蹤濾波后的目標(biāo)級別track信息，輸出為融合模塊維護的全局track。那全局track中一般包含哪些信息，不同公司有不同的接口設(shè)計。

如下為Apollo的目標(biāo)接口表：

	接口	描述
	id	sensor name
	polygon	目標(biāo)點云
track信息	track_id	track id
	tracking_time	跟蹤時間
	latest_tracked_time	最近一個更新的時間戳
motion 信息	motion_state	"UNKNOWN = 0, MOVING = 1,STATIONARY = 2"
	center	車中心點坐標(biāo)
	center_uncertainty	位置不確定度
	anchor_point	錨點
	velocity	速度
	velocity_uncertainty	速度不確定度
	velocity_converged	是否收斂的flag
	velocity_confidence	收斂可信度
	acceleration	三方向加速度
	acceleration_uncertainty	加速度不確定度
	direction	方向矢量
	theta	朝向角
	theta_variance	朝向角方差
	drops	最多100幀的歷史軌跡
feature 信息	size	長寬高
	size_variance	長寬高不確定度
	type	"UNKNOWN = 0, UNKNOWN_MOVABLE = 1,UNKNOWN_UNMOVABLE = 2,PEDESTRIAN = 3,BICYCLE = 4,VEHICLE = 5,MAX_OBJECT_TYPE = 6"
	type_probs	每個類別的概率
	sub_type	"UNKNOWN = 0, UNKNOWN_MOVABLE = 1,UNKNOWN_UNMOVABLE = 2,CAR = 3,VAN = 4,TRUCK = 5,BUS = 6,CYCLIST = 7,MOTORCYCLIST = 8,TRICYCLIST = 9,PEDESTRIAN = 10,TRAFFICCONE = 11,MAX_OBJECT_TYPE = 12,"
	sub_type_probs	每個細(xì)分類別的概率
	confidence	存在概率
其他flag	b_cipv	obj是否為CIPV
	car_light	"brake_visible = 0.0f brake_switch_on = 0.0fleft_turn_visible = 0.0fleft_turn_switch_on = 0.0fright_turn_visible = 0.0fright_turn_switch_on = 0.0f"

后融合策略

4.1 無記憶策略與有記憶策略

既然融合端要輸出全局track，那如何創(chuàng)建與維護它呢，一般有兩種融合策略：

無記憶融合策略
有記憶融合策略

如下圖為無記憶的融合策略，此框架中，不保留全局track的信息，傳感器級別的數(shù)據(jù)在預(yù)設(shè)定的融合周期時進(jìn)行融合。

融合端在kG-3時刻sensor i在ki-1時刻上報數(shù)據(jù)時，先不進(jìn)行融合，而將這部分?jǐn)?shù)據(jù)存儲下來，在KG-2時刻收到sensor j上報的數(shù)據(jù)時，再進(jìn)行融合生成全局track。自然kG-3時刻收到的sensor i的數(shù)據(jù)需要預(yù)測到kG-2時刻。如果有多個傳感器，那就依次融合。因為在每個融合周期，全局track都是新生成的，和上一融合周期的全局track沒有關(guān)系，所以此策略叫無記憶的融合策略。

如下圖為有記憶的融合策略，此框架保留全局track的信息，在傳感器數(shù)據(jù)來時，對全局track進(jìn)行預(yù)測后馬上融合傳感器數(shù)據(jù)。

融合端在kG-3時刻sensor i在ki-1時刻上報數(shù)據(jù)時，立刻就進(jìn)行融合了。而后將全局track數(shù)據(jù)進(jìn)行預(yù)測，在kG-2時刻sensor j的數(shù)據(jù)來了，再次進(jìn)行融合，以此類推。與無記憶策略相比，在于全局track在創(chuàng)建后進(jìn)行保留維護。此策略靈活性較強，具有“即插即用”的優(yōu)勢，因為不同傳感器數(shù)據(jù)之前被隔離開了，傳感器數(shù)據(jù)沒有直接接觸，都是間接通過全局track進(jìn)行接觸，所以添加與移除某一幀傳感器的數(shù)據(jù)不會對融合端造成較大影響。

當(dāng)前使用這兩種策略的廠商都有，Valeo一篇論文明確說明了他們使用的是無記憶融合策略，Apollo、BMW、huawei等公司使用的是有記憶的融合策略。

接下來主要介紹有記憶的融合策略，雖然這種策略使用起來不錯，但是會出現(xiàn)兩個問題，如下：

相關(guān)性問題
失序問題

相關(guān)性問題主要有兩種：1.過程噪聲相關(guān)性。2.歷史信息相關(guān)性。

過程噪聲相關(guān)性：一般融合都采用卡爾曼濾波框架進(jìn)行預(yù)測和更新，而一次卡爾曼濾波流程，添加過程噪聲次數(shù)應(yīng)為一次，而實際過程中，如果兩個sensor觀測到同一個物體，則這個物體在不同sensor中均加過一次過程噪聲，也就是兩次Qi與Qj，這是不對的。為了解決這種情況，一般需要將兩個sensor之間的交叉方差計算出來然后減去。

歷史信息相關(guān)性：如下圖所示在Ki-1時間時，sensori的測量信息第一次被更新在全局航跡中，此時全局航跡包含sensori從0時刻到Ki-1時刻所有的信息了。到了Ki時刻，sensori的測量又來了，此時測量含有0時刻到Ki時刻的所有信息，如果全局航跡直接使用這部分測量進(jìn)行更新，相當(dāng)于又一次融了一次0到Ki-1時刻的信息，實際需要的僅僅是Ki-1到Ki時刻的信息。

失序問題產(chǎn)生的原因在于測量數(shù)據(jù)可能存在延遲處理時間和通信延遲。舊數(shù)據(jù)進(jìn)入新目標(biāo)了，就卷起來了，引入了錯誤信息。如下圖所示，全局track應(yīng)該融合kj然后融合ki，但由于通訊延遲，變成了先融合ki再融合kj，這就出現(xiàn)了錯誤。

解決失序問題可用方法：

軟件架構(gòu)層面：及時消除緩存數(shù)據(jù)
框架層面：可以采用循環(huán)周期策略 + 預(yù)測傳感器數(shù)據(jù)到同一節(jié)點。(下面介紹)
算法層面：采用自適應(yīng)卡爾曼濾波/信息矩陣融合方法

4.2 循環(huán)周期策略與觸發(fā)式策略

在傳感器端將信息上報給融合端后，那融合端如何將融合后的結(jié)果上報給后續(xù)模塊呢，一般有兩種策略：

循環(huán)周期策略
觸發(fā)式策略

循環(huán)周期策略是根據(jù)傳感器的上報周期以及融合處理數(shù)據(jù)時間合理制定融合應(yīng)該的上報周期，比如上報周期就是60ms，那融合就每60ms發(fā)一次融合數(shù)據(jù)，風(fēng)雨無阻。

循環(huán)周期策略有兩種實現(xiàn)方式，在傳感器上報到融合端后，就馬上進(jìn)行融合，只不過不上報，最后在對應(yīng)時間周期到了時候再進(jìn)行上報。如下圖，在KG-3時刻上報后，Ki時刻sensor i上報信息，全局track進(jìn)行預(yù)測到KG-2時刻進(jìn)行融合更新，在Kj時刻sensor j上報信息，全局track預(yù)測到KG-1時刻進(jìn)行融合更新，最后預(yù)測到KG時刻進(jìn)行上報。

相比與上一種方式對全局track進(jìn)行操作的方式，第二種方式則對融合周期內(nèi)的傳感器端結(jié)果預(yù)測到上報節(jié)點，全局track在對應(yīng)時刻進(jìn)行分別融合更新。如下圖，KG-1時刻進(jìn)行融合結(jié)果上報后，對Ki時刻的sensor i數(shù)據(jù)預(yù)測到KG時刻，Kj時刻的sensor j數(shù)據(jù)預(yù)測到KG時刻，全局track 預(yù)測到KG時刻，使用預(yù)測后的傳感器數(shù)據(jù)進(jìn)行兩次關(guān)聯(lián)與更新。此方法不會出現(xiàn)失序問題。

觸發(fā)式策略是每次傳感器來，就做一次融合，而后將融合后的結(jié)果上報。所謂觸發(fā)就是融合上報是被傳感器上報數(shù)據(jù)所觸發(fā)，故得到此名字。

Apollo使用的是一種觸發(fā)式策略，在Apollo 7.0中其選擇了一個主傳感器Lidar，只有在Lidar信息輸入后，融合端在融合后才會上報。這樣在傳感器均正常工作的情況下，融合的上報周期和Lidar周期相同。

相比與循環(huán)周期策略，觸發(fā)式策略更加靈活，容易處理，但是在某一傳感器掛掉時候，融合對應(yīng)的也掛掉了，這并不符合量產(chǎn)的系統(tǒng)穩(wěn)定性需求。而循環(huán)周期測量的兩種方法，精度會有差別(不同的預(yù)測手段導(dǎo)致)，后者則會在問題回歸定位時方便，可直接采用幀號對齊。

一般情況下，有記憶策略與循環(huán)周期策略/觸發(fā)式策略均可配套使用，但是無記憶策略通常與循環(huán)周期策略配套使用。原因可以理解，觸發(fā)式策略在任意傳感器來時候進(jìn)行融合而后上報，無記憶策略由于不維護全局track，需要等待兩個傳感器數(shù)據(jù)到齊后進(jìn)行融合，二者結(jié)合不是很合適。

后融合處理流程

如下圖為基本RC后融合系統(tǒng)流程圖，接下來將介紹各個模塊：

如下圖為Apollo融合部分代碼UML圖，代碼層序清晰，BaseDataAssociation、BaseTracker、BaseGateKeeper三部分分別對應(yīng)數(shù)據(jù)關(guān)聯(lián)、track預(yù)測更新以及特征融合、航跡管理。

5.1 數(shù)據(jù)對齊

在進(jìn)行關(guān)聯(lián)操作之前，需要將信息統(tǒng)一到相同的坐標(biāo)系以及時間節(jié)點，也就是時空間轉(zhuǎn)換。

5.1.1 空間轉(zhuǎn)換

因為后融合是目標(biāo)級別融合，輸入以及輸出都是點。但是自車以及目標(biāo)車都是有size的，此時需要統(tǒng)一接口定義?？v向距離和橫向距離究竟指的是自車的什么位置到目標(biāo)車的什么位置的距離，比如究竟是自車的幾何中心/前保險杠到目標(biāo)車的幾何中心/后軸中心的距離。

傳感器端應(yīng)定義上報給融合端的距離為傳感器安裝位置到目標(biāo)車哪個位置的距離，例如可定義：

camera上報橫縱向距離為camera安裝位置到目標(biāo)車幾何中心的位置。
radar上報橫縱向距離為radar安裝位置到目標(biāo)車車后軸中心的位置。

因為不同傳感器的安裝位置不同，應(yīng)將其統(tǒng)一到車體坐標(biāo)系中，一般為自車幾何中心。一般會有標(biāo)定矩陣，乘上去就好，公式如下：

5.1.2 時間對齊

時間對齊容易理解，fusion與measure必須在同一時間下，如果差了一個△t，就是用卡爾曼濾波中的運動學(xué)外推即可。CV模型比較合適，當(dāng)然CTRV也可以，那就需要EKF了。

一般這部分位移差都會補上去，但是也有公司-Valeo在論文中認(rèn)為如果△t比較小不用補也可以，因為這補償?shù)奈⑿∥灰撇顣辉紲y量噪聲所淹沒。

5.2 航跡預(yù)測

對于有記憶的融合策略，需要維護全局track，在有傳感器測量上報后，需要從上一時間節(jié)點預(yù)測到此時時間節(jié)點，而后進(jìn)行關(guān)聯(lián)，選擇到測量后進(jìn)行更新。其實就是一個關(guān)聯(lián)模塊插入到濾波過程中。常見的預(yù)測模型如下：

CV：勻速模型。假設(shè)目標(biāo)車直行，且速度不變。線性模型。
CA：勻加速度模型。假設(shè)目標(biāo)車直行，且加速度不變。線性模型。
CTRV：恒定速度與轉(zhuǎn)速模型。假設(shè)目標(biāo)車轉(zhuǎn)彎，且速度以及角加速度不變。非線性模型。
CTRA：恒定加速度與加速度模型。假設(shè)目標(biāo)車轉(zhuǎn)彎，且加速度以及角加速度不變。非線性模型。

由于CTRV以及CTRA模型認(rèn)為速度v與角速度w是沒有相關(guān)性，所以即使速度為0，加速度也會改變yaw角，相當(dāng)于原地轉(zhuǎn)圈，這是不合常理的。

為了解決上述問題，有CCV以及CCA模型。這兩個模型假定運動的曲率半徑一定，曲率半徑的計算公式如下，可見曲率半徑變量補充速度v與角速度w的相關(guān)性?？捎胏 = 1/R放到變量中參與濾波。

具體這些預(yù)測模型相關(guān)關(guān)系如下：

在論文《Comparison and Evaluation of Advanced Motion Models for Vehicle Tracking》中作者對比了如上不同的模型(更新全都用的UKF)，得到精度對比表如下：

文中得到結(jié)論如下：

加速度的引入有助于提升精度。
CTRV與CTRA模型在角速度w>2度/s時候相比CV與CA模型表現(xiàn)提升。
CCA與CTRA模型性能基本無提升，原因在于非線性程度高，計算誤差大。下圖為二者在同一轉(zhuǎn)彎場景誤差對比。

在實際研發(fā)過程中可以得到的結(jié)論如下：

CV模型：可適合絕大部分場景，計算復(fù)雜度低且效果良好，性價比高。但對于stop&go場景會出現(xiàn)問題，尤其是急剎，這會影響功能。
CA模型：對加速度的準(zhǔn)確性需求上升，可解決剎車等場景的問題。在加速度不出問題情況下，直行用此模型就可以。缺點就是對彎道場景有局限性，損失一定精度。
CTRV：對航向角準(zhǔn)確性需求上升，直行場景如果航向角錯誤/橫向速度錯誤，可能導(dǎo)致預(yù)測到其他車道，導(dǎo)致功能性問題。
CA+CT+IMM算法：綜合二者優(yōu)點，保證直行性能基礎(chǔ)上可在在彎道獲得收益，但具有滯后性。

5.3 數(shù)據(jù)關(guān)聯(lián)

目標(biāo)級別關(guān)聯(lián)一般遵循以下兩個假設(shè)：

每個測量目標(biāo)僅屬于一個track，如果它不屬于任何一個track，那它就是虛警。
每個track如果可和多個測量相關(guān)聯(lián)，那只有一個是正確的，其他都是錯誤的。

以上兩個假設(shè)將關(guān)聯(lián)問題簡化，關(guān)聯(lián)問題變?yōu)橐粚σ坏淖顑?yōu)問題。

熟悉多目標(biāo)跟蹤的大家都知道，相比于單目標(biāo)跟蹤，其會出現(xiàn)一個復(fù)雜的問題就是分配問題，如下圖所示，兩個global track T1與T2，落入T1關(guān)聯(lián)門限的測量有Z2、Z3，落入T2關(guān)聯(lián)門限的測量有Z1、Z2和Z3，不難計算他們有10種關(guān)聯(lián)的可能，而只有一種是正確的。

數(shù)據(jù)關(guān)聯(lián)模塊一般算法步驟為：

設(shè)定門限(如上橢圓范圍)
計算全局track與門限范圍內(nèi)每個測量的相似度。
分配算法以實現(xiàn)全局最優(yōu)

根據(jù)經(jīng)驗可以知道，距離自車40m的全局track不能與距離自車100m的測量關(guān)聯(lián)上，所以就沒必要計算這二者的相似度而后進(jìn)入分配算法。步驟1設(shè)定門限的目的便是在此，對于不在門限范圍內(nèi)的測量，系統(tǒng)將不認(rèn)為其應(yīng)該與對應(yīng)全局track關(guān)聯(lián)上，二者相似度可設(shè)為極大值，不參與后續(xù)分配。這樣做優(yōu)點可以降低算法復(fù)雜度，提升效率。

一個好的門限選取是重要的，因為：

過大的門限會在雜波較多時候放入過多目標(biāo)，造成算力提升并提升關(guān)聯(lián)錯誤的概率。
過小的門限會使得正確的關(guān)聯(lián)目標(biāo)被卡在門限外面，會導(dǎo)致關(guān)聯(lián)不上的情況。

門限選取一般根據(jù)全局track與測量的距離差、速度差進(jìn)行設(shè)定，同時根據(jù)傳感器特性進(jìn)行調(diào)整，一般基本思想如下：

全局track距離自車越遠(yuǎn)距離門限應(yīng)該放的越大。由于camera測距不準(zhǔn)，但其角度測量準(zhǔn)確，所以可設(shè)定對應(yīng)角度門限，如果滿足角度門限，可對camera測量放大距離門限。
速度門限也應(yīng)隨著全局track距離自車的距離進(jìn)行變化，同時考慮camera存在速度收斂的情況，可根據(jù)跟蹤camera測量的幀數(shù)適當(dāng)放大速度門限，

關(guān)于計算全局track與門限內(nèi)測量的相似度，其用途就是構(gòu)建一個track與測量的相似度矩陣，有很多種方法去計算，基本思想就是我們希望track與測量的state差距越小，相似度越大。以下是一些計算方法：

對X、Y、Vx、Vy的歐式/馬氏距離差進(jìn)行一定規(guī)則的權(quán)重加和。
使用JPDA/CJPDA算法前部分計算概率。
根據(jù)高斯分布等特性，利用卡方分布查表獲得相似度。(Apollo)

關(guān)于分配算法，其實此數(shù)據(jù)關(guān)聯(lián)就是個二分圖的最大匹配問題。

常用方法：

findbest方法
拍賣算法
匈牙利匹配
KM算法

關(guān)于find best方法其實就是對相似度矩陣進(jìn)行一次排序，安裝相似度從大到小進(jìn)行分配。相比原始的NN算法，其不會因為obj排序不同而導(dǎo)致不同的關(guān)聯(lián)結(jié)果。

根據(jù)一些仿真結(jié)果，基本結(jié)論是拍賣算法時間復(fù)雜度相對較小，但存在大顆粒度問題，可能達(dá)不到全局最優(yōu)。匈牙利匹配存在不收斂情況，穩(wěn)定性存疑。

在實際工程中，數(shù)據(jù)關(guān)聯(lián)模塊如果按照上述流程，同樣會出現(xiàn)很多穩(wěn)定性問題。我們希望的系統(tǒng)應(yīng)該是有穩(wěn)定的關(guān)聯(lián)，不會總出現(xiàn)跳變，所以需要一定的邏輯來維持其關(guān)聯(lián)的穩(wěn)定性?？梢圆捎萌缦路绞剑?/span>

當(dāng)一個track和對應(yīng)的RC關(guān)聯(lián)超過100幀就將其直接關(guān)聯(lián)起來作為強綁定邏輯。
使用CJPDA算法+alpha濾波跟蹤落入門限中測量的關(guān)聯(lián)概率，滿足條件再關(guān)聯(lián)切換，可作為弱綁定邏輯。

5.4 航跡更新

經(jīng)過前面數(shù)據(jù)關(guān)聯(lián)模塊，全局track已經(jīng)和R/C的測量關(guān)聯(lián)上，如何使用二者的state以及cov矯正全局track的state便是此模塊的工作。

一般更新方法如下：

KF及變種
Convex Combination 、Covariance Intersection

Julier S, Uhlmann J K. General decentralized data fusion with covariance intersection[M]//Handbook of multisensor data fusion. CRC Press, 2017: 339-364.

Information Matrix Fusion

M. Aeberhard, A. Rauch, M. Rabiega, N. Kaempchen and T. Bertram, "Track-to-track fusion with asynchronous sensors and out-of-sequence tracks using information matrix fusion for advanced driver assistance systems," 2012 IEEE Intelligent Vehicles Symposium, 2012, pp. 1-6, doi: 10.1109/IVS.2012.6232115.

卡爾曼濾波算法是最普遍的方法，將傳感器層面上報的track作為輸入，但是卡爾曼濾波基于的假設(shè)是測量之間彼此獨立，但由于傳感器層track之間有共同的歷史觀測，測量之間具有相關(guān)性了，違反了彼此獨立的假設(shè)，所以使用KF濾波無法獲得無偏估計。

自適應(yīng)卡爾曼濾波則可應(yīng)用于此場景：

預(yù)測：

更新：

而且Bar-Shalom retrodiction算法被成功用于此算法中去解決失序問題。

A. Novoselsky, S. E. Sklarz and M. Dorfan, "Track to track fusion using out-of-sequence track information," 2007 10th International Conference on Information Fusion, 2007, pp. 1-5, doi: 10.1109/ICIF.2007.4408008.

CC算法是一種最簡單的融合方法，算法如下。中心思想就是使用協(xié)方差做了一次加權(quán)。

如果考慮測量之間的相關(guān)性，則可對CC算法進(jìn)行一定的變形，如下：

可以看出其需要計算cross-covariance matrix，一般計算方法如下，參數(shù)取0.4是一個不錯的選擇。

CI算法則在考慮了相關(guān)性的同時，不需要顯示計算cross-covariance matrix，算法流程如下：

其中w的計算方式一般為：

論文中提到，此方法融合的協(xié)方差矩陣中最大化的保留了測量之間的相關(guān)信息。

Information Matrix Fusion方法是BMW公司主要使用的一個方法，其主要可以解決記憶框架的歷史相關(guān)性問題。如下圖所示，在KG-2時刻，global track含有sensor i 的從0到Ki-1時刻的信息，在KG-1時刻，融合Kj時候，在解決掉兩傳感器的測量噪聲相關(guān)性之后，global track含有從0到Kj時刻sensor i與sensor j的信息。主要問題就在KG時刻。KG時刻如果再次直接融合Ki傳來的數(shù)據(jù)，就多融合了一遍sensor i在0到Ki-1時刻的信息，而我們真正需要的信息是Ki-1時刻到Ki時刻的協(xié)方差信息，所以信息矩陣融合想到的方法就是，減下去就完事了。

算法流程如下：

更新協(xié)方差：

更新state：

可以看出此方法需要存儲上一幀傳感器的state以及協(xié)方差矩陣，這是一個缺點。

下圖為論文中三種方法的精度以及NEES對比圖。

Apollo的方法當(dāng)前僅使用KF濾波加一些自己的邏輯，比如根據(jù)track質(zhì)量調(diào)整了協(xié)方差、對主sensor降低協(xié)方差、限幅等操作?？梢钥闯銎渲饕ＷC系統(tǒng)的穩(wěn)定性與可控性，而非理論最優(yōu)。

以上的方法其實都是在xy方向上進(jìn)行的更新，業(yè)界在更加信任哪個傳感器上也有自己的考量，使用策略調(diào)整來調(diào)整傳感器的協(xié)方差，如下：

xy方向進(jìn)行位置更新，縱向信任radar，橫向信任camera。
信任R的徑向距離+camera的角度(Ti)。
信任近處的目標(biāo)。可能造成功能過早觸發(fā)。
frenet方法進(jìn)行更新：結(jié)合車道線信息，轉(zhuǎn)換到frenet坐標(biāo)系上，進(jìn)行更新。此方法會在彎道產(chǎn)生收益。因為傳統(tǒng)坐標(biāo)系可能出現(xiàn)過大彎道更新后踩線或更新到別的車道的情況。但是此方法對車道線質(zhì)量有依賴。

5.5 特征融合

5.5.1 長寬融合

業(yè)界當(dāng)前對長寬的融合基本都需要Lidar傳感器，可分為以下兩種：

基于占位柵格的方法
基于模型的方法

Apollo對于長寬融合采用的策略如下圖，可看出Apollo的邏輯是，有l(wèi)idar就信lidar，沒lidar信任使用3D檢測模型的camera，radar壓根不信任。

在RC融合中，camera上報長寬(高)一般基于3D bounding box的角點進(jìn)行一定解算后獲得。CIPV的長度信息無法保證，一定距離外精度無法保證；radar因其特性，點云稀疏，無法擬合處應(yīng)有形狀特征，上報的長寬相比camera不具備優(yōu)勢，一般根據(jù)其類別結(jié)果放一個默認(rèn)值上去。

融合端可以camera上報的長寬為主，在此基礎(chǔ)上抑制波動、盡量提升精度。融合理論上相比camera有更準(zhǔn)確的距離信息以及類別信息，若camera解算size時與其本身估計的距離相關(guān)(小孔成像遠(yuǎn)離)，則可根據(jù)融合端更準(zhǔn)確的距離做一定矯正；根據(jù)類別信息做合理性判別。

5.5.2 存在概率融合

前面關(guān)于state、類別、長寬的估計的用途都很好理解。那存在概率是用來干什么的呢，有什么用呢？這是由于在實際系統(tǒng)中，傳感器上報的目標(biāo)很可能是虛警(false positive)，融合端需要有一個概率來確定這個目標(biāo)是否真的存在，在概存在概率低于某一個閾值時系統(tǒng)可以將這個目標(biāo)濾除，以防功能端誤剎、CIPV誤選擇。

RC融合中，radar由于其傳感器特性(多徑、鏡像、受到護欄影響)很容易上報虛警，雖然radar內(nèi)部有算法會對虛警進(jìn)行抑制，但無法解決所有，這時候融合可根據(jù)camera的信息進(jìn)行進(jìn)一步抑制，這也是融合存在的價值之一。

上述可知：存在概率與航跡管理模塊強相關(guān)。同時可以與功能端共同建立一套基于概率的控制系統(tǒng)。

融合端應(yīng)該得到RC分別上報的存在概率后進(jìn)行融合。而傳感器端上報的存在概率是通過他們跟蹤過程的質(zhì)量算出來，一般計算方法一般有如下兩種：

數(shù)據(jù)關(guān)聯(lián)算法有計算：IPAD與JIPDA算法

D. Musicki, R. Evans and S. Stankovic, "Integrated probabilistic data association," in IEEE Transactions on Automatic Control, vol. 39, no. 6, pp. 1237-1241, June 1994, doi: 10.1109/9.293185.
D. Musicki and R. Evans, "Joint integrated probabilistic data association: JIPDA," in IEEE Transactions on Aerospace and Electronic Systems, vol. 40, no. 3, pp. 1093-1099, July 2004, doi: 10.1109/TAES.2004.1337482.

廣義貝葉斯框架(BMW)

M. Aeberhard, S. Paul, N. Kaempchen and T. Bertram, "Object existence probability fusion using dempster-shafer theory in a high-level sensor data fusion architecture," 2011 IEEE Intelligent Vehicles Symposium (IV), 2011, pp. 770-775, doi: 10.1109/IVS.2011.5940430.

其實這兩種方法通過推導(dǎo)殊途同歸，只不過在建模參數(shù)采用的模型相同，計算方法是相同的。BMW的文章通過建模Pp-持續(xù)概率、Pb-出生概率、Pd-檢測概率以及Pc-雜波概率，對不同的傳感器引入不同的建模方式，計算出的存在概率可與以下目標(biāo)特性有關(guān)：

目標(biāo)的所處位置(是否要出最大檢測距離/出FOV)
目標(biāo)的關(guān)聯(lián)狀態(tài)(關(guān)聯(lián)是否穩(wěn)定)
傳感器是否易產(chǎn)生雜波

當(dāng)然我們也可以通過車道線與路邊沿約束降低不合理目標(biāo)的存在概率。

那融合端拿到傳感器端的存在概率，采用DS證據(jù)理論的方法去融合是業(yè)界的主要方法。如下圖所示：

我們設(shè)定DS的判別空間為[存在、不存在]，而后調(diào)用DST流程融合即可。Apollo就采用這種方法。

RC融合過程中，由于radar可以檢測到被遮擋的目標(biāo)，然而camera卻無法檢測到，以上的power set的分配方式會造成被遮擋的存在的目標(biāo)在使用radar進(jìn)行融合時候，提升存在概率，而在融合camera時降低存在概率，造成沖突。

為解決上述問題，可以將遮擋信息引入來擴大DS的判別空間，判別空間擴大為[遮擋時存在、未遮擋時存在、不存在]，可解決以上問題。BMW采用此種方法。

5.6 航跡管理

5.6.1 航跡起始

融合端在接受到新的、未出現(xiàn)過的測量時，此測量有兩種可能，一為新檢測到的真實目標(biāo)，二為虛警。融合端不可能一接收到新測量就開始起始全局track進(jìn)行跟蹤，這樣會引入大量的虛警目標(biāo)，造成誤剎等功能性錯誤。也不能確認(rèn)很久仍未起始，這樣可能會漏報目標(biāo)，可能會撞上去。當(dāng)前主要的航跡起始方法包含以下集中：

邏輯法：M/N邏輯。在N幀中如果有M幀觀測到該目標(biāo)就起始。一般為2/3邏輯，3幀中有2幀觀測到就起始。
SPRT：建模目標(biāo)存在的似然函數(shù)P0和不存在的似然函數(shù)P1，計算二者的比值，超過門限就起始。
IPAD算法：計算目標(biāo)的存在概率，可以用來做航跡的起始以及終結(jié)。主要思想為根據(jù)預(yù)測狀態(tài)的位置信息和實際傳感器的測量信息計算偏差，動態(tài)計算測量落入關(guān)聯(lián)門限的概率，進(jìn)而迭代增加或減少存在的概率，在概率超過一定閾值時進(jìn)行起始。由于IPAD本身也包含關(guān)聯(lián)算法，方便與后面數(shù)據(jù)關(guān)聯(lián)方法結(jié)果進(jìn)行對比。

論文中：在強雜波的環(huán)境中，起始效果IPAD>SPRT>M/N

在實際系統(tǒng)中，在傳感器性能良好的情況下，M/N方法仍為簡單可控且表現(xiàn)不錯的方法。

SPRT方法中，Pd、Pf 、alpha、beta等調(diào)參復(fù)雜，可能出現(xiàn)目標(biāo)長時間無法刪除的情況。調(diào)整好參數(shù)情況下，與其他的方法差距不大。但是在FOV附近可以通過對FOV附近以及非FOV附近調(diào)整設(shè)置不同參數(shù)，使得在非FOV附近目標(biāo)正常上報，在FOV附近目標(biāo)延遲上報等增益。

IPAD法由于存在概率與track的運動狀態(tài)有關(guān)系，對于radar存在持續(xù)上報的虛警，如果目標(biāo)運動狀態(tài)不穩(wěn)定，相比N/M方法，能夠延緩起始；可以根據(jù)加減分方法，靈活調(diào)整radar與camera的參數(shù)，應(yīng)用于FOV附近與非FOV區(qū)域目標(biāo)。

5.6.2 航跡合并

所謂merge目標(biāo)，即為合并單R與單C目標(biāo)。若radar與camera上報穩(wěn)定的目標(biāo)，二者無法在初始時刻在融合端關(guān)聯(lián)上，分成了兩個目標(biāo)進(jìn)行起始。融合端應(yīng)該提供在二者生命周期內(nèi)，若可關(guān)聯(lián)上便將其合并為一個目標(biāo)的功能。

就算單R目標(biāo)不上報，當(dāng)仍需要此功能。若無此功能，單C目標(biāo)在整個生命周期內(nèi)便一直無法再與radar目標(biāo)關(guān)聯(lián)上，會損失精度。

若單R與單C目標(biāo)關(guān)聯(lián)上，在合并為一個目標(biāo)的過程中，要防止跳變。不能出現(xiàn)一下子從Camera位置跳到radar那邊的情況。

5.6.3 航跡降級

RC融合中，global track可能為RC的融合目標(biāo)、單R目標(biāo)、一般情況下，對于單R與單C目標(biāo)會與融合目標(biāo)有不同的處理邏輯(透傳)。所以一般會對全局track維護一個objSource接口表示目標(biāo)為何種融合方式，而后根據(jù)objSource選擇不同的邏輯。

融合端便要維護objSource接口，當(dāng)單C/單R目標(biāo)關(guān)聯(lián)上互補傳感器后形成RC融合目標(biāo)后，objSource升級，這可以理解。而我們也應(yīng)有一定邏輯將track級別降下來，因為RC融合目標(biāo)在某一個傳感器消亡后，將退化為單C/R目標(biāo)。

若某一幀R/C的測量未出現(xiàn)，便直接將融合目標(biāo)降級為單傳感器目標(biāo)是欠考量的。傳感器上報的測量可能會閃爍，如果使用這種策略會造成objSource不斷跳變，從而導(dǎo)致選擇邏輯不斷切換，影響上報的state。

可以看出系統(tǒng)應(yīng)維護objSource的穩(wěn)定。一般業(yè)界在某一幀測量沒來時不會降級，而是維持融合狀態(tài)，并在關(guān)聯(lián)模塊記錄R/C在關(guān)聯(lián)期間未關(guān)聯(lián)上的幀數(shù)，而后可以使用邏輯，在合適時候?qū)⑵浣导墶?/span>

5.6.4 航跡刪除

消亡的全局track應(yīng)將其從維護的track列表刪除。因為傳感器可能出現(xiàn)閃爍、因為遮擋暫時未上報等情況，不可在全局track某一幀均未關(guān)聯(lián)上RC測量便將其刪除。

一般維持3幀是一個比較合適的選擇。

5.6.5?航跡上報

上報模塊為融合與后端的最后模塊，可以將其理解為閘門。之前融合端起始跟蹤的全局track均會經(jīng)過此模塊來確定是否要報給后端。

上報與不上報與全局track起始和刪除是不同的，后者主要是融合端內(nèi)部自嗨，而前者就屬于匯報階段了。

上報模塊是非常重要的，因為融合端信息通過此模塊后便要對功能負(fù)責(zé)。一個合適的上報模塊應(yīng)該會讓穩(wěn)定可靠的目標(biāo)快速上報，而對于錯誤/不穩(wěn)定目標(biāo)有過濾作用(融合意義所在)。

一般情況此上報模塊應(yīng)有一個參考的數(shù)值作為衡量track是否存在且穩(wěn)定可靠的指標(biāo)，前文提到的存在概率便是一個合適指標(biāo)，前提是需要經(jīng)過合理的建模。僅僅一個數(shù)值無法cover所有場景，此時需要一些硬邏輯來識別錯誤目標(biāo)延遲上報(問題驅(qū)動)。

Valeo在論文中提出過一個全局可信任分?jǐn)?shù)來作為上報模塊，其實也就是存在概率，但是有它自己的一套建模方式。

難點

6.1 毫米波雷達(dá)害怕靜止車輛

從最底層的工作原理來說，毫米波雷達(dá)主要是依靠多普勒效應(yīng)來感知移動目標(biāo)。多普勒效應(yīng)的特性是動態(tài)對動態(tài)最容易感知，動態(tài)對靜態(tài)較難感知、靜態(tài)對靜態(tài)極難感知。這是因為如果前方車輛靜止，目標(biāo)信息容易和地雜波等摻雜在一起，需要一定的算法才能從中分辨出目標(biāo)。而如果是一個行駛中的汽車，基于其多普勒信息，從而比較好探測到目標(biāo)。

6.2 毫米波雷達(dá)沒有高度信息，同時空間分辨率不足

沒有高度信息，意味著雷達(dá)很難區(qū)分橫穿馬路的路牌和橋下的車；空間分辨率不足，意味著兩個距離很近的物體，其回波會被混在一起，很難知道有幾個目標(biāo)。如下為路中央的路牌以及其下的車。

2021.1.25 ES8在沈海高速碰撞護欄邊正在放置三角警示牌的行人然后碰撞邊上爆胎的五菱宏光問題

這個場景中五菱宏光車身和護欄挨得很近，并且都是靜止?fàn)顟B(tài)，這個時候雷達(dá)對于這兩個目標(biāo)的區(qū)分就得依靠角分辨率來實現(xiàn)了。在較遠(yuǎn)的距離雷達(dá)反射點混在一起，要到很近的距離才能區(qū)分護欄和車輛。

如果很難區(qū)分，把靜態(tài)目標(biāo)錯誤的識別為車輛，然后進(jìn)行剎車會嚴(yán)重影響用戶體驗，甚至增加事故，所以一些雷達(dá)公司和自動駕駛公司會選擇將靜態(tài)物體（包括車）過濾掉，來減少誤觸發(fā)的情況。

當(dāng)前博世、大陸等企業(yè)，已經(jīng)可以通過不同物體RCS反射面積的不同和不同幀之間的反射點的不同來區(qū)分路牌、立交橋和車輛。但準(zhǔn)確率并不算高。

利用RCS直接進(jìn)行分類仍是不現(xiàn)實的，因為不同形狀、材質(zhì)的物體RCS都不相同。而即使是同一個物體，不同角度的RCS也不相同，車載場景下變量太多暫時很難以簡單通過RCS來確定一個物體的類型。

注意事項

融合端搭建出一套可使用的系統(tǒng)是較為容易的，而且這套系統(tǒng)還可解決70%的場景，然而如何將70%場景做到90%甚至95%則是困難的，需要大量的路測數(shù)據(jù)進(jìn)行調(diào)試。

融合需要構(gòu)建場景的白名單與黑名單，明確融合的能力邊界，確定這個邊界因什么而被限制，哪些可以隨后續(xù)升級而放開。而后續(xù)升級是應(yīng)該sensor升級性能還是融合這邊進(jìn)行cover，如果是融合這邊cover，對sensor又有哪些依賴，這些都是需要明確的。對于sensor解決不了的場景，融合要有策略進(jìn)行兜底。

融合應(yīng)梳理不同場景傳感器的性能基線，總結(jié)特性并在融合端更好的進(jìn)行邏輯預(yù)埋。傳感器特性例子如下：

radar對近處目標(biāo)、行人、truck目標(biāo)檢測能力比較弱。如果目標(biāo)旁邊有路沿，那這個目標(biāo)存在可信度降低。radar對靜止目標(biāo)檢測能力弱，可能檢測不到或閃爍。
Cut in場景應(yīng)該camera比radar更早的識別，因為DL；FOV附近，camera橫漂嚴(yán)重；camera對ax測量不準(zhǔn)。

對融合架構(gòu)來說，應(yīng)該擴展性良好，模塊間耦合性低。支持功能的升級擴展與降級裁剪。

而后續(xù)測試出來了問題場景，我們應(yīng)該有方法能確定這個問題是方案類問題還是極端場景問題，做好分類歸檔。

轉(zhuǎn)載自知乎@Minne，文中觀點僅供分享交流，不代表本公眾號立場，如涉及版權(quán)等問題，請您告知，我們將及時處理。

-- END --

登錄閱讀全文



免責(zé)聲明：該內(nèi)容由專欄作者授權(quán)發(fā)布或作者轉(zhuǎn)載，目的在于傳遞更多信息，并不代表本網(wǎng)贊同其觀點，本站亦不保證或承諾內(nèi)容真實性等。若內(nèi)容或圖片侵犯您的權(quán)益，請及時聯(lián)系本站刪除。侵權(quán)投訴聯(lián)系：? nick.zong@aspencore.com！

智駕最前沿「智駕最前沿」深耕自動駕駛領(lǐng)域技術(shù)、資訊等信息，解讀行業(yè)現(xiàn)狀、緊盯行業(yè)發(fā)展、挖掘行業(yè)前沿，致力于助力自動駕駛發(fā)展與落地！公眾號：智駕最前沿

進(jìn)入專欄

評論

文章：2506篇粉絲：42人

 私信