[發(fā)明專利]一種基于九軸IMU的姿態(tài)解算方法、裝置及存儲介質(zhì)

有效

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

本申請?zhí)峁┮环N基于九軸IMU的姿態(tài)解算方法、裝置及存儲介質(zhì)，姿態(tài)解算方法包括利用九軸IMU的陀螺儀、加速度計和磁力計輸出的原始數(shù)據(jù)，標(biāo)定三個內(nèi)部模塊的內(nèi)部參數(shù)，借助標(biāo)定內(nèi)部參數(shù)，將原始數(shù)據(jù)轉(zhuǎn)化為標(biāo)定數(shù)據(jù)；對陀螺儀和加速度計標(biāo)定數(shù)據(jù)進行數(shù)據(jù)融合，輸出IMU的初步姿態(tài)數(shù)據(jù)；在基于磁修正時刻對應(yīng)周期和姿態(tài)修正時刻對應(yīng)周期的磁力和姿態(tài)數(shù)據(jù)修正算法中，利用重力矢量和權(quán)重值最高的磁場矢量修正陀螺儀漂移，輸出IMU的最終姿態(tài)數(shù)據(jù)。通過長短周期的磁力和姿態(tài)數(shù)據(jù)的互相校正來對IMU姿態(tài)進行修正，抑制磁場干擾對姿態(tài)解算的不良影響，實現(xiàn)IMU姿態(tài)的準(zhǔn)確、穩(wěn)定輸出，滿足在室內(nèi)復(fù)雜磁場環(huán)境中對IMU器件的應(yīng)用需求。

技術(shù)領(lǐng)域

本申請涉及導(dǎo)航定位技術(shù)領(lǐng)域，具體而言，涉及一種基于九軸IMU的姿態(tài)解算方法、裝置及存儲介質(zhì)。

背景技術(shù)

隨著信息技術(shù)和經(jīng)濟建設(shè)的發(fā)展，交通運輸、工業(yè)、農(nóng)業(yè)、測繪等領(lǐng)域?qū)?dǎo)航技術(shù)提出了更高的要求，通過IMU姿態(tài)解算方法，對九軸IMU（Inertial measurement unit，慣性測量單元）輸出的加速度計（Acc）、陀螺儀（Gyro）和磁力計（Mag）的原始數(shù)據(jù)進行綜合分析計算，使算法穩(wěn)定地向上位機輸出IMU器件三自由度旋轉(zhuǎn)量的姿態(tài)數(shù)據(jù)，可以廣泛應(yīng)用于導(dǎo)航定位等領(lǐng)域。

傳統(tǒng)的姿態(tài)解算方法，是借助于陀螺儀的三軸角速度數(shù)據(jù)，通過計算可以得到IMU的實時姿態(tài)信息。但是，單純依靠累積運算，長期下來會有一定漂移，導(dǎo)致姿態(tài)解算結(jié)果有偏差。因此，對于九軸IMU器件來說，需要借助加速度計和磁力計來定期或不定期地進行姿態(tài)校正，以期望輸出準(zhǔn)確的姿態(tài)數(shù)據(jù)。

但是真實的室內(nèi)場景具有復(fù)雜的磁場環(huán)境，IMU器件的磁力計容易受其干擾影響，當(dāng)磁力計輸出不準(zhǔn)確的數(shù)據(jù)時，將明顯影響傳統(tǒng)的姿態(tài)解算方法，受干擾嚴(yán)重時，將直接導(dǎo)致校正失敗，姿態(tài)數(shù)據(jù)錯誤。

發(fā)明內(nèi)容

有鑒于此，本申請的目的在于提供一種基于九軸IMU的姿態(tài)解算方法、裝置及存儲介質(zhì)，能夠通過長短周期磁力和姿態(tài)數(shù)據(jù)的互相校正來對IMU姿態(tài)進行修正，抑制磁場干擾對姿態(tài)解算的不良影響，進而實現(xiàn)了IMU姿態(tài)的準(zhǔn)確、穩(wěn)定輸出，滿足了在室內(nèi)復(fù)雜磁場環(huán)境中對IMU器件的應(yīng)用需求。

第一方面，本申請?zhí)峁┝艘环N基于九軸IMU的姿態(tài)解算方法，所述姿態(tài)解算方法包括：

基于獲取到的九軸IMU的陀螺儀、加速度計和磁力計的原始數(shù)據(jù)，分別得到經(jīng)過標(biāo)定后的陀螺儀標(biāo)定數(shù)據(jù)、加速度計標(biāo)定數(shù)據(jù)和磁力計標(biāo)定數(shù)據(jù)；

對得到的陀螺儀標(biāo)定數(shù)據(jù)和加速度計標(biāo)定數(shù)據(jù)進行數(shù)據(jù)融合，得到初步姿態(tài)數(shù)據(jù)；

確定一個表征磁場時間統(tǒng)計和空間統(tǒng)計的磁場矢量-權(quán)重值集合；

將加速度計標(biāo)定數(shù)據(jù)和磁力計標(biāo)定數(shù)據(jù)轉(zhuǎn)換成用于矢量描述的加速度矢量和磁場矢量；

當(dāng)?shù)竭_預(yù)設(shè)的磁力修正時刻時，根據(jù)所述初步姿態(tài)數(shù)據(jù)和當(dāng)前磁力修正時刻獲得的磁場矢量對所述磁場矢量-權(quán)重值集合進行修正，得到修正后的磁場矢量-權(quán)重值集合；

當(dāng)?shù)竭_預(yù)設(shè)的姿態(tài)修正時刻時，根據(jù)修正后的磁場矢量-權(quán)重值集合中權(quán)重值最高的磁場矢量以及當(dāng)前處于靜止?fàn)顟B(tài)的加速度計的加速度矢量，對所述初步姿態(tài)數(shù)據(jù)進行修正，得到最終姿態(tài)數(shù)據(jù)，其中，所述姿態(tài)修正時刻對應(yīng)周期是所述磁力修正時刻對應(yīng)周期的整數(shù)倍。

優(yōu)選地，所述對得到的陀螺儀標(biāo)定數(shù)據(jù)和加速度計標(biāo)定數(shù)據(jù)進行數(shù)據(jù)融合，得到初步姿態(tài)數(shù)據(jù)，包括：

根據(jù)上一采樣時刻已計算得到的初步姿態(tài)數(shù)據(jù)，結(jié)合獲得的當(dāng)前采樣時刻陀螺儀標(biāo)定數(shù)據(jù)，計算得到當(dāng)前采樣時刻的候選初步姿態(tài)數(shù)據(jù)；

若所述九軸IMU處于非靜止?fàn)顟B(tài)，則基于所述候選初步姿態(tài)數(shù)據(jù)對所述初步姿態(tài)數(shù)據(jù)進行更新，得到更新后的初步姿態(tài)數(shù)據(jù)；

若所述九軸IMU處于靜止?fàn)顟B(tài)，由當(dāng)前采樣時刻的加速度計標(biāo)定數(shù)據(jù)確定在IMU初始坐標(biāo)系下的當(dāng)前采樣時刻重力矢量；