本發(fā)明涉及礦井罐籠位姿檢測領(lǐng)域，具體涉及一種基于組合導(dǎo)航技術(shù)的超深礦井罐籠位姿測量系統(tǒng)及方法。

背景技術(shù)：
礦井提升系統(tǒng)是井下與地面聯(lián)系的主要工具，主要用于提升煤炭、礦石，升降設(shè)備和人員，是礦山生產(chǎn)中的關(guān)鍵設(shè)備之一。由于礦井環(huán)境惡劣，容易造成提升系統(tǒng)元器件失效，現(xiàn)階段缺乏對提升過程的全程實時監(jiān)控，常常發(fā)生提升機超速、過卷，導(dǎo)致礦井生產(chǎn)中斷、設(shè)備損壞、人員傷亡等事故發(fā)生，給煤礦生產(chǎn)帶來極大的經(jīng)濟損失。提升系統(tǒng)的罐籠是直接與煤礦、設(shè)備和人員相接觸的，提升系統(tǒng)的性能主要反映在罐籠的安全性和準確性。罐籠定位不精確，將降低生產(chǎn)效率；一旦發(fā)生傾斜、過卷、卡罐等故障將導(dǎo)致重大事故。因此，其可靠、高效、安全運行具有十分重要的意義，將直接關(guān)系到礦山生產(chǎn)的安全和效益，必須對其進行實時監(jiān)控。隨著我國淺表層礦產(chǎn)資源的枯竭，部分礦山已進入超過1000m深部開采，未來15～20年內(nèi)，我國將有大批金屬礦山轉(zhuǎn)入深部開采。相較于淺層礦井，深井及超深井提升系統(tǒng)大量程、高速度、重載荷的工作特點，要求系統(tǒng)有更高的安全性和可靠性；同時要求提升系統(tǒng)定位精度高，縮短提升時間，提高開采效率，降低開采成本。目前國內(nèi)礦井罐籠無姿態(tài)檢測，缺少傾斜預(yù)警，存在安全問題；位置測量根據(jù)卷筒旋轉(zhuǎn)帶動深度指示器或編碼器間接計算出罐籠位置，而鋼絲繩在自身重力及重載荷下發(fā)生彈性伸長、在卷筒上打滑、纏繞半徑隨纏繞層數(shù)變化等問題導(dǎo)致定位精度差；以上問題隨著井深增加更加嚴重，不能滿足超深井采礦要求。因此，有必要開發(fā)一種測量精度高、安全可靠的新型罐籠位姿測量裝置。

技術(shù)實現(xiàn)要素：
為解決上述現(xiàn)有技術(shù)的不足，本發(fā)明提供了一種基于組合導(dǎo)航技術(shù)的超深礦井罐籠位姿測量系統(tǒng)及方法，該系統(tǒng)抗干擾能力強、測量精度高、可靠性高。一種基于組合導(dǎo)航技術(shù)的超深礦井罐籠位姿測量系統(tǒng)，包括捷聯(lián)慣性導(dǎo)航裝置、地面處理裝置、旋轉(zhuǎn)編碼器、接近開關(guān)和感應(yīng)通信電纜；所述捷聯(lián)慣性導(dǎo)航裝置安裝在罐籠頂部；所述旋轉(zhuǎn)編碼器安裝在提升機卷筒的卷軸上；所述感應(yīng)通信電纜鋪設(shè)于礦井側(cè)壁；所述接近開關(guān)設(shè)置在井筒壁上；所述旋轉(zhuǎn)編碼器和接近開關(guān)與安裝在礦井地表面的地面處理裝置進行通信連接，所述地面處理裝置通過感應(yīng)通信電纜與捷聯(lián)慣性導(dǎo)航裝置進行通信連接；以捷聯(lián)慣性導(dǎo)航裝置、接近開關(guān)和旋轉(zhuǎn)編碼器構(gòu)建成組合導(dǎo)航式位姿測量系統(tǒng)。所述捷聯(lián)慣性導(dǎo)航裝置包括慣性單元、數(shù)據(jù)采集及處理模塊、導(dǎo)航解算及信息融合模塊、數(shù)據(jù)存儲模塊及通信模塊；所述慣性單元通過SPI總線與數(shù)據(jù)采集及處理模塊相連，所述數(shù)據(jù)采集及處理模塊與導(dǎo)航解算及信息融合模塊相連，所述數(shù)據(jù)采集及處理模塊與導(dǎo)航解算及信息融合模塊均與所述數(shù)據(jù)存儲模塊相連，所述數(shù)據(jù)存儲模塊與通信模塊相連。所述數(shù)據(jù)采集及處理模塊與導(dǎo)航解算及信息融合模塊均采用TI公司的TMS320系列C6748浮點型DSP處理器。一種基于組合導(dǎo)航技術(shù)的超深礦井罐籠位姿測量方法，將數(shù)據(jù)采集與處理和導(dǎo)航解算與信息融合分配給兩個不同的處理器處理；所述數(shù)據(jù)采集與處理的過程如下：首先讀取捷聯(lián)慣性導(dǎo)航裝置中的慣性單元的數(shù)據(jù)，根據(jù)慣性單元偏置溫度系數(shù)進行線性溫度誤差補償，然后讀取地面處理裝置數(shù)據(jù)，最后存儲數(shù)據(jù)，上傳數(shù)據(jù)至上位機；所述導(dǎo)航解算與信息融合的過程如下：通過將捷聯(lián)慣性導(dǎo)航裝置安裝在罐籠頂部，利用捷聯(lián)慣性導(dǎo)航裝置的慣性單元測量出罐籠運動加速度和角速度，采用四元數(shù)法建立罐籠姿態(tài)矩陣，通過SINS解算出罐籠位置和姿態(tài)信息；通過地面處理裝置采集井筒壁上的接近開關(guān)和提升機卷筒軸上的旋轉(zhuǎn)編碼器測量罐籠的位置和速度，采用聯(lián)合卡爾曼濾波算法將捷聯(lián)慣性導(dǎo)航裝置的速度和位置信息與罐籠位置與速度進行信息融合，反饋校正捷聯(lián)慣性導(dǎo)航裝置，實現(xiàn)罐籠位置和姿態(tài)的精確測量。所述信息融合的步驟如下：步驟1：建立捷聯(lián)慣性導(dǎo)航裝置誤差模型，包括三軸陀螺儀誤差模型、加速度測量誤差模型、位置誤差模型及姿態(tài)角誤差模型[1]；步驟2：以步驟1中的捷聯(lián)慣性導(dǎo)航裝置誤差模型建立離散化的組合導(dǎo)航式位姿測量系統(tǒng)的狀態(tài)方程[2]；所述組合導(dǎo)航式位姿測量系統(tǒng)是指由捷聯(lián)慣性導(dǎo)航裝置、接近開關(guān)和旋轉(zhuǎn)編碼器組成；步驟3：構(gòu)建信息融合卡爾曼濾波器，并依據(jù)組合導(dǎo)航式位姿測量系統(tǒng)的狀態(tài)向量建立信息融合卡爾曼濾波器中各子濾波器的觀測方程，求得各子濾波器的狀態(tài)向量估計值；所述信息融合卡爾曼濾波器包括第一子濾波器、第二子濾波器、第三子濾波器以及主濾波器，所述第一子濾波器、第二子濾波器及第三子濾波器均與主濾波器相連；所述第一子濾波器、第二子濾波器、第三子濾波器以及主濾波器均為卡爾曼濾波器；各濾波器狀態(tài)方程與組合導(dǎo)航式位姿測量系統(tǒng)狀態(tài)方程結(jié)構(gòu)相同；將捷聯(lián)慣性導(dǎo)航裝置的速度與編碼器速度之差作為第一子濾波器的輸入信號；將捷聯(lián)慣性導(dǎo)航裝置的位置信號與接近開關(guān)位置信號之差作為第二子濾波器的輸入信號；將捷聯(lián)慣性導(dǎo)航裝置的東、北方向的零速度和零位置信號作為約束條件，構(gòu)造成虛擬信號作為第三子濾波器的輸入信號；第一子濾波器的觀測方程為：Z1(k)＝H1(k)X1(k)+v1(k)；第一子濾波器觀測向量Z1(k)＝[vu-vb]，vb為地面處理裝置基于旋轉(zhuǎn)編碼器計算得到的罐籠速度，v1(k)為第一子濾波器觀測噪聲，是均值為零的高斯白噪聲序列，由高斯函數(shù)可得，X1(k)為第一子濾波器的狀態(tài)向量，第一子濾波器觀測矩陣H1(k)＝[01×8101×6]1×15；第二子濾波器的觀測方程為：Z2(k)＝H2(k)X2(k)+v2(k)；第二子濾波器觀測向量Z2(k)＝[H-Hj]，H為捷聯(lián)慣性導(dǎo)航裝置解算出的罐籠垂直位置，Hj為接近開關(guān)位置信號，v2(k)為第二子濾波器位置觀測噪聲，是均值為零的高斯白噪聲序列；X2(k)為第二子濾波器的狀態(tài)向量，H2(k)＝[01×5101×9]1×15為第二子濾波器位置觀測矩陣；第三子濾波器觀測方程為：Z3(k)＝H3(k)X3(k)+v3(k)；第三子濾波器觀測向量Z3(k)＝[LEvevn]T，L和E為捷聯(lián)慣性導(dǎo)航裝置東向和西向位置，ve和vn為捷聯(lián)慣性導(dǎo)航裝置東向和西向速度，v3(k)為第三子濾波器觀測噪聲，是均值為零的高斯白噪聲序列；X3(k)為第三子濾波器的狀態(tài)向量；第三子濾波器觀測矩陣H3(k)為：k表示離散時刻；步驟4：按照如下規(guī)則將組合導(dǎo)航式位姿測量系統(tǒng)整體的過程信息包括Qi(k)、及Pi(k)平均分配到各子濾波器：且滿足其中，Qi(k)、及Pi(k)分別為各濾波器的噪聲方差陣、狀態(tài)估計向量、估計誤差協(xié)方差，i＝1,2,3,m；Qg(k)、Pg(k)分別為組合導(dǎo)航式位姿測量系統(tǒng)的噪聲方差陣、狀態(tài)估計向量、估計誤差協(xié)方差；為分配參數(shù)βi(k)的倒數(shù)；E[]是期望函數(shù)，的初始值為[0]1×15；由步驟3的觀測方程求得，Qi＝E[Wi(k)Wi(k)T]/T，Wi為各濾波器的噪聲向量；步驟5：基于步驟4所得各濾波器的噪聲方差強度矩陣、狀態(tài)估計向量、估計誤差協(xié)方差即Qi(k)、及Pi(k)，將各濾波器狀態(tài)估計與估計誤差協(xié)方差按組合導(dǎo)航式位姿測量系統(tǒng)的轉(zhuǎn)移矩陣進行轉(zhuǎn)移，得到各濾波器轉(zhuǎn)移后的狀態(tài)估計向量和估計誤差協(xié)方差[3]；步驟6：利用步驟5所得各濾波器轉(zhuǎn)移后的狀態(tài)估計向量與估計誤差協(xié)方差，采用如下測量方程，對Ki(k)、Pi(k)按以下公式進行更新：Pi(k)＝(I-Ki(k)Hi(k))Pi(k,k-1)其中，i＝1,2,3,m，當(dāng)i＝m時，Pm(k)＝Pm(k,k-1)；Ki為各濾波器的增益矩陣，Ri＝E[vi(k)vi(k)T]/T為各濾波器觀測噪聲的協(xié)方差矩陣，vi為各濾波器觀測白噪聲序列，I為15×15的單位矩陣；T為慣性單元采樣周期，Pi(k,k-1)為轉(zhuǎn)移估計誤差協(xié)方差陣；步驟7：將各子濾波器的估計信息按照下式進行融合，得到組合導(dǎo)航式位姿測量系統(tǒng)的狀態(tài)估計與估計誤差協(xié)方差全局最優(yōu)估計Pg，并返回步驟4，依據(jù)Pg將組合導(dǎo)航式位姿測量系統(tǒng)的過程信息對各子濾波器進行重新分配，對各濾波器進行反饋修正：有益效果本發(fā)明一種基于組合導(dǎo)航技術(shù)的超深礦井罐籠位姿測量系統(tǒng)及方法，相對于現(xiàn)有技術(shù)，本發(fā)明有以下有益的技術(shù)和經(jīng)濟效果：1)本系統(tǒng)全封閉設(shè)計，抗干擾能力強，適應(yīng)性強，結(jié)構(gòu)簡單，維護方便；2)本方法能提高罐籠定位精度，位置誤差不受鋼絲繩影響，不隨礦井深度增加而擴大，能提高礦井生產(chǎn)效益；3)本方法采用多傳感器信息融合技術(shù)，單一傳感器出錯，不會造成測量失效，只會影響測量精度，系統(tǒng)容錯性好，可靠性高；4)本方法能有效的監(jiān)測罐籠姿態(tài)，提供傾斜預(yù)警，大大提高罐籠安全性和可靠性；5)采用雙DSP處理器，提高了整個系統(tǒng)的處理速度和測量的準確度。本發(fā)明檢測精度不受礦井深度和提升鋼絲繩的彈性伸長、打滑、蠕動、纏繞半徑變化等影響，能實時有效的監(jiān)測罐籠位姿狀態(tài)，提供傾斜預(yù)警及精確定位，滿足超深礦井對位姿檢測系統(tǒng)的要求。附圖說明圖1是本發(fā)明所述系統(tǒng)結(jié)構(gòu)示意圖；圖2是罐籠位姿測量系統(tǒng)原理；圖3是信息融合卡爾曼濾波器結(jié)構(gòu)圖；圖4是系統(tǒng)硬件結(jié)構(gòu)圖；其中，附圖1中：1—提升機卷筒，2—旋轉(zhuǎn)編碼器，3—天輪，4—地面處理裝置，5—感應(yīng)電纜，6—接近開關(guān)，7—捷聯(lián)慣性導(dǎo)航裝置，8—罐籠，9—罐耳，10—導(dǎo)向鋼絲繩，11—提升鋼絲繩。具體實施方式下面將結(jié)合附圖和實施例對本發(fā)明作進一步的說明。如圖1所示，一種基于組合導(dǎo)航技術(shù)的超深礦井罐籠位姿測量系統(tǒng)，包括捷聯(lián)慣性導(dǎo)航裝置7、地面處理裝置、旋轉(zhuǎn)編碼器2、接近開關(guān)6和感應(yīng)通信電纜5；所述提升鋼絲繩11纏繞在卷筒1上，末端與罐籠8相連；所述罐耳9在罐籠8的8個頂角，穿過4根導(dǎo)向鋼絲繩10，對罐籠產(chǎn)生柔性約束；所述提升鋼絲繩11纏繞在卷筒1上，穿過天輪3，末端與罐籠8相連；所述捷聯(lián)慣性導(dǎo)航裝置安裝在罐籠8頂部；所述旋轉(zhuǎn)編碼器安裝在提升機卷筒1的卷軸上；所述感應(yīng)通信電纜鋪設(shè)于礦井側(cè)壁；所述接近開關(guān)設(shè)置在井筒壁上；所述旋轉(zhuǎn)編碼器和接近開關(guān)與安裝在礦井地表面的地面處理裝置進行通信連接，所述地面處理裝置通過感應(yīng)通信電纜與捷聯(lián)慣性導(dǎo)航裝置進行通信連接；以捷聯(lián)慣性導(dǎo)航裝置、接近開關(guān)和旋轉(zhuǎn)編碼器構(gòu)建成組合導(dǎo)航式位姿測量系統(tǒng)。所述捷聯(lián)慣性導(dǎo)航裝置包括慣性單元、數(shù)據(jù)采集及處理模塊、導(dǎo)航解算及信息融合模塊、數(shù)據(jù)存儲模塊及通信模塊；所述慣性單元通過SPI總線與數(shù)據(jù)采集及處理模塊相連，所述數(shù)據(jù)采集及處理模塊與導(dǎo)航解算及信息融合模塊相連，所述數(shù)據(jù)采集及處理模塊與導(dǎo)航解算及信息融合模塊均與所述數(shù)據(jù)存儲模塊相連，所述數(shù)據(jù)存儲模塊與通信模塊相連。所述數(shù)據(jù)采集及處理模塊與導(dǎo)航解算及信息融合模塊均采用TI公司的TMS320系列C6748浮點型DSP處理器。一種基于組合導(dǎo)航技術(shù)的超深礦井罐籠位姿測量方法，將數(shù)據(jù)采集與處理和導(dǎo)航解算與信息融合分配給兩個不同的處理器處理；所述數(shù)據(jù)采集與處理的過程如下：首先讀取捷聯(lián)慣性導(dǎo)航裝置中的慣性單元的數(shù)據(jù)，根據(jù)慣性單元偏置溫度系數(shù)進行線性溫度誤差補償，然后讀取地面處理裝置數(shù)據(jù)，最后存儲數(shù)據(jù)，上傳數(shù)據(jù)至上位機；所述導(dǎo)航解算與信息融合的過程如下：通過將捷聯(lián)慣性導(dǎo)航裝置安裝在罐籠頂部，利用捷聯(lián)慣性導(dǎo)航裝置的慣性單元測量出罐籠運動加速度和角速度，采用四元數(shù)法建立罐籠姿態(tài)矩陣，通過SINS解算出罐籠位置和姿態(tài)信息；通過地面處理裝置采集井筒壁上的接近開關(guān)和提升機卷筒軸上的旋轉(zhuǎn)編碼器測量罐籠的位置和速度，采用聯(lián)合卡爾曼濾波算法將捷聯(lián)慣性導(dǎo)航裝置的速度和位置信息與罐籠位置與速度進行信息融合，反饋校正捷聯(lián)慣性導(dǎo)航裝置，實現(xiàn)罐籠位置和姿態(tài)的精確測量。對于柔性罐道，罐籠在井筒中垂直提升運動，水平方向運動及姿態(tài)變化受罐道鋼絲繩約束，因此可將罐籠運動簡化為沿垂直方向直線運動及三軸轉(zhuǎn)動的運動模型，水平方向運動作為零約束修正，罐籠位姿測量系統(tǒng)原理如圖2所示，捷聯(lián)慣導(dǎo)系統(tǒng)與罐籠固聯(lián)的坐標(biāo)系為載體坐標(biāo)系(b系)，取東北天坐標(biāo)系(g系)為導(dǎo)航坐標(biāo)系(n系)。將捷聯(lián)慣性導(dǎo)航裝置安裝在罐籠頂部，慣性單元測量出罐籠運動加速度和角速度，采用四元數(shù)法建立罐籠姿態(tài)矩陣，通過導(dǎo)航系統(tǒng)解算出罐籠位置和姿態(tài)信息；通過信息融合算法將SINS速度和位置信息與傳感器測量的信息融合，反饋校正慣性單元及導(dǎo)航解算，提高位姿測量精度。姿態(tài)解算是捷聯(lián)慣導(dǎo)系統(tǒng)的核心，通過姿態(tài)矩陣可計算出罐籠姿態(tài)角，同時將載體坐標(biāo)系下的比力分解為導(dǎo)航坐標(biāo)系下的加速度，進行速度和位置計算。常用算法有歐拉角法、方向余弦法和四元數(shù)法，由于四元數(shù)法計算量小、計算無奇點，罐籠姿態(tài)信息采用四元數(shù)法進行解算。解算過程如下：步驟1：設(shè)置初始向量Q＝[1000]，通過慣性單元采集三軸加速度f＝[fx、fy、fz]，三軸角速度w＝[wxwywz]。步驟2：通過公式(1)(2)進行四元數(shù)迭代更新：式中T為慣性單元采樣周期。更新四元數(shù)后即可更新的姿態(tài)矩陣：其簡化表示為：步驟3：從更新后的姿態(tài)矩陣可求出姿態(tài)信息：式中：θ為俯仰角，ψ為橫滾角，γ為航向角。步驟4：通過采集到的三軸加速度f＝[fx、fy、fz]，和更新后的姿態(tài)矩陣，計算罐籠在導(dǎo)航坐標(biāo)系下東北天方向的速度：v＝[vevnvu]式中：ωie≈15°/h為地球自轉(zhuǎn)角速率，L為所處地理緯度，R＝6371km為地球平均半徑，h為罐籠距地球表面距離，g＝9.78m/s2為重力加速度常量。步驟5：聯(lián)立求解(3)式中的三個微分方程，得到罐籠速度信息v，利用數(shù)值積分法直接對速度積分計算罐籠位置：式中：L、E、h分別表示罐籠東、北、天方向位移，L0、E0、h0為相應(yīng)初始值，T為慣性單元采樣周期。本發(fā)明所述系統(tǒng)采用聯(lián)合卡爾曼濾波算法進行多傳感器信息融合校正，信息融合濾波器結(jié)構(gòu)如圖3所示，由一個全局濾波器和三個子濾波器組成。SINS與旋轉(zhuǎn)編碼器構(gòu)成速度子濾波器1，進行垂直速度融合校正；SINS與接近開關(guān)構(gòu)成位置子濾波器2，進行位置融合校正；罐籠垂直運動，由柔性鋼絲繩約束其水平運動，可認為水平面兩個方向位置和速度為零，SINS與約束修正構(gòu)成約束子濾波器3，進行動態(tài)零速、零位修正；約束子濾波器3的速度和位置觀測量與其他濾波器觀測量更新頻率不同，因此不與其他濾波器合并。所述信息融合的步驟如下：步驟1：建立捷聯(lián)慣性導(dǎo)航裝置誤差模型，包括三軸陀螺儀誤差模型、加速度測量誤差模型、位置誤差模型及姿態(tài)角誤差模型[1]；步驟2：以步驟1中的捷聯(lián)慣性導(dǎo)航裝置誤差模型建立離散化的組合導(dǎo)航式位姿測量系統(tǒng)的狀態(tài)方程[2]；所述組合導(dǎo)航式位姿測量系統(tǒng)是指由捷聯(lián)慣性導(dǎo)航裝置、接近開關(guān)和旋轉(zhuǎn)編碼器組成；步驟3：構(gòu)建信息融合卡爾曼濾波器，并依據(jù)組合導(dǎo)航式位姿測量系統(tǒng)的狀態(tài)向量建立信息融合卡爾曼濾波器中各子濾波器的觀測方程，求得各子濾波器的狀態(tài)向量估計值；所述信息融合卡爾曼濾波器包括第一子濾波器、第二子濾波器、第三子濾波器以及主濾波器，所述第一子濾波器、第二子濾波器及第三子濾波器均與主濾波器相連；所述第一子濾波器、第二子濾波器、第三子濾波器以及主濾波器均為卡爾曼濾波器；所述第一子濾波器即為速度子濾波器1、第二子濾波器即為位置子濾波器2、第三子濾波器即為約束子濾波器3；各濾波器狀態(tài)方程與組合導(dǎo)航式位姿測量系統(tǒng)狀態(tài)方程結(jié)構(gòu)相同；將捷聯(lián)慣性導(dǎo)航裝置的速度與編碼器速度之差作為第一子濾波器的輸入信號；將捷聯(lián)慣性導(dǎo)航裝置的位置信號與接近開關(guān)位置信號之差作為第二子濾波器的輸入信號；將捷聯(lián)慣性導(dǎo)航裝置的東、北方向的零速度和零位置信號作為約束條件，構(gòu)造成虛擬信號作為第三子濾波器的輸入信號；第一子濾波器的觀測方程為：Z1(k)＝H1(k)X1(k)+v1(k)；第一子濾波器觀測向量Z1(k)＝[vu-vb]，vb為地面處理裝置基于旋轉(zhuǎn)編碼器計算得到的罐籠速度，v1(k)為第一子濾波器觀測噪聲，是均值為零的高斯白噪聲序列，由高斯函數(shù)可得，X1(k)為第一子濾波器的狀態(tài)向量，第一子濾波器觀測矩陣H1(k)＝[01×8101×6]1×15；第二子濾波器的觀測方程為：Z2(k)＝H2(k)X2(k)+v2(k)；第二子濾波器觀測向量Z2(k)＝[H-Hj]，H為捷聯(lián)慣性導(dǎo)航裝置解算出的罐籠垂直位置，Hj為接近開關(guān)位置信號，v2(k)為第二子濾波器位置觀測噪聲，是均值為零的高斯白噪聲序列；X2(k)為第二子濾波器的狀態(tài)向量，H2(k)＝[01×5101×9]1×15為第二子濾波器位置觀測矩陣；第三子濾波器觀測方程為：Z3(k)＝H3(k)X3(k)+v3(k)；第三子濾波器觀測向量Z3(k)＝[LEvevn]T，L和E為捷聯(lián)慣性導(dǎo)航裝置東向和西向位置，ve和vn為捷聯(lián)慣性導(dǎo)航裝置東向和西向速度，v3(k)為第三子濾波器觀測噪聲，是均值為零的高斯白噪聲序列；X3(k)為第三子濾波器的狀態(tài)向量；第三子濾波器觀測矩陣H3(k)為：k表示離散時刻；步驟4：按照如下規(guī)則將組合導(dǎo)航式位姿測量系統(tǒng)整體的過程信息包括Qi(k)、及Pi(k)平均分配到各子濾波器：且滿足其中，Qi(k)、及Pi(k)分別為各濾波器的噪聲方差陣、狀態(tài)估計向量、估計誤差協(xié)方差，i＝1,2,3,m；Qg(k)、Pg(k)分別為組合導(dǎo)航式位姿測量系統(tǒng)的噪聲方差陣、狀態(tài)估計向量、估計誤差協(xié)方差；為分配參數(shù)βi(k)的倒數(shù)；E[]是期望函數(shù)，的初始值為[0]1×15；由步驟3的觀測方程求得，Qi＝E[Wi(k)Wi(k)T]/T，Wi為各濾波器的噪聲向量；在本實例中，組合導(dǎo)航式位姿測量系統(tǒng)的噪聲方差陣初始值取值如下：Qg＝diag[(0.05°/h)2,(0.05°/h)2,(0.05°/h)2,(0.01°/h)2,(0.01°/h)2,(0.01°/h)2]組合導(dǎo)航式位姿測量系統(tǒng)的估計誤差協(xié)方差初始值：Pg＝diag[(0.1m/s)2,(0.1m/s)2,(0.1m/s)2,(0.002°)2,(0.004°)2,(1°)2,(1°)2,(1°)2,(1°)2,(1°)2,(1°)2,(1°)2,(1°)2,(10-4g)2,(10-4g)2]速度子濾波器1、位置子濾波器2、約束子濾波器3的觀測噪聲方差陣初始值：R1＝diag[(0.002°)2,(0.004°)2]R2＝diag[(0.5m/s)2,(0.5m/s)2]R3＝diag[(0.002°)2,(0.004°)2,(0.5m/s)2,(0.5m/s)2]步驟5：基于步驟4所得各濾波器的噪聲方差強度矩陣、狀態(tài)估計向量、估計誤差協(xié)方差即Qi(k)、及Pi(k)，將各濾波器狀態(tài)估計與估計誤差協(xié)方差按組合導(dǎo)航式位姿測量系統(tǒng)的轉(zhuǎn)移矩陣進行轉(zhuǎn)移，得到各濾波器轉(zhuǎn)移后的狀態(tài)估計向量和估計誤差協(xié)方差[3]；步驟6：利用步驟5所得各濾波器轉(zhuǎn)移后的狀態(tài)估計向量與估計誤差協(xié)方差，采用如下測量方程，對Ki(k)、Pi(k)按以下公式進行更新：Pi(k)＝(I-Ki(k)Hi(k))Pi(k,k-1)其中，i＝1,2,3,m，當(dāng)i＝m時，Pm(k)＝Pm(k,k-1)；Ki為各濾波器的增益矩陣；Ri＝E[vi(k)vi(k)T]/T為各濾波器觀測噪聲的協(xié)方差矩陣，vi為各濾波器觀測白噪聲序列，I為15×15的單位矩陣；T為慣性單元采樣周期，Pi(k,k-1)為轉(zhuǎn)移估計誤差協(xié)方差陣；步驟7：將各子濾波器的估計信息按照下式進行融合，得到組合導(dǎo)航式位姿測量系統(tǒng)的狀態(tài)估計與估計誤差協(xié)方差全局最優(yōu)估計Pg，并返回步驟4，依據(jù)Pg將組合導(dǎo)航式位姿測量系統(tǒng)的過程信息對各子濾波器進行重新分配，對各濾波器進行反饋修正：在各子濾波器計算得到各自局部估計值后，經(jīng)過全局濾波器融合，得到位置、速度全局最優(yōu)估計，利用全局濾波結(jié)果，按照信息守恒原理在各濾波器進行分配，對局部濾波器進行反饋修正，提高測量精度。組合導(dǎo)航式位姿測量系統(tǒng)硬件結(jié)構(gòu)如圖4所示，采用雙DSP處理器，DSP1進行數(shù)據(jù)采集及處理；DSP2進行導(dǎo)航解算和信息融合運算；慣性測量單元(IMU)以500Hz頻率實時測量加速度、陀螺儀、溫度信號，通過SPI接口輸入DSP1；編碼器和接近開關(guān)信號通過地面AD模塊轉(zhuǎn)換處理后，由通信模塊發(fā)送至DSP1；通信模塊負責(zé)各模塊數(shù)據(jù)交換，并將導(dǎo)航解算后的位姿數(shù)據(jù)通過通信模塊以10Hz頻率傳給上位機。參考文獻:[1]李大威.卡爾曼濾波器在INS_GPS組合導(dǎo)航中的應(yīng)用研究[D].太原：中北大學(xué)，2006[2]袁克非.組合導(dǎo)航系統(tǒng)多源信息融合關(guān)鍵技術(shù)研究[D].哈爾濱：哈爾濱工程大學(xué)，2012[3]肖乾.多傳感器組合導(dǎo)航系統(tǒng)信息融合技術(shù)研究[D].哈爾濱：哈爾濱工程大學(xué)，2005