航姿系統(tǒng)矢量場(chǎng)傳感器校正的雙內(nèi)積法*

李 翔,宋百麒,王勇軍,2

(1.桂林電子科技大學(xué)電子工程與自動(dòng)化學(xué)院,桂林 541004;2.桂林航天工業(yè)學(xué)院無(wú)人機(jī)遙測(cè)重點(diǎn)實(shí)驗(yàn)室,桂林 541004)

航姿系統(tǒng)廣泛用于無(wú)人飛行器、車輛及船只的導(dǎo)航定位,可提供航向角、俯仰角、橫滾角等數(shù)據(jù)。目前,航姿系統(tǒng)中普遍采用三軸磁強(qiáng)計(jì)和三軸加速度計(jì),這兩種傳感器均屬于矢量場(chǎng)傳感器,分別通過(guò)測(cè)量地磁場(chǎng)和重力矢量獲取姿態(tài)信息,是簡(jiǎn)便可靠的無(wú)源導(dǎo)航手段。但磁強(qiáng)計(jì)和加速度計(jì)易受多種因素干擾,如零偏、靈敏度(標(biāo)度因數(shù))、非正交誤差、軟硬磁干擾等。每當(dāng)使用環(huán)境發(fā)生變化時(shí),往往需要重新進(jìn)行校正[1-2]。

三軸矢量場(chǎng)傳感器的校正方法從原理上可分為與姿態(tài)相關(guān)的校正方法和與姿態(tài)無(wú)關(guān)的校正方法。由于磁強(qiáng)計(jì)和加速度計(jì)的輸出直接取決于其載體的三維姿態(tài),因此可借助精確的航姿基準(zhǔn)實(shí)現(xiàn)二者的校正,此即與姿態(tài)相關(guān)的校正方法,又稱多位置法[3]。但在實(shí)際應(yīng)用中,為減少對(duì)外部設(shè)備的依賴并簡(jiǎn)化操作步驟,普遍采用與姿態(tài)無(wú)關(guān)的校正方法,又稱橢球擬合法[4-8]。此類方法假定同一地點(diǎn)的地磁場(chǎng)強(qiáng)度和重力加速度在短時(shí)間內(nèi)保持為常數(shù),其主要優(yōu)點(diǎn)是校正過(guò)程中僅用到傳感器自身讀數(shù),無(wú)需外部設(shè)備或基準(zhǔn),便于實(shí)施現(xiàn)場(chǎng)校正;其缺點(diǎn)是會(huì)導(dǎo)致數(shù)學(xué)上的欠定問(wèn)題[9-10]以及校正后的磁強(qiáng)計(jì)存在非對(duì)準(zhǔn)誤差[11-18],需要作進(jìn)一步校正。

針對(duì)上述問(wèn)題,本文提出一種基于多內(nèi)積的校正方法。該方法既利用地磁場(chǎng)和重力矢量自身的模值不變性,又利用傳感器轉(zhuǎn)動(dòng)時(shí)地磁場(chǎng)和重力矢量在旋轉(zhuǎn)軸上的投影不變性,可實(shí)現(xiàn)三軸磁強(qiáng)計(jì)和加速度計(jì)的完全校正。在航姿模塊上對(duì)該方法及其他常用校正方法進(jìn)行了對(duì)比實(shí)驗(yàn),驗(yàn)證了該方法的有效性。

1 校正原理

1.1 誤差模型

三軸加速度計(jì)和磁強(qiáng)計(jì)的誤差模型如式(1)所示,其中:u=(u1u2u3)T為載體坐標(biāo)系中的被測(cè)矢量(對(duì)于磁強(qiáng)計(jì)為地磁場(chǎng)矢量,對(duì)于加速度計(jì)為重力矢量),v=(v1v2v3)T為傳感器輸出。

v=Ku+b+ε

(1)

式中,矢量ε、b和矩陣K描述傳感器的各種誤差來(lái)源:①零偏:傳感器各軸的零偏依次由矢量b的3個(gè)分量表示;②靈敏度(標(biāo)度系數(shù)):傳感器各軸的靈敏度分別由矩陣K的3個(gè)對(duì)角元表示;③非正交及非對(duì)準(zhǔn)誤差:由矩陣K的6個(gè)非對(duì)角元表示;④磁強(qiáng)計(jì)受到來(lái)自載體和外界的磁干擾:分為硬磁干擾和軟磁干擾兩部分,分別包含于b和K中。⑤傳感器噪聲及高階誤差項(xiàng):包含于ε中。

對(duì)于目前廣泛應(yīng)用的低成本磁強(qiáng)計(jì)和加速度計(jì),校正通常主要針對(duì)常值誤差及線性誤差,即式(1)中的K與b,而視ε為小量并忽略之。記L=K-1,則誤差補(bǔ)償可由式(2)實(shí)現(xiàn)。

u=L(v-b)

(2)

1.2 橢球擬合法

航姿系統(tǒng)及其載體作原地轉(zhuǎn)動(dòng)時(shí),載體坐標(biāo)系中地磁場(chǎng)和重力矢量的方向發(fā)生變化,但矢量模值保持不變,這一性質(zhì)可由式(3)描述。

(3)

式中:u0表示被測(cè)矢量u的模(即地磁場(chǎng)強(qiáng)度或重力加速度)。將式(2)代入式(3)即可得到關(guān)于L與b的方程,進(jìn)而可根據(jù)傳感器原始數(shù)據(jù)利用各種非線性擬合或優(yōu)化算法求解L與b. 由于式(3)表示的是一個(gè)標(biāo)量,故基于式(3)的校正方法也稱為標(biāo)量校正法(scalar calibration)。另一方面,采用式(2)進(jìn)行誤差補(bǔ)償?shù)那疤崾鞘?1)中的誤差矩陣K可逆(即L=K-1存在)。此時(shí),將式(2)代入式(3)所得關(guān)于u的二次型必為正定二次型,其對(duì)應(yīng)的幾何圖形為一橢球面。因此,凡基于式(3)的校正方法,不論其是否顯式地求解橢球的中心與主軸等參數(shù),數(shù)學(xué)上均等價(jià)于對(duì)橢球面進(jìn)行擬合,故此類方法又統(tǒng)稱為橢球擬合法。

橢球擬合法的幾何意義是將關(guān)于矢量v的橢球面變換為關(guān)于矢量u的球面。由該變換的數(shù)學(xué)特性可知,橢球擬合法的優(yōu)點(diǎn)是:能實(shí)現(xiàn)矢量模值誤差的最小化,以及傳感器各軸間的正交化。而其主要缺點(diǎn)是:式(3)的二次型最多只能提供9個(gè)獨(dú)立參數(shù),而式(1)及式(2)的誤差模型含12個(gè)獨(dú)立參數(shù),從而造成數(shù)學(xué)上的欠定問(wèn)題。其根源在于式(3)完全沒有涉及矢量的指向,因而缺失的3個(gè)參數(shù)對(duì)應(yīng)的正是3維空間中的3個(gè)轉(zhuǎn)動(dòng)自由度。因此,橢球擬合法無(wú)法保證校正后傳感器的三軸與載體坐標(biāo)系的三軸對(duì)應(yīng)重合,即存在非對(duì)準(zhǔn)誤差。這一非對(duì)準(zhǔn)誤差可用一個(gè)正交矩陣R表示,即從載體坐標(biāo)系到傳感器坐標(biāo)系的三維旋轉(zhuǎn)變換。

1.3 點(diǎn)積不變法

航姿系統(tǒng)及其載體原地轉(zhuǎn)動(dòng)時(shí),重力矢量與地磁場(chǎng)矢量夾角不變,故二者的點(diǎn)積是一個(gè)常數(shù)。記重力矢量為g=(g1g2g3)T,地磁場(chǎng)矢量為h=(h1h2h3)T,二者夾角為ζ,則上述原理可由式(4)描述。

gTh=g0h0cosζ

(4)

式中:g0和h0分別為本地重力加速度及地磁場(chǎng)強(qiáng)度。若加速度計(jì)已經(jīng)由橢球擬合法校正,則利用加速度計(jì)讀數(shù)及式(4)可對(duì)三軸磁強(qiáng)計(jì)進(jìn)行校正,此即三軸磁強(qiáng)計(jì)校正的點(diǎn)積不變法[11-12]。

相對(duì)而言,加速度計(jì)較易實(shí)現(xiàn)與載體坐標(biāo)系的對(duì)準(zhǔn),這是由于其讀數(shù)直接與傾角(俯仰角與橫滾角)有關(guān)。又由于加速度計(jì)不存在軟硬磁干擾等問(wèn)題,受環(huán)境變化影響相對(duì)較小,因此若借助校正良好的加速度計(jì)作為基準(zhǔn),點(diǎn)積不變法可實(shí)現(xiàn)磁強(qiáng)計(jì)的完全校正[10-13]。

點(diǎn)積不變法可以直接將式(2)代入式(4)求解L與b。但也可先由橢球擬合法得到L與b,再由點(diǎn)積不變法求解非對(duì)準(zhǔn)誤差(即矩陣R),此時(shí)式(4)變?yōu)槭?5)。此即磁強(qiáng)計(jì)的二步校正法[13-15]。

gTRL(v-b)=g0h0cosζ

(5)

注意矩陣R應(yīng)當(dāng)是一個(gè)表示三維旋轉(zhuǎn)的正交矩陣,但若直接采用式(5)而不對(duì)R作任何約束,則易導(dǎo)致R失去正交性,而使校正效果變差[12-13]。

1.4 繞軸旋轉(zhuǎn)法

當(dāng)傳感器原地旋轉(zhuǎn)時(shí),地磁場(chǎng)或重力矢量在旋轉(zhuǎn)軸上的投影也可構(gòu)造出類似式(4)的點(diǎn)積不變量。設(shè)沿旋轉(zhuǎn)軸的單位矢量為w,其與被測(cè)矢量u的夾角為γ,則有

wTu=u0cosγ

(6)

如以載體坐標(biāo)系的坐標(biāo)軸作為旋轉(zhuǎn)軸,或者旋轉(zhuǎn)軸在載體坐標(biāo)系下的指向?yàn)橐阎?則式(6)可用于求解傳感器非對(duì)準(zhǔn)誤差,且不依賴于加速度計(jì)。此方法與點(diǎn)積不變法原理相同,但通常不單獨(dú)使用,而是與橢球擬合法相結(jié)合,構(gòu)成二步法或多步法[16-18]。

2 雙內(nèi)積法

2.1 基于雙內(nèi)積的校正原理

若同時(shí)利用式(3)所述的模值不變性及式(6)所述的點(diǎn)積不變性,便可將橢球擬合法、點(diǎn)積不變法和繞軸旋轉(zhuǎn)法各自的優(yōu)點(diǎn)相結(jié)合,實(shí)現(xiàn)三軸矢量場(chǎng)傳感器的完全校正。由于式(3)及式(6)均屬于向量的內(nèi)積,故此法可稱為“雙內(nèi)積法”。

仍采用式(2)中的誤差模型,并假定共有r組測(cè)量數(shù)據(jù),包括傳感器測(cè)量值{v1,v2,…,vr}以及沿旋轉(zhuǎn)軸的單位矢量{w1,w2,…,wr}。引入式(7)所示的代價(jià)函數(shù)e:

(7)

這樣,三軸矢量場(chǎng)傳感器的校正便轉(zhuǎn)化為求解令‖e‖2=eTe最小化的L與b,二者全部元素共計(jì)12個(gè)參數(shù)。

理論上,式(7)中的u0應(yīng)等于本地的重力加速度(對(duì)于加速度計(jì))或者地磁場(chǎng)強(qiáng)度(對(duì)于磁強(qiáng)計(jì)),但其具體數(shù)值并不影響姿態(tài)解算,故在精確值未知時(shí)亦可用近似值代替。

另一方面,重力矢量或地磁場(chǎng)矢量與旋轉(zhuǎn)軸的夾角γ如不能精確確定,則需將λ=cosγ也列為待估計(jì)參數(shù)。如數(shù)據(jù)采集過(guò)程中有n個(gè)不同的旋轉(zhuǎn)軸取向,則待估計(jì)參數(shù)將有(12+n)個(gè)。

2.2 非線性優(yōu)化算法實(shí)現(xiàn)

根據(jù)式(7)令‖e‖2=eTe最小化從而求解L與b,是一個(gè)非線性優(yōu)化問(wèn)題,可采用最速下降法、共軛梯度法、高斯-牛頓法、Levenberg-Marquardt法(簡(jiǎn)稱L-M法)等算法求解。首先將待估計(jì)參數(shù)排列成向量形式(此處假定有且僅有一個(gè)待估計(jì)的λ=cosγ):

x=(l11l33b1b2b3λ)T

(8)

式中,lij表示矩陣L第i行第j列的元素,bi表示b的第i個(gè)元素。以采用高斯-牛頓法為例,求解過(guò)程如下:

①初始化:令迭代次數(shù)k=0,Lk=I3×3(3階單位陣),bk=03×1(3維零向量),λk=1。

②按式(8)構(gòu)造待估計(jì)參數(shù)向量xk。

③根據(jù)式(7)計(jì)算代價(jià)函數(shù)ek,注意ek為2r維列向量。

④計(jì)算雅可比矩陣Jk=?ek/?xk,參見式(9)～式(11),其中:ek,s為ek的第s個(gè)元素,αs,i表示矢量Lk(vs-bk)的第i個(gè)元素,βs,j表示矢量(vs-bk)的第j個(gè)元素,ws,i為ws的第i個(gè)元素,vs和ws分別是第s組磁強(qiáng)計(jì)讀數(shù)及對(duì)應(yīng)的旋轉(zhuǎn)軸。

(9)

(10)

(11)

⑤根據(jù)式(12)計(jì)算xk+1,進(jìn)而由xk+1得到Lk+1,bk+1和λk+1。

(12)

⑥根據(jù)式(7)計(jì)算ek+1.

⑦若‖ek+1‖2小于給定閾值,或迭代次數(shù)k達(dá)到上限,則結(jié)束;否則k=k+1,轉(zhuǎn)第4步。

3 實(shí)驗(yàn)驗(yàn)證

3.1 實(shí)驗(yàn)條件

在基于MPU9250的航姿模塊上驗(yàn)證本文所述校正方法,并與其他方法進(jìn)行對(duì)比。MPU9250內(nèi)含三軸磁強(qiáng)計(jì)、三軸加速度計(jì)和三軸陀螺儀。該航姿模塊如圖1所示,其坐標(biāo)系是依外殼的幾何外形來(lái)定義,已在圖1中標(biāo)明。

圖1 實(shí)驗(yàn)所用航姿模塊及其坐標(biāo)系定義

采用角度分辨率為0.1°的三自由度手動(dòng)轉(zhuǎn)臺(tái)提供三維姿態(tài)基準(zhǔn),用于檢驗(yàn)校正前后航姿模塊的角度誤差。

實(shí)驗(yàn)中,將航姿模塊的外殼表面緊貼于豎直墻面上,通過(guò)繞軸旋轉(zhuǎn)的方式采集傳感器原始數(shù)據(jù),具體步驟為:①外殼側(cè)面靠墻,使y軸水平指北,并繞該軸旋轉(zhuǎn)一圈;②外殼底面靠墻,使z軸水平指北,并繞該軸旋轉(zhuǎn)一圈;③外殼頂面靠墻,使z軸水平指南,并繞該軸旋轉(zhuǎn)一圈。

旋轉(zhuǎn)過(guò)程需保持平穩(wěn)緩慢,以避免干擾加速度計(jì)讀數(shù)。注意以上3個(gè)步驟中,沿轉(zhuǎn)軸的單位矢量w在載體坐標(biāo)系中依次表示為(0 1 0)T,(0 0 1)T,(0 0 -1)T。

3.2 加速度計(jì)校正實(shí)驗(yàn)

圖2為按3.1節(jié)所述步驟采集的加速度計(jì)數(shù)據(jù),分別采用以下3種方法對(duì)加速度計(jì)進(jìn)行校正:①只用橢球擬合法校正;②先用橢球擬合法校正,再用繞軸旋轉(zhuǎn)法修正非對(duì)準(zhǔn)誤差;③采用本文所述方法即雙內(nèi)積法。

圖2 加速度計(jì)采樣數(shù)據(jù)(單位:m/s2)

圖3 校正前后俯仰角誤差

實(shí)驗(yàn)中,采用上述方法先后對(duì)5個(gè)航姿模塊中的加速度計(jì)進(jìn)行了校正,并利用三軸轉(zhuǎn)臺(tái)檢驗(yàn)了校正前后的俯仰角和橫滾角誤差(包括平均誤差和均方根誤差),結(jié)果分別如圖3和圖4所示。實(shí)驗(yàn)結(jié)果中,各姿態(tài)角的均方根誤差反映誤差的波動(dòng)幅度,各姿態(tài)角的平均誤差則主要來(lái)自加速度計(jì)各軸的非對(duì)準(zhǔn)誤差。由圖3和圖4可見,雙內(nèi)積法可使這兩項(xiàng)指標(biāo)均得到明顯改善,且效果優(yōu)于其他校正方法。

圖4 校正前后橫滾角誤差

3.3 磁強(qiáng)計(jì)校正實(shí)驗(yàn)

圖5為按3.1節(jié)所述步驟采集的磁強(qiáng)計(jì)數(shù)據(jù)。先對(duì)各航姿模塊的加速度計(jì)進(jìn)行校正,再分別采用如下4種方法對(duì)其磁強(qiáng)計(jì)進(jìn)行校正:①橢球擬合法;②點(diǎn)積不變法;③先用橢球擬合法校正,再用繞軸旋轉(zhuǎn)法修正非對(duì)準(zhǔn)誤差;④雙內(nèi)積法。

圖5 磁強(qiáng)計(jì)采樣數(shù)據(jù)(單位:μT)

利用三軸轉(zhuǎn)臺(tái)檢驗(yàn)校正前后的航向角誤差(包括平均誤差及均方根誤差),結(jié)果如圖6所示。

圖6 校正前后航向角誤差

由圖6可見,除橢球擬合法校正后誤差較大外,其余校正方法效果較為接近,但仍以雙內(nèi)積法精度為最高。

3.4 討論

在對(duì)三軸加速度計(jì)和磁強(qiáng)計(jì)進(jìn)行誤差校正和補(bǔ)償時(shí),采樣數(shù)據(jù)的充分性是影響校正效果的重要因素。在實(shí)際應(yīng)用場(chǎng)合,傳感器數(shù)據(jù)采集過(guò)程可能會(huì)受到航姿系統(tǒng)安裝條件及載體運(yùn)動(dòng)約束,導(dǎo)致采樣數(shù)據(jù)集中分布在較窄的范圍內(nèi),這將使校正算法的誤差增大甚至不能求解。為模擬此類情況,設(shè)計(jì)了另一個(gè)磁強(qiáng)計(jì)校正實(shí)驗(yàn),其數(shù)據(jù)采集步驟如下:①航姿模塊置于水平面上并旋轉(zhuǎn)一圈;②航姿模塊置于向北傾斜約15°的平面上,并旋轉(zhuǎn)一圈;③航姿模塊置于向南傾斜約15°的平面上,并旋轉(zhuǎn)一圈。

圖7 磁強(qiáng)計(jì)采樣數(shù)據(jù)(單位:μT)

以上三個(gè)步驟中,旋轉(zhuǎn)軸均為圖1中的z軸,用以模擬載體只能在水平面或小傾角的斜面上運(yùn)動(dòng)的情況。采集到的磁強(qiáng)計(jì)數(shù)據(jù)如圖7所示。與圖5相比,可見圖7中數(shù)據(jù)點(diǎn)的分布范圍明顯較窄,這將不利于校正算法的求解。

分別采用3.3節(jié)中所述各方法對(duì)磁強(qiáng)計(jì)進(jìn)行校正,并驗(yàn)證校正前后的航向角誤差。航向角的均方根誤差和平均誤差分別如表1和表2所示。

表1 航向角均方根誤差 單位:(°)

表2 航向角平均誤差 單位:(°)

觀察表1和表2,首先可以看到這兩個(gè)表均未給出繞軸旋轉(zhuǎn)法的結(jié)果。其原因是圖8所示采樣數(shù)據(jù)不足以求解式(5)中的非對(duì)準(zhǔn)誤差矩陣R。實(shí)際上,圖7中的數(shù)據(jù)均是在繞航姿模塊z軸旋轉(zhuǎn)的過(guò)程中采集的,從數(shù)學(xué)角度而言,矩陣R中包含的繞z軸的非對(duì)準(zhǔn)誤差無(wú)法與采樣過(guò)程中繞z軸的旋轉(zhuǎn)操作相區(qū)分,從而導(dǎo)致非對(duì)準(zhǔn)誤差矩陣R無(wú)法準(zhǔn)確求解。

圖8 5#航姿模塊校正前后航向角誤差曲線

其次值得注意的是,點(diǎn)積不變法校正后,航向角平均誤差很大,但均方根誤差不大。其原因同樣是由于繞z軸的非對(duì)準(zhǔn)誤差無(wú)法求出,使得校正后航向角出現(xiàn)較大固定偏差。在圖8給出的5#航姿模塊的航向角誤差曲線中可清楚地看到,經(jīng)過(guò)點(diǎn)積不變法校正后,航向角誤差曲線的波動(dòng)幅度雖然減小,但卻存在大約30°的固定偏差。相比之下,雙內(nèi)積法始終具有良好的校正效果。雖然雙內(nèi)積法在形式上相當(dāng)于將橢球擬合法+繞軸旋轉(zhuǎn)法的兩步校正合并為一步完成,但由于雙內(nèi)積法求解過(guò)程中同時(shí)利用了兩個(gè)不變性(矢量場(chǎng)強(qiáng)度不變及其在旋轉(zhuǎn)軸上投影不變)的約束,改善了算法的魯棒性,同時(shí)也降低了對(duì)數(shù)據(jù)采集過(guò)程的要求。上述實(shí)驗(yàn)僅利用航姿模塊的外殼以及平整的墻面、臺(tái)面即可完成對(duì)加速度計(jì)與磁強(qiáng)計(jì)的校正,簡(jiǎn)便易行。

4 結(jié)論

本文提出了用于校正航姿系統(tǒng)中三軸磁強(qiáng)計(jì)與加速度計(jì)的雙內(nèi)積法,該方法結(jié)合了常用的橢球擬合法、點(diǎn)積不變法和繞軸旋轉(zhuǎn)法的優(yōu)點(diǎn),能解決傳感器的非對(duì)準(zhǔn)誤差,且校正過(guò)程操作簡(jiǎn)便,校正效果良好、可靠,能明顯改善航姿系統(tǒng)精度。