基于增強(qiáng)型顯式互補(bǔ)濾波的無(wú)人機(jī)姿態(tài)算法

馬 力， 李天松， 陽(yáng)榮凱， 黃艷虎

(1.桂林電子科技大學(xué) 信息與通信學(xué)院，廣西 桂林 541004； 2.玉林師范學(xué)院 電子與通信工程學(xué)院,廣西 玉林 537000)

微機(jī)電系統(tǒng)(micro-electro-mechanical system，簡(jiǎn)稱MEMS)傳感器因體積小、經(jīng)濟(jì)實(shí)惠、精度適中等優(yōu)點(diǎn)被廣泛應(yīng)用于無(wú)人機(jī)捷聯(lián)式慣性導(dǎo)航系統(tǒng)。慣導(dǎo)系統(tǒng)啟動(dòng)后，機(jī)體的初始姿態(tài)未知，利用有效信息解算得到初始姿態(tài)矩陣的過(guò)程稱為慣導(dǎo)系統(tǒng)的初始對(duì)準(zhǔn)?，F(xiàn)有的對(duì)準(zhǔn)方法分為靜基座和動(dòng)基座對(duì)準(zhǔn)。靜基座精對(duì)準(zhǔn)是先進(jìn)行解析式粗對(duì)準(zhǔn)，再運(yùn)用卡爾曼濾波估計(jì)初始誤差角及傳感器誤差進(jìn)行修正對(duì)準(zhǔn)[1]。動(dòng)基座對(duì)準(zhǔn)實(shí)時(shí)記錄載體的姿態(tài)變化，結(jié)合初始姿態(tài)的最優(yōu)估計(jì)值，進(jìn)而得到導(dǎo)航前一刻的姿態(tài)[2]。然而，MEMS傳感器的精度不夠高，并不適用現(xiàn)有的對(duì)準(zhǔn)方法。采用四元數(shù)方法更新姿態(tài)，四元數(shù)初始值由捷聯(lián)慣導(dǎo)的初始對(duì)準(zhǔn)確定[3-4]，一些開(kāi)源項(xiàng)目或文獻(xiàn)對(duì)四元數(shù)初值賦予[1,0,0,0]T[5-6]，四元數(shù)微分方程初值不夠精確，導(dǎo)致后續(xù)解算精度降低。文獻(xiàn)[7-9]使用擴(kuò)展卡爾曼濾波、無(wú)跡卡爾曼濾波和粒子濾波進(jìn)行九軸數(shù)據(jù)融合，但是這幾種算法計(jì)算量較大，流程復(fù)雜，工程應(yīng)用較為困難。ECF算法[10]利用加速度計(jì)測(cè)量地球重力與通過(guò)姿態(tài)矩陣得到的地球重力作向量叉乘，得到誤差通過(guò)PI運(yùn)算修正陀螺儀x、y軸角速度，缺點(diǎn)是未修正z軸角速度，即未修正航向角。為此，在ECF算法基礎(chǔ)上，利用磁強(qiáng)計(jì)測(cè)量的地磁場(chǎng)強(qiáng)度信息來(lái)修正陀螺儀z軸角速度，能有效提高航向角估計(jì)的精度，得到的航向角不存在漂移現(xiàn)象。

1 系統(tǒng)設(shè)計(jì)

系統(tǒng)采用STM32F407處理器為控制核心，GY-86模塊包含慣性敏感元件MPU6050(加速度計(jì)和陀螺儀)和高精度磁強(qiáng)計(jì)HMC5883。MPU6050最高可測(cè)量±16gn(gn為重力加速度)，±2000°/s的角速度，具有良好的動(dòng)態(tài)響應(yīng)特性。HMC5883采用霍尼韋爾各向異性磁阻技術(shù)，具備高靈敏度和高可靠性，測(cè)量范圍達(dá)±8.1 Gs，用于輔助測(cè)量航向角。通過(guò)I2C總線與主控通信，速率可達(dá)400 kHz/s。初始對(duì)準(zhǔn)與姿態(tài)解算系統(tǒng)框圖如圖1所示。

圖1 初始對(duì)準(zhǔn)與姿態(tài)解算系統(tǒng)框圖

2 初始對(duì)準(zhǔn)

導(dǎo)航常用3個(gè)坐標(biāo)系。1)機(jī)體坐標(biāo)系(b系，xbybzb軸)：原點(diǎn)位于飛行器重心，xb沿飛機(jī)橫軸指右，yb軸沿飛機(jī)縱軸指前，zb軸沿飛機(jī)豎軸，xbybzb構(gòu)成右手直角坐標(biāo)系，此坐標(biāo)系與IMU本體系s重合。2)東北天地理坐標(biāo)系(n系，ENU軸)：原點(diǎn)位于飛行器重心，E軸指東，N軸指北，U軸和重力g方向相反，取此坐標(biāo)系為導(dǎo)航坐標(biāo)系。3)地心慣性坐標(biāo)系(i系，xiyizi軸)：原點(diǎn)位于地球中心，xi、yi在赤道平面內(nèi)，指向恒星方向，xiyizi構(gòu)成右手坐標(biāo)系。

加速度計(jì)測(cè)量的是比力。若將加速度計(jì)平放在地面上，則其受慣性力為0，在豎直方向受到重力產(chǎn)生重力加速度g，忽略物體繞地球產(chǎn)生的向心加速度，則z軸加速度計(jì)的輸入量為-g，“-”號(hào)表示豎直向下，x、y軸的輸入量為0。在四軸飛行條件下，假設(shè)加速度計(jì)只測(cè)量重力加速度，忽略其他瞬時(shí)加速度。對(duì)于MPU6050加速度計(jì)，有以下關(guān)系式成立：

A=AADC/SA。

(1)

其中：AADC為加速度計(jì)ADC輸出；SA為靈敏度；A為機(jī)體加速度，在四軸飛行條件下，也就是重力加速度在機(jī)體各軸的分量。

陀螺儀角速度關(guān)系式為

(2)

為了得到初始四元數(shù)，利用加速度計(jì)求俯仰角、橫滾角，磁強(qiáng)計(jì)求航向角，并分別與陀螺儀融合得到精度相對(duì)較高的姿態(tài)角。對(duì)于三軸加速度計(jì)，設(shè)α1為加速度計(jì)x軸相對(duì)地平面的角度，α2為加速度計(jì)y軸相對(duì)地平面的角度，α3為加速度計(jì)z軸相對(duì)重力方向的角度[11]，有

(3)

其中：Ax、Ay、Az為加速度計(jì)3個(gè)方向的ADC輸出。|α1|與|α3|相等，其意義為偏離程度相同。

按照姿態(tài)角的定義：俯仰角(pitch)為機(jī)體yb軸與地平面間的夾角，以飛行器抬頭為正；橫滾角(roll)為機(jī)體zb軸與包含機(jī)體yb軸的鉛垂面間的夾角，以飛行器向右傾斜為正；航向角(yaw)為機(jī)體yb軸在地平面上的投影與N軸間的夾角，以機(jī)頭右偏為正。由于s系與b系重合，有

θacc=α2，γacc=α1，

(4)

其中θacc為俯仰角，γacc為橫滾角。

可由磁強(qiáng)計(jì)測(cè)得航向角，通過(guò)θ與γ對(duì)磁強(qiáng)計(jì)進(jìn)行傾斜補(bǔ)償[12]，在b系下，

(5)

其中：Mx、My、Mz為磁強(qiáng)計(jì)ADC輸出；Hx、Hy為傾斜的磁強(qiáng)計(jì)回歸水平方向的ADC輸出修正值；ψ為航向角。在機(jī)體受到晃動(dòng)干擾的情況下，單獨(dú)使用式(4)、(5)計(jì)算精度較低，必須濾除干擾。為此，對(duì)加速度計(jì)ADC輸出值進(jìn)行滑動(dòng)平均濾波，再用式(4)、(5)進(jìn)行計(jì)算。對(duì)于仍存在的高頻干擾，使用傳統(tǒng)的互補(bǔ)濾波方法去除。

(6)

θgyro=ω·dt+θgyro。

(7)

對(duì)于θgyro初始值，用式(4)得到的角度代入。由于加速度計(jì)、磁強(qiáng)計(jì)具有高頻噪聲，通過(guò)上述方法求姿態(tài)角，雖然每次得出的結(jié)果精度稍低，但不會(huì)隨時(shí)間產(chǎn)生累積誤差；陀螺儀具有低頻噪聲，每次的角速度輸出精度較高，但角度由于是積分得到，從而不斷形成累積誤差。為抑制陀螺儀積分累積誤差、濾除加速度計(jì)和磁強(qiáng)計(jì)輸出中的高頻干擾，通過(guò)一階互補(bǔ)濾波融合計(jì)算：

(8)

其中：τ為濾波器時(shí)間常數(shù)；ψ為航向角。

3 四元數(shù)姿態(tài)更新算法

3.1 四元數(shù)

2個(gè)坐標(biāo)系間的位置關(guān)系可認(rèn)為是剛體的定點(diǎn)轉(zhuǎn)動(dòng)，可用四元數(shù)描述。四元數(shù)是由4個(gè)元構(gòu)成的數(shù)，

Q(q0,q1,q2,q3)=q0+q1i+q2j+q3k=

[q0,q1,q2,q3]T。

(9)

其中：q0、q1、q2、q3為實(shí)數(shù)；i、j、k為相互正交的單位向量。

(10)

(11)

將式(11)簡(jiǎn)化為

(12)

結(jié)合式(10)、(11)可得到四元數(shù)轉(zhuǎn)換為姿態(tài)角的關(guān)系式：

(13)

在已知姿態(tài)角情況下，可通過(guò)姿態(tài)角轉(zhuǎn)換為四元數(shù)，

(14)

由于

(15)

只要確定了q0的正負(fù)，其他3個(gè)數(shù)的正負(fù)也隨之確定。由式(13)可知，若q0、q1、q2、q3的正負(fù)號(hào)全部取反，得到的姿態(tài)角不變，因此，q0的符號(hào)可任取正負(fù)。

東北天和北東地2個(gè)地理坐標(biāo)系都可作為導(dǎo)航參考系，但是不同參考系下的計(jì)算公式略有不同，不能混用。

3.2 解四元數(shù)微分方程

用四元數(shù)方法更新姿態(tài)，歸根結(jié)底是求四元數(shù)?？芍苯訉⑺脑獢?shù)應(yīng)用到程序中，或者先將其轉(zhuǎn)換為姿態(tài)矩陣和姿態(tài)角。四元數(shù)微分方程

(16)

對(duì)于不具備特殊形式的常微分方程式，

(17)

假設(shè)其解為y(x)，在定義域內(nèi)某區(qū)間[xi,xi+1]，使用微分中值定理，有

y(xi+1)-y(xi)=y′(εi)(xi+1-xi)，

(18)

其中εi∈[xi,xi+1]。記(xi+1-xi)為步長(zhǎng)h，K=hy′(ε)，式(18)可改寫(xiě)為

y(xi+1)=y(xi)+hy′(εi)=y(xi)+

hf(xi,yi)=y(xi)+K。

(19)

將K視作在[xi,xi+1]上的平均斜率近似值，只要求出K，再利用初值y(m)就可迭代求解。為了獲得更高的精度，在區(qū)間[xi,xi+1]內(nèi)選擇4個(gè)點(diǎn)，將這4個(gè)點(diǎn)的斜率進(jìn)行加權(quán)平均，獲取平均斜率誤差較小的近似值K，這就是經(jīng)典的4階龍格庫(kù)塔法(RK4)。

4 增強(qiáng)型顯式互補(bǔ)濾波

式(8)是一階互補(bǔ)濾波，求出角度后進(jìn)行互補(bǔ)濾波，不必通過(guò)四元數(shù)求姿態(tài)角。由四元數(shù)描述的EECF算法是先將加速度計(jì)、磁強(qiáng)計(jì)數(shù)據(jù)融合至陀螺儀角速度信息中，以修正飛行器相對(duì)導(dǎo)航坐標(biāo)系的角速度，再求修正四元數(shù)。

重力加速度g在地理坐標(biāo)系的分量為

gn=[gx,gy,gz]T=[0,0,-g]T,

(20)

(21)

(22)

對(duì)式(21)和式(22)進(jìn)行向量叉乘：

axsz,axsy-aysx)T。

(23)

(24)

其中：Ki為積分系數(shù)；Kp為比例系數(shù)。由ωb,new可求出修正的四元數(shù)，并對(duì)其歸一化處理得qnew。此時(shí)，將b系的任一向量經(jīng)過(guò)新的姿態(tài)矩陣映射到n系，即將b系旋轉(zhuǎn)到n系所在位置，則這2個(gè)坐標(biāo)系的xOy平面是重合的。

顯式互補(bǔ)濾波(ECF)算法的缺點(diǎn)是未修正航向角誤差，可利用磁強(qiáng)計(jì)修正航向角誤差，構(gòu)成了增強(qiáng)型顯式互補(bǔ)(EECF)算法，其原理如圖2所示。

圖2 增強(qiáng)型顯式互補(bǔ)算法原理框圖

磁強(qiáng)計(jì)測(cè)量的地磁場(chǎng)強(qiáng)度在機(jī)體坐標(biāo)系的分量歸一化為

mb=(mx,my,mz)T，

(25)

(26)

在北半球，任意一點(diǎn)的地磁場(chǎng)強(qiáng)度水平分量平行于地平面指向北，豎直分量豎直向下。設(shè)其在地理坐標(biāo)系的投影為

d=(0,dy,dz)T，

(27)

其中dy和dz為未知量，取dy=uy,dz=uz，則

d=(0,uy,uz)T，

(28)

即mn的ux分量應(yīng)為0。將式(26)與式(28)進(jìn)行向量叉乘，得

merr=mn×d=(0,-uxuz,uxuy)T，

(29)

與式(24)類似，再次對(duì)ωb修正，得

(30)

5 算法驗(yàn)證分析

搭建以STM32主控、GY86模塊、數(shù)傳、電子調(diào)速器、遙控發(fā)射機(jī)與接收機(jī)等為核心部件的硬件平臺(tái)，進(jìn)行飛行試驗(yàn)。系統(tǒng)上電后，由STM32獲取GY86模塊的原始數(shù)據(jù)，設(shè)置采樣率為100 Hz，采集初始對(duì)準(zhǔn)以及姿態(tài)解算2個(gè)過(guò)程的數(shù)據(jù)，由數(shù)傳模塊傳輸至電腦端，通過(guò)MEX指令將Matlab與C語(yǔ)言混合編程，用本算法分析結(jié)果。初始對(duì)準(zhǔn)在晃動(dòng)條件下完成，初始對(duì)準(zhǔn)的姿態(tài)角如圖3所示，姿態(tài)解算的姿態(tài)角如圖4所示。

圖3 初始對(duì)準(zhǔn)的姿態(tài)角

圖4 姿態(tài)解算的姿態(tài)角

首先進(jìn)行經(jīng)初始對(duì)準(zhǔn)與直接賦予四元數(shù)[1,0,0,0]T的姿態(tài)角對(duì)比，分別記為方式1、方式2，使用增強(qiáng)型互補(bǔ)濾波算法，以航向角為例，進(jìn)行誤差分析。四元數(shù)[1,0,0,0]T對(duì)應(yīng)θ=0,γ=0,ψ=0，由初始對(duì)準(zhǔn)得到的初始姿態(tài)角θ=-1.74°，γ=0.43°，ψ=20.13°，兩者之間存在誤差。圖5為2種方式航向角估計(jì)值。從圖5可看出，使用方式2相對(duì)方式1進(jìn)行解算得到的航向角發(fā)生了偏移。

圖5 2種方式航向角估計(jì)值

固定初始俯仰角和初始橫滾角，變換初始航向角，觀測(cè)姿態(tài)解算中航向角偏移量，如圖6所示。從圖6可看出，初始姿態(tài)角偏移越大，則解算得到的姿態(tài)角偏移越大。加入初始對(duì)準(zhǔn)算法能得到初始姿態(tài)角，有助于提高后續(xù)姿態(tài)解算的準(zhǔn)確性。為了比較算法的濾波效果，使用卡爾曼濾波以及MPU9250自帶的DPM算法得到靜態(tài)時(shí)的姿態(tài)角，采用姿態(tài)角標(biāo)準(zhǔn)差比較各算法濾波精度，靜態(tài)姿態(tài)角標(biāo)準(zhǔn)差如表1所示。從表1可看出，EECF算法不僅能解算出機(jī)體的俯仰角和橫滾角，還可解算得到航向角，靜態(tài)姿態(tài)角標(biāo)準(zhǔn)差小于0.05°，且解算精度相比卡爾曼濾波、MPU9250 DMP濾波，航向角標(biāo)準(zhǔn)差分別下降了63.8%、35.4%。

圖6 初始航向角與姿態(tài)解算中航向角偏移

算法θ/(°)γ/(°)ψ/(°)卡爾曼濾波0.0610.0620.116MPU9250 DMP0.0370.0340.065EECF0.0230.0210.042

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

針對(duì)中小型飛行器使用微機(jī)電系統(tǒng)(MEMS)器件無(wú)初始對(duì)準(zhǔn)過(guò)程導(dǎo)致姿態(tài)估計(jì)精度低的問(wèn)題，提出一種基于增強(qiáng)型顯式互補(bǔ)濾波的無(wú)人機(jī)姿態(tài)算法。一方面，加入初始對(duì)準(zhǔn)算法，可解算出初始航向角，提高了后續(xù)姿態(tài)解算的準(zhǔn)確性；另一方面，在顯式互補(bǔ)濾波(ECF)基礎(chǔ)上，增加使用地磁場(chǎng)強(qiáng)度信息修正陀螺儀z軸角速度，構(gòu)成了增強(qiáng)型顯式互補(bǔ)濾波(EECF)，提高了航向角解算精度，得到的航向角不存在漂移現(xiàn)象。搭建硬件平臺(tái)進(jìn)行了算法驗(yàn)證，為后續(xù)實(shí)驗(yàn)提供了實(shí)踐基礎(chǔ)。