趙 玥，陳奕梅

天津工業(yè)大學(xué) 電氣工程與自動化學(xué)院，天津 300387

四旋翼無人飛行器控制算法設(shè)計

趙 玥，陳奕梅

天津工業(yè)大學(xué) 電氣工程與自動化學(xué)院，天津 300387

針對Qball-X4四旋翼無人飛行器的自身特點，建立系統(tǒng)的非線性模型，采用姿態(tài)內(nèi)環(huán)和位置外環(huán)的雙閉環(huán)控制算法。線性二次型調(diào)節(jié)器（LQR）可以快速簡便地求解出最優(yōu)的狀態(tài)反饋控制率，并且具有良好的魯棒性，因而利用LQR控制算法來控制姿態(tài)內(nèi)環(huán)。由于PID控制算法結(jié)構(gòu)簡單、魯棒性強，因而控制位置外環(huán)。通過Matlab/Simulink和飛行試驗對控制算法進行仿真和驗證，結(jié)果表明，設(shè)計的控制算法能成功地實現(xiàn)飛行器的懸?？刂疲⑦_到較好的控制效果。

四旋翼無人飛行器；非線性模型；線性二次型調(diào)節(jié)器；PID控制算法

1 引言

四旋翼無人飛行器是一種微小型飛行器，能夠進行自主懸停，前飛、側(cè)飛、倒飛等動作，具有性能優(yōu)良、體積小、飛行速度快、續(xù)航能力強等許多特點[1]，因此，被廣泛用于軍事偵察、航拍、搜索、緊急救援、情報獲取等任務(wù)，具有很好的實際應(yīng)用性和發(fā)展前景。由于無人機在國防、民用、商用等方面的巨大價值，已經(jīng)成為控制方面研究的熱點[2]。

四旋翼無人飛行器是典型的非線性系統(tǒng)，具有多變量，欠驅(qū)動，強耦合等特點。針對飛行器控制算法問題，國內(nèi)外高校和研究機構(gòu)都進行了許多研究[3]。早期，在研究此類問題時，都將無人機模型線性化，運用經(jīng)典線性控制理論解決此類問題，例如：PID控制算法[4]，LQR控制算法[5]等。之后，為了解決不確定的外部干擾以及模型的不精確，又將滑?？刂扑惴╗6]，自抗擾控制算法[7]等應(yīng)用在飛行器中。但是由于實際系統(tǒng)的復(fù)雜性，大多數(shù)研究也僅限于仿真，并沒有在實際系統(tǒng)中進行驗證。

本文針對Quanser公司生產(chǎn)的Qball-X4四旋翼飛行器試驗平臺，考慮到實際飛行中的各種不確定情況，設(shè)計了雙閉環(huán)的四旋翼無人飛行器的控制算法，并應(yīng)用到實際平臺中，實現(xiàn)了室內(nèi)的懸停飛行試驗。控制算法分為兩部分，內(nèi)環(huán)采用LQR控制算法，外環(huán)采用PID控制算法。這種控制算法結(jié)構(gòu)簡單、魯棒性強、可靠性高，并且試驗結(jié)果驗證控制方法是有效的，因此可以將此方法應(yīng)用于飛行器實際控制中[8]。

2 動力學(xué)建模

四旋翼飛行器呈十字交叉結(jié)構(gòu)，四個驅(qū)動電機對稱安裝在十字結(jié)構(gòu)末端，通過調(diào)節(jié)四個電機的轉(zhuǎn)速完成飛行器的飛行控制。1，3號電機順時針旋轉(zhuǎn)，2，4號電機逆時針旋轉(zhuǎn)。Fi是第i個電機產(chǎn)生的升力（i=1，2，3，4），在機體坐標(biāo)系下，定義飛行器繞Y軸旋轉(zhuǎn)產(chǎn)生的角度為φ滾轉(zhuǎn)角（Roll），定義繞X軸旋轉(zhuǎn)產(chǎn)生的角度為θ俯仰角（Pitch），定義繞Z軸旋轉(zhuǎn)產(chǎn)生的角度為φ偏航角（Yaw）。圖1為Qball-X4的實物模型和動力學(xué)建模示意圖。

圖1 四旋翼飛行器實物圖與動力學(xué)建模示意圖

考慮到實際飛行的特點，在慢速飛行下，忽略空氣阻力且飛行器旋翼沒有彈性形變及認(rèn)定其為剛體的情況下，對飛行器進行建模，其數(shù)學(xué)模型如下[9]：其中，(x,y,z)是指飛行器相對于地面坐標(biāo)系原點的位置，m是指飛行器質(zhì)量，g是指重力加速度，l是指飛行器坐標(biāo)系中心到旋翼中心的長度，(Jx,Jy,Jz)是指飛行器繞機體坐標(biāo)系X軸Y軸Z軸旋轉(zhuǎn)的轉(zhuǎn)動慣量，τi是指第i個旋翼產(chǎn)生的偏轉(zhuǎn)扭矩。

由電機參數(shù)可知，飛行器每個旋翼產(chǎn)生的升力和電機的PWM輸入滿足以下關(guān)系[10]：

其中，ui是指第i個電機的PWM輸入，K是指正增益系數(shù)，ω是指帶寬。

四個旋翼產(chǎn)生的扭矩和電機PWM之間的關(guān)系可近似為：

為了便于設(shè)計控制量，定義虛擬控制量U1=u1+u2+u3+u4，U2=u3-u4，U3=u1-u2，U4=u1+u2-u3-u4。根據(jù)以上條件，將飛行器模型簡化為：

觀察比較2組患者手術(shù)時間，術(shù)后引流時間及術(shù)中出血量，術(shù)后3 d、5 d及7 d患者VAS評分，術(shù)后并發(fā)癥，患者治療滿意率。隨訪1年，觀察2組患者復(fù)發(fā)情況。VAS評分[9]：評估患者疼痛程度，評分范圍0～10分，分值越高表示患者疼痛越嚴(yán)重。

3 控制算法設(shè)計

四旋翼飛行器是典型的欠驅(qū)動系統(tǒng)，在控制過程中，需要調(diào)整四個電機的PWM輸入而達到對飛行器的位置 (x,y,z)和姿態(tài) (θ,φ,φ)控制。因此，在設(shè)計控制算法時，采用內(nèi)外雙閉環(huán)控制策略。先設(shè)計內(nèi)環(huán)姿態(tài)角控制算法，再設(shè)計外環(huán)位置控制算法。其中，外環(huán)控制位置，飛行器按照目標(biāo)軌跡信號進行飛行或到達期望參考位置并提供內(nèi)環(huán)的期望輸入；通過非線性約束條件反解，可得到內(nèi)環(huán)姿態(tài)環(huán)的期望輸入，通過姿態(tài)控制器控制姿態(tài)角，保證飛行器姿態(tài)穩(wěn)定?？刂瓶驁D如下，其中，(xcmd,ycmd,zcmd,θcmd,φcmd,φcmd)為位置和角度的期望值，(x,y,z,θ,φ,φ)為位置和角度的實際值，(Ux,Uy)為X、Y 方向的控制量輸出，(U1,U2,U3,U4)為虛擬控制量，(u1,u2,u3,u4)為實際輸入給無人機電機的PWM值。

圖2 四旋翼飛行器系統(tǒng)控制框圖

3.1 姿態(tài)環(huán)控制算法設(shè)計

LQR（Linear Quadratic Regulator）即線性二次型調(diào)節(jié)器，是一種基于狀態(tài)空間模型的最優(yōu)控制算法，其最優(yōu)解具有統(tǒng)一的表達式，容易快速地求解出狀態(tài)反饋控制率，使系統(tǒng)快速達到預(yù)定狀態(tài)并使系統(tǒng)性能指標(biāo)最優(yōu)。針對四旋翼飛行器系統(tǒng)，采用輸出跟蹤系統(tǒng)的解決模式，使輸出量始終跟蹤輸入量的變化，因此，轉(zhuǎn)化為保證誤差在0平衡狀態(tài)[11]。

采用如下模型：

則最優(yōu)控制向量為：

使以下性能指標(biāo)達到最小值：

其中，輸出誤差向量e(t)=z(t)-y(t)，z(t)為理想輸出向量，F(xiàn),Q(t),R(t)為加權(quán)矩陣且為非負矩陣[5]。

根據(jù)數(shù)學(xué)模型，令 φe=φcmd-φ ，取為狀態(tài)變量，則偏航角的狀態(tài)空間模型為：

LQR算法中的Q(t)和R(t)分別是對狀態(tài)變量和輸入向量的加權(quán)矩陣，通常情況下，R(t)為對稱的正定矩陣，Q(t)為對稱的半正定矩陣，在工程應(yīng)用中，經(jīng)常取為對角陣。最終系統(tǒng)的動態(tài)性能，取決于Q(t)和R(t)的選擇。利用Matlab中的 K=lqr（A，B，C，D）函數(shù)可求解出最優(yōu)反饋矩陣K。同理，可寫出滾轉(zhuǎn)角和偏航角的誤差狀態(tài)空間模型，以便后續(xù)試驗調(diào)整Q(t)和R(t)參數(shù)。

3.2 非線性約束條件

由飛行器建模可知，外環(huán)位置和內(nèi)環(huán)姿態(tài)建模的坐標(biāo)系存在一個轉(zhuǎn)換關(guān)系，因此，飛行器內(nèi)環(huán)的滾轉(zhuǎn)角和俯仰角的期望值是由外環(huán)X方向和Y方向的控制量反解得到，存在以下非線性約束關(guān)系：

因此，通過上述關(guān)系反解推導(dǎo)出俯仰角和滾轉(zhuǎn)角的期望值為：

3.3 位置環(huán)控制算法設(shè)計

由于四旋翼飛行器是典型的非線性系統(tǒng)，非線性控制算法對模型的準(zhǔn)確性依賴較高，因此，在模型存在不確定的因素時，PID控制算法更加實用，并且魯棒性好[12]。

根據(jù)模型所設(shè)計的位置PID控制算法為：

根據(jù)上述控制算法，搭建Simulink框圖，進行位置外環(huán)仿真，確定PID控制算法參數(shù)的大致范圍。下一章將對所設(shè)計的控制算法進行試驗。

4 實驗驗證

將上章的內(nèi)外環(huán)控制算法用于Qball-X4四旋翼無人飛行器，進行姿態(tài)控制和定點懸?？刂圃囼?，對控制算法進行驗證。

4.1 姿態(tài)試驗

由于三個姿態(tài)角分別由U2,U3,U4分別控制，且控制量由位置環(huán)解算得到，因此在調(diào)試姿態(tài)角參數(shù)時，可以將內(nèi)環(huán)外環(huán)獨立分開，然后將姿態(tài)環(huán)分通道，單獨調(diào)試。以偏航角為例，圖3為偏航角Simulink控制圖，考慮到電池在工作時會出現(xiàn)電量的高低不同，因此設(shè)計兩個最優(yōu)反饋控制量ki_yaw_up和ki_yaw_down，保證飛行器在不同電量時，飛行狀態(tài)的穩(wěn)定。LQR控制框圖如圖3。

圖3 偏航角LQR控制框圖

在調(diào)節(jié)偏航角時，手動設(shè)置總升力U1=0.15,U2=U3=0，偏航角控制量U4由控制算法給出。通過觀察偏航角變化，調(diào)整Q和R參數(shù)，使偏航角達到穩(wěn)定。同理，另外兩個角使用相同的調(diào)試方法。在調(diào)試時發(fā)現(xiàn)，Q(t)對角陣中的對應(yīng)加權(quán)數(shù)值的大小，代表了控制量對輸出量的控制作用，加權(quán)數(shù)值越大，表示控制作用越明顯，超調(diào)量變大，響應(yīng)速度變快。R(t)則顯示了控制量的大小而引起的能量消耗，加權(quán)數(shù)值越大，控制量越小[13]。綜合系統(tǒng)的動態(tài)性能，保證控制量和響應(yīng)速度的前提下，選取偏航角加權(quán)矩陣Q_yup=diag（[160 90 1]）、R_yup=1 000 和 Q_ydown=diag（[185 80 1]）、R_ydown=1 000。滾轉(zhuǎn)角和俯仰角選取加權(quán)矩陣Qup=diag（[230 4 7]）、Rup=100 和 Qdown=diag（[590 22 16]）、Rdown=100。仿真結(jié)果如圖4。

圖4 姿態(tài)控制角度響應(yīng)曲線

通過試驗數(shù)據(jù)可得：首先設(shè)定偏航角、滾轉(zhuǎn)角和俯仰角的期望值為0，觀察響應(yīng)曲線，俯仰角在響應(yīng)初期，存在超調(diào)。達到穩(wěn)定狀態(tài)之后，存在一定的穩(wěn)態(tài)誤差（俯仰角誤差為-0.2°左右，滾轉(zhuǎn)角誤差在-0.4°左右），在誤差允許范圍內(nèi)。在運行一段時間后，手動將偏航角期望值改為3rad（≈17°），從試驗結(jié)果看，偏航角能迅速達到預(yù)定目標(biāo)，穩(wěn)定后，誤差在1°左右。達到穩(wěn)定后，再將偏航角手動調(diào)整為0°，在控制作用下，偏航角仍能迅速調(diào)整，達到期望值0°附近，并且上下波動。試驗結(jié)果表明，姿態(tài)控制器能達到預(yù)期效果。

4.2 懸停試驗

通過試驗調(diào)試，在Simulink的基礎(chǔ)上，整定的位置環(huán)PID參數(shù)如下：

根據(jù)上述參數(shù)，設(shè)定初始位姿期望值(xcmd,ycmd,zcmd,φcmd)=(0,0,0.3,0)，進行飛行器定點懸停試驗。試驗的角度和位置響應(yīng)曲線如圖5、圖6。

圖5 懸停控制姿態(tài)角響應(yīng)曲線

圖6 懸?？刂莆恢庙憫?yīng)曲線

由圖7得到：為保證飛行器能抵消重力起飛，需保證四個電機的PWM值輸入均達到0.5左右[10]，需適當(dāng)調(diào)整內(nèi)環(huán)LQR控制算法的R(t)參數(shù)，以確保達到所需控制量。在飛行器起飛之后，俯仰角和滾轉(zhuǎn)角在期望值附近波動，偏航角波動較大，達到穩(wěn)定之后，俯仰角和滾轉(zhuǎn)角的誤差在±2°以內(nèi)，偏航角誤差在±4°以內(nèi)。位置方面，X的初始位置與地面坐標(biāo)系原點有0.02 m的偏差，由于在地面鋪設(shè)保護措施，高度Z方面存在0.028 8 m的偏差。起飛之后，高度Z平穩(wěn)達到預(yù)定0.3 m左右，誤差±0.02 m，X和Z方向相應(yīng)波形均在期望值上下波動，誤差±0.05 m左右。在其他室內(nèi)懸停試驗中，位置誤差均保證在±0.3 m以內(nèi)，對比可知，本次位置控制在較小幅震蕩范圍內(nèi)，在實際飛行時，上述誤差均不影響控制效果，控制方法有效[14]。

圖7 懸?？刂茖嶋H電機PWM輸入曲線

5 總結(jié)

本文依據(jù)LQR控制算法，以姿態(tài)的狀態(tài)方程為內(nèi)環(huán)模型，采用內(nèi)環(huán)姿態(tài)環(huán)LQR控制算法，并且，為減小非線性模型的不準(zhǔn)確，采用PID控制算法算法設(shè)計外環(huán)姿態(tài)環(huán)控制器。本套飛行器的控制算法，經(jīng)過試驗驗證，可以達到預(yù)期的控制效果，驗證了控制算法的有效性，對四旋翼飛行器實際系統(tǒng)有一定參考價值。在未來工作中，還會在此方法上進一步調(diào)整信號融合方面問題。

[1]刁琛.四旋翼無人飛行器非線性控制研究[D].天津：天津大學(xué)，2013.

[2]Zhao B，Xian B，Zhang Y，et al.Nonlinear robust adaptive tracking control of a quadrotor UAV via immersion and invariance methodology[J].IEEE Transactions on Industrial Electronics，2015，62（5）：2891-2902.

[3]Kerma M，Mokhtari A，Abdelaziz B，et al.Nonlinear H∞control of a Quadrotor（UAV），using high order sliding mode disturbance estimator[J].International Journal of Control，2012，85（12）：1876-1885.

[4]García R A，Rubio F R，Ortega M G.Robust PID control of the quadrotorhelicopter[J].IFAC Proceedings Volumes，2012，45（3）：229-234.

[5]張忠民，叢夢苑.基于線性二次調(diào)節(jié)器的四旋翼飛行器控制[J].應(yīng)用科技，2011，38（5）：38-42.

[6]Espinoza T，Dzul A E，Lozano R，et al.Backsteppingsliding mode controllers applied to a fixed-wing UAV[J].Journal of Intelligentamp;Robotic Systems，2014，73（1）：67-79.

[7]Gao Tongyue，Tang Rui，Rao Junjin，et al.On-line adaptive controller system used on small UAV[J].Procedia Engineering，2012，45：980-985.

[8]甄子洋，浦黃忠，陳琦，等.Nonlinear intelligent flight control for quadrotor unmanned helicopter[J].南京航空航天大學(xué)學(xué)報：英文版，2015，32（1）：29-34.

[9]凌金福.四旋翼飛行器飛行控制算法的研究[D].南昌：南昌大學(xué)，2013.

[10]Lucia W，Sznaier M，F(xiàn)ranze G.An obstacle avoidance and motion planning command governor based scheme：The Qball-X4 quadrotor case of study[C]//53rd IEEE Conference on Decision and Control，2014：6135-6140.

[11]胡壽松，自動控制原理[M].5版.北京：科學(xué)出版社，2007.

[12]何芝強.PID控制器參數(shù)整定方法及其應(yīng)用研究[D].杭州：浙江大學(xué)，2005.

[13]鄭大鐘，王珊珊.線性二次型最優(yōu)調(diào)節(jié)系統(tǒng)的參數(shù)穩(wěn)定域[J].清華大學(xué)學(xué)報：自然科學(xué)版，1987（1）：34-37.

[14]白永強，劉昊，石宗英，等.四旋翼無人直升機魯棒飛行控制[J].機器人，2012（5）：519-524.

[15]李瑞琪，王洪福，李瑞雪，等.基于模型參考自適應(yīng)的四旋翼飛行器控制[J].計算機測量與控制，2013（12）：3260-3263.

ZHAO Yue,CHEN Yimei

School of Electrical Engineering and Automation,Tianjin Polytechnic University,Tianjin 300387,China

Design of four rotor unmanned spacecraft control algorithm.Computer Engineering and Applications,2017,53（21）：49-53.

For Qball-X4 four-rotor unmanned aircraft with its own characteristics,creating nonlinear model of system and adopting double closed-loop control algorithm with inner and outer loop are controlled by attitude and position respectively.Linear quadratic regulator is easy to solve state feedback control rate quickly,and has good robustness,thus LQR is utilized to design attitude inner loop controller.Due to the PID control algorithm has simple structure,strong robustness,thus PID is utilized to design position loop controller.Utilize Matlab/Simulink and flight test to verify control algorithm,and the result shows that the control algorithm can successfully achieve the hovering control,and achieve good control effect.

four-rotor unmanned aircraft;nonlinear model;linear quadratic regulator;PID control algorithm

A

TP273

10.3778/j.issn.1002-8331.1606-0249

天津市自然科學(xué)基金（No.15JCYBJC47800）。

趙玥（1991—），女，碩士研究生，主要研究領(lǐng)域為四旋翼無人飛行器，E-mail：cynthia_zyue@foxmail.com；陳奕梅（1972—），女，博士，副教授，主要研究領(lǐng)域為機器人控制技術(shù)。

2016-06-17

2016-08-19

1002-8331（2017）21-0049-05

CNKI網(wǎng)絡(luò)優(yōu)先出版：2016-12-21,http://www.cnki.net/kcms/detail/11.2127.TP.20161221.0843.024.html