劉 宇，吳林志，路永樂(lè)，陳俊杰

(重慶郵電大學(xué) 光電信息感測(cè)與傳輸技術(shù)重慶市重點(diǎn)實(shí)驗(yàn)室，重慶 460005)

基于極大似然估計(jì)法的磁力計(jì)誤差補(bǔ)償算法

劉 宇，吳林志，路永樂(lè)，陳俊杰

(重慶郵電大學(xué) 光電信息感測(cè)與傳輸技術(shù)重慶市重點(diǎn)實(shí)驗(yàn)室，重慶 460005)

針對(duì)現(xiàn)有三軸磁力計(jì)誤差補(bǔ)償速度慢、需要外部輔助設(shè)備、磁力計(jì)和慣性傳感器組合存在多傳感器軸位敏感重合誤差問(wèn)題，提出了一種基于極大似然估計(jì)法(maximum likelihood estimation，MLE)的快速有效的磁力計(jì)誤差補(bǔ)償算法。根據(jù)傳感器組合系統(tǒng)中誤差來(lái)源建立測(cè)量誤差模型，建立高斯分布的極大似然參數(shù)估計(jì)模型，用牛頓最優(yōu)法解算出誤差補(bǔ)償參數(shù)，并給出求解理想初始值的算法。仿真數(shù)據(jù)顯示,補(bǔ)償后的磁力計(jì)航向角解算精度達(dá)到0.81°，相比補(bǔ)償前精度提高94.9%。實(shí)驗(yàn)結(jié)果表明，該算法可簡(jiǎn)單快速的實(shí)現(xiàn)誤差補(bǔ)償，多傳感器軸位敏感重合誤差得到校準(zhǔn)。

磁力計(jì)；軸位敏感重合誤差；極大似然估計(jì)法；誤差補(bǔ)償

0 引 言

磁力計(jì)與慣性傳感器組合能夠解算出運(yùn)動(dòng)物體的姿態(tài)信息，由于其低價(jià)格、高性能優(yōu)點(diǎn)在姿態(tài)探測(cè)方面占據(jù)著重要地位[1]。然而磁力計(jì)零偏、非正交、靈敏度等誤差和磁場(chǎng)干擾[2]，以及當(dāng)磁力計(jì)與慣性傳感器組合時(shí)多軸位敏感誤差，導(dǎo)致姿態(tài)解算的精度降低。因此，需要對(duì)磁力計(jì)以上誤差因素進(jìn)行補(bǔ)償，提高姿態(tài)解算精度，增強(qiáng)磁力計(jì)的實(shí)用性。

目前針對(duì)磁力計(jì)誤差補(bǔ)償?shù)姆椒ㄓ型獠啃畔⑷诤?、給定基準(zhǔn)法和橢圓擬合等方法。外部信息融合使用GPS、慣性測(cè)量單元采集數(shù)據(jù)對(duì)磁力計(jì)進(jìn)行濾波校準(zhǔn)[3]，但是經(jīng)該算法校準(zhǔn)后的磁力計(jì)精度仍然較低；給定基準(zhǔn)法利用外部輔助設(shè)備測(cè)定磁力計(jì)在各個(gè)方向的姿態(tài)信息，標(biāo)定后在磁場(chǎng)環(huán)境變化不大時(shí)，具有良好的補(bǔ)償精度[4]，同時(shí)該方法對(duì)外部設(shè)備的要求較高，不適合終端用戶用來(lái)自適應(yīng)校準(zhǔn)；橢圓擬合算法不需要輔助設(shè)備，僅在水平方向旋轉(zhuǎn)一周便可擬合出橢圓函數(shù)，效果理想[5]，但僅能應(yīng)用于軟磁干擾較小的場(chǎng)合，軟磁干擾較大時(shí)，誤差也較大；多平面橢圓擬合方法需要在至少2個(gè)相互垂直的平面內(nèi)獨(dú)立旋轉(zhuǎn)一周，然后分別采用橢圓擬合法計(jì)算三軸的補(bǔ)償參數(shù)，補(bǔ)償后效果很好，但對(duì)補(bǔ)償過(guò)程要求較高。以上這些算法補(bǔ)償效果明顯但都是針對(duì)單獨(dú)磁力計(jì)而沒(méi)有考慮到磁力計(jì)與慣性傳感器組合時(shí)多軸位敏感的問(wèn)題。

針對(duì)以上問(wèn)題，本文提出一種簡(jiǎn)單有效校準(zhǔn)磁力計(jì)和慣性傳感器組合算法。該算法一方面考慮了傳感器軸位敏感重合誤差，建立一種完整的磁力計(jì)誤差校準(zhǔn)模型；另一方面利用極大似然估計(jì)思想求解誤差補(bǔ)償系數(shù)，實(shí)現(xiàn)對(duì)磁力計(jì)存在誤差的補(bǔ)償。實(shí)驗(yàn)驗(yàn)證了提出算法的優(yōu)越性，證明該算法是一種簡(jiǎn)單有效的誤差校準(zhǔn)算法。

1 誤差模型

地磁場(chǎng)向量mn由垂直方向和水平指向磁北方向的2個(gè)分量組成

(1)

(1)式中：δ是向量mn與水平面之間的傾角；d是mn的垂直方向分量值。

磁力計(jì)受自身誤差、外部磁場(chǎng)干擾因素影響[6]，其測(cè)量模型定義為

(2)

磁力計(jì)和慣性傳感器軸位之間不重合使垂直分量測(cè)量d′不等于地磁場(chǎng)垂直分量d，如圖1所示，定義誤差矩陣為Rim。

圖1 傳感器軸位未重合示意圖Fig.1 Schematic of the sensors axes are misaligned

引入磁力計(jì)與慣性傳感器軸位不重合誤差因素后，(2)式為

(4)

D和o滿足關(guān)系式

D=csccnocsiRim

o=csccnoohi+ozb

(5)

三軸磁力計(jì)的誤差模型可以由D和o表示。

2 三軸磁力計(jì)誤差補(bǔ)償算法

em,k～N(0,φm)

ez,k～N(0,φz)

根據(jù)高斯分布的MLE，(4)式和(5)式的極大似然估計(jì)的問(wèn)題可以表示為

(6)

(6)式中：

變量θ為

對(duì)磁力計(jì)測(cè)量值標(biāo)準(zhǔn)化處理，即‖mn‖2=1，可以得到

(7)

(7)式中：噪聲em,k被忽略掉，其中

AD-TD-1

(8)

bT-2OTD-TD-1

(9)

cOTD-TD-1O-1

(10)

標(biāo)準(zhǔn)橢球擬合可求解出A，b和c。

將等式(7)表示為J與η的線性關(guān)系

Jη≈0

(11)

(12)

根據(jù)等式(10)可以確定刻度因子α

(13)

可以得到

(14)

(15)

(16)

(17)

(17)式中：R表示慣性傳感器和磁力計(jì)坐標(biāo)系轉(zhuǎn)換矩陣。

對(duì)zn和mn做內(nèi)積得到

(18)

根據(jù)極大似然估計(jì)法對(duì)(18)式求解

R∈SO(3)

(19)

(19)式中：變量θ∈{R,d}。

(19)式中變量的初始值可選擇θ0={E3,-sinδ}，δ是地磁場(chǎng)的傾角大小。

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

為了驗(yàn)證上述算法的有效性，采用以HMC5883磁傳感器為核心的航姿系統(tǒng)為實(shí)驗(yàn)設(shè)備，磁力計(jì)和慣性傳感器被封裝在鋁制外殼內(nèi)，周圍沒(méi)有變化的磁場(chǎng)，此時(shí)磁力計(jì)存在的誤差因素為自身誤差、軸位敏感誤差、硬磁干擾和軟磁干擾。

3.1 實(shí)驗(yàn)步驟

圖2 校準(zhǔn)之前的橢球擬合Fig.2 Ellipsoid fitting before calibration

圖3 校準(zhǔn)之后的橢球擬合Fig.3 Ellipsoid fitting after calibration

3.2 誤差分析

將實(shí)驗(yàn)設(shè)備安裝在無(wú)磁雙軸轉(zhuǎn)臺(tái)上(磁力計(jì)的誤差因素不變)，保持轉(zhuǎn)臺(tái)俯仰角固定，航向角以3°/s轉(zhuǎn)動(dòng)一周，記錄磁磁力計(jì)輸出數(shù)據(jù)，并與轉(zhuǎn)臺(tái)數(shù)據(jù)比較，得出航向誤差值。

圖4 磁場(chǎng)垂直分量測(cè)量值Fig.4 Vertical component of magnetic field

磁力計(jì)補(bǔ)償前后的航向角與轉(zhuǎn)臺(tái)的參考航向進(jìn)行比較，如圖5所示。

圖5 校準(zhǔn)前后的磁力計(jì)航向角Fig.5 Heading of the magnetometer before and after calibration

由圖5可知，未經(jīng)算法補(bǔ)償?shù)拇帕τ?jì)航向角的最大誤差為15.94°，經(jīng)過(guò)算法補(bǔ)償后的磁力計(jì)航向角最大誤差為0.81°，精度提高了94.9%。

根據(jù)補(bǔ)償方法不同，對(duì)4種算法補(bǔ)償后的誤差結(jié)果進(jìn)行比較，如圖6所示。圖6中，橫軸表示轉(zhuǎn)臺(tái)轉(zhuǎn)動(dòng)角度，縱軸表示航向角誤差v；曲線v1表示未補(bǔ)償前的測(cè)量誤差；v2表示采用外部信息融合算法補(bǔ)償以后得到的測(cè)量誤；v3表示采用橢圓擬合法補(bǔ)償后的測(cè)量誤差；v4表示采用給定基準(zhǔn)法補(bǔ)償以后得到的測(cè)量誤差；v5表示采用極大似然估計(jì)法補(bǔ)償后的測(cè)量誤差。

圖6 磁航向角誤差對(duì)比Fig.6 Magnetic heading errors comparison

不同算法測(cè)得的標(biāo)準(zhǔn)差和最大值如表1所示。由表1可知，采用橢圓擬合算法補(bǔ)償后的精度達(dá)到0.85°，然而該算法需要在至少2個(gè)相互垂直的平面內(nèi)旋轉(zhuǎn)一周并且分別采用橢球擬合算法解算三軸補(bǔ)償參數(shù)，補(bǔ)償過(guò)程較為復(fù)雜；給定基準(zhǔn)法補(bǔ)償后的精度達(dá)到0.79°，精度較高，然而該算法對(duì)外部設(shè)備的要求較高，不適合用戶自適應(yīng)校準(zhǔn)；極大似然估計(jì)法補(bǔ)償后的效果可以達(dá)到給定基準(zhǔn)法的精度，并且不需要外部輔助設(shè)備僅僅手持旋轉(zhuǎn)即可實(shí)現(xiàn)補(bǔ)償。

表1 不同算法測(cè)得的標(biāo)準(zhǔn)差和最大值Tab.1 Standard deviation and maximum one from the measurement errors of different algorithms

4 結(jié) 論

根據(jù)地磁場(chǎng)向量在局部區(qū)域內(nèi)不變的特性，提出針對(duì)磁力計(jì)誤差補(bǔ)償?shù)乃惴?，不僅考慮了單獨(dú)的磁力計(jì)誤差因素，而且還針對(duì)磁力計(jì)與慣性傳感器組合時(shí)軸位敏感重合誤差建立了校準(zhǔn)模型。本文算法采用極大似然估計(jì)的非線性最優(yōu)化進(jìn)行推導(dǎo)運(yùn)算，求解出最優(yōu)化需要的初始估計(jì)值，最后采用牛頓最優(yōu)化法求解出補(bǔ)償參數(shù)。經(jīng)過(guò)實(shí)驗(yàn)和仿真證實(shí)該算法對(duì)存在的誤差干擾和多傳感器軸位敏感誤差進(jìn)行了有效地補(bǔ)償，補(bǔ)償后的航向精度為0.81°。經(jīng)該算法補(bǔ)償后的效果可以達(dá)到傳統(tǒng)的給定基準(zhǔn)補(bǔ)償法的精度，而補(bǔ)償方式更加靈活，適合無(wú)外部設(shè)備的野外環(huán)境中使用，具有實(shí)用意義。

[1] HU C, MENG Q H, MANDAL M. A Linear Algorithm for Tracing Magnet Position and Orientation by Using Three-Axis Magnetic Sensors[J]. IEEE Transactions on Magnetics, 2007, 43(12):4096-4101.

[2] LI Zhi, LI Xiang, WANG Yongjun. A calibration method for magnetic sensors and accelerometer in tilt-compensated digital com-pass[C]//Proceeding of 9th International Conference on Electronic Measurement and Instruments. Beijing: IEEE, 2009: 868-871.

[3] MARKOVIC R, KRAJNC A, MATKO D. Calibration of asolid-state magnetic compass using angular-rate information from low-cost sensors[J]. IET Science, Measurement and Technology, 2011, 5(2): 54-58.

[4] 劉仁浩, 王 華. 數(shù)字磁羅盤的全姿態(tài)羅差補(bǔ)償[J]. 光學(xué)精密工程, 2011, 19(8): 1867-1873.

LIU Renhaqo,WANG Hua.All attitude magnetic deviation compensation for digital magnetic compass[J].Optics and Precision Engineering,2011,19(8):1867-1873.

[5] 朱建良,王興全,吳盤龍,等.基于橢球曲面擬合的三維磁羅盤誤差補(bǔ)償算法[J].中國(guó)慣性技術(shù)學(xué)報(bào),2012,20(5): 562-566.

ZHU Jianliang,WANG Xingquan,WU Panlong,et al.Three-dimensional magnetic compass error compensation algorithm based on ellipsoid surface fitting[J].Journal of Chinese Inertial Technology,2012,20(5):562-566.

[6] RENAUDIN V, MUHAMMAD H A, LACHAPELLE G. Complete Tri-axis Magnetometer Calibration in the Magnetic Field Domain[J]. Journal of Sensors, 2010, 10(1):23-59.

[7] HOL J. Sensor Fusion and Calibration of Inertial Sensors, Vision, Ultra-Wideband and GPS[D]. Linkoping: Linkping University the Institute of Technology, 2011.

[8] WAHDAN A,GEORGY J,NOURELDIN A.Three-Dimensional Magnetometer Calibration With Small Space Coverage for Pedestrians[J].IEEE Sensors Journal,2015,15(1):598-609.

s：The Supported by the National Natural Science Foundation of China(51175535);The Technology Platform and Base Construction of International Joint Research Center for MEMS Vibration Sensing and Micro Attitude Combined Logging Technology(cstc2014gjhz0038)

Compensationalgorithmofmagnetometerbasedonmaximumlikelihoodestimator

LIU Yu, WU Linzhi, LU Yongle, CHEN Junjie

Chongqing Municipal Level Key laboratory of Photo-electronic Information Sensing and Transmitting Technology, Chongqing University of Post and Telecommunications, Chongqing 460005, P.R.China)

To solve the problems in existing Tri-axial magnetometer (such as, error compensation is slow, External auxiliary equipment is needed and the axial-sensitive coincidence error when combined magnetometer and inertial sensors), This paper put forward a fast-effective error compensation algorithm of magnetometer based on MLE. First, establishing the error model through analyzing the errors source of the system and then, setting up the parameter estimation model based on the Gaussian distribution; finally, we use the Newton optimization method to solve the compensation parameters as well as giving the algorithm to solve the initial values. Simulation shows that the heading measurement precision after compensation can reach 0.81°, and the accuracy is improved 94.9% compared to the previous. Experimental result shows that the errors can be compensated easily and fast, and the axial-sensitive coincidence error of the multi-sensor can also be calibrated.

magnetometer; axial-sensitive coincidence error; maximum likelihood estimation (MLE); error compensation

10.3979/j.issn.1673-825X.2017.06.014

2016-09-13

2017-11-18

吳林志 Wulinzhi325@sina.com

國(guó)家自然科學(xué)基金項(xiàng)目資助(51175535)；MEMS振動(dòng)傳感與微姿態(tài)組合測(cè)井技術(shù)國(guó)際聯(lián)合研究中心科技平臺(tái)與基地建設(shè)(cstc2014gjhz0038)

TP212.13

A

1673-825X(2017)06-0801-05

劉 宇(1972 -)，男，重慶人，教授，博士，碩士生導(dǎo)師，長(zhǎng)期從事微振動(dòng)慣性傳感器、光機(jī)電一體化科學(xué)技術(shù)研發(fā)和MEMS慣性傳感技術(shù)研究。E-mail：liuyu@cqupt.edu.cn。

吳林志(1990 -)，男，河南信陽(yáng)人，碩士研究生，主要從事MEMS慣性傳感技術(shù)研究。E-mail：wulinzhi325@sina.com。

(編輯：張 誠(chéng))