基于2D激光雷达测距的折线提取极大似然估计的方法.pdf

《基于2D激光雷达测距的折线提取极大似然估计的方法.pdf》由会员分享，可在线阅读，更多相关《基于2D激光雷达测距的折线提取极大似然估计的方法.pdf（16页完成版）》请在专利查询网上搜索。

1、(19)中华人民共和国国家知识产权局 (12)发明专利申请 (10)申请公布号 (43)申请公布日 (21)申请号 201910368726.3 (22)申请日 2019.05.05 (71)申请人 桂林电子科技大学 地址 541004 广西壮族自治区桂林市金鸡 路1号 (72)发明人 匡兵田春月陈凤冉孙毛毛 曾宪锋 (74)专利代理机构 桂林市华杰专利商标事务所 有限责任公司 45112 代理人 杨雪梅 (51)Int.Cl. G01S 17/08(2006.01) G01S 17/88(2006.01) G01S 7/48(2006.01) (54)发明名称 一种基于2D激光雷达测距的折线。

2、提取极大 似然估计的方法 (57)摘要 本发明公开了一种基于2D激光雷达测距的 折线提取极大似然估计的方法， 该方法选取Ros 框架作为本次算法实验的平台， 并安装基于 RoboSence3D激光雷达的接口驱动， 调用驱动雷 达对实验环境进行扫描， 为了便于特征线的提 取， 将3D点云线束进行转换为2D的激光点云线 束， 获取激光雷达的点云数据话题， 同时启动 riviz可视化建图指令， 构建二维点云地图， 为下 一步的折线提取和折线优化做准备， 最终实现激 光基于折线特征提取的较为精确的平面地图。 该 方法的折线提取简单、 效率和准确性高， 为后续 机器人在环境地图中进行路径规划和导航提供 。

3、了可能。 权利要求书3页 说明书7页 附图5页 CN 110109134 A 2019.08.09 CN 110109134 A 1.一种基于2D激光雷达测距的折线提取极大似然估计的方法， 其特征在于， 包括如下 步骤： 1)在ROS平台中建立工作区间， 在工作区间内安装robortsence3D激光雷达驱动并与激 光雷达的wifi连接， 在避免强光照射下， 使用3D激光雷达对周围环境进行扫描， 进行3D激光 雷达数据采集， 得到原始的3D点云数据包， 并储存在rosbag中， 并建立三维点云地图； 2)对步骤1)得到的原始3D点云数据进行数据转换， 转换成二维激光数据， 从而便于环 境地图的。

4、特征提取和优化； 3)根据获得的二维激光数据， 建立基于机器人的运动模型， 通过平面上机器人的坐标 和航向角推算从而获得点云数据的相对位姿信息， 在任意t时刻和激光雷达的观测模型相 结合， 完成机器人定位， 然后通过最小二乘法的优化算法建立静态SLAM下的环境初始地图 得到二维点云地图； 4)在环境初始地图的基础上， 对二维激光扫描点进行最大似然估计， 同时通过折线提 取和折线优化两部分， 寻找使扫描测量可能性最大化的一组多段线,得到极大似然估计下 的概率最大的环境地图。 2.根据权利要求1所述的一种基于2D激光雷达测距的折线提取极大似然估计的方法， 其特征在于， 步骤1)中， 所述的3D激光。

5、雷达数据采集， 是在Ros框架中调用robortsence3D激 光雷达的点云数据包， 为环境初始地图的建立做准备。 3.根据权利要求1所述的一种基于2D激光雷达测距的折线提取极大似然估计的方法， 其特征在于， 步骤2)中， 所述的数据转换， 是将3D点云转换成2维的激光线束， 具体包括如下 步骤： 2-1)首先在roscore节点下调用roslaunch启动3D激光雷达驱动； 2-2)通过pointcloud-to-laserscan节点获得多条3D扫描线， 并将扫描数据封装成数 据集， 便于后续使用； 2-3)将3D线束转换成2D线束， 将z轴高度信息和角度yaw进行坐标转换， 同时输出角。

6、度 增量信息和x、 y轴坐标， 确定2D激光雷达的有效测量范围， 将获取的话题数据在riviz中显 示， 在ROS中将三维空间中的点投影到可以展开的圆柱形表面上， 以将其平面化， 利用此方 法可以将点3D点云数据转换2D激光数据， 创建转换包中的launch文件， 并设置节点和订阅 信息， 启动雷达驱动包和转换包节点， 在RVIZ中可以看到三维点云数据， 测试是否能够成功 显示二维激光数据， 还需要在RVIZ中添加laserscan.然后将topic改为/scan即可。 4.根据权利要求1所述的一种基于2D激光雷达测距的折线提取极大似然估计的方法， 其特征在于， 步骤3)中， 所述的环境初始地。

7、图， 是激光SLAM在构建机器人位姿时首先通过传 感器获取轮式机器人的速度位置信息， 通过运动解算构建机器人的运动学模型， 获得车体 的位姿矩阵， 使用欧式变换对车体自身坐标进行航迹推算， 最终获得机器人在全局坐标系 中的位姿， 即激光在地图中初始位置已知； 建立激光SLAM的观测模型， 通过传感器获得的在 t0到tn的时间内传感器的观测值Zt， 估计状态量在t时刻的概率分布， 从而计算出环境特征 值 t和机器人的初始位姿S0， 通过建立环境地图和确定自身位姿的不断重复， 构建出基于点 云信息的二维局部地图。 5.根据权利要求1所述的一种基于2D激光雷达测距的折线提取极大似然估计的方法， 其特。

8、征在于， 步骤3)中， 所述的机器人定位， 由于单独的轮式里程计和激光雷达的航迹推算 权利要求书 1/3 页 2 CN 110109134 A 2 信息缺少必要的回环检测， 因此为了避免原始环境地图产生运动畸变问题， 将步骤2-3)中 成功转换的2D点云数据的当前帧与其之前帧进行匹配， 通过SVD变换求解出帧间匹配的解， 从而求解出两组点云数据间的最小均方差， 假设两帧数据无限接近， 即概率格子地图中接 近1， 然后构造最小二乘法， 计算使误差偏导为0， 求解高斯牛顿方程， 计算出激光在后一帧 时刻的位姿， 完成定位。 6.根据权利要求1所述的一种基于2D激光雷达测距的折线提取极大似然估计的方。

9、法， 其特征在于， 步骤4)中， 所述的概率最大的环境地图， 具体是： 首先连接环境初始地图边缘 的相邻数据点集， 经选定合适的阈值匹配后， 根据其坐标数据确定一组初始化多边形， 之后 迭代删除在测量概率方面产生最小误差的顶点直到达到最大似然估计中给定的阈值， 从而 得到一组顶点位置扫描端点已知的位置折线， 根据已提取折线建立相应的条件概率模型， 通过求解出非线性方程组的近似解， 得到使扫描概率最大化的几何多边形位置， 从而得到 极大似然估计下的概率最大的环境地图。 7.根据权利要求1所述的一种基于2D激光雷达测距的折线提取极大似然估计的方法， 其特征在于， 步骤4)中， 所述的折线提取和折线。

10、优化， 具体是通过连接所有扫描端点构造初 始化多边形结构， 然后在初始化多边形的基础上迭代地移除在测量概率方面引起最小误差 的顶点， 计算点云到多边形边的距离， 从而提出无用的点云数据， 若得到的距离均小于设定 的阈值dmax， 则得到初始多边形顶点的位置与扫描端点的子集的位置一致的一组折线， 其中 阈值dmax的设定通常取决于直线剪裁的程度， 若dmax选择较大， 则会删除线段中间的顶点， 若 选择的过小， 则可能会误删其它线段两端的顶点， 其中： P(z|L)N(r(z)， r(z， l)， ) (1) 上述公式(1)中， 在一组多段线中， P(z|L)为单射线激光的半径分布模型， 其中定。

11、义单 线激光的测量为模型zx,y， 它两个双元素的列向量组成， 分别为射线的起始点x和终点 y， 确定多边形段线的集合Lzi， 其中z为多边形的顶点， 相对于多段线l的坐标系指定， 在扫描面建立线的概率服从高斯分布， r为测量射线的半径， 为径向噪声； 上述公式(1)中， 测量的射线半径为r|y-x|， 推广到整个扫描的测量概率为： 从而通过建立的条件概率模型， 力求得到的概率最大化； 定义L*作为多边形的最大顶点数Jmax， 用于折线提取， 从而将问题转换成已知多边形线 段集合L， 求解最大顶点个数的问题， 如下公式(3)所示， 通过迭代方法找出合适的线段L*的 集合， 此时顶点可达到最多的。

12、Jmax， 从而确定几何多边形的范围， L*的表达式如下： 使用最大似然估计移除顶点的过程如下： 权利要求书 2/3 页 3 CN 110109134 A 3 上述公式(4)中， p(z|Lmj)代表条件测量的概率密度函数， 目的是建图时在一组多边形 线L上移除一组不需要的顶点mj， c*为给定地图的扫描测量概率的最小值， 并进行比较分析， 函数d(z,L)表示了射线端点之间的距离以及与射线地图的交点， 求得与d(zk,Lmj)移除顶 点mj后的距离的差值， 从而得到误差变量ej的测量误差概率， 找到拥有最小概率的几何环境 地图。 权利要求书 3/3 页 4 CN 110109134 A 4 。

13、一种基于2D激光雷达测距的折线提取极大似然估计的方法 技术领域 0001 本发明涉及移动机器人地图创建领域， 具体是一种基于2D激光雷达测距的折线提 取极大似然估计的方法。 背景技术 0002 同时定位和构建地图(Simultaneous Localization And Mapping， SLAM)是一种 在没有环境先验信息的情况下， 在运动过程中建立环境的模型， 同时估计自己运动位姿的 技术。 随着激光雷达距离探测的出现和普及， 使得激光SLAM技术发展更为灵活广泛， 它具备 在地图创建这一领域更快更准， 信息更丰富的特点。 0003 激光SLAM应用场景， 如家庭、 办公室或工厂车间等人。

14、造环境， 通常由线性结构组 成， 通过折线构造的多边形地图可获得自然的结构化环境。 激光雷达采集到的物体信息呈 现出一系列分散的， 具有准确角度和距离信息的点， 被称为点云数据。 传统的激光雷达首先 从传感器采集的数据中提取特征点信息， 对不同的特征模式进行区域分割后确定分割阈 值， 在状态变量中进行估计和特征优化， 从而获得较精确的地图形式， 越精确的地图会使得 机器人的路径规划和导航变得更加直观。 如聚类算法中的分割合并算法， 通过人为选择合 适的阈值对点云进行区域的划分， 在单独的分割域中进行线性拟合， 但是此方法没有考虑 点云数据随雷达距离远而稀疏的问题， 对阈值的选用较为复杂。 基于。

15、ICP的点云匹配算法在 应用中也十分广泛， 但是没有加入速度影响， 其不能满足动态环境下的机器人定位和建图， 同时计算复杂内存消耗较大。 因此本文采用反向的计算模式， 通过极大似然估计， 在已知多 边形边缘顶点的样本集下， 反推最有可能导致拟合多边形概率最大的参数值， 从而实现扫 描区域最大的可能， 同时补充面上近似线状环境中存在的漏洞。 0004 因此， 提供一种新的概率方法， 从2-D激光范围扫描中提取折线， 绘制出更加精确 的地图。 发明内容 0005 本发明的目的在于提供一种基于2D激光雷达测距的折线提取极大似然估计的方 法， 该方法的折线提取简单、 效率和准确性高， 为后续机器人在环。

16、境地图中进行路径规划和 导航提供了可能。 0006 实现本发明目的的技术方案是： 0007 一种基于2D激光雷达测距的折线提取极大似然估计的方法， 包括如下步骤： 0008 1)在ROS平台中建立工作区间， 在工作区间内安装robortsence3D激光雷达驱动并 与激光雷达的wifi连接， 在避免强光照射下， 使用3D激光雷达对周围环境进行扫描， 进行3D 激光雷达数据采集， 得到原始的3D点云数据包， 并储存在rosbag中， 并建立三维点云地图； 0009 2)对步骤1)得到的原始3D点云数据进行数据转换， 转换成二维激光数据， 从而便 于环境地图的特征提取和优化； 0010 3)根据获。

17、得的二维激光数据， 建立基于机器人的运动模型， 通过平面上机器人的 说明书 1/7 页 5 CN 110109134 A 5 坐标和航向角推算从而获得点云数据的相对位姿信息， 在任意t时刻和激光雷达的观测模 型相结合， 完成机器人定位， 然后通过最小二乘法的优化算法建立静态SLAM下的环境初始 地图得到二维点云地图； 0011 4)在环境初始地图的基础上， 对二维激光扫描点进行最大似然估计， 同时通过折 线提取和折线优化两部分， 寻找使扫描测量可能性最大化的一组多段线,得到极大似然估 计下的概率最大的环境地图。 0012 步骤1)中， 所述的3D激光雷达数据采集， 是在Ros框架中调用robo。

18、rtsence3D激光 雷达的点云数据包， 为环境初始地图的建立做准备。 0013 步骤2)中， 所述的数据转换， 是将3D点云转换成2维的激光线束， 具体包括如下步 骤： 0014 2-1)首先在roscore节点下调用roslaunch启动3D激光雷达驱动； 0015 2-2)通过pointcloud-to-laserscan节点获得多条3D扫描线， 并将扫描数据封装 成数据集， 便于后续使用； 0016 2-3)之后需要将3D线束转换成2D线束， 将z轴高度信息和角度yaw进行坐标转换， 同时输出角度增量信息和x、 y轴坐标， 确定2D激光雷达的有效测量范围， 将获取的话题数据 在riv。

19、iz中显示， 在ROS中将三维空间中的点投影到可以展开的圆柱形表面上， 以将其平面 化， 利用此方法可以将点3D点云数据转换2D激光数据， 创建转换包中的launch文件， 并设置 节点和订阅信息， 启动雷达驱动包和转换包节点， 在RVIZ中可以看到三维点云数据， 测试是 否能够成功显示二维激光数据， 还需要在RVIZ中添加laserscan.然后将topic改为/scan即 可。 0017 步骤3)中， 所述的环境初始地图， 是激光SLAM在构建机器人位姿时首先通过传感 器获取轮式机器人的速度位置信息， 通过运动解算构建机器人的运动学模型， 获得车体的 位姿矩阵， 使用欧式变换对车体自身坐标。

20、进行航迹推算， 最终获得机器人在全局坐标系中 的位姿， 即激光在地图中初始位置已知； 建立激光SLAM的观测模型， 通过传感器获得的在t0 到tn的时间内传感器的观测值Zt， 估计状态量在t时刻的概率分布， 从而计算出环境特征值 t和机器人的初始位姿S0， 通过建立环境地图和确定自身位姿的不断重复， 构建出基于点云 信息的二维局部地图。 0018 步骤3)中， 所述的机器人定位， 由于单独的轮式里程计和激光雷达的航迹推算信 息缺少必要的回环检测， 因此为了避免原始环境地图产生运动畸变问题， 将步骤2-3)中成 功转换的2D点云数据的当前帧与其之前帧进行匹配， 通过SVD变换求解出帧间匹配的解，。

21、 从 而求解出两组点云数据间的最小均方差， 假设两帧数据无限接近， 即概率格子地图中接近 1， 然后构造最小二乘法， 计算使误差偏导为0， 求解高斯牛顿方程， 计算出激光在后一帧时 刻的位姿， 完成定位。 0019 步骤4)中， 所述的概率最大的环境地图， 具体是： 首先连接环境初始地图边缘的相 邻数据点集， 经选定合适的阈值匹配后， 根据其坐标数据确定一组初始化多边形， 之后迭代 删除在测量概率方面产生最小误差的顶点直到达到最大似然估计中给定的阈值， 从而得到 一组顶点位置扫描端点已知的位置折线， 根据已提取折线建立相应的条件概率模型， 通过 求解出非线性方程组的近似解， 得到使扫描概率最大。

22、化的几何多边形位置， 从而得到极大 似然估计下的概率最大的环境地图。 说明书 2/7 页 6 CN 110109134 A 6 0020 步骤4)中， 所述的折线提取和折线优化， 具体是通过连接所有扫描端点构造初始 化多边形结构， 然后在初始化多边形的基础上迭代地移除在测量概率方面引起最小误差的 顶点， 计算点云到多边形边的距离， 从而提出无用的点云数据， 若得到的距离均小于设定的 阈值dmax， 则得到初始多边形顶点的位置与扫描端点的子集的位置一致的一组折线， 其中阈 值dmax的设定通常取决于直线剪裁的程度， 若dmax选择较大， 则会删除线段中间的顶点， 若选 择的过小， 则可能会误删其。

23、它线段两端的顶点， 其中： 0021 P(z|L)N(r(z)， r(z， l)， ) (1) 0022 上述公式(1)中， 在一组多段线中， P(z|L)为单射线激光的半径分布模型， 其中定 义单线激光的测量为模型zx,y， 它两个双元素的列向量组成， 分别为射线的起始点x和 终点y， 确定多边形段线的集合Lzi， 其中z为多边形的顶点， 相对于多段线l的坐标系指 定， 在扫描面建立线的概率服从高斯分布， r为测量射线的半径， 为径向噪声； 0023 上述公式(1)中， 测量的射线半径为r|y-x|， 推广到整个扫描的测量概率为： 0024 0025 从而通过建立的条件概率模型， 力求得到的。

24、概率最大化； 0026 定义L*作为多边形的最大顶点数Jmax， 用于折线提取， 从而将问题转换成已知多边 形线段集合L， 求解最大顶点个数的问题， 如下公式(3)所示， 通过迭代方法找出合适的线段 L*的集合， 此时顶点可达到最多的Jmax， 从而确定几何多边形的范围， L*的表达式如下： 0027 0028 使用最大似然估计移除顶点的过程如下： 0029 0030 上述公式(4)中， p(z|Lmj)代表条件测量的概率密度函数， 目的是建图时在一组 多边形线L上移除一组不需要的顶点mj， c*为给定地图的扫描测量概率的最小值， 并进行比 较分析， 函数d(z,L)表示了射线端点之间的距离以。

25、及与射线地图的交点， 求得与d(zk,Lmj) 移除顶点mj后的距离的差值， 从而得到误差变量ej的测量误差概率， 找到拥有最小概率的几 何环境地图。 0031 有益效果： 本发明提供的一种基于2D激光雷达测距的折线提取极大似然估计的方 法， 首先在Ros平台中启动激光雷达驱动进行范围扫描， 通过获取的3D激光点云数据的位姿 信息进行坐标系转换将其转换成2维点云地图， 在此基础上对原始点云进行优化后， 进行基 于最大似然估计的折线提取和折线优化， 通过求解出基于多边形线段的非线性方程组， 最 终求解出使得扫描概率范围最大的多边形折线地图， 降低了激光雷达建图的复杂度， 同时 说明书 3/7 页。

26、 7 CN 110109134 A 7 提高了建图精度。 附图说明 0032 图1为本发明一种基于2D激光雷达测距的折线提取极大似然估计的方法的工作流 程图； 0033 图2通过robortsence3D激光雷达采集的数据信息在Ros平台上建立的3D点云地 图； 0034 图3为在3D点云地图的基础上将其点云数据转换成2D点云信息， 图中白色部分为 提取出的二维折线； 0035 图4为使用pointcloud-to-laserscan节点将点云信息转换成2D环境地图； 0036 图5为在2D环境地图的基础上进行折线提取， 提取出边缘的折线信息， 从而构建出 地图的环境结构。 具体实施方式 00。

27、37 下面结合附图和实施例对本发明做进一步阐述， 但不是对本发明的限定。 0038 一种基于2D激光雷达测距的折线提取极大似然估计的方法， 包括如下步骤： 0039 1)在ROS平台中建立工作区间， 在工作区间内安装robortsence3D激光雷达驱动并 与激光雷达的wifi连接， 在避免强光照射下， 使用3D激光雷达对周围环境进行扫描， 进行3D 激光雷达数据采集， 得到原始的3D点云数据包， 并储存在rosbag中， 并建立三维点云地图， 如图2所示； 0040 2)对步骤1)得到的原始3D点云数据进行数据转换， 转换成二维激光数据如图3中 加粗部分， 从而便于环境地图的特征提取和优化；。

28、 0041 3)根据获得的二维激光数据， 建立基于机器人的运动模型， 通过平面上机器人的 坐标和航向角推算从而获得点云数据的相对位姿信息， 在任意t时刻和激光雷达的观测模 型相结合， 完成机器人定位， 然后通过最小二乘法的优化算法建立静态SLAM下的环境初始 地图得到如图4所示的二维点云地图； 0042 4)在环境初始地图的基础上， 对二维激光扫描点进行最大似然估计， 同时通过折 线提取和折线优化两部分， 寻找使扫描测量可能性最大化的一组多段线,得到极大似然估 计下的概率最大的环境地图。 0043 步骤1)中， 所述的3D激光雷达数据采集， 是在Ros框架中调用robortsence3D激光 。

29、雷达的点云数据包， 为环境初始地图的建立做准备。 0044 步骤2)中， 所述的数据转换， 是将3D点云转换成2维的激光线束， 具体包括如下步 骤： 0045 2-1)首先在roscore节点下调用roslaunch启动3D激光雷达驱动； 0046 2-2)通过pointcloud-to-laserscan节点获得多条3D扫描线， 并将扫描数据封装 成数据集， 以便接下来使用。 0047 2-3)之后需要将3D线束转换成2D线束。 将z轴高度信息和角度yaw进行坐标转换， 同时输出角度增量信息和x、 y轴坐标， 确定2D激光雷达的有效测量范围， 将获取的话题数据 在riviz中显示。 在ROS。

30、中将三维空间中的点投影到可以展开的圆柱形表面上， 以将其平面 说明书 4/7 页 8 CN 110109134 A 8 化， 利用此方法可以将点3D点云数据转换2D激光数据。 创建转换包中的launch文件， 并设置 节点和订阅信息， 启动雷达驱动包和转换包节点， 在RVIZ中可以看到三维点云数据， 测试是 否能够成功显示二维激光数据， 还需要在RVIZ中添加laserscan.然后将topic改为/scan即 可。 0048 步骤3)中， 所述的环境初始地图， 是激光SLAM在构建机器人位姿时首先通过传感 器获取轮式机器人的速度位置信息， 通过运动解算构建机器人的运动学模型， 获得车体的 位。

31、姿矩阵， 使用欧式变换对车体自身坐标进行航迹推算， 最终获得机器人在全局坐标系中 的位姿， 即激光在地图中初始位置已知； 建立激光SLAM的观测模型， 通过传感器获得的在t0 到tn的时间内传感器的观测值Zt， 估计状态量在t时刻的概率分布， 从而计算出环境特征值 t和机器人的初始位姿S0， 通过建立环境地图和确定自身位姿的不断重复， 构建出基于点云 信息的二维局部地图。 0049 步骤3)中， 所述的机器人定位， 由于单独的轮式里程计和激光雷达的航迹推算信 息缺少必要的回环检测， 因此为了避免原始环境地图产生运动畸变问题， 将步骤2-3)中成 功转换的2D点云数据的当前帧与其之前帧进行匹配，。

32、 通过SVD变换求解出帧间匹配的解， 从 而求解出两组点云数据间的最小均方差， 假设两帧数据无限接近， 即概率格子地图中接近 1， 然后构造最小二乘法， 计算使误差偏导为0， 求解高斯牛顿方程， 计算出激光在后一帧时 刻的位姿， 完成定位， 如图4所示。 0050 步骤4)中， 所述的概率最大的环境地图， 具体是： 首先连接环境初始地图边缘的相 邻数据点集， 经选定合适的阈值匹配后， 根据其坐标数据确定一组初始化多边形， 之后迭代 删除在测量概率方面产生最小误差的顶点直到达到最大似然估计中给定的阈值， 从而得到 一组顶点位置扫描端点已知的位置折线， 根据已提取折线建立相应的条件概率模型， 通过。

33、 求解出非线性方程组的近似解， 得到使扫描概率最大化的几何多边形位置， 从而得到极大 似然估计下的概率最大的环境地图。 0051 步骤4)中， 所述的折线提取和折线优化， 具体是通过连接所有扫描端点构造初始 化多边形结构， 然后在初始化多边形的基础上迭代地移除在测量概率方面引起最小误差的 顶点， 计算点云到多边形边的距离， 从而提出无用的点云数据， 若得到的距离均小于设定的 阈值dmax， 则得到初始多边形顶点的位置与扫描端点的子集的位置一致的一组折线， 其中阈 值dmax的设定通常取决于直线剪裁的程度， 若dmax选择较大， 则会删除线段中间的顶点， 若选 择的过小， 则可能会误删其它线段两。

34、端的顶点， 其中： 0052 P(z|L)N(r(z)， r(z， l)， ) (1) 0053 上述公式(1)中， 在一组多段线中， P(z|L)为单射线激光的半径分布模型， 其中定 义单线激光的测量为模型zx,y， 它两个双元素的列向量组成， 分别为射线的起始点x和 终点y， 确定多边形段线的集合Lzi， 其中z为多边形的顶点， 相对于多段线l的坐标系指 定， 在扫描面建立线的概率服从高斯分布， r为测量射线的半径， 为径向噪声； 0054 上述公式(1)中， 测量的射线半径为r|y-x|， 推广到整个扫描的测量概率为： 0055 0056 从而通过建立的条件概率模型， 力求得到的概率最大。

35、化； 0057 定义L*作为多边形的最大顶点数Jmax， 用于折线提取， 从而将问题转换成已知多边 说明书 5/7 页 9 CN 110109134 A 9 形线段集合L， 求解最大顶点个数的问题， 如下公式(3)所示， 通过迭代方法找出合适的线段 L*的集合， 此时顶点可达到最多的Jmax， 从而确定几何多边形的范围， L*的表达式如下： 0058 0059 使用最大似然估计移除顶点的过程如下： 0060 0061 上述公式(4)中， p(z|Lmj)代表条件测量的概率密度函数， 目的是建图时在一组 多边形线L上移除一组不需要的顶点mj， c*为求得地图扫描测量概率的最小值， 并进行比较 分。

36、析， 函数d(z,L)表示了射线端点之间的距离以及与射线地图的交点， 求得与d(zk,Lmj)移 除顶点mj后的距离的差值， 从而得到误差变量ej的测量误差概率， 找到拥有最小概率的几何 环境地图， 如图5所示。 0062 实施例： 0063 本实施例针对激光雷达在建图过程中计算量较大， 同时点云配时会产生运动畸 变， 对建图精度造成影响的问题， 提出一种基于2D激光雷达测距的折线提取极大似然估计 的方法， ， 如图1所示， 本实施例首先选取Ros框架作为本次算法实验的平台， 并安装基于 RoboSence3D激光雷达的接口驱动， 其中需配置速腾雷达专用的IP地址： 192.168.1.10，。

37、 调 用rslidar_driver测试是否安装成功。 之后调用roslaunch rslidar_pointcloud rslidar_16point.launch驱动雷达对实验环境进行扫描， 如图2所示， 为了便于特征线的提 取， 本实施例将3D点云线束进行转换为2D的激光点云线束， 启动一个新的终端并调用 roslaunch pointcloud_to_laserscan rslidar_16point.launch指令， 获取激光雷达的点 云数据话题， 同时启动riviz可视化建图指令， 构建二维点云地图， 为下一步的折线提取和 折线优化做准备， 最终实现激光基于折线特征提取的较为精确。

38、的平面地图。 0064 如图3所示， 得到的线段为在三维点云数据中转化后的二维点云地图。 将点云数据 转换成功后， 可以开启riviz可视化界面进行建图， 得到如图4所示的基于二维激光的环境 地图。 0065 图5为在二维环境地图的基础上调用基于极大似然估计的特征提取算法， 对环境 的边缘线段进行折线提取和折线优化后的结构图。 特征提取算法是激光雷达建图中一个常 使用的方法， 首先是通过聚类算法对点云数据进行分割和合并， 将扫面的数据分割成几个 不同特征的多段线， 之后对各个区域内的线段进行拟合， 从而得到一个完整的地图面结构， 但是此方法需要人为的规定点云与点云之间， 线段与线段之间的距离的。

39、阈值， 否则容易导 致区域的过度分割现象， 或丢弃了路径上原本有价值的信息。 本文采用概率估计的形式， 首 先连接点云边缘处的端点形成初始的几何多边形， 之后采用最大似然估计的方法， 删除误 说明书 6/7 页 10 CN 110109134 A 10 差项中无用的线段， 得到误差项最小的几何多边形。 0066 如图5所示， 利用对16线雷达三维点云数据转换成二维的激光数据， 并在扫描的过 程中很好的提取了地图中的折线， 这无论是对物体准确的描述还是高精度地图的制作， 此 方法比以往线性提取的方法都简单、 高效且精度很高， 这为后续的路径规划和导航提供了 可能。 0067 从二维激光扫描中提取。

40、折线， 解决了两个问题： 0068 (1)哪条折线反映了哪一条扫描端点？ 0069 (2)什么是最佳位置折线顶点？ 0070 在解决第一个问题时， 使用了一种贪婪算法， 首先将顶点mj与线段几何L中的端点 的误差值， 并将误差较大的顶点移除， 知道达到所需的定点数Jmax， 目的是使每个一多段线 对应的顶点都是最好的或是最优的选择， 可以最大限度地减少由线段表示各个扫描端点所 导致的测量概率降低的问题， 0071 解决的第二个问题， 是利用了最大似然估计发该优化器以最大化测量概率移动顶 点。 0072 线提取始终要考虑内存和准确性之间的矛盾， 一般情况下， 嵌入式应用程序可能 专注于最小的内存。

41、占用， 而离线映射系统可能会以牺牲由数百或数千个顶点组成的折线为 代价来提高精度。 因此， 每个线提取算法都需要某种参数。 0073 本发明的方法可以轻松使用任意参数， 例如光线半径的最大均方根误差， 赤池信 息量准则， 提取的折线与原始扫描端点的多边形之间的最大区域差异等。 将折线提取方法 的示例性结果应用于在我们实验室中捕获的扫描， 如图4所示。 并和各种折线提取方法进行 对比， 具有更高的精确度。 说明书 7/7 页 11 CN 110109134 A 11 图1 说明书附图 1/5 页 12 CN 110109134 A 12 图2 说明书附图 2/5 页 13 CN 110109134 A 13 图3 说明书附图 3/5 页 14 CN 110109134 A 14 图4 说明书附图 4/5 页 15 CN 110109134 A 15 图5 说明书附图 5/5 页 16 CN 110109134 A 16 。