CN113821059A - 一种多旋翼无人机传感器故障安全飞行控制系统及方法 - Google Patents
一种多旋翼无人机传感器故障安全飞行控制系统及方法 Download PDFInfo
- Publication number
- CN113821059A CN113821059A CN202111400225.2A CN202111400225A CN113821059A CN 113821059 A CN113821059 A CN 113821059A CN 202111400225 A CN202111400225 A CN 202111400225A CN 113821059 A CN113821059 A CN 113821059A
- Authority
- CN
- China
- Prior art keywords
- sensor
- data
- imu inertial
- inertial sensor
- backup
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical compound C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 title claims abstract description 46
- 238000000034 method Methods 0.000 title claims abstract description 35
- 238000004891 communication Methods 0.000 claims abstract description 13
- 230000004927 fusion Effects 0.000 claims description 43
- 239000011159 matrix material Substances 0.000 claims description 12
- 238000003745 diagnosis Methods 0.000 claims description 11
- 230000000007 visual effect Effects 0.000 claims description 9
- 230000007704 transition Effects 0.000 claims description 7
- 238000004364 calculation method Methods 0.000 claims description 4
- 238000001514 detection method Methods 0.000 claims description 4
- 230000002159 abnormal effect Effects 0.000 claims description 3
- 238000011045 prefiltration Methods 0.000 claims description 3
- 231100000279 safety data Toxicity 0.000 description 3
- 150000001875 compounds Chemical class 0.000 description 2
- 230000005856 abnormality Effects 0.000 description 1
- 230000001133 acceleration Effects 0.000 description 1
- 238000000265 homogenisation Methods 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
- 230000017105 transposition Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
本发明公开一种多旋翼无人机传感器故障安全飞行控制系统,包括飞控计算机、飞行传感器组、视觉处理器和对地摄像头;所述飞行传感器组包括与飞控计算机通信连接的主IMU惯性传感器、两个备份IMU惯性传感器、磁罗盘、气压计、主GNSS接收机、备份GNSS接收机和对地高度表;本发明适用于装备在多旋翼无人机上,在不显著增加成本和系统复杂度的情况下,实现在飞行过程中对所有传感器进行热备份,极大提高了整个系统工作的可靠性,避免传感器受到同源干扰;提高多旋翼无人机飞行的可靠性和安全性,稳定实现传感器故障安全。
Description
技术领域
本发明涉及一种多旋翼无人机传感器故障安全飞行控制系统及方法,属于无人机控制系统技术领域。
背景技术
现有的多旋翼无人机受限于成本和体积重量,普遍在传感器冗余备份上设计得较为简单,对故障安全的考虑不够周全;部分无人机仅依赖一套传感器测量飞行状态,一旦单个传感器出现故障则无法安全飞行;还有部分无人机仅仅通过简单增加同质化传感器的方式实现安全备份,没有考虑到同源传感器会受到同源干扰的情况;例如,部分无人机为了避免单个磁罗盘传感器受扰影响飞机航向测定,又在无人机上简单地再增加一个同型号的磁罗盘传感器,却忽略了磁罗盘受扰常常是因为某个特定环境下相对大范围的磁场异常,而不是磁罗盘传感器本身的原因,即使再增加一个磁罗盘,也只会两个磁罗盘数据同时出问题,依然会导致飞行器坠毁;导航数据正常是飞行控制系统正常工作的前提,一旦导航数据信息出错,飞行控制系统中飞行控制算法得到的反馈量就是错误的,必然无法实现飞行器的正确控制,只能导致坠机。
发明内容
本发明的目的是针对现有技术中存在的上述问题,提供了一种多旋翼无人机传感器故障安全飞行控制系统,同时公开一种多旋翼无人机传感器故障安全飞行控制方法;适用于装备在多旋翼无人机上,在不显著增加成本和系统复杂度(不增加高精度传感器、不增加额外处理器、不增加额外的通讯和判决电路)的情况下,实现在飞行过程中对所有传感器进行热备份,所有传感器随时参与数据融合,而非简单地切换逻辑,在传感器全部工作正常的情况下可以提高精度,而在传感器故障情况下数据可以立即平滑过渡,不存在硬切换导致的数据跳变或备份传感器启动延时问题;对于飞行安全至关重要的三轴姿态角、高度、速度和位置信息中,每个关键的飞行安全数据均由至少三个不同工作原理的传感器提供,实现了故障安全的导航数据融合算法,并通过故障诊断算法对不同传感器输出的数据进行判决,能够可靠、稳定实现多源传感器的数据融合,输出正确的导航数据信息;极大提高了整个系统工作的可靠性,避免传感器受到同源干扰;提高多旋翼无人机飞行的可靠性和安全性,稳定实现传感器故障安全。
为了实现上述目标,本发明采用如下的技术方案:
一种多旋翼无人机传感器故障安全飞行控制系统,包括飞控计算机、飞行传感器组、视觉处理器和对地摄像头;所述飞行传感器组包括与飞控计算机通信连接的主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惯性传感器数据进行时间更新后的状态变量为;用此状态值校验状态更新传感器数据值是否有效;设状态变量转移到观测值的转移矩阵为H,则状态更新传感器的估计值状态更新传感器实际值为z,则两者残差 ;若在子滤波器进行状态更新时发现状态更新传感器实际值与估计值的残差超出预设的传感器噪声值范围,则判定状态更新传感器数据无效,同时断开数据无效的状态更新传感器对应的子滤波器到主滤波器的输出;若在子滤波器进行状态更新时发现状态更新传感器实际值与估计值的残差未超出预设的传感器噪声值范围,则判定状态更新传感器数据有效,同时将数据有效的状态更新传感器对应的子滤波器的输出送入下一级主滤波器进行融合。
作为本发明方法的进一步优选,所述传感器零偏值范围的设定方法如下:
将IMU惯性传感器所有的噪声建模为一个叠加在真值上的高斯分布,影响其分布范围即噪声大小的参数用一个方差参数σ2表示,而其分布中心即为零偏;IMU惯性传感器的零偏值在一个合理范围内波动,bias是指其零偏合理波动的最大值,即IMU惯性传感器正常工作的状况下零偏分布于[-bias,+bias]范围内,主IMU惯性传感器与备份IMU惯性传感器的输出值相减后误差分布的最大零偏合理范围和方差合理范围的计算公式为:
其中,biasmain是主IMU惯性传感器的零偏,biasbackup是备份IMU惯性传感器的零偏,biaserror是主IMU惯性传感器与备份IMU惯性传感器的输出值相减后误差分布的最大零偏合理范围,σ2 main是主IMU惯性传感器的方差,σ2 backup是备份IMU惯性传感器的方差,σ2 error是主IMU惯性传感器与备份IMU惯性传感器的输出值相减后误差分布的方差合理范围。
作为本发明方法的进一步优选,所述传感器零偏值范围是:
[-biaserror-3σerror,+biaserror+3σerror]。
本发明的有益之处在于:
适用于装备在多旋翼无人机上,在不显著增加成本和系统复杂度(不增加高精度传感器、不增加额外处理器、不增加额外的通讯和判决电路)的情况下,实现在飞行过程中对所有传感器进行热备份,所有传感器随时参与数据融合,而非简单地切换逻辑,在传感器全部工作正常的情况下可以提高精度,而在传感器故障情况下数据可以立即平滑过渡,不存在硬切换导致的数据跳变或备份传感器启动延时问题;对于飞行安全至关重要的三轴姿态角、高度、速度和位置信息中,每个关键的飞行安全数据均由至少三个不同工作原理的传感器提供,实现了故障安全的导航数据融合算法,并通过故障诊断算法对不同传感器输出的数据进行判决,能够可靠、稳定实现多源传感器的数据融合,输出正确的导航数据信息;极大提高了整个系统工作的可靠性,避免传感器受到同源干扰;提高多旋翼无人机飞行的可靠性和安全性,稳定实现传感器故障安全。
附图说明
图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惯性传感器数据进行时间更新后的状态变量为;用此状态值校验状态更新传感器数据值是否有效;设状态变量转移到观测值的转移矩阵为H,则状态更新传感器的估计值 ;状态更新传感器实际值为z,则两者残差 ;若在子滤波器进行状态更新时发现状态更新传感器实际值与估计值的残差超出预设的传感器噪声值范围,则判定状态更新传感器数据无效,同时断开数据无效的状态更新传感器对应的子滤波器到主滤波器的输出;若在子滤波器进行状态更新时发现状态更新传感器实际值与估计值的残差未超出预设的传感器噪声值范围,则判定状态更新传感器数据有效,同时将数据有效的状态更新传感器对应的子滤波器的输出送入下一级主滤波器进行融合。
传感器零偏值范围的设定方法如下:
将IMU惯性传感器所有的噪声建模为一个叠加在真值上的高斯分布,影响其分布范围即噪声大小的参数用一个方差参数σ2表示,而其分布中心即为零偏;IMU惯性传感器的零偏值在一个合理范围内波动,bias是指其零偏合理波动的最大值,即IMU惯性传感器正常工作的状况下零偏分布于[-bias,+bias]范围内,主IMU惯性传感器与备份IMU惯性传感器的输出值相减后误差分布的最大零偏合理范围和方差合理范围的计算公式为:
其中,biasmain是主IMU惯性传感器的零偏,biasbackup是备份IMU惯性传感器的零偏,biaserror是主IMU惯性传感器与备份IMU惯性传感器的输出值相减后误差分布的最大零偏合理范围,σ2 main是主IMU惯性传感器的方差,σ2 backup是备份IMU惯性传感器的方差,σ2 error是主IMU惯性传感器与备份IMU惯性传感器的输出值相减后误差分布的方差合理范围。
本实施例中,传感器零偏值范围是[-biaserror-3σerror,+biaserror+3σerror]。
每个IMU惯性传感器均可输出三轴角速度和三轴加速度数据,影响IMU惯性传感器输出值的关键参数为传感器零偏和噪声,将IMU惯性传感器所有的噪声建模为一个叠加在真值上的高斯分布,影响其分布范围即噪声大小的参数可用一个方差参数σ2表示,而其分布中心即为零偏;这也是学术和工程上常见的、可用的等效做法;IMU惯性传感器的零偏值是不固定的,且在一个合理范围内波动,bias是指其零偏合理波动的最大值,即IMU惯性传感器正常工作的状况下零偏分布于[-bias,+bias]范围内,对于高精度IMU惯性传感器,其零偏和噪声方差较小;对于备份的低精度IMU惯性传感器,其零偏和噪声方差较大,因三个IMU惯性传感器之间的噪声是相互独立的,则其互相之间的误差为高斯分布相减,依然为一个高斯分布,主IMU惯性传感器与备份IMU惯性传感器的输出值相减后误差分布的最大零偏合理范围和方差合理范围的计算公式为:
其中,biasmain是主IMU惯性传感器的零偏,biasbackup是备份IMU惯性传感器的零偏,biaserror是主IMU惯性传感器与备份IMU惯性传感器的输出值相减后误差分布的最大零偏合理范围,σ2 main是主IMU惯性传感器的方差,σ2 backup是备份IMU惯性传感器的方差,σ2 error是主IMU惯性传感器与备份IMU惯性传感器的输出值相减后误差分布的方差合理范围;
IMU惯性传感器的零偏参数和噪声参数,可通过IMU惯性传感器数据手册或试验获取;高斯分布有99%的概率会落在零偏的正负3σ范围以内,故在IMU惯性传感器全部工作正常、输出数据符合理论高斯模型时,主IMU惯性传感器与备份IMU惯性传感器的输出值相减的值有99%概率分布在[-biaserror-3σerror,+biaserror+3σerror]以内;通过以上算法,可以得到较为可靠的IMU惯性传感器的数据,同时虚警率低于1%,如需进一步降低虚警率,可以根据高斯分布公式进一步扩大合理分布范围。
如图2所示,关于水平位置和水平速度的数据融合算法;
本实施算法中,两个备份IMU惯性传感器均投票主IMU惯性传感器数据有效,因此采用主IMU惯性传感器的数据作为输出值;时间更新传感器指IMU惯性传感器中的加速度计,状态更新传感器包括主GNSS接收机输出的水平位置和速度信息、备份GNSS接收机输出的水平位置和速度信息、视觉数据;使用两级滤波器进行传感器故障检测和数据融合,其中子滤波器基本框架仍使用卡尔曼滤波器,相关基础概念不做赘述;具体如下:
设飞行器水平位置和水平速度的状态变量为:
Pe表示东向位置,Pn表示北向位置,Ve表示东向速度,Vn表示北向速度;该数据的子滤波器有三路,分别将加速度计数据与主GNSS接收机、备份GNSS接收机和视觉数据融合;
则第一个卡尔曼滤波时间更新方程为:
其中A1为状态转移矩阵:
预测方差时间更新方程为:
其中P1为预测方差,Q1为滤波参数,T为矩阵转置运算;
卡尔曼滤波器状态更新方程如下:
其中H1为观测矩阵,R是滤波参数,即状态变量x到状态更新传感器实际值z的转移矩阵,对于主GNSS接收机,其观测矩阵为:
将残差定义为状态更新传感器实际值和从时间更新推测出的观测值之差,即:
根据res值的大小和合理分布范围,对其值进行判断,如在预设的传感器噪声值范围内则认为状态更新传感器数据有效,采纳该值,按照下式继续计算,并将数据有效的状态更新传感器对应的子滤波器的输出接入到主滤波器;否则认为状态更新传感器数据无效,不予采纳,报告故障,并断开数据无效的状态更新传感器对应的子滤波器到主滤波器的输出;
主滤波器接收到多个子滤波器的输出后,首先统计其中有效的子滤波器数量,将有效的子滤波器的输出值按照预测方差矩阵P进行加权融合,若三个子滤波器均可用,则按照下式融合作为最终输出:
得出数据融合的最终结果。
同理,对于姿态角数据融合算法,如图3所示,时间更新传感器指IMU惯性传感器中的陀螺仪,状态更新传感器包括IMU惯性传感器中的加速度计、磁罗盘、主GNSS接收机输出的双天线航向、视觉数据;
对于高度数据融合算法,如图4所示,时间更新传感器指IMU惯性传感器中的加速度计,状态更新传感器包括气压计、对地高度表、RTK模式下主GNSS接收机输出的高度信息、主GNSS接收机输出的垂直速度信息、备份GNSS接收机输出的垂直速度信息。
本发明适用于装备在多旋翼无人机上,在不显著增加成本和系统复杂度(不增加高精度传感器、不增加额外处理器、不增加额外的通讯和判决电路)的情况下,实现在飞行过程中对所有传感器进行热备份,所有传感器随时参与数据融合,而非简单地切换逻辑,在传感器全部工作正常的情况下可以提高精度,而在传感器故障情况下数据可以立即平滑过渡,不存在硬切换导致的数据跳变或备份传感器启动延时问题;对于飞行安全至关重要的三轴姿态角、高度、速度和位置信息中,每个关键的飞行安全数据均由至少三个不同工作原理的传感器提供,实现了故障安全的导航数据融合算法,并通过故障诊断算法对不同传感器输出的数据进行判决,能够可靠、稳定实现多源传感器的数据融合,输出正确的导航数据信息;极大提高了整个系统工作的可靠性,避免传感器受到同源干扰;提高多旋翼无人机飞行的可靠性和安全性,稳定实现传感器故障安全。
以上显示和描述了本发明的基本原理、主要特征和优点;本行业的技术人员应该了解,上述实施例不以任何形式限制本发明,凡采用等同替换或等效变换的方式所获得的技术方案,均落在本发明的保护范围内。
Claims (7)
1.一种多旋翼无人机传感器故障安全飞行控制系统,其特征在于:包括飞控计算机、飞行传感器组、视觉处理器和对地摄像头;所述飞行传感器组包括与飞控计算机通信连接的主IMU惯性传感器、两个备份IMU惯性传感器、磁罗盘、气压计、主GNSS接收机、备份GNSS接收机和对地高度表;
所述主IMU惯性传感器和两个备份IMU惯性传感器用于测量飞行器的运动信息;主IMU惯性传感器采用高精度IMU惯性传感器,备份IMU惯性传感器采用低精度IMU惯性传感器;
所述磁罗盘用于测量飞行器的姿态角;
所述气压计用于测量飞行器的高度;
所述主GNSS接收机和备份GNSS接收机通过卫星导航系统测量飞行器的姿态角、速度和位置信息;
所述对地高度表用于测量飞行器相对地面的高度;
所述对地摄像头与视觉处理器通信连接,视觉处理器与飞控计算机通信连接;视觉处理器处理对地摄像头的数据得到视觉数据,所述视觉数据包括飞行器相对地面的速度和位置信息;
所述飞控计算机读取飞行传感器组的数据和视觉数据来控制飞行器飞行。
2.根据权利要求1所述一种多旋翼无人机传感器故障安全飞行控制系统,其特征在于,所述对地摄像头与飞行器机身固定连接,对地摄像头的Y轴与飞行器机身的纵轴对齐,对地摄像头的X轴与飞行器机身的横轴对齐。
3.基于权利要求1或2所述系统的一种多旋翼无人机传感器故障安全飞行控制方法,其特征在于:
所述飞控计算机读取飞行传感器组的数据和视觉数据,飞控计算机对读取到的两种数据首先经过数据融合算法计算出飞行器的飞行状态信息,所述飞行状态信息包括姿态角、高度、速度和位置;得出的飞行状态信息再经过飞行控制算法计算后通过反馈控制飞行器飞行的姿态角、高度、速度和位置。
4.根据权利要求3所述的一种多旋翼无人机传感器故障安全飞行控制方法,其特征在于,所述数据融合算法包括以下步骤:
第一步,利用两个备份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进行加权融合,得出数据融合的最终结果。
5.根据权利要求4所述的一种多旋翼无人机传感器故障安全飞行控制方法,其特征在于,第二步中所述故障诊断的方法如下:
设工作正常时,前一个时刻子滤波器的状态变量为x,通过IMU惯性传感器数据进行时间更新后的状态变量为;用此状态值校验状态更新传感器数据值是否有效;设状态变量转移到观测值的转移矩阵为H,则状态更新传感器的估计值;状态更新传感器实际值为z,则两者残差;若在子滤波器进行状态更新时发现状态更新传感器实际值与估计值的残差超出预设的传感器噪声值范围,则判定状态更新传感器数据无效,同时断开数据无效的状态更新传感器对应的子滤波器到主滤波器的输出;若在子滤波器进行状态更新时发现状态更新传感器实际值与估计值的残差未超出预设的传感器噪声值范围,则判定状态更新传感器数据有效,同时将数据有效的状态更新传感器对应的子滤波器的输出送入下一级主滤波器进行融合。
6.根据权利要求4所述的一种多旋翼无人机传感器故障安全飞行控制方法,其特征在于,所述传感器零偏值范围的设定方法如下:
将IMU惯性传感器所有的噪声建模为一个叠加在真值上的高斯分布,影响其分布范围即噪声大小的参数用一个方差参数σ2表示,而其分布中心即为零偏;IMU惯性传感器的零偏值在一个合理范围内波动,bias是指其零偏合理波动的最大值,即IMU惯性传感器正常工作的状况下零偏分布于[-bias,+bias]范围内,主IMU惯性传感器与备份IMU惯性传感器的输出值相减后误差分布的最大零偏合理范围和方差合理范围的计算公式为:
其中,biasmain是主IMU惯性传感器的零偏,biasbackup是备份IMU惯性传感器的零偏,biaserror是主IMU惯性传感器与备份IMU惯性传感器的输出值相减后误差分布的最大零偏合理范围,σ2 main是主IMU惯性传感器的方差,σ2 backup是备份IMU惯性传感器的方差,σ2 error是主IMU惯性传感器与备份IMU惯性传感器的输出值相减后误差分布的方差合理范围。
7.根据权利要求6所述的一种多旋翼无人机传感器故障安全飞行控制方法,其特征在于,所述传感器零偏值范围是[-biaserror-3σerror,+biaserror+3σerror]。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111400225.2A CN113821059B (zh) | 2021-11-24 | 2021-11-24 | 一种多旋翼无人机传感器故障安全飞行控制系统及方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111400225.2A CN113821059B (zh) | 2021-11-24 | 2021-11-24 | 一种多旋翼无人机传感器故障安全飞行控制系统及方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113821059A true CN113821059A (zh) | 2021-12-21 |
CN113821059B CN113821059B (zh) | 2022-02-18 |
Family
ID=78918151
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111400225.2A Active CN113821059B (zh) | 2021-11-24 | 2021-11-24 | 一种多旋翼无人机传感器故障安全飞行控制系统及方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113821059B (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115167508A (zh) * | 2022-07-05 | 2022-10-11 | 南京邮电大学 | 一种多旋翼无人机传感器故障安全飞行控制系统及方法 |
Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107110975A (zh) * | 2014-12-11 | 2017-08-29 | 空客直升机 | 用于旋翼飞行器的驾驶传感器的冗余设备 |
CN108168509A (zh) * | 2017-12-06 | 2018-06-15 | 南京航空航天大学 | 一种升力模型辅助的四旋翼飞行器高度容错估计方法 |
CN109931926A (zh) * | 2019-04-04 | 2019-06-25 | 山东智翼航空科技有限公司 | 一种基于站心坐标系的小型无人机无缝自主式导航算法 |
CN110095800A (zh) * | 2019-04-19 | 2019-08-06 | 南京理工大学 | 一种多源融合的自适应容错联邦滤波组合导航方法 |
CN110426032A (zh) * | 2019-07-30 | 2019-11-08 | 南京航空航天大学 | 一种解析式冗余的飞行器容错导航估计方法 |
CN111024124A (zh) * | 2019-12-25 | 2020-04-17 | 南京航空航天大学 | 一种多传感器信息融合的组合导航故障诊断方法 |
CN111044051A (zh) * | 2019-12-30 | 2020-04-21 | 星际(江苏)航空科技有限公司 | 一种复合翼无人机容错组合导航方法 |
CN112046783A (zh) * | 2020-07-31 | 2020-12-08 | 苏州千诚锐创机器人科技有限公司 | 一种三imu冗余技术的飞控方法和系统 |
CN112596535A (zh) * | 2020-12-14 | 2021-04-02 | 一飞(海南)科技有限公司 | 无人机多传感器三余度飞行控制系统、方法及存储介质 |
-
2021
- 2021-11-24 CN CN202111400225.2A patent/CN113821059B/zh active Active
Patent Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107110975A (zh) * | 2014-12-11 | 2017-08-29 | 空客直升机 | 用于旋翼飞行器的驾驶传感器的冗余设备 |
CN108168509A (zh) * | 2017-12-06 | 2018-06-15 | 南京航空航天大学 | 一种升力模型辅助的四旋翼飞行器高度容错估计方法 |
CN109931926A (zh) * | 2019-04-04 | 2019-06-25 | 山东智翼航空科技有限公司 | 一种基于站心坐标系的小型无人机无缝自主式导航算法 |
CN110095800A (zh) * | 2019-04-19 | 2019-08-06 | 南京理工大学 | 一种多源融合的自适应容错联邦滤波组合导航方法 |
CN110426032A (zh) * | 2019-07-30 | 2019-11-08 | 南京航空航天大学 | 一种解析式冗余的飞行器容错导航估计方法 |
CN111024124A (zh) * | 2019-12-25 | 2020-04-17 | 南京航空航天大学 | 一种多传感器信息融合的组合导航故障诊断方法 |
CN111044051A (zh) * | 2019-12-30 | 2020-04-21 | 星际(江苏)航空科技有限公司 | 一种复合翼无人机容错组合导航方法 |
CN112046783A (zh) * | 2020-07-31 | 2020-12-08 | 苏州千诚锐创机器人科技有限公司 | 一种三imu冗余技术的飞控方法和系统 |
CN112596535A (zh) * | 2020-12-14 | 2021-04-02 | 一飞(海南)科技有限公司 | 无人机多传感器三余度飞行控制系统、方法及存储介质 |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115167508A (zh) * | 2022-07-05 | 2022-10-11 | 南京邮电大学 | 一种多旋翼无人机传感器故障安全飞行控制系统及方法 |
CN115167508B (zh) * | 2022-07-05 | 2023-05-09 | 南京邮电大学 | 一种多旋翼无人机传感器故障安全飞行控制系统及方法 |
Also Published As
Publication number | Publication date |
---|---|
CN113821059B (zh) | 2022-02-18 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107885219B (zh) | 用于监控无人机飞行的飞行监控系统和方法 | |
CN109900300B (zh) | 一种用于无人机的组合导航完好性监测系统 | |
JP6506302B2 (ja) | モバイルプラットフォームを操作するための方法及び装置 | |
US5841537A (en) | Synthesized attitude and heading inertial reference | |
CN110426032B (zh) | 一种解析式冗余的飞行器容错导航估计方法 | |
CN108981709B (zh) | 基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法 | |
CN108981708B (zh) | 四旋翼扭矩模型/航向陀螺/磁传感器容错组合导航方法 | |
CN108168509B (zh) | 一种升力模型辅助的四旋翼飞行器高度容错估计方法 | |
PL194230B1 (pl) | Sposób i układ do syntetycznego wyliczania rezerwowego położenia samolotu | |
CN111352433B (zh) | 一种无人机水平姿态角的故障诊断方法 | |
US20080140315A1 (en) | System and method for estimating inertial acceleration bias errors | |
CN112650281B (zh) | 多传感器三余度系统、控制方法、无人机、介质及终端 | |
CN112902967A (zh) | 一种基于残差卡方-改进序贯概率比的抗欺骗导航方法 | |
CN112046783A (zh) | 一种三imu冗余技术的飞控方法和系统 | |
CN113821059B (zh) | 一种多旋翼无人机传感器故障安全飞行控制系统及方法 | |
CN114610074A (zh) | 适用于多旋翼无人机的多余度飞控系统及多旋翼无人机 | |
Layh et al. | Design for graceful degradation and recovery from GNSS interruptions | |
CN111964695A (zh) | 一种无人机组合导航系统故障检测与隔离方法 | |
Kopecki et al. | Algorithms of measurement system for a micro UAV | |
CN114625155B (zh) | 自动驾驶软件三重冗余管理系统及方法 | |
CN109931930B (zh) | 一种无人机双余度测量系统及其控制方法 | |
Dolega et al. | Possibilities of using software redundancy in low cost aeronautical control systems | |
Kugler et al. | Enhancing the auto flight system of the SAGITTA Demonstrator UAV by fault detection and diagnosis | |
CN116203931A (zh) | 一种空速计失效下的飞行容错控制与重构系统 | |
CN115857399A (zh) | 一种基于集中式多余度的多旋翼无人机飞控系统及多旋翼无人机 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |