[發明專利]一種九軸傳感器的誤差參數標定方法

有效

• 申請號: 202011385008.6
• 申請日: 2020-11-30
• 公開（公告）號: CN112539777B
• 公開（公告）日: 2021-09-14
• 發明（設計）人: 曠儉;牛小驥;李泰宇;劉韜
• 申請（專利權）人: 武漢大學
• 主分類號: G01D18/00
• 分類號: G01D18/00
• 代理公司: 武漢科皓知識產權代理事務所(特殊普通合伙) 42222
• 代理人: 王琪

1.一種九軸傳感器的誤差參數標定方法，其特征在于，包括如下步驟：

步驟1，以九軸傳感器的測量中心為旋轉中心，使其繞著傳感器坐標系的X、Y、Z三軸分別旋轉多圈；

步驟2，建立九軸傳感器中陀螺儀、加速度計、磁力計的測量模型，確定需要標定的參數；

步驟3，使用捷聯慣導算法解算九軸傳感器在標定過程中各個時刻的位置、速度及姿態，結合當地坐標系下磁場強度與磁力計原始觀測值的關系，構建卡爾曼濾波器中的系統誤差狀態方程；

步驟3的實現方式包括以下子步驟，

31)慣導算法和磁場預測算法建立系統狀態方程

其中，由于MEMS九軸傳感器器件精度低，地球自轉對捷聯慣導推算造成的誤差影響很小，故慣導算法中不考慮地球自轉，慣導算法和磁場預測算法具體如下：

式中，上標n表示當地坐標n系，n系是以慣性傳感器IMU相位中心為原點，x軸平行于當地水平面指向正北，y軸平行于當地水平面指向正東，z軸垂直于當地水平面向下，三者構成右手系；k表示第k個歷元；為第k個歷元時n系下的位置向量；為第k個歷元時n系下的速度向量；為第k個歷元時b系到n系的方向余弦轉換矩陣；為第k個歷元時磁場強度在n系的投影；表示第k個歷元加速度計的原始輸出；gⁿ表示重力加速度在n系下的投影向量；Δt＝t_k-t_k-1為第k-1個第k個觀測歷元的時間間隔；表示第k個歷元陀螺儀計的原始輸出；為磁力計傳感器觀測坐標系到IMU傳感器坐標系的轉換矩陣；為第k個歷元時磁力計原始輸出；b_m為磁力計零偏；(·×)為向量叉乘形式；

32)建立離散時間系統誤差狀態微分方程

卡爾曼濾波的狀態向量包含導航誤差狀態和傳感器誤差，30維的連續誤差狀態向量定義如下：

式中，δx表示狀態向量誤差；δrⁿ、δvⁿ、Ψ分別為n系下位置誤差、速度誤差和姿態誤差向量；δb_g、δb_a、δb_m分別為陀螺、加速度計、磁力計的零偏誤差向量；δS_g、δS_a分別為陀螺、加速度計比例因子誤差向量；為b系到m系的轉換矩陣誤差；δmⁿ表示估計的n系磁場強度分量誤差；δx中各個分量均為時間的函數，為方便書寫，省略了時間變量t的書寫，離散后的系統誤差模型表示如下：

δx_k＝Φ_k/k-1δx_k-1/k-1+Γ_k-1w_k-1 (6)

式中，下標k-1和k分別表示時刻t_k-1和t_k；Φ_k/k-1表示t_k-1到t_k時刻的狀態一步轉移矩陣；Γ_k-1為系統噪聲驅動陣；w_k-1為系統激勵噪聲；

為得到離散后的系統誤差模型，首先需要得到δx的連續時間微分方程，具體形式為：

式中，表示各個誤差狀態向量的微分方程；F(t)為各個狀態向量誤差微分方程的系數矩陣；G(t)為驅動噪聲系數矩陣；w(t)為各個狀態向量誤差的驅動噪聲；

各個系統誤差狀態微分方程如下：

式中，分別表示n系下位置誤差、速度誤差和姿態誤差、陀螺儀零偏誤差、加速度計零偏誤差、陀螺儀比例因子誤差、加速度計比例因子誤差、磁力計零偏誤差、磁力計轉換矩陣誤差、n系磁場強度分量誤差的微分方程；δrⁿ、δvⁿ、Ψ分別為n系下位置誤差、速度誤差和姿態誤差向量；δb_g、δb_a、b_m分別為陀螺、加速度計、磁力計的零偏誤差向量；δS_g、δS_a分別為陀螺、加速度計比例因子誤差向量；為b系到m系的轉換矩陣誤差；δmⁿ表示估計的n系磁場強度分量誤差；w_a為加速度計傳感器噪聲，為高斯白噪聲；w_g為陀螺儀傳感器噪聲，為高斯白噪聲；τ_bg、τ_ba、τ_sg、τ_sa分別為陀螺儀零偏相關時間、加速度計零偏相關時間、陀螺比例因子相關時間、加速度計比例因子相關時間；w_bg、w_ba、w_sg、w_sa分別為陀螺儀零偏驅動白噪聲、加速度計零偏驅動白噪聲、陀螺比例因子驅動白噪聲、加速度計比例因子驅動白噪聲；

33)構建誤差狀態的系統誤差狀態方程

根據32)中得到的系統誤差狀態微分方程，得到系統誤差狀態方程的一步轉移矩陣，即δx_k＝Φ_k/k-1δx_k-1/k-1+Γ_k-1w_k-1中的Φ_k/k-1；

Φ_k/k-1＝exp{F(t_k-1)Δt}≈I+F(t_k)Δt (9)

式中，I為單位陣，至此，卡爾曼濾波器的系統誤差狀態方程建立完成；誤差狀態向量每次濾波反饋后都需要進行置零處理，因此不對誤差狀態向量預測，只對誤差狀態量的協方差進行預測；

步驟4，標定過程中，基于位置不變和單一位置下環境磁場不變的假設，構建卡爾曼濾波器的量測方程，完成傳感器誤差參數標定。