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

摘要

本发明公开了一种基于MEMS惯性传感器组的多旋翼飞行器姿态检测方法。本方法根据多旋翼飞行器工作过程中，惯性传感器组所引入的噪声干扰，动态调节陀螺仪与加速度计的增益系数，弥补了传统的姿态检测方法在加速度计受到干扰时精度下降的缺陷。步骤为先验估计姿态四元数的获取，干扰噪声强度估计，改变卡尔曼滤波算法中的增益系数，计算后验估计姿态四元数。优点根据噪声改变卡尔曼滤波中的增益系数能够很好的抑制惯性传感器组所引入的噪声干扰，提高多旋翼飞行器工作时的姿态检测精度。

主权项

一种基于MEMS惯性传感器组的多旋翼飞行器姿态检测方法，其特征在于：通过陀螺仪传感器获取先验估计姿态四元数；通过加速度传感器的数据估计噪声强度并优化卡尔曼滤波器的增益系数；通过优化后的卡尔曼滤波器融合先验估计姿态四元数和加速度传感器数据计算后验估计姿态四元数；在通过陀螺仪传感器获取先验估计姿态四元数的过程中，采用如下方法进行计算：Q(tk)=[I(1-Δθ28)+(12-Δθ248)ΔΘ]Q(tk-1)]]>ΔΘ=0-Δθx-ΔθyΔθzΔθx0Δθy-ΔθzΔθx-Δθy0ΔθzΔθxΔθy-Δθz0]]>其中I为单位矩阵，ΔΘ为陀螺仪在[tk‑1，tk]采样时间间隔内的角增量阵，Δθx、Δθy、Δθz为x、y、z陀螺仪在[tk‑1，tk]采样时间间隔内的角增量，Q(tk‑1)为上一时刻的后验估计姿态四元数，Q(tk)为当前时刻的先验估计姿态四元数；在通过加速度传感器估计噪声强度并优化卡尔曼滤波器的增益系数的过程中，采用如下步骤：(1)将加速度传感器的观测方程线性化，得到雅可比行列式Hk：Hk=-2q22q3-2q02q12q12q02q32q22q0-2q1-2q22q3]]>其中q0，q1，q2，q3为先验估计姿态四元数Q(tk)的元素；(2)计算卡尔曼滤波器参数：状态量Xk，量测量Zk，系统方差阵Qk，量测量方差阵Rk，一步姿态预测矩阵Φk/k‑1：Xk=qkZk=akQk=QωRk=Rg]]>Φk/k-1=I(1-Δθ28)+(12-Δθ248)ΔΘ]]>其中qk为先验估计姿态四元数，ak为加速度传感器数据，Qω为陀螺仪传感器的测量误差方差阵，Rg为加速度传感器的测量误差方差阵；(3)计算卡尔曼滤波算法中的一步预测均方误差Pk/k‑1：Pk/k-1=Φk/k-1Pk-1Φk/k-1T+Qω]]>其中Pk‑1为k‑1时刻的估计均方误差；(4)通过UDUT分解卡尔曼滤波器中的一步均方误差Pk/k‑1：其中Uk/k‑1为UDUT分解后的上三角矩阵，Dk/k‑1为UDUT分解后的对角矩阵，为UDUT分解后的下三角矩阵，Fk，Gk，Wk均为后续计算需要用到的中间变量；(5)计算有害加速度的方差矩阵(51)计算加速度计的估计误差矩阵ΔZk：ΔZk＝Zk‑CkXk其中Ck为k时刻的先验估计姿态四元数转换成的方向余弦矩阵：Ck=q02+q12-q22-q322(q1q2+q0q3)2(q1q3+q0q2)2(q1q2-q0q3)q02-q12+q22-q322(q2q3+q0q1)2(q1q3+q0q2)2(q2q3+q0q1)q02-q12-q22+q32;]]>(52)构造矩阵其中ra,k‑i为ΔZk矩阵中的第i行列向量，M为列向量的个数；(53)求取外界有害加速度的方差矩阵Qab,k=Σi=1M(λi,k-μi,k)ui,kui,kT]]>其中λi,k为矩阵Uk的特征值，ui,k为对应的特征向量，(6)通过步骤(5)中的有害加速度方差矩阵优化卡尔曼滤波的增益阵Kk：Kk=Gk(Wk+Qab,k)-1]]>其中Gk，Wk为一步预测均方误差Pk/k‑1进行UDUT分解时的中间变量；在通过优化后的卡尔曼滤波器融合先验估计姿态四元数和加速度传感器数据计算后验估计姿态四元数过程中，采用如下步骤：(1)通过优化后的增益矩阵Kk和加速度传感器的数据Zk计算后验估计姿态四元数ΔX^k=KkΔZk]]>X^k=X^k/k-1+ΔX^k]]>其中为估计误差四元数，即为后验估计姿态四元数；(2)根据后验估计姿态四元数估计均方误差Pk：Pk＝Pk/k‑1‑KkHkPk/k‑1Pk作为k+1时刻的估计均方误差Pk+1/k继续姿态检测计算。