楊金顯,楊 闖

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

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

楊金顯*,楊 闖

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

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

慣性隨鉆測量;陀螺漂移估計;重力四元數(shù);姿態(tài)解算

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

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

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

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

1 鉆具姿態(tài)四元數(shù)描述

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

(1)

通過四元數(shù)有關(guān)旋轉(zhuǎn)理論可將n系向b系的坐標(biāo)旋轉(zhuǎn)變換過程用坐標(biāo)變換矩陣表示為:

(2)

(3)

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

圖1 鉆具姿態(tài)角四元數(shù)解算過程

2 姿態(tài)四元數(shù)誤差補償算法

2.1 重力四元數(shù)

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

(4)

聯(lián)立式(1)、式(4),求取當(dāng)前加速度姿態(tài)四元數(shù)qa(k+1),有[15]:

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

(5)

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

2.2 陀螺儀誤差四元數(shù)傳遞模型

(6)

式中:q表示當(dāng)前鉆具真實姿態(tài)四元數(shù),其一階龍格—庫塔計算式為:

(7)

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

(8)

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

考慮式(8)陀螺儀角速度誤差模型,聯(lián)立式(6)、式(7)得當(dāng)前時刻陀螺儀鉆具姿態(tài)四元數(shù):

(9)

2.3 重力四元數(shù)估計陀螺漂移算法

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

(10)

(11)

將式(11)陀螺漂移估計值代入式(9)結(jié)合當(dāng)前陀螺儀姿態(tài)四元數(shù),可得當(dāng)前鉆具姿態(tài)四元數(shù)最佳估計值:

(12)

將代入(12)代入式(3)四元數(shù)計算鉆具姿態(tài)即得當(dāng)前時刻鉆具最佳姿態(tài)估計。

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

圖2 重力四元數(shù)估計陀螺漂移流程圖

3 實驗設(shè)計及結(jié)果分析

3.1 轉(zhuǎn)臺實驗

圖3 MIMU轉(zhuǎn)臺實驗

圖4 轉(zhuǎn)臺實驗陀螺儀漂移補償前后姿態(tài)角誤差

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

3.2 振動臺實驗

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

圖5 MIMU振動臺實驗

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

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

3.3 鉆進模擬實驗

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

圖7 模擬鉆進實驗

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

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

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

4 結(jié)論

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

[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]楊全進,徐寶昌,左信,等. 旋轉(zhuǎn)導(dǎo)向鉆具姿態(tài)的無跡卡爾曼濾波方法[J]. 石油學(xué)報,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]徐濤,溫東,孫曉磊. 基于加速度計和磁強計的方位測量與校正技術(shù)研究[J]. 儀器儀表學(xué)報,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]高怡,汪躍龍,程為彬. 抗差自適應(yīng)濾波的導(dǎo)向鉆具動態(tài)姿態(tài)測量方法[J]. 中國慣性技術(shù)學(xué)報,2016,24(4):437-442.

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

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

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

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

[12]吳濤,白茹,朱禮堯,等. 基于卡爾曼濾波的航姿參考系統(tǒng)設(shè)計[J]. 傳感技術(shù)學(xué)報,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-)，男，山東曹縣人，博士，副教授，碩士生導(dǎo)師，1999～2008年于哈爾濱工程大學(xué)獲得學(xué)士、碩士和博士學(xué)位，主要從事MEMS慣性測量及在隨鉆、電網(wǎng)運動和變形監(jiān)測中的應(yīng)用研究，yangjinxian@hpu．edu．cn；楊 闖(1992-)，男，河南滑縣人，現(xiàn)為河南理工大學(xué)電氣工程與自動化學(xué)院碩士研究生，主要從事慣性測量研究，yang＿ch126@126．com。

項目來源:國家自然科學(xué)基金項目(41672363,U1404510,61440007);河南省科技攻關(guān)項目(172102210289);河南省創(chuàng)新型科技人才隊伍建設(shè)工程項目(CXTD2016054);河南省高校基本科研業(yè)務(wù)費專項資金項目(NSFRF1619);河南理工大學(xué)杰出青年基金項目(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