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

李 翔，劉耀華，宋百麒

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

0 引言

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

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

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

1 傳感器誤差模型

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

(1)

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

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

u=L(v-b)

(2)

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

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

2 橢球擬合法概述

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

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

(3)

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

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

(4)

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

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

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

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

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

u=RL(v-b)

(5)

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

gTRh=g0h0cosγ

(6)

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

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

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

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

eTu=eTRL(v-b)=u0cosλ

(7)

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

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

2) 旋轉(zhuǎn)軸e一旦選定，數(shù)據(jù)采集過程中應(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)動1周，采集到的磁強(qiáng)計和加速度計讀數(shù)便可滿足以上要求。且按上述要求采集的數(shù)據(jù)也適用于橢球擬合法，亦即兩步校正可共用同一組數(shù)據(jù)集而無需再次采樣。

4) 旋轉(zhuǎn)軸e需以載體坐標(biāo)系下的分量形式代入式⑺，方能實現(xiàn)傳感器與載體坐標(biāo)系之間非對準(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個元素，但由于正交性的約束，實際只有3個獨(dú)立參數(shù)，即分別繞3個坐標(biāo)軸的旋轉(zhuǎn)角度δ1、δ2和δ3。又考慮到非正交誤差通常較小，故可采用下式所示的近似形式以簡化計算。

(8)

5 實物驗證

5.1 實驗器材

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

圖1 航姿模塊

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

實驗中，采集磁強(qiáng)計和加速度計讀數(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。

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

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

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

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

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

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

5.3 實驗結(jié)果

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

圖3 航向角誤差曲線

圖4 俯仰角誤差曲線

圖5 橫滾角誤差曲線

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

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

續(xù)表

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

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

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

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

6 結(jié)束語

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