文獻標識碼： A
DOI：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.
0 引言
四旋翼飛行器體積小、重量輕，與載人飛機相比成本很低，可廣泛應用在交通檢測、園林維護、災區搜救等場合，目前在國內外正處于熱門研究中[1-3]。但四旋翼飛行器是一個非線性、多變量、高度耦合、欠驅動系統（6個自由度，4個輸入量），控制系統較為復雜，系統抗干擾能力差。只有采用了合適的控制方案才能夠實現平穩的飛行，這就需要對飛行器進行力學和運動學上的分析并建立相對應的數學模型，設計單片機控制電路及軟件控制系統等，實現對飛行器的穩定控制。
1 四旋翼飛行器數學模型建立
為簡化分析，假設以下條件成立：
（1）機體坐標系的原點為飛行器的質心，并且與飛行器幾何中心重合；
（2）只考慮螺旋槳產生的氣流，而空氣流動速度及空氣阻力忽略不計；
（3）飛行器機體與螺旋槳均為剛體結構，機體的幾何構造及質量對稱；
（4）螺旋槳旋轉時產生的反扭矩與轉速的平方成正比，螺旋槳產生的升力與轉速的平方成正比。
根據以上條件，設F為飛行器受到的外力和，m為飛行器質量，V為飛行速度，M為飛行器所受力矩之和，H是飛行器相對于導航坐標系的相對動量矩，受力分析如圖1所示。根據牛頓-歐拉方程對四旋翼飛行器平移運動與旋轉運動建立模型[4-7]。
根據歐拉方程
則有三軸力矩平衡方程式：
由于姿態穩定控制不需對位置和高度進行控制，只需考慮角度控制即可，穩定控制時姿態變化較小，可忽略空氣阻力的影響，這樣得到簡化后的動力學模型為：
從以上公式可以看出，在四旋翼飛行器的姿態控制模型中，對任一個角度的控制均可采用PID控制器對其進行控制。
2 硬件電路設計
2.1 硬件系統主要組成
系統硬件主要包括飛行架構、微控制器最小系統、無線通信系統、動力系統、慣性測量單元、電源等幾個部分。其中飛行架構是其他幾部分的載體，微控制器最小系統用于數據融合、姿態解算與調節，控制協調各模塊的工作。無線通信系統用于遙控飛行器和接收數據；動力系統由驅動器、電機、螺旋槳組成，為飛行器提供升力和推動力；慣性測量及系統為飛行器提供姿態和高度等信息，是飛行器系統的重要組成部分。
2.2 主要電路原理圖
系統主要電路原理圖如圖2所示。采用STM32F103C8T6高速單片機作為主控制單元，MPU6050和HMC5883L組成慣性測量及姿態檢測單元，實現對三軸的加速度、三軸陀螺儀、三軸磁場方向的檢測，輸出原始數據給微控制器，進行融合姿態解算后得到橫滾角、俯仰角、偏航角。采用空心杯直流電機，使用PWM進行調速。驅動電路由N溝道的場效應管形成的開關電路實現，選擇漏源電阻較小的場效應管，防止電機輸入電壓變小造成電機轉速的范圍變窄，便于對電機轉速調節的控制。
3 軟件系統設計
3.1 姿態解算與卡爾曼濾波[8-12]
飛行器在運行過程中，CPU通過傳感器MPU6050內部的三軸加速度傳感器、三軸陀螺儀來實時采集當前各方向姿態數據，通過解算后得到三個方向的角度、角速度值，然后調節各電機轉速，使其達到期望姿態。加速度傳感器用來測量x、y、z三個方向的加速度值，然后與重力加速度比較即可算出三個方向的角度值，但是存在一定誤差。三軸陀螺儀傳感器用來測量x、y、z三個方向角速度，乘以時間即為角度。
通過陀螺儀計算出來的角度存在一定的誤差，多次累加以后累積誤差越來越大，最終導致計算出的角度與實際角度相差很大。本文的解決方法是把加速度計讀出的角度與陀螺儀計算出的角度相結合，通過卡爾曼濾波計算出更為準確的角度值。
3.2 四旋翼飛行器PID控制器設計[13-15]
通過傳感器準確獲取角度、角速度數據后，再與期望姿態角進行對比，實現對飛行器姿態的調節控制。控制系統采用位置式PID控制器進行調節，四旋翼飛行器主要包括橫滾角、俯仰角、偏航角3種姿態，只有對3個姿態角可控，才能實現對四旋翼飛行器的懸停、前進、后退、左轉、右轉等控制，因此需設計三路串級PID控制器，其框圖如圖3所示。
在設計中，外環采用角度環進行PI控制，內環采用角速度環進行D控制。設置這樣的串級PID可以使飛行器在大角度偏差時修正速度快，小角度偏差時修正速度慢。外環期望角度為設定值，如懸停的期望值為0°，而實際角度值由卡爾曼濾波后的角度數據提供。外環PI控制器的輸出值作為內環D控制器的期望值，內環角速度實際值由傳感器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為電機輸入固定值，也稱油門。為保證飛行器穩定，內環控制器的采樣頻率為200 Hz，外環控制器采樣頻率50 Hz即可。為保證采樣時間間隔均勻，PID控制器采樣周期由高優先級定時器中斷提供。串級PID調節、卡爾曼濾波軸波形圖如圖4所示。由圖中可以看出飛行器在受到環境干擾時，通過串級PID調節可迅速讓機身恢復至平衡位置。恢復時間很短，基本無振蕩現象，說明飛行器在串級PID調節下具有較強的抗干擾能力，驗證了PID調節系統設計的可靠性。
4 系統測試
4.1 姿態角測試
四旋翼飛行器采集到加速度計和陀螺儀的數據后，經過數據融合、卡爾曼濾波將兩種數據融合在一起計算出較為精確的姿態角數據，再將數據通過串口發送到由匿名科創提供的上位機軟件，將三軸姿態角用3D效果圖直觀顯示出來。其中橫滾角姿態解算3D效果如圖5所示，由圖中可以看出系統響應穩定、姿態平穩，說明系統能很好地實現這些姿態控制。
4.2 平衡飛行測試
將飛行器半固定在水平桿上調試內外環PID各項系數，在調試俯仰這一路的PID控制器時，先將Ki和Kp設為零，Kp增大到控制回路的輸出出現臨界振蕩較為合適，當選取到12左右時，飛行器在上升過程中出現上下擺動的不穩定現象，通過設置Kd參數抵消比例作用調節時的過度反應和過沖干擾，使飛行器系統在上升時能夠趨于穩定飛行，Kd取0.05左右就可以滿足要求，為消除靜態誤差，Ki參數可以選取0.02左右。經反復調試達到穩定狀態，在室內可使飛行器平穩懸停在空中，通過調節俯仰角的大小可讓飛行器在室外水平穩定向前飛行。
5 結論
本文分析了四旋翼飛行器的控制原理，使用牛頓第二定律和歐拉方程建立了系統的數學模型，設計了以STM32高速單片機為核心的控制系統電路，通過卡爾曼濾波濾去了可能的干擾信號，保證了角度數據的精確性。設計三路串級PID調節控制器，實現了對橫滾角、俯仰角、偏航角3種姿態角控制。室內外飛行測試結果表明，系統可以穩定平衡飛行。
參考文獻
[1] 王貞琪，馬潔.四旋翼幾種控制方法比較[J].北京信息科技大學學報，2016，31(2)：35-39.
[2] 趙世榮，孫運強，石喜玲.基于信息融合四旋翼飛行器穩定性改進設計[J].電子器件，2015，38(6)：1417-1420.
[3] 郝偉，鮮斌.四旋翼無人機姿態系統的非線性容錯控制設計[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] 廖衛中，宗群，馬亞麗.小型四旋翼無人機建模與有限時間控制[J].控制理論與應用，2015，32(10)：1343-1350.
[7] 陳航科，張東升，盛曉超.四旋翼飛行器懸停狀態姿態控制建模與仿真[J].計算機仿真，2013，30(11)：41-45.
[8] 黃建，胡越黎，楊文榮，等.組合導航系統在四旋翼無人機上的實現[J].電子技術應用，2015，41(5)：167-170.
[9] 葉锃鋒，馮恩信.基于四元數和卡爾曼濾波的兩輪車姿態穩定方法[J].傳感技術學報，2012，25(4)：524-528.
[10] 吳濤，白茹，朱禮堯，等.基于卡爾曼濾波的航姿參考系統設計[J].傳感技術學報，2016，29(4)：531-535.
[11] 何川，李智，王勇軍.基于STM32的四旋翼飛行器的姿態最優估計研究[J].電子技術應用，2015，41(12)：61-64.
[12] 蔡偉平，胡越黎，楊文榮.四旋翼雙環滑模姿態控制系統設計與仿真[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] 甄紅濤，齊曉慧，夏明旗，等.四旋翼無人機魯棒自適應姿態控制[J].控制工程，2013，20(5)：915-919.