張昊，葉宇煌

　　（福州大學 物理與信息工程學院，福建 福州 350116）

       摘要：測斜儀在石油勘探、地質勘查等方面已成為一種有力的工具，但基于磁通門的測斜儀存在著易受磁場干擾的缺點，導致其測量精度降低，甚至無法工作。為了解決這一問題，提出了一種基于捷聯慣性導航原理的連續測斜算法。算法利用微慣性傳感器MPU6050獲取姿態數據，采用四元數方法進行姿態解算，最終得到鉆探過程中的傾斜角、方位角以及工具面角等數據。通過仿真驗證，得到測量誤差曲線。仿真結果表明，該算法可以實現全方位井眼軌跡連續測量，提高了測量效率。

　　關鍵詞：測斜儀；捷聯慣性導航；四元數；姿態解算

　　中圖分類號：TP391.9文獻標識碼：ADOI： 10.19358/j.issn.1674-7720.2017.01.005

　　引用格式：張昊，葉宇煌. 基于微慣性傳感器的測斜儀姿態算法的研究［J］.微型機與應用，2017,36（1）：15-17.

0引言

　　隨著科學技術的發展，煤炭開采范圍也越來越大，復雜的地質環境使得安全生產問題日益嚴重。查明工作面的地質構造以及精確的工程鉆探，都需要精度高、穩定性好且能全方位測量鉆孔軌跡的測斜儀。目前存在的測斜儀主要有兩類：一類是基于磁通門的測斜儀，但這種測斜儀在鐵礦豐富的地區會受到極大的干擾，以至于無法工作；第二類采用機械式陀螺作為測量單元，雖然不受周圍環境的影響，但它只能進行靜態單點測量，測量時間長［13］。

　　本文將捷聯慣性導航應用在測斜儀中，利用測斜儀中的三軸陀螺儀和三軸加速度微慣性傳感器測得姿態數據，輸出的數據經過姿態解算，可直接輸出測斜儀的三個姿態角信息。捷聯慣性導航直接將慣性器件捆綁在載體之上，不依賴于外界信息，因此具有結構簡單、體積小、成本低、可靠性高等優點［4］。

1捷聯慣性導航原理

　　由于慣性傳感器是與載體捆綁在一起的，因此與載體一起運動，陀螺儀測得的角速度和加速度計測得的比力都是在載體坐標系當中的，因此只有將其中的角速度和比力信息轉換到導航坐標系中，才能夠實現對載體姿態的解算［5］，基本原理如圖1所示。所以姿態解算算法是捷聯慣性導航的關鍵技術，解算算法的好壞直接影響導航系統的精度。常見的姿態解算方法有以下4種［6］：

　　（1）歐拉角法，當俯仰角為90°時，歐拉微分方程存在著退化問題，不能全姿態工作，只適合應用在水平姿態變化不大的載體上。

　　（2）方向余弦法可以全姿態工作，但是需要解算9個微分方程，計算復雜。

　　（3）四元數算法只需要求解4個微分方程，計算簡便，但引入不可交換性誤差，一般不適合于激烈運動的運動載體的姿態解算。

　　（4）等效旋轉矢量法，通過多子樣算法彌補了四元數算法存在的不可交換性誤差的缺點，適用于高動態運動載體的姿態解算。子樣數越多，姿態解算的精度也就越高，同時計算復雜度也隨之增加。相比于四元數法的計算量增加了30%。

　　四元數法和等效旋轉矢量法都能夠全姿態工作且精度高，所以在工程中經常用到。雖然四元數法相對于等效旋轉矢量法存在不可交換性誤差，不能在激烈運動的載體上應用，但可以應用到低速運動的測斜儀中。

　　1.1坐標系定義

　　慣性坐標系記為i，原點位于地心，xi指向春分點，zi沿地球自轉軸，yi與xi、zi軸構成右手系。選取地理坐標系作為導航坐標系，記為n系，原點位于載體質心，zn分別指向所在地的東、北、天。載體坐標系記為b系，xb、yb、zb分別指向右、前、上。

　　1.2坐標變換

　　兩坐標系間任何復雜的角位置關系都可以看作有限次基本旋轉的復合，變換矩陣等于基本旋轉確定的變換矩陣的連乘，連乘順序依基本旋轉的先后次序由右向左排列。所以載體坐標系與導航坐標系之間的轉換可以用下面的矩陣表示：

　　其中φ為方位角（航向角），θ為井斜角（俯仰角），γ為工具面角（橫滾角），Cnb稱為載體的姿態矩陣。式中Cnb與旋轉次序有關，即當旋轉角φ、θ、γ不都為小角時，對應于不同的旋轉次序，坐標系n的最終空間位置是不同的，這就是常說的有限轉動的不可交換性。但當φ、θ、γ都為小角時，忽略小角間的高階小量,式（1）變為：

　　式中，φ、θ、γ的單位為弧度。此時由φ、θ、γ構成的列向量［φθγ］T可視為三維空間矢量，各分量正負號的規定為：當產生小角的旋轉方向與坐標軸指向相同時該小角取正，否則取負。此時旋轉后坐標系的最終角位置與旋轉次序無關，這就是常說的無限轉動與次序無關［7］。由于直角坐標系之間的變換矩陣為單位正交矩陣，所以如果在坐標系b至坐標系n的等效旋轉中各坐標系都保持為直角坐標系，則根據正交單位矩陣的性質有［8］：

　　Cnb=(Cbn)－1=(Cbn)T（3）

　　2四元數算法

　　四元數是由4個元構成的數：

　　q=q1+q2i+q3j+q4k（4）

　　式中，q1、q2、q3、q4是實數，i、j、k是相互正交的單位向量，又是虛單位－1。根據歐拉定理，空間中兩坐標系的相對位置，可以等效為動坐標系繞某一瞬軸u（單位向量）轉過角θ一次完成。u和θ可以用來表示一個四元數：

　　2.1四元數表示的姿態矩陣

　　式（5）描述了剛體的定點轉動，即當只關心b系相對于n系的角位置時，可認為b系是由n系經過無中間過程的一次性等效旋轉形成的，q包含了這種等效旋轉的全部信息：u為旋轉瞬軸和旋轉方向，θ為轉過的角度。因此可以確定出b系至n系的坐標變換矩陣為：

　　由式（1）與式（6）可以得出:

　　θ主=arcsin(T32)

　　γ主=arctan(－T31/T33)

　　φ主=arctan(T12/T22)（7）

　　其中方位角和工具面角的真值可以按照表1和表2進行確定。

　　2.2四元數的微分方程及求解

　　四元數的姿態更新微分方程表示如下：表1方位角的真值T11T21φ0+90°0-－90°++φ主-φ主-+φ主+180°--φ主－180°表2工具面角的真值T33γ主γ++-γ主-+γ主－180°--γ主+180°dqdt=12qωbnb（8）

　　式中ωbnb=ωxωyωzT。寫成矩陣形式：

　　其中，ωbnb可由下式計算得到：

　　ωbnb=ωbib－Cbn(ωnie+ωnen)（11）

　　式中，ωbib為三軸陀螺儀的輸出，表示載體坐標系相對于慣性坐標系的角速度； ωnie表示地球坐標系相對于慣性坐標系e在載體坐標系中的角速度；ωnen表示導航坐標系相對于慣性坐標系在導航坐標系下的角速度。

　　利用畢卡求解法對式（10）進行解算得：

　　3驗證

　　利用MATLAB軟件對以上算法進行仿真驗證。設計的方位角、井斜角和工具面角的變化規律如圖2所示，初始方位角為68.9°，初始井斜角為0°，初始工具面角為178°；采樣頻率為100 Hz。由于加入儀器誤差后使得誤差來源不明，所以本次仿真實驗默認慣性傳感器的輸出為理想狀態下的輸出。通過本文設計的算法對傳感器的輸出進行解算，最終結果如圖3所示。　　

　　由圖3可以看出方位角的理論解算誤差控制在±1°以內，井斜角誤差控制在±0.1°以內，工具面角誤差控制在±1°以內，具有良好的解算精度，證明本文提出的解算方法有效。

4結論

　　本文提出了一種新的基于捷聯慣性導航原理的連續測斜算法，并進行了模擬仿真。仿真結果表明該解算算法可以實現解算姿態的功能，在保證測量精度的同時，比之前的靜態測量縮短了測量時間，提高了測量的效率。但該算法在長時間測量的情況下可能會出現測量精度降低的情況，這可能是因為積分的累計誤差造成，有待進一步的研究。

參考文獻

　　［1］ 謝子殿, 朱秀. 基于磁通門與重力加速度傳感器的鉆井測斜儀［J］. 傳感器與微系統, 2004, 23(7):30-33.

　　［2］ NOY K A, LEONARD J G. A new rate gyroscopicwellbore survey system achieves the accuracy and operational flexibility needed for today's complex drilling challenges［C］.SPE/IADC Drilling Conference, Society of Petroleum Engineers, 1997.

　　［3］ 劉宇, 李秋俊, 劉俊. 基于壓電陀螺的快速方位測井短接［J］. 電子測量與儀器學報, 2007, 21(2):90-94.

　　［4］ 張春慧. 高精度捷聯式慣性導航系統算法研究［D］. 哈爾濱：哈爾濱工程大學, 2005.

　　［5］ 丁君. 基于微慣性傳感器的姿態算法研究［D］. 上海：上海交通大學, 2013.

　　［6］ 張露. 基于捷聯慣導的采煤機位姿解算算法研究［D］. 西安:西安科技大學, 2014.

　　［7］ 秦永元. 慣性導航［M］. 北京：科學出版社, 2014.

　　［8］ 彭勁松. 機載激光捷聯慣導優化算法研究［D］. 西安：西北工業大學, 2006.