基于磁干擾補(bǔ)償?shù)闹悄苁謾C(jī)車載AHRS 算法?

劉少兵，紀(jì)新春，魏東巖，李 雯，劉宇欣，鞏應(yīng)奎，袁 洪

(1.中國科學(xué)院空天信息創(chuàng)新研究院，北京 100094；2.中國科學(xué)院大學(xué)，北京 100094)

車輛導(dǎo)航需求日益迫切，隨著智能手機(jī)應(yīng)用的普及，智能手機(jī)導(dǎo)航已經(jīng)逐漸取代專用導(dǎo)航終端，成為大眾車輛導(dǎo)航的主流方式之一[1]。目前絕大多數(shù)智能手機(jī)中均內(nèi)置了全球?qū)Ш叫l(wèi)星系統(tǒng)(Global Navigation Satellite System，GNSS)模塊、微機(jī)電慣性測量單元(MEMS-based Inertial Measurement Unit，MIMU)、磁力計(jì)等傳感器，具有強(qiáng)大的感知能力、運(yùn)算能力與通信能力[2]。車輛導(dǎo)航通常選取航位推算系統(tǒng)(Dead Reckoning System，DR)與GNSS 進(jìn)行組合定位[3]。在開闊場景下，GNSS 可用時，可由GNSS 提供準(zhǔn)確的定位信息，對DR 累積誤差進(jìn)行校正。而在如城市峽谷等復(fù)雜場景中，GNSS 失鎖后DR 累計(jì)誤差無法得到及時校正。智能手機(jī)內(nèi)置的MIMU 成本較低，存在各種器件誤差，定位結(jié)果會迅速發(fā)散，其中航向發(fā)散是其主要因素之一。而磁力計(jì)通常作為地磁場敏感器件，測量磁場的大小和方向，將磁力計(jì)與MIMU 測量信息融合，進(jìn)行航姿參考系統(tǒng)(Attitude and Heading Reference System，AHRS)解算，得到車輛實(shí)時航向[4－5]，對DR 累積誤差進(jìn)行校正，完成實(shí)時、連續(xù)的車輛定位。

為了提高AHRS 估算精度，需要對傳感器進(jìn)行誤差標(biāo)定和干擾補(bǔ)償。目前關(guān)于MIMU 的誤差標(biāo)定和補(bǔ)償研究已較為成熟，考慮MIMU 誤差的影響，對其零偏誤差進(jìn)行了標(biāo)定和補(bǔ)償[4，6]，有效提高了AHRS 算法的估算精度，除MIMU 誤差外，磁力計(jì)誤差也是影響系統(tǒng)精度的主要因素之一，但與MIMU誤差大多源自器件本身不同，磁力計(jì)還受車輛載體磁干擾影響[7－8]，從而引起較大的測量誤差，進(jìn)而導(dǎo)致航向的偏差。對于磁力計(jì)標(biāo)定的研究，目前常用的方法有橢球擬合法、多傳感器輔助法等，文獻(xiàn)[9－12]中提出利用橢球擬合法對三軸磁強(qiáng)計(jì)各項(xiàng)誤差進(jìn)行標(biāo)定并補(bǔ)償，提高了磁強(qiáng)計(jì)的測量精度。文獻(xiàn)[13]利用加速度計(jì)輔助磁力計(jì)標(biāo)定，利用重力矢量和地磁矢量的點(diǎn)積不變性求解磁力計(jì)誤差。文獻(xiàn)[14]中利用加速度、陀螺儀以及磁力計(jì)解算的姿態(tài)角，構(gòu)建誤差模型，完成了9 軸傳感器各項(xiàng)誤差的標(biāo)定。但橢球擬合法和多傳感器輔助法需要采集磁強(qiáng)計(jì)和輔助傳感器多個姿態(tài)下的數(shù)據(jù)，在無環(huán)境磁場干擾下才能保證較好的標(biāo)定效果，否則無法確保標(biāo)定結(jié)果的準(zhǔn)確性，且都屬于離線標(biāo)定方法，不適用于車輛導(dǎo)航中需要實(shí)時去除車輛載體干擾的場景。當(dāng)磁場測量設(shè)備固聯(lián)在載體內(nèi)部時，為了估算載體磁干擾，需要將載體連同設(shè)備全方位地旋轉(zhuǎn)或翻轉(zhuǎn)[15－16]，這在車輛導(dǎo)航場景下也難以實(shí)現(xiàn)。

針對上述問題，提出了一種在線磁干擾補(bǔ)償?shù)能囕dAHRS 算法。在開闊環(huán)境下利用GNSS 獲取航向信息作為輔助，在線估計(jì)車輛載體磁干擾，將補(bǔ)償后的磁力計(jì)與MIMU 數(shù)據(jù)融合進(jìn)行AHRS 解算。并提出一種基于方差和多傳感器聯(lián)合的車載準(zhǔn)靜態(tài)磁場檢測方法，在環(huán)境磁場干擾過大時自適應(yīng)調(diào)整車載AHRS 算法中磁力計(jì)測量噪聲參數(shù)權(quán)值。選取城市道路場景進(jìn)行跑車實(shí)驗(yàn)，結(jié)果表明，經(jīng)過所提出方法對車輛載體引起的磁力計(jì)測量干擾在線補(bǔ)償及準(zhǔn)靜態(tài)磁場檢測后，AHRS 解算的航向精度顯著提高，滿足智能手機(jī)車輛導(dǎo)航應(yīng)用需求。

1 磁力計(jì)誤差模型

磁力計(jì)誤差主要源于器件自身誤差和環(huán)境干擾引起的誤差，其中器件自身誤差主要包含零偏誤差、比例因數(shù)誤差以及三軸非正交誤差，一般由器件制造以及安裝所引起。環(huán)境干擾主要包含硬磁干擾和軟磁干擾，在車輛導(dǎo)航場景下，硬磁誤差一般是由車輛載體的硬磁材料磁化后產(chǎn)生的外加磁場引起，可將其整合為零偏誤差。軟磁誤差一般是由車輛載體的軟磁材料受外部磁場磁化后產(chǎn)生的感應(yīng)磁場引起，可將其整合為比例因數(shù)誤差和非正交誤差。綜合分析各類型誤差的機(jī)理及其對磁力計(jì)測量結(jié)果的影響[8－12]，一般可建立磁力計(jì)測量誤差模型如下:

式中:m為磁力計(jì)的三軸測量值，mb為b系的磁力計(jì)測量的三軸地磁場強(qiáng)度。A為比例因數(shù)誤差和非正交誤差矩陣，由磁力計(jì)自身靈敏度、非正交誤差以及環(huán)境中的軟磁干擾引起。bm為磁力計(jì)的零偏誤差，由磁力計(jì)自身零偏誤差和環(huán)境中的硬磁干擾引起，nm為測量噪聲，其方差為。

若要對式(1)中完整的誤差參數(shù)進(jìn)行估計(jì)，需要通過橢球擬合等方法，利用轉(zhuǎn)臺等設(shè)備輔助來翻轉(zhuǎn)載體以采集標(biāo)定數(shù)據(jù)。而在智能手機(jī)車輛導(dǎo)航過程中，手機(jī)需要與車輛載體固聯(lián)，不可能將手機(jī)連同車輛載體進(jìn)行翻轉(zhuǎn)，故難以激勵所有誤差項(xiàng)。對于智能手機(jī)車輛導(dǎo)航而言，其誤差主要由器件自身零偏和車輛載體等硬磁干擾引起，而比例因數(shù)和非正交誤差影響相對較小[17]，僅考慮自身零偏和車輛載體磁干擾的影響，式(1)可寫成:

式中:bm為磁力計(jì)自身零偏和車輛載體磁干擾，也是車輛導(dǎo)航過程中影響航向精度的主要誤差源，本文主要對該部分干擾和誤差進(jìn)行補(bǔ)償。

2 基于磁干擾補(bǔ)償?shù)闹悄苁謾C(jī)車載AHRS 算法框架

本文提出的基于磁干擾補(bǔ)償?shù)闹悄苁謾C(jī)車載AHRS 具體包含基于GNSS 輔助的車輛載體磁干擾在線估計(jì)算法和基于準(zhǔn)靜態(tài)磁場檢測的車載AHRS算法兩部分。算法框圖如圖1 所示。

圖1 算法框圖

其中基于GNSS 輔助的車輛載體磁干擾在線估計(jì)算法部分，利用GNSS 信息作為量測信息，構(gòu)建擴(kuò)展卡爾曼濾波器(Extended Kalman Filter，EKF)對車輛載體磁干擾進(jìn)行在線估計(jì)并補(bǔ)償，圖中分別表示車輛載體引起的磁力計(jì)測量干擾。基于準(zhǔn)靜態(tài)磁場檢測的車載AHRS 算法部分，采用三軸加速度計(jì)和載體磁干擾在線補(bǔ)償后的三軸磁力計(jì)輔助三軸陀螺儀的形式構(gòu)建擴(kuò)展卡爾曼濾波器，融合三種傳感器數(shù)據(jù)，實(shí)時解算車輛的姿態(tài)角信息，圖中θ、γ、φ分別表示俯仰角、橫滾角和航向角。利用基于方差和多傳感器聯(lián)合的車載準(zhǔn)靜態(tài)磁場檢測方法，在非準(zhǔn)靜態(tài)磁場檢測區(qū)間下自適應(yīng)調(diào)整磁力計(jì)的量測噪聲協(xié)方差，提高算法在磁場干擾復(fù)雜環(huán)境下的航向解算精度。

3 基于準(zhǔn)靜態(tài)磁場檢測的車載AHRS算法

3.1 車載AHRS 算法EKF 初始化

磁力計(jì)的輸出模型如式(2)所示，對于MIMU輸出模型，同樣僅考慮零偏誤差，忽略比例因子和非正交誤差的影響，即:

式中:a、ωg分別為加速度計(jì)和陀螺儀的三軸測量值，ab、ωb分別為b系下加速度計(jì)和陀螺儀的三軸理想輸入值，ba、bg分別為加速度計(jì)和陀螺儀的三軸零偏，na、ng分別為測量噪聲，其方差分別為、。

車輛導(dǎo)航中，導(dǎo)航坐標(biāo)系(n系)為東北天坐標(biāo)系，載體坐標(biāo)系(b系)為右前上坐標(biāo)系。利用加速度計(jì)與磁力計(jì)測量信息可完成載體姿態(tài)角估計(jì)，其具體公式為:

式中:θ、γ、φ為俯仰角、橫滾角和航向角，ax、ay、az分別為加速度計(jì)的三軸測量值，mx、my、mz分別為磁力計(jì)的三軸測量值，g為當(dāng)?shù)刂亓铀俣龋翞楫?dāng)?shù)氐拇牌恰?/p>

由姿態(tài)矩陣可計(jì)算初始的四元數(shù)為:

式中:q0，q1，q2，q3可由初始姿態(tài)矩陣求得。

四元數(shù)微分方程為:

式中:?為四元數(shù)乘法符號，令ωg＝[ωxωy ωz]T，則:

姿態(tài)四元數(shù)更新公式為:

式中:T為采樣時間間隔。

3.2 狀態(tài)預(yù)測過程

將姿態(tài)四元數(shù)作為AHRS 算法中EKF 濾波器的狀態(tài)矢量:

3.3 量測更新過程

將ab、mb與加速度計(jì)、磁力計(jì)測量值的差值作為觀測量，即

3.4 準(zhǔn)靜態(tài)磁場檢測

在車輛導(dǎo)航應(yīng)用場景，通常車輛載體引起的磁場干擾是磁力計(jì)測量干擾的主要來源，磁干擾在線補(bǔ)償?shù)闹饕康囊彩窍@部分干擾誤差。但在實(shí)際情況下，如會車時或路況復(fù)雜時難以避免會存在較大干擾源。這部分干擾主要以硬磁干擾為主，造成相對于車輛載體更復(fù)雜的磁干擾，進(jìn)而會嚴(yán)重影響載體磁干擾在線估計(jì)精度及航向解算精度，因此需要對磁場環(huán)境進(jìn)行實(shí)時的準(zhǔn)靜態(tài)磁場檢測。

通常僅將沒有環(huán)境磁場干擾時的地磁場認(rèn)為是準(zhǔn)靜態(tài)磁場。本文針對車載導(dǎo)航特殊應(yīng)用場景，認(rèn)為準(zhǔn)靜態(tài)磁場由地磁場和車輛載體干擾磁場共同構(gòu)成。由于車輛載體會引起較大的硬磁干擾，使得磁力計(jì)三軸測量值的合磁場強(qiáng)度一般要大于當(dāng)?shù)氐卮艌鰪?qiáng)度。此外，當(dāng)車輛航向發(fā)生改變時，三軸磁力計(jì)測量值的合磁場并非一個固定值。此時將當(dāng)?shù)氐卮艌鰪?qiáng)度作為閾值判斷是否處于準(zhǔn)靜態(tài)磁場環(huán)境，無法滿足車輛導(dǎo)航應(yīng)用需求。本文提出一種準(zhǔn)靜態(tài)磁場檢測方法，將滑動窗口內(nèi)三軸磁力計(jì)測量值方差大小作為基礎(chǔ)條件，多傳感器測量信息聯(lián)合，檢測是否處于準(zhǔn)靜態(tài)磁場環(huán)境:

式中:vark:k＋L(mx)、vark:k＋L(my)、vark:k＋L(my)為第k個長度為L的窗口內(nèi)三軸磁力計(jì)測量磁場的方差，varinitial為靜態(tài)時刻下由磁力計(jì)測量數(shù)據(jù)統(tǒng)計(jì)的方差經(jīng)驗(yàn)值，Δφm，k為第k窗口內(nèi)由磁力計(jì)求得的航向角變化量，Δφg，k為第k窗口內(nèi)由陀螺儀求得的航向角變化量，Δφinitial為靜態(tài)時刻下由磁力計(jì)和陀螺儀統(tǒng)計(jì)的航向角差值的統(tǒng)計(jì)經(jīng)驗(yàn)值。當(dāng)滿足上式時，視該窗口內(nèi)測量磁場為準(zhǔn)靜態(tài)磁場區(qū)間，此時自適應(yīng)調(diào)整EKF算法中磁力計(jì)觀測噪聲的協(xié)方差權(quán)值。當(dāng)判斷該窗口屬于準(zhǔn)靜態(tài)磁場區(qū)間時，減小協(xié)方差權(quán)值；當(dāng)判斷該窗口為非準(zhǔn)靜態(tài)磁場區(qū)間時，意味著除車輛載體干擾外，環(huán)境中還存在較大的磁干擾，此時增大協(xié)方差權(quán)值。自適應(yīng)方差公式如下:

4 基于GNSS 輔助的車輛載體磁干擾在線估計(jì)算法

上述車載AHRS 算法中，車輛載體引起的磁力計(jì)測量干擾會嚴(yán)重影響航向估算精度。基于式(2)的磁力計(jì)輸出模型，提出一種基于GNSS 輔助的車輛載體磁干擾在線估計(jì)算法，在GNSS 可用時，利用GNSS 航向信息作為觀測量，構(gòu)建擴(kuò)展卡爾曼濾波器，對車輛載體引起的磁力計(jì)測量干擾進(jìn)行在線估計(jì)并補(bǔ)償。

4.1 狀態(tài)預(yù)測過程

構(gòu)建擴(kuò)展卡爾曼濾波器對磁力計(jì)三軸干擾進(jìn)行在線估計(jì)，選取磁力計(jì)自身零偏和載體磁干擾共同引起三軸磁力計(jì)的零偏作為狀態(tài)矢量:

系統(tǒng)狀態(tài)預(yù)測方程為:

4.2 量測更新過程

以GNSS 航向和磁力計(jì)解算航向的差值作為系統(tǒng)觀測量，系統(tǒng)狀態(tài)觀測方程為:

式中:Z＝[Δφ]，Δφ＝φgnss－φm，φgnss為GNSS 提供的航向，φm為式(5)加速度計(jì)和磁力計(jì)測量信息解算的航向，V為GNSS 航向量測噪聲，協(xié)方差矩陣R＝系統(tǒng)觀測矩陣H＝[H1H2H3]。

引入三軸磁力計(jì)零偏，由式(4)可得航向角計(jì)算公式為:

可得:

利用上述算法在線估算的磁力計(jì)的零偏，對磁力計(jì)進(jìn)行實(shí)時補(bǔ)償，補(bǔ)償后磁力計(jì)輸出為:

5 實(shí)驗(yàn)與分析

5.1 實(shí)驗(yàn)場景

實(shí)驗(yàn)采用華為Mate20 內(nèi)置的MIMU 及磁力計(jì)獲取實(shí)驗(yàn)數(shù)據(jù)，MIMU 型號為意法半導(dǎo)體公司生產(chǎn)的iNemo LSM6DSM，磁力計(jì)型號為AKM 公司生產(chǎn)的AKM09918。傳感器參數(shù)指標(biāo)如表1 所示。

表1 MIMU 和磁力計(jì)參數(shù)指標(biāo)

智能手機(jī)放置位置及實(shí)驗(yàn)車輛如圖2 所示，進(jìn)行跑車實(shí)驗(yàn)，實(shí)驗(yàn)區(qū)域?yàn)橹锌圃罕本┬录夹g(shù)基地周邊區(qū)域。具體車輛行駛路線如圖3 所示，在圖中對車庫區(qū)域進(jìn)行了標(biāo)識。

圖2 實(shí)驗(yàn)設(shè)備圖

圖3 實(shí)驗(yàn)路線圖

5.2 結(jié)果分析

采用本文提出的算法對車輛載體引起的磁力計(jì)測量干擾進(jìn)行在線估計(jì)和補(bǔ)償，根據(jù)GNSS 可用星數(shù)和位置精度因子(Position Dilution of Precision，Pdop)來判斷信號質(zhì)量的優(yōu)劣。當(dāng)GNSS 信號較好時，進(jìn)行載體磁干擾在線估計(jì)。補(bǔ)償前磁力計(jì)水平軸輸出結(jié)果如圖4 中實(shí)線和點(diǎn)線所示，其中100 s～400 s 和400 s～700 s 車輛行駛方向完全相反，由于車輛載體干擾的存在，磁力計(jì)的水平軸測量數(shù)據(jù)產(chǎn)生了嚴(yán)重的偏移。經(jīng)在線標(biāo)定補(bǔ)償后，磁力計(jì)輸出結(jié)果如圖4 中點(diǎn)劃線和虛線所示。可以看出，車輛載體干擾引起的磁力計(jì)測量干擾得到了有效修正。需要說明的是，由于車輛行駛過程中，俯仰角和橫滾角的變化幅度較小，難以激勵出天向磁力計(jì)的誤差，其誤差估算結(jié)果收斂性較差，故未對磁力計(jì)Z軸測量干擾進(jìn)行補(bǔ)償。

圖4 補(bǔ)償前后磁力計(jì)水平軸數(shù)據(jù)

首先采用本文提出的方法進(jìn)行準(zhǔn)靜態(tài)磁場檢測，當(dāng)處于非準(zhǔn)靜態(tài)磁場環(huán)境中時，自適應(yīng)調(diào)整車載AHRS 算法解算過程中的磁力計(jì)觀測噪聲參數(shù)。為了達(dá)到較好的標(biāo)定和航向解算效果，在室外開闊區(qū)域，參數(shù)閾值設(shè)置較為嚴(yán)格，其中varinitial為0.8，Δφinitial為0.5。而在車庫等干擾較大的區(qū)域，參數(shù)閾值設(shè)置較為寬松，其中varinitial為8，Δφinitial為2。得到準(zhǔn)靜態(tài)磁場檢測結(jié)果如圖5 所示，當(dāng)檢測器(圖5中實(shí)線所示)值為0 時，認(rèn)為該點(diǎn)磁場為準(zhǔn)靜態(tài)磁場，否則為非準(zhǔn)靜態(tài)磁場。從圖中約800 s 后的車庫區(qū)域檢測結(jié)果可以看出，車庫停放車輛較多，硬磁干擾較大，但仍有部分區(qū)間處于準(zhǔn)靜態(tài)磁場區(qū)間。

圖5 準(zhǔn)靜態(tài)磁場檢測結(jié)果

Mata20 內(nèi)置MIMU 采集的數(shù)據(jù)如圖6 所示，利用上文所述的車載AHRS 算法進(jìn)行航向解算。設(shè)置3.4 節(jié)中參數(shù)k1＝200，k2＝150。

圖6 實(shí)驗(yàn)中Mata20 內(nèi)置MIMU 數(shù)據(jù)

AHRS 算法解算結(jié)果如圖7 所示，其中實(shí)線為參考航向。車輛載體磁干擾補(bǔ)償前航向解算結(jié)果如圖7 中點(diǎn)劃線所示，由于車輛載體存在嚴(yán)重的磁干擾，解算的航向嚴(yán)重失準(zhǔn)。補(bǔ)償車輛載體的磁干擾后，航向解算結(jié)果如圖7中點(diǎn)線所示，由圖8 所示的誤差圖(圖8 中點(diǎn)線部分)可以看出，在環(huán)境干擾較小時，航向精度在10°以內(nèi)。但在車庫環(huán)境中，航向精度在20°左右。結(jié)合準(zhǔn)靜態(tài)磁場檢測并補(bǔ)償車輛載體的磁干擾后，航向解算結(jié)果如圖7 中虛線所示，其基本與真值重合，由圖8 中的誤差圖(圖8 中實(shí)線部分)可以看出，經(jīng)過準(zhǔn)靜態(tài)磁場檢測后，非準(zhǔn)靜態(tài)磁場區(qū)間中的誤差大幅度減小，總體航向誤差約在5°以內(nèi)。

圖7 補(bǔ)償前后航向與參考航向

圖8 補(bǔ)償前后航向誤差

為驗(yàn)證本文所提出算法的實(shí)際定位效果，將車輛從室外開闊場景行駛到室內(nèi)地下車庫中，利用普通車載AHRS＋DR＋GNSS 進(jìn)行組合定位解算，其結(jié)果如圖9 所示。由于室內(nèi)地下車庫(圖9 左下角實(shí)線部分所示)無GNSS 信號，且車輛載體誤差未消除，車載AHRS 解算的航向嚴(yán)重失準(zhǔn)，無法利用GNSS 信息或車載AHRS 航向校正DR 誤差，導(dǎo)致定位結(jié)果出現(xiàn)明顯偏移。根據(jù)本文提出的算法，利用在室外開闊場景標(biāo)定的車輛載體干擾誤差，對車輛載體磁干擾進(jìn)行補(bǔ)償。定位結(jié)果如圖10 所示，可以看出，本文提出的磁干擾補(bǔ)償?shù)能囕dAHRS 算法能夠提供準(zhǔn)確的航向，修正DR 系統(tǒng)的累積誤差，保證良好的定位結(jié)果。

圖9 誤差補(bǔ)償前定位結(jié)果

圖10 誤差補(bǔ)償后定位結(jié)果

6 結(jié)論

提出了一種基于磁干擾補(bǔ)償?shù)闹悄苁謾C(jī)車載AHRS 算法，利用手機(jī)內(nèi)置GNSS 傳感器提供的測量信息，對車輛載體磁干擾在線估計(jì)。結(jié)合準(zhǔn)靜態(tài)磁場檢測，進(jìn)行車輛AHRS 航向解算。跑車結(jié)果表明，該方法能夠準(zhǔn)確估計(jì)車輛載體磁干擾，大幅度提高AHRS 航向精度，有效抑制GNSS 失鎖時DR 系統(tǒng)定位發(fā)散速度快的問題，提高定位精度，滿足車輛導(dǎo)航應(yīng)用需求。