胡開明，李躍忠，劉 薇

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

0 引言

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

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

1.1 四旋翼飛行器工作原理

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

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

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

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

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

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

2 四旋翼飛行器控制算法設(shè)計(jì)

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

2.1.1機(jī)身坐標(biāo)系

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

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

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

2.1.2姿態(tài)表達(dá)式

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

r0=Crb

(1)

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

(2)

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

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

(3)

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

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

(4)

2.2 卡爾曼濾波器

2.2.1卡爾曼濾波器設(shè)計(jì)

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

(5)

(6)

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

(7)

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

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

(8)

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

h=CV

(9)

對h求偏導(dǎo)數(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è)計(jì)

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

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

2.3 PID控制

2.3.1PID控制回路

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

2.3.2電機(jī)控制

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

姿態(tài)控制回路中輸出升力u1，u2，u3進(jìn)行電機(jī)控制。根據(jù)飛行器的基本原理可以由u1，u2，u3和位置控制回路中的u4導(dǎo)出每個電機(jī)的升力大小：

(12)

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

(13)

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

3 試驗(yàn)與結(jié)果

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

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

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

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

圖5 Now_P的散點(diǎn)圖Fig.5 Scatter diagram of Now_P

圖6 Last_P的散點(diǎn)圖Fig.6 Scatter diagram of Last_P

圖7 為Kg的散點(diǎn)圖Fig.7 Scatter diagram of Kg

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

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

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

3.2 運(yùn)行測試

設(shè)計(jì)制作四旋翼飛行器，采用PID控制以PWM方式驅(qū)動電機(jī)，分別控制偏航角、俯仰角和翻滾角。整定翻滾角PID控制參數(shù)Kp為1.6，Ki為0.4，Kd為 0.3，飛行器處于懸停狀況下進(jìn)行動態(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的控制算法最后得到的控制量為控制電機(jī)的PWM的點(diǎn)空比，通過改變飛行器在旋飛時的姿態(tài)角或者通過遙控器改變飛行器的姿態(tài)角期望值，查看各個電機(jī)的PWM輸入量的變化來驗(yàn)證電機(jī)控制的效果。圖11為向前飛行時四個電機(jī)的PWM值，同樣可以得到向后飛行、向左飛行和向右飛行的PWM曲線。

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

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

4 結(jié)論

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