基于捷聯(lián)慣導(dǎo)的懸臂式掘進(jìn)機(jī)位姿解算算法研究

鄭偉雄，沈 陽，馬浚清，胡立同，符世琛，吳 淼

(1.中國礦業(yè)大學(xué)(北京) 機(jī)電與信息工程學(xué)院，北京 100083；2.棗莊礦業(yè)(集團(tuán))付村煤業(yè)有限公司，山東 濟(jì)寧 277605)

隨著我國煤炭開采的不斷深入，煤炭開采的深度和難度越來越大，煤炭生產(chǎn)安全形勢日益嚴(yán)峻，實現(xiàn)無人采掘技術(shù)、智能采掘技術(shù)有重要意義，而掘進(jìn)機(jī)位姿的自動檢測又是實現(xiàn)采掘無人化的基礎(chǔ)[1]。目前已經(jīng)有許多掘進(jìn)機(jī)位姿檢測的技術(shù)，如基于激光指向器、全站儀、iGPS、UWB的檢測技術(shù)等，這些檢測方法在某些方面仍有不足[2，3]。激光指向器法只能達(dá)到定向功能，不能精確檢測掘進(jìn)機(jī)的位姿；全站儀在大量粉塵并且照明差的井下環(huán)境中，測量結(jié)果可能不準(zhǔn)確；iGPS技術(shù)由于技術(shù)限制了測量距離只能短距離測量；基于UWB的位姿檢測技術(shù)在Z軸上的誤差較大，不能滿足使用要求。

捷聯(lián)慣導(dǎo)技術(shù)是一種不受外界干擾、結(jié)構(gòu)簡單、測量范圍廣、精度高的導(dǎo)航技術(shù)，并能夠?qū)崿F(xiàn)實時測量、自主測量。捷聯(lián)慣導(dǎo)技術(shù)的位姿解算算法決定著計算的復(fù)雜性和解算結(jié)果的精度，而捷聯(lián)慣導(dǎo)系統(tǒng)的位姿解算算法包括姿態(tài)和位置解算算法兩個部分。國內(nèi)外研究學(xué)者對位姿解算算法進(jìn)行了深入的推導(dǎo)和研究，1971年Bortz[4]提出了等效旋轉(zhuǎn)矢量法，之后，Robin[5]對等效旋轉(zhuǎn)矢量法的單子樣、雙子樣和三子樣算法進(jìn)行了分析比較，對該領(lǐng)域做出了巨大的貢獻(xiàn)。吳簡彤等從計算精度上對歐拉角法、四元數(shù)法、方向余弦法和等效旋轉(zhuǎn)矢量法進(jìn)行了分析比較，四元數(shù)法和等效旋轉(zhuǎn)矢量法精度更高且姿態(tài)更新周期較小，優(yōu)于其它兩種算法[6]。陶云飛[7]分析了基于捷聯(lián)慣導(dǎo)的掘進(jìn)機(jī)位姿檢測的可行性，但并未進(jìn)行深入研究。

本文對四元數(shù)法和等效旋轉(zhuǎn)矢量法進(jìn)行了分析和對比，在正弦運動和模擬掘進(jìn)機(jī)行走兩種情況下進(jìn)行了仿真，并搭建了模擬實驗平臺，利用小車循跡代替掘進(jìn)機(jī)掘進(jìn)進(jìn)行了實驗。

1 捷聯(lián)慣導(dǎo)位姿檢測原理和解算算法

1.1 捷聯(lián)慣導(dǎo)位姿檢測原理

捷聯(lián)慣導(dǎo)系統(tǒng)需要計算機(jī)定時獲取慣性傳感器的加速度和角速度信息，姿態(tài)解算算法根據(jù)陀螺儀測出的離散角速度信息解算出姿態(tài)角，位置解算算法根據(jù)加速度傳感器測得的離散加速度信息解算出位置信息。陀螺儀測得的原始數(shù)據(jù)中包含地球自轉(zhuǎn)角速度，加速度計測得的加速度包含載體當(dāng)?shù)氐闹亓铀俣龋萋?lián)慣性導(dǎo)航的自校準(zhǔn)就是根據(jù)這兩者進(jìn)行的。位姿解算算法的作用一是將測得的離散運動信息進(jìn)行平滑的、更接近實際的積分來獲得位姿信息，二是將由于慣性傳感器的離散性導(dǎo)致的不可測得的運動信息通過增加積分項進(jìn)行補(bǔ)償。

捷聯(lián)慣導(dǎo)算法的簡要流程如圖1所示。

圖1 捷聯(lián)慣導(dǎo)算法簡要解算流程

1.2 捷聯(lián)慣導(dǎo)姿態(tài)解算算法

坐標(biāo)系定義如下：

1)懸臂式掘進(jìn)機(jī)坐標(biāo)系(簡稱B系-ObXbYbZb)：與懸臂式掘進(jìn)機(jī)固連，原點在懸臂式掘進(jìn)機(jī)的中心，Xb指向懸臂式掘進(jìn)機(jī)的右側(cè)，Yb指向懸臂式掘進(jìn)機(jī)的前進(jìn)方向，Zb指向懸臂式掘進(jìn)機(jī)的豎軸方向。

2)導(dǎo)航坐標(biāo)系(簡稱N系-OnXnYnZn)：根據(jù)慣導(dǎo)系統(tǒng)的實際工作需要選取定義的坐標(biāo)系，用來作為掘進(jìn)機(jī)導(dǎo)航的基準(zhǔn)。在接下來的仿真解算中直接選用地理坐標(biāo)系作為此坐標(biāo)系，兩個坐標(biāo)系之間沒有角度差。

3)地理坐標(biāo)系(簡稱T系-OtXtYtZt)：是在懸臂式掘進(jìn)機(jī)上用來表示掘進(jìn)機(jī)在東北天三個方向位置的坐標(biāo)系。原點Ot選在懸臂式掘進(jìn)機(jī)重心處，東向為Xt軸，北向為Yt軸，Zt沿重力方向指向天。

4)地球坐標(biāo)系(簡稱E系-OeXeYeZe)：此坐標(biāo)系Oe在地球中心點處，它Ze軸軸沿地軸北極方向，與I 系之間存在一個地球自轉(zhuǎn)角速率ωe，Xe軸指向本初子午線(0°經(jīng)線)，Ye軸垂直于XeOeZe平面指向東經(jīng) 90°，固定在地球上。

5)地心慣性坐標(biāo)系(簡稱I系-OiXiYiZi)：此坐標(biāo)系的原點Oi同樣在地球中心點處，Zi軸重合于Ze軸，而Xi軸由原點指向春分點，與慣性空間相固連。

要解算的位置和姿態(tài)角即掘進(jìn)機(jī)坐標(biāo)系與地理坐標(biāo)系之間的關(guān)系。

1.2.1 四元數(shù)法

四元數(shù)由一個實數(shù)部分和三個虛數(shù)部分組成，它將載體的三維姿態(tài)表示為載體繞固定點的旋轉(zhuǎn)，四元數(shù)的形式為：

Q=q0+q1i+q2j+q3k

(1)

如果一個四元數(shù)Q1中的實數(shù)q0為零，則稱作純四元數(shù)，它可以用來表示向量，掘進(jìn)機(jī)的姿態(tài)可以由一個三維向量表示。如果另一個四元數(shù)Q2中實數(shù)q0不為0，則可以來表示繞一個等效軸的旋轉(zhuǎn)。如果Q1中的向量轉(zhuǎn)換Q2中的角度，則轉(zhuǎn)換之后的向量可以用四元數(shù)表示為：

姿態(tài)矩陣可以用四元數(shù)表示為：

姿態(tài)矩陣由姿態(tài)角表示為：

式中γ、θ、φ分別為懸臂式掘進(jìn)機(jī)的滾轉(zhuǎn)角、俯仰角和航向角。利用式(3)和式(4)中對應(yīng)元素相等，可得姿態(tài)角與四元數(shù)的關(guān)系如下：

θ=asin(2q0q1+2q2q3)

(5)

四元數(shù)更新微分方程如下：

式中，t為時間，T更為姿態(tài)角更新周期，不難看出四元數(shù)法的求解過程較為簡單，計算量較小，所以適合對時間要求苛刻的場合。

1.2.2 等效旋轉(zhuǎn)矢量法

旋轉(zhuǎn)矢量的微分方程近似為：

本文中對旋轉(zhuǎn)矢量法的解算采用三子樣算法，公式為：

姿態(tài)變化的四元數(shù)為：

姿態(tài)四元數(shù)的更新為：

Q(t+T)=Q(t)?q(T)

(13)

1.3 捷聯(lián)慣導(dǎo)位置解算算法

由于目前基于捷聯(lián)慣導(dǎo)的位置解算算法尚未發(fā)展成熟，位置解算誤差隨時間發(fā)散較快，因此本次只選取了改進(jìn)的特定力積分算法[8]，對誤差發(fā)散速度進(jìn)行仿真和實驗。

位置解算需要求出懸臂式掘進(jìn)機(jī)在導(dǎo)航坐標(biāo)系的x、y、z三個方向的值，捷聯(lián)慣導(dǎo)解算位置需要先解算出速度。當(dāng)T=tm-tm-1時，捷聯(lián)慣導(dǎo)系統(tǒng)的速度更新方程為：

上式右邊的第三項是重力速度增量，它很小并且在較小的時間周期內(nèi)變化緩慢，可以用線性插值法計算。上式右邊第二項為比力積分項，Vn的精度取決于此項：

利用特定力積分法，上式可以近似為：

其中：

式中，ΔVrotm是旋轉(zhuǎn)運動的補(bǔ)償項，ΔVsculm是劃船運動誤差的補(bǔ)償項。劃船運動誤差是剛體同時承受同一頻率的線振動和角振動產(chǎn)生的一種校正誤差。

在計算得到速度之后，則可以開始位置的更新。位移可以用兩時刻速度的平均值近似表示為：

2 仿真與比較

2.1 姿態(tài)解算算法誤差分析

由于目前單基于捷聯(lián)慣導(dǎo)的位置解算誤差較大而且發(fā)散較快，與井下掘進(jìn)機(jī)位姿檢測的要求相差甚遠(yuǎn)，所以這里不再分析位置的解算誤差。捷聯(lián)慣導(dǎo)的解算算法誤差是指數(shù)值求解姿態(tài)矩陣和比力方程時產(chǎn)生的原理性誤差，它包含不可交換性誤差、量化誤差、舍入誤差、積分誤差、正交化誤差和漂移誤差[6]。在這些誤差中，正交性誤差可以通過正交化來很好的解決，計算誤差、漂移誤差、量化誤差和舍入誤差在高性能計算機(jī)上對結(jié)果影響較小，因此對結(jié)果影響較大的主要有截斷誤差和不可交換性誤差。

等效旋轉(zhuǎn)矢量算法把剛體在一段時間內(nèi)的轉(zhuǎn)動等效為繞某個定軸矢量的旋轉(zhuǎn)，對不可交換性誤差抑制的效果較好，因此對測量物體的震動有較強(qiáng)的抗干擾性，但對運動信息的測量速度有較高要求，采樣頻率在很大程度上決定著這種算法的解算精度，增加了此算法的計算量，導(dǎo)致解算速度和精度不能兼得。等效旋轉(zhuǎn)矢量三子樣算法的截斷誤差是運動角速度與更新周期乘積wT的七階，是姿態(tài)角φ的二階[8]，而四元數(shù)法的截斷誤差是w的五階，與姿態(tài)角無太大關(guān)系[9]，從這一點來看四元數(shù)法優(yōu)于等效旋轉(zhuǎn)矢量法，并且四元數(shù)法只需要求解四個方程，計算量小，計算精度較高，微分方程的解算可以應(yīng)用數(shù)值微分解法，變成簡單的方程，比如四階龍格庫塔算法[10-13]。四元數(shù)法不足的是因解算過程是直接對角速度矢量進(jìn)行積分，因此不可交換性誤差較大。

2.2 姿態(tài)角解算算法仿真

對姿態(tài)角的解算算法在有干擾正弦運動情況下進(jìn)行仿真比較，并對位置的解算算法在模擬掘進(jìn)機(jī)行走時的誤差發(fā)散情況進(jìn)行仿真[14，15]。

設(shè)定姿態(tài)角的運動函數(shù)為：

φ=φ0sinw0t

θ=θ0sinw1t

γ=γ0sinw2t

(18)

式中，φ、θ、γ分別為懸臂式掘進(jìn)機(jī)的航向角、俯仰角和滾轉(zhuǎn)角；φ0、θ0、γ0分別為懸臂式掘進(jìn)機(jī)三個姿態(tài)角的運動幅度，都設(shè)為5°，w0、w1、w2為三個姿態(tài)角正弦運動的周期，分別設(shè)為2π/100、3π/100、4π/100，采樣周期設(shè)為T=0.01s。

設(shè)白噪聲e1、e2、e3隨均值為0，方差為0.01的正態(tài)分布，將白噪聲加入模擬得到的測量數(shù)據(jù)之中，此時可得陀螺儀的實時輸出函數(shù)為：

對誤差求解，四元數(shù)法姿態(tài)角誤差如圖2所示，等效旋轉(zhuǎn)矢量法姿態(tài)角誤差如圖3所示。

圖2 四元數(shù)法姿態(tài)角誤差

圖3 等效旋轉(zhuǎn)矢量法姿態(tài)角誤差

從仿真結(jié)果中可以看出兩種算法解算精度都在0.1°以內(nèi)，小于國家煤礦井巷工程驗收標(biāo)準(zhǔn)中的7.13°，兩種算法均能滿足要求。等效旋轉(zhuǎn)矢量法的誤差與運動狀態(tài)有較大關(guān)系，四元數(shù)法的誤差沒有明顯的規(guī)律。四元數(shù)法的誤差略小于等效旋轉(zhuǎn)矢量法，所以認(rèn)為四元數(shù)法略好一些。

2.3 位置解算算法仿真

本次位置解算算法仿真主要是通過模擬掘進(jìn)機(jī)行走的路線，將模擬得到的數(shù)據(jù)進(jìn)行位姿解算。這樣不僅可以排除測量元件誤差的影響，還可以驗證算法的準(zhǔn)確性和優(yōu)越性。在進(jìn)行位置解算時，需要實時參考掘進(jìn)機(jī)的姿態(tài)矩陣，以便將方向變化之后的加速度計測得的數(shù)據(jù)進(jìn)行坐標(biāo)變換，在此對姿態(tài)角的求解算法采用四元數(shù)法中的四階龍格塔庫法。

掘進(jìn)機(jī)工作時的基本運動形式有靜止、加速、勻速、轉(zhuǎn)彎、減速、上坡下坡等。模擬的掘進(jìn)機(jī)行走軌跡如圖4和圖5所示。

圖4 模擬的位置變化軌跡

圖5 模擬的姿態(tài)角變化軌跡

軌跡模擬中的中的橫滾角未發(fā)生變化。對姿態(tài)角運動函數(shù)求一階導(dǎo)數(shù)得角速度函數(shù)，可作為陀螺儀的輸入數(shù)據(jù)；對位置運動函數(shù)求求二階導(dǎo)數(shù)可得加速度函數(shù)，但加速度計測得的為比力，與實際的求導(dǎo)得到加速度含義不同，根據(jù)比力與角速度、速度和加速度三者之間的關(guān)系：

由上式可得加速度計的理想測量數(shù)據(jù)即比力信息。

姿態(tài)角和位置的求解誤差分別如圖6、圖7所示。

圖6 姿態(tài)角解算誤差

圖7 位置解算誤差

從仿真中可以看出，姿態(tài)角誤差在0.5°以內(nèi)，小于國家煤礦井巷工程驗收標(biāo)準(zhǔn)中的7.13°，滿足要求。從圖中還可以看出姿態(tài)角運動的幅度越大，解算誤差越大，所以當(dāng)運動角度較大時，四元數(shù)法的誤差與運動形勢也有一定關(guān)系。位置誤差較大且呈發(fā)散狀態(tài)，400s時z軸誤差達(dá)到20.0586m，遠(yuǎn)遠(yuǎn)不能滿足國家煤礦井巷工程驗收標(biāo)準(zhǔn)中的250mm的要求[10]，需其他裝置輔助校準(zhǔn)。

3 捷聯(lián)慣導(dǎo)位姿檢測模擬實驗

本章通過搭建模擬實驗臺，利用小車代替掘進(jìn)機(jī)行走，通過小車上的單片機(jī)和慣性傳感器將運動數(shù)據(jù)傳送到上位機(jī)進(jìn)行位姿解算，驗證選用的算法的可行性。

3.1 利用小車代替掘進(jìn)機(jī)模擬實驗設(shè)計

由于條件限制，利用小車模擬懸臂式掘進(jìn)機(jī)，規(guī)劃出一條比較符合實際的路線，用小車的循跡功能代替掘進(jìn)機(jī)的掘進(jìn)行走，利用小車上的慣性傳感器測量運動數(shù)據(jù)，設(shè)置傳感器的采樣頻率為100Hz。數(shù)據(jù)傳送到計算機(jī)并用MATLAB進(jìn)行解算。將解算得到的位姿軌跡與實際的軌跡相比較，計算出誤差。

規(guī)劃的軌跡如圖8所示。

圖8 小車軌跡路線圖(mm)

小車的運動為三維運動，上圖所示軌跡中，左邊第一個坐標(biāo)系為XOY水平面直角坐標(biāo)系，即從O1到O2，所有的運動都為水平面上的運動；第二個坐標(biāo)系為豎直面直角坐標(biāo)系，即O2之后為爬坡運動。

本次實驗選擇的慣性傳感器的是MPU6050，它可以同時測量角速度和加速度，小巧輕便，單片機(jī)選擇Stm32f407GT，利用單片機(jī)定時器的定時功能，每10ms產(chǎn)生一次中斷，在中斷程序里讀取慣性傳感器的測量結(jié)果。

3.2 傳感器數(shù)據(jù)采集

將集成有慣性測量元件的開發(fā)板安裝到小車上部，利用小車的循跡功能產(chǎn)生測量元件的運動數(shù)據(jù)，將開發(fā)板通過數(shù)據(jù)線與上位機(jī)相連，波特率設(shè)置為500kHz，單片機(jī)通過定時中斷程序采集慣性原件的測量數(shù)據(jù)并發(fā)送到上位機(jī)。實驗總體布置如圖9所示。

圖9 實驗總體布置

照片為俯視拍得，小車上面固定有開發(fā)板，開發(fā)板與計算機(jī)有一條白色的數(shù)據(jù)線連接。

3.3 實驗數(shù)據(jù)解算

姿態(tài)角解算結(jié)果如圖10所示，位置解算結(jié)果如圖11所示。

圖10 姿態(tài)角解算

圖11 位置解算

由解算結(jié)果可以看出，姿態(tài)角的解算存在較大的波動，這大部分是尋跡小車循跡過程中的車身波動導(dǎo)致的；位置解算結(jié)果呈發(fā)散狀態(tài)。對姿態(tài)角和位置計算誤差分別如圖12、圖13所示。

圖12 姿態(tài)角解算誤差

圖13 位置解算誤差

橫滾角和俯仰角的誤差都在5°以內(nèi)，滿足要求。航向角誤差存在較大的波動，但大致趨與0°誤差線，誤差波動可歸因于小車循跡過程中前進(jìn)方向的調(diào)整導(dǎo)致的航向角方向的波動。而且由于選擇的慣性傳感器本身誤差較大(高精度的慣性傳感器成本非常高昂，由于實驗室條件限制，本文采用了低成本的慣性傳感器)，是模擬實驗誤差的一個重要原因。位置誤差呈發(fā)散狀態(tài)，16s時y軸位置誤差達(dá)到了18m，遠(yuǎn)遠(yuǎn)不能滿足國家煤礦井巷工程驗收標(biāo)準(zhǔn)中的250mm的要求[9]，且發(fā)散速度較快。

4 結(jié) 論

1)說明了應(yīng)用捷聯(lián)慣導(dǎo)對懸臂式掘進(jìn)機(jī)進(jìn)行位姿解算的優(yōu)點，不受外界環(huán)境干擾、結(jié)構(gòu)簡單，并能夠?qū)崿F(xiàn)實時測量、自主測量。

2)通過仿真對比，表明兩種解算方法解算精度都在0.1°以內(nèi)，四元數(shù)解算方法中的姿態(tài)角解算誤差小于0.05°，略好于等效旋轉(zhuǎn)矢量法，小于國家煤礦井巷工程驗收標(biāo)準(zhǔn)中的7.13°，適合進(jìn)行位姿解算。位置誤差較大且呈發(fā)散狀態(tài)，400s時z軸誤差達(dá)到20.06m，且在模擬實驗中y軸位置誤差在16s時達(dá)到了18m，遠(yuǎn)遠(yuǎn)不能滿足國家煤礦井巷工程驗收標(biāo)準(zhǔn)中的250mm的要求，且發(fā)散速度較快，需其他裝置輔助校準(zhǔn)。

3)根據(jù)實驗結(jié)果姿態(tài)角的解算誤差小于5°，小于國家煤礦井巷工程驗收標(biāo)準(zhǔn)中的7.13°，滿足要求，驗證了選用的姿態(tài)角解算算法的可行性和有效性。