本发明涉及小型旋翼飞行器的飞行控制领域。
背景技术：
姿态估计算法是小型四旋翼无人机飞行控制系统不可或缺的重要组成部分，惯性测量单元集成了三轴陀螺仪和三轴加速度计，可以测量无人机三个体轴方向的角速度和除重力外其他所有力的合力对传感器产生的加速度。当载机的运动加速度较小时，三轴加速度计可以近似测量重力矢量的反向在载机体坐标系下各轴的分量，俯仰角和滚转角可以由此直接计算出来。因此，很多现有算法基于小加速度假设，将重力矢量观测模型作为加速度计的量测模型。但是这类算法只能在近悬停条件下才能很好地发挥作用。当载机存在数值较大的不可忽略的运动加速度的情况下，由于加速度计提供的不可靠信息，算法的估计精度较差。而对于四旋翼无人机而言，加速飞行非常普遍，此外，风的干扰也会引起运动加速度。因此，如何在运动加速度存在时准确估计四旋翼无人机的姿态是一大研究热点。一般来说，有两种方法：第一种方法是使用额外的传感器，如利用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模式)。基于扩展卡尔曼滤波器的多模式旋翼飞行器姿态估计方法基于图2的多模式姿态估计器实现；如图2所示，决策树在多模式姿态估计器中处于中枢位置，通过读取传感器数据和ekf反馈的新息矢量进行飞行状态分类，并通过改变两个二值滤波状态参数ξk和σk(如表1)来改变ekf的模式。由于三轴加速度计量测矢量的变化率的求取需要对加速度计测量值做离散时间差分，量测噪声被放大，因此使用低通滤波器进行降噪得到滤波后的即扩展卡尔曼滤波器的工作原理：本发明的核心内容是具有多个滤波模式的扩展卡尔曼滤波器。ekf的状态矢量x为姿态四元数，定义为x＝[q1q2q3q4]t其中q＝[q1q2q3]t为四元数的矢量部分，q4为四元数的标量部分。时间离散的状态更新模型为：xk+1＝φkxk+wg,k其中，其中，δt是采样周期；i3×3表示3行3列的单位矩阵；ωk为k时刻陀螺仪测量得到的带有噪声的角速度；wg,k为系统的过程噪声，由陀螺仪量测噪声vg,k和与当前姿态相关的矩阵ξg,k确定；qk为k时刻姿态四元数的矢量部分；vg,k是三轴陀螺仪的三维噪声矢量。为了实现扩展卡尔曼滤波，需要设定过程噪声矩阵qk的值，qk的值代表了对系统模型的置信度。在本发明提出的过程模型中，qk与陀螺仪的噪声特性和k时刻姿态相关，陀螺仪量测噪声的协方差矩阵为其中为陀螺仪量测噪声的方差，则qk可表示为扩展卡尔曼滤波器的量测方程为zk＝h(xk)+va,k其中zk表示k时刻三维量测矢量，xk表示k时刻状态矢量，h表示量测函数，va,k表示与加速度计量测噪声和滤波模式相关的量测噪声矢量，认为该噪声为高斯噪声。本发明的多模式扩展卡尔曼滤波器在不同的滤波模式下具有不同的量测矢量和量测模型。平衡模式(b模式)的量测矢量为zb,k＝acc＝[accxaccyaccz]t，acc表示三轴加速度计的原始量测矢量，量测函数hb为量测矩阵hb为其中，表示k时刻状态先验估计矢量。在加速模式(a模式)下，加速度计测量值的差分被引入量测矢量，离散时间条件下的量测矢量定义为其中，μ为旋翼飞行器的线性阻力系数，其物理意义为飞行速度与体坐标系水平面内加速度计测量值的线性比例关系，可通过在飞行实验中采集飞行速度真值和加速度计测量值计算获得。为了方便模式切换，将加速模式的量测矢量za,k设计为前两维有意义的三维矢量。量测函数h(xk)形式与平衡模式类似，第三维置零同理，量测矩阵ha定义为由于陀螺模式(g模式)不使用加速度计的量测信息，故没有相应的量测模型，量测模型为0矩阵。如表1，经过决策树的判断，ekf得以确定，进而确定滤波状态参数σk和ξk的值，σk和ξk可以通过改变矩阵形式来实现对滤波模式的切换。表1扩展卡尔曼滤波器模式与滤波状态参数对应关系滤波器模式滤波器参数陀螺模式σk＝0,ξk＝0加速模式σk＝1,ξk＝0平衡模式σk＝1,ξk＝1本实施方式中，两种量测模型的量侧噪声近似为高斯噪声，代表对量测数据置信度的量测噪声协方差矩阵定义为：除了使用低通滤波器降低加速度计测量值所含的噪声，飞行中载机机体的振动和载机所可能采用的隔振措施，都会使得加速度计测量值的真实噪声特性与传感器数据手册提供的数据大不相同。因此通过采集飞行器悬停飞行时的三轴加速度计输出来计算各轴的量测噪声的方差基于上述的过程模型和量测模型，进行步骤二；步骤二、扩展卡尔曼滤波器对旋翼飞行器姿态进行估计，获得姿态估计结果。步骤二一、根据状态更新方程计算k时刻的状态先验估计矢量其中，为k-1时刻的状态后验估计矢量，δt是采样周期，ωk为k时刻陀螺仪测量得到的带有噪声的角速度，步骤二二、计算k时刻的先验误差协方差矩阵其中，pk-1为k-1时刻的后验误差协方差矩阵，qk为过程噪声矩阵；步骤二三、计算k时刻的量测矩阵hk，步骤二四、计算k时刻的卡尔曼增益矩阵kk，其中r为量测噪声矩阵；步骤二五、计算k时刻的量测矢量zk，其中，accx,k、accy,k和accz,k分别为k时刻三轴加速度计x轴、y轴和z轴的测量值，μ为旋翼飞行器的线性阻力系数；步骤二六、计算k时刻的量测矢量估计值步骤二七、计算k时刻的新息矢量rk，步骤二八、计算k时刻的后验误差协方差矩阵，步骤二九、计算k时刻的状态后验估计矢量即姿态四元数，然后进行归一化，再由扩展卡尔曼滤波器输出归一化后的姿态四元数，完成姿态的估计。为了表示有效的旋转关系，姿态四元数的模必须为1。尽管状态转移矩阵φk是正交的，但是后验四元数的单位范数性质必须通过归一化步骤来保证，因此通过将除以其欧几里德范数来实现归一化。本方法经过仿真数据和实际飞行数据的验证，表现出优良的性能。如图3和图4所示，细实线代表风速(单位为m/s)，宽点划线表示姿态真值(角度)，窄点划线表示现有的基于重力矢量观测模型的ekf的姿态估计值(角度)，虚线表示非线性互补滤波的姿态估计值(角度)，粗实线表示本发明的方法得到的姿态估计值(角度)，点表示各个时刻多模式扩展卡尔曼滤波器所处的状态(度数为10则扩展卡尔曼滤波器处于g模式，度数为20表示扩展卡尔曼滤波器处于a模式，度数为30表示b模式)。仿真中，本发明提出的方法估计获得的欧拉角比现有算法更接近真实值。当旋翼飞行器在悬停或以恒定速度运动时(大约第8秒和第22秒)，扩展卡尔曼滤波器处于b模式。当姿态变化很快时(第15秒和第40秒前后)，扩展卡尔曼滤波器处于g模式。在刚刚完成姿态机动的平移加速度显著的时间段内，如第4秒和第44秒前后，扩展卡尔曼滤波器切换到a模式。当第45秒出现风扰动时，传统滤波器误差较大。风速梯度的变化使新息矢量偏离零值，因此决策树将扩展卡尔曼滤波器切换到g模式，以避免引入不可靠的加速度计测量值。如图5和图6所示，点划线为由光学运动捕捉系统测得的姿态真值(角度)，实线为本发明提出的方法获得的姿态估计值(角度)，点表示各个时刻多模式扩展卡尔曼滤波器所处的状态。尽管与机身固连安装的加速度计引入了较大的测量噪声，本发明提出的方法能够在姿态快速变化、加速飞行条件下较为准确地估计无人机的姿态。当前第1页12