一种多旋翼无人机传感器故障安全飞行控制系统及方法

技术领域

本发明涉及一种多旋翼无人机传感器故障安全飞行控制系统及方法，属于无人机控制系统技术领域。

背景技术

现有的多旋翼无人机受限于成本和体积重量，普遍在传感器冗余备份上设计得较为简单，对故障安全的考虑不够周全；部分无人机仅依赖一套传感器测量飞行状态，一旦单个传感器出现故障则无法安全飞行；还有部分无人机仅仅通过简单增加同质化传感器的方式实现安全备份，没有考虑到同源传感器会受到同源干扰的情况；例如，部分无人机为了避免单个磁罗盘传感器受扰影响飞机航向测定，又在无人机上简单地再增加一个同型号的磁罗盘传感器，却忽略了磁罗盘受扰常常是因为某个特定环境下相对大范围的磁场异常，而不是磁罗盘传感器本身的原因，即使再增加一个磁罗盘，也只会两个磁罗盘数据同时出问题，依然会导致飞行器坠毁；导航数据正常是飞行控制系统正常工作的前提，一旦导航数据信息出错，飞行控制系统中飞行控制算法得到的反馈量就是错误的，必然无法实现飞行器的正确控制，只能导致坠机。

发明内容

本发明的目的是针对现有技术中存在的上述问题，提供了一种多旋翼无人机传感器故障安全飞行控制系统，同时公开一种多旋翼无人机传感器故障安全飞行控制方法；适用于装备在多旋翼无人机上，在不显著增加成本和系统复杂度（不增加高精度传感器、不增加额外处理器、不增加额外的通讯和判决电路）的情况下，实现在飞行过程中对所有传感器进行热备份，所有传感器随时参与数据融合，而非简单地切换逻辑，在传感器全部工作正常的情况下可以提高精度，而在传感器故障情况下数据可以立即平滑过渡，不存在硬切换导致的数据跳变或备份传感器启动延时问题；对于飞行安全至关重要的三轴姿态角、高度、速度和位置信息中，每个关键的飞行安全数据均由至少三个不同工作原理的传感器提供，实现了故障安全的导航数据融合算法，并通过故障诊断算法对不同传感器输出的数据进行判决，能够可靠、稳定实现多源传感器的数据融合，输出正确的导航数据信息；极大提高了整个系统工作的可靠性，避免传感器受到同源干扰；提高多旋翼无人机飞行的可靠性和安全性，稳定实现传感器故障安全。

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

一种多旋翼无人机传感器故障安全飞行控制系统，包括飞控计算机、飞行传感器组、视觉处理器和对地摄像头；所述飞行传感器组包括与飞控计算机通信连接的主IMU惯性传感器、两个备份IMU惯性传感器、磁罗盘、气压计、主GNSS接收机、备份GNSS接收机和对地高度表；

所述主IMU惯性传感器和两个备份IMU惯性传感器用于测量飞行器的运动信息；主IMU惯性传感器采用高精度IMU惯性传感器，备份IMU惯性传感器采用低精度IMU惯性传感器；

所述磁罗盘用于测量飞行器的姿态角；

所述气压计用于测量飞行器的高度；

所述主GNSS接收机和备份GNSS接收机通过卫星导航系统测量飞行器的姿态角、速度和位置信息；

所述对地高度表用于测量飞行器相对地面的高度；

所述对地摄像头与视觉处理器通信连接，视觉处理器与飞控计算机通信连接；视觉处理器处理对地摄像头的数据得到视觉数据，所述视觉数据包括飞行器相对地面的速度和位置信息；

所述飞控计算机读取飞行传感器组的数据和视觉数据来控制飞行器飞行。

进一步，所述对地摄像头与飞行器机身固定连接，对地摄像头的Y轴与飞行器机身的纵轴对齐，对地摄像头的X轴与飞行器机身的横轴对齐。

一种多旋翼无人机传感器故障安全飞行控制方法，具体如下：

所述飞控计算机读取飞行传感器组的数据和视觉数据，飞控计算机对读取到的两种数据首先经过数据融合算法计算出飞行器的飞行状态信息，所述飞行状态信息包括姿态角、高度、速度和位置；得出的飞行状态信息再经过飞行控制算法计算后通过反馈控制飞行器飞行的姿态角、高度、速度和位置。

作为本发明方法的进一步优选，所述数据融合算法包括以下步骤：

第一步，利用两个备份IMU惯性传感器对主IMU惯性传感器进行投票输出结果，具体如下：

飞控计算机分别读取每个IMU惯性传感器的数据，并对IMU惯性传感器数据进行检查；

将主IMU惯性传感器的数据与第一个备份IMU惯性传感器的数据相减，判断相减后的结果是否分布在预设的传感器零偏值范围内；若相减后的结果分布在预设的传感器零偏值范围内，则认为第一个备份IMU惯性传感器投票主IMU惯性传感器的数据有效，否则认为第一个备份IMU惯性传感器投票主IMU惯性传感器的数据无效；

将主IMU惯性传感器的数据与第二个备份IMU惯性传感器的数据相减，判断相减后的结果是否分布在预设的传感器零偏值范围内；若相减后的结果分布在预设的传感器零偏值范围内，则认为第二个备份IMU惯性传感器投票主IMU惯性传感器的数据有效，否则认为第二个备份IMU惯性传感器投票主IMU惯性传感器的数据无效；

若两个备份IMU惯性传感器均投票主IMU惯性传感器数据有效，则采用主IMU惯性传感器的数据作为输出值；

若其中一个备份IMU惯性传感器投票主IMU惯性传感器数据有效、另外一个备份IMU惯性传感器投票主IMU惯性传感器数据无效，则认为投票主IMU惯性传感器数据无效的备份IMU惯性传感器失效，继续采用主IMU惯性传感器的数据作为输出值，并报警请求返航；

若两个备份IMU惯性传感器均投票主IMU惯性传感器数据无效，则认为主IMU惯性传感器失效，采用两个备份IMU惯性传感器的数据平均值作为输出值，并报警请求返航；

第二步，使用两级滤波器进行传感器故障检测和数据融合，具体如下：前级滤波器分别将时间更新传感器与状态更新传感器进行数据融合，并进行故障诊断；故障诊断后，若状态更新传感器数据有效，则将数据有效的状态更新传感器对应的子滤波器的输出送入下一级主滤波器进行融合；若状态更新传感器数据无效，则判断状态更新传感器异常并报警，同时断开数据无效的状态更新传感器对应的子滤波器到主滤波器的输出，下一级主滤波器使用仍能正常工作的子滤波器的输出进行融合；主滤波器接收到多个子滤波器的输出后，首先统计其中有效的子滤波器数量，将有效的子滤波器的输出值按照预测方差矩阵P进行加权融合，得出数据融合的最终结果。

作为本发明方法的进一步优选，第二步中所述故障诊断的方法如下：

设工作正常时，前一个时刻子滤波器的状态变量为x，通过IMU惯性传感器数据进行时间更新后的状态变量为

作为本发明方法的进一步优选，所述传感器零偏值范围的设定方法如下：

将IMU惯性传感器所有的噪声建模为一个叠加在真值上的高斯分布，影响其分布范围即噪声大小的参数用一个方差参数σ

其中，bias

作为本发明方法的进一步优选，所述传感器零偏值范围是：

[-bias

本发明的有益之处在于：

适用于装备在多旋翼无人机上，在不显著增加成本和系统复杂度（不增加高精度传感器、不增加额外处理器、不增加额外的通讯和判决电路）的情况下，实现在飞行过程中对所有传感器进行热备份，所有传感器随时参与数据融合，而非简单地切换逻辑，在传感器全部工作正常的情况下可以提高精度，而在传感器故障情况下数据可以立即平滑过渡，不存在硬切换导致的数据跳变或备份传感器启动延时问题；对于飞行安全至关重要的三轴姿态角、高度、速度和位置信息中，每个关键的飞行安全数据均由至少三个不同工作原理的传感器提供，实现了故障安全的导航数据融合算法，并通过故障诊断算法对不同传感器输出的数据进行判决，能够可靠、稳定实现多源传感器的数据融合，输出正确的导航数据信息；极大提高了整个系统工作的可靠性，避免传感器受到同源干扰；提高多旋翼无人机飞行的可靠性和安全性，稳定实现传感器故障安全。

附图说明

图1是本发明多旋翼无人机传感器故障安全飞行控制系统的结构示意图；

图2是本发明关于水平位置和水平速度的数据融合算法流程示意图；

图3是本发明对于姿态角数据融合算法流程示意图；

图4是本发明对于高度数据融合算法流程示意图。

具体实施方式

以下结合附图和具体实施例对本发明作具体的介绍。

如图1所示，本实施例是一种多旋翼无人机传感器故障安全飞行控制系统，包括飞控计算机、飞行传感器组、视觉处理器和对地摄像头；飞行传感器组包括与飞控计算机通信连接的主IMU惯性传感器、两个备份IMU惯性传感器、磁罗盘、气压计、主GNSS接收机、备份GNSS接收机和对地高度表；

主IMU惯性传感器和两个备份IMU惯性传感器用于测量飞行器的运动信息；主IMU惯性传感器采用高精度IMU惯性传感器，备份IMU惯性传感器采用低精度IMU惯性传感器；

磁罗盘用于测量飞行器的姿态角；

气压计用于测量飞行器的高度；

主GNSS接收机和备份GNSS接收机通过卫星导航系统测量飞行器的姿态角、速度和位置信息；

对地高度表用于测量飞行器相对地面的高度；

对地摄像头与视觉处理器通信连接，视觉处理器与飞控计算机通信连接；视觉处理器处理对地摄像头的数据得到视觉数据，视觉数据包括飞行器相对地面的速度和位置信息；

飞控计算机读取飞行传感器组的数据和视觉数据来控制飞行器飞行。

本实施例中，对地摄像头与飞行器机身固定连接，对地摄像头的Y轴与飞行器机身的纵轴对齐，对地摄像头的X轴与飞行器机身的横轴对齐。

基于上述实施例所述系统的一种多旋翼无人机传感器故障安全飞行控制方法，具体如下：

飞控计算机读取飞行传感器组的数据和视觉数据，飞控计算机对读取到的两种数据首先经过数据融合算法计算出飞行器的飞行状态信息，飞行状态信息包括姿态角、高度、速度和位置；得出的飞行状态信息再经过飞行控制算法计算后通过反馈控制飞行器飞行的姿态角、高度、速度和位置。

其中，数据融合算法包括以下步骤：

第一步，利用两个备份IMU惯性传感器对主IMU惯性传感器进行投票输出结果，具体如下：

飞控计算机分别读取每个IMU惯性传感器的数据，并对IMU惯性传感器数据进行检查；

将主IMU惯性传感器的数据与第一个备份IMU惯性传感器的数据相减，判断相减后的结果是否分布在预设的传感器零偏值范围内；若相减后的结果分布在预设的传感器零偏值范围内，则认为第一个备份IMU惯性传感器投票主IMU惯性传感器的数据有效，否则认为第一个备份IMU惯性传感器投票主IMU惯性传感器的数据无效；

将主IMU惯性传感器的数据与第二个备份IMU惯性传感器的数据相减，判断相减后的结果是否分布在预设的传感器零偏值范围内；若相减后的结果分布在预设的传感器零偏值范围内，则认为第二个备份IMU惯性传感器投票主IMU惯性传感器的数据有效，否则认为第二个备份IMU惯性传感器投票主IMU惯性传感器的数据无效；

若两个备份IMU惯性传感器均投票主IMU惯性传感器数据有效，则采用主IMU惯性传感器的数据作为输出值；

若其中一个备份IMU惯性传感器投票主IMU惯性传感器数据有效、另外一个备份IMU惯性传感器投票主IMU惯性传感器数据无效，则认为投票主IMU惯性传感器数据无效的备份IMU惯性传感器失效，继续采用主IMU惯性传感器的数据作为输出值，并报警请求返航；

若两个备份IMU惯性传感器均投票主IMU惯性传感器数据无效，则认为主IMU惯性传感器失效，采用两个备份IMU惯性传感器的数据平均值作为输出值，并报警请求返航；

第二步，使用两级滤波器进行传感器故障检测和数据融合，具体如下：前级滤波器分别将时间更新传感器与状态更新传感器进行数据融合，并进行故障诊断；故障诊断后，若状态更新传感器数据有效，则将数据有效的状态更新传感器对应的子滤波器的输出送入下一级主滤波器进行融合；若状态更新传感器数据无效，则判断状态更新传感器异常并报警，同时断开数据无效的状态更新传感器对应的子滤波器到主滤波器的输出，下一级主滤波器使用仍能正常工作的子滤波器的输出进行融合；主滤波器接收到多个子滤波器的输出后，首先统计其中有效的子滤波器数量，将有效的子滤波器的输出值按照预测方差矩阵P进行加权融合，得出数据融合的最终结果。

第二步中所述故障诊断的方法如下：

设工作正常时，前一个时刻子滤波器的状态变量为x，通过IMU惯性传感器数据进行时间更新后的状态变量为

传感器零偏值范围的设定方法如下：

将IMU惯性传感器所有的噪声建模为一个叠加在真值上的高斯分布，影响其分布范围即噪声大小的参数用一个方差参数σ

其中，bias

本实施例中，传感器零偏值范围是[-bias

每个IMU惯性传感器均可输出三轴角速度和三轴加速度数据，影响IMU惯性传感器输出值的关键参数为传感器零偏和噪声，将IMU惯性传感器所有的噪声建模为一个叠加在真值上的高斯分布，影响其分布范围即噪声大小的参数可用一个方差参数σ

其中，bias

IMU惯性传感器的零偏参数和噪声参数，可通过IMU惯性传感器数据手册或试验获取；高斯分布有99%的概率会落在零偏的正负3σ范围以内，故在IMU惯性传感器全部工作正常、输出数据符合理论高斯模型时，主IMU惯性传感器与备份IMU惯性传感器的输出值相减的值有99%概率分布在[-bias

如图2所示，关于水平位置和水平速度的数据融合算法；

本实施算法中，两个备份IMU惯性传感器均投票主IMU惯性传感器数据有效，因此采用主IMU惯性传感器的数据作为输出值；时间更新传感器指IMU惯性传感器中的加速度计，状态更新传感器包括主GNSS接收机输出的水平位置和速度信息、备份GNSS接收机输出的水平位置和速度信息、视觉数据；使用两级滤波器进行传感器故障检测和数据融合，其中子滤波器基本框架仍使用卡尔曼滤波器，相关基础概念不做赘述；具体如下：

设飞行器水平位置和水平速度的状态变量为：

P

将k时刻，第i个子滤波器的状态变量记为

则第一个卡尔曼滤波时间更新方程为：

其中A1为状态转移矩阵：

预测方差时间更新方程为：

其中P1为预测方差，Q1为滤波参数，T为矩阵转置运算；

卡尔曼滤波器状态更新方程如下：

其中H1为观测矩阵，R是滤波参数，即状态变量x到状态更新传感器实际值z的转移矩阵，对于主GNSS接收机，其观测矩阵为：

将残差定义为状态更新传感器实际值和从时间更新推测出的观测值之差，即：

根据res值的大小和合理分布范围，对其值进行判断，如在预设的传感器噪声值范围内则认为状态更新传感器数据有效，采纳该值，按照下式继续计算，并将数据有效的状态更新传感器对应的子滤波器的输出接入到主滤波器；否则认为状态更新传感器数据无效，不予采纳，报告故障，并断开数据无效的状态更新传感器对应的子滤波器到主滤波器的输出；

主滤波器接收到多个子滤波器的输出后，首先统计其中有效的子滤波器数量，将有效的子滤波器的输出值按照预测方差矩阵P进行加权融合，若三个子滤波器均可用，则按照下式融合作为最终输出：

得出数据融合的最终结果。

同理，对于姿态角数据融合算法，如图3所示，时间更新传感器指IMU惯性传感器中的陀螺仪，状态更新传感器包括IMU惯性传感器中的加速度计、磁罗盘、主GNSS接收机输出的双天线航向、视觉数据；

对于高度数据融合算法，如图4所示，时间更新传感器指IMU惯性传感器中的加速度计，状态更新传感器包括气压计、对地高度表、RTK模式下主GNSS接收机输出的高度信息、主GNSS接收机输出的垂直速度信息、备份GNSS接收机输出的垂直速度信息。

本发明适用于装备在多旋翼无人机上，在不显著增加成本和系统复杂度（不增加高精度传感器、不增加额外处理器、不增加额外的通讯和判决电路）的情况下，实现在飞行过程中对所有传感器进行热备份，所有传感器随时参与数据融合，而非简单地切换逻辑，在传感器全部工作正常的情况下可以提高精度，而在传感器故障情况下数据可以立即平滑过渡，不存在硬切换导致的数据跳变或备份传感器启动延时问题；对于飞行安全至关重要的三轴姿态角、高度、速度和位置信息中，每个关键的飞行安全数据均由至少三个不同工作原理的传感器提供，实现了故障安全的导航数据融合算法，并通过故障诊断算法对不同传感器输出的数据进行判决，能够可靠、稳定实现多源传感器的数据融合，输出正确的导航数据信息；极大提高了整个系统工作的可靠性，避免传感器受到同源干扰；提高多旋翼无人机飞行的可靠性和安全性，稳定实现传感器故障安全。

以上显示和描述了本发明的基本原理、主要特征和优点；本行业的技术人员应该了解，上述实施例不以任何形式限制本发明，凡采用等同替换或等效变换的方式所获得的技术方案，均落在本发明的保护范围内。