本發(fā)明涉及GPS/SINS組合導(dǎo)航技術(shù)領(lǐng)域，特別是一種基于魯棒SCKF濾波的緊組合導(dǎo)航方法。

二、

背景技術(shù)：

GPS/SINS(Global Positioning System/Strap down Inertial Navigation System)組合導(dǎo)航融合了各自的優(yōu)點(diǎn)，使導(dǎo)航精度高于各系統(tǒng)單獨(dú)工作的精度。由于導(dǎo)航環(huán)境的多變性和復(fù)雜性，GPS/SINS組合導(dǎo)航的系統(tǒng)模型往往具有非線性的特點(diǎn)，因此研究應(yīng)用于組合導(dǎo)航的非線性濾波極為重要。

傳統(tǒng)非線性濾波包括擴(kuò)展卡爾曼濾波(EKF)和無跡卡爾曼濾波(UKF)兩種。EKF將非線性函數(shù)線性化，會(huì)產(chǎn)生截?cái)嗾`差；UKF對非線性系統(tǒng)有較高的濾波精度，但沒有嚴(yán)格的數(shù)學(xué)推導(dǎo)，在濾波的迭代過程中，由于矩陣分解和求逆，難以保證狀態(tài)協(xié)方差矩陣的正定性。平方根容積卡爾曼濾波(SCKF)是一種新興的非線性濾波方法，由Arasaratnam等人提出。該算法通過嚴(yán)格的數(shù)學(xué)推證明，基于球面徑向規(guī)則采取一組權(quán)值相等的容積點(diǎn)逼近系統(tǒng)狀態(tài)的后驗(yàn)分布，可保證協(xié)方差矩陣的正定性。同時(shí)，傳遞預(yù)測和估計(jì)誤差協(xié)方差陣采用平方差，避免了矩陣分解和求逆運(yùn)算，保證了其半正定性。該算法被廣泛運(yùn)用于混合濾波、姿態(tài)估計(jì)和導(dǎo)航制導(dǎo)等領(lǐng)域。

然而，在實(shí)際運(yùn)用中，由于內(nèi)、外部環(huán)境不確定因素的影響，還會(huì)發(fā)生觀測異常等擾動(dòng)，從而導(dǎo)致較大的估計(jì)誤差。

三、

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

本發(fā)明的目的在于提供一種實(shí)時(shí)性好、精度高的基于魯棒SCKF濾波的緊組合導(dǎo)航方法，以消除觀測異常對系統(tǒng)的影響并提高組合濾波的性能。

實(shí)現(xiàn)本發(fā)明目的的技術(shù)解決方案為：一種基于魯棒SCKF濾波的緊組合導(dǎo)航方法，其特征在于，包括以下步驟：

步驟1、通過軌跡發(fā)生器、衛(wèi)星信號模擬器依次模擬生成導(dǎo)彈的IMU數(shù)據(jù)和GPS中頻信號；

步驟2、將模擬生成的IMU數(shù)據(jù)進(jìn)行慣導(dǎo)解算，將GPS中頻信號注入接收機(jī)進(jìn)行導(dǎo)航解算；

步驟3、建立發(fā)射慣性坐標(biāo)系下GPS/SINS緊組合導(dǎo)航系統(tǒng)模型；

步驟4、在標(biāo)準(zhǔn)平方根容積卡爾曼濾波即SCKF的基礎(chǔ)上，引入穩(wěn)健M，對系統(tǒng)觀測噪聲陣進(jìn)行自適應(yīng)調(diào)節(jié)，構(gòu)成魯棒平方根容積卡爾曼濾波即RSCKF算法，對系統(tǒng)狀態(tài)進(jìn)行濾波校正。

本發(fā)明與現(xiàn)有技術(shù)相比，其顯著優(yōu)點(diǎn)在于：(1)將標(biāo)準(zhǔn)SCKF濾波算法引入組合導(dǎo)航中，符合系統(tǒng)模型的非線性特性，與非線性濾波UKF、CKF相比，避免了矩陣分解和求逆運(yùn)算，保證了協(xié)方差陣的半正定性；(2)將抗差M估計(jì)原理引入SCKF濾波中，進(jìn)一步增強(qiáng)了組合濾波的魯棒性，提高了在惡劣環(huán)境中的組合導(dǎo)航定位精度、跟蹤能力。

四、附圖說明

圖1是本發(fā)明基于魯棒SCKF濾波的緊組合導(dǎo)航方法的流程圖。

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

圖3是實(shí)施例1中魯棒SCKF與SCKF濾波在X方向的定位誤差對比結(jié)果。

圖4是實(shí)施例1中魯棒SCKF與SCKF濾波在Y方向的定位誤差對比結(jié)果。

圖5是實(shí)施例1中魯棒SCKF與SCKF濾波在Z方向的定位誤差對比結(jié)果。

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

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

結(jié)合圖1，本發(fā)明基于強(qiáng)跟蹤UKF濾波的GPS/SINS緊組合導(dǎo)航方法，步驟如下：

步驟1、通過軌跡發(fā)生器、衛(wèi)星信號模擬器依次模擬生成導(dǎo)彈的IMU數(shù)據(jù)和GPS中頻信號；

根據(jù)彈道導(dǎo)彈飛行的物理模型，設(shè)置飛行各階段的參數(shù)，如加速度、角速率變化，可生成導(dǎo)彈的飛行彈道軌跡與相應(yīng)的IMU數(shù)據(jù)，將飛行彈道軌跡導(dǎo)入衛(wèi)星模擬器并處理即可得到相應(yīng)的GPS中頻信號。

步驟2、模擬生成的IMU數(shù)據(jù)進(jìn)行慣導(dǎo)解算，將GPS中頻信號注入軟件接收機(jī)進(jìn)行導(dǎo)航解算；

慣導(dǎo)解算更新彈體的位置、速度、姿態(tài)信息；軟件接收機(jī)導(dǎo)航解算得到彈體位置，衛(wèi)星位置、偽距信息。

步驟3、建立發(fā)射慣性坐標(biāo)系下GPS/SINS緊組合導(dǎo)航系統(tǒng)模型，具體如下：

(3.1)狀態(tài)方程：

其中，X_s包含SINS的15個(gè)狀態(tài)誤差量；X_g包含GPS的2個(gè)狀態(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和分別為陀螺常值漂移、加速度計(jì)常值偏置在三軸方向的分量，Δl_u為等效鐘差的距離誤差，Δl_ru為與鐘漂等效的距離率誤差；

其中，T_ru為GNSS時(shí)鐘頻率漂移的相關(guān)時(shí)間，w_u為GNSS時(shí)鐘誤差白噪聲；w_ru為GNSS時(shí)鐘頻率誤差白噪聲；F₁為X、Y、Z三軸比力的反對稱矩陣；G_e為觀測點(diǎn)到地下矢量的三軸偏導(dǎo)矩陣；為彈體坐標(biāo)系轉(zhuǎn)換到導(dǎo)航坐標(biāo)系的轉(zhuǎn)換矩陣；I是單位矩陣；系統(tǒng)的噪聲驅(qū)動(dòng)矩陣G_s(t)為：

系統(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三軸下的高斯白噪聲。

(3.2)系統(tǒng)觀測方程

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

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

步驟4、在標(biāo)準(zhǔn)平方根容積卡爾曼濾波(SCKF)基礎(chǔ)上，引入穩(wěn)健M，對系統(tǒng)觀測噪聲陣進(jìn)行自適應(yīng)調(diào)節(jié)，構(gòu)成魯棒平方根容積卡爾曼濾波(RSCKF)算法，對系統(tǒng)狀態(tài)進(jìn)行濾波校正，具體如下：

首先將步驟3中建立的系統(tǒng)模型進(jìn)行離散化，得到下式：

式中：X_k∈Rⁿ,Z_k∈R^m分別為k時(shí)刻系統(tǒng)的狀態(tài)向量和量測向量；F_k為線性狀態(tài)轉(zhuǎn)移矩陣，h(·)為系統(tǒng)非線性量測函數(shù)；w_k、v_k為互不相關(guān)的零均值高斯白噪聲序列，統(tǒng)計(jì)特性滿足如下條件：

式中：Q_k為非負(fù)定矩陣，R_k為正定矩陣；δ_kj為Kronecker-δ函數(shù)；

要實(shí)現(xiàn)SCKF算法，首先按照三階容積準(zhǔn)則，選取一組2n個(gè)等權(quán)值分布的容積點(diǎn){ω_i，ξ_i}實(shí)現(xiàn)非線性逼近，其中：

其中：ξ_i為容積點(diǎn)向量；ω_i為對應(yīng)權(quán)重；n為系統(tǒng)狀態(tài)變量的維數(shù)；[l]∈Rⁿ，為生成算子，當(dāng)n＝2時(shí)，表示為如下所示點(diǎn)集：

[l]_i是[l]∈R²中第i列元素；SCKF算法具體步驟如下：

1)濾波初始化

S₀＝chol(P₀)

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

2)時(shí)間更新：

魯棒平方根容積卡爾曼濾波的時(shí)間更新標(biāo)準(zhǔn)平方根卡爾曼濾波一致；

其中，S_k-1為k-1時(shí)刻的平方根因子；為k-1時(shí)刻的狀態(tài)向量更新值；為第i個(gè)狀態(tài)容積采樣點(diǎn)；u_k-1為系統(tǒng)輸入量；f(·)為非線性狀態(tài)轉(zhuǎn)移函數(shù)；為第i個(gè)一步預(yù)測狀態(tài)采樣值；為加權(quán)平均得到的系統(tǒng)狀態(tài)一步預(yù)測值；Tria(·)表示QR分解；S_k|k-1為估計(jì)的k時(shí)刻平方根因子；矩陣表示為：

3)量測更新：

與標(biāo)準(zhǔn)SCKF量測更新過程一致，僅對噪聲矩陣R做出改動(dòng)：

其中，為量測更新的第i個(gè)容積采樣點(diǎn)；h(·)為非線性量測函數(shù)；為第i個(gè)量測值采樣點(diǎn)；為量測值采樣點(diǎn)加權(quán)平均所得的量測估計(jì)值；為根據(jù)穩(wěn)健M估計(jì)算法對噪聲R進(jìn)行自適應(yīng)調(diào)節(jié)后的觀測噪聲矩陣；矩陣ζ_k|k-1表示為：

估計(jì)互協(xié)方差矩陣為：

其中，矩陣χ_k|k-1表示為：

S_k＝Tria([χ_k|k-1-K_k·ζ_k|k-1,K_k·S_R,k])

其中，K_k為所求得k時(shí)刻的卡爾曼濾波增益；S_k為為經(jīng)過QR分解的平方根因子；其它參數(shù)定義如上。

4)抗差修正R，具體步驟如下：

基于標(biāo)準(zhǔn)SCKF方程，建立抗差SCKF的濾波模型；由于量測信息只影響模型中的量測更新過程，所以相對于標(biāo)準(zhǔn)SCKF算法，魯棒SCKF算法僅對量測更新方程中的相關(guān)表達(dá)式進(jìn)行了調(diào)整修正，抗差噪聲陣為與R_k等價(jià)的量測噪聲方差陣，由抗差M估計(jì)方法中的等價(jià)權(quán)矩陣求逆獲得，即

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

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

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

將抗差修正得到的代入步驟3)量測更新中，即得到魯棒平方根容積容積卡爾曼濾波。按照以上步驟，進(jìn)行濾波后，得到系統(tǒng)中位置、速度、姿態(tài)狀態(tài)量的誤差量，再對組合導(dǎo)航的SINS狀態(tài)進(jìn)行校正，輸出最終的導(dǎo)航結(jié)果。

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

實(shí)施例1

為了驗(yàn)證本發(fā)明提出的魯棒SCKF濾波方法的有效性與優(yōu)越性，將此濾波方法運(yùn)用在彈載GPS/SINS緊組合導(dǎo)航系統(tǒng)中進(jìn)行仿真，并將仿真導(dǎo)航結(jié)果與標(biāo)準(zhǔn)SCKF濾波結(jié)果比較。仿真如下：

1)仿真條件

設(shè)置彈體初始俯仰為90°，初始滾轉(zhuǎn)角和偏航角均為0°；初始位置為：緯度30.03°，經(jīng)度109.6°，高度為10m；初始速度：前向?yàn)?94.8917m/s(地球自轉(zhuǎn)速度)，天向和側(cè)向均為0m/s；發(fā)射方位角為90°；陀螺零偏設(shè)為10°/h，白噪聲設(shè)為1°/h；加速度計(jì)零偏設(shè)為1mg，白噪聲設(shè)為0.5mg；收星數(shù)m＝4。GPS采樣頻率1HZ，INS采樣頻率200HZ，濾波周期1s，仿真時(shí)間360s。在200s-210s時(shí)間段內(nèi)，使GPS偽距加入白噪聲滿足均值0，標(biāo)準(zhǔn)差100的高斯分布；GPS偽距率加入白噪聲滿足均值為0，標(biāo)準(zhǔn)差為1的高斯分布。

設(shè)置導(dǎo)彈的彈道軌跡如圖2；圖3-5分別為標(biāo)準(zhǔn)SCKF和魯棒SCKF緊組合導(dǎo)航在X、Y、Z三個(gè)方向的定位誤差。

2)結(jié)果分析

圖2-圖4為分別為經(jīng)過SCKF、抗差SCKF濾波的組合導(dǎo)航位置誤差對比圖?？梢钥闯?，在初始位置誤差最高達(dá)到10m，但都能快速收斂。在20s～200s時(shí)段，GPS/SINS組合導(dǎo)航系統(tǒng)的量測噪聲處于正常狀態(tài)的情況下，兩種濾波算法均能實(shí)現(xiàn)導(dǎo)彈的準(zhǔn)確定位；在200s～210s時(shí)段，由于受到加入的偽距、偽距率觀測粗差影響，標(biāo)準(zhǔn)SCKF存在X方向誤差[-9.5m，4.1m]，Y方向定位誤差[-9.6m，3.8m]，Z方向定位誤差[-3.6m，3.8m]，且收斂時(shí)間較長；而抗差SCKF濾波的三軸方向誤差均保持在[-1.5m，2.1m]區(qū)間內(nèi)，定位結(jié)果明顯優(yōu)于標(biāo)準(zhǔn)SCKF算法；在250s～360s時(shí)段，標(biāo)準(zhǔn)SCKF濾波收斂，達(dá)到與抗差SCKF相當(dāng)?shù)男Ч?。因此，可得出結(jié)論：本發(fā)明提出的一種基于魯棒SCKF濾波的緊組合導(dǎo)航方法，在滿足了正常情況下的組合導(dǎo)航工作的同時(shí)，減小了觀測量異常誤差常生的影響，有效改善了濾濾波性能，提高了導(dǎo)航定位精度。