[發明專利]一種基于已知姿態角的磁力計標定方法

有效

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

1.一種基于已知姿態角的磁力計標定方法，其特征在于，包括如下步驟：

步驟1，在室外空曠區域選擇任意選擇一個位置，并根據位置的經緯度坐標查詢地球磁場參考模型，得到該位置的磁場總強度參考值M_{n_IGRF}；

步驟2，以傳感器測量中心為旋轉中心，使傳感器繞著載體坐標系的X、Y、Z三軸各旋轉多圈，并獲取標定過程中每個歷元的姿態角及磁場信息；

步驟3，建立磁力計測量模型，確定待估參數；對磁力計的測量模型進行建模時，考慮磁力計受到軟磁誤差、比例因子誤差、交軸耦合誤差、零位偏置誤差、硬鐵誤差及傳感器噪聲，載體坐標系下磁力計的測量模型如下：

對磁力計原始觀測數據m_{b_init}進行均值濾波，降低磁力計噪聲n_M，得到m_b；將的乘積作為一個待估矩陣，記為將b_M和b_HI之和作為一個待估矩陣，記為B_M；模型簡化后如下式，待估參數為和B_M；

其中，b表示載體坐標系b系，b系是以慣性傳感器IMU相位中心為原點，x軸指向載體前進方向，y軸垂直于x軸指向載體右側，z軸與x軸和y軸垂直并構成右手系；表示載體坐標系下磁場強度的真實值；表示磁力計坐標系到慣性傳感器IMU坐標系的姿態旋轉矩陣，為3×3方陣；C_SI表示軟磁效應變化矩陣；C_NO是磁力計三軸由非正交轉化為正交的變化矩陣，為3×3方陣，主對角線元素為0；S_M為比例因子，為3×3方陣，只有主對角線元素有數據，其余元素為0；m_{b_init}表示磁力計原始觀測數據；b_M表示磁力計零位偏置；b_HI表示硬磁效應誤差；n_M為磁力計噪聲矢量；

將磁力計的測量模型進行泰勒展開，使用最小二乘法迭代求得待估參數。

2.根據權利要求1所述一種基于已知姿態角的磁力計標定方法，其特征在于：對磁力計測量模型進行迭代計算時，首先以m_b作為初次迭代計算時載體坐標系下磁場強度參考真值，即之后迭代計算時通過求得，然后求得當地坐標系下的磁場強度真值計算關系如下式：

式中，k表示第k個歷元，j表示歷元的總個數；表示第k個歷元載體坐標系到當地坐標系的姿態旋轉矩陣，表示第k個歷元載體坐標系下磁場強度參考真值；n表示當地坐標n系，n系是以慣性傳感器IMU相位中心為原點，x軸平行于當地水平面指向正北，y軸平行于當地水平面指向正東，z軸垂直于當地水平面向下，三者構成右手系；

在基于磁強總強度不變的假設下，根據M_{n_IGRF}求得其磁場總強度||M_{n_IGRF}||，通過計算出與||M_{n_IGRF}||的比例關系，將各軸的數據等比例變化，得到M_{n_ture}，將其作為當地坐標系下的磁場真值，進而得到載體坐標系下磁場強度參考真值M_{n_ture}及計算過程如下：

其中，表示當地坐標系到載體坐標系的姿態旋轉矩陣。