李臣旭， 江浩斌， 王成雨， 馬世典

(1. 江蘇大學(xué) 汽車與交通工程學(xué)院， 江蘇 鎮(zhèn)江 212013； 2. 江蘇大學(xué) 汽車工程研究院， 江蘇 鎮(zhèn)江 212013)

智能車輛作為智能交通系統(tǒng)的重要組成部分，其關(guān)鍵技術(shù)的研究已成為國(guó)內(nèi)外科研機(jī)構(gòu)關(guān)注的焦點(diǎn).自動(dòng)泊車是智能車輛關(guān)鍵技術(shù)的研究熱點(diǎn)之一，其中車輛位姿的精確定位是實(shí)現(xiàn)智能車輛自動(dòng)泊車的一項(xiàng)關(guān)鍵技術(shù)，它是實(shí)現(xiàn)對(duì)泊車路徑精準(zhǔn)跟蹤的前提，更是提高泊車精度的關(guān)鍵[1].

自動(dòng)泊車過(guò)程中，系統(tǒng)識(shí)別到目標(biāo)車位后會(huì)控制車輛按照規(guī)劃好的路徑泊車入位，常規(guī)泊車控制方法是以車輛位姿的跟蹤誤差作為輸入，該誤差指車輛在參考坐標(biāo)系中位姿與規(guī)劃路徑之間的誤差[2].在對(duì)控制方法進(jìn)行仿真驗(yàn)證時(shí)，通常認(rèn)為車輛實(shí)際位姿已知.在實(shí)際泊車場(chǎng)景中，車輛的相對(duì)位姿無(wú)法直接獲知，需要通過(guò)車載傳感器獲取的周邊環(huán)境信息以及車輛的自身運(yùn)動(dòng)狀態(tài)推算出車輛的當(dāng)前位姿信息.

車輛的位姿定位方法主要分為相對(duì)定位法和絕對(duì)定位法.相對(duì)定位法利用的是車輛內(nèi)部傳感器觀測(cè)車輛本身的狀態(tài)信息，并由當(dāng)前時(shí)刻的位姿狀態(tài)去估算下一時(shí)刻的狀態(tài)，例如基于慣性導(dǎo)航系統(tǒng)和基于里程計(jì)的定位方法.這種方法的優(yōu)點(diǎn)是根據(jù)車輛運(yùn)動(dòng)學(xué)方程和傳感器反饋的當(dāng)前車輛運(yùn)動(dòng)狀態(tài)即可估算出該時(shí)刻車輛的位姿變化，缺點(diǎn)是車輛狀態(tài)信息均通過(guò)傳感器獲取，系統(tǒng)誤差不可避免，累積誤差降低了車輛的定位精度[3].絕對(duì)定位法是利用車上安裝的傳感器感知車輛周邊環(huán)境信息，從而估算出車輛在當(dāng)前環(huán)境中所處的絕對(duì)位姿，例如基于GPS(global positioning system)的定位、基于地圖匹配的定位以及基于視覺(jué)的定位等.這種方法的優(yōu)點(diǎn)是能夠獲取豐富的環(huán)境信息，易于獲取車輛的位姿信息，實(shí)現(xiàn)車輛的全局定位，但缺點(diǎn)是對(duì)傳感器的精度要求較高，實(shí)現(xiàn)成本較高，且處理的數(shù)據(jù)量較大，實(shí)現(xiàn)定位的算法較為復(fù)雜[4].

對(duì)于車輛的位姿估計(jì)技術(shù)的研究，成果多集中在移動(dòng)機(jī)器人定位領(lǐng)域，泊車系統(tǒng)位姿估計(jì)方法的研究尚較少.J. BORENSTEIN[5]利用里程計(jì)實(shí)現(xiàn)移動(dòng)機(jī)器人的位姿估計(jì)，通過(guò)定位誤差分析發(fā)現(xiàn)，輪徑不等以及軸距長(zhǎng)度的不確定性為主要誤差來(lái)源，并利用定時(shí)的絕對(duì)位置更新提高定位精度.李力等[6]利用單目視覺(jué)傳感器，通過(guò)對(duì)地面標(biāo)靶的特征提取與EPnP的方法求解相機(jī)位姿，以實(shí)現(xiàn)對(duì)飛行機(jī)器人位姿的估計(jì).豐俊丙[7]基于視覺(jué)傳感器開(kāi)展了移動(dòng)機(jī)器人自定位以及目標(biāo)物位姿測(cè)量技術(shù)的研究，通過(guò)機(jī)載相機(jī)和物體空間姿態(tài)估計(jì)算法計(jì)算出標(biāo)志物的相對(duì)距離和方位，然后結(jié)合標(biāo)志物位置信息利用擴(kuò)展卡爾曼濾波算法估算得到機(jī)器人在世界坐標(biāo)系中的位置.羅勇等[8]利用車輛兩后輪的輪速傳感器信號(hào)與前輪轉(zhuǎn)角信號(hào)，通過(guò)卡爾曼濾波算法對(duì)車輛的位姿進(jìn)行估計(jì)，實(shí)現(xiàn)了較高的車輛位姿估計(jì)精度，并將方法利用到自動(dòng)泊車中.

近年來(lái)，隨著智能輔助駕駛技術(shù)研發(fā)的不斷升溫，用于環(huán)境感知與車輛自身定位的傳感器類型也逐漸增多，通過(guò)傳感器融合技術(shù)以及相關(guān)的濾波算法實(shí)現(xiàn)對(duì)車輛精準(zhǔn)定位，在該領(lǐng)域的應(yīng)用越來(lái)越廣泛，基于單一定位方法實(shí)現(xiàn)車輛位姿定位的局限性被進(jìn)一步放大.利用智能汽車中的多種傳感器數(shù)據(jù)進(jìn)行信息融合，可以有效彌補(bǔ)單一采用上述任一位姿定位方法的不足，更好地實(shí)現(xiàn)車輛定位定姿，且能夠極大地降低系統(tǒng)對(duì)于周邊設(shè)施的依賴程度，從而提高自動(dòng)泊車系統(tǒng)的智能化水平和對(duì)復(fù)雜環(huán)境的適應(yīng)性，因此，基于傳感器信息融合的車輛位姿定位成為高智能化自動(dòng)泊車系統(tǒng)技術(shù)的發(fā)展趨勢(shì)[9-11].

筆者采用融合輪速傳感器信息與視覺(jué)傳感器信息的方法，對(duì)泊車過(guò)程中車輛位姿的估算方法進(jìn)行研究.首先利用輪速傳感器對(duì)車輛位姿進(jìn)行估算，并分析誤差產(chǎn)生的原因；然后利用視覺(jué)傳感器信息與輪速傳感器信息進(jìn)行融合，減小系統(tǒng)誤差，提高車輛位姿估算準(zhǔn)確性；最后進(jìn)行仿真試驗(yàn)，驗(yàn)證提出的位姿估算方法的有效性.

1 航跡推算位姿估算

航跡推算位姿估算是利用傳感器信息估算車輛的相對(duì)運(yùn)動(dòng)位姿，筆者利用車輛ABS/ESC(antilock brake system/electronic stability controller)系統(tǒng)中安裝于兩后輪的輪速傳感器，結(jié)合車輛當(dāng)前時(shí)刻位姿與傳感器的數(shù)據(jù)，推算出車輛下一時(shí)刻的位姿.航跡推算的算法實(shí)現(xiàn)簡(jiǎn)單，且成本較低，但是位姿估算結(jié)果受累計(jì)誤差的影響，隨著時(shí)間的推移，位姿估算的置信度隨之降低.

1.1 車輛運(yùn)動(dòng)學(xué)模型

利用里程計(jì)估算車輛的位姿，屬于二維空間的位姿問(wèn)題.以時(shí)間為參考，結(jié)合車輛線位移、角位移以及前一時(shí)刻的位姿對(duì)現(xiàn)在的位姿進(jìn)行估算，屬于二維空間的位姿問(wèn)題.笛卡爾坐標(biāo)系下離散化的汽車運(yùn)動(dòng)學(xué)模型如圖1所示，車輛后車軸中心點(diǎn)在k時(shí)刻的位置為(xk,yk)，航向角為θk.

圖1 車輛運(yùn)動(dòng)模型

車輛在第k時(shí)刻的位姿為

(1)

由于系統(tǒng)的取樣時(shí)間是一個(gè)很小的值，車輛在一個(gè)時(shí)刻內(nèi)的位姿變化可以分解為車輛的位移δd和轉(zhuǎn)過(guò)的角度δθ，第k個(gè)時(shí)刻內(nèi)位姿改變表示為δ(k)=[δd(k),δθ(k)]T，位姿變化的過(guò)程可以近似等效為車輛先繞后軸中點(diǎn)的轉(zhuǎn)動(dòng)，后進(jìn)行平移.k+1時(shí)刻，車輛先繞后軸中點(diǎn)旋轉(zhuǎn)δθ后的車輛位姿為

(2)

車輛位移δd后的位姿為

(3)

k+1時(shí)刻的位姿用向量形式表示為

(4)

1.2 里程計(jì)定位

利用輪速傳感器來(lái)獲取汽車的δd和δθ，車輪行駛過(guò)的距離為

(5)

式中：n為記錄的上升沿個(gè)數(shù)；r為車輪的滾動(dòng)半徑；N為齒圈的總齒數(shù).

車輛的δd和δθ均可通過(guò)兩車輪行駛里程表示：

(6)

式中：dl、dr分別為車輛左后輪和右后輪行駛的里程；nl、nr分別為兩輪齒圈轉(zhuǎn)過(guò)的齒數(shù)；l為車輛后軸有效長(zhǎng)度.

將式(6)所得結(jié)果代入式(4)，即可得到車輛在k+1時(shí)刻的位姿，但前提是所測(cè)得的里程是完全準(zhǔn)確的.實(shí)際應(yīng)用中，里程計(jì)的測(cè)量結(jié)果會(huì)受到多種誤差影響，主要分為系統(tǒng)誤差和隨機(jī)誤差2種，其中：系統(tǒng)誤差主要包含車位滾動(dòng)半徑誤差、輪距測(cè)量誤差和轉(zhuǎn)子碼盤分辨率誤差等；隨機(jī)誤差主要來(lái)源于車輪與地面的滑動(dòng)、輪胎載荷變化以及受路面不平度影響等因素.

(7)

將式(7)代入式(4)可得車輛的真實(shí)位姿為

(8)

2 融合視覺(jué)信息的車輛位姿估算

航跡推算的位姿估算方法由于受到測(cè)量噪聲的影響，估算的位姿和實(shí)際位姿之間存在一定偏差，同時(shí)隨著時(shí)間推移，僅依靠之前的位姿及不太準(zhǔn)確的傳感器結(jié)果進(jìn)行估算，會(huì)使該測(cè)量值的不確定性進(jìn)一步增大，累積誤差也隨之增加，為解決該問(wèn)題，需要引入新的觀測(cè)信息.利用視覺(jué)傳感器來(lái)獲取車輛的周邊環(huán)境信息，通過(guò)擴(kuò)展卡爾曼濾波算法對(duì)視覺(jué)傳感器信息和輪速傳感器信息進(jìn)行融合，降低測(cè)量噪聲對(duì)位姿誤差的影響，提高車輛位姿信息的估算精度.

2.1 卡爾曼濾波與信息融合方法

2.1.1卡爾曼濾波

對(duì)于一個(gè)動(dòng)態(tài)的線性離散系統(tǒng)，系統(tǒng)方程定義為

xk=Fk-1xk-1+Gk-1uk-1+vk-1,

(9)

式中：xk、xk-1分別為該系統(tǒng)的k、k-1時(shí)刻的狀態(tài)向量；Fk-1為k-1時(shí)刻的系統(tǒng)矩陣；Gk-1為k-1時(shí)刻的輸入矩陣；uk-1為k-1時(shí)刻的系統(tǒng)輸入向量；vk-1為k-1時(shí)刻的系統(tǒng)測(cè)量的隨機(jī)誤差向量.

假設(shè)有一觀測(cè)器，對(duì)系統(tǒng)的狀態(tài)進(jìn)行觀測(cè)，觀測(cè)方程定義為

yk=Hkxk+wk,

(10)

式中：yk為k時(shí)刻的觀測(cè)向量；Hk為k時(shí)刻的觀測(cè)矩陣；wk為k時(shí)刻觀測(cè)的隨機(jī)誤差向量.

卡爾曼濾波的具體運(yùn)算過(guò)程如下：

初始化濾波器為

(11)

式中：E表示取期望，若有確定的初始位置x0，那么系統(tǒng)的初始協(xié)方差P0+=0，但若x0也是一個(gè)隨機(jī)變量，那么P0+則是這個(gè)隨機(jī)變量的協(xié)方差矩陣.

系統(tǒng)狀態(tài)的先驗(yàn)為

(12)

系統(tǒng)狀態(tài)的更新，卡爾曼增益矩陣為

(13)

式中：Wk為觀測(cè)誤差協(xié)方差矩陣.

后驗(yàn)估算為

(14)

式中：I為單位矩陣.

需要注意的是，上述卡爾曼濾波的推算過(guò)程只能運(yùn)用在線性離散系統(tǒng)中，車輛的定位問(wèn)題屬于非線性系統(tǒng)，本研究使用基于擴(kuò)展卡爾曼濾波(extended Kalman filter,EKF)方法，通過(guò)對(duì)矩陣函數(shù)進(jìn)行泰勒展開(kāi)，取其展開(kāi)式第1項(xiàng)將系統(tǒng)轉(zhuǎn)化為近似線性系統(tǒng).

2.1.2信息融合

圖2 位姿估算的信息融合方法

2.2 基于擴(kuò)展卡爾曼濾波算法的位姿估算

視覺(jué)傳感器作為環(huán)境感知單元，在自動(dòng)泊車系統(tǒng)中所起的作用除了要能實(shí)現(xiàn)車位的識(shí)別，還需對(duì)車輛與目標(biāo)車位的相對(duì)位置進(jìn)行計(jì)算.但視覺(jué)傳感器易受環(huán)境的干擾，例如光線、雨霧等，且受到像素和分辨率的影響，所以僅依靠視覺(jué)傳感器進(jìn)行車輛的位姿估算并非完全準(zhǔn)確[12].基于上述分析，利用擴(kuò)展卡爾曼濾波算法，結(jié)合輪速傳感器和視覺(jué)傳感器的融合信息，對(duì)自動(dòng)泊車過(guò)程中的車輛位姿進(jìn)行估算.

2.2.1系統(tǒng)模型

由式(8)可得車輛運(yùn)動(dòng)學(xué)方程為

Xk+1=f(Xk,δk,vk),

(15)

式中：Xk=(xk,yk,θk)T為車輛在k時(shí)刻的位姿狀態(tài)向量；δ(k)=(δd(k),δθ(k))T為傳感器測(cè)得的位姿變化向量；vk=(vd(k),vθ(k))T為傳感器測(cè)量噪聲.

測(cè)量噪聲v可認(rèn)為滿足高斯分布的隨機(jī)變量，即

v=(vd,vθ)～N(0,V),

(16)

測(cè)量噪聲的協(xié)方差矩陣為

(17)

式中：σd、σθ分別為位移方差與航向角方差.

(18)

式中：Fk和Lk分別是對(duì)矩陣函數(shù)進(jìn)行求導(dǎo)后所得的雅可比矩陣.

在vk=0時(shí)，F(xiàn)k和Lk可表示為

(19)

2.2.2觀測(cè)模型

通過(guò)視覺(jué)傳感器獲取觀測(cè)信息，觀測(cè)模型如圖3所示.

圖3 系統(tǒng)觀測(cè)模型

由于觀測(cè)點(diǎn)與車輛都在地平面上，所以這是個(gè)二維位姿問(wèn)題.其中：OXY為世界坐標(biāo)系；oxy為車輛坐標(biāo)系；A為空間中的觀測(cè)點(diǎn)，目標(biāo)車位線角點(diǎn)可以作為圖像中被觀測(cè)的特征點(diǎn)，(x,y,θ)T為車輛在世界坐標(biāo)系中的位姿.點(diǎn)A在地面世界坐標(biāo)系上的坐標(biāo)和在車輛坐標(biāo)系中的坐標(biāo)之間的關(guān)系為

(20)

式中： (VxA，VyA，1)為A在車輛坐標(biāo)系中的齊次坐標(biāo); (WxA，WyA，1)為A在世界坐標(biāo)系中的齊次坐標(biāo).

世界坐標(biāo)系和車輛坐標(biāo)系之間的位姿關(guān)系為

(21)

對(duì)圖像進(jìn)行去畸變與逆透視變換，將圖像轉(zhuǎn)化為對(duì)地面的俯視圖，車輛坐標(biāo)系OxVyV與圖像坐標(biāo)系oxPyP之間的關(guān)系如圖4所示.

圖4 車輛坐標(biāo)與圖像坐標(biāo)

圖像坐標(biāo)系與車輛坐標(biāo)系之間僅有縮放與位移，故A點(diǎn)的坐標(biāo)關(guān)系可表示為

(22)

式中：s為縮放比例系數(shù)；x′、y′為坐標(biāo)系的位移量.

由式(22)可得基于視覺(jué)傳感器觀測(cè)結(jié)果為

(23)

對(duì)于觀測(cè)方程，由于圖像會(huì)受到噪聲影響，且攝像頭固定在車身上易受到車身振動(dòng)的影響，進(jìn)一步影響觀測(cè)結(jié)果，對(duì)包含噪聲的觀測(cè)方程可表示為

(24)

測(cè)量噪聲可認(rèn)為滿足高斯分布的隨機(jī)變量：

w=(wx,wy)～N(0,W).

(25)

(26)

式中：W為測(cè)量協(xié)方差矩陣；σx、σy為位移在x、y方向上的觀測(cè)方差.

綜上，觀測(cè)模型在k時(shí)刻的函數(shù)形式為

zk=h(Xk,wk).

(27)

(28)

雅可比矩陣Hk、Mk可表示為

(29)

2.2.3位姿估算

在k+1時(shí)刻，車輛位姿的先驗(yàn)與此時(shí)的協(xié)方差矩陣可以用上一時(shí)刻的后驗(yàn)結(jié)果表示，即

(30)

先驗(yàn)結(jié)果是不包含觀測(cè)信息的預(yù)測(cè)結(jié)果，利用視覺(jué)傳感器獲取觀測(cè)信息對(duì)狀態(tài)先驗(yàn)進(jìn)行修正，從而得到車輛位姿的后驗(yàn)結(jié)果與協(xié)方差，這個(gè)結(jié)果就是k+1時(shí)刻經(jīng)過(guò)卡爾曼濾波后的位姿估算值與其協(xié)方差矩陣，修正過(guò)程可表示為

(31)

式中：Kk+1為k+1時(shí)刻的卡爾曼增益矩陣；εk+1為k+1時(shí)刻的觀測(cè)殘差.

k時(shí)刻卡爾曼增益矩陣為

(32)

k時(shí)刻的觀測(cè)殘差為

(33)

3 位姿估算仿真試驗(yàn)

為驗(yàn)證算法的有效性，首先建立車輛運(yùn)動(dòng)學(xué)仿真模型、相機(jī)模型和輪速傳感器模型，然后控制車輛沿設(shè)定的軌跡進(jìn)行運(yùn)動(dòng)，最后通過(guò)Simulink建立位姿估算模塊模型并設(shè)定相關(guān)參數(shù)進(jìn)行仿真試驗(yàn)，驗(yàn)證設(shè)計(jì)方法對(duì)車輛位姿的估算效果.

3.1 車輛運(yùn)動(dòng)學(xué)模型的建立

基于某SUV(suburban utility vehicle)車型開(kāi)展算法驗(yàn)證，車輛的相關(guān)尺寸參數(shù)如下：車長(zhǎng)為4.788 m；車寬為1.933 m；軸距為2.850 m；前懸長(zhǎng)為0.966 m；后懸長(zhǎng)為0.972 m；前軸輪距為1.641 m；后軸輪距為1.642 m；前輪最大等效轉(zhuǎn)角為0.541 rad；最大前輪轉(zhuǎn)角速度為0.541 rad·s-1；車輛最大加速度為4.2 m·s-2.

基于試驗(yàn)車參數(shù)在Simulink中建立車輛的運(yùn)動(dòng)學(xué)模型，如圖5所示.通過(guò)對(duì)車速與前輪轉(zhuǎn)角的輸入，輸出車輛的位姿狀態(tài)(x,y,θ)T.

圖5 車輛運(yùn)動(dòng)學(xué)仿真模型

3.2 相機(jī)模型的建立

為實(shí)現(xiàn)對(duì)車身周圍進(jìn)行全方位的環(huán)境感知，視覺(jué)信息采集是基于四路攝像頭的環(huán)視圖像，生成車輛的環(huán)視俯視圖.相機(jī)模型利用Simulink中的FSN模塊實(shí)現(xiàn)，如圖6所示.

圖6 相機(jī)模型

通過(guò)調(diào)用Matlab的相機(jī)成像函數(shù)，實(shí)現(xiàn)相機(jī)模型的輸入輸出功能，相機(jī)模型的輸入為點(diǎn)在相機(jī)坐標(biāo)系中的坐標(biāo)，輸出為點(diǎn)在成像平面上的坐標(biāo).

3.3 輪速傳感器模型的建立

輪速傳感器對(duì)車輛位姿估算模型如圖7所示，其中：uv為車輛行駛的速度；k為放大倍數(shù).輸入為車輛位姿(x,y,θ)T，輸出為傳感器測(cè)得的相對(duì)位姿變化向量δ.

圖7 輪速傳感器仿真模型

3.4 位姿估算模型的建立

利用Simulink中的FSN模塊對(duì)傳感器融合的位姿估算算法進(jìn)行封裝，構(gòu)成車輛信息融合位姿估算模塊，如圖8所示.利用視覺(jué)傳感器觀測(cè)模型z和基于輪速傳感器的車輛運(yùn)動(dòng)狀態(tài)向量作為輸入δ，結(jié)合車輛的初始狀態(tài)，通過(guò)EKF模塊，最后輸出車輛位姿狀態(tài)的估算值(xest,yest,θest)T與系統(tǒng)的協(xié)方差Pest.

圖8 信息融合位姿估算模型

3.5 仿真試驗(yàn)

車輛初始狀態(tài)X0=(4.4,2.7,0)T，系統(tǒng)初始協(xié)方差矩陣的值較小，取P0=diag(0.0052,0.0052,0.0012)，視覺(jué)傳感器觀測(cè)模型協(xié)方差矩陣W=diag(0.12,0.12)，車輛跟蹤的三段式泊車軌跡由車速uv與前輪轉(zhuǎn)角φ隨時(shí)間t的變化表示，軌跡為

(34)

(35)

當(dāng)傳感器的測(cè)量方差V=diag(0.02,0.01)時(shí)，利用航跡推算方法的位姿估算與利用傳感器信息融合的位姿估算結(jié)果進(jìn)行對(duì)比，如圖10所示.

圖9 V=diag(0.000 2,0.000 1)時(shí)的仿真試驗(yàn)結(jié)果對(duì)比

圖10 V=diag(0.02,0.01)時(shí)的仿真試驗(yàn)結(jié)果對(duì)比

基于上述2種仿真試驗(yàn)結(jié)果可以看出：在測(cè)量協(xié)方差較大時(shí)，提出的基于傳感器信息融合的位姿估算精度明顯優(yōu)于航跡推算法.

4 結(jié) 論

針對(duì)自動(dòng)泊車過(guò)程中車輛位姿確定的問(wèn)題進(jìn)行了研究，提出了一種基于擴(kuò)展卡爾曼濾波算法融合輪速傳感器信息與視覺(jué)傳感器信息車輛位姿定位的估算方法，提高自動(dòng)泊車過(guò)程的車輛位姿定位精度.首先研究了僅利用兩后輪輪速傳感器信息對(duì)車輛進(jìn)行位姿估算的航跡推算法，分析了誤差產(chǎn)生的原因；然后提出了基于輪速傳感器與視覺(jué)傳感器融合的車輛定位方法，通過(guò)擴(kuò)展卡爾曼濾波算法將視覺(jué)傳感器與輪速傳感器信息進(jìn)行融合，減小系統(tǒng)誤差的同時(shí)實(shí)現(xiàn)自動(dòng)泊車過(guò)程中的車輛位姿定位；最后使用Simulink建模仿真對(duì)算法進(jìn)行驗(yàn)證，并將航跡推算法與傳感器信息融合方法進(jìn)行比較.結(jié)果表明：本研究的利用視覺(jué)傳感器與輪速傳感器進(jìn)行信息融合的車輛位姿估算方法能夠有效降低系統(tǒng)定位誤差，提高泊車過(guò)程中車輛位姿的估算精度.