CN110688440B - 一种适用于子地图重叠部分较少的地图融合方法 - Google Patents

Abstract

本发明涉及地图融合术领域，更具体地，涉及一种适用于子地图重叠部分较少的地图融合方法。首先使用点云平面提取，法向量估计，法向量投影角统计的方法迭代计算旋转矩阵R，然后用R迭代变换源点云，最后对两组点云做三视图投影得到两组灰度图，对灰度图使用相位相关法计算平移向量t；最终得到两组点云之间的变换矩阵(R，t)。本发明提供的一种适用于子地图重叠部分较少的地图融合方法，能够正确完成低重叠度下的子地图的融合，既规避了前几种方法寻找对应点的误差，又使该方法适用于重叠部分较少，且三轴同时都有旋转角度的两组大型地图点云之间的配准。

Description

一种适用于子地图重叠部分较少的地图融合方法

技术领域

本发明涉及地图融合术领域，更具体地，涉及一种适用于子地图重叠部分较少的地图融合方法。

背景技术

执行测绘、探索未知环境的任务中，通常可以派出单台或者多台无人车进行建图，以提升效率；但当缺少GPS等位置信息的时候，如何将单台或者多台无人车采集得到的点云地图数据进行融合成为一个难题。这时候，点云地图融合技术就尤为重要。

最常用的点云配准算法以ICP为代表的迭代最近邻点算法，这一类方法中主要步骤是找到两组点云集合中距离最近的点对，根据估计的变换关系(R和t)来计算距离最近点对经过变换之后的误差，经过不断的迭代直至误差小于某一阈值或者达到迭代次数得到最终的变换矩阵。迭代最近邻点类算法的缺点是它需要提供一个较准确的初始变换矩阵才能得到较好的配准效果；另外对于两组重叠部分较少的点云，这种算法配准效果较差。导致该缺点的原因是该类算法通过寻找最近点确定对应点，然而最近点对于两组重叠部分较少的点云多数情况下不是正确的对应点。

另一种思路是基于正态分布寻找变换的方法，其基本思想是先根据目标点云来构建多维变量的正态分布，如果变换矩阵能使得两组点云匹配的很好，那么变换点在目标点云参考系中的概率密度将会很大。因此，该方法使用优化的方法求出使得概率密度之和最大的变换矩阵。基于正态分布寻找变换的算法同样不适用于配准重叠部分较少的两组大型点云，原因是在进行概率分布估计的时候，重叠区域较小会使得分布不准确影响后续优化步骤。

还有基于点的几何特征的进行配准的思路，如四点法，根据源点云中不共面四点的仿射不变性，从目标点云中寻找对应点，从而获得变换矩阵。另外还有利用FPFH特征的配准，具体做法是在计算两组点云的FPPH特征之后，使用SAC-IA(采样一致性初始配准算法)做粗配准得到初始变换，然后再用ICP做精配准的算法。两类基于点的几何特征的配准方法也各有缺点：四点法对于两组重叠部分较少的点云，在点云数据量较小时配准效果较好，但是对于大型地图点云的配准效果较差，原因是四点法通过寻找两组点云不共面点的仿射不变性确定对应点，小型点云可以正确确定对应点，而对于大型地图点云，确定的对应点不一定是重叠部分，所以配准效果差。基于FPPH特征的配准算法缺点是计算FPFH特征耗时较长，另外在配准两组重叠部分较少的点云也是效果较差，原因同迭代最近点算法一样，都是通过最近点确定对应点。

最后还有一种基于统计直方图的点云配准方案，该方法将变换矩阵的旋转和平移进行分别计算，首先对两组点云进行法向量估计，并将法向量投影到三个坐标轴两两构成的平面，计算反正切值得到欧拉角的集合，然后再用统计方法得到一个最大概率的欧拉角作为该点云朝向的代表，最后两组点云欧拉角作差并转换得到旋转矩阵。求解平移向量则是将两组点云做三视图投影，然后对两组点云对应的投影图应用相位相关算法计算图像平移量，最后做平均求出平移向量。该方法一定程度上可以配准两组重叠部分较少的点云，但是仍然有缺陷，它仅仅适用于两组点云之间只有z轴旋转的情况。基于统计直方图的点云配准方案一定程度上可以配准两组重叠部分较少的点云，但是仅仅适用于两组点云之间只有z轴旋转的情况。原因主要有两点，一是该方法使用地图中的所有点进行法向量的计算和投影，计算法向量投影的反正切值时会产生大量零值和无穷值影响角度统计；二是在三轴同时都有旋转角度时，该算法按z-y-x轴顺序计算欧拉角会导致前一个角度计算产生的误差影响到后面的角度计算，并且由于设置了迭代步骤，误差随着迭代次数的增加而增大，所以无法正确配准。

发明内容

本发明为克服上述现有技术中的缺陷，提供一种适用于子地图重叠部分较少的地图融合方法，既规避了寻找对应点的误差，又使该方法适用于重叠部分较少，且三轴同时都有旋转角度的两组大型地图点云之间的配准。

为解决上述技术问题，本发明采用的技术方案是：一种适用于子地图重叠部分较少的地图融合方法，首先使用点云平面提取，法向量估计，法向量投影角统计的方法迭代计算旋转矩阵R，然后用R迭代变换源点云，最后对两组点云做三视图投影得到两组灰度图，对灰度图使用相位相关法计算平移向量t；最终得到两组点云之间的变换矩阵(R，t)。

在本发明相中，针对基于统计直方图的配准算法改进了旋转矩阵的计算步骤，使得改进后可以完成两组重叠部分较少的大型地图点云的配准，并且从误差值以及实际效果上都远远优于现有技术。在只有z轴单轴旋转时，基于统计直方图的方案比其他配准方案效果更好，但是三轴同时旋转时，该方案也无法正确配准两片部分重叠的点云。而本发明提出的方案在这种情况下可以正确配准两片部分重叠的点云，基于它们具有共同的即使只有部分重叠的平面。

在本发明中，地平面点云提取与对齐独立进行，解决两组重叠部分较少的点云之间法向量差异大的问题；在地平面对齐的基础上，计算点云法向量投影的方向角，统计得到偏航角的估计值，并采用迭代对旋转矩阵进行进一步修正，以此得到两组重叠部分较少的点云的旋转矩阵。

进一步的，求解旋转矩阵R具体包括以下步骤：

S11.使用RANSAC算法提取目标点云P_t以及源点云P_s中的平面并得到各自对应的法向量n_t＝(n_x，n_y，n_z)以及n_s＝(n_x′，n_y′，n_z′)；然后由两个法向量分别对YOZ和ZOX平面作投影，得到四个向量，分别是：

n_t对YOZ平面投影向量n_YZ＝(n_y，n_z)，

n_t对ZOX平面投影向量n_XZ＝(n_x，n_z)，

n_s对YOZ平面投影向量n′_YZ＝(n_y′，n_z′)，

n_s对ZOX平面投影向量n′_XZ＝(n_x′，n_z′)

之后计算通过反正切函数计算n_YZ绕X轴的旋转角R_X，n′_YZ绕X轴的旋转角R_X′，n_XZ绕Y轴的旋转角R_Y，n′_XZ绕Y轴的旋转角R_Y′，对应相减得到俯仰角pitch和滚转角roll：

pitch＝R_X′-R_X；

roll＝R_Y′-R_Y；

S12.使用最小二乘拟合平面的方法，求解点云里其余每个点的法向量n＝(n_x，n_y，n_z)，然后再将它们投影到XOY平面上，得到该点对应的投影向量n_XY＝(n_x，n_y)；之后计算每一个投影向量的反正切值R_Z，得到z轴方向角R_Z的集合：

同理对源点云P_s进行计算得到z轴旋转角集合R′_Z；

S13.采用统计的方法，先对目标点云z轴方向角集合用直方图进行频数统计，取频数最大的区间的中值作为z轴方向角估计值

式中的bin参数是分组数，依据点云的规模而定；

同理可得源点云的z轴方向角估计值

然后对两组点云的z轴方向角估计值作差，即可得偏航角：

得到欧拉角：偏航角、俯仰角和滚转角之后，将其转换得到旋转矩阵R。

进一步的，使用迭代的方法，每计算一次偏航角便更新一次旋转矩阵，再用该矩阵变换源点云，然后重复步骤S13直至达到最大迭代次数或者旋转矩阵收敛。

进一步的，所述的计算平移向量t具体包括以下步骤：

S21.将目标点云数据和源点云数据进行降维处理，分别投影到XOY、YOZ、ZOX平面，得到六个投影灰度图；

S22.对投影图做相位相关计算，得到图像的平移量；包括以下步骤：

S221.对待处理的两幅图像ima1和ima2应用汉宁窗函数去除图像的边界效应；

S222.求两幅图像的离散傅里叶变换：F₁[ima1],F₂[ima2]；

S223.计算互功率谱G：

S224.对互功率谱求傅里叶逆变换g：g＝DFT^-1[G]；

S225.对r计算最大值的位置，即两幅图像的平移量；

S226.通过步骤S221至S225可以求得三组平移量，分别是：

目标点云与源点云俯视图对应平移量t_xy＝(t_x,t_y,0)；

目标点云与源点云测视图对应平移量t_xz＝(t_x,0,t_z)；

目标点云与源点云正视图对应平移量t_yz＝(0,t_y,t_z)；

S23.将三组平移量取平均得到最终的平移向量t：

t＝0.5*(t_xy+t_xz+t_yz)。

进一步的，所述的S21步骤具体包括：先将点坐标四舍五入取整，然后在原坐标基础上加上一个足够大的常量，将所有坐标值化为非负数，再在空白图像中进行描摹，最终得到六个投影灰度图。

与现有技术相比，有益效果是：本发明提供的一种适用于子地图重叠部分较少的地图融合方法，能够正确完成低重叠度下的子地图的融合，既规避了前几种方法寻找对应点的误差，又使该方法适用于重叠部分较少，且三轴同时都有旋转角度的两组大型地图点云之间的配准。

附图说明

图1是本发明整体方法流程图，其中，k为点云法向量估计时，需要搜索的近邻点数量，取值范围20～200，当点云重叠部分共面点较多时，考虑取较大的k值，反之取较小的k值，默认值为100；i为进行旋转矩阵计算的迭代次数，与icp算法类似，迭代次数影响配准效果，一般5次迭代以内计算收敛，取值范围1～5，默认值为3；b为法向量投影角度的统计直方图的分组数，b主要影响角度的统计频数，较大的b值虽然使角度细分但是得到的最大频数可能不是真正的点云朝向，而过小的b值又使得到的角度不够精确，所以建议依据输入点云的规模取b的值，取值范围为输入点云规模的1～2％，默认值为1％。

图2是使用本发明的匹配算法效果图。

具体实施方式

附图仅用于示例性说明，不能理解为对本发明的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。附图中描述位置关系仅用于示例性说明，不能理解为对本发明的限制。

如图1所示，本发明主要解决了部分重叠的大型地图点云之间的配准问题，得到两组点云之间的变换矩阵(R，t)。本发明先使用点云平面提取，法向量估计，法向量投影角统计的方法迭代计算旋转矩阵R，然后用R迭代变换源点云，最后对两组点云做三视图投影得到两组灰度图，对灰度图使用相位相关法计算平移向量t。

第一部分，求解旋转矩阵R，通过求解欧拉角然后换算的方式得到。

步骤一：使用RANSAC算法提取目标点云P_t以及源点云P_s中的平面并得到各自对应的法向量。由于两组点云都是地图，所以这一步可以得到各自的地面点云。然后由两个法向量分别对YOZ和ZOX平面作投影，得到四个向量，分别是：

n_YZ＝(n_y，n_z)，n_XZ＝(n_x，n_z)，n′_YZ＝(n_y′，n_z′)，n′_XZ＝(n_x′，n_z′)

之后计算其反正切值R_X，R_X′，R_Y，R_Y′，对应相减得到俯仰角(pitch)和滚转角(roll)：

pitch＝R_X′-R_X

roll＝R_Y′-R_Y

步骤二：

使用最小二乘拟合平面的方法，求解点云里其余每个点的法向量n＝(n_x，n_y，n_z)，然后再将它们投影到XOY平面上，得到该点对应的投影向量n_XY＝(n_x，n_y)。之后得到计算每一个投影向量的反正切值R_z，得到z轴方向角R_Z的集合：

同理对源点云P_s进行计算得到z轴旋转角集合R′_Z。

步骤三：

此时，由于不知道两组点云的重叠部分是由哪一些对应点所构成的，也即没有点与点之间的对应关系，所以我们无法直接计算偏航角。于是，这里采用了统计方法，先对目标点云z轴方向角集合用直方图进行频数统计，取频数最大的区间的中值作为z轴方向角估计值：

这里的bin参数是分组数，依据点云的规模而定，这里根据点云数目取1000。

同理可得源点云的r′_Z ^bin，然后对两组点云的z轴方向角估计值作差，即可得偏航角：

得到欧拉角(偏航角、俯仰角和滚转角)之后，可以将其转换得到旋转矩阵R。

步骤四：

由于多数情况下，仅仅做一次统计和计算得到的旋转矩阵不一定能够完全配准，所以，这里使用迭代的方法，每计算一次偏航角便更新一次旋转矩阵，再用该矩阵变换源点云，然后对重复步骤三直至达到最大迭代次数或者旋转矩阵收敛(保持不变)。

值得注意的是，这个方案基于一个默认的前提，点云中平面点的法向量的朝向可以一定程度上代表点云的整体朝向，也即两组点云中都有较为明显的平面。所以，该方案不适用于共面点数量极少的点云数据。

第二部分，求解平移向量t，通过对数据进行降维处理，并运用相位相关算法计算得到。

步骤一：

将目标点云数据和源点云数据进行降维处理，分别投影到XOY、YOZ、ZOX平面，得到六个投影灰度图。具体做法是先将点坐标四舍五入取整，然后在原坐标基础上加上一个足够大的常量，将所有坐标值化为非负数，再在空白图像中进行描摹。

步骤二：

对投影图做相位相关计算，得到图像的平移量。相位相关算法基于傅里叶变换的位移定理，一个函数经过平移变换之后，其傅里叶变换仅仅是原函数的傅里叶变换与一个具有线性相位的指数因子的乘积，也即，函数在空间域中发生平移，会导致频域中频谱发生相移。该定理说明，函数在平移前后，互功率谱的相位等于两幅图像之间的相移。相位相关算法具体计算步骤如下：

1.对待处理的两幅图像ima1和ima2应用汉宁窗函数去除图像的边界效应；

2.求两幅图像的离散傅里叶变换：F₁[ima1],F₂[ima2]；

3.计算互功率谱G：

4.对互功率谱求傅里叶逆变换g：g＝DFT^-1[G]；

5.对r计算最大值的位置，即两幅图像的平移量。

以上步骤可以求得三组平移量，分别是：

目标点云与源点云俯视图对应平移量t_xy＝(t_x，t_y，0)

目标点云与源点云测视图对应平移量t_xz＝(t_x，0，t_z)

目标点云与源点云正视图对应平移量t_yz＝(0，t_y，t_z)

步骤三：将三组平移量取平均得到最终的平移向量t：

t＝0.5*(t_xy+t_xz+t_yz)

通过以上步骤，最终得到两组点云之间的变换矩阵(R，t)。

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (4)

1.一种适用于子地图重叠部分较少的地图融合方法，其特征在于，首先使用点云平面提取，法向量估计，法向量投影角统计的方法迭代计算旋转矩阵R，然后用R迭代变换源点云，最后对两组点云做三视图投影得到两组灰度图，对灰度图使用相位相关法计算平移向量t；最终得到两组点云之间的变换矩阵(R，t)；求解旋转矩阵R具体包括以下步骤：

S11.使用RANSAC算法提取目标点云P_t以及源点云p_s中的平面并得到各自对应的法向量n_t＝(n_x,n_y,n_z)以及n_s＝(n_x′,n_y′,n_z′)；然后由n_t和n_s分别对YOZ和ZOX平面作投影，得到四个向量，分别是：

n_t对YOZ平面投影向量n_YZ＝(n_y,n_z)；

n_t对ZOX平面投影向量n_XZ＝(n_x,n_z)；

n_s对YOZ平面投影向量n′_YZ＝(n_y′,n_z′)；

n_s对ZOX平面投影向量n′_XZ＝(n_x′,n_z′)；

之后计算通过反正切函数计算n_YZ绕X轴的旋转角R_X，n′_YZ绕X轴的旋转角R_X′，n_XZ绕Y轴的旋转角R_Y，n′_XZ绕Y轴的旋转角R_Y′，对应相减得到俯仰角pitch和滚转角roll：

pitch＝R_X′-R_X；

roll＝R_Y′-R_Y；

S12.使用最小二乘拟合平面的方法，求解点云里其余每个点的法向量n＝(n_x,n_y,n_z)，然后再将它们投影到XOY平面上，得到该点对应的投影向量n_XY＝(n_x,n_y)；之后计算每一个投影向量的反正切值R_Z，得到z轴方向角R_Z的集合：

同理对源点云P_s进行计算得到z轴旋转角集合R^′ _Z；

S13.采用统计的方法，先对目标点云z轴方向角集合用直方图进行频数统计，取频数最大的区间的中值作为z轴方向角估计值

式中的bin参数是分组数，依据点云的规模而定；

同理可得源点云的z轴方向角估计值

然后对两组点云的z轴方向角估计值作差，即可得偏航角yaw：

得到欧拉角：偏航角、俯仰角和滚转角之后，将其转换得到旋转矩阵R。

2.根据权利要求1所述的适用于子地图重叠部分较少的地图融合方法，其特征在于，使用迭代的方法，每计算一次偏航角便更新一次旋转矩阵，再用该矩阵变换源点云，然后重复步骤S13直至达到最大迭代次数或者旋转矩阵收敛。

3.根据权利要求1所述的适用于子地图重叠部分较少的地图融合方法，其特征在于，所述的计算平移向量t具体包括以下步骤：

S21.将目标点云数据和源点云数据进行降维处理，分别投影到XOY、YOZ、ZOX平面，得到六个投影灰度图；

S22.对投影图做相位相关计算，得到图像的平移量；包括以下步骤：

S221.对待处理的两幅图像ima1和ima2应用汉宁窗函数去除图像的边界效应；

S222.求两幅图像的离散傅里叶变换：F₁[ima1],F₂[ima2]；

S223.计算互功率谱G：

S224.对互功率谱求傅里叶逆变换g：g＝DFT^-1[G]；

S225.对r计算最大值的位置，即两幅图像的平移量；

S226.通过步骤S221至S225可以求得三组平移量，分别是：

目标点云与源点云俯视图对应平移量t_xy＝(t_x,t_y,0)

目标点云与源点云测视图对应平移量t_xz＝(t_x,0,t_z)

目标点云与源点云正视图对应平移量t_yz＝(0,t_y,t_z)

S23.将三组平移量取平均得到最终的平移向量t：

t＝0.5*(t_xy+t_xz+t_yz)。

4.根据权利要求3所述的适用于子地图重叠部分较少的地图融合方法，其特征在于，所述的S21步骤具体包括：先将点坐标四舍五入取整，然后在原坐标基础上加上一个足够大的常量，将所有坐标值化为非负数，再在空白图像中进行描摹，最终得到六个投影灰度图。

Images