基于改進Levenberg-Marquardt算法的地磁矢量校準方法*

宋忠國，許丙琳，高久翔，李 婷，張金生，席曉莉

(1.西安理工大學自動化與信息工程學院，陜西 西安 710048；2.火箭軍工程大學，陜西 西安 710025)

三軸磁傳感器廣泛被應用于地磁匹配導航領域進行地磁場矢量測量[1－2]，然而加工工藝限制使得磁傳感器存在誤差，主要包括非正交誤差、靈敏度誤差和零偏誤差[3－4]，而且安裝工藝限制使得磁傳感器和其他慣性傳感器坐標系之間還存在非對準誤差[5]，所以必須校準這些誤差才能將三軸磁傳感器應用于地磁矢量的高精度測量。

橢球擬合方法是一種常見的標量校準方法[6]，通常需要先利用三軸磁傳感器的測量數據求得橢球參數，再通過矩陣分解得到誤差校準參數，是一種間接求解誤差校準參數的方法。該方法求得橢球參數后只能得到DTD(D為誤差校準參數)，雖然可以使用Cholesky分解[7－8]、奇異值分解[6，9－10]等矩陣分解方法從DTD中得到其中的某一個解D1，但是任意旋轉矩陣R和D1的乘積RD1依然是校準參數D的解[7，11]，這是因為(RD1)T(RD1)＝D1TRTRD1＝D1TD1，所以橢球擬合方法得到的校準參數會引入一個任意的旋轉矩陣R[12]，即橢球擬合法所求解誤差校準參數D理論上存在無窮多解。此外，鐘浩等人提出使用非線性最小二乘方法[13]直接求解三軸磁傳感器誤差校準參數，將非正交、靈敏度、零偏誤差參數整合為待估計參數，通過Levenberg-Marquard(LM)迭代算法直接求解誤差校準參數矩陣D并且無需矩陣分解；羅靜博等人提出基于型入侵野草算法校準三軸磁傳感器非正交誤差[14]也無需矩陣分解。但是這些文獻僅僅使用了三軸磁傳感器的測量數據進行誤差校準，相比于使用更高精度的磁傳感器提供地磁場真值參考，這將導致誤差校準后三軸磁傳感器測量地磁總場的精度偏低，而且這些文獻并沒有考慮非對準誤差，不能獲取載體坐標系下地磁場分量信息。李婷等人提出了基于阻尼粒子群優化的地磁場測量誤差補償方法[15]來直接求解誤差校準參數并且使用了高精度的質子磁傳感器提供地磁場真值，該方法提高了地磁總量校準精度，但仍未考慮非對準誤差。總之，為了避免橢球擬合方法需要矩陣分解的弊端和彌補現有直接求解誤差校準參數方法在三軸磁傳感器測量地磁場總量以及分量上的不足，需要研究新的誤差校準方法。

本文提出一種基于改進LM算法的地磁矢量校準方法，首先使用質子磁傳感器提供地磁場真值，利用改進LM算法直接求解三軸磁傳感器誤差校準參數并且無需矩陣分解，然后利用加速度計測量恒定重力場作為輔助信息校準三軸磁傳感器和加速度計坐標系之間的非對準誤差。通過仿真和實驗驗證了該方法可以得到加速度計坐標系下高精度地磁矢量。

1 三軸磁傳感器誤差及非對準誤差模型

1.1 三軸磁傳感器誤差模型

根據誤差產生機理的不同，三軸磁傳感器誤差主要分為3類:非正交誤差、靈敏度誤差、零偏誤差。

①非正交誤差

非正交誤差是指由于三軸磁傳感器工藝制造水平的影響，三個測量軸無法保證兩兩正交，從而引起的測量誤差，其幾何定義如圖1所示。

圖1 非正交誤差

假設xyz坐標軸構成的坐標系為三軸磁傳感器理想正交坐標系，而x′y′z′坐標軸構成的坐標系為實際情況下的非正交坐標系。將兩套坐標系的坐標原點重合、x坐標軸和x′坐標軸重合，y坐標軸和y′坐標軸共面，可以得到非正交誤差角(α1α2α3)。理想正交坐標系和實際非正交坐標系之間的非正交誤差可以等效為矩陣O，其數學模型可以表示為:

②靈敏度誤差

靈敏度誤差是指由于三個測量軸各自的靈敏度不能做到完全一致、測量信號的放大電路特性不完全相同，從而引起的測量誤差。

③零偏誤差

零偏誤差是指實際使用過程中磁傳感器各個測量軸、模擬調理電路、A/D轉換的零點不為零所帶來的測量誤差。

靈敏度誤差K和零偏誤差b的數學模型如下所示，其中Kx、Ky、Kz以及bx、by、bz分別代表三軸磁傳感器x、y、z三個測量軸各自不同的靈敏度和零偏。

式中，n為實際測量時的高階噪聲。

令C＝b＋n，則誤差模型可以整理為:

那么有(3)成立:

式中，D和E稱為誤差校準參數，求出后即可得到三軸磁傳感器坐標系下的地磁場矢量Bt。

1.2 非對準誤差模型

由于安裝工藝限制，三軸磁傳感器坐標系和加速度計坐標系或其他慣性坐標系之間還存在坐標系非對準誤差，只有校準該誤差才能將Bt投影至加速度計或其他慣性坐標系下，從而提高加速度計坐標系下或其他慣性坐標系下地磁場矢量測量精度。

本文考慮了三軸磁傳感器坐標系和加速度計坐標系之間的非對準誤差。非對準誤差可以等價為一個正交旋轉矩陣Rm，設xyz坐標系為三軸磁傳感器正交坐標系，x2y2z2坐標系為加速度計正交坐標系，xyz坐標系到x2y2z2坐標系轉換所需的旋轉歐拉角為(u1u2u3)，其幾何定義如圖2所示，則Rm的數學模型可以表示為式。

圖2 xyz坐標系到x2y2z2坐標系的變換

式中，

求得Rm即可得到加速度計坐標系下的地磁場矢量。

2 基于改進LM算法的三軸磁傳感器校準方法

求解三軸磁傳感器誤差校準參數的問題可以被視為非線性最小二乘參數估計問題，本節首先建立目標函數模型，再通過改進LM算法直接求解誤差校準參數，且無需矩陣分解。

2.1 非線性最小二乘目標函數構建

理想情況下三軸磁傳感器測量的地磁總場值Bt可以表示為:

式中，

將質子磁傳感器測量地磁總場值BM作為地磁總場的真實值，那么求解三軸磁傳感器誤差校準參數的非線性最小二乘問題可以表示為式:

式中，F(Bc，x)為目標函數。

式中，r(Bc，x)為殘差函數，采用三軸磁傳感器測量的地磁總場Bt和地磁總場真實值BM之間的相對誤差來表示，描述了誤差校準后Bt相對于真值BM的偏差程度。待估計參數向量為x＝[bxbybzKxKyKzα1α2α3]，它包含了三軸磁傳感器零偏誤差、靈敏度誤差以及非正交誤差模型中的9個參數。

采用Bt相對于BM的相對誤差作為殘差函數，以最小二乘為準則，通過非線性最小二乘方法求解誤差校準參數可以使得三軸磁傳感器的總量輸出盡可能地逼近質子磁傳感器的輸出。這種方法的好處:非線性最小二乘方法求得估計參數x后可以根據1.1節中的誤差模型直接得到靈敏度誤差矩陣K、非正交誤差矩陣O以及零偏向量b，無需矩陣分解便可以由式得到三軸磁傳感器誤差校準參數矩陣D和E。這樣不僅可以避免橢球擬合方法矩陣分解時引入任意旋轉矩陣R的問題，而且由于使用了更高精度的質子磁傳感器作為了校準參考，誤差校準后三軸磁傳感器在地磁場總量測量方面的精度會更高。

2.2 改進的LM算法

本文使用的LM算法是牛頓高斯法和最陡下降法相結合的一種算法，其保留了兩者各自的優越性。LM算法的傳統迭代形式為式(9)，當λ較大時，式(9)退化為最陡下降法，在距極值點較遠的位置具有較大的下降速度，而當λ較小時，式(9)退化為牛頓高斯法，在極值點附近也能快速收斂。

改進的LM算法迭代形式如式(10)所示:

式(11)中:Jr(m×9)為殘差函數r的雅可比矩陣，m為采集數據量的個數，9為待估計參數個數，式(12)為Jr中元素的一般表達形式；為目標函數F的梯度，r(m×1)為m個測量數據對應的殘差向量；代替了二階偏導數構成的黑塞矩陣；λ為阻尼因子。

3 非對準誤差校準原理

經改進LM算法校準三軸磁傳感器誤差后，即可得到三軸磁傳感器坐標系下高精度地磁場矢量Bt，由于并未使用矩陣分解，Bt中不會引入任意旋轉矩陣，這將在校準非對準誤差時避免任意旋轉矩陣所帶來的影響。由于矩陣分解會引入旋轉矩陣，求解的非對準誤差參數將是安裝工藝帶來的坐標系旋轉和矩陣分解引入的坐標系旋轉二者疊加的參數，此時矢量校準精度較低。此外，當矩陣分解時引入的旋轉矩陣所對應的旋轉歐拉角較大時，將無法再利用重力場測量數據作為輔助使得三軸磁傳感器測量的地磁場投影到加速度計坐標系上。

式中，G＝[gxgygz]T為重力場G對應的單位矢量，為對應的單位矢量，Bt＝[bxbybz]T為Bt對應的單位矢量，使用最小二乘參數擬合法即可求解式(13)中非對準誤差矩陣Rm，將Rm代入式(5)中即可得到加速度坐標系下的高精度地磁場矢量。

4 數值仿真驗證

首先，模擬理想情況下三軸磁傳感器采集的地磁場矢量信號Bt，其總量真實值由模擬的質子磁傳感器測量值提供；其次，加入預先設置的非正交、靈敏度、零偏和非對準誤差以及隨機測量噪聲，得到三軸磁傳感器實際工作情況下測量的地磁場信號Bc；最后對本文提出的基于改進LM算法的地磁矢量校準方法進行驗證并分析校準效果。

預先設置的非正交誤差角為(1° 1.5° 2°)，則非正交誤差O如下所示:

預設的靈敏度誤差K，零偏誤差b(單位nT)為:

將上述預設的誤差模型代入式中可以得到誤差校準參數D和E的真實值:

模擬質子磁傳感器測量的地磁總場真實值BM為52 590 nT，模擬測量噪聲n為信噪比60 dB的高斯白噪聲。預先設置的三軸磁傳感器坐標系和加速度計坐標系之間非對準旋轉歐拉角為(1°0.5°1.5°)，重力矢量G取[0 0 9.8]T。三軸磁傳感器和加速度計經過模擬姿態變換可以得到不同姿態下的測量數據Bc和G。

將模擬的三軸磁傳感器測量數據Bc和質子磁傳感器測量的地磁場真值BM代入下述LM算法迭代步驟中，分別采用2.2節描述的兩種LM算法迭代形式均可以求得x參數的估計值＾x，兩種迭代形式的結果對比分別如表1、表2所示(表中α1、α2、α3為其弧度制表示，單位為rad)，表明使用改進LM算法求出＾x的誤差更小。

表1 基于傳統LM算法x參數估計結果

表2 基于改進LM算法x參數估計結果

LM算法迭代求解待估計參數x的步驟如下:

①初始迭代次數n＝1，待估計參數向量x初始值x0＝[0 0 0 1 1 1 0 0 0](K－1中分母不能為0，這里取1)，阻尼因子初始值λ0＝0.01；

③若‖rn＋1‖2≤‖rn‖2，則λn＋1＝λn/10，否則，λn＋1＝10λn，轉(4)；

④迭代次數n＝n＋1，更新，更新δxn，返回②。

使用改進LM算法求解出后代入第1.1節中的誤差模型中，可以得到校準參數D和E的估計值和，其與真實值相比差距很小，將和代入式(3)即可實現三軸磁傳感器誤差校準。

校準三軸磁傳感器誤差后，殘差r的頻數分布直方圖如圖3所示，該頻數分布滿足均值為0的高斯分布，紅色曲線為對應的高斯分布曲線，這符合測量噪聲為高斯分布的預設。

圖3 殘差頻數分布直方圖

三軸磁傳感器誤差校準前后測量地磁總場對比如圖4所示，誤差校準后的絕對誤差如圖5所示。經過分析，校準前，地磁總場測量誤差較大，均方根誤差為2 813.9 nT，校準后，誤差大幅度減小，均方根誤差減小至29.8 nT。

圖4 校準前后地磁場總量

圖5 校準后地磁場總量絕對誤差

由預設的旋轉歐拉角可以得到非對準誤差參數的真實值Rm，而利用校準磁傳感器誤差后得到的地磁場矢量Bt和重力矢量測量值G，并根據第3節非對準誤差校準原理可以求得Rm的估計值，將其代入式(5)即可得到加速度坐標系下的地磁場矢量。

圖6為非對準誤差校準前后三軸磁傳感器測量地磁場分量的絕對誤差。非對準誤差校準前加速度計坐標系下X軸、Y軸、Z軸地磁場分量的均方根誤差分別為:583.7 nT、580.1 nT、582.3 nT，誤差校準后，三分量的均方根誤差分別為:28.3 nT、29.6 nT、34.1 nT。

圖6 校準前后分量絕對誤差

5 實驗結果分析

在西安理工大學金花校區遠離建筑物的操場中心位置處進行實驗，驗證基于改進LM算法的地磁矢量校準方法的有效性。實驗過程中使用的三軸磁傳感器為MS－03A型磁通門磁傳感器；使用GSM－19 Overhauser質子磁傳感器提供地磁場真值。將三軸磁傳感器以及加速度計模塊(模塊外殼使用無磁的鋁合金材料)放置在無磁轉臺上對地磁場和重力場進行測量，實驗系統示意圖如圖7所示，各個實驗設備及實驗操作如圖8所示。

圖7 實驗系統示意圖

圖8 實驗設備與實驗操作示意圖

首先在操場中心位置使用質子磁傳感器測量地磁場的真實值并取平均，得到該位置處的地磁場真實值大小為52 260 nT，然后在相同位置放置支架、無磁轉臺并固定磁通門磁傳感器，分別將磁傳感器的X軸(＋/－)、Y(＋/－)軸向上和向下，每隔15°旋轉一次轉臺獲取實驗數據，每個點測量兩次并取其平均值。

圖9為實驗數據驗證改進LM算法校準三軸磁傳感器誤差前后測量地磁總場的對比圖。經過分析，誤差校準前測量地磁總場的誤差較大，校準前地磁場總量均方根誤差為693.1 nT，校準后均方根誤差降為184.9 nT。

圖9 校準前后地磁場總量

非對準誤差造成的地磁場分量誤差可以通過航向角誤差來反映。圖10為校準非對準誤差前后航向角絕對誤差，經過分析，非對準誤差校準前航向角均方根誤差為17.2°，而校準非對準誤差后航向角均方根誤差降至2.2°，這證明該方法得到了加速度計坐標系下高精度地磁場矢量。

圖10 非對準誤差校準前后航向角絕對誤差

6 總結

本文提出了一種基于改進LM算法的地磁矢量校準方法，首先使用改進LM算法直接求解三軸磁傳感器誤差參數，避免了橢球擬合方法因矩陣分解引入任意旋轉矩陣的問題；然后校準了三軸磁傳感器和加速度計坐標系之間的非對準誤差。仿真結果表明，基于改進LM算法的矢量校準方法可使地磁總場的均方根誤差減小兩個數量級，分量精度提高一個數量級；實驗結果表明本文所提方法能夠大大提高三軸磁傳感器測量精度，最終獲得了加速度計坐標系下的高精度地磁場矢量。