基于增強型顯式互補濾波的無人機姿態算法

馬 力， 李天松， 陽榮凱， 黃艷虎

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

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

1 系統設計

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

圖1 初始對準與姿態解算系統框圖

2 初始對準

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

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

A=AADC/SA。

(1)

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

陀螺儀角速度關系式為

(2)

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

(3)

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

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

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

(4)

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

可由磁強計測得航向角，通過θ與γ對磁強計進行傾斜補償[12]，在b系下，

(5)

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

(6)

θgyro=ω·dt+θgyro。

(7)

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

(8)

其中：τ為濾波器時間常數；ψ為航向角。

3 四元數姿態更新算法

3.1 四元數

2個坐標系間的位置關系可認為是剛體的定點轉動，可用四元數描述。四元數是由4個元構成的數，

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

[q0,q1,q2,q3]T。

(9)

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

(10)

(11)

將式(11)簡化為

(12)

結合式(10)、(11)可得到四元數轉換為姿態角的關系式：

(13)

在已知姿態角情況下，可通過姿態角轉換為四元數，

(14)

由于

(15)

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

東北天和北東地2個地理坐標系都可作為導航參考系，但是不同參考系下的計算公式略有不同，不能混用。

3.2 解四元數微分方程

用四元數方法更新姿態，歸根結底是求四元數。可直接將四元數應用到程序中，或者先將其轉換為姿態矩陣和姿態角。四元數微分方程

(16)

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

(17)

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

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

(18)

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

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

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

(19)

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

4 增強型顯式互補濾波

式(8)是一階互補濾波，求出角度后進行互補濾波，不必通過四元數求姿態角。由四元數描述的EECF算法是先將加速度計、磁強計數據融合至陀螺儀角速度信息中，以修正飛行器相對導航坐標系的角速度，再求修正四元數。

重力加速度g在地理坐標系的分量為

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

(20)

(21)

(22)

對式(21)和式(22)進行向量叉乘：

axsz,axsy-aysx)T。

(23)

(24)

其中：Ki為積分系數；Kp為比例系數。由ωb,new可求出修正的四元數，并對其歸一化處理得qnew。此時，將b系的任一向量經過新的姿態矩陣映射到n系，即將b系旋轉到n系所在位置，則這2個坐標系的xOy平面是重合的。

顯式互補濾波(ECF)算法的缺點是未修正航向角誤差，可利用磁強計修正航向角誤差，構成了增強型顯式互補(EECF)算法，其原理如圖2所示。

圖2 增強型顯式互補算法原理框圖

磁強計測量的地磁場強度在機體坐標系的分量歸一化為

mb=(mx,my,mz)T，

(25)

(26)

在北半球，任意一點的地磁場強度水平分量平行于地平面指向北，豎直分量豎直向下。設其在地理坐標系的投影為

d=(0,dy,dz)T，

(27)

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

d=(0,uy,uz)T，

(28)

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

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

(29)

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

(30)

5 算法驗證分析

搭建以STM32主控、GY86模塊、數傳、電子調速器、遙控發射機與接收機等為核心部件的硬件平臺，進行飛行試驗。系統上電后，由STM32獲取GY86模塊的原始數據，設置采樣率為100 Hz，采集初始對準以及姿態解算2個過程的數據，由數傳模塊傳輸至電腦端，通過MEX指令將Matlab與C語言混合編程，用本算法分析結果。初始對準在晃動條件下完成，初始對準的姿態角如圖3所示，姿態解算的姿態角如圖4所示。

圖3 初始對準的姿態角

圖4 姿態解算的姿態角

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

圖5 2種方式航向角估計值

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

圖6 初始航向角與姿態解算中航向角偏移

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

6 結束語

針對中小型飛行器使用微機電系統(MEMS)器件無初始對準過程導致姿態估計精度低的問題，提出一種基于增強型顯式互補濾波的無人機姿態算法。一方面，加入初始對準算法，可解算出初始航向角，提高了后續姿態解算的準確性；另一方面，在顯式互補濾波(ECF)基礎上，增加使用地磁場強度信息修正陀螺儀z軸角速度，構成了增強型顯式互補濾波(EECF)，提高了航向角解算精度，得到的航向角不存在漂移現象。搭建硬件平臺進行了算法驗證，為后續實驗提供了實踐基礎。