抗干扰飞行控制方法及装置与流程

本发明涉及无人机技术领域，具体而言，涉及一种抗干扰飞行控制方法及装置。

背景技术：

四旋翼无人机是一种可通过远程遥控或基于自身传感器实现自主飞行的飞行器，能够适应各种环境，并在军用和民用领域都具有广阔的应用前景。

由于四旋翼无人机是一个复杂的强耦合非线性欠驱动控制系统，陀螺仪和加速度计容易受到机体震动的影响，且系统本身具有参数不确定性，容易受外部环境因素的干扰，从而影响飞行控制系统对无人机的姿态控制及位置控制。目前最常用的控制算法为pid算法，但是采用pid算法调节时间长且超调量大，对于易受到外界干扰的强耦合、非线性控制系统，控制效果通常不够理想，有较大的稳态误差。

在实际的飞行过程中，四旋翼无人机时常会受到外界干扰，例如气流扰动、噪声等，在超调量、调节时间和鲁棒性方面的控制效果不理想，易出现抗干扰能力差，飞行不稳定的问题。因此，如何解决上述问题，一直以来都是本领域技术人员关注的重点。

技术实现要素：

本发明的目的在于提供一种抗干扰飞行控制方法，以解决现有的无人机在实际飞行过程中控制效果不理想、抗干扰能力差、飞行不稳定的问题。

本发明的目的还在于提供一种抗干扰飞行控制装置，以解决现有的无人机在实际飞行过程中控制效果不理想、抗干扰能力差、飞行不稳定的问题。

为了实现上述目的，本发明实施例采用的技术方案如下：

第一方面，本发明实施例提出一种抗干扰飞行控制装置，应用于无人机。所述抗干扰飞行控制方法包括：获取所述无人机的位置信息；获取所述无人机的姿态数据，并依据所述姿态数据解算出姿态角；依据所述位置信息与设定的参考数据计算位置误差；依据所述位置误差计算位置控制量；依据所述位置控制量及所述姿态角得到期望姿态角；依据解算出的所述姿态角及所述期望姿态角计算角度误差；依据所述角度误差计算所述姿态角的控制量；依据所述位置控制量以及所述姿态角的控制量控制所述无人机的位置及姿态。

第二方面，本发明实施例还提出一种抗干扰飞行控制装置，应用于无人机，所述抗干扰飞行控制装置包括位置信息获取模块、姿态角计算模块、位置误差计算模块、位置控制量计算模块、期望姿态角计算模块、角度误差计算模块、姿态角控制量计算模块及控制模块。其中，所述位置信息获取模块用于获取所述无人机的位置信息；所述姿态角计算模块用于获取所述无人机的姿态数据，并依据所述姿态数据解算出姿态角；所述位置误差计算模块用于依据所述位置信息与设定的参考数据计算位置误差；所述位置控制量计算模块用于依据所述位置误差计算位置控制量；所述期望姿态角计算模块用于依据所述位置控制量及所述姿态角得到期望姿态角；所述角度误差计算模块用于依据解算出的所述姿态角及所述期望姿态角计算角度误差；所述姿态角控制量计算模块用于依据所述角度误差计算所述姿态角的控制量；所述控制模块用于依据所述位置控制量以及所述姿态角的控制量控制所述无人机的位置及姿态。

相对现有技术，本发明具有以下有益效果：

本发明提供的抗干扰飞行控制装置通过获取所述无人机的位置信息，获取所述无人机的姿态数据，并依据所述姿态数据解算出姿态角，依据所述位置信息与设定的参考数据计算位置误差，依据所述位置误差计算位置控制量；依据所述位置控制量及所述姿态角得到期望姿态角，依据解算出的所述姿态角及所述期望姿态角计算角度误差，依据所述角度误差计算所述姿态角的控制量，依据所述位置控制量以及所述姿态角的控制量控制所述无人机的位置及姿态。本发明提供的抗干扰飞行控制方法及装置针对确定性干扰和非确定性干扰采用反步鲁棒自适应和增稳积分反步自适应算法进行抑制，进而提高控制精度，增强抗干扰能力，以使无人机的飞行快速到达稳定。

为使本发明的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1示出了本发明的一个实施例所提供的飞行控制系统的结构框图。

图2示出了图1中主控模块的控制原理图。

图3示出了本发明的一个实施例所提供的抗干扰飞行控制装置的结构框图。

图4示出了本发明的一个实施例所提供的抗干扰飞行控制方法的流程示意图。

图标：100-飞行控制系统；200-抗干扰飞行控制装置；110-主控模块；120-传感器模块；130-定位模块；140-电调；150-电机；160-无线通信模块；210-位置信息获取模块；220-姿态角计算模块；230-位置误差计算模块；240-位置控制量计算模块；250-期望姿态角计算模块；260-角度误差计算模块；270-姿态角控制量计算模块；280-控制模块。

具体实施方式

下面将结合本发明实施例中附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。同时，在本发明的描述中，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

请参照图1，本发明的一个实施例所提供的飞行控制系统100的结构框图。在本实施例中，所述飞行控制系统100可应用于无人机，例如四旋翼飞行器，其包括主控模块110、传感器模块120、定位模块130、电调140、电机150及无线通信模块160，所述传感器模块120、定位模块130及所述无线通信模块160均与所述主控模块110电连接，所述电机150通过所述电调140与所述主控模块110块电连接。

所述无线通信模块160被配置为与遥控设备通信连接，用于接收所述遥控设备发送的参考数据。其中，所述参考数据可由用户自行设定并通过所述遥控设备发送。

所述传感器模块120用于采集所述无人机的姿态数据以便所述主控模块110读取所述姿态数据并依据所述姿态数据解算出姿态角。在本实施例中，所述传感器模块120可包括陀螺仪、加速度计、磁罗盘、温度_气压计(或称为度_气压补偿器)。

所述温度_气压计用于采集外界环境温度及冗余气压数据，所述主控模块110用于读取所述外界环境温度及冗余气压数据并依据读取的所述外界环境温度及冗余气压数据得到高度补偿数据。

所述定位模块130用于在无人机飞行时采集数据以便所述主控模块110读取数据并依据所述定位模块130采集的数据得到所述无人机的位置信息。在本实施例中，所述位置信息包括三维坐标系下x轴、y轴及z轴方向的位移，上述的高度补偿数据可用于补偿位置信息中z轴方向的位移(即高度信息)，以使获得的高度信息更为精确。

所述主控模块110用于依据所述无人机的位置信息、设定的参考数据以及解算出的所述姿态角，计算出位置控制量及姿态角的控制量，依据所述位置控制量以及所述姿态角的控制量控制所述电调140调节所述电机150的转速，进而控制所述无人机的位置及姿态，以使无人机稳定飞行。

优选地，在本实施例中，所述电调140及电机150的数量均为4。

在本实施例中，所述姿态角包括横滚角、俯仰角及偏航角，所述设定的参考数据包括期望偏航角以及x轴、y轴、z轴方向的期望位移，所述位置控制量包括x轴、y轴及z轴方向位移的控制量。

请参照图2，为所述主控模块110的控制原理图。所述主控模块110采用双环控制，内环控制无人机的飞行姿态，外环控制无人机的飞行位置，设定输入所述主控模块110的指令信号(即上述设定的参考数据)为(xd，yd，zd，ψd)，其中，xd、yd、zd表示x轴、y轴、z轴方向的期望位移，ψd表示期望偏航角，其中双环控制的最终输出控制量为u1为外环控制输出的控制量，表示无人机的垂直轴控制量(类似油门大小)，u2、u3、u4为内环控制的输出量，分别表示横滚角φ的控制量、俯仰角θ的控制量、偏航角ψ的控制量，该4个控制量与飞行控制系统100的4个电调140对应，以控制所述电调140调节所述电机150的转速。其中，外环控制回路以反步法为基础，结合自适应原理，引入积分环节，基于当前位置(x、y、z)与期望位置(xd、yd、zd)的误差，采用增稳积分反步自适应控制算法，得到垂直轴控制量u1以控制无人机上升的速度；同时外环控制回路计算得到期望横滚角φd及期望俯仰角θd输入内环控制回路，内环控制回路将外环控制输出的期望横滚角φd、期望俯仰角θd以及指令信号中的期望偏航角ψd作为期望姿态角(φd、θd、ψd)，基于解算出的实际姿态角(φ、θ、ψ)与期望姿态角(φd、θd、ψd)的误差，采用反步鲁棒自适应控制算法，计算出横滚角φ的控制量u2、俯仰角θ的控制量u3及偏航角ψ的控制量u4，进而所述主控模块110依据该控制量u1、u2、u3、u4实现无人机的位置和姿态控制。

由上述可知，φ、θ、ψ、x、y、z这6个参数只通过4个控制量u1、u2、u3、u4进行控制，因此四旋翼无人机是一个强耦合欠驱动系统，其姿态控制和位置控制具有耦合关系。在本实施例中，采用反步法的基本思想，将复杂的飞行性系统分解成不超过系统阶数的子系统，然后为每个子系统分别设计lyapunov函数和中间虚拟控制量，以倒推的方式直到完成所有控制量的设计。

请参照图3，为本发明的一个实施例所提供的抗干扰飞行控制装置200的结构框图。所述抗干扰飞行控制装置200可应用于上述的主控模块110中，用于控制无人机的稳定飞行，其包括位置信息获取模块210、姿态角计算模块220、位置误差计算模块230、位置控制量计算模块240、期望姿态角计算模块250、角度误差计算模块260、姿态角控制量计算模块270及控制模块280。可以理解，上述的位置信息获取模块210、姿态角计算模块220、位置误差计算模块230、位置控制量计算模块240、期望姿态角计算模块250、角度误差计算模块260、姿态角控制量计算模块270及控制模块280可以为存储于存储器中的软件功能模块及计算机程序，并且可被所述主控模块110执行。

所述位置信息获取模块210用于获取所述无人机的位置信息。

在本实施例中，所述位置信息获取模块210将所述定位模块130采集的数据经计算转换得到所述无人机的位置信息，其中，所述位置信息包括三维坐标系下x轴、y轴、z轴方向的位移，可表示为(x、y、z)。

所述姿态角计算模块220用于获取所述无人机的姿态数据，并依据所述姿态数据解算出姿态角。

在本实施例中，所述无人机的姿态数据由所述传感器模块120采集得到，所述姿态角计算模块220将所述姿态数据采用非线性互补滤波的姿态融合算法，解算得到精确的姿态角。在本实施例中，该姿态角包括横滚角φ、俯仰角θ及偏航角ψ。在本实施例中，所述姿态角计算模块220采用非线性互补滤波的方式对姿态数据进行融合，可减小零点漂移及温度漂移对飞行控制系统100的影响，提高抗干扰能力。

所述位置误差计算模块230用于依据所述位置信息与设定的参考数据计算位置误差。

在本实施例中，所述设定的参考数据为(xd，yd，zd，ψd)，其中xd，yd，zd作为外环位置控制的输入，所述位置误差计算模块230依据所述设定的参考数据(xd，yd，zd)及位置信息(x、y、z)得到位置误差。例如，根据当前位置信息中z轴方向位移z(可理解为无人机的飞行高度)与z轴方向的期望位移zd可得到z轴方向的位移误差e7，同理，可得到x轴方向的位移误差e9及y轴方向的位移误差e11。

所述位置控制量计算模块240用于依据所述位置误差计算位置控制量。

在本实施例中，设定状态变量：定义x1＝φ，x3＝θ，x5＝ψ，x7＝z，x9＝x，x11＝y，

则可建立无人机数学模型：

其中，m表示所述无人机的质量，l表示所述无人机的质心到旋翼旋转轴的距离，ωr表示所述无人机的角速度，ix、iy、iz分别表示x、y、z轴3个轴向的转动惯量，g表示重力加速度，ir表示无人机旋翼绕电机轴的转动惯量。

在本实施例中，位置控制主要控制(x，y，z)3个位移变量，以位置控制中的高度控制为例。定义外界干扰r2＝(rx,ry,rz)(也即是沿x、y、z三个坐标轴方向的作用力)，其中高度通道的动力学模型的状态方程可表示为：由于rz的真实值未知，故用估计值代替。定义估计误差：在本实施例中，认为模型的不确定性为常值变化，则有

在本实施例中，引入积分项其中为被积函数，所述被积函数的取值满足由于引入积分环节，倘若误差比较大时，积分之后会得到一个较大的值，使得系统产生比较剧烈的抖震，超调量也会增加，系统响应时间也会延迟。因此，采用切换控制的思想，对积分项中的被积函数进行了改进。具体地，定义xid(i＝7,8,9,10,11,12)为期望值，取限定调节带状区域为(xid-δ,xid+δ)，当实际值xi在该预设的限定调节带状区域内时(即向该区域靠拢)，所述积分项中的被积函数取值为零；当xi未在预设的限定调节带状区域内时(即其它情况)，所述积分项中的被积函数取值为对应的误差e7、e8、e9、e10、e11、e12。通过对该积分项的改进，使得当飞行控制系统100受到强干扰信号时，可以有效地减少系统的振荡幅度，提高系统对外部扰动和模型不确定性的鲁棒性。

由此，得到z轴方向位移的控制量：

其中，z轴方向的速度误差

在本实施例中，为了求出估计值的自适应律，选取lyapunov函数：选取估计值的自适应律：则可得因此z轴方向位移的控制量可表示为：

同理，可得到x轴、y轴方向位移的控制量分别为：

其中，x7d、x9d、x11d分别为所述z轴、x轴、y轴方向的期望位移，e7、e8分别为z轴方向的位移误差及速度误差，e9、e10分别为x轴方向的位移误差及速度误差，e11、e12分别为y轴方向的位移及速度误差，c7、c8、c9、c10、c11、c12、λ1、λ2、λ3、ε1、ε2、ε3均表示控制参数变量，ρ⁷、ρ⁸、ρ⁹、ρ¹⁰、ρ¹¹、ρ¹²均为积分项。

因此，当x、y、z轴方向位移的控制量分别为上述所得控制量uz、ux、uy时，可保证x轴、y轴、z轴方向的位移误差是指数渐进稳定的。并且，在本实施例中，根据上述控制量uz、ux、uy可以得到垂直轴控制量u1，该垂直轴控制量u1是一个合矢量，应当理解，该垂直轴控制量u1在三维坐标系下的投影与uz、ux、uy有关联性，可控制无人机上升的速度，进而达到无人机位置控制的目的。

所述期望姿态角计算模块250用于依据所述位置控制量及所述姿态角得到期望姿态角。

具体地，由于姿态控制和位置控制具有耦合关系，所述期望姿态角计算模块250可依据上述得到的所述x轴、y轴方向位移的控制量ux、uy与姿态角(φ、θ、ψ)之间的耦合关系解算得到期望横滚角φd及期望俯仰角θd，从而得到期望姿态角(φd、θd、ψd)作为内环姿态控制的输入。

所述角度误差计算模块260用于依据解算出的所述姿态角及所述期望姿态角计算角度误差。

在本实施例中，所述期望姿态角包括所述期望横滚角φd、期望俯仰角θd及所述期望偏航角ψd，所述角度误差计算模块260用于依据所述姿态角(φ、θ、ψ)、期望横滚角φd、期望俯仰角θd及所述期望偏航角ψd分别计算出所述横滚角φ、俯仰角θ及所述偏航角ψ的角度误差e1、e3、e5。

所述姿态角控制量计算模块270用于依据所述角度误差计算所述姿态角的控制量。

在本实施例中，姿态控制主要控制(φ、θ、ψ)3个姿态角度变量，以计算横滚角φ的控制量为例。定义外界干扰r1＝(rφ,rθ,rψ)，e2为横滚角速度误差，所述横滚角φ的动力学模型可表示为：

定义非负光滑函数μφ(x)，使得|rφ|≤γφμφ(x)。引入反步函数虚拟控制量αφ用于抑制不确定性rφ对系统的影响，其中，τ1>0。定义未知常量γφ的估计值为sat(·)表示饱和函数。的自适应更新律为cφ>0。

定义未知常量γφ的误差为选取lyapunov函数根据lyapunov函数的稳定性定理，时，横滚角φ的误差是渐进稳定的。故可得横滚角φ的鲁棒自适应控制律为：

同理可得，俯仰角θ和偏航角ψ的控制率分别为：

上述计算公式中，x1d、x3d、x5d分别表示所述期望横滚角、期望俯仰角及所述期望偏航角，x2、x4、x6分别表示横滚角速度、俯仰角速度及偏航角速度，e1、e2分别表示横滚角的角度误差及横滚角速度误差，e3、e4分别表示俯仰角的角度误差及俯仰角速度误差，e5、e6分别表示偏航角的角度误差及偏航角速度误差，αφ、αθ及αψ分别表示反步函数虚拟控制量，a1、a2、a3、b1、b2、b3、c1、c2、c3、c4、c5、c6均表示控制参数变量。

当姿态角(φ、θ、ψ)的控制量分别取u2、u3、u4时，根据lyapunov函数的稳定性定理，姿态角的误差是渐进稳定的。

所述控制模块280用于依据所述位置控制量以及所述姿态角的控制量控制所述无人机的位置及姿态。

在本实施例中，所述控制模块280依据x、y、z轴方向位移的控制量uz、ux、uy可得到垂直控制量u1，然后依据该垂直控制量u1、横滚角φ的控制量u2、俯仰角θ的控制量u3及偏航角ψ的控制量u4输出控制信号以控制所述电调140调节所述电机150的转速，进而控制所述无人机的位置及姿态，使无人机的位置误差和姿态角的角度误差趋近于0，控制精度高，提高了无人机飞行的稳定性。

请参照图4，为本发明的一个实施例所提供的抗干扰飞行控制方法的流程示意图。需要说明的是，本发明实施例所述的抗干扰飞行控制方法并不以图4以及以下所述的具体顺序为限制，其基本原理及产生的技术效果与上述实施例所提供的抗干扰飞行控制装置200相同。应当理解，在其他实施例中，本发明所述的抗干扰飞行控制方法其中部分步骤的顺序可以根据实际需要相互交换，或者其中的部分步骤也可以省略或删除。所述抗干扰飞行控制方法可应用于上述的主控模块110中，下面将对图4所示的具体流程进行详细阐述。

步骤s101，获取所述无人机的位置信息。

可以理解，该步骤s101可以由上述的位置信息获取模块210执行。

步骤s102，获取所述无人机的姿态数据，并依据所述姿态数据解算出姿态角。

可以理解，该步骤s102可以由上述的姿态角计算模块220执行。

步骤s103，依据所述位置信息与设定的参考数据计算位置误差。

可以理解，该步骤s103可以由上述的位置误差计算模块230执行。

步骤s104，依据所述位置误差计算位置控制量。

可以理解，该步骤s104可以有上述的位置控制量计算模块240执行。

步骤s105，依据所述位置控制量及所述姿态角得到期望姿态角。

可以理解，该步骤s105可以由上述的期望姿态角计算模块250执行。

步骤s106，依据解算出的所述姿态角及所述期望姿态角计算角度误差。

可以理解，该步骤s106可以由上述的角度误差计算模块260执行。

步骤s107，依据所述角度误差计算所述姿态角的控制量。

可以理解，该步骤s107可以由上述的姿态角控制量计算模块270执行。

步骤s108，依据所述位置控制量以及所述姿态角的控制量控制所述无人机的位置及姿态。

可以理解，该步骤s108可以由上述的控制模块280执行。

综上所述，本发明实施例所提供的抗干扰飞行控制方法及装置，该抗干扰飞行控制方法包括外环位置控制及内环姿态控制，其中外环采用增稳积分反步自适应控制算法，基于位置误差计算得出x、y、z轴方向位移的控制量ux、uy、uz，依据控制量ux、uy、uz输出垂直轴控制量u1，并依据控制量ux、uy与姿态角φ、θ、ψ之间的关系计算出期望横滚角φd及期望俯仰角θd。内环采用鲁棒自适应控制算法，基于姿态角(φ、θ、ψ)与期望姿态角(φd、θd、ψd)的误差计算得到横滚角φ的控制量u2、俯仰角θ的控制量u3及偏航角ψ的控制量u4，依据输出的垂直轴控制量u1、横滚角φ的控制量u2、俯仰角θ的控制量u3及偏航角ψ的控制量u4控制电调调节所述电机的转速，使无人机的位置误差和姿态角的角度误差趋近于0，进而实现所述无人机的位置及姿态控制，以使无人机稳定飞行。本发明实施例所提供的抗干扰飞行控制方法及装置针对外部确定性干扰和非确定性干扰采用反步鲁棒自适应和增稳积分反步自适应算法来使控制无人机的飞行快速到达稳定，控制精度高，增强了飞行控制系统对外界的抗干扰能力，在超调量、调节时间及鲁棒性方面有较好的控制效果。

需要说明的是，在本文中，诸如“第一”和“第二”等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。