基于里程計/地磁/GPS的導航系統設計

孫 華，李 偉，吳亞明

(哈爾濱工程大學 自動化學院，黑龍江 哈爾濱 150001)

基于里程計/地磁/GPS的導航系統設計

孫 華，李 偉，吳亞明

(哈爾濱工程大學 自動化學院，黑龍江 哈爾濱 150001)

導航是機器人技術中的核心問題。針對單一導航系統的不足，設計了一種基于里程計/地磁/GPS的移動機器人的組合導航系統。通過建立各導航系統的誤差模型，并對誤差進行分析，利用卡爾曼濾波器對導航系統的誤差進行估計，得到系統誤差的最優估計。最后利用間接法濾波輸出對導航系統進行校正。通過MATLAB進行仿真實驗，證明該導航系統能夠有效地降低導航誤差，提高了移動機器人的導航定位精度，而且當某一導航系統接收不到信號或者壞掉時，系統仍然可以保持高精度的導航。

組合導航；卡爾曼濾波；移動機器人；里程計

0 引言

移動機器人導航技術的關鍵在于導航與定位的信息檢測和路徑規劃等多種傳感器信息的融合。機器人只有通過自身的傳感器感知周圍的環境信息和自身的位置、姿態，才能在有障礙物的環境中無碰撞地完成面向目標點的任務。目前廣泛應用于移動機器人的導航與定位信息檢測的傳感器有里程計、陀螺儀、GPS、地磁傳感器、超聲波傳感器等[1]。

里程計是一種位置傳感器，可以用來測量車輪的速度信息，從而計算出機器人的位置和姿態，但是存在累計誤差；GPS具有高精度、全球性、全天候等特點，但其信號容易受到干擾，常作為輔助導航系統；地磁傳感器可以測出地球自生的磁場信息，從而計算出機器人與地理北的夾角，但是它易受周圍磁場影響。

由此可見，每一種導航系統都有自身的獨特性和局限性，很難獨立地去完成導航任務，因此本文設計了一種以FPGA為導航計算機，基于里程計/地磁傳感器/GPS的組合導航系統，實現移動機器人的導航定位，提高導航精度。

1 移動機器人的導航定位

1.1 光電編碼器測量機器人位置及航向角

光電編碼器又稱光電碼盤，是一種測量速度和位置的裝置。假設光電碼盤編碼原盤上的光柵總數為N，電動機通過K倍的減速器驅動車輪，車輪的直徑為D，在采樣時間Ts內，光電碼盤輸出的脈沖增量為Q，則在單位采樣時間內車輪的增量ΔS為：

(1)

假設左、右兩個驅動輪之間的距離為w，單位采樣時間內左、右兩輪的位置增量分別為ΔSL和ΔSR，機器人從t-1時刻St-1=(xt-1,yt-1,θt-1)運動到t時刻的St=(xt,yt,θt)，則單位采樣時間內位姿增量為：

(2)

光電編碼器位置及測量航向角原理圖如圖1所示。

機器人在短時間內的運動可以近似看作是直線運動。直線模型具體描述如下：

(3)

1.2 三軸地磁傳感器測量機器人航向角

地磁傳感器用來測量地球周圍磁場，通過各個坐標系上磁敏感軸測出的地磁分量值，再經過計算和誤差校正，可以得到載體與地磁北的夾角，再加上磁偏角即可得到載體準確的航向角[2]。

假設移動機器人運動在水平的路面上，這時就不用考慮Z軸上的地磁分量。三軸傳感器的X敏感軸和Y敏感軸分別沿機器人的橫軸和縱軸方向。假設此時磁敏感軸的X軸和Y軸測得的地磁場強度分別為α和β。由圖2三軸地磁傳感器測量航向角原理圖，根據三角關系可求出此時的磁航角θ，由tanθ=α/β可求出θ=arctan(α/β)。

圖2 三軸地磁傳感器測量航向角原理圖

1.3 GPS導航

GPS主要由GPS衛星星座、地面監控和用戶接受設備三部分組成。GPS通過觀測信號傳播時間來計算出衛星與用戶之間的距離，再通過導航電文里衛星星歷、時鐘改正等信息可以反推出目標位置在WGS-84坐標系下的三維坐標。設用戶坐標為(Xu,Yu,Zu)，第i顆衛星的坐標為(Xsi,Ysi,Zsi)，可以求出用戶到第i顆衛星的真實距離Ri為[3]：

(4)

在實際應用中，由于各種誤差的存在，接收機無法測出用戶與衛星之間的真實距離，只能測出包含各種誤差在內的“偽距”。用戶與第i顆衛星之間的偽距Di可以表示為：

Di=Ri+δI(t)+δT(t)+c(tu-tsi)

(5)

電離層和對流層產生的誤差可以通過雙頻傳輸和誤差補償模型來補償，衛星的坐標、時鐘偏差和延遲誤差可以從導航電文中獲得。則上述方程可寫為：

ρi=Ri+ctu

(6)

式中，ρi=Di-δI(t)-δT(t)；i的范圍是1～4，代表不同的4顆衛星。

從上述原理中可以看到測量一顆衛星可以得到一個方程，上式中有四個未知數，所以至少得測量四顆衛星的距離才可以求出用戶坐標。

由于GPS采用的是WGS-84坐標系，因此在計算時需要對坐標進行轉換。坐標轉換修正后的P(X1，Y1)點的平面坐標的公式[4]為：

(7)

2 基于FPGA的移動機器人導航系統

在本文中，移動機器人作為導航系統的載體，需要承載著各種傳感器和FPGA控制板、無線數據傳輸裝置、電池等物品。移動機器人導航系統硬件結構主要可分四個部分：(1)主控單元DE2 Board開發板，負責與各個傳感器進行通信、整個系統的信息處理以及對移動機器人運動的控制等；(2)系統導航單元，由GPS、地磁傳感器、光電編碼器和無線數據傳輸模塊組成；(3)避障單元，由超聲波測距和紅外線測距共同組成，實現對障礙物的檢測和避讓；(4)硬件接口板單元，負責提供各種不同的電源電壓以及各個傳感器和DE2開發板之間的硬件接口。系統的硬件結構圖如圖3所示。

3 導航系統卡爾曼濾波器的設計

本文使用卡爾曼濾波器對系統誤差進行估計，得到系統誤差的最優估計，再把這一估計值從實際測得的位置和方位角中減去，從而得到機器人此時此刻的位置和方位角等參量[5]。

3.1 里程計的誤差模型

在平坦路面下移動機器人的運動學模型：

(8)

本設計中光電編碼器安裝在移動機器人的左右兩個驅動輪上，構成了里程計導航系統，基于里程計系統的航位推算公式為：

(9)

里程計的誤差可分為系統誤差和非系統誤差兩種。系統誤差是由移動機器人的驅動輪安裝位置的誤差以及左、右兩輪半徑不同以及兩輪之間的距離測量不準確造成的。非系統誤差主要是由車輪打滑造成的，但它對里程計造成的誤差是隨機產生的，可以當做零均值白噪聲來處理[6]。考慮到系統誤差因素的影響，可得出：

(10)

(11)

(12)

3.2 GPS和地磁傳感器的誤差模型

本文根據GPS導航系統原理，直接給出GPS的誤差模型：

(13)

其中，φg、λg分別為GPS導航電文中的緯度、經度；φ、λ分別為系統真實的緯度、經度；δφg、δλg分別為系統的緯度、經度誤差；mpx、mpy可看作為GPS觀測信息中的系統白噪聲。地磁傳感器測出的航向角只是移動機器人與地磁北的夾角，地磁北與地理北之間存在著一定的夾角，稱為磁偏角。為了獲得移動機器人與地理北的航向角信息，需要加上磁偏角。由此得出地磁傳感器測航向角的誤差模型為：

θEC=θ+δθEC+εEC

(14)

其中，θEC是地磁傳感器經過處理后最終輸出的航向角信息；δθEC為地磁北與地理北之間磁偏角的誤差；εEC是受到周圍環境中隨機干擾磁場影響產生的誤差，可以看作系統白噪聲來處理。

3.3 里程計/GPS/地磁傳感器的卡爾曼濾波器設計

圖4 移動機器人導航系統濾波方案

其中，對里程計和GPS輸出的位置誤差信息進行濾波輸出，作為導航系統的位置誤差估計；對里程計和地磁傳感器輸出的航向角誤差進行濾波輸出，作為導航系統的航向角誤差估計。系統的狀態變量包括：里程計的位置誤差和航向角誤差δX(k)、δY(k)、δθ(k)，GPS信號的位置誤差δφg、δλg，地磁傳感器輸出的航向角誤差δθEC以及移動機器人測量時的結構誤差δSL(k)、δSR(k)、δW(k)。系統的狀態變量、狀態方程以及狀態矩陣分別為：

(15)

(16)

(17)

由于GPS和地磁傳感器測量的是絕對量，它們的誤差不隨時間積累，綜合式(11)、(13)和(14)，可得到系統的觀測矩陣為：

(18)

(1)狀態變量預測：

(19)

(2)協方差陣預測：

(20)

(3)卡爾曼濾波增益：

Kk+1=Pk+1/kCT[CPk+1/kCT+R]-1

(21)

(4)狀態更新：

(22)

(5)協方差矩陣更新：

(23)

通過上式可以對移動機器人的位置誤差以及航向角誤差δX、δY、δθ這三個參數做出最優估計，再通過公式(11)可以計算出移動機器人的位置和航向角X、Y、θ這三個量，從而實現移動機器人精確的導航。

4 仿真結果

為了對本設計中組合導航性能和卡爾曼濾波器的準確性進行驗證，采用MATLAB對系統進行仿真。結合本文的實際情況，將移動機器人左右兩輪半徑的測量誤差δSL(k)、δSR(k)和它們之間距離的測量誤差δW(k)均設置為0.1 cm，導航計算機程序執行的時間周期應該與系統的采樣周期相同，將它們設置為0.2 s，左、右兩輪在一個采樣周期里面行駛的距離UL、UR分別設置為5.97 cm和6.02 cm，仿真的時長設置為60 s。仿真結果如圖5～圖7所示。

5 結論

圖5 東向位置誤差估計

圖6 北向位置誤差估計

圖7 航向角誤差估計

本文設計的基于里程計/地磁/GPS的組合導航系統，結合卡爾曼濾波對系統的位置誤差和航向角誤差進行估計。通過MATLAB仿真驗證，該系統能夠有效地減小移動機器人導航系統的誤差，提高系統導航精度；并且在GPS信號丟失時，在一定的時間內同樣可以實現高精度的導航功能，該系統的各方面性能都優于單一導航系統。

[1] 李磊,葉濤,譚民.移動機器人技術研究現狀與來[J].機器人,2002,24(5):475-480.

[2] 徐德,鄒偉.室內移動式服務機器人的感知、定位與控制[M].北京：科學出版社，2008.

[3] 于金霞,蔡自興,鄒小兵,等.非平坦地形下移動機器人航跡推算系統設計[J].河南理工大學學報,2005,24(3):210-216.

[4] 徐衛明,趙俊生. GPS測量坐標轉換實用性問題的分析[J]. 測繪工程， 2000,9(2): 10-15.

[5] 張國良,曾靜.組合導航原理與技術[M].西安:西安交通大學出版社,2008.

[6] 常青,鄭平方,柳重堪,等.車載GPS/DR組合導航系統數據融合算法研究[J].通信學報，2000,21(2): 42-47.

Navigation system design based on odometer/geomagnetic /GPS

Sun Hua, Li Wei, Wu Yaming

(College of Automation,Harbin Engineering University,Harbin 150001,China)

Navigation is the core problem in robotics. Aiming at the disadvantages of single navigation system, a mobile robot’s combined navigation system based on odometer/geomagnetic/GPS is designed. Through establishing the error model of the navigation systems and analyzing the error, using the Calman filter to estimate the error of the navigation system, the system’s optimal estimation is cbtained. Finally, the navigation system is corrected by the indirect method. The simulation experiment is carried out by MATLAB. The results show that the navigation system can effectively reduce the navigation error and improve the mobile robot’s navigation and positioning precision. And when a navigation system can’t receive the signal or bad, the system can still maintain high precision navigation.

combined navigation; Calman filter; mobile robot; odometer

TN967.1

A

1674-7720(2016)01-0081-04

孫華，李偉，吳亞明．基于里程計/地磁/GPS的導航系統設計[J] .微型機與應用，2016，35(1)：81-84，87.

2015-09-13)

孫華(1963-)，女，博士，教授，碩士研究生導師，主要研究方向：機器人導航定位、路徑規劃以及智能控制。

李偉(1991-)，通訊作者，男，在讀碩士研究生，主要研究方向：機器人導航與路徑規劃。E-mail：179994303@qq.com。

吳亞明(1991-)，男，在讀碩士研究生，主要研究方向：控制工程與科學。