基于自適應(yīng)upf的sins大方位失準(zhǔn)角初始對(duì)準(zhǔn)方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種基于自適應(yīng)UPF的SINS大方位失準(zhǔn)角初始對(duì)準(zhǔn)方法，屬于導(dǎo)航技 術(shù)領(lǐng)域。
【背景技術(shù)】
[0002] 隨著導(dǎo)航系統(tǒng)應(yīng)用領(lǐng)域的不斷拓展，大多數(shù)應(yīng)用環(huán)境不能滿足初始失準(zhǔn)角為大 角度和噪聲為高斯白噪聲的條件，此時(shí)繼續(xù)使用傳統(tǒng)導(dǎo)航系統(tǒng)線性化模型和KF(Kalman Filter，卡爾曼濾波）將會(huì)產(chǎn)生較大的模型誤差和估計(jì)誤差，使得導(dǎo)航參數(shù)不可信。針對(duì)這 種情況，國內(nèi)外的研究主要分為兩個(gè)方面：一是慣性導(dǎo)航系統(tǒng)的非線性模型的研究，二是非 線性濾波器的研究。常用的非線性濾波方法有EKF(ExtendedKalmanFilter，擴(kuò)展卡爾曼 濾波）、UKF(UnscentedKalmanFilter，無跡卡爾曼濾波）、PF(ParticleFilter，粒子濾 波）、EKF-PF、UKF-PF(UPF)，在一定程度上UPF的使用限制更少、濾波效果要優(yōu)于其他幾種。

【發(fā)明內(nèi)容】

[0003] 發(fā)明目的：本發(fā)明的目的在于針對(duì)大多數(shù)應(yīng)用環(huán)境不能滿足初始失準(zhǔn)角為大角度 和噪聲為高斯白噪聲的條件以及UPF的不足進(jìn)行改進(jìn)，本發(fā)明提出一種基于強(qiáng)跟蹤濾波器 技術(shù)的自適應(yīng)UPF的大方位失準(zhǔn)角初始對(duì)準(zhǔn)的方法。本發(fā)明將UPF和自適應(yīng)UPF兩種非線 性濾波器在初始對(duì)準(zhǔn)中的濾波效果進(jìn)行了比較，結(jié)果表明自適應(yīng)UPF比UPF具有更快的收 斂速度并且在一定程度上提高了對(duì)準(zhǔn)精度。本發(fā)明的方法是通過引入衰減記憶因子有效增 強(qiáng)當(dāng)前信息殘差對(duì)系統(tǒng)修正作用，在一定程度上降低了由于系統(tǒng)簡化、噪聲統(tǒng)計(jì)特性不確 定對(duì)系統(tǒng)造成的影響，同時(shí)較好的克服了UPF中粒子退化的現(xiàn)象。
[0004] 技術(shù)方案：本發(fā)明所述的基于自適應(yīng)UPF的SINS大方位失準(zhǔn)角初始對(duì)準(zhǔn)方法，包 括以下步驟：
[0005] 步驟1:建立SINS非線性誤差模型：
[0006] 使用歐拉平臺(tái)誤差角來表示理想導(dǎo)航坐標(biāo)系與計(jì)算導(dǎo)航坐標(biāo)系之間的失準(zhǔn)角，且 該組誤差角需要考慮轉(zhuǎn)動(dòng)的先后順序，建立相應(yīng)的SINS非線性誤差模型；
[0007] 本發(fā)明坐標(biāo)系選取如下：
[0008]i系--地心慣性坐標(biāo)系，原點(diǎn)位于地心0，X;軸指向春分點(diǎn)，z;軸沿地球自轉(zhuǎn)軸， 軸與Xi、Zl構(gòu)成右手系；
[0009]e系--地球坐標(biāo)系，原點(diǎn)位于地心，xe軸穿越本初子午線與赤道的交點(diǎn)，ze軸穿 越地球北極點(diǎn)，軸穿越東經(jīng)90°子午線與赤道的交點(diǎn)；
[0010] n系 導(dǎo)航坐標(biāo)系，這里選取"東-北 -天（E_N_U) "地理坐標(biāo)系為導(dǎo)航坐標(biāo)系；
[0011] b系 "右-前-上"坐標(biāo)系為載體坐標(biāo)系（b系）；
[0012] n系先后依次經(jīng)過三次歐拉角轉(zhuǎn)動(dòng)至b系，這三個(gè)歐拉角分別記為航向角 邊G(-31 31]、俯仰角0G[-31/2 31/2]和橫滾角YG(-31 31]，n系與b系之間的旋 轉(zhuǎn)變換關(guān)系可用姿態(tài)矩陣cr描述；
[0013] 步驟2:SINS大方位失準(zhǔn)角初始對(duì)準(zhǔn)誤差模型：
[0014] 現(xiàn)假設(shè)兩水平失準(zhǔn)角都為小角度；假設(shè)陀螺測量誤差主要為常值漂移誤差 eb和零均值高斯白噪聲加速度計(jì)測量誤差主要為常值偏置誤差和零均值高斯 白噪聲忽略重力誤差項(xiàng)sgn，靜基座下>? = 成立，則初始對(duì)準(zhǔn)濾波模型的狀態(tài)方 程為：
[0015] 丨V、二U
[0016] 令狀態(tài)向量#/ (加f(AMvYf:,噪聲向量價(jià)# 濾波狀態(tài)模型，并直接以SINS速度誤差z=S，作為觀測量建立觀測方程如下：
[0017]
[0018] f(x)和g(x)的具體表達(dá)式參考上式的初始對(duì)準(zhǔn)濾波模型的狀態(tài)方程，H= [0I 0 0]，v為量測噪聲；
[0019] 步驟3 :標(biāo)準(zhǔn)UPF算法：
[0020] 假設(shè)初始狀態(tài)變量X。~p(X。)，方差陣為P。，對(duì)觀測方程進(jìn)行無跡粒子濾波，算法 如下：
[0021] 步驟3. 1:初始化：k=0 ;從初始的先驗(yàn)概率分布p(x。）中進(jìn)行粒子；^4)，i= 1， 2,3."』的采樣，為簡化計(jì)算，令；^~1(藝'4/)，其中斤1：=扃' /^=4;[0022] 步驟3.2:加權(quán)粒子的預(yù)測、采樣：k= 1，2,利用無跡卡爾曼濾波對(duì)粒子進(jìn)行預(yù)測更新，并計(jì)算〇樣本點(diǎn)：[0023]
[0024]
[0025]
[0026] 式中，;/ ：=：^Jn+X，入=a2(n+i〇-n，10a< 1，k= 3_n，< :為估計(jì)均 方誤差，n為狀態(tài)維數(shù)；
[0027] 時(shí)間更新為： 「00281
[0033]入=a2(n+K)-n，10a彡 1，k= 3-n，對(duì)于正態(tài)分布而言，# =2,/g4 為 預(yù)測均方差，W為權(quán)值，Z為量測量，K為濾波增益；
[0034] 量測更新為：
[0035]
[0036] 以推薦密度函數(shù)wdga)，々)）忽V(藝，]，/^)生成粒子;^作為二次采樣 的原始粒子；
[0037]步驟3. 3:根據(jù)權(quán)值更新公式
對(duì)N個(gè)粒子相應(yīng)的權(quán)值進(jìn)行計(jì)算并做歸一化處理；其中， (八~ "二、VM .￡，：
；
[0038] 步驟3. 4 :通過重采樣算法對(duì)原始粒子(i= 1，2,…，N)作二次采樣生成二次 采樣粒子(j= 1，2,…，N)，并計(jì)算每個(gè)二次采樣粒子的權(quán)重系數(shù)；
[0039] 步驟3. 5 :根據(jù)t 計(jì)算狀態(tài)變量的最優(yōu)估計(jì)及每個(gè)粒子對(duì)應(yīng)的方差 i:=i 矩陣；
[0040] 步驟3. 6 :將步驟3. 4重采樣后的粒子尤°及步驟3. 5計(jì)算的if1代入第2步進(jìn)行 迭代運(yùn)算；
[0041] 步驟4 :自適應(yīng)UPF算法：
[0042] 基于強(qiáng)跟蹤濾波的思想，采用時(shí)變漸消因子削弱陳舊數(shù)據(jù)對(duì)當(dāng)前濾波值的影響， 實(shí)時(shí)調(diào)整狀態(tài)預(yù)測誤差的協(xié)方差以及相應(yīng)的增益矩陣來達(dá)到。這在一定程度上能夠減緩 UPF粒子退化的現(xiàn)象，并且加快粒子濾波的收斂速度。
[0043] 本發(fā)明采取的自適應(yīng)措施是對(duì)濾波器的協(xié)方差進(jìn)行判斷，具體判斷公式如下：
[0044]
[0045] 其中，S為設(shè)定的調(diào)整系數(shù)，一般取S
> 1，此處取S= 1. 5 ; &為系統(tǒng)的殘差序列， 4 二A-hCD;
[0046] 當(dāng)上式的判斷公式不成立時(shí)，需要對(duì)巧；L:進(jìn)行有效修正，本發(fā)明采用的方式是引 入哀減記憶因子的自適應(yīng)加權(quán)系數(shù)Ak，其具體的定義如下：
[0047]
[0048] 修正公式為：[0049]
[0050]
[0051]
[0052]
[0053] 式中，P-般取0<P< 1，其主要作用是增強(qiáng)濾波器對(duì)系統(tǒng)狀態(tài)的快速跟蹤能 力，其越大，則當(dāng)前信息分配的權(quán)值也越大，當(dāng)前信息的殘差對(duì)系統(tǒng)估計(jì)的影響也更突出。 為了保證系統(tǒng)對(duì)緩慢變化情況和突變狀態(tài)的強(qiáng)跟蹤能力，本發(fā)明取P= 〇. 95 ;
[0054] 進(jìn)一步地，步驟1具體包括：
[0055] 步驟1. 1 :建立姿態(tài)誤差方程
[0056] 實(shí)際工作的導(dǎo)航系統(tǒng)由于存在各種干擾和量測誤差，SINS的計(jì)算的導(dǎo)航坐標(biāo)系 (n'系）通常與理想的導(dǎo)航坐標(biāo)系（n系）之間存在轉(zhuǎn)動(dòng)誤差，n系需按照一定的順序依次 轉(zhuǎn)過三個(gè)角度才能與n'系重合，現(xiàn)假設(shè)這三次轉(zhuǎn)動(dòng)依次繞z軸、x軸、y軸旋轉(zhuǎn)，且轉(zhuǎn)過的 角度分別記為0、A、％，其矢量表示形式為P= %A]、三次旋轉(zhuǎn)對(duì)應(yīng)的姿 態(tài)變換陣依次為、Cs._、. 故n系至n'系的變換矩陣為：
[0057]
[0058
[0059] 若設(shè)n'系相對(duì)于n系的角速度為，則與歐拉平臺(tái)誤差角@的關(guān)系可表示 如下：
[0060]
[0061] 則由上式可得歐拉平臺(tái)誤差角微分方程：
[0062]
[0065] 在導(dǎo)航坐標(biāo)系中，SINS的姿態(tài)矩陣微分方程表示如下：
[0066]
[0067] 式中，運(yùn)算符（?X)表示由向量?構(gòu)成的反對(duì)稱矩陣，符號(hào)表示坐標(biāo)系c相對(duì) 于坐標(biāo)系b的運(yùn)動(dòng)向量在坐標(biāo)系d中的投影，符號(hào)兮;表示b系至c系的變換矩陣，而實(shí)際 上用于姿態(tài)更新的微分方程是帶有誤差的，其形式可以表示為：
[0068]
[0069] 式中，符號(hào)"~"表不測量值，私。為成的測量 值，為兩；：的計(jì)算誤差，為陀螺測量誤差；令理論姿態(tài)矩陣與實(shí)際姿態(tài)矩陣的解算 誤差為：
[0070]
[0071] 式中，I表示單位矩陣，對(duì)AC=Q兩邊同時(shí)求導(dǎo)，并將SINS的姿態(tài)矩陣微 分方程和實(shí)際用于姿態(tài)更新的微分方