李 希，陳 洋，陳鵬震

(武漢科技大學(xué) 信息科學(xué)與工程學(xué)院，武漢 430081)

基于PID算法的四旋翼飛行器定點(diǎn)跟蹤控制

李 希，陳 洋，陳鵬震

(武漢科技大學(xué) 信息科學(xué)與工程學(xué)院，武漢 430081)

四旋翼飛行器是一種結(jié)構(gòu)緊湊、飛行方式獨(dú)特的垂直起降式飛行器，具有欠驅(qū)動(dòng)、強(qiáng)耦合的特點(diǎn)；為實(shí)現(xiàn)對(duì)飛行器的控制，使之能從起始位置在一定時(shí)間內(nèi)到達(dá)設(shè)定目標(biāo)點(diǎn)并穩(wěn)定懸停，根據(jù)其飛行特點(diǎn)建立動(dòng)力學(xué)模型并在此基礎(chǔ)上提出了一種雙閉環(huán)PID控制方法；其中，外環(huán)實(shí)現(xiàn)飛行器的位置控制，使用目標(biāo)點(diǎn)與反饋回的實(shí)際坐標(biāo)的偏差作為控制器的輸入，內(nèi)環(huán)實(shí)現(xiàn)飛行器的姿態(tài)控制，其姿態(tài)的參考量由外環(huán)的輸出經(jīng)逆向求解獲得的歐拉角與實(shí)際姿態(tài)角求得的偏差作為控制器的輸入；通過(guò)PID算法后輸出的姿態(tài)控制信號(hào)，經(jīng)過(guò)四旋翼飛行器“十”字動(dòng)力學(xué)模型解耦得到4個(gè)電機(jī)的轉(zhuǎn)速控制值，從而完成定點(diǎn)跟蹤任務(wù)；提出的雙閉環(huán)PID控制方法在仿真中獲得了驗(yàn)證，為飛行器的進(jìn)一步研究提供了基礎(chǔ)。

四旋翼飛行器；動(dòng)力學(xué)模型；雙閉環(huán)；PID控制器；定點(diǎn)跟蹤

0 引言

隨著計(jì)算機(jī)技術(shù)和自動(dòng)化技術(shù)的不斷發(fā)展，旋翼式飛行器技術(shù)在越來(lái)越多的領(lǐng)域有廣闊的應(yīng)用前景，四旋翼飛行器是一種結(jié)構(gòu)緊湊、飛行方式獨(dú)特的垂直起降式飛行器，具有欠驅(qū)動(dòng)、強(qiáng)耦合的特點(diǎn)，通過(guò)調(diào)節(jié)對(duì)稱分布的四個(gè)旋翼轉(zhuǎn)速，實(shí)現(xiàn)對(duì)飛行姿態(tài)的控制。由于它具有普通無(wú)人機(jī)沒(méi)有的優(yōu)勢(shì)，如垂直起降、協(xié)調(diào)轉(zhuǎn)彎、空中懸停、側(cè)飛、前飛等多種飛行模態(tài)，這決定了它在軍用方面可以應(yīng)用于進(jìn)行偵查、攻擊、通信、監(jiān)視等工作，在民用方面可以應(yīng)用于航拍、交通監(jiān)控、大氣檢測(cè)、電力巡檢、除冰等工作[1]。

目前，對(duì)于四旋翼飛行器的控制系統(tǒng)設(shè)計(jì)，國(guó)內(nèi)外眾多高校和科研機(jī)構(gòu)進(jìn)行了相關(guān)方面的研究，目前較為常用的控制算法有：PID控制器[2]，自抗擾控制器(ADRC)，線性二次型最優(yōu)調(diào)節(jié)控制器(LQR)，滑?？刂扑惴?Sliding-mode)，BackStepping控制算法。自抗擾控制器針對(duì)動(dòng)力學(xué)模型中的未建模項(xiàng)，在傳統(tǒng)的PID控制器中加入不確定函數(shù)和外界干擾，增強(qiáng)了機(jī)體的魯棒性[3]，由于加入了外界未知干擾導(dǎo)致控制精度不夠。線性二次型最優(yōu)調(diào)節(jié)控制器通過(guò)構(gòu)造線性二次型問(wèn)題的狀態(tài)方程，求解不同拉力下各個(gè)電機(jī)角度以及角速度[4]，該方法能縮短機(jī)體的響應(yīng)時(shí)間，但是需要建立電機(jī)模型，過(guò)程復(fù)雜，使得控制系統(tǒng)的實(shí)時(shí)性不足；本文將采用經(jīng)典的PID控制器，通過(guò)設(shè)計(jì)雙環(huán)結(jié)構(gòu)來(lái)實(shí)現(xiàn)對(duì)四旋翼飛行器的跟蹤控制。該方法在四旋翼動(dòng)力學(xué)模型的基礎(chǔ)上設(shè)計(jì)了位置控制器和姿態(tài)控制器，能較好的控制機(jī)體的姿態(tài)角和移動(dòng)速度，通過(guò)設(shè)置合適的參數(shù)也能克服跟蹤精度不高的缺點(diǎn)。

本文運(yùn)用Newton-Euler公式建立簡(jiǎn)化的四旋翼飛行器動(dòng)力學(xué)模型[5-6]，并進(jìn)行簡(jiǎn)化，運(yùn)用PID算法，通過(guò)建立位置環(huán)、姿態(tài)環(huán)雙閉環(huán)控制系統(tǒng)，實(shí)現(xiàn)了對(duì)四旋翼飛行器各通道控制律的設(shè)計(jì)，并在實(shí)驗(yàn)室模型樣機(jī)上通過(guò)Simulink仿真實(shí)驗(yàn)，實(shí)現(xiàn)了四旋翼飛行器精確、穩(wěn)定的定點(diǎn)飛行和穩(wěn)定懸停，論證了算法的實(shí)時(shí)性、準(zhǔn)確性。

1 動(dòng)力學(xué)模型

圖1 四旋翼飛行器動(dòng)力學(xué)建模示意圖

1.1 平移動(dòng)力學(xué)模型

為得到飛行器在機(jī)體坐標(biāo)系中的受力FB與在導(dǎo)航系中受力FE的關(guān)系，假設(shè)機(jī)體按照先Z軸，Y軸，再X軸的順序轉(zhuǎn)動(dòng)，這3次轉(zhuǎn)動(dòng)可表示為3個(gè)獨(dú)立方向余弦矩陣。

則在此種轉(zhuǎn)動(dòng)順序下，可定義：

從E系到B系的方向余弦矩陣：

式中，Cφ=cosφ,Sφ=sinφ，同下。

從B系到E的方向余弦矩陣為：

在導(dǎo)航系中機(jī)體的升力為：

(1)

(2)

1.2 轉(zhuǎn)動(dòng)動(dòng)力學(xué)模型

四旋翼飛行器在飛行過(guò)程中，若忽略空氣阻力和旋翼的陀螺效應(yīng)，則飛行器在機(jī)體系中的轉(zhuǎn)動(dòng)力矩為：

(3)

式(3)中，Jx、Jy、Jz分別為機(jī)體繞機(jī)體坐標(biāo)系xb,yb,zb軸轉(zhuǎn)動(dòng)的轉(zhuǎn)動(dòng)慣量；假設(shè)旋翼質(zhì)心到機(jī)體質(zhì)心的距離為l；p、q、r分別為機(jī)體繞機(jī)體坐標(biāo)系B的xb,yb,zb軸轉(zhuǎn)動(dòng)角速度。

根據(jù)上圖，可推得QE與QB的關(guān)系：

(4)

飛行器在懸停或者是勻速運(yùn)動(dòng)時(shí)，可以認(rèn)為機(jī)體的歐拉角變化很小，接近0度，將θ=0,γ=0,φ=0代入公式(4)，得：

(5)

式(5)表明，機(jī)體在機(jī)體系中的角速度等于歐拉角的角速度，而轉(zhuǎn)動(dòng)力矩為:

(6)

式(6)表明，轉(zhuǎn)動(dòng)力矩M與角速度ω的導(dǎo)數(shù)成正比，而E系中的轉(zhuǎn)動(dòng)慣量J與B系中的轉(zhuǎn)動(dòng)慣量J相同，所以:

(7)

由公式(6)和公式(7)可得轉(zhuǎn)動(dòng)動(dòng)力學(xué)模型：

(8)

通過(guò)改變飛行器電機(jī)轉(zhuǎn)速，可以實(shí)現(xiàn)各種不同的運(yùn)動(dòng)。可將其合運(yùn)動(dòng)分解到四個(gè)獨(dú)立方向，分別為：

1)上下運(yùn)動(dòng)(控制律U1)。

實(shí)現(xiàn)方法：四個(gè)電機(jī)轉(zhuǎn)速一致，四個(gè)旋翼產(chǎn)生的升力大于或小于自身重力。

2)前后運(yùn)動(dòng)和俯仰角θ(控制律U2)。

實(shí)現(xiàn)方法：電機(jī)2、4轉(zhuǎn)速不變，電機(jī)1，3轉(zhuǎn)速一增一減且改變值相同。

3)左右運(yùn)動(dòng)和橫滾角γ(控制律U3)。

實(shí)現(xiàn)方法：電機(jī)1、3轉(zhuǎn)速不變，電機(jī)2、4轉(zhuǎn)速一增一減且改變值相同。

4)偏航運(yùn)動(dòng)和偏航角φ(控制律U4)。

實(shí)現(xiàn)方法：電機(jī)1、3為一組，電機(jī)2、4為一組，一增一減且改變值相同，機(jī)體的旋轉(zhuǎn)方向與轉(zhuǎn)速快的旋翼的旋轉(zhuǎn)方向相反。

注意：上下、前后、左右運(yùn)動(dòng)需抵消機(jī)身的反扭矩，使機(jī)身保持平衡，所以改變電機(jī)轉(zhuǎn)速實(shí)現(xiàn)這幾種運(yùn)動(dòng)的前提條件是τ1+τ3=τ2+τ4，即ω12+ ω32= ω22+ ω42。

假設(shè)1電機(jī)朝前為正方向，2電機(jī)朝右為正方向，電機(jī)1、3的旋轉(zhuǎn)方向?yàn)槠浇堑恼较?，則每個(gè)通道的控制律如下：

(9)

將公式(9)代入公式(2)和公式(8)中即可得到簡(jiǎn)化的動(dòng)力學(xué)模型，如式(10)所示。

(10)

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

根據(jù)以上動(dòng)力學(xué)模型的分析可知，可以通過(guò)改變四個(gè)電機(jī)的轉(zhuǎn)速來(lái)控制飛行器的運(yùn)動(dòng)狀態(tài)，機(jī)體在空間的移動(dòng)速度主要是通過(guò)改變相應(yīng)姿態(tài)角的大小來(lái)控制的，所以為實(shí)現(xiàn)飛行器定點(diǎn)跟蹤，關(guān)鍵在于得到準(zhǔn)確的姿態(tài)控制信號(hào)，控制器的設(shè)計(jì)如圖2所示。

圖2 控制結(jié)構(gòu)示意圖

從圖2中可看出，該控制器采用雙閉環(huán)控制結(jié)構(gòu)，姿態(tài)控制作為內(nèi)環(huán)，位置控制為外環(huán)，內(nèi)環(huán)的設(shè)定值由外環(huán)的輸出值通過(guò)逆向求解歐拉角得來(lái)。輸出的姿態(tài)控制信號(hào)通過(guò)“十”模型解耦得到4個(gè)電機(jī)的轉(zhuǎn)速?gòu)亩刂骑w行器的運(yùn)動(dòng)狀態(tài)。

(11)

外環(huán)位置PID控制器的輸出作為逆向求解歐拉角模塊的輸入。飛行器在定點(diǎn)跟蹤的運(yùn)動(dòng)過(guò)程中，可以僅依靠上下、左右、前后運(yùn)動(dòng)就可完成在空間中從一點(diǎn)到另一點(diǎn)的飛行任務(wù)，所以，為了控制的方便，不妨假設(shè)偏航角φ=0，代入平移動(dòng)力學(xué)模型公式(11)中可得：

(12)

由公式(12)可解得：

而φr=0

(13)

根據(jù)控制律公式(9)及其實(shí)現(xiàn)方法，可得到：

U11U2U3U4 ω21=ω22=ω23=ω24=ì?í??????U14bU14bU14bU14b+eθ+0-eθ+0+0+er+0-er-eφ+eφ-eφ+eφ(14)

利用公式(14)即可用3個(gè)姿態(tài)控制信號(hào)得到4個(gè)電機(jī)的轉(zhuǎn)速控制信號(hào)，這就是“十”字模型解耦。

然后將轉(zhuǎn)速控制信號(hào)相應(yīng)地轉(zhuǎn)換成各個(gè)電機(jī)的PWM波的占空比，使電機(jī)輸出轉(zhuǎn)速，從而使飛行器能在規(guī)定時(shí)間內(nèi)完成定點(diǎn)跟蹤任務(wù)。

3 實(shí)驗(yàn)結(jié)果與分析

用Matlab的Simulink模塊搭建上述四旋翼飛行器動(dòng)力學(xué)模型及其控制器，通過(guò)仿真結(jié)果驗(yàn)證此方法的可行性。其中飛行器參數(shù)暫定為：

b=2.289 3×10-5N·s2

d=1.189 7×10-6N·s2

m=2.467 kg

l=0.387 5 m

Jx=0.058 87 kg·s2

Jy=0.058 87 kg·s2

Jz=0.131 51 kg·s2

由圖3和圖4可以看出四旋翼飛行器在4s左右能夠很快的到達(dá)設(shè)定目標(biāo)點(diǎn)并能夠穩(wěn)定懸停，圖5為飛行器的三維飛行軌跡圖，由此可以看出通過(guò)設(shè)計(jì)出的雙閉環(huán)PID控制器來(lái)控制飛行器完成指定飛行任務(wù)是可行的。

圖3 四旋翼飛行器飛行位置坐標(biāo)曲線

圖4 四旋翼飛行器飛行姿態(tài)曲線

圖5 四旋翼飛行器三維飛行軌跡曲線

4 結(jié)論

本文通過(guò)對(duì)飛行器各部分的運(yùn)動(dòng)機(jī)理進(jìn)行分析，依據(jù)其所遵循的基本規(guī)律得到動(dòng)力學(xué)模型，并在此基礎(chǔ)上對(duì)飛行器的定點(diǎn)跟蹤任務(wù)進(jìn)行分析，提出由外環(huán)位置環(huán)和內(nèi)環(huán)姿態(tài)環(huán)構(gòu)成的雙閉環(huán)PID控制系統(tǒng)，并通過(guò)仿真驗(yàn)證了該系統(tǒng)的可行性。本文重點(diǎn)在于對(duì)飛行器建模和雙閉環(huán) PID控制器的分析，對(duì)于通過(guò)GPS、氣壓計(jì)檢測(cè)出飛行器的實(shí)際坐標(biāo)位置的方法以及姿態(tài)解算算法(比如四元數(shù)算法)并未做詳細(xì)分析。另外，對(duì)于PID算法中比例、積分、微分調(diào)節(jié)系數(shù)KP、KI、KD的整定是根據(jù)飛行器的實(shí)際情況和以上的理論分析利用試湊法來(lái)實(shí)現(xiàn)的。

但是，從圖5的三維飛行軌跡圖來(lái)看雙閉環(huán)PID控制系統(tǒng)并不能控制飛行器沿最優(yōu)的路徑飛行——直線，并且其飛行路徑無(wú)法確定，完全是由控制算法控制其飛向目標(biāo)，所以路徑規(guī)劃是很有必要的，這也將是下一步努力的方向。

[1] 聶博文, 馬宏緒, 王 劍，等. 微小型四旋翼飛行器的研究現(xiàn)狀與關(guān)鍵技術(shù)[J].電光與控制，2007(6)：113-117.

[2] 吳成富, 劉小齊, 袁 旭. 四旋翼無(wú)人機(jī)建模及其PID控制律設(shè)計(jì)[J].電子設(shè)計(jì)工程，2012，16：68-70.

[3] 齊曉慧, 王洪軍. 四旋翼無(wú)人機(jī)的自抗擾控制研究[J]. 測(cè)控技術(shù)，2014(5)：61-64.

[4] 叢夢(mèng)苑. 基于線性二次調(diào)節(jié)器的四旋翼飛行器控制系統(tǒng)的設(shè)計(jì)與研究[D]. 哈爾濱：哈爾濱工程大學(xué)，2011.

[5] 趙育善, 師 鵬. 航天器飛行動(dòng)力學(xué)建模理論與方法[M]. 北京：北京航空航天大學(xué)出版社，2012.

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

[7] Alexis K, Nikolakopoulos G, Tzes A. Switching model predictive attitude control for a quadrotor helicopter subject to atmospheric disturbances[J]. Control Engineering Practice, 2011, 19(10): 1195-1207.

[8] Xu R, Ozguner U. Sliding Mode Control of a Quadrotor Helicopter[A]. IEEE Conference on Decision and Control[C]. 2006:4957-4962.

[9] Hoffman G M, Huang H M, Waslander S L, et al. Precision flight control for a multi-vehicle quadrotor helicopter testbed[J]. Control Engineering Practice, 2011,19(9): 1023-1036.

[10] Wang J, Huang Y Ma L. Extending the Player Project for flexible cooperation of multiple robots[A]. International Conference on Mechatronics and Automation(ICMA)[C]. 2012: 1657-1662.

Quadrotor Tracking Control Based on PID Algorithm

Li Xi，Chen Yang，Chen Pengzhen

(College of information science and Engineering, Wuhan University of Science and Technology, Wuhan 430081, China)

Quadrotor is a way to VTOL aircraft that has compact structure and unique fight, with underactuated and strong coupling characteristics. For controlling the aircraft to arrive at set-point and maintains a stable hover from initial location, the article proposed the method of double close-loop PID control based on the dynamic model according to its flight characteristics, the outer ring accomplishes location control,we set the error between set-point and feedback actual location as the controller. the inner ring accomplishes attitude control, the set-value of the attitude results form the inverse solution of outer ring’s output, that the error between the set-value and actual attitude is the input of controller .after double close-loop PID, the signals for controlling attitude will be output, which can convert to four value used to control motor speed via the decoupling of cross dynamic model of quadrotor. The task of set-point tracking can be accomplished according to above all. the method of double close-loop PID control in this article can be verified with simulation experiments, providing a basis for furture study on the quadrotor.

quadrotor; dynamic model; double close-loop; PID control; set- point tracking

2016-01-13；

2016-03-07。

國(guó)家自然科學(xué)基金項(xiàng)目(62103331, 61573263)；湖北省科技支撐計(jì)劃項(xiàng)目(2015BAA018)。

李 希(1994-)，女，湖北咸寧人，主要從事旋翼飛行器自主控制方向的研究 。

1671-4598(2016)07-0109-04

10.16526/j.cnki.11-4762/tp.2016.07.029

TP301.6 文獻(xiàn)標(biāo)識(shí)碼：A