龍杜輝

（200082 上海市 上海理工大學(xué)機械工程學(xué)院）

0 引言

目前車輛上使用的定位技術(shù)主要有全球定位系統(tǒng)（GPS）、航位定位系統(tǒng)（DR）、慣性導(dǎo)航系統(tǒng)（INS）和同時定位與建圖（SLAM）。GPS 具有全球性、精度高等特點，但是受外界干擾最大；DR 利用車載的傳感器測量車輛的車速、加速度、偏航角速度來實現(xiàn)車輛的自主定位。在短時間內(nèi)，DR 精度是很高的,但由于累計誤差的存在，經(jīng)過一段時間后，定位系統(tǒng)就會失效；INS 跟DR 一樣短時間的精度高，但是定位誤差會隨時間累積[1-2]；SLAM 通過雷達或相機對周圍環(huán)境進行掃描，來估計自身的位置，但是隨著環(huán)境的幾何特征和紋理特征的不明顯，定位也會失效，而且SLAM 使用的傳感器成本比較高[3]。每一種傳感器進行定位都有優(yōu)點，同時也存在弊端。利用多傳感器的信息融合估算運動軌跡，可以避免單一傳感器定位的弊端。本文利用擴展卡爾曼濾波器對車載編碼器和IMU 采集的信息進行融合，實現(xiàn)車輛的自主定位，對比融合后的軌跡和真實軌跡，證明了該方法的有效性。

1 差速模型的建立

本文實驗對象是兩后輪獨立驅(qū)動的小車模型，如圖1 所示，它能通過調(diào)節(jié)兩輪的速度差達到轉(zhuǎn)彎目的，這種運動模型叫做雙輪差分驅(qū)動模型[4]。

圖1 小車模型Fig.1 Car model

小車的原點位于兩輪軸的中心位置。用該模型估算小車位置只需2 個主要參數(shù)：車輪之間的距離d 和車輪半徑R。運動模型如圖2 所示，一般情況下，通過兩車輪上編碼器測量每個時間步長δt 的左右車輪轉(zhuǎn)過角度δψL、δψR計算小車位姿的變化。假設(shè)x 正方向為小車前進的方向，小車左邊為y 正方向，z 的正方向垂直于小車向上。因此各運動狀態(tài)下模型如式（1）—式（7）。

圖2 運動模型Fig.2 Motion model

直行時：

轉(zhuǎn)彎時：

小車運動方式通過局部位姿增量進行改變，累加局部位姿增量即可得到小車的運動軌跡，因此利用小車的編碼器數(shù)據(jù)獲得了車輛的里程計模型。由于車輛運動累積具有不確定性，需要協(xié)方差矩陣Q 來表示這種不確定性關(guān)系，它可以通過兩驅(qū)動輪轉(zhuǎn)角測量值的不確定性來如式（8）表示：

式中：J——運動模型的雅可比矩陣；Qη——輪子角度測量值的協(xié)方差。

2 IMU 驅(qū)動模型的建立

IMU 由加速計和陀螺儀組成[5]，它能測量車輛的線加速度as和角速率ωs。通過IMU 得到的線加速度和角速率可以用來預(yù)測小車位姿。式（9）為IMU 計算的狀態(tài)量和誤差，式（10）—式（14）為狀態(tài)量和誤差更新過程。

式中：p，v，q ——慣性測量單元的參考系的位置、速度和方向四元數(shù)；ab，ωb——加速度計和陀螺儀的偏差；vi，θi——在時間間隔Δt 內(nèi)的測量誤差；ai，ωi——加速度計和陀螺儀偏差的隨機游走?！脑獢?shù)相乘，R{q}——關(guān)于方向角q 的旋轉(zhuǎn)矩陣，這里僅考慮2D 情況下，則R{q}如式（15）所示：

3 基于擴展卡爾曼濾波的車輛位置預(yù)測

3.1 擴展卡爾曼濾波器

擴展卡爾曼濾波器[6]是在卡爾曼濾波基礎(chǔ)上進行改進，使其適用于非線性系統(tǒng)。主要應(yīng)用的方法是在上一時刻的后驗估計狀態(tài)量下進行泰勒展開得到這附近的線性系統(tǒng)，然后再對系統(tǒng)狀態(tài)進行估計。擴展卡爾曼濾波分為預(yù)測和更新2 個部分，如圖3 所示。

圖3 擴展卡爾曼濾波過程Fig.3 Extended Kalman filtering process

3.2 多傳感信息融合

由于慣性測量單元IMU 估算的車輛的運動軌跡相對于編碼器的估算來說在短時間內(nèi)準(zhǔn)確度更高，但由于IMU 中加速度計和陀螺儀的雙重誤差累積，在長時間的運作下，對車輛的定位準(zhǔn)確度明顯下降，此時應(yīng)更相信由車輪編碼器估算的軌跡。

為此，提出了一種基于信息融合的擴展卡爾曼濾波器策略，來精準(zhǔn)定位車輛位置和軌跡預(yù)測方法。此系統(tǒng)同時接收來自編碼器信息以及慣性測量單元的數(shù)據(jù)。通過組合定位系統(tǒng)的數(shù)學(xué)模型，可以融合兩個系統(tǒng)的數(shù)據(jù)，獲得車輛位置的精準(zhǔn)估計。融合過程的流程圖如圖4 所示。

圖4 多傳感數(shù)據(jù)融合過程Fig.4 Multi-sensor data fusion process

多傳感的數(shù)據(jù)融合可以將其中一個傳感器的數(shù)據(jù)作為測量量，另外一個傳感器的數(shù)據(jù)當(dāng)作觀測量進行融合[7]。以下為多傳感的融合模型。

（1）以車輪編碼器數(shù)據(jù)作為預(yù)測模型：

式中：xk=[xo，yo，θo]T——車輛在k 時刻平面坐標(biāo)及轉(zhuǎn)角的三維向量；f ——k-1 時刻與狀態(tài)向量xk-1、系統(tǒng)控制輸入量uk-1有關(guān)的非線性函數(shù)；Wk-1——系統(tǒng)在k-1 時刻的高斯白噪聲，呈正態(tài)分布。

（2）以慣性測量單元數(shù)據(jù)作為觀測模型：

式中：zk=[xi，yi，θi]T——k 時刻三維的觀測向量；h——k 時刻關(guān)于狀態(tài)向量xk、系統(tǒng)控制輸入量vk的非線性函數(shù)；Vk——系統(tǒng)在k 時刻的觀測噪聲，呈高斯正態(tài)分布。

3.3 定位評價標(biāo)準(zhǔn)

評價一種算法設(shè)立評價標(biāo)準(zhǔn)是必要的[8]。本文采用定位誤差和定位的成功率作為評價準(zhǔn)則，其方法如下：

3.3.1 定位誤差

定位誤差表示估計位置與真實位置偏差，表達式為：

式中：d——使用融合算法后的位置與真實位置的偏差；xe、ye——通過融合算法估計的車輛坐標(biāo)；xt、yt——此時刻車輛真實坐標(biāo)。

3.3.2 定位準(zhǔn)確率

定義當(dāng)使用融合算法后與真實位置的偏差d 小于車輛的1/5 時為定位成功，否則設(shè)為定位失效。這里使用的小車模型長寬都為38 cm，故取判斷定位成功的閾值為7.6 cm。定位準(zhǔn)確率即為定位成功次數(shù)與實驗測量總數(shù)的比值：

4 實驗驗證

本文基于ROS 操作系統(tǒng)中g(shù)azebo 的仿真環(huán)境，完成車輛行駛的仿真實驗。傳感器數(shù)據(jù)由gazebo環(huán)境中，小車上傳感器插件模型提供話題數(shù)據(jù)，將IMU 和差速模型得到的里程計數(shù)據(jù)在ROS 的工作空間中進行融合，得到融合后的話題數(shù)據(jù)，將這些話題打包成bag 文件，并導(dǎo)入MATLAB 軟件，結(jié)果如下所示。

圖5 中，小車從圖5（a）依次運動到圖5（d）。由圖6 可知，由多傳感融合算法估計出來的軌跡與真實軌跡是十分接近的，在抖動區(qū)間也能準(zhǔn)確預(yù)測真實軌跡。

圖5 小車在gazebo 運動過程Fig.5 Movement of trolley in Gazebo

圖6 小車運動軌跡Fig.6 Trajectory of trolley

為了定量判斷擴展卡爾曼濾波器對多傳感融合的定位效果，在所有的數(shù)據(jù)點中隨機依次抽取10 個測量點作為判斷指標(biāo)，分別與真實值進行比較，處理后的數(shù)據(jù)如圖7—圖9 所示。

圖7 小車X 軸方向上對比Fig.7 Comparison of trolley in X-axis direction

圖8 小車Y 軸方向上對比Fig.8 Comparison of trolley in Y-axis direction

圖9 小車與原點距離對比Fig.9 Comparison of the distance between trolley and origin

根據(jù)定位評價標(biāo)準(zhǔn)對該實驗結(jié)果進行分析。由圖10 可知，在隨機的10 個測量點中只有測量點4 和測量點6 大于判斷閾值，故定位失敗。由式（20）可得小車在本次實驗的定位成功率μ=80%。

圖10 定位誤差分析Fig.10 Positioning error analysis

由于卡爾曼濾波器只利用了上一時刻的數(shù)據(jù)和系統(tǒng)的輸入量，計算量是比較小的，所以相比于其它算法而言，實時性和魯棒性都是比較優(yōu)異的。

5 結(jié)論

（1）本文以小車模型為研究對象，針對車輛定位容易失效的問題。通過利用擴展卡爾曼濾波對編碼器和IMU 數(shù)據(jù)進行融合，提高了定位的準(zhǔn)確性，實現(xiàn)了自主定位。

（2）通過對定位誤差的定量分析，驗證了本文算法對提高車輛定位是有效的，并且可以僅通過車載傳感器實現(xiàn)自我定位，解決了在復(fù)雜環(huán)境下定位失效的困惑。

（3）本次研究也存在許多不足的地方。首先本次實驗是基于仿真的，故誤差設(shè)定會比較不合理，設(shè)定編碼器和IMU誤差類型均符合高斯正態(tài)分布，這與實際誤差存在著差異，且設(shè)定的方差比較?。黄浯畏抡娴哪Ｐ托≤嚺c真實車輛也存在一定的差異。后續(xù)研究可以考慮采用真實的模型進行實驗驗證，然后加入相機、雷達進行多傳感融合，繼續(xù)提高定位進度。