摘 要：本文介紹了工作在特殊環境下的機器人導航系統，實現了利用羅盤和里程計等內部傳感器對GPS定位誤差的修正。文章給出了消防機器人導航的主要算法流程，以及利用改進自適應卡爾曼濾波器實現多傳感器信息融合。
關鍵詞：移動機器人，導航系統，GPS，多傳感器，自適應卡爾曼濾波器
引言
消防偵察機器人是一類實用型機器人，它能替代消防救援人員遙控進入易燃、易爆、有毒、有害、易坍塌建筑物、大型倉庫堆垛、缺氧、濃煙等室內外危險災害現場，進行現場探測、偵察。由于火災現場的特殊環境（建筑物遮擋，濃煙，火焰等），控制人員必須能夠在視野之外準確地操控消防機器人，因而必須采用可靠精確的導航系統。一般消防機器人都載有攝像頭，可以通過電纜或無線傳輸模塊將視頻信號返回給控制人員，由此可以實現機器人的視覺導航。然而視頻信息的帶寬需求比較大，而且易受外部環境干擾，在許多場合無法使用。本文提出一種融合GPS，陀螺儀和里程計等傳感器信息的綜合導航系統方案，系統結構如圖一所示。
GPS定位
GPS全球衛星定位系統是隨著現代航天及無線電通訊科學技術的發展建立起來的一個高精度、全天候和全球性的無線電導航電位、定時的多功能系統。GPS 可提供實時的三維坐標、三維速度和高精度的時間信息，目前已成為當今世界上應用最廣泛、最實用的全球精密授時、測距、導航、指揮、調度系統。
GPS模塊可提供定位時間，經緯度，定位功能指示等12項數據，輸出格式采用標準NMEA協議，可以直接使用獲得的經緯度數據作為方位信息（東經 XXX.XXXX，北緯XXX.XXXX）。也可以根據功能指數和精度因子計算相對于出發點的位置信息（北：±XXXX.XXm; 東：±XXXX.XXm）。模塊單機定位時精度為10～30米，如果使用了基準站差分定位技術則精度可以提高至5～10米[1]。這樣的精度仍然無法滿足消防機器人準確操控的需求，另一方面，消防偵察機器人需要在工廠區，室內等無法接收到GPS衛星信號的環境下工作，因此必須用其他技術作為GPS定位的補充。
電子羅盤系統
電子羅盤COMPASS的操作原理是根據磁阻感應地球磁場變化而即時輸出物體的運行方向和運動姿態數據。運動方向數據包括方位角（順時針偏離正北方向的角度）等數據;運動姿態數據包括俯仰角、旋轉角等數據。COMPASS的使用目的是針對GPS接收數據的補償，使得本系統在GPS衛星信號接收不理想的情況下，依舊能產生恒定頻率的機器人運行方向和姿態信息，并進一步根據相應算法得出其位置信息。
機器人運行時的震動和外界的強磁干擾會影響電子羅盤的精度。震動對其影響約 ，可以在CPU端使用軟件濾波器來解決這個問題[2];當外界的強磁場遠遠超過地球磁場時，甚至可能會使電子羅盤完全失效。本系統利用軟件里程計來計算航向，并可用于判定電子羅盤是否工作正常。
里程計與航位推算
消防機器人左右輪有各自的驅動電機和傳動系統，控制兩個電機運行不同的時間以實現不同角度轉彎。所以也可以利用左右里程計的數值差來計算機器人車體的相對轉角。可以使用光碼盤對驅動輪的轉動次數計數，然后再用先驗公式計算里程，精確性較好。還可以在CPU內部開辟寄存器空間，記錄所有電機運行命令中的時間參數，以計算左右輪的里程，作為預期參考值，但是驅動電機的誤差會影響精確性。
假設機器人兩個驅動輪間隔L，驅動輪半徑r，驅動電機每秒鐘可驅動輪子轉n圈;則若要向左旋轉Δθ度，只要控制右輪比左輪多運行 秒。由于機器人硬件參數相對固定，因而轉過一個固定角度的時間參數也可以先驗獲取，經過多次試驗可以獲得比較精確的數值。為簡化計算復雜度，機器人的前進和轉向是作為兩種運動方式來處理的，即機器人只會直行，轉向時產生的位移由先驗參數進行補償。
由于消防機器人主要在地面工作，因此定位時也暫不考慮水平高度的變化。系統二維定位坐標中，以正北方向為Y軸正方向，航向角度為機器人前進方向順時針偏離正北方向的角度。如圖二所示，機器人先直行ΔS，然后向右轉
，這里認為機器人直行時軌跡為理想直線，轉向時的軌跡為理想圓弧，實際使用時需添加修正因子。
直行的相對方位變化可由以下公式得到：
（式中ΔS提取命令中時間參數計算得來，相對方位角由航向記錄修正電子羅盤數據提供;γ和λ為修正因子）
根據機器人實際航行情況，測得轉彎半徑為R，則轉向位移為：
（式中順時針轉向時n=1，逆時針轉向時n=0;φ和ψ為修正因子）
由于驅動輪地面摩擦情況可能發生變化，電機驅動誤差等因素，實際運行中的軌跡并非理想狀態，可利用先驗誤差因子作修正;并且在GPS和電子羅盤工作正常的情況下，周期性校驗方位及航向記錄，以避免誤差的疊加。而在外界干擾嚴重（建筑物遮擋，無法收到GPS信號;環境磁場擾亂地磁場）的情況下，就要使用可靠的預測算法來提取有用信息。
改進自適應卡爾曼濾波與信息綜合
在誤差干擾下提取機器人的正確位置信息，需要使用到卡爾曼濾波器[3]。這是一種遞推線性最小方差估計，廣泛應用于信息提取，信息融合，追蹤、導航等方面。它基于以下兩個前提：首先，系統狀態可以由以下線性方程定義：
狀態方程：
;（wk為過程誤差）
輸出（測量）方程：
（zk為測量誤差）
其次，過程誤差與測量誤差分布滿足零均高斯，且不相關;需要先驗的誤差分布參數。基本濾波公式如下：
K是卡爾曼增益，P是預計方差矩陣， 是過程誤差矩陣期望值, 是測量誤差矩陣期望值。
卡爾曼濾波器的性能與誤差分布的先驗參數密切相關，因此在實際應用中存在諸多問題。首先系統的狀態方程可能是非線性的，線性擬合方程隨時間推移會引入較大的誤差;其次外界干擾產生的誤差特性是未知的，錯誤的先驗信息會導致濾波結果與實際被離。自適應卡爾曼濾波器[4]使用滑動窗法及最大似然準則，利用實際測量值動態改變濾波器參數（測量方差矩陣和系統方差矩陣），其根本思想在于找出與預期誤差最小的結果，并給予最大的權重。
實際應用中，自適應卡爾曼濾波器需要一定的訓練序列與收斂時間，如果突然出現超出預計的誤差值（如GPS接收機進入信號陰影區域），不可避免會引入誤差，甚至可能導致濾波器無法收斂，得不到正確的解。如圖四所示，為使用[4]提供的自適應卡爾曼濾波算法，得到的機器人運行軌跡（Matlab仿真）。前 20個測量點是自適應濾波器的學習過程，這時使用先驗的誤差參數;結束學習過程后，預測誤差有所減小。可以看到，在GPS接收良好的情況下，預計方位與真實方位擬合得相當好，但是進入陰影區域后，就引入偏差;經過自適應收斂過程后，降低誤差很大的GPS信息權重，軌跡曲線形態仍然相似，但是偏差已經無法挽回了。
消防機器人是遠程操控機器人，它的所有運動都是接收遠程指令，然后送執行機構執行，因此它的預期運動方向及方位是可精確獲知的。針對系統設計的這個特點，在傳感器信息進入自適應濾波之前，先進行判斷與加權，與系統預期值距離遠的數據獲得較輕的權重。如此，當GPS信號突然惡化，誤差加劇的時候，這部分誤差對濾波結果的影響很小，在濾波器收斂過程中不會引入很大的偏移，改進后的算法得到的預測軌跡如圖四中所示。由于GPS信息在估算中幾乎沒有影響，無法修正航位估算中的背離，因而經過一段時間的誤差累加，估計值會逐漸偏離真實位置，如圖中所示。
圖四 Matlab仿真結果
結論與展望
在GPS定位盲區，本系統利用電子羅盤和路程記錄進行精確方位的估算，能為遠程盲操控消防機器人提供較為準確的導航信息。實際應用中，利用路程記錄的小范圍短距離航位推算較為精確。但如果處于極端情況下，即在長時間接收不到正確的GPS定位信息，缺少定位基準點的情況下，方位推算將逐漸出現偏移。
電子羅盤主要用于指示航向，如改用慣性陀螺儀就可以避免外界磁干擾，增加系統強壯度，但要周期性修正陀螺儀偏移;如果使用光電相對位移傳感器來測量機器人與地面的相對移動，就可以大大提高航位估算的精度，并且避免由于驅動輪打滑等引起的計算誤差，但由于目前光電傳感器對使用環境要求較為苛刻，有待進一步的改進。
參考文獻：
【1】王廣運,郭秉義,李洪濤編. 差分GPS定位技術與應用 北京:電子工業出版社,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