基于四元數的四軸飛行器姿態控制

荊學東 潘翔

摘 要： 為了實現四軸飛行器的姿態控制，建立四軸飛行器四元數運動學方程，給出了四元數微分方程的解析解和數值解，在此基礎上求解出歐拉角。四軸飛行器采用串級PID控制算法，以歐拉角作為系統外環，補償后的角速度作為系統內環。通過Matlab/Simulink仿真，對比單級PID控制效果，驗證了算法的可靠性。最后，搭建了飛行器試驗平臺，在STM32飛控板上編程實現算法。實驗證明，該控制系統較單級PID具有響應速度快，超調量小等優點，基本滿足室外飛行要求。

關鍵詞： 四軸飛行器； 四元數法； PID控制算法； 串級PID； 姿態控制； 微分方程

中圖分類號： TN967.6?34； TP273 文獻標識碼： A 文章編號： 1004?373X（2018）16?0116?04

Abstract： A quaternion kinematic equation for the quadrotor aircraft is established， the analytical solution and numerical solution of the quaternion differential equation are given， and the Euler angle is solved on this basis， so as to realize attitude control of the quadrotor aircraft. The cascade PID control algorithm is used for the quadrotor aircraft， taking the Euler angle as the outer ring of the system and angular velocity after compensation as the inner ring of the system. The reliability of the algorithm is verified by using the Matlab/Simulink simulation to compare the effects of single?level PID control and cascade PID control. The aircraft test platform is established. The algorithm is implemented by means of programming on the STM32 flight control board. The experimental results show that in comparison with the single?level PID， the control system has the advantages of faster response speed and less overshoot， which can basically meet the requirements of outdoor flight.

Keywords： quadrotor aircraft； quaternion method； PID control algorithm； cascade PID； attitude control； differential equation

四軸飛行器與普通的飛行器相比具有結構簡單、故障率低及單位體積能夠產生更大升力等優點，在軍事、民用和科技領域發揮著越來越重要的作用，非常適合在狹小空間內執行任務。因此四旋翼飛行器具有廣闊的應用前景，吸引了眾多科研人員，成為國內外新的研究熱點。

本文通過建立四軸飛行器四元數運動學方程，得到該方程的數值解，再將四元數轉換為歐拉角，避免了歐拉角法運算量大、存在奇異點的問題。在控制算法方面，采用串級PID控制，增加了系統的可靠性和魯棒性。通過Matlab/Simulink仿真，并搭建四軸飛行器實驗平臺，驗證了算法的正確性。

1 姿態描述與姿態角表示

1.1 四軸飛行器姿態描述

為了更好地描述四軸飛行器的姿態，建立如圖1所示的坐標系，分別為參考坐標系n系、機體坐標系b系。參考坐標系的原點為機體的旋轉中心，機體坐標系的原點為機體的中心。

從仿真圖可以看出，俯仰通道階躍響應上升時間為0.7 s，超調量為22%，在1.2 s達到穩定，在調節過程中出現震蕩，震蕩的幅度較小頻率較低，曲線穩定后沒有震蕩。單級PID自動控制效果不理想，為達到更理想的控制效果，本文采用串級PID控制，增強系統的穩定性和魯棒性。

3.2 串級PID控制

控制系統在只用角度單環的情況下，系統很難穩定運行，本文加入加速度作為內環，角速度由陀螺儀和加速度計互補濾波得出。四軸飛行器串級PID控制流程圖如圖3所示，其中角度作為外環，角速度作為內環。

從仿真圖可以看出，俯仰通道階躍響應上升時間為0.25 s，超調量為2%，在0.35 s達到穩定。通過仿真圖對比可知，在串級PID控制下，控制系統響應速度快，超調量明顯減小，且沒有出現震蕩，比單級PID控制更加穩定可靠，達到了良好的控制效果。

4 實 驗

考慮到仿真結果與實際飛行情況的差異，為驗證雙閉環PID控制算法的可行性，搭建了基于STM32微控制器的四軸飛行器，調試確定了各個參數見表1，并進行了室外飛行實驗，飛行效果良好，如圖5所示。

5 結 論

本文通過建立四軸飛行器四元數運動學微分方程，解得微分方程的數值解，從而得到四軸飛行器的歐拉角。在控制方面，對比了單級PID和串級PID控制的仿真效果，單級PID存在明顯的超調，經過較長時間才能穩定；而串級PID響應速度快，超調量小，穩定所需時間短，滿足四軸飛行器控制的基本要求。

參考文獻

[1] 何瑜.四軸飛行器控制系統設計及其姿態解算和控制算法研究[D].成都：電子科技大學，2015.

HE Yu. The design of quadrotor aircraft control system and research of its attitude solving & control algorithm [D]. Chengdu： University of Electronic Science and Technology of China， 2015.

[2] 楊成順.多旋翼飛行器建模與飛行控制技術研究[D].南京：南京航空航天大學，2013.

YANG Chengshun. Research on modeling and flight control technology of multi?rotor aircraft [D]. Nanjing： Nanjing University of Aeronautics and Astronautics， 2013.

[3] 劉豹，唐萬生.現代控制理論[M].3版.北京：機械工業出版社，2006.

LIU Bao， TANG Wansheng. Modern control theory [M]. 3rd ed. Beijing： China Machine Press， 2006.

[4] 秦永元.慣性導航[M].2版.北京：科學出版社，2014.

QIN Yongyuan. Inertial navigation [M]. 2nd ed. Beijing： Science Press， 2014.

[5] 張承岫，李鐵鷹，王耀力.基于MPU6050和互補濾波的四旋翼飛控系統設計[J].傳感技術學報，2016，29（7）：1011?1015.

ZHANG Chengxiu， LI Tieying， WANG Yaoli. Design of quadrotor aircraft flight control system based on MPU6050 and adaptive complementary filter [J]. Chinese journal of sensors and actuators， 2016， 29（7）： 1011?1015.

[6] 陳航科，張東升，盛曉超，等.四旋翼飛行器懸停狀態姿態控制建模與仿真[J].計算機仿真，2013，30（11）：41?45.

CHEN Hangke， ZHANG Dongsheng， SHENGXiaochao， et al. Modeling and attitude control simulation for quadrotor aircraft in hover [J]. Computer simulation， 2013， 30（11）： 41?45.

[7] 龔瑞.微型四旋翼飛行器飛行控制系統設計與驗證[D].南京：南京航空航天大學，2015.

GONG Rui. Design and verification of flight control system for miniature quadrotor helicopter [D]. Nanjing： Nanjing University of Aeronautics and Astronautics， 2015.

[8] 李世光，王文文，申夢茜，等.基于STM32的姿態測量系統設計[J].現代電子技術，2016，39（9）：12?14.

LI Shiguang， WANG Wenwen， SHEN Mengxi， et al. Design of attitude measurement system based on STM32 [J]. Modern electronics technique， 2016， 39（9）： 12?14.

[9] DERAFA L， BENALLEGUE A， FRIDMAN L. Super twisting control algorithm for the attitude tracking of a four rotors UAV [J]. Journal of the Franklin Institute， 2012， 349（2）： 685?699.

[10] ZHAN Q， WANG J， XI X. Control system design and experiments of a quadrotor [C]// Proceedings of IEEE International Conference on Robotics and Biomimetics. Guangzhou： IEEE， 2012： 1152?1157.