[發(fā)明專利]一種姿態(tài)角的融合解算方法和裝置

有效

• 申請?zhí)枺?/td>: 201810060960.5
• 申請日: 2018-01-22
• 公開（公告）號: CN108398128B
• 公開（公告）日: 2021-08-24
• 發(fā)明（設計）人: 時廣軼;謝偰偉;國林旗
• 申請（專利權）人: 北京大學深圳研究生院
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 深圳鼎合誠知識產權代理有限公司 44281
• 代理人: 郭燕

1.一種姿態(tài)角的融合解算方法，其特征在于，包括：

根據(jù)陀螺儀的測量數(shù)據(jù)解算出第一姿態(tài)角；

根據(jù)加速度計的測量數(shù)據(jù)和磁力計的測量數(shù)據(jù)解算出第二姿態(tài)角；

根據(jù)互補濾波原理對所述第一姿態(tài)角和所述第二姿態(tài)角進行融合計算，得到第三姿態(tài)角；

以所述第三姿態(tài)角為第一狀態(tài)變量且互補濾波后獲得的所述第二姿態(tài)角為觀測量，利用擴展卡爾曼濾波計算得到融合姿態(tài)角；

所述根據(jù)加速度計的測量數(shù)據(jù)和磁力計的測量數(shù)據(jù)解算出第二姿態(tài)角，包括：根據(jù)加速度計的測量數(shù)據(jù)和磁力計校準后的測量數(shù)據(jù)解算出第二姿態(tài)角；

所述根據(jù)互補濾波原理對所述第一姿態(tài)角和所述第二姿態(tài)角進行融合計算，得到第三姿態(tài)角，包括：

根據(jù)加速度計的測量數(shù)據(jù)、磁力計校準后的測量數(shù)據(jù)和地球的當?shù)卮艌鲋担霉揭挥嬎銠嘀礔；

根據(jù)所述第一姿態(tài)角、所述第二姿態(tài)角和F，利用公式二計算出第三姿態(tài)角；

所述公式一為：

其中，M_G為所述地球的當?shù)卮艌鲋担琣_x、a_y、a_z分別為加速度計的三個軸歸一化后的測量數(shù)據(jù)，m_x、m_y、m_z分別為磁力計校準后的三個軸的測量數(shù)據(jù)；

所述公式二為：

其中，Φ、θ和Ψ為計算出的所述第三姿態(tài)角，Φ_g、θ_g和Ψ_g為所述第一姿態(tài)角，Φ_am、θ_am和Ψ_am為所述第二姿態(tài)角。

2.如權利要求1所述的方法，其特征在于，在所述根據(jù)加速度計的測量數(shù)據(jù)和磁力計的測量數(shù)據(jù)解算出第二姿態(tài)角之前，所述方法還包括：

以磁力計的零偏為第二狀態(tài)變量，利用卡爾曼濾波迭代對磁力計的測量數(shù)據(jù)進行校準，得到磁力計校準后的測量數(shù)據(jù)。

3.如權利要求2所述的方法，其特征在于，所述以磁力計的零偏為第二狀態(tài)變量，利用卡爾曼濾波迭代對磁力計的測量數(shù)據(jù)進行校準，得到磁力計校準后的測量數(shù)據(jù)，包括：

以磁力計的零偏為第二狀態(tài)變量，建立磁力計的零偏狀態(tài)方程為x＝f(x)+w，其中，x＝[b_x b_y b_z]^T為第二狀態(tài)變量，f(x)＝0為x的微分值，w為磁力計零偏的隨機噪聲，b_x、b_y、b_z分別為磁力計在x、y、z三軸方向上的零偏，T代表轉置；

對所述零偏狀態(tài)方程進行離散化處理，得到卡爾曼濾波的狀態(tài)轉移矩陣A；

建立磁力計零偏的觀測方程為y＝(M_bx-b_x)²+(M_by-b_y)²+(M_bz-b_z)²，其中，y為當?shù)卮艌鲋担琈_bx、M_by、M_bz為磁力計三軸的實際測量輸出值；

對所述觀測方程進行離散化處理，得到量測方程；

計算所述量測方程的雅可比矩陣，得到第一量測矩陣H1_k；

根據(jù)所述第二狀態(tài)變量x、所述狀態(tài)轉移矩陣A、所述磁力計零偏的觀測方程和所述第一量測矩陣H1_k，利用卡爾曼濾波迭代對磁力計的測量數(shù)據(jù)進行校準，得到磁力計校準后的測量數(shù)據(jù)。