[发明专利]一种基于优化的UKF算法的双通道组合定姿算法的实现方法

说明书

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

技术领域

本发明属于姿态解算算法领域，设计了一种基于优化的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时刻的一步预测状态量；