于 飛，李元朋，李志恒，羅其俊，高慶吉

（1.中國民航大學(xué)機(jī)器人研究所，天津 300300；2.東北電力大學(xué)自動化系，吉林 吉林 132022）

基于置信度的自適應(yīng)Kalman濾波定位方法

于 飛1，李元朋2，李志恒2，羅其俊1，高慶吉1

（1.中國民航大學(xué)機(jī)器人研究所，天津 300300；2.東北電力大學(xué)自動化系，吉林 吉林 132022）

針對室外自主移動機(jī)器人因感知信息缺失或異常波動造成的定位失敗或偏差過大的問題，設(shè)計了一種基于置信度的自適應(yīng)Kalman濾波定位方法。該方法根據(jù)置信距離和置信度函數(shù)計算局部濾波器的置信度，并將其進(jìn)行加權(quán)運算后作為全局濾波器的自適應(yīng)因子，以得到更為準(zhǔn)確的位姿估計。采用D-S證據(jù)理論對全局位姿進(jìn)行評價，并給出單一傳感器失效時的組合方案。試驗結(jié)果驗證了方法的有效性和魯棒性，能滿足實際定位的需要。

組合定位；置信度函數(shù)；自適應(yīng)濾波；D-S證據(jù)理論

定位問題是移動機(jī)器人研究中的關(guān)鍵問題[1]。由航跡推算（DR，dead reckoning）和全球定位系統(tǒng)[2]（GPS，global positioning system）組合實現(xiàn)機(jī)器人定位是近年來研究較多的一種方法[3-4]。但在定位過程中，存在很多不確定性因素[5]，如DR系統(tǒng)中的里程計易受路面濕滑及粗糙度影響，數(shù)字羅盤容易受到周圍磁場以及載體顛簸的干擾，而GPS在信號受到建筑物遮擋發(fā)生多路徑效應(yīng)時定位精度會下降，甚至完全失效。怎樣有效融合傳感器信息在保證定位精度的前提下，抑制或消除傳感器失效及環(huán)境干擾等因素的影響是組合定位系統(tǒng)實際應(yīng)用中的關(guān)鍵問題。

Kalman濾波是常用的多傳感器信息融合方法，但該方法要求建立準(zhǔn)確的數(shù)學(xué)模型，而且沒有考慮誤差模型的變化，因此不能直接應(yīng)用于DR/GPS組合系統(tǒng)。文獻(xiàn)[6]針對系統(tǒng)非線性問題提出了基于U變換的卡爾曼濾波算法（UKF，unscented Kalman filter）。文獻(xiàn)[3]采用UKF和擴(kuò)展Kalman濾波結(jié)合的非線性濾波算法，既解決了觀測方程的非線性問題又減小了擴(kuò)展Kalman濾波引入的線性化誤差。文獻(xiàn)[7]利用DR估計速度和加速度，再與GPS觀測結(jié)合采用聯(lián)合濾波方法確定最終的定位結(jié)果。而文獻(xiàn)[8-9]針對Kalman濾波中因擾動異常對狀態(tài)估計的影響提出了自適應(yīng)Kalman濾波，減小有較大擾動的局部濾波器的狀態(tài)估計在全局狀態(tài)估計中的權(quán)值，實現(xiàn)組合定位中的自適應(yīng)調(diào)節(jié)。這些算法在一定程度上提高了融合結(jié)果的精度，但系統(tǒng)的魯棒性有待進(jìn)一步研究，而且在自適應(yīng)因子的選擇上，需要基于一定的先驗知識。

為此，擬研究置信距離和置信度函數(shù)來估計傳感器數(shù)據(jù)間的一致性；而后以局部濾波器的置信度為依據(jù)計算自適應(yīng)因子，進(jìn)而參與全局濾波；最后引入DS證據(jù)理論計算全局位姿的置信度，為導(dǎo)航控制提供有效的決策依據(jù)。

1 組合定位系統(tǒng)模型

設(shè)計的組合定位系統(tǒng)包括2個局部濾波器LF1和LF2及1個全局濾波器，如圖1所示。2個局部濾波器分別處理來自DR和GPS系統(tǒng)的信息，二者的狀態(tài)估計再送到全局濾波器。全局濾波器通過自適應(yīng)因子分配策略調(diào)整DR和GPS狀態(tài)估計的權(quán)重，得到全局的最優(yōu)估計。

圖1 組合系統(tǒng)整體模型Fig.1 Whole model of combined system

實際定位系統(tǒng)中，感知信息缺失或發(fā)生異常波動的情況是存在的，為了保障整體系統(tǒng)的魯棒性，對各局部濾波器的輸出進(jìn)行分析是必要的。如果子系統(tǒng)的某個傳感器失效，其他傳感器可以彌補時，能夠保證短時間內(nèi)的定位精度，但對應(yīng)的局部濾波器權(quán)值會下降，如羅盤數(shù)據(jù)缺失時，可以用里程計推算角度信息。而當(dāng)某一局部濾波器輸出信息完全缺失或局部濾波器的置信度小于設(shè)定的閾值時，全局狀態(tài)估計只能依靠其他的局部濾波器得到，如GPS信息完全失效時，全局濾波器只能通過局部濾波器1的狀態(tài)估計得到全局狀態(tài)的估計值。

2 局部濾波器的設(shè)計

建立機(jī)器人全局坐標(biāo)系XOY，以環(huán)境的東向為x軸正方向、北向為y軸正方向。假設(shè)機(jī)器人在Δt時段內(nèi)勻速運動，k 時刻的狀態(tài)向量 X（k）=[x（k），vx(k)，ax(k)，y(k)，vy(k)，ay(k)]T可簡化為

其中（x（k），y（k））表示 k 時刻機(jī)器人的位置，vx(k)、vy(k)分別表示在x軸、y軸方向上的速度分量。狀態(tài)方程如式（1）所示

其中，A為狀態(tài)轉(zhuǎn)移矩陣；W（k-1）為系統(tǒng)過程的噪聲向量。

其中，T為采樣周期。W（k-1）的協(xié)方差陣為

將里程計采樣周期內(nèi)輸出的距離S和數(shù)字羅盤輸出的角度θ作為觀測量，系統(tǒng)的觀測方程為

其中觀測噪聲V（k）的協(xié)方差陣為

采用擴(kuò)展Kalman濾波進(jìn)行線性化，將h[X（k）]在預(yù)測值（k）附近展開成泰勒級數(shù)，忽略二次以上的高次項得

局部濾波器1的求算方法如下：

計算預(yù)測值

計算先驗誤差協(xié)方差

計算增益矩陣

更新估計

更新誤差協(xié)方差

基于GPS系統(tǒng)的局部濾波器2的設(shè)計過程與局部濾波器1的相似，求算方法如下

3 全局濾波器設(shè)計及其置信度計算

3.1 全局濾波器的設(shè)計

為研究傳感器數(shù)據(jù)間的一致性，在此引入置信距離的概念，以描述不確定性問題所引起的測量數(shù)據(jù)與無偏估計的一致性程度。k時刻某傳感器的置信距離Δ（k）的定義如下

每一個局部濾波器的輸入來自一個或幾個傳感器，因此，局部濾波器i的置信度函數(shù)可以由傳感器的置信度函數(shù)得到。定義里程計、數(shù)字羅盤和GPS的置信距離分別為Δ1（k）、Δ2（k）、Δ3（k），對應(yīng)的置信度函數(shù)m1（k）、m2（k）、m3（k）定義如下

在DR系統(tǒng)中，由里程計得到的距離信息和數(shù)字羅盤得到的角度信息不同，其對局部濾波器1位姿的支持程度也不相同，可以引入不同的加權(quán)因子。局部濾波器1、2的置信度β1（k）、β2（k）如下

其中，c1、c2根據(jù)里程計和羅盤本身的精度及其經(jīng)驗確定且c1+c2=1。全局的自適應(yīng)因子為

則全局濾波器的實現(xiàn)過程如下

由以上分析可知通過傳感器的置信度函數(shù)實時調(diào)整各局部濾波器的置信度，而對置信度加權(quán)得到的自適應(yīng)因子實現(xiàn)了多傳感器信息的有效融合。當(dāng)某一局部濾波器結(jié)果不好時，系統(tǒng)具有一定的容錯能力，并最終影響到全局濾波器的整體性能，此時的全局濾波就具備自適應(yīng)的能力。

3.2 全局位姿置信度的計算

組合定位方法可以得到最終的位姿，但以往的研究對其可靠程度卻很少討論。位姿的可靠程度是機(jī)器人進(jìn)行控制決策的前提和安全運行的保障。為此提出以全局位姿置信度表示最終位姿估計的可靠程度。全局位姿置信度可以采用多種方法計算，其中D-S證據(jù)理論[10-11]為處理不確定、不精確、不完善信息提供了有效途徑，在信息融合、目標(biāo)檢測、故障診斷等領(lǐng)域得到了廣泛的應(yīng)用。

根據(jù)D-S證據(jù)理論構(gòu)建辨識框架X為{A，B}，A表示全局位姿可信，B表示全局位姿不可信。局部濾波器1、2構(gòu)成2個相互獨立的證據(jù)m1、m2。局部濾波器的輸出位姿對全局濾波器的輸出位姿有不同的支持，而每個局部濾波器輸出的位姿又具有各自的置信度，如3.1中所述，由不確定性的傳遞算法得到基本概率賦值函數(shù)計算公式如下

其中，（x1，y1）、（x2，y2）、（x，y）分別表示局部濾波器 1、2和全局濾波器輸出的位置估計。由正交和規(guī)則可得合成后的綜合概率并且由D－S證據(jù)推理得到置信度函數(shù)

D－S證據(jù)理論雖然是一種有效的不確定性推理方法，但當(dāng)某一局部濾波器因數(shù)據(jù)缺失時，D－S證據(jù)理論合成公式會失效。為了避免上述情況出現(xiàn)，采取的措施是：某一局部濾波器在k時刻數(shù)據(jù)缺失，全局位姿由其他局部濾波器的結(jié)果得到，經(jīng)過Δt時段后全局位姿的置信度如下

其中：f（A）(k)表示 k 時刻全局位姿的置信度；β1(k+Δt)表示 k+ Δt時刻時局部濾波器的置信度；e－rΔt決定 Δt時間段內(nèi)置信度的變化。

4 實驗及結(jié)果分析

為了驗證濾波方法的有效性，以國內(nèi)首款保安巡邏機(jī)器人為實驗平臺，在校園內(nèi)進(jìn)行多次實驗選取其中的兩組。實驗中，路寬為6 m，設(shè)定最大誤差為3 m，并且當(dāng)全局位姿置置信度連續(xù)低于設(shè)定閾值的時間超限時，機(jī)器人停止運動。試驗中，基于DR系統(tǒng)的局部濾波器1的主要參數(shù)選擇為==0.05==0.01=1.2=0.26；基于GPS系統(tǒng)的局部濾波器2的主要參數(shù)為：==0.08，==0.02=3.6=0.35；采樣周期 T=1 s。

在第1組實驗中，從研究所門前出發(fā)沿逆時針方向運動，為405 m矩形軌跡，共采集819 s數(shù)據(jù)，機(jī)器人速度為0.5 m/s。運動過程中最大偏差1.40 m。運動軌跡如圖2（b）所示。第52 s、53 s因GPS發(fā)生較大誤差全局置信度分別為0.23、0.4；第66 s、331 s因羅盤數(shù)據(jù)發(fā)生較大誤差全局置信度分別為0.39、0.4，但全局濾波后的結(jié)果與期望軌跡接近，沒有大的波動，具有較長時間內(nèi)的自適應(yīng)能力，如圖2（c）所示。

從所采集的數(shù)據(jù)中分析第40～190 s數(shù)據(jù)，比較各濾波器的誤差，如圖2（d）所示。從表1中可以看出全局位姿誤差的均值和均方差小于各局部位姿誤差的均值和均方差，這表明全局濾波器的精度要高于局部濾波器。

表1 各濾波器誤差比較Tab.1 Errors comparison of various filter （單位：m）

在同樣的環(huán)境進(jìn)行的第二組實驗中如圖3（a）所示，首先在62～96 s期間人為停止羅盤數(shù)據(jù)采集，姿態(tài)角由里程計推算，最大誤差為2.74 m，最小置信度0.51，平均置信度下降0.08；在580～616 s期間GPS數(shù)據(jù)丟失，此時全局位姿由局部濾波器1得到，最大誤差為2.36 m，最小置信度0.47，平均置信度下降0.13，如圖3（b）所示。可以看出當(dāng)一種傳感器信息異常時，全局濾波器精度和置信度都會下降。但從整體來看，全局濾波器可以增強傳感器信息的互補性和對信息波動的適應(yīng)性。

5 結(jié)語

針對室外自主移動機(jī)器人因感知信息缺失或異常波動造成的定位失敗或偏差過大的問題，定義置信距離和置信度函數(shù)表示傳感器數(shù)據(jù)間的一致性，提出以局部濾波器輸出結(jié)果的置信度分配全局Kalman濾波器的自適應(yīng)因子，引入D－S證據(jù)理論對全局位姿進(jìn)行評價。試驗結(jié)果表明，該方法具有一定的容錯能力和較強的魯棒性，能夠滿足室外移動機(jī)器人實際需要，為自主導(dǎo)航提供了保障。

[1] 厲茂海，洪炳熔.移動機(jī)器人的概率定位方法研究進(jìn)展[J].機(jī)器人，2005，27（4）：380－384.

[2]REINA G，VARGAS A，NAGATANI K，et al.Adaptive Kalman Filtering for GPS－Based Mobile Robot Localization[C]//Safety，Security and Rescue Robotics，2007.SSRR 2007.IEEE International Workshop on 27－29 September，2007：1－6.

[3] 劉 旭，張其善，楊東凱.一種用于GPS/DR組合定位的非線性濾波算法[J].北京航空航天大學(xué)學(xué)報，2007，33（2）：184－187.

[4]HAJIYEV C，TUTUCU M A.Development of GPS Aided INS Via Federated Kalman Filter[C]//Recent Advances in Space Technoloqies，2003.RAST′03.International Conference on Proceedings of 20－22 November，2003：569－574.

[5] THRUN S，F(xiàn)OX D，BURGARD W.Probabilistic algorithms and the interactive museum tour－guide robot Minerva[J].The International Journal of Robotics Research，2000，19（11）：972－999.

[6]WAN E A，MERWE R.The Square－Root Unscented Kalman Filter for State and Parameter Estimation[C]//Proceedings（ICASSP ′01）2001 IEEE International Conference，IEEE Press，2001：3461－3464.

[7] 柴艷菊，歐吉坤.GPS/DR組合導(dǎo)航中一種新的數(shù)據(jù)融合算法[J].武漢大學(xué)學(xué)報（信息科學(xué)版），2005，30（12）：1048－1051.

[8] YANG YUAN－XI，GAO WEI－GUANG.A new learning statistic for adaptive filter based on predicted residuals[J].Progressin Natural Science，2006，16（5）：833－837.

[9]CUI XIAN－QIANG，YANG YUAN－XI.Adaptively robust filtering with classified adaptive factors[J].Progress in Natural Science，2006，16（8）：846－841.

[10] DEMPSTER A P.Upper and lower probabilities induced by a multivalued mapping[J].Annals of Math Stat，1967，38：325－339．

[11] SHAFER G．A Mathematical Theory of Evidence[M].Princeton，NJ：Princeton Univ Press，1976.

Self-Adaptive Kalman Filtering Algorithm Based on Confidence Level

YU Fei1，LI Yuan-peng2，LI Zhi-heng2，LUO Qi-jun1，GAO Qing-ji1
（ 1.Robot Institute，CAUC，Tianjin300300，China；2.Department of Automation，Northeast Dianli University，Jilin132022，China）

The problem of positioning failure or excessive deviation of outdoor autonomous mobile robot often appears due to missed perceptional information or abnormal fluctuations.A self-adaptive federated Kalman filtering method based on confidence level was given in this paper.This method firstly defines confidence distance and the confidence function of the sensor data consistency，according to which confidence level of local filter can be calculated.Finally，we carry out weighted computing for confidence level which can distribute an appropriate adaptive factor for global filter，therefore，a more accurate pose estimation is obtained.In order to ensure the safely operation of robot，D-S theory is introduced to evaluate the global pose and give some kind of portfolio programs when a sensor is failure.Experimental results prove that this method is effective and robust and it also can meet the needs of the actual location.

integrated localization；confidence function；self-adaptive filtering；D-S theory of evidence

TP242.6

A

1674－5590（2010）02－0042－05

2009-10-12；

2009-11-18

中國民用航空局科技基金項目（MHRD0702）；中國民航大學(xué)科技基金項目（08CAUC-E07）

于 飛（1981—），男，河北黃驊人，助理研究員，學(xué)士，研究方向為機(jī)器人導(dǎo)航定位.

（責(zé)任編輯：楊媛媛）