四旋翼無人飛行器雙閉環(huán)PID控制器設(shè)計(jì)

賀 翔，陳奕梅，郭建川，修春波

HE Xiang, CHEN Yi-mei, GUO Jian-chuan, XIU Chun-bo

（天津工業(yè)大學(xué) 電氣工程與自動(dòng)化，天津 300387）

0 引言

四旋翼無人飛行器因其能夠垂直起降、自主懸停、可以側(cè)飛、倒飛等高機(jī)動(dòng)性能的特點(diǎn)，特別適用于在復(fù)雜地形與狹小空間使用，因此近年來，迅速成為UAV研究領(lǐng)域中的熱點(diǎn)[1]。

四旋翼無人飛行器運(yùn)動(dòng)系統(tǒng)是由四個(gè)輸入力產(chǎn)生六個(gè)自由度方向運(yùn)動(dòng)，有著非線性、多變量、欠驅(qū)動(dòng)和強(qiáng)耦合等特點(diǎn)。運(yùn)動(dòng)系統(tǒng)的描述一般采用歐拉角或者四元素法[2]。針對(duì)四旋翼無人飛行器運(yùn)動(dòng)系統(tǒng)的控制和系統(tǒng)實(shí)現(xiàn)，國(guó)內(nèi)外多家高校和研究機(jī)構(gòu)進(jìn)行了研究[3]，比較典型的控制方法有PID控制[4]，線性二次型最優(yōu)調(diào)節(jié)控制（LQR）[5]，滑模變結(jié)構(gòu)控制[6]，反步控制[7]和魯棒控制[8]等，由于控制器自身的復(fù)雜性，大多數(shù)都限于仿真研究，并未能真正用于實(shí)際系統(tǒng)中。

針對(duì)四旋翼無人飛行器的控制方法研究，加拿大Quanser公司開發(fā)了Qball-X4實(shí)驗(yàn)平臺(tái)，該平臺(tái)由定位攝像頭，四旋翼無人飛行器以及上位機(jī)組成。其中定位攝像頭用來采集飛行器的位置坐標(biāo)和姿態(tài)角。上位機(jī)則可以通過MATLAB編寫控制算法并生成可執(zhí)行程序通過Wi-Fi下載到飛行器上，并且在飛行過程中把定位攝像頭采集到的飛行器位置坐標(biāo)和姿態(tài)角實(shí)時(shí)傳輸?shù)斤w行器形成閉環(huán)[9]。

本文針對(duì)Qball-X4四旋翼飛行器，設(shè)計(jì)了一種基于雙閉環(huán)PID的四旋翼無人飛行器控制方法，并將其用于實(shí)驗(yàn)平臺(tái)，實(shí)現(xiàn)了室內(nèi)的定點(diǎn)懸停飛行，該控制器由內(nèi)環(huán)姿態(tài)控制器和外環(huán)位置控制器，內(nèi)外環(huán)之間通過實(shí)際偏航角進(jìn)行了控制量校正，提高了位移的控制精度。這種方法結(jié)構(gòu)清晰，控制器參數(shù)可通過實(shí)驗(yàn)平臺(tái)在線調(diào)節(jié)，便于工程實(shí)現(xiàn)。實(shí)驗(yàn)結(jié)果驗(yàn)證了控制方法的有效性。

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

1.1 四旋翼無人機(jī)的運(yùn)動(dòng)控制分析

如圖1所示，F(xiàn)i為第i個(gè)旋翼產(chǎn)生的升力，ui為第i個(gè)電機(jī)的PWM輸入值，roll、pitch、yaw分別為飛行器的滾轉(zhuǎn)、俯仰和偏航角。四旋翼直升機(jī)的1、3旋翼為順時(shí)針旋轉(zhuǎn)，2、4旋翼為逆時(shí)針旋轉(zhuǎn)，所以當(dāng)四個(gè)電機(jī)的轉(zhuǎn)速相等且升力之和等于機(jī)體重力時(shí)，無人機(jī)可實(shí)現(xiàn)懸停，保持轉(zhuǎn)速相等，升力之和增加或者減少，無人機(jī)可上升或者下降；保持升力之和等于重力，減少（增加）1、3旋翼轉(zhuǎn)速，同時(shí)增加（減少）2、4旋翼轉(zhuǎn)速，就會(huì)在繞z軸方向產(chǎn)生扭矩，無人機(jī)可實(shí)現(xiàn)關(guān)于y軸的偏航運(yùn)動(dòng)；若保持升力之和等于重力，減少（增加）1旋翼轉(zhuǎn)速，同時(shí)增加（減少）3旋翼轉(zhuǎn)速，無人機(jī)可實(shí)現(xiàn)關(guān)于x軸的俯仰運(yùn)動(dòng)；保持升力之和等于重力，減少（增加）2旋翼轉(zhuǎn)速，同時(shí)增加（減少）4旋翼轉(zhuǎn)速，無人機(jī)可實(shí)現(xiàn)關(guān)于y軸的滾轉(zhuǎn)運(yùn)動(dòng)。

圖1 Qball-X4動(dòng)力學(xué)示意圖

1.2 Qball-X4無人機(jī)建模

在建模過程中，假設(shè)四旋翼都能很好地與z軸平行排列且低速飛行的空氣阻力為零，則可以建立無人機(jī)的動(dòng)力學(xué)模型方程為[10,11]：

Qball-X4無人飛行器的每個(gè)旋翼產(chǎn)生的升力與電機(jī)的PWM輸入滿足一階慣性環(huán)節(jié)，傳遞函數(shù)為式(2)[9]：

其中ui是第i個(gè)電機(jī)的PWM輸入，ω是帶寬，K是正向增益。

每個(gè)旋翼產(chǎn)生的扭矩與電機(jī)的PWM輸入近似滿足如下關(guān)系[9]：

把式(2)和式(3)代入式(1)中可得Qball-X4無人機(jī)動(dòng)力學(xué)模型：

表1 Qball-X4參數(shù)表

為了便于控制器的設(shè)計(jì)，定義向量U=[U1,U2,U3,U4]T為虛擬控制量，與實(shí)際控制量即電機(jī)PWM輸入為u=[u1,u2,u3,u4]T的關(guān)系為：

即U=A-1u，其中變換矩陣A滿足：

求逆可知：

則u=AU。

通過變換矩陣A-1得到Qball-X4的最終模型為式(6)：

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

從四旋翼無人機(jī)的動(dòng)力學(xué)模型可以發(fā)現(xiàn)，無人機(jī)的飛行過程可以分解為在機(jī)體坐標(biāo)系中的姿態(tài)角變換和在慣性坐標(biāo)系中的質(zhì)心運(yùn)動(dòng)，其中慣性坐標(biāo)系中的質(zhì)心移動(dòng)依賴于機(jī)體坐標(biāo)系中的姿態(tài)角側(cè)傾來獲取側(cè)向加速度，因此通過嵌套的內(nèi)外環(huán)實(shí)現(xiàn)對(duì)四旋翼無人機(jī)的控制，其中內(nèi)環(huán)為姿態(tài)環(huán)，外環(huán)為位置環(huán)。綜合考慮方程本身的約束及定點(diǎn)飛行的任務(wù)目標(biāo)，在設(shè)計(jì)中滾轉(zhuǎn)角與仰俯角的期望值是通過位置控制系統(tǒng)反解得到。圖1給出了控制器結(jié)構(gòu)框圖，其中分別是位移和偏航角的參考輸入， ),,( zyx 和 ),,( ψθφ 分別是無人機(jī)位移和姿態(tài)的實(shí)際值， ),,,(4321uuuu 是四個(gè)電機(jī)的PWM輸入值， ),,,(4321UUUU 是經(jīng)過變換后的控制量， ),(rrΘΦ 是位移控制器的輸出。

2.1 內(nèi)環(huán)姿態(tài)控制器設(shè)計(jì)

根據(jù)式(6)設(shè)計(jì)姿態(tài)控制器如式(7)：

圖2 控制器結(jié)構(gòu)框圖

考慮到控制量可能超出電機(jī)PWM值得工作范圍，故姿態(tài)角的一階狀態(tài)量期望值全部置為零。通過合理調(diào)整的值，可使飛行器的姿態(tài)角達(dá)到穩(wěn)定。

2.2 控制量校正

由于外環(huán)位置控制器是在慣性坐標(biāo)系下經(jīng)過控制器計(jì)算得到了中間控制量，而內(nèi)環(huán)姿態(tài)角則是在機(jī)體坐標(biāo)系下進(jìn)行閉環(huán)控制，故內(nèi)外環(huán)需經(jīng)過控制量較正后進(jìn)行連接。

在高度z軸上滿足式(8)：

其中ue為高度控制器計(jì)算輸出的控制量，u是為了抵消重力所加的控制量偏置。

反解式(8)可得式(9)：

則式(10)簡(jiǎn)化為式(12)：

對(duì)式(12)進(jìn)行反解可得式(13)：

通過式(9)和式(13)進(jìn)行控制量校正，連接內(nèi)環(huán)姿態(tài)控制器和外環(huán)位置控制器。

2.3 位移控制器設(shè)計(jì)

本文位置控制器均采用經(jīng)典PID實(shí)現(xiàn)控制，根據(jù)四旋翼無人機(jī)動(dòng)力學(xué)模型設(shè)計(jì)如式(14)所示：

考慮控制量可能超過電機(jī)PWM值得輸入范圍，把位移的一階狀態(tài)量的期望值置為零。通過合理調(diào)整控制器參數(shù)可使四旋翼無人機(jī)的位移達(dá)到閉環(huán)穩(wěn)定，下章將通過實(shí)驗(yàn)對(duì)該控制器進(jìn)行驗(yàn)證。

3 實(shí)驗(yàn)驗(yàn)證

將上章設(shè)計(jì)的雙閉環(huán)PID控制器用于Qball-X4四旋翼無人直升機(jī)，進(jìn)行了定點(diǎn)懸停控制實(shí)驗(yàn)，對(duì)所設(shè)計(jì)的控制器進(jìn)行了實(shí)驗(yàn)驗(yàn)證。圖3是高度PID控制器的Simulink框圖。

圖3 高度PID控制器的Simulink框圖

圖3 中，高度由于存在外部重力作用，所以加入了控制量偏置，微分部分經(jīng)過濾波器進(jìn)行了濾波。其他位移、姿態(tài)控制器與高度控制器類似。

3.1 姿態(tài)控制器實(shí)驗(yàn)

由于本文采用嵌套的內(nèi)外環(huán)PID實(shí)現(xiàn)四旋翼無人機(jī)的控制，而內(nèi)環(huán)三個(gè)姿態(tài)角分別由控制量U2,U3,U4單獨(dú)控制，三個(gè)通道之間完全獨(dú)立，故在姿態(tài)角控制器參數(shù)調(diào)整實(shí)驗(yàn)過程中，可把各個(gè)通道單獨(dú)調(diào)試[12]。

例 如： 若 要 調(diào) 整 滾 轉(zhuǎn) 角φ ， 則 手 動(dòng) 使U1=u,U3=0,U4=0（其中u為電機(jī)工作范圍內(nèi)的定值），而控制量U2由滾轉(zhuǎn)角控制器解算，然后通過控制量變換輸入到四旋翼無人機(jī)，通過觀察無人機(jī)滾轉(zhuǎn)角的變化，在線調(diào)整參數(shù)，使?jié)L轉(zhuǎn)角達(dá)到控制要求。另外兩個(gè)姿態(tài)角也可用相同的方法進(jìn)行參數(shù)整定。整定后的控制器參數(shù)如式(15)所示：

圖4、5、6分別為滾轉(zhuǎn)角、俯仰角和偏航角的期望值都為0°，且在初始姿態(tài) ),,( ψθφ =(-0.18°，-15°，-6.5°時(shí)的響應(yīng)曲線。由圖可知，俯仰角與滾轉(zhuǎn)角的穩(wěn)態(tài)誤差在±2°范圍內(nèi)，而偏航角的控制誤差不超過±1.5°，取得了較好的控制效果。

圖4 滾轉(zhuǎn)角響應(yīng)曲線

圖5 俯仰角響應(yīng)曲線

圖6 偏航角響應(yīng)曲線

3.2 定點(diǎn)懸停實(shí)驗(yàn)

位移控制器整定參數(shù)如式(16)所示：

把所設(shè)計(jì)的控制器下載到飛行器上，設(shè)位置期望值為(x,y,z)=(0,0,0.5)，進(jìn)行飛行器起飛和定點(diǎn)懸停實(shí)驗(yàn)，圖7、圖8為實(shí)驗(yàn)的位移和姿態(tài)角響應(yīng)曲線。

由圖7可知，飛行器在起飛過程中存在誤差范圍內(nèi)的振蕩，達(dá)到相對(duì)穩(wěn)定后，高度(z)誤差在±0.05mm的范圍內(nèi)，位移誤差(x，y)在±0.1m的范圍內(nèi)小幅振蕩。圖8中，姿態(tài)角的跟蹤誤差控制在±3°之內(nèi)小幅振蕩。

在其他類似實(shí)驗(yàn)中，采用PID室內(nèi)懸停的x、y軸的位移誤差在±0.2m范圍內(nèi)[13]，比較可知，本文的控制方法是有效的，并具有較高的精度。

圖9和圖10是控制量的輸入波形，為了保障電機(jī)的可靠運(yùn)行，電機(jī)的PWM輸入的額定范圍應(yīng)限制在[0.05,0.1][9]，本次實(shí)驗(yàn)的電機(jī)輸入在電機(jī)的工作范圍內(nèi)，說明本文的控制器設(shè)計(jì)是合理的。其中，由于飛行器偏航角方向沒有調(diào)平，所以虛擬控制量U在-0.01附近振蕩調(diào)節(jié)，而不是在0附近。由式(5)可知，圖10中控制量u2,u4高于u1,u3是符合實(shí)際情況的。

圖7 位移響應(yīng)曲線

圖8 姿態(tài)角響應(yīng)曲線

圖9 虛擬控制量輸入U(xiǎn)

圖10 電機(jī)PWM值輸入u

4 結(jié)論

本文根據(jù)四旋翼飛行器動(dòng)力學(xué)模型，設(shè)計(jì)了雙閉環(huán)PID控制器，實(shí)現(xiàn)了四旋翼無人飛行器的定點(diǎn)懸?？刂疲⒃赒ball-X4實(shí)驗(yàn)平臺(tái)進(jìn)行了姿態(tài)控制實(shí)驗(yàn)和定點(diǎn)懸停實(shí)驗(yàn)，由實(shí)驗(yàn)結(jié)果可知，姿態(tài)角跟蹤誤差在±3°之內(nèi)，位移(x,y軸)誤差在±0.1m之內(nèi)，高度控制誤差在±0.05m之內(nèi)，實(shí)驗(yàn)結(jié)果驗(yàn)證了控制器的有效性和精度。

在未來的工作中，將會(huì)使用該控制器結(jié)合有限狀態(tài)機(jī)理論進(jìn)行飛行器的自主起飛和降落以及路徑跟蹤方面的研究。

[1] 王璐.四旋翼無人飛行器控制技術(shù)研究[D].哈爾濱工程大學(xué),2012.

[2] Xian Bin,Diao Chen,Zhao Bo,Zhang Yao.Nonlinear robust output feedback tracking control of a quadrotor UAV using quaternion representation[J].Nonlinear dynamics,2015,79(4):2735-2752.

[3] Zhen Zi-yang,Pu Huang-zhong,Chen Qi,Wang Xin-hua.Nonlinear intelligent flight control for quadrotor unmanned helicopter[J].Transactions of Nanjing University of Aeronautics and Astronautics,2015,01:29-34.

[4] Wahyudie A.,Susilo,T.,Noura, H. Robust.PID Controller for Quadrotors[J].Journal of Unmanned System Technology,2013,1(1):14-19.

[5] 高青,袁亮,吳金強(qiáng).基于新型LQR的四旋翼無人機(jī)姿態(tài)控制[J].制造業(yè)自動(dòng)化,2014, 36(10):13-16.

[6] Mohd Basri,Mohd Ariffanan,Husain,Abdul Rashid,Danapalasingam, Kumeresan A.Intelligent adaptive backstepping control for MIMO uncertain non-linear quadrotor helicopter systems[J].Transactions of the Institute of Measurement and Control,2015,37(3):345-361.

[7] Zheng En-Hui,Xiong Jing-Jing,Luo Ji-Liang.Second order sliding mode control for a quadrotor UAV[J].ISA Transactions,2014,53(4):1350-1356.

[8] 甄紅濤,齊曉慧,夏明旗,蘇立軍.四旋翼無人機(jī)魯棒自適應(yīng)姿態(tài)控制[J].控制工程,2013,20(5):915-919.

[9] Lucia,W.,Sznaier,M.,Franze,G. An obstacle avoidance and motion planning CommandGovernor based scheme: The Qball-X4quadrotor case of study[C].2014 IEEE 53rd Annual Conference on Decision and Control,2014:6135-6140.

[10] 譚廣超.四旋翼飛行器姿態(tài)控制系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)[D].大連理工大學(xué),2013.

[11] WANG Hong-yu,ZHAO Jian-kang,YU Wen-xian,TIAN Wei-feng. Modelling and position tracking control for quadrotor vehicle[J]. Journal of Chinese Inertial Technology,2012,20(4):455-458.

[12] 郭建川,鮮斌,王福,張旭.三自由度無人直升機(jī)實(shí)驗(yàn)平臺(tái)與飛行姿態(tài)控制器設(shè)計(jì)[A].第三十二屆中國(guó)控制會(huì)議[C].2013:733-738.

[13] 白永強(qiáng),劉昊,石宗英,鐘宜生.四旋翼無人直升機(jī)魯棒飛行控制[J].機(jī)器人,2012,34(5):519-524.