萬航雁，　張求知，　張伊慧，　張勁峰，　劉世良，　張寶尚，　陳　君

(1.光電控制技術重點實驗室，河南 洛陽　471000;　2.中國航空工業(yè)集團公司洛陽電光設備研究所，河南 洛陽　471000)

0　引言

空間姿態(tài)測量技術是針對空間兩個相對運動物體，將其中一個作為觀察者，另一個作為目標，測量兩者在運動過程中的相對位姿關系的過程。應用于航空領域的頭盔空間姿態(tài)測量技術，可以在飛機運動、頭部擺動的情況下，準確獲得頭部相對于飛機的姿態(tài)信息。主要可以分為以下幾種[1]：機械連桿法是最早的實現頭盔位姿測量的方法，通過在盔體上固連一套機械連桿系統(tǒng)，在理論上實現了頭盔位姿的測量，但是在一定程度上限制了飛行員的頭部運動，影響緊急逃生安全；電磁場法是通過計算固連在盔體上接收信號的三軸正交的環(huán)形天線內的磁場變化得到頭盔位姿變化，這種方法容易受到座艙周圍金屬的干擾[2]，但是測量角度大，不易受到陽光干擾；此外還有視覺測量方法與慣性測量方法等。這些方法各有其優(yōu)缺點，單一的測量方法往往會受到器件本身和算法的限制。為此，考慮利用數據融合技術[3-4]的同時使用慣性測量與視覺測量方法對頭盔姿態(tài)進行跟蹤與分析處理，期望得到更優(yōu)的性能。

1　綜合測量方法

基于MIMU的頭盔空間位姿綜合測量方法流程如圖1所示。MIMU固定在頭盔盔體上，輸出頭盔坐標系(OXvYvZv)相對于地球慣性坐標系(OXiYiZi)的角速率。飛機上的慣性器件具有較高的精度，輸出飛機坐標系(OXbYbZb)相對于地球慣性坐標系的角速率。

圖1　系統(tǒng)流程圖Fig.1　System flow diagram

1.1　慣性測量系統(tǒng)

(1)

(2)

(3)

式中，Cr，Cθ分別是橫滾角、俯仰角對應的旋轉矩陣。整理后可得

(4)

利用一階歐拉積分[5]，可求解頭盔相對于飛機的姿態(tài)角。

1.2　基于MIMU頭盔瞄準線綜合測量系統(tǒng)

采用基于MIMU的慣性測量與視覺測量綜合的方法，可以在很大程度上彌補單一測量方法的不足。僅依靠MIMU的慣性測量雖然能夠提供較高的測量速度和全向的測量范圍，但是無法獲得位姿的初始值，且MIMU器件存在零漂、誤差會隨時間積累等原因，導致慣性測量無法作為獨立的可信賴系統(tǒng)測量運動平臺上頭盔的姿態(tài)[6-8]。雖然視覺測量方法測量精度較高，但存在測量范圍不足、測量短時失效、抗環(huán)境干擾能力差等缺點。采用綜合的測量方法[9-11]可克服單一測量方法的缺點。

1.2.1零偏估計原理

陀螺儀零偏的存在會使解算結果出現極大的偏差，故需要準確估計零偏并跟蹤零偏的變化，來保證姿態(tài)解算的精度。為了準確估計陀螺儀零偏，根據姿態(tài)解算模型建立狀態(tài)方程，通過狀態(tài)擴維的方法利用卡爾曼濾波實時標定MIMU陀螺儀零偏，可以對陀螺儀零偏進行有效的估計。

(5)

(6)

取狀態(tài)變量X為

(7)

式中：ψ，θ，γ表示頭部運動的姿態(tài)角；ex，ey，ez表示實時標定的MIMU零偏。由于通過卡爾曼濾波得到的零偏會隨時間圍繞真值上下抖動，為了得到更精確的零偏，取方差閾值σ,對超過閾值的零偏進行平滑處理。

1.2.2慣性測量和視覺測量的數據融合

采用卡爾曼濾波進行相對姿態(tài)的自適應數據融合，通過對MIMU與飛機間相對轉動關系的計算，建立相對姿態(tài)角變化的數學模型，獲得相對姿態(tài)角遞推函數關系作為狀態(tài)方程；以視覺測量得到的相對姿態(tài)角作為觀測信息，建立觀測方程。從而可以獲得完整的濾波模型，實現圍繞頭盔、飛機間相對姿態(tài)角測量的數據融合，達到抑制慣性測量誤差、提高輸出精度的目的。

取狀態(tài)變量(如式(7)所示)，建立濾波模型

(8)

tk+1時刻的最優(yōu)估計為

(9)

協(xié)方差為

P(k+1|k+1)=P(k+1|k)-P(k+1|k)HT(k+1)·[H(k+1)P(k+1|k)HT(k+1)+R(k+1)]-1H(k+1)P(k+1|k)

(10)

卡爾曼濾波增益矩陣為

K(k+1)=P(k+1|k)HT(k+1)[H(k+1)P(k+1)·HT(k+1)+R(k+1)]-1。

(11)

本文算法中，觀測噪聲由視覺測量系統(tǒng)的噪聲決定，系統(tǒng)噪聲由陀螺儀角速率隨機游走的大小決定。仿真流程如圖2所示，可以得到頭盔的實時姿態(tài)角。

圖2　計算流程圖Fig.2　Flow chart of calculation

2　系統(tǒng)測試與分析

為驗證本文算法，進行系統(tǒng)測試試驗。由于受到有關試驗條件限制，對綜合測量系統(tǒng)的測試進行了簡化。

測試環(huán)境由STIM300、轉臺、數據采集和處理計算機組成。測試方法和有關條件如下：

1) 飛機的運動假設為勻速直線運動，假設飛機上安裝的慣性系統(tǒng)具有足夠高的測量精度，飛機坐標系和轉臺坐標系一致；

2) 將MIMU固定在轉臺上，以轉臺輸出的數據作為頭盔姿態(tài)的理想值；

3) 由頭盔姿態(tài)的理想值，根據已有的視覺測量系統(tǒng)的統(tǒng)計特性疊加噪聲，獲得視覺系統(tǒng)的測量結果；

4) 用MIMU的測量值，根據本文算法，進行解算獲得頭盔測量值并與轉臺位置比較，獲得MIMU的測量結果。

運行慣性測量系統(tǒng)，讓轉臺在各個方向分別轉過一定的角度，MIMU與轉臺同時進行數據采集。將采集得到的數據進行解算處理。方位、俯仰、橫滾3個方向上的測試結果如表1所示。

表1　測試結果統(tǒng)計

部分測量結果和誤差見圖3。

圖3　不同測量方式對比Fig.3　The comparison of different pose measurements

圖3a是靜態(tài)解算誤差，靜態(tài)情況下解算誤差穩(wěn)定且誤差均在0.35以內。圖3b是在動態(tài)情況下，因為安裝誤差，解算誤差稍有波動；轉臺運動過程中，誤差出現短時變大的情況，轉臺運動暫停，誤差迅速收斂；結合表1可以看出，總體精度與視覺測量觀測的誤差0.5相比，仍有提高。圖3b～圖3d是將視覺測量值、綜合解算值與真實值進行對比，可以看出，視覺測量相對姿態(tài)角的噪聲較大，而融合算法能夠更好地跟蹤盔體的動態(tài)特性，相對姿態(tài)解算結果圍繞真值上下浮動，浮動幅度較視覺測量有了明顯的降低，也未出現慣性測量誤差隨時間漂移的情況。圖3f是3種方式的解算誤差對比，與兩種單一測量方法相比，綜合測量方法誤差幅值小、穩(wěn)定性高。

由于不同的MIMU陀螺儀性能差異，零偏大小、測量精度也各不相同，其中，零偏是影響MIMU精度的主要因素。為驗證本文算法對陀螺儀零偏的有效抑制作用，假設陀螺儀三軸零偏初值均為0，在轉臺運動中，記錄陀螺儀零偏數據，如圖4所示。

可見，轉臺運轉前，MIMU零偏不為零。因此在濾波初始階段，零偏瞬時值有較大波動，由于卡爾曼濾波的在線標定作用，零偏值迅速收斂。2 s以后能夠完全適應MIMU的運動，在整個運動過程中都保持對MIMU零偏的有效抑制。

圖4　MIMU零偏統(tǒng)計Fig.4　Statistics data of the MIMU bias

3　結束語

本文提出了一種基于MIMU的慣性測量與視覺測量融合的運動物體相對運動平臺的空間位姿測量方法。通過測量數據的融合，以視覺測量數據作為觀測值，對慣性測量數據進行校正。初步測試表明，本文提出的算法綜合了兩種不同測量方式各自的優(yōu)點，實現了更高精度、更快速度的測量，為將來的工程應用奠定了理論基礎。在初步測試中發(fā)現，在觀測失效的情況下，解算精度稍有降低。今后的研究工作將集中在MIMU的安裝校準、運動平臺測量性能對綜合測量結果的影響、觀測失效情況下如何保證測量精度等方面，希望能夠得到穩(wěn)定的、高性能的綜合測量方法。

[1]丁全心. 機載瞄準顯示系統(tǒng)[M]. 北京：航空工業(yè)出版社,2015.

[2]張求知,劉建業(yè),王立偉.電磁式跟蹤器磁場畸變的姿態(tài)校正技術研究[J].南京航空航天大學學報，2007，39(6):711-715.

[3]劉延新,劉世良,丁全心,等.基于數據融合的座艙頭部姿態(tài)跟蹤方法研究[J].電光與控制,2016，23(8):65-69.

[4]陳靖,王涌天,劉越,等.適用于戶外增強現實系統(tǒng)的混合跟蹤定位算法[J].計算機輔助設計與圖形學學報,2010，22(2):204-209.

[5]MURPHY-CHUTORIAN E,TRIVEDI M M.Head pose estimation and augmented reality tracking:an integrated driver awareness[J].IEEE Transactions on Intelligent Transportation Systems,2010,11(2):300-311.

[6]羅斌,王涌天,劉越.高精度魯棒的座艙頭部姿態(tài)跟蹤器研究[J].北京理工大學學報,2011,31(3):367-372.

[7]羅斌，王涌天，劉越.雙目光學跟蹤器姿態(tài)精度的預測和估計研究[J].儀器儀表學報，2010，31(1)：194-200.

[8]LI P，DUAN G R，SONG S M.Design of relative measurement system for docking and separation platform based on MIMU/CCD[C]//Proceedings of the 30th Chinese Control Conference，2011:5063-5067.

[9]羅斌,王涌天,劉越.光學頭部姿態(tài)跟蹤的多傳感器數據融合研究[J].自動化學報,2010，36(9)：1239-1249.

[10]孫長庫,徐懷遠,張寶尚,等.基于IMU與單目視覺融合的姿態(tài)測量方法研究[J].天津大學學報：自然科學與工程技術版，2017，50(3)：313-320.

[11]田雪怡,李一兵,李志剛.航跡融合算法在多傳感器融合中的應用[J].計算機仿真,2012,29(1):53-56.