張 攀，劉錫祥，黃 榮，黃永江

(東南大學(xué) 儀器科學(xué)與工程學(xué)院/微慣性?xún)x表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，南京 210096)

0 引言

地磁場(chǎng)具有無(wú)源無(wú)輻射、不受載體運(yùn)動(dòng)影響且無(wú)誤差積累等優(yōu)勢(shì)，可以提供全天候、全地域的導(dǎo)航信息；根據(jù)地磁場(chǎng)信息進(jìn)行航向修正或者地磁匹配是組合導(dǎo)航定位的關(guān)鍵步驟之一。行人導(dǎo)航定位設(shè)備采用的捷聯(lián)式三軸磁力計(jì)能實(shí)時(shí)測(cè)量其周?chē)艌?chǎng)信息，由地磁場(chǎng)計(jì)算得到的航向在無(wú)源行人導(dǎo)航定位系統(tǒng)的初始對(duì)準(zhǔn)和導(dǎo)航解算誤差抑制方面有著十分重要的作用；然而地磁匹配用的地磁場(chǎng)和其他干擾磁場(chǎng)相互耦合，這嚴(yán)重影響磁力計(jì)的測(cè)量精度和地磁匹配應(yīng)用效果：因此磁力計(jì)的標(biāo)定和磁補(bǔ)償技術(shù)是地磁應(yīng)用的前提和關(guān)鍵技術(shù)，也是制約行人導(dǎo)航定位精度的關(guān)鍵因素之一[1]。

關(guān)于磁力計(jì)的標(biāo)定和磁補(bǔ)償技術(shù)一直以來(lái)推陳出新，目前廣泛使用的是文獻(xiàn)[2]提出的經(jīng)典Two-Step法，該算法利用最小二乘估計(jì)(least squares，LS)對(duì)磁力計(jì)誤差模型中的刻度因子和硬磁誤差進(jìn)行估計(jì)。文獻(xiàn)[3]考慮了安裝誤差和非正交誤差，擴(kuò)展了數(shù)學(xué)模型，并通過(guò)中間變量代數(shù)求解出各項(xiàng)誤差參數(shù)。在此基礎(chǔ)上，為獲得更精確的解，文獻(xiàn)[4]利用非線(xiàn)性參數(shù)估計(jì)如牛頓迭代法等進(jìn)行求解。文獻(xiàn)[5]針對(duì)磁力計(jì)的觀(guān)測(cè)方程兩邊同時(shí)含有噪聲的情況，提出了基于總體最小二乘算法的標(biāo)定方法。基于橢圓擬合的思想，文獻(xiàn)[6-7]分別利用易于實(shí)現(xiàn)的遞推最小二乘法和截?cái)嗫傮w最小二乘法估計(jì)橢圓參數(shù)對(duì)磁場(chǎng)進(jìn)行補(bǔ)償，均取得不錯(cuò)的效果。

固聯(lián)在載體上的捷聯(lián)磁力計(jì)對(duì)周?chē)艌?chǎng)十分敏感，數(shù)據(jù)采集中不可避免會(huì)出現(xiàn)誤差，如果不對(duì)其進(jìn)行處理，將會(huì)嚴(yán)重影響誤差參數(shù)的估計(jì)結(jié)果。本文基于Two-Step法，將自適應(yīng)抗差最小二乘估計(jì)(adaptive robust least squares，ARLS)用于磁力計(jì)誤差模型的參數(shù)估計(jì)中，采用IGG權(quán)函數(shù)給出自適應(yīng)調(diào)整權(quán)函數(shù)的閾值，其中IGG方案由中國(guó)科學(xué)院大地測(cè)量與地球物理研究所(Institute of Geodesy & Geophysics，Chinese Academy of Sciences，IGG)提出，結(jié)合行人導(dǎo)航定位的應(yīng)用場(chǎng)景，對(duì)初始航向和航向誤差抑制進(jìn)行研究。

1 磁力計(jì)誤差數(shù)學(xué)模型

由于軟磁材料的存在，在外部磁場(chǎng)激勵(lì)下其會(huì)產(chǎn)生隨著外部磁場(chǎng)大小和方向變化的磁場(chǎng)并疊加在磁力計(jì)輸出上。通過(guò)選擇標(biāo)定現(xiàn)場(chǎng)區(qū)域可以避免較復(fù)雜的磁場(chǎng)環(huán)境。而相對(duì)于地磁場(chǎng)和硬磁場(chǎng)來(lái)說(shuō)，磁力計(jì)及外圍電路的磁場(chǎng)可以忽略。因此本文只考慮地磁場(chǎng)和硬磁為外部激勵(lì)磁場(chǎng)，并假設(shè)軟磁誤差與外部磁場(chǎng)為線(xiàn)性且無(wú)磁滯。

結(jié)合上述5種誤差，捷聯(lián)三軸磁力計(jì)的誤差模型如圖1所示，其輸出模型為

(1)

圖1 捷聯(lián)磁力計(jì)的誤差模型

在實(shí)際標(biāo)定和應(yīng)用中，測(cè)量噪聲較小，暫不考慮。從輸出模型可以發(fā)現(xiàn)，非正交誤差、刻度因子誤差與軟磁誤差在數(shù)學(xué)上是無(wú)法分離的(當(dāng)然，在特定環(huán)境可以將3者區(qū)分，就標(biāo)定而言3者耦合不影響標(biāo)定結(jié)果)，則記K-1=CmCsfCsi。將K-1代入原輸出模型并展開(kāi)可以得到

(2)

(3)

2 Two-Step方法

在航向未知且無(wú)其他設(shè)備輔助情形下，Two-Step方法通過(guò)將磁力計(jì)輸出模型(1)進(jìn)行轉(zhuǎn)化，通過(guò)最小二乘法解算未知量，并通過(guò)代數(shù)分解求出了磁力計(jì)的誤差參數(shù)。

基于磁場(chǎng)標(biāo)定現(xiàn)場(chǎng)的地磁場(chǎng)模值R為一常量的原則，有如下方程

(4)

(5)

A×X=L

(6)

式中：A∈Rm×n(m為采樣點(diǎn)個(gè)數(shù)，n為未知數(shù)個(gè)數(shù)，3維模式下n為9，2維模式下n為6；X∈Rn×1；L∈Rm×1；

一般有m>n，Two-Step法標(biāo)定的第一步就利用如下最小二乘法解上述超定方程

(7)

第二步就是通過(guò)最小二乘估計(jì)得到的解，代數(shù)分解出磁力計(jì)輸出模型中的誤差參數(shù)。

在實(shí)際標(biāo)定過(guò)程中，A和L都含有觀(guān)測(cè)量的分量，因此都不可避免地引入一定誤差影響到最小二乘估計(jì)，此時(shí)最小二乘估計(jì)有偏，并不是最優(yōu)的；傳感器一旦出現(xiàn)較大粗差就會(huì)嚴(yán)重影響到參數(shù)估計(jì)結(jié)果，致使標(biāo)定失敗，進(jìn)而影響到組合導(dǎo)航精度。

3 自適應(yīng)抗差估計(jì)方法

從構(gòu)造一類(lèi)自適應(yīng)抗差最小二乘求解方程式出發(fā)，自適應(yīng)抗差最小二乘估計(jì)使得該估計(jì)過(guò)程能自動(dòng)地在廣泛的誤差分布類(lèi)之間選擇最適估計(jì)[8]。在磁力計(jì)標(biāo)定過(guò)程中，相比最小二乘估計(jì)，自適應(yīng)抗差估計(jì)通過(guò)殘差調(diào)整權(quán)重，能夠在粗差出現(xiàn)時(shí)獲得更加精確的誤差參數(shù)估計(jì)結(jié)果。

根據(jù)磁力計(jì)的參數(shù)估計(jì)模型和抗差估計(jì)原理[8]，參數(shù)平差模型為

V=AX-L權(quán)陣P

(8)

vi=aiX-Li權(quán)Pi

(9)

式中：ai為A陣之第i行元素組成的行向量；vi和Li分別為殘差V和L的分量；等價(jià)權(quán)陣P是殘差V和Pi的函數(shù)[9]。根據(jù)IGG方案[8-9]取等價(jià)權(quán)函數(shù)為

(10)

式中：v′i為第i組測(cè)量值的標(biāo)準(zhǔn)化殘差；根據(jù)經(jīng)驗(yàn)K0選擇1.0～1.5較好，K1在3.0左右為宜[5]。則誤差參數(shù)的抗差解為

(11)

自適應(yīng)抗差最小二乘估計(jì)的迭代形式為

(12)

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

4.1 2維平面實(shí)驗(yàn)

為了驗(yàn)證基于ARLS的磁力計(jì)標(biāo)定的有效性，先進(jìn)行2維平面上的標(biāo)定，以滿(mǎn)足無(wú)法充分旋轉(zhuǎn)獲得3維數(shù)據(jù)的情況。實(shí)驗(yàn)時(shí)，在數(shù)據(jù)中隨機(jī)加入幅值為地磁場(chǎng)10 %的粗差，以驗(yàn)證本文提出的方法對(duì)粗差的抑制效果。將三軸磁力計(jì)置于水平面，繞天向軸緩慢旋轉(zhuǎn)若干周，按照ARLS和Two-Step方法分別進(jìn)行標(biāo)定，結(jié)果如圖2～圖3所示。

圖2 Two-Step與ARLS標(biāo)定前后水平磁場(chǎng)

圖3 Two-Step與ARLS標(biāo)定前后磁場(chǎng)幅值

圖2、圖3的結(jié)果顯示：標(biāo)定前磁力計(jì)測(cè)量值波動(dòng)較大，誤差干擾嚴(yán)重，無(wú)法測(cè)量地磁場(chǎng)強(qiáng)度；而標(biāo)定和磁補(bǔ)償后，地磁場(chǎng)水平分量不再是橢圓，而是圓心在原點(diǎn)的圓，且磁場(chǎng)幅值穩(wěn)定，表明LS和ARLS在無(wú)法做3維全姿態(tài)運(yùn)動(dòng)時(shí)均可以對(duì)磁力計(jì)進(jìn)行有效標(biāo)定。在圖3的采樣區(qū)間700到900段加入粗差后，LS方法受粗差影響導(dǎo)致參數(shù)估計(jì)偏離真值嚴(yán)重，在粗差出現(xiàn)處產(chǎn)生明顯的突變。而ARLS能夠?qū)⒋植畹挠绊懴拗圃诤瘮?shù)模型的殘差中，很好地抑制粗差，減小粗差對(duì)標(biāo)定效果的影響。

4.2 3維標(biāo)定實(shí)驗(yàn)

為了檢驗(yàn)3維旋轉(zhuǎn)情況下的標(biāo)定效果，在某室內(nèi)環(huán)境下先將三軸磁力計(jì)繞天向軸旋轉(zhuǎn)若干周，再將三軸磁力計(jì)繞東向軸旋轉(zhuǎn)若干周。2種方法標(biāo)定前后的磁場(chǎng)分布及幅值如圖4、圖5所示。

圖4 磁力計(jì)3維標(biāo)定前后磁場(chǎng)分布

圖5 標(biāo)定前后3維地磁場(chǎng)幅值

在圖5所示3維實(shí)驗(yàn)的采樣區(qū)間內(nèi)出現(xiàn)了磁場(chǎng)幅值約20 %的粗差，圖中可見(jiàn)：ARLS能動(dòng)態(tài)地調(diào)整抗差因子，減小含有粗差的測(cè)量信息在估計(jì)中的權(quán)重，降低粗差對(duì)參數(shù)估計(jì)的影響，有效提高磁力計(jì)標(biāo)定的精度；而LS不具有抗差能力，無(wú)法應(yīng)對(duì)現(xiàn)場(chǎng)出現(xiàn)不良數(shù)據(jù)的情況，使得其標(biāo)定結(jié)果不可信。為了判斷標(biāo)定質(zhì)量和標(biāo)定方法的效果，可通過(guò)均方根誤差RMSE來(lái)比較。

(16)

表1 標(biāo)定實(shí)驗(yàn)殘差均方根統(tǒng)計(jì)

從表1中可知，ARLS算法的殘差均方根誤差小于Two-Step，參數(shù)估計(jì)效果更佳，具有更高的精度。

4.3 行人導(dǎo)航定位系統(tǒng)實(shí)驗(yàn)

行人導(dǎo)航定位系統(tǒng)結(jié)構(gòu)如圖6所示。

圖6 行人導(dǎo)航定位系統(tǒng)算法結(jié)構(gòu)

微機(jī)電系統(tǒng)慣性測(cè)量單元(microelectromechanical systems-inertial measurement unit，MEMS-IMU)與磁力計(jì)置于人體足部[10]。由于MEMS-IMU精度較低，初始對(duì)準(zhǔn)過(guò)程無(wú)法獲得準(zhǔn)確的航向信息；在由加速度計(jì)敏感的重力投影值求得水平姿態(tài)角后，行人導(dǎo)航定位系統(tǒng)初始對(duì)準(zhǔn)中航向信息可以由磁力計(jì)測(cè)得的地磁場(chǎng)獲?。阂虼嗽诔跏紝?duì)準(zhǔn)之前的磁力計(jì)標(biāo)定和磁補(bǔ)償對(duì)于初始對(duì)準(zhǔn)和后續(xù)導(dǎo)航解算十分重要。

在行人行走過(guò)程中，慣性測(cè)量單元置于人體足部，利用陀螺儀和加速度計(jì)可以檢測(cè)出人行走時(shí)腳部與地面完全接觸(即零速區(qū)間)的瞬間，此時(shí)速度為零成為Kalman濾波器的觀(guān)測(cè)量。在僅有零速這一個(gè)觀(guān)測(cè)量時(shí)，速度、位置等誤差發(fā)散能夠被抑制[10-11]；但是由于航向誤差無(wú)法估計(jì)，因此需要引入磁力計(jì)來(lái)修正航向角誤差[12]。

將MEMS-IMU置于足部，在行走前按照上述2種標(biāo)定方法對(duì)磁力計(jì)進(jìn)行標(biāo)定，然后在東南大學(xué)四牌樓校區(qū)做2組行走實(shí)驗(yàn)：實(shí)驗(yàn)一為在某南北向路上進(jìn)行50 m直線(xiàn)折返行走，行人導(dǎo)航定位結(jié)果如圖7所示；實(shí)驗(yàn)二為在四牌樓田徑場(chǎng)400 m跑道進(jìn)行單圈行走，定位和航向角結(jié)果如圖8所示。

圖7 南北路50 m直線(xiàn)折返行走實(shí)驗(yàn)

圖8 田徑場(chǎng)400 m跑道單圈行走實(shí)驗(yàn)

在初始對(duì)準(zhǔn)前，對(duì)固聯(lián)在足部的捷聯(lián)磁力計(jì)按照上述方法進(jìn)行標(biāo)定，利用地磁來(lái)獲得初始航向。在上述試驗(yàn)中，由法國(guó)SBG系統(tǒng)給出的INS/GNSS組合導(dǎo)航系統(tǒng)的位置和航向角作為實(shí)驗(yàn)的參照標(biāo)準(zhǔn)值。在行走過(guò)程中，在零速時(shí)刻對(duì)航向進(jìn)行補(bǔ)償使得航向漂移得以抑制。通過(guò)實(shí)驗(yàn)結(jié)果對(duì)比，ARLS方法標(biāo)定后，初始航向偏差由原來(lái)的6.14°減小到3.78°，標(biāo)定后ARLS方法的初始航向更加準(zhǔn)確。行人導(dǎo)航定位系統(tǒng)定位結(jié)果的終點(diǎn)誤差由原來(lái)的8.2減小到0.7 m，終點(diǎn)誤差減小了15 %；隨著時(shí)間推移，磁力計(jì)的輔助能夠有效抑制誤差發(fā)散，航向角得到較好的校正。

5 結(jié)束語(yǔ)

磁力計(jì)的標(biāo)定是地磁匹配和組合導(dǎo)航的前提和關(guān)鍵。本文將自適應(yīng)抗差最小二乘估計(jì)應(yīng)用到捷聯(lián)三軸磁力計(jì)的在線(xiàn)標(biāo)定上，考慮到現(xiàn)場(chǎng)測(cè)量值中可能出現(xiàn)粗差的情況，對(duì)原有的最小二乘估計(jì)做出了改進(jìn)，當(dāng)載體無(wú)法做全姿態(tài)運(yùn)動(dòng)時(shí)，該算法也可以進(jìn)行較好的標(biāo)定，增強(qiáng)算法的適應(yīng)性和抗干擾能力。實(shí)驗(yàn)表明，無(wú)需借助轉(zhuǎn)臺(tái)等大型設(shè)備，只需要簡(jiǎn)單的旋轉(zhuǎn)，基于ARLS的標(biāo)定方法就能完成捷聯(lián)三軸磁力計(jì)的標(biāo)定，并且能有效地校正初始航向角，提高行人定位系統(tǒng)的導(dǎo)航定位精度。

[1] 陳卓，田風(fēng)勛，孫建軍.地磁導(dǎo)航關(guān)鍵技術(shù)研究進(jìn)展綜述[J].測(cè)繪與空間地理信息，2016，39(1)：16-18.

[2] GEBRE-EGZIABHER D, ELKAIM G H, POWELL J D, et al. A non-linear, two-step estimation algorithm for calibrating solid-state strapdown magnetometers[EB/OL].[2017-06-28].https://users.soe.ucsc.edu/～elkaim/Documents/TwoVec.pdf.

[3] FOSTER C C, ELKAIM G H. Extension of a two-step calibration methodology to include nonorthogonal sensor axes[J]. Aerospace & Electronic Systems IEEE Transactions on, 2008, 44(3):1070-1078.

[4] DORVEAUX E, VISSIERE D, MARTIN A P, et al. Iterative calibration method for inertial and magnetic sensors[EB/OL].[2017-06-28].https://hal-mines-paristech.archives-ouvertes.fr/hal-00473610/document.

[5] 吳志添，武元新，胡小平，等．基于總體最小二乘的捷聯(lián)三軸磁力儀標(biāo)定與地磁場(chǎng)測(cè)量誤差補(bǔ)償[J]．兵工學(xué)報(bào)，2012，33(10)：1202-1209.

[6] 盛蔚，蔣勇. 三軸磁傳感器系統(tǒng)的在線(xiàn)標(biāo)定[J]. 彈箭與制導(dǎo)學(xué)報(bào),2015,35(3):179-182.

[7] 張瀅，楊任農(nóng)，李明陽(yáng)，等. 基于截?cái)嗫傮w最小二乘算法的車(chē)載三軸磁力儀標(biāo)定[J]. 兵工學(xué)報(bào),2015,36(3):427-432.

[8] 楊元喜. 自適應(yīng)抗差最小二乘估計(jì)[J]. 測(cè)繪學(xué)報(bào),1996,25(3):206-211.

[9] 任夏，李鑄洋，丁陽(yáng). 抗差自適應(yīng)濾波算法在實(shí)時(shí)定軌中的應(yīng)用[J]. 導(dǎo)航定位學(xué)報(bào),2016,4(2):26-28+35.

[10] FISCHER C, SUKUMAR P T, HAZAS M. Tutorial: implementation of a pedestrian tracker using foot-mounted inertial sensors[EB/OL].[2017-06-28].http://www.comp.lancs.ac.uk/～hazas/Fischer13_ImplementingaPedestrianTracker.pdf.

[11] 馬明，宋千，李楊寰，等.基于地磁輔助的室內(nèi)行人定位航向校正方法[J].電子與信息學(xué)報(bào)，2017，39(3)：647-653.

[12] 錢(qián)偉行，朱欣華，蘇巖.基于足部微慣性/地磁測(cè)量組件的個(gè)人導(dǎo)航方法[J].中國(guó)慣性技術(shù)學(xué)報(bào)，2012，20(5)：567-572.