王曉初 盧 琛

(廣東工業(yè)大學(xué)機(jī)電工程學(xué)院，廣東 廣州 510006)

四旋翼飛行控制系統(tǒng)的設(shè)計(jì)與研究

王曉初 盧 琛

(廣東工業(yè)大學(xué)機(jī)電工程學(xué)院，廣東 廣州 510006)

四旋翼飛行器以其結(jié)構(gòu)簡(jiǎn)單、運(yùn)動(dòng)靈活和操控方便等優(yōu)點(diǎn)，被廣泛應(yīng)用于各個(gè)領(lǐng)域，它具有二階系統(tǒng)所包含的不穩(wěn)定性、非線性和較強(qiáng)的耦合性等典型特點(diǎn)。控制系統(tǒng)是四旋翼飛行器設(shè)計(jì)的難點(diǎn)。針對(duì)這一問(wèn)題，設(shè)計(jì)以STM32微控制器為硬件平臺(tái)的控制系統(tǒng)，并搭載μC/OS-II嵌入式系統(tǒng)，結(jié)合慣性傳感器與互補(bǔ)濾波算法，實(shí)現(xiàn)對(duì)四旋翼的串級(jí)PID控制。試驗(yàn)結(jié)果表明，該控制系統(tǒng)具有較強(qiáng)的魯棒性，對(duì)四旋翼具有良好的控制效果。

四旋翼飛行器 互補(bǔ)濾波器 姿態(tài)解算 STM32 μC/OS-II PID

0 引言

無(wú)人機(jī)是當(dāng)今航空領(lǐng)域研究的熱點(diǎn)，而微小型無(wú)人機(jī)則更是世界無(wú)人機(jī)發(fā)展的重要方向之一。四旋翼作為微小型無(wú)人機(jī)的一類(lèi)，具有結(jié)構(gòu)簡(jiǎn)單、運(yùn)動(dòng)靈活等優(yōu)點(diǎn)，無(wú)論是在民用還是在軍用領(lǐng)域，都具有很高的利用價(jià)值[1]。

不同于其他種類(lèi)的無(wú)人機(jī)，四旋翼在控制上具有低阻尼、非線性和多耦合等特點(diǎn)，其控制系統(tǒng)是四旋翼設(shè)計(jì)的關(guān)鍵[2]。文獻(xiàn)[3]給出了四旋翼的飛行原理與硬件架構(gòu)，其模塊化的硬件組成有利于系統(tǒng)維護(hù)。文獻(xiàn)[4]和[5]給出了一種捷聯(lián)式慣性導(dǎo)航算法，從理論上分析了姿態(tài)解算的實(shí)現(xiàn)。文獻(xiàn)[1]給出了一種基于互補(bǔ)濾波器的姿態(tài)修正算法，為無(wú)人機(jī)的精確導(dǎo)航提供了方向。本文則結(jié)合模塊化設(shè)計(jì)的思路，以μC/OS-II操作系統(tǒng)為軟件平臺(tái)，采用互補(bǔ)濾波器與串級(jí)PID算法，設(shè)計(jì)了一種四旋翼的控制系統(tǒng)。

1 硬件系統(tǒng)的設(shè)計(jì)

系統(tǒng)采用意法半導(dǎo)體公司的32位微控制器STM32作為控制核心，其最高運(yùn)行主頻可達(dá)72 MHz，滿足整個(gè)控制系統(tǒng)對(duì)實(shí)時(shí)性的要求。姿態(tài)檢測(cè)傳感器采用集成有陀螺儀和加速度儀的慣性元件MPU6050，避免了組合傳感器之間的安裝誤差。數(shù)據(jù)傳輸設(shè)備采用2.4 Gbit/s無(wú)線傳輸模塊NRF24L01，可以實(shí)現(xiàn)飛行數(shù)據(jù)的快速傳輸。系統(tǒng)整體架構(gòu)如圖1所示。

圖1 四旋翼的硬件架構(gòu)Fig.1 The hardware architecture of the quadrotor

圖1中，遙控設(shè)備、電源監(jiān)測(cè)和慣性元件是控制器的輸入設(shè)備，主要負(fù)責(zé)控制信號(hào)與姿態(tài)信號(hào)的采集;無(wú)線傳輸模塊、電機(jī)驅(qū)動(dòng)和LED作為控制器的輸出設(shè)備，主要負(fù)責(zé)與外界的聯(lián)系與控制。

2 軟件的設(shè)計(jì)

采用μC/OS-II操作系統(tǒng)作為軟件平臺(tái)[6]，有利于模塊化設(shè)計(jì)，便于系統(tǒng)設(shè)計(jì)與維護(hù)，其流程框圖如圖2所示。

其中，初始化操作最先完成，為操作系統(tǒng)的運(yùn)行做前期的準(zhǔn)備工作。系統(tǒng)時(shí)鐘由STM32滴答計(jì)時(shí)器產(chǎn)生，為操作系統(tǒng)提供有序的控制節(jié)拍，而任務(wù)的創(chuàng)建則是具體任務(wù)的分工，并且由操作系統(tǒng)掌控任務(wù)之間的協(xié)同與調(diào)度。

在設(shè)定任務(wù)中，采樣任務(wù)優(yōu)先級(jí)最高，主要完成輸入設(shè)備的數(shù)據(jù)采集與飛行姿態(tài)解算工作，姿態(tài)的解算直接影響著飛行器的控制輸出?？刂迫蝿?wù)是系統(tǒng)的核心，其根據(jù)姿態(tài)輸出與期望輸出構(gòu)成姿態(tài)偏差，并以此控制電機(jī)輸出而達(dá)到預(yù)期效果。通信任務(wù)與主循環(huán)任務(wù)分別負(fù)責(zé)與外界的數(shù)據(jù)傳遞和狀態(tài)顯示等實(shí)時(shí)性要求不高的操作。

2.1 四旋翼飛行器的姿態(tài)解算

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

四元數(shù)是由1個(gè)實(shí)數(shù)和3個(gè)虛數(shù)構(gòu)成的超復(fù)數(shù)，

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

圖3 姿態(tài)解算流程圖Fig.3 Flowchart of posture algorithm

四元數(shù)微分方程矩陣形式如下：

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

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

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

(1)

由歐拉角表示導(dǎo)航坐標(biāo)系[xn]到載體坐標(biāo)系[xb]的余弦矩陣，即[8]：

(2)

θ=-arcsinC13

(3)

(4)

另外，由四元數(shù)表出導(dǎo)航坐標(biāo)系[xn]到載體坐標(biāo)系[xb]的余弦矩陣為[8]：

設(shè)陀螺儀更新姿態(tài)為qg，加速度計(jì)更新姿態(tài)為qc，其對(duì)應(yīng)的測(cè)量噪聲分別為ξ1和ξ2，則有修正簡(jiǎn)化模型如圖4所示。

圖4 姿態(tài)修正簡(jiǎn)化模型Fig.4 The simplified model of posture correction

其噪聲傳遞函數(shù)分別為[9]：

(5)

(6)

式(5)和(6)可以用來(lái)設(shè)計(jì)k的取值，并以此確定互補(bǔ)濾波器的截?cái)囝l率fτ[1]。圖5所示為此互補(bǔ)濾波器在k=0.02時(shí)的俯仰角測(cè)試數(shù)據(jù)。

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

2.2PID控制器的設(shè)計(jì)

PID控制流程如圖6所示，整個(gè)控制回路分為角度環(huán)和角速度環(huán)。角速度環(huán)采用P控制，其作用是產(chǎn)生阻尼效應(yīng)，抵抗干擾，使得被控系統(tǒng)G(s)更加穩(wěn)定[11]。角度環(huán)采用PI控制，實(shí)時(shí)姿態(tài)與目標(biāo)姿態(tài)構(gòu)成偏差，對(duì)偏差進(jìn)行PI控制，可使得四旋翼穩(wěn)定在指定位置[6]。

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

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

(7)

uω(k)=Kp2ω

(8)

式中:ε(k)為期望姿態(tài)與當(dāng)前姿態(tài)的誤差;輸入?yún)?shù)ω為陀螺儀測(cè)量的角速度向量。

則整個(gè)串級(jí)PID控制器輸出為：

(9)

如圖7所示，為四旋翼在單軸懸停狀態(tài)下，通過(guò)無(wú)線模塊實(shí)時(shí)傳輸其X軸的加速度、角速度和姿態(tài)角度偏差的輸出數(shù)據(jù)。

圖7 懸停狀態(tài)時(shí)X軸方向的輸出Fig.7 The outputs of the X axis direction in hovering state

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

3 結(jié)束語(yǔ)

采用μC/OS-II操作系統(tǒng)作為整個(gè)系統(tǒng)的構(gòu)建平臺(tái)，為硬件的模塊化架構(gòu)和軟件的模塊化設(shè)計(jì)提供了條件，不僅有利于系統(tǒng)的設(shè)計(jì)與維護(hù)，而且在實(shí)際應(yīng)用中更有利于硬件的測(cè)試與軟件的移植。在控制算法上采用串級(jí)PID控制，將整個(gè)控制回路分為兩個(gè)控制環(huán)，其中角度環(huán)負(fù)責(zé)回正自穩(wěn)，角速度環(huán)負(fù)責(zé)抵抗外界干擾，這樣有利于試驗(yàn)參數(shù)的設(shè)計(jì)與調(diào)整。在姿態(tài)解算方面采用組合傳感器與融合算法，互補(bǔ)融合陀螺儀的高頻特性與加速度儀的低頻特性，結(jié)合四元數(shù)微分方程可以實(shí)現(xiàn)較高精度的全角度解算。

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

[2] 劉峰,呂強(qiáng).四軸飛行器姿態(tài)控制系統(tǒng)設(shè)計(jì)[J].計(jì)算機(jī)測(cè)量與控制,2011(3):583-585,616.

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

[4] 張榮輝.基于四元數(shù)法的捷聯(lián)式慣性導(dǎo)航系統(tǒng)的姿態(tài)解算[J].光學(xué)精密工程,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] 任哲.嵌入式實(shí)時(shí)操作系統(tǒng)μC/OS-II原理及應(yīng)用[M].3版.北京：北京航空航天大學(xué)出版社，2014.

[7] 胡慶.基于STM32單片機(jī)的無(wú)人機(jī)飛行控制系統(tǒng)設(shè)計(jì)[D].南京：南京航空航天大學(xué),2012.

[8] 夏喜旺.關(guān)于大角度范圍內(nèi)四元數(shù)與歐拉角轉(zhuǎn)換的思考[J].導(dǎo)彈與航天運(yùn)載技術(shù),2012(5):47-53.

[9] 高鐘毓．慣性導(dǎo)航系統(tǒng)技術(shù)[M].北京：清華大學(xué)出版社，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]許震,毛麗民.四軸飛行器控制系統(tǒng)設(shè)計(jì)[J].常熟理工學(xué)院學(xué)報(bào),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年畢業(yè)于湖南大學(xué)電控制專(zhuān)業(yè)，獲碩士學(xué)位，副教授；主要從事機(jī)械電子方向的研究。

TP273

A

10.16086/j.cnki.issn1000-0380.201506002

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