四旋翼飛行控制系統的設計與研究

王曉初 盧 琛

(廣東工業大學機電工程學院，廣東 廣州 510006)

四旋翼飛行控制系統的設計與研究

王曉初 盧 琛

(廣東工業大學機電工程學院，廣東 廣州 510006)

四旋翼飛行器以其結構簡單、運動靈活和操控方便等優點，被廣泛應用于各個領域，它具有二階系統所包含的不穩定性、非線性和較強的耦合性等典型特點。控制系統是四旋翼飛行器設計的難點。針對這一問題，設計以STM32微控制器為硬件平臺的控制系統，并搭載μC/OS-II嵌入式系統，結合慣性傳感器與互補濾波算法，實現對四旋翼的串級PID控制。試驗結果表明，該控制系統具有較強的魯棒性，對四旋翼具有良好的控制效果。

四旋翼飛行器 互補濾波器 姿態解算 STM32 μC/OS-II PID

0 引言

無人機是當今航空領域研究的熱點，而微小型無人機則更是世界無人機發展的重要方向之一。四旋翼作為微小型無人機的一類，具有結構簡單、運動靈活等優點，無論是在民用還是在軍用領域，都具有很高的利用價值[1]。

不同于其他種類的無人機，四旋翼在控制上具有低阻尼、非線性和多耦合等特點，其控制系統是四旋翼設計的關鍵[2]。文獻[3]給出了四旋翼的飛行原理與硬件架構，其模塊化的硬件組成有利于系統維護。文獻[4]和[5]給出了一種捷聯式慣性導航算法，從理論上分析了姿態解算的實現。文獻[1]給出了一種基于互補濾波器的姿態修正算法，為無人機的精確導航提供了方向。本文則結合模塊化設計的思路，以μC/OS-II操作系統為軟件平臺，采用互補濾波器與串級PID算法，設計了一種四旋翼的控制系統。

1 硬件系統的設計

系統采用意法半導體公司的32位微控制器STM32作為控制核心，其最高運行主頻可達72 MHz，滿足整個控制系統對實時性的要求。姿態檢測傳感器采用集成有陀螺儀和加速度儀的慣性元件MPU6050，避免了組合傳感器之間的安裝誤差。數據傳輸設備采用2.4 Gbit/s無線傳輸模塊NRF24L01，可以實現飛行數據的快速傳輸。系統整體架構如圖1所示。

圖1 四旋翼的硬件架構Fig.1 The hardware architecture of the quadrotor

圖1中，遙控設備、電源監測和慣性元件是控制器的輸入設備，主要負責控制信號與姿態信號的采集;無線傳輸模塊、電機驅動和LED作為控制器的輸出設備，主要負責與外界的聯系與控制。

2 軟件的設計

采用μC/OS-II操作系統作為軟件平臺[6]，有利于模塊化設計，便于系統設計與維護，其流程框圖如圖2所示。

其中，初始化操作最先完成，為操作系統的運行做前期的準備工作。系統時鐘由STM32滴答計時器產生，為操作系統提供有序的控制節拍，而任務的創建則是具體任務的分工，并且由操作系統掌控任務之間的協同與調度。

在設定任務中，采樣任務優先級最高，主要完成輸入設備的數據采集與飛行姿態解算工作，姿態的解算直接影響著飛行器的控制輸出。控制任務是系統的核心，其根據姿態輸出與期望輸出構成姿態偏差，并以此控制電機輸出而達到預期效果。通信任務與主循環任務分別負責與外界的數據傳遞和狀態顯示等實時性要求不高的操作。

2.1 四旋翼飛行器的姿態解算

姿態解算由陀螺儀和加速度傳感器通過融合算法得到穩定、準確和實時的姿態角。常用的融合算法有歐拉角法、方向余弦法和四元數法。姿態角由歐拉角表示，可直接作為PID的控制量。但用歐拉角進行姿態解算存在大量三角函數，且俯仰角為±90°時方程會出現奇點。方向余弦法求解姿態避免了歐拉角方程退化的現象，可以實現全姿態解算，但是方向余弦有9個參數，解算9個微分方程工作量大。相對于方向余弦法，四元數法則計算量小了很多，同時可以避免奇異現象[7]。

四元數是由1個實數和3個虛數構成的超復數，

表示為[q]=q0+q1i+q2j+q3k，其中，q0、q1、q2、q3是實數，i、j和k既是互相正交的單位向量，又是虛單位。因此，四元數既可以看作四維空間中的向量，又可看作超復數。

圖3 姿態解算流程圖Fig.3 Flowchart of posture algorithm

四元數微分方程矩陣形式如下：

式中：ωbx、ωby、ωbz分別為慣性傳感器繞參考坐標系測量的角速度。

將其近似為一階差分形式，有：

令ΔΘ=ΩbΔt，則求解四元數微分方程的迭代格式為：

(1)

由歐拉角表示導航坐標系[xn]到載體坐標系[xb]的余弦矩陣，即[8]：

(2)

θ=-arcsinC13

(3)

(4)

另外，由四元數表出導航坐標系[xn]到載體坐標系[xb]的余弦矩陣為[8]：

設陀螺儀更新姿態為qg，加速度計更新姿態為qc，其對應的測量噪聲分別為ξ1和ξ2，則有修正簡化模型如圖4所示。

圖4 姿態修正簡化模型Fig.4 The simplified model of posture correction

其噪聲傳遞函數分別為[9]：

(5)

(6)

式(5)和(6)可以用來設計k的取值，并以此確定互補濾波器的截斷頻率fτ[1]。圖5所示為此互補濾波器在k=0.02時的俯仰角測試數據。

圖5 互補濾波器輸出Fig.5 Outputs of the complementary filter

2.2PID控制器的設計

PID控制流程如圖6所示，整個控制回路分為角度環和角速度環。角速度環采用P控制，其作用是產生阻尼效應，抵抗干擾，使得被控系統G(s)更加穩定[11]。角度環采用PI控制，實時姿態與目標姿態構成偏差，對偏差進行PI控制，可使得四旋翼穩定在指定位置[6]。

圖6 PID控制框圖Fig.6 Block diagram of PID control

采用位置式PID控制器，則角度環和角速度環的控制律分別為：

(7)

uω(k)=Kp2ω

(8)

式中:ε(k)為期望姿態與當前姿態的誤差;輸入參數ω為陀螺儀測量的角速度向量。

則整個串級PID控制器輸出為：

(9)

如圖7所示，為四旋翼在單軸懸停狀態下，通過無線模塊實時傳輸其X軸的加速度、角速度和姿態角度偏差的輸出數據。

圖7 懸停狀態時X軸方向的輸出Fig.7 The outputs of the X axis direction in hovering state

如圖7所示，在未引入干擾的狀態下，角度偏差基本維持在零度左右;為檢驗控制器的穩定性而引入外界干擾，可以看到干擾消除后，角度偏差在控制器作用下迅速收斂到零度附近，說明此控制器效果明顯，基本達到預期設定要求。

3 結束語

采用μC/OS-II操作系統作為整個系統的構建平臺，為硬件的模塊化架構和軟件的模塊化設計提供了條件，不僅有利于系統的設計與維護，而且在實際應用中更有利于硬件的測試與軟件的移植。在控制算法上采用串級PID控制，將整個控制回路分為兩個控制環，其中角度環負責回正自穩，角速度環負責抵抗外界干擾，這樣有利于試驗參數的設計與調整。在姿態解算方面采用組合傳感器與融合算法，互補融合陀螺儀的高頻特性與加速度儀的低頻特性，結合四元數微分方程可以實現較高精度的全角度解算。

[1] 梁延德,程敏.基于互補濾波器的四旋翼飛行器姿態解算[J].傳感器與微系統,2011(11):56-58,61.

[2] 劉峰,呂強.四軸飛行器姿態控制系統設計[J].計算機測量與控制,2011(3):583-585,616.

[3] 郭曉鴻,楊忠.一種基于STM32的四旋翼飛行器控制器[J].應用科技,2011(7):35-40.

[4] 張榮輝.基于四元數法的捷聯式慣性導航系統的姿態解算[J].光學精密工程,2008(10):1963-1970.

[5] Fresk E，Nikolakopoulos G.Full quaternion based attitude control for a quadrotor[C]//Control Conference(ECC),2013 European,2013:17-19.

[6] 任哲.嵌入式實時操作系統μC/OS-II原理及應用[M].3版.北京：北京航空航天大學出版社，2014.

[7] 胡慶.基于STM32單片機的無人機飛行控制系統設計[D].南京：南京航空航天大學,2012.

[8] 夏喜旺.關于大角度范圍內四元數與歐拉角轉換的思考[J].導彈與航天運載技術,2012(5):47-53.

[9] 高鐘毓．慣性導航系統技術[M].北京：清華大學出版社，2012.

[10]Bachmann E R.Orientation tracking for humans and robots using inertial sensors[C]//Computational Intelligence in Robotics and Automation,CIRA′99.Proceedings.1999 IEEE International Symposium on.1999.

[11]許震,毛麗民.四軸飛行器控制系統設計[J].常熟理工學院學報,2013(2):109-113.

Design and Research on the Flight Control System for Quadrotor

The quadrotor features many advantages, e.g., simple structure, flexible movement and ease operation, etc., so it has been widely applied in various areas. While it also possesses the typical characteristics of second order system, including unstable, non-linear and stronger coupling, thus control system is its design difficulty. Aiming at these problems, the control system is designed with STM32 microprocessor as hardware platform and equipped the μC/OS-II embedded system, the PID cascade control for quadrotor is implemented by combining the inertial sensors and complementary filtering algorithm. The experimental results indicate that this control system possesses stronger robustness and excellent effect for quadrotor.

Quadmotor Complementary filter Posture solver STM32 μC/OS-II PID

王曉初(1964-)，男，1989年畢業于湖南大學電控制專業，獲碩士學位，副教授；主要從事機械電子方向的研究。

TP273

A

10.16086/j.cnki.issn1000-0380.201506002

修改稿收到日期：2014-12-24。