一种六旋翼无人机动力缺失保护系统、保护方法

技术领域

本发明涉及无人驾驶飞机领域，更具体地说，尤其涉及一种六旋翼无人机动力缺失保护系统、保护方法。

背景技术

现阶段，无人机(旋翼机)的应用环境越来越复杂，例如：电力巡检、森林防火、海洋环境检测、消防、喷洒农药等。

在上述应用场景中，超视距作业是无人机运行的常态，而在超视距作业时，系统的安全性至关重要；特别的，若相关保护机制不够健全，可能会带来非常大的危害。

对于多旋翼无人机而言，其是依靠多个旋翼来完成飞行任务，桨叶的破损、电机的失控、电路的损害、或机臂的断裂等，都有可能造成一个旋翼方向的动力缺失；从而使飞机丧失平衡性而失控，甚至发生坠机危害。虽然四旋翼，在缺失一个机臂动力的时候，无法完成飞机姿态的平稳；但是对于六旋翼、八旋翼、四轴八桨、甚至更多旋翼的机型，当缺失一个动力或多个动力时候，系统的结构上仍是一个可控模型。

针对上述问题，现有技术进行如下研究：

方式一：CN108341072A公开了一种无人机动力系统故障检测的方法、装置及无人机，其技术方案是：将动力缺失异常的情况当作外部扰动处理，得到扰动的大小和方向，从而定位故障电机，让飞控系统及时改变控制分配矩阵，让余下的动力系统能够保持无人机机体稳定飞行，大大降低了多旋翼无人机因动力故障造成的坠机概率。

方式二：CN106628194A公开了一种无人机的降落伞系统、安全保护方法及装置，其技术方案是：传感器用于检测无人机的飞行状态，控制器用于从传感器获取无人机的飞行状态，当无人机处于失稳状态时控制开启降落伞。该方案的缺点在于：①这种方案是被动的挽救方法；②降落伞只有在飞行高度大于100m时候才能起到很好保护作用；③当环境中有风的时候，无人机的坠落不可控，甚至起不到保护作用。

方式三：CN108646779A公开了一种无人机的保护方法，针对无人机飞行时飞行姿态和高度控制误差超过设定阈值的情况提出的保护方法，该方法能自动检测当前飞行姿态和高度控制误差，并与预先设定好的安全阈值进行比较，根据比较结果对超出阈值的情况进行报警或控制自动回收；该保护措施具备响应速度快，准确度高的特点，能够有效提高靶机在姿态和高度差超限情况下的安全性和生存能力。

方式四：动力冗余方案：在一个机臂方向设计冗余动力结构，当其中一个损失动力后，另一个也可弥补该方向的动力；存在缺点：①增加了无人机的重量及空间；②增加了系统的成本；③降低了桨叶升力效率，使系统作业时间降低。

而如何解决上述问题，仍然值得深入研究。

发明内容

本发明的目的在于针对上述现有技术的不足，提供一种六旋翼无人机动力缺失保护系统、保护方法。

一种六旋翼无人机动力缺失保护系统,所述的六旋翼无人机包括：6个浆叶，按照逆时针编号为：0、1、2、3、4、5号浆叶；0号浆叶与1号浆叶关于X轴对称，2号浆叶与5号浆叶关于X轴对称，3号浆叶与4号浆叶关于X轴对称；1号浆叶与3号浆叶关于Y轴对称，0号浆叶与4号浆叶关于Y轴对称；其中，0、1号桨叶提供正的俯仰力矩，3、4号桨叶提供负的俯仰力矩；1、2、3号桨叶提供正的横滚力矩，0、4、5号桨叶提供负的横滚力矩；无人机通过桨叶的反扭矩提供航向的力矩，0、2、4号桨叶提供顺时针的航向力矩，而1、3、5号桨叶提供逆时针的航向力矩；

六旋翼无人机动力缺失保护系统与飞控系统双向连接；

六旋翼无人机动力缺失保护系统的输出端与电调的输入端连接；

其包括：数据输入模块、数据查询模块、存储模块，分析模块；

数据输入模块，用于输入六旋翼无人机的六个浆叶的最大油门值thr_max_i和最小油门值thr_min_i，thr_min_i表示第i号浆叶电机的最大油门值；thr_max_i表示第i号浆叶电机的最小油门值，其中，i＝0～5；

数据查询模块，用于从飞控系统中查询Pitch_thr(飞控系统计算出来用于保持无人机俯仰动作的油门量)，Roll_thr(飞控系统计算出来用于保持无人机横滚动作的油门量)、alt_thr(表示是飞控系统计算得到保证飞机做高度方向动作的油门)、yaw_thr(航向油门)

分析模块，用于计算best_thr_i，thr_i、motor_thr_max、motor_thr_min、thr_max、thr_min、thr_adj_max、thr_adj_min、alt_adj、yaw_allowed、thr_motor_i，last_thr_max，last_thr_min；

所述存储模块用于存储数据输入模块、数据查询模块、分析模块的数据；

六旋翼无人机动力缺失保护系统将thr_motor_i传递给各个电调，以控制各个浆叶电机。

一种六旋翼无人机动力缺失保护方法，包括以下步骤：

S1：设置飞机的最佳油门为每个电机的中间油门：

best_thr_i＝(thr_min_i+thr_max_i)/2

thr_min_i表示第i号浆叶电机的最大油门值；

thr_max_i表示第i号浆叶电机的最小油门值；

best_thr_i表示第i号浆叶电机的最佳油门值；

其中，i＝0、1、2、3、4、5；

S2：

按照上述分配原则，分别把横滚油门、俯仰油门分配到每一个电机:

thr_0＝best_thr_0+picth_thr×cos(-30°)+roll_thr×sin(-30°)

thr_1＝best_thr_1+pitch_thr×cos30°+roll_thr×sin30°

thr_2＝best_thr_2+pitch_thr×cos90°+roll_thr×sin90°

thr_3＝best_thr_3+pitch_thr×cos150°+roll_thr×sin150°

thr_4＝best_thr_4+pitch_thr×cos(-150°)+roll_thr×sin(-150°)

thr_5＝best_thr_5+pitch_thr×cos(-90°)+roll_thr×sin(-90°)

thr_0表示：第0号浆叶电机的油门输出值；

thr_1表示：第1号浆叶电机的油门输出值；

thr_2表示：第2号浆叶电机的油门输出值；

thr_3表示：第3号浆叶电机的油门输出值；

thr_4表示：第4号浆叶电机的油门输出值；

thr_5表示：第5号浆叶电机的油门输出值；

其中，Pitch_thr表示：飞控系统计算出来用于保持无人机俯仰动作的油门量；Roll_thr表示：飞控系统计算出来用于保持无人机横滚动作的油门量；

S3：

S3-1,求取所有电机中的油门最大值motor_thr_max和最小值motor_thr_min，即：

所有电机中的油门最大值motor_thr_max为thr_0、thr1、thr2、thr3、thr4、thr5的最大值；

所有电机中的油门最小值motor_thr_min为thr_0、thr1、thr2、thr3、thr4、thr5的最小值；

S3-2,计算还剩下的油门空间：

首先，计算：thr_max、thr_min；

thr_max是thr_max_0、thr_max_1、thr_max_2、thr_max_3、thr_max_4、thr_max_5的最大值；

thr_min是thr_min_0、thr_min_1、thr_min_2、thr_min_3、thr_min_4、thr_min_5的最小值；

然后，然后计算参数：thr_adj_max、thr_adj_min；

thr_adj_max＝thr_max-motor_thr_max；

thr_adj_min＝thr_min-motor_thr_min；

thr_adj_max表示为了不使每个电机达到上限还剩余的油门量；

thr_adj_min表示为了不使每个电机达到下限还剩的油门量。

S3的思想在于：对飞控系统计算出来的量，进行电机分配的原则是保证：分配后的每个电机油门都在合理范围内(电调可执行油门)；即经过上述计算，即把横滚油门、俯仰油门加到电机的最佳油门上后，为了保证每个电机不达到上限，还剩余多少油门量。

S4：

比较油门量与剩下的油门空间，并确定还剩的油门区间：

当飞机在升高时，即alt_thr大于0时；

alt_adj等于min(alt_thr,thr_adj_max)，即取为alt_thr、thr_adj_max的较小值；

当飞机在升高时，即alt_thr小于0时；

alt_adj＝max(alt_thr,thr_adj_min)，即取为alt_thr,thr_adj_min的较大值；

其中，alt_thr表示是飞控系统计算得到保证飞机做高度方向动作的油门；

其中，alt_adj表示油门适应值参数(即alt_adjust值)。

S5：

S5-1,计算航向油门剩余量yaw_allowed，并分配航向油门：

yaw_allowed＝min(min(thr_adj_max,-thr_adj_min)-alt_adj,yaw_thr)

即“thr_adj_max,-thr_adj_min的较小值减去-alt_adj的值”与“yaw_thr”的较小值赋值给yaw_allowed；

yaw_allowed表示分配完横滚油门、俯仰油门、高度油门后，能用于控制飞机机头方向(注意不是航线方向)的油门剩余量，其采用上式计算得到；

yaw_thr表示航向油门(该数值有飞控系统计算得到，属于现有技术，在此不再熬述)。S5-2,

限制航向油门分配的最小值为100，即防止无人机旋转太快，在落地时候损坏设备：

若S5-1中的yaw_allowed计算结果小于100，则yaw_allowed取值为100。

若S5-1中的yaw_allowed计算结果大于100，则yaw_allowed不变。

S6：

S6-1,将alt_adj与yaw_allowed分别加到S2得到的每个电机上，即：

thr_motor_i＝thr_i+alt_adj+yaw_allowed；

其中，i＝0、1、2、3、4、5。

其中，thr_motor_i表示第i号浆叶电机的油门；

S6-2,计算6个电机油门中的最大值last_thr_max，最小值last_thr_min

last_thr_max为thr_motor_0、thr_motor_1、thr_motor_2、thr_motor_3、thr_motor_4、thr_motor_5的最大值。

last_thr_min表示thr_motor_0、thr_motor_1、thr_motor_2、thr_motor_3、thr_motor_4、thr_motor_5的最小值。

S7：判断每个电机是否在正常油门范围内：

即thr_motor_i是否在：区间[thr_min_i，thr_max_i]之间：

当thr_motor_i在区间[thr_min_i，thr_max_i]时，thr_motor_i不变，转入步骤S8；

当thr_motor_i大于thr_max_i，或者last_thr_min小于thr_min_i时，即如果有电机超过限幅；则进行以下操作：

调整次数为奇数次时，alt_adj采用原先计算的90％，即降低10％；alt_adj调整后的结果代入步骤S6中，重新计算；

调整次数为偶数次时，thr_i采用原先计算的90％，即降低10％，thr_i调整后的结果代入步骤S6中，重新计算；

S8：

将每个电机油门thr_motor_i发送到每个电机：完成各个动作优先级的分配。

本申请的优点在于：

(1)本申请的基础构思是：当一个机臂或电机的动力发生故障，例如2号电机，则由于该动力缺失：横滚的量无法满足，飞机会向左倾斜，为了飞机保持平衡，因此飞控系统输出的roll_thr就会增大；2号电机还提供顺时针旋转的力矩，由于缺失，飞机就会逆时针转头，为了保持当前机头方向，飞控系统就会增大yaw_thr；2号电机还提供向上的拉力，动力缺失还会引起飞机高度下降，为了保持高度，飞控系统就会增大alt_thr。结果就是，三个输出量都增大了，如果还按照正常的分配算法，无人机就会都无法满足，发生翻转，从而导致“炸机”。

对于上述问题，本申请将无人机提供的动力，分为保证高度、横滚、俯仰、方位的动作，动作按优先级划分：

1)将姿态中，横滚、俯仰的优先级保证最高；

2)航向、高度优先级次之，并设置航向允许的最小值；

3)将该原则分配的每个电机量，在油门的约束下给定到每个电机。

(2)本申请提出的方法是通用的，即对于任何一个电机发生故障都可以采用这种方式。因为：不同的电机发生故障，飞控系统算法输出的横滚油门、俯仰油门、航向油门、高度油门是不同的。

(3)本申请的核心在于：本申请提出的一种六旋翼无人机动力缺失保护方法，特别的，给出了具体如图4所示的各个参数计算流程步骤。

附图说明

下面结合附图中的实施例对本发明作进一步的详细说明，但并不构成对本发明的任何限制。

图1是六旋翼无人机的浆叶编号示意图。

图2是实施例1的六旋翼无人机动力缺失保护系统的设计图。

图3是实施例1的六旋翼无人机动力缺失保护方法流程图。

图4是实施例1的六旋翼无人机动力缺失保护方法的各个参数计算流程图。

具体实施方式

实施例1，无人机飞行控制系统(即飞控系统)是无人机的“大脑”，无人机在空中的稳定的飞行，控制分为姿态控制与高度保持；其中，姿态的控制又分为：①俯仰姿态的控制，即飞机抬头、低头的动作；②横滚姿态的控制，即飞机的左倾、右倾动作；③飞机的航向控制，即飞机头的左右转动。

这一系列动作，并不都会导致飞机直接“炸机”---坠毁。例如：当飞机的航向失控时，飞机只是头部在左右摆动，甚至转圈，但并不会发生“炸机”；当飞机高度无法保持时，飞机会慢慢降落，也并不会发生“炸机”。

为解决现有技术中提及的“无人机在缺失一个动力或多个动力”时产生的问题，本发明采取如下基础构思：

将无人机提供的动力，分为保证高度、横滚、俯仰、方位的动作，动作按优先级划分：

1)将姿态中，横滚、俯仰的优先级保证最高；

2)航向、高度优先级次之，并设置航向允许的最小值；

3)将该原则分配的每个电机量，在油门的约束下给定到每个电机。

无人机的动力来源是桨叶的转动，转动越快提供的拉力越大；桨叶受电调(即电子调速器)控制，电调有一定的输入区间，也就限制了每个桨叶能提供的拉力大小，设每个电调的油门控制区间为[throttle_min,throttle_max],一般这个范围为[0,1000]。

根据无人机各个机臂所处的位置不同，相同的油门，对高度的作用相同，而对横滚、俯仰、航向提供的作用并不同。

定义，飞机绕X轴旋转为横滚，绕Y轴旋转为俯仰，其中Z轴与XY符合右手定则，所以，对于航向，正方向为逆时针方向；

X轴与Y轴垂直；

六旋翼无人机包括：6个浆叶，按照逆时针编号为：0、1、2、3、4、5号浆叶；0号浆叶与1号浆叶关于X轴对称，2号浆叶与5号浆叶关于X轴对称，3号浆叶与4号浆叶关于X轴对称；1号浆叶与3号浆叶关于Y轴对称，0号浆叶与4号浆叶关于Y轴对称；

其中，0、1号桨叶提供正的俯仰力矩，3、4号桨叶提供负的俯仰力矩；1、2、3号桨叶提供正的横滚力矩，0、4、5号桨叶提供负的横滚力矩；无人机通过桨叶的反扭矩提供航向的力矩，0、2、4号桨叶提供顺时针的航向力矩，而1、3、5号桨叶提供逆时针的航向力矩。

下面对炸机的机理进行分析：

无人机的动力分配，遵循效率最优原则：例如当无人机需要俯仰的动作时候，设所需俯仰力矩为pitch_thr；根据各个桨叶的布局，需要0、1各提供，pitch_thr*cos(30°)≈0.866*pitch_thr的油门量，而3、4各提供-0.866*pitch_thr的油门量；其它动作的动力需求类似。

但是每一个电机的油门都是固定的[throttle_min,throttle_max]；动力的分配原则是，每一个桨叶所分配的动力都需要在油门范围内。

无人机的飞控系统，可分别输出对于高度方向控制的油门量alt_thr、对于横滚姿态的油门量roll_thr、对于俯仰姿态的油门量pitch_thr、对于航向的油门量yaw_thr。

当各个动力机构都正常的情况下，飞控系统输出的每个动作所需求的油门量，都可以得到满足。

但是当一个机臂或电机的动力发生故障，例如2号电机，则由于该动力缺失：横滚的量无法满足，飞机会向左倾斜，为了飞机保持平衡，因此飞控系统输出的roll_thr就会增大；2号电机还提供顺时针旋转的力矩，由于缺失，飞机就会逆时针转头，为了保持当前机头方向，飞控系统就会增大yaw_thr；2号电机还提供向上的拉力，动力缺失还会引起飞机高度下降，为了保持高度，飞控系统就会增大alt_thr。结果就是，三个输出量都增大了，如果还按照正常的分配算法，无人机就会都无法满足，发生翻转，从而导致“炸机”。

为了防止这种情况发生，结合四种动作对“炸机”的影响，本申请采取以下动力分配的算法，通过丧失航向的保持、高度的保持，达到飞机的安全。

一种六旋翼无人机动力缺失保护方法，包括以下步骤：

S1：设置飞机的最佳油门为每个电机的中间油门(通过控制油门舵机，能够实现控制油门量为最佳油门)(特别需要的是，这个油门值叠加上对飞机进行实际控制的姿态、高度油门后，直接输出给无人机的电调以便控制电机旋转；损坏的电机也需要分配，只是：1如果该电机彻底损坏，则不起作用，2如果该电机只是卡转一定时间，当恢复后，可以马上恢复正常工作)：

best_thr_i＝(thr_min_i+thr_max_i)/2

thr_min_i表示第i号浆叶电机的最大油门值；

thr_max_i表示第i号浆叶电机的最小油门值；

best_thr_i表示第i号浆叶电机的最佳油门值；

其中，i＝0、1、2、3、4、5；

特别的，如果对于同一台无人机，选用6个电机采用同样的品牌、并且同样品牌的电调来控制该无人机上电机的6个电机，即对于六个电机而言，thr_min_i、thr_max_i、best_thr_i均为一个定值(即thr_max_0＝thr_max_1＝thr_max_2＝thr_max_3＝thr_max_4＝thr_max_5，thr_min_0＝thr_min_1＝thr_min_2＝thr_min_3＝thr_min_4＝thr_min_5)。

S2：

按照上述分配原则，分别把横滚油门、俯仰油门分配到每一个电机:

thr_0＝best_thr_0+picth_thr×cos(-30°)+roll_thr×sin(-30°)

thr_1＝best_thr_1+pitch_thr×cos30°+roll_thr×sin30°

thr_2＝best_thr_2+pitch_thr×cos90°+roll_thr×sin90°

thr_3＝best_thr_3+pitch_thr×cos150°+roll_thr×sin150°

thr_4＝best_thr_4+pitch_thr×cos(-150°)+roll_thr×sin(-150°)

thr_5＝best_thr_5+pitch_thr×cos(-90°)+roll_thr×sin(-90°)

thr_0表示：第0号浆叶电机的油门输出值；

thr_1表示：第1号浆叶电机的油门输出值；

thr_2表示：第2号浆叶电机的油门输出值；

thr_3表示：第3号浆叶电机的油门输出值；

thr_4表示：第4号浆叶电机的油门输出值；

thr_5表示：第5号浆叶电机的油门输出值；

其中，Pitch_thr表示：飞控系统计算出来用于保持无人机俯仰动作的油门量；Roll_thr表示：飞控系统计算出来用于保持无人机横滚动作的油门量，即Pitch_thr、Roll_thr两个数据由飞控系统给出(其计算属于本领域的现有技术，在此不再熬述)。

S3：

S3-1,求取所有电机中的油门最大值motor_thr_max和最小值motor_thr_min，即：

所有电机中的油门最大值motor_thr_max为thr_0、thr1、thr2、thr3、thr4、thr5的最大值；

所有电机中的油门最小值motor_thr_min为thr_0、thr1、thr2、thr3、thr4、thr5的最小值；

S3-2,计算还剩下的油门空间：

首先，计算：thr_max、thr_min；

thr_max是thr_max_0、thr_max_1、thr_max_2、thr_max_3、thr_max_4、thr_max_5的最大值；

thr_min是thr_min_0、thr_min_1、thr_min_2、thr_min_3、thr_min_4、thr_min_5的最小值；

然后，然后计算参数：thr_adj_max、thr_adj_min；

thr_adj_max＝thr_max-motor_thr_max；

thr_adj_min＝thr_min-motor_thr_min；

thr_adj_max表示为了不使每个电机达到上限还剩余的油门量；

thr_adj_min表示为了不使每个电机达到下限还剩的油门量。

S3的思想在于：对飞控系统计算出来的量，进行电机分配的原则是保证：分配后的每个电机油门都在合理范围内(电调可执行油门)；即经过上述计算，即把横滚油门、俯仰油门加到电机的最佳油门上后，为了保证每个电机不达到上限，还剩余多少油门量。

S4：

比较油门量与剩下的油门空间，并确定还剩的油门区间：

当飞机在升高时，即alt_thr大于0时；

alt_adj等于min(alt_thr,thr_adj_max)，即取为alt_thr、thr_adj_max的较小值；

当飞机在升高时，即alt_thr小于0时；

alt_adj＝max(alt_thr,thr_adj_min)，即取为alt_thr,thr_adj_min的较大值。

S4亦可表达为：

其中，alt_thr表示是飞控系统计算得到保证飞机做高度方向动作的油门(即值由飞控系统根据飞行高度自动计算，其属于现有技术)，例如：飞机现在要升高，则飞控系统计算出来为正；反之，如果要降高，则为负值。

其中，alt_adj表示油门适应值参数(即alt_adjust值)。

S5：

S5-1,计算航向油门剩余量yaw_allowed，并分配航向油门：

yaw_allowed＝min(min(thr_adj_max,-thr_adj_min)-alt_adj,yaw_thr)

即“thr_adj_max,-thr_adj_min的较小值减去-alt_adj的值”与“yaw_thr”的较小值赋值给yaw_allowed；

yaw_allowed表示分配完横滚油门、俯仰油门、高度油门后，能用于控制飞机机头方向(注意不是航线方向)的油门剩余量，其采用上式计算得到；

yaw_thr表示航向油门(该数值有飞控系统计算得到，属于现有技术，在此不再熬述)。S5-2,

限制航向油门分配的最小值为100，即防止无人机旋转太快，在落地时候损坏设备：

若S5-1中的yaw_allowed计算结果小于100，则yaw_allowed取值为100。

若S5-1中的yaw_allowed计算结果大于100，则yaw_allowed不变。

S6：

S6-1,将alt_adj与yaw_allowed分别加到S2得到的每个电机上，即：

thr_motor_i＝thr_i+alt_adj+yaw_allowed；

其中，i＝0、1、2、3、4、5。

其中，thr_motor_i表示第i号浆叶电机的油门；

S6-2,计算6个电机油门中的最大值last_thr_max，最小值last_thr_min

last_thr_max为thr_motor_0、thr_motor_1、thr_motor_2、thr_motor_3、thr_motor_4、thr_motor_5的最大值。

last_thr_min表示thr_motor_0、thr_motor_1、thr_motor_2、thr_motor_3、thr_motor_4、thr_motor_5的最小值。

S7：判断每个电机是否在正常油门范围内：

即thr_motor_i是否在：区间[thr_min_i，thr_max_i]之间：

当thr_motor_i在区间[thr_min_i，thr_max_i]时，thr_motor_i不变，转入步骤S8；

当thr_motor_i大于thr_max_i，或者last_thr_min小于thr_min_i时，即如果有电机超过限幅；则进行以下操作：

调整次数为奇数次时，alt_adj采用原先计算的90％，即降低10％；alt_adj调整后的结果代入步骤S6中，重新计算；

调整次数为偶数次时，thr_i采用原先计算的90％，即降低10％，thr_i调整后的结果代入步骤S6中，重新计算；

上式过程亦可表达为：

S8：

将每个电机油门thr_motor_i发送到每个电机：完成各个动作优先级的分配。

以上所举实施例为本发明的较佳实施方式，仅用来方便说明本发明，并非对本发明作任何形式上的限制，任何所属技术领域中具有通常知识者，若在不脱离本发明所提技术特征的范围内，利用本发明所揭示技术内容所作出局部更动或修饰的等效实施例，并且未脱离本发明的技术特征内容，均仍属于本发明技术特征的范围内。