CN112596535A - 无人机多传感器三余度飞行控制系统、方法及存储介质 - Google Patents

Abstract

本发明属于无人机控制技术领域，公开了一种无人机多传感器三余度飞行控制系统、方法及存储介质，数据采集模块利用三组冗余传感器采集飞机数据；预处理模块对传感器原始数据进行初步解算；余度管理模块对传感器采集的数据进行投票选择；飞行控制模块融合传感器数据进行无人机运动状态估计和控制。本发明通过余度管理算法，对三组IMU和磁力计进行有效的投票机制，选择出最优的传感器组，无人机运动状态的估计和控制。本发明通过互补滤波算法对三组传感器的原始数据进行预处理，解决了多组传感器原始数据之间难以有效比较，无法检测出传感器软故障的问题，提高了无人机飞行控制系统的稳定性和可靠性。

Description

无人机多传感器三余度飞行控制系统、方法及存储介质

技术领域

本发明属于无人机控制技术领域，尤其涉及一种无人机多传感器三余度飞行控制系统、方法及存储介质。

背景技术

目前，无人机是利用无线电遥控设备和自备的程序控制装置操纵的不载人飞机，其中飞行控制系统是无人机中最核心的部分。

无人机飞行控制系统通过收集各个传感器的信息来对飞机的位置、速度和姿态等运动状态信息进行滤波和估计，同时利用这些信息对无人机进行控制，从而实现飞机的手动和自动飞行，完成预设的各项任务。

其中，惯性测量单元(Inertial Measurement Unit，IMU)和磁力计是无人机飞行控制系统中非常重要的传感器设备。IMU中包含一个三轴的加速度计和一个三轴的陀螺仪，加速度计用于检测无人机在载体坐标系中的三轴的加速度，陀螺仪用于检测无人机相对于导航坐标系的角速度信息。磁力计用于检测飞机附近的地磁场方向，从而获得飞机的航向信息。通过对IMU和磁力计信息的融合和处理，无人机飞控可以解算出飞机的姿态信息，对于飞机进行飞行控制至关重要。一旦其中某个传感器出现故障，无人机将失去控制，无法正常工作。

现有的无人机为了避免传感器故障导致的飞机失控，通常会对IMU和磁力计进行多余度的冗余备份，一旦出现传感器故障，则切换到备份，提高系统的稳定性。

一般的多冗余系统，通常采用同类型传感器原始数据相互对比的方式，投票选择出没有故障的器件参与后续的计算。但是作为冗余的主备份传感器，通常使用不同型号的器件。他们有时具有不同的数据输出频率。通常他们也位于同一总线上，或缺少数据同步的机制。所以对于传感器的原始数据，投票模块难以做出有效的比较。仅能够根据一些简单的故障指标选择最优传感器，检测出如数据中断等类型的“硬故障”。但此类故障通常很少发生，更多的则是如信号混叠、削波和漂移的“软故障”，通过对比原始数据的方法难以进行诊断。

通过上述分析，现有技术存在的问题及缺陷为：传感器原始数据难以进行有效比较，且提高多余度传感器投票算法对传感器软故障检测能力不高，导致无人机飞行控制系统传感器单元不可靠。

解决以上问题及缺陷的难度为：

(1)现有小型无人机控制系统，若选用可靠性高的传感器，如机械陀螺、光纤陀螺等，成本较高，且对无人机的重量和体积产生影响。

(2)现有冗余系统选用的主备份传感器，通常使用不同型号的器件，他们有时具有不同的数据输出频率。他们之间的数据无法同步，很难对传感器原始数据进行直接有效的比较。

(3)现有多传感器冗余系统，当传感器出现如信号混叠、削波和漂移等“软故障”时，直接对比原始数据通常难以被检测出，将会导致飞机姿态的估计异常。

解决以上问题及缺陷的意义为：

在飞机整体重量和成本增加几乎可以忽略不计的前提下，提出一种具有三组传感器切换的冗余系统及其控制方法。增加预处理模块，利用AHRS算法对三组传感器首先进行预处理得到姿态解算结果。通过对比预处理的结果来判断三组传感器可能出现的故障，可以有效检测出传感器信号出现混叠、削波和漂移等“软故障”，从而提高无人机飞行的可靠性。

发明内容

针对现有技术存在的问题，本发明提供了一种无人机多传感器三余度飞行控制系统、方法及存储介质。

本发明是这样实现的，一种无人机多传感器三余度飞行控制系统，所述无人机多传感器三余度飞行控制系统包括：三组冗余传感器单元、三个预处理模块、余度管理模块和飞行状态估计模块；

三组冗余传感器单元，每组各包括一个由三轴加速度计和三轴陀螺仪组成的IMU，和一个磁力计；三轴加速度计用于检测无人机在载体坐标系中的三轴的加速度，三轴陀螺仪用于检测无人机相对于导航坐标系的角速度信息。磁力计用于检测飞机附近的地磁场方向，从而获得飞机的航向信息。

预处理模块，用于对传感器原始数据进行初步解算。该模块利用AHRS算法融合加速度计、陀螺仪和磁力计的数据，解算出飞机当前的俯仰角、横滚角和航向角等姿态信息；

余度管理模块，用于对预处理模块输出的姿态信息进行监控和判断，利用故障判断逻辑对三组传感器组的故障状态进行比较，选择出无故障最优的一组传感器组进行后续的飞行状态估计。

数据采集模块，用于利用三组冗余传感器单元采集飞机数据；

飞行控制模块，用于融合传感器数据进行无人机运动状态估计和控制。

进一步，所述三组冗余传感器单元，还包括温度检测子单元，用于进行温度检测和控制。

进一步，所述无人机多传感器三余度飞行控制系统还包括：

恒温控制模块，包括多个均匀分布于冗余传感器单元四角的加热模块；用于为数据采集模块提供热量。

本发明的另一目的在于提供一种应用于所述无人机多传感器三余度飞行控制系统的无人机多传感器三余度飞行控制方法，所述无人机多传感器三余度飞行控制方法包括：

步骤一，利用三组冗余传感器同时采集数据；通过互补滤波法对三组冗余传感器采集的数据进行预处理；

步骤二，通过冗余管理算法对预处理的结果进行监控，并基于监控的预处理结果进行传感器组调整；

步骤三，根据冗余管理算法选择最优传感器原始数据，作为飞控系统中卡尔曼滤波器的输入信息对无人机运动状态进行估计；

步骤四，判断主卡尔曼滤波器的估计结果的可用性，监测无人机的估计状态。步骤五，将预处理的结果，与主卡尔曼滤波器对姿态的估计进行对比，如果超过一定阈值，则判断主卡尔曼滤波器可能由于其他测量信息导致对无人机运动状态的估计异常，并判断主卡尔曼滤波器的估计结果的可用性和监测系统的估计状态。

进一步，步骤一中，所述通过互补滤波法对三组传感器采集的数据进行预处理包括：

获取三轴加速度计、三轴陀螺仪和三轴磁力计采集的数据，采用互补滤波器对采集到数据进行融合，计算无人机俯仰、横滚和偏航角，得到准确的三轴姿态信息。

步骤二中，所述AHRS算法是采用Madgwick法，利用三轴加速度计、三轴陀螺仪和三轴磁力计的数据，解算出无人机俯仰、横滚和偏航角的算法。Madgwick算法的本质是加权整合t时刻的陀螺仪算出的姿态

和加速度计磁场计共同算出的姿态

从而得到最终的姿态

其加权公式如下。

α₁+α₂＝1 0≤α₁≤1,0≤α₂≤1

其中，α₁和α₂是加权系数，它们是由各自的误差占总体误差的比重所决定的，误差所占的比重越小则加权系数越大。设采样时间间隔为Δt。陀螺仪的单位时间的误差β可以通过查陀螺仪的手册得到，一般是一个很小的值，所以陀螺仪的误差为βΔt。而加速度计磁场计共同算出的姿态的误差是由计算方法决定的，计算方法有梯度下降法、高斯牛顿迭代法、牛顿法、共轭梯度法等。

进一步，步骤二中，所述基于监控的预处理结果进行传感器组调整包括：

当主传感器组的数据连续N个检测时刻未输出三轴姿态信息，或连续M个检测时刻输出相同的姿态数据，则将主卡尔曼滤波器使用的传感器原始数据设置为优先级第二的传感器组；若优先级第二的传感器组的数据连续N个检测时刻未输出三轴姿态信息，或连续M个检测时刻输出相同的姿态数据，则将主卡尔曼滤波器使用的传感器原始数据设置为优先级第三的传感器组；

当主传感器组的预处理结果中，三轴姿态信息中的任一轴，与另外两组之间的差值均超过阈值X，则将主卡尔曼滤波器使用的传感器原始数据切换为优先级第二的传感器组；

当某一组传感器组的三轴姿态信息中的任一轴数据，与另外两组之间的差值均超过阈值Y，则将该组传感器组的故障次数kn加一；当某一组的传感器故障次数kn超过阈值Z，则将该组传感器组的优先级调至最低。

进一步，步骤四中，所述判断主卡尔曼滤波器的估计结果的可用性，监测无人机的估计状态包括：

将互补滤波预处理的结果与主卡尔曼滤波器对姿态的估计进行对比，当超出一定阈值时，则判断无人机运动状态的估计异常。

本发明的另一目的在于提供一种无人机，所述无人机实施权利要求1～4任意一项所述无人机多传感器三余度飞行控制方法。

本发明的另一目的在于提供一种计算机设备，所述计算机设备包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行所述无人机多传感器三余度飞行控制方法。

本发明的另一目的在于提供一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，使得所述处理器执行所述无人机多传感器三余度飞行控制方法。

结合上述的所有技术方案，本发明所具备的优点及积极效果为：本发明通过余度管理算法，对三组IMU和磁力计进行有效的投票机制，选择出最优的传感器组，无人机运动状态的估计和控制。

本发明通过互补滤波算法对三组传感器的原始数据进行预处理，解决了多组传感器原始数据之间难以有效比较，无法检测出传感器软故障的问题，提高了无人机飞行控制系统的稳定性和可靠性。

本发明利用互补滤波法对每组IMU和磁力计传感器数据进行预处理，通过传感器冗余管理算法投票选择出最优的传感器组，参与飞行控制系统的融合解算，同时利用相关信息完成对无人机的控制，提高飞行控制系统的安全性和稳定性。通过预处理算法，解决了传感器原始数据难以进行有效比较的问题，提高多余度传感器投票算法对传感器软故障的检测能力。

本发明使用互补滤波法对传感器原始数据进行预处理，根据每组传感器数据解算的姿态信息进行余度管理，解决了不同型号传感器之间数据不同步导致的原始数据无法直接对比的问题，可以有效检测出传感器数据漂移、削波等软故障，控制策略和切换由软件完成，实现简单，可靠性高。

附图说明

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

图1是本发明实施例提供的无人机多传感器三余度飞行控制系统结构示意图；

图中：1、数据采集模块；2、预处理模块；3、余度管理模块；4、飞行控制模块；5、恒温控制模块；6、三组冗余传感器单元。

图2是本发明实施例提供的恒温控制模块结构示意图。

图3是本发明实施例提供的无人机多传感器三余度飞行控制方法原理图。

图4是本发明实施例提供的无人机多传感器三余度飞行控制方法流程图。

图5是本发明实施例提供的恒温控制模块流程图。

图6是本发明实施例提供的有无加热模块时传感器温度对比图。

图7是本发明实施例提供的有无加热模块时脱落数据漂移对比图。

图8是本发明实施例提供的互补滤波法数据预处理流程图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

针对现有技术存在的问题，本发明提供了一种无人机多传感器三余度飞行控制系统及方法，下面结合附图对本发明作详细的描述。

如图1-图2所示，本发明实施例提供的无人机多传感器三余度飞行控制系统包括：

数据采集模块1，用于利用三组冗余传感器单元采集飞机数据；

预处理模块2，用于对传感器原始数据进行初步解算；

余度管理模块3，用于对传感器采集的数据进行投票选择；

飞行控制模块4，用于融合传感器数据进行无人机运动状态估计和控制；

恒温控制模块5，包括4个均匀分布于冗余传感器单元四角的加热模块；用于为数据采集模块提供热量。

三组冗余传感器单元6，每组各包括一个由三轴加速度计和三轴陀螺仪组成的IMU，和一个磁力计；三轴加速度计用于检测无人机在载体坐标系中的三轴的加速度，三轴陀螺仪用于检测无人机相对于导航坐标系的角速度信息。磁力计用于检测飞机附近的地磁场方向，从而获得飞机的航向信息。

所述三组冗余传感器单元，还包括温度检测子单元，用于进行温度检测和控制。

其中，预处理模块2，利用AHRS算法融合加速度计、陀螺仪和磁力计的数据，解算出飞机当前的俯仰角、横滚角和航向角等姿态信息；

在本发明实施例中，余度管理模块3，还用于对预处理模块输出的姿态信息进行监控和判断，利用故障判断逻辑对三组传感器组的故障状态进行比较，选择出无故障最优的一组传感器组进行后续的飞行状态估计。

如图3-图4所示，本发明实施例提供的无人机多传感器三余度飞行控制方法包括以下步骤：

S101，利用三组冗余传感器同时采集数据；通过互补滤波法对三组冗余传感器采集的数据进行预处理；

S102，通过冗余管理算法对预处理的结果进行监控，并基于监控的预处理结果进行传感器组调整；

S103，根据冗余管理算法选择最优传感器原始数据，作为飞控系统中卡尔曼滤波器的输入信息对无人机运动状态进行估计；

S104，判断主卡尔曼滤波器的估计结果的可用性，监测无人机的估计状态。

步骤S101中，本发明实施例提供的通过互补滤波法对三组传感器采集的数据进行预处理包括：

获取三轴加速度计、三轴陀螺仪和三轴磁力计采集的数据，采用互补滤波器对采集到数据进行融合，计算无人机俯仰、横滚和偏航角，得到准确的三轴姿态信息。

步骤S102中，冗余管理算法AHRS算法是采用Madgwick法，利用三轴加速度计、三轴陀螺仪和三轴磁力计的数据，解算出无人机俯仰、横滚和偏航角的算法。Madgwick算法的本质是加权整合t时刻的陀螺仪算出的姿态

和加速度计磁场计共同算出的姿态

从而得到最终的姿态

其加权公式如下。

α₁+α₂＝1 0≤α₁≤1,0≤α₂≤1

其中，α₁和α₂是加权系数，它们是由各自的误差占总体误差的比重所决定的，误差所占的比重越小则加权系数越大。设采样时间间隔为Δt。陀螺仪的单位时间的误差β可以通过查陀螺仪的手册得到，一般是一个很小的值，所以陀螺仪的误差为βΔt。而加速度计磁场计共同算出的姿态的误差是由计算方法决定的，计算方法有梯度下降法、高斯牛顿迭代法、牛顿法、共轭梯度法等。

步骤S102中，基于监控的预处理结果进行传感器组调整包括：

当主传感器组的数据连续N个检测时刻未输出三轴姿态信息，或连续M个检测时刻输出相同的姿态数据，则将主卡尔曼滤波器使用的传感器原始数据设置为优先级第二的传感器组；若优先级第二的传感器组的数据连续N个检测时刻未输出三轴姿态信息，或连续M个检测时刻输出相同的姿态数据，则将主卡尔曼滤波器使用的传感器原始数据设置为优先级第三的传感器组；

当主传感器组的预处理结果中，三轴姿态信息中的任一轴，与另外两组之间的差值均超过阈值X，则将主卡尔曼滤波器使用的传感器原始数据切换为优先级第二的传感器组；

当某一组传感器组的三轴姿态信息中的任一轴数据，与另外两组之间的差值均超过阈值Y，则将该组传感器组的故障次数kn加一；当某一组的传感器故障次数kn超过阈值Z，则将该组传感器组的优先级调至最低。

步骤S104中，本发明实施例提供的判断主卡尔曼滤波器的估计结果的可用性，监测无人机的估计状态包括：

将互补滤波预处理的结果与主卡尔曼滤波器对姿态的估计进行对比，当超出一定阈值时，则判断无人机运动状态的估计异常。

下面结合具体实施例对本发明的技术效果作进一步描述。

实施例：

本发明提供一种无人机多传感器三余度系统，主要包括一个用于融合传感器数据进行无人机运动状态估计和控制的飞行控制模块；三组采集飞机数据的冗余传感器单元，每组传感器单元包括一个由三轴加速度计和三轴陀螺仪组成的惯性测量单元(IMU)，和一个磁力计；三个对传感器原始数据进行初步解算的预处理模块；一个用于对三组传感器数据进行投票选择的余度管理模块。

本发明提供的无人机多传感器三余度系统，为提高低温环境下，数据的可靠性，设计了一套恒温系统，硬件构成如图1所示，4个加热模块给整个IMU系统单元提供热量，4个加热模块均匀分布于整个模块四角，保证了3组IMU冗余单元受热的均匀性，IMU冗余单元包含3组IMU+磁力计模块，利用IMU模块集成的温度检测单元，实现整个系统温度的检测和控制。保证了低温环境下IMU和磁力计数据的准确性。

本发明提供的一种无人机多传感器三余度飞行控制方法，其包括以下具体步骤：

步骤1：无人机上电后三组传感器同时采集数据，飞行控制软件首先使用默认优先级最高的传感器组作为主传感器组。

步骤2：通过互补滤波法对三组传感器单元进行预处理。互补滤波法是利用三轴加速度计、三轴陀螺仪和三轴磁力计的数据，解算出无人机俯仰、横滚和偏航角的算法。陀螺仪动态相应特性良好，但计算姿态时会产生累计误差，磁力计和加速度计测量姿态没有累计误差，但动态响应差。他们在频域上特性互补，采用互补滤波器对三种传感器数据进行融合，可得到准确的三轴姿态信息。互补滤波器的解算。

本发明提供的一种无人机多传感器三余度飞行控制方法，其包括以下具体步骤：

步骤1：无人机上电后三组传感器同时采集数据，飞行控制软件首先使用默认优先级最高的传感器组作为主传感器组。

步骤2：通过互补滤波法对三组传感器单元进行预处理。互补滤波法是利用三轴加速度计、三轴陀螺仪和三轴磁力计的数据，解算出无人机俯仰、横滚和偏航角的算法。陀螺仪动态相应特性良好，但计算姿态时会产生累计误差，磁力计和加速度计测量姿态没有累计误差，但动态响应差。他们在频域上特性互补，采用互补滤波器对三种传感器数据进行融合，可得到准确的三轴姿态信息。互补滤波器的解算框图为。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，都应涵盖在本发明的保护范围之内。

Claims (10)

1.一种无人机多传感器三余度飞行控制方法，其特征在于，所述无人机多传感器三余度飞行控制方法包括：

利用三组冗余传感器采集数据；通过互补滤波法对采集的所述数据进行预处理；

通过冗余管理算法对所述预处理的结果进行监控，并基于监控后的预处理结果进行传感器组调整；

根据冗余管理算法选择最优传感器原始数据，作为飞控系统中卡尔曼滤波器的输入信息对无人机运动状态进行估计；

判断所述卡尔曼滤波器的估计结果的可用性，并监测无人机的估计状态。

2.如权利要求1所述无人机多传感器三余度飞行控制方法，其特征在于，所述通过互补滤波法对三组传感器采集的数据进行预处理包括：

获取三轴加速度计、三轴陀螺仪和三轴磁力计采集的数据，采用互补滤波器对采集到数据进行融合，计算无人机俯仰、横滚和偏航角，得到准确的三轴姿态信息。

3.如权利要求1所述无人机多传感器三余度飞行控制方法，其特征在于，

所述冗余管理算法采用Madgwick法，利用三轴加速度计、三轴陀螺仪和三轴磁力计的数据，解算出无人机俯仰、横滚和偏航角；所述Madgwick算法包括：加权整合t时刻的陀螺仪算出的姿态

和加速度计磁场计共同算出的姿态

得到最终的姿态

加权公式如下。

α₁+α₂＝1 0≤α₁≤1,0≤α₂≤1

其中，α₁和α₂是加权系数，是由各自的误差占总体误差的比重决定，误差所占的比重越小则加权系数越大；设采样时间间隔为Δt；陀螺仪的单位时间的误差β通过查陀螺仪的手册得到，陀螺仪的误差为βΔt；加速度计磁场计共同算出的姿态的误差由计算方法决定，计算方法包括梯度下降法、高斯牛顿迭代法、牛顿法、共轭梯度法；

所述基于监控的预处理结果进行传感器组调整包括：

当主传感器组的数据连续N个检测时刻未输出三轴姿态信息，或连续M个检测时刻输出相同的姿态数据，则将主卡尔曼滤波器使用的传感器原始数据设置为优先级第二的传感器组；若优先级第二的传感器组的数据连续N个检测时刻未输出三轴姿态信息，或连续M个检测时刻输出相同的姿态数据，则将主卡尔曼滤波器使用的传感器原始数据设置为优先级第三的传感器组；

当主传感器组的预处理结果中，三轴姿态信息中的任一轴，与另外两组之间的差值均超过阈值X，则将主卡尔曼滤波器使用的传感器原始数据切换为优先级第二的传感器组；

当某一组传感器组的三轴姿态信息中的任一轴数据，与另外两组之间的差值均超过阈值Y，则将该组传感器组的故障次数kn加一；当某一组的传感器故障次数kn超过阈值Z，则将该组传感器组的优先级调至最低。

4.如权利要求1所述无人机多传感器三余度飞行控制方法，其特征在于，所述判断主卡尔曼滤波器的估计结果的可用性，监测无人机的估计状态包括：

将互补滤波预处理的结果与主卡尔曼滤波器对姿态的估计进行对比，当超出一定阈值时，则判断无人机运动状态的估计异常。

5.一种无人机多传感器三余度飞行控制系统，其特征在于，所述无人机多传感器三余度飞行控制系统包括：

三组冗余传感器单元，每组各包括：由三轴加速度计和三轴陀螺仪组成的IMU和磁力计；三轴加速度计用于检测无人机在载体坐标系中的三轴的加速度，三轴陀螺仪用于检测无人机相对于导航坐标系的角速度信息；磁力计用于检测飞机附近的地磁场方向，获得飞机的航向信息；

预处理模块，用于对传感器原始数据进行初步解算；利用AHRS算法融合加速度计、陀螺仪和磁力计的数据，解算出飞机当前的俯仰角、横滚角和航向角姿态信息；

余度管理模块，用于对预处理模块输出的姿态信息进行监控和判断，利用故障判断逻辑对三组传感器组的故障状态进行比较，选择出无故障最优的一组传感器组进行后续的飞行状态估计；

数据采集模块，用于利用三组冗余传感器采集飞机数据；

飞行控制模块，用于融合传感器数据进行无人机运动状态估计和控制。

6.如权利要求5所述无人机多传感器三余度飞行控制系统，其特征在于，所述冗余传感器单元还包括温度检测子单元，用于进行温度检测和控制。

7.如权利要求5所述无人机多传感器三余度飞行控制系统，其特征在于，所述无人机多传感器三余度飞行控制系统还包括：

恒温控制模块，包括多个均匀分布于冗余传感器单元四角的加热模块；用于为数据采集模块提供热量。

8.一种无人机，其特征在于，所述无人机实施权利要求1～4任意一项所述无人机多传感器三余度飞行控制方法。

9.一种计算机设备，其特征在于，所述计算机设备包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行权利要求1～4任意一项所述无人机多传感器三余度飞行控制方法。

10.一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，使得所述处理器执行权利要求1～4任意一项所述无人机多传感器三余度飞行控制方法。

Images