[發明專利]一種基于失準角的卡爾曼濾波姿態估計方法

有效

• 申請號: 201810380263.8
• 申請日: 2018-04-25
• 公開（公告）號: CN108592917B
• 公開（公告）日: 2021-02-23
• 發明（設計）人: 劉兵;劉恒;呂元宙;莊秋彬
• 申請（專利權）人: 珠海全志科技股份有限公司
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C21/16
• 代理公司: 深圳市君之泉知識產權代理有限公司 44366
• 代理人: 張丕陽

1.一種基于失準角的卡爾曼濾波姿態估計方法，其特征在于，所述方法包含以下步驟：

步驟一：設置載體坐標系，所述載體坐標系三軸記作xyz，所述載體為無人機；

步驟二：通過慣性傳感器確定當前時刻的載體姿態，記作att＝[φ θ ψ]，其中φ代表滾轉角，θ代表俯仰角，ψ代表航向角；

步驟三：確定載體坐標系向導航坐標系的轉換矩陣，記作

簡化轉換矩陣為以下形式

步驟四：采用東向水平失準角北向水平失準角和陀螺漂移ω_bx,ω_by作為系統狀態，記作

采用水平加速度f_x,f_y作為量測狀態，記作Z＝[f_y f_x]^T；

步驟五：構建基于失準角的卡爾曼濾波；

步驟六：根據所述基于失準角的卡爾曼濾波進行載體姿態的估計；

所述基于失準角的卡爾曼濾波包括時間更新和量測修正，在載體處于平穩狀態時，進行時間更新和量測修正，在載體處于非平穩狀態時，僅進行時間更新；

判斷載體是否處于平穩狀態的步驟為：

步驟1：計算所述慣性傳感器的三軸加速度計的矢量和記為其中f_x,f_y,f_z分別為三軸加速度值；

步驟2：計算載體低機動狀態下的三軸加速度；具體步驟為：

在載體低機動的狀態下有

其中，重力向量為g_n＝[0 0 g]^T，g為當地重力加速度，f₁為低機動狀態下三軸加速度計輸出的加速度向量，記作f₁＝[f_x1,f_y1,f_z1]^T；

由式(3)可計算得出理想加速度f_x1,f_y1,f_z1：

步驟3：定義一偏差數值為eps，分別計算|f_x1-f_x|、|f_y1-f_y|、|f_z1-f_z|，若上述任一結果大于偏差數值eps，則載體處于非平穩狀態；否則，載體處于平穩狀態。

2.根據權利要求1所述的方法，其特征在于，所述載體坐標系與載體固聯，坐標系符合右手法則，原點在載體重心處，x軸指向載體前進方向，y軸由原點指向載體右側，z軸方向根據xy軸由右手法則確定。

3.根據權利要求1所述的方法，其特征在于，設卡爾曼濾波系統的狀態方程為：

其中，為系統狀態的導數，f(x)為系統狀態的函數，w為系統噪聲，Z為系統的量測狀態，h(x)為系統的量測狀態函數，v為量測噪聲；

記失準角為陀螺漂移為ω_b＝[ω_bx ω_by 0]^T，根據失準角方程，式(5)可表示為

對f(x)和h(x)其求解雅克比矩陣得到矩陣F和H，得到線性化后的狀態方程：

其中，向量X為上一時刻系統狀態向量，向量為系統狀態向量的一步預測值，矩陣F為系統傳遞矩陣，向量W,V為系統噪聲向量，向量Z為系統量測向量，矩陣H為量測矩陣，其中

4.根據權利要求1所述的方法，其特征在于，所述時間更新的具體步驟包括：

其中，向量為系統狀態向量的一步預測值，向量為上一時刻的估計值，矩陣Φ為矩陣F的離散化形式，Φ_k/k-1為一步轉移矩陣，矩陣P_k/k-1為狀態向量的一步預測方差矩陣，矩陣P_k-1為狀態向量的上一時刻方差矩陣，矩陣Q_k-1為系統噪聲方差矩陣；時間更新后的預測值作為姿態估計值。

5.根據權利要求4所述的方法，其特征在于，所述量測修正的具體步驟包括：

P_k＝(I-K_kH_k)P_k/k-1 (14)

其中，向量為系統狀態向量的當前最優估算值，矩陣K_k為增益矩陣，矩陣H_k為量測矩陣，矩陣R_k為量測噪聲序列方差陣，矩陣I為單位矩陣；所述量測修正即為姿態修正。