多傳感器信息融合的四旋翼姿態(tài)解算研究*

，，

(1.鄭州自來水投資控股有限公司，鄭州 450000；2.中原工學院)

引 言

四旋翼無人機相對于固定翼和直升機來說，體積小、重量輕、使用方便，集合了各種飛行器的優(yōu)點，已經廣泛應用于各行各業(yè)中。而在四旋翼的飛行過程中，核心問題就是姿態(tài)的解算[1]。一般常用的單個傳感器檢測元件(如陀螺儀、加速度計和電子羅盤)，雖然性能穩(wěn)定，價格適中，但是又有各自的缺點。如傳感器信息融合對來自加速度計、陀螺儀以及單個加速度計在四旋翼無人機機體抖動的時候容易帶來高斯白噪聲，單個陀螺儀存在溫度漂移問題，單個電子羅盤受外部磁場干擾也會造成測量誤差。本文基于互補濾波算法，采用多個電子羅盤的數據信號進行綜合，不僅降低了姿態(tài)解算的運算量，而且提高了效率和精度[2]。同時，加入模糊PID 控制方法對四旋翼進行控制，使無人機的飛行更加穩(wěn)定可靠。

1 姿態(tài)角的表述方式推導過程

四旋翼無人機的姿態(tài)解算是指將機體上的慣性單元的輸出信號實時轉換為機體姿態(tài)信號,即把機體坐標系(b系，b表示Body)轉換為地面慣性坐標系(e系，e表示Earth)的角位置。四旋翼無人機的姿態(tài)角包括橫滾角(roll)、俯仰角(pitch)、偏航角(yaw)。地面慣性坐標系用于確定機體的空間位置坐標，將地球表面假設成平面，首先在地面上任選一點，作為四旋翼起飛位置。然后對三軸進行定義：其中xe軸在水平面內指向某一方向，ze軸垂直于地面向下，按右手定則確定ye軸。對于機體坐標系，其原點o取在四旋翼的重心上，其中xb軸在四旋翼對稱平面內指向四旋翼機體頭部方向，zb軸垂直xb軸向下，同樣按右手定則確定yb軸。方向確定以后，機體坐標系和地面慣性坐標系的關系可以在一個圖示上說明。

圖1 兩個坐標系的關系圖

(1)

已知旋轉矩陣，求歐拉角的表達式為：

(2)

得出歐拉角函數：

(3)

而四元數與歐拉角的關系轉換為：

?qy(θ)qx(φ)

(5)

(6)

則姿態(tài)角表示為：

基礎埋深確定后，再確定零層板標高。根據上部泵房部分初選鋼柱柱腳尺寸，柱腳高度大約為450mm，按照建筑專業(yè)要求，柱腳不宜露出地坪，因此，初步確定零層板標高為-0.500m。

(7)

2 四旋翼無人機姿態(tài)角測量和解算

2.1 多傳感器信息融合的姿態(tài)測量

采用基于加速度計、電子羅盤、陀螺儀的捷聯(lián)式慣性測量系統(tǒng)，屬于多傳感器信息融合的智能測量系統(tǒng)。傳感器固定在四旋翼無人機上，傳感器的坐標軸和機體坐標軸保持一致。對三軸電子羅盤Xb、Yb的輸出值之比進行反正切函數處理可得到偏航角，三軸陀螺儀的三軸Xb、Yb、Zb的輸出值分別為ωx、ωy、ωz，即翻滾角速度、俯仰角速度和偏航角速度，如表1所列。

表1 多傳感器姿態(tài)測量描述

加速度計的缺點很明顯，動態(tài)響應慢，受機體振動的影響大,而電子羅盤受外部磁場干擾的影響大，所以這兩種傳感器測得的翻滾角φ、俯仰角θ、偏航角ψ都存在一定誤差，對姿態(tài)角度要求精確的四旋翼無人機來說，不能滿足有效的實時控制要求[8]。對于陀螺儀，雖然通過對測量出來的角速度進行積分可以來獲取三軸具有良好動態(tài)特性的姿態(tài)角，然而陀螺儀的溫度漂移會使誤差隨著時間的增加而累加變大，積分后會得到更大的姿態(tài)角誤差，數據甚至無法使用[6-7]。

結合三種單個傳感器的優(yōu)缺點，科學合理地進行姿態(tài)解算，采用加速度計、電子羅盤用于低頻段的姿態(tài)測量，陀螺儀用于高頻段的姿態(tài)測量，測量的整體誤差將大大減小。融合各種傳感器的數據信息將有助于綜合各種特征值，得出更準確、有效的數據。

2.2 基于互補濾波的四旋翼無人機姿態(tài)解算

根據上述各傳感器的優(yōu)缺點，對電子羅盤、陀螺儀、加速度計三種傳感器所測得的數據進行綜合、校正。線加速度的測量由電子羅盤和加速度計完成，角加速度由陀螺儀完成[8]。陀螺儀經過積分得到姿態(tài)角，測量誤差會隨時間的積累而增加。而加速度計測量，雖然動態(tài)響應速度比較慢，但在一段時間內，測量誤差幾乎不變。鑒于三種傳感器在頻率上的互補關系，采用互補濾波算法作為基本的數據處理方法，把測得的姿態(tài)數據進行融合，提高了姿態(tài)數據的動態(tài)響應和精度。

互補濾波結構框圖如圖2所示。

圖2 互補濾波結構框圖

圖中低通濾波器為dt/(τ+dt)，高通濾波器為τ/(τ+dt)，其中參數τ為時間常數，1/τ為濾波器的截止頻率，dt為濾波器采樣時間。對于低通濾波器，頻率高于1/τ將被濾除，低于1/τ的能順利通過，高通濾波器則相反。加速度計通過低通濾波器能限制運動加速度對于姿態(tài)測量的高頻抖動；高通濾波器的加入，可以有效抑制陀螺儀長時間內積累的漂移誤差，提高陀螺儀的測量精度。由圖2可得：

(8)

式中: φa為電子羅盤和加速度計的姿態(tài)角，φg為陀螺儀測量姿態(tài)角，φ為互補濾波處理后的姿態(tài)角。

又因為:

(9)

所以有：

(10)

由式(10)分析可得在姿態(tài)解算中，以截止頻率為界限值，在高于此界限值的高頻段，陀螺儀起主要作用，低于界限值的低頻段，電子羅盤和加速度計占決定作用。對電子羅盤、加速度計和陀螺儀權重的調整，可通過調整時間常數τ改變截止頻率。

2.3 PID控制的改進型互補濾波

互補濾波相對于其他濾波方法，濾波效果比較明顯，能高效地實現姿態(tài)數據的融合，同時消除了高頻和低頻的干擾問題。但是由于在低通阻帶，互補濾波的衰減速度較慢且干擾噪聲較大時，誤差相對也較大，濾波效果并不理想，只適用于噪聲比較小的情況。為了改變這種現象，在互補濾波基礎上增加PID 控制，構成改進型互補濾波器，圖3 為橫滾角互補濾波結構框圖[9-12]。

圖3 改進型互補濾波結構框圖

又因

x1=(φa-φ)KpKi

(11)

(12)

x2=y1+2×(φa-φ)Kp+φg

(13)

(14)

式(11)～(14)中，Kp、Ki為比例系數，x1、x2、y1為中間變量，x2等于修正后的姿態(tài)值加上陀螺儀積分的姿態(tài)值，整個系統(tǒng)框圖屬于負反饋結構。

3 實驗結果分析

通過C語言進行編程，模仿出PWM波，電機提供動力來使飛行器起飛。重復多次測試后，準確找出飛行器起飛所對應的PWM值。 MPU60506軸傳感器通過串口向控制器STM32發(fā)送數據，控制器接收并檢測接收的數據是否正確，通過軟件編程進一步對得到的數據進行修正，使四旋翼無人機能夠實現穩(wěn)定的起飛、懸停、前進、后退以及降落等功能。

圖4 程序流程圖

主要實現思路為：STM32單片機上電，實現初始化。利用單片機定時器設定電機PWM周期，實現串口初始化。在初始化結束后，設定目標姿態(tài)，實時檢測AHRS是否接收到有效的姿態(tài)數據，從而利用姿態(tài)控制函數進行控制。結合模糊PID融合控制算法，利用當前MPU6050檢測到的實時姿態(tài)數據解算出不同姿態(tài)，從而調整輸出量，如此循環(huán)往復，不斷地接收姿態(tài)數據、解算數據、調整量輸出，才能控制四旋翼飛行器各個角度的飛行姿態(tài)，使四軸飛行器穩(wěn)定、正確地飛行，程序運行結束[13-15]。圖4為本設計的軟件流程圖。

根據本系統(tǒng)中所述的基于多傳感器信息融合的改進型互補濾波的四旋翼自主飛行器，采用模糊PID控制規(guī)則，選取仿真控制對象：四旋翼飛行器的偏航角[14-15]，進行了大量的軟件仿真實驗。

圖5所示為實際角度和互補濾波角度對比圖，采用改進型互補濾波算法得到的解算角度和實際角度相比，偏航角的最大誤差約為±1.2°，動態(tài)誤差小于±4°。

圖5 實際角度和互補濾波角度對比圖

假設輸入條件為階躍信號，階躍響應的仿真結果如圖6所示。

圖6 互補濾波的模糊PID控制階躍響應結果

由仿真結果可知，基于多傳感器信息融合的互補濾波算法，系統(tǒng)響應速度快、超調量小，控制精度高、抗干擾能力強，仿真控制效果較好，因此，其構成的模糊PID控制器能更好地實現對四旋翼飛行器的控制。

結 語

本文基于多傳感器信息融合的改進型互補濾波的四旋翼自主飛行器，利用四元數法進行姿態(tài)解算，并通過互補濾波算法進行數據融合，利用電子羅盤、陀螺儀、加速度計在姿態(tài)測量中的優(yōu)點，為四旋翼完成各種飛行任務提供保證。