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

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

0　引言

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

1　綜合測量方法

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

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

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

(1)

(2)

(3)

式中，Cr，Cθ分別是橫滾角、俯仰角對應(yīng)的旋轉(zhuǎn)矩陣。整理后可得

(4)

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

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

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

1.2.1零偏估計原理

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

(5)

(6)

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

(7)

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

1.2.2慣性測量和視覺測量的數(shù)據(jù)融合

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

取狀態(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)測試試驗。由于受到有關(guān)試驗條件限制，對綜合測量系統(tǒng)的測試進行了簡化。

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

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

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

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

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

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

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

部分測量結(jié)果和誤差見圖3。

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

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

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

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

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

3　結(jié)束語

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

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

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

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

[4]陳靖,王涌天,劉越,等.適用于戶外增強現(xiàn)實系統(tǒng)的混合跟蹤定位算法[J].計算機輔助設(shè)計與圖形學(xué)學(xué)報,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].北京理工大學(xué)學(xué)報,2011,31(3):367-372.

[7]羅斌，王涌天，劉越.雙目光學(xué)跟蹤器姿態(tài)精度的預(yù)測和估計研究[J].儀器儀表學(xué)報，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]羅斌,王涌天,劉越.光學(xué)頭部姿態(tài)跟蹤的多傳感器數(shù)據(jù)融合研究[J].自動化學(xué)報,2010，36(9)：1239-1249.

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

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