消防機器人GPS導航系統的精度提高方案

機器人運行時的震動和外界的強磁干擾會影響電子羅盤的精度。震動對其影響約 ，可以在CPU端使用軟件濾波器來解決這個問題[2];當外界的強磁場遠遠超過地球磁場時，甚至可能會使電子羅盤完全失效。本系統利用軟件里程計來計算航向，并可用于判定電子羅盤是否工作正常。

里程計與航位推算

　　消防機器人左右輪有各自的驅動電機和傳動系統，控制兩個電機運行不同的時間以實現不同角度轉彎。所以也可以利用左右里程計的數值差來計算機器人車體的相對轉角?？梢允褂霉獯a盤對驅動輪的轉動次數計數，然后再用先驗公式計算里程，精確性較好。還可以在CPU內部開辟寄存器空間，記錄所有電機運行命令中的時間參數，以計算左右輪的里程，作為預期參考值，但是驅動電機的誤差會影響精確性。

　　假設機器人兩個驅動輪間隔L，驅動輪半徑r，驅動電機每秒鐘可驅動輪子轉n圈;則若要向左旋轉Δθ度，只要控制右輪比左輪多運行 秒。由于機器人硬件參數相對固定，因而轉過一個固定角度的時間參數也可以先驗獲取，經過多次試驗可以獲得比較精確的數值。為簡化計算復雜度，機器人的前進和轉向是作為兩種運動方式來處理的，即機器人只會直行，轉向時產生的位移由先驗參數進行補償。

　　由于消防機器人主要在地面工作，因此定位時也暫不考慮水平高度的變化。系統二維定位坐標中，以正北方向為Y軸正方向，航向角度為機器人前進方向順時針偏離正北方向的角度。如圖二所示，機器人先直行ΔS，然后向右轉這里認為機器人直行時軌跡為理想直線，轉向時的軌跡為理想圓弧，實際使用時需添加修正因子。

　　直行的相對方位變化可由以下公式得到：

（式中ΔS提取命令中時間參數計算得來，相對方位角由航向記錄修正電子羅盤數據提供;γ和λ為修正因子）

　　根據機器人實際航行情況，測得轉彎半徑為R，則轉向位移為：

（式中順時針轉向時n=1，逆時針轉向時n=0;φ和ψ為修正因子）

　　由于驅動輪地面摩擦情況可能發(fā)生變化，電機驅動誤差等因素，實際運行中的軌跡并非理想狀態(tài)，可利用先驗誤差因子作修正;并且在GPS和電子羅盤工作正常的情況下，周期性校驗方位及航向記錄，以避免誤差的疊加。而在外界干擾嚴重（建筑物遮擋，無法收到GPS信號;環(huán)境磁場擾亂地磁場）的情況下，就要使用可靠的預測算法來提取有用信息。

改進自適應卡爾曼濾波與信息綜合

　　在誤差干擾下提取機器人的正確位置信息，需要使用到卡爾曼濾波器[3]。這是一種遞推線性最小方差估計，廣泛應用于信息提取，信息融合，追蹤、導航等方面。它基于以下兩個前提：首先，系統狀態(tài)可以由以下線性方程定義：

　　狀態(tài)方程：;（wk為過程誤差）

輸出（測量）方程：（zk為測量誤差）

　　其次，過程誤差與測量誤差分布滿足零均高斯，且不相關;需要先驗的誤差分布參數?；緸V波公式如下：

K是卡爾曼增益，P是預計方差矩陣， 是過程誤差矩陣期望值, 是測量誤差矩陣期望值。

　　卡爾曼濾波器的性能與誤差分布的先驗參數密切相關，因此在實際應用中存在諸多問題。首先系統的狀態(tài)方程可能是非線性的，線性擬合方程隨時間推移會引入較大的誤差;其次外界干擾產生的誤差特性是未知的，錯誤的先驗信息會導致濾波結果與實際被離。自適應卡爾曼濾波器[4]使用滑動窗法及最大似然準則，利用實際測量值動態(tài)改變?yōu)V波器參數（測量方差矩陣和系統方差矩陣），其根本思想在于找出與預期誤差最小的結果，并給予最大的權重。