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

一种基于slam建图的多传感器数据融合的算法
技术领域
1.本发明涉及数据融合技术领域，更具体地说，本发明涉及一种基于slam建图的多传感器数据融合的算法。


背景技术：

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


技术实现要素：

5.为了克服现有技术的上述缺陷，本发明的实施例提供一种基于slam建图的多传感器数据融合的算法，通过激光雷达、深度相机、imu、里程计的融合以解决上述背景技术中提出的问题。
6.为实现上述目的，本发明提供如下技术方案：
7.一种基于slam建图的多传感器数据融合的算法，包括如下步骤：
8.步骤s1、激光雷达和深度相机接收imu数据并对机器人进行初始位姿标定；
9.步骤s2、激光雷达扫描周围环境获取激光数据，深度相机获取物体的深度信息生成三维点云图像；
10.步骤s3、激光雷达利用ukf算法进行位姿的实时预测和估计，并依据激光雷达得到的位姿信息与双目摄像头的位置进行转换，辅助双目摄像头采集三维点云数据；
11.步骤s4、激光数据粒子滤波构建的二维栅格地图；且深度相机获取的三维点云地图向下投影成二维信息，并通过cartographer算法辅助构建二维栅格地图；
12.步骤s5、将激光雷达生成的二维栅格地图与深度相机形成的二维栅格地图通过贝叶斯估计滤波算法在决策级进一步融合，最终形成更加精准的全局二维栅格地图。
13.在一个优选的实施方式中，在步骤s4中，对深度相机构建的二维栅格地图通过回环检测算法进行往复回环检测；具体的，将深度相机构建二维栅格地图中的位姿信息通过
变换矩阵t将其变换到子图中，其公式如下：
[0014][0015]
式中，当前雷达帧在子图坐标系下的位姿为t
ξ
，ξ
θ
为角度信息，ξ
x
、ξy为坐标信息。
[0016]
在一个优选的实施方式中，将每一幅子图看作由多个位姿构成的概率网格地图，在位姿插入之前确定命中a和未命中b的点集，定义命中和未命中的比例为：
[0017][0018]
对于每个网格命中概率更新公式为：
[0019]mnew
(x)＝clamp(z-1
(z(m
old
(x))
·
z(pa)))
[0020]
未命中概率为：
[0021]mnew
(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＝(xk)，且已获得k组的测量数据zk＝{z1,
…
,zk}和先验分布如下：
[0031][0032]
其中，p(zk∣xk)为基于激光雷达观测模型得到的似然函数；p(xk∣z
k-1
)为基于上一时刻的先验分布函数；p(zk∣z
k-1
)为在上一时刻时对当前时刻的估计；
[0033]
对于融合多传感器的栅格地图，得到改进的融合公式：
[0034]
[0035]
其中表示被占据的先验概率，表示传感器采集的占据概率，po表示经过融合后的结果；
[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]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0047]
实施例
[0048]
参照图1所述，本发明一种基于slam建图的多传感器信息融合的算法流程图，主要包括如下步骤：
[0049]
步骤s1、激光雷达和深度相机接收imu数据并对机器人进行初始位姿标定。
[0050]
具体的，机器人或小车内部单片机采集惯性测量单元(imu(inertial measurement unit))的数据，激光雷达和深度相机接收imu数据进行初始位姿标定。
[0051]
步骤s2、激光雷达扫描周围环境获取激光数据，深度相机获取物体的深度信息生成三维点云图像。
[0052]
具体的，激光雷达扫描周围环境建立二维激光点云数据；深度相机扫描物体获取深度信息并生成三维点云图像，并采用orb(oriented fast and rotated brief)特征点提取对周围环境进行描述。
[0053]
其中，深度相机采集的深度数据(如图4所示)，可以弥补由于激光雷达的局限性而无法检测到的物体。本发明利用orb算法，如图3所示，对由深度相机产生的三维点云地图进
行特征点提取，在一个图像中，可提取到很多特征点，故对相机位姿的估计可转化为对误差求解最小二乘化的问题(每一个特征点都有一个误差)，可用公式表示为：
[0054][0055]
式中为第k帧所有特征点(n为第k帧特征点总数)；为第k-1帧中全部具有深度值的特征点；ξ为第k-1帧到第k帧的位姿变化。
[0056]
为了满足所有重投影误差最小，需要通过不断调整ξ的值，使得上述误差不断下降，最终得到一个对当前匹配对最优的相机姿态，并将三维点云地图投影成为二维栅格地图(如图5所示)。
[0057]
步骤s3、激光雷达采集激光数据和imu等信息，利用ukf算法进行位姿的实时预测和估计，进而实时生成二维数据，并且依据激光雷达得到的位姿信息与双目摄像头的位置进行转换，辅助双目摄像头采集三维点云数据。
[0058]
其中，ukf算法的关键在于ut变换，利用一组sigma采样点来描述随机变量的高斯分布。对于该算法的sigma点选取采样，采用比例修正采样策略来近似非线性函数的后验分布，相比于对称采样提高了精度，其精度相当于泰勒二阶展开精度。
[0059]
比例修正算法为：
[0060]
χ'i＝χ0+α(χ
i-χ0)
[0061][0062][0063]
式中，α为比例缩放因子，其范围为0≤α≤1，控制α的取值可以控制sigma点集的范围，α取得过大虽然能有效控制噪声影响，但是会导致采样非局部效应，数值鲁棒性变弱，因此α通常设置为一个很小的正数；β反应状态历史信息的高阶特性，调节β可以提高协方差的近似精度，对于高斯分布，β＝2为最优取值。
[0064]
将上述比例修正算法用在对称采样中，从而构成了比例修正对称采样策略，相应的sigma采样点与权值为：
[0065][0066]
相应的权值为：
[0067]
[0068][0069]
式中，λ＝α2(n+κ)-n；κ依然为比例因子，其取值只需保证协方差非负定即可。针对随机变量高斯分布的情况，当变量x为单变量时，κ＝0，当其为多变量时，κ＝3-n通常被采用。
[0070]
ukf实现对初始位姿的估计，其关键在于，在激光数据中采集一组sigma点用来估计激光雷达的位姿。ut变换则是根据一定的采样策略，并设置相对应的均值权值和方差权值来近似非线性函数的后验均值和方差。
[0071]
其中为sigma采样点经过非线性函数传递后对应的点，其具体公式如下：
[0072][0073][0074][0075]
其中p
yy
和p
xy
分别为预测更新后的互协方差和协方差。
[0076]
对于特征点的提取，orb特征对于fast特征提取算法上进行了改进，并且结合使用速度极快的用二进制描述的brief描述子，在保留原有的提取速度之外提高了实时性。
[0077]
步骤s4、激光数据粒子滤波构建二维栅格地图，深度相机获取的三维点云地图向下投影成二维信息，并通过cartographer算法辅助构建二维栅格地图。
[0078]
具体的，由于激光数据点云稀疏数据不完整，无法得到一定高度的障碍物信息，需要视觉slam辅助，丰富点云信息和对周围环境的描述。因此，需要对激光数据和三维点云数据进行融合。具体的，采用深度相机生成的三维点云图像采用3d-2d投影的方式把三维点云图像转为二维信息，对此数据进行cartographer算法生成局部二维栅格地图，此算法的优势在于生成多个子图，并且对局部地图进行更新。
[0079]
其中，基于图优化的cartographer算法，其优点在于加入了闭环检测环节。
[0080]
参照图2，图优化slam是通过保存所有传感器信息对智能小车的运动位姿和地图进行完整估计，构建完位姿图后，还需协调位姿序列以满足边约束，从而得出智能小车的运动轨迹及地图。
[0081]
基于图优化的slam主要分为两个部分：前端部分和后端部分。
[0082]
前端部分主要是对接收到的数信息进行一个初步的构建。图由节点和边构成，节点是指智能小车位姿，位姿之间的关系则构成边(edge)，如t时刻与t+1时刻之间的里程计推算或是算法解出的位姿转换矩阵也可以构成边。
[0083]
后端部分通过节点间的边约束对节点位置进行调整，使之回到正确的地方，此时各节点之间的误差总和值为最小。
[0084]
闭环检测主要是针对全局数据关系，通过传感器所获得数据判断智能小车当前位姿与之前已访问区域的匹配相似度。当小车运行到之前经过的同一个位置时，通过周围环境特征与历史关键帧作比较，从而能有效地消除累计误差优化方案一般是根据特征点的相似程度进行识别，使用词袋模型bow(big-of-words)识别特征点并聚类寻找回环关键帧，并将历史位姿与当前位姿的匹配来优化全局地图。
[0085]
随着建图规模的不断扩大，累计误差将会逐渐增大，严重影响小车在位姿估计和地图构建中的精度。
[0086]
将双目摄像头得到的深度数据向下投影成二维数据，这个二维数据包含小车的基本位姿信息，即(x,y,θ)，与imu传感器结合得到初始位姿。对于得到的这些数据，可以利用变换矩阵t将其变换到子图中；
[0087][0088]
其中当前雷达帧在子图坐标系下的位姿为t
ξ
，ξ
θ
为角度信息，ξ
x
、ξy为坐标信息。
[0089]
对于每一幅子图，可以看作多个位姿构成的概率网格地图。在位姿插入之前要确定a(命中)和b(未命中)的点集。命中对应有雷达数据，未命中则没有。定义命中和未命中的比例为：
[0090][0091]
对于每个网格命中概率更新公式为：
[0092]mnew
(x)＝clamp(z-1
(z(m
old
(x))
·
z(pa)))
[0093]
未命中概率为：
[0094]mnew
(x)＝clamp(z-1
(z(m
old
(x))
·
z(pb)))
[0095]
其中，clamp()将数字限制在一个范围[min,max]内，在这即进行归一化将数字限制在[0,1]之间。m
old
(x)为上一次的概率预测值。
[0096]
另外，在位姿插入子图之前要先进行优化，对于闭环检测优化所有子图的位姿，遵循稀疏姿态调整。插入时的相对姿态(相对于子图位姿的)存储在内存中，用于闭环优化。除了这些相对姿态外，一旦子图不再更新了，则将考虑由包含位姿信息的子图对进行循环闭合。扫面匹配在后台运行，如果找到匹配的对象，则将对应的相对姿态添加到优化问题中。
[0097]
步骤s5、激光栅格地图与视觉栅格地图通过改进的滤波算法进行融合修正生成全局二维栅格地图。
[0098]
具体的，深度相机投影生成的二维信息通过执行视觉slam生成局部二维栅格地图。为提高稳定性和鲁棒性，将激光雷达生成的二维栅格地图与深度相机形成的二维栅格地图通过贝叶斯估计滤波算法在决策级进一步融合，最终形成更加精准的全局二维栅格地图。此栅格地图比激光雷达单独构建的栅格地图包含更丰富的障碍物信息和地图信息，使构建的地图更精准更符合实际情况。
[0099]
其中，决策级数据融合利用贝叶斯估计融合算法将得到而栅格地图和深度相机投影得到的二维栅格地图进行融合。
[0100]
贝叶斯估计是基于贝叶斯定理的条件或后验概率的统计数据融合算法，通过贝叶斯估计可以得到一种计算后验概率的方法，若k时刻的概率为p＝(xk)，且已获得k组的测量数据zk＝{z1,
…
,zk}和先验分布如下：
[0101]
[0102]
其中，p(zk∣xk)为基于激光雷达观测模型得到的似然函数；p(xk∣z
k-1
)为基于上一时刻的先验分布函数；p(zk∣z
k-1
)为在上一时刻时对当前时刻的估计。
[0103]
对于本项目设定的规则如下：用o1表示栅格地图的点已被占据，o2表示未被占据，e表示观测到障碍物。由此得到以下改进的条件概率公式：
[0104][0105][0106]
其中p(e)表示每个栅格单位的先验概率，一般初始化为0.5。
[0107]
对于栅格g中的任意一个单元点：可以根据贝叶斯推导结合当前观测值r
t
＝(r
t
,r
t-n
,
…
,ro)得出如下融合公式计算估值：
[0108][0109]
其中表示被占据的先验概率，表示传感器采集的占据概率，po表示经过融合后的结果。
[0110]
由此得到的每一栅格单元占据概率是原始数据和局部更新后的融合数据。(如图6所示)。
[0111]
对于决策级的融合，使用改进数据滤波融合，对每一个传感器的概率分布进行联合，形成联合的后验概率分布，之后将求取等到的联合分布对应的似然函数的最小值，作为多传感器融合后的值。通过已知向量z，估计未知的n维状态向量x。可以得到一种计算后验概率的方法如下：
[0112]
若k时刻的概率为p＝(xk)，且已获得k组的测量数据zk＝{z1,
…
,zk}和先验分布如下：
[0113][0114]
其中，p(zk∣xk)为基于激光雷达观测模型得到的似然函数；p(xk∣z
k-1
)为基于上一时刻的先验分布函数；p(zk∣z
k-1
)为在上一时刻时对当前时刻的估计。
[0115]
对于融合多传感器的栅格地图，可得到改进的融合公式：
[0116][0117]
其中表示被占据的先验概率，表示传感器采集的占据概率，po表示经过融合后的结果。
[0118]
将融合后得到的二维栅格地图和三维点云地图投影得到的二维地图进行融合，以
上述公式作为算法的依据，融合后的二维地图比原本的二维地图更加精确，当融合后的地图与原本的地图相差过大时，会自动再次进行数据的收集分析。
[0119]
最终得到全局地图。
[0120]
本说明书的实施例所述的内容仅仅是对发明构思的实现形式的列举，仅说明用途。本发明的保护范围不应当被视为仅限于本实施所陈述的具体形式，本发明的保护范围也及于本领域的普通技术人员根据本发明构思所能想到的等同技术手段。
[0121]
需要说明的是，在本文中，如若存在第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性地包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个
……”
限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
[0122]
最后：以上所述仅为本发明的优选实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。