第5章 CHAPTER 5 四旋翼飞行机器人 视频讲解 四旋翼无人飞行器(国外又称之为Quadrotor等)是一种具有4个螺旋桨的飞行器，其4个螺旋桨呈十字形交叉结构，相对的四旋翼具有相同的旋转方向，分两组，两组的旋转方向不同。与传统的直升机不同，四旋翼飞行器只能通过改变螺旋桨的速度来实现各种动作。 本章主要讲述的内容包含四旋翼飞行器动力学建模以及飞行系统的抗干扰控制器设计两方面。首先，结合建立的四旋翼动力学模型的假设条件、目标设计参数进行选型计算。然后，设计四旋翼飞行器样机结构确定样机物理参数。针对确定界干扰问题，本章设计指数型时变增益趋近律反步滑模姿态控制，改善执行器的负荷同时保证姿态系统的强鲁棒性。由于实际应用的环境风场复杂多变导致的不确定干扰问题，在四旋翼飞行器姿态系统中需设计非线性干扰观测器。结合反步滑模姿态控制器设计方法，在控制量中进行前馈补偿。 视频讲解 5.1四旋翼飞行器动力学建模 5.1.1坐标系设定 四旋翼飞行器运动形式由4个分布均匀的螺旋桨的转速所决定。飞行器在空间的三维信息可以用3个位置坐标和姿态坐标来表示。四旋翼飞行器通过成对改变电机转速，以此使得四旋翼飞行器在空间上正常运动，如滚转运动、俯仰运动、偏航运动。位置、速度、姿态等参数的表示与所建立的坐标系有关。因此要明确地描述飞行器的运动状态，需要建立合适的坐标系。所建立的地面坐标系与机体坐标系，如图5.1所示，Fi为第i个电机的提升力(i=1,2,3,4)，其与旋翼的转速平方成正比。 图5.1四旋翼飞行器运动示意图 1. 地面坐标系{E} 图5.1为四旋翼飞行器运动示意图，图中{E}为地面坐标系，用于研究四旋翼飞行器相对于地面的运动，确定该飞行器相对于地面的位置和速度及航向。 2. 机体坐标系{B} {B}为机体坐标系，固定在机体上，原点设在四旋翼飞行器重心位置。Zb垂直于机体上平面，Xb指向机体正前方，由右手坐标系确定Yb方向。机体坐标系可描述机体的姿态角及姿态角速度信息。惯性测量单元(Inertial Measuring Unit,IMU)初始数据就是建立在机体坐标系下的。 3. 转换矩阵 在四旋翼飞行器飞行动力学中，通过3个欧拉角描述地面坐标系与机体坐标系之间的关系，即滚转角、俯仰角θ、偏航角ψ。其中，欧拉角满足∈(-π/2，π/2)，θ∈(-π/2，π/2)，ψ∈(-π，π)。令R为从机体坐标系到地面坐标系的旋转矩阵。 按照x-y-z的顺序进行旋转变化，对应的旋转矩阵分别是 R(x,)=100 0cos-sin 0sincos R(y,θ)=cosθ0sinθ 010 -sinθ0cosθ R(z,ψ)=cosψ-sinψ0 sinψcosψ0 001 则机体坐标系到地面坐标系的旋转矩阵为 REB=R(x,)·R(y,θ)·R(z,ψ) =cosψcosθcosψsinθsin-sinψcoscosψsinθcos+sinψsin sinψcosθsinψsinθsin+cosψcossinψsinθcos-cosψsin -sinθcosθsincosθcos(5.1) 四旋翼飞行器有四输入六输出，其中六输出包括3个位置信息和3个姿态信息，飞行模态参照四旋翼结构设计是一种欠驱动系统。分别以ξ=(xyz)T表示飞行器的重心在地面坐标系下的位置向量，V=(uvω)T表示飞行器在地面坐标系下的线速度向量。以η=(θψ)T表示地面坐标系下的飞行器姿态角度向量，Ω=(pqr)T表示机体坐标系下的角速度向量。电机1、电机3和电机2、4之间的转向不同，以此来保持平衡。通过成对地调节电机的转速来产生平行坐标轴的力矩，以此3个力矩产生的复合力矩来调整飞行器姿态，完成滚转、俯仰、偏航、悬停、起降等动作。 四旋翼飞行器在机体坐标系下的角速度Ω与地面坐标系下的角速度η·=(·θ·ψ·)存在映射关系，即 · θ· ψ·=1sintanθcostanθ 0cos-sin 0sin/cosθcos/cosθp q r(5.2) 5.1.2刚体动力学模型 由于四旋翼飞行器系统具有强耦合、非线性和欠驱动的特点，所以为获得较好的飞行控制效果，设计非线性鲁棒控制器至关重要。为了更好地建立四旋翼飞行器动力学模型，首先做出以下假设： (1) 四旋翼飞行器质量恒定且机身呈刚性且质心与形心保持在同一位置。 (2) 结构对称，电机的提升力与转速的平方成正比。 根据5.1.1节建立的坐标系，基于牛顿欧拉方法建立四旋翼飞行器动力学模型 ξ·=V mV·=REBTB+GE η·=WΩ JΩ·=-Ω×JΩ+MB+D(5.3) 其中， TB=00U1T GE=00-gT W=1sintanθcostanθ 0cos-sin 0sin/cosθcos/cosθ(5.4) J=diag(Jxx,Jyy,Jzz) D=(ddθdψ)T U1=∑4i=1Fi=CT∑4i=1ω2i 式中，m为四旋翼飞行器质量，TB为飞行器总体提升力，GE为重力加速度项。W为从机体坐标系转换到地面坐标系下的姿态转换矩阵。由于四旋翼飞行器主要被用于跨越移动底盘无法跨越的障碍，姿态角度变化范围小，故飞行器在小角度运动下满足W=diag(1,1,1)。J为四旋翼飞行器的转动惯量矩阵，MB为控制力矩。CT为正数，是已知的升力系数； U1为总提升力，ωi为第i个电机的转速； D为外部干扰力矩，Fi为第i个电机的升力。 旋翼控制转矩为 MB=MBx MBy MBz=U Uθ Uψ=22l(F4+F3-F2-F1) 22l(-F4+F3+F2-F1) kd(ω21+ω23-ω22-ω24)(5.5) 结合式(5.4)和式(5.5)，可以得出式(5.6)所示的旋翼控制输入与转矩的映射关系式，式中l为电机轴线到四旋翼质心的距离，Kd是阻尼力矩系数 U1 U Uθ Uψ=CTCTCTCT 22lCT22lCT-22lCT-22lCT 22lCT-22lCT-22lCT22lCT kd-kdkd-kdω21 ω22 ω23 ω24(5.6) 将式(5.2)、式(5.6)代入式(5.3)，可以得到展开的四旋翼飞行器位置和姿态系统两部分动力学方程 x¨=(cossinθcosψ+sinsinψ)U1m y¨=(cossinθsinψ-sincosψ)U1m z¨=(coscosθ)U1m-g(5.7a) ¨=θ·ψ·Jyy-JzzJxx+UJxx+dJxx θ¨=·ψ·Jzz-JxxJyy+UθJyy+dθJyy ψ¨=·θ·Jxx-JyyJzz+UψJzz+dψJzz(5.7b) 其中，式(5.7a)为位置系统动力学模型，式(5.7b)为干扰下姿态系统动力学模型。 从展开的动力学模型中可以看出，姿态系统相对于位置系统是独立的。位置系统位于姿态系统的外环，由位置系统动力学模型可见，位置系统受到姿态系统的影响。 通过牛顿欧拉方法所建立的四旋翼飞行器动力学模型搭建dynamic模型，如图5.2所示。 图5.2四旋翼飞行器动力学模型dynamic部分 程序详见配套资料中的程序代码chapter_5>>Strong_index_tightens>>quadrotors.lx>>dynamic。 5.2抗干扰反步滑模控制器设计 全面深入分析四旋翼飞行器确定界干扰下的动力学模型，重点分析外部确定界干扰下的位置保持以及姿态表现。以滑模控制理论为出发点，对恶劣环境运作的四旋翼飞行器突出的外部干扰以及系统不确定性问题进行分析，其中干扰的主要来源为环境风场。根据实际工程项目需求以及灾区泥石流山区环境的使用背景，提出更具有实际工程价值的抗干扰飞行控制方案。 与其余控制方法相比，滑模控制具有鲁棒性强的显著优势。通过设计滑模面的方式，将系统状态划分成多个不同的子空间。随后通过系统状态运动到不同的子空间时，切换其控制结构。滑模控制强鲁棒性的原因是其在所设计的滑模面两侧切换了控制结构。通过高频增益逼迫系统状态接近所设计的滑模面，并且沿着系统滑模面收敛到期望点。 5.2.1滑模变结构控制 1. 滑模控制的基本概念 滑模控制可以在系统状态处于不同阶段时改变原有的控制策略，采用新的控制策略，由此对应的控制输入随着系统控制结构的切换发生改变。通过设计滑模面，将完整的系统空间分解成多个无交集的状态子空间。系统的状态进入不同的状态子空间后，控制策略变化，发生切换控制。由此产生对应控制策略下的输出信号。例如下面的系统： x·=f(t,x,u) ui=u+i(x,t)，si(x)>0 u-i(x,t)，si(x)<0（i=1,2,…,m）(5.8) 其中，x=［x1,x2,…,xn］T∈Rn为系统状态向量，u=［u1,u2,…,um］T∈Rm为系统的控制输入，s=［s1,s2,…,sm］T∈Rm为滑模超平面。 选择合适的控制输入u，以保证系统状态能到达所设计的滑模面。对于滑模到达阶段的设计往往通过趋近律来实现，从而保证在ss·<0的前提下，确定到达滑模面的速度以及趋近的方式。滑模面有多种设计类型，包括线性滑模面、非线性滑模面或者动态滑模面。具体根据实际被控对象的需求进行设计， 图5.3滑模相平面图 以保证系统状态能沿着滑模面收敛到期望点或者期望点附近。从而获得较高的控制效果，同时保证系统的抗干扰特性。 由式(5.8)可知，当系统状态x从Π+i={x∈Rn： si(x)>0}区域进入Π-i={x∈Rn： si(x)<0}区域时，系统结构从x·=f(t,x,u+)变为x·=f(t,x,u-)，其中u±=［u1,u2,…,ui-1,u±i,ui+1,…,um］T。图5.3为滑模相平面图，可以看到，当系统初始状态不在滑模相平面Πi={x∈Rn： si(x)=0}上时，在控制量u±中设计的趋近律作用下，系统状态趋近并到达滑模面且维持在流形Πi上。之后系统状态沿着其收敛至期望值。但由于实际中执行器件存在空间或时间的滞后性，导致在流形Πi附近的系统运动状态的微分方程不连续。由于这种不连续的切换控制，使得滑模控制方法具有强鲁棒性。由于这种不连续的切换，导致系统的控制量输出项中含有不连续项，控制项中符号函数的不连续切换是导致抖振现象产生的根本原因。在滑模控制中，在采用单一的控制策略和不切换系统结构的情况下，系统都是不稳定的。当系统在各个流形区域切换时，闭环系统是稳定的，能收敛至期望位置。 2. 滑动模态的数学描述 考虑如下非线性系统： x·=f(t,x)+b(t,x)u(5.9) 式中，x∈Rn，u∈Rm，其中，f(t,x)和b(t,x)为连续函数。 由图5.3可见，系统的状态运动到滑模面上时，其状态轨迹会保持在滑模面上，并且系统状态沿着滑模面收敛到期望点，这便是理想的滑动模态。在实际使用中，由于执行器存在的迟滞以及建模不准确的原因，系统状态一般不会始终保持在滑模面上。由于开关的滞后特性，通常表现为在滑模面来回穿梭的形式，并且沿着滑模面向期望点收敛，这便是实际的滑动模态。在理想滑动模态下，系统状态处于滑模面上，满足s(x)=0，从而有s·(x)=0。系统满足以下等式： s·(x,t)=st+sx(f(t,x)+b(t,x)u)=0(5.10) 由式(5.10)推导控制输入量u，可求得将状态轨迹保持在滑模面上所需的系统控制量，称为等效控制量ueq。 3. 滑动模态的到达条件 综上所述，系统控制的目的是使得系统状态可以到达滑模面上。要保证滑模到达阶段满足ss·<0。部分研究者为了进一步提高系统性能，使系统状态在有限时间到达滑模面，在满足滑模存在的前提下，同时满足ss·<-δ，δ>0。考虑到系统中存在的不确定性，部分学者还对到达阶段的速度做了相应的研究。高为炳教授提出了趋近律的概念和几种常用的设计形式，如等速趋近率s·=-εsgn(s)、指数趋近律s·=-εsgn(s)-ks等。 5.2.2指数型时变趋近律设计 为了使得系统状态到达滑模面上，并且沿着滑模面收敛到平衡点。基于李雅普诺夫稳定性理论设计滑模控制律。设计方法主要有两种： 一是直接根据系统稳定性条件给出控制律； 二是在滑模到达阶段，设计系统状态到滑模面上的趋近律，保证到达阶段的趋近速度，反推出系统的控制律。两种方法推导在最后的控制量表达式中都会存在符号函数这个不连续项。该控制方式在理想情况下，能在切换面上生成滑动模态，而后系统状态沿着切换面收敛至平衡点。而这种滑动模态对系统建模不确定、参数摄动、外部确定界的匹配干扰具有很强的鲁棒性。但是在实际工程应用中，由于符号函数导致控制量中的符号函数项一直处在阶跃切换的状态，这种特点同样也被称为开关特性。这种开关特性导致了控制量输出不连续。这种不连续的控制量输出也同样给执行器带来了负荷。同样，由于执行器存在时间和空间上的滞后，无法满足这种开关特性，故在实际控制中会存在抖振现象。抖振现象会影响控制精度，增加系统的能量消耗，激发系统的高频未建模动态对系统造成危害。针对上述问题，研究者大多采用了两种途径来改善： 一是采用饱和函数或者双曲正切函数来替换控制项中的符号函数，这也直接导致了失去了滑模控制的强鲁棒特性，以及抗振、抗扰动能力； 二是通过对符号函数系数的设计优化方法，来减小系统抖振。 1. 问题描述 根据式(5.3)所示四旋翼飞行器6自由度动力学模型，建立确定界干扰下的动力学方程以及需满足的前提条件，即 ξ·=V mV·=REBTB+GE η·=WΩ JΩ·=-Ω×JΩ+MB+D D=(ddθdψ)，‖D‖≤Dm(5.11) 条件： D为姿态系统受到的外部风场干扰，干扰满足范数有界，Dm已知。 针对上述的确定界干扰问题，设计采用反步滑模控制方法进行处理。反步滑模控制方法被应用于针对存在匹配或非匹配干扰的复杂高阶非线性系统中。主要的优点是对系统的扰动和参数变化的低灵敏度，避免系统的精确建模以及抑制系统不确定性的影响。在设计控制律时，控制项中含有符号函数项。这种不连续项在系统状态经过滑模超平面s(x)=0时切换控制，导致控制输入量的不连续变换。这种不连续变换产生的抖振现象在传统滑模控制中必然存在。通过设计合适的趋近律，可减少系统状态在滑模面上的波动。使得处在滑动模态阶段的系统状态在收敛过程中表现平滑，从而达到有效改善系统抖振现象的目的。 将动力学模型见式(5.7a)、式(5.7b)转换为状态空间描述，即 X·=AX+BU+D(5.12) 式中，X=(,·,θ,θ·,ψ,ψ·,z,z·,x,x·,y,y·)T表示系统的状态变量，A∈R12×12为系统参数矩阵，B∈R12×4为增益矩阵，U∈R4为控制输入向量，D∈R12为干扰向量。 飞行器系统的状态空间描述如下： x·1=x2 x·2=a1x4x6+b1U+b1d x·3=x4 x·4=a2x2x6+b2Uθ+b2dθ x·5=x6 x·6=a3x2x4+b3Uψ+b3dψ x·7=x8 x·8=cosx1cosx3U1m-g x·9=x10 x·10=UxU1/m x·11=x12 x·12=UyU1/m(5.13) 式中， a1=(Jyy-Jzz)/Jxx，a2=(Jzz-Jxx)/Jyy，a3=(Jyy-Jxx)/Jzz b1=1/Jxx，b2=1/Jyy，b3=1/Jzz Ux=sinsinψ+coscosψsinθ，Uy=cossinψsinθ-cosψsin 2. 传统滑模趋近律与指数型时变增益趋近律的设计 设计滑模趋近律需满足以下要求： (1) 控制系统存在滑动模态。 (2) 系统的滑动模态渐近稳定。 (3) 具有良好的动态品质。 趋近律可以有效改善滑模趋近阶段的动态品质。由于滑模趋近阶段系统对外部扰动表现敏感，故探讨最小化趋近阶段并保持系统在滑动模态的抗干扰特性尤为重要。其中，以高为炳教授提出的几种趋近方式为著。 1) 等速趋近律 s·=-εsgn(s)，ε>0(5.14) 式中，ε为常数，表示系统状态趋近滑模面的速率为常数。通过增大或减小ε的值可以直接改变滑模趋近的速度。当外部存在已知界干扰时，通过设计增益ε可以达到李雅普诺夫稳定。在进入滑动模态时，系统具有强鲁棒性以及抗干扰能力。 2) 指数趋近律 s·=-εsgn(s)-ks，ε>0,k>0(5.15) 式中，s·=-ks的解为s=s(0)e-kt，s(0)为系统初始状态。可以看出，s·呈指数形式减小，而当系统状态偏离滑模面很远时，趋近的速度主要靠指数型增益项决定。k的取值会直接影响指数项的趋近速度。当k的取值较大时，系统状态能在较远处快速趋近滑模超平面。在接近滑模面时，趋近律中的等速项起主要作用，系统进入滑动模态。当k的取值较小时，等速趋近项起主要作用，指数趋近效果较小。在设计指数趋近律时，增大k、减小ε可以有效改善系统状态，减小系统抖振。 3) 幂次趋近律 s·=-ksαsgn(s)，k>0,α∈(0,1)(5.16) 幂次趋近的特点是，当系统状态远离滑模面时，符号函数前会获得较大的增益系数。迫使系统状态快速接近滑模超平面。当接近滑模面，增益会呈幂次项形式减小，从而获得较好的系统动态表现。在存在干扰的系统中，幂次趋近设计的滑模控制鲁棒性表现不佳。这是由于系统进入滑动模态后，符号函数增益接近于零，故系统抗干扰特性下降。 4) 指数型时变增益趋近 由式(5.15)可见，传统的滑模控制趋近律中的指数趋近律的设计方法为 s·=-εsgn(s)-ks,ε>0,k>0(5.17) 式中，ε和k为传统指数趋近律设计中的常数项。在系统存在匹配干扰时，通过设计增益系数满足ε≥‖disturbance‖，(disturbance为系统受到的匹配干扰向量)。 指数趋近律中的增益系数ε选取不小于外部干扰的上界，从而保证整个控制系统渐近稳定。然而干扰量可以在任意时刻出现，没有全局存在特性。在设计系统控制律时将开关函数项的增益系数设计成定值，会导致滑模面上的系统状态抖动明显。而式(5.18)对幂次趋近特性的分析可见，动态表现较好的传统幂次趋近律在应对系统存在干扰的情况下控制表现不佳。 s·=-(γ|s|+kc)sign(s)-Fs(5.18) 式中,γ、kc、F为常数。式（5.18）为设计提出的指数型时变增益趋近律。开关函数增益系数设计成指数型的时变值。其中，增益系数的大小会根据系统状态偏离滑模面的程度进行动态调整，以改善滑模面上的系统动态性能，提高控制效果。 在干扰的上界已知的情况下，探讨使得系统在滑模状态下最小化趋近阶段并且保持系统的抗干扰动态特性。使用传统的幂次趋近设计的滑模控制器无法有效地消除外部干扰的影响，不能满足系统李雅普诺夫稳定条件且鲁棒性不高。由式(5.17)对传统的指数趋近律特性的研究可知，对于存在确定上界的匹配干扰的系统使用传统的指数趋近律设计滑模控制器时，为使得系统满足李雅普诺夫稳定条件以及最小化趋近阶段，通常将增益系数设计为超出干扰上界值过多。导致在干扰量非全局存在时，系统状态在滑模面附近波动幅度较大。指数型时变增益趋近律的特点是，开关函数的增益项系数会根据系统状态偏离滑模面的程度呈现指数曲线的动态特性来调整。增益会随系统状态发生呈现指数特性的方式动态衰减或提升增益系数，保证系统抗干扰特性。 5.2.3指数型时变增益趋近反步滑模控制 四旋翼飞行器采用双环控制策略将系统分成两部分子系统，分为姿态内环和位置外环，分别设计非线性鲁棒控制算法。用李雅普诺夫理论来证明位置子系统和姿态子系统渐近稳定。采取如图5.4所示的控制策略来实现四旋翼飞行器对确定界干扰的抑制以及期望轨迹的鲁棒跟踪控制。在双环控制策略中，在姿态内环采用反步滑模控制，使得四旋翼飞行器的姿态系统保持自稳定，从而保证四旋翼飞行器可靠性。将位置子系统设计在外环，设计鲁棒控制方法来实现飞行机器人位置控制。为了改善系统状态在滑模超平面上的动态效果，在姿态子系统中设计采用指数型时变增益趋近反步滑模姿态控制方法。在滑模到达阶段，符号函数前的增益系统在系统状态接近滑模面时，通过所设计的指数型增益自衰，减小系统状态趋近滑模面的速度。在进入滑动模态后，由于指数型增益的存在，从而提高系统干扰下的控制表现。 图5.4确定界干扰下四旋翼飞行器控制策略 1. 姿态子系统控制 指数型时变增益趋近的反步滑模姿态控制步骤如下： 步骤1： 引入姿态跟踪误差 e eθ eψ=x1-x1d x3-x3d x5-x5d(5.19) 式中，x1d为期望滚转角，x3d为期望俯仰角，x5d为期望偏航角。 将式(5.19)两边对时间进行求导，得 e· e·θ e·ψ=x·1-x·1d x·3-x·3d x·5-x·5d=x2-x·1d x4-x·3d x6-x·5d(5.20) 为了使得姿态环虚拟子系统稳定，取李雅普诺夫函数 v1 v3 v5=12e2 12e2θ 12e2ψ(5.21) 对式(5.21)两边进行求导，得 v·1 v·3 v·5=ee· eθe·θ eψe·ψ=e(x2-x·1d) eθ(x4-x·3d) eψ(x6-x·5d)(5.22) 为了使虚拟子系统稳定，设式(5.22)中的虚拟控制量为 x2d=s-ce+x·1d x4d=sθ-cθeθ+x·3d x6d=sψ-cψeψ+x·5d(5.23) 式中，c,cθ,cψ为正数，s,sθ,sψ为姿态系统滑模面。 步骤2，设计姿态系统的滑模面 s sθ sψ=e·+ce e·θ+cθeθ e·ψ+cψeψ(5.24) 所设计的滑模面满足Hurwitz条件。 将式(5.24)两侧分别对时间t求导，得 s· s·θ s·ψ=e¨+ce· e¨θ+cθe·θ e¨ψ+cψe·ψ(5.25) 为了获得姿态系统控制律，选取李雅普诺夫候选函数 v2 v4 v6=v1+12s2 v3+12s2θ v5+12s2ψ(5.26) 将式(5.26)两边对时间t进行求导，并将式(5.25)代入后得 v·2 v·4 v·6=-ce2+se+s(ce·+ e¨) -cθe2θ+sθeθ+sθ(cθe·θ+ e¨θ) -cψe2ψ+sψeψ+sψ(cψe·ψ+ e¨ψ)(5.27) 通过式(5.27)，可得四旋翼飞行器的姿态内环的等效控制律为 Ueq=1b1(-a1x4x6+x¨1d-c(s-ce)-e) Uθeq=1b2(-a2x2x6+x¨3d-cθ(sθ-cθeθ)-eθ) Uψeq=1b3(-a3x2x4+x¨5d-cψ(sψ-cψeψ)-eψ)(5.28) 根据式(5.28)，设计姿态系统反步滑模控制的指数型时变增益趋近律 Us=1b1(-(γ|s|+k1)sign(s)-k2s) Uθs=1b2(-(γ|sθ|θ+kθ1)sign(sθ)-kθ2sθ) Uψs=1b3(-(γ|sψ|ψ+kψ1)sign(sψ)-kψ2sψ)(5.29) 式中，γ,γθ,γψ均大于1，k1,kθ1,kψ1为常数，k2,kθ2,kψ2均为正数。 因此，四旋翼飞行器姿态系统的控制律设计为 U=Ueq+Us=1b1(-a1x4x6+x¨1d-c(s -ce)-e-(γ|s|+k1)sign(s)-k2s) Uθ=Uθeq+Uθs=1b2(-a2x2x6+x¨3d-cθ(sθ -cθeθ)-eθ-(γ|sθ|θ+kθ1)sign(sθ)-kθ2sθ) Uψ=Uψeq+Uψs=1b3(-a3x2x4+x¨5d-cψ(sψ -cψeψ)-eψ-(γ|sψ|ψ+kψ1)sign(s)-kψ2sψ)(5.30) 根据式(5.11)中假设的干扰模型，所设计的控制律(5.28)中的增益系数项应满足的前提条件为 γ|s|+k1≥1+k1≥sup|d| γ|sθ|θ+kθ1≥1+kθ1≥sup|dθ| γ|sψ|ψ+kψ1≥1+kψ1≥sup|dψ| ‖K‖≥‖Dm-I‖,K=［K1Kθ1Kψ1］T (5.31) 式中，sup|di|(i=,θ,ψ)为对干扰绝对值取上确界。 将式(5.30)代入式(5.27)后，并结合式(5.31)求得 v·2 v·4 v·6=-ce2-γ|s||s|-k1|s|-k2s2+ds -cθe2θ-γ|sθ|θ|sθ|-kθ1|sθ|-kθ2s2θ+dθsθ -cψe2ψ-γ|sψ|ψ|sψ|-kψ1|sψ|-kψ2s2ψ+dψsψ ≤-ce2-γ|s||s|-k1|s|-k2s2+|d||s| -cθe2θ-γ|sθ|θ|sθ|-kθ1|sθ|-kθ2s2θ+|dθ||sθ| -cψe2ψ-γ|sψ|ψ|sψ|-kψ1|sψ|-kψ2s2ψ+|dψ||sψ| =-ce2-k2s2-(k1+γ|s|-|d|)|s| -cθe2θ-kθ2s2θ-(kθ1+γ|sθ|θ-|dθ|)|sθ| -cψe2ψ-kψ2s2ψ-(kψ1+γ|sψ|ψ-|dψ|)|sψ|<0 (5.32) 为了证明在所设计的控制器下，姿态子系统全局渐近稳定，选取李雅普诺夫候选函数 vas=12(e2+s2+e2θ+s2θ+e2ψ+s2ψ)(5.33) 将式(5.33)两边对时间t进行求导，并将式(5.32)代入，整理得 v·as=-ce2-k2s2-(γ|s|+k1-|d|)|s|- cθe2θ-kθ2s2θ-(γ|sθ|θ+kθ1-|dθ|)|sθ|- cψe2ψ-kψ2s2ψ-(γ|sψ|ψ+kψ1-|dψ|)|sψ|<0(5.34) 由式(5.34)和式(5.31)，证得姿态系统全局渐近稳定。 2. 位置系统控制 根据四旋翼飞行器模型式(5.11)中的位置系统动力学模型可见，位置环中无干扰项存在。在此使用反步滑模方法时，由于符号函数的存在，会影响系统的动态表现，降低系统控制效果。在此将符号函数项以饱和函数来替代，从而明显改善滑模抖振现象。保持输出的控制律连续以减轻执行器的负荷，提高控制系统动态品质。同时在饱和函数作用下，控制系统在滑动模态的鲁棒性能有所下降，也被称为准滑动模态。 传统的符号函数定义为 sign(h)=1,h>0 0,h=0 -1,h<0(5.35) 图5.5(a)描述的是符号函数的特性，可以看出,符号函数会在零点附近来回切换。这也是直接导致输出量的不连续性，以及产生滑模抖振现象的根本原因。在系统不存在外部干扰的情况下，使用符号函数项，会降低系统的控制表现。 饱和函数的函数描述为 sat(h)=1,h>Δ 0,|h|≤Δ -1,h<-Δ(5.36) 为了解决位置环采用符号函数造成的滑模抖振问题，用边界层的方法来代替符号函数，在此采用式(5.36)的方法设计饱和函数。饱和函数的特性如图5.5(b)所示，保证了控制律过渡的连续性，常被用于实际工程中的准滑模控制设计。 图5.5符号函数与饱和函数特性图 反步准滑模位置控制方法设计如下所述。 步骤1，设计位置跟踪误差 ez ex ey=x7-x7d x9-x9d x11-x11d(5.37) 式中，x7d,x9d,x11d为期望的位置轨迹。 将式(5.37)两边进行求导，得 e·z e·x e·y=x·7-x·7d x·9-x·9d x·11-x·11d=x8-x·7d x10-x·9d x12-x·11d(5.38) 为了保证位置系统全局渐近稳定，选取李雅普诺夫候选函数 v7 v9 v11=12e2z 12e2x 12e2y(5.39) v·7 v·9 v·11=ez(x8-x·7d) ex(x10-x·9d) ey(x12-x·11d)(5.40) 设计位置环虚拟子系统中的虚拟控制量为 x8d x10d x9d=sz-czez+x·7d sx-cxex+x·9d sy-cyey+x·11d(5.41) 式中，cz,cx,cy为正数,sz,sx,sy为位置系统滑模面。 步骤2，选取位置系统的滑模面 sz sx sy=e·z+czez e·x+cxex e·y+cyey(5.42) 位置系统所设计的滑模面满足Hurwitz条件。 将式(5.42)两边对时间t求导，可得 s·z s·x s·y=e¨z+cze·z e¨x+cxe·x e¨y+cye·y(5.43) 为了确保四旋翼飞行器位置系统满足全局渐近稳定，选取李雅普诺夫候选函数 v8 v10 v12=v7+12s2z v9+12s2x v10+12s2y(5.44) 对式(5.44)两边进行求导，并将式(5.40)、式(5.41)和式(5.43)代入得 v·8 v·10 v·12=-cze2z+szez+sz(cze·z+e¨z) -cxe2x+sxex+sx(cxe·x+e¨x) -cye2y+syey+sy(cye·y+e¨y)(5.45) 设计快速幂次趋近律为 s·z s·x s·y=-|sz|βzsign(sz)-kzsz -|sx|βxsign(sx)-kxsx -|sy|βysign(sy)-kysy(5.46) 式中，kz,kx,ky,βz,βx,βy为正数，0<βz,βx,βy<1。 结合式(5.45)和式(5.46)，得出位置系统控制输入 U1=mcosx1cosx3(-cze·z+g+x¨7d-ez-|sz|βzsat(sz)-kzsz) Ux=mU1(-cxe·x+x¨9d-|sx|βxsat(sx)-ex-kxsx) Uy=mU1(-cye·y+x¨11d-|sy|βysat(sy)-ey-kysy)(5.47) 四旋翼飞行器为欠驱动系统，期望滚转角和俯仰角满足如下非线性等式关系： Ux=sinsinψ+coscosψsinθ Uy=cossinψsinθ-cosψsin(5.48) 通过解式(5.48)的非线性等式，反解得期望的滚转角d和俯仰角θd，即 d=arcsin(Uxsinψ-Uycosψ) θd=arcsinUxcosψ+Uysinψcosd(5.49) 将式(5.46)代入式(5.45)，整理得 v·8 v·10 v·12=-cze2z-|sz|βz|sz|-kzs2z -cxe2x-|sx|βx|sx|-kxs2x -cye2y-|sy|βy|sy|-kys2y≤0(5.50) 为了确保位置系统稳定性，用李雅普诺夫方法证明整个位置外环控制的稳定性，取李雅普诺夫候选函数 vps=12(e2x+s2x+e2y+s2y+e2z+s2z)(5.51) 将式(5.51)两侧求导，并结合式(5.50)，整理可得 v·ps=-cze2z-|sz|βz|sz|-kzssz-cxe2x -|sx|βx|sx|-kzssz-cye2y-|sy|βy|sy|-kzssz≤0(5.52) 由式(5.52)证明了位置系统全局渐近稳定。 视频讲解 5.3抗干扰反步滑模控制仿真分析 下面验证采用指数型时变增益趋近设计的四旋翼飞行器反步滑模抗干扰姿态控制器，相比于传统指数趋近设计的控制器，提高了系统动态特性且保证系统抗干扰性能。给出参考轨迹与外部已知干扰与随机干扰，进行悬停仿真以及轨迹跟踪。表5.1为控制系统参数。 表5.1指数型时变增益趋近反步滑模控制系统参数 系数值系数值 c,cθ,cψ0.5cx,cy0.2 k2,kθ2,kψ20.05βx,βy,βz0.5 k1,kθ1,kψ10.5215kx,ky,kz0.2 γ,γθ,γψ1.5Δ,cz0.01(2.0) 给定期望的偏航角ψd=π/4，外部干扰模型设为 Exd1=0 D1=0 D1c+D1s,20>Strong_index_tightens>>quadrotors.slx>>control。 图5.11为四旋翼飞行器确定界干扰下的轨迹跟踪表现。由于姿态控制系统的抗干扰特性，使得飞行器在干扰下仍能满足飞行器跟踪期望轨迹的要求。实际位置会与期望位置存在较小的偏差。 彩色图片 可见，在一定干扰范围内采用所设计的控制策略可以较好地完成飞行器对任务轨迹的鲁棒跟踪，验证所设计控制系统的鲁棒性。图5.12所示为滑模面上的系统动 图5.11确定界干扰下四旋翼飞行器轨迹跟踪表现 图5.12滑模面上的系统动态表现 态表现。图中将指数型时变增益趋近反步滑模姿态控制下的系统状态沿滑模面收敛的表现与采用传统的指数型趋近律下的系统动态表现进行对比。可见，采用所设计的指数型时变增益趋近可以有效改善系统状态沿着滑模面收敛的动态表现，从而有效改善系统控制性能，提高系统控制效果。 可以整理出指数型时变增益趋近反步滑模姿态控制下，四旋翼飞行器在无干扰、常值干扰、结合波动干扰与常值干扰的混合干扰下的姿态误差及均方根误差表现，如表5.3所示。 表5.3指数型时变增益趋近反步滑模姿态控制误差对比 干 扰 形 式稳定姿态误差范围（rad）均方根误差（rad） 无干扰(-0.00020,0.00014)0.0002 常值干扰(-0.00051,-0.00027)0.0004 常值+波动干扰(-0.00068,-0.00024)0.0005 在不同形式干扰的作用下，四旋翼飞行器的姿态系统误差不同。对比表5.3中的数据可见，外部干扰对于飞行器的控制存在影响。对比无干扰与存在干扰情况下的控制效果，可见干扰下姿态系统的稳态误差会加大。同样可得，波动形式干扰会进一步影响所设计的指数型时变增益趋近反步滑模控制下的飞行器姿态系统的控制效果。 通过仿真试验，验证了对于外部风场作用于飞行器产生确定界干扰力矩的问题，指数型时变增益趋近反步滑模姿态控制方法可以达到干扰下良好的控制效果。相比于采用传统指数趋近律的设计方法，改善了系统状态沿着滑模面收敛的动态表现。通过数值仿真试验，同样验证了滑模控制对外部干扰的不敏感特性。同时，对比不同干扰形式下的姿态误差数据，发现波动干扰对于指数型时变增益趋近的反步滑模姿态控制方法下的四旋翼飞行器的控制效果存在负面影响。波动形式的干扰会引起姿态系统中滚转角和俯仰角实际值的波动。 习题 5.1什么是变结构系统？为什么要采用变结构控制？ 5.2简述机器人滑模变结构控制的基本原理。 5.3简述滑模控制系统设计的要求及步骤。 5.4给系统 x¨+α1(t)|x|x·+α2(t)x3cos2x=v 设计一个切换控制器，其中α1(t)和α2(t)满足 t≥0,|α1(t)|≤2,-1≤α2(t)≤5 5.5考虑对象 θ¨(t)=-f(θ,t)+b(t)+d(t) 设计滑模控制器，利用MATLAB画出输入为周期信号sin(t)的角度及角速度跟踪曲线。