消防機(jī)器人GPS導(dǎo)航系統(tǒng)的精度提高方案

機(jī)器人運(yùn)行時(shí)的震動(dòng)和外界的強(qiáng)磁干擾會(huì)影響電子羅盤的精度。震動(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ì)使電子羅盤完全失效。本系統(tǒng)利用軟件里程計(jì)來(lái)計(jì)算航向，并可用于判定電子羅盤是否工作正常。

里程計(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ī)器人車體的相對(duì)轉(zhuǎn)角?？梢允褂霉獯a盤對(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ì)方位變化可由以下公式得到：

（式中ΔS提取命令中時(shí)間參數(shù)計(jì)算得來(lái)，相對(duì)方位角由航向記錄修正電子羅盤數(shù)據(jù)提供;γ和λ為修正因子）

　　根據(jù)機(jī)器人實(shí)際航行情況，測(cè)得轉(zhuǎn)彎半徑為R，則轉(zhuǎn)向位移為：

（式中順時(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和電子羅盤工作正常的情況下，周期性校驗(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)方程：;（wk為過(guò)程誤差）

輸出（測(cè)量）方程：（zk為測(cè)量誤差）

　　其次，過(guò)程誤差與測(cè)量誤差分布滿足零均高斯，且不相關(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)重。