[發明專利]一種基于無跡卡爾曼濾波的飛機角速率信號重構方法

有效

• 申請號: 201310295946.0
• 申請日: 2013-07-06
• 公開（公告）號: CN103363993A
• 公開（公告）日: 2013-10-23
• 發明（設計）人: 劉小雄;李鵬輝;史靜平;章衛國;李廣文
• 申請（專利權）人: 西北工業大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種基于無跡卡爾曼濾波的飛機角速率信號重構方法，其特征在于，包括以下步驟：

(1)以周期T讀取捷聯慣導系統中的三個姿態角信息，三個姿態角分別為俯仰角θ、滾轉角φ、偏航角ψ；

(2)以周期T讀取飛行控制系統輸出的飛機所受到的合外力矩，合外力矩在機體坐標系下x軸、y軸和z軸的分量分別為滾轉力矩俯仰力矩M和偏航力矩N；

(3)利用飛機的轉動慣量(I_x，I_y，I_z)和慣性積I_xz按照下式計算力矩方程組的系數

c1=(Iy-Iz)Iz-Ixz2Σ,]]>c2=(Ix-Iy+Iz)IxzΣ,]]>c3=IzΣ,]]>c4=IxzΣ,]]>c5=Iz-IxIy,]]>c6=IxzIy,]]>c7=1Iy,]]>c8=(Ix-Iy)Ix-Ixz2Σ,]]>c9=IxΣ,]]>∑＝I_xI_z-I_xz²；

(4)將步驟(1)獲取的飛機的俯仰角θ、滾轉角φ、偏航角ψ分別輸入到非線性跟蹤微分器得到俯仰角微分滾轉角微分和偏航角的微分

x1(k+1)=x1(k)+h*x2(k)x2(k+1)=x2(k)+h*fst2(x1(k),x2(k),vv(k),r,h1)]]>

上式中將含測量噪聲的θ、φ和ψ分別代替vv(k)，則相應的x₂(k)分別對應于和其中h為離散型非線性跟蹤微分器的仿真步長，這里取h＝T，r和h₁分別為濾波因子和快慢因子；

(5)根據飛行動力學模型，選取飛機的俯仰角速率q、滾轉角速率p和偏航角速率r作為狀態變量，即狀態向量X＝[p?q?r]^T，進而建立無跡卡爾曼濾波器的狀態方程；選取步驟(4)得到的俯仰角微分滾轉角微分和偏航角微分作為觀測變量，即觀測向量為進而建立無跡卡爾曼濾波器量測方程；

(6)根據步驟(1)和步驟(4)獲取的當前時刻即t_k+1時刻的量測信息，步驟(2)獲取的上一時刻即t_k時刻的合外力矩，步驟(3)計算得到的9個力矩方程組系數，利用無跡卡爾曼濾波方程得到t_k+1時刻狀態量的最優估計值，從而實現t_k+1時刻的三軸角速率信號的實時精確估計；

(7)將步驟(6)得到的t_k+1時刻的角速率信號的精確估計值反饋給無跡卡爾曼濾波算法模塊，用于完成步驟(6)中的下一時刻即t_k+2時刻角速率信號的估計。

2.根據權利要求1所述的基于無跡卡爾曼濾波的飛機角速率信號重構方法，其特征在于，所述步驟(6)中無跡卡爾曼濾波算法的具體步驟是：

(a)無跡卡爾曼濾波器狀態方程的建立

根據飛行動力學模型，選取飛機的俯仰角速率q、滾轉角速率p和偏航角速率r作為狀態變量，即狀態向量X＝[p?q?r]^T，進而建立無跡卡爾曼濾波器的狀態方程：

狀態方程的具體形式為：

其中，u(t)為控制輸入量，包括升降舵偏角δe，副翼偏轉角δa和方向舵偏角δr，滾轉力矩俯仰力矩M以及偏航力矩由下式計算得到

L‾=fL‾(δe,δa,δr,α,β,p,q,r,L)]]>

M＝f_M(δe，δa，δr，α，β，p，q，r，L)

N＝f_N(δe，δa，δr，α，β，p，q，r，L)

w(t)為激勵噪聲序列，這里取為高斯白噪聲；

(b)無跡卡爾曼濾波器量測方程的建立

將由捷聯慣導系統獲取的俯仰角θ、滾轉角φ、偏航角ψ分別輸入到非線性跟蹤微分器得到俯仰角微分滾轉角微分和偏航角的微分其中非線性跟蹤微分器的離散形式為：

x1(k+1)=x1(k)+h*x2(k)x2(k+1)=x2(k)+h*fst2(x1(k),x2(k),vv(k),r,h1)]]>

vv(k)為含噪聲的輸入信號，x₁(k)用于跟蹤輸入信號vv(k)，而x₂(k)則跟蹤vv(k)的微分信號，在上式中將含測量噪聲的θ、φ和ψ分別代替vv(k)，則相應的x₂(k)分別對應于和其中h為離散型非線性跟蹤微分器的仿真步長，這里取h＝T，r和h₁分別為濾波因子和快慢因子，

選取俯仰角微分滾轉角微分和偏航角微分作為觀測變量，即觀測向量為進而建立無跡卡爾曼濾波器量測方程：

Z(t)＝h[X(t)，u(t)，t]+v(t)

量測方程具體可表示為：

v(t)為量測噪聲序列，同樣選擇為高斯白噪聲；

(c)狀態方程和觀測方程的離散化

假定離散化時間為T，對狀態方程離散化有

X(k+1)＝F(X(k)，u(k)，T)·X(k)+w(k)

即：

p(k+1)q(k+1)r(k+1)=1+f1(·)·T0001+f2(·)·T0001+f3(·)·T·p(k)q(k)r(k)+w1(k)w2(k)w3(k)]]>

對觀測方程進行離散化有

Z(k+1)＝h(X(k+1))+v(k)

即：

其中φ(k+1)、θ(k+1)、ψ(k+1)分別為(k+1)時刻的滾轉角、俯仰角和偏航角測量值，由捷聯慣導系統輸出，w(k)＝[w₁(k)?w₂(k)?w₃(k)]^T，v(k)＝[v₁(k)?v₂(k)?v₃(k)]^T分別為系統激勵噪聲序列和觀測噪聲序列，滿足：

E[w(k)]＝0，Cov[w(k)，w(j)]＝E[w(k)w^T(j)]＝Q_kδ_kj

E[v(k)]＝0，Cov[v(k)，v(j)]＝E[v(k)v^T(j)]＝R_kδ_kj

Cov[w(k)，v(j)]＝E[w(k)v^T(j)]＝0

式中，(Qk)3×3=σw1(k)2000σw2(k)2000σw3(k)2,]]>(Rk)3×3=σv1(k)2000σv2(k)2000σv3(k)2]]>分別為激勵噪聲矢量的協方差矩陣和觀測噪聲矢量的協方差矩陣，Q_k為非負定矩陣，R_k為正定矩陣，為相關噪聲矢量的標準方差常數，δ為狄拉克函數，滿足

δ(x-c)=0x≠c∫abδ(x-c)=1a<c<b;]]>

(d)無跡卡爾曼濾波算法

假定三軸角速率的初始估計值X^0=p0q0r0T,]]>初始條件下的估計均方誤差陣為Pk0=diagσp02σq02σr02,]]>分別是滾轉角速率，俯仰角速率，偏航角速率的初始估計方差，

選擇對稱采樣策略，則相應的均值加權值W_k^(m)和方差加權值W_k^(c)分別可表示為：

W0(m)=λn+λ]]>

W0(c)=λn+λ+(1-α2+β)]]>

Wi(m)=Wi(c)=12·(n+λ),i=1,L2n]]>

其中n為狀態維數，本文中n＝3；λ＝α²(n+κ)-n為尺度因子，κ用于保證方差矩陣的半正定性，一般取κ＝0或κ＝3-n，其取值大小對算法影響并不大；α為比例修正參數(常取1e-4≤α＜1)，以避免在系統非線性較強時的非局部采樣，β用于引入狀態先驗分布的高階項信息，取值范圍β≥0，對于高斯分布，β＝2最優；對于非高斯分布，該參數還具有控制誤差的作用，可以控制后驗分布拖尾的大小，

系統狀態Sigma點采樣

根據k-1時刻的系統狀態估計值和協方差矩陣P_k-1|k-1進行Sigma點采樣，對稱采樣Sigma點樣本個數為L＝2n+1＝7個，則對應于k-1時刻的Sigma采樣點為

χk-1=X^k-1|k-1X^k-1|k-1-(L+λ)Pk-1|k-1X^k-1|k-1+(L+λ)Pk-1|k-1]]>

UKF時間更新

根據離散化的狀態方程式，對上述采樣的7個Sigma點進行狀態預測，則有

χ_i，k，k-1＝F(χ_i，k-1)?????i＝0，1L，6

利用上述采樣預測值確定系統狀態向量和協方差矩陣的最終預測值為：

X^k|k-1=Σi=06Wim·χi,k|k-1]]>

Pk|k-1=Σi=06Wic(χi,k|k-1-X^k|k-1)(χi,k|k-1-X^k|k-1)T+Qk]]>

η_k|k-1＝h(χ_k|k-1)

Z^k|k-1=Σi=06Wim·ηi,k|k-1]]>

UKF量測更新

PZZk|k-1=Σi=06Wi(c)(ηi,k|k-1-Z^k|k-1)·(ηi,k|k-1-Z^k|k-1)T+Rk]]>

PXZk|k-1=Σi=06Wi(c)(χi,k|k-1-X^k|k-1)·(ηi,k|k-1-Z^k|k-1)T]]>

K_k＝P_XZk|k-1·(P_ZZk|k-1)^-1

X^k=X^k|k-1+Kk(Zk-Z^k|k-1)]]>

Pk=Pk|k-1-KkPZZk|k-1KkT]]>

其中的為k時刻的系統觀測值，由非線性跟蹤微分器輸出獲得。