[发明专利]基于变分贝叶斯的移动机器人视觉惯性融合SLAM方法

说明书

本发明提供了基于变分贝叶斯的移动机器人视觉惯性融合SLAM方法，包括：S1，获得移动机器人k时刻的预测状态向量和协方差矩阵；S2，建立k时刻观测噪声的符合基于伽马分布的学生t混合分布的模型，基于移动机器人k时刻的视觉信息、观测噪声模型和预测状态向量获得层次高斯形式的似然函数分布；S3，基于似然函数分布构建联合后验概率密度函数，利用变分贝叶斯方法对后验联合概率密度函数进行联合求解获得移动机器人k时刻的状态向量。提出一种新的分布对观测噪声进行建模，以降低非平稳厚尾噪声对视觉和惯性数据融合结果精度的影响，并且利用变分贝叶斯实现对移动机器人状态的联合估计，提高最终输出的状态向量精度。

技术领域

本发明涉及智能机器人技术领域，尤其涉及一种基于变分贝叶斯的移动机器人视觉惯性融合SLAM方法。

背景技术

同步定位和地图构建(Simultaneous Localization And Mapping，SLAM)是指移动机器人在未知环境中运行的同时将其中的有用信息提取并组合，确定自身位姿并逐步建立出周围环境地图的过程。SLAM目前已经在水下无人航行器、无人驾驶、空中无人机等方面得到了成功应用，受到的关注越来越多。

基于贝叶斯估计的滤波广泛应用于SLAM的后端优化，依据移动机器人上一时刻的状态向量与当前时刻的估计值求取当前时刻状态向量。最早出现的是基于扩展卡尔曼滤波(extended Kalman filter，EKF)的SLAM算法，它在利用EKF估计机器人位姿和地图特征位置之前需要利用一阶泰勒展开式对SLAM算法中的运动模型和观测模型进行线性化，但是对两个模型的线性化过程都会引入相应的误差，此外雅可比矩阵的计算过程也会给算法带来极大的负担。之后出现了无迹卡尔曼滤波(unscented Kalman filter，UKF)，专利CN109141436A便使用了该方法，将具有一定权值的采样点集代入到SLAM算法的运动模型和观测模型中，在对移动机器人的状态信息和观测信息的计算中避免了线性化。然而当系统维度逐渐增加时，在UKF中将会出现非局部取样的现象，直接导致SLAM的计算结果不稳定甚至出现发散。紧接着出现了基于容积卡尔曼滤波(cubature Kalman filter，CKF)的SLAM算法，将一组具有相同权值的采样点集代入非线性模型进行计算，但是该方法舍去了部分近似化误差，造成滤波精度降低。上述方法均未考虑厚尾噪声的影响，针对这一问题，专利CN113971752A中利用学生t分布来模拟具有野值的非高斯噪声以提高估计精度，将后验概率密度函数近似为具有固定自由度参数的学生t概率密度函数。但在实际中，厚尾噪声也有着非平稳的特性，不考虑非平稳特性的影响会极大降低卡尔曼滤波的估计精度。

发明内容

本发明旨在解决现有技术中卡尔曼滤波不考虑噪声的非平稳性影响最终融合后的数据精度降低的问题，提供一种基于变分贝叶斯的移动机器人视觉惯性融合SLAM方法。

为了实现本发明的上述目的，根据本发明的第一个方面，本发明提供了基于变分贝叶斯的移动机器人视觉惯性融合SLAM方法，其特征在于，包括：步骤S1，基于移动机器人k-1时刻的状态向量和协方差矩阵，以及移动机器人k时刻的IMU数据和系统噪声协方差矩阵，获得移动机器人k时刻的预测状态向量和一步预测误差协方差矩阵；步骤S2，建立k时刻观测噪声的符合基于伽马分布的学生t混合分布的模型，基于移动机器人k时刻的视觉信息、观测噪声模型和预测状态向量获得层次高斯形式的似然函数分布；步骤S3，基于似然函数分布构建联合后验概率密度函数，利用变分贝叶斯方法对后验联合概率密度函数进行联合求解获得移动机器人k时刻的状态向量。

为了实现本发明的上述目的，根据同样的发明构思，本发明提供了一种计算机可读存储介质，所述存储介质中存储有至少一条指令、至少一段程序、代码集或指令集，所述至少一条指令、所述至少一段程序、所述代码集或指令集由处理器加载并执行以实现如本发明第一方面所述的基于变分贝叶斯的移动机器人视觉惯性融合SLAM方法。