[發明專利]一種基于多傳感器融合的運動體姿態解算方法

有效

• 申請號: 201910107827.5
• 申請日: 2019-02-02
• 公開（公告）號: CN109612471B
• 公開（公告）日: 2021-06-25
• 發明（設計）人: 鄧志紅;嚴丹;韓奮凱;王鵬宇;尚克軍
• 申請（專利權）人: 北京理工大學
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C21/16
• 代理公司: 北京理工大學專利中心 11120
• 代理人: 李愛英;仇蕾安

1.一種基于多傳感器融合的運動體姿態解算方法，其特征在于，將微機電慣性及磁測量單元固定在待測載體上，采集載體加速度、角速率和磁感應強度數據；根據加速度計的量測模型建立第一級損失函數F₁(·)，利用加速度數據y_A，t通過梯度下降法估計當前姿態四元數并將其與通過角速率數據y_G，t更新的姿態四元數加權，得到第一級后驗姿態四元數利用更新載體的俯仰角pitch_A和橫滾角roll_A；在考慮鐵磁干擾的情況下根據磁力計的量測模型建立第二級損失函數F₂(·)，利用上一時刻計算得到的鐵磁干擾估計量和當前時刻磁感應強度數據y_M，t通過梯度下降法估計當前姿態四元數并將其與通過角速率數據y_G，t更新的姿態四元數加權，得到第二級后驗姿態四元數利用更新載體的偏航角yaw_M；

將更新后的偏航角yaw_M、俯仰角pitch_A和橫滾角roll_A轉換成姿態四元數，即當前時刻后驗姿態四元數基于磁力計的量測模型，利用后驗姿態四元數和磁感應強度數據y_M，t對鐵磁干擾估計量進行更新。

2.如權利要求1所述的一種基于多傳感器融合的運動體姿態解算方法，其特征在于，第二級損失函數F₂(x)＝ε₂(x)^Tε₂(x)，

其中，x＝[q d]，y_M，t為t時刻磁力計所測的磁感應強度數據，d為鐵磁干擾，q表示載體姿態，表示載體坐標系下所觀測的當地地球磁場。