[发明专利]一种基于MEMS惯性传感器组的多旋翼飞行器姿态检测方法

说明书

技术领域

本发明涉及的是飞行器控制过程中，通过纯惯性传感器来检测多旋翼飞行器飞行姿态的算法。属于惯性导航技术领域。

背景技术

飞行器的姿态检测对于飞行器的控制是极其关键的，也是影响惯性导航系统精度的最主要因素。而由于MEMS工艺的惯性传感器精度较低，使得传统的姿态检测方法无法直接嫁接在其之上，需结合MEMS传感器的自身特性，以及飞行器的工作状态研究相应的姿态检测方法。

目前，对于基于MEMS惯性传感器组的姿态检测方法，主要是通过卡尔曼滤波算法对陀螺仪传感器和加速度传感器进行数据融合，从而检测出物体姿态信息。然后对于多旋翼飞行器，该方法并没有考虑到惯性传感器组在飞行器工作过程中所引入的干扰误差，从而导致该方法在飞行器加速减速过程中，姿态检测精度大幅下降。

发明内容

本发明所要解决的技术问题是针对上述缺陷，提出了一种对干扰误差进行处理从而提高飞行器姿态检测精度的基于MEMS惯性传感器组的多旋翼飞行器姿态检测方法。

为解决上述技术问题，本发明采用的技术方案是：

一种基于MEMS惯性传感器组的多旋翼飞行器姿态检测方法，其特征在于：通过陀螺仪传感器获取先验估计姿态四元数；通过加速度传感器的数据估计噪声强度并优化卡尔曼滤波器的增益系数；通过优化后的卡尔曼滤波器融合先验估计姿态四元数和加速度传感器数据计算后验姿态四元数。

在通过陀螺仪传感器获取先验估计姿态四元数的过程中，采用如下方法进行计算：

其中I为单位矩阵，ΔΘ为陀螺仪在[t_k-1，t_k]采样时间间隔内的角增量阵，Δθ_x、Δθ_y、Δθ_z为x、y、z陀螺仪在[t_k-1，t_k]采样时间间隔内的角增量，Q(t_k-1)为上一时刻的后验估计姿态四元数，Q(t_k)为当前时刻的先验估计姿态四元数。

在通过加速度传感器估计噪声强度并优化卡尔曼滤波器的增益系数的过程中，采用如下步骤：

(1)将加速度传感器的观测方程线性化，得到雅可比行列式H_k：

其中q₀，q₁，q₂，q₃为先验估计姿态四元数Q(t_k)的元素；

(2)计算卡尔曼滤波器参数：状态量X_k，量测量Z_k，系统方差阵Q_k，量测量方差阵R_k，一步姿态预测矩阵Φ_k/k-1：

其中q_k为先验估计姿态四元数，a_k为加速度传感器数据，Q_ω为陀螺仪传感器的测量误差方差阵，R_g为加速度传感器的测量误差方差阵；

(3)计算卡尔曼滤波算法中的一步预测均方误差P_k/k-1：

其中P_k-1为k-1时刻的估计均方误差；

(4)通过UDU^T分解卡尔曼滤波器中的一步均方误差P_k/k-1：

其中U_k/k-1为UDU^T分解后的上三角矩阵，D_k/k-1为UDU^T分解后的对角矩阵，为UDU^T分解后的下三角矩阵，F_k，G_k，W_k均为后续计算需要用到的中间变量；

(5)计算有害加速度的方差矩阵

(51)计算加速度计的估计误差矩阵ΔZ_k：

ΔZ_k＝Z_k-C_kX_k

其中C_k为k时刻的先验姿态四元数转换成的方向余弦矩阵：

(52)构造矩阵

其中r_a,k-i为ΔZ_k矩阵中的第i行列向量，M为列向量的个数；