CN115950414A - 一种不同传感器数据的自适应多重融合slam方法 - Google Patents
一种不同传感器数据的自适应多重融合slam方法 Download PDFInfo
- Publication number
- CN115950414A CN115950414A CN202310114324.7A CN202310114324A CN115950414A CN 115950414 A CN115950414 A CN 115950414A CN 202310114324 A CN202310114324 A CN 202310114324A CN 115950414 A CN115950414 A CN 115950414A
- Authority
- CN
- China
- Prior art keywords
- mobile robot
- point cloud
- data
- map
- pose
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 54
- 230000003044 adaptive effect Effects 0.000 title claims description 10
- 230000000007 visual effect Effects 0.000 claims abstract description 73
- 238000001514 detection method Methods 0.000 claims abstract description 21
- 238000005457 optimization Methods 0.000 claims abstract description 21
- 230000004927 fusion Effects 0.000 claims abstract description 9
- 238000005259 measurement Methods 0.000 claims description 21
- 230000008569 process Effects 0.000 claims description 20
- 238000012545 processing Methods 0.000 claims description 13
- 230000009466 transformation Effects 0.000 claims description 9
- 238000013528 artificial neural network Methods 0.000 claims description 8
- 230000001133 acceleration Effects 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 5
- 238000013527 convolutional neural network Methods 0.000 claims description 4
- 238000012937 correction Methods 0.000 claims description 3
- 238000012886 linear function Methods 0.000 claims description 3
- 239000011159 matrix material Substances 0.000 claims description 3
- 238000010606 normalization Methods 0.000 claims description 3
- 230000009467 reduction Effects 0.000 claims description 3
- 238000012360 testing method Methods 0.000 claims description 3
- 238000012549 training Methods 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 230000006872 improvement Effects 0.000 claims description 2
- 238000010586 diagram Methods 0.000 description 7
- 238000013507 mapping Methods 0.000 description 5
- 230000000694 effects Effects 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- LFQSCWFLJHTTHZ-UHFFFAOYSA-N Ethanol Chemical compound CCO LFQSCWFLJHTTHZ-UHFFFAOYSA-N 0.000 description 2
- 238000000265 homogenisation Methods 0.000 description 2
- 238000007689 inspection Methods 0.000 description 2
- 238000013473 artificial intelligence Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000009826 distribution Methods 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 238000009776 industrial production Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 238000011160 research Methods 0.000 description 1
Images
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开的是一种不同传感器数据的自适应多重融合SLAM方法，属于移动机器人自主导航领域。首先获取移动机器人的传感器数据并进行处理，同时提取到深度相机获得的图像的特征，生成全局深度视觉信息和局部深度视觉信息。然后利用改进的IMLS‑ICP算法将激光雷达数据、全局深度视觉信息和局部深度视觉信息进行点云匹配，生成点云地图，并利用改进的模糊自适应UKF算法进行数据融合，得到移动机器人的位姿。最后对移动机器人的运动轨迹进行闭环检测，将机器人位姿、点云地图和闭环检测结果作为约束进行联合优化，得到移动机器人的路径地图。本发明减小了移动机器人的位姿估计误差，移动机器人所建地图更加准确，提升了移动机器人对所处环境的鲁棒性。
Description
技术领域
本发明属于移动机器人自主导航领域，具体是一种在未知室内环境下能充分利用不同传感器数据的自适应多重融合SLAM方法。
背景技术
随着科学技术的进步，机器人得到了快速的发展。机器人技术从最初的工业生产，逐渐地向物流、会议和医疗等领域普及。移动机器人可以通过获取所处环境数据和自己在所处环境中的位置，自主地进行导航，因此在多个领域中得到了普遍的使用。随着传感器和人工智能技术的发展，移动机器人同时定位与建图(SLAM)方法成为了近些年移动机器人自主导航领域中研究的热点。
2D激光雷达由于成本较低、精度高、抗干扰能力强且实时性好，在SLAM中得到了广泛的应用。2D激光雷达SLAM目前存在激光点云配准计算量大、位姿估计累计误差大、闭环检测效果差、容易产生噪声和泄漏以及传感器数据利用不充分等问题。
当前已经有许多方法来解决移动机器人2D激光雷达SLAM问题，比如Gmapping算法、Karto算法和Hector算法等。Gmapping利用RaoBlackwellized粒子滤波器(RBPF)来构建SLAM系统，但算法的计算量大，且不适用于大场景。Karto算法引入了后端优化和闭环检测，有效减少了累计误差，但算法比较耗时。Cartographer算法是一种基于图优化的2D激光雷达SLAM算法，分为前端和后端两部分。前端利用滤波方法融合激光雷达点云数据和IMU数据获得机器人位姿，同时利用帧间匹配技术构建子地图，后端根据子地图间的约束关系进行优化，来修正子地图的位姿节点完成地图构建。该算法构建的地图在准确度上没有明显提升，且定位精度较低。
发明内容
本发明针对现有移动机器人2D激光雷达SLAM存在的问题，以及移动机器人在室内环境中进行定位建图的需求，在Cartographer算法的基础上进行改进，提出一种不同传感器数据的自适应多重融合SLAM方法。
所述不同传感器数据的自适应多重融合SLAM方法，具体步骤如下：
步骤1：针对处于某未知室内环境中的移动机器人，获取其传感器数据并进行处理，同时提取到深度相机获得的图像的特征，生成全局深度视觉信息和局部深度视觉信息。
1)生成全局深度视觉信息的过程为：
首先，获得深度相机的灰度图，并利用GCNv2神经网络对灰度图进行处理，生成特征图。
然后，将特征图上的特征点进行均匀化处理。
最后，通过损失函数训练和二值描述分别对均匀化的特征点进行处理，生成特征点云和描述符并保存，作为全局深度视觉信息。
损失函数如下：
Ldet＝Lce(ocur(xcur,0))+Lce(otar(xtar,xcur))
其中，(xcur,0)是特征点在当前帧的位置，(xtar,xcur)是xcur在目标帧中匹配的位置。Lce是加权交叉熵。ocur是当前帧的概率距离，otar是目标帧的概率距离。
二值描述公式如下：
其中，b(x)是根据f(x)的值区分得到的特征值，f(x)是特征点的观察概率。
2)生成局部深度视觉信息的过程如下：
首先，以2D激光雷达圆心为原点，利用IMU(Inertial Measurement Unit，惯性测量装置)数据、深度相机平面投影与激光雷达二维坐标的一致性，构建移动机器人的全局坐标系并得到IMU的观测数据。
假设移动机器人做匀变速运动，在全局坐标系中，移动机器人的状态变量如下：
上式中
分别是移动机器人tk时刻在全局坐标系中的横坐标、纵坐标和航向角，vk和ωk分别是移动机器人tk时刻的线速度和角速度。
利用全局坐标系，得到IMU的观测数据，由于IMU输出频率很高，[tk-1,tk]时间段可分为n段，其公式如下：
上式中
和
分别是移动机器人的角速度测量值和加速度测量值，
和
分别是移动机器人的角速度实际值和加速度实际值，
和
分别是陀螺仪偏差和加速度计偏差，ng和na分别是采用高斯白噪声进行建模的IMU测量噪声。
同时，获取深度相机数据，利用改进后的LPP算法提取深度相机获得的图像的局部特征。
结合移动机器人运动一致性改进后的LPP算法公式如下：
其中，i,j是全局坐标系中移动机器人经过的两个不同位置，Xij是全局坐标系中移动机器人i,j两点间的运动状态，li,lj是全局坐标系中i,j两点到初始坐标点的距离，Wij是全局坐标系中i,j两点间距离权重系数矩阵。
然后，图像的局部特征结合IMU观测数据对移动机器人运动的描述，用引入加权目标函数的R-CNN神经网络生成局部深度视觉信息。
加权目标函数如下：
其中，Lexist是既有图像特征，Ladd是新增图像特征，ρ(ω,a)是IMU观测数据权重。
步骤2：利用改进的IMLS-ICP算法将激光雷达数据、全局深度视觉信息和局部深度视觉信息进行点云匹配，用于数据融合和生成点云地图。
点云匹配的具体步骤如下：
步骤201：利用局部深度视觉信息得到的移动机器人状态量之间的变换，去除激光雷达数据中的运动畸变。
步骤202：删去地面点云和聚类，根据移动机器人和激光雷达数据点云、全局深度视觉信息的特征点云以及局部深度视觉信息点云之间的运动变换，去除全局坐标系中的移动物体。
步骤203：利用激光点云的中心点到视觉参考点云的之间一定范围内的点云来构建曲面，最后对曲面进行降维处理，生成二维地图。
视觉参考点云即为未经归一化处理的视觉点云；
曲面公式如下：
其中，((x,y)-pi)是激光点云中心点(x,y)到视觉参考点云上的p的法向投影，Wi(x,y)是参考权重，pi是视觉参考点云上被选中的参考点，Pk是视觉参考点云集合，
是构建的曲面，
是((x,y)-pi)的单位向量，h是调节系数。
步骤204：选取一定数量的激光点云和视觉点云，并根据构建的曲面，对选取的激光点云和视觉点云进行匹配求解，得到点云配准结果。
点云匹配公式如下：
其中，(ui,vi)是新构建的二维地图中一点(xi,yi)在曲面上的投影，
是(xi,yi)到曲面的距离，A是(xi,yi)和(ui,vi)的匹配距离，
是新构建的二维地图的调节向量，R(xi,yi)是(xi,yi)到曲面的平均距离。
步骤3：利用改进的模糊自适应UKF算法融合IMU数据和点云配准数据，得到移动机器人的位姿。
具体步骤如下：
步骤301：IMU数据和点云配准数据进行χ2检验，对移动机器人位姿信息和地图信息进行评估，其公式如下：
其中，qk是k时刻的χ2检验值，zk是k时刻的量测向量，
是k-1时刻的一步量测更新，Pzz是位姿和地图间的最优概率。
步骤302：进一步引入模糊自适应规则对IMU数据和点云配准数据的过程噪声和观测噪声进行估计，其公式如下：
其中，
分别是过程噪声和观测噪声，α＞0，β＞0，γ＞0，η＞0是选定的常数，
是观测误差。
步骤303：将经过χ2检验和模糊自适应规则修正后的IMU数据和点云配准数据进行UKF融合，得到移动机器人的位姿；
移动机器人的位姿计算公式如下：
其中，Zk是量测变量，uk是控制输入命令，Wk-1是系统中的噪声变量，h(Xk)是观测值的非线性函数，Vk是量测中的噪声变量，f(Xk-1,uk)是状态转移变量。
步骤4：对移动机器人的运动轨迹进行闭环检测，将机器人位姿、点云地图和闭环检测结果作为约束进行联合优化，得到移动机器人的路径地图。
具体步骤如下：
步骤401：将全局深度视觉信息中的描述符和联合优化后的位姿进行匹配，检测移动机器人的路径是否为一个闭合环。若路径为一个闭合环，则从点云配准中得到移动机器人的位姿变换，形成闭环约束，执行步骤402。若不是一个闭合环，则继续搜索起始点位置，直至达到闭环约束。
步骤402：利用高斯牛顿法的收敛性，将机器人位姿、点云地图和闭环检测结果作为约束进行联合优化，得到移动机器人的路径地图；
优化函数的公式如下：
其中，F(xk)是移动机器人状态量xk的损失函数。G(xk)是位姿和地图的优化函数。
本发明的优点在于：
(1)本发明为一种不同传感器数据的自适应多重融合SLAM方法，充分利用IMU数据并引入全局深度视觉信息和局部深度视觉信息，为点云配准、闭环优化和地图更新提供了更精准的位姿和地图数据。
(2)本发明为一种不同传感器数据的自适应多重融合SLAM方法，通过改进的IMLS-ICP算法，将具有代表性的2D激光雷达点云和3D相机点云进行匹配，有效降低了点云配准计算量，提升了定位和建图的精度。
(3)本发明为一种不同传感器数据的自适应多重融合SLAM方法，利用改进的模糊自适应UKF算法，将IMU和点云配准数据进行融合，充分利用了传感器数据，减少了噪声和泄漏，得到的位姿状态量更精确，提升了移动机器人对所处环境的鲁棒性。
(4)本发明为一种不同传感器数据的自适应多重融合SLAM方法，将位姿约束、点云配准约束和闭环检测约束进行联合优化，修正子地图的位姿节点，减小了移动机器人的位姿估计误差，使移动机器人所建地图更加准确。
附图说明
图1为本发明基于一种不同传感器数据的自适应多重融合SLAM方法的原理框架示意图；
图2为本发明利用GCNv2神经网络生成全局深度视觉信息的过程示意图；
图3为本发明利用改进LPP算法提取相机数据局部特征点，结合IMU描述用改进的R-CNN神经网络生成局部深度视觉信息的过程示意图；
图4为本发明在全局坐标系中利用改进的IMLS-ICP算法将激光点云和视觉点云进行点云匹配的示意图；
图5为本发明利用改进的模糊自适应UKF算法融合IMU和点云匹配数据，得到移动机器人的位姿的过程示意图；
图6为本发明机器人位姿、点云地图和闭环检测结果之间的约束关系示意图。
具体实施方式
下面结合附图对本发明进行详细说明。
本发明寻找一种融合激光雷达、深度相机、惯性测量单元(IMU)等多个传感器数据的SLAM方法，可以改善移动机器人定位建图的性能，减小移动机器人位姿估计误差，提升移动机器人的闭环检测效果，使移动机器人所建地图更加准确，提升移动机器人对所处环境的鲁棒性。
针对现有2D激光雷达SLAM激光点云配准计算量大、闭环检测效果差和位姿估计误差大的问题，本发明引入了全局深度视觉信息和局部深度视觉信息，用于点云配准、闭环检测和地图更新。针对2D激光雷达点云配准数据容易出现遗漏和建图精度较低的问题，利用改进的IMLS-ICP算法将激光雷达点云、全局深度视觉信息点云和局部深度视觉信息点云进行匹配，用于数据融合和生成点云地图。针对2D激光雷达SLAM容易产生噪声和泄漏以及传感器数据利用不充分的问题，利用改进的模糊自适应UKF算法融合IMU和点云匹配数据，得到移动机器人的位姿。本发明将机器人位姿、点云地图和闭环检测结果作为约束进行联合优化，性能得到了显著的提升，减小了移动机器人的位姿估计误差，检测到的闭合路径更加接近真实的闭合路径，移动机器人所建地图更加准确，提升了移动机器人对所处环境的鲁棒性。
一种不同传感器数据的自适应多重融合SLAM方法，原理如图1所示，具体步骤如下：
步骤1：针对处于某未知室内环境中的移动机器人，获取其传感器数据，并进行处理，同时提取深度相机获得的图像的特征，生成全局深度视觉信息和局部深度视觉信息。
1)生成全局深度视觉信息的过程，如图2所示，具体为：
首先，获得深度相机的灰度图，并利用GCNv2神经网络对灰度图进行处理，生成1280×720×256像素的特征图。
然后，将特征图上的特征点进行均匀化处理。
最后，通过损失函数训练和二值描述分别对均匀化的特征点进行处理，生成特征点云和描述符并保存，作为全局深度视觉信息。
损失函数如下：
Ldet＝Lce(ocur(xcur,0))+Lce(otar(xtar,xcur))
其中，(xcur,0)是特征点在当前帧的位置，(xtar,xcur)是xcur在目标帧中匹配的位置。Lce是加权交叉熵。ocur是当前帧的概率距离，otar是目标帧的概率距离。
二值描述公式如下：
其中，b(x)是根据f(x)的值区分得到的特征值，f(x)是特征点的观察概率。
2)生成局部深度视觉信息的过程如图3所示，具体如下：
首先，以2D激光雷达圆心为原点，利用IMU(Inertial Measurement Unit，惯性测量装置)数据、深度相机平面投影与激光雷达二维坐标的一致性，构建移动机器人的全局坐标系并得到IMU的观测数据。
假设移动机器人做匀变速运动，在全局坐标系中，移动机器人的状态变量如下：
上式中
分别是移动机器人tk时刻在全局坐标系中的横坐标、纵坐标和航向角，vk和ωk分别是移动机器人tk时刻的线速度和角速度。
利用全局坐标系，得到IMU的观测数据，由于IMU输出频率很高，[tk-1,tk]时间段可分为n段，其公式如下：
上式中
和
分别是移动机器人的角速度测量值和加速度测量值，
和
分别是移动机器人的角速度实际值和加速度实际值，
和
分别是陀螺仪偏差和加速度计偏差，ng和na分别是采用高斯白噪声进行建模的IMU测量噪声。
同时，获取深度相机数据，利用改进后的LPP算法提取深度相机获得的图像的局部特征。
结合移动机器人运动一致性改进后的LPP算法公式如下：
其中，i,j是全局坐标系中移动机器人经过的两个不同位置，Xij是全局坐标系中移动机器人i,j两点间的运动状态，li,lj是全局坐标系中i,j两点到初始坐标点的距离，Wij是全局坐标系中i,j两点间距离权重系数矩阵。
然后，图像的局部特征结合IMU观测数据对移动机器人运动的描述，用引入加权目标函数的R-CNN神经网络生成局部深度视觉信息。
加权目标函数如下：
其中，Lexist是既有图像特征，Ladd是新增图像特征，ρ(ω,a)是IMU观测数据权重。
步骤2：利用改进的IMLS-ICP算法将激光雷达数据点云、全局深度视觉信息点云和局部深度视觉信息进行点云匹配，用于数据融合和生成点云地图。
点云匹配原理如图4所示，图中的坐标系是移动机器人的全局坐标系，圆点是激光雷达点云，曲面是视觉点云，具体步骤如下：
步骤201：利用局部深度视觉信息得到的移动机器人状态量之间的变换，去除激光雷达数据中的运动畸变。
步骤202：删去地面点云和聚类，根据移动机器人和激光点云、视觉点云之间的运动变换，去除全局坐标系中的移动物体。
步骤203：选取具有丰富特征、曲率较小且分布均衡的激光点云和视觉点云，而且要保证选取的激光点云和视觉点云契合度高，实现激光点云和视觉点云的充分利用。
步骤204：利用激光点云的中心点到视觉参考点云(未经归一化处理的视觉点云)之间的一定范围内的点云来构建曲面，最后对曲面进行降维处理，生成二维地图。
曲面公式如下：
其中，((x,y)-pi)是激光点云中心点(x,y)到视觉参考点云上的p的法向投影，Wi(x,y)是参考权重，pi是视觉参考点云上被选中的参考点，Pk是视觉参考点云集合，
是构建的曲面，
是((x,y)-pi)的单位向量，h是调节系数。
步骤205：根据构建的曲面，对选取的激光点云和视觉点云进行匹配求解，得到点云配准结果；
点云匹配公式如下：
其中，(ui,vi)是新构建的二维地图中一点(xi,yi)在曲面上的投影，
是(xi,yi)到曲面的距离，A是(xi,yi)和(ui,vi)的匹配距离，
是新构建的二维地图的调节向量，R(xi,yi)是(xi,yi)到曲面的平均距离。
步骤3：利用改进的模糊自适应UKF算法融合IMU数据和点云配准数据，得到移动机器人的位姿。
原理如图5所示，本过程是先将IMU数据和点云配准数据进行χ2检验和模糊自适应规则修正，然后进行UKF融合得到移动机器人的位姿的过程，具体步骤如下：
步骤301：IMU数据和点云配准数据进行χ2检验，对移动机器人位姿信息和地图信息进行评估，其公式如下：
其中，qk是k时刻的χ2检验值，zk是k时刻的量测向量，
是k-1时刻的一步量测更新，Pzz是位姿和地图间的最优概率。
步骤302：进一步引入模糊自适应规则对IMU数据和点云配准数据的过程噪声和观测噪声进行估计，其公式如下：
其中，
分别是过程噪声和观测噪声，α＞0，β＞0，γ＞0，η＞0为选定的常数，
是观测误差。
步骤303：将经过χ2检验和模糊自适应规则修正后的IMU和点云匹配数据进行UKF融合，得到移动机器人的位姿，其公式如下：
其中，Zk是量测变量，uk是控制输入命令，Wk-1是系统中的噪声变量，h(Xk)是观测值的非线性函数，Vk是量测中的噪声变量，f(Xk-1,uk)是状态转移变量。
步骤4：对移动机器人的运动轨迹进行闭环检测，将机器人位姿、点云地图和闭环检测结果作为约束进行联合优化，得到移动机器人的路径地图。
各状态量之间的约束关系如图6所示，图中X0,X1,X2,...,XN是移动机器人状态量，XR是闭环检测结果。具体步骤如下：
步骤401：将全局深度视觉信息中的描述符和步骤三联合优化后的位姿进行匹配，检测移动机器人的路径是否为一个闭合环。若路径为一个闭合环，则从点云配准中得到移动机器人的位姿变换，形成闭环约束，执行步骤402。若不是一个闭合环，则继续搜索起始点位置，直至达到闭环约束。
步骤402：利用高斯牛顿法的收敛性，将机器人位姿、点云地图和闭环检测结果作为约束进行联合优化，得到移动机器人的路径地图
优化函数的公式如下：
其中，F(xk)是移动机器人状态量xk的损失函数。G(xk)是位姿和地图的优化函数。
Claims (5)
1.一种不同传感器数据的自适应多重融合SLAM方法，其特征在于，具体为：
首先，针对处于某未知室内环境中的，融合激光雷达、深度相机和若干传感器的移动机器人，获取其传感器数据并进行处理，同时提取到深度相机获得的图像特征，生成全局深度视觉信息和局部深度视觉信息；
然后，利用改进的IMLS-ICP算法将激光雷达数据、全局深度视觉信息和局部深度视觉信息进行点云匹配，生成点云地图，并利用改进的模糊自适应UKF算法进行数据融合，得到移动机器人的位姿；
最后，对移动机器人的运动轨迹进行闭环检测，将机器人位姿、点云地图和闭环检测结果作为约束进行联合优化，得到移动机器人的路径地图；
具体过程为：
首先，将全局深度视觉信息中的描述符和联合优化后的位姿进行匹配，检测移动机器人的路径是否为一个闭合环，若是，则从点云配准中得到移动机器人的位姿变换，形成闭环约束，执行下一步；否则，继续搜索起始点位置，直至达到闭环约束；
然后，利用高斯牛顿法的收敛性，将机器人位姿、点云地图和闭环检测结果作为约束进行联合优化，得到移动机器人的路径地图；
优化函数的公式如下：
其中，F(xk)是移动机器人状态量xk的损失函数；；G(xk)是位姿和地图的优化函数。
2.根据权利要求1所述的一种不同传感器数据的自适应多重融合SLAM方法，其特征在于，所述的生成全局深度视觉信息的过程为：
首先，获得深度相机的灰度图，并利用GCNv2神经网络对灰度图进行处理，生成特征图；
然后，将特征图上的特征点进行均匀化处理；；
最后，通过损失函数训练和二值描述分别对均匀化的特征点进行处理，生成特征点云和描述符并保存，作为全局深度视觉信息；；
损失函数如下：
Ldet＝Lce(ocur(xcur,0))+Lce(otar(xtar,xcur))
其中，(xcur,0)是特征点在当前帧的位置，(xtar,xcur)是xcur在目标帧中匹配的位置；Lce是加权交叉熵；ocur是当前帧的概率距离，otar是目标帧的概率距离；
二值描述公式如下：
其中，b(x)是根据f(x)的值区分得到的特征值，f(x)是特征点的观察概率。
3.根据权利要求1所述的一种不同传感器数据的自适应多重融合SLAM方法，其特征在于，所述的生成局部深度视觉信息的过程如下：
首先，以2D激光雷达圆心为原点，利用IMU数据、深度相机平面投影与激光雷达二维坐标的一致性，构建移动机器人的全局坐标系并得到IMU的观测数据；
假设移动机器人做匀变速运动，在全局坐标系中，移动机器人的状态变量如下：
上式中xk,yk,
分别是移动机器人tk时刻在全局坐标系中的横坐标、纵坐标和航向角，vk和ωk分别是移动机器人tk时刻的线速度和角速度；
利用全局坐标系，得到IMU的观测数据，由于IMU输出频率很高，[tk-1,tk]时间段可分为n段，其公式如下：
上式中
和
分别是移动机器人的角速度测量值和加速度测量值，
和
分别是移动机器人的角速度实际值和加速度实际值，
和
分别是陀螺仪偏差和加速度计偏差，ng和na分别是采用高斯白噪声进行建模的IMU测量噪声；
同时，获取深度相机数据，利用改进后的LPP算法提取深度相机获得的图像的局部特征；
结合移动机器人运动一致性改进后的LPP算法公式如下：
其中，i,j是全局坐标系中移动机器人经过的两个不同位置，Xij是全局坐标系中移动机器人i,j两点间的运动状态，li,lj是全局坐标系中i,j两点到初始坐标点的距离，Wij是全局坐标系中i,j两点间距离权重系数矩阵；
然后，图像的局部特征结合IMU观测数据对移动机器人运动的描述，用引入加权目标函数的R-CNN神经网络生成局部深度视觉信息；
加权目标函数如下：
其中，Lexist是既有图像特征，Ladd是新增图像特征，ρ(ω,a)是IMU观测数据权重。
4.根据权利要求1所述的一种不同传感器数据的自适应多重融合SLAM方法，其特征在于，所述的点云匹配的具体步骤如下：
步骤201：利用局部深度视觉信息得到的移动机器人状态量之间的变换，去除激光雷达数据中的运动畸变；
步骤202：删去地面点云和聚类，根据移动机器人和激光雷达数据点云、全局深度视觉信息的特征点云以及局部深度视觉信息点云之间的运动变换，去除全局坐标系中的移动物体；
步骤203：利用激光点云的中心点到视觉参考点云的之间一定范围内的点云来构建曲面，最后对曲面进行降维处理，生成二维地图；
视觉参考点云即为未经归一化处理的视觉点云；
曲面公式如下：
其中，((x,y)-pi)是激光点云中心点(x,y)到视觉参考点云上的p的法向投影，Wi(x,y)是参考权重，pi是视觉参考点云上被选中的参考点，Pk是视觉参考点云集合，
是构建的曲面，
是((x,y)-pi)的单位向量，h是调节系数；
步骤204：选取一定数量的激光点云和视觉点云，并根据构建的曲面，对选取的激光点云和视觉点云进行匹配求解，得到点云配准结果；
点云匹配公式如下：
其中，(ui,vi)是新构建的二维地图中一点(xi,yi)在曲面上的投影，
是(xi,yi)到曲面的距离，A是(xi,yi)和(ui,vi)的匹配距离，
是新构建的二维地图的调节向量，R(xi,yi)是(xi,yi)到曲面的平均距离。
5.根据权利要求1所述的一种不同传感器数据的自适应多重融合SLAM方法，其特征在于，所述的移动机器人的位姿计算方法如下：
步骤301：IMU数据和点云配准数据进行χ2检验，对移动机器人位姿信息和地图信息进行评估，其公式如下：
其中，qk是k时刻的χ2检验值，zk是k时刻的量测向量，
是k-1时刻的一步量测更新，Pzz是位姿和地图间的最优概率；
步骤302：进一步引入模糊自适应规则对IMU数据和点云配准数据的过程噪声和观测噪声进行估计，其公式如下：
其中，
分别是过程噪声和观测噪声，α＞0，β＞0，γ＞0，η＞0是选定的常数，
是观测误差；
步骤303：将经过χ2检验和模糊自适应规则修正后的IMU数据和点云配准数据进行UKF融合，得到移动机器人的位姿；
移动机器人的位姿计算公式如下：
其中，Zk是量测变量，uk是控制输入命令，Wk-1是系统中的噪声变量，h(Xk)是观测值的非线性函数，Vk是量测中的噪声变量，f(Xk-1,uk)是状态转移变量。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310114324.7A CN115950414A (zh) | 2023-01-29 | 2023-01-29 | 一种不同传感器数据的自适应多重融合slam方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310114324.7A CN115950414A (zh) | 2023-01-29 | 2023-01-29 | 一种不同传感器数据的自适应多重融合slam方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115950414A true CN115950414A (zh) | 2023-04-11 |
Family
ID=87289355
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310114324.7A Pending CN115950414A (zh) | 2023-01-29 | 2023-01-29 | 一种不同传感器数据的自适应多重融合slam方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115950414A (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116160458A (zh) * | 2023-04-26 | 2023-05-26 | 广州里工实业有限公司 | 一种移动机器人多传感器融合快速定位方法、设备及系统 |
-
2023
- 2023-01-29 CN CN202310114324.7A patent/CN115950414A/zh active Pending
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116160458A (zh) * | 2023-04-26 | 2023-05-26 | 广州里工实业有限公司 | 一种移动机器人多传感器融合快速定位方法、设备及系统 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Rozenberszki et al. | LOL: Lidar-only Odometry and Localization in 3D point cloud maps | |
| CN111076733B (zh) | 一种基于视觉与激光slam的机器人室内建图方法及系统 | |
| Li et al. | Deep sensor fusion between 2D laser scanner and IMU for mobile robot localization | |
| Zhao et al. | A robust laser-inertial odometry and mapping method for large-scale highway environments | |
| Nieto et al. | Recursive scan-matching SLAM | |
| Mu et al. | Research on SLAM algorithm of mobile robot based on the fusion of 2D LiDAR and depth camera | |
| CN110686677A (zh) | 一种基于几何信息的全局定位方法 | |
| Engel et al. | Deeplocalization: Landmark-based self-localization with deep neural networks | |
| CN111998862A (zh) | 一种基于bnn的稠密双目slam方法 | |
| CN113776519A (zh) | 一种无光动态开放环境下agv车辆建图与自主导航避障方法 | |
| CN114964212A (zh) | 面向未知空间探索的多机协同融合定位与建图方法 | |
| CN113763549A (zh) | 融合激光雷达和imu的同时定位建图方法、装置和存储介质 | |
| CN116758153A (zh) | 用于机器人精准位姿获取的基于多因子图的后端优化方法 | |
| CN116429116A (zh) | 一种机器人定位方法及设备 | |
| CN117367427A (zh) | 一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法 | |
| CN115950414A (zh) | 一种不同传感器数据的自适应多重融合slam方法 | |
| Peng et al. | Vehicle odometry with camera-lidar-IMU information fusion and factor-graph optimization | |
| Chang et al. | Robust accurate LiDAR-GNSS/IMU self-calibration based on iterative refinement | |
| Fu et al. | Semantic Map-based Visual Localization with Consistency Guarantee | |
| Liu et al. | Laser 3D tightly coupled mapping method based on visual information | |
| CN115482282A (zh) | 自动驾驶场景下具有多目标追踪能力的动态slam方法 | |
| Pan et al. | LiDAR-IMU Tightly-Coupled SLAM Method Based on IEKF and Loop Closure Detection | |
| CN114462545A (zh) | 一种基于语义slam的地图构建方法及装置 | |
| CN114721377A (zh) | 一种基于改进Cartographer的SLAM的室内导盲机器人控制方法 | |
| Yuan et al. | LIWO: LiDAR-Inertial-Wheel Odometry |
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 |