[发明专利]一种基于激光雷达的室外三维同步定位与建图方法

权利要求书

1.一种基于激光雷达的室外三维同步定位与建图方法，其特征在于，包括以下步骤：

利用激光雷达获取周围环境的完整点云数据；

对点云数据进行预处理，去除离群点，并进行地面分割，提取地面点集合；

分别从地面点集合和非地面点集合中提取边缘特征点和平面特征点；

利用提取到的边缘特征点和平面特征点，通过点到边线和点到平面的扫描匹配，建立相邻两次扫描之间点云数据的关联，完成运动估计；

将提取到的边缘特征点和平面特征点匹配到周围的点云图中，进一步优化位姿变换；

利用回环检测算法减少累积漂移误差，实现位姿估计和地图构建；所述利用回环检测算法减少累积漂移误差，实现位姿估计和地图构建具体为：

首先对当前扫描的轨迹点进行邻域搜索，当有轨迹点出现在其邻域范围内，比较当前扫描与历史扫描的几何形状和强度特征，完成回环检测，当检测到回环时，对其进行优化，实现位姿估计和地图构建；

所述利用回环检测算法减少累积漂移误差，实现位姿估计和地图构建具体包括以下步骤：

步骤S61：首先，对当前扫描的轨迹点进行邻域搜索，根据公式(7)判断是否有历史轨迹点在其δ_r范围内：

式中，(x_i,y_i,z_i)^T为当前轨迹点的坐标，(x_j,y_j,z_j)^T为历史轨迹点的坐标，δ_r为距离阈值；

步骤S62：当检测到有轨迹点在其δ_r范围内时，比较当前扫描和历史扫描的几何形状和强度特征；

步骤S63：用ξ表示强度读数，则点a表示为[x_a,y_a,z_a,ξ_a]^T，转换为极坐标为：

步骤S64：将极坐标沿方位角和径向方向均等的分成N_s个扇区和N_r个环，对点云进行分段；则扇区的圆心角为环之间的径向间隙为其中，L_max为激光雷达扫描的最大有效扫描半径；将第i环和第j扇区重叠的段内点集记为P_ij，则P_ij为：

式中，其中，N表示N_r或N_s；遍历整帧点云P^L；

步骤S65：对于每个分段，应用编码函数来减少强度维度：

若P_ij为空集，则ξ_ij为0；最终将强度几何特征Θ表示为N_r×N_s矩阵为：

步骤S66：引入一种有效的二进制运算几何重识别以实现快速索引；给定Θ，其在局部坐标上的几何分布可以表示为二进制矩阵Ω：

对于查询强度几何特征Θ^q，候选强度几何特征Θ^c，及其对应的二进制矩阵Ω^q，Ω^c，几何相似度为：

式中，|Ω^q|代表Ω^q中元素的个数，XOR(Ω^q，Ω^c)是指两个矩阵之间逐元素异或运算后的元素之和；Ω的列向量表示方位角方向，则将视角变化看作是Ω的列偏移；加入视角变化后的几何相似度计算为：

式中，是Ω^q的第i列移位；同时，根据公式(14)确定最匹配的用于校正视角变化；设定阈值δ_o过滤掉不匹配的对；

步骤S67：对于查询强度几何特征Θ^q和候选强度几何特征Θ^c，通过比较对应列之间的余弦距离，计算强度相似度：

式中，v_j是Θ的第j列；由公式(14)确定了视角变化k，则加入视角变化后的强度相似度计算为：

式中，表示Θ^q的第k列移位；

设定阈值δ_i过滤掉不匹配的对，完成回环检测；

步骤S68：当检测到回环时，利用GTSAM进行优化，完成全局位姿优化和全局地图的构建。