[发明专利]基于点云片段匹配约束和轨迹漂移优化的回环检测方法

摘要

一种基于点云片段匹配约束和轨迹漂移优化的回环检测方法，采用实时激光雷达里程计与建图LOAM系统，但是LOAM系统在长时间建图之后会产生较大漂移，所以提出一种点云片段匹配约束和轨迹漂移优化的回环检测方法。通过对激光雷达获取的点云进行分割、描述、匹配，从而找到回环关系，发现回环之后利用显式循环闭合ELCH算法进行回环调整，并且提出优化算法来消除点云地图调整过程出现的局部不一致。本发明在优化之后提出位姿预测与补偿算法对回环之后的位姿进行预测和补偿。从而能达到最大化减少漂移误差的效果。

主权项

1.一种基于点云片段匹配约束和轨迹漂移优化的回环检测方法，其特征在于，所述方法包括以下步骤：1)利用激光雷达获取时序点云数据，该数据经过实时激光雷达里程计与建图LOAM系统产生位姿，设当前获取的点云为Pi，其中帧序号为i，获得的时序点云数据通过LOAM系统生成初始的点云地图；2)将获取的点云和位姿输入到点云分割与匹配系统中，首先将点云进行分割，分割算法采用欧几里得分割方法；在分割的过程中，采用EigenValue的方法，在这个方法中,我们通过计算点云片段的特征值并将其组合在维度为1×7的特征向量中,该特征向量由点云片段的线性,平面性,散射性,全方位性,各向异性,特征根和曲率测量值组成；然后将分割后的点云进行描述，描述好的点云片段储存在建立好的数据结构中作为点云片段数据库；3)新获取的点云数据都要与之前建立的点云数据库进行比对，来找到匹配的数据，并且利用Kd‑Tree来加快搜索的速度；利用随机森林算法，先对点云数据进行匹配训练，建立训练好的随机森林模型；将输入的分割片段通过随机森林算法与点云片段数据库中进行决策，提取候选点云片段，然后利用滤除欧几里得距离过远的点云来减少搜索空间；点云片段的H的重心为其中p为点云片段中的点，N为点云片段的点的数量，n为该点云片段中点的序号，设空间中的预匹配点云片段为Hi和Hj,i和j分别为点云的序号,Hi和Hj点云片段的重心之间的欧几里得距离表示为如果α为自定义的阈值，那么将该点云片段从搜索的空间中滤除，然后采用随机抽样一致性测试方法来滤除存在偏差的候选匹配对，从而获得鲁棒的候选匹配点云对；4)利用迭代最近点ICP算法，计算出当前点云和匹配的点云之间的漂移矩阵，在回环调整上，采用显式循环闭合ELCH算法，在图模型中，图包含顶点和边，设置边的测量为Ei.j＝Xi‑Xj            (3)其中Xi与Xj是边连接的对应顶点的位姿，并且边的观测值为其中是观测值与真实值的偏差，那么图模型优化的目标为最小化下列马氏距离其中为对应的协方差，然后再通过线性插值的方法将漂移误差转移到图的各个节点中，当只存在一个回环的情况下Ri＝slerp(n‑1,R)其中Ri,ti分别为回环图中i节点分配的旋转量和平移量，n为图中产生回环顶点的距离，为总的偏移距离，R为总的旋转偏移，slerp()函数为旋转矩阵的插值函数，该差值函数的步骤为：4.1首先将旋转矩阵转化为四元数其中ri,j为旋转矩阵R中的元素，例如r1,2为R中第一行第二列的元素，tr(R)为该旋转矩阵的迹；当r1,1>r2,2，r1,1>r3,3时，当r2,2>r3，3时，其他情况时，4.2)然后将对应的四元数转化为对应的轴角a4.3)最后插值后的旋转矩阵为，其中q₀，q_x，q_y，q_z为四元数中的分量；当存在多个回环时，使用Dijistra算法，找出图中回环路径最短的路径，再用上述公式进行优化；5)检测到回环之后，提出一种消除局部不一致性的方法，利用将位姿分解为相对位姿的方法，在e+1>0时刻的绝对位姿为Se+1，那么该时刻对于前一时刻的相对位姿为其中最后将系统中新输入n时刻的绝对位姿通过其中Sn‑1为经过回环调整后的绝对位姿，Sn为新输入帧的绝对位姿；6)在回环之后，提出一种位姿预测和补偿的方法，如下公式为位姿的补偿，其中为从s时刻到e时刻的位置偏移，X_n+1,X_n为n+1，n对应节点的位姿，N为回环中节点的总数，其中最后预测的位姿为S_f，S_f为其中Sc为补偿参数的李代数写法，Sn为当前获得相对位姿，计算公式为：S_j为当前获得的位姿，为前一帧位姿的逆，S_j，S_j‑1都为李代数的写法。