萬云霞，許倫豹，胡 龍，吳佳鑫，王言章

(吉林大學(xué)a.地球信息探測儀器教育部重點(diǎn)實(shí)驗(yàn)室;b.儀器科學(xué)與電氣工程學(xué)院，長春130026)

基于Arduino 的四旋翼飛行器控制系統(tǒng)設(shè)計(jì)

萬云霞a，b，許倫豹b，胡 龍b，吳佳鑫b，王言章a，b

(吉林大學(xué)a.地球信息探測儀器教育部重點(diǎn)實(shí)驗(yàn)室;b.儀器科學(xué)與電氣工程學(xué)院，長春130026)

為改善濾波效果，針對四旋翼飛行器濾波算法計(jì)算量大的問題，采用基于 Kalman與 DMP (Digital Motion Processing)濾波相結(jié)合的姿態(tài)數(shù)據(jù)處理算法及 PID(Proportion-Integration-Differentiation)姿態(tài)控制算法，設(shè)計(jì)了四旋翼飛行器控制系統(tǒng)。系統(tǒng)硬件由Arduino控制板及四旋翼飛行器平臺組成，在此平臺基礎(chǔ)上建立了飛行器動(dòng)力學(xué)模型并對Kalman濾波器及PID控制器參數(shù)進(jìn)行調(diào)試。實(shí)際飛行結(jié)果表明，該系統(tǒng)能對飛行姿態(tài)的偏移進(jìn)行快速調(diào)整，調(diào)整靈敏度和穩(wěn)態(tài)時(shí)間得到明顯改善，有效地完成對四旋翼飛行器的穩(wěn)定控制。

四旋翼飛行器;Kalman濾波;DMP濾波;PID控制器;姿態(tài)控制

0 引 言

四旋翼飛行器是由4個(gè)轉(zhuǎn)子推動(dòng)飛行的直升機(jī)，其方向和速度的控制是通過改變每個(gè)轉(zhuǎn)子的相對速度實(shí)現(xiàn)的。從Breguet-Richet發(fā)明世界上第1架四旋翼飛行器至今，四旋翼飛行器發(fā)展了一個(gè)多世紀(jì)，但由于結(jié)構(gòu)和操作技術(shù)的限制，大型四旋翼飛行器卻沒有得到快速發(fā)展。近年來隨著飛行控制理論、微慣導(dǎo)(MIMU:Micro Inertial Measurement Unit)、微機(jī)電(MEMS:Micro Electro Mechanical System)以及新型材料等技術(shù)的進(jìn)步，微小型四旋翼飛行器的發(fā)展愈發(fā)迅速［1］。針對四旋翼的姿態(tài)數(shù)據(jù)處理及控制問題出現(xiàn)了許多濾波算法和控制理論算法，例如高斯濾波，互補(bǔ)濾波，卡爾曼濾波等。但對于軟件濾波仍存在降低控制速度和數(shù)據(jù)穩(wěn)定度不理想的問題;同時(shí)反步(Backstepping)控制，線性二次型最優(yōu)控制(LQR:Linear Quadratic Regulator)，滑模(Sliding Mode)控制等控制算法也可應(yīng)用于飛行器控制。但因計(jì)算量大且較為繁瑣，對處理器要求較高，故筆者提出了簡易Kalman算法與硬件濾波相結(jié)合的濾波措施和簡單的PID(Proportion-Integration-Differentiation)控制算法，設(shè)計(jì)了基于Arduino的四旋翼飛行器控制系統(tǒng)。

1 飛行器姿態(tài)控制原理

以飛機(jī)中心為原點(diǎn)建立X-Y-Z坐標(biāo)系(見圖1)，機(jī)身繞X軸轉(zhuǎn)過的角度稱為翻滾角(Roll)，繞Y軸轉(zhuǎn)過的角度稱為俯仰角(Pitch)，繞Z軸轉(zhuǎn)過的角度稱為偏航角(Yaw)，這3個(gè)角度統(tǒng)稱為歐拉角(φ，φ，θ)。當(dāng)同一軸上兩電機(jī)的轉(zhuǎn)速不同時(shí)，就會產(chǎn)生傾角，其大小取決于同軸電機(jī)的轉(zhuǎn)速差。四旋翼姿態(tài)控制是通過對3個(gè)角度的調(diào)節(jié)實(shí)現(xiàn)的，增加高度控制后，就可完成簡單的飛行任務(wù)。

四旋翼有“+”和“x”兩種飛行方式，為了操作簡便，筆者采用十字飛行的方式。圖2中1、3電機(jī)順時(shí)針轉(zhuǎn)動(dòng)，同時(shí)會產(chǎn)生逆時(shí)針的扭矩;2、4電機(jī)逆時(shí)針轉(zhuǎn)動(dòng)，同時(shí)產(chǎn)生順時(shí)針的扭矩，當(dāng)兩軸的扭矩相同而抵消時(shí)，四旋翼將不會繞Z軸旋轉(zhuǎn)。

如圖2f所示，當(dāng)增大電機(jī)1的轉(zhuǎn)速而減小電機(jī)3的轉(zhuǎn)速時(shí)，飛行器將會向后飛行(沿箭頭所指方向運(yùn)動(dòng))。如果4個(gè)電機(jī)的轉(zhuǎn)速同時(shí)增大(或減小)相同的轉(zhuǎn)速，四旋翼將會爬升(或下降);當(dāng)飛行器轉(zhuǎn)速所產(chǎn)生的升力與其自身重力相同時(shí)，飛行器將會處于懸停狀態(tài)。通過控制4個(gè)電機(jī)的轉(zhuǎn)速可實(shí)現(xiàn)對四旋翼飛行方向的控制，圖2給出了各種飛行方向的轉(zhuǎn)速控制圖。

圖1 四旋翼飛行器原理圖Fig.1 Four-rotor aircraft flight principle photo

圖2 四旋翼各種飛行圖示Fig.2 Illustration of the variousmovements of a quad-rotor

2 飛行器結(jié)構(gòu)組成

四旋翼飛行器的飛行控制系統(tǒng)通常由傳感器測量裝置、主控制器、電機(jī)驅(qū)動(dòng)裝置及電機(jī)和螺旋槳等部分組成。傳感器用來測量飛行器的姿態(tài)數(shù)據(jù)，主控制器根據(jù)設(shè)定姿態(tài)對該數(shù)據(jù)進(jìn)行處理，經(jīng)過控制算法最終以4路PWM(Pulse-Width-Modulation)信號控制電機(jī)轉(zhuǎn)速并校正飛行角度，以實(shí)現(xiàn)自動(dòng)調(diào)節(jié)旋轉(zhuǎn)力矩穩(wěn)定飛行姿態(tài)。圖3、圖4分別為四旋翼飛行器控制構(gòu)架和飛行器實(shí)物圖。

圖3 四旋翼飛行器控制架構(gòu)Fig.3 Control architecture of four-rotor aircraft

圖4 四旋翼飛行器實(shí)物圖Fig.4 Quad-rotor

2.1 傳感器測量裝置

為獲得四旋翼姿態(tài)數(shù)據(jù)，系統(tǒng)選用MPU-6050作為姿態(tài)傳感器，獲取3軸的角速率和線性加速度。該傳感器為全球首例6軸運(yùn)動(dòng)處理傳感器，將一個(gè)3軸MEMS陀螺、一個(gè)3軸MEMS加速度計(jì)以及一個(gè)數(shù)字運(yùn)動(dòng)處理器DMP(Digital Motion Processor)集成一體，其中DMP自帶硬件濾波器，通過編程得到穩(wěn)定的姿態(tài)數(shù)據(jù)［2］。

2.2 主控制器

圖3所示的控制算法是在基于Arduino硬件平臺上開發(fā)完成的。系統(tǒng)選用Arduino Uno R3作為主控制器，該平臺核心處理器采用ATmega328，具有14路數(shù)字輸入/輸出口(其中6路作為PWM輸出)，6路模擬輸入，串口、I2C和SPI通信接口以及10位內(nèi)置A/D，能滿足系統(tǒng)設(shè)計(jì)要求［3］。

2.3 電源及電機(jī)驅(qū)動(dòng)

四旋翼飛行器的電機(jī)需要滿足長時(shí)間高速旋轉(zhuǎn)的要求，一般選擇無刷直流電機(jī)(BLDC:Brushless Direct Current)，可運(yùn)用數(shù)字變頻控制技術(shù)進(jìn)行控制并具有較大的可調(diào)轉(zhuǎn)速范圍(1～10 000 rad/s)，噪音小且維護(hù)方便［4］。無刷直流電調(diào)(ESC:Electronic Speed Control)用來驅(qū)動(dòng)無刷直流電機(jī)的驅(qū)動(dòng)器件，并根據(jù)控制信號(PWM波占空比)的變化調(diào)節(jié)電機(jī)的轉(zhuǎn)速。飛行器動(dòng)力電源選用輸出電壓為11.1 V的3 s鋰鐵電池，能滿足電調(diào)對大輸出電流的要求且攜帶方便。另外螺旋槳選用5045規(guī)格的正反槳。

3 數(shù)學(xué)模型

3.1 模型建立

四旋翼飛行器共有4個(gè)固定的推力角度轉(zhuǎn)子，代表4個(gè)由相應(yīng)螺旋槳產(chǎn)生的輸入力量。對四旋翼的控制實(shí)際是對歐拉角(φ，φ，θ)以及空間坐標(biāo)系中的3個(gè)方向上的位移(x、y、z)控制。可以看出，系統(tǒng)只有4個(gè)輸入輸出力而卻有6個(gè)輸出控制狀態(tài)(x、y、z軸，φ，φ，θ)，因此，四旋翼飛行器是個(gè)六自由度的欠穩(wěn)定系統(tǒng)［4］。

為了建立不失一般性的動(dòng)力學(xué)模型，對四旋翼飛行器做出以下4點(diǎn)假設(shè):1)把四旋翼飛行器當(dāng)做均勻、對稱的剛體;2)慣性坐標(biāo)系E的原點(diǎn)位置與四旋翼飛行器的幾何中心、質(zhì)心重合;3)四旋翼飛行器所受的阻力與重力不受高度、區(qū)域的影響而保持不變;4)螺旋槳所產(chǎn)生的力與電機(jī)轉(zhuǎn)速的平方成正比。此外為了對四旋翼飛行器的空間位置進(jìn)行精確控制建立方向余弦轉(zhuǎn)換矩陣［5］

地面坐標(biāo)為笛卡爾直角坐標(biāo)系［5］。其中φ為翻滾角、φ為偏航角、θ為俯仰角。

利用力與力矩平衡關(guān)系的運(yùn)動(dòng)方程以及拉格朗日方法可將飛行器的動(dòng)力學(xué)模型簡化為如下形式［6］

其中g(shù)為重力加速度，m為飛行器的總質(zhì)量，K1、K2與K3為飛行器各方向上的阻力系數(shù)，當(dāng)轉(zhuǎn)速較低時(shí)可忽略飛行器所受的阻力。

假定飛行器的重心在設(shè)定原點(diǎn)處向上(或向下)移動(dòng)了d個(gè)單位，從而降低了角加速度對力敏感程度，因而使系統(tǒng)的穩(wěn)定性得到提高。為方便運(yùn)算，用以下方程式表達(dá)飛行器各個(gè)方向上的輸入力矩

其中U1為4個(gè)電機(jī)總共產(chǎn)生的垂直升力，U2為俯仰通道所受力矩，U3為偏航通道所受力矩，U4為滾動(dòng)通道所受力矩，Hi(i=1，2，3，4)為飛行器上相應(yīng)電機(jī)產(chǎn)生的推力，Ii(i=1，2，3)為以飛行器自身建立的坐標(biāo)系的3個(gè)軸上所受的轉(zhuǎn)動(dòng)慣量。飛行器的歐拉角表示如下［6］

其中l(wèi)為飛行器支架的半軸長度，Ki(i=4，5，6)代表阻力系數(shù)，分別以X，Y，Z和φ為一組控制輸出。飛行器控制系統(tǒng)數(shù)學(xué)模型框如圖5所示。

圖5 四旋翼飛行器控制系統(tǒng)數(shù)學(xué)模型框圖Fig.5 Block diagram modeling of four rotor-craft control system

3．2 PID系統(tǒng)狀態(tài)控制方程

由于對飛行器進(jìn)行仿真需要整體的系統(tǒng)傳遞函數(shù)，通過對四旋翼進(jìn)行數(shù)學(xué)建模已得到相關(guān)輸入、輸出的變量關(guān)系，設(shè)狀態(tài)空間方程的形式如下

其中U=(U1，U2，U3，U4)T為輸入矢量，X=(x，y，z，˙x，˙y，˙z，θ，φ，φ，˙θ，˙φ，˙φ，g)T為狀態(tài)矢量，Y=(x，y，z，φ，θ，φ)T為輸出量，則由數(shù)學(xué)模型可得系統(tǒng)的狀態(tài)空間方程的各矩陣如下［6］

將對飛行器進(jìn)行測量得到的參數(shù)帶入式(11)并用Matlab求出相應(yīng)狀態(tài)空間方程的傳遞函數(shù)，由于筆者僅針對四旋翼飛行器的懸停狀態(tài)進(jìn)行仿真，故θ=φ≈0，傳遞函數(shù)可簡化為

4 姿態(tài)數(shù)據(jù)處理及系統(tǒng)控制

4．1 濾波處理

飛行器的姿態(tài)數(shù)據(jù)質(zhì)量直接影響系統(tǒng)控制精度，故采用硬件與軟件相結(jié)合的方案實(shí)現(xiàn)對噪聲較大且不穩(wěn)定的姿態(tài)數(shù)據(jù)進(jìn)行濾波處理。硬件濾波采用MPU-6050自帶的數(shù)字運(yùn)動(dòng)處理器(DMP:Digital Motion Processing)，它可接收并處理來自陀螺儀、加速度計(jì)的數(shù)據(jù)，處理結(jié)果可直接從DMP寄存器讀出，且比從一般的加速度計(jì)與陀螺儀讀出的數(shù)據(jù)更加穩(wěn)定，減少了飛行過程中因震動(dòng)和外部環(huán)境所產(chǎn)生的噪聲。

為了進(jìn)一步提高數(shù)據(jù)的可靠性，對數(shù)據(jù)進(jìn)行軟件濾波?？柭鼮V波算法能在一系列受噪聲干擾的數(shù)據(jù)中準(zhǔn)確估計(jì)動(dòng)態(tài)系統(tǒng)的狀態(tài)?？柭鼮V波算法具有編程簡單、能對現(xiàn)場采集的數(shù)據(jù)進(jìn)行實(shí)時(shí)更新和處理的優(yōu)點(diǎn)，因而采用卡爾曼濾波算法對采集的數(shù)據(jù)進(jìn)行融合得到正確的姿態(tài)角，從而更有利于對飛行器進(jìn)行控制。

卡爾曼濾波算法具體的實(shí)現(xiàn)流程如圖6所示［7］。

圖6 卡爾曼濾波算法流程圖Fig.6 Flowchart of Kalman filter algorithm

圖7 為濾波后的示意圖，在靜態(tài)時(shí)測得，X軸角速率漂移較大，加速度受噪聲干擾強(qiáng);經(jīng)過DMP與卡爾曼算法濾波后，角度數(shù)據(jù)穩(wěn)定在-0.01°附近，信噪比與穩(wěn)定性得到較大改善。

圖7 數(shù)據(jù)處理對比圖Fig.7 Data processing comparison chart

4.2 姿態(tài)控制算法

經(jīng)過硬件、軟件濾波后的姿態(tài)數(shù)據(jù)，其信噪比與數(shù)據(jù)質(zhì)量有明顯改善，有利于進(jìn)行姿態(tài)控制。該系統(tǒng)的姿態(tài)控制算法采用PID控制。PID控制器是一種線性控制器。它由被控量的期望值r(t)與實(shí)際輸出值y(t)構(gòu)成控制偏差e(t)，然后將偏差e(t)按比例(P)、積分(I)、微分(D)的關(guān)系通過線性組合構(gòu)成控制量u(t)去控制被控對象［8，9］，其輸出和輸入之間關(guān)系可描述為

其中Kp為比例系數(shù)，Ti為積分時(shí)間常數(shù)，Td為微分時(shí)間常數(shù)。

系統(tǒng)控制算法以角速率為內(nèi)環(huán)反饋和角度為外環(huán)反饋組成的內(nèi)外環(huán)嵌套控制實(shí)現(xiàn)對飛行姿態(tài)調(diào)整［10］。在對PID參數(shù)進(jìn)行調(diào)試實(shí)驗(yàn)中，采用PI(Proportion-Integration)控制器比PID控制器的控制效果更好。PID姿態(tài)控制器原理框圖如圖8所示。

控制器外環(huán)是角度的PI控制，內(nèi)環(huán)是角速率的PI控制。期望角度與傳感器測得的實(shí)際角度經(jīng)比較后，誤差經(jīng)控制器處理并以此輸出量作為內(nèi)環(huán)的輸入量，該輸出量與傳感器測得的角速率進(jìn)行比較后得到最終輸出值。經(jīng)過轉(zhuǎn)換后以PWM信號作為控制量控制電機(jī)的轉(zhuǎn)速。其中內(nèi)環(huán)的穩(wěn)定是實(shí)現(xiàn)四旋翼穩(wěn)定飛行的關(guān)鍵因素。在實(shí)際調(diào)試中只用P時(shí)外環(huán)往往也能實(shí)現(xiàn)較好的角度控制。

在實(shí)現(xiàn)高度控制時(shí)可以高度進(jìn)行粗略控制和精確鎖定，在高度控制的基礎(chǔ)上可實(shí)現(xiàn)飛行器的懸?？刂啤＞_PID高度控制器框圖如圖9所示。

圖8 PID姿態(tài)控制器原理框圖Fig.8 PID Controller of attitude

圖9 精確高度PID控制器Fig.9 Precise PID controller of height

精確高度PID控制系統(tǒng)中，飛行器高度、垂直方向的速度和Z軸方向的加速度為需要的實(shí)測值。其中加速度控制器執(zhí)行速率為100 Hz，速率控制器執(zhí)行速率為50 Hz。而在一般飛行中不需要對高度進(jìn)行精確控制，只需四旋翼維持在設(shè)定的高度，所以可采用圖10所示的簡單控制方式。

圖10 簡單高度PID控制器Fig.10 Simple PID controller of height

5 系統(tǒng)仿真

圖11 單通道PID控制流程圖Fig.11 Flow of signal-channel of PID control

由圖12可見，當(dāng)參數(shù)值較合適時(shí)系統(tǒng)響應(yīng)時(shí)間較短、超調(diào)較低、延遲時(shí)間較短，從而能保證四旋翼飛行器的姿態(tài)角與期望角有偏差時(shí)能通過PID調(diào)節(jié)迅速地達(dá)到期望姿態(tài)。若需要更加快速地調(diào)節(jié)則需更細(xì)致地改變參數(shù)觀察響應(yīng)曲線。

6 結(jié) 語

圖12 俯仰通道階躍響應(yīng)Fig.12 The step response of pitch channel

筆者完成了四旋翼飛行器控制結(jié)構(gòu)的設(shè)計(jì)，通過DMP與Kalman濾波算法提高了PID處理數(shù)據(jù)的信噪比，并在系統(tǒng)建模的基礎(chǔ)上對飛行器的姿態(tài)控制與高度調(diào)節(jié)進(jìn)行了設(shè)計(jì)。系統(tǒng)仿真證明了PID控制的實(shí)效性，實(shí)際飛行結(jié)果表明了該控制系統(tǒng)在四旋翼飛行器姿態(tài)控制方面的有效性。筆者所進(jìn)行的理論研究與實(shí)驗(yàn)結(jié)果為四旋翼飛行器的自穩(wěn)定控制提供了有價(jià)值的參考。

［1］聶博文，馬宏緒，王劍，等.微小型四旋翼飛行器的研究現(xiàn)狀與關(guān)鍵技術(shù)［J］.電光與控制，2007，14(6):113-117. NIE Bowen，MA Hongxu，WANG Jian，etal.Small Quad-Totor Aircraft Present Research Situation and Key Technology［J］. Electronics Optics＆Control，2007，14(6):113-117.

［2］賴義漢，王凱.基于 MPU6050的雙輪平衡車控制系統(tǒng)設(shè)計(jì)［J］.河南工程學(xué)院學(xué)報(bào):自然科學(xué)版，2014，26(1):53-57. LAIYihan，WANG Kai.Two-Wheel Balancing Car Control System Based on MPU6050［J］.Journal of Henan Institute ofEngineering:Natural Science Edition，2014，26(1):53-57.

［3］蔡睿妍.Arduino的原理及應(yīng)用［J］.電子設(shè)計(jì)工程，2012，20(16):155-157. CAIRuiyan.The Principle and Application of Arduino［J］.Electronic Design Engineering，2012，20(16):155-157.

［4］李秀英，劉彥博.基于PWM的四旋翼飛行器控制方法［J］.吉林大學(xué)學(xué)報(bào):信息科學(xué)版，2011，29(5):464-472. LIXiuying，LIU Yanbo.Control Method of Quad-Totor Based on PWM［J］.Journal of Jilin University:Information Science Edition，2011，29(5):464-472.

［5］國倩倩.微型四旋翼飛行器控制系統(tǒng)設(shè)計(jì)及控制方法研究［D］.長春:吉林大學(xué)通信工程學(xué)院，2013. GUO Qianqian.Research of Micro Quad in Design and Control Method［D］.Changchun:College of Communication Engineering，Jilin University，2013.

［6］MIAN A A，DAOBOW.Modeling and Backstepping-Based Nonlinear Control Strategy for a 6 DOFQuadrotor Helicopter［J］. Chinese Journal of Aeronautics，2008，21(3):261-268.

［7］王帥，魏國.卡爾曼濾波在四旋翼飛行器姿態(tài)測量中的應(yīng)用［J］.兵工自動(dòng)化，2011，30(1):73-80. WANG Shuai，WEIGuo.Application of Kalman Filter in the Attitude Measurement of Quad-Rotor［J］.Ordnance Industry Automation，2011，30(1):73-80.

［8］王素青，姜維福.基于MATLAB/Simulink的PID參數(shù)整定［J］.自動(dòng)化技術(shù)與應(yīng)用，2009，28(3):24-25. WANG Suqing，JIANGWeifu.PID Parameters Tuning Based on MATLAB/Simulink［J］.Industrial Control and Application，2009，28(3):24-25.

［9］段世華.四旋翼飛行器控制系統(tǒng)的設(shè)計(jì)和實(shí)現(xiàn)［D］.成都:成都電子科技大學(xué)自動(dòng)化工程學(xué)院，2012. DUAN Shihua.Design and Implementation of Quad Control System［D］.Chengdu:School of Automation Engineering，Chengdu Electronic Technology University，2012.

［10］LIM H，PARK J，LEE D，et al.Build Your Own Quadrotor:Open-Source Projects on Unmanned Aerial Vehicles［J］. Robotics＆Automation Magazine，IEEE，2012，19(3):33-45.

［11］PATEL V K R S，PANDEY A K.Modeling and Performance Analysis of PID Controlled BLDCMotor and Different Schemes of PWM Controlled BLDCMotor［J］.International Journal of Scientific and Research Publications，2013，3(4):1-14.

(責(zé)任編輯:劉俏亮)

Design of Quad-Rotor Control System Based on Arduino

WAN Yunxiaa，b，XU Lunbaob，HU Longb，WU Jiaxinb，WANG Yanzhanga，b

(a.Key Laboratory for Geophysical Instrumentation of Ministry of Education; b.College of Instrumentation and Electrical Engineering，Jilin University，Changchun 130026，China)

In order to adjust the flightattitude of the quad-rotor，an attitude data processing jointalgorithm based on Kalman and DMP(Digital Motion Processing) filter was adopted，and PID(Proportion-Integration-Differentiation)algorithm was used to control its flight attitude.Based on the hardware platform，the dynamic model of the aircraftwas established and the parameters of Kalman filter and PID controller were debugged.The flying experiment shows that after using the control algorithm，the control system can quickly adjust the flight attitude offset to control the quad-rotor effectively.

quad-rotor; Kalman filtering; digital motion processing(DMP); proportion-integrationdifferentiation(PID)controller;attitude control

TP273

A

1671-5896(2015)04-0389-08

2015-03-11

吉林省重大科技攻關(guān)基金資助項(xiàng)目(20140203015GX);吉林大學(xué)“大學(xué)生創(chuàng)新創(chuàng)業(yè)訓(xùn)練計(jì)劃”基金資助項(xiàng)目(2014B65250)

萬云霞(1980— )，女，山東煙臺人，吉林大學(xué)工程師，主要從事地球探測儀器、數(shù)字信號處理研究，(Tel)86-1894361464 (E-mail)wangyx@jlu.edu.cn。