(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210938849.8 (22)申请日 2022.08.05 (71)申请人 太原理工大 学 地址 030024 山西省太原市迎泽西大街79 号 申请人 清泽智能 （太原） 科技有限公司 (72)发明人 张东光　刘治　牛蔺楷　吴亚丽　 (74)专利代理 机构 太原新航路知识产权代理事 务所(特殊普通 合伙) 14112 专利代理师 王云峰 (51)Int.Cl. G06T 17/05(2011.01) G01C 21/00(2006.01) G01C 21/20(2006.01) G06T 5/00(2006.01)G06T 7/13(2017.01) G06T 7/136(2017.01) G06V 10/30(2022.01) G06V 10/44(2022.01) G06V 10/75(2022.01) G06V 10/762(2022.01) G06V 20/17(2022.01) (54)发明名称 一种用于温室巡检机器人的激光SLAM建图 方法 (57)摘要 本发明具体为一种用于温室巡检机器人的 激光SLAM建图方法， 解决了现有SLA M建图方法建 图精度较差、 计算成本较高的问题。 一种用于温 室巡检机器人的激光SLAM建图方法， 该方法是采 用如下步骤实现的； a、 接收3D激光雷达、 IMU及轮 速计的信息， 点云畸变去除后对每个激光点进行 划分； b、 地面 分割， 得到地面点云数据； c、 对非地 面点云数据进行点云聚类， 剔除噪声点； 提取面 特征和边缘特征； d、 特征匹配， 得到位姿变换矩 阵； e、 将当前帧点云数据投影到世界坐标系下； f、 重复执行a ‑e， 即可完成建图。 本发明通过对点 云数据进行分割、 对边缘特征和面特征进行强度 划分， 提高了计算效率， 提高了建图的精度， 减少 温室巡检机器人的计算负荷。 权利要求书2页 说明书6页 附图3页 CN 115294287 A 2022.11.04 CN 115294287 A 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 获取的点云数据为pt＝{p1,p2,p3…pn}， 其中pi是pt中的一个 点， pi＝(xi,yi,zi)， pid为待地面 分割的点云数据中的一个 点， 3D激光 雷达(1)距离地面的高度为H， 计 算质心A到点pid的向量 在竖直方向的投影h， 考虑到温室的非结构化路面环境， 如H ‑h≤0.05m， 则pid为地面 点， 将其划入地 面点云数据； 去畸变后的点云数据中的其 他点划入非地 面点云数据。 5.根据权利要求1所述的一种用于温室巡检机器人的激光SLAM建图方法， 其特征在于： 步骤c中点云聚类时通过欧几里得聚类算法进行计算， 具体步骤为： 首先选取非地面点云数 据中某点pi， i＝1～n， 通 过KD‑Tree近邻搜索算法找到 k个离点pi最近的点， 这 些点中距离小 于设定阈值的便聚类到集合Q中； 如果集合Q中元素的数目不再增加， 整个聚类过程便结束； 否则须在集合Q中选取pi点以外的点， 重复上述过程， 直到集合Q中元素的数目不再增加为 止， 然后统计集合Q中点的数目m， 若m≤20， 则判断集合Q中元素为对机器人实际运动不会造 成影响的噪音点， 并剔除掉该聚类。 6.根据权利要求5所述的一种用于温室巡检机器人的激光SLAM建图方法， 其特征在于： 步骤c中提取面特征和边缘特征 的具体步骤为： 首先计算剔除噪声点后的非地面点云数据权　利　要　求　书 1/2 页 2 CN 115294287 A 2的点云曲率， 曲率计算是利用与该点位于同一扫描线上 的前后各五个点来计算的， 曲率公 式如下： 其中， c表示曲率； k代表了一次扫描周期， k∈Z+， 一个扫描周期内获取到的点云记作p， pk代表第k个周期的扫描的点云， i 是pk中的一个 点， L代表激光 雷达坐标系， S是同一扫描中i 的一组连续点集， 令S＝10， 是在激光雷达坐标系下i在该扫描线上前后相邻的五个 点； 而后将计算得到的曲率从大到小进行排列， 设置曲率阈值0≤C≤1， 曲率大于等于C的 点为边缘特征点， 曲率小于C的为面特征点， 然后分别对边缘特征点和面特征点进一步划 分， 曲率大于等于(1+C)/2的为强边缘特征点， 其余的边缘特征点为弱边缘特征点； 曲率小 于C/2的为强面特 征点， 其余的面特 征点为弱面特 征点。 7.根据权利要求6所述的一种用于温室巡检机器人的激光SLAM建图方法， 其特征在于： 步骤d中进行特征匹配时， 首先从一帧点云数据提取曲率较大的10个强边缘特征点与曲率 较大的20个强面特征点， 若该帧点云数据中强边缘特征点与强面特征点数量不足， 则从弱 边缘特征点与弱面特征点中补充； 而后按照先强特征点、 后弱特征点的顺序分别进行边缘 特征的匹配与面特征的匹配， 将当前帧扫描的边缘特征点和面特征点与前一帧的边缘特征 点和面特征点建立对应关系， 然后采用LM方法寻找两帧连续扫描之间的最小距离变换； 最 后得出两帧连续扫描的位姿变换矩阵 8.根据权利要求7所述的一种用于温室巡检机器人的激光SLAM建图方法， 其特征在于： 在k+1帧扫描 结束时， 3D激光雷达(1)里程计会产生一个不失真的点云数据PK+1， 同时产生一 个姿态变换矩阵 它包含了在tk+1和tk+2之间的3D激光雷达(1)的运动， 步骤e中将 当前 帧点云数据投影到世界坐标系下是指将在世界坐标{W}下匹配和对齐点云PK+1， 具体步骤如 下： 将QK定义为累计到第k次扫描所形成的点云地图上的点云数据， 将 定义为第k次扫描 到第k+1次扫描结束时世界坐标系下上3D激光雷达(1)的姿态变换矩阵， 通过3D激光雷达 (1)里程计的输出， 建图算法将 扩展以获得 并将PK+1投影到世界坐标{W}， 表示为 QK+1， 然后通过优化 将QK+1和QK匹配。权　利　要　求　书 2/2 页 3 CN 115294287 A 3