一种用于温室巡检机器人的激光SLAM建图方法与流程

一种用于温室巡检机器人的激光slam建图方法
技术领域
1.本发明涉及机器人定位与建图技术领域，具体为一种用于温室巡检机器人的激光slam建图方法。


背景技术：

2.温室巡检机器人的核心功能巡检的实现，离不开地图构建与状态估计。
3.现在比较成熟的技术是slam(同时定位与建图)，对于室内巡检机器人，建图常用的是2d激光雷达、3d激光雷达。
4.在温室环境中，存在大量农作物，整体环境结构密集复杂。若使用常规2d激光雷达进行建图，存在以下问题：1、由于2d激光雷达只扫描激光雷达所处的平面，激光点较少，会导致一些植物无法在地图体现或不能体现完整；2、由于点云数量少，无法有效的滤除对机器人运动产生干扰的激光点云数据，进而导致机器人自主移动时规划的路径较长。
5.使用3d激光雷达，其多激光线束可以更加准确的识别各种农作物，防止机器人在巡检过程中对农作物造成碾压、撞击等破坏，其次，通过对3d激光雷达生成的点云数据进行优化滤除，可以减少环境中存在的伪障碍物，如散乱的农作物枝干、叶片，优化机器人的巡检路线。
6.但是3d激光雷达建图也存在一些问题，如点云数据量过大，导致机器人计算效率低；其次，由于温室环境结构特征比较杂乱，可能导致建图鲁棒性降低。基于此，有必要发明一种用于温室巡检机器人的激光slam建图方法，能够提高建图精度，降低机器人计算成本。


技术实现要素：

7.本发明为了解决现有slam建图方法建图精度较差、计算成本较高的问题，提供了一种用于温室巡检机器人的激光slam建图方法。
8.本发明是采用如下技术方案实现的：
9.一种用于温室巡检机器人的激光slam建图方法，该方法是采用如下步骤实现的：
10.步骤a：温室巡检机器人接收3d激光雷达、imu及轮速计的信息，通过数据结合的方式进行点云畸变去除，而后利用3d激光雷达的线束对去畸变后的点云数据中的每个激光点进行划分，将位于相同线束上的点划分在一起；然后将位于方向朝下的线束上的激光点集合在一起，形成待地面分割的点云数据；
11.步骤b：将待地面分割的点云数据进行地面分割，得到地面点云数据，并将去畸变后的点云数据中的其他点划入非地面点云数据；
12.步骤c：通过聚类算法对非地面点云数据进行点云聚类，剔除噪声点；接着提取剔除噪声点后的非地面点云数据的面特征和边缘特征；
13.步骤d：根据提取的面特征、边缘特征将连续帧点云数据进行特征匹配，进而得到在激光雷达坐标系下的连续帧点云数据的位姿变换矩阵；
14.步骤e：根据位姿变换矩阵将当前帧点云数据投影到世界坐标系下，由此构建温室
巡检机器人的子地图；
15.步骤f：重复执行步骤a-步骤e，即可完成温室巡检机器人的激光slam建图。
16.进一步地，步骤a中点云畸变去除的具体步骤为：首先根据imu的加速度、角速度和轮速计的速度得到每个激光点采集时刻的3d激光雷达位姿，而后将每个激光点投影到同一个位姿下，然后封装成一帧激光数据发布出去，由此得到去畸变后的一帧点云数据。
17.进一步地，步骤a中对去畸变后的点云数据中的激光点进行划分的具体步骤为：首先根据激光点的坐标，求得激光点与3d激光雷达的连线与激光坐标x-y平面之间的夹角θ，而后将夹角值相同的激光点划分在一起。
18.进一步地，步骤b中地面分割的具体步骤为：将3d激光雷达的质心设为点a，3d激光雷达在时间t获取的点云数据为p
t
＝{p1,p
2,
p3…
pn}，其中pi是p
t
中的一个点，pi＝(xi,yi,zi)，p
id
为待地面分割的点云数据中的一个点，3d激光雷达距离地面的高度为h，计算质心a到点p
id
的向量在竖直方向的投影h，考虑到温室的非结构化路面环境，如h-h≤0.05m，则p
id
为地面点，将其划入地面点云数据；去畸变后的点云数据中的其他点划入非地面点云数据。
19.进一步地，步骤c中点云聚类时通过欧几里得聚类算法进行计算，具体步骤为：首先选取非地面点云数据中某点pi，i＝1～n，通过kd-tree近邻搜索算法找到k个离点pi最近的点，这些点中距离小于设定阈值的便聚类到集合q中；如果集合q中元素的数目不再增加，整个聚类过程便结束；否则须在集合q中选取pi点以外的点，重复上述过程，直到集合q中元素的数目不再增加为止，然后统计集合q中点的数目m，若m≤20，则判断集合q中元素为对机器人实际运动不会造成影响的噪音点，并剔除掉该聚类。
20.进一步地，步骤c中提取面特征和边缘特征的具体步骤为：首先计算剔除噪声点后的非地面点云数据的点云曲率，曲率计算是利用与该点位于同一扫描线上的前后各五个点来计算的，曲率公式如下：
[0021][0022]
其中，c表示曲率；k代表了一次扫描周期，k∈z
+
，一个扫描周期内获取到的点云记作p，pk代表第k个周期的扫描的点云，i是pk中的一个点，l代表激光雷达坐标系，s是同一扫描中i的一组连续点集，令s＝10，是在激光雷达坐标系下i在该扫描线上前后相邻的五个点；
[0023]
而后将计算得到的曲率从大到小进行排列，设置曲率阈值0≤c≤1，曲率大于等于c的点为边缘特征点，曲率小于c的为面特征点，然后分别对边缘特征点和面特征点进一步划分，曲率大于等于(1+c)/2的为强边缘特征点，其余的边缘特征点为弱边缘特征点；曲率小于c/2的为强面特征点，其余的面特征点为弱面特征点。
[0024]
进一步地，步骤d中进行特征匹配时，首先从一帧点云数据提取曲率较大的10个强边缘特征点与曲率较大的20个强面特征点，若该帧点云数据中强边缘特征点与强面特征点数量不足，则从弱边缘特征点与弱面特征点中补充；而后按照先强特征点、后弱特征点的顺
序分别进行边缘特征的匹配与面特征的匹配，将当前帧扫描的边缘特征点和面特征点与前一帧的边缘特征点和面特征点建立对应关系，然后采用lm方法寻找两帧连续扫描之间的最小距离变换；最后得出两帧连续扫描的位姿变换矩阵
[0025]
进一步地，在k+1帧扫描结束时，3d激光雷达(1)里程计会产生一个不失真的点云数据p
k+1
，同时产生一个姿态变换矩阵它包含了在t
k+1
和t
k+2
之间的3d激光雷达(1)的运动，步骤e中将当前帧点云数据投影到世界坐标系下是指将在世界坐标{w}下匹配和对齐点云p
k+1
，具体步骤如下：将qk定义为累计到第k次扫描所形成的点云地图上的点云数据，将定义为第k次扫描到第k+1次扫描结束时世界坐标系下上3d激光雷达(1)的姿态变换矩阵，通过3d激光雷达(1)里程计的输出，建图算法将扩展以获得并将p
k+1
投影到世界坐标{w}，表示为q
k+1
，然后通过优化将q
k+1
和qk匹配。
[0026]
本发明所述的一种用于温室巡检机器人的激光slam建图方法通过对点云数据进行分割，提高了计算效率，方便对点云数据进行特征提取，然后使用聚类算法滤除少于20个点的噪声点云集合，并通过设定曲率参数c的范围，对边缘特征和面特征进行提取和强度划分，进而最终提高建图的精度，减少温室巡检机器人的计算负荷。
[0027]
本发明提在满足建图精度的同时，减小了机器人建图的计算成本，提出一种简单高效的地面分割方法，对地平面和立体空间进行分割；并且在特征提取与特征匹配环节，提出对特征点进行强弱划分，在提高建图精度的同时可以加快建图的速度。
附图说明
[0028]
图1为本发明中温室巡检机器人的结构示意图；
[0029]
图2为本发明的计算流程示意图；
[0030]
图3为本发明步骤b中地面分割的原理图；
[0031]
图4为本发明步骤e中将当前帧点云数据投影到世界坐标系下的过程示意图。
[0032]
图中：1-3d激光雷达，2-imu，3-轮速计。
具体实施方式
[0033]
需要说明的是，在不冲突的情况下，本技术中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本发明。
[0034]
如附图1-2所示，根据本发明的一个实施例，提供了一种用于温室巡检机器人的激光slam建图方法，该方法是采用如下步骤实现的：
[0035]
步骤a：温室巡检机器人接收3d激光雷达1、imu2及轮速计3的信息，通过数据结合的方式进行点云畸变去除，而后利用3d激光雷达1的线束对去畸变后的点云数据中的每个激光点进行划分，将位于相同线束上的点划分在一起；然后将位于方向朝下的线束上的激光点集合在一起，形成待地面分割的点云数据；
[0036]
点云畸变去除的具体步骤为：首先根据imu2的加速度、角速度和轮速计3的速度得到每个激光点采集时刻的3d激光雷达1位姿，而后将每个激光点投影到同一个位姿下，然后封装成一帧激光数据发布出去，由此得到去畸变后的一帧点云数据。
[0037]
对去畸变后的点云数据中的激光点进行划分的具体步骤为：首先根据激光点的坐标，求得激光点与3d激光雷达1的连线与激光坐标x-y平面之间的夹角θ，而后将夹角值相同的激光点划分在一起，依据夹角的不同划分不同的线束并进行标记。
[0038]
步骤b：将待地面分割的点云数据进行地面分割，得到地面点云数据，并将去畸变后的点云数据中的其他点划入非地面点云数据；
[0039]
地面分割的具体步骤为：将3d激光雷达1的质心设为点a，3d激光雷达1在时间t获取的点云数据为p
t
＝{p1,p
2,
p3…
pn}，其中pi是p
t
中的一个点，pi＝(xi,yi,zi)，p
id
为待地面分割的点云数据中的一个点，由于激光雷达在移动机器人上都是水平于地面安装，且3d激光雷达1距离地面的高度为h，且质心a到点p
id
的向量与激光雷达竖直方向的夹角为α
id
，计算质心a到点p
id
的向量在竖直方向的投影h，考虑到温室的非结构化路面环境，如h-h≤0.05m，则p
id
为地面点，对其标记后，将其划入地面点云数据；去畸变后的点云数据中的其他点划入非地面点云数据。
[0040]
标记的地面点不再用于特征提取，这样减少了用于特征提取的点云数据，提高了之后特征提取的效率。
[0041]
步骤c：通过聚类算法对非地面点云数据进行点云聚类，剔除噪声点；接着提取剔除噪声点后的非地面点云数据的面特征和边缘特征；
[0042]
考虑到温室环境存在大量农作物、杂草，包括一些其他微小体积的不确定物体，这些物体会造成最终构建的点云地图环境杂乱，因此采用欧几里得聚类算法对这种微小的、对机器人实际运动不会造成影响的点云数据进行滤除，对于欧式聚类来说，距离判断准则为欧氏距离。
[0043]
点云聚类时通过欧几里得聚类算法进行计算，具体步骤为：首先选取非地面点云数据中某点pi，i＝1～n，通过kd-tree近邻搜索算法找到k个离点pi最近的点，这些点中距离小于设定阈值的便聚类到集合q中；如果集合q中元素的数目不再增加，整个聚类过程便结束；否则须在集合q中选取pi点以外的点，重复上述过程，直到集合q中元素的数目不再增加为止，然后统计集合q中点的数目m，若m≤20，则判断集合q中元素为对机器人实际运动不会造成影响的噪音点，并剔除掉该聚类。
[0044]
特征提取采用与loam中相同的方式，但是对特征的强弱进行区分。提取面特征和边缘特征的具体步骤为：
[0045]
首先计算剔除噪声点后的非地面点云数据的点云曲率，曲率计算是利用与该点位于同一扫描线上的前后各五个点来计算的，曲率公式如下：
[0046][0047]
其中，c表示曲率；k代表了一次扫描周期，k∈z
+
，一个扫描周期内获取到的点云记作p，pk代表第k个周期的扫描的点云，i是pk中的一个点，l代表激光雷达坐标系，s是同一扫描中i的一组连续点集，令s＝10，是在激光雷达坐标系下i在该扫描线上前后相邻的五个点；
[0048]
在三维空间中处于尖锐边缘上的点，曲率较高，处于平滑平面上的点，曲率较低，而后将计算得到的曲率从大到小进行排列，设置曲率阈值0≤c≤1，曲率大于等于c的点为边缘特征点，曲率小于c的为面特征点，然后分别对边缘特征点和面特征点进一步划分，曲率大于等于(1+c)/2的为强边缘特征点，其余的边缘特征点为弱边缘特征点；曲率小于c/2的为强面特征点，其余的面特征点为弱面特征点。
[0049]
步骤d：根据提取的面特征、边缘特征将连续帧点云数据进行特征匹配，进而得到在激光雷达坐标系下的连续帧点云数据的位姿变换矩阵；
[0050]
本步骤的目的在于通过特征匹配，scan-to-scan推算两帧激光之间的相对位姿变换；
[0051]
进行特征匹配时，首先从一帧点云数据提取曲率较大的10个强边缘特征点与曲率较大的20个强面特征点，若该帧点云数据中强边缘特征点与强面特征点数量不足，则从弱边缘特征点与弱面特征点中补充；因为对每一特征点的强弱特征都做了记录，所以在匹配的时候按照先强特征点、后弱特征点的顺序分别进行边缘特征的匹配与面特征的匹配，将当前帧扫描的边缘特征点和面特征点与前一帧的边缘特征点和面特征点建立对应关系。
[0052]
现在有pk,p
k+1
两帧点云，首先是边缘特征点的匹配，取点云数据p
k+1
中的边缘点i，通过kd-tree从上一帧点云数据pk中找到一个最近点j，然后在j点相邻的上下扫描线中找到另外一个距离点i最近的点l，那么点j和点l构成一个边缘，因此，对于边缘点，优化的目标就是，点i到直线jl的距离最近，即：
[0053][0054]
式中，d
ζ
是点i到直线jl的距离，xi,xj,x
l
分别是点i，j，l的坐标，分子为两个向量的积，即两个向量构成平行四边形的面积，分母为平行四边形的底边，所以结果是点i到直线jl的距离。
[0055]
然后是面特征点的匹配，在点云p
k+1
中取面点a，在上一帧pk中找到与a点最近的面点b，在同一条扫描线找到距离a点最近的另一个点c，然后在b点的上下相邻的扫描线找距离点a最近的点d，那么点b，点c，点d就构成了一个平面，对于平面特征点来说，优化的目标是点a到平面bcd的距离最近，即
[0056][0057]
式中，dh是点a到平面bcd的距离，xa,xb,xc,xd分别是点a、点b、点c、点d的坐标，分子为3个向量的混合积，即三个向量构成的三维物体的体积，分母为底面的面积，结果为点a到平面bcd的距离。
[0058]
然后采用lm方法寻找两帧连续扫描之间的最小距离变换；最后得出两帧连续扫描的位姿变换矩阵
[0059]
通过这种方法寻找特征对应关系可以提高特征匹配精度，缩小了特征匹配的范围，用有更高的效率。
[0060]
步骤e：根据位姿变换矩阵将当前帧点云数据投影到世界坐标系下，由此构建温室
巡检机器人的子地图；
[0061]
本步骤的目的在于通过建图，scan-to-map，获得世界坐标系下的位姿；
[0062]
建图算法运行的频率比3d激光雷达1里程计的频率低，每次扫描只调用一次，在k+1帧扫描结束时，3d激光雷达1里程计会产生一个不失真的点云数据p
k+1
，同时产生一个姿态变换矩阵它包含了在t
k+1
和t
k+2
之间的3d激光雷达1的运动，步骤e中将当前帧点云数据投影到世界坐标系下是指将在世界坐标{w}下匹配和对齐点云p
k+1
，如附图4所示，具体步骤如下：将qk定义为累计到第k次扫描所形成的点云地图上的点云数据，将定义为第k次扫描到第k+1次扫描结束时世界坐标系下上3d激光雷达1的姿态变换矩阵，通过3d激光雷达1里程计的输出，建图算法将扩展以获得并将p
k+1
投影到世界坐标{w}，表示为q
k+1
，然后通过优化将q
k+1
和qk匹配。
[0063]qk+1
和qk匹配过程中，同样需要进行特征的提取与匹配，特征提取方法与前文所述方法基本一致，在提取特征中，对强特征与弱特征的数量进行统计，设q
k+1
中的总特征数量为n，边缘特征数量为n1，平面特征数量为n2，强边缘特征数量为n
1s
，弱边缘特征数量为n
1w
，强平面特征数量为n
2s
，弱平面特征数量为n
2w
，即n＝n1+n2＝n
1s
+n
1w
+n
2s
+n
2w
，
[0064]
在q
k+1
中，若强边缘特征数量n
1s
≥25，强平面特征数量n
2s
≥50，则认为q
k+1
环境特征明显，使用强特征对q
k+1
和qk匹配，使用特征数量：25≤n
1s
≤50，50≤n
2s
≤100；若强边缘特征数量n
1s
≤25，强平面特征数量n
2s
≤50，则认为q
k+1
环境特征不明显，使用强特征结合弱对q
k+1
和qk匹配，使用特征数量：n1＝75，n2＝150；若强边缘特征数量n
1s
≤25，强平面特征数量n
2s
≥50，则认为q
k+1
环境特征普通，使用强特征结合弱对q
k+1
和qk匹配，使用特征数量：n1＝50；50≤n
2s
≤100；若强边缘特征数量n
1s
≥25，强平面特征数量n
2s
≤50，则认为q
k+1
环境特征普通，使用强特征结合弱对q
k+1
和qk匹配，使用特征数量：25≤n
1s
≤50，n2＝100。
[0065]
步骤f：重复执行步骤a-步骤e，即可完成温室巡检机器人的激光slam建图。
[0066]
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。