基于直方图和面特征匹配的3D激光SLAM方法及移动机器人与流程

基于直方图和面特征匹配的3d激光slam方法及移动机器人
技术领域
1.本发明属于机器人技术领域，更具体地，本发明涉及一种基于直方图和面特征匹配的3d激光slam方法及移动机器人。


背景技术：

2.随着移动机器人在工业仓储物流和无人驾驶应用越来越广泛，其定位导航的slam技术也越发重要。现有技术主要是磁导航、视觉导航、激光slam导航，其中磁导航工作轨迹固定，路径需要铺设电磁导线或磁条，一般应用于室内小范围场景。视觉导航轨迹相对自由，成本较低，在室内多使用二维码或特殊图形码等标记，要定期维护。室外视觉导航受光源和雨雪环境影响较大，可靠性较低。激光slam导航定位精准，路径灵活多变，适应多种现场环境，在激光雷达技术突破及成熟后，成本已经降下来，现如今成为了移动机器人主流的定位导航方式。
3.激光slam可分为室内2d和室外3d的激光slam，室内2d激光slam主要是建立二维地图和基于二维地图的定位；室外3d激光slam主要是建立三维地图和基于三维地图的定位，现有的室外3d的激光提取角点、线和面特征，并对线和面特征进行了法向量估计，之后通过角点和面点特征进行前端配准，最后进行了回环检测和优化。但是该方案存在如下问题：通过曲率计算的边缘点并不是实际环境边缘点，在雷达角分辨率较大，或者扫描距离过远时，测距点不可能直接打到环境边缘上，所以将其带入匹配优化是不稳定的，定位结果会出现较大跳动。


技术实现要素：

4.本发明提供一种基于直方图和面特征匹配的3d激光slam方法，旨在提高室内室外的定位精准度。
5.本发明是这样实现的，、一种直方图和面特征匹配的3d激光slam方法，所述方法具体包括如下步骤：
6.s1、构建3d激光雷达当前激光帧的直方图hist
t
；
7.s2、基于num个角度候选项旋转直方图hist
t
，获取旋转后的直方图hist
tnum
；
8.s3、将直方图hist
tnum
与上一时刻直方图hist
t-1
匹配，得分最高的角度候选项yaw^作为当前时刻的姿态初值；
9.s4、提取当前激光帧的平面特征点集；
10.s5、基于当前激光帧及上一激光帧帧的面特征匹配来优化姿态初值的位姿增量；
11.s6、基于累计的平面特征点云地图进行二次约束，获得激光雷达当前的位姿。
12.进一步的，所述直方图hist
t
的构建方法具体包括如下步骤：
13.将当前激光帧点云横切为sp个子点云集，每个子点云集构建一个直方图hist_sp，sp个子点云集的直方图的累加得到当前激光帧的直方图hist
t
，直方图hist_sp的构建方法具体如下：
14.计算子点云集的质心，各子点与质心的连线得到矢量vw，计算每个子点对应矢量vw与x轴正方向的夹角，通过角度进行排序，可得到有序的切片点云；
15.直方图数值weigh的计算公式具体如下：
16.weight＝1-vp
·
vw
17.其中，vw为各子点与质心的连线得到矢量；vp为各子点与参考点连线所得矢量，将当前子点云集中的第一个点作为初始参考点，实时检测当前点距参考点的距离，若距离大于距离阈值，则更新当前参考点为当前子点；
18.直方图参数x计算公式具体如下：
19.x＝k*(v_angle/(2π) 0.5)
20.其中，v_angle为矢量vp与x轴正方向夹角，k为矢量角度区间的设定值；
21.基于直方图参数x和直方图数值weight构建对应子云点集的直方图hist_sp。
22.进一步的，当前激光帧的平面特点点集的提取方法具体如下：
23.s41、以任一子点pi索引(r,c)为中心向周围4点扩散；
24.s42、基于子点pi和相邻的周围4点中任意一点pj建立矢量v_sub，在点pi和点pj中选取距雷达坐标原点较远的点，该点与雷达坐标原点的连线设置为矢量v_long,，计算两矢量特征角度，若角度满足阈值要求，则点pi、点pj之间具有平面特征，并进行标记平面特征点；
25.s43、遍历剩余的周围4点，依次执行步骤s42；
26.s44、以4点的每个点为中心向周围4点扩散，执行步骤s42，直到周围没有平面特征点，若平面特点数量满足点数阈值要求，标记该点集为平面特征点集plane
t
。
27.进一步的，直方图hist
tnum
匹配得分计算公式具体如下:
28.score
num
＝{1-hist
t-1
·
hist
tnum
},num∈[1,num]
[0029]
score
num
表示直方图hist
tnum
与直方图hist
t-1
的匹配得分，hist
t-1
表示t-1时刻激光帧的直方图，hist
tnum
表示t时刻激光帧经第num个候选角度项对应角度旋转后的直方图；
[0030]
本发明是这样实现的，一种移动机器人，所述移动机器人包括处理器和存储器，所述存储器中存储有至少一条指令、至少一段程序、代码集或指令集，所述至少一条指令、所述至少一段程序、所述代码集或指令集由所述处理器加载并执行以实现上述直方图和面特征匹配的3d激光slam方法。
[0031]
本发明通过直方图确定机器人的初始姿态，基于帧间面特征约束，地图面特征二次约束来优化机器人的姿态，并确定机器人的位置，提高现机器人室内室外定位的高精度和稳定性实。
附图说明
[0032]
图1为本发明实施例提供的基于直方图和面特征匹配的3d激光slam方法流程图；
[0033]
图2为本发明实施例提供的地图平面点判别示意图。
具体实施方式
[0034]
下面对照附图，通过对实施例的描述，对本发明的具体实施方式作进一步详细的说明，以帮助本领域的技术人员对本发明的发明构思、技术方案有更完整、准确和深入的理
解。
[0035]
图1为本发明实施例提供的基于直方图和面特征匹配的3d激光slam方法流程图，该方法具体包括如下步骤：
[0036]
(1)构建3d激光雷达当前激光帧的直方图；
[0037]
将当前激光帧点云横切为sp个切片点云，每个切片点云对应一个子点云集，针对每个子点云集(切片点云)构建一个直方图，其构建方法具体如下：
[0038]
计算子点云集的质心，由每个子点和质心的连线得到矢量vw，进而得到每个点对应矢量vw与x轴正方向的夹角，通过角度进行排序，可得到有序的切片点云。
[0039]
实时检测当前子点距参考点的距离，若距离大于距离阈值，则更新当前参考点为当前子点，第一个子点为初始的参考点，由当前子点和参考点的连线得到矢量vp，进而得到该矢量vp与x轴正方向夹角v_angle，矢量vp与矢量vw的点乘结果就是两个矢量的相似度，当前点的直方图数值weight如下：
[0040]
weight＝1-vp
·
vw
[0041]
直方图自变量参数代表矢量角度区间，设置k个参数区间，对应v_angle的角度区间(-pi,pi]，那么可以得到任意一子点的直方图参数x：
[0042]
x＝k*(v_angle/(2π) 0.5)
[0043]
由直方图参数x和对应数值weight可以建立切片点云的直方图hist_sp,即以直方图参数x为横轴，以数值weight为纵轴构建切片点云(切皮点云)的直方图hist_sp，该直方图hist_sp是一个k维的向量；将sp个切片点云的直方图向量的累加，最终得到当前3d激光雷达激光帧的直方图hist
t
。
[0044]
(2)开角度匹配窗口，计算匹配候选项
[0045]
由机器人运动参数，获取机器人的最大旋转速度v_w，那么在一个雷达扫描周期t时间内机器人运动角度范围不会超过2t*v_w，这个范围即激光帧的角度匹配窗口。
[0046]
设置角度步进阈值er，通过阈值得到角度匹配候选项，总候选数目如下：
[0047][0048]
(3)直方图匹配筛选掉分数较低的角度候选项
[0049]
当前点云直方图矢量为hist
t
，通过num个角度候选项旋转直方图矢量，得到num个旋转后的直方图矢量，再和t-1时刻(上一时刻)的直方图匹配获取最优值。假设其中一个角度候选项为delta_ang，那么旋转直方图的操作就是偏移直方图的参数x，其偏移量offst计算如下：
[0050]
offst＝k*(delta_ang/(2π) 0.5)
[0051]
然后通过如下公式计算直方图匹配得分列表score
num
:
[0052]
score
num
＝{1-hist
t-1
·
hist
tnum
},num∈[1,num]
[0053]
hist
t-1
表示t-1时刻激光帧的直方图，即所有子点云集对应直方图的累加，hist
tnum
表示t时刻激光帧经第num个候选角度项对应角度旋转后的直方图；两直方图点乘即默认两个矢量点乘，点乘公式就是矢量点乘公式，各个对应元素相乘累计和。由于得分呈正态分布，所以取最大得分对应的角度候选项yaw^作为后续优化的姿态初值。
[0054]
(4)通过广度优先搜索聚类提取平面点集
[0055]
点云每个三维点(x,y,z)都有一个二维的索引(r,c)，是雷达激光头水平旋转和竖直方向旋转对应的角度索引，这里不再给出具体计算方法。
[0056]
遍历聚类时，以任一子点pi索引(r,c)为中心向周围4点扩散，以当前点和相邻的周围4点中任意一点pj建立矢量v_sub，在点pi和点pj中选取距距离雷达坐标原点较远的点，该点与雷达坐标原点的连线设置为矢量v_long,如下公式计算两矢量特征角度f_ang，如图2所示，如果角度f_ang满足阈值要求，说明点pi与点pj的矢量基本垂直于扫描线，点pi、点pj之间具有平面特征。
[0057]
f_ang＝arcos(v_sub
·
v_long/|v_sub|/|v_long|)
[0058]
其中，f_ang表示矢量v_long与矢量v_sub间的特征角度，|v_sub|表示矢量v_sub的长度，|v_long|表示矢量v_long的长度。
[0059]
遍历周围4点，标记具有平面特征的点，然后再以4点的每个点为中心，再向周围4点扩散计算聚类特征。已经标记过的不再计算，直到周围没有平面特征点，聚类结束，当聚类点集数量满足点数阈值要求，标记点集为平面特征点集plane。
[0060]
(5)以直方图匹配姿态为初始姿态，让t时刻的平面特征点和t-1时刻平面特征点集构建残差约束，优化求解位姿增量t
delta
[0061]
有直方图匹配姿态yaw^为初始姿态，设置位姿增量初值t
delta
^(yaw^)，将当前平面特征点集的每一个点planei左乘增量初值，计算t时刻激光帧中所有的平面特征点到t-1时刻激光帧平面特征点集中的平面距离dist，将此作为代价函数的残差项，非线性优化代价函数f如下所示：
[0062]
f(t
delta
)＝∑(dist
t-1
(t
delta
^(yaw^)*planei))
→0[0063]
最后通过如高斯牛顿的非线性优化方法进行迭代优化，可得到优化后的位姿增量t
delta
(dtrans,drpy)。
[0064]
(6)将历史的平面特征点集累计为点云地图mp，二次构建平面特征约束；
[0065]
已知t时刻的估计姿态t
t
^＝t
t-1
*t
delta
(dtrans,drpy)，t
t-1
为t-1时刻的姿态；将t时刻之前的所有平面特征点云累积为特征地图mp，通过mp中更多的平面特征点集构建稳定的平面约束。将t时刻点云通过估计姿态t
t
^变换到mp地图中，计算t时刻激光帧中所有的平面特征点到地图平面特征的距离dist
mp
作为残差项，具体代价函数f如下：
[0066]
f(t
t
)＝∑(dist
mp
(t
t
^)planei)-＞0
[0067]
以t
t
^为初值通过高斯牛顿优化代价函数f，得到t时刻的最终优化位姿t
t
，将t时刻特征点云通过位姿t
t
投影添加到特征点云地图，最终实现slam的同步建图和定位。
[0068]
本技术的实施例还提供了一种移动机器人，移动机器人包括:处理器和存储器，所述存储器中存储有至少一条指令、至少一段程序、代码集或指令集，所述至少一条指令、所述至少一段程序、所述代码集或指令集由所述处理器加载并执行以实现上述直方图和面特征匹配的3d激光slam方法。
[0069]
上面结合附图对本发明进行了示例性描述，显然本发明具体实现并不受上述方式的限制，只要采用了本发明的方法构思和技术方案进行的各种非实质性的改进，或未经改进将本发明的构思和技术方案直接应用于其它场合的，均在本发明的保护范围之内。