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

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

【技术实现步骤摘要】
一种变电站巡检机器人的3D点云地图融合方法和系统
本专利技术涉及智能机器人
，具体涉及一种变电站巡检机器人的3D点云地图融合方法和系统。
技术介绍
SLAM是轮式智能巡检机器人的核心功能之一，而3D激光SLAM是近年来该领域的研究热点，其代替2D激光SLAM也是近几年的趋势，变电站智能巡检机器人是其一个应用载体。500KV及以上变电站具有大尺度、巡检路线复杂、空旷及观测噪声大等典型特点。环境越大，需要数据采集和构建地图所需的时间越长，累计误差越大；巡检路线越复杂，对闭环检测的准确性要求越高；特征稀疏、观测噪声大，匹配的误差越大。上述因素对一次性构建完整地图提出了挑战，数据采集和构建地图的时间越长，SLAM可能会因为各种原因而失败，如操作条件的变化或硬件故障，但在故障发生前估计的部分地图可能是和实际环境观测一致且可用的。现有的相关技术及缺点：【1】CN105447911B，一种3D地图融合方法、装置及电子设备，主要将各待融合3D地图分别投影到预设的n个二维平面进行处理、融合、估计再获得3D地图；【2】CN110704本文档来自技高网...

【技术保护点】
1.一种变电站巡检机器人的3D点云地图融合方法，其特征在于，包括以下步骤：/nS100、在巡检机器人扫图前对待巡检场景进行局部区域划分，并对每个局部区域进行扫图路径规划；/nS200、控制巡检机器人进行多个局部区域扫图，每个局部区域有闭环且检测到回环，融合的相邻区域存在共同部分；/nS300、依次导入待配准的3D点云地图和参考3D点云地图，人工进行框选两幅地图中相同的区域；/nS400、通过事先设置的融合算法对上述框选的两幅地图中相同的区域进行从粗到精的匹配，并得到融合地图。/n

【技术特征摘要】
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的采样点集M1＝{m1i|i＝1,2,...,k}，为尽量避免采样点集中存在相同的快速稳定点特征，对采样点对之间的距离设定最小距离阈值d；
Step6：从R2中选取数量为k的采样点集M2＝{m2i|i＝1,2,...,k}，对采样点对之间的距离设定最小距离阈值d；
Step7：对于特征点m1i在M2中的两个匹配点m2j和m2l，若
d(m1i,m2j)/d(m1i,m2l)>thre(1)
则m1i和m2l为相对可靠的匹配点对，否则m1i在M2中无匹配点，得到筛选后的匹配点集其中，d(,)为点对的快速稳定点特征距离，thre为筛选阈值；
Step8：对于特征点m2i在M1中寻找匹配点方法同Step5，得到筛选后的匹配点集
Step9：在中寻找相同的匹配对，即若在中m1i和m2k为匹配对，在中m2k和m1i为匹配对，则保留该匹配对，并遍历两集合中所有匹配对；
Step10：用上述配对点云集计算R1、R2之间刚体变换矩阵，并计算对应点变换后的距离误差和来评估当前配准变换的准确性；距离误差和函数使用Huber罚函数表示：



其中，



上式中，he为预先设定阈值，ei为第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(tx,ty,tz)·R(α,β,γ)(6)
其中，T为平移矩阵，R未旋转矩阵；
Step15：经过从粗到精的融合矩阵估计，由式step14得到最优融合矩阵Gopt，获得融合地图：
Map＝Ms+Gopt·Mt。


4.根据权利要求3所述的变电站巡检机器人的3D点云地图融合方法，其特征在于：
所述Step14还包括：
要最小化(5)式，有tx、ty、tz、α、β、γ6个参数，但后3个参数与非线性函数cos，sin有关难以求解，因此采取近似求解；
取角度θ≈0时，则sinθ≈θ，cosθ≈1，因此当α、β、γ约为0时，G可以近似为：



优化目标函数为：



其中，



如此，给定N个对应点，损失函数可以用矩阵的形式表示：
xopt＝argminx|Ax-b|2(10)
再用SVD来求解线性方程组，可得：
xopt＝A+b
其中，A+＝V∑+UT，A＝U∑UT，∑+是∑的伪逆。


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

【专利技术属性】
技术研发人员：田定胜，章海兵，汪中原，褚衍超，
申请(专利权)人：合肥科大智能机器人技术有限公司，科大智能科技股份有限公司，科大智能电气技术有限公司，
类型：发明
国别省市：安徽;34