錢瑞，馮成濤

（常州大學 微電子與控制工程學院，江蘇常州，213159）

0 引言

定位是無人駕駛的關鍵技術之一?，F(xiàn)階段的定位系統(tǒng)主要通過RTK、GPS、IMU、輪式里程計、攝像頭、激光雷達等傳感器來實現(xiàn)?？紤]到單一傳感器難以應用于不同復雜多變的環(huán)境，多傳感器融合的思想應運而生[1]。Thomas 等人[2]在他們的機器人實驗平臺上搭載了GPS、IMU、輪式里程計這三種傳感器，并測試并對比了搭載單一種類傳感器與多傳感器兩種情況得到的定位信息。結果表明，多傳感器結合的方案可以顯著改善機器人返回位置信息的準確度。

Kalman 于20 世紀60 年代提出的EKF 算法可以對非線性系統(tǒng)進行最優(yōu)化估計[3]，現(xiàn)已是傳感器融合算法的最優(yōu)選擇之一，也廣泛應用于定位領域[4]，其可以對特定傳感器的數(shù)據(jù)進行矯正，也可以進行多傳感器數(shù)據(jù)融合，導出最優(yōu)位置信息。Chen 等人[5]研究了基于GPS、磁力計與INS的連續(xù)定位方案，在室外部分采用GPS 定位，室內采用INS 與磁力計獲取位置信息，采用卡爾曼濾波解決噪聲問題，發(fā)現(xiàn)在濾波之后，軌跡噪聲下降了2/3。Li 等人[6]研究了基于EKF 的實時VIO 算法，并利用該技術對視覺慣性里程計進行矯正，實驗表明這種估計算法具有優(yōu)良的準確性與一致性。Cai[7]等設計了基于WiFi 與手機慣性導航的室內定位系統(tǒng)，結合兩種傳感器的適用場景，并基于EKF 算法對其數(shù)據(jù)進行融合處理，最終實現(xiàn)了高精度的定位系統(tǒng)。

本研究以雙輪差速機器人為平臺的EKF 數(shù)據(jù)融合算法，結合基于輪式里程計與IMU 的DR 算法，以及GPS 模塊獲取機器人位置信息。同時針對DR 積分誤差隨時間增長的問題，我們將采用GPS 數(shù)據(jù)直接對DR 進行校準。

1 傳感器的選型及其原理

1.1 輪式里程計推算雙輪差速機器人航位

本研究采用13 線霍爾編碼器來構成輪式里程計，其旋轉一圈產(chǎn)生的脈沖數(shù)PPR 為390。當電機轉動帶動編碼器的碼盤旋轉時，根據(jù)碼盤的編碼原理輸出A、B 兩相正交脈沖信號。微型處理器會根據(jù)這兩相信號的相位差對編碼器計數(shù)。

在得到編碼器計數(shù)值之后，我們可以推出機器人左、右輪速，隨后通過建立其運動學模型的方法來實現(xiàn)輪式里程計的功能。在二維理想環(huán)境中對機器人進行運動學分析，如圖1 和圖2 所示。

圖1 雙輪差速機器人運動學建模

圖2 雙輪差速機器人行駛時的運動模型

上述兩圖中，（x,y,θ）表示雙輪差速機器人在笛卡爾坐標系下某一時刻的位置與姿態(tài)信息，（x′,y′,θ′）為經(jīng)過δt時間（實驗中為100ms）后的位置、姿態(tài)信息。v和ω分別為其線速度與角速度。假設左輪編碼器在某一時刻產(chǎn)生的累計計數(shù)值為cnt_1，經(jīng)過時間間隔δt后累計計數(shù)值變?yōu)閏nt_2，于是可以得到期間機器人的左輪輪速lV：

同理可以求解得到右輪輪速Vr。則雙輪差速機器人的運動學方程如公式（3）所示：

其中d為前后時刻兩坐標間距，D為機器人車輪半徑，實驗中D測得為0.065m。

雖然輪式里程計結構簡單，易于實現(xiàn)，在理想的環(huán)境下可以得到精確位置信息。但是在實際行駛的過程中，編碼器難免受到機器人慣性作用以及車輪打滑等因素的影響[8]，使得微機采集到的編碼器計數(shù)值出現(xiàn)誤差，從而導致小車輪速出現(xiàn)誤差項vd，并以加性噪聲的形式存在于推算出的坐標中，進而在里程計系統(tǒng)長時間的迭代過程中形成不可忽略的積分誤差，極大地影響定位的準確度。

1.2 IMU 姿態(tài)解算算法

IMU 是用于檢測與測量搭載平臺加速度與旋轉運動的傳感器，能夠提供平臺的姿態(tài)角數(shù)據(jù)。由于采用歐拉角表示描述姿態(tài)矩陣時使用大量的三角函數(shù)會造成運算量較大的問題，使用四元數(shù)法進行姿態(tài)角的變換[9]。通常使用數(shù)字運動處理器（Digital Motion Processor, DMP）直接讀取四元數(shù)，或者用互補濾波、擴展卡爾曼濾波等數(shù)據(jù)融合算法對加速度計與陀螺儀的測量結果進行融合[10,11]。

我們選用MPU6050 模塊檢測機器人的姿態(tài)信息，其內部包含三軸陀螺儀與三軸加速度計，由自帶的DMP 對六軸數(shù)據(jù)進行整合[9]。

用I2C 總線與MPU6050 通信，讀取三軸加速度計與三軸陀螺儀測量到的加速度與角速度值，計算出表示姿態(tài)角的四元數(shù)據(jù)q0、1q、q2、q3[10]，通過下述公式（4）可以解算出機器人的橫滾角（Roll）、航偏角（Yaw）和俯仰角（Pitch），分別用Φ、Ψ 和θ表示，并分別對應指向東天北坐標系（ENU）中的東、北和天向[12]：

由此便可以獲得機器人行駛過程中的姿態(tài)信息。本研究主要使用IMU 來以取平臺航偏角Ψ 信息，以替代輪式里程計推算模型中的航向角θ。

但是MPU6050 等低成本IMU 會受到擾動噪聲、隨機漂移、零偏等干擾因素的影響[13]，隨著時間累積，這些干擾項對的傳感器輸出結果的影響會越來越大，最終導致其無法為機器人提供準確的姿態(tài)信息。

1.3 GPS 定位

GPS 定位具有高精度、測量時間短、可全天候作業(yè)的優(yōu)勢[13]，可以為搭載平臺提供穩(wěn)定可靠的定位服務以及獲取姿態(tài)信息。本研究采用由U-BOX 公司設計的NEO-6M GPS 模塊來確定雙輪差速機器人的位置信息。實驗中我們采用RS232 串口與其進行通信，通過讀取到的GPRMC 最小定位語句，我們可以確定某一時刻機器人的經(jīng)、緯坐標、地面速率、地面航向以及UTC 日期等信息。經(jīng)過坐標轉換，我們將WGS-84 坐標系下的位置信息轉換到ENU 坐標系中，得到四個狀態(tài)參數(shù)：橫坐標x、縱坐標y、航向角θ以及線速度v，方便其與IMU 以及輪式里程計數(shù)據(jù)融合。但是，在隧道與高樓環(huán)境中，GPS 信號將受到不可忽視的噪聲干擾，產(chǎn)生多徑效應，其定位結果將不再可信，所以單一GPS 模塊的應用受限于使用場景[14]。

1.4 融合算法

由上文分析可得，輪式里程計與IMU 組成的DR 系統(tǒng)抗干擾的能力較強，泛用性強。但是在長期獨立工作的情況下，DR 由于模型誤差、傳感器精度以及漂移等問題，導致其積分誤差隨著時間的增長而急速增加，從而無法再發(fā)揮功能。

而GPS 幾乎不會為積分誤差所困擾。它可以為機器人提供全天候的實時絕對定位信息。然而在遇到衛(wèi)星信號為建筑物所遮擋的情況的時候，GPS 傳回的位置信息就會受到極大的干擾，可信度不再。

所以，在DR 與GPS 相結合的定位系統(tǒng)中，我們使用DR 對機器人的航位進行推算，同時使用頻率較低的GPS 對DR 對推算進行修正，取長補短。

2 模型與方法

2.1 基于擴展卡爾曼濾波的模型最優(yōu)估計推導

卡爾曼濾波器（KF）利用上一刻系統(tǒng)狀態(tài)的最優(yōu)解來預測當前系統(tǒng)狀態(tài)的值，同時使用觀測值來修正當前值，以得到最優(yōu)解。

擴展卡爾曼濾波器（EKF）是對一般卡爾曼濾波器在非線性狀態(tài)下的擴展，與一般的卡爾曼濾波器一樣，也分為預測和兩大步驟。

2.1.1 模型的狀態(tài)方程

我們取雙輪差速機器人在笛卡爾坐標系下的橫坐標x、縱坐標y、航向角θ、線速度v以及角速度ω這5 個維度的狀態(tài)數(shù)值作為其的狀態(tài)向量：

其中v與ω由輪式里程計和IMU 直接測量得到，其余三個狀態(tài)量將由前文建立的雙輪差速機器人運動學模型推算得出。對上述的狀態(tài)向量進行簡易運動學分析（如圖3所示），得到如下狀態(tài)方程：

圖3 雙輪差速機器人行駛時的簡易運動模型

f（xt-1）是系統(tǒng)的非線性狀態(tài)轉移函數(shù)，考慮到模型的誤差，我們可以將系統(tǒng)的狀態(tài)方程進一步優(yōu)化成如下方程：

其中，ωt-1是模型的過程噪聲，假設其服從數(shù)學期望為0，方差為Q 的正態(tài)分布。

2.1.2 模型的觀測方程

取機器人在笛卡爾坐標系下的橫坐標x、縱坐標y、航向角θ以及線速度ν這4 個維度的狀態(tài)數(shù)值作為觀測方程Ζt?？梢杂萌缦路匠堂枋鯣PS 對其的狀態(tài)的觀測：

其中，vt-1是模型的觀測噪聲，假設其服從數(shù)學期望為0，方差為R的正態(tài)分布；h為將狀態(tài)映射到測量空間的非線性傳感器模型。

2.1.3 EKF 的預測步

通過前面得到的狀態(tài)方程與觀測方程我們可以寫出系統(tǒng)的先驗故估計方程及其協(xié)方差矩陣：

其中F 為狀態(tài)轉移矩陣。由于該模型中，機器人的航向角θ、橫坐標x、縱坐標y均由線速度與角速度推導得出，所以模型誤差來源于線加速度av以及角加速度aω，設其誤差值分別為vaδ與aωδ，可以推導得出過程噪聲的方差Q：

其中vaδ與aωδ需要手動調定，當給定值越大，說明對齊信任度越低，而給定值越小，說明對其信任度越高。

2.1.4 EKF 的更新步

其中H 為測量矩陣?？柭鲆鍷是卡爾曼濾波器更新步中最為核心的變量。我們希望最優(yōu)估計值能夠盡量逼近真實值xt，從式（13）中可以看出，這一定程度上取決于觀測值與預測值在模型中的權重。K 值較大，說明我們更加信任觀測值；反之則說明我們更加信任預測值。通過調整Q、R 兩個矩陣的初值，可以決定K 值大小。實驗過程中GPS 模塊傳回的觀測值是我們更傾向于信任的，所以賦予觀測噪聲R 的初值較小，而過程噪聲Q 的初值較大，得到K 值較大。

2.1.5 EKF 的狀態(tài)轉移矩陣

由于線性噪聲wt1-、vt在通過非線性系統(tǒng)之后，會變成非線性的信號。在EKF 中，我們習慣于對非線性量采用泰勒級數(shù)展開，將非線性公式為線性公式，然后對每個單獨狀態(tài)變量分別求偏導，得到一個5*5 的雅可比矩。上述公式中提及的狀態(tài)轉移矩陣F 和H 分別為狀態(tài)方程xt和觀測方程zt的雅可比矩陣：

2.2 使用互補濾波解算MPU6050 姿態(tài)角

2.2.1 互補濾波

由于加速度計具有較好的靜態(tài)特性，而陀螺儀在高頻段動態(tài)響應特性好。所以對加速度計采用低通濾波、對陀螺儀采用高通濾波，通過互補算法就可以得到高頻和低頻動態(tài)響應特性都很好的數(shù)據(jù)[10]。

對陀螺儀輸出的角速度積分，可以得到我們所需要的角度，但是由于陀螺儀存在誤差，且這個誤差會隨著時間的變化累加。在互補濾波中，我們采用加速度計對誤差進行補償。首先規(guī)定誤差的定義：

我們在機體坐標系中對陀螺儀的誤差進行糾正。當由地理坐標系旋轉得到機體坐標系時，需要用四元數(shù)表示坐標系的轉換，即如公式（18）所示的旋轉矩陣[15]：

由于當加速度計處于靜止狀態(tài)時，輸出恒為[ 0 0g]，其中g表示重力加速度。則可以得到機體坐標系重力向量：

用加速度計糾正陀螺儀，可以用PI 算法對角速度進行誤差補償，其中補償量如下：

其中Kp為比例系數(shù)，Ki為積分系數(shù)。調節(jié)Kp與Ki的值將直接影響互補濾波的效果：Kp越大，則越信任加速度計的測量值；Kp越小則越信任陀螺儀的測量值。將補償量累加可以得到修正后陀螺儀數(shù)據(jù)：

其中T表示控制周期。將計算得到的四元數(shù)代入公式（4）便能解出歐拉角所描述的姿態(tài)矩陣。

2.2.2 互補濾波調參

由于互補濾波算法控制系統(tǒng)的特征多項式為如下形式[16]：

我們分別用時間常數(shù)τ與無阻尼振蕩頻率nω表示Kp與Ki：

通常認為阻尼比ξ的值為0.707 時系統(tǒng)的穩(wěn)態(tài)響應最好，于是我們可以得到Kp：

于是調節(jié)nω的值便可實現(xiàn)對Kp與Ki的調參，從而改變互補濾波的結果。

經(jīng)過調試，我們取ωn= 0.31。

2.3 航位推算算法的改進

考慮到本研究構建的輪式里程計模型與IMU 測量數(shù)據(jù)會隨著時間累計產(chǎn)生積分誤差，以至于超出閾值，進而干擾定位結果。我們計劃首先在一開始使用GPS 數(shù)據(jù)第一幀對DR 初始化，矯正其航向與位置，并消除IMU 的零點漂移。同時每間隔一段時間 1tδ（實驗中為10s），我們用GPS 傳回的位置及姿態(tài)信息對DR 推算模型的航向角以及x、y坐標進行修正，即用相同時間戳對應的數(shù)據(jù)替代上述待修正值，防止DR 模型產(chǎn)生過大的積分誤差。在接下來實驗與仿真部分，我們將比對DR 修正前后機器人的定位軌跡，以說明該算法的優(yōu)越性。本研究具體的算法流程如圖4 所示。

圖4 傳感器融合流程框圖

3 實驗與仿真

3.1 實驗與仿真

我們的實驗平臺是一個雙輪差速機器人。其裝備了霍爾編碼器，來實現(xiàn)輪式里程計的功能。通過編碼器傳回的數(shù)據(jù)得到左、右輪速，對機器人的運動學模型進行分析，便可以推算出其線速度、角速度以及平面坐標。此外，平臺裝備了一個MPU6050 模塊與一個NEO-6M 模塊。IMU 傳感器來讀取Yaw 角數(shù)據(jù)作為機器人航向角的觀測值，GPS 模塊實時傳回觀測的其線速度、航向角與經(jīng)緯坐標。

在選擇姿態(tài)角解算方法的時候，我們比對了互補濾波與DMP 姿態(tài)解算的效果：將IMU 模塊分別繞著縱笛卡爾坐標系的X、Y、Z 軸旋轉90°，得到相應的Roll、Pitch 和Yaw角數(shù)據(jù)，隨后對采集到的數(shù)據(jù)進行處理，分別得到圖6、7、8 所示的結果，經(jīng)過比對，選擇解算效果更好的方法到數(shù)據(jù)融合中。

圖6 DMP 四元數(shù)解算（藍色曲線）與互補濾波姿態(tài)解算YAW 角效果

圖7 DMP 四元數(shù)解算（藍色曲線）與互補濾波姿態(tài)解算PITCH 角效果

圖8 DMP 四元數(shù)解算（藍色曲線）與互補濾波姿態(tài)解算ROLL 角效果

由于GPS 模塊得到的位置信息基于經(jīng)緯高坐標系（LLA），無法直接與輪式里程計推算的位置信息直接進行數(shù)據(jù)融合，所以我們需要將位置信息轉換到轉換為東北天坐標系（ENU）下，具體步驟是先將LLA 下的數(shù)據(jù)轉換到地心地固直角坐標系（ECEF）下，再將ECEF 中的位置信息轉換到ENU 中。

坐標轉換結束之后，便可以通過前面提及的方法對三個傳感器的數(shù)據(jù)進行融合。值得注意的是，輪式里程計與IMU的采樣頻率均為10Hz，而GPS 模塊的采樣頻率為1Hz。所以以GPS 數(shù)據(jù)為基準，對IMU 與輪式里程計返回的數(shù)據(jù)進行同步，其中對IMU 的預處理不僅僅是降采樣，還包括了對兩個采樣點之間的數(shù)據(jù)進行的均值濾波。最后，將預處理完成的數(shù)據(jù)代入EKF 數(shù)學模型中，對小車的實際位置信息進行最優(yōu)估計。

3.2 實驗結果分析

對比圖6、7、8 中互補濾波的效果，發(fā)現(xiàn)對Roll 角和Pitch 進行互補濾波的效果較好，但是對YAW 角進行互補濾波效果很差。同時，互補濾波容易受高頻噪聲影響，當IMU 從相對靜止的狀態(tài)開始翻轉時，往往互補濾波的結果會出現(xiàn)較大的突變，這對于DR 推算的結果會產(chǎn)生不利的影響；相較之下，DMP 四元數(shù)解算得到的姿態(tài)角幾乎不受噪聲影響，效果很好。

之后，我們對互補濾波輸出結果進行低通濾波，試圖濾除干擾以得到更加可靠的結果，如圖9 所示。

圖9 IMU 的Roll 角旋轉時互補濾波結果、加入低通濾波器后互補濾波的結果與DMP 四元數(shù)解算結果（黑色曲線）比較

可以發(fā)現(xiàn)，盡管低通濾波器濾除了許多高頻噪聲，但是濾波后的結果對比濾波前的結果滯后了8 個采樣點，即800毫秒，且整體波形的平滑度仍不及DMP 四元數(shù)解算結果。所以在進行數(shù)據(jù)融合的時候，我們選用實驗中表現(xiàn)更好的DMP 解算機器人的Yaw 角數(shù)據(jù)，用于最終的數(shù)據(jù)融合中。

我們將采集結束的數(shù)據(jù)在matlab 中進行時間戳對齊之后，直接代入EKF 模型，我們可以得到機器人位置的最優(yōu)估計，并將DR 推算、GPS 觀測與融合結果三條曲線整合到同一張圖中（圖10、圖11）以便與真實路徑進行直觀的比對。

圖10 DR 推算、GPS 觀測、EKF 融合結果三種軌跡與真實軌跡對比。其中紅色曲線為EKF 預測結果（我們數(shù)據(jù)融合得到的結果）、黃色曲線為DR 推斷結果、藍色曲線為GPS 觀測結果、綠色曲線為真實路徑

圖11 DR 推算、GPS 觀測結果與真實路徑的比較（圖片上半部分）。EKF 融合結果與真實路徑的比較（圖片下半部分）。

觀察圖10 與圖11 中的比較結果，可以發(fā)現(xiàn)DR 推算結果是最差的，其終點與真實路徑終點相差為14.79 米；GPS單獨觀測的結果與真實路徑終點相差1.29 米；而EKF 融合結果毋庸置疑是最好的，與實際終點相差僅有0.5814 米。

直接對比圖中路徑可以發(fā)現(xiàn)DR 推算結果與真實路徑相差甚遠，而雖然GPS 直接觀測得到的路徑相較DR 要更加貼合真實路徑，在機器人第二次轉彎附近，其觀測結果還是出現(xiàn)了較大誤差。最后，用紅色曲線表示的EKF 融合得到的路徑很明顯是最佳的，與真實路徑的貼合程度最高。

因此，采用擴展卡爾曼濾波可以有效地提升傳感器對機器人的定位精度。

4 結束語

本文介紹了基于卡爾曼濾波的多傳感器數(shù)據(jù)信息融合，將DR 推算的結果與GPS 觀測的結果相結合，解算雙輪差速機器人的航位信息。我們比較了純DR 推算與GPS+DR兩種方案與預定軌跡的偏差，發(fā)現(xiàn)多傳感器融合的定位系統(tǒng)可以極大地改善定位結果的準確性。

同時，在IMU 姿態(tài)解算方式的性能上，我們也發(fā)現(xiàn)DMP 硬件解算的方法是優(yōu)于互補濾波算法所得結果的。

在實際應用中一般會借助路網(wǎng)信息、高精地圖等其他無人駕駛關鍵技術來輔助機器人定位。那么如何學習并使用上述方法為機器人定位以及更好地解決此類問題也無疑會成為我們接下來研究的方向。