摘要：導(dǎo)航是機(jī)器人技術(shù)中的核心問題。針對(duì)單一導(dǎo)航系統(tǒng)的不足,，設(shè)計(jì)了一種基于里程計(jì)/地磁/GPS的移動(dòng)機(jī)器人的組合導(dǎo)航系統(tǒng),。通過建立各導(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)行校正,。通過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ī)器人只有通過自身的傳感器感知周圍的環(huán)境信息和自身的位置,、姿態(tài)，才能在有障礙物的環(huán)境中無碰撞地完成面向目標(biāo)點(diǎn)的任務(wù),。目前廣泛應(yīng)用于移動(dòng)機(jī)器人的導(dǎo)航與定位信息檢測(cè)的傳感器有里程計(jì),、陀螺儀、GPS,、地磁傳感器,、超聲波傳感器等［1］。
里程計(jì)是一種位置傳感器,，可以用來測(cè)量車輪的速度信息,，從而計(jì)算出機(jī)器人的位置和姿態(tài)，但是存在累計(jì)誤差,；GPS具有高精度,、全球性、全天候等特點(diǎn),，但其信號(hào)容易受到干擾,，常作為輔助導(dǎo)航系統(tǒng),；地磁傳感器可以測(cè)出地球自生的磁場(chǎng)信息,，從而計(jì)算出機(jī)器人與地理北的夾角,，但是它易受周圍磁場(chǎng)影響。
由此可見,，每一種導(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ī)通過K倍的減速器驅(qū)動(dòng)車輪,，車輪的直徑為D，在采樣時(shí)間Ts內(nèi),，光電碼盤輸出的脈沖增量為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ī)器人航向角
地磁傳感器用來測(cè)量地球周圍磁場(chǎng),，通過各個(gè)坐標(biāo)系上磁敏感軸測(cè)出的地磁分量值,，再經(jīng)過計(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通過觀測(cè)信號(hào)傳播時(shí)間來計(jì)算出衛(wèi)星與用戶之間的距離，再通過導(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ī)無法測(cè)出用戶與衛(wèi)星之間的真實(shí)距離，只能測(cè)出包含各種誤差在內(nèi)的“偽距”,。用戶與第i顆衛(wèi)星之間的偽距Di可以表示為：
Di=Ri+δI(t)+δT(t)+c(tu－tsi)(5)
電離層和對(duì)流層產(chǎn)生的誤差可以通過雙頻傳輸和誤差補(bǔ)償模型來補(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］為：
2基于FPGA的移動(dòng)機(jī)器人導(dǎo)航系統(tǒng)
在本文中，移動(dòng)機(jī)器人作為導(dǎo)航系統(tǒng)的載體,，需要承載著各種傳感器和FPGA控制板,、無線數(shù)據(jù)傳輸裝置、電池等物品,。移動(dòng)機(jī)器人導(dǎo)航系統(tǒng)硬件結(jié)構(gòu)主要可分四個(gè)部分：（1）主控單元DE2 Board開發(fā)板,，負(fù)責(zé)與各個(gè)傳感器進(jìn)行通信、整個(gè)系統(tǒng)的信息處理以及對(duì)移動(dòng)機(jī)器人運(yùn)動(dòng)的控制等,；（2）系統(tǒng)導(dǎo)航單元,，由GPS、地磁傳感器,、光電編碼器和無線數(shù)據(jù)傳輸模塊組成,；（3）避障單元，由超聲波測(cè)距和紅外線測(cè)距共同組成,，實(shí)現(xiàn)對(duì)障礙物的檢測(cè)和避讓,；（4）硬件接口板單元，負(fù)責(zé)提供各種不同的電源電壓以及各個(gè)傳感器和DE2開發(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)做零均值白噪聲來處理［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)過處理后最終輸出的航向角信息,；δθEC為地磁北與地理北之間磁偏角的誤差；εEC是受到周圍環(huán)境中隨機(jī)干擾磁場(chǎng)影響產(chǎn)生的誤差,，可以看作系統(tǒng)白噪聲來處理,。
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è)：
通過上式可以對(duì)移動(dòng)機(jī)器人的位置誤差以及航向角誤差δX、δY,、δθ這三個(gè)參數(shù)做出最優(yōu)估計(jì),，再通過公式(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ì)。通過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)
［1］ 李磊,葉濤,譚民.移動(dòng)機(jī)器人技術(shù)研究現(xiàn)狀與來［J］.機(jī)器人,2002,24(5):475-480.
?。?］ 徐德,鄒偉.室內(nèi)移動(dòng)式服務(wù)機(jī)器人的感知,、定位與控制［M］.北京：科學(xué)出版社，2008.
?。?］ 于金霞,蔡自興,鄒小兵,等.非平坦地形下移動(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í)用性問題的分析［J］. 測(cè)繪工程， 2000,9(2): 10-15.
?。?］ 張國(guó)良,曾靜.組合導(dǎo)航原理與技術(shù)［M］.西安:西安交通大學(xué)出版社,2008.
?。?］ 常青,鄭平方,柳重堪,等.車載GPS/DR組合導(dǎo)航系統(tǒng)數(shù)據(jù)融合算法研究［J］.通信學(xué)報(bào)，2000,21(2): 42-47.