[發明專利]臨近空間飛行器傳遞對準模型不確定性的魯棒濾波方法

有效

• 申請號: 201510064836.2
• 申請日: 2015-02-06
• 公開（公告）號: CN104613984B
• 公開（公告）日: 2018-09-21
• 發明（設計）人: 程向紅;陳紅梅;戴晨曦;王磊
• 申請（專利權）人: 東南大學
• 主分類號: G01C25/00
• 分類號: G01C25/00
• 代理公司: 南京蘇高專利商標事務所(普通合伙) 32204
• 代理人: 李昊

1.一種臨近空間飛行器傳遞對準模型不確定性的魯棒濾波方法，其特征在于，該魯棒濾波方法具體步驟如下：

步驟1)建立臨近空間飛行器傳遞對準系統的數學平臺失準角誤差方程、速度誤差方程、位置誤差方程和觀測方程；

步驟2)根據數學平臺失準角誤差方程、速度誤差方程、位置誤差方程和觀測方程建立模型不確定的狀態方程和觀測方程；

x_k＝f(x_k-1)+Φ(x_k-1)η_k+G_k|k-1w_k-1 (1)

z_k＝h(x_k)+Ψ(x_k)υ_k+v_k (2)

式中：

x_k是n維狀態向量,z_k是m維觀測向量，f(·)和h(·)分別對應非線性狀態方程和觀測方程；G_k|k-1是n×r維系統過程噪聲輸入矩陣，w_k-1是r維系統過程噪聲序列,v_k是m維系統觀測噪聲序列；Φ(·)∈R^n×n是系統狀態方程中模型不確定性部分的有界輸入矩陣，Ψ(·)∈R^m×m是系統觀測方程中模型不確定性部分的有界輸入矩陣；η_k∈Rⁿ是系統狀態方程中模型不確定性未知有界變量，υ_k∈R^m是系統觀測方程中模型不確定性未知有界變量；具體如下：

2.1)x_k是

可簡寫為

包括導航系為發射點慣性坐標系下失準角φⁱ＝[φ_x φ_y φ_z]^T、速度誤差δVⁱ＝[δV_x δV_y δV_z]^T、位置誤差δSⁱ＝[δS_x δS_y δS_z]^T、陀螺儀隨機常值誤差ε_gⁱ＝[ε_gx ε_gy ε_gz]^T、加速度計隨機常值誤差安裝誤差μⁱ＝[μ_x μ_y μ_z]^T；系統噪聲向量為：

2.2)系統狀態方程中模型不確定性部分的有界輸入矩陣Φ(·)具體為

η_k滿足如下關系：

式中：η_k＝[η_φ 0_1×12]^T,η_φ∈R⁶,ε為魯棒濾波參數；

2.3)模型不確定的狀態方程具體為：

式中，為子慣導解算的姿態矩陣；是子慣導IMU的加速度計測量值；δgⁱ是引力場模型的引力加速度誤差；

2.4)系統觀測方程中模型不確定性部分的有界輸入矩陣Ψ(·)具體為：

Ψ_φ是姿態匹配模型不確定部分，主要由初始安裝誤差確定，具體為：

2.5)模型不確定的觀測方程具體為:

υ_φ∈R³是與姿態相關的觀測模型不確定性未知有界變量，具體為

觀測向量為:z_k＝[Z_a Z_v Z_s]^T；其中姿態觀測量Z_a＝[Z_ax Z_ay Z_az]^T，速度觀測量Z_v＝[Z_vxZ_vy Z_vz]^T，位置觀測量Z_s＝[Z_sx Z_sy Z_sz]^T；ε為魯棒濾波參數；Z_am是姿態匹配的觀測量；

步驟3)給出狀態變量初始值(x₀)和預測誤差方差矩陣初始值(Σ_0|0)，給出N_p個稀疏網格求積分點集(ξ_j，ε_j)，其中j＝1,2，…，N_p；

狀態變量初始值x₀＝[0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]^T；

預測誤差方差矩陣初始值：

系統過程噪聲初始值

系統觀測噪聲初始值

式中：

φ_x0、φ_y0和φ_z0是初始數學平臺失準角；

δV_x0、δV_y0和δV_z0是初始速度誤差；δS_x0、δS_y0和δS_z0是初始位置誤差；

ε_gx0、ε_gy0和ε_gz0是陀螺儀常值漂移初值；和是加速度計常值偏移初值；μ_x0、μ_y0和μ_z0是主子慣導間安裝誤差初值；

w_gx、w_gy和w_gz是陀螺儀隨機噪聲；w_ax、w_ay和w_az是加速度計隨機噪聲；

σ_ax、σ_ay和σ_az是姿態觀測噪聲標準差；σ_vx、σ_vy和σ_vz是速度觀測噪聲標準差；

σ_sx、σ_sy和σ_sz是位置觀測噪聲標準差；

根據稀疏網格求積分準則給定一組積分點集(ξ_j，ε_j)其中j＝1,2,…N_p，N_p表示積分點集的個數；

其中Np＝2n²+2n+1,n為狀態變量維數；

給定滿足γ>1和ε>0的魯棒濾波參數；具體為：第一組γ₁＝500，ε₁＝0.05：

第二組γ₁＝100，ε₁＝0.01：

步驟4)利用魯棒稀疏網格求積分濾波經過初始采樣、時間更新、重采樣、量測更新和魯棒更新過程對臨近空間飛行器傳遞對準系統狀態進行估計，并對子慣導系統進行誤差修正，判斷k+1是否大于等于步長L，如果是，狀態估計結束，完成傳遞對準過程，否則返回初始采樣過程進行下一次估計。