龍杜輝

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

0 引言

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

1 差速模型的建立

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

圖1 小車模型Fig.1 Car model

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

圖2 運(yùn)動模型Fig.2 Motion model

直行時(shí)：

轉(zhuǎn)彎時(shí)：

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

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

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

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

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

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

3.1 擴(kuò)展卡爾曼濾波器

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

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

3.2 多傳感信息融合

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

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

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

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

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

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

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

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

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

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

3.3.1 定位誤差

定位誤差表示估計(jì)位置與真實(shí)位置偏差，表達(dá)式為：

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

3.3.2 定位準(zhǔn)確率

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

4 實(shí)驗(yàn)驗(yàn)證

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

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

圖5 小車在gazebo 運(yùn)動過程Fig.5 Movement of trolley in Gazebo

圖6 小車運(yùn)動軌跡Fig.6 Trajectory of trolley

為了定量判斷擴(kuò)展卡爾曼濾波器對多傳感融合的定位效果，在所有的數(shù)據(jù)點(diǎn)中隨機(jī)依次抽取10 個(gè)測量點(diǎn)作為判斷指標(biāo)，分別與真實(shí)值進(jìn)行比較，處理后的數(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 小車與原點(diǎn)距離對比Fig.9 Comparison of the distance between trolley and origin

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

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

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

5 結(jié)論

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

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

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