基于STM32單片機四旋翼飛行器建模分析與設計*

曾憲陽，楊紅莉，郁漢琪

(1.南京工程學院 工業(yè)中心，江蘇 南京 211167；2.南京大學 數(shù)學系，江蘇 南京 210093；3.南京工程學院 數(shù)理部，江蘇 南京211167)

基于STM32單片機四旋翼飛行器建模分析與設計*

曾憲陽1，2，楊紅莉2，3，郁漢琪1

(1.南京工程學院 工業(yè)中心，江蘇 南京 211167；2.南京大學 數(shù)學系，江蘇 南京 210093；3.南京工程學院 數(shù)理部，江蘇 南京211167)

針對四旋翼飛行器穩(wěn)定性差、控制難的問題，使用牛頓-歐拉方程建立了數(shù)學模型，提出了姿態(tài)解算的實現(xiàn)方法。設計了以STM32單片機為控制核心，加速度計、陀螺儀及磁力計等組成的硬件控制電路。提出將加速度計解算出的角度數(shù)據(jù)與陀螺儀解算出的角度數(shù)據(jù)進行融合，通過卡爾曼濾波濾去干擾信號，保證了角度數(shù)據(jù)的準確性。設計了三路串級PID控制器，通過對橫滾角、俯仰角、偏航角3種姿態(tài)角進行控制，實現(xiàn)了對飛行器的懸停、前進、后退、左轉、右轉等控制。經室內外飛行測試表明，飛行器可以平穩(wěn)飛行。

四旋翼飛行器；STM32單片機；歐拉方程；數(shù)學模型；卡爾曼濾波；PID調節(jié)

0 引言

四旋翼飛行器體積小、重量輕，與載人飛機相比成本很低，可廣泛應用在交通檢測、園林維護、災區(qū)搜救等場合，目前在國內外正處于熱門研究中[1-3]。但四旋翼飛行器是一個非線性、多變量、高度耦合、欠驅動系統(tǒng)（6個自由度，4個輸入量），控制系統(tǒng)較為復雜，系統(tǒng)抗干擾能力差。只有采用了合適的控制方案才能夠實現(xiàn)平穩(wěn)的飛行，這就需要對飛行器進行力學和運動學上的分析并建立相對應的數(shù)學模型，設計單片機控制電路及軟件控制系統(tǒng)等，實現(xiàn)對飛行器的穩(wěn)定控制。

1 四旋翼飛行器數(shù)學模型建立

為簡化分析，假設以下條件成立：

（1）機體坐標系的原點為飛行器的質心，并且與飛行器幾何中心重合；

（2）只考慮螺旋槳產生的氣流，而空氣流動速度及空氣阻力忽略不計；

（3）飛行器機體與螺旋槳均為剛體結構，機體的幾何構造及質量對稱；

（4）螺旋槳旋轉時產生的反扭矩與轉速的平方成正比，螺旋槳產生的升力與轉速的平方成正比。

根據(jù)以上條件，設F為飛行器受到的外力和，m為飛行器質量，V為飛行速度，M為飛行器所受力矩之和，H是飛行器相對于導航坐標系的相對動量矩，受力分析如圖1所示。根據(jù)牛頓-歐拉方程對四旋翼飛行器平移運動與旋轉運動建立模型[4-7]。

圖1 四旋翼飛行器受力分析

設螺旋槳的轉速為Ωn，根據(jù)假設條件(4)得出升力為，其中 b為升力系數(shù)，則總升力為：

在機體坐標系中飛行器的受力向量為FB=[0 0 U1]，導航坐標系E中的受力向量FE為：

在導航坐標系 ΓE=[x y z]T下，沿 x、y、z三個軸向上受到的阻力系數(shù)為 Kx、Ky、Kz，根據(jù)牛頓第二定律建立沿導航坐標系的三軸受力方程式：

根據(jù)角動量守恒定律，設繞x、y、z三個軸上的受到的力矩分別為 Mφ、Mθ、Mψ，所受阻力系數(shù)分別為 Kxφ、Kyθ、Kzψ，根據(jù)假設條件(1)和(3)，將慣性張量定義為對角陣I：

由于姿態(tài)穩(wěn)定控制不需對位置和高度進行控制，只需考慮角度控制即可，穩(wěn)定控制時姿態(tài)變化較小，可忽略空氣阻力的影響，這樣得到簡化后的動力學模型為：

從以上公式可以看出，在四旋翼飛行器的姿態(tài)控制模型中，對任一個角度的控制均可采用PID控制器對其進行控制。

2 硬件電路設計

2.1 硬件系統(tǒng)主要組成

系統(tǒng)硬件主要包括飛行架構、微控制器最小系統(tǒng)、無線通信系統(tǒng)、動力系統(tǒng)、慣性測量單元、電源等幾個部分。其中飛行架構是其他幾部分的載體，微控制器最小系統(tǒng)用于數(shù)據(jù)融合、姿態(tài)解算與調節(jié)，控制協(xié)調各模塊的工作。無線通信系統(tǒng)用于遙控飛行器和接收數(shù)據(jù)；動力系統(tǒng)由驅動器、電機、螺旋槳組成，為飛行器提供升力和推動力；慣性測量及系統(tǒng)為飛行器提供姿態(tài)和高度等信息，是飛行器系統(tǒng)的重要組成部分。

2.2 主要電路原理圖

系統(tǒng)主要電路原理圖如圖2所示。采用STM32F103-C8T6高速單片機作為主控制單元，MPU6050和HMC5883L組成慣性測量及姿態(tài)檢測單元，實現(xiàn)對三軸的加速度、三軸陀螺儀、三軸磁場方向的檢測，輸出原始數(shù)據(jù)給微控制器，進行融合姿態(tài)解算后得到橫滾角、俯仰角、偏航角。采用空心杯直流電機，使用PWM進行調速。驅動電路由N溝道的場效應管形成的開關電路實現(xiàn)，選擇漏源電阻較小的場效應管，防止電機輸入電壓變小造成電機轉速的范圍變窄，便于對電機轉速調節(jié)的控制。

圖2 系統(tǒng)主要電路原理圖

3 軟件系統(tǒng)設計

3.1 姿態(tài)解算與卡爾曼濾波[8-12]

飛行器在運行過程中，CPU通過傳感器MPU6050內部的三軸加速度傳感器、三軸陀螺儀來實時采集當前各方向姿態(tài)數(shù)據(jù)，通過解算后得到三個方向的角度、角速度值，然后調節(jié)各電機轉速，使其達到期望姿態(tài)。加速度傳感器用來測量x、y、z三個方向的加速度值，然后與重力加速度比較即可算出三個方向的角度值，但是存在一定誤差。三軸陀螺儀傳感器用來測量 x、y、z三個方向角速度，乘以時間即為角度。

通過陀螺儀計算出來的角度存在一定的誤差，多次累加以后累積誤差越來越大，最終導致計算出的角度與實際角度相差很大。本文的解決方法是把加速度計讀出的角度與陀螺儀計算出的角度相結合，通過卡爾曼濾波計算出更為準確的角度值。

3.2 四旋翼飛行器 PID控制器設計[13-15]

通過傳感器準確獲取角度、角速度數(shù)據(jù)后，再與期望姿態(tài)角進行對比，實現(xiàn)對飛行器姿態(tài)的調節(jié)控制?？刂葡到y(tǒng)采用位置式PID控制器進行調節(jié)，四旋翼飛行器主要包括橫滾角、俯仰角、偏航角3種姿態(tài)，只有對3個姿態(tài)角可控，才能實現(xiàn)對四旋翼飛行器的懸停、前進、后退、左轉、右轉等控制，因此需設計三路串級 PID控制器，其框圖如圖3所示。

圖3 三路串級PID控制器框圖

在設計中，外環(huán)采用角度環(huán)進行PI控制，內環(huán)采用角速度環(huán)進行D控制。設置這樣的串級PID可以使飛行器在大角度偏差時修正速度快，小角度偏差時修正速度慢。外環(huán)期望角度為設定值，如懸停的期望值為0°，而實際角度值由卡爾曼濾波后的角度數(shù)據(jù)提供。外環(huán)PI控制器的輸出值作為內環(huán)D控制器的期望值，內環(huán)角速度實際值由傳感器 MPU6050內部的陀螺儀提供。三路PID控制器的輸出分別為 roll_out、pitch_out、yaw_out，分別以PWM的形式輸出給4個電機驅動電路，每個電機PWM線性組合如下：

Y軸前端電機：Throttle-roll_out-yaw_out；

Y軸后端電機：Throttle+pitch_out-yaw_out；

X軸右端電機：Throttle-roll_out+yaw_out；

X軸左端電機：Throttle+roll_out+yaw_out。

其中Throttle為電機輸入固定值，也稱油門。為保證飛行器穩(wěn)定，內環(huán)控制器的采樣頻率為200 Hz，外環(huán)控制器采樣頻率50 Hz即可。為保證采樣時間間隔均勻，PID控制器采樣周期由高優(yōu)先級定時器中斷提供。串級PID調節(jié)、卡爾曼濾波軸波形圖如圖4所示。由圖中可以看出飛行器在受到環(huán)境干擾時，通過串級PID調節(jié)可迅速讓機身恢復至平衡位置。恢復時間很短，基本無振蕩現(xiàn)象，說明飛行器在串級PID調節(jié)下具有較強的抗干擾能力，驗證了PID調節(jié)系統(tǒng)設計的可靠性。

圖4 串級PID調節(jié)、卡爾曼濾波前后X軸波形圖

4 系統(tǒng)測試

4.1 姿態(tài)角測試

四旋翼飛行器采集到加速度計和陀螺儀的數(shù)據(jù)后，經過數(shù)據(jù)融合、卡爾曼濾波將兩種數(shù)據(jù)融合在一起計算出較為精確的姿態(tài)角數(shù)據(jù)，再將數(shù)據(jù)通過串口發(fā)送到由匿名科創(chuàng)提供的上位機軟件，將三軸姿態(tài)角用3D效果圖直觀顯示出來。其中橫滾角姿態(tài)解算3D效果如圖5所示，由圖中可以看出系統(tǒng)響應穩(wěn)定、姿態(tài)平穩(wěn)，說明系統(tǒng)能很好地實現(xiàn)這些姿態(tài)控制。

圖5 三軸姿態(tài)解算3D效果圖——橫滾角解算效果圖

4.2 平衡飛行測試

將飛行器半固定在水平桿上調試內外環(huán)PID各項系數(shù)，在調試俯仰這一路的PID控制器時，先將 Ki和Kp設為零，Kp增大到控制回路的輸出出現(xiàn)臨界振蕩較為合適，當選取到12左右時，飛行器在上升過程中出現(xiàn)上下擺動的不穩(wěn)定現(xiàn)象，通過設置Kd參數(shù)抵消比例作用調節(jié)時的過度反應和過沖干擾，使飛行器系統(tǒng)在上升時能夠趨于穩(wěn)定飛行，Kd取 0.05左右就可以滿足要求，為消除靜態(tài)誤差，Ki參數(shù)可以選取 0.02左右。經反復調試達到穩(wěn)定狀態(tài)，在室內可使飛行器平穩(wěn)懸停在空中，通過調節(jié)俯仰角的大小可讓飛行器在室外水平穩(wěn)定向前飛行。

5 結論

本文分析了四旋翼飛行器的控制原理，使用牛頓第二定律和歐拉方程建立了系統(tǒng)的數(shù)學模型，設計了以STM32高速單片機為核心的控制系統(tǒng)電路，通過卡爾曼濾波濾去了可能的干擾信號，保證了角度數(shù)據(jù)的精確性。設計三路串級PID調節(jié)控制器，實現(xiàn)了對橫滾角、俯仰角、偏航角3種姿態(tài)角控制。室內外飛行測試結果表明，系統(tǒng)可以穩(wěn)定平衡飛行。

[1]王貞琪，馬潔.四旋翼幾種控制方法比較[J].北京信息科技大學學報，2016，31(2)：35-39.

[2]趙世榮，孫運強，石喜玲.基于信息融合四旋翼飛行器穩(wěn)定性改進設計[J].電子器件，2015，38(6)：1417-1420.

[3]郝偉，鮮斌.四旋翼無人機姿態(tài)系統(tǒng)的非線性容錯控制設計[J].控制理論與應用，2015，32(11)：1457-1463.

[4]SCHREIER M，DARMSTADT T.Modeling and adaptive control of a quadrotor[C].Proceedings of 2012 IEEE International Conference on Mechatronics and Automation，IEEE，2012：383-390.

[5]王偉，邱啟明.六旋翼飛行器建模及位置跟蹤控制[J].電子器件，2014，37(3)：507-512.

[6]廖衛(wèi)中，宗群，馬亞麗.小型四旋翼無人機建模與有限時間控制[J].控制理論與應用，2015，32(10)：1343-1350.

[7]陳航科，張東升，盛曉超.四旋翼飛行器懸停狀態(tài)姿態(tài)控制建模與仿真[J].計算機仿真，2013，30(11)：41-45.

[8]黃建，胡越黎，楊文榮，等.組合導航系統(tǒng)在四旋翼無人機上的實現(xiàn)[J].電子技術應用，2015，41(5)：167-170.

[9]葉锃鋒，馮恩信.基于四元數(shù)和卡爾曼濾波的兩輪車姿態(tài)穩(wěn)定方法[J].傳感技術學報，2012，25(4)：524-528.

[10]吳濤，白茹，朱禮堯，等.基于卡爾曼濾波的航姿參考系統(tǒng)設計[J].傳感技術學報，2016，29(4)：531-535.

[11]何川，李智，王勇軍.基于STM32的四旋翼飛行器的姿態(tài)最優(yōu)估計研究[J].電子技術應用，2015，41(12)：61-64.

[12]蔡偉平，胡越黎，楊文榮.四旋翼雙環(huán)滑模姿態(tài)控制系統(tǒng)設計與仿真[J].電子技術應用，2015，41(7)：150-153.

[13]SADEGHZADEH I，MEHTA A，ZHANG Y M，et al.Faulttolerant trajectory tracking control of a quadrotor helicopter using gainscheduled PID and model reference adaptive control[C].Annual Conference of the Prognostics and Health Management Society.Montreal，2011，9：1-10.

[14]彭程，白越，喬冠宇.四旋翼無人機的偏航抗飽和與多模式PID控制[J].機器人，2015，37(4)：415-423.

[15]甄紅濤，齊曉慧，夏明旗，等.四旋翼無人機魯棒自適應姿態(tài)控制[J].控制工程，2013，20(5)：915-919.

Modeling analysis and design for four rotor aircraft based on the STM32 MCU

Zeng Xianyang1，2，Yang Hongli2，3，Yu Hanqi1
(1.Industry Center，Nanjing Institute of Technology，Nanjing 211167，China；2.Department of Mathematics，Nanjing University，Nanjing 210093，China；
3.Department of Mathematics and Physics，Nanjing Institute of Technology，Nanjing 211167，China)

Because of the low stability and the difficulty in controlling,Newton-Euler equation is used to establish a mathematical model for the four-rotor aircraft,the implementation of attitude algorithm is proposed too.The hardware control circuit is designed，which chooses the STM32 MCU as the core and is composed of accelerometer,gyroscope,magnetometers and so on.In order to get accurate data of angles,Kalman filter is used to filter the interfering signal from the data which is calculated from the accelerometer and from the gyroscope.To make the aircraft hover,fly forward,fly backward,turn left,turn right,etc.,a three-way cascade PID controller is adjusted which can control three kinds of attitude angle,i.e.,the roll angle,pitch angle and yaw angle.The flight tests indoor and outdoor show that the aircraft can fly smoothly.

four-rotor aircraft；STM32 MCU；Euler′s equation；mathematical model；Kalman filter；PID control

V212.4

A

10.16157/j.issn.0258-7998.2016.12.017

曾憲陽，楊紅莉，郁漢琪.基于 STM32單片機四旋翼飛行器建模分析與設計[J].電子技術應用，2016，42 (12)：65-68.

英文引用格式：Zeng Xianyang，Yang Hongli，Yu Hanqi.Modeling analysis and design for four rotor aircraft based on the STM32 MCU[J].Application of Electronic Technique，2016，42(12)：65-68.

2016-07-11)

曾憲陽（1979-），男，博士研究生，主要研究方向：電子與通信、控制理論與控制工程。

楊紅莉（1980-），通信作者，女，博士后，主要研究方向：算法分析與設計、系統(tǒng)建模與仿真等，E-mail：yanghongli1016@163.com。

郁漢琪（1961-），男，教授，主要研究方向：電子技術應用。

南京工程學院青年基金重點項目(QKJA201404)；南京工程學院校級教學改革項目(JG201440)；南京工程學院創(chuàng)新基金重大項目(CKJA201410)