集总干扰下六旋翼飞行器的轨迹跟踪控制

工程科学学报，第40卷，第5期：622-628,2018年5月 Chinese Joural of Engineering,Vol.40,No.5:622-628,May 2018 DOI:10.13374/j.issn2095-9389.2018.05.013;http://journals.ustb.edu.cn 集总干扰下六旋翼飞行器的轨迹跟踪控制 丁力12)，吴洪涛3)，李兴成) 1)江苏理工学院机械工程学院，常州2130012)香港理工大学工业及系统工程学系，香港999077 3)南京航空航天大学机电学院，南京210016 ☒通信作者，E-mail:nuaadli(@163.com 摘要针对复杂集总干扰下六旋翼飞行器轨迹跟踪控制问题，给出了混合积分反步法控制与线性自抗扰控制的控制算法 首先，通过牛顿-欧拉方程建立六旋翼飞行器的非线性动力学模型，并剖析系统输入输出的数学关系。其次，根据六旋翼飞行 器动力学模型的特点，将其分为位置与姿态两个控制环.位置环采用积分反步法控制理论设计控制器，通过引入积分项来提 高系统的抗干扰能力，消除轨迹跟踪的静态误差；姿态环采用线性自抗扰控制技术设计控制器，通过线性扩张观测器估计和 补偿集总干扰影响，提高系统的鲁棒性.最后，通过2组仿真算例和1组飞行试验验证了本文所提飞行控制算法的有效性.研 究结果表明：该控制算法对集总干扰有较好的抑制作用，能够使六旋翼飞行器既快又稳地跟踪上参考轨迹，具有一定的工程 应用价值 关键词六旋翼飞行器：轨迹跟踪控制：积分反步法控制：线性自抗扰控制：集总干扰 分类号TP242 Trajectory tracking control for an unmanned hexrotor with lumped disturbance DING Li),WU Hong-tao,LI Xing-cheng) 1)School of Mechanical Engineering,Jiangsu University of Technology,Changzhou 213001,China 2)Department of Industrial and Systems Engineering,The Hong Kong Polytechnic University,Hong Kong 999077,China 3)College of Mechanical and Electrical Engineering,Nanjing University of Aeronautics and Astronautics,Nanjing 210016,China Corresponding author,E-mail:nuaadli@163.com ABSTRACT In recent years,unmanned aerial vehicles (UAVs)have experienced an important growth both in research activities and industrial field.With the abilities to take off,land vertically,and hover along with natural agility and controllability,a hexrotor can extend the potential roles of UAVs.From the view of mechanical structure,hexrotors can be considered simpler than the helicopters because they do not have the swash-plate and do not need to eliminate the gyroscopic torques created by the spinning motors.However, hexrotors are not only extremely sensitive to control inputs and disturbances,they are also complex systems that are nonlinear,highly unstable and with multiple input-multiple output(MIMO)and a high degree of coupling characteristics.This study proposes a hybrid control algorithm combined integral backstepping control with linear active disturbance rejection control to solve the problem of trajectory tracking control for an unmanned hexrotor with lumped disturbance.First,the nonlinear dynamical model of the hexrotor was deduced with the Newton-Euler equation,and the mathematic relation of the input and the output was analyzed.Second,the hexrotor system was divided into the position loop and the attitude loop according to the characteristic of the dynamical model.In the position loop,an integral backstepping control algorithm was applied to design the controller by introducing an integral term to improve the disturbance resistance and eliminate the static error of the trajectory tracking.In the attitude loop,a linear active disturbance rejection control algo- rithm was used to design the controller by introducing a linear extended state observer to estimate and compensate for the lumped dis- turbance.Lastly,the effectiveness of the proposed control algorithm was verified through two simulation cases and a flight experiment. 收稿日期：2017-08-18 基金项目：江苏省基础研究计划（自然科学基金）资助项目(BK20170315):常州市应用基础研究计划资助项目(C20179017)

工程科学学报,第 40 卷,第 5 期:622鄄鄄628,2018 年 5 月 Chinese Journal of Engineering, Vol. 40, No. 5: 622鄄鄄628, May 2018 DOI: 10. 13374 / j. issn2095鄄鄄9389. 2018. 05. 013; http: / / journals. ustb. edu. cn 集总干扰下六旋翼飞行器的轨迹跟踪控制 丁 力1,2) 苣 , 吴洪涛3) , 李兴成1) 1) 江苏理工学院机械工程学院, 常州 213001 2) 香港理工大学工业及系统工程学系, 香港 999077 3) 南京航空航天大学机电学院, 南京 210016 苣 通信作者, E鄄mail: nuaadli@ 163. com 摘 要 针对复杂集总干扰下六旋翼飞行器轨迹跟踪控制问题,给出了混合积分反步法控制与线性自抗扰控制的控制算法. 首先,通过牛顿鄄鄄欧拉方程建立六旋翼飞行器的非线性动力学模型,并剖析系统输入输出的数学关系. 其次,根据六旋翼飞行 器动力学模型的特点,将其分为位置与姿态两个控制环. 位置环采用积分反步法控制理论设计控制器,通过引入积分项来提 高系统的抗干扰能力,消除轨迹跟踪的静态误差;姿态环采用线性自抗扰控制技术设计控制器,通过线性扩张观测器估计和 补偿集总干扰影响,提高系统的鲁棒性. 最后,通过2 组仿真算例和1 组飞行试验验证了本文所提飞行控制算法的有效性. 研 究结果表明:该控制算法对集总干扰有较好的抑制作用,能够使六旋翼飞行器既快又稳地跟踪上参考轨迹,具有一定的工程 应用价值. 关键词 六旋翼飞行器; 轨迹跟踪控制; 积分反步法控制; 线性自抗扰控制; 集总干扰 分类号 TP242 收稿日期: 2017鄄鄄08鄄鄄18 基金项目: 江苏省基础研究计划(自然科学基金)资助项目(BK20170315); 常州市应用基础研究计划资助项目(CJ20179017) Trajectory tracking control for an unmanned hexrotor with lumped disturbance DING Li 1,2) 苣 , WU Hong鄄tao 3) , LI Xing鄄cheng 1) 1) School of Mechanical Engineering, Jiangsu University of Technology, Changzhou 213001, China 2) Department of Industrial and Systems Engineering, The Hong Kong Polytechnic University, Hong Kong 999077, China 3) College of Mechanical and Electrical Engineering, Nanjing University of Aeronautics and Astronautics, Nanjing 210016, China 苣 Corresponding author, E鄄mail: nuaadli@ 163. com ABSTRACT In recent years, unmanned aerial vehicles (UAVs) have experienced an important growth both in research activities and industrial field. With the abilities to take off, land vertically, and hover along with natural agility and controllability, a hexrotor can extend the potential roles of UAVs. From the view of mechanical structure, hexrotors can be considered simpler than the helicopters because they do not have the swash鄄plate and do not need to eliminate the gyroscopic torques created by the spinning motors. However, hexrotors are not only extremely sensitive to control inputs and disturbances, they are also complex systems that are nonlinear, highly unstable and with multiple input鄄multiple output (MIMO) and a high degree of coupling characteristics. This study proposes a hybrid control algorithm combined integral backstepping control with linear active disturbance rejection control to solve the problem of trajectory tracking control for an unmanned hexrotor with lumped disturbance. First, the nonlinear dynamical model of the hexrotor was deduced with the Newton鄄鄄Euler equation, and the mathematic relation of the input and the output was analyzed. Second, the hexrotor system was divided into the position loop and the attitude loop according to the characteristic of the dynamical model. In the position loop, an integral backstepping control algorithm was applied to design the controller by introducing an integral term to improve the disturbance resistance and eliminate the static error of the trajectory tracking. In the attitude loop, a linear active disturbance rejection control algo鄄 rithm was used to design the controller by introducing a linear extended state observer to estimate and compensate for the lumped dis鄄 turbance. Lastly, the effectiveness of the proposed control algorithm was verified through two simulation cases and a flight experiment

丁力等：集总干扰下六旋翼飞行器的轨迹跟踪控制 ·623· The research results show that the proposed algorithm has a strong ability to resist the lumped disturbance and make the hexrotor quickly and steadily track the referenced trajectory.Hence,the algorithm has an important engineering application value. KEY WORDS unmanned hexrotor;trajectory tracking control;integral backstepping control;linear active disturbance rejection con- trol:lumped disturbance 多旋翼飞行器是一种集控制、机械、传感器、人 算法的理论设计效果进行了验证. 工智能等跨学科先进技术为一体的空中飞行机器 1动力学建模 人，近年来一直是国内外学者们研究的热点[).作 为多旋翼飞行器的一种，六旋翼飞行器的动力学性 六旋翼飞行器的模型如图1所示.模型在两套 能与四旋翼飞行器很类似，仅在机械机构上比后者 坐标系下描述，其中：Fe={OEXE YEZE}为大地坐标 多了一对冗余桨叶，故抗风性能与稳定性被大大提 系，FB={OBXEYEZE}为机身坐标系.在Fg中，L:(i= 高了.然而，六旋翼飞行器是一个高非线性、强耦 1,…,6)为旋翼的拉力，P=[x,y,]T为三轴位置， 合、欠驱动的多输入多输出系统，要设计高精度的轨 V=-[u,",o]r为三轴线速度，卫=[p,9,r]'为三轴 迹跟踪控制器难度颇大.同时，由系统外部不确定 角速度，⊙=[中，0，山]为三轴姿态角，其中中为滚 性与内部动态变化组成的复杂集总干扰会进一步加 转角，0为俯仰角，山为偏航角. 剧飞行控制器设计的难度. 对于旋翼飞行器轨迹跟踪控制器的设计大致可 分为两类：线性控制器与非线性控制器.前者需 要被控对象的精确模型才能设计出控制律，诸如 LQRs]、模型预测控制6、H控制[]等.上述控制 策略很适合某些工作点或小范围飞行包线内的线性 模型，并不能涵盖大范围的飞行包线.与线性控制 器相比，非线性控制器能够实现大范围飞行包线控 制，具有半全局或全局稳定性，理论研究价值更高. 例如，Basi等s)利用反步法设计了四旋翼飞行器的 图1六旋翼飞行器系统示意图 轨迹跟踪控制器，并引入粒子群算法整定控制器参 Fig.1 Schematic illustration of a hexrotor 数，在仿真环境下验证了该控制策略的有效性.将 一般而言，六旋翼飞行器的机臂相对较短且刚 四旋翼动力学模型分成全驱动与欠驱动两个子系统 后，王辰璐等]采用基于反步法的滑模控制策略设 度大，因此可将整个机体当作一个刚体在不考虑 计了全驱动子系统的控制器，再利用传统的滑模控 空气阻力影响时，利用牛顿-欧拉方程描述六旋翼 飞行器的动力学方程为[]： 制策略设计了欠驱动子系统的控制器，仿真结果表 明该混合控制算法具有较好的鲁棒性与快速动态跟 [x=U1(cos usin Ocos中+sinsin中)/m 踪性能，并且能有效抑制抖振现象.然而，实际飞行 y=U1(sin usin0cos中-cos usin中)/m 中的旋翼飞行器总是存在着复杂集总干扰（像阵风 z=U1cos中cos0/m-g 扰动、驱动电机失效、测量误差等)的影响，这必然 (1) Φ=U2/1.+t(In-1)/八. 影响飞行控制器的设计，甚至会导致控制器失效 针对上述问题，本文提出了一种基于积分反步 6=U31n+地(Ia-Ia)/八w 法控制(integral backstepping control,.IBC)和线性自 =U4/1.+Φ0(1.-I)/I 抗扰控制(linear active disturbance rejection control-- 其中，U=[U1,U2,U3,U4]为控制输入(U1为总 ler,LADRC)结合的混合控制策略(IBC-LADRC)来 距，U2为俯仰输入，U3为滚转输入，U,为偏航输 设计六旋翼飞行器的轨迹跟踪控制器.具体来说， 入)，m为整机质量，g为重力加速度，I= 位置环采用BC设计控制律，可提高抗干扰能力与 [1x,1n,Ia]'为惯性矩. 鲁棒性，消除静态误差：姿态环采用LADRC来设计 在实际控制中，四个控制输入并不能直接生成， 控制律，可实现对集总干扰的估计与补偿，提高动态 而是靠改变旋翼转速产生的.根据文献[11]可知， 跟踪能力.最后，通过软件仿真与飞行试验对控制 通过改变六旋翼旋翼转速可控制其四个通道的输入

丁 力等: 集总干扰下六旋翼飞行器的轨迹跟踪控制 The research results show that the proposed algorithm has a strong ability to resist the lumped disturbance and make the hexrotor quickly and steadily track the referenced trajectory. Hence, the algorithm has an important engineering application value. KEY WORDS unmanned hexrotor; trajectory tracking control; integral backstepping control; linear active disturbance rejection con鄄 trol; lumped disturbance 多旋翼飞行器是一种集控制、机械、传感器、人 工智能等跨学科先进技术为一体的空中飞行机器 人,近年来一直是国内外学者们研究的热点[1鄄鄄3] . 作 为多旋翼飞行器的一种,六旋翼飞行器的动力学性 能与四旋翼飞行器很类似,仅在机械机构上比后者 多了一对冗余桨叶,故抗风性能与稳定性被大大提 高了. 然而,六旋翼飞行器是一个高非线性、强耦 合、欠驱动的多输入多输出系统,要设计高精度的轨 迹跟踪控制器难度颇大. 同时,由系统外部不确定 性与内部动态变化组成的复杂集总干扰会进一步加 剧飞行控制器设计的难度. 对于旋翼飞行器轨迹跟踪控制器的设计大致可 分为两类:线性控制器与非线性控制器[4] . 前者需 要被控对象的精确模型才能设计出控制律,诸如 LQR [5] 、模型预测控制[6] 、H肄 控制[7] 等. 上述控制 策略很适合某些工作点或小范围飞行包线内的线性 模型,并不能涵盖大范围的飞行包线. 与线性控制 器相比,非线性控制器能够实现大范围飞行包线控 制,具有半全局或全局稳定性,理论研究价值更高. 例如,Basri 等[8]利用反步法设计了四旋翼飞行器的 轨迹跟踪控制器,并引入粒子群算法整定控制器参 数,在仿真环境下验证了该控制策略的有效性. 将 四旋翼动力学模型分成全驱动与欠驱动两个子系统 后,王辰璐等[9]采用基于反步法的滑模控制策略设 计了全驱动子系统的控制器,再利用传统的滑模控 制策略设计了欠驱动子系统的控制器,仿真结果表 明该混合控制算法具有较好的鲁棒性与快速动态跟 踪性能,并且能有效抑制抖振现象. 然而,实际飞行 中的旋翼飞行器总是存在着复杂集总干扰(像阵风 扰动、驱动电机失效、测量误差等) 的影响,这必然 影响飞行控制器的设计,甚至会导致控制器失效. 针对上述问题,本文提出了一种基于积分反步 法控制(integral backstepping control, IBC)和线性自 抗扰控制( linear active disturbance rejection control鄄 ler,LADRC)结合的混合控制策略(IBC鄄鄄 LADRC)来 设计六旋翼飞行器的轨迹跟踪控制器. 具体来说, 位置环采用 IBC 设计控制律,可提高抗干扰能力与 鲁棒性,消除静态误差;姿态环采用 LADRC 来设计 控制律,可实现对集总干扰的估计与补偿,提高动态 跟踪能力. 最后,通过软件仿真与飞行试验对控制 算法的理论设计效果进行了验证. 1 动力学建模 六旋翼飞行器的模型如图 1 所示. 模型在两套 坐标系下描述,其中:FE = {OE XE YE ZE }为大地坐标 系,FB = {OBXBYBZB}为机身坐标系. 在 FB 中,Li(i = 1,…,6)为旋翼的拉力,P = [ x,y,z] T 为三轴位置, V = [u,v,w] T 为三轴线速度,赘 = [p,q,r] T 为三轴 角速度,专 = [准,兹,鬃] T 为三轴姿态角,其中 准 为滚 转角,兹 为俯仰角,鬃 为偏航角. 图 1 六旋翼飞行器系统示意图 Fig. 1 Schematic illustration of a hexrotor 一般而言,六旋翼飞行器的机臂相对较短且刚 度大,因此可将整个机体当作一个刚体. 在不考虑 空气阻力影响时,利用牛顿鄄鄄 欧拉方程描述六旋翼 飞行器的动力学方程为[10] : x ·· = U1 (cos 鬃sin 兹cos 准 + sin 鬃sin 准) / m y ·· = U1 (sin 鬃sin 兹cos 准 - cos 鬃sin 准) / m z ·· = U1 cos 准cos 兹 / m - g 准 ·· = U2 / Ixx + 兹 · 鬃 · (Iyy - Izz) / Ixx 兹 ·· = U3 / Iyy + 准 · 鬃 · (Izz - Ixx) / Iyy 鬃 ·· = U4 / Izz + 准 · 兹 · (Ixx - Iyy) / I ì î í ï ï ï ï ï ï ï ï ï ï zz (1) 其中,U = [U1 ,U2 ,U3 ,U4 ] T 为控制输入( U1 为总 距,U2 为俯仰输入,U3 为滚转输入,U4 为偏航输 入), m 为 整 机 质 量, g 为 重 力 加 速 度, I = [Ixx,Iyy,Izz] T 为惯性矩. 在实际控制中,四个控制输入并不能直接生成, 而是靠改变旋翼转速产生的. 根据文献[11]可知, 通过改变六旋翼旋翼转速可控制其四个通道的输入 ·623·

·624· 工程科学学报，第40卷，第5期 量，而U与六个旋翼转速w(i=1,…,6)的数学关 为了达到上述要求，本节根据六旋翼飞行器动力学 系可表示为： 模型的特点将其解耦成位置与姿态两个控制环，分 k k k 别设计IBC和LADRC控制算法来实现系统状态的 02 镇定 U2 -k 3 2.1位置控制器设计 5 BC的本质是基于带有积分项的非线性反步法 21 0 0 21 2 2 控制器[].它不仅适合用来控制级联式的六旋翼 -kk -kz k -k2 飞行器动力学结构，而且能保证系统渐进稳定和对 6 集总干扰的鲁棒性，同时积分项还能抑制系统稳态 (2) 误差.本节采用BC来设计六旋翼飞行器的位置控 其中，k1=PCTAR2,k2=CoPAR/k·CT为升力系数， 制器，详细的设计步骤如下. C。为反扭矩系数，P为空气密度，A为旋翼桨盘面 在参考位置P,的误差中加入位置误差的积分 积，R为旋翼半径.在本文中，控制输入U被归一化 项可得： 至[-1,1]，k1=3.8×10-7,k2=1.5×10-3 另外，动力学模型中的惯性矩可以通过悬挂 61=P.-P+n16,(e)de (3) 法[2]获得，如图2所示.实验获得的结果为：1= 其中，n,∈R×3为积分项参数矩阵，P为实际位置，t 8.748×10-3kg°m2,1n=8.748×10-3kg°m2,12= 为时间阈值，6，为位置误差函数，e为数据序列. 1.425×10-2kgm2. 引入Lyapunov方程： V1)=266 (4) 对上式求导可得： V,(t)=661=6(P.-P+n,61)= 6(P.-a1+n161) (5) 为了镇定上述系统，选择虚拟控制量α，为： a1=P.+n161+A161 (6) 其中，A1∈R3x3为预先设定的正定对角矩阵 图2惯性矩测量实验 将式(6)代入到式(5)中，可得： Fig.2 Measurement method of the rotational inertia V1(t)=-8A61<0,H61≠0 (7) 六旋翼飞行器的其他参数如表1所示. 这样便可以保证上述系统是渐进稳定的4)， 设置线速度误差δ，为： 表1六旋翼飞行器的结构参数 Table 1 Structure parameters of the hexrotor 62=a1-P=P.+n161+A161-P=i1+A161 参数 数值 (8) 总质量，m/kg 2.812 引入Lyapunov方程： 重力加速度，g/(m·s2) 9.8 4(0=2(66,+6a,) (9) 旋翼半径，/m 0.0335 对上式求导可得： 空气密度，p/(kgm3) 11.69 桨盘面积A/m2 0.0035 1(t)=66,+662=6(62-A,6,)+ 6[P.+n,6,+A(62-A6,)-P]= 2 飞行控制器的设计 -6A6,+6[6,+P.+n,6,+A1(62-A,61)-P] 六旋翼飞行器轨迹跟踪控制时，主要应满足以 (10) 下两点要求：(1)应尽可能快地跟踪上参考轨迹，并 选择位置环的控制量为： 且跟踪误差不能超过GPS测量精度的5%：(2)姿 P=P.+ni1+A1(62-A61)+A,62 (11) 态控制过程应满足小角度飞行假设，即sinO≈⑨. 其中，A2∈R3×3为预先设定的正定对角矩阵

工程科学学报,第 40 卷,第 5 期 量,而 U 与六个旋翼转速 棕i (i = 1,…,6)的数学关 系可表示为: U1 U2 U3 U é ë ê ê ê ê ê ù û ú ú ú ú ú 4 = k1 k1 k1 k1 k1 k1 1 2 k1 k1 1 2 k1 - 1 2 k1 - k1 - 1 2 k1 - 3 2 k1 0 3 2 k1 3 2 k1 0 - 3 2 k1 k2 - k2 k2 - k2 k2 - k é ë ê ê ê ê ê ê ê ê ù û ú ú ú ú ú ú ú ú 2 棕1 棕2 棕3 棕4 棕5 棕 é ë ê ê ê ê ê ê ê ê ê ù û ú ú ú ú ú ú ú ú ú 6 (2) 其中,k1 = 籽CT AR 2 ,k2 = CQ 籽AR 2 / k1 . CT 为升力系数, CQ 为反扭矩系数,籽 为空气密度,A 为旋翼桨盘面 积,R 为旋翼半径. 在本文中,控制输入 U 被归一化 至[ - 1,1],k1 = 3郾 8 伊 10 - 7 ,k2 = 1郾 5 伊 10 - 3 . 另外,动力学模型中的惯性矩可以通过悬挂 法[12]获得,如图 2 所示. 实验获得的结果为:Ixx = 8郾 748 伊 10 - 3 kg·m 2 ,Iyy = 8郾 748 伊 10 - 3 kg·m 2 ,Izz = 1郾 425 伊 10 - 2 kg·m 2 . 图 2 惯性矩测量实验 Fig. 2 Measurement method of the rotational inertia 六旋翼飞行器的其他参数如表 1 所示. 表 1 六旋翼飞行器的结构参数 Table 1 Structure parameters of the hexrotor 参数 数值 总质量,m/ kg 2郾 812 重力加速度,g / (m·s - 2 ) 9郾 8 旋翼半径,R/ m 0郾 0335 空气密度,籽 / (kg·m - 3 ) 11郾 69 桨盘面积 A / m 2 0郾 0035 2 飞行控制器的设计 六旋翼飞行器轨迹跟踪控制时,主要应满足以 下两点要求:(1)应尽可能快地跟踪上参考轨迹,并 且跟踪误差不能超过 GPS 测量精度的 5% ;(2) 姿 态控制过程应满足小角度飞行假设,即 sin 专抑专. 为了达到上述要求,本节根据六旋翼飞行器动力学 模型的特点将其解耦成位置与姿态两个控制环,分 别设计 IBC 和 LADRC 控制算法来实现系统状态的 镇定. 2郾 1 位置控制器设计 IBC 的本质是基于带有积分项的非线性反步法 控制器[13] . 它不仅适合用来控制级联式的六旋翼 飞行器动力学结构,而且能保证系统渐进稳定和对 集总干扰的鲁棒性,同时积分项还能抑制系统稳态 误差. 本节采用 IBC 来设计六旋翼飞行器的位置控 制器,详细的设计步骤如下. 在参考位置 Pr 的误差中加入位置误差的积分 项可得: 啄1 = Pr - P + 浊1 乙 t 0 啄1 (着)d着 (3) 其中,浊1沂R 3 伊 3为积分项参数矩阵,P 为实际位置,t 为时间阈值,啄1 为位置误差函数,着 为数据序列. 引入 Lyapunov 方程: V1 (t) = 1 2 啄 T 1 啄1 (4) 对上式求导可得: V · 1 (t) = 啄 T 1 啄 · 1 = 啄 T 1 (P · r - P · + 浊1 啄1 ) = 啄 T 1 (P · r - a1 + 浊1 啄1 ) (5) 为了镇定上述系统,选择虚拟控制量 a1 为: a1 = P · r + 浊1 啄1 + A1 啄1 (6) 其中,A1沂R 3 伊 3为预先设定的正定对角矩阵. 将式(6)代入到式(5)中,可得: V · 1 (t) = - 啄 T 1A1 啄1 < 0,坌啄1屹0 (7) 这样便可以保证上述系统是渐进稳定的[14] . 设置线速度误差 啄2 为: 啄2 = a1 - P · = P · r + 浊1 啄1 + A1 啄1 - P · = 啄 · 1 + A1 啄1 (8) 引入 Lyapunov 方程: V2 (t) = 1 2 (啄 T 1 啄1 + 啄 T 2 啄2 ) (9) 对上式求导可得: V · 1 (t) = 啄 T 1 啄 · 1 + 啄 T 2 啄 · 2 = 啄 T 1 (啄2 - A1 啄1 ) + 啄 T 2 [P ·· r + 浊1 啄 · 1 + A1 (啄2 - A1 啄1 ) - P ·· ] = - 啄 T 1A1 啄1 + 啄 T 2 [啄1 + P ·· r + 浊1 啄 · 1 + A1 (啄2 - A1 啄1 ) - P ·· ] (10) 选择位置环的控制量为: P ·· = P ·· r + 浊1 啄 · 1 + A1 (啄2 - A1 啄1 ) + A2 啄2 (11) 其中,A2沂R 3 伊 3为预先设定的正定对角矩阵. ·624·

丁力等：集总干扰下六旋翼飞行器的轨迹跟踪控制 625· 同样地，将式(11)代入到式(10)中，可得： P.4 V,(t)=-8A,61-6A,62<0,H61,62≠0 二阶 U LADRC 姿 (12) 控制器 模型 p.0 这样便可以保证整个位置环是渐进稳定的. 通常在六旋翼飞行器飞行时，除了要镇定三轴 图3姿态环的控制结构 位置外，还需要保持姿态镇定.姿态控制中，由于偏 Fig.3 Control structure of the attitude loop 航角不需要频繁控制，故只需考虑滚转角中与俯仰 角0的镇定，而业≈0.若将式(11)代入到式(1)中， 中9个(n1A1及A2)和姿态环中的6个(w。,ω。及 可得： b。各2个)，作者引入时间乘以误差绝对值积分 (TAE)作为控制系统性能评价指标1)，采用人工 (13) 蜂群算法来寻找最优控制器参数，具体的过程可参 考文献[17] 其中，六旋翼的总距U1=√++U. 仿真中，在位置环输入端加入阵风模型，该模型 经过简单的矩阵变换，可计算出参考的俯仰角 可由Gauss--Markov方程生成[191： 与滚转角为： 「-1/T 0 0 .tan 0 +pB9 +g (14 0 中，=sin-1 -U, (U+U+(U.+g) (15) 2.2姿态控制器设计 其中，[d.,d.,d.]T为三轴方向的风扰速度向量， 在飞行过程中，六旋翼飞行器动力学模型的参 [9。,9,9]「为零均值噪声信号，B为干扰输入项， 数（像惯性矩）会因周围阵风干扰而发生变化，这会 p·=0.5为权重因子，r。为与风速相关时间常数，这 导致姿态环失稳.同时，测量噪声、机械振动等不确 里取三轴方向的风速为6ms,T。为3.2. 定因素也会进一步放大系统误差.为此，本节采用 为了更好地模拟六旋翼飞行器的实际飞行，在 LADRC对这些复杂集总干扰进行估计与补偿，实现 位置环测量端中添加幅值为0.04m的白噪声，在姿 姿态环的抗干扰控制. 态环测量端添加幅值为0.01°的白噪声.位置环的 LADRC是在韩京清教授提出的自抗扰控制的 阶跃响应如图4所示，三轴位置的上升时间约为 基础上改进而来的5-6).与自抗扰控制相比， 2.5s,但响应并不完全一致.以稳态误差为10%的 LADRC的扩张观测器与控制器都是线性的，并且不 稳态时间为例，x方向上的该时间约为2.4s,y方向 使用微分跟踪器，结构更为简单.另外，LADRC可 上的该时间约为2.3s,而z方向上的该时间约为 将控制器参数降到3个，即观测器带宽(observer 2.1s.这可能是由于六旋翼飞行器不完全对称的动 bandwidth)w。、控制器带宽(control bandwidth)w。及 力学性能引起的.但总体来说，位置环在BC的作 增益b。·作者曾在文献[17]中详细地给出了二阶 用下跟踪误差逐渐变小（如图5所示），能够满足飞 LADRC控制器的推导过程，这里就不再赘述.从六 行控制器设计要求. 旋翼的动力学模型可以看出，俯仰角与滚转角分别 如图6所示，在姿态环的响应过程中，姿态输出 与控制输入U2、U,呈二阶导数关系，故姿态环的控 量能够既快又好地跟踪上伪控制量（中，、0），时延约 制可采用两个二阶LADRC控制器来实现.具体的 为0.1s,这表明了LADRC响应的快速度性.中和0 控制结构框图如图3所示. 的最大值约为10°，这也说明姿态控制过程能满足 小角度飞行假设，即sin(10m/180)≈10π/180≈ 3仿真分析 0.17.另外，图7也给出了姿态环的跟踪误差 (1)轨迹跟踪仿真分析.定义参考航迹点P,= (2)LADRC性能分析.姿态控制器性能的好坏 [xy,]T=[1,1,1]'(单位为m)和参考偏航角 直接影响整个飞行控制器的性能，故得保证姿态控 中，=0°，其中x,y,,均为参数的位置.这里需指 制器具有较高的跟踪精度.本例中，以一阶跃信号 出，整个控制器中有15个参数需要整定，即位置环 为输入信号，并在姿态环的输入端与测量端分别引

丁 力等: 集总干扰下六旋翼飞行器的轨迹跟踪控制 同样地,将式(11)代入到式(10)中,可得: V · 2 (t) = - 啄 T 1A1 啄1 - 啄 T 2A2 啄2 < 0,坌啄1 ,啄2屹0 (12) 这样便可以保证整个位置环是渐进稳定的. 通常在六旋翼飞行器飞行时,除了要镇定三轴 位置外,还需要保持姿态镇定. 姿态控制中,由于偏 航角不需要频繁控制,故只需考虑滚转角 准 与俯仰 角 兹 的镇定,而 鬃抑0. 若将式(11)代入到式(1)中, 可得: P ·· = Ux Uy U é ë ê ê ê ê ù û ú ú ú ú z = x ·· y ·· z é ë ê ê ê ù û ú ú ··ú (13) 其中,六旋翼的总距 U1 = U 2 x + U 2 y + U 2 z . 经过简单的矩阵变换,可计算出参考的俯仰角 与滚转角为: 兹r = tan ( - 1 Ux Uz + ) g 准r = sin ( - 1 - Uy (U 2 x + U 2 y + (Uz + g) ) ì î í ï ï ï ï 2 (14) 2郾 2 姿态控制器设计 在飞行过程中,六旋翼飞行器动力学模型的参 数(像惯性矩)会因周围阵风干扰而发生变化,这会 导致姿态环失稳. 同时,测量噪声、机械振动等不确 定因素也会进一步放大系统误差. 为此,本节采用 LADRC 对这些复杂集总干扰进行估计与补偿,实现 姿态环的抗干扰控制. LADRC 是在韩京清教授提出的自抗扰控制的 基础上改进而来的[15鄄鄄16] . 与 自 抗 扰 控 制 相 比, LADRC 的扩张观测器与控制器都是线性的,并且不 使用微分跟踪器,结构更为简单. 另外,LADRC 可 将控制器参数降到 3 个,即观测器带宽( observer bandwidth)棕o、控制器带宽( control bandwidth)棕c 及 增益 b0 . 作者曾在文献[17] 中详细地给出了二阶 LADRC 控制器的推导过程,这里就不再赘述. 从六 旋翼的动力学模型可以看出,俯仰角与滚转角分别 与控制输入 U2 、U3 呈二阶导数关系,故姿态环的控 制可采用两个二阶 LADRC 控制器来实现. 具体的 控制结构框图如图 3 所示. 3 仿真分析 (1)轨迹跟踪仿真分析. 定义参考航迹点 Pr = [xr,yr,zr] T = [1,1,1] T (单位为 m) 和参考偏航角 鬃r = 0毅,其中 xr,yr,zr 均为参数的位置. 这里需指 出,整个控制器中有 15 个参数需要整定,即位置环 图 3 姿态环的控制结构 Fig. 3 Control structure of the attitude loop 中 9 个(浊1 、A1 及 A2 )和姿态环中的 6 个(棕o、棕c 及 b0 各 2 个),作者引入时间乘以误差绝对值积分 (ITAE)作为控制系统性能评价指标[18] ,采用人工 蜂群算法来寻找最优控制器参数,具体的过程可参 考文献[17]. 仿真中,在位置环输入端加入阵风模型,该模型 可由 Gauss鄄鄄Markov 方程生成[19] : d · u d · v d · é ë ê ê ê ê ê ù û ú ú ú ú ú w = - 1 / 子c 0 0 0 - 1 / 子c 0 0 0 - 1 / 子 é ë ê ê ê ê ù û ú ú ú ú c du dv d é ë ê ê ê ê ù û ú ú ú ú w + 籽 *B qu qv q é ë ê ê ê ê ù û ú ú ú ú w (15) 其中,[ du ,dv,dw ] T 为三轴方向的风扰速度向量, [qu ,qv,qw ] T 为零均值噪声信号,B 为干扰输入项, 籽 * = 0郾 5 为权重因子,子c 为与风速相关时间常数,这 里取三轴方向的风速为 6 m·s - 1 ,子c 为 3郾 2. 为了更好地模拟六旋翼飞行器的实际飞行,在 位置环测量端中添加幅值为 0郾 04 m 的白噪声,在姿 态环测量端添加幅值为 0郾 01毅的白噪声. 位置环的 阶跃响应如图 4 所示,三轴位置的上升时间约为 2郾 5 s,但响应并不完全一致. 以稳态误差为 10% 的 稳态时间为例,x 方向上的该时间约为 2郾 4 s,y 方向 上的该时间约为 2郾 3 s,而 z 方向上的该时间约为 2郾 1 s. 这可能是由于六旋翼飞行器不完全对称的动 力学性能引起的. 但总体来说,位置环在 IBC 的作 用下跟踪误差逐渐变小(如图 5 所示),能够满足飞 行控制器设计要求. 如图 6 所示,在姿态环的响应过程中,姿态输出 量能够既快又好地跟踪上伪控制量(准r、兹r),时延约 为 0郾 1 s,这表明了 LADRC 响应的快速度性. 准 和 兹 的最大值约为 10毅,这也说明姿态控制过程能满足 小角度飞行假设,即 sin (10仔/ 180) 抑10仔/ 180 抑 0郾 17. 另外,图 7 也给出了姿态环的跟踪误差. (2)LADRC 性能分析. 姿态控制器性能的好坏 直接影响整个飞行控制器的性能,故得保证姿态控 制器具有较高的跟踪精度. 本例中,以一阶跃信号 为输入信号,并在姿态环的输入端与测量端分别引 ·625·

.626. 工程科学学报，第40卷，第5期 1.0 (a 号0.5 时间 50.5 时间s 图7姿态环的跟踪误差 4 10 Fig.7 Tracking error of the attitude loop 时间/s 1.0 精度也比PID提高了32%. E0.5 16r 14 ---参考值 -LADRC 4 6 10 12 ---PD 时间s 10 15.4 图4位置环的响应.(a)x;(b)y:(c): 8 15,2 150 Fig.4 Response of the position loop:(a)x;(b)y;(c)z 148 4 14.6 8.68.89.0 15 2 时间的 1.0 0 0.5 4 6 10 张 时间/s 0 050 45678910 图8滚转角的响应 3 时间s Fig.8 Response of the roll angle 图5位置环的跟踪误差 Fig.5 Tracking error of the position loop 4试验验证 为了进一步验证所设计飞行控制器的有效性， 作者搭建了六旋翼飞行器样机.该样机装载了低成 本的GPS模块、数传、飞行控制系统、摄像机等航空 电子设备，如图9所示.另外，表2也给出了飞行控 制系统的性能指标，这些指标表明所选飞行控制系 时间s 统足以测量任意常规六旋翼飞行器的飞行数据.由 于试验条件有限，集总干扰（像阵风干扰、测量噪 声、机械振动等)无法通过传感器直接获得，因此本 文采用如下方法完成六旋翼飞行器的轨迹跟踪控制 --…0 试验，即：先用仿真获得无集总干扰下的BC- 10 4 10 LADRC控制器参数，再通过Matlab/Simulink的代码 时间s 转化功能[2O将IBC-LADRC控制算法转换成C代 图6姿态环的响应.(a)滚转角：(b)俯仰角 Fig.6 Response of the attitude loop:(a)roll angle;(b)pitch angle -GPS 数传 入幅值为0.02°的白噪声.同时，也采用比例-积分- 飞行控制 微分(proportion integration differentiation,PID)控制 系统 器进行比较分析.PID的参数整定也采用人工蜂群 摄像机 算法来实现，具体过程这里就不再赘述.以滚转角 中为例，两种控制器的控制效果如图8所示.从图 中可以看出，LADRC在线性扩张观测器(LESO)的 作用下能有效抑制外界扰动，跟踪效果大大优于传 图9六旋翼飞行器系统 统的PID控制器.另外，基于LADRC的滚转角跟踪 Fig.9 Hexrotor system

工程科学学报,第 40 卷,第 5 期 图 4 位置环的响应. (a) x; (b) y; (c) z Fig. 4 Response of the position loop: (a) x; (b) y; (c) z 图 5 位置环的跟踪误差 Fig. 5 Tracking error of the position loop 图 6 姿态环的响应 郾 (a) 滚转角; (b) 俯仰角 Fig. 6 Response of the attitude loop: (a) roll angle; (b) pitch angle 入幅值为0郾 02毅的白噪声. 同时,也采用比例鄄鄄积分鄄鄄 微分( proportion integration differentiation,PID) 控制 器进行比较分析. PID 的参数整定也采用人工蜂群 算法来实现,具体过程这里就不再赘述. 以滚转角 准 为例,两种控制器的控制效果如图 8 所示. 从图 中可以看出,LADRC 在线性扩张观测器(LESO) 的 作用下能有效抑制外界扰动,跟踪效果大大优于传 统的 PID 控制器. 另外,基于 LADRC 的滚转角跟踪 图 7 姿态环的跟踪误差 Fig. 7 Tracking error of the attitude loop 精度也比 PID 提高了 32% . 图 8 滚转角的响应 Fig. 8 Response of the roll angle 4 试验验证 为了进一步验证所设计飞行控制器的有效性, 作者搭建了六旋翼飞行器样机. 该样机装载了低成 图 9 六旋翼飞行器系统 Fig. 9 Hexrotor system 本的 GPS 模块、数传、飞行控制系统、摄像机等航空 电子设备,如图 9 所示. 另外,表 2 也给出了飞行控 制系统的性能指标,这些指标表明所选飞行控制系 统足以测量任意常规六旋翼飞行器的飞行数据. 由 于试验条件有限,集总干扰(像阵风干扰、测量噪 声、机械振动等)无法通过传感器直接获得,因此本 文采用如下方法完成六旋翼飞行器的轨迹跟踪控制 试验, 即: 先 用 仿 真 获 得 无 集 总 干 扰 下 的 IBC鄄鄄 LADRC 控制器参数,再通过 Matlab / Simulink 的代码 转化功能[20] 将 IBC鄄鄄 LADRC 控制算法转换成 C 代 ·626·