基于模值估計(jì)的三軸磁力計(jì)標(biāo)定方法研究*

鐘 浩,章衛(wèi)國(guó),劉小雄

(西北工業(yè)大學(xué)自動(dòng)化學(xué)院,西安 710129)

項(xiàng)目來(lái)源:國(guó)家自然科學(xué)基金項(xiàng)目(61374032);航空科學(xué)基金項(xiàng)目(20150753009)

2017-04-13修改日期2017-06-07

基于模值估計(jì)的三軸磁力計(jì)標(biāo)定方法研究*

鐘 浩,章衛(wèi)國(guó)*,劉小雄

(西北工業(yè)大學(xué)自動(dòng)化學(xué)院,西安 710129)

現(xiàn)有的三軸磁力計(jì)標(biāo)定方法大都是對(duì)采樣數(shù)據(jù)進(jìn)行歸一化,然后利用樣本數(shù)據(jù)求解參數(shù)。為了提高標(biāo)定算法的通用性和有效性。提出了一種改進(jìn)的標(biāo)定方法,首先將磁力計(jì)所處位置磁場(chǎng)向量的模作為誤差模型參數(shù),然后對(duì)其進(jìn)行估計(jì)算法設(shè)計(jì),接著提出一種兩步標(biāo)定方法進(jìn)行參數(shù)初值的選取,然后應(yīng)用Levenberg-Marquardt方法進(jìn)行磁力計(jì)標(biāo)定算法設(shè)計(jì)。仿真和實(shí)驗(yàn)結(jié)果表明,本文所提算法對(duì)類似的三軸傳感器具有通用性,不依賴于傳感器的觀測(cè)向量,同時(shí)能夠?qū)λ^測(cè)向量的模值進(jìn)行精確的估計(jì),可以標(biāo)定三軸磁力計(jì)的安裝誤差、靜態(tài)靈敏度誤差和零位誤差,標(biāo)定過(guò)程簡(jiǎn)便,適用性廣。

三軸磁力計(jì);誤差分析;兩步標(biāo)定方法;Levenberg-Marquardt方法

磁力計(jì)是通過(guò)測(cè)量地磁場(chǎng)強(qiáng)度來(lái)指示方向的設(shè)備,其輸出具有連續(xù)性、誤差不積累等特性[1]。在低成本高性能的導(dǎo)航系統(tǒng)中,磁力計(jì)是用于姿態(tài)估計(jì)的關(guān)鍵性輔助傳感器[2],因此廣泛應(yīng)用于低成本飛行器的組合導(dǎo)航系統(tǒng)中。這種低成本、低功耗傳感器通過(guò)比較機(jī)體系和導(dǎo)航系的磁場(chǎng)強(qiáng)度來(lái)給出相對(duì)精確的航向[3]。

由于制造、安裝以及環(huán)境等因素的影響,實(shí)際使用中,三軸磁力計(jì)都會(huì)存在著未對(duì)準(zhǔn)、靈敏度不一致和零偏等誤差,導(dǎo)致測(cè)量值誤差過(guò)大[4-6],因此必須對(duì)三軸磁力計(jì)進(jìn)行標(biāo)定之后,才能將三軸磁力計(jì)用于組合導(dǎo)航系統(tǒng)中。本文通過(guò)建立三軸磁力計(jì)的數(shù)學(xué)模型,使用兩步標(biāo)定方法求解模型參數(shù)。通過(guò)仿真和實(shí)驗(yàn)結(jié)果表明所改進(jìn)的算法的有效性。

1 誤差分析及模型建立

根據(jù)傳感器誤差產(chǎn)生機(jī)理的不同,可將三軸磁力計(jì)的誤差分為安裝誤差、靈敏度不匹配誤差和零位誤差。

1.1 安裝誤差

安裝誤差也叫未對(duì)準(zhǔn)誤差,是由于在運(yùn)載體上以捷聯(lián)方式安裝三軸磁力計(jì)時(shí),磁力計(jì)的3個(gè)測(cè)量軸分別與載體的3個(gè)軸不平行而引起的測(cè)量誤差[7]。理想的三軸磁力計(jì)相對(duì)于載體坐標(biāo)系的安裝誤差表示為三軸磁力計(jì)依次繞Z軸、Y軸和X軸轉(zhuǎn)動(dòng)3個(gè)微小的安裝角。根據(jù)兩坐標(biāo)系之間的轉(zhuǎn)換矩陣,和安裝誤差的小角度特性,可以得到安裝誤差的數(shù)學(xué)模型:

(1)

1.2 三軸間靜態(tài)靈敏度不匹配誤差

實(shí)際應(yīng)用中一般使用三軸磁力計(jì),用來(lái)測(cè)量磁場(chǎng)在3個(gè)軸上的投影分量。磁力計(jì)的三軸的測(cè)量特性總會(huì)存在一些差異,因此造成三軸間靜態(tài)靈敏度不匹配誤差。一般三軸的靜態(tài)靈敏度均趨近于1,因此三軸間靜態(tài)靈敏度不匹配誤差的數(shù)學(xué)模型為:

(2)

1.3 三軸磁力計(jì)的零位誤差

零位誤差又稱為零偏誤差[7],是由于傳感器、模擬電路和A/D轉(zhuǎn)換的零點(diǎn)不為零以及數(shù)據(jù)處理過(guò)程中數(shù)據(jù)偏移所引起的誤差。此外硬磁干擾誤差是一種常值干擾誤差,其值一般不隨時(shí)間而發(fā)生變化,因此一般也可等效為三軸磁力計(jì)的零位誤差[8-9]。三軸磁力計(jì)的零位誤差的數(shù)學(xué)模型為:

(3)

1.4 三軸磁力計(jì)標(biāo)定的完整數(shù)學(xué)模型

為了在工程上應(yīng)用的方便性,以及降低對(duì)校準(zhǔn)條件的要求,本文忽略了由于三軸磁力計(jì)軸間非正交而產(chǎn)生的誤差以及由于軟磁干擾而產(chǎn)生的誤差以及其他干擾很小可以忽略的誤差。由式(1)～式(3)可得三軸磁力計(jì)標(biāo)定的完整數(shù)學(xué)模型為:

(4)

式中:H也就是標(biāo)定后的載體坐標(biāo)系上的磁場(chǎng)向量的投影。

2 傳感器標(biāo)定算法設(shè)計(jì)

2.1 模型分析

在三軸磁力計(jì)軸間靜態(tài)靈敏度不匹配誤差模型中,三軸靈敏度都趨近于1。為了可以得到磁場(chǎng)向量的模大小,本文以三軸中的Z軸的靜態(tài)靈敏度作為基準(zhǔn),X和Y軸的靈敏度都是相對(duì)于Z軸的比值。

這樣會(huì)帶來(lái)一些新的問題:也就是真實(shí)的磁場(chǎng)向量的模與本文所得到的磁場(chǎng)向量的模之間會(huì)有一個(gè)Z軸靜態(tài)靈敏度的系數(shù)倍數(shù)偏差,三軸真實(shí)的靜態(tài)靈敏度系數(shù)也會(huì)與本文所得到得靜態(tài)靈敏度系數(shù)有一個(gè)Z軸靜態(tài)靈敏度系數(shù)的偏差。不過(guò)由于Z軸靜態(tài)靈敏度系數(shù)本身趨近于1,而且在使用三軸磁力計(jì)測(cè)量值計(jì)算方向角時(shí)是兩軸之間測(cè)量值的比值,因此并不會(huì)使計(jì)算出的方向角產(chǎn)生誤差。

如果不以Z軸靜態(tài)靈敏度系數(shù)作為基準(zhǔn),會(huì)造成計(jì)算出的磁場(chǎng)向量長(zhǎng)度,以及三軸靈敏度之間存在一個(gè)倍數(shù)關(guān)系,在使用Levenberg-Marquardt方法求解模型參數(shù)最優(yōu)解時(shí)會(huì)出現(xiàn)靈敏度系數(shù)和向量模與真實(shí)值之間偏差一個(gè)倍數(shù)關(guān)系,而這個(gè)倍數(shù)關(guān)系可大可小,會(huì)帶來(lái)估計(jì)上的不準(zhǔn)確性。

2.2 算法設(shè)計(jì)

根據(jù)上述分析和標(biāo)定模型,進(jìn)一步分析如下,對(duì)式(4)有:

(5)

進(jìn)一步有:

(6)

為了簡(jiǎn)化算法分析過(guò)程,本文將式進(jìn)一步寫為:

(7)

即

當(dāng)三軸磁力計(jì)在一固定地理位置作各種姿態(tài)變化時(shí),可以將磁場(chǎng)向量視為常向量,其磁場(chǎng)向量的模為一常數(shù)。因此對(duì)于式(7)有:

(8)

式中:

通過(guò)采集多組數(shù)據(jù),使用最小二乘法擬合的方法,本文中的優(yōu)化目標(biāo)函數(shù)為:

(9)

由于ri(β)是非線性的,故式(9)所表示的目標(biāo)函數(shù)的極小點(diǎn)通常稱為非線性最小二乘問題。由于Levenberg-Marquardt型方法是求解這類問題的最有效方法之一,特別是對(duì)于殘差為零或接近于零的問題[10]。因此本文采用此方法對(duì)此非線性最小二乘問題進(jìn)行求解。

2.3 算法第1步:初值求解

由于本文中的模型一共有9個(gè)參數(shù)需要確定,而且各個(gè)參數(shù)值得范圍以及大小都是不同的。而且由于Levenberg-Marquardt型方法的收斂性問題,在初值的選擇上不能偏離真實(shí)值太多,否則會(huì)造成難以收斂、收斂時(shí)間較長(zhǎng)或者收斂到其他局部最優(yōu)解[11]。其中可以確定的是β1,β2,β3為趨近于0的數(shù)值,β4,β5趨近于1的數(shù)值。但是由于不同的傳感器測(cè)量的物理量的不同,造成物理量的模β9的大小難以確定具體的范圍。而且即使在同一類型的傳感器下,由于設(shè)置的增測(cè)量范圍不同,傳感器AD采樣的輸出數(shù)值范圍也是不一樣的。因此在使用Levenberg-Marquardt型方法時(shí)難以確定具體的參數(shù)β9的大小。對(duì)于參數(shù)β6,β7,β8也具有同樣的結(jié)果。因此在使用Levenberg-Marquardt型方法對(duì)目標(biāo)函數(shù)進(jìn)行求解時(shí)需要先確定的參數(shù)β6,β7,β8,β9的適當(dāng)范圍。

本文中先忽略其他誤差因素,認(rèn)為β1,β2,β3,β4,β5均為理想值。因此式(4)可以寫為:

(10)

進(jìn)一步有:

(11)

也就是:

對(duì)于樣本數(shù)據(jù)有:

(12)

對(duì)于n個(gè)樣本數(shù)據(jù)有:

(13)

通過(guò)解算式(13)的結(jié)果,可以得到參數(shù)β6、β7、β8、β9的值,本文將該值作為第2步求解的初始值對(duì)目標(biāo)函數(shù)進(jìn)行求解。

2.4 算法第2步:非線性最小二乘求解

針對(duì)優(yōu)化目標(biāo)函數(shù)式(9),使用Levenberg-Marquardt方法進(jìn)行求解[10]。該方法的主要計(jì)算公式為:

(JTJ+μI)β=-g

(14)

式中:J為r(β)函數(shù)的雅克比矩陣,g=?f/?β為優(yōu)化目標(biāo)函數(shù)的梯度,μ≥0為阻尼因子,I為單位陣,β為參數(shù)變量的變化值。下次迭代計(jì)算是βk+1=βk+β。

3 實(shí)驗(yàn)結(jié)果與分析

3.1 數(shù)值仿真部分

取磁場(chǎng)向量模為10,安裝誤差角度為5°,三軸靜態(tài)靈敏度系數(shù)分別為0.98,1.02,0.96,三軸零位誤差分別為-3,-3,-3,噪聲均值為0,方差為0.01。產(chǎn)生原始的樣本數(shù)據(jù),利用本文提出的算法進(jìn)去求解。

解算模型參數(shù)如表1所示。從解算的參數(shù)可以看出,在其他參數(shù)基本一致的情況下,第2步解算明顯提高了第1步解算的向量的模值精度。

表1 仿真參數(shù)對(duì)比

標(biāo)準(zhǔn)差能夠從向量模的大小來(lái)反映參數(shù)的優(yōu)劣程度。當(dāng)標(biāo)準(zhǔn)差越小,說(shuō)明標(biāo)定后的向量模越趨近于一個(gè)常值,這與磁場(chǎng)向量在一固定地理位置具有固定的模是吻合的,也就是說(shuō)標(biāo)準(zhǔn)差越小,參數(shù)和真實(shí)的參數(shù)越接近。表2中的數(shù)據(jù)也表明了標(biāo)定的有效性。

表2 仿真向量模標(biāo)準(zhǔn)差對(duì)比

圖2為標(biāo)定前后數(shù)據(jù)圖形對(duì)比。在同樣的仿真數(shù)據(jù)環(huán)境下,當(dāng)不使用文中的最小二乘法來(lái)計(jì)算參數(shù)的初值時(shí),選取參數(shù)的初值為[0.001,0.001,0.001,1,1,1,1,1,1]T,可以得到仿真結(jié)果為:[7.519,1.049,0.7147,0.163,0.143,23.448,57.858,36.92,50.567]T。圖1為未使用最小二乘法計(jì)算初值的仿真圖形,可以明顯看出解算結(jié)果偏離理想結(jié)果。

圖1 仿真數(shù)據(jù)標(biāo)定前后對(duì)比圖

圖2 仿真數(shù)據(jù)標(biāo)定前后對(duì)比圖

3.2 實(shí)驗(yàn)數(shù)據(jù)分析

對(duì)于三軸的傳感器一般都具有相似的誤差源,因此本文所建立的模型具有一定的通用性[12]。首先利用三軸加速計(jì)的輸出,測(cè)試模型的效果。

3.2.1 三軸加速度計(jì)

通常認(rèn)為重力加速度的值是固定的,一般為9.8 m/s2。因此將模型解算的參考向量和該值進(jìn)行對(duì)比可以說(shuō)明對(duì)向量模估計(jì)值得精確度。

采樣數(shù)據(jù)來(lái)源為MPU6050。MPU6050模塊是InvenSense公司推出的一款低成本的6軸傳感器模塊,包括三軸加速度,其體積小巧,用途非常廣。采集多種姿態(tài)下,加速度計(jì)的輸出,共145組樣本數(shù)據(jù)。表3為經(jīng)過(guò)算法標(biāo)定后的標(biāo)定參數(shù)。表4為數(shù)據(jù)向量的標(biāo)準(zhǔn)差比較。

表3 三軸加速度計(jì)真實(shí)參數(shù)解算

表4 重力加速度標(biāo)準(zhǔn)差比較

從表中可以看出第1步只能給出偏差較大的參數(shù)初值,第2步會(huì)在該初值的基礎(chǔ)上得到進(jìn)一步優(yōu)化的結(jié)果。

通過(guò)10.043/9.8=1.024 8可知,Z軸的靜態(tài)靈敏度系數(shù)最大不過(guò)1.024 8,與估計(jì)的X軸靜態(tài)靈敏度系數(shù)1.017和Z軸靜態(tài)靈敏度系數(shù)1.018比較接近。

圖3為標(biāo)定前后數(shù)據(jù)圖形對(duì)比。

圖3 三軸加速度計(jì)標(biāo)定前后對(duì)比圖

3.3 三軸磁力計(jì)

本文從HMC5883L三軸磁力計(jì)采集數(shù)據(jù)。HMC5883L是霍尼韋爾公司生產(chǎn)的帶有數(shù)字接口的弱磁傳感器芯片。在空間中隨意轉(zhuǎn)動(dòng)三軸磁力計(jì),采集三軸磁力計(jì)的輸出。表5為經(jīng)過(guò)算法標(biāo)定之后的參數(shù),其中向量模為傳感器輸出的ADC值。將表3和表5對(duì)比可知,由于重力向量和磁場(chǎng)向量的不同,參數(shù)的范圍差別比較大。在選取參數(shù)的初值時(shí),無(wú)法確定參數(shù)的具體范圍,因?yàn)椴煌挠^測(cè)向量模差別比較大。從而表明了兩步法的有效性。

表5 三軸磁力計(jì)真實(shí)參數(shù)解算

表6為標(biāo)定前后數(shù)據(jù)向量模標(biāo)準(zhǔn)差大小。

表6 磁場(chǎng)向量模標(biāo)準(zhǔn)差比較

圖4為標(biāo)定圖形。

圖4 三軸磁力計(jì)標(biāo)定前后數(shù)據(jù)對(duì)比

將表3和表5對(duì)比可知,由于重力向量和磁場(chǎng)向量的不同,參數(shù)的范圍差別比較大。在選取參數(shù)的初值時(shí),無(wú)法確定參數(shù)的具體范圍,因?yàn)椴煌挠^測(cè)向量模差別比較大。從而表明了兩步法的有效性。

4 總結(jié)

考慮現(xiàn)有的三軸磁力計(jì)的標(biāo)定方法大部分是對(duì)三軸磁力計(jì)的采樣數(shù)據(jù)進(jìn)行歸一化處理,然后利用歸一化的樣本數(shù)據(jù)對(duì)三軸磁力計(jì)模型的參數(shù)進(jìn)行求解,沒有將所處位置的磁場(chǎng)向量模作為模型參數(shù),因而在模型中忽略了磁場(chǎng)向量的模。本文提出了一種改進(jìn)的三軸傳感器的標(biāo)定方法。構(gòu)建標(biāo)定模型的非線性最小二乘算法模型,采用兩步解算方法進(jìn)行參數(shù)的結(jié)算,先確定出參數(shù)的大致范圍,然后使用Levenberg-Marquardt方法對(duì)目標(biāo)函數(shù)進(jìn)行求解。通過(guò)仿真和實(shí)驗(yàn)結(jié)果表明該方法能夠提高算法的收斂性和通用性,并且具有較廣泛的工程應(yīng)用價(jià)值。

[1] Fang J,Sun H,Cao J,et al. A Novel Calibration Method of Magnetic Compass Based on Ellipsoid Fitting[J]. IEEE Transactions on Instrumentation and Measurement,2011,60(6):2053-2061.

[2] Vasconcelos J. A Geometric Approach to Strapdown Magnetometer Calibration in Sensor Frame[C]//Navigation,Guidance and Control of Underwater Vehicles,2008:172-177.

[3] Markley F L. Attitude Determination and Parameter Estimation Using Vector Observations:Theoty[J]. The Journal of the Astronautical Sciences,1989,37(1):41-58.

[4] 張煒,程錦房. 一種三軸磁傳感器正交誤差校正的簡(jiǎn)便方法[J]. 兵工自動(dòng)化,2009(12):75-78.

[5] 范成葉,李杰,陳文蓉,等. 電子羅盤安裝誤差標(biāo)定與補(bǔ)償方法研究[J]. 傳感技術(shù)學(xué)報(bào),2013,23(5):622-626.

[6] 邱秀分,石文峰,孫春艷,等. 一種高精度的小型三維電子羅盤簡(jiǎn)易標(biāo)定方法[J]. 艦船電子工程,2017(2):82-85,125.

[7] 張曉明. 地磁導(dǎo)航理論與實(shí)踐[M]. 北京:國(guó)防工業(yè)出版社,2016:75,79,112.

[8] Gebreegziabher D,Elkaim G H,Powell J D,et al. Calibration of Strapdown Magnetometers in Magnetic Field Domain[J]. Journal of Aerospace Engineering,2006,19(2):87-102.

[9] Valérie R,Afzal M H,Gérard L. Complete Triaxis Magnetometer Calibration in the Magnetic Domain[J]. Journal of Sensors,2010,2010(1):23-59.

[10] 梅芳,張建中. Levenberg-Marquardt型非線性最小二乘算法的收斂性[J]. 計(jì)算數(shù)學(xué),1982(2):182-192.

[11] 張春曉. 解決非線性最小二乘問題的兩種方法[J]. 青海大學(xué)學(xué)報(bào)(自然科學(xué)版),2003(5):60-64.

[12] Foster C C,Elkaim G H. Extension of a Two-Step Calibration Methodology to Include Nonorthogonal Sensor Axes[J]. IEEE Transactions on Aerospace and Electronic Systems,2008,44(3):1070-1078.

ResearchonCalibrationMethodsofThree-AxisMagnetometerBasedonVectorNormEstimation*

ZHONGHao,ZHANGWeiguo*,LIUXiaoxiong

(College of Automation,Northwestern Polytechnical University,Xi’an 710129,China)

In view of calibration methods of three-axis magnetometer,most calibration methodsuse the normalized magnetic field vector to calibrate three-axis magnetometer. In order to improve the generality and effectiveness of calibration methods,a two-step calibration method is proposed. In this work,the error model of three-axis magnetometer uses the magnetic field vector norm as a parameter. First step is used to get the initial parameters,then second step is used to solve nonlinear least squares problem with the initial parameters. Simulation results show that two-step calibration method can improve the convergence of Levenberg-Marquardt method. This method does not depend on the observed vector,and is also applicable to other three-axis sensors with the same error model. And experiment results show that this method can correctly and effectively compensate the misalignment errors,null shift errors and sensitivity errors of the three-axis magnetometer. And the magnetic field vector norm is relatively precise.

three-axis magnetometer;error analysis;two-step calibration method;Levenberg-Marquardt method

V241.61

A

1004-1699(2017)10-0512-06

10.3969/j.issn.1004-1699.2017.10.010

鐘浩(1991-),男,西北工業(yè)大學(xué)自動(dòng)化學(xué)院碩士研究生,研究方向?yàn)樾頇C(jī)組合導(dǎo)航算法,zhonghao@mail.nwpu.edu.cn;

章衛(wèi)國(guó)(1956-),男,教授,西北工業(yè)大學(xué)博士生導(dǎo)師,研究方向?yàn)轱w行控制與仿真、現(xiàn)代飛行控制方法;

劉小雄(1973-),男,博士,西北工業(yè)大學(xué)副教授,研究方向?yàn)轱w行控制與仿真、非線性飛行控制方法、飛行控制系統(tǒng)設(shè)計(jì)。