CN115930971A - 一种机器人定位与建图的数据融合处理方法 - Google Patents
一种机器人定位与建图的数据融合处理方法 Download PDFInfo
- Publication number
- CN115930971A CN115930971A CN202310049730.XA CN202310049730A CN115930971A CN 115930971 A CN115930971 A CN 115930971A CN 202310049730 A CN202310049730 A CN 202310049730A CN 115930971 A CN115930971 A CN 115930971A
- Authority
- CN
- China
- Prior art keywords
- time
- representing
- iteration
- offset information
- robot
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 63
- 238000007499 fusion processing Methods 0.000 title claims abstract description 18
- 238000013507 mapping Methods 0.000 title claims abstract description 17
- 239000011159 matrix material Substances 0.000 claims abstract description 162
- 230000004927 fusion Effects 0.000 claims abstract description 22
- 230000000007 visual effect Effects 0.000 claims abstract description 13
- 239000006185 dispersion Substances 0.000 claims description 28
- 238000005259 measurement Methods 0.000 claims description 28
- 238000005315 distribution function Methods 0.000 claims description 16
- 230000007935 neutral effect Effects 0.000 claims description 3
- 238000006386 neutralization reaction Methods 0.000 claims description 3
- 230000004807 localization Effects 0.000 claims 2
- 238000013398 bayesian method Methods 0.000 claims 1
- 230000010354 integration Effects 0.000 description 6
- 230000001133 acceleration Effects 0.000 description 5
- 238000001914 filtration Methods 0.000 description 4
- 239000000463 material Substances 0.000 description 2
- 230000001360 synchronised effect Effects 0.000 description 2
- 238000013519 translation Methods 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 230000005358 geomagnetic field Effects 0.000 description 1
- 238000012804 iterative process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Landscapes
- Image Analysis (AREA)
Abstract
本发明提供了一种机器人定位与建图的数据融合处理方法，包括以下步骤：预测机器人当前时刻的状态向量；根据上一时刻的协方差矩阵和噪声协方差矩阵计算当前时刻预测误差协方差矩阵；预测当前时刻的偏移信息差量；获取机器人的观测量，根据当前时刻的偏移信息差量、预测状态向量、预测误差协方差矩阵和观测量获得联合后验概率密度函数；对初始的迭代参数进行赋值，根据变分推断方法对联合后验概率密度函数进行迭代更新，直至满足变分推断的迭代条件后结束迭代更新，获得当前时刻机器人的融合数据。本发明对机器人状态向量和偏移信息差量的联合估计，降低测量数据和视觉信息的时间差对融合数据的精度影响，提高机器人的自身位姿定位精度。
Description
技术领域
本发明属于机器人定位与建图技术领域，具体涉及一种机器人定位与建图的数据融合处理方法。
背景技术
机器人的同步定位和地图构建(Simultaneous Localization And Mapping，SLAM)是指在机器人在未知环境中移动时提取并组合有用信息，以此确定自身位姿并逐步建立出周围环境地图的过程。SLAM目前已经在移动机器人、无人驾驶、空中无人机等方面得到成功应用，受到的关注越来越多。
现有技术中，提取并组合有用信息获得机器人自身位姿的方法有以下几种：第一种是利用单目相机和卡尔曼滤波(Kalmanfilter,KF)进行位姿优化的方法，优化过程包括计算旋转向量和平移向量的更新值，并将二者的更新值作为优化后的位姿；第二种方法是应用较广泛的基于扩展卡尔曼滤波(extendedKalman filter,EKF)的SLAM算法，该方法利用EKF估计机器人位姿和地图特征位置，在估计机器人位姿和地图特征位置之前还需要利用一阶泰勒展开式对SLAM中的运动模型和观测模型进行线性化；另外还有一种多传感器融合的SLAM方法，移动机器人根据地磁场强度实际测量信息、加速度实际测量信息和已补偿修正的实际角速度测量值获得航姿参考***位姿信息，并计算单目相机位姿信息，根据卡尔曼滤波对单目相机位姿信息和航姿参考***位姿信息进行滤波融合，获取多传感器融合数据，即机器人的自身位姿；与前述两种方法相比，该方法大大地减小了光线对单目相机位姿信息的影响，提高了机器人的自身位姿精度。
但是上述多传感器融合的SLAM方法中，在多传感器的数据融合过程中存在一个融合误差：在对航姿参考***位姿信息和相机位姿信息进行融合时，首先利用多个传感器获得时刻k的航姿参考***位姿信息，再利用其他传感器获得同时刻的相机位姿信息；由于获取信息的传感器不是同一个传感器，而多个传感器获取数据时的时间戳不完全对齐，使多个数据的获取时刻的误差Δt不可避免，即在获取航姿参考***位姿信息的同一时刻k获取的相机位姿信息实际为时刻k+Δt获取的相机位姿信息。非同一时刻的传感器数据融合导致融合后获得的机器人自身位姿精度仍然较低。
发明内容
本发明旨在至少解决现有技术中存在的技术问题，提供一种机器人定位与建图的数据融合处理方法，在融合多个传感器的测量数据过程中，修正多个测量数据之间因非同一时刻获取造成的信息差量，获得准确的融合后的机器人自身位姿，提高SLAM算法的定位精度。
为了实现本发明的上述目的，根据本发明的第一个方面，本发明提供了一种机器人定位与建图的数据融合处理方法，包括以下步骤：根据机器人上一时刻的状态向量、上一时刻的预测误差协方差矩阵和当前时刻的测量数据预测机器人当前时刻的状态向量；根据机器人上一时刻的误差协方差矩阵和噪声协方差矩阵计算机器人当前时刻的预测误差协方差矩阵；根据上一时刻机器人的偏移信息差量预测当前时刻机器人的偏移信息差量；偏移信息差量为视觉信息和测量数据之间的信息差；获取机器人的观测量，根据机器人当前时刻的偏移信息差量、当前时刻的预测状态向量、预测误差协方差矩阵和观测量获得联合后验概率密度函数；对初始的迭代参数进行赋值，根据变分推断方法对联合后验概率密度函数进行迭代更新，直至满足变分推断的迭代条件后结束迭代更新，根据当前时刻的联合后验概率密度函数获得当前时刻机器人的融合数据；机器人的融合数据包括当前时刻的状态向量及预测误差协方差矩阵、当前时刻的偏移信息差量及偏差量协方差矩阵。
进一步地，根据机器人当前时刻的偏移信息差量、当前时刻的预测状态向量、预测误差协方差矩阵和观测量获得联合后验概率密度函数的步骤具体为：根据机器人当前时刻的预测状态向量和当前时刻的预测协方差矩阵建立的先验概率密度函数；根据机器人当前时刻的偏移信息差量和视觉信息建立当前时刻偏移信息差量的高斯分布函数；根据当前机器人的观测量、当前时刻的偏差信息差量建立似然函数；根据先验概率密度函数、高斯分布函数和似然函数获取联合后验概率密度函数。
进一步地，联合后验概率密度函数具体如下：
具体地，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的观测量概率分布。
进一步地，变分推断方法为变分贝叶斯方法，根据变分贝叶斯方法，最优解公式满足：
其中，
θ表示集合Θ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无关的量。
进一步地，根据变分推断方法对联合后验概率密度函数进行迭代更新的步骤还包括：根据当前时刻的联合后验概率密度函数获得状态向量的近似概率密度函数和偏移信息差量的近似概率密度函数；根据当前时刻的联合后验概率密度函数获得当前时刻机器人的融合数据的步骤具体为：根据状态向量的近似概率密度函数获得当前时刻的状态向量及预测误差协方差矩阵；根据偏移信息差量的近似概率密度函数获得当前时刻的偏移信息差量及偏差量协方差矩阵。
进一步地，状态向量的近似概率密度函数的获取过程如下：将最优解公式带入联合后验概率密度函数的对数函数，并令θ＝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的误差协方差矩阵；状态向量和预测误差协方差矩阵根据第一求解公式获得。
进一步地，第一求解公式具体为：
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的预测误差协方差矩阵；Hk T表示观测矩阵Hk的转置矩阵；
表示时刻k似然函数的协方差矩阵；Pk|k时刻k的误差协方差矩阵；Im表示单位矩阵。
进一步地，偏移信息差量的近似概率密度函数的获取过程如下：将最优解公式带入联合后验概率密度函数的对数函数，并令θ＝Δ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的偏移信息差量的误差协方差矩阵；偏移信息差量和偏移信息差量的协方差矩阵根据第二求解公式获得。
进一步地，第二求解公式具体为：
其中，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表示偏移信息差量的误差协方差矩阵；Hk T表示观测矩阵Hk的转置矩阵；
表示时刻k似然函数的协方差矩阵。
进一步地，变分推断的迭代条件具体如下：
其中，i表示迭代次数，k表示时刻，
表示时刻k第i+1次迭代的状态向量，
表示时刻k第i次迭代的状态向量；ε表示迭代阈值；直至满足变分推断的迭代条件后结束迭代更新的步骤具体为：若第i+1次迭代后，当前时刻的状态向量满足变分推断的迭代条件，则结束迭代；若第i+1次迭代后，当前时刻的状态向量不满足变分推断的迭代条件，则将本次迭代的迭代参数的期望值作为下一次迭代的迭代参数开始下一轮的迭代更新。
本发明的技术原理及有益效果：本发明定义偏移信息差量为Δt引起的视觉信息和测量数据之间的信息差，在滤波融合过程中引入偏移信息差量为变量之一，并对偏移信息差量进行高斯建模；利用变分推断方法对迭代过程中的状态向量和偏移信息差量进行更新，根据当前时刻的偏移信息差量对下一时刻的偏移信息差量进行预测；当满足迭代条件后，输出融合数据；本发明在视觉信息和测量数据的融合过程中引入偏移信息差量，在融合视觉信息和测量数据的过程中，修正数据之间因非同一时刻获取造成的信息差量；利用变分推断方法实现对机器人状态向量和偏移信息差量的联合估计，降低测量数据和视觉信息的时间差对融合数据的精度影响，提高机器人的自身位姿定位精度。
附图说明
图1是本发明一种机器人定位与建图的数据融合处理方法的流程示意图。
具体实施方式
下面详细描述本发明的实施例；下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。
机器人在同步定位和地图构建过程中，使用了一种多传感器融合的SLAM方法，根据卡尔曼滤波对单目相机位姿信息和航姿参考位姿信息进行滤波融合，获取多传感器融合数据，即机器人的自身位姿；但是在数据融合过程中存在一个融合误差。本发明为了在融合多个传感器的测量数据过程中，修正多个测量数据之间因非同一时刻获取造成的信息差量，提出了一种机器人定位与建图的数据融合处理方法，本方法能够获得准确的融合后的机器人自身位姿，提高SLAM算法的定位精度。
如附图1所示，本发明提供了一种机器人定位与建图的数据融合处理方法，包括以下步骤：
根据机器人上一时刻的状态向量、上一时刻的预测误差协方差矩阵和当前时刻的测量数据预测机器人当前时刻的状态向量；状态向量包括机器人的三维位置、速度信息和四元数；当机器人为第一时刻时，状态向量和预测误差协方差矩阵根据机器人的惯性测量单元设定获得；
根据机器人上一时刻的误差协方差矩阵和噪声协方差矩阵计算机器人当前时刻的预测误差协方差矩阵；
根据上一时刻机器人的偏移信息差量预测当前时刻机器人的偏移信息差量；偏移信息差量为视觉信息和测量数据之间的信息差；具体地，视觉信息根据相机拍摄照片分析获得；在首次迭代时，根据惯性测量单元和相机人为设置一个初始的偏移信息差量，在之后的迭代过程中对初始的偏移信息差量进行逐步更新；
根据视觉信息获取机器人的观测量；根据机器人当前时刻的偏移信息差量、当前时刻的预测状态向量、预测误差协方差矩阵和观测量获得当前时刻的联合后验概率密度函数；
对初始的迭代参数进行赋值，根据变分推断方法对联合后验概率密度函数进行迭代更新，直至满足变分推断的迭代条件后结束迭代更新，根据当前时刻的联合后验概率密度函数获得当前时刻机器人的融合数据；具体地，机器人的融合数据包括当前时刻的状态向量及预测误差协方差矩阵、当前时刻的偏移信息差量及偏差量协方差矩阵。
具体地，预测机器人当前时刻的状态向量的过程如下：
Xk|k-1＝Fk-1Xk-1|k-1
其中，k表示时刻，Xk|k-1表示时刻k的预测状态向量，Fk-1表示测量数据的预积分，Xk-1|k-1表示时刻k-1的状态向量；预测状态向量Xk|k-1＝[Pbk,Vbk,Qbk]，Pbk和Vbk分别是移动机器人的三维位置和速度信息，Qbk表示从惯性测量单元坐标系到世界坐标系的四元数；
本实施例中，测量数据通过机器人的惯性测量单元获得；惯性测量单元包括加速度器和陀螺仪，测量数据包括加速度值和角速度值，测量数据的预积分的获取过程如下：对加速度计测得的加速度值去除噪声后进行一次积分可得到速度，二次积分可得到相对位移。对陀螺仪测得的角速度值去除噪声后进行一次积分可得到移动机器人运动过程中的角度变化；其中，去除的噪声为加速度传感器静止时输出的数据值；去除噪声的步骤具体为测得的加速度值减去静止时输出的数据值。
具体地，预测误差协方差矩阵如下：
Pk|k-1＝Fk-1Pk-1|k-1Fk-1 T+Qk
其中，Pk|k-1表示时刻k的预测误差协方差矩阵，Fk-1表示测量数据的预积分，Pk-1|k-1时刻k-1的误差协方差矩阵；( )T表示转置矩阵；Fk-1 T表示预计分的转置矩阵；Qk表示***噪声协方差矩阵，***噪声协方差矩阵根据惯性测量单元设定。
具体地，观测量包括机器人的三维位姿和速度信息，且满足：
Yk＝HkXk|k-1+HkΔXk+vk
其中，Yk表示时刻k的观测量，Hk表示时刻k的观测矩阵，为机器人的观测量与状态向量之间的映射关系，本实施例中观测矩阵为前六阶值为1、后续值为0的对角矩阵，ΔXk表示时刻k的偏移信息差量，vk表示时刻k的观测噪声，观测噪声根据相机获得。
具体地，对初始的迭代参数进行赋值的过程如下：
E(0)[ΔXk]＝ΔXk-1
E(0)[Yk-HkXk]＝Yk-HkXk|k-1
其中，E[ ]表示期望值；ΔXk表示时刻k的偏移信息差量，ΔXk-1表示时刻k-1的偏移信息差量，将ΔXk-1作为迭代参数ΔXk首次迭代的期望值，迭代参数ΔXk首次迭代的期望值作为下一次迭代的迭代参数。
进一步地，根据机器人当前时刻的偏移信息差量、当前时刻的预测状态向量、预测误差协方差矩阵和观测量获得联合后验概率密度函数的步骤具体为：
根据机器人当前时刻的预测状态向量和当前时刻的预测协方差矩阵建立先验概率密度函数：
p(Xk|Yk-1)＝N(Xk；Xk|k-1,Pk|k-1)
其中，p(Xk|Yk-1)表示先验概率密度函数，Xk表示时刻k的状态向量，Yk-1表示时刻k-1的观测量，N(；)表示高斯分布函数；Xk|k-1表示时刻k的预测状态向量；Pk|k-1表示时刻k的预测误差协方差矩阵；
根据机器人当前时刻的偏移信息差量和视觉信息建立当前时刻偏移信息差量的高斯分布函数：
p(ΔXk)＝N(ΔXk；ΔXk-1,RΔX)
其中，p(ΔXk)表示高斯分布函数，ΔXk表示时刻k的偏移信息差量，ΔXk-1表示时刻k-1的偏移信息差量，RΔX表示偏移信息差量的误差协方差矩阵；偏移信息差量的误差协方差矩阵根据当前时刻的偏移信息差量获得。
具体地，ΔXk＝[ΔPk,ΔVk,ΔQk]，ΔPk表示时刻k的移动机器人的三维位姿，ΔVk表示时刻k的速度信息，ΔQk表示时刻k的四元数的偏移信息差量。
根据当前机器人的观测量、当前时刻的偏差信息差量建立似然函数：
其中，p(Yk|Xk|k-1)表示似然函数，Yk表示时刻k的观测量，Hk表示时刻k的观测矩阵，Xk表示时刻k的状态向量，ΔXk表示时刻k的偏移信息差量；
表示时刻k的似然函数对应的协方差矩阵；首个时刻的似然函数对应的协方差矩阵根据惯性测量单元人为设定。
根据先验概率密度函数、高斯分布函数和似然函数获取联合后验概率密度函数：
其中，p(Xk,ΔXk|Y1:k)表示联合后验概率密度函数值，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的观测量概率分布。
本实施例中，变分推断方法为变分贝叶斯方法，根据变分贝叶斯方法，最优解公式满足：
其中，
θ表示集合Θk中的任意一个元素；Θk(-θ)表示由Θk中非θ元素；Cθ表示集合Θk中与元素θ无关的量；E[ ]表示期望值；p(Θk,Y1:k)表示联合概率密度函数；
根据变分推断方法对联合后验概率密度函数进行迭代更新的步骤包括：将迭代参数和联合后验概率密度函数带入最优解公式中获得联合后验概率密度函数的对数函数；
其中，k表示时刻；Yk表示时刻k的观测量；Hk表示时刻k的观测矩阵；Xk表示时刻k的状态向量；ΔXk表示时刻k的偏移信息差量；ΔXk-1表示时刻k-1的偏移信息差量；
表示时刻k似然函数的协方差矩阵；Xk|k-1表示时刻k的预测状态向量；Pk|k-1表示时刻k的预测误差协方差矩阵；RΔX表示偏移信息差量的误差协方差矩阵，
表示集合{Xk,ΔXk}中与集合Θk无关的量。
优选地，根据变分推断方法对联合后验概率密度函数进行迭代更新的步骤还包括：
根据当前时刻的联合后验概率密度函数获得状态向量的近似概率密度函数和偏移信息差量的近似概率密度函数；
根据当前时刻的联合后验概率密度函数获得当前时刻机器人的融合数据的步骤具体为：根据状态向量的近似概率密度函数获得当前时刻的状态向量及预测误差协方差矩阵；根据偏移信息差量的近似概率密度函数获得当前时刻的偏移信息差量及偏差量协方差矩阵。具体地：
p(Xk,ΔXk|Y1:k)≈q(Xk)q(ΔXk)
其中，p(Xk,ΔXk|Y1:k)表示时刻k的联合后验概率密度函数，q(Xk)表示时刻k的状态向量的近似概率密度函数，q(ΔXk)表示时刻k的偏移信息差量的近似概率密度函数；
优选地，状态向量的近似概率密度函数的获取过程如下：将最优解公式带入联合后验概率密度函数的对数函数，并令θ＝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的误差协方差矩阵；状态向量和预测误差协方差矩阵根据第一求解公式获得。
具体地，第一求解公式具体为：
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的预测误差协方差矩阵；Hk T表示观测矩阵Hk的转置矩阵；
表示时刻k似然函数的协方差矩阵；Pk|k时刻k的误差协方差矩阵；Im表示单位矩阵。
优选地，偏移信息差量的近似概率密度函数的获取过程如下：将最优解公式带入联合后验概率密度函数的对数函数，并令θ＝Δ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次迭代的偏移信息差量；偏移信息差量和偏移信息差量的协方差矩阵根据第二求解公式获得。
具体地，第二求解公式具体为：
其中，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表示偏移信息差量的误差协方差矩阵；Hk T表示观测矩阵Hk的转置矩阵；
表示时刻k似然函数的协方差矩阵。
优选地，变分推断的迭代条件具体如下：
其中，i表示迭代次数，k表示时刻，
表示时刻k第i+1次迭代的状态向量，
表示时刻k第i次迭代的状态向量；ε表示迭代阈值；迭代阈值通过人为设定获得。
直至满足变分推断的迭代条件后结束迭代更新的步骤具体为：
若第i+1次迭代后，当前时刻的状态向量满足变分推断的迭代条件，则结束迭代；
若第i+1次迭代后，当前时刻的状态向量不满足变分推断的迭代条件，则将本次迭代的迭代参数的期望值作为下一次迭代的迭代参数开始下一轮的迭代更新。
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。
Claims (10)
1.一种机器人定位与建图的数据融合处理方法，其特征在于，包括以下步骤：
根据机器人上一时刻的状态向量、上一时刻的预测误差协方差矩阵和当前时刻的测量数据预测机器人当前时刻的状态向量；根据机器人上一时刻的误差协方差矩阵和噪声协方差矩阵计算机器人当前时刻的预测误差协方差矩阵；
根据上一时刻机器人的偏移信息差量预测当前时刻机器人的偏移信息差量；偏移信息差量为视觉信息和测量数据之间的信息差；
获取机器人的观测量，根据机器人当前时刻的偏移信息差量、当前时刻的预测状态向量、预测误差协方差矩阵和观测量获得联合后验概率密度函数；
对初始的迭代参数进行赋值，根据变分推断方法对联合后验概率密度函数进行迭代更新，直至满足变分推断的迭代条件后结束迭代更新，根据当前时刻的联合后验概率密度函数获得当前时刻机器人的融合数据；机器人的融合数据包括当前时刻的状态向量及预测误差协方差矩阵、当前时刻的偏移信息差量及偏差量协方差矩阵。
2.如权利要求1所述的一种机器人定位与建图的数据融合处理方法，其特征在于，根据机器人当前时刻的偏移信息差量、当前时刻的预测状态向量、预测误差协方差矩阵和观测量获得联合后验概率密度函数的步骤具体为：
根据机器人当前时刻的预测状态向量和当前时刻的预测协方差矩阵建立先验概率密度函数；
根据机器人当前时刻的偏移信息差量和视觉信息建立当前时刻偏移信息差量的高斯分布函数；
根据当前机器人的观测量、当前时刻的偏差信息差量建立似然函数；
根据先验概率密度函数、高斯分布函数和似然函数获取联合后验概率密度函数。
3.如权利要求2所述的一种机器人定位与建图的数据融合处理方法，其特征在于，联合后验概率密度函数具体如下：
具体地，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的观测量概率分布。
4.如权利要求3所述的一种机器人定位与建图的数据融合处理方法，其特征在于，变分推断方法为变分贝叶斯方法，根据变分贝叶斯方法，最优解公式满足：
其中，
θ表示集合Θ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无关的量。
5.如权利要求4所述的一种机器人定位与建图的数据融合处理方法，其特征在于，根据变分推断方法对联合后验概率密度函数进行迭代更新的步骤还包括：根据当前时刻的联合后验概率密度函数获得状态向量的近似概率密度函数和偏移信息差量的近似概率密度函数；
根据当前时刻的联合后验概率密度函数获得当前时刻机器人的融合数据的步骤具体为：根据状态向量的近似概率密度函数获得当前时刻的状态向量及预测误差协方差矩阵；根据偏移信息差量的近似概率密度函数获得当前时刻的偏移信息差量及偏差量协方差矩阵。
6.如权利要求5所述的一种机器人定位与建图的数据融合处理方法，其特征在于，状态向量的近似概率密度函数的获取过程如下：
将最优解公式带入联合后验概率密度函数的对数函数，并令θ＝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的误差协方差矩阵；状态向量和预测误差协方差矩阵根据第一求解公式获得。
7.如权利要求6所述的一种机器人定位与建图的数据融合处理方法，其特征在于，第一求解公式具体为：
Pk|k＝(Im-KkHK)Pk|k-1
其中，i表示迭代次数，k表示时刻；
表示时刻k第i+1次迭代的状态向量，Xk|k-1表示时刻k的预测状态向量；Kk表示时刻k的增益矩阵；Yk表示时刻k的观测量；Hk表示时刻k的观测矩阵；Hk表示时刻k的状态向量；ΔXk表示时刻k的偏移信息差量，ΔXk为第i次迭代的迭代参数；E(i)[ΔXk]表示第i次迭代ΔXk的期望值；E(i)[ΔXk]表示第i次迭代ΔXk的期望值；Pk|k-1表示时刻k的预测误差协方差矩阵；Hk T表示观测矩阵Hk的转置矩阵；
表示时刻k似然函数的协方差矩阵；Pk|k时刻k的误差协方差矩阵；Im表示单位矩阵。
8.如权利要求5所述的一种机器人定位与建图的数据融合处理方法，其特征在于，偏移信息差量的近似概率密度函数的获取过程如下：将最优解公式带入联合后验概率密度函数的对数函数，并令θ＝Δ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的偏移信息差量的误差协方差矩阵；偏移信息差量和偏移信息差量的协方差矩阵根据第二求解公式获得。
9.如权利要求8所述的一种机器人定位与建图的数据融合处理方法，其特征在于，第二求解公式具体为：
其中，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表示偏移信息差量的误差协方差矩阵；Hk T表示观测矩阵Hk的转置矩阵；
表示时刻k似然函数的协方差矩阵。
10.如权利要求1、2、3、4、5、6、7、8或9所述的一种机器人定位与建图的数据融合处理方法，其特征在于，变分推断的迭代条件具体如下：
其中，i表示迭代次数，k表示时刻，
表示时刻k第i+1次迭代的状态向量，
表示时刻k第i次迭代的状态向量；ε表示迭代阈值；
直至满足变分推断的迭代条件后结束迭代更新的步骤具体为：若第i+1次迭代后，当前时刻的状态向量满足变分推断的迭代条件，则结束迭代；若第i+1次迭代后，当前时刻的状态向量不满足变分推断的迭代条件，则将本次迭代的迭代参数的期望值作为下一次迭代的迭代参数开始下一轮的迭代更新。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310049730.XA CN115930971B (zh) | 2023-02-01 | 2023-02-01 | 一种机器人定位与建图的数据融合处理方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310049730.XA CN115930971B (zh) | 2023-02-01 | 2023-02-01 | 一种机器人定位与建图的数据融合处理方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115930971A true CN115930971A (zh) | 2023-04-07 |
| CN115930971B CN115930971B (zh) | 2023-09-19 |
Family
ID=86651027
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310049730.XA Active CN115930971B (zh) | 2023-02-01 | 2023-02-01 | 一种机器人定位与建图的数据融合处理方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115930971B (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118034304A (zh) * | 2024-03-05 | 2024-05-14 | 广州市东鼎智能装备有限公司 | 一种基于实时建模的机器人路径规划方法及*** |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106052678A (zh) * | 2016-05-23 | 2016-10-26 | 中国空间技术研究院 | 一种聚合式星敏感器及其卫星姿态确定方法 |
| CN109376785A (zh) * | 2018-10-31 | 2019-02-22 | 东南大学 | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 |
| CN109508445A (zh) * | 2019-01-14 | 2019-03-22 | 哈尔滨工程大学 | 一种带有色量测噪声和变分贝叶斯自适应卡尔曼滤波的目标跟踪方法 |
| CN110345944A (zh) * | 2019-05-27 | 2019-10-18 | 浙江工业大学 | 融合视觉特征和imu信息的机器人定位方法 |
| US20200356582A1 (en) * | 2019-05-09 | 2020-11-12 | Ankobot (Shenzhen) Smart Technologies Co., Ltd. | Method for updating a map and mobile robot |
| WO2021035669A1 (zh) * | 2019-08-30 | 2021-03-04 | 深圳市大疆创新科技有限公司 | 位姿预测方法、地图构建方法、可移动平台及存储介质 |
| WO2022062177A1 (zh) * | 2020-09-28 | 2022-03-31 | 南京邮电大学 | 一种基于动态环境的自适应室内融合定位方法 |
| CN115388899A (zh) * | 2022-09-15 | 2022-11-25 | 七腾机器人有限公司 | 基于变分贝叶斯的移动机器人视觉惯性融合slam方法 |
-
2023
- 2023-02-01 CN CN202310049730.XA patent/CN115930971B/zh active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106052678A (zh) * | 2016-05-23 | 2016-10-26 | 中国空间技术研究院 | 一种聚合式星敏感器及其卫星姿态确定方法 |
| CN109376785A (zh) * | 2018-10-31 | 2019-02-22 | 东南大学 | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 |
| WO2020087846A1 (zh) * | 2018-10-31 | 2020-05-07 | 东南大学 | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 |
| CN109508445A (zh) * | 2019-01-14 | 2019-03-22 | 哈尔滨工程大学 | 一种带有色量测噪声和变分贝叶斯自适应卡尔曼滤波的目标跟踪方法 |
| US20200356582A1 (en) * | 2019-05-09 | 2020-11-12 | Ankobot (Shenzhen) Smart Technologies Co., Ltd. | Method for updating a map and mobile robot |
| CN110345944A (zh) * | 2019-05-27 | 2019-10-18 | 浙江工业大学 | 融合视觉特征和imu信息的机器人定位方法 |
| WO2021035669A1 (zh) * | 2019-08-30 | 2021-03-04 | 深圳市大疆创新科技有限公司 | 位姿预测方法、地图构建方法、可移动平台及存储介质 |
| WO2022062177A1 (zh) * | 2020-09-28 | 2022-03-31 | 南京邮电大学 | 一种基于动态环境的自适应室内融合定位方法 |
| CN115388899A (zh) * | 2022-09-15 | 2022-11-25 | 七腾机器人有限公司 | 基于变分贝叶斯的移动机器人视觉惯性融合slam方法 |
Non-Patent Citations (1)
| Title |
|---|
| 余哲琳: "基于视觉惯导融合的智能车辆定位技术研究", 中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑, no. 5, pages 035 - 374 * |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118034304A (zh) * | 2024-03-05 | 2024-05-14 | 广州市东鼎智能装备有限公司 | 一种基于实时建模的机器人路径规划方法及*** |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115930971B (zh) | 2023-09-19 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111207774B (zh) | 一种用于激光-imu外参标定的方法及*** | |
| CN110706279B (zh) | 基于全局地图与多传感器信息融合的全程位姿估计方法 | |
| CN111136660B (zh) | 机器人位姿定位方法及*** | |
| CN110068335B (zh) | 一种gps拒止环境下无人机集群实时定位方法及*** | |
| CN110398257B (zh) | Gps辅助的sins***快速动基座初始对准方法 | |
| Lupton et al. | Visual-inertial-aided navigation for high-dynamic motion in built environments without initial conditions | |
| CN110986939B (zh) | 一种基于imu预积分的视觉惯性里程计方法 | |
| CN107014376B (zh) | 一种适用于农业机械精准作业的姿态倾角估计方法 | |
| CN111288989B (zh) | 一种小型无人机视觉定位方法 | |
| CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
| CN111337020A (zh) | 引入抗差估计的因子图融合定位方法 | |
| CN111795686A (zh) | 一种移动机器人定位与建图的方法 | |
| CN114485643B (zh) | 一种煤矿井下移动机器人环境感知与高精度定位方法 | |
| CN110929402A (zh) | 一种基于不确定分析的概率地形估计方法 | |
| CN115930971B (zh) | 一种机器人定位与建图的数据融合处理方法 | |
| CN115388899A (zh) | 基于变分贝叶斯的移动机器人视觉惯性融合slam方法 | |
| Peng et al. | Vehicle odometry with camera-lidar-IMU information fusion and factor-graph optimization | |
| CN115218906A (zh) | 面向室内slam的视觉惯性融合定位方法及*** | |
| CN114690229A (zh) | 一种融合gps的移动机器人视觉惯性导航方法 | |
| CN113503872B (zh) | 一种基于相机与消费级imu融合的低速无人车定位方法 | |
| CN114397642A (zh) | 一种基于图优化的三维激光雷达与imu外参标定方法 | |
| CN117388830A (zh) | 激光雷达与惯性导航的外参标定方法、装置、设备及介质 | |
| CN115597595B (zh) | 多线激光定位方法及定位装置、计算机设备、存储介质 | |
| CN111982126A (zh) | 一种全源BeiDou/SINS弹性状态观测器模型设计方法 | |
| Bai et al. | Graph-optimisation-based self-calibration method for IMU/odometer using preintegration theory |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| PE01 | Entry into force of the registration of the contract for pledge of patent right | ||
| PE01 | Entry into force of the registration of the contract for pledge of patent right |
Denomination of invention: A Data Fusion Processing Method for Robot Localization and Mapping Granted publication date: 20230919 Pledgee: Industrial and Commercial Bank of China Limited Chongqing Liangjiang Branch Pledgor: Seven Teng Robot Co.,Ltd. Registration number: Y2024980010233 |