[发明专利]一种基于点云分割匹配闭环校正的同步定位与构图方法

权利要求书

1.一种基于点云分割匹配闭环校正的同步定位与构图方法，其特征在于，包括以下步骤：

步骤1.利用多线程三维激光雷达获取环境的三维激光点云数据：点云数据通过机器人操作系统ROS以消息的形式传输给计算机；使用开源的点云处理函数库PCL将其表示为无序的散列坐标点；再根据雷达的扫描方式和时间戳对其进行排序；

步骤2.提取特征点：根据点周围的局部曲率将当前时刻扫描到的点云区分为角点和平面点；任一点的曲率由该点周围的点与该点形成的和向量的模表示；同时根据雷达视角与局部点云的相对关系剔除断点和孤立点；

步骤3.基于特征点的帧间匹配：利用kd-tree最近邻查找算法在上一时刻t_k-1的点云P_k-1中寻找距离当前时刻t_k的特征点p_i最近的特征点，形成特征线与特征面；利用L-M算法求解关于p_i到特征线和特征面的距离的最小二乘问题，从而得到相邻时刻的机器人位姿变换矩阵T_k，进而递推式地求得机器人的位姿；

步骤4.特征点与地图的匹配：利用与步骤3相同的方法，将当前时刻t_k提取的特征点p_i与已建立好的地图中的特征点云进行匹配，从而对机器人当前时刻的位姿进行校正；根据校正后的位姿将当前时刻的点云从雷达坐标系转换到世界坐标系即为建立的地图；

步骤5.点云分割：同时，将t_k时刻获取到的点云数据通过体素分割和聚类的方法，去除地面点云并分割出地上物体的点云片段；

步骤6.利用特征描述子对点云片段进行描述：利用基于点云片段的三维结构张量矩阵的特征值和特征向量，以及点云片段的形状直方图的描述子来描述点云片段的线性、平面性、分散度等特征，并将其作为点云片段的唯一标识进行存储；

步骤7.基于特征描述子的点云片段匹配：通过kd-tree在特征空间中寻找当前点云片段的候选匹配，利用训练好的随机森林分类器为候选匹配打分，再根据设立的阈值和随机抽样一致性检验来确定正确的点云片段配对；根据点云片段的质心计算点云片段间的相对变换关系，以此作为优化的约束条件传入后端图优化步骤；

步骤8.利用图优化对全局位姿和地图进行修正：根据步骤3、步骤4以及步骤7计算得到的不同时刻的机器人位姿和位姿之间的变换关系构建位姿因子图，其中图中节点表示位姿，边表示位姿之间的约束；通过L-M优化算法优化节点状态，使之满足所有的约束，进而实现对全局的位姿和基于位姿的地图进行修正。

2.根据权利要求1所述的基于点云分割匹配闭环校正的同步定位与构图方法，其特征在于步骤3包括：

步骤3-1.将t_k-1时刻的点云P_k-1根据求得的位姿变换矩阵T_k-1重投影到t_k时刻，按照kd-tree的数据结构存储；

步骤3-2.将t_k时刻获得的点云P_k根据步骤2的方法提取出角点E_k和平面点H_k，其中P_k＝{p_i,i＝1...N}，E_k＝{ε_i,i＝1...N₁}，H_k＝{δ_i,i＝1...N₂}，N是激光单次扫描得到点云总数；设迭代次数为1，初始值

步骤3-3.根据最近邻搜索算法在建立好的kd-tree中搜索ε_i的最近的两个点形成特征线，搜索δ_i的最近的三个点形成特征面，根据点到线和点到面的距离公式计算角点到特征线的距离d_ε，平面点到特征面的距离d_δ；

步骤3-4.根据一系列的距离即可得到一组非线性方程，用矩阵表示为其中n为迭代次数；令距离向量d趋于0则可得到一个关于的非线性最小二乘问题，则方程关于变量求导即可得到雅可比矩阵根据L-M算法则迭代次数加一；

步骤3-5.判断当前误差e是否达到预设精度要求，或者迭代次数达到指定次数；如达到要求则进行下一步骤；如未达到要求，返回至步骤3-3。