2. 湖南山河科技股份有限公司, 湖南 株洲 412000
2. Hunan Sunward Aircraft Co., Ltd., Zhuzhou 412000, China
0 引言
四旋翼无人机是一种微小型、性能优良的VTOL(垂直起降)直升机,由于其结构简单、机动性能好、成本较低,现在已经逐渐被广泛应用到国防、民用、商用等领域.但是四旋翼是一个多输入多输出的非线性、强耦合、欠驱动的控制系统,而且由于多种因素造成的不确定干扰使得飞行控制器设计相对复杂,因此无人机的飞行控制算法也成为近年来研究的热点.
目前,国内外学者提出许多控制算法来解决四旋翼无人机姿态控制问题,常用的控制算法有经典PID控制[1-4]、LQR[5-8](线性二次型调节器)、反演法[9-10]、滑模变结构控制[11-12]等.其中经典PID控制器不需要特定的模型参数,易于实现,也是目前使用最多的控制算法,但是抗干扰能力较差而且对于非线性动态特性的控制效果不够理想. PID姿态双闭环回路控制相比于PID姿态单闭环控制回路增强了系统的抗干扰能力,提高了系统适应能力和控制精度. LQR控制器有着不错的鲁棒性,但其控制性能比较依赖模型的精度.鲁棒伺服LQR控制(RSLQR)[13-17]是在LQR控制的基础上发展起来的,它的思想是将状态偏差量定义为新的状态向量引入到系统中,通过控制律设计使偏差量调节为零,从而达到系统状态变量准确跟踪输入控制指令的目的.反步法有着不错的非线性系统跟踪性能且调节时间较快,但计算量较大,鲁棒性也比较差.滑模控制是一种非线性变结构控制,对模型误差、参数不确定性和外部干扰具有不敏感性,鲁棒性较强,但会发生抖振.
本文针对姿态角、姿态角速率分别设计PID控制器与RSLQR控制器组成双闭环回路姿态控制器[18],对四旋翼的角度和角速率同时进行控制相比于只有角度控制有着更高的控制精度.并且在上述控制器基础上引入线性扩张状态观测器(LESO),新型控制器可以对外部扰动和内部模型扰动进行实时控制量补偿,来解决PID-RSLQR控制器由于模型不确定性和外部干扰等造成的控制不稳定.
1 四旋翼无人机动力学模型本文研究的是“X”型布局的四旋翼无人机,其空气动力学模型如图 1所示,四旋翼相邻两组旋翼旋转方向相反,利用相邻旋翼产生的反扭力矩之差产生偏航力矩进行偏航动作,通过控制前后(左右)旋翼升力不同产生俯仰(横滚)力矩执行俯仰(横滚)动作.
为了方便研究机体模型和控制算法可以做如下假设:
1) 四旋翼无人机为均匀对称的刚体;
2) 机体坐标系原点与质心重合;
3) 螺旋桨为刚性,不发生形变.
以右手坐标系分别建立地理坐标系E(X,Y,Z)和机体坐标系B(x,y,z),如图 1所示.定义四旋翼无人机滚转角为φ,俯仰角为θ,偏航角为ψ,根据机体坐标系与地理坐标系间的转换关系,可得
(1) |
式中
(2) |
根据欧拉公式可推导四旋翼转动方程[11]如下
(3) |
式中,Ix,Iy,Iz为机体绕自身x轴,y轴,z轴的转动惯量,Jr为旋翼绕旋转轴的转动惯量,Ω为正转与反转旋翼转速之差,L是电机到机体中心的长度,α是机臂与机体水平轴线的夹角.
(4) |
其中,f1,f2,f3,f4为每个电机产生的升力,M1,M2,M3,M4为每个电机产生的反扭矩.
2 姿态控制器设计为实现对四旋翼无人机姿态的稳定控制,设计由姿态外回路PID控制器和姿态角速率内回路RSLQR控制器组成的串级双闭环回路姿态控制器.对姿态外回路设计PID控制器用于跟踪期望姿态角和产生期望角速率,对于角速率内回路设计RSLQR控制器来跟踪期望的角速率和计算作用于电机的控制量.
2.1 外回路的设计本文以滚转通道为例,其余通道内回路控制器设计方法类似,由式(3)可得
(5) |
滚转角外环利用PID控制器的P控制来跟随期望角度,期望滚转角与当前滚转角输入到P控制器中产生期望滚转角速率,控制律表达式如下:
(6) |
式中φsp表示期望滚转角,kP表示比例系数,比例系数影响滚转角响应的快慢,kP增大时,姿态响应加快;kP减小时,姿态响应变慢.
2.2 内回路的设计设计RSLQR控制器首先需要获取对象的状态空间方程,需对式(5)中进行简化以获取线性模型.假设忽略姿态运动之间的耦合,陀螺效应对控制的影响及空气阻力等因素,滚转通道的数学模型可以简化为
(7) |
则有系统状态空间方程:
(8) |
取状态变量
(9) |
式中,
将LQR控制算法应用于上式(9),性能指标函数为
性能加权矩阵R和Q影响系统的控制能力和调节速度,一般来讲,增大Q值可以减小调节时间,增大R值可以减小控制变量的作用力,由于两者存在比例关系,在确定R和Q时,可以令R=I,再通过比较分析来确定Q,然后借助Matlab中的lqr(A,B,Q,R)线性二次型最优工具箱求解出最优控制增益矩阵K.
对RSLQR控制器稳定性分析如下,定义RSLQR系统的开环传递函数:
(10) |
其共轭转置为
(11) |
由黎卡提方程可得:
(12) |
把式(10)带入式(12),式两边同时左乘
(13) |
RSLQR状态反馈的开环传递函数为
(14) |
把式(14)带入式(13)化简得:
(15) |
进一步可得:
(16) |
其中,(I+L(s))为输入断点处计算得到的回差矩阵,由于
(17) |
上式反映了回差矩阵的幅值信息,对于单输入系统来说,其奈奎斯特曲线不会进入复平面上以(-1,j0)为圆心的单位圆内.可知上文设计的姿态内回路RSLQR控制是稳定收敛的.
姿态滚转角整体控制原理图如下图所示.
3 基于LESO的姿态控制器设计由于RSLQR算法是基于系统模型的一类控制算法,比较依赖于对象建模的精度而且对于非线性时变系统往往达不到理想的控制效果,在姿态控制内环引入LESO(线性扩张状态观测器)来估计系统的总和扰动并对RSLQR控制量进行补偿,可以有效增强抗外部干扰能力和对自身参数不确定等因素的适应能力.
3.1 扩张状态观测器描述扩张状态观测器(ESO)是自抗扰控制技术中最核心的技术,它不仅能估计系统状态量,还能估计出由于模型不确定性引起的内部扰动和实时环境引起的外部扰动,并对其进行补偿[19].
有一类非线性时变二阶系统如下:
(18) |
式中:f(·)表示被控对象的动态,它可以时变、非线性的.取对象中未知部分为
(19) |
可以构造出扩张状态观测器,得到扩张状态X=[x1,x2,x3]T的估计值Z=[z1,z2,z3]T. z3是总和扰动f(·)的估计值.
现将扩张状态观测器用于姿态内回路角速率的控制,设计一阶线性扩张状态观测器(LESO)如下:
(20) |
式中,z1是对系统角速率的观测,z2是对系统总和扰动的估计,β1,β2为线性观测器参数,β1,β2影响对系统状态x1,x2的跟踪速度,选取合适的β2可以较好地估计干扰项z2并能动态补偿姿态内回路RSLQR控制器输出量.
3.2 新型姿态内回路控制设计以滚转通道为例,基于扩张状态观测器的新型姿态控制器设计原理图如3所示.
式(5)可写成
(21) |
式中:
根据式(5)、式(21),可获取加入LESO后新的姿态内回路状态方程:
(22) |
式中,
姿态内环RSLQR控制器输出量为
经过扩张状态观测器扰动补偿得最终的控制量为
加入观测器后系统收敛性分析:设
(23) |
当系统进入稳态时,方程右端全收敛于零,则误差系统稳态误差为
(24) |
只要β2足够大于w0,这些估计误差都会足够小,观测器估计的角速率和总扰动也就越准确.
4 仿真与实验分析 4.1 仿真结果分析为验证所提出的基于LESO的P-RSLQR控制算法的正确性和有效性,在Matlab/Simulink中搭建四旋翼无人机模型进行数值仿真实验,与P-RSLQR控制算法进行对比.仿真采用无人机模型的物理参数如下表 1所示.
无人机在飞行过程中受到的扰动主要来自于风扰动,当机体配重不平衡时,风力干扰产生的诱导气流会对对机体产生额外的倾转力矩[20];阵风干扰会对飞机产生随机干扰力矩;突变气流干扰会对飞机产生突变干扰力矩.
分别对横滚、俯仰、偏航通道设计不同的仿真跟踪信号,横滚方向跟踪方波信号,俯仰方向跟随正弦信号,偏航方向跟随斜坡信号,各方向姿态指令为
在姿态仿真中加入幅值为0.2 N·m,频率为10 rad/s的正弦扰动作为无人机飞行慢时变干扰力矩,在2 s处加入幅值为0.5 N·m,持续时间为0.5 s的突变干扰力矩,定义跟随误差:
从图 4~图 6可以看出,在添加给定慢时变干扰情况下,P-RSLQR姿态控制器的误差在4°左右(不考虑指令阶跃处的瞬时误差),P-RSLQR-LESO姿态控制器误差在2°左右(不考虑指令阶跃处的瞬时误差),在2.5 s出现突变干扰处,P-RSLQR-LESO控制的姿态角跳变很小,而P-RSLQR控制的姿态角出现较大的跳变,因此加入LESO后的P-RSLQR控制器相比于P-RSLQR控制器无论对慢时变干扰还是突变干扰都有着良好的抗扰动性能.
4.2 实验结果分析实验机机架选用DJIF450,该机架是X型布局,机臂长度为450 mm,选用的电机的型号为2212,KV值为920,旋翼型号为1045,供电采用3 s LiPo电池,额定电压为12 V.
飞行控制器采用Pixhawk,Pixhawk是一款基于ARM芯片的32位开源飞控,Pixhawk采用带有FPU的32位STM32F427芯片,它的采用Cortex M4内核,主频为168 M,具有252 MIPS的运算能力、256 kB的RAM以及2 MB的闪存,传感器选用ST公司的MicroL3GD20H 16位陀螺仪和MicroLSM303D 14位加速度计/磁力计,选用Invensense公司MPU 6000 3轴加速度计/陀螺仪,采用MEAS公司的MS5611气压计.
基于开源代码进行二次开发,将本文所用控制算法转换成嵌入式代码写入到飞控软件中.两种不同姿态控制器分别进行两次实验,实验在定高模式下飞行,滚转和俯仰均设置最大限制角度为15°,角度给定方法为遥控器手动打杆,实验环境为室外微风(风速 < 3 m/s),图 7~9为P-RSLQR姿态控制器和P-RSLQR-LESO姿态控制器的姿态跟踪曲线.实验采用的控制器参数如表 2(下标r,p,y分别表示滚转,俯仰,偏航通道).
变量 | 数值 | 变量 | 数值 |
kP(r,p) | 6.5 | b(r,p) | 9.6 |
kP(y) | 3.5 | b(y) | 4.8 |
Q(r,p) | diag(50,0.23) | β1(r,p,y) | 100 |
Q(y) | diag(70,0.47) | β2(r,p,y) | 1 500 |
R(r,p,y) | diag(1,1) |
本次实验中,四旋翼在室外环境下受到了随机的风扰动,从图 7~图 9可以看出加入LESO后的P-RSLQR控制器控制效果明显优于P-RSLQR控制器,在P-RSLQR控制下,滚转和俯仰的姿态角阶跃响应误差在5°左右,稳态误差在3°左右;在基于LESO的P-RSLQR控制下,滚转和俯仰的姿态角阶跃响应误差在2°左右,稳态误差在1°左右.四旋翼在飞行过程中受到的风力扰动主要作用于水平方向,在偏航方向受到的扰动较小,所以两个控制器对于偏航的控制效果相差较小.对比分析表明加入LESO后的P-RSLQR姿态控制器能够较好的估计并补偿外部扰动和内部扰动.
5 结论针对四旋翼无人机姿态控制问题,本文提出了一种基于线性扩张状态观测器(LESO)的PID与RSLQR控制器组成的双闭环回路姿态控制器,在使用基于模型的一类控制器时,建模误差会导致系统出现内部模型不确定扰动,加入LESO后该控制器可以实时估计四旋翼控制系统中的外部环境扰动和内部模型不确定性内部扰动并进行动态补偿.仿真和实验结果均可验证本文所提出的四旋翼无人机姿态控制器的有效性和可行性.
[1] | Astrom K J, Hdgglun T. Advanced PID control[M]. USA: Instrumentation, Systems, and Automation Society, 2006. |
[2] | Li J, Li Y T. Dynamic analysis and PID control for a quadrotor[C]//2011 IEEE International Conference on Mechatronics and Automation. Piscataway, NJ, USA: IEEE, 2011: 573-578. |
[3] | Åström K J, Murray R M. Feedback systems:An introduction for scientists and engineers[M]. Princeton, USA: Princeton University Press, 2008. |
[4] | Khatoon S, Gupta D, Das L K. PID and LQR control for a quadrotor: Modeling and simulation[C]//International Conference on Advances in Computing, Communications and Informatics. New Delhi, India: ICACCI, 2014: 85-102. |
[5] | Liu C L, Pan J. PID and LQR trajectory tracking control for an unmanned quadrotor helicopter: Experimental studies[C]//2016 35th Chinese Control Conference (CCC). Piscataway, NJ, USA: IEEE, 2016: 10845-10850. |
[6] | Zhao S L, An H L, Zhang D B, et al. A new feedback linearization LQR control for attitude of quadrotor[C]//International Conference on Control Automation Robotics and Vision. Piscataway, NJ, USA: IEEE, 2014: 1593-1597. |
[7] | Wang H, Zhang Y, Yi Y, et al. Nonlinear tracking control methods applied to Qball-X4 quadrotor UAV against actuator faults[C]//2016 Control and Decision Conference (CCDC). Piscataway, NJ, USA: IEEE, 2016: 3478-3483. |
[8] |
周杰, 王彪.
基于状态空间模型预测算法的四旋翼无人机飞行控制[J]. 信息与控制, 2018, 47(2): 149–155.
Zhou J, Wang B. Flight control of quadrotor uavs based on state space model predictive control[J]. Information and Control, 2018, 47(2): 149–155. |
[9] | Pollini L, Metrangolo A. Simulation and robust backstepping control of a quadrotor aircraft[C]//AIAA Modeling and Simulation Technologies Conference and Exhibit. Keystone, USA: AIAA, 2008. |
[10] |
王媛媛.基于反步法的四旋翼无人机鲁棒飞行控制设计[D].长沙: 湖南大学, 2017. Wang Y Y. Design of four-rotor UAV's robust system control based on backstepping[D]. Changsha: Hunan University, 2017. |
[11] | Miaolei H, Jilin H. Extended state observer-based robust backstepping sliding mode control for a small-size helicopter[J]. IEEE Access, 2018, 20(7): 33480–33448. |
[12] | González I, Salazar S, Lozano R. Chattering-free sliding mode altitude control for a quad-rotor aircraft:real-time application[J]. Journal of Intelligent and Robotic Systems, 2014, 73(1/2/3/4): 137–155. |
[13] |
段镇.
无人机鲁棒伺服LQR飞行控制律设计[J]. 计算机测量与控制, 2015, 23(8): 2713–2715.
Duan Z. Robust servo LQR flight control law design of UAV[J]. Computer Measurement and Control, 2015, 23(8): 2713–2715. |
[14] | Zhou Z, Yu J, Dong X, et al. Robust adaptive attitude control of the quad-rotor UAV based on the LQR and NESO technique[C]//2018 IEEE 14th International Conference on Control and Automation (ICCA). Piscataway, NJ, USA: IEEE, 2018: 745-750. |
[15] | Reyes-Valeria E, Enriquez-Caldera R, Camacho-Lara S, et al. LQR control for a quadrotor using unit quaternions: Modeling and simulation[C]//23rd International Conference on Electronics, Communications and Computing. Piscataway, NJ, USA: IEEE, 2013: 172-178. |
[16] | Zhou Z, Yu J, Dong X, et al. Robust adaptive attitude control of the quad-rotor UAV based on the LQR and NESO technique[C]//2018 IEEE 14th International Conference on Control and Automation (ICCA). Piscataway, NJ, USA: IEEE, 2018: 745-750. |
[17] |
刘玮, 牛尔卓, 胡龙珍.
一种无人机鲁棒自适应控制律设计[J]. 南京航空航天大学学报, 2017(S1): 109–114.
Liu W, Niu E Z, Hu L Z. A robust adaptive control law design for UAVS[J]. Journal of Nanjing University of Aeronautics and Astronautics, 2017(S1): 109–114. |
[18] |
马敏, 许中冲.
基于PID和LQR的四旋翼无人机控制系统研究[J]. 测控技术, 2016, 35(10): 42–45.
Ma M, Xu Z C. Research on four-rotor UAV control system based on PID and LQR[J]. Measurement and Control Technology, 2016, 35(10): 42–45. DOI:10.3969/j.issn.1000-8829.2016.10.011 |
[19] | Han J Q. Active disturbance rejection control technique[M]. Beijing: National Defense Industry Press, 2008: 183-239. |
[20] | Jean B P, Philippe M. The role of propeller aerodynamics in the model of a quadrotor UAV[C]//2009 European Control Conference (ECC). Piscataway, NJ, USA: IEEE, 2009: 683-688. |