基于卡爾曼濾波的四旋翼飛行器控制算法

胡開明，李躍忠，劉 薇

(1.東華理工大學放射性地質(zhì)與勘探技術國防重點學科實驗室，江西 南昌 330013；2.江西省新能源工藝及裝備工程技術研究中心，江西 南昌 330013 )

0 引言

微型旋翼飛行器因具有飛行姿態(tài)靈活、體積小、懸停和低速性能優(yōu)越等特點而受到廣泛的關注，目前已成為微型無人飛行器研究領域的熱點[1]?？梢苑奖愕貙崿F(xiàn)垂直起降、空中懸停的功能，也可以攜帶攝像機、測繪儀器及各種特定裝置，在自動操控或者人工操作下，按照預期的航線和姿態(tài)飛行，以完成既定的任務。但是由于飛行器姿態(tài)檢測中的加速度數(shù)據(jù)摻雜著高斯白噪聲干擾，致使飛行器穩(wěn)定性較差[2]。目前研究大多數(shù)集中在飛行器的設計與飛行算法的研究上，沒有對飛行器慣性單元產(chǎn)生的干擾信號進行分析處理。本文針對此問題，提出了以四元數(shù)法與卡爾曼濾波相結(jié)合對飛行器的姿態(tài)數(shù)據(jù)做互補的融合處理方法。

1 四旋翼飛行器工作原理與結(jié)構組成

1.1 四旋翼飛行器工作原理

四旋翼飛行器由四臺電機組成，一號和三號電機一對按順時針旋轉(zhuǎn)，二號和四號電機一對按逆時針旋轉(zhuǎn)。當四軸飛行器工作運行時，空氣動力扭矩效應和陀螺效應可以相互抵消，如圖1所示。四軸飛行器具有各旋翼的旋轉(zhuǎn)方向與旋翼對機身所施加的反扭矩方向相反的特點，因此當一號電機和三號電機順時針旋轉(zhuǎn)時，二號電機和四號電機做逆時針旋轉(zhuǎn)，用來平衡各旋翼對機身的扭矩作用。四軸飛行器具有6個自由度，通過調(diào)節(jié)各旋翼的轉(zhuǎn)速來控制這6個自由度，以實現(xiàn)俯仰運動、滾轉(zhuǎn)運動、垂直運動、偏航運動、前后運動和側(cè)向運動[3]。

圖1 四旋翼飛行器工作原理Fig.1 Working principle of of quadroter aircraft

1.2 四旋翼飛行器的結(jié)構組成

四旋翼飛行器主要由慣性測量模塊、姿態(tài)算解模塊、姿態(tài)控制模塊、電機驅(qū)動模塊、通信模塊、電源管理模塊組成?？傮w設計框圖如圖2所示。

圖2 四旋翼飛行器控制框圖Fig.2 Control diagram of quadroter aircraft

設計中姿態(tài)算解、姿態(tài)控制模塊由ST公司的STM32F103單片機完成計算。STM32F103單片機是ST公司推出的高性能ARM-Cortex M3架構微處理器。除此之外，STM32F103還要完成讀取通信模塊信息，讀取慣性測量模塊測量值，產(chǎn)生電機驅(qū)動所需的PWM信號等任務。通信模塊采用臺產(chǎn)NRF24L01 2.4 G無線通信模塊，與STM32F103通過SPI接口相連。慣性測量單元采用MPU6050，內(nèi)部集成3軸加速度計與三軸陀螺儀，與STM32F103通過I2C接口相連。

2 四旋翼飛行器控制算法設計

2.1 飛行器姿態(tài)算解

2.1.1機身坐標系

微型四旋翼飛行器具有飛行高度低、運行速度小的特點，因此在定義參考坐標系和機身坐標系時可以忽略地球自轉(zhuǎn)對飛行器姿態(tài)描述的影響。設大地是一個平面，參考坐標系OoXoYo與地面固定，坐標原點Oo定義為四旋翼飛行器的起飛點，三個坐標軸OoXo指向為北，OoYo指向東，OoZo指向朝上并保持與地面垂直。將飛行器機身坐標系ObXbYb固定在機體上，機身坐標系的原點Ob保持與機體的幾何中心重合，則坐標軸ObXb為飛行器前向飛行的方向，坐標軸ObYb指向飛行器的右方，坐標軸ObZb指向地面且與垂直于機體平面ObXbYb。

根據(jù)上面所定義的參考坐標系與機身坐標系可以引入?yún)⒖甲鴺讼蹬c機身坐標系之間的歐拉角，即偏航角φ、俯仰角θ和翻滾角ψ。偏航角φ表示由坐標軸Xo投影到平面OoXoYo的角度，其定義區(qū)間為[-180°，180°]，繞動方向如果與坐標軸Zb指向相同取值為正，相反為負。俯仰角θ表示坐標軸Xb與平面OoXoYo間的角度，俯仰角的定義區(qū)間為[-90°，90°]，坐標軸Xb指向上為正，向下為負。翻滾角ψ為坐標軸Yb與OoXoYo平面間的角度，翻滾角范圍區(qū)間 [-180°，180°]，坐標軸Yb向下指向為正，向上為負。

如果機身坐標系與參考坐標系的偏航角、俯仰角和翻滾角已知，就可以把相對于機身坐標系的測量參數(shù)轉(zhuǎn)換成相對于參考坐標系的測量參數(shù)。

2.1.2姿態(tài)表達式

機身姿態(tài)采用四元數(shù)法來表示以解決歐拉角的萬向節(jié)鎖死的問題[4]。方向余弦矩陣的列表示機身坐標系中的單位向量在參考坐標系中的投影。在機身坐標系中定義的向量rb通過左乘方向余弦矩陣C來旋轉(zhuǎn)到參考坐標系。

r0=Crb

(1)

將偏航角φ、俯仰角θ和翻滾角ψ寫成三個獨立的方向余弦矩陣[4]。得到歐拉角形式的方向余弦矩陣為：

(2)

式(2)中，C′為C的轉(zhuǎn)置。

四元數(shù)表達式是一個具有四個參數(shù)的姿態(tài)表達式。機身坐標系到大地坐標系的變化可以看作是通過繞定義在參考坐標系中的矢量μ的單次旋轉(zhuǎn)來實現(xiàn)的。 四元數(shù)是一個四維矢量，用符號q來表示：

(3)

式(3)中，μx，μy，μz為μ在各個坐標軸上的分量：

將四元數(shù)用三角函數(shù)代換掉后可得

(4)

2.2 卡爾曼濾波器

2.2.1卡爾曼濾波器設計

機身姿態(tài)算解的核心就是解上節(jié)所述的四元數(shù)微分運動方程。電機運轉(zhuǎn)時加速度計輸出的加速度數(shù)據(jù)參雜著高斯白噪聲，適合采用抗干擾能力較強的卡爾曼濾波器來設計?？柭鼮V波器是一套可以由計算機實現(xiàn)的實時遞推算法[5]。它以隨機信號作為處理對象，依據(jù)系統(tǒng)噪聲和觀測噪聲的統(tǒng)計特性，將觀測量作為濾波器的輸入量，狀態(tài)變量的估計值作為輸出量，根據(jù)系統(tǒng)的狀態(tài)方程和觀測方程對系統(tǒng)狀態(tài)信號進行最優(yōu)估計的算法[6]。

(5)

(6)

式(6)中，R是觀測測量噪聲的協(xié)方差。

(7)

由上述卡爾曼濾波器原理分析可知，只要給定系統(tǒng)初始狀態(tài)和誤差協(xié)方差矩陣，就能根據(jù)觀測值遞推到第k時刻的估計值。

將姿態(tài)四元數(shù)作為狀態(tài)變量，系統(tǒng)狀態(tài)矩陣A為：

(8)

式(8)中，ωx，ωy，ωz為陀螺儀中讀出的三軸角速度,Δt為兩倍采樣時間。

h=CV

(9)

對h求偏導數(shù)得到在狀態(tài)變量X附近的線性化系數(shù)H：

(10)

即Zk可以近似的表示為：

Zk=HXk

(11)

根據(jù)式(8)得到的系統(tǒng)狀態(tài)矩陣和式(11)得到的系統(tǒng)輸出方程，代入卡爾曼濾波器遞推公式(5)—(7)即可推出卡爾曼濾波器的估值。

2.2.2卡爾曼濾波器程序設計

卡爾曼濾波器程序設計將卡爾曼函數(shù)表示成矩陣運算的數(shù)據(jù)結(jié)構體的形式，包含卡爾曼濾波器初始化和卡爾曼濾波器更新?？柭鼮V波器初始化函數(shù)會將卡爾曼濾波器結(jié)構體中的矩陣初始化好，并對它們賦予初值?？柭鼮V波器更新函數(shù)會將根據(jù)MPU6050測量結(jié)果及時更新卡爾曼濾波器結(jié)構體中的矩陣。根據(jù)過去時刻的姿態(tài)和當前時刻的測量值由卡爾曼濾波器計算出當前時刻新的姿態(tài)進行姿態(tài)更新[7]?？柭鼮V波器初始化函數(shù)和更新函數(shù)流程圖如圖3所示。

圖3 卡爾曼濾波器軟件流程圖Fig.3 Software flow chart of kalman filter

2.3 PID控制

2.3.1PID控制回路

系統(tǒng)采用PID控制器，根據(jù)飛行運動狀態(tài)將控制回路簡化為四個運動控制回路，分別是垂直回路、偏航回路、滾轉(zhuǎn)回路和俯仰回路。控制回路的給定信號來源于無線通信模塊。無線通信模塊接收來自遙控系統(tǒng)的信號。機身姿態(tài)反饋信號來自卡爾曼濾波器，卡爾曼濾波器計算出的機身姿態(tài)四元數(shù)轉(zhuǎn)換為姿態(tài)角送給各個控制回路。高度反饋信號來自氣壓計，反饋信號和給定信號之差即誤差信號經(jīng)過PID控制器校正后送到電機驅(qū)動從而驅(qū)動電機運行[8]。

2.3.2電機控制

電機調(diào)速方法采用脈沖寬度調(diào)制(PWM)法[7]。當頻率較高的PWM信號經(jīng)過電機時，電機可以看成一個一階慣性環(huán)節(jié)，所以PWM信號可以等效為一直流信號，其幅值為PWM信號的占空比D乘以PWM信號的峰值Vpp。

姿態(tài)控制回路中輸出升力u1，u2，u3進行電機控制。根據(jù)飛行器的基本原理可以由u1，u2，u3和位置控制回路中的u4導出每個電機的升力大?。?/p>

(12)

已知電機轉(zhuǎn)速和電機升力成平方關系，所以電機轉(zhuǎn)速可以表示為：

(13)

而電機轉(zhuǎn)速乘以系數(shù)C即可變成占空比D。

3 試驗與結(jié)果

3.1 卡爾曼濾波器的調(diào)試

設計中采用串口將機身姿態(tài)數(shù)據(jù)發(fā)送至上位機，通過上位機實現(xiàn)監(jiān)控功能，從而比較各種參數(shù)下的系統(tǒng)性能。圖4為其飛控狀態(tài)監(jiān)控界面圖，可以完成飛行器的姿態(tài)角的3D顯示和各電機的PWM值的顯示，也能對姿態(tài)角等數(shù)據(jù)通過波形的方式顯示出來。

圖4 運行狀況監(jiān)控圖Fig.4 Operation monitoring chart

卡爾曼濾波器在濾波過程中有三個濾波參數(shù)會隨著時間和輸入量而改變，因為三個方向的數(shù)據(jù)都是從一個加速度計中得來的，所以其濾波參數(shù)都是一樣的，其中隨時間變化的三個參數(shù)分別為：卡爾曼增益Kg，新的狀態(tài)誤差方差Now_P和上一次觀測誤差方差Last_P ，而它們隨著濾波的進行會快速的收斂，最后為一定值。為了驗證這三個參數(shù)的變化，將這三個數(shù)值用三個數(shù)組分別提取出來，每個數(shù)組取了80個數(shù)，然后將這三組各80個數(shù)的數(shù)據(jù)用Excel進行數(shù)據(jù)處理得到三個參數(shù)的散點圖。圖5為Now_P的散點圖，圖6為Last_P的散點圖，圖7為Kg的散點圖。

圖5 Now_P的散點圖Fig.5 Scatter diagram of Now_P

圖6 Last_P的散點圖Fig.6 Scatter diagram of Last_P

圖7 為Kg的散點圖Fig.7 Scatter diagram of Kg

從上面三組數(shù)據(jù)曲線可以看出，這三個變量會隨著時間變化而趨于一穩(wěn)定值。

將三個方向的角加速度卡爾曼濾波前和濾波后的值用UART口發(fā)給上位機，通過上位機得到三個方向的各兩組加速度曲線圖，以驗證卡爾曼的濾波效果。其中圖8(a)和圖8(b)分別為X軸方向的加速度濾波前和濾波后的值，圖9(a)和圖9(b)分別為Y軸方向的加速度濾波前和濾波后的值，圖10(a)和圖10(b)分別為Z軸方向的加速度濾波前和濾波后的值。

從三組對比圖可以看出卡爾曼濾波器的濾波效果較好，能很好地濾除高斯噪聲。

3.2 運行測試

設計制作四旋翼飛行器，采用PID控制以PWM方式驅(qū)動電機，分別控制偏航角、俯仰角和翻滾角。整定翻滾角PID控制參數(shù)Kp為1.6，Ki為0.4，Kd為 0.3，飛行器處于懸停狀況下進行動態(tài)測試的數(shù)據(jù)如表1所示。

圖9 Y軸方向加速度曲線Fig.9 Acceleration curve in Y-axis

圖10 Z軸方向加速度曲線Fig.10 Acceleration curve in Z-axis

從表1中的各組數(shù)據(jù)的變化可以看出期望值與反饋值的差值越大控制量的輸出值越大，符合控制要求。偏航角和俯仰角的測試方法類似。

表1 翻滾角PID控制數(shù)據(jù)Tab.1 PID control data of roll angle

PID的控制算法最后得到的控制量為控制電機的PWM的點空比，通過改變飛行器在旋飛時的姿態(tài)角或者通過遙控器改變飛行器的姿態(tài)角期望值，查看各個電機的PWM輸入量的變化來驗證電機控制的效果。圖11為向前飛行時四個電機的PWM值，同樣可以得到向后飛行、向左飛行和向右飛行的PWM曲線。

圖11 向前飛行時四個電機的PWM值Fig.11 PWM values of four motors in forward flight

由圖11可知當搖桿控制飛行器向前飛行時，電機1至電機4的PWM值分別為68%，4%，6.5%，71.5%，飛行器的俯仰角有遞減的趨勢，能實現(xiàn)向前飛行的功能。經(jīng)過測試，飛行器能夠較快地恢復到平衡并最終穩(wěn)定下來，表明設計合理，滿足要求。

4 結(jié)論

本文提出了四元數(shù)法與卡爾曼濾波算法相結(jié)合的方法，對姿態(tài)數(shù)據(jù)進行融合處理，并研究了微型四旋翼飛行器的PID控制方法，完成了飛行控制效果的試驗測試。測試結(jié)果與理論分析表明，該方法實現(xiàn)了良好的濾波效果，達到了飛行器平穩(wěn)飛行的目的。