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

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

智能車輛作為智能交通系統(tǒng)的重要組成部分，其關(guān)鍵技術(shù)的研究已成為國內(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)對泊車路徑精準(zhǔn)跟蹤的前提，更是提高泊車精度的關(guān)鍵[1].

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

車輛的位姿定位方法主要分為相對定位法和絕對定位法.相對定位法利用的是車輛內(nèi)部傳感器觀測車輛本身的狀態(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)信息均通過傳感器獲取，系統(tǒng)誤差不可避免，累積誤差降低了車輛的定位精度[3].絕對定位法是利用車上安裝的傳感器感知車輛周邊環(huán)境信息，從而估算出車輛在當(dāng)前環(huán)境中所處的絕對位姿，例如基于GPS(global positioning system)的定位、基于地圖匹配的定位以及基于視覺的定位等.這種方法的優(yōu)點(diǎn)是能夠獲取豐富的環(huán)境信息，易于獲取車輛的位姿信息，實(shí)現(xiàn)車輛的全局定位，但缺點(diǎn)是對傳感器的精度要求較高，實(shí)現(xiàn)成本較高，且處理的數(shù)據(jù)量較大，實(shí)現(xiàn)定位的算法較為復(fù)雜[4].

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

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

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

1 航跡推算位姿估算

航跡推算位姿估算是利用傳感器信息估算車輛的相對運(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é)果受累計(jì)誤差的影響，隨著時(shí)間的推移，位姿估算的置信度隨之降低.

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

利用里程計(jì)估算車輛的位姿，屬于二維空間的位姿問題.以時(shí)間為參考，結(jié)合車輛線位移、角位移以及前一時(shí)刻的位姿對現(xiàn)在的位姿進(jì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)過的角度δθ，第k個(gè)時(shí)刻內(nèi)位姿改變表示為δ(k)=[δd(k),δθ(k)]T，位姿變化的過程可以近似等效為車輛先繞后軸中點(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ì)定位

利用輪速傳感器來獲取汽車的δd和δθ，車輪行駛過的距離為

(5)

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

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

(6)

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

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

(7)

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

(8)

2 融合視覺信息的車輛位姿估算

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

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

2.1.1卡爾曼濾波

對于一個(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)測量的隨機(jī)誤差向量.

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

yk=Hkxk+wk,

(10)

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

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

初始化濾波器為

(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為觀測誤差協(xié)方差矩陣.

后驗(yàn)估算為

(14)

式中：I為單位矩陣.

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

2.1.2信息融合

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

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

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

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

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

(16)

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

(17)

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

(18)

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

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

(19)

2.2.2觀測模型

通過視覺傳感器獲取觀測信息，觀測模型如圖3所示.

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

由于觀測點(diǎn)與車輛都在地平面上，所以這是個(gè)二維位姿問題.其中：OXY為世界坐標(biāo)系；oxy為車輛坐標(biāo)系；A為空間中的觀測點(diǎn)，目標(biāo)車位線角點(diǎn)可以作為圖像中被觀測的特征點(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)

對圖像進(jìn)行去畸變與逆透視變換，將圖像轉(zhuǎn)化為對地面的俯視圖，車輛坐標(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)可得基于視覺傳感器觀測結(jié)果為

(23)

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

(24)

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

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

(25)

(26)

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

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

(31)

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

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

(32)

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

(33)

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

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

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

基于某SUV(suburban utility vehicle)車型開展算法驗(yàn)證，車輛的相關(guān)尺寸參數(shù)如下：車長為4.788 m；車寬為1.933 m；軸距為2.850 m；前懸長為0.966 m；后懸長為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所示.通過對車速與前輪轉(zhuǎn)角的輸入，輸出車輛的位姿狀態(tài)(x,y,θ)T.

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

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

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

圖6 相機(jī)模型

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

3.3 輪速傳感器模型的建立

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

圖7 輪速傳感器仿真模型

3.4 位姿估算模型的建立

利用Simulink中的FSN模塊對傳感器融合的位姿估算算法進(jìn)行封裝，構(gòu)成車輛信息融合位姿估算模塊，如圖8所示.利用視覺傳感器觀測模型z和基于輪速傳感器的車輛運(yùn)動(dòng)狀態(tài)向量作為輸入δ，結(jié)合車輛的初始狀態(tài)，通過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)，視覺傳感器觀測模型協(xié)方差矩陣W=diag(0.12,0.12)，車輛跟蹤的三段式泊車軌跡由車速uv與前輪轉(zhuǎn)角φ隨時(shí)間t的變化表示，軌跡為

(34)

(35)

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

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

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

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

4 結(jié) 論

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