基于动力学模型的四旋翼飞行器惯性传感器容错导航方法技术

本发明专利技术公开了一种基于动力学模型的四旋翼飞行器惯性传感器容错导航方法，包括以下步骤：步骤1：周期获取四旋翼飞行器机载传感器信息；步骤2：计算加速度、角加速度；步骤3：预测角速度、四元数、速度和位置；步骤4：判断各检测滤波器的故障结果，并对故障进行定位；步骤5：隔离故障传感器信息，通过联邦卡尔曼滤波器对角速度、四元数、速度、位置进行校正；步骤6：对各检测滤波器进行重置更新。本发明专利技术提供的基于动力学模型的四旋翼飞行器惯性传感器容错导航方法的优点在于：当惯性传感器故障时，通过动力学模型代替惯性传感器进行导航解算，解决惯性传感器失效时的导航问题，提高了四旋翼飞行器的导航可靠性。

【技术实现步骤摘要】
基于动力学模型的四旋翼飞行器惯性传感器容错导航方法
本专利技术涉及容错导航
，尤其涉及基于动力学模型的四旋翼飞行器惯性传感器容错导航方法。
技术介绍
四旋翼飞行器具有体积小、结构简单、可悬停和垂直起降等优点，特别适合在近地面环境(如室内、城区和丛林等)中执行监视、侦察等任务，具有广阔的军事和民用前景。导航系统为四旋翼飞行器提供其飞行控制系统所必须的导航信息，是其完成各种复杂飞行任务的必要保障。目前四旋翼飞行器常用的传感器包括惯性传感器、GNSS(卫星导航系统)、磁传感器、气压高度计，其中惯性传感器包括陀螺仪与加速度计。受成本、体积所限，四旋翼飞行器中选用的惯性传感器精度、可靠性较低，易受外界温度、振动干扰而产生性能下降，甚至失效。此时，会导致导航系统精度下降，影响飞行安全。目前，尚未有针对惯性传感器的容错导航方案。
技术实现思路
本专利技术所要解决的技术问题在于提供一种基于动力学模型的四旋翼飞行器惯性传感器容错导航方法。本专利技术是通过以下技术方案解决上述技术问题的：通过对四旋翼飞行器的气动力和气动力矩分析，建立四旋翼飞行器的气动力模型，包括X轴阻力模型、Y轴阻力模型、升力模型，气本文档来自技高网...

【技术保护点】
1.一种基于动力学模型的四旋翼飞行器惯性传感器容错导航方法，其特征在于：包括以下步骤：步骤1：周期获取k时刻四旋翼飞行器机载传感器信息；步骤2：计算k时刻四旋翼飞行器的加速度、角加速度；步骤3：预测k时刻四旋翼飞行器的角速度、四元数、速度和位置；步骤4：判断k时刻各检测滤波器的故障结果，并对故障进行定位；步骤5：根据步骤4的故障检测结果，隔离故障传感器信息，通过联邦卡尔曼滤波器对k时刻四旋翼飞行器的角速度、四元数、速度、位置进行校正；步骤6：根据步骤4的检测结果和步骤5的滤波估计结果对各检测滤波器进行重置更新。

【技术特征摘要】
1.一种基于动力学模型的四旋翼飞行器惯性传感器容错导航方法，其特征在于：包括以下步骤：步骤1：周期获取k时刻四旋翼飞行器机载传感器信息；步骤2：计算k时刻四旋翼飞行器的加速度、角加速度；步骤3：预测k时刻四旋翼飞行器的角速度、四元数、速度和位置；步骤4：判断k时刻各检测滤波器的故障结果，并对故障进行定位；步骤5：根据步骤4的故障检测结果，隔离故障传感器信息，通过联邦卡尔曼滤波器对k时刻四旋翼飞行器的角速度、四元数、速度、位置进行校正；步骤6：根据步骤4的检测结果和步骤5的滤波估计结果对各检测滤波器进行重置更新。2.根据权利要求1所述的一种基于动力学模型的四旋翼飞行器惯性传感器容错导航方法，其特征在于：以当前时刻载体的重心位置为原点构建机体坐标系，其中X轴、Y轴与Z轴分别与当前时刻载体的前向、右向和地向重合；以初始时刻载体的位置为原点构建导航坐标系，其中X轴、Y轴与Z轴分别与当地水平面的北向、东向、地向重合；步骤1中获取的机载传感器信息包括旋翼转速传感器信息ω1(k)、ω2(k)、ω3(k)、ω4(k)，其分别为四个旋翼的转速；惯性传感器信息ωgx(k)、ωgy(k)、ωgz(k)、fax(k)、fay(k)、faz(k)，其分别为机体坐标系下X、Y、Z轴陀螺角速度输出和加速度计输出；GPS信息VNG(k)、VEG(k)、VDG(k)，其分别为北向速度、东向速度、地向速度，PNG(k)、PEG(k)，其分别为北向位置、东向位置；气压高度计信息hb(k)；磁传感器信息ψm(k)。3.根据权利要求2所述的一种基于动力学模型的四旋翼飞行器惯性传感器容错导航方法，其特征在于：步骤2中，加速度信息通过下式进行计算：式中，为k时刻机体系相对于导航系的加速度在机体系X、Y、Z轴上的分量；kHx0、kHx1、kHx2、kHy0、kHy1、kHy2、kT0、kT1为模型参数；为k-1时刻机体系相对于导航系的线速度在机体系X、Y轴上的分量；为k-1时刻机体系相对于导航系的角加速度在机体系X、Y轴上的分量；角加速度信息通过下式计算：式中，为k时刻机体系相对于导航系的角速度在机体系X、Y、Z轴上的分量，分别是的微分，即角加速度；kR0、kR1、kR2、kP0、kP1、kP2、kQ0、kQ1、kQ2为模型参数。4.根据权利要求3所述的一种基于动力学模型的四旋翼飞行器惯性传感器容错导航方法，其特征在于：步骤3中预测k时刻四旋翼飞行器的角速度、四元数、速度和位置的方法如下：1)角速度预测：式中，为k-1时刻机体系相对于导航系的角速度在机体系X、Y、Z轴上的分量；ΔT为离散采样周期；2)四元数预测：式中，q0(k)、q1(k)、q2(k)、q3(k)为k时刻的四元数；3)速度预测：式中，为k时刻机体系相对于导航系的线速度在机体系X、Y、Z轴上的分量；g为重力加速度；4)位置预测：式中，pn(k)、pe(k)、pd(k)分别为k时刻的北向位置、东向位置、地向高度。5.根据权利要求4所述的一种基于动力学模型的四旋翼飞行器惯性传感器容错导航方法，其特征在于：步骤4中对各检测滤波器进行故障检测的方法如下：4.1模型/惯性传感器检测滤波器4.1.1计算X轴阻力模型/X轴加速度计检测滤波器检测结果：①计算统计参数λd11(k)：式中，λd11(k)为k时刻的统计参数；②计算检测函数J11(k)：式中，T11是阈值，当J11(k)＝1时，X轴阻力模型或X轴加速度计故障，当J11(k)＝0时，X轴阻力模型或X轴加速度计均无故障；4.1.2计算Y轴阻力模型/Y轴加速度计检测滤波器检测结果：①计算统计参数λd12(k)：式中，λd12(k)为k时刻的统计参数；②计算检测函数J12(k)：式中，T12是阈值，当J12(k)＝1时，Y轴阻力模型或Y轴加速度计故障，当J12(k)＝0时，Y轴阻力模型或Y轴加速度计均无故障；4.1.3计算升力模型/Z轴加速度计检测滤波器检测结果：①计算统计参数λd13(k)：式中，λd13(k)为k时刻的统计参数，②计算检测函数J13(k)：式中，T13是阈值，当J13(k)＝1时，升力模型或Z轴加速度计故障，当J13(k)＝0时，升力模型或Z轴加速度计均无故障；4.1.4计算横滚力矩模型/X轴陀螺检测滤波器检测结果：①计算统计参数λd14(k)：λd14(k)＝ed14(k)Pdr14(k)-1ed14(k)T式中，λd14(k)是k时刻的统计参数，ed14(k)为k时刻残差，Pdr14为k时刻残差方差；Yd14(k)＝ωgx(k)Hd14(k)＝1Ad14(k)＝1Gd14(k-1)＝ΔT式中，R14(k)为k时刻量测噪声；为k时刻X轴陀螺噪声；Wd14(k-1)为k时刻状态噪声；为k-1时刻横滚力矩模型噪声；②计算检测函数J14(k)：式中，T14是阈值，当J14(k)＝1时，横滚力矩模型或X轴陀螺故障，当J14(k)＝0时，横滚力矩模型或X轴陀螺均无故障；4.1.5计算俯仰力矩模型/Y轴陀螺检测滤波器检测结果：①计算统计参数λd15(k)：λd15(k)＝ed15(k)Pdr15(k)-1ed15(k)T式中，λd15(k)为k时刻的统计参数，ed15(k)为k时刻残差，Pdr15(k)为k时刻残差方差；Yd15(k)＝ωgy(k)Hd15(k)＝1Ad15(k)＝1Gd15(k-1)＝ΔT式中，R15(k)为k时刻量测噪声；为k时刻Y轴陀螺噪声；Wd15(k-1)为k时刻状态噪声；为k-1时刻俯仰力矩模型噪声；②计算检测函数J15(k)：式中，T15是阈值，当J15(k)＝1时，俯仰力矩模型或Y轴陀螺故障，当J15(k)＝0时，俯仰力矩模型或Y轴陀螺均无故障；4.1.6计算扭矩模型/Z轴陀螺检测滤波器检测结果：①计算统计参数λd16(k)：λd16(k)＝ed16(k)Pdr16(k)-1ed16(k)T式中，λd16(k)为k时刻的统计参数，ed16(k)为k时刻残差，Pdr16为k时刻残差方差；Yd16(k)＝ωgz(k)Hd16(k)＝1Ad16(k)＝1Gd16(k-1)＝ΔT式中，R16(k)为k时刻量测噪声；为k时刻Z轴陀螺噪声；Wd16(k-1)为k时刻状态噪声；为k-1时刻扭矩模型噪声；②计算检测函数J16(k)：式中，T16是阈值，当J16(k)＝1时扭矩模型或Z轴陀螺故障，当J16(k)＝0时，扭矩模型或Z轴陀螺均无故障；4.2模型/量测传感器检测滤波器4.2.1计算动力学模型/磁传感器检测滤波器检测结果：①计算统计参数λd21(k)：λd21(k)＝ed21(k)Pdr21(k)-1ed21(k)T式中，λd21(k)为k时刻的统计参数，ed21(k)为k时刻残差；Pdr21为k时刻残差方差；Yd21(k)＝ψm(k)式中，Wd21(k-1)为k-1时刻状态噪声；R21(k)为k时刻量测噪声；为k时刻磁传感器噪声；②计算检测函数J21(k)：式中，T21是阈值，当J21(k)＝1时动力学模型或磁传感器故障，当J21(k)＝0时，动力学模型或磁传感器均无故障；4.2.2计算动力学模型/GPS检测滤波器检测结果：①计算统计参数λd22(k)：λd22(k)＝ed22(k)Pdr22(k)-1ed22(k)T式中，λd22(k)为k时刻的统计函数，ed22(k)为k时刻残差；Pdr22(k)为时刻残差方差；Yd22(k)＝[VNG(k)VEG(k)]THd22(k)＝[02×7ξ2×3]式中，Wd22(k-1)为k-1时刻状态噪声，分别为k-1时刻机体系X、Y、Z轴速度噪声；R22(k)为k时刻量测噪声，分为k时刻GPS北向速度、东向速度噪声；②计算检测函数J22(k)：式中，T22是阈值，当J22(k)＝1时动力学模型或GPS故障，当J22(k)＝0时，动力学模型或GPS均无故障；4.2.3计算动力学模型/气压计检测滤波器检测结果：①计算统计参数λd23(k)：λd23(k)＝ed23(k)Pdr23(k)-1ed23(k)T式中，λd23(k)为k时刻的统计参数，ed23(k)为k时刻残差；Pdr23为k时刻残差方差；Z1×3＝[2ΔT(q1(k)q3(k)-q0(k)q2(k))2ΔT(q2(k)q3(k)+q0(k)q1(k))ΔT(q02(k)-q12(k)-q22(k)+q32(k))]式中，...

【专利技术属性】
技术研发人员：刘士超，吕品，赖际舟，王炳清，许晓伟，
申请(专利权)人：南京航空航天大学，
类型：发明
国别省市：江苏,32