羅 勇，陳 慧

(同濟(jì)大學(xué) 汽車學(xué)院，上海 201804)

隨著我國汽車保有量逐年上升，路邊停車位、車庫等資源變得越來越緊張，增加了泊車的難度。因此，可提高安全性并能減輕駕駛員負(fù)擔(dān)的自動(dòng)泊車系統(tǒng)受到了廣泛關(guān)注，并成為研究的熱點(diǎn)。

自動(dòng)泊車系統(tǒng)大都是根據(jù)檢測出的庫位信息，規(guī)劃一條泊車軌跡，然后控制車輛跟蹤軌跡進(jìn)行泊車[1-4]。因此，自動(dòng)泊車中實(shí)時(shí)準(zhǔn)確地獲得車輛當(dāng)前的定位信息十分必要，可增強(qiáng)系統(tǒng)的安全性和可靠性。文獻(xiàn)[5]利用四輪小型機(jī)器人在室內(nèi)進(jìn)行試驗(yàn)，基于運(yùn)動(dòng)學(xué)模型開發(fā)了泊車控制算法，但由于四輪機(jī)器人的機(jī)械特性和乘用車有所差別，并且室內(nèi)環(huán)境相比于室外環(huán)境要理想很多，所以結(jié)果參考性不強(qiáng)；文獻(xiàn)[6]在乘用車上開發(fā)了泊車系統(tǒng)，其位姿估計(jì)算法用了4個(gè)ABS輪速傳感器和1個(gè)方向盤轉(zhuǎn)角傳感器，試驗(yàn)中利用定位精度1 cm的差分GPS來驗(yàn)證算法精度，軌跡終點(diǎn)誤差達(dá)到了30 cm；文獻(xiàn)[7]用4個(gè)輪速傳感器和1個(gè)方向盤轉(zhuǎn)角傳感器設(shè)計(jì)了位姿估計(jì)算法，并在乘用車上進(jìn)行試驗(yàn)，同樣針對(duì)軌跡終點(diǎn)的位置偏差進(jìn)行討論，但并未提及參考位置的獲取方式，對(duì)結(jié)果的論證缺乏嚴(yán)謹(jǐn)性。

對(duì)于位姿估計(jì)算法的研究，難點(diǎn)在于解決誤差累積的問題。常見的精度影響因素有車輛實(shí)際參數(shù)與理論值不同(如車輪半徑、輪距等)，參數(shù)的實(shí)時(shí)變化，傳感器信號(hào)噪聲等。常采用設(shè)計(jì)參數(shù)標(biāo)定實(shí)驗(yàn)、濾除信號(hào)噪聲、多傳感器信號(hào)融合等來減少誤差累積。

本文基于擴(kuò)展卡爾曼濾波(EKF)技術(shù)，利用2個(gè)后輪輪速信號(hào)和方向盤轉(zhuǎn)角信號(hào)設(shè)計(jì)了車輛位姿估計(jì)算法。通過離線仿真及實(shí)車試驗(yàn)，驗(yàn)證了該算法的可行性和估計(jì)精度。

1 位姿估計(jì)算法設(shè)計(jì)

1.1 坐標(biāo)系定義和運(yùn)動(dòng)學(xué)模型

如圖1所示，在全局坐標(biāo)系xOy下，k時(shí)刻車輛的位姿信息表述為：車輛后軸中點(diǎn)的坐標(biāo)(xk,yk)，以及車輛中軸線指向車頭的方向與x軸的夾角θk。

泊車工況下，以運(yùn)動(dòng)學(xué)模型來近似描述車輛狀態(tài)，將系統(tǒng)離散化后，k時(shí)刻的位姿信息滿足

Δxk=vkT0cos θk，

(1)

Δyk=vkT0sin θk，

(2)

圖1 坐標(biāo)系定義

(3)

式中 Δxk，Δyk為k時(shí)刻車輛位姿信息的增量；T0為采樣時(shí)間步長;vk為車輛后軸中點(diǎn)縱向速度;ωk為車輛橫擺角速度。

初始時(shí)刻起，對(duì)式(1) ～ (3)積分，可得k時(shí)刻車輛的位置(xk,yk)和航向角θk。然而，利用式(1) ～ (3)來計(jì)算車輛位姿，信號(hào)的噪聲可能會(huì)在積分運(yùn)算中累積，進(jìn)而影響算法的估計(jì)精度。所以，需要濾除算法中所用信號(hào)的噪聲。

1.2 卡爾曼濾波器

圖2 卡爾曼濾波器結(jié)構(gòu)

卡爾曼濾波器只適用于線性系統(tǒng)，當(dāng)狀態(tài)方程或測量方程為非線性方程時(shí)，常使用擴(kuò)展卡爾曼濾波算法。其基本思想是求非線性方程的一階Taylor展開式，忽略其余高階項(xiàng)，從而將非線性轉(zhuǎn)為線性，進(jìn)而可將卡爾曼濾波理論應(yīng)用于非線性系統(tǒng)中。本文基于式(1) ～ (3)設(shè)計(jì)位姿估計(jì)算法，由于方程是非線性的，需要使用EKF算法。

1.3 算法實(shí)現(xiàn)

卡爾曼濾波器原理設(shè)計(jì)算法，需要計(jì)算k+1時(shí)刻的系統(tǒng)矩陣Φk+1,k和k+1時(shí)刻的測量矩陣Hk+1。

設(shè)系統(tǒng)狀態(tài)向量X為

X=[x,y,θ,kl,kr]T，

式中 x、y和θ為車輛位姿信息;kl和kr為兩個(gè)后輪半徑變動(dòng)系數(shù)，用來描述實(shí)際運(yùn)行過程中車輪半徑的變化量。

設(shè)系統(tǒng)觀測向量為Z，有

Z=[v,ω]T，

式中 v為車輛后軸中點(diǎn)的速度;ω為車輛的橫擺角速度。

圖3 算法結(jié)構(gòu)

算法的總體結(jié)構(gòu)如圖3所示，輸入量為系統(tǒng)觀測向量Z，輸出量為車輛的位姿信息。實(shí)際應(yīng)用中，觀測量v和ω的獲取方式為：取兩后輪輪速的平均值算得后軸中點(diǎn)的速度v；提前標(biāo)定好方向盤轉(zhuǎn)角和后軸中點(diǎn)轉(zhuǎn)彎半徑的關(guān)系，通過查表插值的方式獲得某方向盤轉(zhuǎn)角對(duì)應(yīng)的車輛后軸中點(diǎn)轉(zhuǎn)彎半徑R0，通過v和R0計(jì)算得到車輛橫擺角速度ω。

1)時(shí)間更新

式中f(Xk,k)為k時(shí)刻系統(tǒng)狀態(tài)的增量，f(Xk,k)=[Δxk,Δyk,Δθk,0,0]T。

利用k時(shí)刻左右2個(gè)后輪輪速(v3k,v4k,)和各自車輪半徑變動(dòng)系數(shù)(klk,krk)來計(jì)算vk和ωk，有

vk=(klkv3k+krkv4k)/2，

(4)

ωk=(-klkv3k+krkv4k)/D，

(5)

式中 D為后輪輪距。

綜合公式(1)～(3)及(4) ～ (5)，求得f(Xk,k)為

(6)

使式(6)線性化，得k+1時(shí)刻的系統(tǒng)矩陣Φk+1,k為

2)測量更新

k時(shí)刻，系統(tǒng)觀測向量Zk為

對(duì)Zk線性化后得Hk+1為

對(duì)Φk+1,k和Hk+1的計(jì)算完成之后，再根據(jù)圖2所示的算法結(jié)構(gòu)進(jìn)行計(jì)算，便完成了基于EKF技術(shù)的位姿估計(jì)算法的設(shè)計(jì)。

2 仿真試驗(yàn)

在veDYNA中進(jìn)行仿真，它是基于Matlab/Simulink平臺(tái)開發(fā)的車輛動(dòng)力學(xué)模型，被廣泛應(yīng)用于車輛動(dòng)力學(xué)仿真等領(lǐng)域。

按照所用實(shí)驗(yàn)車輛的參數(shù)對(duì)veDYNA車輛模型進(jìn)行相應(yīng)的設(shè)置：整車質(zhì)量1 184 kg，軸距2.407 m，后輪距1.45 m，車輪半徑0.288 m，轉(zhuǎn)向傳動(dòng)比16.3。

仿真時(shí)間為40 s，最高車速為2.5 m/s。在輪速信號(hào)中添加隨機(jī)擾動(dòng)來模擬實(shí)際輪速信號(hào)的噪聲。所加的隨機(jī)擾動(dòng)滿足高斯分布，其均值和標(biāo)準(zhǔn)差分別為0，0.7 r/min，運(yùn)行時(shí)左后輪輪速信號(hào)如圖4所示，車輛行駛軌跡如圖5所示。

(1)地質(zhì)災(zāi)害高易發(fā)區(qū)(Ⅰ)。地質(zhì)災(zāi)害高易發(fā)區(qū)主要分布縣境內(nèi)巴水河、尕干曲下游段兩岸河谷平原區(qū)以及賽欠曲兩岸低山丘陵區(qū)、黃河沿岸一帶中高山區(qū)，總面積760.51 km2，占總面積的15.18%，該區(qū)地貌單元主要為河谷沖積帶狀平原、低山丘陵區(qū)和中高山區(qū)。區(qū)內(nèi)共發(fā)育地質(zhì)災(zāi)害點(diǎn)199個(gè)，泥石流災(zāi)害最為發(fā)育，共計(jì)116條，其次為滑坡65處，不穩(wěn)定斜坡18段。

圖4 左后輪輪速信號(hào)波形 圖5 車輛行駛軌跡

車輛行駛了26.26 m，位置誤差結(jié)果如圖6所示。本文所設(shè)計(jì)的算法在x和y方向上的最大估計(jì)偏差分別3.76 cm和2.03 cm，此精度的估計(jì)結(jié)果對(duì)于自動(dòng)泊車系統(tǒng)的開發(fā)具有實(shí)際應(yīng)用價(jià)值。

圖6 位置誤差

3 實(shí)車試驗(yàn)

3.1 試驗(yàn)方案介紹

圖7 春暉2號(hào)電動(dòng)汽車

圖7所示為同濟(jì)大學(xué)汽車學(xué)院“春暉2號(hào)”分布式四輪驅(qū)動(dòng)電動(dòng)汽車，以它為平臺(tái)進(jìn)行實(shí)車試驗(yàn)。

使用MicroAutoBox快速原型控制器采集傳感器數(shù)據(jù)以及運(yùn)行估計(jì)算法。用RT3002型GPS/INS導(dǎo)航儀采集車輛位置和航向角信息作為參考值，并與算法估計(jì)的結(jié)果對(duì)比，以驗(yàn)證算法的精度。導(dǎo)航儀的位置精度為：2 cm(1σ)，航向角精度為0.1(°)(1σ)。

3.2 直線行駛工況

試驗(yàn)中，車輛沿直線行駛，如圖8所示，對(duì)比GPS所測行駛軌跡和算法估計(jì)的行駛軌跡。x和y方向上的偏差分別如圖9所示。車輛行駛12.22 m，位置估計(jì)的最大誤差在x方向?yàn)?.31 cm，y方向?yàn)?.74 cm。

圖8 行駛軌跡 圖9 位置誤差

3.3 圓弧軌跡行駛工況

在行駛過程中，保持方向盤轉(zhuǎn)角為-178°。對(duì)比GPS所測車輛行駛軌跡和算法估計(jì)得到的行駛軌跡，如圖10所示。x和y方向的偏差分別如圖11所示。車輛行駛距離13.53 m，位置最大誤差x方向?yàn)?.02 cm，y方向?yàn)?.77 cm。

圖10 行駛軌跡 圖11 位置誤差

3.4 泊車工況

3.5 試驗(yàn)小結(jié)

算法估計(jì)得到的車輛直線、圓弧和泊車軌跡均能與GPS所測車輛實(shí)際軌跡保持較高的一致性，尤其是沿直線和圓弧軌跡行駛的時(shí)候。算法估計(jì)得到的車輛位姿信息，可以為自動(dòng)泊車系統(tǒng)提供車輛的軌跡信息，為增強(qiáng)泊車安全性奠定了基礎(chǔ)。

圖12 行駛軌跡 圖13 位置誤差

4 結(jié)論

基于擴(kuò)展卡爾曼濾波技術(shù)設(shè)計(jì)了一套自動(dòng)泊車系統(tǒng)位姿估計(jì)算法，算法用到了兩個(gè)后輪輪速傳感器及方向盤轉(zhuǎn)角傳感器。仿真和實(shí)車試驗(yàn)的結(jié)果表明，算法具有可行性，且位置估計(jì)的最大誤差控制在3%的范圍內(nèi)，換言之，車輛行駛10 m，x和y方向上最大位置估計(jì)誤差在30 cm之內(nèi)。本文提出的算法可以為自動(dòng)泊車系統(tǒng)提供車輛軌跡信息的支持，為增強(qiáng)泊車安全性奠定了基礎(chǔ)。

參考文獻(xiàn)：

[1]吳鍇.智能自動(dòng)泊車系統(tǒng)研究[D].南京: 南京理工大學(xué), 2008.

[2]久保田有一, 巖切英之, 巖崎克彥，等.用于車輛的停車輔助裝置及停車輔助方法:中國, CN100535598C[P].2009-09-09.

[3]Sven Chlosta, Michael Dr Rohlfs. Method and Device for Parking a Motor Vehicle：Germany, EP1561673A2[P].2005-08-10.

[5]Kooktae Lee, Dalhyung Kim, Woojin Chung, et al. Car Parking Control Using a Trajectory Tracking Controller[C]//International Joint Conference.Busan： SICE-ICASE,2006.

[6]Kochem M, Wagner N, Hamann, et al. Data Fusion for Precise Dead Reckoning of Passenger Cars[C].Spain: IFAC, 2002.

[7]張誠, 羅勇, 陳慧. 基于擴(kuò)展卡爾曼濾波算法的智能泊車航位推算研究[J].上海汽車, 2012(6)：56-58，62.

[8]付夢(mèng)印, 鄧志紅, 閆莉萍. Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用[M].2版.北京：科學(xué)出版社，2010.