[发明专利]一种小型多旋翼无人机姿态解算方法

说明书

本发明公开了一种小型多旋翼无人机姿态解算方法，采用四元数作为状态向量，使用陀螺仪数据对状态向量进行更新，将加速度数据和磁力计数据分为两个阶段进行处理。第一阶段，使用改进的无迹卡尔曼滤波算法结合加速度计数据对四元数状态向量进行初步校正；第二阶段，使用改进的无迹卡尔曼滤波算法结合磁力计数据对四元数状态向量进行进一步的校正。该方法将加速度计和磁力计数据分为两个阶段来处理，降低了量测向量的维度，可使用较小的矩阵进行计算，所需的计算能力较少，还减小了磁异常对横滚角和俯仰角估计精度的影响。

技术领域

本发明属于无人机控制及惯性导航领域，更具体地，涉及一种小型多旋翼无人机姿态解算方法。

背景技术

虽然多旋翼无人机的应用已经非常广泛，但是在多旋翼无人机的研究过程中依旧存在着许多的问题。其中一个很重要的问题就是多旋翼无人机通常尺寸较小，无法使用传统的体积和重量都比较大的高精度的传感器，而是使用的是价格低廉的MEMS惯性传感器，但是目前的MEMS惯性传感器相较于那些军用级别的高精度惯性传感器来说精度低、稳定性较差并且其系统误差随时间积累比较快，无法满足多旋翼无人机姿态测量的要求。然而姿态估计的精度是影响多旋翼飞行器性能的关键因素，因此为了解决在姿态估计中系统迅速发散的问题，通常采用的办法是综合多个传感器的优点，设计基于多传感器的姿态估计系统。目前多旋翼无人机的导航通常采用MEMS磁力计、加速度计和陀螺仪的组合(MARG)来作为其姿态估计系统。MARG传感器可看作具有一定冗余度的传感器配置，从而产生了数据融合的问题，如何将各传感器的数据取长补短、提高姿态解算精度具有重要的实际意义和研究价值。

出于对制造成本、功耗和体积尺寸等多方面的考虑，现在的小型多旋翼无人机多使用低成本的微控制器作为主控芯片，这种低成本微控制器计算能力有限。无人机作为一种复杂的嵌入式系统，其姿态解算算法对实时性有很高的要求，所以无人机姿态解算算法除了需要达到足够的解算精度外，也需要有较少的解算时间。

发明内容

针对上述现有技术存在的缺陷或不足，本发明的目的在于，提供一种小型多旋翼无人机姿态解算方法。

为了实现上述任务，本发明采用如下的技术解决方案：

一种小型多旋翼无人机姿态解算方法，其特征在于，该方法采用四元数作为状态向量，使用陀螺仪数据对状态向量进行更新，将加速度数据和磁力计数据分为两个阶段进行处理，在第一阶段，使用一种改进的无迹卡尔曼滤波器结合加速度计数据对四元数状态向量进行初步校正；在第二阶段，使用改进的无迹卡尔曼滤波算法结合磁力计数据对四元数状态向量进行进一步的校正。

根据本发明，具体按以下步骤进行：

步骤1：确定初始状态估计值及其协方差

读取陀螺仪数据w_x，w_y和w_z，计算离散时间状态转移矩阵A_k/k-1，计算状态预测值及其协方差

步骤2：Sigma点选取：

根据状态预测值及其协方差选择一组加权Sigma点；

步骤3：第一阶段量测更新：

计算经过非线性量测方程h₁(·)变换后的Sigma点，计算量测预测值及其协方差计算状态预测值与量测预测值间的互协方差确定第一阶段卡尔曼增益矩阵K_k1，读取加速度计数据计算第一阶段校正参数q_ε1，更新第一阶段状态估计值及其协方差

步骤4：第二阶段量测更新：