CN110109134B - 一种基于2d激光雷达测距的折线提取极大似然估计的方法 - Google Patents

Abstract

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

Description

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

技术领域

本发明涉及移动机器人地图创建领域，具体是一种基于2D激光雷达测距的折线提取极大似然估计的方法。

背景技术

同时定位和构建地图(Simultaneous Localization And Mapping，SLAM)是一种在没有环境先验信息的情况下，在运动过程中建立环境的模型，同时估计自己运动位姿的技术。随着激光雷达距离探测的出现和普及，使得激光SLAM技术发展更为灵活广泛，它具备在地图创建这一领域更快更准，信息更丰富的特点。

激光SLAM应用场景，如家庭、办公室或工厂车间等人造环境，通常由线性结构组成，通过折线构造的多边形地图可获得自然的结构化环境。激光雷达采集到的物体信息呈现出一系列分散的，具有准确角度和距离信息的点，被称为点云数据。传统的激光雷达首先从传感器采集的数据中提取特征点信息，对不同的特征模式进行区域分割后确定分割阈值，在状态变量中进行估计和特征优化，从而获得较精确的地图形式，越精确的地图会使得机器人的路径规划和导航变得更加直观。如聚类算法中的分割合并算法，通过人为选择合适的阈值对点云进行区域的划分，在单独的分割域中进行线性拟合，但是此方法没有考虑点云数据随雷达距离远而稀疏的问题，对阈值的选用较为复杂。基于ICP的点云匹配算法在应用中也十分广泛，但是没有加入速度影响，其不能满足动态环境下的机器人定位和建图，同时计算复杂内存消耗较大。因此本文采用反向的计算模式，通过极大似然估计，在已知多边形边缘顶点的样本集下，反推最有可能导致拟合多边形概率最大的参数值，从而实现扫描区域最大的可能，同时补充面上近似线状环境中存在的漏洞。

因此，提供一种新的概率方法，从2-D激光范围扫描中提取折线，绘制出更加精确的地图。

发明内容

本发明的目的在于提供一种基于2D激光雷达测距的折线提取极大似然估计的方法，该方法的折线提取简单、效率和准确性高，为后续机器人在环境地图中进行路径规划和导航提供了可能。

实现本发明目的的技术方案是：

一种基于2D激光雷达测距的折线提取极大似然估计的方法，包括如下步骤：

1)在ROS平台中建立工作区间，在工作区间内安装robortsence3D激光雷达驱动并与激光雷达的wifi连接，在避免强光照射下，使用3D激光雷达对周围环境进行扫描，进行3D激光雷达数据采集，得到原始的3D点云数据包，并储存在rosbag中，并建立三维点云地图；

2)对步骤1)得到的原始3D点云数据进行数据转换，转换成二维激光数据，从而便于环境地图的特征提取和优化；

3)根据获得的二维激光数据，建立基于机器人的运动模型，通过平面上机器人的坐标和航向角推算从而获得点云数据的相对位姿信息，在任意t时刻和激光雷达的观测模型相结合，完成机器人定位，然后通过最小二乘法的优化算法建立静态SLAM下的环境初始地图得到二维点云地图；

4)在环境初始地图的基础上，对二维激光扫描点进行最大似然估计，同时通过折线提取和折线优化两部分，寻找使扫描测量可能性最大化的一组多段线,得到极大似然估计下的概率最大的环境地图。

步骤1)中，所述的3D激光雷达数据采集，是在Ros框架中调用robortsence3D激光雷达的点云数据包，为环境初始地图的建立做准备。

步骤2)中，所述的数据转换，是将3D点云转换成2维的激光线束，具体包括如下步骤：

2-1)首先在roscore节点下调用roslaunch启动3D激光雷达驱动；

2-2)通过pointcloud-to-laserscan节点获得多条3D扫描线，并将扫描数据封装成数据集，便于后续使用；

2-3)之后需要将3D线束转换成2D线束，将z轴高度信息和角度yaw进行坐标转换，同时输出角度增量信息和x、y轴坐标，确定2D激光雷达的有效测量范围，将获取的话题数据在riviz中显示，在ROS中将三维空间中的点投影到可以展开的圆柱形表面上，以将其平面化，利用此方法可以将点3D点云数据转换2D激光数据，创建转换包中的launch文件，并设置节点和订阅信息，启动雷达驱动包和转换包节点，在RVIZ中可以看到三维点云数据，测试是否能够成功显示二维激光数据，还需要在RVIZ中添加laserscan.然后将topic改为/scan即可。

步骤3)中，所述的环境初始地图，是激光SLAM在构建机器人位姿时首先通过传感器获取轮式机器人的速度位置信息，通过运动解算构建机器人的运动学模型，获得车体的位姿矩阵，使用欧式变换对车体自身坐标进行航迹推算，最终获得机器人在全局坐标系中的位姿，即激光在地图中初始位置已知；建立激光SLAM的观测模型，通过传感器获得的在t₀到t_n的时间内传感器的观测值Z_t，估计状态量在t时刻的概率分布，从而计算出环境特征值θ_t和机器人的初始位姿S₀，通过建立环境地图和确定自身位姿的不断重复，构建出基于点云信息的二维局部地图。

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

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

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

P(z|L)＝N(r(z)，r(z，l)，∑) (1)

上述公式(1)中，在一组多段线中，P(z|L)为单射线激光的半径分布模型，其中定义单线激光的测量为模型z＝{x,y}，它两个双元素的列向量组成，分别为射线的起始点x和终点y，确定多边形段线的集合L＝{z_i}，其中z为多边形的顶点，相对于多段线l的坐标系指定，在扫描面建立线的概率服从高斯分布，r为测量射线的半径，∑为径向噪声；

上述公式(1)中，测量的射线半径为r＝||y-x||，推广到整个扫描的测量概率为：

从而通过建立的条件概率模型，力求得到的概率最大化；

定义L*作为多边形的最大顶点数J_max，用于折线提取，从而将问题转换成已知多边形线段集合L，求解最大顶点个数的问题，如下公式(3)所示，通过迭代方法找出合适的线段L*的集合，此时顶点可达到最多的J_max，从而确定几何多边形的范围，L*的表达式如下：

使用最大似然估计移除顶点的过程如下：

上述公式(4)中，p(z|L\m_j)代表条件测量的概率密度函数，目的是建图时在一组多边形线L上移除一组不需要的顶点m_j，c*为给定地图的扫描测量概率的最小值，并进行比较分析，函数d(z,L)表示了射线端点之间的距离以及与射线地图的交点，求得与d(z_k,L\m_j)移除顶点m_j后的距离的差值，从而得到误差变量e_j的测量误差概率，找到拥有最小概率的几何环境地图。

有益效果：本发明提供的一种基于2D激光雷达测距的折线提取极大似然估计的方法，首先在Ros平台中启动激光雷达驱动进行范围扫描，通过获取的3D激光点云数据的位姿信息进行坐标系转换将其转换成2维点云地图，在此基础上对原始点云进行优化后，进行基于最大似然估计的折线提取和折线优化，通过求解出基于多边形线段的非线性方程组，最终求解出使得扫描概率范围最大的多边形折线地图，降低了激光雷达建图的复杂度，同时提高了建图精度。

附图说明

图1为本发明一种基于2D激光雷达测距的折线提取极大似然估计的方法的工作流程图；

图2通过robortsence3D激光雷达采集的数据信息在Ros平台上建立的3D点云地图；

图3为在3D点云地图的基础上将其点云数据转换成2D点云信息，图中白色部分为提取出的二维折线；

图4为使用pointcloud-to-laserscan节点将点云信息转换成2D环境地图；

图5为在2D环境地图的基础上进行折线提取，提取出边缘的折线信息，从而构建出地图的环境结构。

具体实施方式

下面结合附图和实施例对本发明做进一步阐述，但不是对本发明的限定。

一种基于2D激光雷达测距的折线提取极大似然估计的方法，包括如下步骤：

1)在ROS平台中建立工作区间，在工作区间内安装robortsence3D激光雷达驱动并与激光雷达的wifi连接，在避免强光照射下，使用3D激光雷达对周围环境进行扫描，进行3D激光雷达数据采集，得到原始的3D点云数据包，并储存在rosbag中，并建立三维点云地图，如图2所示；

2)对步骤1)得到的原始3D点云数据进行数据转换，转换成二维激光数据如图3中加粗部分，从而便于环境地图的特征提取和优化；

3)根据获得的二维激光数据，建立基于机器人的运动模型，通过平面上机器人的坐标和航向角推算从而获得点云数据的相对位姿信息，在任意t时刻和激光雷达的观测模型相结合，完成机器人定位，然后通过最小二乘法的优化算法建立静态SLAM下的环境初始地图得到如图4所示的二维点云地图；

4)在环境初始地图的基础上，对二维激光扫描点进行最大似然估计，同时通过折线提取和折线优化两部分，寻找使扫描测量可能性最大化的一组多段线,得到极大似然估计下的概率最大的环境地图。

步骤1)中，所述的3D激光雷达数据采集，是在Ros框架中调用robortsence3D激光雷达的点云数据包，为环境初始地图的建立做准备。

步骤2)中，所述的数据转换，是将3D点云转换成2维的激光线束，具体包括如下步骤：

2-1)首先在roscore节点下调用roslaunch启动3D激光雷达驱动；

2-2)通过pointcloud-to-laserscan节点获得多条3D扫描线，并将扫描数据封装成数据集，以便接下来使用。

2-3)之后需要将3D线束转换成2D线束。将z轴高度信息和角度yaw进行坐标转换，同时输出角度增量信息和x、y轴坐标，确定2D激光雷达的有效测量范围，将获取的话题数据在riviz中显示。在ROS中将三维空间中的点投影到可以展开的圆柱形表面上，以将其平面化，利用此方法可以将点3D点云数据转换2D激光数据。创建转换包中的launch文件，并设置节点和订阅信息，启动雷达驱动包和转换包节点，在RVIZ中可以看到三维点云数据，测试是否能够成功显示二维激光数据，还需要在RVIZ中添加laserscan.然后将topic改为/scan即可。

步骤3)中，所述的环境初始地图，是激光SLAM在构建机器人位姿时首先通过传感器获取轮式机器人的速度位置信息，通过运动解算构建机器人的运动学模型，获得车体的位姿矩阵，使用欧式变换对车体自身坐标进行航迹推算，最终获得机器人在全局坐标系中的位姿，即激光在地图中初始位置已知；建立激光SLAM的观测模型，通过传感器获得的在t₀到t_n的时间内传感器的观测值Z_t，估计状态量在t时刻的概率分布，从而计算出环境特征值θ_t和机器人的初始位姿S₀，通过建立环境地图和确定自身位姿的不断重复，构建出基于点云信息的二维局部地图。

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

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

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

P(z|L)＝N(r(z)，r(z，l)，∑) (1)

上述公式(1)中，在一组多段线中，P(z|L)为单射线激光的半径分布模型，其中定义单线激光的测量为模型z＝{x,y}，它两个双元素的列向量组成，分别为射线的起始点x和终点y，确定多边形段线的集合L＝{z_i}，其中z为多边形的顶点，相对于多段线l的坐标系指定，在扫描面建立线的概率服从高斯分布，r为测量射线的半径，∑为径向噪声；

上述公式(1)中，测量的射线半径为r＝||y-x||，推广到整个扫描的测量概率为：

从而通过建立的条件概率模型，力求得到的概率最大化；

定义L^*作为多边形的最大顶点数J_max，用于折线提取，从而将问题转换成已知多边形线段集合L，求解最大顶点个数的问题，如下公式(3)所示，通过迭代方法找出合适的线段L*的集合，此时顶点可达到最多的J_max，从而确定几何多边形的范围，L^*的表达式如下：

使用最大似然估计移除顶点的过程如下：

上述公式(4)中，p(z|L\m_j)代表条件测量的概率密度函数，目的是建图时在一组多边形线L上移除一组不需要的顶点m_j，c^*为求得地图扫描测量概率的最小值，并进行比较分析，函数d(z,L)表示了射线端点之间的距离以及与射线地图的交点，求得与d(z_k,L\m_j)移除顶点m_j后的距离的差值，从而得到误差变量e_j的测量误差概率，找到拥有最小概率的几何环境地图，如图5所示。

实施例：

本实施例针对激光雷达在建图过程中计算量较大，同时点云配时会产生运动畸变，对建图精度造成影响的问题，提出一种基于2D激光雷达测距的折线提取极大似然估计的方法，，如图1所示，本实施例首先选取Ros框架作为本次算法实验的平台，并安装基于RoboSence3D激光雷达的接口驱动，其中需配置速腾雷达专用的IP地址：192.168.1.10，调用rslidar_driver测试是否安装成功。之后调用roslaunch rslidar_pointcloudrslidar_16point.launch驱动雷达对实验环境进行扫描，如图2所示，为了便于特征线的提取，本实施例将3D点云线束进行转换为2D的激光点云线束，启动一个新的终端并调用roslaunch pointcloud_to_laserscan rslidar_16point.launch指令，获取激光雷达的点云数据话题，同时启动riviz可视化建图指令，构建二维点云地图，为下一步的折线提取和折线优化做准备，最终实现激光基于折线特征提取的较为精确的平面地图。

如图3所示，得到的线段为在三维点云数据中转化后的二维点云地图。将点云数据转换成功后，可以开启riviz可视化界面进行建图，得到如图4所示的基于二维激光的环境地图。

图5为在二维环境地图的基础上调用基于极大似然估计的特征提取算法，对环境的边缘线段进行折线提取和折线优化后的结构图。特征提取算法是激光雷达建图中一个常使用的方法，首先是通过聚类算法对点云数据进行分割和合并，将扫面的数据分割成几个不同特征的多段线，之后对各个区域内的线段进行拟合，从而得到一个完整的地图面结构，但是此方法需要人为的规定点云与点云之间，线段与线段之间的距离的阈值，否则容易导致区域的过度分割现象，或丢弃了路径上原本有价值的信息。本文采用概率估计的形式，首先连接点云边缘处的端点形成初始的几何多边形，之后采用最大似然估计的方法，删除误差项中无用的线段，得到误差项最小的几何多边形。

如图5所示，利用对16线雷达三维点云数据转换成二维的激光数据，并在扫描的过程中很好的提取了地图中的折线，这无论是对物体准确的描述还是高精度地图的制作，此方法比以往线性提取的方法都简单、高效且精度很高，这为后续的路径规划和导航提供了可能。

从二维激光扫描中提取折线，解决了两个问题：

(1)哪条折线反映了哪一条扫描端点？

(2)什么是最佳位置折线顶点？

在解决第一个问题时，使用了一种贪婪算法，首先将顶点mj与线段几何L中的端点的误差值，并将误差较大的顶点移除，知道达到所需的定点数Jmax，目的是使每个一多段线对应的顶点都是最好的或是最优的选择，可以最大限度地减少由线段表示各个扫描端点所导致的测量概率降低的问题，

解决的第二个问题，是利用了最大似然估计发该优化器以最大化测量概率移动顶点。

线提取始终要考虑内存和准确性之间的矛盾，一般情况下，嵌入式应用程序可能专注于最小的内存占用，而离线映射系统可能会以牺牲由数百或数千个顶点组成的折线为代价来提高精度。因此，每个线提取算法都需要某种参数。

本发明的方法可以轻松使用任意参数，例如光线半径的最大均方根误差，赤池信息量准则，提取的折线与原始扫描端点的多边形之间的最大区域差异等。将折线提取方法的示例性结果应用于在我们实验室中捕获的扫描，如图4所示。并和各种折线提取方法进行对比，具有更高的精确度。

Claims (5)

1.一种基于2D激光雷达测距的折线提取极大似然估计的方法，其特征在于，包括如下步骤：

1)在ROS平台中建立工作区间，在工作区间内安装robortsence3D激光雷达驱动并与激光雷达的wifi连接，在避免强光照射下，使用3D激光雷达对周围环境进行扫描，进行3D激光雷达数据采集，得到原始的3D点云数据包，并储存在rosbag中，并建立三维点云地图；

2)对步骤1)得到的原始3D点云数据进行数据转换，转换成二维激光数据，从而便于环境地图的特征提取和优化；

3)根据获得的二维激光数据，建立基于机器人的运动模型，通过平面上机器人的坐标和航向角推算从而获得点云数据的相对位姿信息，在任意t时刻和激光雷达的观测模型相结合，完成机器人定位，然后通过最小二乘法的优化算法建立静态SLAM下的环境初始地图得到二维点云地图；

4)在环境初始地图的基础上，对二维激光扫描点进行最大似然估计，同时通过折线提取和折线优化两部分，寻找扫描测量可能性最大化的一组多段线，得到极大似然估计下的概率最大的环境地图；

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

步骤4)中，所述的折线提取和折线优化，具体是通过连接所有扫描端点构造初始化多边形结构，然后在初始化多边形的基础上迭代地移除在测量概率方面引起最小误差的顶点，计算点云到多边形边的距离，从而提出无用的点云数据，若得到的距离均小于设定的阈值d_max，则得到初始多边形顶点的位置与扫描端点的子集的位置一致的一组折线，其中：

P(z|L)＝N(r(z)，r(z，l)，∑) (1)

公式(1)中，在一组多段线中，P(z|L)为单射线激光的半径分布模型，其中定义单线激光的测量为模型z＝{x,y}，它两个双元素的列向量组成，分别为射线的起始点x和终点y，确定多边形段线的集合L＝{z_i}，其中z为多边形的顶点，相对于多段线l的坐标系指定，在扫描面建立线的概率服从高斯分布，r为测量射线的半径，∑为径向噪声；

公式(1)中，测量的射线半径为r＝||y-x||，推广到整个扫描的测量概率为：

通过建立的条件概率模型，求得到的概率最大化；

设L^*为多边形的最大顶点数J_max，用于折线提取，从而将问题转换成已知多边形线段集合L，求解最大顶点个数的问题，如公式(3)所示，通过迭代方法找出合适的线段L^*的集合，此时顶点可达到最多的J_max，从而确定几何多边形的范围，L^*的表达式如下：

使用最大似然估计移除顶点的过程如下：

公式(4)中，p(z|L\m_j)代表条件测量的概率密度函数，目的是建图时在一组多边形线L上移除一组不需要的顶点m_j，c^*为给定地图的扫描测量概率的最小值，函数d(z,L)表示了射线端点之间的距离，通过对射线端点之间的距离与射线地图的交点进行比较分析，求得射线端点与d(z_k,L\m_j)移除顶点m_j之后的距离差值，从而得到误差变量e_j的测量误差概率，找到拥有最小概率的几何环境地图。

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

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

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

Images