蔣海濤, 辛 吉， 管春洋，申?duì)幑?/p>

(1.海裝駐北京地區(qū)第三軍事代表室，北京 100074; 2.空裝駐北京地區(qū)第二軍事代表室，北京 100074； 3.北京自動(dòng)化控制設(shè)備研究所，北京 100074)

0 引言

隨著導(dǎo)航技術(shù)和微加工技術(shù)的發(fā)展，微機(jī)電系統(tǒng)(Micro-Electro-Mechanical System，MEMS)慣性器件與衛(wèi)星導(dǎo)航、磁航向計(jì)和氣壓高度計(jì)等組成的導(dǎo)航系統(tǒng),廣泛應(yīng)用于低成本軍用飛行器和商用飛行器。在電磁環(huán)境復(fù)雜、衛(wèi)星干擾強(qiáng)烈的使用場(chǎng)景中，例如軍事要地、重要工業(yè)區(qū)域和復(fù)雜地形環(huán)境下，衛(wèi)星導(dǎo)航輔助的低精度慣導(dǎo)系統(tǒng)往往會(huì)失效。無(wú)衛(wèi)星輔助的情況下，低精度慣導(dǎo)系統(tǒng)的水平姿態(tài)隨時(shí)間發(fā)散較快，進(jìn)而會(huì)造成飛行器控制系統(tǒng)失穩(wěn)失控。

在衛(wèi)星拒止的情況下，低精度慣導(dǎo)系統(tǒng)如何利用慣性器件信息和飛行器的動(dòng)力學(xué)模型保持水平姿態(tài)精度是當(dāng)今學(xué)者研究的重點(diǎn)方向。文獻(xiàn)[1]提出了一種低成本高精度航姿系統(tǒng)，通過(guò)擴(kuò)展Kalman濾波進(jìn)行數(shù)據(jù)融合以獲得姿態(tài)的準(zhǔn)確輸出，姿態(tài)角動(dòng)態(tài)精度優(yōu)于2°。文獻(xiàn)[2]提出了利用運(yùn)動(dòng)狀態(tài)判別，根據(jù)運(yùn)動(dòng)狀態(tài)采用不同的量測(cè)值進(jìn)行Kalman濾波以估計(jì)姿態(tài)誤差角，從而提高航向姿態(tài)精度。文獻(xiàn)[3]設(shè)計(jì)了由3個(gè)微陀螺儀、3個(gè)微加速度計(jì)和3個(gè)微磁航向計(jì)組成的航姿參考系統(tǒng)，利用加速度計(jì)對(duì)磁航向計(jì)數(shù)據(jù)進(jìn)行正交化處理，并利用基于最小二乘法的橢球擬合校正方法對(duì)測(cè)量數(shù)據(jù)進(jìn)行校正。文獻(xiàn)[4]研究了基于磁航向計(jì)的航姿測(cè)量系統(tǒng)誤差補(bǔ)償。由于磁航向計(jì)體積小、成本低，在商業(yè)級(jí)和娛樂(lè)級(jí)飛行器中使用廣泛[5]。而在軍事領(lǐng)域，由于任務(wù)區(qū)的電磁環(huán)境復(fù)雜，受到環(huán)境磁場(chǎng)以及載體的硬鐵磁場(chǎng)和軟鐵磁場(chǎng)的影響，磁航向計(jì)測(cè)量值誤差大，不能作為可靠穩(wěn)定的傳感器。

針對(duì)巡飛型飛行器的飛行特點(diǎn)，本文基于重力矢量和載體的動(dòng)力學(xué)特性，提出了一種基于動(dòng)態(tài)檢測(cè)和Kalman數(shù)據(jù)融合[6-8]的航姿算法[9-10]。本算法中航向角和姿態(tài)角根據(jù)三軸陀螺儀數(shù)據(jù)進(jìn)行實(shí)時(shí)更新，經(jīng)過(guò)動(dòng)態(tài)判斷和飛控狀態(tài)判斷，Kalman濾波器對(duì)水平加速度數(shù)據(jù)進(jìn)行數(shù)據(jù)融合，并在低動(dòng)態(tài)情況下進(jìn)行量測(cè)更新，以修正水平姿態(tài)誤差。

1 低精度慣性導(dǎo)航的航姿算法研究

低精度的慣性導(dǎo)航系統(tǒng)的陀螺漂移通常大于40(°)/h，加速度計(jì)零位誤差大于2mg。由于陀螺漂移誤差大，水平姿態(tài)角采用傳統(tǒng)姿態(tài)角解算方法發(fā)散較快。本文采用Kalman濾波方式，利用水平加速度計(jì)測(cè)量值為觀測(cè)量，對(duì)水平姿態(tài)角誤差進(jìn)行估計(jì)和修正。本方法的原理與慣導(dǎo)自對(duì)準(zhǔn)計(jì)算水平姿態(tài)角的原理相同[11-13]，當(dāng)載體晃動(dòng)即存在有害加速度(載體加速、轉(zhuǎn)彎引起的向心加速度)時(shí)，水平姿態(tài)角誤差較大，且姿態(tài)誤差與有害加速度的大小正相關(guān)。

1.1 慣性導(dǎo)航自主對(duì)準(zhǔn)原理

慣導(dǎo)自對(duì)準(zhǔn)的計(jì)算采用直接解析法，即通過(guò)矩陣運(yùn)算直接求解出載體的姿態(tài)。在靜態(tài)情況下，采用重力矢量g、地球自轉(zhuǎn)矢量w和2個(gè)向量的叉乘V計(jì)算載體坐標(biāo)系相對(duì)于導(dǎo)航坐標(biāo)系的姿態(tài)矩陣[14]

(1)

(2)

式中，gb為載體坐標(biāo)系下的加速度;wb為載體坐標(biāo)系下的角速度;Vb=gb×wb。

(3)

為降低傳感器高頻噪聲及高頻環(huán)境晃動(dòng)的影響，加速度和角速度需要取一段時(shí)間的均值進(jìn)行計(jì)算。

對(duì)準(zhǔn)誤差的分析參見(jiàn)文獻(xiàn)[15]，在此直接給出分析結(jié)論

(4)

1.2 航姿模式Kalman濾波器設(shè)計(jì)

根據(jù)航姿系統(tǒng)的特點(diǎn)，Kalman濾波器的系統(tǒng)狀態(tài)方程采用3個(gè)失準(zhǔn)角誤差和3個(gè)陀螺漂移誤差共六維，觀測(cè)方程采用水平加速度計(jì)的測(cè)量值，其中狀態(tài)方程定義如下

(5)

式中，F(xiàn)(t) 為t時(shí)刻連續(xù)狀態(tài)方程系統(tǒng)矩陣;w(t)為t時(shí)刻系統(tǒng)隨機(jī)噪聲向量。

根據(jù)Kalman濾波狀態(tài)量建立系統(tǒng)誤差模型，建立系統(tǒng)方程如下

(6)

即F(t)中非零元素如下

狀態(tài)變量定義如下

X=[φNφUφEεxεyεz]T

其中：X為Kalman濾波估計(jì)狀態(tài)量；φN、φU、φE、εx、εy、εz分別為北向失準(zhǔn)角、天向失準(zhǔn)角、東向失準(zhǔn)角、X陀螺漂移、Y陀螺漂移和Z陀螺漂移。

當(dāng)系統(tǒng)處于低動(dòng)態(tài)時(shí)，加速度計(jì)測(cè)量值主要為重力矢量，而2個(gè)水平方向的分量幾乎為0，因此有以下關(guān)系式成立

(7)

H為測(cè)量矩陣

式中，g為當(dāng)?shù)刂亓铀俣取?/p>

1.3 濾波流程

濾波過(guò)程中，每對(duì)陀螺和加速度計(jì)進(jìn)行一次采樣即計(jì)算一次系統(tǒng)轉(zhuǎn)移矩陣，并根據(jù)濾波周期對(duì)系統(tǒng)矩陣進(jìn)行離散化，如下所示

(8)

Φk,k-1=Fsum

式中，Tn為慣性導(dǎo)航周期，此處為0.005s。離散化完成后對(duì)Fsum清零。

連續(xù)系統(tǒng)離散化以后為離散系統(tǒng)，離散系統(tǒng)的模型方程為

(9)

1.4 航姿模式反饋修正算法

Kalman濾波器估計(jì)出水平失準(zhǔn)角后，對(duì)水平姿態(tài)角進(jìn)行誤差補(bǔ)償，為保證輸出水平姿態(tài)的平穩(wěn)性，對(duì)修正值進(jìn)行限幅，采用固定周期(1s)修正的方式。

當(dāng)滿(mǎn)足水平失準(zhǔn)角修正條件后，同時(shí)修正2個(gè)水平失準(zhǔn)角，修正量限幅為φLimit°。記修正量分別為φN和φE

(10)

(11)

修正后，從Xk中對(duì)應(yīng)元素扣掉修正值

X1=X1-φNX3=X3-φE

式中，X1和X3分別為Kalman濾波估計(jì)狀態(tài)量X的第1項(xiàng)(北向失準(zhǔn)角φN)和第3項(xiàng)(東向失準(zhǔn)角φE)。

2 飛行器動(dòng)態(tài)檢測(cè)

飛控模塊根據(jù)目前的控制指令發(fā)送給航姿系統(tǒng)當(dāng)前飛行器的飛行狀態(tài)。在飛控模塊發(fā)送平飛狀態(tài)時(shí)，受到結(jié)構(gòu)干擾、推阻不匹配和風(fēng)力等影響，飛行器可能處于側(cè)滑或小角度轉(zhuǎn)彎等飛行狀態(tài)，因此需要利用慣測(cè)信息進(jìn)行低動(dòng)態(tài)判斷。低動(dòng)態(tài)的判斷門(mén)限跟載體的飛行狀態(tài)、轉(zhuǎn)彎特性和控制品質(zhì)密切相關(guān)。根據(jù)低空固定翼飛行器的飛行特點(diǎn)和動(dòng)力學(xué)特性，低動(dòng)態(tài)需要同時(shí)滿(mǎn)足以下3個(gè)條件：

選用一組具有轉(zhuǎn)彎和直航的飛行試驗(yàn)數(shù)據(jù)如圖1所示，對(duì)低動(dòng)態(tài)判斷進(jìn)行分析。根據(jù)動(dòng)態(tài)情況判斷，在飛行器航向機(jī)動(dòng)時(shí)，首先進(jìn)行滾轉(zhuǎn)，通過(guò)X向角速度1s均值(見(jiàn)圖2)的門(mén)限2(°)/s，可以先于Y向角速度和合加速度判斷出航向機(jī)動(dòng)，但是X向角速度僅在機(jī)動(dòng)的前段和后段；通過(guò)Y軸角速度1s均值(見(jiàn)圖3)的門(mén)限0.5(°)/s，可以判斷出航向存在持續(xù)性的轉(zhuǎn)動(dòng)，作為X向角速度判斷的延續(xù)；合加速度的門(mén)限(見(jiàn)圖4)0.2m/s2作為Y軸角速度判斷的補(bǔ)充，不僅可以判斷出存在航向機(jī)動(dòng)時(shí)的向心加速度，還可以判斷出縱向和天向的加速狀態(tài)。3個(gè)量聯(lián)合使用可以確保載體處于低動(dòng)態(tài)飛行狀態(tài)。

圖1 飛行器飛行姿態(tài)Fig.1 Attitude of the aircraft

圖2 X向角速度1s均值(門(mén)限Fig.2 Average of X angular velocity in 1s(threshold

圖3 Y向角速度1s均值(門(mén)限Fig.3 Average of Y angular velocity in 1s(threshold

圖4 合加速度1s均值(門(mén)限Fig.4 Average of acceleration in 1s(threshold

3 仿真驗(yàn)證

根據(jù)上述算法和策略，利用實(shí)際飛行數(shù)據(jù)進(jìn)行離線(xiàn)仿真，飛行器為固定翼巡飛器，采用小型渦扇發(fā)動(dòng)機(jī)，任務(wù)半徑100km。下面繪制曲線(xiàn)均為進(jìn)入航姿模式后系統(tǒng)輸出的結(jié)果，誤差的比較基準(zhǔn)為實(shí)際飛行的慣性衛(wèi)星組合導(dǎo)航結(jié)果。三軸陀螺漂移約為40(°)/h，加速度計(jì)零位約為2mg。

濾波參數(shù)設(shè)置如下

P0=diag{(5°)2,(15°)2,(5°)2,(200°/3600s)2, (200°/3600s)2,(200°/3600s)2}

Q=diag{(40°/3600s)2,(40°/3600s)2,(40°/3600s)2, (5°/3600s)2,(5°/3600s)2,(5°/3600s)2}

R=diag{(0.1m/s2)2,(0.1m/s2)2}

飛行器的飛行軌跡為U形軌跡如圖5所示，航向角和水平姿態(tài)曲線(xiàn)分別如圖6和圖1所示。飛行器處于低動(dòng)態(tài)情況下(根據(jù)低動(dòng)態(tài)判斷結(jié)果)，在直飛段可以利用航姿濾波器進(jìn)行水平失準(zhǔn)角修正。航向角由于缺乏航向傳感器(磁航向計(jì)、雙天線(xiàn)衛(wèi)星導(dǎo)航接收機(jī)等)的輔助，依靠航向陀螺精度保證航向角的準(zhǔn)確性。航向陀螺設(shè)為40(°)/h，導(dǎo)航600s的航向角誤差約為6.7°，能夠滿(mǎn)足飛行器穩(wěn)定控制飛行的要求。若飛行器中存在精度較高且可靠的航向角信息，可以對(duì)航向角進(jìn)行進(jìn)一步修正，以保證航向角的精度。

圖5 飛行軌跡Fig.5 Flight path

圖6 飛行器飛行航向角Fig.6 Heading angle of the aircraft

當(dāng)導(dǎo)航模式未采用航姿算法時(shí)，水平姿態(tài)采用慣性解算的方式，隨時(shí)間發(fā)散較快如圖7所示，無(wú)法維持飛行器長(zhǎng)時(shí)間穩(wěn)態(tài)飛行。當(dāng)導(dǎo)航模式采用航姿算法時(shí)，飛行器在直飛段的飛行動(dòng)態(tài)滿(mǎn)足低動(dòng)態(tài)判斷門(mén)限，航姿濾波器利用水平加速度計(jì)的信息進(jìn)行濾波，濾波收斂后反饋校正水平姿態(tài)，水平姿態(tài)誤差快速變小至零附近，如圖8所示；飛行器進(jìn)入轉(zhuǎn)彎段則不滿(mǎn)足低動(dòng)態(tài)判斷門(mén)限，航姿濾波器不進(jìn)行量測(cè)更新只進(jìn)行時(shí)間更新，水平姿態(tài)誤差不進(jìn)行反饋校正，水平姿態(tài)誤差發(fā)散趨勢(shì)與純慣性狀態(tài)一致，水平姿態(tài)誤差如圖8所示。航姿模式計(jì)數(shù)和修正次數(shù)計(jì)數(shù)如圖9所示，在低動(dòng)態(tài)情況下，航姿模式計(jì)數(shù)每1s累加1次，非低動(dòng)態(tài)情況下航姿模式計(jì)數(shù)維持不變。在航姿模式下，當(dāng)姿態(tài)角每修正1次時(shí)，修正計(jì)數(shù)累加1次，非航姿模式時(shí)清零。結(jié)合圖8和圖9可以看出，在航姿模式下，隨著修正次數(shù)的累加，東向和北向失準(zhǔn)角誤差逐漸變小，濾波器估計(jì)的水平失準(zhǔn)角準(zhǔn)確。

圖7 無(wú)航姿模式輔助情況下水平姿態(tài)角誤差Fig.7 Attitude error without the assist of attitude and heading reference

圖8 航姿模式輔助情況水平姿態(tài)角誤差Fig.8 Attitude error with the assist of attitude and heading reference

圖9 航姿模式計(jì)數(shù)累計(jì)值Fig.9 Count of attitude and heading reference

4 結(jié)論

在衛(wèi)星拒止情況下，低精度慣導(dǎo)系統(tǒng)利用航姿算法可以維持水平姿態(tài)在較高的精度，滿(mǎn)足飛行器的飛行控制要求。

1)本文的航姿算法是一種適用于低空巡飛型固定翼無(wú)人機(jī)和旋翼型無(wú)人機(jī)等裝備的低成本慣導(dǎo)系統(tǒng)在衛(wèi)星拒止情況下的航向姿態(tài)計(jì)算方法。

2)本文利用機(jī)動(dòng)狀態(tài)判斷和飛控指令相結(jié)合的方式，采用六維狀態(tài)方程和二維觀測(cè)方程的Kalman濾波器估計(jì)和修正水平失準(zhǔn)角，水平姿態(tài)誤差可控制在2°以?xún)?nèi)。