基于重力四元數的陀螺漂移估計與補償*

楊金顯,楊 闖

(河南理工大學電氣工程與自動化學院導航制導實驗室,河南 焦作 454003)

基于重力四元數的陀螺漂移估計與補償*

楊金顯*,楊 闖

(河南理工大學電氣工程與自動化學院導航制導實驗室,河南 焦作 454003)

為提高鉆探中的鉆具姿態測量精度,提出一種基于重力四元數的MEMS慣性隨鉆姿態測量方法。采用MEMS慣性器件構建鉆具姿態測量系統,把加速度計數據解算的姿態四元數作為觀測四元數,陀螺儀數據解算的姿態四元數作為誤差四元數;然后將陀螺儀漂移融入誤差四元數,建立重力四元數估計陀螺儀誤差四元數的模型,采用最小二乘法估計陀螺儀三軸漂移,進而補償陀螺儀姿態四元數;通過補償后的姿態四元數解算出鉆具姿態。最后設計了轉臺、振動臺實驗和鉆進模擬實驗,實驗結果表明,姿態四元數補償后的井斜角和工具面角漂移由平均10 °/h減小到約0.2 °/h,方位角誤差由平均12 °/h減小到約0.46 °/h,實現了加速度計補償陀螺的三軸漂移,表明該方法能夠有效提高鉆具的姿態測量精度。

慣性隨鉆測量;陀螺漂移估計;重力四元數;姿態解算

近年來,基于MEMS加速度計/陀螺儀的微慣性姿態測量單元(MIMU)以其成本低、體積小、壽命長、集成化、抗沖擊能力強和可靠性高等優勢,不但在石油鉆井、地質勘探和非開挖領域得到廣泛應用,近幾年也逐步推廣到煤礦領域,如煤層氣(瓦斯)抽采、底板加固、注漿堵水的鉆孔等,這就要求實時對鉆具的姿態進行測量和監測,以保障鉆具的正常運行。采用MEMS慣性器件構造的慣性隨鉆測量單元MIMU,通過敏感的鉆具三軸加速度和角速度經過解算可得鉆具井斜角θ、工具面角φ、方位角ψ,陀螺儀作為MIMU的主要測量元件,但由于陀螺儀存在漂移,長時間累積計算會產生較大誤差,需要利用輔助傳感器對陀螺漂移進行估計和誤差補償。盡管國外的慣性隨鉆測量技術已趨于成熟,但嚴格的技術封鎖,因此進行相關研究具有重要意義。

陀螺儀傾角信息補償一般不是問題,可以靠MIMU中加速度計來完成,由于加速度計精度要高于陀螺儀,且無累計誤差,結合陀螺儀和加速度計解算的結果可以獲得較高精度傾角信息,這也是國內外加速度計在隨鉆測量中大量使用的原因[1-4]。但加速度計不能很好的獲得方位角信息,難以對陀螺進行方位角修正,而用磁強計輔助的方式進行方位修正,需建立精確地磁場模型和屏蔽鉆具本身的磁干擾以及外界磁干擾[2,4-7],考慮到隨鉆測量復雜的磁場環境磁強計輔助方式很難實施,因此補償陀螺方位漂移這個重擔還得落到加速度計身上。

由上面分析,若簡單通過加速度計解算鉆具角度補償陀螺漂移,僅能補償陀螺傾角信息而鉆具方位角補償卻無能為力。為實現加速度計對陀螺儀三軸漂移的補償,李啟光等對6只加速度計按回轉軸對稱布局建立加速度與角速度數學模型,并利用Kalman濾波對該解算角速度與陀螺儀的輸出進行融合補償陀螺儀漂移[8],雖實現對陀螺漂移全補償,但需要對加速度計嚴格對稱安裝,較難應用到隨鉆測量。Churchill提出了利用加速度計估計陀螺處于鉛垂方向時的漂移以修正航向角的方法[9],該方法需要確保旋轉是繞水平方向以保證修正精度,不適用于MIMU姿態角處于任意值的情形。顏翚等建立了陀螺漂移隨機游走模型,并用加速度計陀螺儀的低頻值作為觀測量,通過擴展卡爾曼濾波算法進行鉆具姿態的導航推算[10],在靜態下雖有一定效果,但強振動強沖擊條件下陀螺漂移模型不一定適用。吳哲明提出一種通過IMU旋轉實現加速度計估計陀螺漂移方法,建立了傾角誤差和陀螺漂移之間的數學模型[11],需要穩定旋轉機構驅動,在隨鉆測量強振動強沖擊環境很難實現。

由于加速度計精度要高于陀螺儀,且無累計誤差,鑒于此,為進一步解決加速度計難以估計陀螺儀三軸漂移,不通過加速度計求角度補償陀螺儀積分角度誤差,在不增加額外輔助部件情況下,根據四元數旋轉理論,建立重力四元數估計陀螺儀誤差四元數模型,采用最小二乘法估計陀螺儀三軸漂移,進而補償陀螺儀姿態四元數,通過補償后的姿態四元數解算出鉆具姿態。最后設計了轉臺實驗、振動實驗和鉆進模擬實驗,實驗結果表明,該方法能夠有效提高鉆具的姿態測量精度。

1 鉆具姿態四元數描述

假定地理坐標系作為參考坐標系,有n系oxnynzn(正向分別為東、北、天),鉆具坐標系b系oxbybzb。參考坐標系到鉆具坐標系的單位旋轉四元數記為q=[q1,q2,q3,q4]T,有如下關系;

(1)

通過四元數有關旋轉理論可將n系向b系的坐標旋轉變換過程用坐標變換矩陣表示為:

(2)

(3)

由式(3)求取當前鉆具姿態角,需要先獲取鉆具當前姿態四元數,鉆具姿態四元數可分別由加速度計、陀螺儀數據解算獲得,理想情況下鉆具姿態四元數=重力四元數=陀螺儀姿態四元數,陀螺儀四元數通過陀螺儀敏感鉆具三軸角速度來完成。由于陀螺儀漂移導致陀螺儀四元數存在累積誤差,加速度計精度要高于陀螺儀,且無累計誤差,但易受鉆具振動影響[13-14],因此為獲取較高精度鉆具姿態四元數,進而提取較高精度姿態角,需要融合重力四元數和陀螺儀四元數,鉆具姿態解算過程如圖1所示。

圖1 鉆具姿態角四元數解算過程

2 姿態四元數誤差補償算法

2.1 重力四元數

鉆具停止鉆進時加速度計測量的鉆具主要為重力加速度及測量噪聲。加速度計測量的鉆具重力向量記為a=[ax,ay,az]T;在參考系(地理系)中,歸一化的重力向量記為gn=[0,0,1]T,經式(2)坐標變換得鉆具坐標系下的重力向量gb為:

(4)

聯立式(1)、式(4),求取當前加速度姿態四元數qa(k+1),有[15]:

qa(k+1)=q(k+1)+qa0(k+1)

(5)

式中:qa0(k+1)為加速度計提取重力加速度過程中噪聲四元數,作為白噪聲四元數處理。q(k+1)為鉆具真實姿態四元數(k+1表示當前時刻,k表示上一時刻,k=0,1,2,…,下同)。

2.2 陀螺儀誤差四元數傳遞模型

(6)

式中:q表示當前鉆具真實姿態四元數,其一階龍格—庫塔計算式為:

(7)

T為角速度采樣周期。式(6)、式(7)為理想角速度分量下姿態四元數表示,實際角速度由與鉆具固連陀螺儀所測,陀螺儀存在漂移,其漂移模型簡記為[11]:

(8)

式中:ω為陀螺儀所測三軸角速度向量,ω0=[ω0x,ω0y,ω0z]T為陀螺漂移誤差向量。陀螺漂移是一個隨機漸變過程,在相鄰采樣間隔認為其不變[16]。式(7)基于陀螺所測角速度通過迭代加求取當前時刻鉆具姿態四元數,因陀螺漂移會有累計誤差,若不加以補償,短時內就會積累很大姿態四元數誤差,因此需對陀螺漂移估計并補償修正實時姿態四元數。

考慮式(8)陀螺儀角速度誤差模型,聯立式(6)、式(7)得當前時刻陀螺儀鉆具姿態四元數:

(9)

2.3 重力四元數估計陀螺漂移算法

當前時刻鉆具真實姿態四元數無法精確獲得,鑒于加速度計精度高于陀螺儀,以當前加速度姿態四元數觀測陀螺儀姿態四元數,由式(5)、式(9)可得重力四元數觀測陀螺儀四元數誤差最小二乘估計模型:

(10)

(11)

將式(11)陀螺漂移估計值代入式(9)結合當前陀螺儀姿態四元數,可得當前鉆具姿態四元數最佳估計值:

(12)

將代入(12)代入式(3)四元數計算鉆具姿態即得當前時刻鉆具最佳姿態估計。

基于上述原理,重力四元數估計陀螺漂算法流程如圖2所示。

圖2 重力四元數估計陀螺漂移流程圖

3 實驗設計及結果分析

3.1 轉臺實驗

圖3 MIMU轉臺實驗

圖4 轉臺實驗陀螺儀漂移補償前后姿態角誤差

觀察圖4可知,陀螺儀在較短時間內漂移累積誤差較小,長時間工作累計誤差較大,且工作時間越長,累計誤差越大,因此需要對陀螺漂移進行估計與補償。經過1h的姿態解算,加速度計連續補償陀螺漂移,井斜角誤差由9.35 °/h減小到0.17 °/h;工具面角誤差由9.35 °/h減小到0.19 °/h;方位角誤差由9.96 °/h減小到0.28 °/h,顯著減小了因陀螺漂移造成的姿態解算誤差,可見算法實現了加速度計對陀螺儀的三軸漂移估計與補償。同時,實驗發現當傾斜角小于3°或接近90°時,方位角變化引起的重力四元數變化較小,導致重力四元數觀測陀螺方位軸漂移能力降低,致使方位角誤差補償能力略有降低。

3.2 振動臺實驗

為驗證算法在鉆進振動情況下估計并補償陀螺漂移性能,設計了振動臺實驗,將設計的藍牙MIMU以傾斜角45°固定于振動臺,如圖5所示,采集3 600sMIMU信號,用本文方法實時估計并補償陀螺漂移,陀螺漂移補償前后姿態解算誤差隨時間變化情況如圖6所示。

圖5 MIMU振動臺實驗

觀察圖6發現,加速度計補償陀螺漂移前后井斜角誤差由11.14 °/h減小到0.24 °/h;工具面角誤差由10.09 °/h減小到0.42 °/h;方位角誤差由12.63 °/h減小到0.38 °/h,雖顯著減小因陀螺漂移造成的姿態解算累積誤差,但由于受振動影響,重力估計并補償陀螺漂移誤差會有所增大。

圖6 振動臺實驗陀螺儀漂移補償前后姿態角誤差

3.3 鉆進模擬實驗

為進一步驗證重力四元數估計陀螺漂移算法性能,設計了鉆進模擬實驗,將設計的藍牙無線MIMU系統通過夾具固定于鉆桿上部如圖7所示,并通過轉臺對失準角進行標定。鉆進過程存在振動沖擊,為提高鉆具姿態解算精度,硬件上采用高精度抗振MEMS加速度計和陀螺儀并做抗振處理(MIMU精度越高姿態結算精度也會越高);軟件處理方法上由于陀螺儀抗振性能優于加速度計(可從加速度計和陀螺儀的工作原理上分析得知),在振動較大時用陀螺解算鉆具姿態,振動較小時利用重力四元數估計并補償陀螺漂移,可充分利用連接鉆桿或人為控制停止鉆進期間重力四元數估計并補償陀螺漂移,進行重置姿態初始值。

圖7 模擬鉆進實驗

實驗開始前,先調整鉆機傾斜角為45°并記錄50s靜止姿態數據;然后以初始傾斜角/方位角直行鉆進,持續10 800s,每1 800s停止鉆進模擬安裝鉆桿過程,通過藍牙實時采集MIMU加速度、角速度及解算的姿態角數據,并在試驗結束后對采集的角速度數據進行離線姿態角解算。為便于對比陀螺漂移補償前后姿態角解算誤差,本次實驗僅對傾斜角和方位角解算誤差進行測試,將實驗開始前采集的多組傾斜角、方位角數據均值作為真值;將離線角速度解算的角度作為陀螺漂移補償前角度數據,MIMU模塊實時解算的角度為補償后的角度。

圖8 鉆進實驗陀螺儀漂移補償前后姿態角誤差

圖8為陀螺漂移補償前后傾斜角/方位角解算誤差對比結果,由圖8可知,重力四元數間歇補償陀螺漂移前后井斜角誤差由11.15 °/h減小到0.22 °/h以內;方位角誤差由10.18 °/h減小到0.46 °/h以內,顯著減小因陀螺漂移造成的姿態解算累積誤差。

4 結論

針對MIMU在慣性隨鉆測量中加速度計難以估計陀螺三軸漂移問題,本文通過四元數旋轉理論,利用加速度姿態四元數與陀螺儀姿態四元數關系,建立重力四元數估計陀螺儀誤差四元數模型,采用最小二乘法估計陀螺儀三軸漂移,進而補償鉆具姿態四元數,通過補償后的姿態四元數解算出鉆具姿態,利用連接鉆桿或人為控制停止鉆進期間進行重置姿態初始值。最后轉臺和振動臺實驗結果表明,陀螺漂移補償修正后姿態估計精度有了明顯提高,實現了加速度計估計陀螺儀三軸漂移,鉆具傾角及方位角精度有了較大提高。鉆進模擬實驗,陀螺漂移補償前后傾斜角誤差由11.15 °/h減小到0.22 °/h以內;方位角誤差由10.18 °/h減小到0.46 °/h以內,進一步說明了重力四元數間歇補償陀螺漂移算法的有效性,計算量小,表明該方法有效能夠提高鉆具的姿態測量精度。

[1]Jurkov A S,Cloutier J,Pecht E,et al. Experimental Feasibility of the In-Drilling Alignment Method for Inertial Navigation in Measurement while Drilling[J]. IEEE Transactions on Instrumentation and Measurement,2011,60(3):1080-1090.

[2]Wang Z,Poscente M,Filip D,et al. Rotary In-Drilling Alignment Using an Autonomous MEMS-Based Inertial Measurement Unit for Measurement while Drilling Processes[J]. IEEE Instrumentation and Measurement Magazine,2013,16(6):26-34.

[3]楊全進,徐寶昌,左信,等. 旋轉導向鉆具姿態的無跡卡爾曼濾波方法[J]. 石油學報,2013,34(4):1168-1175.

[4]Xue Qilong,Leung H,Wang Ruihe,et al. Continuous Real-Time Measurement of Drilling Trajectory with New State Space Models of Kalman Filter[J]. IEEE Transactions on Instrumentation and Measurement,2016,65(1):144-154.

[5]徐濤,溫東,孫曉磊. 基于加速度計和磁強計的方位測量與校正技術研究[J]. 儀器儀表學報,2009,30(10):2018-2022.

[6]Afzal M H,Renaudin V,Lachapelle G. Use of Earth’s Magnetic Field for Mitigating Gyroscope Errors Regardless of Magnetic Perturbation[J]. Sensors,2011,11(12):11390-414.

[7]高怡,汪躍龍,程為彬. 抗差自適應濾波的導向鉆具動態姿態測量方法[J]. 中國慣性技術學報,2016,24(4):437-442.

[8]李啟光,張海龍. 基于六加速度計的陀螺儀漂移補償算法的研究[J]. 傳感器與微系統,2009,28(12):42-44.

[9]Churchill D L. Inertial Measurement System with Self Corrertion[P]. No.8010308.USA,2011.

[10]顏翚,葛彤,楊柯,等. 水下攻泥器隨鉆姿態慣性測量方法[J]. 上海交通大學學報,2012,46(03):446-450,457.

[11]吳哲明,孫振國,張文增,等. 基于慣性測量單元旋轉的陀螺漂移估計和補償方法[J]. 清華大學學報(自然科學版),2014,54(9):1143-1147.

[12]吳濤,白茹,朱禮堯,等. 基于卡爾曼濾波的航姿參考系統設計[J]. 傳感技術學報,2016,29(4):531-535.

[13]Rohac J,Reinstein M,Draxler K. Data Processing of Inertial Sensors in Strong-Vibration Environment[C]//Intelligent Data Acquisition and Advanced Computing Systems(IDAACS),2011 IEEE 6th International Conference.IEEE,2011,1:71-75.

[14]Alam M,Rohac J. Adaptive Data Filtering of Inertial Sensors with Variable Bandwidth[J]. Sensors,2015,15(2):3282-3298.

[15]Sebastian O H Madgwick,Andrew J L Harrison,Ravi Vaidyanathan. Estimation of IMU and MARG Orientation Using Agradient Descent Algorithm[C]//IEEE International Conference on Rehabilitation Robotics. USA,2011:332-338.

[16]Mahony R,Euston M,Kim J,et al. A Non-Linear Observer for Attitude Estimation of a Fixed-Wing Unmanned Aerial Vehicle without GPS Measurements[J]. Transactions of the Institute of Measurement and Control,2011,33(6):699-717.

Gyro Drift Estimation and Compensation Based on Gravity Quaternion*

YANGJinxian*,YANGChuang

(Navigation and Guidance Laboratory,School of Electrical Engineering and Automation,Henan Polytechnic University,Jiaozuo He’nan 454003,China)

In order to decrease the attitude measurement error while drilling,an gravity quaternion estimating triaxial gyro drift-based(AQEGD)attitude measurement algorithm is proposed using an inertial sensor composed of a triaxial accelerometer and a triaxial gyroscope. The gravity quaternion based on accelerometer data is regarded as an observation vector,the gyro quaternion based on gyro data as an error vector blended in triaxial gyro drift;The triaxial gyro drift errors are sensed by the gravity quaternion using the linear least square estimation algorithm,by which the gyro quaternion used to calculate drilling tools dynamic attitude orientation is compensated;At last turntable experiment,shaking-table experiment and drilling experiment are conducted to verify the performance of the proposed algorithm in various dynamic condition settings and to provide further insight into the variations in the estimation accuracy. Furthermore,the experiments indicate that the proposed AQEGD can lower the triaxial gyro drift,with corresponding orientation average errors of deviation angle and tool face angle from 10 °/h(before compensation)to 0.2 °/h(after compensation),azimuth from 12 °/h to 0.46 °/h.

inertial measurement while drilling;gyro drift estimation;gravity quaternion;attitude algorithm

楊金顯(1980-)，男，山東曹縣人，博士，副教授，碩士生導師，1999～2008年于哈爾濱工程大學獲得學士、碩士和博士學位，主要從事MEMS慣性測量及在隨鉆、電網運動和變形監測中的應用研究，yangjinxian@hpu．edu．cn；楊 闖(1992-)，男，河南滑縣人，現為河南理工大學電氣工程與自動化學院碩士研究生，主要從事慣性測量研究，yang＿ch126@126．com。

項目來源:國家自然科學基金項目(41672363,U1404510,61440007);河南省科技攻關項目(172102210289);河南省創新型科技人才隊伍建設工程項目(CXTD2016054);河南省高校基本科研業務費專項資金項目(NSFRF1619);河南理工大學杰出青年基金項目(J2017-5)

2017-01-25 修改日期:2017-04-18

TH763

A

1004-1699(2017)08-1187-06

C:7210

10.3969/j.issn.1004-1699.2017.08.010