[發明專利]基于直接導航模型的無跡卡爾曼非線性初始對準方法

有效

• 申請號: 201310724264.7
• 申請日: 2013-12-25
• 公開（公告）號: CN103644913A
• 公開（公告）日: 2014-03-19
• 發明（設計）人: 冉昌艷;程向紅;王磊;陳紅梅;戴晨曦
• 申請（專利權）人: 東南大學
• 主分類號: G01C21/16
• 分類號: G01C21/16
• 代理公司: 江蘇永衡昭輝律師事務所 32250
• 代理人: 王斌

1.基于直接導航模型的無跡卡爾曼非線性初始對準方法，包括下列步驟：

步驟1、根據慣性測量單元中陀螺儀測得的角速度信息和加速度計測得的線加速度信息，使用基于凝固解析粗對準算法完成捷聯慣性導航系統粗對準，求得粗略的初始縱搖角θ、橫搖角γ和航向角ψ；

步驟2、根據捷聯慣性導航系統的速度微分方程、歐拉角微分方程、傳感器模型，以導航系速度為量測量，建立基于導航參數微分方程的非線性濾波連續系統模型；

步驟3、將步驟2中建立的非線性連續系統模型離散化，形成非線性離散系統模型；

步驟4、根據步驟3中建立的非線性離散系統模型構建用于初始對準的簡化無跡卡爾曼非線性濾波器，濾波算法中，時間更新與量測更新不同步；

步驟5、將步驟1提取的粗對準姿態角以及陀螺儀和加速度計的輸出代入步驟4中，完成捷聯慣性導航系統初始對準，直接輸出導航參數。

2.根據權利要求1所述的基于直接導航模型的無跡卡爾曼非線性初始對準方法的步驟，其中所述的步驟2根據捷聯慣性導航系統的速度微分方程、歐拉角微分方程、傳感器模型，以導航系速度為量測量，建立基于導航參數微分方程（即直接導航模型）的非線性濾波連續系統模型，具體為：

捷聯慣性導航系統的速度微分方程為

v.enn=fn-(2ωien+ωenn)×venn+gn---(1)]]>

式中，v_e、v_n和v_u分別為東、北、天速度，[.]^T表示矩陣轉置；

ω_ie為地球自轉角速率，L為當地地理緯度；

R_n為子午圈曲率半徑，R_e為卯酉圈曲率半徑；R_n=R_q(1-2e+3esin²L)，R_e=R_q(esin²L+1)，R_q為參考橢球赤道平面半徑，e為地球扁率；

gⁿ=[0,0,-g]^T，g為重力加速度；

fn=Cbnfibb,]]>且fibb=[f~ibxb-δfibxb,f~ibyb-δfibyb,f~ibzb-δfibzb]T,]]>和分別為三只加速度計的測量值，δfibxb=▿x+wax,δfibyb=▿y+way,δfibzb=▿z+waz,]]>和分別為三只加速度計的測量誤差，分為兩部分，一部分為隨機常值▽_x、▽_y和▽_z，一部分為隨機噪聲w_ax、w_ay和w_az；

Cnb=cosγcosψ-sinθsinγsinψcosγsinψ+sinθsinγcosψ-cosθsinγ-cosθsinψcosθcosψsinθsinγcosψ+sinθcosγsinψsinγsinψ-sinθcosγcosψcosθcosγ.]]>

捷聯慣性導航系統的歐拉角微分方程

θ.γ.ψ.=1cosθcosθcosγ0sinγcosθsinθsinγcosθ-cos-sinγ0cosγγsinθωnbxbωnbybωnbzb---(2)]]>

式中，ωnbb=ωibb-Cnb(ωien+ωenn),]]>ωibb=[ω~ibxb-δωibxb,ω~ibyb-δωibyb,ω~ibzb-δωibzb]T,]]>和為三只陀螺儀測量值，δωibxb=ϵx+wgx,δωibyb=ϵy+wgy,δωibzb=ϵz+wgz,]]>和為三只陀螺儀的測量誤差，分為兩部分，一部分為隨機常值ε_x、ε_y和ε_z，一部分為隨機噪聲w_gx、w_gy和w_gz；

捷聯慣性導航系統的傳感器模型

▿.x=0,▿.y=0,▿.z=0,ϵ.x=0,ϵ.y=0,ϵ.z=0]]>

(3)]]>

取12維系統狀態向量為

x＝[v_e,v_n,v_u,P,R,H,▽_x,▽_y,▽_z,ε_x,ε_y,ε_z]^T

式中，x為狀態向量；

系統噪聲向量為

w＝[w_ax,w_ay,w_az,w_gx,w_gy,w_gz,0,0,0,0,0,0]^T

式中，w為系統噪聲向量；

取3維量測向量為

z＝[v_e,v_n,v_u]^T

式中，z為量測向量；

量測噪聲向量為

η＝[w_ve,w_vn,w_vu]^T

式中，η為量測噪聲向量，w_ve、w_vn、w_vu分別為東、北、天速度的量測噪聲，假設為零均值高斯白噪聲；

根據捷聯慣性導航系統的速度微分方程、姿態歐拉角微分方程和傳感器模型以及量測量建立的基于導航參數微分方程（即直接導航模型）的非線性濾波連續系統模型為：

x.=F(x)+G(x)wz=Hx+η]]>

式中，H=[I_3×30_3×9]；F(.)為非線性的狀態函數，F(x)和G(x)參照式（1）-（3）寫出。