自適應(yīng)抗差最小二乘估計的捷聯(lián)磁力計標(biāo)定

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

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

0 引言

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

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

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

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

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

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

(1)

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

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

(2)

(3)

2 Two-Step方法

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

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

(4)

(5)

A×X=L

(6)

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

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

(7)

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

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

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

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

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

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

(8)

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

(9)

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

(10)

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

(11)

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

(12)

4 實驗與結(jié)果分析

4.1 2維平面實驗

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

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

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

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

4.2 3維標(biāo)定實驗

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

圖4 磁力計3維標(biāo)定前后磁場分布

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

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

(16)

表1 標(biāo)定實驗殘差均方根統(tǒng)計

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

4.3 行人導(dǎo)航定位系統(tǒng)實驗

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

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

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

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

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

圖7 南北路50 m直線折返行走實驗

圖8 田徑場400 m跑道單圈行走實驗

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

5 結(jié)束語

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

[1] 陳卓，田風(fēng)勛，孫建軍.地磁導(dǎo)航關(guān)鍵技術(shù)研究進(jìn)展綜述[J].測繪與空間地理信息，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)定與地磁場測量誤差補償[J]．兵工學(xué)報，2012，33(10)：1202-1209.

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

[7] 張瀅，楊任農(nóng)，李明陽，等. 基于截斷總體最小二乘算法的車載三軸磁力儀標(biāo)定[J]. 兵工學(xué)報,2015,36(3):427-432.

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

[9] 任夏，李鑄洋，丁陽. 抗差自適應(yīng)濾波算法在實時定軌中的應(yīng)用[J]. 導(dǎo)航定位學(xué)報,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é)報，2017，39(3)：647-653.

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