[發明專利]一種基于九軸IMU的姿態解算方法、裝置及存儲介質

有效

• 申請號: 202011542749.0
• 申請日: 2020-12-24
• 公開（公告）號: CN112304340B
• 公開（公告）日: 2021-04-06
• 發明（設計）人: 黃永鑫;路晗;方珺
• 申請（專利權）人: 北京輕威科技有限責任公司
• 主分類號: G01C25/00
• 分類號: G01C25/00;G01C21/20;G01C21/08;G01C21/16;G01C21/18;G01C21/00
• 代理公司: 北京超凡宏宇專利代理事務所(特殊普通合伙) 11463
• 代理人: 劉鳳

1.一種基于九軸IMU的姿態解算方法，其特征在于，所述姿態解算方法包括：

基于獲取到的九軸IMU的陀螺儀、加速度計和磁力計的原始數據，分別得到經過標定后的陀螺儀標定數據、加速度計標定數據和磁力計標定數據；

對得到的陀螺儀標定數據和加速度計標定數據進行數據融合，得到初步姿態數據；

確定一個表征磁場時間統計和空間統計的磁場矢量-權重值集合；

其中，通過以下步驟確定所述磁場矢量-權重值集合：

基于每個初始磁場矢量以及該初始磁場矢量對應的權重值，得到多個初始集合元素；

當新增磁場矢量與任意一個初始磁場矢量的差值在預設圓錐范圍內時，確定與該新增磁場矢量對應的初始磁場矢量的累計權重值；

當新增磁場矢量與任意一個初始磁場矢量的差值不在預設圓錐范圍內時，確定該新增磁場矢量以及該新增磁場矢量對應的權重值為新增集合元素；

基于與該新增磁場矢量對應的初始磁場矢量的累計權重值，或所述新增集合元素，更新所述磁場矢量-權重值集合；

將加速度計標定數據和磁力計標定數據轉換成用于矢量描述的加速度矢量和磁場矢量；

當到達預設的磁力修正時刻時，根據所述初步姿態數據和當前磁力修正時刻獲得的磁場矢量對所述磁場矢量-權重值集合進行修正，得到修正后的磁場矢量-權重值集合；

其中，所述當到達預設的磁力修正時刻時，根據所述初步姿態數據和當前磁力修正時刻獲得的磁場矢量對所述磁場矢量-權重值集合進行修正，得到修正后的磁場矢量-權重值集合，包括：

在九軸IMU處于靜止狀態下，確定所述磁場矢量-權重值集合中最大的權重值對應的磁場矢量；

基于當前磁力修正時刻下所述磁場矢量以及所述初步姿態數據中的陀螺儀的姿態四元數，確定IMU坐標系下的磁場矢量；

將所述IMU坐標系下的磁場矢量與所述磁場矢量-權重值集合中最大的權重值對應的磁場矢量進行比較，得到當前磁力修正時刻所述磁力計檢測到的磁場方向與穩定磁場方向之間的角度差值；

若所述角度差值在預設角度差范圍內時，將當前磁力修正時刻IMU坐標系下的磁場矢量添加到所述磁場矢量-權重值集合中；

若所述角度差值不在預設角度差范圍內時，則不將當前磁力修正時刻IMU坐標系下的磁場矢量添加到所述磁場矢量-權重值集合中；

當到達預設的姿態修正時刻時，根據修正后的磁場矢量-權重值集合中權重值最高的磁場矢量以及當前處于靜止狀態的加速度計的加速度矢量，對所述初步姿態數據進行修正，得到最終姿態數據，其中，所述姿態修正時刻對應周期是所述磁力修正時刻對應周期的整數倍；

其中，所述當到達預設的姿態修正時刻時，根據修正后的磁場矢量-權重值集合中權重值最高的磁場矢量以及當前處于靜止狀態的加速度計的加速度矢量，對所述初步姿態數據進行修正，得到最終姿態數據，包括：

在九軸IMU處于靜止狀態下，確定所述磁場矢量-權重值集合中最大的權重值對應的磁場矢量；

基于最大的權重值對應的磁場矢量和當前姿態修正時刻由所述加速度計確定出的加速度矢量，更新所述九軸IMU的姿態四元數；

確定更新后的九軸IMU的姿態四元數為最終姿態數據。

2.根據權利要求1所述的姿態解算方法，其特征在于，所述對得到的陀螺儀標定數據和加速度計標定數據進行數據融合，得到初步姿態數據，包括：

根據上一采樣時刻已計算得到的初步姿態數據，結合獲得的當前采樣時刻陀螺儀標定數據，計算得到當前采樣時刻的候選初步姿態數據；

若所述九軸IMU處于非靜止狀態，則基于所述候選初步姿態數據對所述初步姿態數據進行更新，得到更新后的初步姿態數據；

若所述九軸IMU處于靜止狀態，由當前采樣時刻的加速度計標定數據確定在IMU初始坐標系下的當前采樣時刻重力矢量；

根據在所述IMU初始坐標系下預先確定出的初始重力矢量，確定當前采樣時刻重力矢量和所述初始重力矢量之間的誤差矢量；

根據所述候選初步姿態數據以及所述誤差矢量，采用數據融合算法，計算得到一個融合后初步姿態數據；基于所述融合后初步姿態數據對所述初步姿態數據進行更新，得到更新后的初步姿態數據；其中，所述IMU初始坐標系為九軸IMU的陀螺儀、加速度計和磁力計經過標定后的初始時刻確定出的坐標系。