李 希，陳 洋，陳鵬震

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

基于PID算法的四旋翼飛行器定點跟蹤控制

李 希，陳 洋，陳鵬震

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

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

四旋翼飛行器；動力學模型；雙閉環(huán)；PID控制器；定點跟蹤

0 引言

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

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

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

1 動力學模型

圖1 四旋翼飛行器動力學建模示意圖

1.1 平移動力學模型

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

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

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

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

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

在導航系中機體的升力為：

(1)

(2)

1.2 轉(zhuǎn)動動力學模型

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

(3)

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

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

(4)

飛行器在懸?；蛘呤莿蛩龠\動時，可以認為機體的歐拉角變化很小，接近0度，將θ=0,γ=0,φ=0代入公式(4)，得：

(5)

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

(6)

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

(7)

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

(8)

通過改變飛行器電機轉(zhuǎn)速，可以實現(xiàn)各種不同的運動?？蓪⑵浜线\動分解到四個獨立方向，分別為：

1)上下運動(控制律U1)。

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

2)前后運動和俯仰角θ(控制律U2)。

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

3)左右運動和橫滾角γ(控制律U3)。

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

4)偏航運動和偏航角φ(控制律U4)。

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

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

假設1電機朝前為正方向，2電機朝右為正方向，電機1、3的旋轉(zhuǎn)方向為偏航角的正方向，則每個通道的控制律如下：

(9)

將公式(9)代入公式(2)和公式(8)中即可得到簡化的動力學模型，如式(10)所示。

(10)

2 控制器的設計

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

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

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

(11)

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

(12)

由公式(12)可解得：

而φr=0

(13)

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

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

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

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

3 實驗結(jié)果與分析

用Matlab的Simulink模塊搭建上述四旋翼飛行器動力學模型及其控制器，通過仿真結(jié)果驗證此方法的可行性。其中飛行器參數(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左右能夠很快的到達設定目標點并能夠穩(wěn)定懸停，圖5為飛行器的三維飛行軌跡圖，由此可以看出通過設計出的雙閉環(huán)PID控制器來控制飛行器完成指定飛行任務是可行的。

圖3 四旋翼飛行器飛行位置坐標曲線

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

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

4 結(jié)論

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

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

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

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

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

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

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

[6] 聶博文. 微小型四旋翼無人直升機建模及控制方法研究[D]. 長沙：國防科學技術(shù)大學，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。

國家自然科學基金項目(62103331, 61573263)；湖北省科技支撐計劃項目(2015BAA018)。

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

1671-4598(2016)07-0109-04

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

TP301.6 文獻標識碼：A