基于里程計(jì)/地磁/GPS的導(dǎo)航系統(tǒng)設(shè)計(jì)

孫 華，李 偉，吳亞明

(哈爾濱工程大學(xué) 自動(dòng)化學(xué)院，黑龍江 哈爾濱 150001)

基于里程計(jì)/地磁/GPS的導(dǎo)航系統(tǒng)設(shè)計(jì)

孫 華，李 偉，吳亞明

(哈爾濱工程大學(xué) 自動(dòng)化學(xué)院，黑龍江 哈爾濱 150001)

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

組合導(dǎo)航；卡爾曼濾波；移動(dòng)機(jī)器人；里程計(jì)

0 引言

移動(dòng)機(jī)器人導(dǎo)航技術(shù)的關(guān)鍵在于導(dǎo)航與定位的信息檢測(cè)和路徑規(guī)劃等多種傳感器信息的融合。機(jī)器人只有通過(guò)自身的傳感器感知周圍的環(huán)境信息和自身的位置、姿態(tài)，才能在有障礙物的環(huán)境中無(wú)碰撞地完成面向目標(biāo)點(diǎn)的任務(wù)。目前廣泛應(yīng)用于移動(dòng)機(jī)器人的導(dǎo)航與定位信息檢測(cè)的傳感器有里程計(jì)、陀螺儀、GPS、地磁傳感器、超聲波傳感器等[1]。

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

由此可見(jiàn)，每一種導(dǎo)航系統(tǒng)都有自身的獨(dú)特性和局限性，很難獨(dú)立地去完成導(dǎo)航任務(wù)，因此本文設(shè)計(jì)了一種以FPGA為導(dǎo)航計(jì)算機(jī)，基于里程計(jì)/地磁傳感器/GPS的組合導(dǎo)航系統(tǒng)，實(shí)現(xiàn)移動(dòng)機(jī)器人的導(dǎo)航定位，提高導(dǎo)航精度。

1 移動(dòng)機(jī)器人的導(dǎo)航定位

1.1 光電編碼器測(cè)量機(jī)器人位置及航向角

光電編碼器又稱光電碼盤，是一種測(cè)量速度和位置的裝置。假設(shè)光電碼盤編碼原盤上的光柵總數(shù)為N，電動(dòng)機(jī)通過(guò)K倍的減速器驅(qū)動(dòng)車輪，車輪的直徑為D，在采樣時(shí)間Ts內(nèi)，光電碼盤輸出的脈沖增量為Q，則在單位采樣時(shí)間內(nèi)車輪的增量ΔS為：

(1)

假設(shè)左、右兩個(gè)驅(qū)動(dòng)輪之間的距離為w，單位采樣時(shí)間內(nèi)左、右兩輪的位置增量分別為ΔSL和ΔSR，機(jī)器人從t-1時(shí)刻St-1=(xt-1,yt-1,θt-1)運(yùn)動(dòng)到t時(shí)刻的St=(xt,yt,θt)，則單位采樣時(shí)間內(nèi)位姿增量為：

(2)

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

機(jī)器人在短時(shí)間內(nèi)的運(yùn)動(dòng)可以近似看作是直線運(yùn)動(dòng)。直線模型具體描述如下：

(3)

1.2 三軸地磁傳感器測(cè)量機(jī)器人航向角

地磁傳感器用來(lái)測(cè)量地球周圍磁場(chǎng)，通過(guò)各個(gè)坐標(biāo)系上磁敏感軸測(cè)出的地磁分量值，再經(jīng)過(guò)計(jì)算和誤差校正，可以得到載體與地磁北的夾角，再加上磁偏角即可得到載體準(zhǔn)確的航向角[2]。

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

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

1.3 GPS導(dǎo)航

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

(4)

在實(shí)際應(yīng)用中，由于各種誤差的存在，接收機(jī)無(wú)法測(cè)出用戶與衛(wèi)星之間的真實(shí)距離，只能測(cè)出包含各種誤差在內(nèi)的“偽距”。用戶與第i顆衛(wèi)星之間的偽距Di可以表示為：

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

(5)

電離層和對(duì)流層產(chǎn)生的誤差可以通過(guò)雙頻傳輸和誤差補(bǔ)償模型來(lái)補(bǔ)償，衛(wèi)星的坐標(biāo)、時(shí)鐘偏差和延遲誤差可以從導(dǎo)航電文中獲得。則上述方程可寫為：

ρi=Ri+ctu

(6)

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

從上述原理中可以看到測(cè)量一顆衛(wèi)星可以得到一個(gè)方程，上式中有四個(gè)未知數(shù)，所以至少得測(cè)量四顆衛(wèi)星的距離才可以求出用戶坐標(biāo)。

由于GPS采用的是WGS-84坐標(biāo)系，因此在計(jì)算時(shí)需要對(duì)坐標(biāo)進(jìn)行轉(zhuǎn)換。坐標(biāo)轉(zhuǎn)換修正后的P(X1，Y1)點(diǎn)的平面坐標(biāo)的公式[4]為：

(7)

2 基于FPGA的移動(dòng)機(jī)器人導(dǎo)航系統(tǒng)

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

3 導(dǎo)航系統(tǒng)卡爾曼濾波器的設(shè)計(jì)

本文使用卡爾曼濾波器對(duì)系統(tǒng)誤差進(jìn)行估計(jì)，得到系統(tǒng)誤差的最優(yōu)估計(jì)，再把這一估計(jì)值從實(shí)際測(cè)得的位置和方位角中減去，從而得到機(jī)器人此時(shí)此刻的位置和方位角等參量[5]。

3.1 里程計(jì)的誤差模型

在平坦路面下移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型：

(8)

本設(shè)計(jì)中光電編碼器安裝在移動(dòng)機(jī)器人的左右兩個(gè)驅(qū)動(dòng)輪上，構(gòu)成了里程計(jì)導(dǎo)航系統(tǒng)，基于里程計(jì)系統(tǒng)的航位推算公式為：

(9)

里程計(jì)的誤差可分為系統(tǒng)誤差和非系統(tǒng)誤差兩種。系統(tǒng)誤差是由移動(dòng)機(jī)器人的驅(qū)動(dòng)輪安裝位置的誤差以及左、右兩輪半徑不同以及兩輪之間的距離測(cè)量不準(zhǔn)確造成的。非系統(tǒng)誤差主要是由車輪打滑造成的，但它對(duì)里程計(jì)造成的誤差是隨機(jī)產(chǎn)生的，可以當(dāng)做零均值白噪聲來(lái)處理[6]?？紤]到系統(tǒng)誤差因素的影響，可得出：

(10)

(11)

(12)

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

本文根據(jù)GPS導(dǎo)航系統(tǒng)原理，直接給出GPS的誤差模型：

(13)

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

θEC=θ+δθEC+εEC

(14)

其中，θEC是地磁傳感器經(jīng)過(guò)處理后最終輸出的航向角信息；δθEC為地磁北與地理北之間磁偏角的誤差；εEC是受到周圍環(huán)境中隨機(jī)干擾磁場(chǎng)影響產(chǎn)生的誤差，可以看作系統(tǒng)白噪聲來(lái)處理。

3.3 里程計(jì)/GPS/地磁傳感器的卡爾曼濾波器設(shè)計(jì)

圖4 移動(dòng)機(jī)器人導(dǎo)航系統(tǒng)濾波方案

其中，對(duì)里程計(jì)和GPS輸出的位置誤差信息進(jìn)行濾波輸出，作為導(dǎo)航系統(tǒng)的位置誤差估計(jì)；對(duì)里程計(jì)和地磁傳感器輸出的航向角誤差進(jìn)行濾波輸出，作為導(dǎo)航系統(tǒng)的航向角誤差估計(jì)。系統(tǒng)的狀態(tài)變量包括：里程計(jì)的位置誤差和航向角誤差δX(k)、δY(k)、δθ(k)，GPS信號(hào)的位置誤差δφg、δλg，地磁傳感器輸出的航向角誤差δθEC以及移動(dòng)機(jī)器人測(cè)量時(shí)的結(jié)構(gòu)誤差δSL(k)、δSR(k)、δW(k)。系統(tǒng)的狀態(tài)變量、狀態(tài)方程以及狀態(tài)矩陣分別為：

(15)

(16)

(17)

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

(18)

(1)狀態(tài)變量預(yù)測(cè)：

(19)

(2)協(xié)方差陣預(yù)測(cè)：

(20)

(3)卡爾曼濾波增益：

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

(21)

(4)狀態(tài)更新：

(22)

(5)協(xié)方差矩陣更新：

(23)

通過(guò)上式可以對(duì)移動(dòng)機(jī)器人的位置誤差以及航向角誤差δX、δY、δθ這三個(gè)參數(shù)做出最優(yōu)估計(jì)，再通過(guò)公式(11)可以計(jì)算出移動(dòng)機(jī)器人的位置和航向角X、Y、θ這三個(gè)量，從而實(shí)現(xiàn)移動(dòng)機(jī)器人精確的導(dǎo)航。

4 仿真結(jié)果

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

5 結(jié)論

圖5 東向位置誤差估計(jì)

圖6 北向位置誤差估計(jì)

圖7 航向角誤差估計(jì)

本文設(shè)計(jì)的基于里程計(jì)/地磁/GPS的組合導(dǎo)航系統(tǒng)，結(jié)合卡爾曼濾波對(duì)系統(tǒng)的位置誤差和航向角誤差進(jìn)行估計(jì)。通過(guò)MATLAB仿真驗(yàn)證，該系統(tǒng)能夠有效地減小移動(dòng)機(jī)器人導(dǎo)航系統(tǒng)的誤差，提高系統(tǒng)導(dǎo)航精度；并且在GPS信號(hào)丟失時(shí)，在一定的時(shí)間內(nèi)同樣可以實(shí)現(xiàn)高精度的導(dǎo)航功能，該系統(tǒng)的各方面性能都優(yōu)于單一導(dǎo)航系統(tǒng)。

[1] 李磊,葉濤,譚民.移動(dòng)機(jī)器人技術(shù)研究現(xiàn)狀與來(lái)[J].機(jī)器人,2002,24(5):475-480.

[2] 徐德,鄒偉.室內(nèi)移動(dòng)式服務(wù)機(jī)器人的感知、定位與控制[M].北京：科學(xué)出版社，2008.

[3] 于金霞,蔡自興,鄒小兵,等.非平坦地形下移動(dòng)機(jī)器人航跡推算系統(tǒng)設(shè)計(jì)[J].河南理工大學(xué)學(xué)報(bào),2005,24(3):210-216.

[4] 徐衛(wèi)明,趙俊生. GPS測(cè)量坐標(biāo)轉(zhuǎn)換實(shí)用性問(wèn)題的分析[J]. 測(cè)繪工程， 2000,9(2): 10-15.

[5] 張國(guó)良,曾靜.組合導(dǎo)航原理與技術(shù)[M].西安:西安交通大學(xué)出版社,2008.

[6] 常青,鄭平方,柳重堪,等.車載GPS/DR組合導(dǎo)航系統(tǒng)數(shù)據(jù)融合算法研究[J].通信學(xué)報(bào)，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

孫華，李偉，吳亞明．基于里程計(jì)/地磁/GPS的導(dǎo)航系統(tǒng)設(shè)計(jì)[J] .微型機(jī)與應(yīng)用，2016，35(1)：81-84，87.

2015-09-13)

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

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

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