[發明專利]一種長時間導航系統全參數精度評估方法

在審

• 申請號: 201410125919.3
• 申請日: 2014-03-31
• 公開（公告）號: CN104949687A
• 公開（公告）日: 2015-09-30
• 發明（設計）人: 劉沖;徐海剛;李海軍;裴玉鋒
• 申請（專利權）人: 北京自動化控制設備研究所
• 主分類號: G01C25/00
• 分類號: G01C25/00
• 代理公司: 核工業專利中心 11007
• 代理人: 高尚梅

1.一種長時間導航系統全參數精度評估方法，其特征在于：利用局部基準系統航行實驗的原始數據及衛星信息，采用基于RTS平滑的離線處理方法，通過對存儲的慣導數據及衛星數據的處理，得到基準系統高精度的位置、姿態及方位信息，作為基準系統的全參數精度評估的基準；

具體包括以下兩個步驟以獲取位置、速度及航姿數據：

（1)利用慣性/衛星數據信息進行前向的閉環卡爾曼濾波過程；

前向卡爾曼濾波算法采取GPS與慣性導航系統位置匹配模式，采用閉環校正方式進行；

（1.1）確定算法模型和狀態誤差量

算法采用18階導航誤差模型，選取18個狀態誤差量為：

X=ΔLΔhΔλΔVnΔVuΔVeΦnΦuΦe▿x▿y▿zϵxϵyϵzRxbRybRzb]]>

其中：

ΔL、Δh、Δλ：慣導的緯度、經度、高度誤差；

ΔV_n、ΔV_u、ΔV_e：慣導北向、天向、東向速度誤差；

Φ_n、Φ_u、Φ_e：慣導北向、天向、東向失準角誤差；

慣導X、Y、Z軸加速度計零偏；

ε_x、ε_y、ε_z：慣導X、Y、Z軸陀螺漂移；

慣導與GPS間桿臂誤差。

（1.2）確定系統狀態方程

系統狀態方程為：

其中：

F=F11F1203×303×303×303×3F21F22F23Cbn03×303×3F31F32F3303×3-Cbn03×303×303×303303×303×303×303×303×303×303×303×303×3]]>

F11=0-Vn(RM+h)20000VeRN+htanLsecL-VesecL(RN+h)20;F12=1RM+h0001000secLRN+h;]]>

F21=-(2VeωiecosL+Ve2sec2LRN+h)VnVu(RM+h)2+Ve2tanL(RN+h)20-2VeωiesinL-(Vn2(RM+h)2+Ve2(RN+h)2)02ωie(VusinL+VncosL)+VeVnsec2LRN+hVeVu-VeVntanL(RN+h)20;]]>

F22=-VuRM+h-VnRM+h-2(ωiesinL+VetanLRN+h)2VnRM+h02(ωiecosL+VeRN+h)2ωiesinL+VetanLRN+h-(2ωiecosL+VeRN+h)VntanL-VuRN+h;]]>

F23=0-fefufe0-fn-fufn0;]]>

F31=-ωiesinL-Ve(RN+h)20ωiecosL+Vesec2LRN+h-VetanL(RN+h)200Vn(RM+h)20;F32=001RN+h00tanLRN+h-1RM+h00;]]>

F33=0-VnRM+h-(ωiesinL+VetanLRN+h)VnRM+h0ωiecosL+VeRN+hωiesinL+VetanLRN+h-(ωiecosL+VeRN+h)0]]>

其中：Vn、Vu、Ve表示慣導系統的北向、天向和東向速度；L表示緯度，h表示高度，R_M表示地球的子午圈半徑，R_N表示地球的卯酉圈半徑，ω_ie表示地球的自轉角速度，表示慣導系統從載體坐標系到導航坐標系的姿態轉換矩陣，f_n、f_u、f_e表示慣導系統加速度計敏感到的比力在導航坐標系內的北向、天向和東向表示值；

（1.3）確定量測方程

量測方程為：Z=HX+V

其中：Z為量測量，即慣導系統的位置信息與GPS位置信息的差值，并考慮兩者間桿臂誤差：Z=[L-L^GPS-R_n?h-h^GPS-R_u?λ-λ^GPS-R_e]^T；h^GPS、λ^GPS、L^GPS分別表示GPS系統輸出的高度信息、經度信息和緯度信息；R_n、R_u、R_e分別表示慣導系統的位置信息與GPS位置信息之間的北向桿臂誤差、天向桿臂誤差和東向桿臂誤差；

H為量測陣，H=I3×303×12-Cbn]]>

（1.4）確定卡爾曼濾波方程

狀態一步預測：其中：表示由k-1時刻到k時刻的狀態一步預測值，表示k-1時刻的狀態估計值，Φ_k,k-1表示離散化的卡爾曼濾波狀態轉移矩陣；

一步預測均方誤差陣：其中：表示由k-1時刻到k時刻的一步預測均方誤差陣，表示k-1時刻的估計均方誤差陣，Q_k表示離散化后的系統噪聲矩陣；

濾波增益矩陣：其中：表示k時刻的濾波增益陣，H_k表示k時刻的系統量測陣，R_k表示離散化后的量測噪聲矩陣；

狀態估計：其中：表示k時刻的狀態估計值，Z_k表示系統觀測量矩陣；

估計均方誤差陣：PkF=(I-KkFHk)Pk,k-1F(I-KkFHk)T+KkFRk(KkF)T,]]>其中：表示k時刻的估計均方誤差陣，F_i表示未離散化的系統狀態轉移矩陣對應元素值，I表示對應維數的單位矩陣，T_n表示慣導系統導航周期，T_f表示濾波周期；

（2)在步驟（1）卡爾曼濾波的基礎上，進行反向最優固定區間平滑；

設整個導航時間區間為[0?N]，t表示此時間間隔中的任一時刻，0≤t≤N，固定區間平滑估值表示為

（2.1）首先進行0→N的正向Kalman濾波，同時存儲濾波估計出的狀態轉移矩陣Φ_k,k-1、狀態一步預測一步預測均方誤差陣狀態估計估計均方誤差陣

（2.2）濾波結束后，將正向濾波最后時刻N的估計值作為反向R-T-S平滑起始時刻的初始值，令k=N，進行N→0的R-T-S平滑過程，其中表示N時刻的平滑估計值，表示N時刻的卡爾曼濾波狀態估計值，表示N時刻的平滑估計均方誤差陣，表示k時刻的卡爾曼濾波估計均方誤差陣，方程如下：

平滑增益陣：其中，表示k時刻的平滑增益陣，Φ_k+1,k表示離散化的卡爾曼濾波狀態轉移矩陣，表示由k時刻到k+1時刻的一步預測均方誤差陣；

平滑的狀態估計：X^k/NS=X^kF+Kk/NS(X^k+1/NS-X^k+1,kF),]]>其中：表示第k時刻的平滑估計量，表示第k+1時刻的平滑估計量，表示由k時刻到k+1時刻的狀態一步預測值；

平滑的估計均方誤差陣：Pk/NS=PkF+Kk/NS(Pk+1/NS-Pk+1,kS)(Kk/NS)T,]]>其中：表示k時刻的平滑估計均方誤差陣，表示k+1時刻的平滑估計均方誤差陣，表示由k時刻到k+1時刻的一步預測均方誤差陣，表示k時刻的平滑增益陣；

從公式可見，第k時刻的平滑估計量是在正向Kalman濾波估計量的基礎上線性補償了一個修正量得到的，該修正量是第k+1時刻的平滑估計量與正向一步預測估計量的差值；

由上面的分析過程得到，k時刻的R-T-S平滑估計量是k時刻的正向Kalman濾波估計量和k+1時刻的平滑估計量的線性融合，k+1時刻的平滑估計量利用了全局數據。