CN108519090A - 一种基于优化的ukf算法的双通道组合定姿算法的实现方法 - Google Patents

Abstract

本发明提供了一种基于优化的UKF算法的双通道组合定姿算法的实现方法，包括步骤：步骤1：用加速度计与磁力计分别与陀螺仪组成滤波系统；通过MEMS九轴传感器采集九轴数据，并传输至主机端；步骤2：主机端通过UKF算法分别对陀螺仪/加速度计通道及陀螺仪/磁力计通道进行姿态解算，分别得到定姿系统的横滚角和俯仰角以及航向角；步骤3：将步骤2得到的定姿系统的横滚角和俯仰角以及航向角进行数据融合得到最终姿态。本发明针对常规的姿态解算系统，成功解决了速度、精度、稳定性之间的制约关系，在尽可能减少计算量的同时，提高了精度和系统稳定性。本发明提出的算法，使用全数字实现，且与平台无关，方式较为简单实用。

Description

一种基于优化的UKF算法的双通道组合定姿算法的实现方法

技术领域

本发明属于姿态解算算法领域，设计了一种基于优化的UKF算法的双通道组合定姿算法的实现方法，可用于无人机、可穿戴设备姿态信息解算领域。

背景技术

在近几十年，伴随着微机电系统(MEMS)技术的出现和发展，使得微型测量系统的实现成为可能，相关的惯性导航技术有了更加广泛的应用场景，如无人机控制导航技术，机器人运动控制，具有动作识别的可穿戴设备等。与传统的惯性传感器相比，基于MEMS技术的传感器体积小，功耗低，价格便宜，使得在近几年得到了很多研究的青睐。在锁小型运动测量系统中，凡乎所有的运动测量都是以载体姿态测量为基础，因此一个良好的测姿系统是实现高精度运动测量的关键。虽然整体原理出自于惯性导航技术，但是由于MEMS器件的自身特点，需要在己有的理论基础上进行优化创新。本课题就微小型测姿系统为研究方向，不同于往的单陀螺仪测姿方法，而是采用多MEMS传感器来进行组合测姿，其中陀螺仪、加速度计、磁力计协同工作，采用数据融合的方法提高系统的整体精度。对于组合定姿系统的姿态计算与更新，目前主流的算法是互补滤波法、梯度下降法、拓展卡尔曼滤波法(EKF)，其中拓展卡尔曼滤波作为一种最优估计被业内广泛使用。但当系统的非线性函数以泰勒展开得到的高阶项精度无法忽略时，再增加更高阶项会使计算量成指数增长，而且非线性函数的雅可比矩阵难以求导。针对EKF的不足，又引进了UKF(无极卡尔曼滤波)算法,UKF算法框架也是以卡尔曼线性滤波为基础，通过非线性状态方程来估算状态向量，最终实现滤波的一种概率密度函数，克服了EKF的一些缺点。众所周知，加速度计和磁力计无法单独测出完整的姿态，也就无法全信息修正陀螺仪数据，现在主流算法是陀螺仪和加速度计融合得到横滚角和航向角，由单一的磁力计计算航向角，这样不仅浪费了陀螺仪测得的全姿态信息，还会因为磁力计被干扰而产生较大误差。依据上述，本文用UKF代替传统的EKF，对陀螺仪/加速度计和陀螺仪/磁力计双重数据融合来更新姿态，虽然增加了计算量，但相比于传统的方法，有更高的精度和更强的抗干扰能力，并且本文设计的系统中，载体只负责采集传感器信息，姿态结算与数据融合算法放在计算能力强大的PC端，计算速度得到保障。

发明内容

发明目的：本发明针对姿态解算，提出了一种基于优化的UKF算法的双通道组合定姿算法的实现方法，能够在静态和动态的状态下精确的计算出载体姿态。

技术方案：

一种基于优化的UKF算法的双通道组合定姿算法的实现方法，包括步骤：

步骤1：用加速度计与磁力计分别与陀螺仪组成滤波系统；通过MEMS九轴传感器采集九轴数据，并传输至主机端；

步骤2：主机端通过UKF算法分别对陀螺仪/加速度计通道及陀螺仪/磁力计通道进行姿态解算，分别得到定姿系统的横滚角和俯仰角以及航向角；

步骤3：将步骤2得到的定姿系统的横滚角和俯仰角以及航向角进行数据融合得到最终姿态。

所述步骤2的具体流程为：

1)读取k时刻陀螺仪、加速度计及磁力计数据；并计算得到k-1时刻的最优状态量和误差协方差矩阵；

2)根据陀螺仪/加速度计当前时刻的数值计算一步转移矩阵，根据计算得到的一步转移矩阵、步骤1)中得到的k-1时刻的最优状态量计算k时刻的一步预测状态量；

3)由步骤2)得到的一步转移矩阵、步骤1)得到的k-1时刻的误差协方差矩阵和系统状态噪声矩阵计算k时刻的预测误差协方差矩阵；

4)对步骤3)得到的预测误差协方差矩阵进行正定化检测与处理；

5)对步骤2)得到的一步预测状态量进行UT分解得到预测状态量的Sigma点集和相应权重；

6)将步骤5)得到的预测状态量的Sigma点集代入观测方程解算出观测量预测点集；其中，观测方程根据具体的传感器采用相应方程；

7)对步骤6)得到的观测量预测点集按照权重求和得到观测量预测点集的均值作为观测量预测值；

8)由步骤6)得到的观测量预测点集、步骤7)得到的观测量预测值、由预测状态量的Sigma点集、一步预测状态量和系统观测噪声矩阵计算观测预测值协方差矩阵和状态预测值协方差矩阵；

9)由观测预测值协方差矩阵和状态预测值协方差矩阵计算卡尔曼增益矩阵；

10)由一步预测状态量、步骤9)得到的卡尔曼增益矩阵、观测量预测值和加速度计/磁力计表示的观测量测量值计算k时刻最优状态量；

11)根据系统预测误差协方差矩阵、卡尔曼增益矩阵和观测预测值协方差矩阵计算出k时刻的系统误差协方差矩阵；

12)将k时刻的最优状态量转化为欧拉角作为系统k时刻的输出。

对误差协方差矩阵非负定性判定，如果负定，则改变UT变化中的参数κ。

所述陀螺仪的姿态解算的状态方程计算如下：

四元数与旋转的关系表示为：

其中，Q是表示姿态的四元数，μ^R表示参考坐标系下的旋转轴，θ表示是旋转的角度，符号服从右手定则；

上式两边对时间求导，再展开成矩阵形式为：

其中是四元数对时间求导的结果，ω_x、ω_y、ω_z为载体在载体坐标系的三个轴上k时刻的角速度，是k时刻四元数符号；

采用龙哥库塔算法解此微分方程得：

其中，表示t时刻的姿态，表示t+h时刻的姿态，h是采样周期；

将上式作为UKF的状态方程。

对于加速度计和磁力计，计算姿态的模型表示为：

其中u^b是载体坐标系下的测量值，u^R是参考坐标系下的测量值，由于本系统的参考坐标系是固定的地理坐标系，所以u^R是常量。是由四元数组成的旋转矩阵。

加速度姿态解算公式如下：

其中，a_x、a_y、a_z是加速度计在载体坐标系上的加速度归一化的结果，V(k)是观测噪声，满足E[v_k]＝0,Cov(v_kv_j)＝R_gδ_kj，该参数根据经验和实际情况，初始化为一极小常量。

磁力计姿态解算公式如下：

其中，m_x、m_y、m_z是磁力计在载体坐标系上的磁场强度归一化的结果；M_y、M_z是地理磁场在地理坐标系的y和z轴分量，由当地地理磁场分布决定。

有益效果：本发明针对常规的姿态解算系统，成功解决了速度、精度、稳定性之间的制约关系，在尽可能减少计算量的同时，提高了精度和系统稳定性。通过使用UKF算法，相对于传统的EKF提高了精度；通过对UKF步骤的简化，减少了一次UT变化过程，减少了算法计算量；通过动态调整UT变化参数和预测协方差反馈机制，保证了实际运算过程中协方差正定，提高了系统的稳定性。通过充分利用了陀螺仪得到的信息，使航向角不再单独依靠磁力计，增强了抗磁场干扰能力。本发明提出的算法，使用全数字实现，且与平台无关，方式较为简单实用。

附图说明

图1为本发明的软件流程图；

图2为本发明实现双通道姿态解算与数据融合的系统框图；

图3为本发明针对姿态解算与数据融合优化后的UKF算法流程图；

图4为本发明的预测协方差正定检测与调整模块的结构框图。

具体实施方式

下面结合附图对本发明作更进一步的说明。

图2是整个系统的设计图，用姿态四元数作为系统的状态量，整个系统的核心算法在陀螺仪/加速度计通道和陀螺仪/磁力计通道，它们的实现流程是一样的，区别是输入量是各自的姿态四元数，系统协方差矩阵和观测量。利用陀螺仪/加速度计通道提供的姿态四元数转化为欧拉角后，提供定姿系统的横滚角和俯仰角，利用陀螺仪/磁力计通道提供的姿态四元数转化为欧拉角后，提供定姿系统的航向角；最后将得到的定姿系统的横滚角和俯仰角以及航向角进行数据融合得到最终姿态。

本发明提出的姿态解算与数据融合算法，首先通过MEMS传感器采集九轴数据，再通过低功耗蓝牙将数据传输到主机端进行姿态解算与数据融合。

姿态解算实现方法如下：

其中，陀螺仪姿态解算的状态方程如下：

四元数与旋转的关系可以表示为：

其中，Q是表示姿态的四元数，μ^R表示参考坐标系下的旋转轴，θ表示是旋转的角度，符号服从右手定则。

由于陀螺仪测量的是传感器的角速度，所以要对上式两边对时间求导，再展开成矩阵形式为：

其中是四元数对时间求导的结果，ω_x、ω_y、ω_z为载体在载体坐标系的三个轴上k时刻的角速度，是k时刻四元数符号；

采用龙哥库塔算法解此微分方程可得：

其中，表示t时刻的姿态，表示t+h时刻的姿态，h是采样周期。这样下一时刻的姿态、当前姿态以及陀螺仪数据建立了联系。

将上式作为UKF的状态方程。

对于加速度计和磁力计，计算姿态的模型可表示为：

其中u^b是载体坐标系下的测量值，u^R是参考坐标系下的测量值，由于本系统的参考坐标系是固定的地理坐标系，所以u^R是常量。是由四元数组成的旋转矩阵。

加速度姿态解算公式如下：

其中，a_x、a_y、a_z是加速度计在载体坐标系上的加速度归一化的结果，V(k)是观测噪声，满足E[v_k]＝0,Cov(v_kv_j)＝R_gδ_kj，该参数根据经验和实际情况，初始化为一极小常量。

磁力计姿态解算公式如下：

其中，m_x、m_y、m_z是磁力计在载体坐标系上的磁场强度归一化的结果；M_y、M_z是地理磁场在地理坐标系的y和z轴分量，由当地地理磁场分布决定。

图1为本发明的软件流程图，图3是图2中两个通道的算法流程，如图1、图3所示，本发明的具体步骤如下：

1)读取k时刻陀螺仪、加速度计及磁力计数据，并计算得到k-1时刻的最优状态量和误差协方差矩阵。

2)根据陀螺仪/加速度计当前时刻的数值计算一步转移矩阵，根据计算得到的一步转移矩阵、步骤1)中得到的k-1时刻的最优状态量计算k时刻的一步预测状态量。

3)由步骤2)得到的一步转移矩阵、步骤1)得到的k-1时刻的误差协方差矩阵和系统状态噪声矩阵计算k时刻的预测误差协方差矩阵。系统状态噪声矩阵是对环境与设备造成的误差的补偿，系统噪声具体指图3中的第二个公式的Q，是一个常量，根据经验调整获取，可假设为一个很小的值。

4)对步骤3)得到的预测误差协方差矩阵进行正定化检测与处理。

5)对步骤2)得到的一步预测状态量进行UT分解得到预测状态量的Sigma点集和相应权重。

6)将步骤5)得到的预测状态量的Sigma点集代入观测方程解算出观测量预测点集；其中，观测方程根据具体的传感器采用公式(5)或公式(6)。

7)对步骤6)得到的观测量预测点集按照权重求和得到观测量预测点集的均值作为观测量预测值。

8)由步骤6)得到的观测量预测点集、步骤7)得到的观测量预测值、由预测状态量的Sigma点集、一步预测状态量和系统观测噪声矩阵计算观测预测值协方差矩阵和状态预测值协方差矩阵。系统观测噪声矩阵是对观测时造成的误差的补偿，体现在图3公式里的R。

9)由观测预测值协方差矩阵和状态预测值协方差矩阵计算卡尔曼增益矩阵。

10)由预测状态量、步骤9)得到的卡尔曼增益、观测量预测值和加速度计/磁力计表示的观测量测量值计算k时刻最优状态量。

11)根据系统预测误差协方差矩阵、卡尔曼增益矩阵和观测预测值协方差矩阵计算出k时刻的系统误差协方差矩阵。

12)将k时刻的最优状态量和系统误差协方差矩阵保存作为下一时刻的输入量；

13)将k时刻的最优状态量转化为欧拉角作为系统k时刻的输出。

图4是图3中的正定化系统。在系统运行过程中，会因为系统模型原因和计算机浮点数近似原因，可能会出现误差协方差矩阵负定，导致UT变化不能进行下去，影响系统的稳定性。本系统先是对误差协方差矩阵非负定性判定，如果负定，则改变UT变化中的参数κ，虽然卡尔曼的特性是可以不断调整误差协方差矩阵的，但如果连续非负定超过一定阈值，则主动调整误差协方差矩阵。

以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (5)

1.一种基于优化的UKF算法的双通道组合定姿算法的实现方法，其特征在于：包括步骤：

步骤1：用加速度计与磁力计分别与陀螺仪组成滤波系统；通过MEMS九轴传感器采集九轴数据，并传输至主机端；

步骤2：主机端通过UKF算法分别对陀螺仪/加速度计通道及陀螺仪/磁力计通道进行姿态解算，分别得到定姿系统的横滚角和俯仰角以及航向角；

步骤3：将步骤2得到的定姿系统的横滚角和俯仰角以及航向角进行数据融合得到最终姿态。

2.根据权利要求1所述的实现方法，其特征在于：所述步骤2的具体流程为：

1)读取k时刻陀螺仪、加速度计及磁力计数据；并计算得到k-1时刻的最优状态量和误差协方差矩阵；

2)根据陀螺仪/加速度计当前时刻的数值计算一步转移矩阵，根据计算得到的一步转移矩阵、步骤1)中得到的k-1时刻的最优状态量计算k时刻的一步预测状态量；

3)由步骤2)得到的一步转移矩阵、步骤1)得到的k-1时刻的误差协方差矩阵和系统状态噪声矩阵计算k时刻的预测误差协方差矩阵；

4)对步骤3)得到的预测误差协方差矩阵进行正定化检测与处理；

5)对步骤2)得到的一步预测状态量进行UT分解得到预测状态量的Sigma点集和相应权重；

6)将步骤5)得到的预测状态量的Sigma点集代入观测方程解算出观测量预测点集；其中，观测方程根据具体的传感器采用相应方程；

7)对步骤6)得到的观测量预测点集按照权重求和得到观测量预测点集的均值作为观测量预测值；

8)由步骤6)得到的观测量预测点集、步骤7)得到的观测量预测值、由预测状态量的Sigma点集、一步预测状态量和系统观测噪声矩阵计算观测预测值协方差矩阵和状态预测值协方差矩阵；

9)由观测预测值协方差矩阵和状态预测值协方差矩阵计算卡尔曼增益矩阵；

10)由一步预测状态量、步骤9)得到的卡尔曼增益矩阵、观测量预测值和加速度计/磁力计表示的观测量测量值计算k时刻最优状态量；

11)根据系统预测误差协方差矩阵、卡尔曼增益矩阵和观测预测值协方差矩阵计算出k时刻的系统误差协方差矩阵；

12)将k时刻的最优状态量转化为欧拉角作为系统k时刻的输出。

3.根据权利要求2所述的实现方法，其特征在于：所述正定化处理为：对

误差协方差矩阵非负定性判定，如果负定，则改变UT变化中的参数κ。

4.根据权利要求2所述的实现方法，其特征在于：所述陀螺仪的姿态解算的状态方程计算如下：

四元数与旋转的关系表示为：

其中，Q是表示姿态的四元数，μ^R表示参考坐标系下的旋转轴，θ表示是旋转的角度，符号服从右手定则；

上式两边对时间求导，再展开成矩阵形式为：

其中是四元数对时间求导的结果，ω_x、ω_y、ω_z为载体在载体坐标系的三个轴上k时刻的角速度，是k时刻四元数符号；

采用龙哥库塔算法解此微分方程得：

其中，表示t时刻的姿态，表示t+h时刻的姿态，h是采样周期；

将上式作为UKF的状态方程。

5.根据权利要求2所述的实现方法，其特征在于：对于加速度计和磁力计，计算姿态的模型表示为：

其中u^b是载体坐标系下的测量值，u^R是参考坐标系下的测量值，由于本系统的参考坐标系是固定的地理坐标系，所以u^R是常量。是由四元数组成的旋转矩阵。

加速度姿态解算公式如下：

其中，a_x、a_y、a_z是加速度计在载体坐标系上的加速度归一化的结果，V(k)是观测噪声，满足E[v_k]＝0,Cov(v_kv_j)＝R_gδ_kj，该参数根据经验和实际情况，初始化为一极小常量。

磁力计姿态解算公式如下：

其中，m_x、m_y、m_z是磁力计在载体坐标系上的磁场强度归一化的结果；M_y、M_z是地理磁场在地理坐标系的y和z轴分量，由当地地理磁场分布决定。