CN112183285A - 一种变电站巡检机器人的3d点云地图融合方法和系统 - Google Patents

Abstract

本发明的一种变电站巡检机器人的3D点云地图融合方法和系统，包括以下步骤：在巡检机器人扫图前对待巡检场景进行局部区域划分，并对每个局部区域进行扫图路径规划；控制巡检机器人进行多个局部区域扫图，每个局部区域有闭环且检测到回环，融合的相邻区域存在共同部分；依次导入待配准的3D点云地图和参考3D点云地图，人工进行框选两幅地图中相同的区域；通过事先设置的融合算法对上述框选的两幅地图中相同的区域进行从粗到精的匹配，并得到融合地图。本发明的变电站巡检机器人的3D点云地图融合方法，可规避全局优化失败风险，实现10万平以上变电站场景的多局部3D点云地图高精度融合，并满足巡检机器人定位、导航。

Description

一种变电站巡检机器人的3D点云地图融合方法和系统

技术领域

本发明涉及智能机器人技术领域，具体涉及一种变电站巡检机器人的3D点云地图融合方法和系统。

背景技术

SLAM是轮式智能巡检机器人的核心功能之一，而3D激光SLAM是近年来该领域的研究热点，其代替2D激光SLAM也是近几年的趋势，变电站智能巡检机器人是其一个应用载体。500KV及以上变电站具有大尺度、巡检路线复杂、空旷及观测噪声大等典型特点。环境越大，需要数据采集和构建地图所需的时间越长，累计误差越大；巡检路线越复杂，对闭环检测的准确性要求越高；特征稀疏、观测噪声大，匹配的误差越大。上述因素对一次性构建完整地图提出了挑战，数据采集和构建地图的时间越长，SLAM可能会因为各种原因而失败，如操作条件的变化或硬件故障，但在故障发生前估计的部分地图可能是和实际环境观测一致且可用的。

现有的相关技术及缺点：

【1】CN105447911B，一种3D地图融合方法、装置及电子设备，主要将各待融合3D地图分别投影到预设的n个二维平面进行处理、融合、估计再获得3D地图；

【2】CN110704563A，地图融合方法及装置、设备、存储介质，主要为视觉点云地图，且通过视觉特征进行融合，融合后还需进行全局优化，当地图较多时，难以进行全局优化；

【3】CN110688440A，一种适用于子地图重叠部分较少的地图融合方法，用点云平面提取法向量、三视图投影方法进行地图融合，但该方案所述地图重合较多，且易出现误匹配，全局一致性上未表述，存在融合后在融合出定位精度较低的情况；

【4】CN110930495A，基于多无人机协作的ICP点云地图融合方法、系统、装置及存储介质，该方法主要利用SAC-IA进行估计对应点对，并筛选点对，然后采用ICP进行匹配，当两幅地图变换较大时，容易出现融合失败。

可采用地图融合技术来将不同时间段、不同机器人所构建的地图进行融合，离线构建高度完整和一致性的地图，满足机器人在大尺度、巡检路线复杂、空旷及大观测噪声的巡检场景下的应用。

发明内容

本发明提出的一种变电站巡检机器人的3D点云地图融合方法和系统，可实现同一场景下，多幅3D激光点云局部地图融合成该场景的完整地图，并且可用于机器人定位导航并满足定位精度。

为实现上述目的，本发明采用了以下技术方案：

一种变电站巡检机器人的3D点云地图融合方法，包括以下步骤：

S100、在巡检机器人扫图前对待巡检场景进行局部区域划分，并对每个局部区域进行扫图路径规划；

S200、控制巡检机器人进行多个局部区域扫图，每个局部区域有闭环且检测到回环，融合的相邻区域存在共同部分；

S300、依次导入待配准的3D点云地图和参考3D点云地图，人工进行框选两幅地图中相同的区域；

S400、通过事先设置的融合算法对上述框选的两幅地图中相同的区域进行从粗到精的匹配，并得到融合地图。

进一步的，还包括S500、对2个以上的3D点云地图融合，重复S300、S400得到待巡检场景的全局地图。

进一步的，所述S400中的融合算法步骤如下：

Step1：获取参考3D点云地图和待匹配3D点云地图，两幅地图的坐标系不同且包括了实际场景中相同的区域，该区域在参考3D点云地图记为R1，在待匹配3D点云地图记为R2；

Step2：对R1、R2中所有点求取该点及其k领域内每个点之间的三元组，计算得到简化点特征直方图；

Step3：分别对k领域中的每个点确定其k邻域，同Step2分别计算得到各自的简化点特征直方图；

Step4：对邻域中的各个简化点特征直方图进行加权，计算得到每个点的快速稳定点特征；

Step5：从R1中选取数量为k的采样点集M₁＝{m_1i|i＝1,2,...,k}，为尽量避免采样点集中存在相同的快速稳定点特征，对采样点对之间的距离设定最小距离阈值d；

Step6：从R2中选取数量为k的采样点集M₂＝{m_2i|i＝1,2,...,k}，对采样点对之间的距离设定最小距离阈值d；

Step7：对于特征点m_1i在M₂中的两个匹配点m_2j和m_2l，若

d(m_1i,m_2j)/d(m_1i,m_2l)>thre (1)

则m_1i和m_2l为相对可靠的匹配点对，否则m_1i在M₂中无匹配点，得到筛选后的匹配点集

其中，d(,)为点对的快速稳定点特征距离，thre为筛选阈值；

Step8：对于特征点m_2i在M₁中寻找匹配点方法同Step5，得到筛选后的匹配点集

Step9：在

中寻找相同的匹配对，即若在

中m_1i和m_2k为匹配对，在

中m_2k和m_1i为匹配对，则保留该匹配对，并遍历两集合中所有匹配对；

Step10：用上述配对点云集计算R1、R2之间刚体变换矩阵，并计算对应点变换后的距离误差和来评估当前配准变换的准确性；距离误差和函数使用Huber罚函数表示：

其中，

上式中，h_e为预先设定阈值，e_i为第i组对应点变换之后的距离差；

Step11：重复Step5至Step10，直至E最小，得到估计变换矩阵；

Step12：采用Levenberg-Marquardt算法对上步结果进行非线性局部优化，得到粗匹配阶段的初始变换矩阵G；

Step13：利用初始变换矩阵将R2进行坐标变换得到R2’，将R1、R2’作为精匹配的初始点集；

R2’＝R2*G；

Step14：设定参考点云和待配准点云的最优化矩阵的最小化误差目标函数：

其中，d为待配准点云顶点，s为参考点云顶点，n为待配准点云顶点的法向量，G和Gopt为4*4的3D刚体变换矩阵：

G＝T(t_x,t_y,t_z)·R(α,β,γ) (6)

其中，T为平移矩阵，R未旋转矩阵；

Step15：经过从粗到精的融合矩阵估计，由式step14得到最优融合矩阵Gopt，获得融合地图：

Map＝M_s+G_opt·M_t。

进一步的，所述Step14还包括：

要最小化(5)式，有t_x、t_y、t_z、α、β、γ6个参数，但后3个参数与非线性函数cos，sin有关难以求解，因此采取近似求解；

取角度θ≈0时，则sinθ≈θ，cosθ≈1，因此当α、β、γ约为0时，G可以近似为：

优化目标函数为：

其中，

如此，给定N个对应点，损失函数可以用矩阵的形式表示：

x_opt＝argmin_x|Ax-b|² (10)

再用SVD来求解线性方程组，可得：

x_opt＝A⁺b

其中，A⁺＝V∑⁺U^T，A＝U∑U^T，∑⁺是∑的伪逆。

进一步的，所述S400中的融合算法Step15之后还包括以下步骤Step16：由于融合后的地图在相似域区域的的点集分布不符合一定的分布特性，为能够为定位提供稳定的位姿估计，最后还需对融合相似域进行滤波：

Map′＝filter(Map)。

另一方面，本发明还公开一种变电站巡检机器人的3D点云地图融合系统，包括以下单元：

路径规划单元，用于在巡检机器人扫图前对待巡检场景进行局部区域划分，并对每个局部区域进行扫图路径规划；

扫图单元，用于控制巡检机器人进行多个局部区域扫图，每个局部区域有闭环且检测到回环，融合的相邻区域存在共同部分；

地图导入单元，用于依次导入待配准的3D点云地图和参考3D点云地图，人工进行框选两幅地图中相同的区域；

融合计算单元，用于通过融合算法对上述框选的两幅地图中相同的区域进行从粗到精的匹配，并得到融合地图。

进一步的，还包括以下子单元：

融合计算单元，用于通过以下步骤进行计算获得融合地图；步骤如下：

Step1：获取参考3D点云地图和待匹配3D点云地图，两幅地图的坐标系不同且包括了实际场景中相同的区域，该区域在参考3D点云地图记为R1，在待匹配3D点云地图记为R2；

Step2：对R1、R2中所有点求取该点及其k领域内每个点之间的三元组，计算得到简化点特征直方图；

Step3：分别对k领域中的每个点确定其k邻域，同Step2分别计算得到各自的简化点特征直方图；

Step4：对邻域中的各个简化点特征直方图进行加权，计算得到每个点的快速稳定点特征；

Step5：从R1中选取数量为k的采样点集M₁＝{m_1i|i＝1,2,...,k}，为尽量避免采样点集中存在相同的快速稳定点特征，对采样点对之间的距离设定最小距离阈值d；

Step6：从R2中选取数量为k的采样点集M₂＝{m_2i|i＝1,2,...,k}，对采样点对之间的距离设定最小距离阈值d；

Step7：对于特征点m_1i在M₂中的两个匹配点m_2j和m_2l，若

d(m_1i,m_2j)/d(m_1i,m_2l)>thre (1)

则m_1i和m_2l为相对可靠的匹配点对，否则m_1i在M₂中无匹配点，得到筛选后的匹配点集

其中，d(,)为点对的快速稳定点特征距离，thre为筛选阈值；

Step8：对于特征点m_2i在M₁中寻找匹配点方法同Step5，得到筛选后的匹配点集

Step9：在

中寻找相同的匹配对，即若在

中m_1i和m_2k为匹配对，在

中m_2k和m_1i为匹配对，则保留该匹配对，并遍历两集合中所有匹配对；

Step10：用上述配对点云集计算R1、R2之间刚体变换矩阵，并计算对应点变换后的距离误差和来评估当前配准变换的准确性；距离误差和函数使用Huber罚函数表示：

其中，

上式中，h_e为预先设定阈值，e_i为第i组对应点变换之后的距离差；

Step11：重复Step5至Step10，直至E最小，得到估计变换矩阵；

Step12：采用Levenberg-Marquardt算法对上步结果进行非线性局部优化，得到粗匹配阶段的初始变换矩阵G；

Step13：利用初始变换矩阵将R2进行坐标变换得到R2’，将R1、R2’作为精匹配的初始点集；

R2’＝R2*G；

Step14：设定参考点云和待配准点云的最优化矩阵的最小化误差目标函数：

其中，d为待配准点云顶点，s为参考点云顶点，n为待配准点云顶点的法向量，G和Gopt为4*4的3D刚体变换矩阵：

G＝T(t_x,t_y,t_z)·R(α,β,γ) (6)

其中，T为平移矩阵，R未旋转矩阵；

Step15：经过从粗到精的融合矩阵估计，由式step14得到最优融合矩阵Gopt，获得融合地图：

Map＝M_s+G_opt·M_t。

进一步的，还包括以下子单元：

优化单元，用于执行以下步骤：

取角度θ≈0时，则sinθ≈θ，cosθ≈1，因此当α、β、γ约为0时，G可以近似为：

优化目标函数为：

其中，

如此，给定N个对应点，损失函数可以用矩阵的形式表示：

x_opt＝argmin_x|Ax-b|² (10)

再用SVD来求解线性方程组，可得：

x_opt＝A⁺b

其中，A⁺＝V∑⁺U^T，A＝U∑U^T，∑⁺是∑的伪逆。

进一步的，还包括以下子单元：

滤波单元，用于对融合相似域进行滤波，计算公式如下：

Map′＝filter(Map)。

由上述技术方案可知，本发明通过针对存在相同区域的参考3D点云地图和待匹配3D点云地图，核心思想是基于上述相同区域，采用从粗到精的配准策略。在粗匹配阶段，进行稀疏数据、快速配准来计算初始变换矩阵；在精匹配阶段，利用上述初始变换矩阵并采用一种改进的ICP算法获得精确的融合矩阵。利用上述融合矩阵及参考3D点云地图和待匹配3D点云地图，融合得到整个巡检场景的3D点云地图。

本发明的变电站巡检机器人的3D点云地图融合方法和系统，可规避全局优化失败风险，实现10万平以上变电站场景的多局部3D点云地图高精度融合，并满足巡检机器人定位、导航。

附图说明

图1是本发明的方法流程图；

图2、3所示分别为本实施例3D参考点云地图和待融合点云地图的俯视图；

图4、图5分别为图2、图3对应的侧视图；

图6为本实施例在图2、3中选取的相同区域；

图7为本实施例基于同源域的从粗到精的匹配结果；

图8、图9为利用Gopt、图2和图3点云数据，融合得到的整张3D点云地图的俯视图和侧视图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。

如图1所示，本实施例所述的变电站巡检机器人的3D点云地图融合方法，包括：

S100、在巡检机器人扫图前对待巡检场景进行局部区域划分，并对每个局部区域进行扫图路径规划；

S200、控制巡检机器人进行多个局部区域扫图，每个局部区域有闭环且检测到回环，融合的相邻区域存在共同部分；

S300、依次导入待配准的3D点云地图和参考3D点云地图，人工进行框选两幅地图中相同的区域；

S400、通过事先设置的融合算法对上述框选的两幅地图中相同的区域进行从粗到精的匹配，并得到融合地图；

S500、对2个以上的3D点云地图融合，重复S300、S400得到待巡检场景的全局地图。

其中，所述S400中的融合算法步骤如下：

Step1：获取参考3D点云地图和待匹配3D点云地图，两幅地图的坐标系不同且包括了实际场景中相同的区域，该区域在参考3D点云地图记为R1，在待匹配3D点云地图记为R2；

Step2：对R1、R2中所有点求取该点及其k领域内每个点之间的三元组，计算得到简化点特征直方图；

Step3：分别对k领域中的每个点确定其k邻域，同Step2分别计算得到各自的简化点特征直方图；

Step4：对邻域中的各个简化点特征直方图进行加权，计算得到每个点的快速稳定点特征；

Step5：从R1中选取数量为k的采样点集M₁＝{m_1i|i＝1,2,...,k}，为尽量避免采样点集中存在相同的快速稳定点特征，对采样点对之间的距离设定最小距离阈值d；

Step6：从R2中选取数量为k的采样点集M₂＝{m_2i|i＝1,2,...,k}，对采样点对之间的距离设定最小距离阈值d；

Step7：对于特征点m_1i在M₂中的两个匹配点m_2j和m_2l，若

d(m_1i,m_2j)/d(m_1i,m_2l)>thre (1)

则m_1i和m_2l为相对可靠的匹配点对，否则m_1i在M₂中无匹配点，得到筛选后的匹配点集

其中，d(,)为点对的快速稳定点特征距离，thre为筛选阈值；

Step8：对于特征点m_2i在M₁中寻找匹配点方法同Step5，得到筛选后的匹配点集

Step9：在

中寻找相同的匹配对，即若在

中m_1i和m_2k为匹配对，在

中m_2k和m_1i为匹配对，则保留该匹配对，并遍历两集合中所有匹配对；

Step10：用上述配对点云集计算R1、R2之间刚体变换矩阵，并计算对应点变换后的距离误差和来评估当前配准变换的准确性。距离误差和函数使用Huber罚函数表示：

其中，

上式中，h_e为预先设定阈值，e_i为第i组对应点变换之后的距离差；

Step11：重复Step5至Step10，直至E最小，得到估计变换矩阵；

Step12：采用Levenberg-Marquardt算法对上步结果进行非线性局部优化，得到粗匹配阶段的初始变换矩阵G；

Step13：利用初始变换矩阵将R2进行坐标变换得到R2’，将R1、R2’作为精匹配的初始点集；

R2’＝R2*G； (4)

Step14：设定参考点云和待配准点云的最优化矩阵的最小化误差目标函数：

其中，d为待配准点云顶点，s为参考点云顶点，n为待配准点云顶点的法向量，G和Gopt为4*4的3D刚体变换矩阵：

G＝T(t_x,t_y,t_z)·R(α,β,γ) (6)

其中，T为平移矩阵，R未旋转矩阵。要最小化(5)式，有t_x、t_y、t_z、α、β、γ6个参数，但后3个参数与非线性函数cos，sin有关难以求解，因此采取近似求解。

取角度θ≈0时，则sinθ≈θ，cosθ≈1，因此当α、β、γ约为0时，G可以近似为：

优化目标函数为：

其中，

如此，给定N个对应点，损失函数可以用矩阵的形式表示：

x_opt＝argmin_x|Ax-b|² (10)

再用SVD来求解线性方程组，可得：

x_opt＝A⁺b

其中，A⁺＝V∑⁺U^T，A＝U∑U^T，∑⁺是∑的伪逆；

Step15：经过从粗到精的融合矩阵估计，由式step14得到最优融合矩阵Gopt，获得融合地图：

Map＝M_s+G_opt·M_t

Step16：由于融合后的地图在相似域区域的的点集分布不符合一定的分布特性，为能够为定位提供稳定的位姿估计，最后还需对融合相似域进行滤波：

Map′＝filter(Map)。

以下举例说明：

地图融合实验采用的是公司巡检机器人变电站模拟试验场地的3D点云地图。图2、3所示分别为3D参考点云地图和待融合点云地图的俯视图，图4、图5分别为对应的侧视图。由于在构建两幅地图时没有重合路径，实际场地相同环境的观测距离不同，因此相同区域的相似性比较低，主要表现为：1、同一区域的激光点云数量不同；2、垂直方向上的点云分布很不均衡。

根据实际场景，在图2、3中选取的相同区域如图6所示，点云数量分别为165321和53231，数据量及局部分布相差较大。

基于同源域的从粗到精的匹配结果如图7所示，匹配效果相当不错。在粗阶段的匹配得分为1.241(分值越高误差越大)，融合矩阵为：

在精匹配阶段的匹配得分为0.097，融合矩阵为：

利用Gopt、图2和图3点云数据，融合得到的整张3D点云地图的俯视图和侧视图如图8、9所示。基于该融合后的点云地图进行了机器人的定位导航测试，满足正常巡检需求。

综上所述，本发明采用从粗到精的匹配策略来稳定、快速地获取高精度融合地图；更加实用且更适用于变电站场景下3D点云地图的多地图融合，在两幅地图相对变化较大时仍可以稳定、快速融合。

另一方面，本发明还公开一种变电站巡检机器人的3D点云地图融合系统，包括以下单元：

路径规划单元，用于在巡检机器人扫图前对待巡检场景进行局部区域划分，并对每个局部区域进行扫图路径规划；

扫图单元，用于控制巡检机器人进行多个局部区域扫图，每个局部区域有闭环且检测到回环，融合的相邻区域存在共同部分；

地图导入单元，用于依次导入待配准的3D点云地图和参考3D点云地图，人工进行框选两幅地图中相同的区域；

融合计算单元，用于通过融合算法对上述框选的两幅地图中相同的区域进行从粗到精的匹配，并得到融合地图。

进一步的，还包括以下子单元：

融合计算单元，用于通过以下步骤进行计算获得融合地图；步骤如下：

Step1：获取参考3D点云地图和待匹配3D点云地图，两幅地图的坐标系不同且包括了实际场景中相同的区域，该区域在参考3D点云地图记为R1，在待匹配3D点云地图记为R2；

Step2：对R1、R2中所有点求取该点及其k领域内每个点之间的三元组，计算得到简化点特征直方图；

Step3：分别对k领域中的每个点确定其k邻域，同Step2分别计算得到各自的简化点特征直方图；

Step4：对邻域中的各个简化点特征直方图进行加权，计算得到每个点的快速稳定点特征；

Step5：从R1中选取数量为k的采样点集M₁＝{m_1i|i＝1,2,...,k}，为尽量避免采样点集中存在相同的快速稳定点特征，对采样点对之间的距离设定最小距离阈值d；

Step6：从R2中选取数量为k的采样点集M₂＝{m_2i|i＝1,2,...,k}，对采样点对之间的距离设定最小距离阈值d；

Step7：对于特征点m_1i在M₂中的两个匹配点m_2j和m_2l，若

d(m_1i,m_2j)/d(m_1i,m_2l)>thre (1)

则m_1i和m_2l为相对可靠的匹配点对，否则m_1i在M₂中无匹配点，得到筛选后的匹配点集

其中，d(,)为点对的快速稳定点特征距离，thre为筛选阈值；

Step8：对于特征点m_2i在M₁中寻找匹配点方法同Step5，得到筛选后的匹配点集

Step9：在

中寻找相同的匹配对，即若在

中m_1i和m_2k为匹配对，在

中m_2k和m_1i为匹配对，则保留该匹配对，并遍历两集合中所有匹配对；

Step10：用上述配对点云集计算R1、R2之间刚体变换矩阵，并计算对应点变换后的距离误差和来评估当前配准变换的准确性；距离误差和函数使用Huber罚函数表示：

其中，

上式中，h_e为预先设定阈值，e_i为第i组对应点变换之后的距离差；

Step11：重复Step5至Step10，直至E最小，得到估计变换矩阵；

Step12：采用Levenberg-Marquardt算法对上步结果进行非线性局部优化，得到粗匹配阶段的初始变换矩阵G；

Step13：利用初始变换矩阵将R2进行坐标变换得到R2’，将R1、R2’作为精匹配的初始点集；

R2’＝R2*G；

Step14：设定参考点云和待配准点云的最优化矩阵的最小化误差目标函数：

其中，d为待配准点云顶点，s为参考点云顶点，n为待配准点云顶点的法向量，G和Gopt为4*4的3D刚体变换矩阵：

G＝T(t_x,t_y,t_z)·R(α,β,γ) (6)

其中，T为平移矩阵，R未旋转矩阵；

Step15：经过从粗到精的融合矩阵估计，由式step14得到最优融合矩阵Gopt，获得融合地图：

Map＝M_s+G_opt·M_t。

进一步的，还包括以下子单元：

优化单元，用于执行以下步骤：

取角度θ≈0时，则sinθ≈θ，cosθ≈1，因此当α、β、γ约为0时，G可以近似为：

优化目标函数为：

其中，

如此，给定N个对应点，损失函数可以用矩阵的形式表示：

x_opt＝argmin_x|Ax-b|² (10)

再用SVD来求解线性方程组，可得：

x_opt＝A⁺b

其中，A⁺＝V∑⁺U^T，A＝U∑U^T，∑⁺是∑的伪逆。

进一步的，还包括以下子单元：

滤波单元，用于对融合相似域进行滤波，计算公式如下：

Map′＝filter(Map)。

可理解的是，本发明实施例提供的系统与本发明实施例提供的方法相对应，相关内容的解释、举例和有益效果可以参考上述方法中的相应部分。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (9)

1.一种变电站巡检机器人的3D点云地图融合方法，其特征在于，包括以下步骤：

S100、在巡检机器人扫图前对待巡检场景进行局部区域划分，并对每个局部区域进行扫图路径规划；

S200、控制巡检机器人进行多个局部区域扫图，每个局部区域有闭环且检测到回环，融合的相邻区域存在共同部分；

S300、依次导入待配准的3D点云地图和参考3D点云地图，人工进行框选两幅地图中相同的区域；

S400、通过事先设置的融合算法对上述框选的两幅地图中相同的区域进行从粗到精的匹配，并得到融合地图。

2.根据权利要求1所述的变电站巡检机器人的3D点云地图融合方法，其特征在于：还包括S500、对2个以上的3D点云地图融合，重复S300、S400得到待巡检场景的全局地图。

3.根据权利要求2所述的变电站巡检机器人的3D点云地图融合方法，其特征在于：所述S400中的融合算法步骤如下：

Step1：获取参考3D点云地图和待匹配3D点云地图，两幅地图的坐标系不同且包括了实际场景中相同的区域，该区域在参考3D点云地图记为R1，在待匹配3D点云地图记为R2；

Step2：对R1、R2中所有点求取该点及其k领域内每个点之间的三元组，计算得到简化点特征直方图；

Step3：分别对k领域中的每个点确定其k邻域，同Step2分别计算得到各自的简化点特征直方图；

Step4：对邻域中的各个简化点特征直方图进行加权，计算得到每个点的快速稳定点特征；

Step5：从R1中选取数量为k的采样点集M₁＝{m_1i|i＝1,2,...,k}，为尽量避免采样点集中存在相同的快速稳定点特征，对采样点对之间的距离设定最小距离阈值d；

Step6：从R2中选取数量为k的采样点集M₂＝{m_2i|i＝1,2,...,k}，对采样点对之间的距离设定最小距离阈值d；

Step7：对于特征点m_1i在M₂中的两个匹配点m_2j和m_2l，若

d(m_1i,m_2j)/d(m_1i,m_2l)>thre (1)

则m_1i和m_2l为相对可靠的匹配点对，否则m_1i在M₂中无匹配点，得到筛选后的匹配点集

其中，d(,)为点对的快速稳定点特征距离，thre为筛选阈值；

Step8：对于特征点m_2i在M₁中寻找匹配点方法同Step5，得到筛选后的匹配点集

Step9：在

中寻找相同的匹配对，即若在

中m_1i和m_2k为匹配对，在

中m_2k和m_1i为匹配对，则保留该匹配对，并遍历两集合中所有匹配对；

Step10：用上述配对点云集计算R1、R2之间刚体变换矩阵，并计算对应点变换后的距离误差和来评估当前配准变换的准确性；距离误差和函数使用Huber罚函数表示：

其中，

上式中，h_e为预先设定阈值，e_i为第i组对应点变换之后的距离差；

Step11：重复Step5至Step10，直至E最小，得到估计变换矩阵；

Step12：采用Levenberg-Marquardt算法对上步结果进行非线性局部优化，得到粗匹配阶段的初始变换矩阵G；

Step13：利用初始变换矩阵将R2进行坐标变换得到R2’，将R1、R2’作为精匹配的初始点集；

R2’＝R2*G； (4)

Step14：设定参考点云和待配准点云的最优化矩阵的最小化误差目标函数：

其中，d为待配准点云顶点，s为参考点云顶点，n为待配准点云顶点的法向量，G和Gopt为4*4的3D刚体变换矩阵：

G＝T(t_x,t_y,t_z)·R(α,β,γ) (6)

其中，T为平移矩阵，R未旋转矩阵；

Step15：经过从粗到精的融合矩阵估计，由式step14得到最优融合矩阵Gopt，获得融合地图：

Map＝M_s+G_opt·M_t。

4.根据权利要求3所述的变电站巡检机器人的3D点云地图融合方法，其特征在于：

所述Step14还包括：

要最小化(5)式，有t_x、t_y、t_z、α、β、γ6个参数，但后3个参数与非线性函数cos，sin有关难以求解，因此采取近似求解；

取角度θ≈0时，则sinθ≈θ，cosθ≈1，因此当α、β、γ约为0时，G可以近似为：

优化目标函数为：

其中，

如此，给定N个对应点，损失函数可以用矩阵的形式表示：

x_opt＝arg min_x|Ax-b|² (10)

再用SVD来求解线性方程组，可得：

x_opt＝A⁺b

其中，A⁺＝V∑⁺U^T，A＝U∑U^T，∑⁺是∑的伪逆。

5.根据权利要求3所述的变电站巡检机器人的3D点云地图融合方法，其特征在于：所述S400中的融合算法Step15之后还包括以下步骤

Step16：由于融合后的地图在相似域区域的的点集分布不符合一定的分布特性，为能够为定位提供稳定的位姿估计，最后还需对融合相似域进行滤波：

Map′＝filter(Map)。

6.一种变电站巡检机器人的3D点云地图融合系统，其特征在于：包括以下单元：

路径规划单元，用于在巡检机器人扫图前对待巡检场景进行局部区域划分，并对每个局部区域进行扫图路径规划；

扫图单元，用于控制巡检机器人进行多个局部区域扫图，每个局部区域有闭环且检测到回环，融合的相邻区域存在共同部分；

地图导入单元，用于依次导入待配准的3D点云地图和参考3D点云地图，人工进行框选两幅地图中相同的区域；

融合计算单元，用于通过融合算法对上述框选的两幅地图中相同的区域进行从粗到精的匹配，并得到融合地图。

7.根据权利要求6所述的一种变电站巡检机器人的3D点云地图融合系统，其特征在于：还包括以下子单元：

融合计算单元，用于通过以下步骤进行计算获得融合地图；步骤如下：

Step1：获取参考3D点云地图和待匹配3D点云地图，两幅地图的坐标系不同且包括了实际场景中相同的区域，该区域在参考3D点云地图记为R1，在待匹配3D点云地图记为R2；

Step2：对R1、R2中所有点求取该点及其k领域内每个点之间的三元组，计算得到简化点特征直方图；

Step3：分别对k领域中的每个点确定其k邻域，同Step2分别计算得到各自的简化点特征直方图；

Step4：对邻域中的各个简化点特征直方图进行加权，计算得到每个点的快速稳定点特征；

Step5：从R1中选取数量为k的采样点集M₁＝{m_1i|i＝1,2,...,k}，为尽量避免采样点集中存在相同的快速稳定点特征，对采样点对之间的距离设定最小距离阈值d；

Step6：从R2中选取数量为k的采样点集M₂＝{m_2i|i＝1,2,...,k}，对采样点对之间的距离设定最小距离阈值d；

Step7：对于特征点m_1i在M₂中的两个匹配点m_2j和m_2l，若

d(m_1i,m_2j)/d(m_1i,m_2l)>thre (1)

则m_1i和m_2l为相对可靠的匹配点对，否则m_1i在M₂中无匹配点，得到筛选后的匹配点集

其中，d(,)为点对的快速稳定点特征距离，thre为筛选阈值；

Step8：对于特征点m_2i在M₁中寻找匹配点方法同Step5，得到筛选后的匹配点集

Step9：在

中寻找相同的匹配对，即若在

中m_1i和m_2k为匹配对，在

中m_2k和m_1i为匹配对，则保留该匹配对，并遍历两集合中所有匹配对；

Step10：用上述配对点云集计算R1、R2之间刚体变换矩阵，并计算对应点变换后的距离误差和来评估当前配准变换的准确性；距离误差和函数使用Huber罚函数表示：

其中，

上式中，h_e为预先设定阈值，e_i为第i组对应点变换之后的距离差；

Step11：重复Step5至Step10，直至E最小，得到估计变换矩阵；

Step12：采用Levenberg-Marquardt算法对上步结果进行非线性局部优化，得到粗匹配阶段的初始变换矩阵G；

Step13：利用初始变换矩阵将R2进行坐标变换得到R2’，将R1、R2’作为精匹配的初始点集；

R2’＝R2*G；

Step14：设定参考点云和待配准点云的最优化矩阵的最小化误差目标函数：

其中，d为待配准点云顶点，s为参考点云顶点，n为待配准点云顶点的法向量，G和Gopt为4*4的3D刚体变换矩阵：

G＝T(t_x,t_y,t_z)·R(α,β,γ) (6)

其中，T为平移矩阵，R未旋转矩阵；

Step15：经过从粗到精的融合矩阵估计，由式step14得到最优融合矩阵Gopt，获得融合地图：

Map＝M_s+G_opt·M_t。

8.根据权利要求7所述的一种变电站巡检机器人的3D点云地图融合系统，其特征在于：还包括以下子单元：

优化单元，用于执行以下步骤：

取角度θ≈0时，则sinθ≈θ，cosθ≈1，因此当α、β、γ约为0时，G可以近似为：

优化目标函数为：

其中，

如此，给定N个对应点，损失函数可以用矩阵的形式表示：

x_opt＝arg min_x|Ax-b|² (10)

再用SVD来求解线性方程组，可得：

x_opt＝A⁺b

其中，A⁺＝V∑⁺U^T，A＝U∑U^T，∑⁺是∑的伪逆。

9.根据权利要求7所述的一种变电站巡检机器人的3D点云地图融合系统，其特征在于：还包括以下子单元：

滤波单元，用于对融合相似域进行滤波，计算公式如下：

Map′＝filter(Map)。

Images