[發(fā)明專利]一種新的排爆機器人隧進(jìn)定位方法

在審

• 申請?zhí)枺?/td>: 201710621214.4
• 申請日: 2017-07-27
• 公開（公告）號: CN107387064A
• 公開（公告）日: 2017-11-24
• 發(fā)明（設(shè)計）人: 蔡磊;雷進(jìn)輝;劉艷昌;羅培恩;白林峰;李國厚;趙明富;余周
• 申請（專利權(quán)）人: 河南科技學(xué)院
• 主分類號: E21B47/09
• 分類號: E21B47/09
• 代理公司: 鄭州優(yōu)盾知識產(chǎn)權(quán)代理有限公司41125
• 代理人: 孫詩雨,謝萍

技術(shù)領(lǐng)域

本發(fā)明屬于排爆機器人地下隧進(jìn)方位檢測的技術(shù)領(lǐng)域，具體設(shè)計一種新的排爆機器人隧進(jìn)定位方法。

背景技術(shù)

隧進(jìn)定位是確定殉爆系統(tǒng)在二維工作環(huán)境中相對于全局坐標(biāo)的位置及其本身的姿態(tài)，以便隨時掌握殉爆系統(tǒng)所處位置，防止意外情況出現(xiàn)時殉爆系統(tǒng)陷入侵徹彈道內(nèi)無法退出。

目前常用定位濾波方法有擴展卡爾曼濾波、無跡卡爾曼濾波和粒子濾波。其中擴展卡爾曼濾波利用泰勒級數(shù)實現(xiàn)非線性問題線性化處理，建立的數(shù)學(xué)模型不準(zhǔn)確，隨著誤差累積而產(chǎn)生野值，致使濾波器可靠性和收斂速度下降。無跡卡爾曼濾波適應(yīng)于非線性系統(tǒng)，但是初值選擇比較敏感，不適用于非高斯噪聲系統(tǒng)。粒子濾波解決了非線性非高斯問題，是基于隨機采樣的濾波方法，算法復(fù)雜響應(yīng)慢，容易出現(xiàn)嚴(yán)重的粒子退化現(xiàn)象。

發(fā)明內(nèi)容

本發(fā)明要解決的是在機場道面下混泥土復(fù)合介質(zhì)環(huán)境中，未爆彈殉爆系統(tǒng)的隧進(jìn)定位方法受到混凝土復(fù)合介質(zhì)的干擾不能正常工作，定位誤差大不可靠等技術(shù)問題，從而提供一種定位準(zhǔn)確、算法簡單、檢測精度和可靠性高的基于改進(jìn)卡爾曼濾波的隧進(jìn)定位方法。

為解決上述技術(shù)問題，本發(fā)明所采用的技術(shù)方案如下：一種新的排爆機器人隧進(jìn)定位方法，步驟如下：S1，采樣并初始化。

具體步驟為：

S1.1，采樣；根據(jù)初始先驗密度函數(shù)p(x₀)，從殉爆系統(tǒng)頭部坐標(biāo)的狀態(tài)向量中隨機抽取采樣樣本，獲得第0時刻的N個采樣粒子其中，i＝1,2,…,N。

產(chǎn)生初始化后的隨機樣本空間為隨著時間推移可以產(chǎn)生k時刻的樣本空間為

其中，表示0時刻的第i個樣本集合，即粒子集合。表示為0時刻第i個粒子的權(quán)重值。

S1.2，賦初值；令：

其中，表示0時刻第i個采樣粒子的均值；表示0時刻第i個隨機狀態(tài)向量采樣值；表示i時刻第i個粒子的協(xié)方差；表示0時刻第i個粒子擴展?fàn)顟B(tài)噪聲和量測噪聲后的增廣狀態(tài)向量的均值；表示0時刻第i個粒子擴展?fàn)顟B(tài)噪聲和量測噪聲后的增廣狀態(tài)向量；a表示狀態(tài)向量擴展；表示0時刻第i個粒子的增廣狀態(tài)向量的協(xié)方差；Q表示狀態(tài)噪聲的協(xié)方差；R表示量測噪聲的協(xié)方差。

S2.根據(jù)UKF(Unscented Kalman Filter，無跡卡爾曼濾波)算法更新粒子。

具體步驟為：

S2.1基于粒子通過UT變換方法計算2n_a個σ點構(gòu)造矩陣χ，矩陣χ內(nèi)的向量χ_i為σ點，計算公式為：

λ＝α²(n+κ)-n (7)；

其中，表示k-1時刻第i個采樣粒子的第j個σ點；表示k-1時刻第i個粒子擴展?fàn)顟B(tài)噪聲和量測噪聲后的增廣狀態(tài)向量的均值；表示k-1時刻第i個粒子的增廣狀態(tài)向量的協(xié)方差；表示矩陣平方根第j列；n_a表示σ點個數(shù)；λ表示尺度因子；α為常數(shù)，表示分布在周圍的散布程度，取值范圍為0.0001≤α≤1；κ為常數(shù)是一個輔助尺度因子，取值為0；表示第j個σ點計算一階統(tǒng)計特性時的權(quán)值；是第j個σ點計算二階統(tǒng)計特性時的權(quán)值。

S2.2，將步驟S2.1中的σ點通過系統(tǒng)模型和測量模型向前傳遞，傳遞方程為：

其中，表示k-1時刻第j個σ點一步狀態(tài)預(yù)測值；表示k-1時刻第i個采樣粒子的第j個σ點；表示非線性狀態(tài)傳遞函數(shù)；表示k-1時刻第i個粒子第j個σ點的一步預(yù)測測量值；表示非線性量測傳遞函數(shù)。

S2.3，根據(jù)步驟S2.2得到一步狀態(tài)預(yù)測值和協(xié)方差一步預(yù)測值

一步狀態(tài)預(yù)測值的計算公式為：

其中，n_a表示UT變換σ點個數(shù)；表示第j個σ點計算一階統(tǒng)計特性時的權(quán)值；表示k-1時刻第j個σ點狀態(tài)一步預(yù)測值。

協(xié)方差一步預(yù)測值的計算公式為：

其中，表示k-1時刻第i個粒子一步預(yù)測協(xié)方差；是第j個σ點計算二階統(tǒng)計特性時的權(quán)值；表示k-1時刻第i個粒子基于UKF算法計算的狀態(tài)一步預(yù)測均值。

S 2.4，根據(jù)步驟S2.2計算一步預(yù)測測量值的均值

均值的計算公式為：

其中，表示k-1時刻第i個粒子一步預(yù)測測量值的均值。

S2.5，通過傳感器直接獲取當(dāng)時新的測量值z_k，并計算新的狀態(tài)預(yù)測值

計算公式為：