航姿系統矢量場傳感器校正的雙內積法*

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

(1.桂林電子科技大學電子工程與自動化學院,桂林 541004;2.桂林航天工業學院無人機遙測重點實驗室,桂林 541004)

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

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

針對上述問題,本文提出一種基于多內積的校正方法。該方法既利用地磁場和重力矢量自身的模值不變性,又利用傳感器轉動時地磁場和重力矢量在旋轉軸上的投影不變性,可實現三軸磁強計和加速度計的完全校正。在航姿模塊上對該方法及其他常用校正方法進行了對比實驗,驗證了該方法的有效性。

1 校正原理

1.1 誤差模型

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

v=Ku+b+ε

(1)

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

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

u=L(v-b)

(2)

1.2 橢球擬合法

航姿系統及其載體作原地轉動時,載體坐標系中地磁場和重力矢量的方向發生變化,但矢量模值保持不變,這一性質可由式(3)描述。

(3)

式中:u0表示被測矢量u的模(即地磁場強度或重力加速度)。將式(2)代入式(3)即可得到關于L與b的方程,進而可根據傳感器原始數據利用各種非線性擬合或優化算法求解L與b. 由于式(3)表示的是一個標量,故基于式(3)的校正方法也稱為標量校正法(scalar calibration)。另一方面,采用式(2)進行誤差補償的前提是式(1)中的誤差矩陣K可逆(即L=K-1存在)。此時,將式(2)代入式(3)所得關于u的二次型必為正定二次型,其對應的幾何圖形為一橢球面。因此,凡基于式(3)的校正方法,不論其是否顯式地求解橢球的中心與主軸等參數,數學上均等價于對橢球面進行擬合,故此類方法又統稱為橢球擬合法。

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

1.3 點積不變法

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

gTh=g0h0cosζ

(4)

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

相對而言,加速度計較易實現與載體坐標系的對準,這是由于其讀數直接與傾角(俯仰角與橫滾角)有關。又由于加速度計不存在軟硬磁干擾等問題,受環境變化影響相對較小,因此若借助校正良好的加速度計作為基準,點積不變法可實現磁強計的完全校正[10-13]。

點積不變法可以直接將式(2)代入式(4)求解L與b。但也可先由橢球擬合法得到L與b,再由點積不變法求解非對準誤差(即矩陣R),此時式(4)變為式(5)。此即磁強計的二步校正法[13-15]。

gTRL(v-b)=g0h0cosζ

(5)

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

1.4 繞軸旋轉法

當傳感器原地旋轉時,地磁場或重力矢量在旋轉軸上的投影也可構造出類似式(4)的點積不變量。設沿旋轉軸的單位矢量為w,其與被測矢量u的夾角為γ,則有

wTu=u0cosγ

(6)

如以載體坐標系的坐標軸作為旋轉軸,或者旋轉軸在載體坐標系下的指向為已知,則式(6)可用于求解傳感器非對準誤差,且不依賴于加速度計。此方法與點積不變法原理相同,但通常不單獨使用,而是與橢球擬合法相結合,構成二步法或多步法[16-18]。

2 雙內積法

2.1 基于雙內積的校正原理

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

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

(7)

這樣,三軸矢量場傳感器的校正便轉化為求解令‖e‖2=eTe最小化的L與b,二者全部元素共計12個參數。

理論上,式(7)中的u0應等于本地的重力加速度(對于加速度計)或者地磁場強度(對于磁強計),但其具體數值并不影響姿態解算,故在精確值未知時亦可用近似值代替。

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

2.2 非線性優化算法實現

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

x=(l11l33b1b2b3λ)T

(8)

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

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

②按式(8)構造待估計參數向量xk。

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

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

(9)

(10)

(11)

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

(12)

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

⑦若‖ek+1‖2小于給定閾值,或迭代次數k達到上限,則結束;否則k=k+1,轉第4步。

3 實驗驗證

3.1 實驗條件

在基于MPU9250的航姿模塊上驗證本文所述校正方法,并與其他方法進行對比。MPU9250內含三軸磁強計、三軸加速度計和三軸陀螺儀。該航姿模塊如圖1所示,其坐標系是依外殼的幾何外形來定義,已在圖1中標明。

圖1 實驗所用航姿模塊及其坐標系定義

采用角度分辨率為0.1°的三自由度手動轉臺提供三維姿態基準,用于檢驗校正前后航姿模塊的角度誤差。

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

旋轉過程需保持平穩緩慢,以避免干擾加速度計讀數。注意以上3個步驟中,沿轉軸的單位矢量w在載體坐標系中依次表示為(0 1 0)T,(0 0 1)T,(0 0 -1)T。

3.2 加速度計校正實驗

圖2為按3.1節所述步驟采集的加速度計數據,分別采用以下3種方法對加速度計進行校正:①只用橢球擬合法校正;②先用橢球擬合法校正,再用繞軸旋轉法修正非對準誤差;③采用本文所述方法即雙內積法。

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

圖3 校正前后俯仰角誤差

實驗中,采用上述方法先后對5個航姿模塊中的加速度計進行了校正,并利用三軸轉臺檢驗了校正前后的俯仰角和橫滾角誤差(包括平均誤差和均方根誤差),結果分別如圖3和圖4所示。實驗結果中,各姿態角的均方根誤差反映誤差的波動幅度,各姿態角的平均誤差則主要來自加速度計各軸的非對準誤差。由圖3和圖4可見,雙內積法可使這兩項指標均得到明顯改善,且效果優于其他校正方法。

圖4 校正前后橫滾角誤差

3.3 磁強計校正實驗

圖5為按3.1節所述步驟采集的磁強計數據。先對各航姿模塊的加速度計進行校正,再分別采用如下4種方法對其磁強計進行校正:①橢球擬合法;②點積不變法;③先用橢球擬合法校正,再用繞軸旋轉法修正非對準誤差;④雙內積法。

圖5 磁強計采樣數據(單位:μT)

利用三軸轉臺檢驗校正前后的航向角誤差(包括平均誤差及均方根誤差),結果如圖6所示。

圖6 校正前后航向角誤差

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

3.4 討論

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

圖7 磁強計采樣數據(單位:μT)

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

分別采用3.3節中所述各方法對磁強計進行校正,并驗證校正前后的航向角誤差。航向角的均方根誤差和平均誤差分別如表1和表2所示。

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

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

觀察表1和表2,首先可以看到這兩個表均未給出繞軸旋轉法的結果。其原因是圖8所示采樣數據不足以求解式(5)中的非對準誤差矩陣R。實際上,圖7中的數據均是在繞航姿模塊z軸旋轉的過程中采集的,從數學角度而言,矩陣R中包含的繞z軸的非對準誤差無法與采樣過程中繞z軸的旋轉操作相區分,從而導致非對準誤差矩陣R無法準確求解。

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

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

4 結論

本文提出了用于校正航姿系統中三軸磁強計與加速度計的雙內積法,該方法結合了常用的橢球擬合法、點積不變法和繞軸旋轉法的優點,能解決傳感器的非對準誤差,且校正過程操作簡便,校正效果良好、可靠,能明顯改善航姿系統精度。