四旋翼飛行器建模及其運(yùn)動(dòng)控制*

郭 勇， 汪大偉， 鄧 宇

(1.中南大學(xué) 高性能復(fù)雜制造國(guó)家重點(diǎn)實(shí)驗(yàn)室，湖南 長(zhǎng)沙 410083； 2.山河智能裝備股份有限公司 國(guó)家級(jí)企業(yè)技術(shù)中心，湖南 長(zhǎng)沙 410100)

四旋翼飛行器建模及其運(yùn)動(dòng)控制*

郭 勇1,2， 汪大偉1， 鄧 宇2

(1.中南大學(xué)高性能復(fù)雜制造國(guó)家重點(diǎn)實(shí)驗(yàn)室，湖南長(zhǎng)沙410083；2.山河智能裝備股份有限公司國(guó)家級(jí)企業(yè)技術(shù)中心，湖南長(zhǎng)沙410100)

針對(duì)四旋翼飛行器是一個(gè)欠驅(qū)動(dòng)、強(qiáng)耦合、非線性系統(tǒng)，提出了運(yùn)用反步法解決系統(tǒng)非線性問(wèn)題，達(dá)到對(duì)飛行器快速、準(zhǔn)確、穩(wěn)定控制目的。研究了以反步法作為非線性設(shè)計(jì)工具對(duì)飛行器控制系統(tǒng)的設(shè)計(jì)問(wèn)題，將飛行控制系統(tǒng)分為內(nèi)外環(huán)2個(gè)子系統(tǒng)。建立四旋翼飛行器動(dòng)力學(xué)及運(yùn)動(dòng)學(xué)方程，并對(duì)數(shù)學(xué)模型進(jìn)行適當(dāng)簡(jiǎn)化。利用反步法求解飛行器內(nèi)環(huán)姿態(tài)控制律，實(shí)現(xiàn)對(duì)目標(biāo)姿態(tài)角的穩(wěn)定控制；利用比例—積分—微分(PID)作為飛行器外環(huán)位置控制律，實(shí)現(xiàn)對(duì)目標(biāo)位置的穩(wěn)定控制。搭建飛行器系統(tǒng)模型，進(jìn)行Matlab/Simulink仿真實(shí)驗(yàn)，結(jié)果表明：在小角度飛行和懸停狀態(tài)下，飛行器的位置與姿態(tài)精度得到了有效控制，驗(yàn)證了數(shù)學(xué)模型與控制律設(shè)計(jì)的準(zhǔn)確性。

四旋翼飛行器； 反步法； 非線性； 控制律； 比例—積分—微分

0 引 言

四旋翼飛行器具有4個(gè)控制輸入和6個(gè)自由度，屬于典型的欠驅(qū)動(dòng)非線性系統(tǒng)。飛行器4個(gè)電機(jī)產(chǎn)生的升力作為系統(tǒng)輸入量，空間位置與姿態(tài)信息作為輸出量[1，2]。根據(jù)飛行過(guò)程中機(jī)體位置與姿態(tài)的耦合關(guān)系，將整個(gè)系統(tǒng)劃分為內(nèi)環(huán)姿態(tài)控制和外環(huán)位置控制2個(gè)子系統(tǒng)。

目前，關(guān)于四旋翼飛行器欠驅(qū)動(dòng)系統(tǒng)模型的控制方法有：1)Sliding-Mode滑?？刂品╗1，4]，具有較好的抗擾性但滑?？刂茣?huì)出現(xiàn)抖動(dòng)現(xiàn)象且容易引起控制量的飽和問(wèn)題；2)自抗擾控制(active disturbance rejection controller,ADRC)法[1,5],不依賴精確的數(shù)學(xué)模型且能夠?qū)ο到y(tǒng)內(nèi)外干擾進(jìn)行估計(jì)補(bǔ)償；3)線性二次調(diào)節(jié)(LQR)法[1,3],分析較為簡(jiǎn)便，但使用時(shí)需要基于一定假設(shè)條件，不可以直接應(yīng)用于非線性系統(tǒng)中。本文采用反步法[5～12]，能夠處理一些非線性、不確定性影響的問(wèn)題，而且其穩(wěn)定性及誤差的收斂性已得到證明。

本文分析并建立了飛行器數(shù)學(xué)模型，利用反步法設(shè)計(jì)內(nèi)環(huán)姿態(tài)控制器，利用經(jīng)典比例—積分—微分(PID)法[2,11]作為外環(huán)位置控制器設(shè)計(jì)方法。最后根據(jù)建立的模型和控制器在計(jì)算機(jī)上進(jìn)行了仿真實(shí)驗(yàn)，仿真結(jié)果表明：反步法和PID法能夠很好地實(shí)現(xiàn)對(duì)飛行器位姿的控制。

1 飛行器建模分析

為了建立四旋翼飛行器的數(shù)學(xué)模型，假設(shè)：1)飛行器是一個(gè)十字形對(duì)稱的剛體結(jié)構(gòu)；2)機(jī)體坐標(biāo)系原點(diǎn)位于機(jī)體幾何中心處；3)不考慮阻力對(duì)飛行器的影響；4)電機(jī)產(chǎn)生升力大小與轉(zhuǎn)速平方成正比。在三維設(shè)計(jì)軟件Pro/E中，建立四飛行器機(jī)體模型，如圖1(a)所示。

圖1 飛行器三維模型及其結(jié)構(gòu)示意

根據(jù)牛頓運(yùn)動(dòng)定律建立飛行器在外力作用下的線運(yùn)動(dòng)方程[4]

(1)

飛行器在外力矩作用下的角運(yùn)動(dòng)方程

(2)

飛行器在三維空間運(yùn)動(dòng)時(shí)，分別受到由電機(jī)產(chǎn)生的力和力矩作用[14]

(3)

式中ωi,i=1,2,3,4為對(duì)應(yīng)4個(gè)電機(jī)轉(zhuǎn)速；kb，kd為升力系數(shù)，反扭力矩系數(shù)；Ui,i=1,2,3,4分別為垂直合力、俯仰合力、滾轉(zhuǎn)合力及偏航合力矩，作為系統(tǒng)控制量。在對(duì)系統(tǒng)進(jìn)行簡(jiǎn)化分析時(shí)可將直流無(wú)刷電機(jī)模型[2]近似為慣性環(huán)節(jié)。

1.1 動(dòng)力學(xué)模型建立

(4)

式中Mx，My，Mz為機(jī)體繞x，y，z軸轉(zhuǎn)動(dòng)產(chǎn)生的力矩

Mx=lU2；My=lU3；Mz=lU4

(5)

將式(4)，式(5)，式(7)代入式(6)得到飛行器角運(yùn)動(dòng)加速度如下

(6)

從機(jī)體坐標(biāo)系到慣性坐標(biāo)系的轉(zhuǎn)換矩陣[5]

(7)

滿足簡(jiǎn)化表達(dá)c·=cos(·),s·=sin(·)。

1.2 運(yùn)動(dòng)學(xué)模型建立

根據(jù)牛頓第二定律建立機(jī)體運(yùn)動(dòng)學(xué)模型，在慣性坐標(biāo)系下機(jī)體受到合外力

(8)

將式(3)、式(9)代入式(10)可得飛行器線運(yùn)動(dòng)加速度

(9)

式(6)，式(9)共同構(gòu)成了飛行器的非線性模型。

2 反步法原理與內(nèi)外環(huán)控制律設(shè)計(jì)

反步法[1]逆向遞推原理如圖2所示。

圖2 反步法原理

考慮非線性系統(tǒng)一般形式[15]

(10)

式中 [ηT,ξ]T∈Rn+1為狀態(tài)量；ξ為輸入；=u，u∈R為輸入控制量；函數(shù)f:D→Rn和g:D→Rn在包含原點(diǎn)η=0和f(0)=0的定義域D?Rn上是光滑的。假設(shè)方程(10)可通過(guò)一個(gè)光滑的狀態(tài)反饋控制律ξ=φ(η),φ(0)=0穩(wěn)定即能夠滿足φ(η)的原點(diǎn)是漸近穩(wěn)定的[15]。

2.1 內(nèi)環(huán)子系統(tǒng)控制律設(shè)計(jì)

以俯仰通道為例推導(dǎo)姿態(tài)子系統(tǒng)控制律設(shè)計(jì)過(guò)程[16]。在飛行器俯仰通道中，定義狀態(tài)變量

x1=θ,x2=

(11)

(12)

(13)

選取Lyapunov函數(shù)V2為

(14)

(15)

取控制輸入量

(16)

(17)

同理得滾轉(zhuǎn)，偏航通道控制量U3，U4

(18)

(19)

式中a2=(Jzz-Jxx)/Jyy；b2=1/Jyy；參數(shù)ci,i=1,2,3,4,5,6均可調(diào);a3=(Jxx-Jyy)/Jzz;b3=1/Jzz;z3，z4，z5，z6為中間虛擬控制量。

2.2 外環(huán)位置控制律

經(jīng)典PID控制由比例、積分和微分三部分組成的，其針對(duì)輸入與輸出的反饋誤差e(t)實(shí)現(xiàn)對(duì)輸出的控制。高度通道[2]不與其他通道存在耦合關(guān)系，可單獨(dú)考慮。實(shí)時(shí)高度值z(mì)可由機(jī)體上氣壓高度傳感器獲得。

高度誤差控制量

ez(t)=z-zd

(20)

飛行器高度控制量

(21)

(22)

文中飛行器模型簡(jiǎn)化和仿真驗(yàn)證是基于小角度飛行及懸停狀態(tài)下進(jìn)行的，當(dāng)控制外環(huán)水平位置時(shí)，式(9)中俯仰、滾轉(zhuǎn)姿態(tài)角作為輸入。為了保證飛行器飛行過(guò)程中姿態(tài)角輸入不能過(guò)大，需要進(jìn)行平滑限幅處理。俯仰、滾轉(zhuǎn)姿態(tài)角參考輸入表達(dá)式為

(23)

3 控制系統(tǒng)仿真實(shí)現(xiàn)

表1 實(shí)驗(yàn)用飛行器模型參數(shù)

文中給出了系統(tǒng)內(nèi)外環(huán)控制回路的實(shí)驗(yàn)結(jié)果，如圖3～圖7所示。其中圖3為在開(kāi)環(huán)狀態(tài)下模型姿態(tài)角自然響應(yīng)曲線。由圖可知，在給定輸入下滾轉(zhuǎn)角、偏航角均出現(xiàn)持續(xù)振蕩現(xiàn)象，俯仰角度趨近于0°。圖4為在開(kāi)環(huán)狀態(tài)下模型位置自然響應(yīng)曲線。由圖可知，在給定輸入下系統(tǒng)沿x,y,z三軸位置輸出均持續(xù)振蕩且發(fā)散。圖5為系統(tǒng)在閉環(huán)狀態(tài)下姿態(tài)角輸出響應(yīng)曲線，由圖可知，滾轉(zhuǎn)、俯仰及偏航角均能夠收斂。系統(tǒng)輸出曲線響應(yīng)時(shí)間約為t=2 s且在5 s內(nèi)達(dá)到穩(wěn)定。圖6為在閉環(huán)狀態(tài)下位置輸出響應(yīng)曲線，由圖可知，位置輸出調(diào)整快速，波動(dòng)較小且飛行器在2 s內(nèi)到達(dá)預(yù)定位置且保持穩(wěn)定。圖7為在閉環(huán)狀態(tài)下位置跟蹤輸出響應(yīng)曲線，設(shè)置飛行器跟蹤起始位置(0,-1,0)，終點(diǎn)位置(0，-1,3.2)。由圖可知，跟蹤曲線(實(shí)線)與參考曲線(虛線)之間存在一定的合理誤差，但整體上位置跟蹤取得了良好效果。

圖3 模型開(kāi)環(huán)姿態(tài)角度曲線

圖4 模型開(kāi)環(huán)位置信息曲線

圖5 反步法閉環(huán)姿態(tài)角度曲線

圖6 PID閉環(huán)位置信息曲線

圖7 閉環(huán)空間位置跟蹤曲線

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

針對(duì)四旋翼飛行器位置與姿態(tài)間的耦合關(guān)系，提出了基于內(nèi)外環(huán)子系統(tǒng)的不同控制策略，即分別對(duì)內(nèi)環(huán)采用反步法、外環(huán)采用經(jīng)典PID法設(shè)計(jì)子系統(tǒng)控制器。通過(guò)Matlab仿真平臺(tái)進(jìn)行實(shí)驗(yàn)，系統(tǒng)動(dòng)態(tài)響應(yīng)快速平穩(wěn)，驗(yàn)證了飛行器數(shù)學(xué)模型建立的合理性，實(shí)現(xiàn)了對(duì)飛行器位置與姿態(tài)較為準(zhǔn)確地控制。

[1] 李文超.X750四旋翼飛行器建模與飛行控制研究[D].南京：南京理工大學(xué)，2014.

[2] 刁 琛.四旋翼無(wú)人飛行器的非線性自適應(yīng)控制器設(shè)計(jì)[D].天津：天津大學(xué)，2010.

[3] 魏麗娜，寧會(huì)峰，陸 旺，等.多旋翼飛行器的關(guān)鍵技術(shù)及應(yīng)用前景[J].自動(dòng)化技術(shù)與應(yīng)用，2016,35(9)：1-4.

[4] 楊成順.多旋翼飛行器建模與飛行控制技術(shù)研究[D].南京：南京航空航天學(xué).2013.

[5] 聶博文.微小型四旋翼無(wú)人直升機(jī)建模及控制方法研究[D].長(zhǎng)沙：國(guó)防科學(xué)技術(shù)大學(xué)，2006.

[6] 田聰玲.基于反步法的四旋翼飛行器非線性控制[D].哈爾濱：哈爾濱工業(yè)大學(xué)，2014.

[7] Song Zhankui, Sun Kaibiao.Adaptive backstepping sliding mode control with fuzzy monitoring strategy for a kind of mechanical system[J].ISA Trans，2014,53:125-133.

[8] 劉金琨.先進(jìn)PID控制MATLAB仿真[M].成都：電子工業(yè)出版社，2004.

[9] 朱斌泉，林景棟.模糊優(yōu)化的迭代控制算法在稱重系統(tǒng)中的應(yīng)用[J].自動(dòng)化技術(shù)與應(yīng)用，2010,29(12):4-7.

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

[11] Long Luong Tuan,Sangchul Won.PID-based sliding mode controller design for the micro quadrotor[C]∥International Conference on Automation and Systems,2013:1860-1865.

[12] Schreier M.Modeling and adaptive control of a quadrotor[C]∥International Conference on Mechatronics and Automation,2012:383-390.

[13] 李 輝，蘆利斌，金國(guó)棟.基于Kinect的四旋翼無(wú)人機(jī)體感控制[J].傳感器與微系統(tǒng)，2015,34(8):99-102.

[14] 唐 順，謝習(xí)華.雙因子自適應(yīng)濾波算法在四旋翼上的應(yīng)用[J].傳感器與微系統(tǒng)，2016,35(6):124-127.

[15] Hassan K Khalil.Nonlinear Systems[M].成都：電子工業(yè)出版社，2005.

[16] 馬 浩.基于Backstepping的多旋翼無(wú)人機(jī)自主飛行控制系統(tǒng)研究[D].南京：南京信息工程大學(xué)，2014.

[17] 薛定宇.控制系統(tǒng)計(jì)算機(jī)輔助設(shè)計(jì)[M].北京：清華大學(xué)出版社，2006.

Modelingandflightcontrolofquadrotor*

GUO Yong1,2, WANG Da-wei1, DENG Yu2

(1.StateKeyLaboratoryofHigh-PerformanceComplexManufacturing,CentralSouthUniversity,Changsha410083,China;2.NationalEnterpriseR&DCenter,SunwardIntelligentEquipmentCoLtd,Changsha410100,China)

Considering quadrotor is an underactuated,strong coupling,nonlinear system,in order to achieve purpose of quick,accurate and stable control of quadrotor，backstepping method is proposed to solve the nonlinear problem of the quadrotor.Meanwhile，overall control of quadrotor is divided into inner loop and outer loop subsystem.The first step is establishing the dynamics and kinematics equations of the quadrotor,and simplify the mathematical model.Backstepping method is used to solve control law.In view of the couple relationships,the backstepping method is applied to the inner loop for attitude control;the PID method is used in the outer loop for position control.Set up system modeles,the control system is simulated on Matlab/simulink platform.The simulation results show that the controls of attitude and position gain good results under small attitude angles or hovering conditions.The validity of controllers designed and math model is proved by simulations.

quadrotor; backstepping; nonlinear; design controlling laws; proportion integration differentiation(PID)

10.13873/J.1000—9787(2017)11—0038—04

TP 273

A

1000—9787(2017)11—0038—04

2016—09—27

湖南省重大科技成果轉(zhuǎn)化項(xiàng)目(2012CK1003);國(guó)家科技支撐計(jì)劃資助項(xiàng)目(2014BAD06B07)

郭 勇(1967-)，男，高工，副教授,碩士生導(dǎo)師，主要從事工程機(jī)械控制、機(jī)電液一體化等方面研究工作。

汪大偉(1990-)，男，通訊作者，碩士研究生，主要研究方向?yàn)闄C(jī)械電子、自動(dòng)控制等，E—mail：143712100@csu.edu.cn。