[发明专利]一种自适应四元数粒子滤波姿态数据融合方法

说明书

本发明提出的一种自适应四元数粒子滤波姿态数据融合方法，属于数字滤波和多传感器数据融合技术领域，主要用于解决四旋翼飞行过程中存在干扰时，无法获得准确姿态角的问题。该方法以四元数粒子滤波为框架，融合陀螺仪、加速度计及磁力计数据。针对干扰造成的传感器异常观测值，采用广义似然比检测方法检测并定位干扰，调整相应传感器的量测噪声方差，进而滤除干扰，解算出精确的姿态角。本发明适用于四旋翼姿态测量系统，具有良好的抗干扰性，能够为四旋翼飞行器的稳定飞行提供保障。

技术领域

本发明提供的一种自适应四元数粒子滤波姿态数据融合方法，属于数字滤波和多传感器数据融合技术领域，本发明提供的方法适用于非线性姿态测量系统。

背景技术

四旋翼的姿态测量系统实际上为一个非线性系统，为了实现对姿态的精确解算，需要采用非线性滤波融合陀螺仪、加速度计及磁力计的数据。传统的非线性滤波主要采用扩展卡尔曼滤波(EKF)对非线性环节进行线性化处理后，采用卡尔曼滤波的框架进行解算，然而该方法的线性化过程带来了高阶截断误差。因此，一种基于蒙特卡罗方法的粒子滤波应运而生，该方法能够直接处理非线性环节。近年来，用于姿态解算的粒子滤波算法引起广泛的关注，特别是与四元数相结合的四元数粒子滤波算法不仅能够解决强非线性问题，而且相较于EKF来说，不存在四元数更新过程中的规范化问题。

传统的四元数粒子滤波算法以四元数微分方程作为系统状态方程，直接以四元数作为粒子进行状态更新获得预测粒子，结合加速度计和磁力计的观测值对预测粒子进行评估，求取相应的权值，获得四元数最优估计值后转换为相应的姿态角。然而在四旋翼实际飞行过程中，用于获得观测值的传感器易受到干扰，例如加速度计易受到非重力加速度的干扰，磁力计易受到磁场干扰，导致观测值不能对粒子进行正确的校正，由此获得的姿态角估计值与真实姿态角间的误差较大。本发明是针对上述问题，基于广义似然比检测GLRT的一种有效的自适应四元数粒子滤波姿态数据融合方法。

发明内容

本发明的目的在于针对上述存在的问题和不足，提出一种改进的用于四旋翼姿态解算的自适应四元数粒子滤波方法，算法较为简单，能够检测出传感器观测异常值，并进行自适应校正，获得精确的姿态角，保证四旋翼飞行过程中的有效控制。

本发明具体过程如下：

步骤1：根据初始观测值及当地重力加速度矢量获取初始四元数粒子；

步骤2：以四元数为系统状态量，采用陀螺仪数据建立四元数微分方程作为系统状态方程，以加速度计及磁力计的输出为观测量建立量测方程；

步骤3：采用步骤2中建立的状态方程对粒子进行状态更新；

步骤4：利用广义似然比检测GLRT判断加速度计、磁力计输出是否存在异常观测值，若传感器观测值正常，则进行量测更新，否则跳转至步骤5；

步骤5：判断异常观测值由何种干扰造成，调整相应传感器的噪声方差矩阵，进行量测更新；

步骤6：根据量测更新得到的权值，求取最优四元数估计值，并将其转换为姿态角。

进一步地，步骤1中定义四元数Q＝[q₁，q₂，q₃，q₄]^T，其中q₁为四元数标量部分，q₂，q₃，q₄为四元数矢量部分，根据初始观测值y₀＝[y_x y_y y_z]^T及当地重力加速度矢量r＝[0，0，g]^T获取初始四元数粒子，共采样Ns个粒子，Ns为大于1的正整数，用如下方法获得初始四元数粒子：

Step1：设置一个整数n，并使Ns/n也为一个整数；