基于容錯慣性網(wǎng)絡(luò)的相對導(dǎo)航方法

李振威，程詠梅，張亞崇，馮鑫濤，陳可正

（1.西北工業(yè)大學(xué) 自動化學(xué)院，西安 710129；2.西安飛行自動控制研究所，西安 710129）

可執(zhí)行多載荷任務(wù)的通用飛機通常搭載雷達、相機等多種任務(wù)設(shè)備，用于對外部環(huán)境的探測、偵查，分布在機翼的不同位置[1]。由于飛機機翼存在動態(tài)的撓曲變形[2]，通常在任務(wù)設(shè)備處安裝子慣性導(dǎo)航系統(tǒng)，測量任務(wù)設(shè)備的自身運動信息，以滿足任務(wù)設(shè)備的工作需求。任務(wù)設(shè)備的性能一定程度上取決于慣性傳感器的精度，受限于體積、重量的要求，任務(wù)設(shè)備處安裝的慣性傳感器精度較低，無法滿足需求[3]。用于飛行導(dǎo)航的主慣性導(dǎo)航系統(tǒng)具有高精度的運動狀態(tài)測量特性，利用主慣性導(dǎo)航系統(tǒng)對子慣性導(dǎo)航系統(tǒng)進行傳遞對準(zhǔn)，提高子慣性導(dǎo)航系統(tǒng)的測量精度，改善任務(wù)設(shè)備的性能，實現(xiàn)對目標(biāo)的精準(zhǔn)探測[4]。

傳遞對準(zhǔn)技術(shù)利用安裝在機身處的主慣性導(dǎo)航系統(tǒng)對任務(wù)設(shè)備處的子慣性導(dǎo)航系統(tǒng)進行誤差估計與補償，是提升子慣性導(dǎo)航系統(tǒng)性能的廣泛應(yīng)用技術(shù)[5]。傳遞對準(zhǔn)方法主要包括姿態(tài)匹配[6]、速度匹配[7]、速度+姿態(tài)匹配[8]、加速度匹配[9]、角速度匹配[10]等。由于機翼結(jié)構(gòu)的復(fù)雜性以及撓曲變形本身的不可預(yù)見性，使得撓曲變形成為傳遞對準(zhǔn)中的一個非常重要問題[1]。針對該問題，國內(nèi)外學(xué)者提出了不同的改進方法。文獻[5]建立了機翼動態(tài)變形影響下的速度匹配模型，文獻[4]提出基于差分濾波器的線加速度+角速度匹配方法，文獻[11]提出基于分布式IMU 的相對姿態(tài)匹配方法。上述方法在進行傳遞對準(zhǔn)時，分別采取一階、二階馬爾科夫模型來描述機翼撓曲。然而，實際的撓曲變形可能并不符合馬爾科夫模型，這將會導(dǎo)致傳遞對準(zhǔn)的性能降低[12]。文獻[12]提出了一種新的基于相對導(dǎo)航的傳遞對準(zhǔn)方法，該方法無需建立機翼撓曲模型，利用主、子相對導(dǎo)航模型估計機翼撓曲，提高了傳遞對準(zhǔn)性能。文獻[13]將相對導(dǎo)航方法擴展到基于多節(jié)點IMU 的慣性網(wǎng)絡(luò)。然而，以上方法忽略了高動態(tài)飛行下，子慣導(dǎo)受干擾易發(fā)生量測異常的問題[14]。因此，面向小體積、低重量的子慣性傳感器精度較低，且存在隨機故障的問題，如何利用多個子慣性傳感器構(gòu)建慣性網(wǎng)絡(luò)提高撓曲變形下子慣性導(dǎo)航系統(tǒng)的精度和可靠性，是亟待解決的一項關(guān)鍵技術(shù)。

本文提出一種基于容錯慣性網(wǎng)絡(luò)的相對導(dǎo)航方法，首先建立撓曲變形下多節(jié)點之間的動態(tài)關(guān)系轉(zhuǎn)換模型以構(gòu)成冗余測量信息，進行基于廣義似然比檢測的最小二乘融合，提高了測量數(shù)據(jù)的可靠性和精度，然后利用慣性網(wǎng)絡(luò)間的局部運動信息進行相對導(dǎo)航解算，完成了撓曲條件下主、子節(jié)點間的高精度相對運動估計，實現(xiàn)了主、子節(jié)點間的高精度傳遞對準(zhǔn)。

1 撓性桿臂下基于廣義似然比檢測的多節(jié)點信息融合

建立機載慣性網(wǎng)絡(luò)系統(tǒng)，其中高精度主慣性導(dǎo)航系統(tǒng)（簡稱主節(jié)點）安裝在機體質(zhì)心處，多個子慣性導(dǎo)航系統(tǒng)（簡稱子節(jié)點）分別安裝在機翼不同位置，見圖1。

圖1 機載任務(wù)設(shè)備分布示意圖Fig.1 Distribution diagram of airborne mission equipment

1.1 撓性桿臂動態(tài)轉(zhuǎn)換

動態(tài)關(guān)系轉(zhuǎn)換的主要思想是：將各個節(jié)點輸出轉(zhuǎn)換到統(tǒng)一坐標(biāo)系下，通過旋轉(zhuǎn)和平移兩個步驟完成，旋轉(zhuǎn)解決不同子慣性導(dǎo)航系統(tǒng)安裝誤差角不同的影響，平移解決桿臂誤差的影響。

剛性桿臂條件下，只需利用節(jié)點間的旋轉(zhuǎn)矩陣和桿臂誤差將各個節(jié)點對準(zhǔn)到統(tǒng)一坐標(biāo)系。撓性桿臂條件下，機翼撓曲變形對各子節(jié)點之間帶來了局部相對運動信息，需要做進一步補償。機翼的長度遠大于厚度，將機翼視為薄板，采用薄板模型對機翼撓曲進行建模[15]。在載荷q的作用下，機翼撓曲模型見圖2。

圖2 機翼撓曲模型Fig.2 Wing flexure model

根據(jù)力學(xué)原理有以下機翼彎曲的平衡方程[15]：

式中，w為位置（x,y）處的撓度，D為彈性系數(shù)，q（x,y）為位置（x,y）處的載荷。

按照有限元分析，實際上機翼的前三階模態(tài)響應(yīng)和力學(xué)響應(yīng)均為彎曲，所以可近似認為w對y的各階偏導(dǎo)數(shù)均為零，從而根據(jù)式(1)，簡化得到新的平衡方程：

對機翼而言，可以將其視為一邊固支，三邊自由的矩形板，那么就有邊界條件[15]：

（1）固支邊x=0，位移邊界條件：

（2）自由邊x=LT，力邊界條件：

結(jié)合式(3)(4)，對式(2)進行積分可得到機翼撓曲模型：

如圖2所示，通過對機翼曲線求偏導(dǎo)可獲得其對應(yīng)的斜率，從而獲得撓曲變形角γ。

以x位置為例，其位置處的撓曲變形角γ為：

考慮到撓曲變形角通常小于5 °，利用小角度近似γ≈tanγ，得到撓曲角與撓曲位移的關(guān)系：

建立第j個子節(jié)點撓曲角速率與第k個子節(jié)點撓曲角速率的關(guān)系：

對子節(jié)點之間的相對撓曲導(dǎo)致的角運動進行補償，子節(jié)點j到子節(jié)點k的角速率的動態(tài)轉(zhuǎn)換關(guān)系為：

對于加速度計，需要對撓曲桿臂加速度進行補償，子節(jié)點j的撓曲桿臂向量Rsj表示為：

則子節(jié)點j處產(chǎn)生的撓曲變形加速度為：

對子節(jié)點之間的桿臂加速度和撓曲變形加速度進行補償，子節(jié)點j到子節(jié)點k加速度計輸出的動態(tài)轉(zhuǎn)換關(guān)系為：

1.2 基于廣義似然比檢測的多節(jié)點信息融合

各節(jié)點接收其他節(jié)點轉(zhuǎn)換的數(shù)據(jù)后，構(gòu)成冗余測量信息，建立基于廣義似然比檢測[16]的最小二乘融合模型，實現(xiàn)慣性網(wǎng)絡(luò)的容錯性能。

1.2.1 奇偶方程

對子節(jié)點1 處的冗余測量信息，建立觀測方程：

式中，y表示冗余的測量數(shù)據(jù)，x表示真實的狀態(tài)向量，Hsr表示測量矩陣，b表示測量數(shù)據(jù)的故障，ε表示服從零均值高斯分布的噪聲，其協(xié)方差矩陣為R1。

對觀測方程中的狀態(tài)x進行解耦，利用奇偶矩陣V，使?jié)M足：

式中，P表示奇偶殘差，P僅與噪聲和故障有關(guān)。V可以通過Potter 算法得到[17]。

測量數(shù)據(jù)無故障情況下，b=0，則：

式中，P服從零均值高斯分布。

測量數(shù)據(jù)有故障情況下，b≠0，此時P的統(tǒng)計特性與式(16)不同。依據(jù)此特性，對測量數(shù)據(jù)進行廣義似然比檢測。

1.2.2 廣義似然比檢測

建立二元假設(shè)，無故障為H0，有故障為H1：

式中，μ=VTb。

基于二元假設(shè)的似然函數(shù)φ(·)為：

式中，K為固定值，在似然函數(shù)比中可約去。

對μ求導(dǎo)，極大對數(shù)似然函數(shù)比Lmax（·）為[17]：

基于以上推導(dǎo)，構(gòu)建廣義似然比故障檢測函數(shù)：

式中，F(xiàn)D表示故障檢測函數(shù)值，服從卡方分布，即FD～χ2（n-m），n表示測量數(shù)據(jù)維數(shù)，m表示狀態(tài)維數(shù)。

故障檢測準(zhǔn)則為：

式中，TD為故障檢測閾值，通過查詢卡方分布表獲得。

1.2.3 多節(jié)點信息融合

定義第i個測量數(shù)據(jù)的故障隔離函數(shù)FIi為：

若FIi越大，發(fā)生故障的似然函數(shù)值ln[φ(P|H1)]越大。因此當(dāng)檢測出故障時，通過比較FIi，可以隔離出故障的測量數(shù)據(jù)：

1.2.4 建立基于加權(quán)最小二乘的測量數(shù)據(jù)融合模型

廣義似然比故障檢測算法的最小可檢測偏差為[16]：

式中，η為最小可檢測偏差，α和β分別表示虛警率和漏檢率，和為對應(yīng)置信區(qū)間下的卡方分布函數(shù)值。

當(dāng)故障幅值小于最小可檢測偏差時，故障檢測算法性能大幅降低，因此利用加權(quán)最小二乘方法進一步對冗余測量進行全局融合，加權(quán)最小二乘估計準(zhǔn)則為：

故障隔離函數(shù)值的大小可以體現(xiàn)測量數(shù)據(jù)的質(zhì)量，利用故障隔離函數(shù)值構(gòu)造權(quán)值矩陣，第i維測量數(shù)據(jù)的權(quán)重為：

構(gòu)造最小二乘權(quán)值矩陣為：

基于加權(quán)最小二乘的測量數(shù)據(jù)融合模型為：

2 相對導(dǎo)航誤差估計與補償

考慮到本文需求為監(jiān)測機翼撓曲的微小變化，捷聯(lián)慣性導(dǎo)航算法利用地球坐標(biāo)系進行解算，解算過程中將會引入坐標(biāo)系間的轉(zhuǎn)換誤差，因為本文建立以主慣導(dǎo)為主坐標(biāo)系的相對慣性導(dǎo)航解算方法。

2.1 相對慣性導(dǎo)航解算

本文采用相對慣性導(dǎo)航算法進行主子慣導(dǎo)間傳遞對準(zhǔn)，通過相對姿態(tài)微分方程、相對速度微分方程以及相對位置微分方程遞推解算。考慮主慣導(dǎo)坐標(biāo)系與子慣導(dǎo)坐標(biāo)系的關(guān)系，有：

從而相對姿態(tài)微分方程為：

在主節(jié)點坐標(biāo)系下，相對速度微分方程為：

式中，V表示主、子節(jié)點間相對速度，R表示相對位置，fm和fs分別表示主、子節(jié)點的加速度計輸出。

式(32)不易計算，定義偽相對速度U：

對式(33)兩端進行微分：

相對位置微分方程為：

式(31)(34)和(35)構(gòu)成相對導(dǎo)航微分方程，通過微分方程的數(shù)值求解方法即可獲得子節(jié)點相對于主節(jié)點的運動關(guān)系。

2.2 誤差估計與補償

相對慣性導(dǎo)航算法會受子節(jié)點IMU 安裝誤差、傳感器噪聲等很多因素的影響，如果不進行誤差補償，導(dǎo)航結(jié)果會很快發(fā)散，因此需要建立相對導(dǎo)航誤差模型。

根據(jù)相對姿態(tài)微分方程，考慮陀螺噪聲ε影響，可得相對姿態(tài)誤差方程：

根據(jù)偽相對速度微分誤差，考慮加速度計噪聲?影響，可得偽相對速度誤差方程為：

相對位置誤差的變化率為相對速度誤差，相對位置誤差方程為：

式(36)(37)和(38)構(gòu)成相對導(dǎo)航誤差方程。

狀態(tài)空間方程可寫為：

式中，F(xiàn)為系統(tǒng)矩陣，G為噪聲激勵矩陣，α為系統(tǒng)噪聲。

考慮相對姿態(tài)誤差、相對速度誤差、相對位置誤差以及陀螺、加速度計常值漂移的影響，此時15 維狀態(tài)向量為：

系統(tǒng)矩陣F為：

噪聲激勵矩陣G為：

系統(tǒng)噪聲α為：

相對慣性導(dǎo)航的解算過程沒有任何外界參考信息，因而給量測值的選擇帶來一定困難。但是考慮到機翼形狀的約束，選用相對位置誤差δ R作為量測量。

量測方程可寫為：

式中，H為觀測矩陣，β為量測噪聲。

觀測矩陣H為：

如圖3所示，表示由相對導(dǎo)航算法計算的相對位置；R表示主、子節(jié)點間的實際相對位置；L表示標(biāo)稱相對位置；表示撓曲位移，即子節(jié)點在機翼變形的影響下，偏離了其標(biāo)稱位置的撓曲位移向量。根據(jù)幾何關(guān)系，δR可表示為：

圖3 相對位置誤差幾何示意圖Fig.3 Geometric diagram of relative position error

考慮撓曲角與撓曲位移的約束關(guān)系，建立約束模型，如圖4所示。根據(jù)圖中幾何關(guān)系，可得：

圖4 撓曲位移與撓曲角關(guān)系圖Fig.4 Relationship between flexure displacement and flexure angle

式中，μf,y由相對姿態(tài)計算值和標(biāo)稱值相減所得，表示桿臂在x軸上的分量。

卡爾曼濾波模型分為時間更新和量測更新[18]：

1）時間更新：

2）量測更新：

式中，K(k)表示濾波增益，表示量測噪聲的協(xié)方差，X(k) 表示估計值，P(k) 表示估計值的誤差協(xié)方差。

使用卡爾曼濾波器估計出的X(k)對相對導(dǎo)航解算值進行反饋校正，完成高精度的局部相對運動估計。

算法總體框架見圖5。

圖5 基于容錯慣性網(wǎng)絡(luò)的相對導(dǎo)航方法流程圖Fig.5 Flow chart of relative navigation method based on fault-tolerant inertial network

3 仿真實驗與分析

仿真條件：飛機搭載一個主慣導(dǎo)、四個子慣導(dǎo)，構(gòu)成了一主、四子的慣性網(wǎng)絡(luò)，四個子慣導(dǎo)在主慣坐標(biāo)系下的安裝位置分別為1 m、2 m、3 m、4 m 處，分別為子節(jié)點1、2、3、4。機翼低頻撓曲為0.01 Hz，在子慣4 處最大撓曲位移為160 mm，高頻撓曲為20 Hz、25 Hz 和30 Hz，幅值分別為1 mm、0.5 mm和0.5 mm。傳感器采樣時間設(shè)置為0.01 s，濾波器解算頻率為50 Hz，傳感器參數(shù)見表1，飛行軌跡見圖6。

圖6 飛行軌跡Fig.6 Flight trajectory

表1 傳感器參數(shù)Tab.1 Sensor parameters

選用相對位置的均方根誤差RMSE 作為衡量相對位置誤差的指標(biāo)。相對位置的均方根誤差RMSE 定義為：

同理，相對姿態(tài)的均方根誤差RMSE 定義為：

為驗證所提的分布式容錯相對導(dǎo)航方法的優(yōu)越性，設(shè)計兩組仿真實驗。

第一組仿真實驗，傳感器無故障下，對單節(jié)點方法和分布式方法進行對比，單節(jié)點表示使用一主、一子進行主子相對導(dǎo)航解算，分布式表示使用一主、四子進行分布式相對導(dǎo)航解算。圖7-8 為子節(jié)點1 的相對導(dǎo)航結(jié)果，由圖中可以看出，分布式的相對位置誤差和相對姿態(tài)明顯小于單節(jié)點。

圖7 相對位置誤差對比（單節(jié)點和分布式）Fig.7 Relative position error (single node and distributed)

圖8 相對姿態(tài)誤差對比（單節(jié)點和分布式）Fig.8 Relative attitude error (single node and distributed)

400 s-500 s 期間的性能統(tǒng)計見表2-3，從表中可以看出，采用單節(jié)點方法，x、y、z三個方向的相對位置估計精度分別為 0.1412 mm、0.1493 mm、0.3062 mm，三個方向的相對姿態(tài)估計精度分別為1.2234'、0.8104'、4.3565'。采用分布式方法，x、y、z三個方向的相對位置估計精度分別為0.0600 mm、0.0563 mm、0.2658 mm，三個方向的相對姿態(tài)估計精度分別為0.2976'、0.2568'、0.9989'。相對于單節(jié)點，分布式提高了相對姿態(tài)和相對位置的估計精度，體現(xiàn)了分布式方法的優(yōu)越性。

表2 相對位置性能統(tǒng)計Tab.2 Performance of relative position

表3 相對姿態(tài)性能統(tǒng)計Tab.3 Performance of relative attitude

第二組仿真實驗，在傳感器故障情況下，對比未加入容錯的分布式方法和分布式容錯方法。在400 s-500 s 向y軸陀螺注入10 倍噪聲幅值的故障，圖9-10 為相對導(dǎo)航結(jié)果。由圖中可以看出，未加入容錯的分布式方法受故障的影響很大，在故障發(fā)生期間，出現(xiàn)了較大的誤差，而分布式容錯方法對傳感器的故障起到了很好的容錯效果。400 s-500 s 期間的性能統(tǒng)計見表4-5，從表中可以看出，采用分布式方法，x、y、z三個方向的相對位置估計精度分別為5.1075 mm、0.4877 mm、5.5251 mm，三個方向的相對姿態(tài)估計精度分別為9.3637'、29.8118'、50.7981'。采用分布式容錯方法，x、y、z三個方向的相對位置估計精度分別為 0.0646 mm、0.0634 mm、0.7377 mm，三個方向的相對姿態(tài)估計精度分別為0.3376'、0.2412'、1.2178'?？梢钥闯鲈诠收习l(fā)生時，相對于分布式，所提的分布式容錯方法改善了相對姿態(tài)和相對位置的估計精度，未加入容錯的分布式方法精度大大降低。

表4 相對位置性能統(tǒng)計Tab.4 Performance of relative position

圖9 相對位置誤差對比（分布式和分布式容錯）Fig.9 Relative position error (distributed and distributed fault tolerance)

圖10 相對姿態(tài)誤差對比（分布式和分布式容錯）Fig.10 Relative attitude error (distributed and distributed fault tolerance)

表5 相對姿態(tài)性能統(tǒng)計Tab.5 Performance of relative attitude

綜上，仿真結(jié)果表明了本文所提方法的有效性。本文方法充分發(fā)揮慣性網(wǎng)絡(luò)的優(yōu)勢，提供了一種機翼撓曲變形下分布式容錯相對導(dǎo)航方法，實現(xiàn)了機翼撓曲變形以及傳感器故障下的高精度相對運動估計。

4 結(jié)論

本文建立了撓曲變形下多節(jié)點間的動態(tài)轉(zhuǎn)換關(guān)系模型，構(gòu)成冗余測量信息，設(shè)計了基于廣義似然比檢測的最小二乘融合方法，提高了慣性測量數(shù)據(jù)的可靠性和精度。構(gòu)建的分布式容錯相對導(dǎo)航系統(tǒng)將撓曲變形下廣義似然比檢測和相對運動估計進行結(jié)合，在慣性傳感器故障情況下，使慣性網(wǎng)絡(luò)幾乎不受故障的影響，改善了相對姿態(tài)和相對位置的估計精度。未來工作中，將進一步把所提方法用于半物理實驗或?qū)嶏w實驗。