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

权利要求书

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激光雷达的点云数据包，为环境初始地图的建立做准备。