[發明專利]一種基于五階CKF的GPS/SINS/CNS組合導航方法

有效

• 申請號: 201610070674.8
• 申請日: 2016-02-01
• 公開（公告）號: CN105737823B
• 公開（公告）日: 2018-09-21
• 發明（設計）人: 徐曉蘇;劉心雨;孫進;楊博;王捍兵
• 申請（專利權）人: 東南大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01S19/48;G01S19/49
• 代理公司: 南京蘇高專利商標事務所(普通合伙) 32204
• 代理人: 李昊

1.一種基于五階CKF的GPS/SINS/CNS組合導航方法，其特征在于，包括如下步驟：

步驟1：建立以SINS誤差方程為基礎的組合導航系統非線性誤差方程和線性量測方程；

步驟1.1：選取“東北天”地理坐標系為導航坐標系n系；選取載體“右前上”坐標系為載體坐標系b系；n系先后經過3次歐拉角轉動至b系，三個歐拉角記為航向角ψ∈(-π,π]，縱搖角橫搖角γ∈(-π,π]；n系與b系之間的姿態矩陣記為真實姿態角記為真實速度記為真實地理坐標系為P＝[L λ H]^T，其中，L為緯度，λ為經度，H為高度；SINS解算出的姿態角記為速度記為地理坐標系為SINS解算出的數學平臺記為n′系，n′系與b系之間的姿態矩陣記為記姿態角誤差為速度誤差為：位置誤差為：則非線性誤差模型如下：

其中，為b系下三軸陀螺的常值誤差，為b系下三軸陀螺的隨機誤差為零均值白噪聲過程，為b系下三軸加速度常值誤差，為n系下三軸加速度的隨機誤差為零均值白噪聲過程，為加速度計的實際輸出，是b系下數學平臺旋轉角速度，為解算出的數學平臺旋轉角速度，為地球旋轉角速度，為解算出的數學平臺相對于地球的旋轉角速度，為對應的計算誤差，R_M、R_N分別為當地子午圈卯西圈曲率半徑，為歐拉角微分方程系數矩陣的逆矩陣；為n′系與n系之間的姿態矩陣，C_ω和具體為：

步驟1.2：所述非線性誤差方程和線性量測方程建立過程如下：

選取速度誤差歐拉角平臺誤差角φ_e、φ_n、φ_u；位置誤差δL、δλ；b系下的加速度計常值誤差以及陀螺常值誤差以及構成12維狀態向量，H近似為零：

和P只取前兩維狀態，和為零均值白噪聲過程；該非線性狀態方程可簡記為：以采樣周期T作為濾波周期，可以使用四階龍格-庫塔積分方法，以T為步長將其離散化，具體步驟如下：

假設選取的狀態向量在初始時刻的值x₀已知，且記t₁＝t+T/2，則可以通過以下迭代公式將離散：

Δx₁＝[f(x,t)+ω(t)]T

Δx₂＝[f(x,t₁)+Δx₁/2+ω(t₁)]T

Δx₃＝[f(x,t₁)+Δx₂/2+ω(t₁)]T

Δx₄＝[f(x,t+T)+Δx₃+ω(t+T)]T

x_k+1＝x_k+(Δx₁+2Δx₂+2Δx₃+Δx₄)/6

記離散后狀態濾波方程為：x_k＝f(x_k-1)+ω_k-1

線性量測方程建立過程如下：

選取GPS速度誤差M_e、M_n分別為GPS接收機的測速誤差項在東向北向坐標軸上的分量；

速度量測量矢量定義如下：

式中，H_v(t)＝[diag[1,1]0_2×10]

在捷聯工作模式下，由CNS輸出的姿態信息可以得到載體的三軸姿態信息而SINS通過慣導解算也會給出載體的三軸姿態信息為因此將兩者相減可得到載體的三軸姿態誤差角Δφ：

Δφ'＝M×Δφ

式中M為姿態誤差角轉換矩陣：

在組合導航系統中將Δφ'作為觀測值建立系統的量測模型，通過最優估計的方法實時估計導航系統中慣性器件的誤差，并以此對SINS進行修正；

量測方程如下：

式中，H_φ＝[0_3×2 I_3×3 0_3×7]

由以上分析可知，全組合的量測方程為：

同樣以T作為濾波周期，并以T作為步長進行簡單離散化，

由狀態方程和量測方程組成如下濾波方程：

步驟2：利用五階球面徑向容積規則準則構建五階容積卡爾曼濾波器；

步驟3：利用五階CKF對SINS與GPS、CNS輸出的信息進行濾波、融合，得到導航參數的最優估計并修正速度信息，位置信息以及捷聯姿態矩陣，以得到精確的導航信息。