礦用陀螺測斜儀捷聯慣導系統姿態解算及零速校正方法與流程
2023-10-09 01:30:59 1
本發明涉及一種礦用定向鑽機隨鑽測量系統軌跡的校正方法，尤其是以姿態傳感器的測量數據作為輸入信息，通過不同階次的龍格庫塔法求解捷聯慣導系統的機械編排方程，實時輸出當前鑽頭姿態角信息和位置信息，並通過零速校正算法消除捷聯慣導系統的累積誤差的技術方案。
背景技術：
隨鑽測量技術是受控定向鑽進的關鍵技術之一。隨著我國煤礦綜合機械化採煤技術的發展，常採用定向鑽機鑽探出水平定向長鑽孔對煤層進行瓦斯預排及探放水活動。隨鑽測斜儀是定向鑽機進行定向鑽進的關鍵設備，它的主要作用是完成方位角、傾角、工具面角的測量，進而繪製出鑽孔軌跡。目前，礦用隨鑽測斜儀主要使用三軸加速度計和三軸磁阻計作為測斜傳感器，依據歐拉角法確定鑽頭姿態。雖然這種方法穩定可靠，但由於歐拉角法本身的局限性，勢必存在測量的奇異點，不能實現全角度測量；此外，磁阻計易受外部磁場幹擾，導致鑽孔的方位角產生測量誤差；該方法只能在更換鑽杆後的靜止狀態下測量，不能在鑽進的過程中實時測量。為此，陀螺測斜儀應運而生。陀螺儀與加速度計構成捷聯慣導系統的測量單元，通過逐次的積分運算可以實時獲得鑽頭的姿態、速度以及位置信息。但是，直接進行積分運算計算量較大,不易在單片機中實現，目前仍缺乏快速有效的數值解法來代替其中的積分運算。
陀螺儀存在漂移現象，即輸出誤差隨時間逐漸累積。慣導系統長時間運行必將導致累積誤差，因此在不斷探索如何提高傳感器本身的精度外，還需要尋求外部信息對捷聯慣導系統進行校正，形成組合導航系統。通常將慣導系統作為主導航系統，其他誤差不隨時間累積的導航系統如gps、無線電導航、地磁導航等作為外部輔助測量系統。由於煤壁對無線電吸收能力較強，且井下沒有gps信號，因此無線電導航技術與gps導航技術不適用於煤礦井下隨鑽測量。地磁導航技術需要已知當地的地磁資料庫，在實際生產中較難實現。因此急需一種穩定可靠的校正手段消除捷聯慣導系統的累積誤差。
技術實現要素：
本發明的任務是提供一種礦用陀螺測斜儀捷聯慣導系統姿態解算及零速校正方法。
具體技術方案如下。
一種礦用陀螺測斜儀捷聯慣導系統姿態解算及零速校正方法，所述方法採用的測斜儀包含有三種姿態傳感器：陀螺儀、加速度計和磁阻計；所述方法是以陀螺儀和加速度計作為捷聯慣導系統的測量單元，以姿態傳感器的測量數據作為輸入信息，通過不同階次的龍格庫塔法求解捷聯慣導系統的機械編排方程，實時輸出當前鑽頭姿態角信息和位置信息，並通過零速校正算法消除捷聯慣導系統的累積誤差。
附件技術方案一，所述捷聯慣導系統姿態解算的方法是以陀螺儀和加速度計作為捷聯慣導系統的測量單元，利用四階龍格庫塔法求解姿態矩陣微分方程，利用二階龍格庫塔法求解速度微分方程，一階龍格庫塔法求解位置微分方程。
附件技術方案二，所述零速校正方法的卡爾曼濾波過程需滿足兩個條件：零速條件和地磁條件；
所述零速條件是根據加速度計輸出的幅值進行判斷，其公式如下：
其中：為加速度計測量噪聲方差；n為採樣點數；g為重力加速度模值；
所述地磁條件是根據磁力計輸出的幅值進行判斷，公式如下：
其中：h為地磁場矢量模值。
本發明中的捷聯慣導姿態解算方法的核心思想是通過在積分區間內插值得到更新後的結果，與現有的直接積分運算方法相比，運算量小，一般的嵌入式單片機便可以完成。
本發明中的零速校正方法的卡爾曼濾波過程需滿足兩個條件：零速條件和地磁條件；其零速條件是根據加速度計輸出的幅值進行判斷，其地磁條件是根
據磁力計輸出的幅值進行判斷，零速校正方法與現有的gps組合導航、無線電組合導航等方法相比，充分結合了鑽機鑽進的工藝，即利用加裝鑽杆的間隙完成姿態角的校正。此外，磁強計體積小，抗振能力強，更適合於鑽杆鑽具的狹小空間。
附圖說明
圖1是本發明捷聯慣導解算初始參數表。
圖2是本發明捷聯慣導系統與磁強計組合的卡爾曼濾波算法示意圖。
圖3是本發明測斜儀的硬體電路結構圖。
圖4是本發明陀螺測斜儀中使用的三種姿態傳感器坐標系示意圖，滿足右手螺旋法則。
圖5是本發明經卡爾曼濾波後的傾角誤差。
圖6是本發明經卡爾曼濾波後的方位角誤差。
圖7是本發明經卡爾曼濾波後的工具面角誤差。
圖8是本發明經卡爾曼濾波後的北向誤差。
圖9是本發明經卡爾曼濾波後的東向誤差。
圖10是本發明經卡爾曼濾波後的地向誤差。
具體實施方式
下面作對本發明的具體方式出進一步的說明。
實施本發明上述所提供的一種適用於煤礦井下定向鑽機陀螺測斜儀姿態解算及零速校正方法，採用的測斜儀包含三種姿態傳感器、陀螺儀、加速度計和磁阻計，以陀螺儀和加速度計作為捷聯慣導系統的測量單元，通過不同階次的龍格庫塔法求解捷聯慣導系統的機械編排方程，實時輸出當前鑽頭姿態角信息和位置信息，同時為消除捷聯慣導系統長時工作所產生的累積誤差，以磁強計作為外部輔助測量裝置，通過零速校正算法實現多傳感器的組合導航，提高鑽孔軌跡測量的精確度。
本發明基於礦用陀螺測斜儀的捷聯慣導數值解算及零速校正方法是以陀螺儀和加速度計作為捷聯慣導系統的測量單元，磁強計作為外部輔助測量單元，其中加速度計測量值，磁強計測量值，陀螺儀測量值，採樣間隔時間為。
本發明以不同階次的龍格庫塔法求解捷聯慣導系統的機械編排方程，是指利用二階龍格庫塔法求解速度微分方程，得到鑽頭在地理坐標系下的速度，一階龍格庫塔法求解位置微分方程，得到鑽頭的位置信息，四階龍格庫塔法求解姿態矩陣微分方程，得到更新之後的四元數矩陣。
本發明通過零速校正算法消除捷聯慣導系統的累積誤差，是指滿足零速條件和地磁測量條件下，構建卡爾曼濾波器，以姿態角誤差和速度誤差作為觀測量，對狀態變量作出最優估計，並以輸出校正的方式對姿態矩陣進行修正。
本發明捷聯慣導系統姿態解算方法為以下具體步驟：
步驟一：採集加速度計、磁強計、陀螺儀的數據。其中加速度計測量值，磁強計測量值，陀螺儀測量值，採樣間隔時間為。
步驟二：利用二階龍格庫塔法求解速度微分方程，速度微分方程如下所示：
進一步寫成矩陣形式為：
具體求解過程如下：假設初始速度，
其中：
如此遞推，得到更新後的地速。
步驟三：利用一階龍格庫塔法求解位置微分方程，其表示如下所示：
進一步寫成矩陣形式為：
求解過程如下：
可得更新後的位置矩陣，從而可求得經度表達式：，緯度表達式：以及深度表達式：
步驟四：利用四階龍格庫塔法求解四元數微分方程，該方程表示如下：
根據四元數乘法寫成相應矩陣形式：
具體求解過程如下：
設
可得到更新後的表示地理坐標繫到載體坐標系的轉動四元數，之後經過歸一化，便完成了姿態矩陣的更新，同時可求出對應的姿態角：
本發明中的零速校正算法為以下具體步驟：
步驟一：判斷測斜儀是否滿足零速條件和地磁測量條件。所述的零速條件是根據加速度計輸出的幅值進行判斷，其公式如下：
其中：為加速度計測量噪聲方差；n為採樣點數；g為重力加速度模值。
所述的地磁條件是根據磁力計輸出的幅值進行判斷，公式如下：
其中：h為地磁場矢量模值。
步驟二：當檢測到系統滿足零速條件和地磁場測量條件後，根據捷聯慣導姿態解算算法步驟二中的姿態角計算公式求出當前的姿態角。捷聯慣導算法解算出的姿態角減去該姿態角作為卡爾曼濾波器的輸入，同時將速度誤差置零。如附圖2所示。
進一步，卡爾曼濾波器模型為：
選取的狀態變量為：
選取速度誤差和姿態角誤差作為觀測量：
狀態轉移矩陣為：
觀測矩陣為：
步驟三：通過輸出校正的方式獲得最優姿態角，根據最優姿態角重新計算姿態矩陣，完成姿態矩陣的校正。
實施例1
本發明提供的捷聯慣導解算及零速校正算法適用於各種類型的陀螺儀所構成的捷聯慣導系統，以mems陀螺儀與mems加速度計構成的捷聯慣導系統為實施例。
採用本發明的陀螺測斜儀的硬體電路結構圖如圖3所示，包括傳感器數據採集模塊、單片機最小系統、溫度檢測模塊、can通訊模塊、存儲模塊。其中，姿態傳感器均設定為每隔0.1s採集一次數據，其敏感軸的排列方式如圖4所示。將陀螺測斜儀固定於三軸轉臺上，完成轉臺姿態角設定後，系統上電運行並預熱五分鐘，五分鐘之後開始採集姿態傳感器數據，並通過can通訊模塊發送上傳的傳感器數據。根據圖1中的初始參數，在matlab中利用導出的傳感器數據和構建的卡爾曼濾波器進行姿態角的解算。
由圖5至圖10可以看出，將磁強計作為外部輔助測量裝置後，捷聯式慣導系統的導航精度大幅提高，漂移現象得到抑制。在15分鐘的仿真時間內，姿態角誤差基本都在2°以內，北向和東向速度誤差控制在2m/s以內，地向速度誤差控制在4m/s以內。仿真結果表明，採用imu/磁強計組合的測量系統並結合本文建立的卡爾曼濾波模型，可以有效抑制慣導系統的累積誤差，系統導航精度大幅改善，從而證明了該算法的正確性和有效性。