一种4D成像毫米波雷达的回环检测方法与流程

一种4d成像毫米波雷达的回环检测方法
技术领域
1.本发明涉及一种回环检测方法。


背景技术：

2.回环检测（loop closure detection）用于在移动主体进行同步定位与建图（simultaneous localization and mapping，下文简称slam）过程中判断主体是否再次经过同一场景位置，并帮助移动主体消除建图过程中因定位存在的累计误差而导致地图构建的偏差。回环检测帮助slam系统构建全局一致的轨迹和地图，对slam系统的精确性和鲁棒性有重大意义。
3.目前回环检测主要采用以下方式：方式一、基于gps的回环检测方法：室外无人车所配备的gps可以提供全局的位置，帮助无人车确认是否回到某个曾经经过的位置。该方式的缺点是受到gps信号强弱的影响，缺乏稳定性。
4.方式二、基于图像的回环检测方法：基于词袋模型的回环检测是通过创建特征来描述图像，从而判断两幅图像之间的相似性来检测回环。该方式的缺点是不适用于4d成像毫米波雷达提供的点云信息，因为点云无法直接通过图像的方式来计算相似性。
5.方式三、用于激光点云回环检测的scan-context方法：采用了每个区域点云的最高点生成bev图像（bird's eyes view，鸟瞰图）实现回环检测。该方式的缺点是，对点云数量较稀疏的4d成像毫米波雷达雷达提供的信息，bev图像像素点较少，存在回环检测召回率和配准率低的问题。


技术实现要素：

6.本发明要解决的技术问题是：现有的目前回环检测缺乏稳定性，不适用于4d成像毫米波雷达提供的点云信息，存在回环检测召回率和配准率低的问题。
7.为了解决上述技术问题，本发明的技术方案是提供了一种4d成像毫米波雷达的回环检测方法，其特征在于，包括以下步骤：步骤1、仅利用4d成像毫米波雷达获得点云数据；步骤2、将当前帧点云数据与前n帧点云数据进行拼接，得到当前帧拼接点云，n≥1；步骤3、通过投影4d成像毫米波雷达提供的所有点云数据，得到当前帧拼接点云的信息图像，具体包括以下步骤：步骤301、获得安装位置高于地面的毫米波雷达的俯仰角范围，将该俯仰角范围划分为n1个区域，计算每个点云数据所对应的俯仰角，并将计算得到的俯仰角的值对应到n1个区域中的一个区域的值，获得p值；步骤302、在鸟瞰图平面上，将以当前帧拼接点云为中心的圆均匀划分为n2个扇形区域，计算每个点云数据所对应的偏航角，并将计算得到的偏航角的值对应到n2个扇形区
域中的一个扇形区域的值，获得y值；步骤303、计算每个点云数据的距离值d，获得d值；步骤304、将p值以及y值作为信息图像的横轴以及纵轴，则每个点云数据对应信息图像上的一个像素点，当前像素点的像素值则为对应点云数据的d值，从而获得大小为n1
×
n2、深度值为m的信息图像，m为根据4d成像毫米波雷达的探测范围所确定的值；步骤4、基于步骤3获得的信息图像获得n1
×
1的信息列向量，并采用最近邻节点算法在已建的kd树中进行搜索，得到与当前信息列向量最近的若干个信息列向量，从而获得与这些信息列向量对应的候选点云帧，其中，kd树用于保存当前信息列向量之间的历史信息列向量；步骤5、从所有候选点云帧中获得与当前帧拼接点云最为相似的候选点云帧，被选中的候选点云帧为最优候选点云帧；步骤6、选取最优候选点云帧对应位姿前后共若干帧的点云数据作为局部地图，并将当前帧拼接点云与局部地图进行迭代最近点匹配得到矩阵h，对矩阵h进行svd分解后得到帧间位姿变换，将帧间位姿变换作为约束插入位姿图估计中进行回环优化。
8.优选地，在步骤2中，将当前帧点云数据与前n帧点云数据进行拼接时，以当前帧点云数据的前一帧点云点云数据所对应的位姿信息为原点，将位姿信息对应的n+1帧点云数据转换到同一坐标下后进行拼接，得到所述当前帧拼接点云。
9.优选地，在步骤301中，将所述俯仰角范围按照1
°
为步长划分为所述n1个区域，则将所述俯仰角的计算值处理为大于0度的值后再向上取整，使得计算得到的俯仰角的值对应到n1个区域中的一个区域的值，获得所述p值。
10.优选地，在步骤302中，将所述以当前帧拼接点云为中心的圆按照1
°
为步长划分为360个扇形区域，则每个点云数据所对应的偏航角的计算值向上取整，使得计算得到的偏航角的值对应到n2个扇形区域中的一个扇形区域的值，获得所述y值。
11.优选地，在步骤303中，所述距离值d=sqrt(x
²
+y
²
+z
²
)，其中，sqrt()表示取平方根运算。
12.优选地，在步骤303中，对计算得到所述距离值d向上取整，获得所述d值。
13.优选地，在步骤4中，遍历点云信息图像的每一行中的像素点，将每个像素点的像素值除以一个预设值后相加并求均值，得到一个所述n1
×
1的信息列向量。
14.优选地，在步骤4中，所述kd树沿着数据轴递归地划分n1个维度的参数空间，将其划分为嵌套的正交各向异性区域。
15.优选地，在步骤4中，采用最近邻节点算法在已建的kd树中进行搜索时，从顶层节点开始依据维度划分标准开始查找，直到找到kd树中的子节点停止二叉搜索，该子节点便是最近邻近似节点；接着由最近邻近似节点开始回溯上一个根节点，比较该最近邻近似节点和根节点之间的点之间距离以及该最近邻近似节点和根节点所在分割轴之间的点轴之间距离，若点之间距离小于点轴之间距离则是最近值，结束回溯；否则，继续往另一个分支进行搜索步骤。
16.优选地，在步骤5中，采用基于哈希算法的图像相似度算法从所有候选点云帧中获得与当前帧拼接点云最为相似的候选点云帧，包括以下步骤：步骤501、计算候选点云帧的点云信息图像的像素平均值，并遍历所有像素点的像
素值，大于平均值的记为1、其他则记为0，得到一个二进制的哈希码一；计算当前帧拼接点的点云信息图像的像素平均值，并遍历所有像素点的像素值，大于平均值的记为1、其他则记为0，得到一个二进制的哈希码二；步骤502、利用哈希码一以及哈希码二计算候选点云帧的点云信息图像与当前帧拼接点的点云信息图像之间的汉明距离d(x,y)，d(x,y)=∑xk
⊕
yk，xk、yk分别为哈希码一的第k位的值以及哈希码二的第k位的值，
⊕
代表模二加法运算；步骤503、将与当前帧拼接点的点云信息图像之间的汉明距离d(x,y) 最小的候选点云帧选为最优候选点云帧。
17.与现有的回环检测方法相比，本发明的创新之处在于：（1）本发明利用4d成像毫米波雷达生成全点云的描述图像和多帧融合的方式，相比用于激光点云回环检测的scan-context方法，本发明更适用于4d成像毫米波雷达的点云信息，提高了回环检测的召回率。
18.（2）本发明另外利用图像相似度算法处理4d成像毫米波雷达的点云图像，提高了回环检测的准确率。
19.（3）本发明只采用成像毫米波雷达，在成本控制上相较于激光雷达传感器具有优势。
20.本发明提出一种基于4d成像毫米波雷达点云的回环检测方法，与现有的其它传感器的回环检测方法相比，对信息图像的方式进行了改进，不仅更加适配根据成像毫米波雷达的点云特性，同时采用多帧叠加的方式提高了信息图像中的信息量，从而解决了回环召回率较低的问题，提高了回环检测的可靠性。更进一步，本发明不仅采用了生成信息列向量并加入kd树搜索的方式提高范围确定的检测速度，还采用了基于哈希算法的图像相似度计算方法进一步确认回环匹配的准确性，大大提高了回环的准确率。最后本发明还采用icp算法进行点云帧之间的匹配，完成回环检测地图的一致性矫正。
附图说明
21.图1为本发明的流程图。
具体实施方式
22.下面结合具体实施例，进一步阐述本发明。应理解，这些实施例仅用于说明本发明而不用于限制本发明的范围。此外应理解，在阅读了本发明讲授的内容之后，本领域技术人员可以对本发明作各种改动或修改，这些等价形式同样落于本技术所附权利要求书所限定的范围。
23.如图1所述，本实施例公开的一种4d成像毫米波雷达的回环检测方法，包括以下步骤：步骤1、仅利用4d成像毫米波雷达获得点云数据。
24.步骤2、通过对传入slam系统中的当前帧点云数据进行拼接，得到拼接点云，具体包括以下步骤：步骤201、首先得到slam系统提供的实时位姿信息队列和对应的点云信息，其中，实时位姿信息队列中每个位姿信息之间的欧式距离为1m。
25.步骤202、根据当前帧点云数据对应的位姿信息，获得当前帧的前两帧点云数据所对应的位姿信息，并以当前帧点云数据的前一帧点云点云数据所对应的位姿信息为原点进行转换，将位姿信息对应的三帧点云数据转换到同一坐标下进行拼接得到拼接点云，增加点云数量从而提高点云信息量，同时采用体素滤波的方式，对拼接后的噪声点和杂点进行滤除。
26.本发明采用三帧是因为点云拼接的数量越多，最后回环检测得到的位置范围越大，检测速度越慢，影响回环的实时性和准确性，三帧的拼接能兼顾两者。
27.步骤3、通过投影4d成像毫米波雷达提供的点云信息，得到当前一帧拼接点云的信息图像，具体包括以下步骤：步骤301、处理4d成像毫米波雷达点云数据（包含x、y、z三种数据）通过公式p=arctan(y/z)得到俯仰角p，得到安装位置略高于地面的毫米波雷达的俯仰角范围为-5
°
～35
°
，y、z分别为每个点云数据的y轴坐标以及z轴坐标。所以本发明将点云数据的俯仰角加5
°
，令所有俯仰角大于0度，并向上求整得到每个点云数据对应的p整数值。
28.步骤302、处理4d成像毫米波雷达点云数据得到偏航角y：因为一帧拼接点云图像是采集360
°
周围环境得到的，所以以拼接点云原点为中心，在bev平面上均匀划分为360个扇形区域。根据公式y=artan(x/y)得到点云数据的偏航角y，并向上求整得到每个点云数据对应的y整数值，其中，x、y分别为每个点云数据的x轴坐标以及y轴坐标。
29.步骤303、处理点云数据得到距离值d：先通过点云信息的x、y、z值，根据公式d=sqrt(x
²
+y
²
+z
²
)计算其距离中心点的欧式距离d。向上求整得到每个点云数据对应的d整数值，利用d作为点云信息图像上像素的像素值。
30.步骤304、根据p整数值、y整数值、d整数值三个值得到当前一帧拼接点云的信息图像：采用点云数据的y整数值作为信息图像的横轴、p整数值作为信息图像的纵轴。因为4d成像毫米波雷达的探测范围为0~250m，所以设置信息图像深度为8，每个历史点云数据对应信息图像的一个像素点，并采用d/2的值给信息图像中对应的像素点赋值。通过以上处理得到一个尺寸为40
×
360、深度值为8的点云信息图像。
31.步骤4、压缩图像信息：遍历点云信息图像的每一行中的像素，除以128后相加并求均值，得到一个40
×
1的信息列向量，该信息列向量通过一个kd树进行保存。kd树沿着数据轴递归地划分参数空间（本发明划分了40个维度的参数空间），将其划分为嵌套的正交各向异性区域，信息列向量中的数据点对应放入其中。在得到当前一帧拼接点云的信息列向量后，采用最近邻节点算法在kd树中进行搜索，该方法根据kd树所构建的空间索引结构，将目标点（为当前一帧拼接点）放入kd树中，从顶层节点开始依据上述维度划分标准开始查找，直到找到kd树中的子节点停止二叉搜索，该子节点便是最近邻近似节点。接着由最近邻近似节点开始回溯上一个根节点，比较该最近邻近似节点和根节点距离以及和根节点所在分割轴之间的距离，若点之间距离小于点轴之间距离则是最近值，结束回溯。否则，继续往另一个分支进行搜索步骤。通过以上计算步骤，可以得到目标点在点云中的最近m个信息列向量（m取值根据地图大小和处理器性能决定），从而获得对应的m个候选点云帧。该方法可以快速的帮助当前位置锁定候选范围，提高搜索速度和效率。
32.步骤5、通过kd树搜索信息列向量得到m个候选点云帧后，需要进一步最终确认哪个候选点云帧和当前一帧拼接点最为相似，所以采用基于哈希算法的图像相似度算法。该
图像相似度算法首先计算候选点云帧以及当前一帧拼接点的点云信息图像的像素平均值，并遍历所有像素点的像素值，大于平均值的记为1，其他则记为0，得到两个256位二进制的哈希码。然后利用两个哈希码计算两幅图之间的汉明距离d(x,y)，d(x,y)=∑xk
⊕
yk，xk、yk分别为通过候选点云帧的点云信息图像得到的哈希码的第k位的值以及通过当前一帧拼接点的点云信息图像得到的哈希码的第k位的值，
⊕
代表模二加法运算。d(x,y）表示两幅图在相同位置上不同像素的数目的总和，它能够反映两图像之间的差异，进而提供图像之间的相似程度的客观依据。通过汉明距离d(x,y)来描述两幅点云信息图像之间的相似度：汉明距离d(x,y)越大，则两幅点云信息图像的相似性越低。汉明距离d(x,y)最小的候选点云帧被选为最优候选点云帧。
33.步骤6、完成候选帧判断后，选取最优候选点云帧对应位姿前后共50帧的点云数据作为局部地图，并将当前一帧拼接点与局部地图进行迭代最近点（ iterative closest point，icp）匹配。icp 匹配首先寻找两个点云之间每个点的最临近点，并求出两个点云的质心pc1和pc2。接着让点云中每个点和质心相减得到每个点的差值，并让两个点云的差值相乘后相加得到一个3
×
3的矩阵h。对矩阵h进行svd分解得到h=u∑v
t
，其中，u表示左奇异值矩阵、∑表示奇异值矩阵、v表示右奇异值矩阵，则两帧点云之间的旋转r=vu
t
，平移为t = pc1
ꢀ‑ꢀ
r*pc2。将得到的帧间位姿变换作为约束插入位姿图估计中进行回环优化。

技术特征：
1.一种4d成像毫米波雷达的回环检测方法，其特征在于，包括以下步骤：步骤1、仅利用4d成像毫米波雷达获得点云数据；步骤2、将当前帧点云数据与前n帧点云数据进行拼接，得到当前帧拼接点云，n≥1；步骤3、通过投影4d成像毫米波雷达提供的所有点云数据，得到当前帧拼接点云的信息图像，具体包括以下步骤：步骤301、获得安装位置高于地面的毫米波雷达的俯仰角范围，将该俯仰角范围划分为n1个区域，计算每个点云数据所对应的俯仰角，并将计算得到的俯仰角的值对应到n1个区域中的一个区域的值，获得p值；步骤302、在鸟瞰图平面上，将以当前帧拼接点云为中心的圆均匀划分为n2个扇形区域，计算每个点云数据所对应的偏航角，并将计算得到的偏航角的值对应到n2个扇形区域中的一个扇形区域的值，获得y值；步骤303、计算每个点云数据的距离值d，获得d值；步骤304、将p值以及y值作为信息图像的横轴以及纵轴，则每个点云数据对应信息图像上的一个像素点，当前像素点的像素值则为对应点云数据的d值，从而获得大小为n1
×
n2、深度值为m的信息图像，m为根据4d成像毫米波雷达的探测范围所确定的值；步骤4、基于步骤3获得的信息图像获得n1
×
1的信息列向量，并采用最近邻节点算法在已建的kd树中进行搜索，得到与当前信息列向量最近的若干个信息列向量，从而获得与这些信息列向量对应的候选点云帧，其中，kd树用于保存当前信息列向量之间的历史信息列向量；步骤5、从所有候选点云帧中获得与当前帧拼接点云最为相似的候选点云帧，被选中的候选点云帧为最优候选点云帧；步骤6、选取最优候选点云帧对应位姿前后共若干帧的点云数据作为局部地图，并将当前帧拼接点云与局部地图进行迭代最近点匹配得到矩阵h，对矩阵h进行svd分解后得到帧间位姿变换，将帧间位姿变换作为约束插入位姿图估计中进行回环优化。2.如权利要求1所述的一种4d成像毫米波雷达的回环检测方法，其特征在于，在步骤2中，将当前帧点云数据与前n帧点云数据进行拼接时，以当前帧点云数据的前一帧点云点云数据所对应的位姿信息为原点，将位姿信息对应的n+1帧点云数据转换到同一坐标下后进行拼接，得到所述当前帧拼接点云。3.如权利要求1所述的一种4d成像毫米波雷达的回环检测方法，其特征在于，在步骤301中，将所述俯仰角范围按照1
°
为步长划分为所述n1个区域，则将所述俯仰角的计算值处理为大于0度的值后再向上取整，使得计算得到的俯仰角的值对应到n1个区域中的一个区域的值，获得所述p值。4.如权利要求1所述的一种4d成像毫米波雷达的回环检测方法，其特征在于，在步骤302中，将所述以当前帧拼接点云为中心的圆按照1
°
为步长划分为360个扇形区域，则每个点云数据所对应的偏航角的计算值向上取整，使得计算得到的偏航角的值对应到n2个扇形区域中的一个扇形区域的值，获得所述y值。5.如权利要求1所述的一种4d成像毫米波雷达的回环检测方法，其特征在于，在步骤303中，所述距离值d=sqrt(x
²
+y
²
+z
²
)，其中，sqrt()表示取平方根运算。6.如权利要求5所述的一种4d成像毫米波雷达的回环检测方法，其特征在于，在步骤
303中，对计算得到所述距离值d向上取整，获得所述d值。7.如权利要求5所述的一种4d成像毫米波雷达的回环检测方法，其特征在于，在步骤4中，遍历点云信息图像的每一行中的像素点，将每个像素点的像素值除以一个预设值后相加并求均值，得到一个所述n1
×
1的信息列向量。8.如权利要求5所述的一种4d成像毫米波雷达的回环检测方法，其特征在于，在步骤4中，所述kd树沿着数据轴递归地划分n1个维度的参数空间，将其划分为嵌套的正交各向异性区域。9.如权利要求5所述的一种4d成像毫米波雷达的回环检测方法，其特征在于，在步骤4中，采用最近邻节点算法在已建的kd树中进行搜索时，从顶层节点开始依据维度划分标准开始查找，直到找到kd树中的子节点停止二叉搜索，该子节点便是最近邻近似节点；接着由最近邻近似节点开始回溯上一个根节点，比较该最近邻近似节点和根节点之间的点之间距离以及该最近邻近似节点和根节点所在分割轴之间的点轴之间距离，若点之间距离小于点轴之间距离则是最近值，结束回溯；否则，继续往另一个分支进行搜索步骤。10.如权利要求5所述的一种4d成像毫米波雷达的回环检测方法，其特征在于，在步骤5中，采用基于哈希算法的图像相似度算法从所有候选点云帧中获得与当前帧拼接点云最为相似的候选点云帧，包括以下步骤：步骤501、计算候选点云帧的点云信息图像的像素平均值，并遍历所有像素点的像素值，大于平均值的记为1、其他则记为0，得到一个二进制的哈希码一；计算当前帧拼接点的点云信息图像的像素平均值，并遍历所有像素点的像素值，大于平均值的记为1、其他则记为0，得到一个二进制的哈希码二；步骤502、利用哈希码一以及哈希码二计算候选点云帧的点云信息图像与当前帧拼接点的点云信息图像之间的汉明距离d(x,y)，d(x,y)=∑xk
⊕
yk，xk、yk分别为哈希码一的第k位的值以及哈希码二的第k位的值，
⊕
代表模二加法运算；步骤503、将与当前帧拼接点的点云信息图像之间的汉明距离d(x,y) 最小的候选点云帧选为最优候选点云帧。

技术总结
本发明公开了一种4D成像毫米波雷达的回环检测方法，与现有的其它传感器的回环检测方法相比，对信息图像的方式进行了改进，不仅更加适配根据成像毫米波雷达的点云特性，同时采用多帧叠加的方式提高了信息图像中的信息量，从而解决了回环召回率较低的问题，提高了回环检测的可靠性。更进一步，本发明不仅采用了生成信息列向量并加入KD树搜索的方式提高范围确定的检测速度，还采用了基于哈希算法的图像相似度计算方法进一步确认回环匹配的准确性，大大提高了回环的准确率。最后本发明还采用ICP算法进行点云帧之间的匹配，完成回环检测地图的一致性矫正。地图的一致性矫正。地图的一致性矫正。


技术研发人员：叶霆锋 陆新飞 薛旦 史颂华 赵映重 李家松 娄慧丽 潘松 张建君
受保护的技术使用者：上海几何伙伴智能驾驶有限公司
技术研发日：2023.04.24
技术公布日：2023/7/22