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

，，

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

引 言

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

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

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

圖1 兩個坐標(biāo)系的關(guān)系圖

(1)

已知旋轉(zhuǎn)矩陣，求歐拉角的表達(dá)式為：

(2)

得出歐拉角函數(shù)：

(3)

而四元數(shù)與歐拉角的關(guān)系轉(zhuǎn)換為：

?qy(θ)qx(φ)

(5)

(6)

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

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

(7)

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

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

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

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

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

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

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

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

互補(bǔ)濾波結(jié)構(gòu)框圖如圖2所示。

圖2 互補(bǔ)濾波結(jié)構(gòu)框圖

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

(8)

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

又因?yàn)?

(9)

所以有：

(10)

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

2.3 PID控制的改進(jìn)型互補(bǔ)濾波

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

圖3 改進(jìn)型互補(bǔ)濾波結(jié)構(gòu)框圖

又因

x1=(φa-φ)KpKi

(11)

(12)

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

(13)

(14)

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

3 實(shí)驗(yàn)結(jié)果分析

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

圖4 程序流程圖

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

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

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

圖5 實(shí)際角度和互補(bǔ)濾波角度對比圖

假設(shè)輸入條件為階躍信號，階躍響應(yīng)的仿真結(jié)果如圖6所示。

圖6 互補(bǔ)濾波的模糊PID控制階躍響應(yīng)結(jié)果

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

結(jié) 語

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