航姿系統(tǒng)矢量場(chǎng)傳感器的完全校正

李 翔，劉耀華，宋百麒

(桂林電子科技大學(xué) 電子工程與自動(dòng)化學(xué)院，廣西 桂林 541004)

0 引言

三軸磁強(qiáng)計(jì)與三軸加速度計(jì)廣泛應(yīng)用于航姿系統(tǒng)(AHRS)以及電子羅盤中，通過(guò)測(cè)量地磁矢量和重力矢量可實(shí)現(xiàn)姿態(tài)與方位(航向)的解算。然而由于制造工藝,安裝誤差及環(huán)境因素等影響，三軸磁強(qiáng)計(jì)與加速度計(jì)在使用中必須進(jìn)行誤差校正與補(bǔ)償，以保證航姿系統(tǒng)的精度[1]。

目前，航姿系統(tǒng)中的磁強(qiáng)計(jì)和加速度計(jì)校正普遍采用基于橢球擬合的校正方法[2-6]，該方法只利用磁強(qiáng)計(jì)和加速度計(jì)的原始測(cè)量數(shù)據(jù)即可實(shí)現(xiàn)校正，便于使用者在各種應(yīng)用場(chǎng)合進(jìn)行快速、便捷的現(xiàn)場(chǎng)校正[7-10]。然而，橢球擬合法校正后的傳感器坐標(biāo)系不能保證與載體坐標(biāo)系相重合[11-15]，這將對(duì)航姿系統(tǒng)造成不可忽視的影響。

本文在對(duì)橢球擬合法的原理及其缺點(diǎn)進(jìn)行理論分析的基礎(chǔ)上，提出一種補(bǔ)償傳感器非對(duì)準(zhǔn)誤差的改進(jìn)方法。實(shí)驗(yàn)表明，該方法能實(shí)現(xiàn)磁強(qiáng)計(jì)與加速度計(jì)的完全校正，減小地磁和重力矢量的測(cè)量誤差，改善航姿系統(tǒng)的精度。

1 傳感器誤差模型

三軸磁強(qiáng)計(jì)及三軸加速度計(jì)通常采用的誤差模型：

(1)

式中:三維矢量u=(u1u2u3)T表示被測(cè)矢量(地磁矢量或重力矢量)；三維矢量v=(v1v2v3)T代表傳感器輸出；3×3矩陣K=(kij)3×3用于描述靈敏度(標(biāo)度系數(shù))誤差、非正交誤差、安裝誤差及磁強(qiáng)計(jì)受到的軟磁干擾等；噪聲項(xiàng)ε一般假定為零均值高斯白噪聲。

式(1)給出的線性誤差模型能滿足一般用途的精度要求。若忽略式(1)中的噪聲項(xiàng)并記L=K-1，則有

u=L(v-b)

(2)

式中矢量b=(b1b2b3)T描述傳感器的常值誤差(零偏)，對(duì)磁強(qiáng)計(jì)而言b還包含硬磁干擾。

由式(2)即可實(shí)現(xiàn)三軸磁強(qiáng)計(jì)或加速度計(jì)的誤差補(bǔ)償。

2 橢球擬合法概述

任一固定地點(diǎn)的地磁場(chǎng)強(qiáng)度和重力加速度在短時(shí)間內(nèi)可認(rèn)為保持不變，因而地磁矢量及重力矢量的模值均為常數(shù)，這一性質(zhì)可用下式來(lái)描述：

‖u‖=‖L(v-b)‖=u0

(3)

式中u0為矢量u的2-范數(shù)即歐氏范數(shù)。對(duì)式(3)取平方，可得到一個(gè)關(guān)于矢量(v-b)的二次型：

(v-b)TLTL(v-b)=‖u‖2

(4)

引入式(2)時(shí)已假定了K可逆，則L=K-1亦可逆，從而LTL對(duì)稱正定，即式(4)是一個(gè)代表橢球面的二次型。因此，凡是基于式(4)的校正方法，無(wú)論采取何種具體算法，本質(zhì)上都是在擬合該式所代表的橢球面，故統(tǒng)稱為橢球擬合法。

由式(4)可知，橢球擬合法只需利用傳感器自身讀數(shù)，完全不需要傳感器載體的姿態(tài)信息。故橢球擬合法的最大優(yōu)點(diǎn)是不需外部設(shè)備或基準(zhǔn)即可實(shí)施校正。橢球擬合法的另一優(yōu)點(diǎn)是能使被測(cè)矢量的模值誤差最小化，這是由式(4)的本質(zhì)決定的。

然而，由于橢球擬合法在校正過(guò)程中完全忽略了三維姿態(tài)信息，因此不能保證傳感器坐標(biāo)系與載體坐標(biāo)系重合，兩坐標(biāo)系間通常存在未完全補(bǔ)償?shù)姆菍?duì)準(zhǔn)誤差，從而導(dǎo)致被測(cè)矢量出現(xiàn)指向誤差，影響航姿系統(tǒng)的精度。

3 非對(duì)準(zhǔn)誤差及其校正

三軸磁強(qiáng)計(jì)或加速度計(jì)的非對(duì)準(zhǔn)誤差可用一個(gè)三階正交矩陣R描述，該矩陣的幾何意義是從傳感器坐標(biāo)系到載體坐標(biāo)系的三維旋轉(zhuǎn)變換。引入矩陣R后，按下式進(jìn)行傳感器誤差補(bǔ)償。

u=RL(v-b)

(5)

目前采用較多的求取R的方法是利用地磁矢量與重力矢量的夾角[12-15]。若磁強(qiáng)計(jì)與加速度計(jì)均已由橢球擬合法校正，并記二者讀數(shù)分別為h與g(均為三維矢量)，由于地磁矢量與重力矢量夾角恒定(記為γ)，二者的點(diǎn)積應(yīng)為常數(shù)，即

gTRh=g0h0cosγ

(6)

式中g(shù)0，h0分別為本地重力加速度與地磁場(chǎng)強(qiáng)度。由式(6)可求解R，稱為“常數(shù)夾角法”[12]。但此方法僅能求出磁強(qiáng)計(jì)坐標(biāo)系到加速度計(jì)坐標(biāo)系的正交矩陣，亦即只能實(shí)現(xiàn)兩傳感器之間的對(duì)準(zhǔn)，而不能補(bǔ)償傳感器與載體坐標(biāo)系之間的非對(duì)準(zhǔn)誤差。

4 改進(jìn)的校正方法

采用二步法實(shí)現(xiàn)三軸磁強(qiáng)計(jì)及加速度計(jì)的完全校正，即先用橢球擬合法確定式⑵中的L與b，再求解正交矩陣R。以下重點(diǎn)討論R的求法。

假定傳感器已由橢球擬合法校正，將其繞某一固定軸轉(zhuǎn)動(dòng)時(shí)，被測(cè)矢量u在該轉(zhuǎn)軸上的投影應(yīng)保持不變。記沿該轉(zhuǎn)軸方向的單位矢量為e，u與e夾角為λ，則有

eTu=eTRL(v-b)=u0cosλ

(7)

利用式⑺即可求解矩陣R，從而補(bǔ)償傳感器非對(duì)準(zhǔn)誤差，但需注意以下幾點(diǎn)：

1) 旋轉(zhuǎn)軸e不應(yīng)與被測(cè)矢量u平行，否則旋轉(zhuǎn)過(guò)程中傳感器讀數(shù)基本無(wú)變化，無(wú)法求解矩陣R。

2) 旋轉(zhuǎn)軸e一旦選定，數(shù)據(jù)采集過(guò)程中應(yīng)保持其指向不變，以保證式(7)成立。

3) 為保證采樣數(shù)據(jù)的充分性[16-17]，應(yīng)將傳感器置于至少3種不同姿態(tài)下分別繞轉(zhuǎn)軸e旋轉(zhuǎn)。如依次將航姿系統(tǒng)的x、y、z軸指向正東，并分別繞軸轉(zhuǎn)動(dòng)1周，采集到的磁強(qiáng)計(jì)和加速度計(jì)讀數(shù)便可滿足以上要求。且按上述要求采集的數(shù)據(jù)也適用于橢球擬合法，亦即兩步校正可共用同一組數(shù)據(jù)集而無(wú)需再次采樣。

4) 旋轉(zhuǎn)軸e需以載體坐標(biāo)系下的分量形式代入式⑺，方能實(shí)現(xiàn)傳感器與載體坐標(biāo)系之間非對(duì)準(zhǔn)誤差的補(bǔ)償。如按3)所述方式采集的數(shù)據(jù)，轉(zhuǎn)軸e在載體坐標(biāo)系中的指向依次為e1=(1 0 0)T，e2=(0 1 0)T，e3=(0 0 1)T。

5) 矩陣R雖含9個(gè)元素，但由于正交性的約束，實(shí)際只有3個(gè)獨(dú)立參數(shù)，即分別繞3個(gè)坐標(biāo)軸的旋轉(zhuǎn)角度δ1、δ2和δ3。又考慮到非正交誤差通常較小，故可采用下式所示的近似形式以簡(jiǎn)化計(jì)算。

(8)

5 實(shí)物驗(yàn)證

5.1 實(shí)驗(yàn)器材

采用圖1所示基于MPU9250芯片的航姿模塊驗(yàn)證本文提出的校正方法。MPU9250芯片內(nèi)部均含加速度計(jì)、磁強(qiáng)計(jì)和陀螺儀3種傳感器。航姿模塊的x、y、z軸定義已在圖1中標(biāo)出。

圖1 航姿模塊

5.2 數(shù)據(jù)采集與校正

實(shí)驗(yàn)中，采集磁強(qiáng)計(jì)和加速度計(jì)讀數(shù)的步驟為：

1) 航姿模塊y軸指北并繞其旋轉(zhuǎn)1周。

2) 航姿模塊z軸指北并繞其旋轉(zhuǎn)1周。

3) 航姿模塊z軸指南并繞其旋轉(zhuǎn)1周。

在這3種姿態(tài)下，轉(zhuǎn)軸e在載體坐標(biāo)系中的指向依次為e1=(0 1 0)T，e2=(0 0 1)T，e3=(0 0 -1)T。

上述操作過(guò)程無(wú)需專門設(shè)備，只需將圖1所示航姿模塊的側(cè)面、底面和頂面分別緊貼在同一直立平整墻面上并旋轉(zhuǎn)即可，對(duì)墻面的朝向無(wú)嚴(yán)格要求。采集到的三維地磁數(shù)據(jù)如圖2所示。

圖2 地磁場(chǎng)采樣數(shù)據(jù)

完成數(shù)據(jù)采集后，分別采用下列3種方法對(duì)磁強(qiáng)計(jì)和加速度計(jì)進(jìn)行校正：

方法1：磁強(qiáng)計(jì)和加速度計(jì)各自采用橢球擬合法進(jìn)行校正。

方法2：先用橢球擬合法分別校正磁強(qiáng)計(jì)和加速度計(jì)，再對(duì)磁強(qiáng)計(jì)采用常數(shù)夾角法。

方法3：采用第4節(jié)所述二步法，分別對(duì)磁強(qiáng)計(jì)和加速度計(jì)進(jìn)行校正。

5.3 實(shí)驗(yàn)結(jié)果

分別采用上述3種方法對(duì)航姿模塊進(jìn)行校正后，再利用三軸轉(zhuǎn)臺(tái)(角度分辨率0.1°)檢驗(yàn)該模塊輸出的航向角、俯仰角及橫滾角的準(zhǔn)確性。圖3～5分別為航向角、俯仰角、橫滾角的誤差曲線。

圖3 航向角誤差曲線

圖4 俯仰角誤差曲線

圖5 橫滾角誤差曲線

由圖3～5可見(jiàn)，本文所介紹的校正方法能在橢球擬合法的基礎(chǔ)上進(jìn)一步提高航姿系統(tǒng)的精度。通過(guò)表1給出的校正前、后的角度誤差統(tǒng)計(jì)結(jié)果可更明顯地看出這一點(diǎn)。

表1 角度誤差統(tǒng)計(jì)

續(xù)表

校正方法未校正方法1方法2方法3橫滾角平均誤差/(°)-0.27-1.97-1.970.22均方根誤差/(°)2.090.240.240.24

表1中所列均方根誤差用于評(píng)價(jià)角度數(shù)據(jù)的均勻性，而平均誤差則主要反映由傳感器非對(duì)準(zhǔn)誤差引起的角度固定偏差。

由表1可見(jiàn)，校正方法1(橢球擬合法)和方法2(常數(shù)夾角法)具有較小的均方根誤差，亦即這兩種方法能改善角度的均勻性。然而角度的平均誤差相對(duì)于校正前反而有所增大，表明校正后仍存在非對(duì)準(zhǔn)誤差導(dǎo)致的固定偏差，這與圖3～5所示誤差曲線相吻合。

與校正方法1、2相比，校正方法3(即本文方法)的均方根誤差及平均誤差都較小，表明傳感器的非對(duì)準(zhǔn)誤差得到了較好的補(bǔ)償。

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

針對(duì)橢球擬合法校正后遺留的傳感器非對(duì)準(zhǔn)誤差，本文提出的改進(jìn)校正方法能對(duì)其進(jìn)行較好的補(bǔ)償，從而實(shí)現(xiàn)三軸磁強(qiáng)計(jì)和加速度計(jì)的完全校正，且校正過(guò)程操作簡(jiǎn)便。實(shí)驗(yàn)結(jié)果表明，采用本文方法校正后，航姿系統(tǒng)的角度平均誤差及均方根誤差均能減小至1°以內(nèi)，可顯著提高其精度。