本發(fā)明涉及組合導(dǎo)航技術(shù)領(lǐng)域，特別是一種強(qiáng)機(jī)動(dòng)條件下的彈載深組合ARCKF濾波方法。

二、

背景技術(shù)：

強(qiáng)機(jī)動(dòng)、高動(dòng)態(tài)條件下對(duì)載體進(jìn)行導(dǎo)航定位是一個(gè)熱點(diǎn)和難點(diǎn)問(wèn)題，在軍事應(yīng)用中的地位尤其重要，引起國(guó)內(nèi)外眾多學(xué)者的廣泛關(guān)注。強(qiáng)機(jī)動(dòng)、高動(dòng)態(tài)條件下的導(dǎo)航定位難點(diǎn)主要有以下幾點(diǎn)：慣性導(dǎo)航系統(tǒng)解算誤差變大，對(duì)定位技術(shù)也提出了很高的要求；系統(tǒng)觀測(cè)傳遞函數(shù)表現(xiàn)為強(qiáng)非線性，線性化誤差導(dǎo)致系統(tǒng)發(fā)散，因此對(duì)數(shù)據(jù)融合技術(shù)的魯棒性與可靠性提出了更高的要求。

傳統(tǒng)組合導(dǎo)航中的非線性濾波一般采取擴(kuò)展卡爾曼濾波EKF、無(wú)跡卡爾曼濾波UKF。EKF需要求導(dǎo)計(jì)算雅克比(Jacobian)陣，計(jì)算量大，線性化后具有較大的截?cái)嗾`差；UKF缺乏嚴(yán)格的數(shù)學(xué)推導(dǎo)過(guò)程，且當(dāng)系統(tǒng)維數(shù)過(guò)高時(shí)，UKF的Sigma點(diǎn)權(quán)值容易出現(xiàn)負(fù)值情況，導(dǎo)致其矩陣積分中引入很大的截?cái)嗾`差，濾波精度下降。近年來(lái)提出的容積卡爾曼濾波CKF，有更嚴(yán)格的理論推導(dǎo)，在高維度系統(tǒng)的濾波性能較UKF更優(yōu)。但在強(qiáng)機(jī)動(dòng)、高動(dòng)態(tài)等復(fù)雜環(huán)境中，有可能給載體帶來(lái)觀測(cè)異常以及由動(dòng)力學(xué)模型不準(zhǔn)確導(dǎo)致的誤差，組合導(dǎo)航濾波性能還需進(jìn)一步提高。

三、

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的在于提供一種精確、穩(wěn)定的強(qiáng)機(jī)動(dòng)條件下的彈載深組合ARCKF濾波方法。

實(shí)現(xiàn)本發(fā)明目的的技術(shù)解決方案為：一種強(qiáng)機(jī)動(dòng)條件下的彈載深組合ARCKF濾波方法，包括以下步驟：

步驟1、通過(guò)軌跡發(fā)生器生成模擬強(qiáng)機(jī)動(dòng)導(dǎo)彈的彈道軌跡和相應(yīng)IMU數(shù)據(jù)；

步驟2、將生成的彈道軌跡注入衛(wèi)星信號(hào)模擬器，生成GNSS中頻數(shù)據(jù)；

步驟3、將生成的GNSS中頻數(shù)據(jù)注入給接收機(jī)進(jìn)行導(dǎo)航解算、IMU數(shù)據(jù)進(jìn)行慣導(dǎo)解算；

步驟4、建立發(fā)射慣性坐標(biāo)系下GNSS/SINS深組合導(dǎo)航系統(tǒng)的狀態(tài)方程和觀測(cè)方程；

步驟5、將抗差估計(jì)理論中的抗差M估計(jì)算法和自適應(yīng)因子結(jié)合到容積卡爾曼濾波即CKF算法中，得出一種自適應(yīng)抗差容積卡爾曼濾波即ARCKF算法，對(duì)系統(tǒng)狀態(tài)進(jìn)行濾波校正。

本發(fā)明與現(xiàn)有技術(shù)相比，其顯著優(yōu)點(diǎn)是：(1)采用非線性濾波CKF，更能符合組合導(dǎo)航模型的非線性特性；(2)與EKF、UKF濾波相比，具有估計(jì)精度更高，不易發(fā)散且計(jì)算量少的優(yōu)點(diǎn)；(3)采用的自適應(yīng)抗差容積卡爾曼濾波(ARCKF)，更能抵御在觀測(cè)異常擾動(dòng)與動(dòng)力學(xué)模型不準(zhǔn)確所導(dǎo)致的誤差。

四、附圖說(shuō)明

圖1是本發(fā)明強(qiáng)機(jī)動(dòng)條件下的彈載深組合ARCKF濾波方法的流程圖。

圖2是實(shí)施例1中模擬生成的發(fā)射慣性坐標(biāo)系下的真實(shí)彈道軌跡。

圖3是實(shí)施例1中ARCKF與KF濾波在X方向的速度誤差對(duì)比結(jié)果。

圖4是實(shí)施例1中ARCKF與KF濾波在Y方向的速度誤差對(duì)比結(jié)果。

圖5是實(shí)施例1中ARCKF與KF濾波在Z方向的速度誤差對(duì)比結(jié)果。

五、具體實(shí)施方式

下面結(jié)合附圖及具體實(shí)施例對(duì)本發(fā)明作進(jìn)一步詳細(xì)說(shuō)明。

結(jié)合圖1，本發(fā)明強(qiáng)機(jī)動(dòng)條件下的彈載深組合ARCKF濾波方法，步驟如下：

步驟1、通過(guò)軌跡發(fā)生器生成模擬強(qiáng)機(jī)動(dòng)導(dǎo)彈的彈道軌跡和相應(yīng)IMU數(shù)據(jù)；

根據(jù)需要，建立動(dòng)力學(xué)模型和發(fā)射慣性坐標(biāo)系下的軌跡參數(shù)，包括彈道導(dǎo)彈的初始位置、姿態(tài)角、加速度、姿態(tài)的變化規(guī)律，然后生成彈道軌跡和相應(yīng)的IMU數(shù)據(jù)。

步驟2、將步驟1中生成的彈道軌跡注入衛(wèi)星信號(hào)模擬器，生成GNSS中頻數(shù)據(jù)。

步驟3、將步驟2中得到的GNSS中頻數(shù)據(jù)注入給軟件接收機(jī)進(jìn)行捕獲跟蹤和導(dǎo)航解算、同時(shí)IMU數(shù)據(jù)進(jìn)行慣導(dǎo)解算。

步驟4、建立發(fā)射慣性坐標(biāo)系下GNSS/SINS深組合導(dǎo)航系統(tǒng)的狀態(tài)方程和觀測(cè)方程，具體如下：

(4.1)狀態(tài)方程為：

其中，X_s為SINS子系統(tǒng)的狀態(tài)誤差量；X_g為選取的GNSS的狀態(tài)誤差量，具體如下：

其中，為系統(tǒng)的姿態(tài)失準(zhǔn)角；δV_x、δV_y、δV_z為發(fā)射慣性坐標(biāo)系下三軸方向的速度誤差；δX、δY、δZ為發(fā)射慣性坐標(biāo)系下三軸方向的位置誤差；ε_x、ε_y、ε_z和▽_x、▽_y、▽_z分別為陀螺常值漂移、加速度計(jì)常值偏置在三軸方向的分量，Δl_u為等效鐘差的距離誤差，Δl_ru為與鐘漂等效的距離率誤差；

其中，T_ru為GNSS時(shí)鐘頻率漂移的相關(guān)時(shí)間，w_u為GNSS時(shí)鐘誤差白噪聲；w_ru為GNSS時(shí)鐘頻率誤差白噪聲；為彈體坐標(biāo)系轉(zhuǎn)換到導(dǎo)航坐標(biāo)系的轉(zhuǎn)換矩陣；I是單位矩陣；G_e如下式：

其中，G_r′觀測(cè)點(diǎn)到地心的矢量；X、Y、Z依次為載體在發(fā)射慣性系下的三軸坐標(biāo)；R_0x、R_0y、R_0z為發(fā)射點(diǎn)地心矢量在發(fā)射慣性坐標(biāo)系內(nèi)的投影分量；分別為G_r′對(duì)X、Y、Z三個(gè)方向求偏導(dǎo)。

矩陣F₁如下式：

其中，依次為加速度計(jì)在發(fā)射慣性坐標(biāo)系(n)下的X、Y、Z三軸的比力值。

系統(tǒng)的噪聲驅(qū)動(dòng)矩陣G_s(t)為：

其中，為坐標(biāo)轉(zhuǎn)換矩陣。

系統(tǒng)的噪聲矩陣w_s(t)為：

w_s(t)＝[ω_gx ω_gy ω_gz ω_ax ω_ay ω_az]^T

其中，w_gx、w_gy、w_gz分別為陀螺儀在X、Y、Z三軸下的高斯白噪聲；w_ax、w_ay、w_az分別為加速度計(jì)在X、Y、Z三軸下的高斯白噪聲。

(4.2)系統(tǒng)觀測(cè)方程

發(fā)射慣性坐標(biāo)系下彈道導(dǎo)彈SINS/GNSS深組合導(dǎo)航系統(tǒng)的觀測(cè)方程分為偽距差和偽距率差兩個(gè)部分，具體為：

其中，m為GNSS接收機(jī)接收到的衛(wèi)星數(shù)目；ρ_I為慣導(dǎo)解算的位置信息計(jì)算所得偽距，ρ_G為GNSS接收機(jī)測(cè)量所得偽距；H(·)為非線性量測(cè)函數(shù)；v(t)為各元素為零均值的高斯白噪聲。

步驟5、將抗差估計(jì)理論中的抗差M估計(jì)算法和自適應(yīng)因子結(jié)合到容積卡爾曼濾波(CKF)算法中，得出一種自適應(yīng)抗差容積卡爾曼濾波(ARCKF)算法，并對(duì)系統(tǒng)狀態(tài)進(jìn)行濾波校正，具體如下：

根據(jù)步驟4所建立的模型，進(jìn)行離散化，得到下式：

其中，X_k為k時(shí)刻的系統(tǒng)狀態(tài)向量；X_k-1為k-1時(shí)刻的系統(tǒng)狀態(tài)向量；F_k為k時(shí)刻的狀態(tài)轉(zhuǎn)移矩陣；Z_k為觀測(cè)向量，H(·)為非線性量測(cè)傳遞函數(shù)；w_k為系統(tǒng)噪聲序列，且滿足下列關(guān)系：其中，Q_k為系統(tǒng)噪聲序列的協(xié)方差陣，是對(duì)稱的非負(fù)定矩陣；v_k量測(cè)噪聲序列。

ARCKF算法流程如下：

(5.1)濾波初始化

其中，x₀為系統(tǒng)狀態(tài)初值；為x₀的均值；P₀為x₀的狀態(tài)誤差協(xié)方差陣；上標(biāo)T為對(duì)該矩陣或向量轉(zhuǎn)置，以下同；E(·)為求數(shù)學(xué)期望。

(5.2)時(shí)間更新，包括以下內(nèi)容：

計(jì)算k-1時(shí)刻Cubature點(diǎn)：

其中，P_k-1|k-1為k-1時(shí)刻的系統(tǒng)狀態(tài)誤差協(xié)方差；S_k-1通過(guò)P_k-1|k-1的Cholesky的分解得到；為k-1時(shí)刻的系統(tǒng)狀態(tài)向量；是第i個(gè)狀態(tài)向量采樣點(diǎn)；

i＝1,2,…,2n。

其中，f(·)為系統(tǒng)狀態(tài)轉(zhuǎn)移非線性函數(shù)；為第i個(gè)狀態(tài)采樣點(diǎn)經(jīng)過(guò)狀態(tài)轉(zhuǎn)移所得容積點(diǎn)值；為一步預(yù)測(cè)狀態(tài)向量值；Q_k-1為系統(tǒng)噪聲協(xié)方差陣；P_k|k-1為一步預(yù)測(cè)狀態(tài)的誤差協(xié)方差陣。

(5.3)量測(cè)更新，包括以下內(nèi)容：

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

其中，S_k|k-1為通過(guò)(5.2)中所得P_k|k-1的Cholesky分解得到；作為量測(cè)更新使用的容積(Cubature)點(diǎn)。

通過(guò)量測(cè)方程傳播Cubature點(diǎn)：

其中，h(·)非線性量測(cè)轉(zhuǎn)移函數(shù)；為第i個(gè)量測(cè)預(yù)測(cè)采樣點(diǎn)；為2n個(gè)量測(cè)采樣點(diǎn)加權(quán)平均所得量測(cè)估計(jì)值；R_k為k時(shí)刻的量測(cè)噪聲協(xié)方差陣；P_yy為量測(cè)值的誤差協(xié)方差陣。

(5.3.1)抗差修正R，具體內(nèi)容如下：

其中，為抗差修正后的量測(cè)值誤差協(xié)方差陣；為與R_k等價(jià)的量測(cè)噪聲協(xié)方差陣，由抗差M估計(jì)方法中的等價(jià)權(quán)矩陣求逆獲得，即

此處采用Huber法求取等價(jià)權(quán)矩陣；設(shè)的矩陣元素為則可有如下方法確定等價(jià)權(quán)矩陣：

其中，分別為等價(jià)權(quán)矩陣的對(duì)角元素與非對(duì)角元素；σ_ii,σ_ij為原R_k陣的對(duì)角元素和非對(duì)角元素；k為常數(shù)，通常取1.2～1.5；v_i為觀測(cè)量z_i的殘差分量，為標(biāo)準(zhǔn)殘差分量，由得出，其中：

其中，(P_yy,k|k-1)_ii為取P_yy,k|k-1矩陣的i行i列元素；z_k為真實(shí)量測(cè)值；為上文求得的量測(cè)估計(jì)值；v_i為觀測(cè)殘差向量v的第i個(gè)元素。

(5.3.2)自適應(yīng)因子調(diào)節(jié)，具體內(nèi)容如下：

動(dòng)力學(xué)模型誤差通常會(huì)整體性地破壞參數(shù)估計(jì)的效果，因此本發(fā)明中考慮采用一個(gè)自適應(yīng)因子對(duì)系統(tǒng)模型進(jìn)行整體修正，具體算法為:用自適應(yīng)因子α_k修正P_k|k-1，等價(jià)于修正Cubature點(diǎn)采樣過(guò)程，使其成為

其中，I為n×n單位矩陣；S_k|k-1通過(guò)P_k|k-1的Cholesky的分解得到的喬里斯基因子；為一步預(yù)測(cè)狀態(tài)向量；為第i個(gè)狀態(tài)向量采樣點(diǎn)；為自適應(yīng)因子倒數(shù)的平方根。

自適應(yīng)因子α_k的確定方法如下:

其中，為抗差修正后的量測(cè)噪聲協(xié)方差陣；為(5.3.1)抗差估計(jì)修正后的量測(cè)誤差協(xié)方差陣，tr(·)為求矩陣的跡；可有下式得出：

其中，v為量測(cè)殘差向量。

(5.3.3)抗差自適應(yīng)后的量測(cè)更新，具體內(nèi)容如下：

通過(guò)量測(cè)方程傳播Cubature點(diǎn)：

其中，為第i個(gè)狀態(tài)向量采樣點(diǎn)；h(·)為非線性量測(cè)傳播函數(shù)；為經(jīng)過(guò)量測(cè)函數(shù)傳播得到的第i個(gè)量測(cè)值。

計(jì)算增益矩陣：

其中，為量測(cè)估計(jì)值；為抗差修正后的量測(cè)噪聲協(xié)方差陣；為量測(cè)值誤差協(xié)方差陣；為矩陣的求逆；P_xy為狀態(tài)誤差與量測(cè)誤差的互協(xié)方差陣；K_k為卡爾曼濾波增益。

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

其中，為k時(shí)刻濾波狀態(tài)更新值；P_k為k時(shí)刻狀態(tài)誤差協(xié)方差陣更新值；為K_k矩陣的轉(zhuǎn)置矩陣。

由ARCKF濾波所估計(jì)的狀態(tài)誤差值對(duì)SINS系統(tǒng)的狀態(tài)量進(jìn)行反饋矯正，完成慣性/衛(wèi)星系統(tǒng)組合導(dǎo)航。

實(shí)施例1

為了對(duì)本發(fā)明方法進(jìn)行說(shuō)明，充分展現(xiàn)出該方法的可靠性與魯棒性，體現(xiàn)該濾波方法在強(qiáng)機(jī)動(dòng)、高動(dòng)態(tài)情況下的濾波性能與效果，進(jìn)行如下仿真試驗(yàn)。將提出的自適應(yīng)抗差容積卡爾曼濾波(ARCKF)方法應(yīng)用于強(qiáng)機(jī)動(dòng)、高動(dòng)態(tài)情況下彈道導(dǎo)彈的GNSS/SINS深組合導(dǎo)航進(jìn)行仿真校驗(yàn)，并且與普通卡爾曼濾波(KF)算法進(jìn)行比較，仿真條件如下：

本方法發(fā)射慣性坐標(biāo)系下GNSS/SINS組合導(dǎo)航系統(tǒng)中，取狀態(tài)方程為偽距、偽距率組合的線性系統(tǒng)方程，取觀測(cè)方程為偽距、偽距率組合的非線性觀測(cè)方程。

(1)仿真條件設(shè)置如下：

彈體初始俯仰角為90°，初始滾轉(zhuǎn)角和偏航角均為0°；初始姿態(tài)誤差均設(shè)為0°；初始位置為：緯度31.98°，經(jīng)度118.8°，高度為0m；發(fā)射慣性坐標(biāo)系下初始速度前向?yàn)?94.8917m/s(地球自轉(zhuǎn)速度)，天向和側(cè)向均為0m/s；發(fā)射方位角為90°；60s～68s間最大加速度達(dá)到60g，完全符合高動(dòng)態(tài)特性。

發(fā)射慣性坐標(biāo)系下超緊組合導(dǎo)航仿真系統(tǒng)的仿真參數(shù)設(shè)置如下表1所示。

表1 仿真參數(shù)設(shè)置

(2)仿真結(jié)果與分析：

仿真結(jié)果如下：圖2為模擬生成的發(fā)射慣性坐標(biāo)系下的真實(shí)彈道軌跡；由于速度更能反映出系統(tǒng)強(qiáng)機(jī)動(dòng)、高動(dòng)態(tài)特性，圖3～5分別給出ARCKF、KF在三個(gè)方向的速度誤差對(duì)比圖。

由上述仿真結(jié)果可以看出，在非高動(dòng)態(tài)時(shí)段，即在0～60s仿真時(shí)段，自適應(yīng)抗差容積卡爾曼濾波(ARCKF)與卡爾曼濾波(KF)取得較相似的仿真結(jié)果；由于高動(dòng)態(tài)時(shí)段60～68s，彈體加速度最高達(dá)到60g，機(jī)動(dòng)性強(qiáng)，經(jīng)卡爾曼濾波(KF)組合的X方向的速度誤差最高達(dá)到-1.5m/s，Y方向速度誤差達(dá)到-0.25m/s，Z方向速度誤差達(dá)到-1.1m/s。經(jīng)過(guò)自適應(yīng)抗差容積卡爾曼濾波(ARCKF)后X方向速度誤差小于0.8/s；Y方向速度誤差小于0.15m/s，Z方向速度誤差小于0.2m/s。ARCKF較KF濾波結(jié)果較有一定改善。由此可證明，本發(fā)明所提出的自適應(yīng)抗差容積卡爾曼濾波(ARCKF)算法有效的提高了導(dǎo)航系統(tǒng)在高動(dòng)態(tài)、強(qiáng)機(jī)動(dòng)的條件下的跟蹤精度與性能。