四旋翼無人飛行器控制算法設計

趙 玥，陳奕梅

天津工業大學 電氣工程與自動化學院，天津 300387

四旋翼無人飛行器控制算法設計

趙 玥，陳奕梅

天津工業大學 電氣工程與自動化學院，天津 300387

針對Qball-X4四旋翼無人飛行器的自身特點，建立系統的非線性模型，采用姿態內環和位置外環的雙閉環控制算法。線性二次型調節器（LQR）可以快速簡便地求解出最優的狀態反饋控制率，并且具有良好的魯棒性，因而利用LQR控制算法來控制姿態內環。由于PID控制算法結構簡單、魯棒性強，因而控制位置外環。通過Matlab/Simulink和飛行試驗對控制算法進行仿真和驗證，結果表明，設計的控制算法能成功地實現飛行器的懸停控制，并達到較好的控制效果。

四旋翼無人飛行器；非線性模型；線性二次型調節器；PID控制算法

1 引言

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

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

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

2 動力學建模

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

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

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

由電機參數可知，飛行器每個旋翼產生的升力和電機的PWM輸入滿足以下關系[10]：

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

四個旋翼產生的扭矩和電機PWM之間的關系可近似為：

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

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

3 控制算法設計

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

圖2 四旋翼飛行器系統控制框圖

3.1 姿態環控制算法設計

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

采用如下模型：

則最優控制向量為：

使以下性能指標達到最小值：

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

根據數學模型，令 φe=φcmd-φ ，取為狀態變量，則偏航角的狀態空間模型為：

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

3.2 非線性約束條件

由飛行器建模可知，外環位置和內環姿態建模的坐標系存在一個轉換關系，因此，飛行器內環的滾轉角和俯仰角的期望值是由外環X方向和Y方向的控制量反解得到，存在以下非線性約束關系：

因此，通過上述關系反解推導出俯仰角和滾轉角的期望值為：

3.3 位置環控制算法設計

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

根據模型所設計的位置PID控制算法為：

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

4 實驗驗證

將上章的內外環控制算法用于Qball-X4四旋翼無人飛行器，進行姿態控制和定點懸停控制試驗，對控制算法進行驗證。

4.1 姿態試驗

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

圖3 偏航角LQR控制框圖

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

圖4 姿態控制角度響應曲線

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

4.2 懸停試驗

通過試驗調試，在Simulink的基礎上，整定的位置環PID參數如下：

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

圖5 懸停控制姿態角響應曲線

圖6 懸停控制位置響應曲線

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

圖7 懸停控制實際電機PWM輸入曲線

5 總結

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

[1]刁琛.四旋翼無人飛行器非線性控制研究[D].天津：天津大學，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]張忠民，叢夢苑.基于線性二次調節器的四旋翼飛行器控制[J].應用科技，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].南京航空航天大學學報：英文版，2015，32（1）：29-34.

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

[10]Lucia W，Sznaier M，Franze 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版.北京：科學出版社，2007.

[12]何芝強.PID控制器參數整定方法及其應用研究[D].杭州：浙江大學，2005.

[13]鄭大鐘，王珊珊.線性二次型最優調節系統的參數穩定域[J].清華大學學報：自然科學版，1987（1）：34-37.

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

[15]李瑞琪，王洪福，李瑞雪，等.基于模型參考自適應的四旋翼飛行器控制[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

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

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

2016-06-17

2016-08-19

1002-8331（2017）21-0049-05

CNKI網絡優先出版：2016-12-21,http://www.cnki.net/kcms/detail/11.2127.TP.20161221.0843.024.html