一种用于温室巡检机器人的激光SLAM建图方法技术

本发明专利技术具体为一种用于温室巡检机器人的激光SLAM建图方法，解决了现有SLAM建图方法建图精度较差、计算成本较高的问题。一种用于温室巡检机器人的激光SLAM建图方法，该方法是采用如下步骤实现的；a、接收3D激光雷达、IMU及轮速计的信息，点云畸变去除后对每个激光点进行划分；b、地面分割，得到地面点云数据；c、对非地面点云数据进行点云聚类，剔除噪声点；提取面特征和边缘特征；d、特征匹配，得到位姿变换矩阵；e、将当前帧点云数据投影到世界坐标系下；f、重复执行a

【技术实现步骤摘要】
一种用于温室巡检机器人的激光SLAM建图方法


[0001]本专利技术涉及机器人定位与建图
，具体为一种用于温室巡检机器人的激光SLAM建图方法。

技术介绍

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

技术实现思路

[0007]本专利技术为了解决现有SLAM建图方法建图精度较差、计算成本较高的问题，提供了一种用于温室巡检机器人的激光SLAM建图方法。
[0008]本专利技术是采用如下技术方案实现的：
[0009]一种用于温室巡检机器人的激光SLAM建图方法，该方法是采用如下步骤实现的：
[0010]步骤a：温室巡检机器人接收3D激光雷达、IMU及轮速计的信息，通过数据结合的方式进行点云畸变去除，而后利用3D激光雷达的线束对去畸变后的点云数据中的每个激光点进行划分，将位于相同线束上的点划分在一起；然后将位于方向朝下的线束上的激光点集合在一起，形成待地面分割的点云数据；
[0011]步骤b：将待地面分割的点云数据进行地面分割，得到地面点云数据，并将去畸变后的点云数据中的其他点划入非地面点云数据；
[0012]步骤c：通过聚类算法对非地面点云数据进行点云聚类，剔除噪声点；接着提取剔除噪声点后的非地面点云数据的面特征和边缘特征；
[0013]步骤d：根据提取的面特征、边缘特征将连续帧点云数据进行特征匹配，进而得到在激光雷达坐标系下的连续帧点云数据的位姿变换矩阵；
[0014]步骤e：根据位姿变换矩阵将当前帧点云数据投影到世界坐标系下，由此构建温室
巡检机器人的子地图；
[0015]步骤f：重复执行步骤a
‑
步骤e，即可完成温室巡检机器人的激光SLAM建图。
[0016]进一步地，步骤a中点云畸变去除的具体步骤为：首先根据IMU的加速度、角速度和轮速计的速度得到每个激光点采集时刻的3D激光雷达位姿，而后将每个激光点投影到同一个位姿下，然后封装成一帧激光数据发布出去，由此得到去畸变后的一帧点云数据。
[0017]进一步地，步骤a中对去畸变后的点云数据中的激光点进行划分的具体步骤为：首先根据激光点的坐标，求得激光点与3D激光雷达的连线与激光坐标x
‑
y平面之间的夹角θ，而后将夹角值相同的激光点划分在一起。
[0018]进一步地，步骤b中地面分割的具体步骤为：将3D激光雷达的质心设为点A，3D激光雷达在时间t获取的点云数据为p
t
＝{p1,p
2,
p3…
p
n
}，其中p
i
是p
t
中的一个点，p
i
＝(x
i
,y
i
,z
i
)，p
id
为待地面分割的点云数据中的一个点，3D激光雷达距离地面的高度为H，计算质心A到点p
id
的向量在竖直方向的投影h，考虑到温室的非结构化路面环境，如H
‑
h≤0.05m，则p
id
为地面点，将其划入地面点云数据；去畸变后的点云数据中的其他点划入非地面点云数据。
[0019]进一步地，步骤c中点云聚类时通过欧几里得聚类算法进行计算，具体步骤为：首先选取非地面点云数据中某点p
i
，i＝1～n，通过KD
‑
Tree近邻搜索算法找到k个离点p
i
最近的点，这些点中距离小于设定阈值的便聚类到集合Q中；如果集合Q中元素的数目不再增加，整个聚类过程便结束；否则须在集合Q中选取p
i
点以外的点，重复上述过程，直到集合Q中元素的数目不再增加为止，然后统计集合Q中点的数目m，若m≤20，则判断集合Q中元素为对机器人实际运动不会造成影响的噪音点，并剔除掉该聚类。
[0020]进一步地，步骤c中提取面特征和边缘特征的具体步骤为：首先计算剔除噪声点后的非地面点云数据的点云曲率，曲率计算是利用与该点位于同一扫描线上的前后各五个点来计算的，曲率公式如下：
[0021][0022]其中，c表示曲率；k代表了一次扫描周期，k∈Z
+
，一个扫描周期内获取到的点云记作p，p
k
代表第k个周期的扫描的点云，i是p
k
中的一个点，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...

【技术保护点】

【技术特征摘要】
1.一种用于温室巡检机器人的激光SLAM建图方法，其特征在于：该方法是采用如下步骤实现的：步骤a：温室巡检机器人接收3D激光雷达(1)、IMU(2)及轮速计(3)的信息，通过数据结合的方式进行点云畸变去除，而后利用3D激光雷达(1)的线束对去畸变后的点云数据中的每个激光点进行划分，将位于相同线束上的点划分在一起；然后将位于方向朝下的线束上的激光点集合在一起，形成待地面分割的点云数据；步骤b：将待地面分割的点云数据进行地面分割，得到地面点云数据，并将去畸变后的点云数据中的其他点划入非地面点云数据；步骤c：通过聚类算法对非地面点云数据进行点云聚类，剔除噪声点；接着提取剔除噪声点后的非地面点云数据的面特征和边缘特征；步骤d：根据提取的面特征、边缘特征将连续帧点云数据进行特征匹配，进而得到在激光雷达坐标系下的连续帧点云数据的位姿变换矩阵；步骤e：根据位姿变换矩阵将当前帧点云数据投影到世界坐标系下，由此构建温室巡检机器人的子地图；步骤f：重复执行步骤a
‑
步骤e，即可完成温室巡检机器人的激光SLAM建图。2.根据权利要求1所述的一种用于温室巡检机器人的激光SLAM建图方法，其特征在于：步骤a中点云畸变去除的具体步骤为：首先根据IMU(2)的加速度、角速度和轮速计(3)的速度得到每个激光点采集时刻的3D激光雷达(1)位姿，而后将每个激光点投影到同一个位姿下，然后封装成一帧激光数据发布出去，由此得到去畸变后的一帧点云数据。3.根据权利要求2所述的一种用于温室巡检机器人的激光SLAM建图方法，其特征在于：步骤a中对去畸变后的点云数据中的激光点进行划分的具体步骤为：首先根据激光点的坐标，求得激光点与3D激光雷达(1)的连线与激光坐标x
‑
y平面之间的夹角θ，而后将夹角值相同的激光点划分在一起。4.根据权利要求1所述的一种用于温室巡检机器人的激光SLAM建图方法，其特征在于：步骤b中地面分割的具体步骤为：将3D激光雷达(1)的质心设为点A，3D激光雷达(1)在时间t获取的点云数据为p
t
＝{p1,p
2,
p3…
p
n
}，其中p
i
是p
t
中的一个点，p
i
＝(x
i
,y
i
,z
i
)，p
id
为待地面分割的点云数据中的一个点，3D激光雷达(1)距离地面的高度为H，计算质心A到点p
id
的向量在竖直方向的投影h，考虑到温室的非结构化路面环境，如H
‑
h≤0.05m，则p
id
为地面点，将其划入地面点云数据；去畸变后的点云数据中的其他点划入非地面点云数据。5.根据权利要求1所述的一种用于温室巡检机器人的激光SLAM建图方法，其特征在于：步骤c中点云聚类时通过欧几里得聚类算法进行计算，具体步骤为：首先选取非地面点云数据中某点p
i
，i＝1～n，通过KD
‑<...

【专利技术属性】
技术研发人员：张东光，刘治，牛蔺楷，吴亚丽，
申请(专利权)人：清泽智能太原科技有限公司，
类型：发明
国别省市：