[發明專利]一種附有運動加速度補償的MARG姿態計算方法

有效

• 申請號: 202011427992.8
• 申請日: 2020-12-07
• 公開（公告）號: CN112683269B
• 公開（公告）日: 2022-05-03
• 發明（設計）人: 周澤波;張澤亮
• 申請（專利權）人: 電子科技大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 電子科技大學專利中心 51203
• 代理人: 陳一鑫

1.一種附有運動加速度補償的MARG姿態計算方法，該方法包括：

步驟1：利用陀螺儀進行一步預測：

建立系統狀態模型：

x_k＝Φ_k-1,kx_k-1+w_k (1)

其中，下標k表示t_k時刻，x_k表示t_k時刻的狀態，此模型中取姿態四元數q作為狀態，即取x＝q；w_k為狀態模型噪聲；由t_k-1時刻到t_k時刻的狀態轉移矩陣Φ_k-1,k為：

其中，I₄表示4維單位陣，T_s表示采樣間隔，M(ω_k)定義如下：

其中，ω_x，ω_y，ω_z分別表示x，y，z方向上的角速率；ω_k表示3維角速率矢量，即ω_k＝[ω_x ω_y ω_z]^T；上標T表示矩陣轉置；

進行一步預測：

其中，上標表示矢量為預測量，表示從t_k-1時刻到t_k時刻的狀態一步預測值，P_k|k-1表示一步預測的方差，P_k-1表示t_k-1時刻狀態最優估計的方差，Q_k表示狀態模型噪聲w_k的方差；

步驟2：利用一步預測值，對加速度計與磁力計進行檢測，具體如下：

步驟2-1：借助角速率姿態更新方程(4)，得t_k時刻姿態的一步預測值其對應的載體坐標系到導航坐標系的方向余弦矩陣為簡記為其中方向余弦矩陣與四元數的關系如下：

其中，q₀，q₁，q₂，q₃分別表示姿態四元數q中的四個分量，即q＝[q₀,q₁,q₂,q₃]^T.

步驟2-2：對磁場干擾進行檢測：

磁力計t_k時刻輸出記為有磁力計輸出模型：

其中，上標b表示載體坐標系；r_mag(k)表示地磁矢量；b_mag(k)表示磁場干擾量；ε_mag(k)為磁力計噪聲，其方差記為其中diag表示對角矩陣，分別表示三軸磁場噪聲的方差；計算磁場干擾檢測量δr_k：

上標n表示導航坐標系，表示導航坐標系下的地磁矢量；根據3σ原則，當δr_k三軸分量滿足：

認為無磁場干擾；反之，則磁場干擾存在；

步驟2-3：對運動加速度進行檢測，當運動加速度持續時間大于設定閾值時，進行運動加速度補償：

加速度計輸出模型為：

其中，表示加速度計的輸出值，g表示重力加速度；a_k表示載體非重力加速度，ε_acc(k)為加速度計噪聲；

計算運動加速度檢測量δa_k：

表示由(4)算得的姿態一步預測值，表示加速度計的輸出；根據3σ原則，當δa_k三軸分量滿足：

認為無運動加速度；反之，則運動加速度存在；其中σ_acc(x),σ_acc(y),σ_acc(z)分別表示加速度計三軸噪聲標準差；

當運動加速度持續時間大于設定閾值時，通過滑動窗口進行運動加速度補償：

對窗口長度為n的運動加速度數據{δa_k-n,…,δa_k-1}進行多項式曲線擬合，多項式擬合階數和窗口長度根據載體實際運動快慢情況及加速度計采樣率進行調整；擬合階數選擇0階、或1階、或2階、或高階中的一種；

借助擬合多項式計算得t_k時刻運動加速度擬合值根據方差傳播定理確定運動加速度擬合值的方差將擬合值補償值補償后有：

經補償后，用于濾波姿態解算，方差記為表示加速度計噪聲的方差；

步驟3：針對不同的檢測結果執行相應的融合算法：

無磁場干擾，無運動加速度或運動加速度已補償：轉至步驟4；

存在磁干擾，無運動加速度或運動加速度已補償：轉步驟5；

無磁干擾，存在運動加速度且運動加速度未補償，轉步驟6；

存在磁干擾，存在運動加速度且運動加速度未補償：將陀螺推算的一步預測結果作為姿態估計值，等待下一時刻的傳感器數據；

步驟4：建立加速度和磁場強度的矢量對方程，通過快速線性四元數姿態估計方法求解獲得量測信息，基于卡爾曼濾波進行姿態估計；

步驟4-1：建立加速度和磁感應強度的矢量對方程：

若選取n系為東北天坐標系，則有gⁿ＝[0 0 -g]^T，g，mⁿ＝[0 m_N m_U]^T，其中m_N和m_U分別表示北向與天向的地磁強度，其中，q表示姿態四元數矢量，y_acc表示加速度矢量，無運動加速度時取加速度計的輸出運動加速度已補償時取補償后的加速度y_mag表示磁感應強度，無磁干擾情況下即磁力計的輸出ε_acc表示加速度噪聲，ε_mag表示磁感應強度噪聲；

步驟4-2：通過快速解算獲得量測信息從四元數與方向余弦矩陣關系出發，建立四元數特征值與特征向量方程，給出特征值數值計算公式，基于特征值接近1的數值特性，快速選出真實解；隨后，進行矩陣初等變換，得到矢量對方程的四元數解；

計算中間變量矩陣

其中，H_x，H_y，H_z，W為計算過程中的中間變量矩陣，I₃表示3維單位陣，H_x1，H_x2，H_x3分別表示3維矩陣H_x的第1、2、3個分量；H_y1，H_y2，H_y3分別表示3維矩陣H_y的第1、2、3個分量，H_z1，H_z2，H_z3分別表示3維矩陣H_z的第1、2、3個分量；

計算中間參數：

τ₂＝8(H_x3H_y2H_z1-H_x2H_y3H_z1-H_x3H_y1H_z2+H_x1H_y3H_z2+H_x2H_y1H_z3-H_x1H_y2H_z3)

τ₃＝det(W)

其中，det表示求矩陣行列式；

計算特征值：

從(17)中選取最接近1的數作為最終的特征值λ，即

λ＝argmin|λ_i-1|,(i＝1,2,3,4) (18)

其中，argmin表示使得后面式子達到最小時自變量的取值；||表示取絕對值；進行初等變換，

其中，χ，ρ，υ，ζ表示完成初等變換后的矩陣最后一列的四個元素；

獲得歸一化的四元數

步驟4-3：建立濾波量測模型：

其中，z_k表示量測矢量，取量測轉移矩陣為H_k＝I₄，v_k為量測噪聲；

步驟4-4：基于卡爾曼濾波進行姿態估計：

P_k＝(I₄-K_kH_k)P_k|k-1 (24)

其中，K_k表示濾波增益，R_k表示量測噪聲v_k的方差，P_k|k-1和P_k分別表示狀態一步預測和狀態最優估計的方差；上標-1表示矩陣求逆運算；

步驟5：利用加速度計數據建立量測模型，基于無跡卡爾曼濾波實現狀態估計，并對磁干擾進行估計；具體如下：

步驟5-1：利用加速度計數據建立量測模型：

y_acc＝-C^T(q)gⁿ+ε_acc (25)

步驟5-2：針對系統非線性模型，采用非線性濾波進行狀態估計；

步驟6：利用磁力計數據建立量測模型，基于無跡卡爾曼濾波實現狀態估計；具體如下：

步驟6-1：利用磁力計數據建立量測模型：

y_mag＝C^T(q)mⁿ+ε_mag (26)

步驟6-2：針對系統非線性模型，采用非線性濾波進行狀態估計。