基于STM32的四旋翼飛行器的姿態最優估計研究*

何 川 ，李 智 ，，王勇軍

（1.桂林電子科技大學 電子工程與自動化學院，廣西 桂林 541004；2.桂林航天工業學院無人遙測重點實驗室，廣西 桂林 541004）

0 引言

姿態最優估算是飛行器自主飛行的先決條件，在飛行過程中，實時準確地獲得飛行器的姿態信息，可以決定飛行器的控制精度和穩定性[1]。隨著微電子技術的發展，飛行器姿態解算系統普遍采用低成本的航姿參考系統（AHRS），其主要包括：基于微機電系統（MEMS）的三軸陀螺儀、三軸加速度計和三軸磁羅盤。由于系統機械結構和傳感器自身三軸并非完全正交，引入了不正交誤差，以及數據積分產生的發散誤差和外部干擾產生的隨機誤差。因此要估計飛行器姿態的最優信息必須對多傳感器進行誤差處理，并尋求最佳數據融合算法。

目前國內對MEMS傳感器姿態解算系統研究的常用算法有：擴展卡爾曼濾波、梯度下降法、互補濾波等。擴展卡爾曼濾波應用非常廣泛，但為其建立可靠穩定的狀態方程、確定合適的量測噪聲和過程噪聲協方差矩陣都比較困難。梯度下降法的好處在于姿態解算過程只有普通的乘法和加法運算，普通的微控制器即能滿足算法要求，但難點在于構造合適的目標函數[2]。本系統在Mahony[3]提出的補償濾波器的基礎上進行優化，采用雙環補償濾波算法，外環引用重力場和地磁場進行補償，內環引用重力場進行補償，并將修正后的陀螺儀和磁羅盤進行互補濾波，以期得到最優姿態輸出。

1 飛行器姿態描述

飛行器常用的姿態描述方法有歐拉角、方向余弦矩陣和四元數三種。三維空間中，飛行器有3個轉動自由度，分別可以通過3個角度參數來描述飛行器的姿態，分別定義為：繞機體坐標系的Z軸轉動為偏航角Ψ；繞X軸轉動為橫滾角φ；繞Y軸轉動為俯仰角θ[4]。

如圖1所示，飛行器在飛行過程中，地理坐標系b通過繞不同坐標軸三次連續轉動來實現對機體坐標n的轉換。第一次轉動，OX0Y0Z0坐標系即地理坐標系ENU繞Z0軸轉動Ψ角，得到坐標系OX1Y1Z0；第二次轉動，OX1Y1Z0坐標系繞OX1軸轉動φ角，得到坐標系OX1Y2Z2；第三次轉動，OX1Y2Z2坐標系繞OY2軸轉動 θ角，得到坐標系OXYZ，OY2軸即OY軸[5]。這三次坐標軸旋轉可以用數學方法表述為3個獨立的方向余弦矩陣。兩個坐標系的轉換可以用這3個獨立變換矩陣的乘積表示：

圖1 飛行器坐標系和姿態角

由于載體的運動，四元數Q=[q0q1q2q3]T是變量，q0、q1、q2、q3是時間函數。 剛體繞瞬時轉軸 n轉過 σ角度來實現坐標的轉換。為參考坐標系下的一個單位矢量，其角速度為：

機體坐標系n和地理坐標系b之間的變換也可以用四元數的三角形式表示[6]：

在忽略地球的自轉和公轉運動對四旋翼飛行器造成的影響，并假設飛行器為剛體運動且質量分布對稱、均勻，中心為重心位置且不隨著運動而發生變化[7]，則四元數矩陣表達式Q的微分方程解為：

2 飛行器姿態解算系統構建

2.1 系統硬件

本姿態解算系統硬件主要包括主控制器和航姿參考系統(AHRS)。其中主控器采用ST(意法半導體)公司的Cortex-M4為內核的32位ARM微控制器STM32F405RGT6；航姿參考系統采用2款高精度的姿態傳感器，分別為：InvenSense公司的 MPU-6500和 ST公司的 LSM303D，構成了一個九軸的姿態測量系統。傳感器均選用數字芯片，內部均集成有高精度的ADC。主控制器通過SPI總線，讀取出傳感器采集到的原始數據，然后通過四元數的坐標換算、姿態誤差的雙環PI控制和互補濾波解算出歐拉角。姿態解算系統框圖如圖2所示。

圖2 姿態解算系統框圖

2.2 傳感器數據標定

在數據融合之前需要對傳感器測量得到的原始數據進行標定，從而消除傳感器的零點偏置誤差。陀螺儀消除偏置誤差的方法是保持器件水平位置不變，多次采集后求取輸出平均值；加速度計消除偏置方法為八面校準法，將飛行器姿態解算系統保持機頭方向不變，正面朝上、下、左、右、前、后和機頭朝上、機頭朝下。在這八種狀態下分別測量三軸加速度計 ADC輸出的X、Y、Z三軸的最大最小值，并與±1 g(重力加速度)所對應的ADC數值進行校準；磁羅盤零點數據誤差采用橢圓假設法進行標定，將姿態解算系統分別在坐標系下XOY和XOZ兩個平面內360°旋轉，并將采集到的數據用MATLAB軟件擬合成橢圓，然后修正橢圓的偏心和半徑。

3 系統數據融合與姿態解算

系統姿態解算分為兩個部分。第一部分是雙環PI控制器修正數據融合后傳感器偏差。第二部分為修正后的三軸陀螺儀與三軸磁羅盤進行互補濾波，消除磁羅盤的高頻干擾，提高磁羅盤的響應能力和抗干擾能力。姿態解算流程圖如圖3所示。

3.1 雙環PI控制器

陀螺儀可以快速響應機體的旋轉，短時間誤差較小、可信度高，但是存在溫漂和零漂，以及積分誤差隨時間累積等因素影響。加速度計在靜止的狀態下漂移很小，傾角求解過程不存在積分誤差，但是受到飛行過程中電機和機架的振動以及轉動和運動加速度的干擾。磁羅盤測量得到的地磁向量在一定地理范圍內可以認為不會發生改變，但是，磁羅盤容易受到硬磁場和軟磁場的干擾。

圖3 姿態解算流程圖

因此，系統外環采用九軸姿態傳感器（三軸加速度計、三軸磁羅盤和三軸陀螺儀）數據融合。由于磁羅盤容易受到干擾，有可能導致外環九軸數據融合后依舊存在較大誤差，所以，內環采用六軸姿態傳感器（三軸加速度計和三軸陀螺儀）數據融合，對數據融合后的傳感器姿態偏差進行二次修正。

外環九軸姿態傳感器數據融合，記在飛行器機體坐標系下 an=[axayaz]T和 mn=[mxmymz]T分別為加速度計和磁羅盤實際測量得到的重力向量和地磁向量。記vn=[vxvyvz]T和wn=[wxwywz]T是將地理坐標系下重力向量kb=[0 0 1g]T和地磁向量nb=[nx0nz]T（不考慮地理磁偏角因素，將機頭固定向北）通過四元數坐標換算成機體坐標系下的重力向量和地磁向量。向量之間的誤差為坐標軸的旋轉誤差，可以用向量的叉積en=[exeyez]T表示，如式（6）所示。由于陀螺儀是對機體直接積分，所以，陀螺儀的誤差可以體現為機體坐標的誤差。因此修正坐標軸的誤差可以達到修正陀螺儀誤差的目的，從而將加速度計和磁羅盤進行修正陀螺儀，實現了九軸的數據融合。即如果陀螺儀按照叉積誤差的軸，轉動叉積誤差的角度，就可以消除機體坐標上實際測量的重力向量和地磁向量和坐標換算后的重力向量和地磁向量之間的誤差。

PI調節器的比例部分用于迅速糾正陀螺儀誤差，積分部分用于消除穩態偏差[7]。通過大量實驗驗證后取PI調節器的比例系數為1.0，積分系數為0.2。陀螺儀經過外環PI控制器修正姿態誤差后輸出值為gn=[gxgygz]T：

3.2 互補濾波

本系統姿態解算的第二部分是磁羅盤的修正，主要采用互補濾波算法?；パa濾波算法簡單可靠且對器件精度要求低?；パa濾波的目的在于將陀螺儀和磁羅盤各自在頻域角度上進行互補。因為磁羅盤動態響應能力差且易受外界環境的干擾，而陀螺儀動態響應快，故可在頻域上將二者形成互補，即對磁羅盤低通濾波，對陀螺儀進行高通濾波。如圖4所示，將修正后得到的陀螺儀數據與磁羅盤數據進行互補濾波，即可消除磁羅盤的高頻干擾。

圖4 互補濾波

如式(10)所示，r為互補濾波中的權值，r值越大，證明陀螺儀占的權重越大。

4 姿態解算結果分析

為了驗證姿態解算系統在多旋翼飛行器實際飛行過程中的性能，搭建了一個半實物仿真平臺。將基于STM32的姿態解算系統的四旋翼飛行器機頭朝向地理正北方，機架底部水平固定在萬向節一端，萬向節另外一端固定在水平的桌面上。啟動四旋翼飛行器電機，將飛行器油門通道值保持在空中懸停狀態，及四個螺旋槳產生的向上的合力等于飛行器自身重力。然后，通過串口讀取出傳感器采集到的原始數據以及姿態解算后的數據，并用MATLAB進行數據分析。

圖5 俯仰角和橫滾角

圖5中采集的是模擬定點懸停時俯仰角和橫滾角。由于單純地通過陀螺儀采集得到的角速度進行積分得到角度會產生積分誤差，并且積分誤差會隨著時間進行累加，所以未進行姿態解算所得到的姿態角零點誤差會不斷變大。圖中的2條線分別代表姿態解算后的橫滾角和俯仰角。由于采用了雙環PI修正陀螺儀，所以姿態角零點誤差與解算前相比不會隨著時間累計不斷變大。但是由于姿態解算系統處于四旋翼飛行器模擬定點懸停狀態，所以，電機引起的機架振動、載體重力分布不均、機械連接處阻尼問題等因素，導致零點誤差相對于水平放置在桌面上時的靜態零點誤差，偏差較大且波動相對厲害。零點誤差基本穩定在±2.2°以內。隨著時間的推移，零點誤差逐步趨近平穩，可以滿足四旋翼飛行器對姿態數據的要求。

圖6中測試結果分別代表磁羅盤姿態解算前采集的數據和姿態解算后測量的數據。本系統采用的互補濾波算法中權值r取0.15，偏航角以磁羅盤采集的數據為主，數據融合后的陀螺儀只進行輔助修正?？梢杂^測到細線代表的是沒有使用濾波算法的磁羅盤測量的偏航角，零點誤差相對較大，達到了±3°左右，而且上下波動也很大，表示磁羅盤受到外界環境嚴重干擾。粗線代表磁羅盤和陀螺儀經過互補濾波后采集得到的偏航角，消除了部分干擾信號，上下波動明顯減弱，并且誤差穩定保持在±1.7°以內。由于磁羅盤自身具有不可忽略的零位、靈敏度及非正交誤差，因此通過軟件算法無法徹底消除零點誤差。

圖6 偏航角

5 結論

旋翼飛行器飛行過程中獲得姿態最優估計是飛行器系統穩定運行的根本保障。本文基于四元數坐標換算，將地理坐標系下的重力向量和地磁向量換算到機體坐標系下。然后通過修正坐標系旋轉誤差，實現了多傳感器的數據融合，避免了陀螺儀零點積分誤差的累積，并將修正后的陀螺儀與磁羅盤進行互補濾波，消除了磁羅盤的高頻干擾。實驗表明：經過雙環PI控制修正姿態偏差的陀螺儀，俯仰角和橫滾角零點動態誤差保持在±2.2°左右；互補濾波后的磁羅盤消除了部分高頻干擾，偏航角誤差保持在±1.7°左右。本文設計的姿態解算系統較好地解決了噪聲干擾與姿態最優估計問題，并在半實物仿真平臺上得到了驗證，實現了長時間穩定地輸出準確可靠的姿態數據，滿足了四旋翼飛行器飛行控制對姿態信息的要求。

[1]張浩，任芊.四旋翼飛行器航姿測量系統的數據融合方法[J].兵工自動化，2013，32（1）：28-31.

[2]杜杉杉，吳昊，張繼文，等.一種面向 AHRS的改進互補濾波融合算法[J].國外電子測量技術，2015(3)：13-18.

[3]MAHONY R，HAMEL T.Complementary filters on the special orthogonal group[C].IEEE Transactions on Automatic Control，2008，53（5）：1203-1218.

[4]張天光，王秀萍，王麗霞.捷聯慣性導航技術[M].北京：國防工業出版社，2007：15-16.

[5]鄧正隆.慣性技術[M].哈爾濱：哈爾濱工業大學出版社，2006：10-11.

[6]李一波，宋述錫.基于模糊自整定PID四旋翼無人機懸?？刂芠J].控制工程，2013（5）：910-914.

[7]廖懿華，張鐵民，廖貽泳.基于模糊-比例積分偏差修正的多旋翼飛行器姿態測算系統[J].農業工程學報，2014(20)：19-27.