一种无人机水平姿态角的故障诊断方法与流程

本发明涉及无人机飞行控制系统领域，具体地说是一种无人机水平姿态角的故障诊断方法。

背景技术：

无人机姿态角是飞行过程中需要重点关注的机体状态，水平姿态角(包括横滚角和俯仰角)更是无人机空中稳定控制的重要参数。因此现有飞行控制系统测量姿态角的关键传感器惯性测量单元，均通过配备硬件冗余来提高可靠性。现行工业级飞行控制器一般具备三余度或更高级别的硬件冗余，通过多余度硬件的投票表决机制，可以有效进行传感器的状态监测和各类故障的诊断定位。但多余度硬件冗余的系统成本较高，且对无人机安装空间和载重能力有更高要求，不适用于微型和小型无人机。

现行消费级微小型无人机的飞行控制器一般只具备传感器硬件双冗余，可以对传感器诸如初始化失败、数据中断、数据持续不变等严重故障进行诊断和隔离，但由于双数据源间相互比较缺乏基准的问题，无法对传感器非完全失效故障引起的数据异常进行准确定位。

技术实现要素：

针对现有技术的不足，本发明提供一种无人机水平姿态角的故障诊断方法，该方法只需硬件双冗余，通过解析冗余交叉组合生成四路水平姿态角信息，可以完成现行三余度及以上硬件冗余才可进行的投票表决故障诊断机制。

本发明为实现上述目的所采用的技术方案是：

一种无人机水平姿态角的故障诊断方法，包括以下过程：

步骤1：获取来自水平姿态角硬件双冗余传感器的两组无人机水平姿态角传感器信息，每组无人机水平姿态角传感器信息包括陀螺仪信息和加速度计信息；

步骤2：将两组传感器信息交叉组合，生成四路陀螺仪和加速度计的组合信息；

步骤3：通过数据融合算法将四路组合信息分别解算为四余度水平姿态角信息；

步骤4：通过运动超限判断和多余度投票表决，进行四余度水平姿态角的信息诊断；

步骤5：根据四余度水平姿态角信息诊断结果，进行水平姿态角传感器的故障诊断。

所述无人机水平姿态角包括横滚角和俯仰角。

所述水平姿态角传感器为惯性测量单元，即imu，每个imu中包括一个三轴陀螺仪和一个三轴加速度计。

所述四路陀螺仪和加速度计的组合信息包括：

第一imu的陀螺仪信息和加速度计信息组合、第一imu的陀螺仪信息和第二imu的加速计信息组合、第二imu的陀螺仪信息和第一imu的加速计信息组合、第二imu的陀螺仪信息和加速计信息组合。

所述数据融合算法，包括：

对陀螺仪角速度信息积分，得到水平姿态角的预测值，加速度信息经过补偿得到机体坐标系下的重力加速度向量，再通过该重力加速度向量在惯性坐标系下的投影关系得到水平姿态角的量测值，将预测值和量测值加权融合得到水平姿态角的输出值。

加速度信息补偿包括：

首先补偿由机体角运动和线运动引起的哥氏加速度得到机体实际加速度，再补偿由速度差分得到的机体运动加速度得到机体坐标系下的重力加速度。

所述四余度水平姿态角的信息诊断，包括：

依据无人机水平姿态角的运动能力极限，分别对四余度水平姿态角进行超限判断：如果某一路水平姿态角的横滚角变化率大于无人机横滚运动能力的阈值，则判定该横滚角运动超限；某一路水平姿态角的俯仰角变化率大于无人机横滚运动能力的阈值，则判定该俯仰角运动超限；如果横滚角或俯仰角超限，则判定该路水平姿态角超限；

如果四余度水平姿态角均未超限，则对四余度水平姿态角进行投票表决。

所述对四余度水平姿态角进行投票表决，包括：

依次对四余度水平姿态角信息进行两两作差得到六组差值，如果某组差值大于比较门限值，可以判定作差的两路水平姿态角中至少有一路存在异常；根据六组差值与门限值的比较结果，分析得出四余度水平姿态角是否存在异常。

所述水平姿态角传感器的故障诊断，包括：

依据四余度水平姿态角超限判断和投票表决的结果对传感器进行故障诊断，定位传感器的单通道或双通道故障，并输出当前计算周期水平姿态角的最终值。

本发明具有以下有益效果及优点：

1.本发明在无人机水平姿态角传感器硬件双冗余的基础上，通过解析冗余交叉组合生成四余度水平姿态信息的方法，可以完成现行多余度(三余度及以上)硬件冗余才可进行的投票表决故障诊断机制，有效诊断更广泛的传感器故障；

2.本发明优化现行飞行控制系统普遍采用的单纯硬件冗余故障诊断方法，简化系统硬件结构，减少对安装空间和载重能力的要求，降低系统成本；

3.本发明为安装空间和载重能力有限的微小型无人机提供了一种更加全面和有效的水平姿态角传感器故障诊断方法，提高了飞行控制系统的可靠性。

附图说明

图1是本发明的方法流程图；

图2是本发明的数据融合的流程示意图；

图3是本发明的传感器故障诊断流程图。

具体实施方式

下面结合附图及实施例对本发明做进一步的详细说明。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图对本发明的具体实施方式做详细的说明。在下面的描述中阐述了很多具体细节以便于充分理解本发明。但本发明能够以很多不同于在此描述的其他方式来实施，本领域技术人员可以在不违背发明内涵的情况下做类似改进，因此本发明不受下面公开的具体实施的限制。

除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本发明。

如图1所示为本发明的方法流程图，本发明提出的一种无人机水平姿态角的故障诊断方法，具体步骤如下：

步骤一：通过飞行控制器读取来自硬件双冗余的水平姿态角传感器信息。无人机水平姿态角包括横滚角和俯仰角，水平姿态角传感器是指惯性测量单元(imu)，水平姿态角传感器信息包括陀螺仪(gyro)信息和加速度计(acc)信息。

具体例为：飞行控制器的核心处理器通过spi通讯方式以100hz的频率读取imu1和imu2的传感器信息，分别记为gyro1、acc1、gyro2、acc2。

步骤二：将步骤一中得到的传感器信息通过解析冗余两两交叉组合，生成四路陀螺仪和加速度计的组合信息。

具体例为：第一路组合信息为gyro1+acc1，第二路组合信息为gyro1+acc2，第三路组合信息为gyro2+acc1，第四路组合信息为gyro2+acc2。

步骤三：通过相应的数据融合算法将步骤二中得到的四路组合信息分别融合为四余度水平姿态角信息。

具体例为：如图2所示，以第一路组合信息gyro1+acc1得到水平姿态角1为例，首先对本计算周期读取到的角速度值gyro1进行积分，再结合上一计算周期最终的姿态角输出值，得到当前计算周期水平姿态角的预测值。

加速度信息acc1为无人机的综合加速度，通过机载全球定位系统(gps)可以测得无人机的线运动速度v1。角速度向量gyro1叉乘线速度向量v1计算得到由机体角运动和线运动引起的哥氏加速度，综合加速度减去哥氏加速度补偿得到机体的实际加速度。通过线速度v1差分可得无人机的运动加速度，实际加速度减去运动加速度补偿可得机体坐标系下的重力加速度。通过机体坐标系下的重力加速度在惯性坐标系下的投影关系可得水平姿态角的量测值。

通过诸如扩展卡尔曼滤波(ekf)算法，将水平姿态角的预测值和量测值加权融合，得到水平姿态角1。其余三路组合信息得到水平姿态角2～4的过程同理。

步骤四：通过运动超限判断和多余度投票表决，进行四余度水平姿态角的信息诊断。诊断流程为：依据无人机水平姿态角的运动能力极限，分别对四余度水平姿态角进行超限判断；若均未超限，则对四余度水平姿态角进行投票表决。

具体例为：计算周期为0.02s，无人机在正常飞行过程中水平姿态角的变化率为0～15°/s，运动极限不超过20°/s，即相临两个计算周期内，水平姿态角变化不会超过20×0.02＝0.4°，若超过此门限值可以认为该水平姿态角的数据源传感器(陀螺仪或加速度计)存在故障。记δ1＝|上一计算周期水平姿态角最终输出值-水平姿态角1|，若δ1>门限值0.4°，则认为水平姿态角1超限。水平姿态角2～4超限判断同理。

若水平姿态角1～4均未超限，则对四余度水平姿态角进行投票表决。投票表决机制为：对四余度水平姿态角信息进行两两作差，分别记为：

δ12＝|水平姿态角1-水平姿态角2|；

δ13＝|水平姿态角1-水平姿态角3|；

δ14＝|水平姿态角1-水平姿态角4|；

δ23＝|水平姿态角2-水平姿态角3|；

δ24＝|水平姿态角2-水平姿态角4|；

δ34＝|水平姿态角3-水平姿态角4|；

差值门限值取为0.05°，差值大于门限值可以认为作差的两路水平姿态角中至少有一路存在异常。进一步地，异常水平姿态角的数据源传感器(陀螺仪或加速度计)存在故障。

如水平姿态角1～4的横滚角分别为3.02°、2.85°、3.03°、3.18°,则计算得δ12＝0.17°，δ13＝0.01°，δ14＝0.16°，δ23＝0.18°，δ24＝0.33°，δ34＝0.15°。由δ13小于差值门限值，δ12、δ14、δ23、δ24、δ34均大于差值门限值，可知水平姿态角1和3正常，水平姿态角2和4异常。

步骤五：根据步骤四的信息诊断结果，结合步骤二中所述的信息组合方式，进行传感器的故障诊断，并依据故障诊断结果计算水平姿态角的最终输出值。

具体例为：如图3所示，依据超限判断结果和投票表决结果对传感器进行故障诊断，并对本计算周期水平姿态角的最终输出值进行计算。图3中结论1～10依次为：

结论1：imu1的陀螺仪传感器(gyro1)和加速度计传感器(acc1)，以及imu2的陀螺仪传感器(gyro2)和加速度计传感器(acc2)均存在故障，当前计算周期水平姿态角无有效输出值；

结论2：imu1的陀螺仪传感器(gyro1)和加速度计传感器(acc1)存在故障，当前计算周期水平姿态角最终输出值为水平姿态角4；

结论3：imu1的陀螺仪传感器(gyro1)和imu2的加速度计传感器(acc2)存在故障，当前计算周期水平姿态角最终输出值为水平姿态角3；

结论4：imu1的陀螺仪传感器(gyro1)存在故障，当前计算周期水平姿态角最终输出值为(水平姿态角3+水平姿态角4)/2；

结论5：imu1的加速度计传感器(acc1)和imu2的陀螺仪传感器(gyro2)存在故障，当前计算周期水平姿态角最终输出值为水平姿态角2；

结论6：imu1的加速度计传感器(acc1)存在故障，当前计算周期水平姿态角最终输出值为(水平姿态角2+水平姿态角4)/2；

结论7：imu2的陀螺仪传感器(gyro2)和加速度计传感器(acc2)存在故障，当前计算周期水平姿态角最终输出值为水平姿态角1；

结论8：imu2的加速度计传感器(acc2)存在故障，当前计算周期水平姿态角最终输出值为(水平姿态角1+水平姿态角3)/2；

结论9：imu2的陀螺仪传感器(gyro2)存在故障，当前计算周期水平姿态角最终输出值为(水平姿态角1+水平姿态角2)/2；

结论10：imu1和imu2的陀螺仪和加速度计传感器均无故障，当前计算周期水平姿态角最终输出值为(水平姿态角1+水平姿态角2+水平姿态角3+水平姿态角4)/4。