一种小型多旋翼无人机姿态解算方法与流程

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

背景技术：

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

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

技术实现要素：

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

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

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

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

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

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

步骤2：sigma点选取：

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

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

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

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

计算经过非线性量测方程h2(·)变换后的sigma点，计算量测预测值及其协方差计算状态预测值与量测预测值间的互协方差确定第二阶段卡尔曼增益矩阵kk2，读取磁力计数据计算第二阶段校正参数qε2，其中，更新第二阶段状态估计值及其协方差

步骤5：完成一个滤波周期的计算，得到滤波校正后的四元数，将其转换为姿态角；

步骤6：重复步骤1到步骤5的过程。

本发明的小型多旋翼无人机姿态解算方法，与现有技术相比，带来的技术效果是：

(1)四元数状态向量的状态方程具有线性特征，使用传统的无迹卡尔曼滤波进行滤波解算时，在求取状态预测值及其方差时，会产生大量的冗余计算，严重影响系统的实时性，使用改进的无迹卡尔曼滤波器可以克服传统无迹卡尔曼滤波的这个缺陷。

(2)在环境中存在磁异常时，可以选择是否打开第二阶段以开启或关闭磁力计校正，以屏蔽磁异常对姿态角估计的影响，两段式滤波器提供了更大的操作灵活性。

(3)使用低维度四元数作为状态向量，并且将加速度计和磁力计数据分为两个阶段来处理，降低了量测向量的维度，可以使用较小的矩阵进行操作，减少了一个滤波周期的的解算时间，提高了系统的实时性。

(4)使用这种两段式滤波器可以消除加速度计数据对偏航角估计的干扰以及磁力计数据对横滚角和俯仰角估计的干扰。

附图说明

图1是本发明的小型多旋翼无人机姿态解算方法流程图。

图2为本发明所述算法(tdukf)和无人机自带算法(ekf)解算的横滚角比较图。

图3为本发明所述算法(tdukf)和无人机自带算法(ekf)解算的俯仰角比较图。

图4为本发明所述算法(tdukf)和无人机自带算法(ekf)解算的航向角比较图。

以下结合附图和实施例对本发明作进一步的详细说明。

具体实施方式

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

具体按以下步骤实施：

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

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

步骤2：sigma点选取：

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

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

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

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

计算经过非线性量测方程h2(·)变换后的sigma点，计算量测预测值及其协方差计算状态预测值与量测预测值间的互协方差确定第二阶段卡尔曼增益矩阵kk2，读取磁力计数据计算第二阶段校正参数qε2，更新第二阶段状态估计值及其协方差

步骤5：完成一个滤波周期的计算，得到滤波校正后的四元数，将其转换为姿态角。

步骤6：重复步骤1到步骤5的过程。

本实施例中，所述四元数状态向量的方程为：

qk＝ak/k-1qk-1+wk(1)

其中，qk∈rⁿ，为k时刻的四元数状态向量，和为k时刻的量测向量，ak/k-1为四元数状态向量方程的状态转移矩阵，h1(·)和h2(·)为描述量测方程的非线性函数，wk，和为互不相关的零均值高斯白噪声过程，其方差为：

所述的状态转移矩阵为：

其中i为单位阵，t为解算周期，ω为陀螺仪数据组成的矩阵，且：

其中，wx，wy和wz分别为k-1时刻陀螺仪测得机体坐标系下角速度分量。

所述的非线性量测方程h1(·)和非线性量测方程h2(·)均为非线性函数，其表达式分别为：

其中：和分别表示为重力场和地磁场在机体坐标系下的估计量，g表示重力加速度，为使用四元数表示的地球坐标系到载体坐标系之间的变换矩阵，其表达式为：

所述四元数状态向量的状态方程为线性，状态预测值及其协方差的计算与线性卡尔曼滤波相同，即：

所述步骤2中选取的sigma点为：

其中，a∈r为调节参数，控制sigma点在周围的分布，为矩阵均方根的第i列，n为状态向量的维数。

所述经过非线性量测方程h1(·)和非线性量测方程h2(·)变换后的sigma点分别为：

步骤3、4中所述的计算量测预测值及其互协方差和计算状态预测值与量测预测值间的互协方差的计算公式分别为：

其中，

步骤3和步骤4中所述的卡尔曼增益矩阵kk1和卡尔曼增益矩阵kk2分别为：

为保证偏航角不被加速度计校正过程影响，校正参数四元数qε的第三个矢量部分被置为0；步骤3中，计算第一阶段校正参数qε1，更新状态估计值及其协方差所用公式分别为：

qε1＝[qε,0qε,1qε,20·qε,3]^t(27)

其中qε,0,qε,1,qε,2和qε,3分别为四元数qε的四个元素；

为保证横滚角和俯仰角不被磁力计校正过程影响，校正参数qε的第一和第二个矢量部分被置为0；步骤4中，计算第二阶段校正参数qε2，更新状态估计值及其协方差所用公式分别为：

qε2＝[qε,00·qε,10·qε,2qε,3]^t(31)

其中qε,0,qε,1,qε,2和qε,3分别为四元数qε的四个元素。

所述步骤5中四元数转姿态角的公式为：

以下给出一个解算周期的完整算法流程的实施例。

如图1所示，本实施例给出一种小型多旋翼无人机姿态解算方法，系统组成包括有陀螺仪、加速度计、磁力计和改进的二段式无迹卡尔曼滤波器，具体计算流程如下：

1、计算系统先验估计

(1)确定上个解算周期的状态估计值及其协方差

(2)读取陀螺仪数据wx，wy和wz。

(3)计算离散时间状态转移矩阵

其中i为单位阵，t为解算周期，ω为陀螺仪数据组成的矩阵：

(4)时间更新:

由于四元数状态向量的状态方程为线性，状态预测值及其协方差的计算与线性卡尔曼滤波相同，即

(5)sigma点选取：

根据状态预测值及其协方差选择一组加权sigma点。选取的sigma点为：

其中，a∈r为调节参数，控制sigma点在周围的分布，为矩阵均方根的第i列，n为状态向量的维数。

2、开始第一阶段

(1)量测更新

经过非线性量测方程h1(·)变换后的sigma点为：

(2)计算量测预测值及其互协方差

计算状态预测值与量测预测值间的互协方差

其中，

(3)确定第一阶段卡尔曼增益矩阵

(4)读取加速度计数据zk1＝[axayaz]^t。

(5)计算第一阶段校正参数

为了保证偏航角不被该校正过程影响，校正参数四元数qε的第三个矢量部分被置为0，即有：

qε1＝[qε,0qε,1qε,20·qε,3]^t

(6)更新状态估计值及其协方差

3、开始第二阶段(可根据需要选择是否开启第二阶段)

(1)量测更新

经过非线性量测方程h2(·)变换后的sigma点为：

(2)计算量测预测值及其互协方差：

计算状态预测值与量测预测值间的互协方差：

其中：

(3)确定第二阶段卡尔曼增益矩阵

(4)读取磁力计数据zk2＝[mxmymz]^t。

(5)计算第二阶段校正参数

为了保证横滚角和俯仰角不被该校正过程影响，校正参数qε的第一和第二个矢量部分被置为0，即有：

qε2＝[qε,00·qε,10·qε,2qε,3]^t

(6)更新状态估计值及其协方差

4、将校正后的状态四元数转换为姿态角

本实施例中，由于四元数状态向量的状态方程存在线性特征，所以在时间更新阶段，按照线性卡尔曼滤波方式进行处理而不进行无迹变换处理；量测方程存在非线性特征，在量测更新阶段时，使用无迹变换进行处理。

本实施例使用低维度四元数作为状态向量，并且将加速度计和磁力计数据分为两个阶段来处理，降低了量测向量的维度，可以使用较小的矩阵进行计算；同时采用改进的无迹卡尔曼滤波器，所需的计算能力较少，有利于增加系统的实时性；在环境中存在磁异常时，可选择是否使用第二阶段以开启或关闭磁力计校正，以屏蔽磁异常对姿态角估计的影响。且二段式滤波器具有更大的操作灵活性；使用加速度计数据，能正确校正横滚角和俯仰角，使用磁力计数据，能正确校正偏航角。这种两段式滤波器还可以减小磁异常对横滚角和俯仰角估计精度的影响。

为验证本发明的小型多旋翼无人机姿态解算方法的有效性，发明人通过采集静止状态无人机的传感器的数据，将经本发明所述的小型多旋翼无人机姿态解算方法(tdukf)处理的姿态角数据和无人机自带算法(ekf)处理的姿态角进行比较。比较结果如图2、3、4所示，本发明所述小型多旋翼无人机姿态解算方法的横滚角、俯仰角和航行角的估计精度均在0.1度左右，与ekf算法相比有明显提高；且通过20次的蒙特卡洛仿真实验表明，与ekf相比本发明所述算法计算速度提高了约10％左右。