一种四旋翼无人机的故障容错控制方法及系统与流程

本发明涉及无人机技术领域，尤其涉及一种四旋翼无人机的故障容错控制方法及系统。

背景技术：

四旋翼无人机因为其易操控、体积小、行动灵活等特点，现已被广泛的应用。但随着其被应用，许多问题也显现出来，因其在飞行状态时，电机和螺旋桨工作在高速旋转的状态，由于执行机构的电圈老化等问题，容易发生失效故障。四旋翼一旦发生上述故障，如果对故障无容错能力，则会导致飞机的稳定性下降，甚至导致无人机失控，威胁到人身安全。

技术实现要素：

本发明针对现有技术中存在的问题，提供了一种四旋翼无人机的故障容错控制方法及系统，能够在四旋翼无人机的电机发生部分失效故障时，保证无人机姿态和高度的可控性，保持无人机姿态的稳定性，避免了事故的发生。

本发明就上述技术问题而提出的技术方案如下：

一方面，本发明提供一种四旋翼无人机的故障容错控制方法，包括：

当无人机的部分电机发生故障时，获取故障电机的故障信息；

根据所述故障信息对四个电机的控制分配信息进行优化，使所述四个电机的虚拟控制指令与无人机所要达到的姿态和高度呈线性关系；所述虚拟控制指令为无人机飞控系统根据用户操作解算出的控制指令；

根据优化后的控制分配信息对所述四个电机的虚拟控制指令进行分配，获得所述四个电机的实际控制指令；

根据所述实际控制指令控制所述无人机达到所需的姿态和高度。

进一步地，所述当无人机的部分电机发生故障时，获取故障电机的故障信息，具体包括：

对无人机的四个电机进行实时检测；

当无人机的部分电机发生故障时，计算故障电机的输出与无故障时的输出的比值，所述比值即为所述故障电机的故障信息；

根据所述故障电机的故障信息构建故障信息矩阵。

进一步地，所述根据所述故障信息对四个电机的控制分配信息进行优化，使所述四个电机的虚拟控制指令与无人机所要达到的姿态和高度呈线性关系，具体包括：

根据优化公式，使所述四个电机的虚拟控制指令与无人机所要达到的姿态和高度呈线性关系，并根据正常时的控制分配矩阵和所述故障信息矩阵，计算优化后的控制分配矩阵，所述优化后的控制分配矩阵即为优化后的控制分配信息；

所述关系公式为：

其中，为四个电机的虚拟控制指令，为无人机所要达到的高度和姿态，m为正常时的控制分配矩阵，h为故障信息矩阵，n为优化后的控制分配矩阵。

进一步地，所述根据优化后的控制分配信息对所述四个电机的虚拟控制指令进行分配，获得所述四个电机的实际控制指令，具体包括：

采用分配公式，使优化后的控制分配信息对所述四个电机的虚拟控制指令进行分配，获得所述四个电机的实际控制指令；

所述分配公式为：

其中，为分配后的四个电机的实际控制指令。

进一步地，所述根据所述实际控制指令控制所述无人机达到所需的姿态和高度，具体包括：

根据所述实际控制指令调整所述四个电机的转速，进而使所述四个电机驱动所述无人机达到所需的姿态和高度；所述姿态包括俯仰、滚转和偏航。

另一方面，本发明提供一种四旋翼无人机的故障容错控制系统，包括：

信息获取模块，用于当无人机的部分电机发生故障时，获取故障电机的故障信息；

优化模块，用于根据所述故障信息对四个电机的控制分配信息进行优化，使所述四个电机的虚拟控制指令与无人机所要达到的姿态和高度呈线性关系；所述虚拟控制指令为无人机飞控系统根据用户操作解算出的控制指令；

分配模块，用于根据优化后的控制分配信息对所述四个电机的虚拟控制指令进行分配，获得所述四个电机的实际控制指令；以及，

控制模块，用于根据所述实际控制指令控制所述无人机达到所需的姿态和高度。

进一步地，所述信息获取模块具体包括：

检测单元，用于对无人机的四个电机进行实时检测；

信息获取单元，用于当无人机的部分电机发生故障时，计算故障电机的输出与无故障时的输出的比值，所述比值即为所述故障电机的故障信息；以及，

构建单元，用于根据所述故障电机的故障信息构建故障信息矩阵。

进一步地，所述优化模块具体用于：

根据优化公式，使所述四个电机的虚拟控制指令与无人机所要达到的姿态和高度呈线性关系，并根据正常时的控制分配矩阵和所述故障信息矩阵，计算优化后的控制分配矩阵，所述优化后的控制分配矩阵即为优化后的控制分配信息；

所述关系公式为：

其中，为四个电机的虚拟控制指令，为无人机所要达到的高度和姿态，m为正常时的控制分配矩阵，h为故障信息矩阵，n为优化后的控制分配矩阵。

进一步地，所述分配模块具体用于：

采用分配公式，使优化后的控制分配信息对所述四个电机的虚拟控制指令进行分配，获得所述四个电机的实际控制指令；

所述分配公式为：

其中，为分配后的四个电机的实际控制指令。

进一步地，所述控制模块具体用于：

根据所述实际控制指令调整所述四个电机的转速，进而使所述四个电机驱动所述无人机达到所需的姿态和高度；所述姿态包括俯仰、滚转和偏航。

本发明实施例提供的技术方案带来的有益效果是：

当无人机的部分电机发生故障时，根据故障信息对四个电机的控制分配信息进行优化，使四个电机的虚拟控制指令与无人机所要达到的姿态和高度呈线性关系，从而使无人机根据优化后的控制分配信息达到其所要达到的姿态和高度，以迅速克服故障对无人机的影响，保证姿态和高度的可控性，保持无人机姿态的稳定性，避免了事故的发生。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明实施例一提供的四旋翼无人机的故障容错控制方法的流程示意图；

图2a是本发明实施例一中发生第一种故障时系统未进行控制分配优化的滚转角的响应曲线图；

图2b是本发明实施例一中发生第一种故障时系统未进行控制分配优化的俯仰角的响应曲线图；

图2c是本发明实施例一中发生第一种故障时系统未进行控制分配优化的偏航角的响应曲线图；

图2d是本发明实施例一中发生第一种故障时系统未进行控制分配优化的高度的响应曲线图；

图2e是本发明实施例一中发生第一种故障时系统未进行控制分配优化的控制指令的响应曲线图；

图2f是本发明实施例一中发生第一种故障时系统未进行控制分配优化的电机转速的响应曲线图；

图3a是本发明实施例一中发生第一种故障时系统进行控制分配优化的滚转角的响应曲线图；

图3b是本发明实施例一中发生第一种故障时系统进行控制分配优化的俯仰角的响应曲线图；

图3c是本发明实施例一中发生第一种故障时系统进行控制分配优化的偏航角的响应曲线图；

图3d是本发明实施例一中发生第一种故障时系统进行控制分配优化的高度的响应曲线图；

图3e是本发明实施例一中发生第一种故障时系统进行控制分配优化的控制指令的响应曲线图；

图3f是本发明实施例一中发生第一种故障时系统进行控制分配优化的电机转速的响应曲线图；

图4a是本发明实施例一中发生第二种故障时系统未进行控制分配优化的滚转角的响应曲线图；

图4b是本发明实施例一中发生第二种故障时系统未进行控制分配优化的俯仰角的响应曲线图；

图4c是本发明实施例一中发生第二种故障时系统未进行控制分配优化的偏航角的响应曲线图；

图4d是本发明实施例一中发生第二种故障时系统未进行控制分配优化的高度的响应曲线图；

图4e是本发明实施例一中发生第二种故障时系统未进行控制分配优化的控制指令的响应曲线图；

图4f是本发明实施例一中发生第二种故障时系统未进行控制分配优化的电机转速的响应曲线图；

图5a是本发明实施例一中发生第二种故障时系统进行控制分配优化的滚转角的响应曲线图；

图5b是本发明实施例一中发生第二种故障时系统进行控制分配优化的俯仰角的响应曲线图；

图5c是本发明实施例一中发生第二种故障时系统进行控制分配优化的偏航角的响应曲线图；

图5d是本发明实施例一中发生第二种故障时系统进行控制分配优化的高度的响应曲线图；

图5e是本发明实施例一中发生第二种故障时系统进行控制分配优化的控制指令的响应曲线图；

图5f是本发明实施例一中发生第二种故障时系统进行控制分配优化的电机转速的响应曲线图；

图6是本发明实施例二提供的四旋翼无人机的故障容错控制系统的结构示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步地详细描述。

实施例一

本发明实施例提供了一种四旋翼无人机的故障容错控制方法，参见图1，该方法包括：

s1、当无人机的部分电机发生故障时，获取故障电机的故障信息；

s2、根据所述故障信息对四个电机的控制分配信息进行优化，使所述四个电机的虚拟控制指令与无人机所要达到的姿态和高度呈线性关系；所述虚拟控制指令为无人机飞控系统根据用户操作解算出的控制指令；

s3、根据优化后的控制分配信息对所述四个电机的虚拟控制指令进行分配，获得所述四个电机的实际控制指令；

s4、根据所述实际控制指令控制所述无人机达到所需的姿态和高度。

需要说明的是，四旋翼无人机的飞行原理是通过改变四个电机的转速来改变飞机的姿态。电机的转速是通过改变pwm信号的占空比改变的，电机转速发生变化后，电机产生的扬力和转矩发生变化，同时根据四旋翼自身电机的位置分布，决定了roll(横滚)方向、pitch(俯仰)方向的转矩和yaw(偏航)方向的反转矩。roll方向和pitch方向的变化产生了飞机x，y轴方向的线速度，yaw角的变化为飞机航向的变化，扬力的变化产生了飞机z轴即高度的变化。

其中，机体坐标系定义为：原点取在无人机的质心，坐标系与机体固连；x轴与机身设计的纵轴平行，且处于无人机的对称平面内，指向前方；y轴垂直于无人机对称平面指向右方；z轴在无人机对称平面内，且垂直于x轴指向下方。整个坐标系符合欧拉坐标系右手定则。地面坐标系即惯性坐标系定义为：采用北东地坐标系，xe轴指向北面，ye轴指向东面，而ze轴指向地心方向。地面坐标系为仿真实验的环境中的坐标系。

无人机根据用户操作所产生的虚拟控制指令经控制分配信息分配为每个电机的实际控制指令。在四个电机正常的情况下，控制分配信息为固定值，不发生改变。但当无人机的部分电机发生故障时，电机无法正确响应分配的实际控制指令，即电机对于相同的控制指令无法调整为对应的转速，不能满足控制的要求，因此需对控制分配信息进行优化再分配。

当无人机的部分电机发生故障时，先获取故障电机的故障信息，通过飞控系统解算出由用户操作所产生的虚拟控制指令，通过执行机构得到无人机的高度和姿态。为了使无人机能够达到其所要达到的高度和姿态，解算出的虚拟控制指令以及无人机的高度和姿态必须呈线性关系。因此根据故障信息对控制分配信息进行调整，使解算出的虚拟控制指令以及无人机的高度和姿态满足线性关系，从而获得优化后的控制分配信息。由用户操作所产生的虚拟控制指令经优化后的控制分配信息分配为每个电机的实际控制指令，使每个电机根据实际控制指令驱动无人机达到所要达到的高度和姿态，从而保证无人机姿态和高度的可控性，保持无人机姿态的稳定性，避免了事故的发生。

进一步地，所述当无人机的部分电机发生故障时，获取故障电机的故障信息，具体包括：

对无人机的四个电机进行实时检测；

当无人机的部分电机发生故障时，计算故障电机的输出与无故障时的输出的比值，所述比值即为所述故障电机的故障信息；

根据所述故障电机的故障信息构建故障信息矩阵。

需要说明的是，当无人机的部分电机发生故障时，获取四个电机的输出值，即电机的效率。未发生故障的电机，即正常电机的效率设为1。发生故障的电机，其效率会衰减，使得故障电机的效率为故障电机的输出相对于正常时的输出的量化值，即故障电机的故障信息。四个电机的效率分别表示为h1、h2、h3、h4，从而构建出故障信息矩阵h＝diag[h1,h2,h3,h4]。

进一步地，所述根据所述故障信息对四个电机的控制分配信息进行优化，使所述四个电机的虚拟控制指令与无人机所要达到的姿态和高度呈线性关系，具体包括：

根据优化公式，使所述四个电机的虚拟控制指令与无人机所要达到的姿态和高度呈线性关系，并根据正常时的控制分配矩阵和所述故障信息矩阵，计算优化后的控制分配矩阵，所述优化后的控制分配矩阵即为优化后的控制分配信息；

所述关系公式为：

其中，为四个电机的虚拟控制指令，为无人机所要达到的高度和姿态，m为正常时的控制分配矩阵，h为故障信息矩阵，n为优化后的控制分配矩阵。

需要说明的是，由于电机的旋转，桨叶产生的扬力与电机的转速w的平方成正比，则总扬力为：

roll方向的合力矩为：

pitch方向的合力矩为：

yaw方向的合力矩为：

通过以上分析可得，扬力和力矩均与转速的平方成正比，整理可得：

其中，ktw为桨叶拉力系数，kqw为桨叶扭矩系数，r1、r2、r3、r4分别为四个旋翼中心到x轴的距离在xy平面上的投影，l1、l2、l3、l4分别为四个旋翼中心到y轴距离在xy平面上的投影，w1、w2、w3、w4分别为四个电机的转速。

令矩阵则有：

u＝aω，即ω＝a^-1u。

而控制指令与杨力、力矩之间呈线性关系，则m＝a^-1。由于矩阵a为无人机几何结构确定的，不会再发生改变，因此在电机均处于正常情况时，m是确定值。

在部分电机发生失效故障时，在中，为了使与呈线性关系，则必须为一个对角矩阵。而为已知值，则可计算出优化后的控制分配矩阵n，即优化后的控制分配信息。

进一步地，所述根据优化后的控制分配信息对所述四个电机的虚拟控制指令进行分配，获得所述四个电机的实际控制指令，具体包括：

采用分配公式，使优化后的控制分配信息对所述四个电机的虚拟控制指令进行分配，获得所述四个电机的实际控制指令；

所述分配公式为：

其中，为分配后的四个电机的实际控制指令。

进一步地，所述根据所述实际控制指令控制所述无人机达到所需的姿态和高度，具体包括：

根据所述实际控制指令调整所述四个电机的转速，进而使所述四个电机驱动所述无人机达到所需的姿态和高度；所述姿态包括俯仰、滚转和偏航。

需要说明的是，无人机根据用户操作分布虚拟控制指令给执行机构，执行机构中的电机的转速发生变化，引起扬力、力矩的改变，从而使得无人机的高度和姿态发生变化，达到无人机所要达到的高度和姿态。

利用本发明实施例的方法进行仿真实验。

roll通道在1s时给幅值为0.2弧度的阶跃信号，其他通道设定值均为0。

(1)故障信息矩阵h＝diag[0.9,1,1,1]，故障发生时间与给阶跃信号的时间一致。

图2为系统未进行控制分配优化的响应曲线：图2a为滚转角的响应曲线；图2b为俯仰角的响应曲线；图2c为偏航角的响应曲线；图2d为高度的响应曲线；图2e为控制指令的响应曲线；图2f为电机转速的响应曲线。

图3为系统进行控制分配优化后的响应曲线：图3a为滚转角的响应曲线；图3b为俯仰角的响应曲线；图3c为偏航角的响应曲线；图3d为高度的响应曲线；图3e为控制指令的响应曲线；图3f为电机转速的响应曲线。

从图2中可以看出在电机1发生故障时，原始系统虽然是可控的，但姿态角均存在静差，不能达到控制的要求。从图3中可以看出此时无人机完全可控。

(2)故障信息矩阵h＝diag[0.8,1,1,1]，故障发生时间与给阶跃信号的时间一致。

图4为系统未进行控制分配优化的响应曲线：图4a为滚转角的响应曲线；图4b为俯仰角的响应曲线；图4c为偏航角的响应曲线；图4d为高度的响应曲线；图4e为控制指令的响应曲线；图4f为电机转速的响应曲线。

图5为系统进行控制分配优化后的响应曲线：图5a为滚转角的响应曲线；图5b为俯仰角的响应曲线；图5c为偏航角的响应曲线；图5d为高度的响应曲线；图5e为控制指令的响应曲线；图5f为电机转速的响应曲线。

从图4中可以看出在电机1发生故障时，原始系统已完全不可控，控制指令与电机转速均超过限定值，系统发散。从图5中可以看出经过对控制分配矩阵优化后，在发生故障时只增加了电机1对应的控制指令，使电机1在发生失效故障后，能够达到与电机3相同的转速，使飞机完全可控。

本发明实施例能够在无人机的部分电机发生故障时，根据故障信息对四个电机的控制分配信息进行优化，使四个电机的虚拟控制指令与无人机所要达到的姿态和高度呈线性关系，从而使无人机根据优化后的控制分配信息达到其所要达到的姿态和高度，以迅速克服故障对无人机的影响，保证姿态和高度的可控性，保持无人机姿态的稳定性，避免了事故的发生。

实施例二

本发明实施例提供了一种四旋翼无人机的故障容错控制系统，能够实现上述四旋翼无人机的故障容错控制方法的所有流程，参见图6，所述四旋翼无人机的故障容错控制系统包括：

信息获取模块1，用于当无人机的部分电机发生故障时，获取故障电机的故障信息；

优化模块2，用于根据所述故障信息对四个电机的控制分配信息进行优化，使所述四个电机的虚拟控制指令与无人机所要达到的姿态和高度呈线性关系；所述虚拟控制指令为无人机飞控系统根据用户操作解算出的控制指令；

分配模块3，用于根据优化后的控制分配信息对所述四个电机的虚拟控制指令进行分配，获得所述四个电机的实际控制指令；以及，

控制模块4，用于根据所述实际控制指令控制所述无人机达到所需的姿态和高度。

进一步地，所述信息获取模块具体包括：

检测单元，用于对无人机的四个电机进行实时检测；

信息获取单元，用于当无人机的部分电机发生故障时，计算故障电机的输出与无故障时的输出的比值，所述比值即为所述故障电机的故障信息；以及，

构建单元，用于根据所述故障电机的故障信息构建故障信息矩阵。

进一步地，所述优化模块具体用于：

根据优化公式，使所述四个电机的虚拟控制指令与无人机所要达到的姿态和高度呈线性关系，并根据正常时的控制分配矩阵和所述故障信息矩阵，计算优化后的控制分配矩阵，所述优化后的控制分配矩阵即为优化后的控制分配信息；

所述关系公式为：

其中，为四个电机的虚拟控制指令，为无人机所要达到的高度和姿态，m为正常时的控制分配矩阵，h为故障信息矩阵，n为优化后的控制分配矩阵。

进一步地，所述分配模块具体用于：

采用分配公式，使优化后的控制分配信息对所述四个电机的虚拟控制指令进行分配，获得所述四个电机的实际控制指令；

所述分配公式为：

其中，为分配后的四个电机的实际控制指令。

进一步地，所述控制模块具体用于：

根据所述实际控制指令调整所述四个电机的转速，进而使所述四个电机驱动所述无人机达到所需的姿态和高度；所述姿态包括俯仰、滚转和偏航。

本发明实施例在无人机的部分电机发生故障时，根据故障信息对四个电机的控制分配信息进行优化，使四个电机的虚拟控制指令与无人机所要达到的姿态和高度呈线性关系，从而使无人机根据优化后的控制分配信息达到其所要达到的姿态和高度，以迅速克服故障对无人机的影响，保证姿态和高度的可控性，保持无人机姿态的稳定性，避免了事故的发生。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。