一种机器人定位与建图的数据融合处理方法与流程

本发明属于机器人定位与建图，具体涉及一种机器人定位与建图的数据融合处理方法。

背景技术：

1、机器人的同步定位和地图构建(simultaneous localization and mapping，slam)是指在机器人在未知环境中移动时提取并组合有用信息，以此确定自身位姿并逐步建立出周围环境地图的过程。slam目前已经在移动机器人、无人驾驶、空中无人机等方面得到成功应用，受到的关注越来越多。

2、现有技术中，提取并组合有用信息获得机器人自身位姿的方法有以下几种：第一种是利用单目相机和卡尔曼滤波(kalmanfilter,kf)进行位姿优化的方法，优化过程包括计算旋转向量和平移向量的更新值，并将二者的更新值作为优化后的位姿；第二种方法是应用较广泛的基于扩展卡尔曼滤波(extendedkalman filter,ekf)的slam算法，该方法利用ekf估计机器人位姿和地图特征位置，在估计机器人位姿和地图特征位置之前还需要利用一阶泰勒展开式对slam中的运动模型和观测模型进行线性化；另外还有一种多传感器融合的slam方法，移动机器人根据地磁场强度实际测量信息、加速度实际测量信息和已补偿修正的实际角速度测量值获得航姿参考系统位姿信息，并计算单目相机位姿信息，根据卡尔曼滤波对单目相机位姿信息和航姿参考系统位姿信息进行滤波融合，获取多传感器融合数据，即机器人的自身位姿；与前述两种方法相比，该方法大大地减小了光线对单目相机位姿信息的影响，提高了机器人的自身位姿精度。

3、但是上述多传感器融合的slam方法中，在多传感器的数据融合过程中存在一个融合误差：在对航姿参考系统位姿信息和相机位姿信息进行融合时，首先利用多个传感器获得时刻k的航姿参考系统位姿信息，再利用其他传感器获得同时刻的相机位姿信息；由于获取信息的传感器不是同一个传感器，而多个传感器获取数据时的时间戳不完全对齐，使多个数据的获取时刻的误差δt不可避免，即在获取航姿参考系统位姿信息的同一时刻k获取的相机位姿信息实际为时刻k+δt获取的相机位姿信息。非同一时刻的传感器数据融合导致融合后获得的机器人自身位姿精度仍然较低。

技术实现思路

1、本发明旨在至少解决现有技术中存在的技术问题，提供一种机器人定位与建图的数据融合处理方法，在融合多个传感器的测量数据过程中，修正多个测量数据之间因非同一时刻获取造成的信息差量，获得准确的融合后的机器人自身位姿，提高slam算法的定位精度。

2、为了实现本发明的上述目的，根据本发明的第一个方面，本发明提供了一种机器人定位与建图的数据融合处理方法，包括以下步骤：根据机器人上一时刻的状态向量、上一时刻的预测误差协方差矩阵和当前时刻的测量数据预测机器人当前时刻的状态向量；根据机器人上一时刻的误差协方差矩阵和噪声协方差矩阵计算机器人当前时刻的预测误差协方差矩阵；根据上一时刻机器人的偏移信息差量预测当前时刻机器人的偏移信息差量；偏移信息差量为视觉信息和测量数据之间的信息差；获取机器人的观测量，根据机器人当前时刻的偏移信息差量、当前时刻的预测状态向量、预测误差协方差矩阵和观测量获得联合后验概率密度函数；对初始的迭代参数进行赋值，根据变分推断方法对联合后验概率密度函数进行迭代更新，直至满足变分推断的迭代条件后结束迭代更新，根据当前时刻的联合后验概率密度函数获得当前时刻机器人的融合数据；机器人的融合数据包括当前时刻的状态向量及预测误差协方差矩阵、当前时刻的偏移信息差量及偏差量协方差矩阵。

3、进一步地，根据机器人当前时刻的偏移信息差量、当前时刻的预测状态向量、预测误差协方差矩阵和观测量获得联合后验概率密度函数的步骤具体为：根据机器人当前时刻的预测状态向量和当前时刻的预测协方差矩阵建立的先验概率密度函数；根据机器人当前时刻的偏移信息差量和视觉信息建立当前时刻偏移信息差量的高斯分布函数；根据当前机器人的观测量、当前时刻的偏差信息差量建立似然函数；根据先验概率密度函数、高斯分布函数和似然函数获取联合后验概率密度函数。

4、进一步地，联合后验概率密度函数具体如下：具体地，p(xk,δxk|y1:k)表示联合后验概率密度函数值，n(；)表示高斯分布函数；k表示时刻；yk表示时刻k的观测量；hk表示时刻k的观测矩阵；xk表示时刻k的状态向量；δxk表示时刻k的偏移信息差量；δxk-1表示时刻k-1的偏移信息差量；表示时刻k似然函数的协方差矩阵；xk|k-1表示时刻k的预测状态向量；pk|k-1表示时刻k的预测误差协方差矩阵；rδx表示偏移信息差量的误差协方差矩阵；p(y1:k-1)表示时刻1至时刻k-1的观测量概率分布。

5、进一步地，变分推断方法为变分贝叶斯方法，根据变分贝叶斯方法，最优解公式满足：其中，θ表示集合θk中的任意一个元素；θk(-θ)表示由θk中非θ元素；cθ表示集合θk中与元素θ无关的量；e[]表示期望值；p(θk,y1:k)表示联合概率密度函数；根据变分推断方法对联合后验概率密度函数进行迭代更新的步骤包括：将迭代参数和联合后验概率密度函数带入最优解公式中获得当前时刻的联合后验概率密度函数；联合后验概率密度函数的对数函数如下：其中，k表示时刻；yk表示时刻k的观测量；hk表示时刻k的观测矩阵；xk表示时刻k的状态向量；δxk表示时刻k的偏移信息差量；δxk-1表示时刻k-1的偏移信息差量；( )t表示转置矩阵；表示时刻k似然函数的协方差矩阵；xk|k-1表示时刻k的预测状态向量；pk|k-1表示时刻k的预测误差协方差矩阵；rδx表示偏移信息差量的误差协方差矩阵，表示集合{xk,δxk}中与集合θk无关的量。

6、进一步地，根据变分推断方法对联合后验概率密度函数进行迭代更新的步骤还包括：根据当前时刻的联合后验概率密度函数获得状态向量的近似概率密度函数和偏移信息差量的近似概率密度函数；根据当前时刻的联合后验概率密度函数获得当前时刻机器人的融合数据的步骤具体为：根据状态向量的近似概率密度函数获得当前时刻的状态向量及预测误差协方差矩阵；根据偏移信息差量的近似概率密度函数获得当前时刻的偏移信息差量及偏差量协方差矩阵。

7、进一步地，状态向量的近似概率密度函数的获取过程如下：将最优解公式带入联合后验概率密度函数的对数函数，并令θ＝xk，获得：其中，i表示迭代次数，k表示时刻；q(i+1)(xk)表示时刻k第i+1次迭代后状态向量的近似概率密度函数；yk表示时刻k的观测量；hk表示时刻k的观测矩阵；xk表示时刻k的状态向量；δxk表示时刻k的偏移信息差量，δxk为第i次迭代的迭代参数；e(i)[δxk]表示第i次迭代δxk的期望值；( )t表示转置矩阵；表示时刻k似然函数的协方差矩阵；xk|k-1表示时刻k的预测状态向量；pk|k-1表示时刻k的预测误差协方差矩阵；表示集合θk中与xk无关的量；根据log q(i+1)(xk)的公式判断状态向量的近似概率密度函数服从高斯分布，则状态向量的近似概率密度函数如下：其中，n(；)表示高斯分布函数；表示时刻k第i+1次迭代的状态向量，pk|k时刻k的误差协方差矩阵；状态向量和预测误差协方差矩阵根据第一求解公式获得。

8、进一步地，第一求解公式具体为：pk|k＝(im-kkhk)pk|k-1；其中，i表示迭代次数，k表示时刻；表示时刻k第i+1次迭代的状态向量，xk|k-1表示时刻k的预测状态向量；kk表示时刻k的增益矩阵；yk表示时刻k的观测量；hk表示时刻k的观测矩阵；xk表示时刻k的状态向量；δxk表示时刻k的偏移信息差量，δxk为第i次迭代的迭代参数；e(i)[δxk]表示第i次迭代δxk的期望值；e(i)[δxk]表示第i次迭代δxk的期望值；pk|k-1表示时刻k的预测误差协方差矩阵；hkt表示观测矩阵hk的转置矩阵；表示时刻k似然函数的协方差矩阵；pk|k时刻k的误差协方差矩阵；im表示单位矩阵。

9、进一步地，偏移信息差量的近似概率密度函数的获取过程如下：将最优解公式带入联合后验概率密度函数的对数函数，并令θ＝δxk，获得：其中，i表示迭代次数，k表示时刻；q(i+1)(δxk)表示时刻k第i+1次迭代后的偏移信息差量的近似概率密度函数；yk表示时刻k的观测量；hk表示时刻k的观测矩阵；xk表示时刻k的状态向量；yk-hkxk为第i次迭代的迭代参数，e(i)[yk-hkxk]表示第i次迭代时迭代参数(yk-hkxk)的期望值；δxk表示时刻k的偏移信息差量；()t表示转置矩阵；表示时刻k似然函数的协方差矩阵；δxk-1表示时刻k-1的偏移信息差量；rδx表示偏移信息差量的误差协方差矩阵；表示集合θk中与δxk无关的量；根据log q(i+1)(δxk)的公式判断偏移信息差量的近似概率密度函数服从高斯分布，则偏移信息差量的近似概率密度函数如下：其中，n(；)表示高斯分布函数；表示时刻k第i+1次迭代的偏移信息差量；表示时刻k的偏移信息差量的误差协方差矩阵；偏移信息差量和偏移信息差量的协方差矩阵根据第二求解公式获得。

10、进一步地，第二求解公式具体为：其中，i表示迭代次数，k表示时刻；表示时刻k第i+1次迭代的偏移信息差量；δxk-1表示时刻k-1的偏移信息差量；kδx表示偏移信息差量的增益矩阵；yk表示时刻k的观测量；hk表示时刻k的观测矩阵；xk表示时刻k的状态向量；e(i)[yk-hkxk]表示第i次迭代(yk-hkxk)的期望值，e(i)[yk-hkxk]为第i次迭代的迭代参数；表示时刻k的偏移信息差量的误差协方差矩阵；im表示单位矩阵；rδx表示偏移信息差量的误差协方差矩阵；hkt表示观测矩阵hk的转置矩阵；表示时刻k似然函数的协方差矩阵。

11、进一步地，变分推断的迭代条件具体如下：其中，i表示迭代次数，k表示时刻，表示时刻k第i+1次迭代的状态向量，表示时刻k第i次迭代的状态向量；ε表示迭代阈值；直至满足变分推断的迭代条件后结束迭代更新的步骤具体为：若第i+1次迭代后，当前时刻的状态向量满足变分推断的迭代条件，则结束迭代；若第i+1次迭代后，当前时刻的状态向量不满足变分推断的迭代条件，则将本次迭代的迭代参数的期望值作为下一次迭代的迭代参数开始下一轮的迭代更新。

12、本发明的技术原理及有益效果：本发明定义偏移信息差量为δt引起的视觉信息和测量数据之间的信息差，在滤波融合过程中引入偏移信息差量为变量之一，并对偏移信息差量进行高斯建模；利用变分推断方法对迭代过程中的状态向量和偏移信息差量进行更新，根据当前时刻的偏移信息差量对下一时刻的偏移信息差量进行预测；当满足迭代条件后，输出融合数据；本发明在视觉信息和测量数据的融合过程中引入偏移信息差量，在融合视觉信息和测量数据的过程中，修正数据之间因非同一时刻获取造成的信息差量；利用变分推断方法实现对机器人状态向量和偏移信息差量的联合估计，降低测量数据和视觉信息的时间差对融合数据的精度影响，提高机器人的自身位姿定位精度。