[發明專利]一種基于擴展卡爾曼濾波的手持云臺姿態估計方法

有效

• 申請號: 202010427203.4
• 申請日: 2020-05-19
• 公開（公告）號: CN111625768B
• 公開（公告）日: 2023-05-30
• 發明（設計）人: 陳朋印;李定涌;閆永馳;陳柯柯;馬一鳴;曾幼涵
• 申請（專利權）人: 西安因諾航空科技有限公司
• 主分類號: G06F17/16
• 分類號: G06F17/16;G06F17/11
• 代理公司: 西安通大專利代理有限責任公司 61200
• 代理人: 賀小停

1.一種基于擴展卡爾曼濾波的手持云臺姿態估計方法，其特征在于，包括以下步驟：

步驟1，選取姿態估計系統的狀態向量，構建狀態向量的預測方程，根據狀態向量的預測方程計算狀態預測的雅克比矩陣，此矩陣為狀態轉移矩陣；

步驟2，根據姿態估計系統中的傳感器選取量測并結合狀態向量構造量測預測方程，然后根據量測預測方程計算量測預測值的雅克比矩陣，此矩陣為量測矩陣，完成姿態估計系統模型的建立；

步驟3，將擴展卡爾曼濾波器編寫為序貫濾波的形式，完成手持云臺姿態估計；

步驟1中，狀態向量包括手持云臺的姿態四元數、速度、陀螺零偏、加速度計零偏、北東地方向地磁場分量和磁力計零偏，狀態向量表示為：

其中q₀為手持云臺姿態四元數實部，q₁、q₂、q₃為手持云臺姿態四元數虛部，v_n為手持云臺北向速度，v_e為手持云臺東向速度，v_d為手持云臺地向速度，ε_x為x軸陀螺零偏，ε_y為y軸陀螺零偏，ε_z為z軸陀螺零偏，為x軸加速度計零偏，為y軸加速度計零偏，為z軸加速度計零偏，m_n為北向地磁場分量，m_e為東向地磁場分量，m_d為地向地磁場分量，m_bx為x軸磁力計零偏，m_by為y軸磁力計零偏，m_bz為z軸磁力計零偏；

狀態方程為：

其中：X_k-1——k-1時刻狀態向量；Φ_k/k-1——狀態轉移矩陣；——k時刻狀態向量的預測；

步驟3中，將上述模型代入擴展卡爾曼濾波即可實時輸出手持云臺高精度姿態信息與速度信息，擴展卡爾曼濾波實施過程如下：

狀態預測

狀態誤差協方差矩陣預測

濾波器增益

狀態誤差協方差矩陣更新

狀態更新

其中：P_k-1——k-1時刻擴展卡爾曼濾波狀態誤差協方差矩陣；——k時刻擴展卡爾曼濾波狀態誤差協方差矩陣預測值；Q_k-1——k-1時刻系統噪聲矩陣；R_k——k時刻量測噪聲矩陣；K_k——k時刻濾波器增益矩陣；P_k——k時刻擴展卡爾曼濾波狀態誤差協方差矩陣。

2.根據權利要求1所述的一種基于擴展卡爾曼濾波的手持云臺姿態估計方法，其特征在于，手持云臺姿態四元數的狀態預測方程

其中：

——k時刻手持云臺姿態四元數預測；q_k-1——k-1時刻手持云臺姿態四元數；Δq——k-1至k時刻手持云臺姿態四元數變化量；——四元數相乘；

k-1至k時刻手持云臺姿態四元數變化量計算公式如下：

其中：Δθ_x——k-1至k時刻手持云臺x軸角增量；Δθ_y——k-1至k時刻手持云臺y軸角增量；Δθ_z——k-1至k時刻手持云臺z軸角增量；

k-1至k時刻手持云臺各軸角增量計算公式如下：

Δθ_x＝(ω_x-ε_x)·Δt

Δθ_y＝(ω_y-ε_y)·Δt

Δθ_z＝(ω_z-ε_z)·Δt

其中：ω_x——手持云臺x軸陀螺輸出的角速度；ε_x——手持云臺x軸陀螺零偏；ω_y——手持云臺y軸陀螺輸出的角速度；ε_y——手持云臺y軸陀螺零偏；ω_z——手持云臺z軸陀螺輸出的角速度；ε_z——手持云臺z軸陀螺零偏；Δt——k-1至k時刻時間間隔。