本發(fā)明涉及一種礦用定向鉆機隨鉆測量系統(tǒng)軌跡的校正方法，尤其是以姿態(tài)傳感器的測量數(shù)據作為輸入信息，通過不同階次的龍格庫塔法求解捷聯(lián)慣導系統(tǒng)的機械編排方程，實時輸出當前鉆頭姿態(tài)角信息和位置信息，并通過零速校正算法消除捷聯(lián)慣導系統(tǒng)的累積誤差的技術方案。

背景技術：

隨鉆測量技術是受控定向鉆進的關鍵技術之一。隨著我國煤礦綜合機械化采煤技術的發(fā)展，常采用定向鉆機鉆探出水平定向長鉆孔對煤層進行瓦斯預排及探放水活動。隨鉆測斜儀是定向鉆機進行定向鉆進的關鍵設備，它的主要作用是完成方位角、傾角、工具面角的測量，進而繪制出鉆孔軌跡。目前，礦用隨鉆測斜儀主要使用三軸加速度計和三軸磁阻計作為測斜傳感器，依據歐拉角法確定鉆頭姿態(tài)。雖然這種方法穩(wěn)定可靠，但由于歐拉角法本身的局限性，勢必存在測量的奇異點，不能實現(xiàn)全角度測量；此外，磁阻計易受外部磁場干擾，導致鉆孔的方位角產生測量誤差；該方法只能在更換鉆桿后的靜止狀態(tài)下測量，不能在鉆進的過程中實時測量。為此，陀螺測斜儀應運而生。陀螺儀與加速度計構成捷聯(lián)慣導系統(tǒng)的測量單元，通過逐次的積分運算可以實時獲得鉆頭的姿態(tài)、速度以及位置信息。但是，直接進行積分運算計算量較大,不易在單片機中實現(xiàn)，目前仍缺乏快速有效的數(shù)值解法來代替其中的積分運算。

陀螺儀存在漂移現(xiàn)象，即輸出誤差隨時間逐漸累積。慣導系統(tǒng)長時間運行必將導致累積誤差，因此在不斷探索如何提高傳感器本身的精度外，還需要尋求外部信息對捷聯(lián)慣導系統(tǒng)進行校正，形成組合導航系統(tǒng)。通常將慣導系統(tǒng)作為主導航系統(tǒng)，其他誤差不隨時間累積的導航系統(tǒng)如gps、無線電導航、地磁導航等作為外部輔助測量系統(tǒng)。由于煤壁對無線電吸收能力較強，且井下沒有gps信號，因此無線電導航技術與gps導航技術不適用于煤礦井下隨鉆測量。地磁導航技術需要已知當?shù)氐牡卮艛?shù)據庫，在實際生產中較難實現(xiàn)。因此急需一種穩(wěn)定可靠的校正手段消除捷聯(lián)慣導系統(tǒng)的累積誤差。

技術實現(xiàn)要素：

本發(fā)明的任務是提供一種礦用陀螺測斜儀捷聯(lián)慣導系統(tǒng)姿態(tài)解算及零速校正方法。

具體技術方案如下。

一種礦用陀螺測斜儀捷聯(lián)慣導系統(tǒng)姿態(tài)解算及零速校正方法，所述方法采用的測斜儀包含有三種姿態(tài)傳感器：陀螺儀、加速度計和磁阻計；所述方法是以陀螺儀和加速度計作為捷聯(lián)慣導系統(tǒng)的測量單元，以姿態(tài)傳感器的測量數(shù)據作為輸入信息，通過不同階次的龍格庫塔法求解捷聯(lián)慣導系統(tǒng)的機械編排方程，實時輸出當前鉆頭姿態(tài)角信息和位置信息，并通過零速校正算法消除捷聯(lián)慣導系統(tǒng)的累積誤差。

附件技術方案一，所述捷聯(lián)慣導系統(tǒng)姿態(tài)解算的方法是以陀螺儀和加速度計作為捷聯(lián)慣導系統(tǒng)的測量單元，利用四階龍格庫塔法求解姿態(tài)矩陣微分方程，利用二階龍格庫塔法求解速度微分方程，一階龍格庫塔法求解位置微分方程。

附件技術方案二，所述零速校正方法的卡爾曼濾波過程需滿足兩個條件：零速條件和地磁條件；

所述零速條件是根據加速度計輸出的幅值進行判斷，其公式如下：

其中：為加速度計測量噪聲方差；n為采樣點數(shù)；g為重力加速度模值；

所述地磁條件是根據磁力計輸出的幅值進行判斷，公式如下：

其中：h為地磁場矢量模值。

本發(fā)明中的捷聯(lián)慣導姿態(tài)解算方法的核心思想是通過在積分區(qū)間內插值得到更新后的結果，與現(xiàn)有的直接積分運算方法相比，運算量小，一般的嵌入式單片機便可以完成。

本發(fā)明中的零速校正方法的卡爾曼濾波過程需滿足兩個條件：零速條件和地磁條件；其零速條件是根據加速度計輸出的幅值進行判斷，其地磁條件是根

據磁力計輸出的幅值進行判斷，零速校正方法與現(xiàn)有的gps組合導航、無線電組合導航等方法相比，充分結合了鉆機鉆進的工藝，即利用加裝鉆桿的間隙完成姿態(tài)角的校正。此外，磁強計體積小，抗振能力強，更適合于鉆桿鉆具的狹小空間。

附圖說明

圖1是本發(fā)明捷聯(lián)慣導解算初始參數(shù)表。

圖2是本發(fā)明捷聯(lián)慣導系統(tǒng)與磁強計組合的卡爾曼濾波算法示意圖。

圖3是本發(fā)明測斜儀的硬件電路結構圖。

圖4是本發(fā)明陀螺測斜儀中使用的三種姿態(tài)傳感器坐標系示意圖，滿足右手螺旋法則。

圖5是本發(fā)明經卡爾曼濾波后的傾角誤差。

圖6是本發(fā)明經卡爾曼濾波后的方位角誤差。

圖7是本發(fā)明經卡爾曼濾波后的工具面角誤差。

圖8是本發(fā)明經卡爾曼濾波后的北向誤差。

圖9是本發(fā)明經卡爾曼濾波后的東向誤差。

圖10是本發(fā)明經卡爾曼濾波后的地向誤差。

具體實施方式

下面作對本發(fā)明的具體方式出進一步的說明。

實施本發(fā)明上述所提供的一種適用于煤礦井下定向鉆機陀螺測斜儀姿態(tài)解算及零速校正方法，采用的測斜儀包含三種姿態(tài)傳感器、陀螺儀、加速度計和磁阻計，以陀螺儀和加速度計作為捷聯(lián)慣導系統(tǒng)的測量單元，通過不同階次的龍格庫塔法求解捷聯(lián)慣導系統(tǒng)的機械編排方程，實時輸出當前鉆頭姿態(tài)角信息和位置信息，同時為消除捷聯(lián)慣導系統(tǒng)長時工作所產生的累積誤差，以磁強計作為外部輔助測量裝置，通過零速校正算法實現(xiàn)多傳感器的組合導航，提高鉆孔軌跡測量的精確度。

本發(fā)明基于礦用陀螺測斜儀的捷聯(lián)慣導數(shù)值解算及零速校正方法是以陀螺儀和加速度計作為捷聯(lián)慣導系統(tǒng)的測量單元，磁強計作為外部輔助測量單元，其中加速度計測量值，磁強計測量值，陀螺儀測量值，采樣間隔時間為。

本發(fā)明以不同階次的龍格庫塔法求解捷聯(lián)慣導系統(tǒng)的機械編排方程，是指利用二階龍格庫塔法求解速度微分方程，得到鉆頭在地理坐標系下的速度，一階龍格庫塔法求解位置微分方程，得到鉆頭的位置信息，四階龍格庫塔法求解姿態(tài)矩陣微分方程，得到更新之后的四元數(shù)矩陣。

本發(fā)明通過零速校正算法消除捷聯(lián)慣導系統(tǒng)的累積誤差，是指滿足零速條件和地磁測量條件下，構建卡爾曼濾波器，以姿態(tài)角誤差和速度誤差作為觀測量，對狀態(tài)變量作出最優(yōu)估計，并以輸出校正的方式對姿態(tài)矩陣進行修正。

本發(fā)明捷聯(lián)慣導系統(tǒng)姿態(tài)解算方法為以下具體步驟：

步驟一：采集加速度計、磁強計、陀螺儀的數(shù)據。其中加速度計測量值，磁強計測量值，陀螺儀測量值，采樣間隔時間為。

步驟二：利用二階龍格庫塔法求解速度微分方程，速度微分方程如下所示：

進一步寫成矩陣形式為：

具體求解過程如下：假設初始速度，

其中：

如此遞推，得到更新后的地速。

步驟三：利用一階龍格庫塔法求解位置微分方程，其表示如下所示：

進一步寫成矩陣形式為：

求解過程如下：

可得更新后的位置矩陣，從而可求得經度表達式：，緯度表達式：以及深度表達式：

步驟四：利用四階龍格庫塔法求解四元數(shù)微分方程，該方程表示如下：

根據四元數(shù)乘法寫成相應矩陣形式：

具體求解過程如下：

設

可得到更新后的表示地理坐標系到載體坐標系的轉動四元數(shù)，之后經過歸一化，便完成了姿態(tài)矩陣的更新，同時可求出對應的姿態(tài)角：

本發(fā)明中的零速校正算法為以下具體步驟：

步驟一：判斷測斜儀是否滿足零速條件和地磁測量條件。所述的零速條件是根據加速度計輸出的幅值進行判斷，其公式如下：

其中：為加速度計測量噪聲方差；n為采樣點數(shù)；g為重力加速度模值。

所述的地磁條件是根據磁力計輸出的幅值進行判斷，公式如下：

其中：h為地磁場矢量模值。

步驟二：當檢測到系統(tǒng)滿足零速條件和地磁場測量條件后，根據捷聯(lián)慣導姿態(tài)解算算法步驟二中的姿態(tài)角計算公式求出當前的姿態(tài)角。捷聯(lián)慣導算法解算出的姿態(tài)角減去該姿態(tài)角作為卡爾曼濾波器的輸入，同時將速度誤差置零。如附圖2所示。

進一步，卡爾曼濾波器模型為：

選取的狀態(tài)變量為：

選取速度誤差和姿態(tài)角誤差作為觀測量：

狀態(tài)轉移矩陣為：

觀測矩陣為：

步驟三：通過輸出校正的方式獲得最優(yōu)姿態(tài)角，根據最優(yōu)姿態(tài)角重新計算姿態(tài)矩陣，完成姿態(tài)矩陣的校正。

實施例1

本發(fā)明提供的捷聯(lián)慣導解算及零速校正算法適用于各種類型的陀螺儀所構成的捷聯(lián)慣導系統(tǒng)，以mems陀螺儀與mems加速度計構成的捷聯(lián)慣導系統(tǒng)為實施例。

采用本發(fā)明的陀螺測斜儀的硬件電路結構圖如圖3所示，包括傳感器數(shù)據采集模塊、單片機最小系統(tǒng)、溫度檢測模塊、can通訊模塊、存儲模塊。其中，姿態(tài)傳感器均設定為每隔0.1s采集一次數(shù)據，其敏感軸的排列方式如圖4所示。將陀螺測斜儀固定于三軸轉臺上，完成轉臺姿態(tài)角設定后，系統(tǒng)上電運行并預熱五分鐘，五分鐘之后開始采集姿態(tài)傳感器數(shù)據，并通過can通訊模塊發(fā)送上傳的傳感器數(shù)據。根據圖1中的初始參數(shù)，在matlab中利用導出的傳感器數(shù)據和構建的卡爾曼濾波器進行姿態(tài)角的解算。

由圖5至圖10可以看出，將磁強計作為外部輔助測量裝置后，捷聯(lián)式慣導系統(tǒng)的導航精度大幅提高，漂移現(xiàn)象得到抑制。在15分鐘的仿真時間內，姿態(tài)角誤差基本都在2°以內，北向和東向速度誤差控制在2m/s以內，地向速度誤差控制在4m/s以內。仿真結果表明，采用imu/磁強計組合的測量系統(tǒng)并結合本文建立的卡爾曼濾波模型，可以有效抑制慣導系統(tǒng)的累積誤差，系統(tǒng)導航精度大幅改善，從而證明了該算法的正確性和有效性。