一种基于SLAM建图的多传感器数据融合的算法制造技术

本发明专利技术公开了一种基于SLAM建图的多传感器数据融合的算法，具体涉及数据融合领域，包括如下步骤：步骤S1、激光雷达和深度相机接收IMU数据并对机器人进行初始位姿标定；步骤S2、激光雷达扫描周围环境获取激光数据，深度相机获取物体的深度信息生成三维点云图像；步骤S3、激光雷达利用UKF算法进行位姿的实时预测和估计，并辅助双目摄像头采集三维点云数据；步骤S4、激光数据粒子滤波构建的二维栅格地图且深度相机构建二维栅格地图；步骤S5、将激光雷达生成的二维栅格地图与深度相机形成的二维栅格地图通过贝叶斯估计滤波算法在决策级进一步融合，最终形成更加精准的全局二维栅格地图。地图。地图。

【技术实现步骤摘要】
一种基于SLAM建图的多传感器数据融合的算法


[0001]本专利技术涉及数据融合
，更具体地说，本专利技术涉及一种基于SLAM建图的多传感器数据融合的算法。

技术介绍

[0002]SLAM(Simultaneous localization and mapping)即“同步定位与建图”，机器人要想实现导航就必须知道自己在什么位置以及周围的环境，SLAM主要用于解决机器人在未知环境运动时的定位与地图构建问题，可以描述为机器人从一个未知的位置开始移动，在移动过程中根据自身位置和地图进行定位并绘制出周围环境的地图，SLAM可以辅助机器人执行路径规划、自主探索、自主导航等任务。
[0003]机器人建图的精度影响着导航的实现，目前主流的建图方法有激光SLAM建图和视觉SLAM建图，激光建图最被广泛采用，激光建图分为2D激光雷达建图和3D激光雷达建图，2D激光雷达因其价格便宜被广泛使用，虽然其精度高技术成熟，但信息量较少只能构建二维地图且对建图环境有一定的要求，视觉建图结构简单成本低信息量大，但建图精度低对光敏感运算负荷量大难以直接用于建图。
[0004]随着技术的发展和环境的日益复杂，单一传感器难以满足机器人导航对环境地图质量和精度的要求，因此，本专利技术提供了一种一种基于SLAM建图的多传感器数据融合的算法来解决上述问题。

技术实现思路

[0005]为了克服现有技术的上述缺陷，本专利技术的实施例提供一种基于SLAM建图的多传感器数据融合的算法，通过激光雷达、深度相机、IMU、里程计的融合以解决上述
技术介绍
中提出的问题。
[0006]为实现上述目的，本专利技术提供如下技术方案：
[0007]一种基于SLAM建图的多传感器数据融合的算法，包括如下步骤：
[0008]步骤S1、激光雷达和深度相机接收IMU数据并对机器人进行初始位姿标定；
[0009]步骤S2、激光雷达扫描周围环境获取激光数据，深度相机获取物体的深度信息生成三维点云图像；
[0010]步骤S3、激光雷达利用UKF算法进行位姿的实时预测和估计，并依据激光雷达得到的位姿信息与双目摄像头的位置进行转换，辅助双目摄像头采集三维点云数据；
[0011]步骤S4、激光数据粒子滤波构建的二维栅格地图；且深度相机获取的三维点云地图向下投影成二维信息，并通过Cartographer算法辅助构建二维栅格地图；
[0012]步骤S5、将激光雷达生成的二维栅格地图与深度相机形成的二维栅格地图通过贝叶斯估计滤波算法在决策级进一步融合，最终形成更加精准的全局二维栅格地图。
[0013]在一个优选的实施方式中，在步骤S4中，对深度相机构建的二维栅格地图通过回环检测算法进行往复回环检测；具体的，将深度相机构建二维栅格地图中的位姿信息通过
变换矩阵T将其变换到子图中，其公式如下：
[0014][0015]式中，当前雷达帧在子图坐标系下的位姿为T
ξ
，ξ
θ
为角度信息，ξ
x
、ξ
y
为坐标信息。
[0016]在一个优选的实施方式中，将每一幅子图看作由多个位姿构成的概率网格地图，在位姿插入之前确定命中A和未命中B的点集，定义命中和未命中的比例为：
[0017][0018]对于每个网格命中概率更新公式为：
[0019]M
new
(x)＝clamp(Z
‑1(Z(M
old
(x))
·
Z(PA)))
[0020]未命中概率为：
[0021]M
new
(x)＝clamp(Z
‑1(Z(M
old
(x))
·
Z(PB)))
[0022]其中，clamp()将数字限制在一个范围[min,max]内，在这即进行归一化将数字限制在[0,1]之间，M
old
(x)为上一次的概率预测值。
[0023]在一个优选的实施方式中，在步骤S3中，在激光数据中采集一组Sigma点用来估计激光雷达的位姿；
[0024]其中为Sigma采样点经过非线性函数传递后对应的点，其具体公式如下：
[0025][0026][0027][0028]式中，P
yy
和P
xy
分别为预测更新后的互协方差和协方差。
[0029]在一个优选的实施方式中，在步骤S5中，对于决策级的融合，使用改进数据滤波融合，对每一个传感器的概率分布进行联合，形成联合的后验概率分布，之后将求取等到的联合分布对应的似然函数的最小值，作为多传感器融合后的值；通过已知向量Z，估计未知的n维状态向量X可以得到一种计算后验概率的方法如下：
[0030]若K时刻的概率为p＝(x
k
)，且已获得K组的测量数据Z
k
＝{z1,
…
,z
k
}和先验分布如下：
[0031][0032]其中，p(z
k
∣x
k
)为基于激光雷达观测模型得到的似然函数；p(x
k
∣Z
k
‑1)为基于上一时刻的先验分布函数；p(Z
k
∣Z
k
‑1)为在上一时刻时对当前时刻的估计；
[0033]对于融合多传感器的栅格地图，得到改进的融合公式：
[0034][0035]其中表示被占据的先验概率，表示传感器采集的占据概率，P
o
表示经过融合后的结果；
[0036]将融合后得到的二维栅格地图和三维点云地图投影得到的二维地图进行融合，最终得到全局地图。
[0037]本专利技术的技术效果和优点：
[0038]本专利技术一种基于SLAM建图的多传感器数据融合的算法，通过将激光雷达、深度相机、IMU、里程计融合，使得构建的地图包含的信息更多且更精确，弥补了单一传感器SLAM建图的缺陷，增加了系统的生存能力，扩展了空间覆盖范围，提高了信息的可信度，降低了信息的模糊度；
[0039]同时，回环检测算法使得SLAM建图更加精确，通过数据融合使得SLAM算法建图速度更快提高其精准度和鲁棒性，使得构建的栅格地图更加精确，提高机器人在未知环境中的感知能力，为小车的导航避障提供保障。
附图说明
[0040]图1为本专利技术的基本实现框架；
[0041]图2为Cartographer算法框架；
[0042]图3为深度相机所构建的三维点云地图；
[0043]图4为深度相机所构建的深度地图；
[0044]图5为融合之前激光SLAM构建的栅格地图；
[0045]图6为最终决策级融合之后的SLAM地图。
具体实施方式
[0046]下面将结合本专利技术实施例中的附图，对本专利技术实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本专利技术一部分实施例，本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于SLAM建图的多传感器数据融合的算法，其特征在于：包括如下步骤：步骤S1、激光雷达和深度相机接收IMU数据并对机器人进行初始位姿标定；步骤S2、激光雷达扫描周围环境获取激光数据，深度相机获取物体的深度信息生成三维点云图像；步骤S3、激光雷达利用UKF算法进行位姿的实时预测和估计，并依据激光雷达得到的位姿信息与双目摄像头的位置进行转换，辅助双目摄像头采集三维点云数据；步骤S4、激光数据粒子滤波构建的二维栅格地图；且深度相机获取的三维点云地图向下投影成二维信息，并通过Cartographer算法辅助构建二维栅格地图；步骤S5、将激光雷达生成的二维栅格地图与深度相机形成的二维栅格地图通过贝叶斯估计滤波算法在决策级进一步融合，最终形成更加精准的全局二维栅格地图。2.根据权利要求1所述的一种基于SLAM建图的多传感器数据融合的算法，其特征在于：在步骤S4中，对深度相机构建的二维栅格地图通过回环检测算法进行往复回环检测；具体的，将深度相机构建二维栅格地图中的位姿信息通过变换矩阵T将其变换到子图中，其公式如下：式中，当前雷达帧在子图坐标系下的位姿为T
ξ
，ξ
θ
为角度信息，ξ
x
、ξ
y
为坐标信息。3.根据权利要求2所述的一种基于SLAM建图的多传感器数据融合的算法，其特征在于：将每一幅子图看作由多个位姿构成的概率网格地图，在位姿插入之前确定命中A和未命中B的点集，定义命中和未命中的比例为：对于每个网格命中概率更新公式为：M
new
(x)＝clamp(Z
‑1(Z(M
old
(x))
·
Z(PA)))未命中概率为：M
new
(x)＝clamp(Z
‑1(Z(M
old
(x))
·
Z(PB)))其中，clamp()将数字限制在一个范围[...

【专利技术属性】
技术研发人员：台流臣，李文宇，王文成，吴萍，尹贻宽，
申请(专利权)人：潍坊学院，
类型：发明
国别省市：