數P1化）的倒數；
[0046] 巧=￡[化-乂:,)化-乂;;)']，E□是期望函數，iy勺初始值為[0]ixi5;
[0047] 《由步驟3的觀測方程求得，Qi=E[W1似Wi似t]/T，Wi為各濾波器的噪聲向量；
[0048] 步驟5 :基于步驟4所得各濾波器的噪聲方差強度矩陣、狀態估計向量、估計誤差 協方差即Qi化)、乂,化)及Pi化)，將各濾波器狀態估計與估計誤差協方差按組合導航式位 姿測量系統的轉移矩陣進行轉移，得到各濾波器轉移后的狀態估計向量和估計誤差協方差
[3] ，
[0049]步驟6 :利用步驟5所得各濾波器轉移后的狀態估計向量與估計誤差協方差，采用 如下測量方程，對義少）、Ki化)、Pi(k)按W下公式進行更新：
W巧Pi似=(I-Ki似Hi似）Pi(k，k-1) 陽化引其中，i= 1，2, 3，m，當i=m時，Pm似=Pm(k，k-1)屯為各濾波器的增益矩陣，Ri=E[V1化)Vi化)T] /T為各濾波器觀測噪聲的協方差矩陣，Vi為各濾波器觀測白噪聲序列， I為15X15的單位矩陣；T為慣性單元采樣周期，Pi化，k-1)為轉移估計誤差協方差陣；[0054] 步驟7:將各子濾波器的估計信息按照下式進行融合，得到組合導航式位姿測量 系統的狀態估計與估計誤差協方差全局最優估計、Pg，并返回步驟4,依據矣、Pg將組合 導航式位姿測量系統的過程信息對各子濾波器進行重新分配，對各濾波器進行反饋修正： 陽化引
。
[0056] 有益效果
[0057] 本發明一種基于組合導航技術的超深礦井罐籠位姿測量系統及方法，相對于現有 技術，本發明有W下有益的技術和經濟效果：
[005引 1)本系統全封閉設計，抗干擾能力強，適應性強，結構簡單，維護方便；
[0059] 2)本方法能提高罐籠定位精度，位置誤差不受鋼絲繩影響，不隨礦井深度增加而 擴大，能提高礦井生產效益；
[0060] 3)本方法采用多傳感器信息融合技術，單一傳感器出錯，不會造成測量失效，只會 影響測量精度，系統容錯性好，可靠性高；
[0061] 4)本方法能有效的監測罐籠姿態，提供傾斜預警，大大提高罐籠安全性和可靠 性；
[0062] 5)采用雙DSP處理器，提高了整個系統的處理速度和測量的準確度。
[0063] 本發明檢測精度不受礦井深度和提升鋼絲繩的彈性伸長、打滑、蠕動、纏繞半徑變 化等影響，能實時有效的監測罐籠位姿狀態，提供傾斜預警及精確定位，滿足超深礦井對位 姿檢測系統的要求。
【附圖說明】
[0064] 圖1是本發明所述系統結構示意圖； 陽0化]圖2是罐籠位姿測量系統原理；
[0066] 圖3是信息融合卡爾曼濾波器結構圖； 陽067] 圖4是系統硬件結構圖；
[0068]其中，附圖1中：1一提升機卷筒，2-旋轉編碼器，3-天輪，4-地面處理裝置，5-感應電纜，6-接近開關，7-捷聯慣性導航裝置，8-罐籠，9-罐耳，10-導向鋼絲繩，11-提升鋼絲繩例子中加上了
【具體實施方式】 W例下面將結合附圖和實施例對本發明作進一步的說明。
[0070] 如圖1所示，一種基于組合導航技術的超深礦井罐籠位姿測量系統，包括捷聯慣 性導航裝置7、地面處理裝置、旋轉編碼器2、接近開關6和感應通信電纜5;
[0071] 所述提升鋼絲繩11纏繞在卷筒1上，末端與罐籠8相連；
[0072] 所述罐耳9在罐籠8的8個頂角，穿過4根導向鋼絲繩10,對罐籠產生柔性約束；
[0073] 所述提升鋼絲繩11纏繞在卷筒1上，穿過天輪3,末端與罐籠8相連；
[0074] 所述捷聯慣性導航系統安裝在罐籠8頂部；
[00巧]所述旋轉編碼器安裝在提升機卷筒1的卷軸上；
[0076] 所述感應通信電纜鋪設于礦井側壁；
[0077] 所述接近開關設置在井筒壁上；
[0078] 所述旋轉編碼器和接近開關與安裝在礦井地表面的地面處理裝置進行通信連接， 所述地面處理裝置通過感應通信電纜與捷聯慣性導航裝置進行通信連接；
[0079]W捷聯慣性導航系統、接近開關和旋轉編碼器構建成組合導航式位姿測量系統。
[0080] 所述捷聯慣性導航系統包括慣性單元、數據采集及處理模塊、導航解算及信息融 合模塊、數據存儲模塊及通信模塊；
[0081] 所述慣性單元通過SPI總線與數據采集及處理模塊相連，所述數據采集及處理模 塊與導航解算及信息融合模塊相連，所述數據采集及處理模塊與導航解算及信息融合模塊 均與所述數據存儲模塊相連，所述數據存儲模塊與通信模塊相連。
[0082] 所述數據采集及處理模塊與導航解算及信息融合模塊均采用TI公司的TMS320系 列C6748浮點型DSP處理器。
[0083] 一種基于組合導航技術的超深礦井罐籠位姿測量方法，將數據采集與處理和導航 解算與信息融合分配給兩個不同的處理器處理；
[0084] 所述數據采集與處理的過程如下：
[0085] 首先讀取捷聯慣性導航系統中的慣性單元的數據，根據慣性單元偏置溫度系數進 行線性溫度誤差補償，然后讀取地面處理裝置數據，最后存儲數據，上傳數據至上位機；
[0086] 所述導航解算與信息融合的過程如下：
[0087] 通過將捷聯慣性導航系統安裝在罐籠頂部，利用捷聯慣性導航系統的慣性單元測 量出罐籠運動加速度和角速度，采用四元數法建立罐籠姿態矩陣，通過SINS解算出罐籠位 置和姿態信息；通過地面處理裝置采集井筒壁上的接近開關和提升機卷筒軸上的旋轉編碼 器測量罐籠的位置和速度，采用聯合卡爾曼濾波算法將捷聯慣性導航系統的速度和位置信 息與罐籠位置與速度進行信息融合，反饋校正捷聯慣性導航系統，實現罐籠位置和姿態的 精確測量。
[0088] 對于柔性罐道，罐籠在井筒中垂直提升運動，水平方向運動及姿態變化受罐道鋼 絲繩約束，因此可將罐籠運動簡化為沿垂直方向直線運動及=軸轉動的運動模型，水平方 向運動作為零約束修正，罐籠位姿測量系統原理如圖2所示，捷聯慣導系統與罐籠固聯的 坐標系為載體坐標系化系），取東北天坐標系（g系）為導航坐標系（n系）。將捷聯慣性 導航裝置安裝在罐籠頂部，慣性單元測量出罐籠運動加速度和角速度，采用四元數法建立 罐籠姿態矩陣，通過導航系統解算出罐籠位置和姿態信息；通過信息融合算法將SINS速度 和位置信息與傳感器測量的信息融合，反饋校正慣性單元及導航解算，提高位姿測量精度。
[0089] 姿態解算是捷聯慣導系統的核屯、，通過姿態矩陣可計算出罐籠姿態角，同時將載 體坐標系下的比力分解為導航坐標系下的加速度，進行速度和位置計算。常用算法有歐拉 角法、方向余弦法和四元數法，由于四元數法計算量小、計算無奇點，罐籠姿態信息采用四 元數法進行解算。
[0090] 解算過程如下： 陽0川步驟1 :設置初始向量Q= [1 0 0 0]，通過慣性單元采集立軸加速度f=氏、與、fz]，ミ軸角速度w=[WXwyWZ]。 陽09引步驟2 :通過公式（1)似進行四元數迭代更新：
[0095] 式中T為慣性單元采樣周期。
[0096] 更新四元數后即可更新的姿態矩陣：巧
[0100] 步驟3 :從更新后的姿態矩陣可求出姿態信息：
[0101] 式中：0為俯仰角，為橫滾角，T為航向角。 陽10引步驟4 :通過采集到的；軸加速度f=氏、fy、。，和更新后的姿態矩陣，計算罐籠 在導航坐標系下東北天方向的速度v= [Vp:V。vj 陽 103]
[0104] 式中：15。A為地球自轉角速率，L為所處地理締度，R= 6371km為地球 平均半徑，h為罐籠距地球表面距離，g= 9. 78m/s2為重力加速度常量。
[01化]