CN110223345A - 基于点云的配电线路作业对象位姿估计方法 - Google Patents

Abstract

本发明公开了一种基于点云的配电线路作业对象位姿估计方法，包括以下步骤：采集包括待测对象在内的作业场景的点云数据；对点云进行裁剪；将点云之间的平均距离设定为置信区间，之后剔除置信区间之外的点云；对点云进行语义分割，获得作业对象点云，作为待配准点云集P；建立待估计位姿的作业对象的三维模型，并将其转换为点云的PCD格式，由此构建待估计位姿的作业对象的点云模型，将其作为参考点云集Q；对待配准点云集P和参考点云集Q进行粗配准使两者参考坐标系一致，获得作业对象的初始位姿；对初始位姿进行修正，获得最终位姿。本发明在背景比较纷乱的配电线路中也能快速且精确地获得作业对象的位姿测量结果，并对光照变化具有较强的鲁棒性。

Description

基于点云的配电线路作业对象位姿估计方法

技术领域

本发明属于带电作业机器人作业对象位姿测量领域，特别是一种基于点云的配电线路作业对象位姿估计方法。

背景技术

随着机器人技术的蓬勃发展，机器人在现代生产生活中的地位越来越重要。将带电作业机器人引入到电力产业中，代替人工进行电力维护检修工作，可以有效避免带电作业时人员伤亡事故的发生，并且能极大提高电力维护检修的作业效率。

目前国内外研发出的应用于配电线路维护的带电作业机器人，大多需要作业人员在高空或借助视频监控，遥操作机械臂执行作业任务。目标的定位精度低，导致对于需要精确定位的操作例如更换避雷器、接搭引线等操作难度巨大，因此作业对象位姿的测量十分必要。然而，在带电作业现场，环境较为复杂，设备器具较多，且这些设备器具颜色单一，不易与背景环境区分，这些因素造成了目标位姿测量的困难。目前常用的位姿测量方法例如针对RGBD图像的基于LINEMOD的方法，其利用稳定的梯度信息和法向量特征得到了较好的位姿估计方法，但对光照条件的鲁棒性不强。当场景中的光照变化时，目标的颜色分布也会发生变化，从而导致模型的不稳定和目标位姿测量的失败。

发明内容

本发明的目的在于提供一种对光照变化鲁棒性好、实时性好的配电线路作业对象位姿估计方法。

实现本发明目的的技术解决方案为：基于点云的配电线路作业对象位姿估计方法，包括以下步骤：

步骤1、由深度相机采集包括待测对象在内的作业场景的点云数据；

步骤2、对步骤1的点云进行裁剪以减少点云的数量；

步骤3、求取点云之间的平均距离，并将该平均距离设定为置信区间，之后剔除置信区间之外的点云；

步骤4、对步骤3处理后的点云进行语义分割以将作业对象从复杂的环境中分割开来，获得作业对象点云；

步骤5、利用建模软件建立待估计位姿的作业对象的三维模型，并将其转换为点云的PCD格式，由此构建待估计位姿的作业对象的点云模型，将其作为参考点云集Q；

步骤6、将所述作业对象点云作为待配准点云集P，对待配准点云集P和参考点云集Q进行粗配准使两者的参考坐标系一致，获得作业对象的初始位姿；

步骤7、对所述初始位姿进行修正，获得作业对象的最终位姿。

本发明与现有技术相比，其显著优点为：1)采用三维视觉对非结构化环境下的作业目标位姿进行测量，针对背景比较纷乱的配电线路作业对象也能获得较精确的位姿测量结果，并对光照变化具有较强的鲁棒性；2)对PointNet进行了改进，先基于Stanford 3DIndoor Dataset预训练PointNet，再通过自定义的数据库微调网络，提高了参数训练的速度，进而提高了整个方法的效率；3)采用PCA结合改进ICP的配准算法对作业对象位姿进行估计，能有效解决经典ICP算法易陷入局部最优的问题，同时保证了配准的实时性。

下面结合附图对本发明作进一步详细的描述。

附图说明

图1为本发明基于点云的配电线路作业对象位姿估计的流程图。

图2为本发明中PointNet语义分割网络结构图。

图3为本发明实施例中的点云图，其中图(a)为深度相机采集到的配电线路维护作业场景的原始点云图，图(b)为经过感兴趣区域选择后的点云图，图(c)为经过自适应体素栅格滤波后的点云图，图(d)为经过离群点去除的点云图。

图4为本发明实施例中作业场景的语义分割结果示意图。

图5为本发明实施例中避雷器的参考点云模型示意图。

图6为本发明实施例中初始变换前待配准的避雷器点云和参考避雷器的点云图。

图7为本发明实施例中初始变换后待配准的避雷器点云和参考避雷器的点云图。

图8为本发明实施例中采用精配准后的待配准的避雷器点云和参考避雷器的点云图。

具体实施方式

结合图1，本发明基于点云的配电线路作业对象位姿估计方法，包括以下步骤：

步骤1、由深度相机采集包括测量对象在内的作业场景的点云数据；

步骤2、对步骤1的点云进行裁剪以减少点云的数量；

步骤3、求取点云之间的平均距离，并将该平均距离设定为置信区间，之后剔除置信区间之外的点云，忽略干扰信息，解决采集到的点云数据存在离群点的问题；

步骤4、对步骤3处理后的点云进行语义分割以将作业对象从复杂的环境中分割开来，获得作业对象点云；

步骤5、利用建模软件建立待估计位姿的作业对象的三维模型，并将其转换为点云的PCD格式，由此构建待估计位姿的作业对象的点云模型，并将其作为参考点云集Q；

步骤6、将所述作业对象点云作为待配准点云集P，对待配准点云集P和参考点云集Q进行粗配准使两者的参考坐标系一致，获得作业对象的初始位姿；

步骤7、对所述初始位姿进行修正，获得作业对象的最终位姿。

进一步地，步骤2中对步骤1的点云进行裁剪以减少点云的数量，进而满足目标点云位姿估计的实时性要求，具体为：

步骤2-1、通过条件滤波算法将整个视野范围内的点云裁剪至只保留感兴趣区域即包含作业对象周围区域的点云；

步骤2-2、在步骤2-1的基础上，在不影响特征提取的情况下采用自适应体素栅格法进一步精简点云的数量。当点云分辨率较小时，减小栅格的尺寸，防止精简后的点云过于稀疏，不利于后续的特征提取，当点云分辨率较大时，相应地增大栅格的大小，实现点云数量的有效减小。

进一步优选地，步骤3中求取点云之间的平均距离，具体采用kd-tree的方式实现。

进一步地，步骤4中对点云进行语义分割，具体采用改进PointNet深度神经网络对点云进行语义分割，点云语义分割是指对点云中每个点进行分类，实现按类别分割。PointNet的整体架构如图2所示，其输入为一个个block，每个block中有N个3D点，经过MLP网络学习每个3D点的D1维局部点特征，并通过Max Pooling层计算该block的D2维全局特征。最后将D1维局部特征和D2维全局特征进行融合并作MLP处理，输出block中每个点语义标签的得分(MLP表示多层感知，M表示Max Pooling层，C表示融合的操作)。本步骤具体为：

步骤4-1、利用点云标注***进行点云的快速标注；

步骤4-2、基于Stanford 3D Indoor Dataset预训练PointNet，设置block大小为b₁×b₁，步长为l₁，输入的点云数量N＝n₁，点云特征维数D＝d，用标准差为σ的高斯分布数值初始化各层参数；

步骤4-3、基于步骤4-2预训练模型的参数，固定前三层卷积层，输入作业对象的样本继续训练，设置block大小为b₂×b₂，步长为l₂，N＝n₂，D＝d，输出点云数据；其中b₂＜b₁，l₂＜l₁，n₂＜n₁。

示例性地，步骤4-2中b₁×b₁＝1×1m²，l₁＝0.5m，N＝n₁＝4096，D＝9，σ＝0.001；步骤4-3中b₂×b₂＝0.1×0.1m²，l₂＝0.05m，N＝n₂＝128。

进一步优选地，步骤6中对待配准点集P和参考点云集Q进行粗配准使两者的参考坐标系一致，具体采用PCA主成分分析法实现。针对空间中的待配准点云集P及参考点云集Q，分别求取其点云的重心和协方差，以重心作为点集的坐标原点，协方差矩阵的特征向量为三坐标轴，建立两个点云各自的参考坐标系。将参考点云与待估计点云的参考坐标系调整到一致，即达到粗配准的目的。

进一步地，步骤7中对初始位姿进行修正，具体为：采用改进ICP算法对初始位姿进行修正。其中，ICP算法的理论依据是：假设给定两幅点云的三维数据，即两个三维点集P＝{p₁,p₂,...,p_n}和Q＝{q₁,q₂,...,q_n}，其中P是待配准点云集，Q是参考点云集。待配准点云集P会在迭代的过程中不断进行刚体变换，逐渐逼近参考点云集Q。采用ICP算法的目的就是寻找一个刚体变换矩阵(包括旋转矩阵R和平移矩阵t)，使得变换后的P与Q的配准误差函数最小。ICP算法需要完成两方面的工作：①根据最近原则获取点集的对应关系；②根据对应关系计算刚体变换矩阵。本步骤利用改进ICP算法对初始位姿进行修正具体为：

步骤7-1、将初始位姿作为刚体变换矩阵[R|t]的初始值；其中R为旋转矩阵、t为平移矩阵，刚体变换矩阵[R|t]即为作业对象位姿；

步骤7-2、建立待配准点云集P与参考点云集Q对应关系：

步骤7-2-1、获取待配准点云集P与参考点云集Q的曲率几何特征，根据曲率值的大小分别对待配准点云集P、参考点云集Q中的点云进行分类；

步骤7-2-2、逐个扫描待配准点云集P中的每个点云，其中的每个点记为待查询点，根据该点所属分类，在相同分类的参考点云中查找曲率相似度高的若干点作为候选点，候选点的选取条件为：

式中，ε₁和ε₂为设定的初始阈值，p_i表示待配准点云集P中第i个点，q_j表示参考点云集Q中的第j个点，k₁(p_i)、k₁(q_j)分别表示p_i、q_j的主曲率，k₂(p_i)、k₂(q_j)分别表示p_i、q_j的法矢量；

步骤7-2-3、查找每个候选点的K邻域，将其中与待查询点距离最近的邻域点与待查询点组合形成点对；

步骤7-3、去除误匹配点对，具体为：根据p_i和q_j的距离与d_t的关系去除误匹配点对，若p_i和q_j的距离超过d_t，则该点对为误匹配，将其移除；其中d_t为距离阈值；

步骤7-4、求解刚体变换矩阵[R|t]即作业对象位姿，所用公式为：

步骤7-5、根据步骤7-4求得的R和t更新待配准点云集P，具体为：判断E(R,t)与设定阈值p1的关系，若E(R,t)小于p1，则直接输出作业对象的最终位姿[R|t]；反之，则返回步骤7-2，直至E(R,t)小于p1或迭代次数大于设定的最大迭代次数C，之后输出作业对象的最终位姿[R|t]。

实施例

本实施例中待估计位姿的作业对象为避雷器，由本发明方法进行位姿估计，包括以下过程：

1、由深度相机采集包括测量对象在内的作业场景的点云图像如图3(a)所示。

2、对上述1中的点云进行裁剪以减少点云的数量，经过感兴趣区域裁剪和自适应体素栅格滤波后的点云图像分别如图3(b)、3(c)所示；

3、求取点云之间的平均距离，并将该平均距离设定为置信区间，剔除置信区间之外的点云，处理后的点云图像如图3(d)所示；

4、对上述3处理后的点云进行语义分割，将作业对象从复杂的环境中分割开来，获得作业对象点云，语义分割结果如图4所示；

5、利用建模软件建立待估计位姿的作业对象的三维模型，并将其转换为点云的PCD格式，由此构建待估计位姿的作业对象的点云模型，并将其作为参考点云集Q，避雷器的点云模型如图5所示；

6、将上述作业对象点云作为待配准点云集P，对待配准点云集P和参考点云集Q进行粗配准，使两者的参考坐标系一致，获得作业对象的初始位姿，结果如下式所示：

式中，T₁为求得的初始位姿，经过初始位姿T₁的矩阵变换前、后待估计位姿避雷器点云和参考点云的位置关系分别如图6和图7所示；

7、对初始位姿进行修正，获得变换矩阵如下式所示：

由T₁和T₂可得避雷器最终的点云位姿为：

应用矩阵T对避雷器的点云进行坐标系变换后，待估计避雷器点云和参考点云的位置关系图8所示。

本发明提出的在非结构化的配电线路环境下作业对象的位姿估计方法，用深度相机采集作业场景的点云数据，并对其进行预处理、作业对象分割和作业对象的位姿估计，保证在背景比较纷乱的配电线路中也能快速且精确地获得作业对象的位姿测量结果，并对光照变化具有较强的鲁棒性。

Claims (7)

1.一种基于点云的配电线路作业对象位姿估计方法，其特征在于，包括以下步骤：

步骤1、由深度相机采集包括待测作业对象在内的作业场景的点云数据；

步骤2、对步骤1的点云进行裁剪以减少点云的数量；

步骤3、求取点云之间的平均距离，并将该平均距离设定为置信区间，之后剔除置信区间之外的点云；

步骤4、对步骤3处理后的点云进行语义分割以将作业对象从复杂的环境中分割开来，获得作业对象点云；

步骤5、利用建模软件建立待估计位姿的作业对象的三维模型，并将其转换为点云的PCD格式，由此构建待估计位姿的作业对象的点云模型，将其作为参考点云集Q；

步骤6、将所述作业对象点云作为待配准点云集P，对待配准点云集P和参考点云集Q进行粗配准使两者的参考坐标系一致，获得作业对象的初始位姿；

步骤7、对所述初始位姿进行修正，获得作业对象的最终位姿。

2.根据权利要求1所述的基于点云的配电线路作业对象位姿估计方法，其特征在于，步骤2所述对步骤1的点云进行裁剪以减少点云的数量，具体为：

步骤2-1、通过条件滤波算法将整个视野范围内的点云裁剪至只保留感兴趣区域即包含作业对象周围区域的点云；

步骤2-2、在步骤2-1的基础上，在不影响特征提取的情况下采用自适应体素栅格法进一步精简点云的数量。

3.根据权利要求1或2所述的基于点云的配电线路作业对象位姿估计方法，其特征在于，步骤3所述求取点云之间的平均距离，具体采用kd-tree的方式实现。

4.根据权利要求3所述的基于点云的配电线路作业对象位姿估计方法，其特征在于，步骤4所述对点云进行语义分割，具体采用改进PointNet深度神经网络对点云进行语义分割，具体为：

步骤4-1、利用点云标注***进行点云的快速标注；

步骤4-2、基于Stanford 3D Indoor Dataset预训练PointNet，设置block大小为b₁×b₁，步长为l₁，输入的点云数量N＝n₁，点云特征维数D＝d，用标准差为σ的高斯分布数值初始化各层参数；

步骤4-3、基于步骤4-2预训练模型的参数，固定前三层卷积层，输入作业对象的样本继续训练，设置block大小为b₂×b₂，步长为l₂，N＝n₂，D＝d，输出点云数据；其中b₂＜b₁，l₂＜l₁，n₂＜n₁。

5.根据权利要求4所述的基于点云的配电线路作业对象位姿估计方法，其特征在于，步骤4-2中b₁×b₁＝1×1m²，l₁＝0.5m，N＝n₁＝4096，D＝9，σ＝0.001；步骤4-3中b₂×b₂＝0.1×0.1m²，l₂＝0.05m，N＝n₂＝128。

6.根据权利要求5所述的基于点云的配电线路作业对象位姿估计方法，其特征在于，步骤6所述对待配准点集P和参考点云集Q进行粗配准使两者的参考坐标系一致，具体采用PCA主成分分析法实现。

7.根据权利要求6所述的基于点云的配电线路作业对象位姿估计方法，其特征在于，步骤7所述对初始位姿进行修正，具体为：采用改进ICP算法对初始位姿进行修正：

步骤7-1、将初始位姿作为刚体变换矩阵[R|t]的初始值；其中R为旋转矩阵、t为平移矩阵，刚体变换矩阵[R|t]即为作业对象位姿；

步骤7-2、建立待配准点云集P与参考点云集Q对应关系：

步骤7-2-1、获取待配准点云集P与参考点云集Q的曲率几何特征，根据曲率值的大小分别对待配准点云集P、参考点云集Q中的点云进行分类；

步骤7-2-2、逐个扫描待配准点云集P中的每个点云，其中的每个点记为待查询点，根据该点所属分类，在相同分类的参考点云中查找曲率相似度高的若干点作为候选点，候选点的选取条件为：

式中，ε₁和ε₂为设定的初始阈值，p_i表示待配准点云集P中第i个点，q_j表示参考点云集Q中的第j个点，k₁(p_i)、k₁(q_j)分别表示p_i、q_j的主曲率，k₂(p_i)、k₂(q_j)分别表示p_i、q_j的法矢量；

步骤7-2-3、查找每个候选点的K邻域，将其中与待查询点距离最近的邻域点与待查询点组合形成点对；

步骤7-3、去除误匹配点对，具体为：根据p_i和q_j的距离与d_t的关系去除误匹配点对，若p_i和q_j的距离超过d_t，则该点对为误匹配，将其移除；其中d_t为距离阈值；

步骤7-4、求解刚体变换矩阵[R|t]即作业对象位姿，所用公式为：

步骤7-5、根据步骤7-4求得的R和t更新待配准点云集P，具体为：判断E(R,t)与设定阈值p1的关系，若E(R,t)小于p1，则直接输出作业对象的最终位姿[R|t]；反之，则返回步骤7-2，直至E(R,t)小于p1或迭代次数大于设定的最大迭代次数C，之后输出作业对象的最终位姿[R|t]。