基于聯(lián)合卡爾曼濾波算法的智能鏟運機組合定位導(dǎo)航分析*

□姜勇

北京礦冶研究總院 北京 100160

基于聯(lián)合卡爾曼濾波算法的智能鏟運機組合定位導(dǎo)航分析*

□姜勇

北京礦冶研究總院 北京 100160

以智能鏟運機為研究對象，將航跡推算系統(tǒng)和激光測距識別系統(tǒng)數(shù)據(jù)同時作用于定位解的求解過程中，提出了一種基于聯(lián)合卡爾曼濾波技術(shù)的智能鏟運機組合定位導(dǎo)航分析方法，并進行仿真分析。結(jié)果表明，該算法可以有效地減小組合系統(tǒng)的定位誤差，提高系統(tǒng)的定位精度。

智能鏟運機 卡爾曼濾波 組合定位導(dǎo)航

卡爾曼濾波技術(shù)是對隨機信號作估計的算法之一，它在濾波技術(shù)中引入現(xiàn)代控制理論中狀態(tài)空間的概念，系統(tǒng)用狀態(tài)方程來描述，采用遞推計算的方法，不需要了解過去時刻的測量值，只需根據(jù)前一時刻的估計值和當(dāng)前時刻的測量值，即可遞推計算出當(dāng)前時刻的估計值，并且可以在一定的條件下給出無偏估計量，能夠很好解決多維非平穩(wěn)隨機過程的估計問題［1，2,3］。筆者應(yīng)用聯(lián)合卡爾曼濾波技術(shù)對智能鏟運機的組合定位導(dǎo)航算法進行研究，將航跡推算系統(tǒng)和激光測距識別系統(tǒng)數(shù)據(jù)同時作用于定位解的求解過程中，根據(jù)兩個系統(tǒng)的數(shù)據(jù)質(zhì)量，自適應(yīng)調(diào)節(jié)兩個系統(tǒng)在定位解中的作用，實現(xiàn)了航跡推算和激光測距識別組合定位導(dǎo)航系統(tǒng)定位信息的最優(yōu)融合。

1 聯(lián)合卡爾曼濾波算法

聯(lián)合卡爾曼濾波器是一種特殊的分散卡爾曼濾波器，與分散卡爾曼濾波器不同的是，聯(lián)合卡爾曼濾波器包含一個信息分配過程，在此過程中，基于理論上的信息分配原理，主濾波器中的動態(tài)信息被分配到每一個局部濾波器中［4］。

1.1 各濾波器估計不相關(guān)條件下的聯(lián)合卡爾曼濾波算法

假設(shè)各子濾波器的估計獨立，對兩個局部濾波器（N=2）設(shè)局部估計狀態(tài)為，相應(yīng)的估計誤差方差為P11和P22?？紤]融合后的全局狀態(tài)估計為局部狀態(tài)估計的線性組合，即：

式中：W1、W2為待定加權(quán)矩陣。

式中：X為真實狀態(tài)。

由以上假設(shè)條件可以推出：

利用數(shù)學(xué)歸納法，將以上結(jié)果推廣到N個局部狀態(tài)估計，得：

1.2 各濾波器估計相關(guān)條件下的聯(lián)合卡爾曼濾波算法

在一般情況下各濾波器是相關(guān)的?？梢圆捎梅讲钌辖缂夹g(shù)對濾波過程進行適當(dāng)改造，使局部濾波估計實際上不相關(guān)。設(shè)各子濾波器的狀態(tài)估計表示如下：

假設(shè)將系統(tǒng)過程噪聲總的信息量分配到各個局部濾波器和主濾波器中，即:

根據(jù)信息守恒原理，由上式可知：

狀態(tài)估計初始信息P0-1也可按照上述方法分配。假設(shè)狀態(tài)估計的信息可同樣分配，即：

2 基于聯(lián)合卡爾曼濾波的組合定位導(dǎo)航算法

智能鏟運機在地下巷道內(nèi)完成自主導(dǎo)航行走，主要靠檢測巷道壁的環(huán)境信息并結(jié)合巷道電子地圖進行匹配，系統(tǒng)通過多傳感器的信息融合及聯(lián)合卡爾曼濾波算法對鏟運機的狀態(tài)進行估計，并對鏟運機的行駛進行指導(dǎo)。圖1所示為智能鏟運機信息融合定位算法圖。

航跡推算/激光測距識別組合定位導(dǎo)航的聯(lián)合卡爾曼濾波器結(jié)構(gòu)設(shè)計如圖2所示，組合系統(tǒng)由航跡推算子系統(tǒng)、基于激光測距識別子系統(tǒng)和主濾波器三部分組成，兩個子系統(tǒng)各有一個局部卡爾曼濾波器，用來處理航跡推算系統(tǒng)和激光測距/識別系統(tǒng)的檢測數(shù)據(jù)。子系統(tǒng)的局部狀態(tài)估計Xd、Xj和估計誤差協(xié)方差矩陣Pd、Pj輸入到主濾波器，主濾波器對局部濾波器的信息進行分析融合，并輸出整個系統(tǒng)的最優(yōu)估計Xg、Pg，同時根據(jù)信息分配因子λ1、λ2對局部濾波器進行反饋重置，作為下一時刻迭代運算的初始值，從而提高了局部濾波器狀態(tài)估計的精度。

▲圖1 信息融合定位算法框圖

▲圖2 航跡推算/激光測距識別組合定位導(dǎo)航的聯(lián)合卡爾曼濾波器結(jié)構(gòu)

3 組合定位導(dǎo)航算法仿真分析

為了驗證航跡推算和激光測距/路標(biāo)識別組合定位導(dǎo)航算法，采用計算機仿真的方法分別仿真車輛的真實行駛路線數(shù)據(jù)，航跡推算系統(tǒng)定位數(shù)據(jù)和激光測距/識別系統(tǒng)定位數(shù)據(jù)，然后利用這些仿真數(shù)據(jù)來對組合定位導(dǎo)航算法的性能進行分析［5～7］。圖3和圖4分別給出了航跡推算定位系統(tǒng)和激光測距識別定位系統(tǒng)單獨定位的鏟運機橫向位移誤差曲線，圖5是由組合定位系統(tǒng)定位的鏟運機橫向位移誤差曲線。

為了進一步縮小組合導(dǎo)航系統(tǒng)的定位誤差，提高定位精度，使鏟運機的控制更加準(zhǔn)確，行駛路線更加合理，不出現(xiàn)前面實驗中出現(xiàn)的非正常的轉(zhuǎn)向振動。筆者

對組合導(dǎo)航系統(tǒng)采用聯(lián)合卡爾曼濾波算法進行了系統(tǒng)仿真。圖6是智能鏟運機經(jīng)過聯(lián)合卡爾曼濾波后系統(tǒng)的位置估計圖。

▲圖3 航跡推算橫向定位誤差

▲圖4 激光測距識別橫向定位誤差

▲圖5 組合定位橫向定位誤差

▲圖6 聯(lián)合卡爾曼濾波后智能鏟運機的橫向位置估計

4 結(jié)論

（1）單獨的航跡推算定位或者激光測距識別定位的定位誤差都相對較大，通過對航跡推算和激光測距識別定位系統(tǒng)進行組合定位后，其定位誤差明顯減小，誤差波動更加平滑，定位精度得到了顯著提高。

（2）卡爾曼濾波器能夠有效地在比較大的干擾下對真實值做出很好的估計，即使觀測端加入干擾較大時（如巷道壁出現(xiàn)很不規(guī)則的情況），卡爾曼濾波器依然可以比較有效地進行濾波，并對鏟運機的狀態(tài)做出比較準(zhǔn)確的估計。采用聯(lián)合卡爾曼濾波算法可以有效地減小組合系統(tǒng)的定位誤差，提高系統(tǒng)的定位精度。

［1］盛三元，王建華.基于多傳感器信息融合技術(shù)的聯(lián)合卡爾曼濾波器的設(shè)計及應(yīng)用［J］.情報指揮控制系統(tǒng)與仿真技術(shù)，2002（2）：53-58.

［2］付夢印，鄧志紅，閆莉萍.Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用（第二版）［M］.北京：科學(xué)出版社，2010.

［3］鄧自立.最優(yōu)濾波理論及其應(yīng)用［M］.哈爾濱：哈爾濱工業(yè)大學(xué)出版社，2000.

［4］陳則王，袁信.聯(lián)合卡爾曼濾波在車輛組合導(dǎo)航系統(tǒng)中的應(yīng)用［J］.重慶大學(xué)學(xué)報（自然科學(xué)版），2005，28（10）：86-90.

［5］張葛祥，李娜.MATLAB仿真技術(shù)與應(yīng)用［M］.北京：清華大學(xué)出版社，2003.

［6］Joseph Nsasi Bakambu，Vladimir Polotski.Autonomous System for Navigation and Surveying in Underground Mines［J］.Journal of Field Robotics,2007,24（10）：829-847.

［7］H Martinez Barbera,D Herrero Perez.Autonomous Navigation of an Automated Guided Vehicle in Industrial Environments［J］.Robotics and Computer-Integrated Manufacturing，2010,26（4）:296-311.

（編輯 小 前）

TH123

A

1000-4998（2015）10-0036-03

*國家863高技術(shù)研究發(fā)展計劃項目（編號：2011AA060403）

2015年4月