基于扩展卡尔曼滤波器的多模式旋翼飞行器姿态估计方法技术

基于扩展卡尔曼滤波器的多模式旋翼飞行器姿态估计方法，涉及小型旋翼飞行器的飞行控制领域，为了解决现有旋翼飞行器姿态估计方法的估计精度低的问题。本发明专利技术的方法包括：步骤一、根据三轴陀螺仪和三轴加速度计的输出、扩展卡尔曼滤波器反馈的新息矢量使扩展卡尔曼滤波器处于不同的工作模式；步骤二、不同工作模式对应不同的量测模型，基于所对应的量测模型扩展卡尔曼滤波器对旋翼飞行器姿态进行估计，获得姿态估计结果。本发明专利技术适用于估计旋翼飞行器姿态。

【技术实现步骤摘要】
基于扩展卡尔曼滤波器的多模式旋翼飞行器姿态估计方法
本专利技术涉及小型旋翼飞行器的飞行控制领域。
技术介绍
姿态估计算法是小型四旋翼无人机飞行控制系统不可或缺的重要组成部分，惯性测量单元集成了三轴陀螺仪和三轴加速度计，可以测量无人机三个体轴方向的角速度和除重力外其他所有力的合力对传感器产生的加速度。当载机的运动加速度较小时，三轴加速度计可以近似测量重力矢量的反向在载机体坐标系下各轴的分量，俯仰角和滚转角可以由此直接计算出来。因此，很多现有算法基于小加速度假设，将重力矢量观测模型作为加速度计的量测模型。但是这类算法只能在近悬停条件下才能很好地发挥作用。当载机存在数值较大的不可忽略的运动加速度的情况下，由于加速度计提供的不可靠信息，算法的估计精度较差。而对于四旋翼无人机而言，加速飞行非常普遍，此外，风的干扰也会引起运动加速度。因此，如何在运动加速度存在时准确估计四旋翼无人机的姿态是一大研究热点。一般来说，有两种方法：第一种方法是使用额外的传感器，如利用GPS测量信息解算出无人机的运动加速度，通过融合视觉测量信息估计无人机位姿等。但是增加额外的传感器会使系统更加复杂、降低导航信息更新效率，不适合在小型无人机上使用；第二种方法是尽可能合理地使用加速度计的输出以适应不同的飞行条件，有的方法是加速度计测量值在载机平稳运动时被用于姿态估计，当载机加速时，不融合加速度计测量值，有的方法是加速度计测量值在融合中的权重根据运动加速度的大小自适应调整，该功能是通过改变量测噪声矩阵来实现；这种方法虽然一定程度上提高了大机动加速飞行中的姿态估计精度，但却是通过在一定程度上弃用加速度计测量值来实现的，因此估计精度低。
技术实现思路
本专利技术的目的是为了解决现有旋翼飞行器姿态估计方法的估计精度低的问题，从而提供基于扩展卡尔曼滤波器的多模式旋翼飞行器姿态估计方法。本专利技术所述的基于扩展卡尔曼滤波器的多模式旋翼飞行器姿态估计方法，该方法包括：步骤一、根据三轴陀螺仪和三轴加速度计的输出、扩展卡尔曼滤波器反馈的新息矢量使扩展卡尔曼滤波器处于不同的工作模式；步骤二、不同工作模式对应不同的量测模型，基于所对应的量测模型扩展卡尔曼滤波器对旋翼飞行器姿态进行估计，获得姿态估计结果。优选的是，步骤二包括：步骤二一、根据状态更新方程计算K时刻的状态先验估计矢量其中，为k-1时刻的状态后验估计矢量，Δt是采样周期，ωk为k时刻陀螺仪测量得到的带有噪声的角速度，步骤二二、计算k时刻的先验误差协方差矩阵其中，Pk-1为k-1时刻的后验误差协方差矩阵，Qk为过程噪声矩阵；步骤二三、计算k时刻的量测矩阵Hk，扩展卡尔曼滤波器的工作模式包括陀螺模式、加速模式和平衡模式，当为陀螺模式时σk＝0、ξk＝0，加速模式时σk＝1、ξk＝0，平衡模式时σk＝1、ξk＝1；σk为加入加速度计量测信息的系数，ξk为平衡模式和加速模式间的切换系数；步骤二四、计算k时刻的卡尔曼增益矩阵Kk，其中R为量测噪声矩阵；步骤二五、计算k时刻的量测矢量Zk，其中，Accx,k、Accy,k和Accz,k分别为k时刻三轴加速度计x轴、y轴和z轴的测量值，μ为旋翼飞行器的线性阻力系数；步骤二六、计算k时刻的量测矢量估计值步骤二七、计算k时刻的新息矢量rk，步骤二八、计算k时刻的后验误差协方差矩阵，步骤二九、计算k时刻的状态后验估计矢量即姿态四元数，然后进行归一化，再由扩展卡尔曼滤波器输出归一化后的姿态四元数，完成姿态的估计。优选的是，步骤二四中，其中，Σa为重力加速度标量g的量测噪声的协方差矩阵，为重力加速度标量g的量测噪声的方差。优选的是，采用旋翼飞行器悬停飞行时的三轴加速度计输出来计算各轴的量测噪声的方差。优选的是，步骤二二中，其中，wg,k为系统的过程噪声，qk为k时刻姿态四元数的矢量部分，q4,k为k时刻姿态四元数的标量部分，Σg为陀螺仪量测噪声的协方差矩阵，为陀螺仪量测噪声的方差。优选的是，步骤一具体为：根据三轴陀螺仪的输出获得载机角速度矢量的模||ω||，判断||ω||是否大于预先给定的角速度阈值δω，如果判断结果为是，则扩展卡尔曼滤波器置于陀螺模式；否则进一步判断三轴加速度计量测矢量Acc的模||Acc||与重力加速度标量g的差值||Acc||-g|是否小于预先给定的加速度阈值δg，及三轴加速度计量测矢量的变化率的模是否小于加速度变化率阈值如果判断结果均为是，则扩展卡尔曼滤波器置于平衡模式；否则进一步判断扩展卡尔曼滤波器反馈的新息矢量的的模是否大于预先给定的新息阈值δr，如果判断结果为是，则扩展卡尔曼滤波器置于陀螺模式，否则扩展卡尔曼滤波器置于加速模式。本专利技术的基于扩展卡尔曼滤波器的多模式旋翼飞行器姿态估计方法可以灵活地切换滤波工作模式，姿态估计精度高，在风扰动中的抗干扰能力强，特别是在加速条件下，精度相较于现有方法优势明显。附图说明图1是用于分类飞行器飞行状态的决策树的示意图；图2是多模式姿态估计器的原理框图；图3是仿真得到的不同方法获得的俯仰角结果图；图4是仿真得到的不同方法获得的滚转角结果图；图5是实验得到的不同方法获得的俯仰角结果图；图6是实验得到的不同方法获得的滚转角结果图。具体实施方式下面将结合本专利技术实施例中的附图，对本专利技术实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本专利技术一部分实施例，而不是全部的实施例。基于本专利技术中的实施例，本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例，都属于本专利技术保护的范围。需要说明的是，在不冲突的情况下，本专利技术中的实施例及实施例中的特征可以相互组合。下面结合附图和具体实施例对本专利技术作进一步说明，但不作为本专利技术的限定。基于扩展卡尔曼滤波器的多模式旋翼飞行器姿态估计方法，该方法包括:步骤一、根据三轴陀螺仪和三轴加速度计的输出、扩展卡尔曼滤波器反馈的新息矢量使扩展卡尔曼滤波器处于不同的工作模式；利用决策树，根据传感器输出和扩展卡尔曼滤波器(EKF)反馈的新息矢量将飞行状态进行分类，并针对各个飞行状态设计了适合的滤波模式。如图1所示，首先，根据三轴陀螺仪测量获得的载机角速度矢量的模||ω||，判断载机姿态是否正在快速变化，如果载机正在进行姿态机动，即||ω||大于预先给定的角速度阈值δω，则将扩展卡尔曼滤波器置于陀螺模式(G模式)。如果载机角速度不大，则进一步判断三轴加速度计量测矢量Acc的模||Acc||与重力加速度标量g的差值||Acc||-g|和三轴加速度计量测矢量的变化率的模如果他们二者都分别小于预先给定的加速度阈值δg和加速度变化率阈值则认为载机处于悬停或匀速飞行状态，将扩展卡尔曼滤波器置于平衡模式(Balance模式，B模式)。如果二者有至少一个大于阈值，则进一步判断扩展卡尔曼滤波器的新息向量(innovationvector)的模如果大于预先给定的新息阈值δr，则表明量测矢量实际值与估计值差距较大，飞行器很可能受到了外部干扰(如风干扰)，加速度计量测模型不再适用，只积分陀螺仪测量值求姿态，扩展卡尔曼滤波器置于陀螺模式(Gyroscope模式，G模式)。如果新息矢量的模较小，则认为飞行器处于无外界干扰的具有较大运动加速度的飞行中，扩展卡尔曼滤波器置于加速模式(Acceleration模式，A模本文档来自技高网...

【技术保护点】
1.基于扩展卡尔曼滤波器的多模式旋翼飞行器姿态估计方法，其特征在于，该方法包括：步骤一、根据三轴陀螺仪和三轴加速度计的输出、扩展卡尔曼滤波器反馈的新息矢量使扩展卡尔曼滤波器处于不同的工作模式；步骤二、不同工作模式对应不同的量测模型，基于所对应的量测模型扩展卡尔曼滤波器对旋翼飞行器姿态进行估计，获得姿态估计结果。

【技术特征摘要】
1.基于扩展卡尔曼滤波器的多模式旋翼飞行器姿态估计方法，其特征在于，该方法包括：步骤一、根据三轴陀螺仪和三轴加速度计的输出、扩展卡尔曼滤波器反馈的新息矢量使扩展卡尔曼滤波器处于不同的工作模式；步骤二、不同工作模式对应不同的量测模型，基于所对应的量测模型扩展卡尔曼滤波器对旋翼飞行器姿态进行估计，获得姿态估计结果。2.根据权利要求1所述的基于扩展卡尔曼滤波器的多模式旋翼飞行器姿态估计方法，其特征在于，步骤二包括：步骤二一、根据状态更新方程计算K时刻的状态先验估计矢量其中，为k-1时刻的状态后验估计矢量，Δt是采样周期，ωk为k时刻陀螺仪测量得到的带有噪声的角速度，步骤二二、计算k时刻的先验误差协方差矩阵其中，Pk-1为k-1时刻的后验误差协方差矩阵，Qk为过程噪声矩阵；步骤二三、计算k时刻的量测矩阵Hk，扩展卡尔曼滤波器的工作模式包括陀螺模式、加速模式和平衡模式，当为陀螺模式时σk＝0、ξk＝0，加速模式时σk＝1、ξk＝0，平衡模式时σk＝1、ξk＝1；σk为加入加速度计量测信息的系数，ξk为平衡模式和加速模式间的切换系数；步骤二四、计算k时刻的卡尔曼增益矩阵Kk，其中，R为量测噪声矩阵；步骤二五、计算k时刻的量测矢量Zk，其中，Accx,k、Accy,k和Accz,k分别为k时刻三轴加速度计x轴、y轴和z轴的测量值，μ为旋翼飞行器的线性阻力系数；步骤二六、计算k时刻的量测矢量估计值步骤二七、计算k时刻的新息矢量rk，步骤二八、计算k时刻的后验误差协方差矩阵，步骤二九、计算k时刻的状态后验估计矢量即姿态四元数，然后进行归一化，再由...

【专利技术属性】
技术研发人员：郭继峰，徐英夫，白成超，郑红星，
申请(专利权)人：哈尔滨工业大学，
类型：发明
国别省市：黑龙江,23