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

Abstract

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

Description

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

技术领域

本发明涉及小型旋翼飞行器的飞行控制领域。

背景技术

姿态估计算法是小型四旋翼无人机飞行控制***不可或缺的重要组成部分，惯性测量单元集成了三轴陀螺仪和三轴加速度计，可以测量无人机三个体轴方向的角速度和除重力外其他所有力的合力对传感器产生的加速度。当载机的运动加速度较小时，三轴加速度计可以近似测量重力矢量的反向在载机体坐标系下各轴的分量，俯仰角和滚转角可以由此直接计算出来。因此，很多现有算法基于小加速度假设，将重力矢量观测模型作为加速度计的量测模型。但是这类算法只能在近悬停条件下才能很好地发挥作用。当载机存在数值较大的不可忽略的运动加速度的情况下，由于加速度计提供的不可靠信息，算法的估计精度较差。而对于四旋翼无人机而言，加速飞行非常普遍，此外，风的干扰也会引起运动加速度。因此，如何在运动加速度存在时准确估计四旋翼无人机的姿态是一大研究热点。

一般来说，有两种方法：第一种方法是使用额外的传感器，如利用GPS测量信息解算出无人机的运动加速度，通过融合视觉测量信息估计无人机位姿等。但是增加额外的传感器会使***更加复杂、降低导航信息更新效率，不适合在小型无人机上使用；第二种方法是尽可能合理地使用加速度计的输出以适应不同的飞行条件，有的方法是加速度计测量值在载机平稳运动时被用于姿态估计，当载机加速时，不融合加速度计测量值，有的方法是加速度计测量值在融合中的权重根据运动加速度的大小自适应调整，该功能是通过改变量测噪声矩阵来实现；这种方法虽然一定程度上提高了大机动加速飞行中的姿态估计精度，但却是通过在一定程度上弃用加速度计测量值来实现的，因此估计精度低。

发明内容

本发明的目的是为了解决现有旋翼飞行器姿态估计方法的估计精度低的问题，从而提供基于扩展卡尔曼滤波器的多模式旋翼飞行器姿态估计方法。

本发明所述的基于扩展卡尔曼滤波器的多模式旋翼飞行器姿态估计方法，该方法包括：

步骤一、根据三轴陀螺仪和三轴加速度计的输出、扩展卡尔曼滤波器反馈的新息矢量使扩展卡尔曼滤波器处于不同的工作模式；

步骤二、不同工作模式对应不同的量测模型，基于所对应的量测模型扩展卡尔曼滤波器对旋翼飞行器姿态进行估计，获得姿态估计结果。

优选的是，步骤二包括：

步骤二一、根据状态更新方程计算K时刻的状态先验估计矢量

其中，为k-1时刻的状态后验估计矢量，Δt是采样周期，ω_k为k时刻陀螺仪测量得到的带有噪声的角速度，

步骤二二、计算k时刻的先验误差协方差矩阵

其中，P_k-1为k-1时刻的后验误差协方差矩阵，Q_k为过程噪声矩阵；

步骤二三、计算k时刻的量测矩阵H_k，

扩展卡尔曼滤波器的工作模式包括陀螺模式、加速模式和平衡模式，当为陀螺模式时σ_k＝0、ξ_k＝0，加速模式时σ_k＝1、ξ_k＝0，平衡模式时σ_k＝1、ξ_k＝1；

σ_k为加入加速度计量测信息的系数，ξ_k为平衡模式和加速模式间的切换系数；

步骤二四、计算k时刻的卡尔曼增益矩阵K_k，

其中R为量测噪声矩阵；

步骤二五、计算k时刻的量测矢量Z_k，

其中，Acc_x,k、Acc_y,k和Acc_z,k分别为k时刻三轴加速度计x轴、y轴和z轴的测量值，μ为旋翼飞行器的线性阻力系数；

步骤二六、计算k时刻的量测矢量估计值

步骤二七、计算k时刻的新息矢量r_k，

步骤二八、计算k时刻的后验误差协方差矩阵，

步骤二九、计算k时刻的状态后验估计矢量即姿态四元数，

然后进行归一化，再由扩展卡尔曼滤波器输出归一化后的姿态四元数，完成姿态的估计。

优选的是，步骤二四中，

其中，Σ_a为重力加速度标量g的量测噪声的协方差矩阵，为重力加速度标量g的量测噪声的方差。

优选的是，采用旋翼飞行器悬停飞行时的三轴加速度计输出来计算各轴的量测噪声的方差。

优选的是，步骤二二中，

其中，w_g,k为***的过程噪声，q_k为k时刻姿态四元数的矢量部分，q_4,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模式)。如果二者有至少一个大于阈值，则进一步判断扩展卡尔曼滤波器的新息向量(innovation vector)的模如果大于预先给定的新息阈值δ_r，则表明量测矢量实际值与估计值差距较大，飞行器很可能受到了外部干扰(如风干扰)，加速度计量测模型不再适用，只积分陀螺仪测量值求姿态，扩展卡尔曼滤波器置于陀螺模式(Gyroscope模式，G模式)。如果新息矢量的模较小，则认为飞行器处于无外界干扰的具有较大运动加速度的飞行中，扩展卡尔曼滤波器置于加速模式(Acceleration模式，A模式)。

基于扩展卡尔曼滤波器的多模式旋翼飞行器姿态估计方法基于图2的多模式姿态估计器实现；如图2所示，决策树在多模式姿态估计器中处于中枢位置，通过读取传感器数据和EKF反馈的新息矢量进行飞行状态分类，并通过改变两个二值滤波状态参数ξ_k和σ_k(如表1)来改变EKF的模式。由于三轴加速度计量测矢量的变化率的求取需要对加速度计测量值做离散时间差分，量测噪声被放大，因此使用低通滤波器进行降噪得到滤波后的即

扩展卡尔曼滤波器的工作原理：

本发明的核心内容是具有多个滤波模式的扩展卡尔曼滤波器。EKF的状态矢量X为姿态四元数，定义为

X＝[q₁ q₂ q₃ q₄]^T

其中q＝[q₁ q₂ q₃]^T为四元数的矢量部分，q₄为四元数的标量部分。

时间离散的状态更新模型为：

X_k+1＝Φ_kX_k+w_g,k

其中，

其中，Δt是采样周期；I_3×3表示3行3列的单位矩阵；ω_k为k时刻陀螺仪测量得到的带有噪声的角速度；w_g,k为***的过程噪声，由陀螺仪量测噪声v_g,k和与当前姿态相关的矩阵Ξ_g,k确定；q_k为k时刻姿态四元数的矢量部分；v_g,k是三轴陀螺仪的三维噪声矢量。为了实现扩展卡尔曼滤波，需要设定过程噪声矩阵Q_k的值，Q_k的值代表了对***模型的置信度。在本发明提出的过程模型中，Q_k与陀螺仪的噪声特性和k时刻姿态相关，陀螺仪量测噪声的协方差矩阵为其中为陀螺仪量测噪声的方差，则Q_k可表示为

扩展卡尔曼滤波器的量测方程为

Z_k＝h(X_k)+v_a,k

其中Z_k表示k时刻三维量测矢量，X_k表示k时刻状态矢量，h表示量测函数，v_a,k表示与加速度计量测噪声和滤波模式相关的量测噪声矢量，认为该噪声为高斯噪声。

本发明的多模式扩展卡尔曼滤波器在不同的滤波模式下具有不同的量测矢量和量测模型。平衡模式(B模式)的量测矢量为Z_B,k＝Acc＝[Acc_x Acc_y Acc_z]^T，Acc表示三轴加速度计的原始量测矢量，量测函数h_B为

量测矩阵H_B为

其中，表示k时刻状态先验估计矢量。

在加速模式(A模式)下，加速度计测量值的差分被引入量测矢量，离散时间条件下的量测矢量定义为

其中，μ为旋翼飞行器的线性阻力系数，其物理意义为飞行速度与体坐标系水平面内加速度计测量值的线性比例关系，可通过在飞行实验中采集飞行速度真值和加速度计测量值计算获得。

为了方便模式切换，将加速模式的量测矢量Z_A,k设计为前两维有意义的三维矢量。量测函数h(X_k)形式与平衡模式类似，第三维置零

同理，量测矩阵H_A定义为

由于陀螺模式(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时刻的先验误差协方差矩阵

其中，P_k-1为k-1时刻的后验误差协方差矩阵，Q_k为过程噪声矩阵；

步骤二三、计算k时刻的量测矩阵H_k，

步骤二四、计算k时刻的卡尔曼增益矩阵K_k，

其中R为量测噪声矩阵；

步骤二五、计算k时刻的量测矢量Z_k，

其中，Acc_x,k、Acc_y,k和Acc_z,k分别为k时刻三轴加速度计x轴、y轴和z轴的测量值，μ为旋翼飞行器的线性阻力系数；

步骤二六、计算k时刻的量测矢量估计值

步骤二七、计算k时刻的新息矢量r_k，

步骤二八、计算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所示，点划线为由光学运动捕捉***测得的姿态真值(角度)，实线为本发明提出的方法获得的姿态估计值(角度)，点表示各个时刻多模式扩展卡尔曼滤波器所处的状态。尽管与机身固连安装的加速度计引入了较大的测量噪声，本发明提出的方法能够在姿态快速变化、加速飞行条件下较为准确地估计无人机的姿态。

Claims (6)

1.基于扩展卡尔曼滤波器的多模式旋翼飞行器姿态估计方法，其特征在于，该方法包括：

步骤一、根据三轴陀螺仪和三轴加速度计的输出、扩展卡尔曼滤波器反馈的新息矢量使扩展卡尔曼滤波器处于不同的工作模式；

步骤二、不同工作模式对应不同的量测模型，基于所对应的量测模型扩展卡尔曼滤波器对旋翼飞行器姿态进行估计，获得姿态估计结果。

2.根据权利要求1所述的基于扩展卡尔曼滤波器的多模式旋翼飞行器姿态估计方法，其特征在于，步骤二包括：

步骤二一、根据状态更新方程计算K时刻的状态先验估计矢量

其中，为k-1时刻的状态后验估计矢量，Δt是采样周期，ω_k为k时刻陀螺仪测量得到的带有噪声的角速度，

步骤二二、计算k时刻的先验误差协方差矩阵

其中，P_k-1为k-1时刻的后验误差协方差矩阵，Q_k为过程噪声矩阵；

步骤二三、计算k时刻的量测矩阵H_k，

扩展卡尔曼滤波器的工作模式包括陀螺模式、加速模式和平衡模式，当为陀螺模式时σ_k＝0、ξ_k＝0，加速模式时σ_k＝1、ξ_k＝0，平衡模式时σ_k＝1、ξ_k＝1；

σ_k为加入加速度计量测信息的系数，ξ_k为平衡模式和加速模式间的切换系数；

步骤二四、计算k时刻的卡尔曼增益矩阵K_k，

其中，R为量测噪声矩阵；

步骤二五、计算k时刻的量测矢量Z_k，

其中，Acc_x,k、Acc_y,k和Acc_z,k分别为k时刻三轴加速度计x轴、y轴和z轴的测量值，μ为旋翼飞行器的线性阻力系数；

步骤二六、计算k时刻的量测矢量估计值

步骤二七、计算k时刻的新息矢量r_k，

步骤二八、计算k时刻的后验误差协方差矩阵，

步骤二九、计算k时刻的状态后验估计矢量即姿态四元数，

然后进行归一化，再由扩展卡尔曼滤波器输出归一化后的姿态四元数，完成姿态的估计。

3.根据权利要求2所述的基于扩展卡尔曼滤波器的多模式旋翼飞行器姿态估计方法，其特征在于，步骤二四中，

其中，Σ_a为重力加速度标量g的量测噪声的协方差矩阵，为重力加速度标量g的量测噪声的方差。

4.根据权利要求3所述的基于扩展卡尔曼滤波器的多模式旋翼飞行器姿态估计方法，其特征在于，采用旋翼飞行器悬停飞行时的三轴加速度计输出来计算各轴的重力加速度标量g的量测噪声的方差。

5.根据权利要求2所述的基于扩展卡尔曼滤波器的多模式旋翼飞行器姿态估计方法，其特征在于，步骤二二中，

其中，w_g,k为***的过程噪声，q_k为k时刻姿态四元数的矢量部分，q_4,k为k时刻姿态四元数的标量部分，Σ_g为陀螺仪量测噪声的协方差矩阵， 为陀螺仪量测噪声的方差。

6.根据权利要求1所述的基于扩展卡尔曼滤波器的多模式旋翼飞行器姿态估计方法，其特征在于，步骤一具体为：

根据三轴陀螺仪的输出获得载机角速度矢量的模||ω||，判断||ω||是否大于预先给定的角速度阈值δ_ω，如果判断结果为是，则扩展卡尔曼滤波器置于陀螺模式；

否则进一步判断三轴加速度计量测矢量Acc的模||Acc||与重力加速度标量g的差值||Acc||-g|是否小于预先给定的加速度阈值δ_g，及三轴加速度计量测矢量的变化率的模是否小于加速度变化率阈值如果判断结果均为是，则扩展卡尔曼滤波器置于平衡模式；

否则进一步判断扩展卡尔曼滤波器反馈的新息矢量的的模是否大于预先给定的新息阈值δ_r，如果判断结果为是，则扩展卡尔曼滤波器置于陀螺模式，否则扩展卡尔曼滤波器置于加速模式。