摘要：導(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)航。
關(guān)鍵詞：組合導(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ī)器人位置及航向角
光電編碼器又稱光電碼盤(pán)，是一種測(cè)量速度和位置的裝置。假設(shè)光電碼盤(pán)編碼原盤(pán)上的光柵總數(shù)為N，電動(dòng)機(jī)通過(guò)K倍的減速器驅(qū)動(dòng)車輪，車輪的直徑為D，在采樣時(shí)間Ts內(nèi)，光電碼盤(pán)輸出的脈沖增量為Q，則在單位采樣時(shí)間內(nèi)車輪的增量ΔS為：
假設(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)位姿增量為：
光電編碼器位置及測(cè)量航向角原理圖如圖1所示。
機(jī)器人在短時(shí)間內(nèi)的運(yùn)動(dòng)可以近似看作是直線運(yùn)動(dòng)。直線模型具體描述如下：
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(α/β)。
1.3GPS導(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］：
在實(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)航電文中獲得。則上述方程可寫(xiě)為：
ρ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］為：
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所示?！?/p>
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é)模型：
本設(shè)計(jì)中光電編碼器安裝在移動(dòng)機(jī)器人的左右兩個(gè)驅(qū)動(dòng)輪上，構(gòu)成了里程計(jì)導(dǎo)航系統(tǒng)，基于里程計(jì)系統(tǒng)的航位推算公式為：
里程計(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)誤差因素的影響，可得出：
X^(k)、Y^(k)、θ^(k)為里程計(jì)觀測(cè)到移動(dòng)機(jī)器人在平面坐標(biāo)系下的位置坐標(biāo)和航向角信息； δW(k)為移動(dòng)機(jī)器人左、右兩輪之間測(cè)量的距離誤差；δSL(k)、δSR(k)為移動(dòng)機(jī)器人左、右兩輪上里程計(jì)測(cè)量的誤差。聯(lián)立式(9)~(11)可得出里程計(jì)系統(tǒng)的誤差模型：
3.2GPS和地磁傳感器的誤差模型
本文根據(jù)GPS導(dǎo)航系統(tǒng)原理，直接給出GPS的誤差模型：
其中，φ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)處理。
33里程計(jì)/GPS/地磁傳感器的卡爾曼濾波器設(shè)計(jì)
卡爾曼濾波器實(shí)際上是一種線性最小方差估計(jì)意義上的最優(yōu)估計(jì)算法，本文將對(duì)移動(dòng)機(jī)器人的位置誤差δX(k)、δY(k)和航向角誤差δθ(k)進(jìn)行濾波，實(shí)現(xiàn)位置誤差δX^、δY^以及方位角誤差δθ^的最優(yōu)估計(jì)。本設(shè)計(jì)采用如圖4所示的方案對(duì)系統(tǒng)進(jìn)行濾波。
其中，對(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)矩陣分別為：
由于GPS和地磁傳感器測(cè)量的是絕對(duì)量，它們的誤差不隨時(shí)間積累，綜合式(11)、(13)和(14)，可得到系統(tǒng)的觀測(cè)矩陣為：
C=［00000011－1］(18)
B為系統(tǒng)的誤差系數(shù)矩陣，可將它設(shè)置為單位矩陣，ω(k)和ν(k)分別為系統(tǒng)噪聲和觀測(cè)噪聲，它們都滿足零均值白噪聲的正態(tài)分布。E［ωk］=E［νk］=0，E［ωk ,ωTk］=Qk，E［νk,νTk］= Rk，Qk和Rk分別為系統(tǒng)噪聲和觀測(cè)噪聲的協(xié)方差矩陣，它們互不相關(guān)。根據(jù)以上信息構(gòu)建系統(tǒng)的卡爾曼濾波器如下：
?。?）狀態(tài)變量預(yù)測(cè)：
通過(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è)置為01 cm，導(dǎo)航計(jì)算機(jī)程序執(zhí)行的時(shí)間周期應(yīng)該與系統(tǒng)的采樣周期相同，將它們?cè)O(shè)置為02 s，左、右兩輪在一個(gè)采樣周期里面行駛的距離UL、UR分別設(shè)置為597 cm和602 cm，仿真的時(shí)長(zhǎng)設(shè)置為60 s。仿真結(jié)果如圖5~圖7所示。
圖5~圖7分別為里程計(jì)的東向位置誤差、北向位置誤差和航向角誤差。實(shí)線為真實(shí)誤差值δX、δY、δθ，虛線為最優(yōu)估計(jì)值δX^、δY^、δθ^。從仿真結(jié)果可以看出，δX^、δY^、δθ^能夠準(zhǔn)確地估計(jì)出真實(shí)誤差δX、δY、δθ，該系統(tǒng)設(shè)計(jì)的卡爾曼濾波器是可行的。
5結(jié)論
本文設(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)航精度；并且在圖5東向位置誤差估計(jì)
GPS信號(hào)丟失時(shí)，在一定的時(shí)間內(nèi)同樣可以實(shí)現(xiàn)高精度的導(dǎo)航功能，該系統(tǒng)的各方面性能都優(yōu)于單一導(dǎo)航系統(tǒng)。
參考文獻(xiàn)
?。?］ 李磊,葉濤,譚民.移動(dòng)機(jī)器人技術(shù)研究現(xiàn)狀與來(lái)［J］.機(jī)器人,2002,24(5):475-480.
?。?］ 徐德,鄒偉.室內(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.
?。?］ 徐衛(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.