對(duì)等結(jié)構(gòu)下多節(jié)點(diǎn)INS誤差修正

李寅龍，張?zhí)焓?，?傲

（海軍航空大學(xué)，山東 煙臺(tái) 264001）

0 引 言

傳統(tǒng)的多傳感器跟蹤系統(tǒng)一般采用集中式與分布式結(jié)構(gòu)，集中式結(jié)構(gòu)中各節(jié)點(diǎn)發(fā)送數(shù)據(jù)到融合中心進(jìn)行信息處理，對(duì)融合中心的計(jì)算量、通信量都提出較高要求。分布式結(jié)構(gòu)中每個(gè)節(jié)點(diǎn)都能獨(dú)立處理局部觀測(cè)數(shù)據(jù)，匯總到融合中心完成全局態(tài)勢(shì)的信息融合[1?3]。但在態(tài)勢(shì)瞬變的現(xiàn)代電子戰(zhàn)中，多傳感器與信號(hào)源傳遞數(shù)據(jù)造成信息爆炸，若我方遭遇電磁干擾導(dǎo)致融合中心癱瘓，此時(shí)依靠融合中心的集中式與分布式結(jié)構(gòu)將失去作戰(zhàn)能力，遭遇毀滅性打擊。在此基礎(chǔ)上提出對(duì)等式結(jié)構(gòu)，其靈感來源于區(qū)塊鏈和Ad?Hoc網(wǎng)絡(luò)，實(shí)際上是一種分布式、去中心化的通信模式[4?5]，不僅解決了集中式與分布式結(jié)構(gòu)抗毀性差的問題，而且提高了成員之間的協(xié)同合作性。

慣性導(dǎo)航系統(tǒng)（INS）利用陀螺儀和加速度計(jì)進(jìn)行自主式導(dǎo)航，不依賴外界信息并且能夠提供全姿態(tài)信息，連續(xù)性好且噪聲低，因此被廣泛利用[6]。

但I(xiàn)NS由于依靠積分算法提供姿態(tài)信息，容易產(chǎn)生累積誤差，且誤差與時(shí)間成正比。因此針對(duì)單個(gè)節(jié)點(diǎn)而言，INS無法長時(shí)間應(yīng)用，現(xiàn)在廣泛利用GPS等算法對(duì)INS誤差進(jìn)行彌補(bǔ)，但是當(dāng)GPS無法使用時(shí)，INS難以進(jìn)行精確定位[7]。利用對(duì)等式結(jié)構(gòu)節(jié)點(diǎn)間可通信協(xié)作的優(yōu)勢(shì)對(duì)INS產(chǎn)生的累積誤差進(jìn)行修正，保證INS長時(shí)間運(yùn)行的可靠性和有效性。

針對(duì)多節(jié)點(diǎn)INS誤差修正的問題，文獻(xiàn)[8]提出基于格網(wǎng)框架的GPS/SINS組合導(dǎo)航系統(tǒng)。文獻(xiàn)[9]建立編隊(duì)成員相對(duì)導(dǎo)航非線性運(yùn)動(dòng)模型，采用量測(cè)重構(gòu)技術(shù)構(gòu)造偽線性觀測(cè)矩陣及協(xié)方差矩陣，提出了多平臺(tái)INS誤差聯(lián)合修正算法。文獻(xiàn)[10]利用GPS/INS數(shù)據(jù)更新節(jié)點(diǎn)位置與速度，通過四元數(shù)將方向線性化，利用EKF處理數(shù)據(jù)以達(dá)到更高的精度。文獻(xiàn)[11]提出一種INS/數(shù)據(jù)鏈相結(jié)合的動(dòng)態(tài)相對(duì)定位算法，利用最小二乘法對(duì)相對(duì)位置進(jìn)行估計(jì)，提出秩虧網(wǎng)平差算法對(duì)定位誤差進(jìn)行校正。文獻(xiàn)[12]提出通過杠桿臂校正傳感器測(cè)量值，利用EKF、EMLog、深度傳感器、陀螺儀對(duì)慣導(dǎo)系統(tǒng)進(jìn)行集成。文獻(xiàn)[13]針對(duì)GPS在遮擋情況下容易失靈和INS易偏離的問題，構(gòu)建距離測(cè)量方程，提出相對(duì)測(cè)距的EKF團(tuán)隊(duì)協(xié)同定位算法。文獻(xiàn)[14]建立INS/GPS運(yùn)動(dòng)模型，將次優(yōu)衰落因子嵌入到預(yù)測(cè)協(xié)方差中，利用UKF修正系統(tǒng)誤差，增強(qiáng)算法的魯棒性。

上述文獻(xiàn)將GPS與INS相結(jié)合建立運(yùn)動(dòng)模型，利用EKF或UKF提高定位精度，但存在以下缺點(diǎn)：若遭遇電磁靜默、信號(hào)干擾等情況，GPS系統(tǒng)可能失靈；EKF需計(jì)算非線性函數(shù)的雅可比矩陣，計(jì)算量較高；UKF算法會(huì)出現(xiàn)中心采樣點(diǎn)權(quán)值為負(fù)、協(xié)方差矩陣非正定的情況，降低定位精度與魯棒性。

而本文在此基礎(chǔ)上提出CKF誤差修正算法，建立高精度節(jié)點(diǎn)非線性高斯運(yùn)動(dòng)模型，利用逼近系統(tǒng)后驗(yàn)均值與協(xié)方差的等權(quán)值采樣點(diǎn)進(jìn)行濾波，利用對(duì)等結(jié)構(gòu)節(jié)點(diǎn)間可協(xié)同合作的通信優(yōu)勢(shì)，修正全局節(jié)點(diǎn)的INS誤差，計(jì)算量小、收斂速度快且穩(wěn)定性高，具有很強(qiáng)的魯棒性。

1 模型建立

對(duì)等式結(jié)構(gòu)采用時(shí)分多址（TDMA）的通信方式且標(biāo)定精確統(tǒng)一的入網(wǎng)時(shí)間，每個(gè)節(jié)點(diǎn)有固定的報(bào)告順序，依次循環(huán)發(fā)言完成整個(gè)周期，最終掌握全局態(tài)勢(shì)。如圖1所示，每個(gè)節(jié)點(diǎn)具有同等的通信地位，節(jié)點(diǎn)間相互通信聯(lián)結(jié)成巨大的信息池，可以有選擇地從中獲取所需的信息。

圖1 信息池示意圖

假設(shè)在對(duì)等式結(jié)構(gòu)中存在多個(gè)節(jié)點(diǎn)，其中，節(jié)點(diǎn)A導(dǎo)航精度最高，建立節(jié)點(diǎn)A在某一時(shí)間段的非線性高斯運(yùn)動(dòng)模型，利用CKF濾波器優(yōu)化節(jié)點(diǎn)A的導(dǎo)航信息，其余待修正節(jié)點(diǎn)在信息池中獲取節(jié)點(diǎn)A的導(dǎo)航信息得到精確的節(jié)點(diǎn)間相對(duì)距離，將各自INS解算的相對(duì)距離與之相減作為量測(cè)值，建立誤差修正模型，對(duì)INS誤差參數(shù)進(jìn)行開環(huán)輸出校正。

2 算法原理

2.1 坐標(biāo)系轉(zhuǎn)換

為了統(tǒng)一全局態(tài)勢(shì)，全局節(jié)點(diǎn)將采用地心直角坐標(biāo)系進(jìn)行信息交換，將直角坐標(biāo)系轉(zhuǎn)換為地心直角坐標(biāo)系的公式為：

式中：（x′，y′，z′）為地心直角坐標(biāo)系坐標(biāo)；l為該點(diǎn)經(jīng)度；φ為該點(diǎn)緯度；δ1，δ2，δ3為轉(zhuǎn)換誤差，本文為簡(jiǎn)化計(jì)算忽略不計(jì)。

2.2 CKF濾波

CKF利用三階Spherical?Radial Cubature準(zhǔn)則選擇2n個(gè)等權(quán)值的Cubature點(diǎn)計(jì)算高斯權(quán)重積分，CKF的具體濾波過程如下[15]。

1）初始化

2）時(shí)間更新

利用Cholesky分解誤差協(xié)方差矩陣Pk得到Sk：

3）量測(cè)值更新

計(jì)算用于量測(cè)值更新的Cubature點(diǎn)：

量測(cè)誤差協(xié)方差矩陣為：

量測(cè)互相關(guān)協(xié)方差矩陣為：

濾波增益為：

狀態(tài)矢量估計(jì)值為：

更新誤差協(xié)方差矩陣為：

3 算法步驟

3.1 CKF濾波模型

建立節(jié)點(diǎn)A某時(shí)間段非線性高斯轉(zhuǎn)彎運(yùn)動(dòng)模型觀測(cè)方程如下：

建立過程噪聲矩陣為：式中：Q為服從高斯分布的非奇異協(xié)方差矩陣；q1，q2的值與過程噪聲的強(qiáng)度有關(guān)。

建立節(jié)點(diǎn)A運(yùn)動(dòng)模型的CKF量測(cè)方程為：

3.2 INS誤差修正模型

以CKF輸出的xk，yk作為輸入，建立慣導(dǎo)誤差修正模型如下：

式中：wax，…，wex和way，…，wey為節(jié)點(diǎn)INS誤差參數(shù)在x，y方向的分量，分別代表距離誤差、速度誤差、加速度計(jì)誤差、陀螺儀誤差、方位偏角誤差；Sx，Sy代表在x，y方向的移動(dòng)距離。

建立狀態(tài)方程為：

式中：S(k)=[wax wbx wcx wdx wex]；A(k,k-1)的非零項(xiàng)為A3,3=1，A4,4=1，A5,5=1。

建立觀測(cè)方程為：

式中γ為地球自轉(zhuǎn)角速度。

設(shè)定各噪聲矩陣，對(duì)INS誤差參數(shù)進(jìn)行開環(huán)輸出校正。INS誤差修正流程如圖2所示。

圖2 INS誤差修正流程

首先在對(duì)等結(jié)構(gòu)中選擇導(dǎo)航精度最高的節(jié)點(diǎn)A建立某時(shí)間段運(yùn)動(dòng)模型，利用CKF優(yōu)化節(jié)點(diǎn)位置精度，其余節(jié)點(diǎn)從信息池中獲取節(jié)點(diǎn)A位置矢量xk，yk，利用實(shí)測(cè)節(jié)點(diǎn)間相對(duì)距離與根據(jù)INS導(dǎo)航信息計(jì)算出的相對(duì)距離之差作為量測(cè)值建立誤差修正模型。通過誤差修正模型、卡爾曼濾波計(jì)算INS誤差參數(shù)，…，，完成對(duì)INS誤差參數(shù)的開環(huán)數(shù)據(jù)矯正。

4 仿真分析

4.1 仿真環(huán)境

假設(shè)導(dǎo)航精度最高的節(jié)點(diǎn)A某時(shí)間段做非線性轉(zhuǎn)彎運(yùn)動(dòng)，初始位置為（1，1）km，初始速度為100 m/s，初始轉(zhuǎn)彎率為-30（°）/s。其余節(jié)點(diǎn)初始運(yùn)動(dòng)速度隨機(jī)在300~700 m/s之間，角度隨機(jī)在0~2π之間。以節(jié)點(diǎn)B代表其余節(jié)點(diǎn)進(jìn)行INS誤差修正，設(shè)定節(jié)點(diǎn)B的INS誤差參數(shù)為：距離誤差wa=50 m，速度誤差wb=10 m/s，加速度計(jì)初始誤差wc=3×10-5g，隨著時(shí)間增長到5×10-5g，并含有加速度計(jì)高斯隨機(jī)零偏1×10-5g，陀螺儀誤差wd=0.01（°）/h，隨著時(shí)間增長到0.02（°）/h，并有高斯隨機(jī)陀螺儀漂移誤差0.01（°）/h，方位偏角誤差we=0.5°。

4.2 仿真結(jié)果

如圖3所示，對(duì)節(jié)點(diǎn)A在某時(shí)間段內(nèi)x，y方向的位置及速度進(jìn)行EKF及CKF濾波。仿真結(jié)果表明，在節(jié)點(diǎn)A做轉(zhuǎn)彎運(yùn)動(dòng)時(shí)各分量的濾波值在10 s后逐漸穩(wěn)定，CKF能以較快速度收斂，其穩(wěn)定性優(yōu)于EKF，且CKF取得較高的精度，具有很強(qiáng)的魯棒性。

圖3 位置及速度濾波結(jié)果對(duì)比圖

x，y方向CKF濾波后的速度及位置誤差示意圖如圖4，圖5所示，總體誤差在10 s后逐漸趨向于0，即濾波值等于真實(shí)值，并能夠在10~100 s的時(shí)刻保持穩(wěn)定濾波，表明CKF濾波具有很好的非線性近似能力。

圖4 CKF在x方向位置、速度估計(jì)誤差

圖5 CKF在y方向位置、速度估計(jì)誤差

節(jié)點(diǎn)A運(yùn)動(dòng)模型轉(zhuǎn)彎率的濾波值與真實(shí)值對(duì)比圖如圖6所示。由于采樣初期容積點(diǎn)數(shù)據(jù)較少，濾波值尚不穩(wěn)定，在10 s后濾波轉(zhuǎn)彎率逐漸穩(wěn)定，20 s后濾波轉(zhuǎn)彎率接近真實(shí)轉(zhuǎn)彎率，直至完全吻合，仿真結(jié)果驗(yàn)證CKF對(duì)轉(zhuǎn)彎運(yùn)動(dòng)模型有較好的跟蹤能力。

圖6 轉(zhuǎn)彎率濾波結(jié)果對(duì)比圖

節(jié)點(diǎn)A運(yùn)動(dòng)軌跡的濾波結(jié)果與真實(shí)軌跡對(duì)比圖如圖7所示。仿真結(jié)果表明，CKF對(duì)于節(jié)點(diǎn)A的轉(zhuǎn)彎運(yùn)動(dòng)模型具有很好的跟蹤效果，濾波值接近真實(shí)值。

圖7 運(yùn)動(dòng)軌跡濾波結(jié)果對(duì)比圖

如圖8，圖9所示，其中：“○”曲線是節(jié)點(diǎn)A與節(jié)點(diǎn)B在x，y方向的相對(duì)距離真實(shí)值曲線；“*”曲線是節(jié)點(diǎn)B的未校正INS推算的相對(duì)距離曲線，由于INS誤差參數(shù)中的加速度計(jì)誤差和陀螺儀誤差與時(shí)間成正比，因此“*”曲線增長的趨勢(shì)越來越快；“△”曲線是利用本文算法校正后節(jié)點(diǎn)B利用INS解算出的相對(duì)距離曲線，仿真表明“△”曲線相比“*”曲線更接近真實(shí)值曲線，表明本文算法能夠?qū)NS誤差進(jìn)行有效的修正。

圖8 x方向真實(shí)值與校正后誤差曲線

圖9 y方向真實(shí)值與校正后誤差曲線

5 結(jié) 語

本文在傳統(tǒng)多傳感器跟蹤分布式結(jié)構(gòu)的基礎(chǔ)上提出無中心化的對(duì)等式結(jié)構(gòu)。針對(duì)INS系統(tǒng)誤差隨時(shí)間累積的問題，在對(duì)等結(jié)構(gòu)下利用CKF對(duì)導(dǎo)航精度高的節(jié)點(diǎn)建模得到導(dǎo)航信息，通過誤差修正模型對(duì)其余節(jié)點(diǎn)進(jìn)行INS誤差修正，仿真結(jié)果表明，CKF能夠?qū)Ψ蔷€性轉(zhuǎn)彎運(yùn)動(dòng)有較好的跟蹤效果，其穩(wěn)定性與收斂速度皆優(yōu)于EKF，且導(dǎo)航精度更高，具有較好的魯棒性，能夠?qū)NS的系統(tǒng)誤差進(jìn)行有效修正。