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

□姜勇

北京礦冶研究總院 北京 100160

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

□姜勇

北京礦冶研究總院 北京 100160

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 結(jié)論

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

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

［1］盛三元，王建華.基于多傳感器信息融合技術(shù)的聯(lián)合卡爾曼濾波器的設(shè)計(jì)及應(yīng)用［J］.情報(bào)指揮控制系統(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é)報(bào)（自然科學(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ā)展計(jì)劃項(xiàng)目（編號：2011AA060403）

2015年4月