[發明專利]一種基于組合導航技術的超深礦井罐籠位姿測量系統及方法

有效

• 申請號: 201510419527.2
• 申請日: 2015-07-16
• 公開（公告）號: CN105173940B
• 公開（公告）日: 2017-06-09
• 發明（設計）人: 譚建平;林波;劉淑奇;薛少華;吳志鵬
• 申請（專利權）人: 中南大學
• 主分類號: B66B5/00
• 分類號: B66B5/00;B66B3/02
• 代理公司: 長沙市融智專利事務所43114
• 代理人: 黃美成

1.一種基于組合導航技術的超深礦井罐籠位姿測量系統，其特征在于，包括捷聯慣性導航裝置、地面處理裝置、旋轉編碼器、接近開關和感應通信電纜；

所述捷聯慣性導航裝置安裝在罐籠頂部；

所述旋轉編碼器安裝在提升機卷筒的卷軸上；

所述感應通信電纜鋪設于礦井側壁；

所述接近開關設置在井筒壁上；

所述旋轉編碼器和接近開關與安裝在礦井地表面的地面處理裝置進行通信連接，所述地面處理裝置通過感應通信電纜與捷聯慣性導航裝置進行通信連接；

以捷聯慣性導航裝置、接近開關和旋轉編碼器構建成組合導航式位姿測量系統。

2.根據權利要求1所述的一種基于組合導航技術的超深礦井罐籠位姿測量系統，其特征在于，所述捷聯慣性導航裝置包括慣性單元、數據采集及處理模塊、導航解算及信息融合模塊、數據存儲模塊及通信模塊；

所述慣性單元通過SPI總線與數據采集及處理模塊相連，所述數據采集及處理模塊與導航解算及信息融合模塊相連，所述數據采集及處理模塊與導航解算及信息融合模塊均與所述數據存儲模塊相連，所述數據存儲模塊與通信模塊相連。

3.根據權利要求2所述的一種基于組合導航技術的超深礦井罐籠位姿測量系統，其特征在于，所述數據采集及處理模塊與導航解算及信息融合模塊均采用TI公司的TMS320系列C6748浮點型DSP處理器。

4.一種基于組合導航技術的超深礦井罐籠位姿測量方法，其特征在于，將數據采集與處理和導航解算與信息融合分配給兩個不同的處理器處理；

所述數據采集與處理的過程如下：

首先讀取捷聯慣性導航裝置中的慣性單元的數據，根據慣性單元偏置溫度系數進行線性溫度誤差補償，然后讀取地面處理裝置數據，最后存儲數據，上傳數據至上位機；

所述導航解算與信息融合的過程如下：

通過將捷聯慣性導航裝置安裝在罐籠頂部，利用捷聯慣性導航裝置的慣性單元測量出罐籠運動加速度和角速度，采用四元數法建立罐籠姿態矩陣，通過SINS解算出罐籠位置和姿態信息；通過地面處理裝置采集井筒壁上的接近開關和提升機卷筒軸上的旋轉編碼器測量罐籠的位置和速度，采用聯合卡爾曼濾波算法將捷聯慣性導航裝置的速度和位置信息與罐籠位置與速度進行信息融合，反饋校正捷聯慣性導航裝置，實現罐籠位置和姿態的精確測量。

5.根據權利要求4所述的一種基于組合導航技術的超深礦井罐籠位姿測量方法，其特征在于，所述信息融合的步驟如下：

步驟1：建立捷聯慣性導航裝置誤差模型，包括三軸陀螺儀誤差模型、加速度測量誤差模型、位置誤差模型及姿態角誤差模型；

步驟2：以步驟1中的捷聯慣性導航裝置誤差模型建立離散化的組合導航式位姿測量系統的狀態方程；

所述組合導航式位姿測量系統是指由捷聯慣性導航裝置、接近開關和旋轉編碼器組成；

步驟3：構建信息融合卡爾曼濾波器，并依據組合導航式位姿測量系統的狀態向量建立信息融合卡爾曼濾波器中各子濾波器的觀測方程，求得各子濾波器的狀態向量估計值；

所述信息融合卡爾曼濾波器包括第一子濾波器、第二子濾波器、第三子濾波器以及主濾波器，所述第一子濾波器、第二子濾波器及第三子濾波器均與主濾波器相連；

所述第一子濾波器、第二子濾波器、第三子濾波器以及主濾波器均為卡爾曼濾波器；

將捷聯慣性導航裝置的速度與編碼器速度之差作為第一子濾波器的輸入信號；

將捷聯慣性導航裝置的位置信號與接近開關位置信號之差作為第二子濾波器的輸入信號；

將捷聯慣性導航裝置的東、北方向的零速度和零位置信號作為約束條件，構造成虛擬信號作為第三子濾波器的輸入信號；

第一子濾波器的觀測方程為：Z₁(k)＝H₁(k)X₁(k)+v₁(k)；

第一子濾波器觀測向量Z₁(k)＝[v_u-v_b]，v_b為地面處理裝置基于旋轉編碼器計算得到的罐籠速度，v₁(k)為第一子濾波器觀測噪聲，是均值為零的高斯白噪聲序列，由高斯函數可得，X₁(k)為第一子濾波器的狀態向量，第一子濾波器觀測矩陣H₁(k)＝[0_1×8 1 0_1×6]_1×15；

第二子濾波器的觀測方程為：Z₂(k)＝H₂(k)X₂(k)+v₂(k)；

第二子濾波器觀測向量Z₂(k)＝[H-H_j]，H為捷聯慣性導航裝置解算出的罐籠垂直位置，H_j為接近開關位置信號，v₂(k)為第二子濾波器位置觀測噪聲，是均值為零的高斯白噪聲序列；X₂(k)為第二子濾波器的狀態向量，H₂(k)＝[0_1×5 1 0_1×9]_1×15為第二子濾波器位置觀測矩陣；

第三子濾波器觀測方程為：Z₃(k)＝H₃(k)X₃(k)+v₃(k)；

第三子濾波器觀測向量Z₃(k)＝[L E v_e v_n]^T，L和E為捷聯慣性導航裝置東向和西向位置，v_e和v_n為捷聯慣性導航裝置東向和西向速度，v₃(k)為第三子濾波器觀測噪聲，是均值為零的高斯白噪聲序列；X₃(k)為第三子濾波器的狀態向量；

第三子濾波器觀測矩陣H₃(k)為：

k表示離散時刻；

步驟4：按照如下規則將組合導航式位姿測量系統整體的過程信息包括Q_i(k)、及P_i(k)平均分配到各子濾波器：

$\left\{\begin{array}{c}{Q}_i(k-1)={\mathrm{\β}}_i^{-1}{Q}_g(k-1)\\ P_i(k-1)={\mathrm{\β}}_i^{-1}{P}_g(k-1)\\ X_i(k-1)=X_g(k-1)\end{array}\right.$

且滿足

其中，Q_i(k)、及P_i(k)分別為各濾波器的噪聲方差陣、狀態估計向量、估計誤差協方差，i＝1,2,3,m；

Q_g(k)、P_g(k)分別為組合導航式位姿測量系統的噪聲方差陣、狀態估計向量、估計誤差協方差；為分配參數β_i(k)的倒數；

E[]是期望函數，的初始值為[0]_1×15；

由步驟3的觀測方程求得，Q_i＝E[W_i(k)W_i(k)^T]/T，W_i為各濾波器的噪聲向量；

步驟5：基于步驟4所得各濾波器的噪聲方差矩陣、狀態估計向量、估計誤差協方差即Q_i(k)、及P_i(k)，按照以下公式將各濾波器狀態估計與估計誤差協方差按組合導航式位姿測量系統的轉移矩陣進行轉移，得到各濾波器轉移后的狀態估計向量和估計誤差協方差；

步驟6：利用步驟5所得各濾波器轉移后的狀態估計向量與估計誤差協方差，采用如下測量方程，對K_i(k)、P_i(k)按以下公式進行更新：

${\hat{X}}_i\left(k\right)={\hat{X}}_i(k,k-1)+K_i\left(k\right)\left(Z_i\right(k)-H_i(k\left){\hat{X}}_i\right(k,k-1\left)\right)$

$K_i\left(k\right)=P_i(k,k-1)H_i^T\left(k\right){\left(H_i\right(k\left)P_i\right(k,k-1)+R_i(k\left)\right)}^{-1}$

P_i(k)＝(I-K_i(k)H_i(k))P_i(k,k-1)

其中，i＝1,2,3,m，當i＝m時，P_m(k)＝P_m(k,k-1)；K_i為各濾波器的增益矩陣，R_i為各濾波器觀測噪聲的協方差矩陣，R_i＝E[v_i(k)v_i(k)^T]/T，v_i為各濾波器觀測白噪聲序列，I為15×15的單位矩陣；T為慣性單元采樣周期，P_i(k,k-1)為轉移估計誤差協方差陣，

步驟7：將各子濾波器的估計信息按照下式進行融合，得到組合導航式位姿測量系統的狀態估計與估計誤差協方差全局最優估計P_g，并返回步驟4，依據P_g將組合導航式位姿測量系統的過程信息對各子濾波器進行重新分配，對各濾波器進行反饋修正：

$\begin{array}{c}{P}_g={\[P_1^{-1}+P_2^{-1}+P_3^{-1}+P_m^{-1}\]}^{-1}\\ \widehat{X_g}=P_g\[P_1^{-1}\widehat{X_1}+P_2^{-1}\widehat{X_2}+P_3^{-1}\widehat{X_3}+P_m^{-1}\widehat{X_m}\]\end{array}.$