基于线性扩张观测器的四旋翼非奇异终端滑模控制方法与流程

本发明涉及飞行器控制技术领域，具体涉及一种基于线性扩张观测器的四旋翼非奇异终端滑模控制方法。

背景技术：

四旋翼飞行器是一种集控制、机械、传感器、人工智能等跨学科先进技术为一体的空中飞行机器人，近年来一直是国内外学者们研究的热点，并且具有广泛的军用与民用价值，例如敌情侦察、地形勘测、城市建模、航拍娱乐等。然而，由于四旋翼飞行器是一种高非线性、强耦合、欠驱动的多输入多输出系统，对其设计高精度的轨迹跟踪控制器难度较大，同时，由系统外部不确定性与内部动态变化组成的复杂集总干扰，也会进一步加剧高精度的轨迹跟踪控制器的设计难度。

目前对于四旋翼的轨迹跟踪控制器的设计大致可分为两类：线性控制器与非线性控制器。其中线性控制器需要被控对象的精确模型才能设计出控制律，诸如lqr、模型预测控制、h∞控制等，上述控制策略适合某些固定工作点或小范围飞行包线内的线性模型，并不能涵盖大范围的飞行包线，但非线性控制器能够实现大范围飞行包线控制，具有半全局或全局稳定性，理论研究价值更高。然而，实际飞行中的四旋翼飞行器总是存在着被复杂集总干扰(像阵风扰动、驱动电机失效、测量误差等)影响的问题，该问题极大影响了非线性控制器的设计。因此，设计一种能够抑制集总干扰的非线性控制器对提高四旋翼轨迹跟踪控制精度的意义很大。

目前的解决办法是采用滑模控制，其本质上是一类特殊的非线性控制，其非线性表现为控制的不连续性，这种控制策略与其它控制的不同之处在于系统的结构并不固定，可以在动态过程中根据系统当前的状态(如偏差及其各阶导数等)有目的地不断变化，迫使系统按照预定“滑动模态”的状态轨迹运动。由于滑动模态可以进行设计且与对象参数及扰动无关，这就使得变结构控制具有快速响应、对参数变化及扰动不灵敏、无需系统在线辩识，物理实现简单等优点。该方法的缺点在于当状态轨迹到达滑模面后，难于严格地沿着滑模面向着平衡点滑动，而是在滑模面两侧来回穿越，从而产生颤动，即抖振问题，除此之外，传统滑模控制还存在收敛速度慢、鲁棒性差的问题。

技术实现要素：

本发明旨在至少在一定程度上解决上述技术中的技术问题之一。为此，本发明的目的在于提出一种基于线性扩张观测器的四旋翼非奇异终端滑模控制方法，能够提高四旋翼轨迹跟踪控制的收敛速度和鲁棒性，并能够有效估计系统集总干扰影响，从而有效抑制抖振，提高四旋翼轨迹跟踪控制的精度。

为达到上述目的，本发明实施例提出了一种基于线性扩张观测器的四旋翼非奇异终端滑模控制方法，包括以下步骤：建立四旋翼的动力学模型；设计与所述动力学模型相对应的线性扩张观测器；设计与所述动力学模型相对应的非奇异快速终端滑模控制律；设计多个性能优化指标，并采用果蝇算法对所述线性扩张观测器和所述非奇异快速终端滑模控制律中的控制参数进行整定，得到四旋翼轨迹跟踪控制器；通过所述四旋翼轨迹跟踪控制器对四旋翼飞行器进行控制。

根据本发明实施例基于线性扩张观测器的四旋翼非奇异终端滑模控制方法，首先通过建立四旋翼的动力学模型，然后设计与动力学模型相对应的线性扩张观测器和非奇异快速终端滑模控制律，并设计多个性能优化指标，采用果蝇算法对线性扩张观测器和非奇异快速终端滑模控制律中的控制参数进行整定，得到四旋翼轨迹跟踪控制器，最后通过此四旋翼轨迹跟踪控制器对四旋翼飞行器进行控制，由此，能够提高四旋翼轨迹跟踪控制的收敛速度和鲁棒性，并能够有效估计系统集总干扰影响，从而有效抑制抖振，提高四旋翼轨迹跟踪控制的精度。

另外，根据本发明上述实施例提出的基于线性扩张观测器的四旋翼非奇异终端滑模控制方法还可以具有如下附加的技术特征：

进一步地，所述动力学模型包括位置动力学与姿态动力学两部分，以以下6个二阶系统作为6个独立的控制通道：

其中，[x,y,z]为三轴位置，[φ,θ,ψ]为三轴姿态角，u1为总距输入，u2为俯仰输入，u3为滚转输入，u4为偏航输入，m为整机质量，g为重力加速度，i＝[ixx,iyy,izz]^t为惯性矩，d1～d6为各通道上的集总干扰。

进一步地，二阶系统对应的线性扩张观测器为：

其中，是二阶系统中状态量[x1,x2,x3]^t的估计值，x3为集总干扰，y为系统实际输出，为系统估计输出，b为控制器增益，通过实现对集总干扰x3的估计。

进一步地，其中，l通过极点配置的方式计算获得：

其中，ωo＞0是观测器增益。

进一步地，所述非奇异快速终端滑模控制律为：

其中，ue为等效控制律，ur为趋近律，ur＝b^-1(k1s+k2sig(s)^p)，

其中，b为控制器参数，β与γ为权重因子，β＞0，1＜γ＜2，跟踪误差e为期望信号xr与估计信号的差，s为滑模面，p为滑模因子。

进一步地，所述性能优化指标为4个，分别为误差绝对值的积分、误差平均积分、误差乘以时间的绝对值积分和误差乘以时间的平方积分。

进一步地，所述线性扩张观测器和所述非奇异快速终端滑模控制律中的控制参数包括ωo、β、k1和k2。

所述果蝇算法包括以下步骤：

s11，初始化参数：初始化果蝇的种群数目gs，总迭代次数tmax和果蝇种群的初始位置(x_axis,y_axis)；

s12，随机计算距离：每个果蝇个体依靠嗅觉去随机寻找食物源，并确定其方向与距离，

其中，(xi,yi)为食物源的位置，sv为果蝇个体到随机食物源的距离；

s13，气味浓度计算：计算原点到随机食物源的距离，再根据距离定义气味浓度：

其中，disti为原点到随机食物源的距离，ei为气味浓度。

s14，目标函数计算：将气味浓度代入到目标函数的公式中，来计算每个果蝇个体的气味浓度smelli，并找出具有最佳的气味浓度的果蝇个体：

s15，算法进化：记录并保留最佳的气味浓度的果蝇个体与其位置，其他剩余果蝇均向最佳果蝇处集中，实现算法的进化：

s16，终止条件：进入迭代寻优，重复执行步骤s12～s15，并判断当前最佳气味浓度是否优于前一代，若是，则保留当前寻优结果；若否，则被摒弃，直到迭代次数等于总迭代次数。

进一步地，所述果蝇算法增加自适应策略：

其中，ωm∈(0,1)为权重因子，bestsmelli-1为前一代最优的气味浓度值，τ∈(0,1)为限制因子，t为当前迭代次数，hmin为最小步长，μ为正整数，

此时步骤s12中食物源的位置为：

附图说明

图1为本发明实施例的基于线性扩张观测器的四旋翼非奇异终端滑模控制方法的流程图；

图2为本发明一个实施例的基于自适用果蝇算法的控制器参数整定流程图；

图3为本发明一个实施例的基于4种性能优化指标的迭代曲线对比图；

图4(a)为本发明一个实施例的以滚转角为例的参数整定效果图；

图4(b)为本发明一个实施例的以俯仰角为例的参数整定效果图；

图4(c)为本发明一个实施例的以偏航角为例的参数整定效果图；

图5(a)为本发明实施例的基于线性扩张观测器的四旋翼非奇异终端滑模控制方法与传统的线性自抗扰控制方法的关于x轴的响应的仿真对比图；

图5(b)为本发明实施例的基于线性扩张观测器的四旋翼非奇异终端滑模控制方法与传统的线性自抗扰控制方法的关于y轴的响应的仿真对比图；

图5(c)为本发明实施例的基于线性扩张观测器的四旋翼非奇异终端滑模控制方法与传统的线性自抗扰控制方法的关于z轴的响应的仿真对比图；

图5(d)为本发明实施例的基于线性扩张观测器的四旋翼非奇异终端滑模控制方法与传统的线性自抗扰控制方法的关于滚转角的响应的仿真对比图；

图5(e)为本发明实施例的基于线性扩张观测器的四旋翼非奇异终端滑模控制方法与传统的线性自抗扰控制方法的关于俯仰角的响应的仿真对比图；

图5(f)为本发明实施例的基于线性扩张观测器的四旋翼非奇异终端滑模控制方法与传统的线性自抗扰控制方法的关于偏航角的响应的仿真对比图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

图1为本发明实施例的基于线性扩张观测器的四旋翼非奇异终端滑模控制方法的流程图。

如图1所示，本发明实施例的基于线性扩张观测器的四旋翼非奇异终端滑模控制方法，包括以下步骤：

s1，建立四旋翼的动力学模型。

在本发明的一个实施例中，四旋翼的动力学模型包括位置动力学与姿态动力学两部分，以以下6个二阶系统作为6个独立的控制通道：

其中，[x,y,z]为三轴位置，[φ,θ,ψ]为三轴姿态角，u1为总距输入，u2为俯仰输入，u3为滚转输入，u4为偏航输入，m为整机质量，g为重力加速度，i＝[ixx,iyy,izz]^t为惯性矩，d1～d6为各通道上的集总干扰。

进一步地，上述四旋翼的动力学模型的四个输入u1、u2、u3和u4可通过改变四个桨叶的转速实现：

其中，l为旋翼中心线到四旋翼z轴间的距离。f1～f4分别为四个旋翼产生的升力，m1～m4分别为四个旋翼产生的扭矩，它们可由下列数学公式计算获得：

fi＝ρctωi²r²

mi＝ρcqωi²r³

其中，ct为升力系数，ρ为空气密度，a为旋翼旋转时的桨盘面积，ωi为桨叶旋转速度，r为旋翼半径，cq为反力矩系数。

s2，设计与动力学模型相对应的线性扩张观测器。

在本发明的一个实施例中，上述四旋翼的动力学模型的6个独立的控制通道可被认为是6个单输入单输出的子动力学模型，其输入输出呈现二阶导数关系。对于一个常见的二阶系统：

其中，x1、x2为状态变量，u是控制输入，b是控制器增益，y是系统输出，f(x1,x2,w(t),t)是包含了系统不确定性和外界干扰的集总干扰。

进一步地，可将上述常见二阶系统的表达式写成状态空间的形式：

其中，x＝[x1,x2]^t，u＝[u1,u2]^t和y＝[y1,y2]^t分别为系统的状态变量，控制输入及系统输出，f为集总干扰。

进一步地，假设f可微且有界，则添加一个扩张状态量x3来估计f，上述状态空间形式可改写成：

其中，

进一步地，上式的线性扩张观测器可被设计为：

其中，是二阶系统中状态量[x1,x2,x3]^t的估计值，x3为集总干扰，y为系统实际输出，为系统估计输出。另外，b为控制器增益，通过可以对集总干扰x3实现估计。

在本发明的一个实施例中，上述公式中的l可通过极点配置的方式计算获得：

其中，ωo＞0是观测器增益。

进一步地，对于上述观察器增益的系统由系统的特征方程计算获得：

s3，设计与所述动力学模型相对应的非奇异快速终端滑模控制律。

在本发明的一个实施例中，非奇异快速终端滑模控制律为：

其中，ue为等效控制律，ur为趋近律，ur＝b^-1(k1s+k2sig(s)^p)。

其中，b为控制器参数，β与γ为权重因子，β＞0，1＜γ＜2，跟踪误差e为期望信号xr与估计信号的差，s为滑模面，p为滑模因子。

在本发明的一个实施例中，上述公式中的等效控制律ue的非奇异快速终端滑模面为：

其中，跟踪误差的一阶导数为期望信号的一阶导数与估计信号的差，tanh函数替换传统的sign函数可有效抑制滑模控制的抖振。

s4，设计多个性能优化指标，并采用果蝇算法对所述线性扩张观测器和所述非奇异快速终端滑模控制律中的控制参数进行整定，得到四旋翼轨迹跟踪控制器。

在本发明的一个实施例中，性能优化指标为4个，分别为误差绝对值的积分(integralofabsoluteerror，简称iae)、误差平均积分(integralofthesquarederror，简称ise)、误差乘以时间的绝对值积分(integralmultipliedbyabsoluteerror，简称itae)和误差乘以时间的平方积分(integraloftimemultipliedbythesquarederror，简称itse)，其数学表达式如下：

在本发明的一个实施例中，线性扩张观测器和所述非奇异快速终端滑模控制律中的控制参数包括ωo、β、k1和k2，可通过果蝇算法对上述参数进行整定。

如图2所示，果蝇算法包括以下步骤：

s11，初始化参数：初始化果蝇的种群数目gs，总迭代次数tmax和果蝇种群的初始位置(x_axis,y_axis)；

s12，随机计算距离：每个果蝇个体依靠嗅觉去随机寻找食物源，并确定其方向与距离，

其中，(xi,yi)为食物源的位置，sv为果蝇个体到随机食物源的距离。

s13，气味浓度计算：计算原点到随机食物源的距离，再根据距离定义气味浓度：

其中，disti为原点到随机食物源的距离，ei为气味浓度。

s14，目标函数计算：将气味浓度代入到目标函数的公式中，来计算每个果蝇个体的气味浓度smelli，并找出具有最佳的气味浓度的果蝇个体：

s15，算法进化：记录并保留最佳的气味浓度的果蝇个体与其位置，其他剩余果蝇均向最佳果蝇处集中，实现算法的进化：

s16，终止条件：进入迭代寻优，重复执行步骤s12～s15，并判断当前最佳气味浓度是否优于前一代，若是，则保留当前寻优结果；若否，则被摒弃，直到迭代次数等于总迭代次数。

在本发明的一个实施例中，对于上述步骤s12，果蝇算法可增加自适应策略：

其中，ωm∈(0,1)为权重因子，bestsmelli-1为前一代最优的气味浓度值，τ∈(0,1)为限制因子，t为当前迭代次数，hmin为最小步长，μ为正整数。

则此时上述步骤s12公式中食物源的位置可为：

根据本发明实施例基于线性扩张观测器的四旋翼非奇异终端滑模控制方法，首先通过建立四旋翼的动力学模型，然后设计与动力学模型相对应的线性扩张观测器和非奇异快速终端滑模控制律，并设计多个性能优化指标，采用果蝇算法对线性扩张观测器和非奇异快速终端滑模控制律中的控制参数进行整定，得到四旋翼轨迹跟踪控制器，最后通过此四旋翼轨迹跟踪控制器对四旋翼飞行器进行控制，由此，能够提高四旋翼轨迹跟踪控制的收敛速度和鲁棒性，并能够有效估计系统集总干扰影响，从而有效抑制抖振，提高四旋翼轨迹跟踪控制的精度

下面将通过两个仿真算例，进一步说明上述基于线性扩张观测器的四旋翼非奇异终端滑模控制方法的有益效果。

算例1，该算例用自适应果蝇算法对上述所提到的控制算法进行参数整定，仅以姿态环为例。在仿真中，姿态环选择[0.1，-0.1，0.05]rad的阶跃信号为期望信号，自适应果蝇算法的参数设置为：gs＝20，tmax＝100，ωm＝0.8，τ＝0.2，μ＝10，hmin＝1，三个姿态环分别添加均值为0，方差为0.01的随机噪声，仿真时间持续10s，结果如图3、图4(a)、图4(b)和图4(c)所示。其中，图3所示的为误差绝对值的积分(iae)、误差平均积分(ise)、误差乘以时间的绝对值积分(itae)和误差乘以时间的平方积分(itse)的迭代曲线对比图，图4(a)所示的为误差绝对值的积分(iae)、误差平均积分(ise)、误差乘以时间的绝对值积分(itae)、误差乘以时间的平方积分(itse)和参考值(reference)对于滚转角的整定效果图，图4(b)所示的为误差绝对值的积分(iae)、误差平均积分(ise)、误差乘以时间的绝对值积分(itae)、误差乘以时间的平方积分(itse)和参考值(reference)对于俯仰角的整定效果图，图4(c)所示的为误差绝对值的积分(iae)、误差平均积分(ise)、误差乘以时间的绝对值积分(itae)、误差乘以时间的平方积分(itse)和参考值(reference)对于偏航角的整定效果图，通过上述迭代曲线对比图和参数整定效果图可看出选用误差乘以时间的平方积分(itse)作为性能优化指标可以获得较好的控制效果。

算例2，该算例用上述四旋翼轨迹跟踪控制器去让四旋翼跟踪[1，1，1]m的位置期望信号，位置环和姿态环均添加均值为0、方差为0.2的随机噪声信号，仿真时间持续10s，并与传统的线性自抗扰控制算法(pd-leso)进行对比，结果如图5(a)、图5(b)、图5(c)、图5(d)、图5(e)和图5(f)所示，分别对应展示了本发明实施例的基于线性扩张观测器的四旋翼非奇异终端滑模控制方法(nftsmc-leso)与传统的线性自抗扰控制方法(pd-leso)关于x轴的响应、y轴的响应、z轴的响应、滚转角的响应、俯仰角的响应和偏航角的响应的仿真对比图，通过上述仿真对比图可看出基于线性扩张观测器的四旋翼非奇异快速终端滑模控制算(nftsmc-leso)比传统的线性自抗扰控制算法(pd-leso)具有更好的跟踪精度和更强的抗扰性。

在本发明的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。

在本发明中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。

在本发明中，除非另有明确的规定和限定，第一特征在第二特征“上”或“下”可以是第一和第二特征直接接触，或第一和第二特征通过中间媒介间接接触。而且，第一特征在第二特征“之上”、“上方”和“上面”可是第一特征在第二特征正上方或斜上方，或仅仅表示第一特征水平高度高于第二特征。第一特征在第二特征“之下”、“下方”和“下面”可以是第一特征在第二特征正下方或斜下方，或仅仅表示第一特征水平高度小于第二特征。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。