基于傳感器信息融合的車輛位姿估算方法

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

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

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

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

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

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

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

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

1 航跡推算位姿估算

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

1.1 車輛運動學模型

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

圖1 車輛運動模型

車輛在第k時刻的位姿為

(1)

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

(2)

車輛位移δd后的位姿為

(3)

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

(4)

1.2 里程計定位

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

(5)

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

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

(6)

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

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

(7)

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

(8)

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

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

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

2.1.1卡爾曼濾波

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

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

(9)

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

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

yk=Hkxk+wk,

(10)

式中：yk為k時刻的觀測向量；Hk為k時刻的觀測矩陣；wk為k時刻觀測的隨機誤差向量.

卡爾曼濾波的具體運算過程如下：

初始化濾波器為

(11)

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

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

(12)

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

(13)

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

后驗估算為

(14)

式中：I為單位矩陣.

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

2.1.2信息融合

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

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

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

2.2.1系統(tǒng)模型

由式(8)可得車輛運動學方程為

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

(15)

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

測量噪聲v可認為滿足高斯分布的隨機變量，即

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

(16)

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

(17)

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

(18)

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

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

(19)

2.2.2觀測模型

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

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

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

(20)

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

世界坐標系和車輛坐標系之間的位姿關系為

(21)

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

圖4 車輛坐標與圖像坐標

圖像坐標系與車輛坐標系之間僅有縮放與位移，故A點的坐標關系可表示為

(22)

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

由式(22)可得基于視覺傳感器觀測結果為

(23)

對于觀測方程，由于圖像會受到噪聲影響，且攝像頭固定在車身上易受到車身振動的影響，進一步影響觀測結果，對包含噪聲的觀測方程可表示為

(24)

測量噪聲可認為滿足高斯分布的隨機變量：

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

(25)

(26)

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

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

zk=h(Xk,wk).

(27)

(28)

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

(29)

2.2.3位姿估算

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

(30)

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

(31)

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

k時刻卡爾曼增益矩陣為

(32)

k時刻的觀測殘差為

(33)

3 位姿估算仿真試驗

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

3.1 車輛運動學模型的建立

基于某SUV(suburban utility vehicle)車型開展算法驗證，車輛的相關尺寸參數(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.

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

圖5 車輛運動學仿真模型

3.2 相機模型的建立

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

圖6 相機模型

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

3.3 輪速傳感器模型的建立

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

圖7 輪速傳感器仿真模型

3.4 位姿估算模型的建立

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

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

3.5 仿真試驗

車輛初始狀態(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)角φ隨時間t的變化表示，軌跡為

(34)

(35)

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

圖9 V=diag(0.000 2,0.000 1)時的仿真試驗結果對比

圖10 V=diag(0.02,0.01)時的仿真試驗結果對比

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

4 結 論

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