[发明专利]多层感知和容积卡尔曼滤波的姿态估计融合方法及系统

说明书

本发明公开了一种基于多层感知和容积卡尔曼滤波的姿态估计多频数据融合方法，基于局部加权线性回归(LWLR)、多层感知(MLP)和容积卡尔曼滤波器。本发明建立视觉和惯性数据的滤波方程，采用CKF利用一组立方点来近似非线性函数的概率分布；采用LWLR线性回归方法，利用高斯核函数对内存中测量点附近的点赋予较高的权值，求解最小均方差的无偏估计；将视觉差异作为MLP的输入，并通过隐层的激活函数，将这种差异视作和的权重，用来补偿惯性数据，解决多频融合固有的散度。本发明在姿态估计精度和发散能力方面具有较好的性能表现。

技术领域

本发明属于姿态估计领域，涉及一种基于多层感知和容积卡尔曼滤波的姿态估计多频数据融合方法。

背景技术

准确、稳定的姿态估计是定位、导航、跟踪等工程应用中必不可少的要素。在诸如头盔跟踪系统、机器人控制和定位系统中，姿态测量方法通过输出俯仰角、航向角、横滚角获取目标的空间姿态。典型的测量方法有惯性测量和视觉测量，其中，惯性测量法具有采样频率高、采集成本低等特点。然而，由于惯性传感器件的偏置漂移现象和校准系数不稳定等影响因素，使得惯性传感器的测量误差随检测时间迅速累积。与惯性传感器相比，视觉传感器工作状态稳定，采样频率相对惯性传感器件较低，因此，视觉传感器很难跟踪快速移动的物体。

在惯性传感器与视觉传感器组成的集成系统中，因为数字采样频率的不同，会造成信息不匹配的问题。综合姿态估计系统采用摄像机和陀螺仪分别获取视觉和惯性数据。通常情况下，陀螺仪的采样频率是几百赫兹，而相机的采样频率是几十甚至十几赫兹。系统采样频率的不一致会导致惯性和视觉综合姿态估计产生偏差。有研究人员利用惯性数据与视觉数据的融合来估计最优姿态。然而，在无效的视频数据间隔区间，许多惯性陀螺仪数据仍然存在。

发明内容

1、本发明的目的

为了克服单传感器测量的局限性，提升检测系统精度和稳定性，本发明利用惯性传感器与视觉传感器的互补优势，将惯性传感器和视觉传感器的集成，结合相应的算法，提出一种多传感器融合姿态估计方法，提高姿态估计的稳定性和估计精度。

2、本发明所采用的技术方案

本发明提出了一种基于多层感知和容积卡尔曼滤波的姿态估计多频数据融合方法，包括：

建立视觉和惯性数据的滤波方程，采用CKF利用一组立方点来近似非线性函数的概率分布；

采用LWLR线性回归方法，利用高斯核函数对内存中测量点附近的点赋予较高的权值，求解最小均方差的无偏估计；

将视觉差异作为MLP的输入，并通过隐层的激活函数，将这种差异视作和的权重，用来补偿惯性数据，解决多频融合固有的散度。

优选的，所述的建立视觉和惯性数据的滤波方程，

1)体积点计算

其中，[1]是主对角线为1的对称矩阵，下角标i表示第i列；ξ_i为体积点；ω_i为每个体积点所占的权值；n为系统维数；ω_i,k|k-1为k时刻预测体积点所占的权值；P_k-1|k-1为k-1时刻方差矩阵；为k-1时刻状态估计值；

2)时间更新

式(4)和式(5)分别定义了估计姿态和协方差矩阵分，其中k表示时间；

其中，是k时刻的状态估计值，X_i,k|k-1为体积点矩阵，P_k|k-1为一步预测协方差矩阵，T代表转置；