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

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

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

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

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

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

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

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

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

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

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

坐標系定義如下：

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

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

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

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

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

要解算的位置和姿態(tài)角即掘進機坐標系與地理坐標系之間的關系。

1.2.1 四元數(shù)法

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

Q=q0+q1i+q2j+q3k

(1)

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

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

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

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

θ=asin(2q0q1+2q2q3)

(5)

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

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

1.2.2 等效旋轉矢量法

旋轉矢量的微分方程近似為：

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

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

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

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

(13)

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

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

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

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

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

其中：

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

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

2 仿真與比較

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

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

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

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

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

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

φ=φ0sinw0t

θ=θ0sinw1t

γ=γ0sinw2t

(18)

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

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

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

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

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

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

2.3 位置解算算法仿真

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

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

圖4 模擬的位置變化軌跡

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

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

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

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

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

圖7 位置解算誤差

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

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

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

3.1 利用小車代替掘進機模擬實驗設計

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

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

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

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

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

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

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

圖9 實驗總體布置

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

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

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

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

圖11 位置解算

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

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

圖13 位置解算誤差

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

4 結 論

1)說明了應用捷聯(lián)慣導對懸臂式掘進機進行位姿解算的優(yōu)點，不受外界環(huán)境干擾、結構簡單，并能夠實現(xiàn)實時測量、自主測量。

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

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