基于变分贝叶斯的移动机器人视觉惯性融合SLAM方法与流程

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


背景技术：

2.同步定位和地图构建(simultaneous localization and mapping，slam)是指移动机器人在未知环境中运行的同时将其中的有用信息提取并组合，确定自身位姿并逐步建立出周围环境地图的过程。slam目前已经在水下无人航行器、无人驾驶、空中无人机等方面得到了成功应用，受到的关注越来越多。
3.基于贝叶斯估计的滤波广泛应用于slam的后端优化，依据移动机器人上一时刻的状态向量与当前时刻的估计值求取当前时刻状态向量。最早出现的是基于扩展卡尔曼滤波(extended kalman filter，ekf)的slam算法，它在利用ekf估计机器人位姿和地图特征位置之前需要利用一阶泰勒展开式对slam算法中的运动模型和观测模型进行线性化，但是对两个模型的线性化过程都会引入相应的误差，此外雅可比矩阵的计算过程也会给算法带来极大的负担。之后出现了无迹卡尔曼滤波(unscented kalman filter，ukf)，专利cn109141436a便使用了该方法，将具有一定权值的采样点集代入到slam算法的运动模型和观测模型中，在对移动机器人的状态信息和观测信息的计算中避免了线性化。然而当系统维度逐渐增加时，在ukf中将会出现非局部取样的现象，直接导致slam的计算结果不稳定甚至出现发散。紧接着出现了基于容积卡尔曼滤波(cubature kalman filter，ckf)的slam算法，将一组具有相同权值的采样点集代入非线性模型进行计算，但是该方法舍去了部分近似化误差，造成滤波精度降低。上述方法均未考虑厚尾噪声的影响，针对这一问题，专利cn113971752a中利用学生t分布来模拟具有野值的非高斯噪声以提高估计精度，将后验概率密度函数近似为具有固定自由度参数的学生t概率密度函数。但在实际中，厚尾噪声也有着非平稳的特性，不考虑非平稳特性的影响会极大降低卡尔曼滤波的估计精度。


技术实现要素：

4.本发明旨在解决现有技术中卡尔曼滤波不考虑噪声的非平稳性影响最终融合后的数据精度降低的问题，提供一种基于变分贝叶斯的移动机器人视觉惯性融合slam方法。
5.为了实现本发明的上述目的，根据本发明的第一个方面，本发明提供了基于变分贝叶斯的移动机器人视觉惯性融合slam方法，其特征在于，包括：步骤s1，基于移动机器人k-1时刻的状态向量和协方差矩阵，以及移动机器人k时刻的imu数据和系统噪声协方差矩阵，获得移动机器人k时刻的预测状态向量和一步预测误差协方差矩阵；步骤s2，建立k时刻观测噪声的符合基于伽马分布的学生t混合分布的模型，基于移动机器人k时刻的视觉信息、观测噪声模型和预测状态向量获得层次高斯形式的似然函数分布；步骤s3，基于似然函数分布构建联合后验概率密度函数，利用变分贝叶斯方法对后验联合概率密度函数进行联合求解获得移动机器人k时刻的状态向量。
6.为了实现本发明的上述目的，根据同样的发明构思，本发明提供了一种计算机可读存储介质，所述存储介质中存储有至少一条指令、至少一段程序、代码集或指令集，所述至少一条指令、所述至少一段程序、所述代码集或指令集由处理器加载并执行以实现如本发明第一方面所述的基于变分贝叶斯的移动机器人视觉惯性融合slam方法。
7.为了实现本发明的上述目的，根据同样的发明构思，本发明提供了一种移动机器人，包括移动机器人本体和设于所述移动机器人本体上的视觉传感器、imu传感器和处理器，所述处理器分别与视觉设别和imu传感器连接，所述处理器执行本发明第一方面所述的基于变分贝叶斯的移动机器人视觉惯性融合slam方法的步骤。
8.上述技术方案：提出一种新的分布(即基于伽马分布的学生t混合分布)来对观测噪声进行建模，该分布以伽马分布为混合密度函数，对学生t分布进行尺度混合，以应对噪声的非平稳行，降低非平稳厚尾噪声对视觉和惯性数据融合结果精度的影响，进而提高卡尔曼滤波的估计精度，并且在imu数据和视觉传感器测量数据融合过程中引入了基于变分贝叶斯的信息迭代更新，利用变分贝叶斯实现对移动机器人状态的联合估计，从而进一步提高最终求得的状态向量的精度。
附图说明
9.图1是本发明实施例1中基于变分贝叶斯的移动机器人视觉惯性融合slam方法的流程示意图；
10.图2是本发明实施例1在一种应用场景中的数据融合流程示意图。
具体实施方式
11.下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。
12.在本发明的描述中，需要理解的是，术语“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
13.在本发明的描述中，除非另有规定和限定，需要说明的是，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是机械连接或电连接，也可以是两个元件内部的连通，可以是直接相连，也可以通过中间媒介间接相连，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语的具体含义。
14.实施例1
15.本实施例公开了一种基于变分贝叶斯的移动机器人视觉惯性融合slam方法，如图1所示，包括：
16.步骤s1，基于移动机器人k-1时刻的状态向量x
k-1||k-1
和协方差矩阵p
k-1|k-1
，以及移动机器人k时刻的imu数据和系统噪声协方差矩阵qk，获得移动机器人k时刻的预测状态向量x
k|k-1
和一步预测误差协方差矩阵p
k|k-1
。
17.imu数据由imu传感器提供，imu传感器包括加速度计和陀螺仪，加速度计提供移动机器人在运动过程中的加速度，陀螺仪提供移动机器人在运动过程中的角速度值。获得imu数据后，对imu数据进行预积分处理，预积分处理包括对加速度计测得的加速度值去除噪声后进行一次积分可得到速度，二次积分可得到相对位移；预积分处理还包括对陀螺仪测得的角速度值去除噪声后进行一次积分可得到移动机器人运动过程中的角度变化。
18.在步骤s1中，利用移动机器人k时刻的imu数据和构建好的容积点获得移动机器人k时刻的预测状态向量x
k|k-1
，具体为：x
k|k-1
＝f
k-1
x
k-1||k-1
，其中，f
k-1
表示基于imu数据获得的k-1时刻和k时刻的移动机器人的状态向量转换关系，优选地，f
k-1
通过imu数据的预积分获得；x
k-1||k-1
表示移动机器人k-1时刻的状态向量；x
k|k-1
＝[p
bk
，v
bk
，q
bk
]，p
bk
和v
bk
分别是移动机器人的三维位姿和速度信息，q
bk
表示从imu坐标系到世界坐标系的四元数。
[0019]
在步骤s1中，利用移动机器人k时刻的预测状态向量x
k|k-1
和系统噪声协方差矩阵qk对移动机器人k时刻的一步预测误差协方差矩阵进行计算，具体为：
[0020]
其中，p
k-1|k-1
表示移动机器人k-1时刻的协方差矩阵，qk表示移动机器人k时刻的系统噪声协方差矩阵。则先验概率密度函数为：p(xk|y
k-1
)＝n(xk；x
k|k-1
p
k|k-1
)。
[0021]
步骤s2，建立k时刻观测噪声的符合基于伽马分布的学生t混合分布的模型，基于移动机器人k时刻的视觉信息、观测噪声模型和预测状态向量获得层次高斯形式的似然函数分布。
[0022]
建立k时刻观测噪声的符合基于伽马分布的学生t混合分布的模型：
[0023][0024]
即
[0025]
其中，vk表示移动机器人k时刻的观测噪声，表示vk的初始值；βk表示形状参数，βk的分布函数为其中，表示第一常实数，y表示第二常实数，im表示m阶单位矩阵；rk表示尺度矩阵；uk表示第一自由度，uk的分布函数为p(uk)＝g(uk；ak，bk)，ak、bk分别是属于uk的伽马分布的第一实数参数和第二实数参数；δk表示第二自由度；λk表示第一辅助参数，λk的分布函数为p(λk)＝g(λk；δk/2，δk/2)；ξk表示第二辅助参数，ξk的分布函数为p(ξk)＝g(ξk；uk/2，uk/2)；n表示高斯分布；g表示伽马分布，gastm表示基于伽马分布的学生t混合分布。
[0026]
提出一种新的分布一一基于伽马分布的学生t混合分布该分布具有位置参数形状参数βk、尺度矩阵rk、第一和第二两个不同的自由度参数uk和δk，它以伽马分布为混合密度函数，对学生t分步进行尺度混合，以应对噪声的非平稳特性。
[0027]
在步骤s2中，基于移动机器人k时刻的视觉信息、观测噪声模型和预测状态向量获得层次高斯形式的似然函数分布的步骤包括：
[0028]
步骤s21，基于移动机器人k时刻的视觉信息获取移动机器人k时刻的视觉状态信息yk，且yk满足yk＝hkx
k|k-1
+vk；其中，hk表示观测矩阵，表示移动机器人k时刻的视觉状态信息yk与预测状态向量x
k|k-1
的映射关系；具体的，根据机器人k时刻的视觉信息提取出视觉信息的orb(oriented fast and rotated brief)特征，根据orb特征获取视觉状态信息yk，获取视觉状态信息的过程为现有技术，在此不再赘述。视觉状态信息yk包括移动机器人的三维位姿和速度信息；
[0029]
步骤s22，建立似然函数分布为：
[0030]
p(yk|x
k|k-1
)＝gastm(yk；hkx
k|k-1
，βk，rk，uk，δk)；
[0031]
似然函数满足gastm分布，将似然函数分布转换为层次高斯形式为：
[0032][0033]
步骤s3，基于似然函数分布构建联合后验概率密度函数，利用变分贝叶斯方法对后验联合概率密度函数进行联合求解获得移动机器人k时刻的状态向量。
[0034]
根据先验概率密度函数p(xk|y
k-1
)＝n(xk；x
k|k-1
，p
k|k-1
)和层次高斯形式的似然函数分布获得联合后验概率密度函数p(xk，λk，ξk，uk，βk，yk)：
[0035][0036]
即：p(xk，λk，ξk，uk，βk，yk)≈q(xk)q(λk)q(ξk)q(uk)q(βk)；
ꢀꢀꢀꢀ
(1)
[0037]
其中，q(xk)表示移动机器人k时刻的状态向量xk的近似概率密度函数；q(λk)表示第一辅助参数λk的近似概率密度函数；q(ξk)表示第二辅助参数ξk的近似概率密度函数；q(uk)表示第一自由度uk的近似概率密度函数；q(βk)表示形状参数βk的近似概率密度函数。因此，只需求解出q(xk)、q(λk)、q(ξk)、q(uk)、q(βk)就能获得最终融合出的机器人k时刻的状态向量xk。
[0038]
利用变分贝叶斯方法对后验联合概率密度函数进行联合求解时xk、λk、ξk、uk、βk的最优解满足：
[0039][0040]
其中，集合θ表示集合θk中的任一元素；θk(-θ)表示由集合θk中除θ外的所有元素组成的子集；c
θ
表示与元素θ无关的常量；e[
·
]表示期望值。
[0041]
在步骤s3中，利用变分贝叶斯方法对后验联合概率密度函数进行联合求解获得k时刻移动机器人的融合状态向量的步骤包括：
[0042]
步骤s31，参数初始化：
[0043][0044]
步骤s32，利用变分贝叶斯方法迭代更新联合后验概率密度函数，每次迭代后分别求取xk、λk、ξk、uk、βk的期望值，若达到迭代停止条件，输出最后一次迭代后获得的移动机器人k时刻的状态向量xk和协方差矩阵p
k|k
。
[0045]
进一步优选地，在步骤s32中，对于第i+1次迭代，执行：
[0046]
步骤s321，将公式1代入公式2，令θ＝xk，可以得到：
[0047][0048]
由上式可以推断出q
(i+1)
(xk)服从高斯分布：q
(i+1)
(xk)表示xk在第i+1次迭代中的近似概率密度函数。其中，在第i+1次迭代中的近似概率密度函数。其中，表示第i+1次迭代中k时刻移动机器人的状态向量，x
k|k-1
表示移动机器人k时刻的预测状态向量，p
k|k-1
表示移动机器人k时刻的一步预测误差协方差矩阵，表示第i+1次迭代的卡尔曼增益，e(i)[λk]表示λk在第i次迭代的期望值，e(i)[βk]表示βk在第i次迭代的期望值，e(i)[ξk]表示ξk在第i次迭代的期望值，表示第i+1次迭代中k时刻移动机器人的协方差矩阵。
[0049]
步骤s322，将公式1代入公式2，令θ＝λk，可以得到：
[0050][0051]
由上式可以分析出无法对q
(i+1)
(λk)进行直接解析，进行近似点处理，可以得到最优解为：
[0052]
[0053]
其中，e(i)[ξk]表示ξk在第i次迭代的期望值，tr{}表示求取矩阵的迹；n1、n2、n3分别为第三辅助参数、第四辅助参数、第五辅助参数；e(i)[(y
k-hkxk)(y
k-hkxk)
t
]表示(y
k-hkxk)(y
k-hkxk)
t
在第i次迭代的期望值；表示在第i次迭代的期望值；m表示单位矩阵im的阶数。
[0054]
步骤s323，公式1代入公式2，令θ＝ξk，可以得到：
[0055][0056]
由上式可以推断出q
(i+1)
(ξk)服从伽马分布，因此ξk在第i+1次迭代中的近似概率密度函数为：
[0057]q(i+1)
(ξk)＝g(ξk；a
(i+1)
，b
(i+1)
)；
[0058]
其中，a
(i+1)
表示第六辅助参数a在第i+1次迭代中的值，b
(i+1)
表示第七辅助参数b在第i+1次迭代中的值，c
(i+1)
表示第八辅助参数c在第i+1次迭代中的值，e(i)[uk]表示uk在第i次迭代的期望值，e(i)[(y
k-hkx
k-λkβk)(y
k-hkx
k-λkβk)
t
]表示(y
k-hkx
k-λkβk)(y
k-hkx
k-λkβk)
t
在第i次迭代的期望值。
[0059]
步骤s324，公式1代入公式2，令θ＝uk，可以得到：
[0060][0061]
由上式可以推断出q
(i+1)
(uk)服从伽马分布，获得uk在第i+1次迭代中的近似概率密度函数为：
[0062]
[0063]
其中，a
k(0)
表示ak的初始值，表示ak在第i+1次迭代中的值，b
k(0)
表示bk的初始值，表示bk在第i+1次迭代中的值。
[0064]
步骤s325，公式1代入公式2，令θ＝βk，可以得到：
[0065][0066]
由上式可以推断出q
(i+1)
(βk)服从高斯分布，获得βk在第i+1次迭代中的近似概率密度函数为：
[0067][0068]
其中，其中，表示βk在第i+1次迭代中的值，表示第八辅助参数k
β
在第i+1次迭代中的值，表示第九辅助参数p
β
在第i+1次迭代中的值。
[0069]
步骤s326，求取第i+1次迭代中各个参数的期望值，这些期望值可作为下一次迭代的输入值或者满足迭代停止条件后的输出值，各期望值：
[0070][0071]e(i+1)
[ξk]＝a
(i+1)
/b
(i+1)
；
[0072]e(i+1)
[uk]＝a
(i+1)
/b
(i+1)
；
[0073][0074][0075][0076][0077][0078]
步骤s327，判断是否满足若满足，将作为k时刻移动机器人的融合状态向量xk输出，将作为k时刻移动机器人的协方差矩阵p
k|k
输出，ε表示预先设置的迭代停止阈值，若不满足，令i＝i+1，返回执行步骤s321到步骤s327。
[0079]
本实施例针对背景技术中提出的卡尔曼滤波不考虑噪声的非平稳特性，影响最终融合后的数据精度降低这一问题，提出一种基于伽马分布的学生t混合卡尔曼滤波slam算法。首先提出一种新的分布-基于伽马分布的学生t混合分布该分布具有位置参数形状参数βk、尺度矩阵rk、第一和第二两个不同的自由度参数uk和δk，它以伽马分布为混合密度函数，对学生t分步进行尺度混合，以应对噪声的非平稳特性。
[0080]
在本实施例中，本slam方法主要过程可分为：第一，利用k-1时刻的状态向量x
k-1|k-1
和imu数据，对k时刻的状态向量xk和一步预测误差协方差矩阵p
k|k-1
进行预测。第二，利用gastm对观测噪声vk进行建模，并利用两个辅助参数推导出层次高斯形式的似然函数分布。第三，利用变分贝叶斯方法对后验联合概率密度函数进行联合求解，依次迭代得到状态向量、辅助参数、第一自由度和形状参数的期望值，保证了imu和视觉传感器测量数据融合的准确性，提高slam的定位精度。该方法在imu和视觉传感器测量数据融合过程中引入了基于变分贝叶斯的信息迭代更新，利用gastm对观测噪声进行建模，分别用来降低非平稳厚尾噪声对融合结果精度的影响，并利用变分贝叶斯实现对移动机器人状态的联合估计，从而提高了最终求得的状态信息的精度。
[0081]
在本实施例的一种应用场景中，图2所示为该应用场景的数据融合流程示意图，首先根据imu数据和当前时刻信息完成下一时刻信息的预测，即利用k-1时刻的信息预测k时刻的信息，之后利用视觉传感器数据获得观测量，并用gastm对观测噪声进行建模，最终利用变分贝叶斯完成对最终结果的求取。
[0082]
实施例2
[0083]
本实施例与实施例1基于同样的发明构思，公开了一种计算机可读存储介质，存储介质中存储有至少一条指令、至少一段程序、代码集或指令集，至少一条指令、至少一段程序、代码集或指令集由处理器加载并执行以实现实施例1提供的基于变分贝叶斯的移动机器人视觉惯性融合slam方法。
[0084]
实施例3
[0085]
基于实施例1的发明构思，本实施例公开了一种移动机器人，包括移动机器人本体和设于所述移动机器人本体上的视觉传感器、imu传感器和处理器，处理器分别与视觉设别和imu传感器连接，所述处理器执行实施例1提供的基于变分贝叶斯的移动机器人视觉惯性融合slam方法的步骤。视觉传感器优选但不限于为摄像头。
[0086]
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
[0087]
尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。