面向停車場場景的多傳感器融合匹配算法與融合數(shù)據(jù)的并行處理

寶鶴鵬，陳超，王磊

（中汽數(shù)據(jù)（天津）有限公司，天津300380）

0 引言

可大規(guī)模量產(chǎn)的自動泊車是當(dāng)今無人駕駛領(lǐng)域重要的商業(yè)落地項(xiàng)目。其中基于停車場場景數(shù)據(jù)采集相關(guān)的測試與場景的累積工作是實(shí)現(xiàn)自動泊車量產(chǎn)的重要工作之一。對自動駕駛本身而言，傳感器，控制器和執(zhí)行器從功能安全角度考慮都有著極高的要求。如果略過數(shù)據(jù)采集，場景的累積（尤其是極端工況場景的累積）這些必備的工作階段，會造成不可預(yù)知的危險(xiǎn)的嚴(yán)重后果。

對于停車場自動泊車而言，我們采集的數(shù)據(jù)主要包括了三種大類的典型的停車場場景，包括：寫字樓地下停車場場景，工業(yè)園區(qū)地上停車場場景，公共場所地上停車場場景，采集的原始數(shù)據(jù)既囊括了無人、低速、車流少的簡單工況，又涵蓋了人多、高速、車流大的復(fù)雜交通環(huán)境。具體的場景分類與定義可參考4.3小節(jié)。

從數(shù)據(jù)采集硬件平臺系統(tǒng)搭建，到數(shù)據(jù)處理融合匹配算法編寫，以及數(shù)據(jù)采集過程，至最后場景提取的數(shù)據(jù)分析過程，本文都給出了詳細(xì)的處理過程和技術(shù)細(xì)節(jié)描述。這種從硬件平臺搭建到軟件算法開發(fā)，再到大數(shù)據(jù)處理分析過程，整個技術(shù)流程工具鏈，無論是采集何種數(shù)據(jù)類型（自動駕駛類數(shù)據(jù)、高級輔助類駕駛數(shù)據(jù)），應(yīng)對何種工況場景（地下停車場、高速公路），為何種自動駕駛功能（自動泊車、智能物流）累積場景數(shù)據(jù)，都有很強(qiáng)的借鑒意義，科研價(jià)值和商業(yè)落地前景。

圖1 公共場所地上停車場、寫字樓地下停車場實(shí)景圖

1 采集硬件平臺的搭建

我們所使用的傳感器硬件平臺采集方案，主要依托于視覺攝像頭傳感器系統(tǒng)和毫米波雷達(dá)探測系統(tǒng)。這種傳感器組合平臺系統(tǒng)的優(yōu)勢在于：既利用了攝像頭系統(tǒng)對周圍障礙物分類識別的能力，對行人的探測能力，對車道線的檢測能力，又融合了毫米波雷達(dá)遠(yuǎn)距離探測障礙物的能力，高精度距離、方位角、速度的檢測能力。

表1 攝像頭、毫米波雷達(dá)各自探測優(yōu)勢比較和融合結(jié)果

同時(shí)，基于攝像頭和毫米波雷達(dá)的自動泊車數(shù)據(jù)采集平臺方案，成本相對低廉，是直接面向量產(chǎn)級別的硬件解決方案。目前自動駕駛平臺必備的激光雷達(dá)傳感器，動輒數(shù)十萬的成本價(jià)格，不適用于自動泊車數(shù)據(jù)采集階段的硬件配置清單。

表2數(shù)據(jù)采集系統(tǒng)主流傳感器價(jià)格比較

傳感器平臺主要架構(gòu)如圖2，整體架構(gòu)共包含下列四個子系統(tǒng)，四個子系統(tǒng)的布局如圖2所示。

表3 傳感器四個子系統(tǒng)功能表

圖2 數(shù)據(jù)采集平臺傳感器安裝示意圖

（1）前向視覺+毫米波雷達(dá)融合系統(tǒng)

①傳感器個數(shù)與種類：一枚ARS408毫米波雷達(dá)，一枚網(wǎng)口級別的攝像頭

②前向傳感器融合系統(tǒng)探測范圍整體布局：

圖3 前向視覺+RADAR系統(tǒng)探測范圍示意圖

③前向融合傳感器系統(tǒng)安裝位置：

表4前向融合系統(tǒng)車輛安裝具體位置

圖4 ARS408毫米波雷達(dá)安裝位置實(shí)景圖

1.2 環(huán)視魚眼攝像頭監(jiān)測系統(tǒng)

①傳感器個數(shù)與種類：4枚魚眼攝像頭，水平視角范圍接近180°

②環(huán)視攝像頭整體布局：

圖5魚眼攝像頭系統(tǒng)安裝布局示意圖

③環(huán)視攝像頭探測范圍：

圖6魚眼攝像頭系統(tǒng)探測范圍示意圖

④環(huán)視攝像頭系統(tǒng)安裝位置：

表5環(huán)視系統(tǒng)車輛安裝具體位置

圖7前向后向環(huán)視攝像頭安裝位置實(shí)景圖

1.3 側(cè)向盲區(qū)攝像頭監(jiān)測系統(tǒng)

①傳感器個數(shù)與種類：5枚網(wǎng)口級別攝像頭；

②盲區(qū)監(jiān)測攝像頭整體布局：

圖8盲區(qū)監(jiān)測視覺系統(tǒng)安裝布局示意圖

③盲區(qū)監(jiān)測攝像頭探測范圍：

圖9盲區(qū)監(jiān)測系統(tǒng)探測范圍示意圖

④盲區(qū)監(jiān)測攝像頭系統(tǒng)安裝位置：

表6盲區(qū)監(jiān)測系統(tǒng)安裝具體位置

圖1 0車輛左側(cè)盲區(qū)攝像頭安裝位置實(shí)景圖

2 硬件傳感器平臺的空間標(biāo)定與時(shí)間標(biāo)定

在傳感器安裝位置確定之后，進(jìn)行數(shù)據(jù)采集之前，需要進(jìn)行硬件系統(tǒng)的標(biāo)定工作，這主要包括了：空間同步標(biāo)定和時(shí)間同步標(biāo)定。

2.1 采集平臺傳感器的空間標(biāo)定

空間同步指的是，將車輛所有傳感器測量得到的數(shù)據(jù)，統(tǒng)一到車輛同一個坐標(biāo)系下（通常選取車輛后軸中心點(diǎn)地面投影作為全車基準(zhǔn)坐標(biāo)）。不同傳感器對同一個物體測量得到的獨(dú)立結(jié)果，經(jīng)過坐標(biāo)系轉(zhuǎn)換后，形成統(tǒng)一致的結(jié)果。這涉及到各個坐標(biāo)系之間的平移變換和旋轉(zhuǎn)變換的問題。通常把傳感器坐標(biāo)系分為兩大類。一類是局部坐標(biāo)系，包括圖像坐標(biāo)系、雷達(dá)坐標(biāo)系、里程計(jì)坐標(biāo)系、本車坐標(biāo)系等，一類是全局坐標(biāo)系，例如世界坐標(biāo)系、WGS-84經(jīng)緯坐標(biāo)系、UTM坐標(biāo)系，用來描述車輛在世界坐標(biāo)的絕對位置（GPS），或者表征車輛在高精地圖上的位置與姿態(tài)。

采集平臺涉及到的空間坐標(biāo)系共包括以下六類：像素坐標(biāo)系，像平面坐標(biāo)系，相機(jī)坐標(biāo)系，本車坐標(biāo)系，毫米波雷達(dá)坐標(biāo)系，以及全局坐標(biāo)系；

攝像頭傳感器原始數(shù)據(jù)映射到本車坐標(biāo)系的空間標(biāo)定為例，（u,v）為像素坐標(biāo)，Zc為景深，（X,Y,Z）為本車坐標(biāo)，轉(zhuǎn)換關(guān)系有如下公式：

非攝像頭傳感器的坐標(biāo)系到本車坐標(biāo)系的坐標(biāo)平移旋轉(zhuǎn)關(guān)系如下公式：

其中R為3×3的空間旋轉(zhuǎn)矩陣，t為1×3的空間平移矩陣

以圖像傳感器為例，具體是利用標(biāo)定板，標(biāo)定得到焦點(diǎn)，焦距等內(nèi)參參數(shù)，進(jìn)行徑向切向畸變校正。其中標(biāo)定3×3矩陣中，內(nèi)參個數(shù)和畸變參數(shù)分別為4個和5個。

圖1 1攝像頭標(biāo)定板標(biāo)定法

其他傳感器標(biāo)定的物理量如表：

表7采集平臺傳感器需要標(biāo)定的參量

正確標(biāo)定了各種傳感器的空間位置關(guān)系，測量其中一種傳感器相對于目標(biāo)物的位置信息，能夠得到本車相對于當(dāng)前目標(biāo)物的位置關(guān)系，圖12為一個計(jì)算demo，目標(biāo)物相對于毫米波雷達(dá)的位置為（1.0，2.0，0.0），平移旋轉(zhuǎn)變換計(jì)算得到目標(biāo)物相對于本車的位置關(guān)系為（1.1，2.0，0.2）。

圖1 2毫米波雷達(dá)空間標(biāo)定計(jì)算示例

2.2 采集平臺傳感器的時(shí)間標(biāo)定

時(shí)間標(biāo)志的統(tǒng)一也是多傳感器數(shù)據(jù)采集系統(tǒng)重要的技術(shù)環(huán)節(jié)。不同種類的傳感器數(shù)據(jù)采集系統(tǒng)，分別對環(huán)境進(jìn)行獨(dú)立的感知，通常情況下各個傳感器的刷新頻率不同，收集的環(huán)境數(shù)據(jù)也并不在同一個時(shí)間點(diǎn)。除了對空間坐標(biāo)系進(jìn)行精確的標(biāo)定之外，各個設(shè)備之間也要進(jìn)行時(shí)間的同步。

表8主流感知傳感器的刷新頻率值

時(shí)間同步通常有三種方式：

①不同傳感器都具有自己獨(dú)立的時(shí)鐘，在數(shù)據(jù)融合的過程當(dāng)中，以低頻率的傳感器為基準(zhǔn)，搜索其每一幀的時(shí)間戳在高頻傳感器中最近時(shí)間戳的數(shù)據(jù)幀，進(jìn)行時(shí)間融合；

②采用GPS的時(shí)間戳作為其他傳感器的時(shí)間標(biāo)準(zhǔn)，對其他低頻傳感器授時(shí)，如果被授時(shí)的傳感器和GPS的采樣點(diǎn)不在同一個時(shí)間點(diǎn)上，進(jìn)行非同步時(shí)間的內(nèi)插和外推；

③多個camera的傳感器系統(tǒng)，可以使用同一個開關(guān)，進(jìn)行曝光和采樣時(shí)間點(diǎn)的同步。

3 前向毫米波雷達(dá)和攝像頭的匹配融合問題

用于多目標(biāo)動態(tài)匹配過程的算法主要包括以下四種：競標(biāo)算法、匈牙利算法、GNN算法和JPDA算法；我們主要利用了GNN（Global Nearest Neighbor Algorithm）算法進(jìn)行了目標(biāo)匹配：

從相對簡單和相對復(fù)雜兩種場景考慮目標(biāo)匹配：

①對于多目標(biāo)物體，相對距離較遠(yuǎn)，

像素坐標(biāo)系下沒有交疊區(qū)域（如圖13），那么跟蹤每一個bounding box，當(dāng)有一個RADAR投影值落入矩形框時(shí)，我們即將此RADAR的投影值和此bouding box做匹配。

圖1 3匹配融合對應(yīng)的簡單場景

②對于障礙物密集區(qū)域，不同bounding box之間會有重疊，同一個boundingbox區(qū)域會落入多個毫米波雷達(dá)的投影點(diǎn)，如圖14，圖15所示，B1，B2，B3表示三個bouding box的相對位置和大小，R1，R2，R3，R4表示毫米波雷達(dá)障礙物感知相對位置。

圖1 4全局最近鄰匹配圖示

可以看到，其中毫米波雷達(dá)反饋點(diǎn)R1與攝像頭感知框B1進(jìn)行matching，毫米波雷達(dá)反饋點(diǎn)R2與攝像頭感知框R2可以匹配，但是R3分別是B2，B3的候選，用GNN算法計(jì)算同一個毫米波雷達(dá)投影點(diǎn)與多矩形框的分配概率，取其中的分配概率最大值（即計(jì)算mahalanobis馬氏距離的最小值），作為匹配結(jié)果：如果R3與B3的馬氏距離最小，則R3與B3進(jìn)行配對；如果R3與B2的馬氏距離最小，再比較R3與R2的馬氏距離，舍棄馬氏距離大的點(diǎn)。對于那些未被匹配的RADAR投影點(diǎn)（R4），對其進(jìn)行刪除。

圖1 5匹配融合對應(yīng)的復(fù)雜場景

匹配后的結(jié)果，通過非線性卡爾曼濾波的方式對毫米波雷達(dá)和攝像頭的距離，速度結(jié)果進(jìn)行融合輸出。

多傳感器匹配融合過程中仍需解決的問題總結(jié)如下：

表9匹配算法仍需要解決的問題

4 原始數(shù)據(jù)的采集輸出與場景標(biāo)注

基于視覺傳感器和毫米波雷達(dá)低成本采集平臺方案，我們將采集到的數(shù)據(jù)分為兩類，總結(jié)如下。

4.1 本車原始數(shù)據(jù)信息

本車原始數(shù)據(jù)共包含38個字段信息，其中主要的數(shù)據(jù)總結(jié)如表10。

表1 0本車主要原始數(shù)據(jù)

4.2 目標(biāo)障礙物原始數(shù)據(jù)信息

目標(biāo)車原始數(shù)據(jù)共包含16個字段信息，其中主要的數(shù)據(jù)總結(jié)如表11。

表1 1目標(biāo)車主要輸出得到的原始數(shù)據(jù)

4.3 場景標(biāo)注信息

基于自動泊車場景，我們定義了停車場，場內(nèi)場外兩種區(qū)域，向下包括駛?cè)胪＼噲觯窜嚾胛?，駛出停車場?個Phase，每種Phase向下又包含路口左轉(zhuǎn)，場內(nèi)直行等18個大類場景，同時(shí)，為每個大類場景下分為31類子場景，根據(jù)輸出的原始采集視頻數(shù)據(jù)，以及4.1,4.2描述的本車目標(biāo)車的原始數(shù)據(jù)，我們對各種場景進(jìn)行原始數(shù)據(jù)標(biāo)注，場景劃分邏輯結(jié)構(gòu)如圖16。

圖1 6停車場場景劃分邏輯結(jié)構(gòu)圖

5 基于原始與標(biāo)注數(shù)據(jù)的大數(shù)據(jù)分析與提取

我們希望通過原始與標(biāo)注數(shù)據(jù)，提取并計(jì)算包括重要障礙物ID信息、TTC、THW等29個字段，人工手動提取速度慢，我們利用Python編程實(shí)現(xiàn)對原始與標(biāo)注數(shù)據(jù)的處理，對數(shù)以萬計(jì)的原始數(shù)據(jù)表格實(shí)現(xiàn)了自動化批量處理，自動化生成功能，通過并行的提交作業(yè)，我們實(shí)現(xiàn)了對寫字樓地下停車場，工業(yè)園區(qū)地上停車場，公共場所地上停車場三類典型的停車場區(qū)域，共計(jì)10天，360個小時(shí)，TB級的原始數(shù)據(jù)表格進(jìn)行程序處理，所有數(shù)據(jù)可以在幾個小時(shí)內(nèi)全部完成。用Py?thon進(jìn)行數(shù)據(jù)處理顯示其在大數(shù)據(jù)數(shù)據(jù)處理的優(yōu)越性和高效性，原始處理數(shù)據(jù)表格如圖17所示。

圖1 7 Python程序輸入的原始數(shù)據(jù)列表

用Python進(jìn)行數(shù)據(jù)處理顯示其在大數(shù)據(jù)數(shù)據(jù)處理的優(yōu)越性和高效性，Python代碼如圖18所示。

圖1 8 Python程序代碼片段示例

6 結(jié)語

本文所主要是面向自動泊車的自動駕駛功能，進(jìn)行包括寫字樓地下停車場，工業(yè)園區(qū)地上停車場，公共場所地上停車場三類典型的停車場區(qū)域的數(shù)據(jù)采集工作，基于采集的原始數(shù)據(jù)，生成相應(yīng)的場景標(biāo)注數(shù)據(jù)，并由原始數(shù)據(jù)，場景標(biāo)注數(shù)據(jù)利用Python原代碼，進(jìn)行大數(shù)據(jù)處理，自動化批量生成目標(biāo)字段表格。從第一步對于傳感器設(shè)備的硬件安裝標(biāo)定，到第二步攝像頭毫米波雷達(dá)軟件算法開發(fā)，到第三步大數(shù)據(jù)批量處理代碼編寫，最后生成需求字段表格報(bào)告，整個流程工具鏈具有非常大的科研和商業(yè)價(jià)值，此工作也是面向上汽數(shù)據(jù)采集項(xiàng)目，對于其他的數(shù)據(jù)采集項(xiàng)目與工作，都有直觀、直接的參考價(jià)值。