基于視覺和車輛動力學(xué)的庫位跟蹤算法研究＊

余卓平 許明煜 張培志 熊璐 黃禹堯

（同濟(jì)大學(xué)，上海 201804）

主題詞：自主泊車 車輛動力學(xué) 定位 庫位跟蹤 擴(kuò)展卡爾曼濾波

1 前言

自動泊車系統(tǒng)的應(yīng)用可以提高交通效率和安全性，具有廣闊的市場前景[1]。在國內(nèi)市場，自動泊車系統(tǒng)搭載范圍逐漸從高檔轎車延伸到中檔轎車中。

定位與庫位跟蹤作為自動泊車環(huán)境感知技術(shù)的一部分，是后續(xù)規(guī)劃和控制的基礎(chǔ)。目前智能駕駛技術(shù)常用的環(huán)境感知傳感器有差分GPS接收器、慣性導(dǎo)航系統(tǒng)、相機(jī)和激光雷達(dá)等，選擇雖然很多，但每種傳感器對于自動泊車來說優(yōu)、缺點都十分明顯。差分GPS接收器成本高昂，且在室內(nèi)或有建筑物遮擋的區(qū)域信號缺失嚴(yán)重；高精度慣導(dǎo)設(shè)備同樣成本高昂，且難以避免長時間運行時的數(shù)據(jù)漂移現(xiàn)象；相機(jī)獲取的數(shù)據(jù)豐富，但易受環(huán)境光線等因素影響，且圖像處理算法對計算資源要求較高；激光雷達(dá)的優(yōu)點是精度高、測量范圍廣，缺點是成本高昂、數(shù)據(jù)信息不夠豐富，且目前沒有成熟的車載級產(chǎn)品?；谝陨蟼鞲衅鞯奶攸c，相機(jī)成為目前自動泊車技術(shù)最主流的傳感器[2]。

環(huán)視魚眼相機(jī)由于其成本低廉、視野范圍廣等優(yōu)點被廣泛使用。在自動泊車中，環(huán)視相機(jī)主要被用于檢測庫位線及獲得庫位的相對位置。受光線等因素影響，現(xiàn)有庫位檢測算法魯棒性較差，不能在整個泊車過程中持續(xù)跟蹤庫位[3]；又由于魚眼相機(jī)在視野邊緣存在較大畸變，庫位檢測的結(jié)果往往有較大誤差[4]。

在自動泊車中，車輛定位主要依靠航位推算。航位推算信號來源為車輛底盤控制數(shù)據(jù)，包括輪速傳感器數(shù)據(jù)和轉(zhuǎn)向盤轉(zhuǎn)角。普通航位推算算法基于阿克曼轉(zhuǎn)角模型推導(dǎo)。雖然自動泊車過程速度較低，但仍然存在一定的質(zhì)心側(cè)偏角，只適用于準(zhǔn)靜態(tài)工況的阿克曼轉(zhuǎn)角模型不能滿足泊車定位精度的需求。由于車輛底盤數(shù)據(jù)噪聲的存在，航位推算的累積誤差也會使泊車過程后期出現(xiàn)定位漂移。

針對上述問題，本文設(shè)計了一種融合車輛底盤數(shù)據(jù)和環(huán)視相機(jī)數(shù)據(jù)的庫位跟蹤算法，可以在泊車過程中實時估計車輛位姿并跟蹤庫位角點，為后續(xù)再規(guī)劃提供更精確的環(huán)境信息。

2 庫位跟蹤算法

2.1 算法基本架構(gòu)

庫位跟蹤的本質(zhì)是狀態(tài)估計問題。將車輛位姿和庫位角點坐標(biāo)視為服從某種概率分布的隨機(jī)變量，在常見情況下，假設(shè)狀態(tài)變量和噪聲項均服從高斯分布，于是問題可以描述成：在已知車輛底盤運動數(shù)據(jù)和環(huán)視觀測數(shù)據(jù)時，如何確定狀態(tài)變量的概率分布。由于泊車環(huán)境對實時性要求較高，且環(huán)境中庫位角點數(shù)量有限，將路標(biāo)坐標(biāo)也加入狀態(tài)變量，采用緊耦合的高斯濾波方法來融合兩種數(shù)據(jù)。又由于運動過程和觀測過程為非線性，使用擴(kuò)展卡爾曼濾波（Extended Kalman Filter，EKF）進(jìn)行庫位跟蹤[5]，算法流程如圖1所示。

2.2 車輛動力學(xué)模型

為了對車輛的動力學(xué)特性進(jìn)行分析，并以此為基礎(chǔ)設(shè)計相應(yīng)的航位推算算法，需要建立正確、有效的車輛動力學(xué)模型。二自由度單軌模型以質(zhì)心側(cè)偏角和橫擺角速度作為系統(tǒng)的狀態(tài)變量，如圖2所示。建模時需作如下簡化：假設(shè)整車質(zhì)量集中于質(zhì)心處；輪胎與地面的附著力通過前、后車輪直接傳遞；忽略懸架的作用；認(rèn)為汽車平行于地面運動，即汽車垂向位移、俯仰角和側(cè)傾角均為零。由牛頓第二定律可以得到等效動力學(xué)方程：

式中，β、ψ分別為質(zhì)心側(cè)偏角和橫擺角；Ff、Fr分別為前、后軸側(cè)向力；lf、lr分別為質(zhì)心到前、后軸的距離；δ為前輪轉(zhuǎn)角；J為車輛質(zhì)心處繞z軸的轉(zhuǎn)動慣量；v為質(zhì)心處的速度。

采用線性模型描述輪胎側(cè)偏角與側(cè)向力的關(guān)系：

式中，F(xiàn)y、α分別為輪胎的側(cè)向力和側(cè)偏角；k為輪胎側(cè)偏剛度。

前、后輪側(cè)偏角分別為：

結(jié)合式（1）～式（3），可以得到以前輪轉(zhuǎn)角和質(zhì)心速度為輸入、以質(zhì)心側(cè)偏角速度和橫擺角加速度為狀態(tài)變量的方程組：

式中，kf為前輪輪胎側(cè)偏剛度；kr為后輪輪胎側(cè)偏剛度。

本文以車輛二自由度模型為基礎(chǔ)進(jìn)行質(zhì)心側(cè)偏角和橫擺角速度估計算法的設(shè)計。采用線性輪胎模型的車輛質(zhì)心側(cè)偏角估計方法在極限工況時誤差較大[6]，但自主泊車一般在低速小激勵工況下進(jìn)行，考慮到即便在低速工況下，質(zhì)心側(cè)偏角仍然存在，而基于阿克曼轉(zhuǎn)角的純幾何模型完全不考慮質(zhì)心側(cè)偏角的影響，因此輪胎剛度為線性的二自由度車輛模型可以滿足航位推算精度要求[7]。

2.3 環(huán)視庫位檢測及圖像拼接

系統(tǒng)從車載的4個魚眼相機(jī)中實時獲取圖像并對其進(jìn)行拼接。通過機(jī)器學(xué)習(xí)算法，可以從獲得的鳥瞰視圖中檢測到相應(yīng)的庫位點，經(jīng)過相似變換計算獲得庫位點相對于車輛的二維平面坐標(biāo)，并實時地發(fā)送給決策系統(tǒng)，如圖3所示。

圖3 基于環(huán)視視覺的泊車庫位檢測系統(tǒng)整體流程

環(huán)視系統(tǒng)鳥瞰圖由前、后、左、右4個環(huán)視圖像分別進(jìn)行坐標(biāo)變換后拼接而成。4個相機(jī)的視角均為180°，因此可以圍成除車身內(nèi)部外的無盲區(qū)視野范圍。由于經(jīng)歷了多次坐標(biāo)變換，如圖4所示，可建立反向查找表以提高算法執(zhí)行的效率。針對鳥瞰圖中的每個像素點，通過一個相似矩陣計算其對應(yīng)的車身固結(jié)坐標(biāo)系中的位置坐標(biāo)，再通過一組仿射變換矩陣計算對應(yīng)世界坐標(biāo)系中的點對應(yīng)到去畸變圖像中的坐標(biāo)，最終通過畸變多項式模型計算其所對應(yīng)的原始圖像像素的位置，取得該像素的顏色。

圖4 從鳥瞰圖到魚眼圖像的坐標(biāo)變換

從鳥瞰圖到車輛坐標(biāo)系的變換為相似變換，其參數(shù)可以由鳥瞰圖的像素范圍及其對應(yīng)的地面視野物理尺寸事先算得；在虛擬場景中，可以使用無畸變的相機(jī)從鳥瞰視角拍攝一個場景作為成像拼接的參考，則可以非常容易地計算得到比例系數(shù)。

從車輛坐標(biāo)系到單個魚眼鏡頭去畸變后的圖像的變換，需采用單應(yīng)矩陣，手工選擇4組對應(yīng)點即可完成相應(yīng)變換。需要注意的是，此處所指的車輛坐標(biāo)系原點在車輛質(zhì)心處，為車身固結(jié)坐標(biāo)系，在標(biāo)定時車輛靜止，僅在此時可認(rèn)為與大地坐標(biāo)系相重合。

從去畸變后的圖像到相機(jī)所成原始圖像之間則依靠多項式桶形畸變模型聯(lián)系在一起。上述步驟均可以查找表的形式保存下來，以便拼接時實時使用。

2.4 基于擴(kuò)展卡爾曼濾波的數(shù)據(jù)融合

擴(kuò)展卡爾曼濾波算法采用的世界坐標(biāo)系和基本狀態(tài)變量定義如圖5所示，定義世界坐標(biāo)系原點為泊車初始時刻車輛質(zhì)心位置。

圖5 EKF算法的坐標(biāo)系和狀態(tài)量定義

2.4.1 運動方程

根據(jù)車身位姿估計和庫位跟蹤的目標(biāo)，構(gòu)建狀態(tài)變量的運動方程：

式中，為根據(jù)運動方程預(yù)測得到的t時刻狀態(tài)先驗值；xt和yt為t時刻世界坐標(biāo)系下車輛位移。

根據(jù)運動學(xué)旋轉(zhuǎn)關(guān)系，有：

式中，X、Y為車輛坐標(biāo)系下的位移；R為二維平面車輛坐標(biāo)系相對于世界坐標(biāo)系的旋轉(zhuǎn)矩陣。

根據(jù)質(zhì)心側(cè)偏角的定義，有：

根據(jù)式（3）和式（4），可以將橫擺角加速度表達(dá)為：

同理，根據(jù)式（3）和式（4），質(zhì)心側(cè)偏角速度可以表示為：

2.4.2 觀測方程

庫位角點檢測結(jié)果為車輛坐標(biāo)系下庫位角點的坐標(biāo)，直接用作觀測變量會導(dǎo)致將狀態(tài)變量映射到觀測變量的觀測方程形式復(fù)雜，同時提高后續(xù)求導(dǎo)步驟的難度，需要增加額外的計算資源。因此，預(yù)先將通過圖像處理算法計算得到的觀測數(shù)據(jù)進(jìn)行數(shù)學(xué)處理（庫位角點與車輛坐標(biāo)系的幾何關(guān)系見圖6），得到庫位角點相對于車輛的距離和方向：

式中，Pi,x、Pi,y分別為第i個庫位角點在車輛坐標(biāo)系下的x、y坐標(biāo)；φi為第i個庫位角點與車輛坐標(biāo)系x軸的夾角。

圖6 庫位觀測示意

選取庫位角點相對車輛的距離和方向為觀測變量，根據(jù)圖6所示的幾何關(guān)系，可以確定觀測方程的形式：

式中，zt、xt分別為t時刻觀測變量和狀態(tài)變量；pi,x、pi,y為對應(yīng)時刻檢測到的庫位角點在世界坐標(biāo)系下的坐標(biāo)。

2.4.3 預(yù)測過程協(xié)方差矩陣傳播

由于運動方程是關(guān)于(t-1)時刻狀態(tài)變量xt-1的非線性函數(shù)，要知道從(t-1)時刻到t時刻系統(tǒng)狀態(tài)協(xié)方差矩陣的傳播，首先需求得運動函數(shù)關(guān)于xt-1的Jacobian矩陣：

式中，F(xiàn)t為t時刻的運動Jacobian矩陣；f為運動過程函數(shù)。

Jacobian矩陣中各偏導(dǎo)具體表達(dá)式為：

得到Jacobian矩陣表達(dá)式后，(t-1)時刻到t時刻運動過程的協(xié)方差矩陣可以由下式更新：

式中，為狀態(tài)協(xié)方差先驗值；Pt-1為(t-1)時刻狀態(tài)協(xié)方差矩陣；Qt為運動噪聲協(xié)方差。

2.4.4 觀測更新

考慮到觀測方程也是關(guān)于車輛狀態(tài)變量xt的非線性函數(shù)，進(jìn)行觀測更新時同樣需要計算觀測函數(shù)關(guān)于xt的Jacobian矩陣，將新觀測到的庫位角點世界坐標(biāo)系坐標(biāo)作為優(yōu)化變量增廣到狀態(tài)變量中：

式中，Ht為t時刻的觀測Jacobian矩陣；h為觀測函數(shù)。

Jacobian矩陣中各偏導(dǎo)具體表達(dá)式為：

至此，可以計算卡爾曼增益：

式中，Rt為觀測噪聲協(xié)方差。

獲得卡爾曼增益后，可以更新狀態(tài)變量的后驗估計：

式中，zt為t時刻觀測變量。

同時更新狀態(tài)變量后驗協(xié)方差：

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

考慮到每幀環(huán)視圖像視野內(nèi)的庫位角點數(shù)量不多，使用歐式距離即可滿足數(shù)據(jù)關(guān)聯(lián)的要求。如果環(huán)視圖像視野內(nèi)檢測到1個庫位角點，則將該庫位角點與已有路標(biāo)信息進(jìn)行匹配，匹配方法為：若與系統(tǒng)狀態(tài)中已有路標(biāo)的歐式距離不小于某一閾值，則認(rèn)為該庫位點是新路標(biāo)，此時對系統(tǒng)的狀態(tài)進(jìn)行增廣；若小于某一閾值，則利用EKF算法進(jìn)行更新。

3 仿真與試驗

3.1 仿真驗證

本文仿真環(huán)境基于MATLAB和PreScan搭建，泊車場景如圖7所示。

圖7 仿真場景

場景設(shè)置了2個庫位角點，由車載傳感器可以獲得角點相對于車輛的距離和角度，仿真中加入高斯噪聲以模擬在光照等因素的影響下環(huán)視相機(jī)檢測獲得的庫位角點坐標(biāo)[8]。車輛按照預(yù)先設(shè)定的路徑進(jìn)入車位，記錄運動過程中庫位位置、純航位推算軌跡、融合軌跡和真實軌跡。航位推算軌跡與真實軌跡對比結(jié)果如圖8所示，融合軌跡與真實軌跡對比結(jié)果如圖9所示。

圖8 純航位推算定位仿真結(jié)果

圖9 融合庫位檢測結(jié)果的組合定位仿真結(jié)果

由仿真結(jié)果可知，整個泊車過程行駛總距離12.5 m，純航位推算定位累積誤差約為0.4 m，且誤差有逐漸增大的趨勢，而融合庫位檢測后的結(jié)果最大誤差在0.3 m以下，且累積誤差逐漸收斂趨于零。

3.2 實車試驗

實車試驗平臺為榮威E50電動車，車上裝有工控機(jī)、環(huán)視相機(jī)和差分GPS接收器。試驗過程中采集的數(shù)據(jù)有車輛輪速、轉(zhuǎn)向盤轉(zhuǎn)角、環(huán)視相機(jī)圖像和差分GPS定位數(shù)據(jù)，由于差分GPS定位精度在10 cm以內(nèi)，認(rèn)為其數(shù)據(jù)為真值。試驗平臺和泊車過程分別如圖10、圖11所示。

圖10 實車試驗平臺

圖11 泊車過程

圖12 一次泊車入庫試驗結(jié)果

圖13 二次泊車入庫試驗結(jié)果

試驗中分別采用一次泊入和二次泊入的方式完成泊車，記錄的結(jié)果分別如圖12、圖13所示。由實車試驗結(jié)果可知，純航位推算結(jié)果在泊車過程中最大誤差約為0.6 m，而融合庫位檢測后的結(jié)果最大誤差約為0.2 m，且整個過程的趨勢與車輛軌跡真值更加接近，說明本文提出的算法能準(zhǔn)確估計泊車過程中的車輛位姿，有效降低航位推算累積誤差。

4 結(jié)束語

本文基于車輛動力學(xué)模型，利用擴(kuò)展卡爾曼濾波融合航位推算和庫位檢測結(jié)果設(shè)計了一種泊車定位及庫位跟蹤算法。仿真和實車試驗結(jié)果表明，該方法可以有效降低航位推算累積誤差，實時估計車輛位姿，并在整個泊車過程中持續(xù)跟蹤庫位角點，為自動泊車后續(xù)過程提供更準(zhǔn)確的環(huán)境信息。

另外，本文在狀態(tài)估計中采用了擴(kuò)展卡爾曼濾波方法，而無跡卡爾曼濾波、粒子濾波、非線性優(yōu)化等常用的狀態(tài)估計方法[9]在實時性、優(yōu)化效率、估計精度等指標(biāo)上的效果值得進(jìn)一步研究。