基于多傳感器融合的雙輪差速機器人定位研究

錢瑞，馮成濤

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

0 引言

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

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

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

1 傳感器的選型及其原理

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

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

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

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

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

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

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

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

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

1.2 IMU 姿態解算算法

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

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

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

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

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

1.3 GPS 定位

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

1.4 融合算法

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

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

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

2 模型與方法

2.1 基于擴展卡爾曼濾波的模型最優估計推導

卡爾曼濾波器（KF）利用上一刻系統狀態的最優解來預測當前系統狀態的值，同時使用觀測值來修正當前值，以得到最優解。

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

2.1.1 模型的狀態方程

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

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

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

f（xt-1）是系統的非線性狀態轉移函數，考慮到模型的誤差，我們可以將系統的狀態方程進一步優化成如下方程：

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

2.1.2 模型的觀測方程

取機器人在笛卡爾坐標系下的橫坐標x、縱坐標y、航向角θ以及線速度ν這4 個維度的狀態數值作為觀測方程Ζt。可以用如下方程描述GPS 對其的狀態的觀測：

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

2.1.3 EKF 的預測步

通過前面得到的狀態方程與觀測方程我們可以寫出系統的先驗故估計方程及其協方差矩陣：

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

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

2.1.4 EKF 的更新步

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

2.1.5 EKF 的狀態轉移矩陣

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

2.2 使用互補濾波解算MPU6050 姿態角

2.2.1 互補濾波

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

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

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

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

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

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

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

2.2.2 互補濾波調參

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

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

通常認為阻尼比ξ的值為0.707 時系統的穩態響應最好，于是我們可以得到Kp：

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

經過調試，我們取ωn= 0.31。

2.3 航位推算算法的改進

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

圖4 傳感器融合流程框圖

3 實驗與仿真

3.1 實驗與仿真

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

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

圖6 DMP 四元數解算（藍色曲線）與互補濾波姿態解算YAW 角效果

圖7 DMP 四元數解算（藍色曲線）與互補濾波姿態解算PITCH 角效果

圖8 DMP 四元數解算（藍色曲線）與互補濾波姿態解算ROLL 角效果

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

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

3.2 實驗結果分析

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

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

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

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

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

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

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

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

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

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

4 結束語

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

同時，在IMU 姿態解算方式的性能上，我們也發現DMP 硬件解算的方法是優于互補濾波算法所得結果的。

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