萬(wàn)航雁，　張求知，　張伊慧，　張勁峰，　劉世良，　張寶尚，　陳　君

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

0　引言

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

1　綜合測(cè)量方法

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

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

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

(1)

(2)

(3)

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

(4)

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

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

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

1.2.1零偏估計(jì)原理

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

(5)

(6)

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

(7)

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

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

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

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

(8)

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

(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)

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

圖2　計(jì)算流程圖Fig.2　Flow chart of calculation

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

為驗(yàn)證本文算法，進(jìn)行系統(tǒng)測(cè)試試驗(yàn)。由于受到有關(guān)試驗(yàn)條件限制，對(duì)綜合測(cè)量系統(tǒng)的測(cè)試進(jìn)行了簡(jiǎn)化。

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

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

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

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

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

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

表1　測(cè)試結(jié)果統(tǒng)計(jì)

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

圖3　不同測(cè)量方式對(duì)比Fig.3　The comparison of different pose measurements

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

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

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

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

3　結(jié)束語(yǔ)

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

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

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

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

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

[7]羅斌，王涌天，劉越.雙目光學(xué)跟蹤器姿態(tài)精度的預(yù)測(cè)和估計(jì)研究[J].儀器儀表學(xué)報(bào)，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].自動(dòng)化學(xué)報(bào),2010，36(9)：1239-1249.

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

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