摘 要：本文介紹了工作在特殊環(huán)境下的機(jī)器人導(dǎo)航系統(tǒng)，實(shí)現(xiàn)了利用羅盤(pán)和里程計(jì)等內(nèi)部傳感器對(duì)GPS定位誤差的修正。文章給出了消防機(jī)器人導(dǎo)航的主要算法流程，以及利用改進(jìn)自適應(yīng)卡爾曼濾波器實(shí)現(xiàn)多傳感器信息融合。 關(guān)鍵詞：移動(dòng)機(jī)器人，導(dǎo)航系統(tǒng)，GPS，多傳感器，自適應(yīng)卡爾曼濾波器 引言 　　消防偵察機(jī)器人是一類(lèi)實(shí)用型機(jī)器人，它能替代消防救援人員遙控進(jìn)入易燃、易爆、有毒、有害、易坍塌建筑物、大型倉(cāng)庫(kù)堆垛、缺氧、濃煙等室內(nèi)外危險(xiǎn)災(zāi)害現(xiàn)場(chǎng)，進(jìn)行現(xiàn)場(chǎng)探測(cè)、偵察。由于火災(zāi)現(xiàn)場(chǎng)的特殊環(huán)境（建筑物遮擋，濃煙，火焰等），控制人員必須能夠在視野之外準(zhǔn)確地操控消防機(jī)器人，因而必須采用可靠精確的導(dǎo)航系統(tǒng)。一般消防機(jī)器人都載有攝像頭，可以通過(guò)電纜或無(wú)線傳輸模塊將視頻信號(hào)返回給控制人員，由此可以實(shí)現(xiàn)機(jī)器人的視覺(jué)導(dǎo)航。然而視頻信息的帶寬需求比較大，而且易受外部環(huán)境干擾，在許多場(chǎng)合無(wú)法使用。本文提出一種融合GPS，陀螺儀和里程計(jì)等傳感器信息的綜合導(dǎo)航系統(tǒng)方案，系統(tǒng)結(jié)構(gòu)如圖一所示。 GPS定位 　　GPS全球衛(wèi)星定位系統(tǒng)是隨著現(xiàn)代航天及無(wú)線電通訊科學(xué)技術(shù)的發(fā)展建立起來(lái)的一個(gè)高精度、全天候和全球性的無(wú)線電導(dǎo)航電位、定時(shí)的多功能系統(tǒng)。GPS可提供實(shí)時(shí)的三維坐標(biāo)、三維速度和高精度的時(shí)間信息，目前已成為當(dāng)今世界上應(yīng)用最廣泛、最實(shí)用的全球精密授時(shí)、測(cè)距、導(dǎo)航、指揮、調(diào)度系統(tǒng)。 　　GPS模塊可提供定位時(shí)間，經(jīng)緯度，定位功能指示等12項(xiàng)數(shù)據(jù)，輸出格式采用標(biāo)準(zhǔn)NMEA協(xié)議，可以直接使用獲得的經(jīng)緯度數(shù)據(jù)作為方位信息（東經(jīng)XXX.XXXX，北緯XXX.XXXX）。也可以根據(jù)功能指數(shù)和精度因子計(jì)算相對(duì)于出發(fā)點(diǎn)的位置信息（北：±XXXX.XXm;東：±XXXX.XXm）。模塊單機(jī)定位時(shí)精度為10～30米，如果使用了基準(zhǔn)站差分定位技術(shù)則精度可以提高至5～10米[1]。這樣的精度仍然無(wú)法滿(mǎn)足消防機(jī)器人準(zhǔn)確操控的需求，另一方面，消防偵察機(jī)器人需要在工廠區(qū)，室內(nèi)等無(wú)法接收到GPS衛(wèi)星信號(hào)的環(huán)境下工作，因此必須用其他技術(shù)作為GPS定位的補(bǔ)充。 電子羅盤(pán)系統(tǒng) 　　電子羅盤(pán)COMPASS的操作原理是根據(jù)磁阻感應(yīng)地球磁場(chǎng)變化而即時(shí)輸出物體的運(yùn)行方向和運(yùn)動(dòng)姿態(tài)數(shù)據(jù)。運(yùn)動(dòng)方向數(shù)據(jù)包括方位角（順時(shí)針偏離正北方向的角度）等數(shù)據(jù);運(yùn)動(dòng)姿態(tài)數(shù)據(jù)包括俯仰角、旋轉(zhuǎn)角等數(shù)據(jù)。COMPASS的使用目的是針對(duì)GPS接收數(shù)據(jù)的補(bǔ)償，使得本系統(tǒng)在GPS衛(wèi)星信號(hào)接收不理想的情況下，依舊能產(chǎn)生恒定頻率的機(jī)器人運(yùn)行方向和姿態(tài)信息，并進(jìn)一步根據(jù)相應(yīng)算法得出其位置信息。 　　機(jī)器人運(yùn)行時(shí)的震動(dòng)和外界的強(qiáng)磁干擾會(huì)影響電子羅盤(pán)的精度。震動(dòng)對(duì)其影響約 ，可以在CPU端使用軟件濾波器來(lái)解決這個(gè)問(wèn)題[2];當(dāng)外界的強(qiáng)磁場(chǎng)遠(yuǎn)遠(yuǎn)超過(guò)地球磁場(chǎng)時(shí)，甚至可能會(huì)使電子羅盤(pán)完全失效。本系統(tǒng)利用軟件里程計(jì)來(lái)計(jì)算航向，并可用于判定電子羅盤(pán)是否工作正常。 里程計(jì)與航位推算 　　消防機(jī)器人左右輪有各自的驅(qū)動(dòng)電機(jī)和傳動(dòng)系統(tǒng)，控制兩個(gè)電機(jī)運(yùn)行不同的時(shí)間以實(shí)現(xiàn)不同角度轉(zhuǎn)彎。所以也可以利用左右里程計(jì)的數(shù)值差來(lái)計(jì)算機(jī)器人車(chē)體的相對(duì)轉(zhuǎn)角?？梢允褂霉獯a盤(pán)對(duì)驅(qū)動(dòng)輪的轉(zhuǎn)動(dòng)次數(shù)計(jì)數(shù)，然后再用先驗(yàn)公式計(jì)算里程，精確性較好。還可以在CPU內(nèi)部開(kāi)辟寄存器空間，記錄所有電機(jī)運(yùn)行命令中的時(shí)間參數(shù)，以計(jì)算左右輪的里程，作為預(yù)期參考值，但是驅(qū)動(dòng)電機(jī)的誤差會(huì)影響精確性。 　　假設(shè)機(jī)器人兩個(gè)驅(qū)動(dòng)輪間隔L，驅(qū)動(dòng)輪半徑r，驅(qū)動(dòng)電機(jī)每秒鐘可驅(qū)動(dòng)輪子轉(zhuǎn)n圈;則若要向左旋轉(zhuǎn)Δθ度，只要控制右輪比左輪多運(yùn)行 秒。由于機(jī)器人硬件參數(shù)相對(duì)固定，因而轉(zhuǎn)過(guò)一個(gè)固定角度的時(shí)間參數(shù)也可以先驗(yàn)獲取，經(jīng)過(guò)多次試驗(yàn)可以獲得比較精確的數(shù)值。為簡(jiǎn)化計(jì)算復(fù)雜度，機(jī)器人的前進(jìn)和轉(zhuǎn)向是作為兩種運(yùn)動(dòng)方式來(lái)處理的，即機(jī)器人只會(huì)直行，轉(zhuǎn)向時(shí)產(chǎn)生的位移由先驗(yàn)參數(shù)進(jìn)行補(bǔ)償。 　　由于消防機(jī)器人主要在地面工作，因此定位時(shí)也暫不考慮水平高度的變化。系統(tǒng)二維定位坐標(biāo)中，以正北方向?yàn)閅軸正方向，航向角度為機(jī)器人前進(jìn)方向順時(shí)針偏離正北方向的角度。如圖二所示，機(jī)器人先直行ΔS，然后向右轉(zhuǎn)，這里認(rèn)為機(jī)器人直行時(shí)軌跡為理想直線，轉(zhuǎn)向時(shí)的軌跡為理想圓弧，實(shí)際使用時(shí)需添加修正因子。 　　直行的相對(duì)方位變化可由以下公式得到： 　　 　?。ㄊ街笑提取命令中時(shí)間參數(shù)計(jì)算得來(lái)，相對(duì)方位角由航向記錄修正電子羅盤(pán)數(shù)據(jù)提供;γ和λ為修正因子） 　　根據(jù)機(jī)器人實(shí)際航行情況，測(cè)得轉(zhuǎn)彎半徑為R，則轉(zhuǎn)向位移為： 　　 　?。ㄊ街许槙r(shí)針轉(zhuǎn)向時(shí)n=1，逆時(shí)針轉(zhuǎn)向時(shí)n=0;φ和ψ為修正因子） 　　由于驅(qū)動(dòng)輪地面摩擦情況可能發(fā)生變化，電機(jī)驅(qū)動(dòng)誤差等因素，實(shí)際運(yùn)行中的軌跡并非理想狀態(tài)，可利用先驗(yàn)誤差因子作修正;并且在GPS和電子羅盤(pán)工作正常的情況下，周期性校驗(yàn)方位及航向記錄，以避免誤差的疊加。而在外界干擾嚴(yán)重（建筑物遮擋，無(wú)法收到GPS信號(hào);環(huán)境磁場(chǎng)擾亂地磁場(chǎng)）的情況下，就要使用可靠的預(yù)測(cè)算法來(lái)提取有用信息。 改進(jìn)自適應(yīng)卡爾曼濾波與信息綜合 　　在誤差干擾下提取機(jī)器人的正確位置信息，需要使用到卡爾曼濾波器[3]。這是一種遞推線性最小方差估計(jì)，廣泛應(yīng)用于信息提取，信息融合，追蹤、導(dǎo)航等方面。它基于以下兩個(gè)前提：首先，系統(tǒng)狀態(tài)可以由以下線性方程定義： 　　狀態(tài)方程：;（w[sub]k[/sub]為過(guò)程誤差） 　　輸出（測(cè)量）方程：（z[sub]k[/sub]為測(cè)量誤差） 　　其次，過(guò)程誤差與測(cè)量誤差分布滿(mǎn)足零均高斯，且不相關(guān);需要先驗(yàn)的誤差分布參數(shù)?；緸V波公式如下： 　　 　　K是卡爾曼增益，P是預(yù)計(jì)方差矩陣， 是過(guò)程誤差矩陣期望值, 是測(cè)量誤差矩陣期望值。 　　卡爾曼濾波器的性能與誤差分布的先驗(yàn)參數(shù)密切相關(guān)，因此在實(shí)際應(yīng)用中存在諸多問(wèn)題。首先系統(tǒng)的狀態(tài)方程可能是非線性的，線性擬合方程隨時(shí)間推移會(huì)引入較大的誤差;其次外界干擾產(chǎn)生的誤差特性是未知的，錯(cuò)誤的先驗(yàn)信息會(huì)導(dǎo)致濾波結(jié)果與實(shí)際被離。自適應(yīng)卡爾曼濾波器[4]使用滑動(dòng)窗法及最大似然準(zhǔn)則，利用實(shí)際測(cè)量值動(dòng)態(tài)改變?yōu)V波器參數(shù)（測(cè)量方差矩陣和系統(tǒng)方差矩陣），其根本思想在于找出與預(yù)期誤差最小的結(jié)果，并給予最大的權(quán)重。 　　實(shí)際應(yīng)用中，自適應(yīng)卡爾曼濾波器需要一定的訓(xùn)練序列與收斂時(shí)間，如果突然出現(xiàn)超出預(yù)計(jì)的誤差值（如GPS接收機(jī)進(jìn)入信號(hào)陰影區(qū)域），不可避免會(huì)引入誤差，甚至可能導(dǎo)致濾波器無(wú)法收斂，得不到正確的解。如圖四所示，為使用[4]提供的自適應(yīng)卡爾曼濾波算法，得到的機(jī)器人運(yùn)行軌跡（Matlab仿真）。前20個(gè)測(cè)量點(diǎn)是自適應(yīng)濾波器的學(xué)習(xí)過(guò)程，這時(shí)使用先驗(yàn)的誤差參數(shù);結(jié)束學(xué)習(xí)過(guò)程后，預(yù)測(cè)誤差有所減小?？梢钥吹剑贕PS接收良好的情況下，預(yù)計(jì)方位與真實(shí)方位擬合得相當(dāng)好，但是進(jìn)入陰影區(qū)域后，就引入偏差;經(jīng)過(guò)自適應(yīng)收斂過(guò)程后，降低誤差很大的GPS信息權(quán)重，軌跡曲線形態(tài)仍然相似，但是偏差已經(jīng)無(wú)法挽回了。 　　消防機(jī)器人是遠(yuǎn)程操控機(jī)器人，它的所有運(yùn)動(dòng)都是接收遠(yuǎn)程指令，然后送執(zhí)行機(jī)構(gòu)執(zhí)行，因此它的預(yù)期運(yùn)動(dòng)方向及方位是可精確獲知的。針對(duì)系統(tǒng)設(shè)計(jì)的這個(gè)特點(diǎn)，在傳感器信息進(jìn)入自適應(yīng)濾波之前，先進(jìn)行判斷與加權(quán)，與系統(tǒng)預(yù)期值距離遠(yuǎn)的數(shù)據(jù)獲得較輕的權(quán)重。如此，當(dāng)GPS信號(hào)突然惡化，誤差加劇的時(shí)候，這部分誤差對(duì)濾波結(jié)果的影響很小，在濾波器收斂過(guò)程中不會(huì)引入很大的偏移，改進(jìn)后的算法得到的預(yù)測(cè)軌跡如圖四中所示。由于GPS信息在估算中幾乎沒(méi)有影響，無(wú)法修正航位估算中的背離，因而經(jīng)過(guò)一段時(shí)間的誤差累加，估計(jì)值會(huì)逐漸偏離真實(shí)位置，如圖中所示。 [align=center] 圖四 Matlab仿真結(jié)果[/align] 結(jié)論與展望 　　在GPS定位盲區(qū)，本系統(tǒng)利用電子羅盤(pán)和路程記錄進(jìn)行精確方位的估算，能為遠(yuǎn)程盲操控消防機(jī)器人提供較為準(zhǔn)確的導(dǎo)航信息。實(shí)際應(yīng)用中，利用路程記錄的小范圍短距離航位推算較為精確。但如果處于極端情況下，即在長(zhǎng)時(shí)間接收不到正確的GPS定位信息，缺少定位基準(zhǔn)點(diǎn)的情況下，方位推算將逐漸出現(xiàn)偏移。 　　電子羅盤(pán)主要用于指示航向，如改用慣性陀螺儀就可以避免外界磁干擾，增加系統(tǒng)強(qiáng)壯度，但要周期性修正陀螺儀偏移;如果使用光電相對(duì)位移傳感器來(lái)測(cè)量機(jī)器人與地面的相對(duì)移動(dòng)，就可以大大提高航位估算的精度，并且避免由于驅(qū)動(dòng)輪打滑等引起的計(jì)算誤差，但由于目前光電傳感器對(duì)使用環(huán)境要求較為苛刻，有待進(jìn)一步的改進(jìn)。 參考文獻(xiàn)： 　　【1】王廣運(yùn),郭秉義,李洪濤編. 差分GPS定位技術(shù)與應(yīng)用 北京:電子工業(yè)出版社,1996 　　【2】Donald G . Polvani. Magnetic Guidance for Autonomous Vehicle. IEEE 1986 CH2363 1407～1412 　　【3】Toshihiro Aono. Positioning of vehicle on undulating ground using GPS and dead reckoning. Proceeding of IEEE 1998 International Conference on Robotic & Automatic Leuven, Belgium, May 1998, P3443～3448 　　【4】A. H. Mohamed, K. P. Schwarz. Adaptive Kalman Filtering for INS/GPS. Journal of Geodesy （1999） 73: 193～203