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

权利要求书

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，如果判断结果为是，则扩展卡尔曼滤波器置于陀螺模式，否则扩展卡尔曼滤波器置于加速模式。