孫逸帆陳洋豪李 鳳徐 祥

(蘇州大學(xué)電子信息學(xué)院，江蘇 蘇州 215006)

隨著MEMS 技術(shù)的不斷發(fā)展，MEMS 磁力計(jì)有著廣泛的應(yīng)用，通過(guò)磁力計(jì)可對(duì)地磁場(chǎng)進(jìn)行測(cè)量，并計(jì)算得到姿態(tài)和航向信息。 但三軸磁強(qiáng)計(jì)的測(cè)量精度會(huì)隨著制作工藝等問(wèn)題存在確定性誤差，例如標(biāo)度因子誤差，零偏誤差，非正交性誤差。 此外，外界環(huán)境也會(huì)對(duì)磁力計(jì)測(cè)量精度帶來(lái)影響，包括軟磁材料引起的軟鐵干擾和硬磁材料引起的硬鐵干擾。 因此，為提高磁力計(jì)的測(cè)量精度，在使用磁力計(jì)前需進(jìn)行標(biāo)定[1－2]。

目前，國(guó)內(nèi)外對(duì)磁標(biāo)定問(wèn)題進(jìn)行了很多研究。 一種是姿態(tài)無(wú)關(guān)法，即不需要外部姿態(tài)信息作為參考，利用歸一化后的磁矢量輸出的模值為1，可對(duì)磁力計(jì)誤差參數(shù)進(jìn)行標(biāo)定。 在文獻(xiàn)[3－4]中，作者通過(guò)將磁矢量的輸出投射到一個(gè)橢球上，構(gòu)建了磁力計(jì)的誤差參數(shù)與橢球參數(shù)的函數(shù)關(guān)系式，通過(guò)擬合橢球參數(shù)完成對(duì)磁力計(jì)誤差參數(shù)的估計(jì)。 為了進(jìn)一步提高標(biāo)定速度與精度，在文獻(xiàn)[5－6]中，作者通過(guò)模值標(biāo)定法構(gòu)建了關(guān)于磁力計(jì)誤差參數(shù)的代價(jià)函數(shù)，并通過(guò)遞推最小二乘法或列文伯格法對(duì)代價(jià)函數(shù)進(jìn)行迭代運(yùn)算進(jìn)而完成標(biāo)定。 為克服磁力計(jì)隨機(jī)噪聲的影響，在文獻(xiàn)[7－8]中，作者對(duì)隨機(jī)噪聲進(jìn)行了考慮，構(gòu)建了極大似然估計(jì)函數(shù)最優(yōu)估計(jì)模型，并利用牛頓法對(duì)代價(jià)函數(shù)進(jìn)行迭代求解。 對(duì)最優(yōu)估計(jì)模型進(jìn)行求解得到磁力計(jì)參數(shù)精度較高，但由于無(wú)外部參考信息，無(wú)法完成磁力計(jì)坐標(biāo)系與慣性單元坐標(biāo)系間未對(duì)準(zhǔn)誤差估計(jì)，影響在實(shí)際應(yīng)用中的精度。

另一種磁力計(jì)標(biāo)定方法為姿態(tài)相關(guān)法，通過(guò)利用外部姿態(tài)信息，即陀螺輸出作為輔助，可對(duì)磁力計(jì)的誤差參數(shù)和磁力計(jì)與陀螺坐標(biāo)系之間的未對(duì)準(zhǔn)誤差進(jìn)行標(biāo)定[9－10]。 但在利用陀螺信息作為輔助時(shí)，首先要保證陀螺的精度。 對(duì)于陀螺標(biāo)定，通常利用轉(zhuǎn)臺(tái)等設(shè)備，但這對(duì)MEMS 陀螺標(biāo)定是昂貴的，且不滿(mǎn)足現(xiàn)場(chǎng)標(biāo)定的實(shí)際情況。 因此，先對(duì)磁力計(jì)單獨(dú)標(biāo)定，再使用加速度計(jì)信息作為輔助，對(duì)磁力計(jì)和磁力計(jì)與慣性單元間的未對(duì)準(zhǔn)誤差進(jìn)行標(biāo)定的兩步標(biāo)定法是近年來(lái)研究的主要方向。 在文獻(xiàn)[11－13]中，作者首先對(duì)加速度計(jì)和磁力計(jì)的誤差參數(shù)都采用極大似然估計(jì)法進(jìn)行估計(jì)。 在完成傳感器的單獨(dú)標(biāo)定后，再利用加速度計(jì)和磁力計(jì)間點(diǎn)積不變的性質(zhì)，對(duì)磁力計(jì)和慣性單元間的未對(duì)準(zhǔn)誤差進(jìn)行估計(jì)。 在文獻(xiàn)[14]中，作者通過(guò)聯(lián)合標(biāo)定的方法，將加速度計(jì)誤差，磁力計(jì)誤差和磁力計(jì)與慣性單元間的未對(duì)準(zhǔn)誤差構(gòu)建在一個(gè)代價(jià)函數(shù)中，通過(guò)對(duì)代價(jià)函數(shù)求解可得到誤差參數(shù)，但這一求解過(guò)程計(jì)算量較大，不適用于對(duì)計(jì)算量有要求的現(xiàn)場(chǎng)標(biāo)定中。 在文獻(xiàn)[15－17]中，作者利用加速度計(jì)和磁力計(jì)點(diǎn)積不變的性質(zhì)對(duì)磁力計(jì)的誤差參數(shù)和未對(duì)準(zhǔn)誤差構(gòu)建關(guān)系式，通過(guò)最小二乘法進(jìn)行求解。但為標(biāo)定準(zhǔn)確，需采集足夠多的數(shù)據(jù)，計(jì)算效率低。

因此，本文提出一種基于遞推最小二乘法，利用加速度計(jì)信息輔助標(biāo)定磁力計(jì)的方法，對(duì)磁力計(jì)誤差參數(shù)和磁力計(jì)與慣性單元間的未對(duì)準(zhǔn)誤差進(jìn)行一步標(biāo)定。 首先，對(duì)加速度計(jì)進(jìn)行標(biāo)定。 再利用加速度計(jì)和磁力計(jì)間點(diǎn)積不變的性質(zhì)，構(gòu)建誤差參數(shù)模型。 最后，通過(guò)遞推最小二乘法，完成對(duì)磁力計(jì)誤差參數(shù)的參數(shù)識(shí)別。

1 相關(guān)工作與問(wèn)題描述

1.1 MEMS 磁力計(jì)建模

常用的磁力計(jì)誤差模型為:

圖1 非正交性誤差示意圖

式中:dm＝－Rmb，nm＝－Rmε。

1.2 MEMS 磁力計(jì)標(biāo)定的橢球擬合法分析

磁力計(jì)標(biāo)定的姿態(tài)無(wú)關(guān)法為橢球擬合標(biāo)定法，由地磁矢量的模值可得到:

式中:E＝(Rm)TRm，F(xiàn)＝(dm)TRm，G＝(dm)Tdm。 由于存在測(cè)量誤差，磁力計(jì)的輸出矢量位于一個(gè)橢球面上，而橢球方程的系數(shù)是磁力計(jì)誤差參數(shù)的函數(shù)。 因此對(duì)磁力計(jì)誤差參數(shù)的估計(jì)可看做為橢球模型參數(shù)的估計(jì)。 在對(duì)參數(shù)Rm求解時(shí)，通常通過(guò)對(duì)E進(jìn)行Cholesk 分解。 但由于無(wú)外部參考信息，因此分解不能得到唯一解。 例如，(Rm)T和Rm為一組解，(Rm)TQT和QRm為一組新的解。 其中Q為正交矩陣，代表標(biāo)定得到的磁力計(jì)坐標(biāo)系(b?系)與磁力計(jì)真實(shí)所在b系間的方向余弦矩陣。 圖2 表示磁力計(jì)在各個(gè)坐標(biāo)系下三軸輸出的示意圖，實(shí)線(xiàn)代表磁力計(jì)在s系的三軸輸出，虛線(xiàn)代表磁力計(jì)在b?系的三軸輸出，半劃線(xiàn)代表磁力計(jì)在b系的三軸輸出。 如果沒(méi)有外部參考信息，則無(wú)法確定b系與b?系間的轉(zhuǎn)換關(guān)系。

圖2 磁力計(jì)在s 系，b 系和b?系坐標(biāo)系三軸輸出示意圖

因此，在只針對(duì)磁力計(jì)傳感器單獨(dú)進(jìn)行標(biāo)定時(shí)，不唯一解對(duì)標(biāo)定結(jié)果沒(méi)有影響。 而在多傳感器的情況下，傳感器的誤差和傳感器間的未對(duì)準(zhǔn)誤差的影響都不可忽略。 為消除這種影響，可通過(guò)加速度計(jì)的輸出作為輔助，完成磁力計(jì)與慣性單元間未對(duì)準(zhǔn)誤差的標(biāo)定。

1.3 MEMS 加速度計(jì)極大似然法標(biāo)定

為實(shí)現(xiàn)加速度計(jì)輔助磁力計(jì)標(biāo)定，首先應(yīng)該對(duì)MEMS 加速度計(jì)進(jìn)行標(biāo)定處理。 MEMS 加速度計(jì)測(cè)量模型為:

對(duì)加速度計(jì)的標(biāo)定，通常采用模值法。 但在標(biāo)定過(guò)程中，存在隨機(jī)噪聲的二次項(xiàng)的數(shù)學(xué)期望不為零這一問(wèn)題，因此傳統(tǒng)方法中標(biāo)定加速度計(jì)過(guò)程中忽略隨機(jī)噪聲ε對(duì)誤差參數(shù)的估計(jì)屬于有偏估計(jì)。為消除隨機(jī)噪聲對(duì)標(biāo)定結(jié)果的影響，構(gòu)建極大似然估計(jì)模型:

對(duì)極大似然函數(shù)模型(6)，通常采用牛頓法或梯度下降法，通過(guò)迭代計(jì)算估計(jì)出MEMS 加速度計(jì)的誤差參數(shù)。 為了保證迭代的準(zhǔn)確性，需要對(duì)迭代的初始值進(jìn)行估計(jì)。 迭代初始值的估計(jì)，可在式(5)中忽略噪聲ε的情況下，利用最小二乘法估計(jì)得到誤差參數(shù)。

2 加速度計(jì)輔助MEMS 磁力計(jì)標(biāo)定方法

2.1 MEMS 加速度計(jì)極大似然法標(biāo)定

因?yàn)榈厍蛏先我晃恢玫闹亓κ噶颗c地磁矢量間的夾角為固定值，磁傾角如圖3 中虛線(xiàn)所示角，磁傾角γ可表示為:

圖3 地球上任一位置矢量示意圖

式(10)為對(duì)MEMS 磁力計(jì)誤差參數(shù)的線(xiàn)性表達(dá)式，針對(duì)這個(gè)線(xiàn)性回歸問(wèn)題，可以通過(guò)最小二乘法或其他方法進(jìn)行解決，進(jìn)而完成標(biāo)定。 為減小計(jì)算量，提高效率，本文采用遞推最小二乘法對(duì)式(10)進(jìn)行迭代求解。

2.2 遞推最小二乘參數(shù)識(shí)別法

由于標(biāo)定是在地球上某個(gè)位置進(jìn)行的，因此可認(rèn)為磁傾角不發(fā)生改變，因此標(biāo)定過(guò)程中的重力矢量與地磁矢量點(diǎn)積值為固定值，觀測(cè)方程可寫(xiě)為:

式中:Kk表示k時(shí)刻的增益矩陣；Pk｜k－1表示k時(shí)刻一步預(yù)測(cè)狀態(tài)誤差協(xié)方差矩陣；Rk表示k時(shí)刻量測(cè)噪聲協(xié)方差矩陣；＾θk｜k表示k時(shí)刻誤差參數(shù)最優(yōu)估計(jì)值；Pk｜k表示k時(shí)刻的誤差協(xié)方差矩陣；I9×9表示大小為9×9 的單位矩陣。 誤差協(xié)方差矩陣初始值設(shè)置如(16)所示，迭代參數(shù)的初始值設(shè)置為(17)所示:

3 仿真和實(shí)驗(yàn)

3.1 仿真部分

為驗(yàn)證本文提出算法，仿真產(chǎn)生多位置MEMS加速度計(jì)與MEMS 磁力計(jì)數(shù)據(jù)。 設(shè)置數(shù)據(jù)采樣頻率為100 Hz。 根據(jù)MEMS 磁力計(jì)誤差模型式(2)，設(shè)置磁力計(jì)誤差參數(shù)如(18)，(19)所示。 MEMS 磁力計(jì)噪聲設(shè)定為均值為零，標(biāo)準(zhǔn)差為0.007 的高斯白噪聲。 仿真產(chǎn)生標(biāo)定后的MEMS 加速度計(jì)數(shù)據(jù)，其隨機(jī)噪聲設(shè)定為均值為零，標(biāo)準(zhǔn)差為0.002 的高斯白噪聲。

仿真產(chǎn)生的50 s 數(shù)據(jù)通過(guò)本文提出的方法進(jìn)行標(biāo)定，圖4～圖6 為估計(jì)出的磁力計(jì)誤差參數(shù)迭代過(guò)程。 其中，虛線(xiàn)代表設(shè)定的磁力計(jì)參數(shù)，實(shí)線(xiàn)代表磁力計(jì)估計(jì)得到參數(shù)。 從圖中可看出，在5 s 左右，dm和Rm的估計(jì)值已經(jīng)收斂到設(shè)定真值的附近。 觀察30 s～50 s 迭代過(guò)程中dm和Rm的估計(jì)值與設(shè)定真值間誤差，可看出誤差分別為10－3和10－4量級(jí)。 因此，可看出本文提出標(biāo)定方法收斂速度快，收斂值準(zhǔn)確。

圖4 MEMS 磁力計(jì)dm 估計(jì)值迭代過(guò)程圖

圖5 MEMS 磁力計(jì)Rm 估計(jì)值的模值迭代過(guò)程圖

圖6 MEMS 磁力計(jì)Rm 估計(jì)值的模值迭代過(guò)程圖

圖7 中，將本文提出方法與橢球擬合法標(biāo)定后的磁矢量模值進(jìn)行比較。 其中，點(diǎn)劃線(xiàn)表示標(biāo)定前磁矢量模值，實(shí)線(xiàn)表示橢球擬合法標(biāo)定后磁矢量模值，虛線(xiàn)表示本文方法標(biāo)定后磁矢量模值。 從圖中可看出，經(jīng)過(guò)標(biāo)定后，兩種方法的磁矢量模值都在1 附近波動(dòng)。 經(jīng)過(guò)橢球擬合法標(biāo)定后的磁矢量模值平均值為0.999 8，標(biāo)準(zhǔn)差為0.004 7。 經(jīng)過(guò)本文標(biāo)定方法標(biāo)定后的磁矢量模值平均值為0.998 4，標(biāo)準(zhǔn)差為0.004 8。從模值上看，兩種方法標(biāo)定效果相似。 但本文提出方法是基于線(xiàn)性式(9)，因此，在標(biāo)定過(guò)程中解決了加速度計(jì)與磁力計(jì)坐標(biāo)系對(duì)準(zhǔn)問(wèn)題，克服了橢球擬合法中分解的不唯一解帶來(lái)的影響。 根據(jù)式(7)，可對(duì)加速度計(jì)矢量與兩種方法標(biāo)定后的磁矢量間的旋轉(zhuǎn)角進(jìn)行求解。 圖8 中，虛線(xiàn)表示橢球擬合法標(biāo)定后矢量間旋轉(zhuǎn)角，實(shí)線(xiàn)表示本文方法標(biāo)定后矢量間旋轉(zhuǎn)角。 仿真中設(shè)置的磁傾角為47.5°，從圖8 中可看出，橢球擬合法標(biāo)定后矢量間旋轉(zhuǎn)角的曲線(xiàn)波動(dòng)較大，其均值為47.48°，標(biāo)準(zhǔn)差為2.924。 而經(jīng)本文方法標(biāo)定后的矢量間旋轉(zhuǎn)角波動(dòng)較小，其均值為47.52°，標(biāo)準(zhǔn)差為0.317。 因此，可認(rèn)為本文在標(biāo)定過(guò)程中穩(wěn)定完成了對(duì)矢量間對(duì)準(zhǔn)誤差的估計(jì)。

圖7 標(biāo)定前后磁矢量模值比較圖

圖8 加速度計(jì)矢量與磁力計(jì)矢量間旋轉(zhuǎn)角

3.2 實(shí)驗(yàn)部分

為采集實(shí)際數(shù)據(jù)對(duì)本文算法進(jìn)行驗(yàn)證，設(shè)計(jì)了硬件部分。 如圖9 所示，其中，STM32F407 作為主控芯片，對(duì)ICM20948 九軸MEMS 傳感器進(jìn)行數(shù)據(jù)采集，通過(guò)串口將數(shù)據(jù)傳輸?shù)缴衔粰C(jī)并存儲(chǔ)。 設(shè)置采樣頻率為100 Hz，對(duì)固定在無(wú)磁3D 打印的外殼中的硬件部分進(jìn)行緩慢旋轉(zhuǎn)，采集100 s 磁力計(jì)與加速度計(jì)數(shù)據(jù)并存儲(chǔ)。

圖9 實(shí)驗(yàn)實(shí)物示意圖

首先，對(duì)采集到的ICM20948 加速度計(jì)數(shù)據(jù)進(jìn)行極大似然標(biāo)定處理。 其次，利用磁力計(jì)和標(biāo)定后的加速度數(shù)據(jù)，分別通過(guò)橢球擬合法和本文提出方法進(jìn)行標(biāo)定處理。 圖10 中，對(duì)標(biāo)定前和經(jīng)過(guò)兩種方法標(biāo)定后的磁矢量模值進(jìn)行了比較。 其中，用o 表示標(biāo)定前磁矢量模值，用＋表示橢球擬合法標(biāo)定后磁矢量模值，實(shí)線(xiàn)表示本文方法標(biāo)定后磁矢量模值。從圖10 中可看出，本文方法標(biāo)定后磁矢量模值曲線(xiàn)高于橢球擬合法模值曲線(xiàn)，且經(jīng)過(guò)兩種方法標(biāo)定后的磁矢量模值在1 附近波動(dòng)。 其中，經(jīng)過(guò)橢球擬合法標(biāo)定后，磁矢量模值平均值為0.998 7，標(biāo)準(zhǔn)差為0.021。 經(jīng)過(guò)本文方法標(biāo)定后，磁矢量模值的平均值為1.012 7，標(biāo)準(zhǔn)差為0.021。 因此，可認(rèn)為完成了磁力計(jì)的標(biāo)定。

圖10 ICM20948 磁矢量模值標(biāo)定前后對(duì)比圖

在圖11 中，可觀察到兩種標(biāo)定方法標(biāo)定后磁力計(jì)與慣性單元間的旋轉(zhuǎn)角。 其中，虛線(xiàn)表示橢球擬合法標(biāo)定后矢量間旋轉(zhuǎn)角，實(shí)線(xiàn)表示本文方法標(biāo)定后矢量間旋轉(zhuǎn)角。 從圖中可看出橢球擬合法標(biāo)定后的矢量旋轉(zhuǎn)角波動(dòng)較大，而經(jīng)過(guò)本文方法標(biāo)定后的矢量旋轉(zhuǎn)角波動(dòng)較小。 在表1 中，可看到經(jīng)過(guò)橢球擬合法標(biāo)定后磁力計(jì)與慣性單元間旋轉(zhuǎn)角均值為29.69°，標(biāo)準(zhǔn)差為29.05。 經(jīng)過(guò)本文方法標(biāo)定后的磁力計(jì)與慣性單元間旋轉(zhuǎn)角均值為47°，標(biāo)準(zhǔn)差為1.682。 而蘇州地區(qū)的磁傾角為47.5°，考慮到消費(fèi)級(jí)ICM20948 九軸MEMS 傳感器精度較低，可認(rèn)為已完成磁力計(jì)與慣性單元間的未對(duì)準(zhǔn)誤差標(biāo)定。 因此，可認(rèn)為本文提出方法在完成磁力計(jì)標(biāo)定的同時(shí)，完成了加速度計(jì)與磁力計(jì)間的未對(duì)準(zhǔn)誤差。

圖11 ICM20948 數(shù)據(jù)加速度計(jì)矢量與磁力計(jì)矢量間旋轉(zhuǎn)角

表1 標(biāo)定后磁力計(jì)與慣性單元間旋轉(zhuǎn)角比較

4 結(jié)論

本文提出了一種基于遞推最小二乘法利用加速度計(jì)信息輔助標(biāo)定磁力計(jì)的方法，在完成磁力計(jì)誤差參數(shù)估計(jì)的同時(shí)，也完成了加速度計(jì)和磁力計(jì)間未對(duì)準(zhǔn)誤差的估計(jì)。 首先，對(duì)加速度計(jì)數(shù)據(jù)完成標(biāo)定，再利用加計(jì)矢量與磁矢量間點(diǎn)積不變的性質(zhì)，通過(guò)遞推最小二乘法對(duì)磁力計(jì)誤差參數(shù)進(jìn)行估計(jì)，進(jìn)而完成標(biāo)定。 通過(guò)仿真與實(shí)際數(shù)據(jù)，對(duì)本文提出方法進(jìn)行了驗(yàn)證，結(jié)果表明，本文提出方法的標(biāo)定精度與橢球擬合法相似，標(biāo)定后的加速度計(jì)矢量與磁矢量間旋轉(zhuǎn)角約等于當(dāng)?shù)卮艃A角。 相比橢球擬合法，本文提出的方法克服了由于無(wú)外部信息參考而無(wú)法確定磁力計(jì)誤差參數(shù)的影響。 并且通過(guò)遞推最小二乘法對(duì)磁力計(jì)誤差參數(shù)估計(jì)，計(jì)算效率高，可以快速完成標(biāo)定。