CN115202355A - 电力杆塔作业平台的控制方法及系统 - Google Patents

Abstract

本发明公开一种电力杆塔作业平台的控制方法及系统，属于作业机器人导航技术领域，所述方法包括：分别对电力杆塔作业平台和电力杆塔进行建模，得到电力杆塔作业平台三维模型和电力杆塔三维模型；将电力杆塔作业平台三维模型和电力杆塔三维模型进行耦合，产生控制指令；向电力杆塔作业平台发送所述控制指令。本发明通过建立电力杆塔作业平台的三维模型和电力杆塔的三维模型，将电力杆塔作业平台和电力杆塔三维模型进行耦合，产生控制指令，从而控制电力杆塔作业平台实现自主开展行进任务。

Description

电力杆塔作业平台的控制方法及系统

技术领域

本发明涉及作业机器人导航技术领域，具体涉及一种电力杆塔作业平台的控制方法及系统。

背景技术

随着我国电力工业的稳步发展以及我国经济建设对大量输配电线路的迫切需求，输电技术在社会发展中的地位日益上升。而高压电力杆塔包括角钢塔、钢管塔等，是高压输电线路的一项重要组成部分。在我国，高压电力杆塔数量多、分布广，且长期显露于野外环境等恶劣环境下。传统方式下，需由检修人员背负检修设备进行检修，这种方法费时费力，甚至可能会威胁到电力工人的生命安全。因此，针对电力杆塔作业环境下的作业机器人的研发应用而生，成为科研的一大热点。

目前，国内现有的针对电力杆塔作业环境下的作业机器人的研究成果较少，相关技术中，公布号为CN112906118A的中国发明专利申请公开了一种虚实耦合空间下的建筑机器人远程作业方法，实现步骤包括S1、建立建筑物虚拟模型，从建筑物虚拟模型中提取建筑物实体的位置形状信息；S2、获取施工场景点云，并进行降采样和滤波处理得到滤波后的点云，对滤波后的点云进行分割处理，得到施工场景每个房间的点云以及障碍物点云，施工场景每个房间的点云以及障碍物点云共同构成环境点云；S3、对建筑物虚拟模型和环境点云进行配准，得到虚实耦合模型；S4、通过Socket通讯协议建立虚拟建筑机器人和真实建筑机器人的远程通讯，实现远程作业。通过操纵虚拟建筑机器人进行作业，施工场景中的真实建筑机器人则同步作业，不需要施工人员在现场监视，保证了施工人员的安全。

该方案是适应于建筑技术领域，其所建立的建筑物虚拟模型为BIM模型，包括建筑物位置、形状在内的建筑信息，模型简单粗陋，因此需获取施工场景点云数据与建筑物虚拟模型进行配准，得到虚实耦合模型；另外由于建筑机器人的自由度较高，需要采用Omega.7力反馈手柄控制虚实耦合环境中虚拟建筑机器人的动作，并在本地计算机利用逆运动学计算出虚拟建筑机器人各个关节的转动角度，将各个关节的转动角度封装为控制指令，发送至真实建筑机器人的控制系统，控制真实建筑机器人运动。

但该现有方案并不能用于电力杆塔，首先，电力杆塔空间结构规模较大，且具有较高高度，采用点云数据形成虚实耦合模型的精度在±5cm左右，但在电力杆塔作业环境下，电力杆塔虚拟模型的精度需控制在1mm甚至0.1mm以内；其次，该现有方案在虚实耦合环境中的虚拟建筑机器人末端触碰到障碍物时会获得碰撞检测数据，并且会有阻力反馈给Omega.7力反馈手柄，阻止虚拟建筑机器人继续触碰障碍物；而应用于电力杆塔作业环境下的作业机器人需绝对避开障碍物，以避免障碍物对作业机器人的抓手造成损坏；再次，作业机器人在电力杆塔行进至一定高度后，定位不准确，且实际场景中各种状态信息难以及时获取，反馈难度大，机器人实体和机器人虚拟模型作业难以保持一致。

进一步地，由于电力杆塔的规划空间结构和所需处理数据量较大，在实际应用中，电力杆塔作业机器人存在越障能力不足、自主步态规划算法低效等问题，电力杆塔作业机器人轨迹规划算法和运动步态规划算法成为一大研究难点。

针对轨迹规划算法主要分为经典算法和现代智能算法，传统经典算法包括人工势场法、模拟退火算法等，这类方法容易陷入局部最优解，在对电力杆塔这么大的规划空间进行搜索时，效率低下。相比之下，现代智能算法收敛程度好，时间复杂度更小，应用范围更广，其中包括A*算法、遗传算法、蚁群算法等。

相关技术中，公布号为CN112304318A的中国发明专利申请公开了一种虚实耦合约束环境下的机器人自主导航方法，包括虚拟空间的搭建，虚实耦合约束环境的构建，虚实耦合约束环境下应用改进的A*算法进行机器人全局路径规划，得到最优规划路径，获取现实环境信息并结合虚实耦合约束环境，再考虑机器人的轮廓特征，在最优规划路径的相邻节点之间应用改进的DWA算法进行机器人局部避障。该方法采用BIM模型与三维重建获得的点云模型耦合的方法，建立建筑机器人工作的虚实耦合约束环境；结合虚实耦合约束环境数据，考虑机器人的安全性与通过性改进A*算法，删除冗余转折点；根据虚实耦合约束环境数据对DWA算法进行改进，使机器人在复杂环境下绕过局部最小陷阱。

公布号为CN112904842A的中国发明专利申请公开了一种基于代价势场的移动机器人路径规划与优化方法，步骤包括：初始化全局静态场景及动态障碍物对象的栅格地图，更新叠加代价势场，由等势线、切点、切线求代价最低的初始路径，再对路径进行进一步的调整和优化，最终得到当前时刻的最优安全路径。该方法相比其它的路径规划与避障方法能够综合考虑路径长度代价和与障碍物之间的距离代价，保证路径的安全性。同时经过调整之后的路径也具有较高的平滑度。通过引入动态障碍物的速度因素对障碍物代价势场的影响，该方法在动态障碍物避障场景下也具有较好的表现。

但上述相关路径规划方案在应用于如电力杆塔这么大的规划空间时，均存在局限性，路径规划和步态规划低效，如何在使电力杆塔作业机器人具备高越障能力的同时，实现作业效率的显著提高，为本领域近年来所亟待解决的技术难题。

发明内容

本发明所要解决的技术问题在于如何控制电力杆塔作业平台实现自主开展行进任务。

本发明通过以下技术手段实现解决上述技术问题的：

本发明提出了一种电力杆塔作业平台的控制方法，所述方法包括以下步骤：

分别对电力杆塔作业平台和电力杆塔进行建模，得到电力杆塔作业平台三维模型和电力杆塔三维模型；

将所述电力杆塔作业平台三维模型和所述电力杆塔三维模型进行耦合，产生控制指令；

向所述电力杆塔作业平台发送所述控制指令。

本发明通过建立电力杆塔作业平台的三维模型和电力杆塔的三维模型，模型精度高，并将电力杆塔作业平台三维模型和电力杆塔三维模型进行耦合，通过电力杆塔作业平台三维模型和电力杆塔三维模型自动计算产生控制指令并直接作用于电力杆塔作业平台，从而控制电力杆塔作业平台实现自主开展行进任务。

此外，本发明还提出了一种电力杆塔作业平台的控制系统，所述系统包括：虚拟控制平台和电力杆塔作业平台，所述虚拟控制平台包括作业机器人三维模型、电力杆塔三维模型和决策器；所述决策器包括控制指令产生模块和信息传输模块：

所述控制指令产生模块，用于将所述电力杆塔作业平台三维模型和所述电力杆塔三维模型进行耦合，产生控制指令；

所述信息传输模块，用于向所述电力杆塔作业平台发送所述控制指令。

本发明的优点在于：

(1)本发明针对电力杆塔空间结构规模较大，电力杆塔作业机器人自由度不高等特点，通过建立电力杆塔作业平台的三维模型和电力杆塔的三维模型，该三维模型精确度较高；将电力杆塔作业平台和电力杆塔三维模型进行耦合，通过电力杆塔作业平台三维模型和电力杆塔三维模型自动计算产生控制指令并直接作用于电力杆塔作业平台，从而控制电力杆塔作业平台实现自主开展行进任务。

(2)针对电力杆塔规划空间较大，状态总数增多规划耗时变长的问题，本发明结合电力杆塔结构构造二维有向加权图，并以节点板的高度作为每一个阶段的状态的划分依据，确定状态转移规划方程，然后基于状态转移规划方程，采用动态规划方法在二维有向加权图中搜索出一条从起始点E到结束点F的最优路径，其中每一个子阶段也是最优的，满足动态规划的最优子结构性质，函数通过作用于后部子过程，得到一个在以往计算中的最优状态，不会对下一步的决策起到影响，满足无后效性原则；进一步，应用动态规划算法进行路径点选择，以节点之间最小欧式距离为代价，并对状态点的数目进行减少，大大减少了计算量，进行路径规划时，在保证全局最优的同时，也提高了算法的规划速度，在确保抓取可靠性以及可重复性的情况下，提高作业机器人的作业效率，从而能大大节省人力巡检成本，提高检修效率，并保障输电系统安全可靠运行。

(3)利用双向动态规划算法把搜索空间重新规划为两个对称区域，在二维有向加权图中从前向后和从后向前同时进行动态规划搜索解空间，若两者存在公共的节点，则将两者中的状态节点依次相连，依次形成一条最优路径节点序列，进一步确保了路径规划的最优。

(4)本发明通过利用电力杆塔作业平台三维模型和电力杆塔三维模型进行仿真计算，产生控制指令控制作业机器人进行整体运动；再获取作业机器人在电力杆塔的实际状态信息，从而对作业机器人的运动状态进行修正，实现作业机器人虚实交互与闭环反馈，通过闭环反馈调整使得作业机器人的作业运动和电力杆塔作业平台三维模型作业保持一致，实现自主导航。

(5)针对作业机器人在电力杆塔作业过程中攀爬一定高度后定位不准确等问题，通过根据作业机器人作业过程中扫描到的电力杆塔实体中的特征点与电力杆塔三维模型中的特征点进行匹配实现辅助定位，简化了定位方式，提高了定位精度，实现高空作业机器人在电力杆塔任意高度位置作业时的精准定位。

(6)在建模得到作业机器人对应的电力杆塔作业平台三维模型和电力杆塔对应的电力杆塔三维模型后，由于电力杆塔较为复杂，通过生成标准的信息化模型数据格式，并采用匹配算法验证电力杆塔的原始数据与在机器人仿真软件中模型数据的匹配精确度，满足电力杆塔作业平台三维模型在电力杆塔三维模型上仿真任务作业的需求。

本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

图1是本发明一实施例提出的一种电力杆塔作业平台的控制方法的流程示意图；

图2是本发明一实施例中作业机器人三维数据建模示意图；

图3是本发明一实施例中电力杆塔三维数据建模示意图；

图4是本发明一实施例中路径规划步骤示意图；

图5是本发明一实施例中路径规划的详细步骤示意图；

图6是本发明一实施例中角钢塔节点板位置拓扑图；

图7是本发明一实施例中动态规划路径节点仿真结果图；

图8是本发明一实施例中作业机器人结构示意图；

图9是本发明一实施例中电力杆塔障碍物分布示意图；

图10是本发明一实施例中步态规划步骤示意图；

图11是本发明一实施例中步态规划的详细步骤示意图；

图12是本发明一实施例中步态规划仿真结果图；

图13是本发明一实施例中作业机器人越障步态图；

图14是本发明一实施例中步态仿真示意图；

图15是本发明一实施例中辅助定位流程示意图；

图16是本发明一实施例中辅助定位详细流程示意图；

图17是本发明一实施例中特征点定位示意图；

图18是本发明一实施例中作业机器人相机处理和位姿估计的步骤流程图；

图19是本发明一实施例中模型匹配验证步骤流程图；

图20是本发明一实施例中电力杆塔零部件层级的父子关系模型图；

图21是本发明一实施例中零件

坐标轴上投影图；

图22是本发明一实施例中零件绕X轴旋转示意图；

图23是本发明一实施例中采用迭代最近点算法进行模型验证示意图；

图24是本发明一实施例中ICP匹配验证迭代过程示意图；

图25是本发明一实施例中电力杆塔作业平台的控制系统的结构框图；

图26是本发明一实施例中电力杆塔作业平台与虚拟控制平台交互示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例一

如图1所示，本发明第一实施例提出了一种电力杆塔作业平台的控制方法，所述方法包括以下步骤：

S10、分别对电力杆塔作业平台和电力杆塔进行建模，得到电力杆塔作业平台三维模型和电力杆塔三维模型。

S20、将所述电力杆塔作业平台三维模型和所述电力杆塔三维模型进行耦合，产生控制指令。

S30、向所述电力杆塔作业平台发送所述控制指令。

需要说明的是，电力杆塔作业机器人的特性不同于建筑机器人，电力杆塔作业机器人自由度不高，且电力杆塔空间结构较大，本实施例针对此特性，通过建立电力杆塔作业平台的三维模型和电力杆塔的三维模型，该三维模型精确度较高，在1mm甚至0.1mm以内；将电力杆塔作业平台和电力杆塔三维模型进行耦合，通过电力杆塔作业平台三维模型和电力杆塔三维模型自动计算产生控制指令并直接作用于电力杆塔作业平台，从而控制电力杆塔作业平台实现自主开展行进任务。

在一实施例中，所述步骤S10，包括以下步骤：

S11、确定所述电力杆塔作业平台的第一建模参数，并基于所述第一建模参数，建立所述电力杆塔作业平台的三维模型，其中，所述第一建模参数包括所述电力杆塔作业平台的零部件尺寸参数和零部件结构之间的空间关系。

其中，电力杆塔作业平台包括作业机器人，通过确定实验设计作业机器人的关键结构，尺寸以及零部参数，明确作业机器人零部件结构之间的空间关系，采用手动方式，可使用SoliWorks、3dsMax和Unity3d软件对作业机器人进行混合建模，并通过模型的组装、渲染，生成作业机器人对应的三维数据模型，如图2所示。

S12、确定所述电力杆塔的第二建模参数，并基于所述第二建模参数，建立所述电力杆塔三维模型，其中，所述第二建模参数包括所述电力杆塔的零部件尺寸参数和零部件结构之间的空间关系。

其中，首先确定实验所需电力杆塔的关键结构、尺寸以及零部参数，明确电力杆塔零部件结构之间的空间关系，分别对各零部件进行建模，通过各零件的连接点进行组装；可采用3dsmax多边形对电力杆塔放样生成包括但不限于BIM、GIM等三维数据模型。

具体地，本实施例选取型号为变XJJ146-1D2SZ1-24米-鼓山角钢塔为例，对XJJ146-1D2SZ1型号的角钢塔进行数据化标注，结合CAD原始信息数据，并根据实际流程操作，进行放样生成三维数据模型如图3所示。

在一实施例中，所述控制指令包括最优路径信息和夹持点序列信息，所述步骤S20，具体包括以下步骤：

S21、利用所述电力杆塔作业平台三维模型在所述电力杆塔三维模型上进行任务仿真，采用动态规划算法，确定节点板序列作为所述电力杆塔作业平台的最优路径；

S22、基于所述最优路径，采用步态规划算法求取所述电力杆塔作业平台的夹持点位置，得到所述夹持点序列。

具体地，本实施例所述控制指令除了包括最优路径和夹持点序列信息之外，还包括螺栓打击器信息、作业机器人的机械臂关节旋转角度、作业机器人的上夹爪和下夹爪的开合状态以及作业机器人上夹爪和下夹爪的移动距离等信息。

本实施例针对电力杆塔规划空间较大，状态总数增多规划耗时变长的问题，应用动态规划算法进行路径点选择，在进行路径规划时，在保证全局最优的同时，也提高了算法的规划速度，同时使用动态规划找出最优轨迹下的步态，实现作业机器人的自主运动步态规划，在确保抓取可靠性以及可重复性的情况下，提高作业机器人的作业效率，从而能大大节省人力巡检成本，提高检修效率，并保障输电系统安全可靠运行。

实施例二

本实施例在上述实施例一公开内容的基础上，具体设置了动态规划算法进行最优路径规划，以及设置了步态规划算法进行步态规划，在保障输电系统安全可靠运行的同时提高检修效率。

本实施例中，如图4至图5所示，所述步骤S21：利用所述电力杆塔作业平台三维模型在所述电力杆塔三维模型上进行任务仿真，采用动态规划算法，确定节点板序列作为所述电力杆塔作业平台的最优路径，包括以下步骤：

S211、以所述电力杆塔三维模型中节点板上某一个螺栓位置点代表该节点板在空间中的位置，并基于各所述节点板在空间中的位置，计算任意两所述节点板之间的欧式距离。

S212、以各所述节点板的位置作为有向图的点，以任意两所述节点板之间的欧式距离作为点到点之间的弧，构造二维有向加权图。

具体地，本实施例基于节点板在空间中的位置，并将欧式距离作为代价权值，构造所述二维有向加权图表示为：

E＝[(v₁,v₂),(v₂,v₃),(v₂,v₄),(v₂,v₅),(v₃,v₆)...(v_n-1,v_n)

其中，(v_i,v_j)是符合所述二维有向加权图的任意两点的权值，当(v_i,v_j)∈E时，W_ij＝L_ij，W＝(W_ij)_n*n为由两两所述节点板之间的欧式距离构成的距离矩阵，L_ij为弧(v_i,v_j)的权，W_ij＝0,i＝1,2...n；当

且W_ij＝inf，inf为无穷大，n为网络节点个数。

需要说明的是，本实施例需要将电力杆塔三维模型离散化，即针对电力杆塔三维模型构造二维有向加权图，首先对作业机器人路径点可能存在的空间进行离散化，在三维坐标系中，针对电力杆塔自身结构特点，图6中不同的形状代表不同的节点板类型，选中节点板上某一个螺栓位置点代表节点板在空间中的位置令作业机器人三维模型经过的航迹点p_i(i＝1,2…n)的坐标为(x_i,y_i,z_i)，其中以z_i为离散化状态阶段的划分依据，以两两节点板之间的欧式距离作为代价权值，将节点和距离信息存储在动态规划表即数据库中，得到距离矩阵；基于距离矩阵确定电力杆塔的节点板位置拓扑图即二维有向加权图；图6中两个节点之间权值即为两节点板之间的欧式距离，如节点1和节点3之间的距离d₁₃：

其中，节点1的位置坐标为：(x₁ y₁ z₁)，节点2的位置坐标为：(x₃ y₃ z₃)。

当i＝＝j时W_ij＝0按上述规则建立的距离矩阵W：

S213、将所述节点板的高度作为每一个阶段的状态的划分依据，确定状态转移规划方程。

具体地，本实施例以电力杆塔上节点板的高度作为状态每一个阶段的划分依据，对作业机器人从初始点E搜索到终止点F的最优路径进行规划，将所述电力杆塔每一层高度划分为K个阶段，令第k阶段的状态为S_k，状态为S_k的决策变量表示为T_k(s_k)，则第k+1阶段的状态表示为：S_k+1＝C(S_k,T_k(S_k))，k＝1,2,......K；状态S_k和状态S_k+1之间的代价定义为两点的欧式距离d(S_k+1,S_k)，第一阶段到第k阶段花费度量值之和v表示为

S214、基于所述状态转移规划方程在所述二维有向加权图中采用顺序和/或逆序方式进行动态规划搜索解空间，形成一条从初始点E到终止点F的节点板序列作为最优路径。

本实施例应用动态规划方法搜索出一条从初始点E到终止点F的角钢塔作业机器人最优路径时，其中每一个子阶段也是最优的，满足动态规划的最优子结构性质，函数通过作用于后部子过程，得到一个在以往计算中的最优状态，不会对下一步的决策起到影响，满足无后效性原则，如图7所示，规划的路径节点为V1→V2→V9→V21→V29→V33。

进一步来说，应用动态规划算法进行路径点选择，以节点之间最小欧式距离为代价，并对状态点的数目进行减少，大大减少了计算量，进行路径规划时，在保证全局最优的同时，也提高了算法的规划速度，在确保抓取可靠性以及可重复性的情况下，提高作业机器人的作业效率，从而能大大节省人力巡检成本，提高检修效率，并保障输电系统安全可靠运行。

进一步地，本实施例可利用顺序或逆序方式在二维有向加权图中进行单向搜索，也可在二维有向加权图中采用顺序和逆序方式分别从前向后和从后向前同时进行动态规划搜索解空间，若两者存在公共电力杆塔节点，则将两者中的状态节点依次相连，形成一条最优路径节点序列。

本实施例利用双向动态规划算法把搜索空间重新规划为两个对称区域，双向动态规划在角钢塔二维有向加权图中从前向后和从后向前同时进行动态规划搜索解空间，若两者存在公共角钢塔节点，则将两者中的状态节点依次相连，依次形成一条最优路径节点序列，进一步确保了路径规划的最优。

在一实施例中，在所述二维有向加权图中采用顺序方式进行动态路径规划的公式表示为：

式中：u_k+1(s_k+1)为从初始点E到第k+1阶段末状态S_k+1的最优指标函数；u₀(s₀)为初始点E的初始状态；u_k(s_k)为初始点E到第k阶段状态的最优指标函数；d(s_k,s_k+1)为状态S_k与状态S_k+1之间的代价。

在所述二维有向加权图中采用逆序方式进行动态路径规划的公式表示为：

式中：u_k(s_k)为从第k阶段末状态S_k到终止点F的最优指标函数；u_K(s_K)为终止点F的结束状态；u_k+1(s_k+1)为结束点F到第k+1阶段状态的最优指标函数；d(s_k,s_k+1)为状态S_k与状态S_k+1之间的代价。

进一步地，电力杆塔作业平台包括作业机器人，作业机器人结构如图8所示，机器人本体M主要包括躯干、夹持组件、背部滑台、作业机械臂，夹持组件包括上夹持组件及下夹持组件，上夹持组件中心线和下夹持组件中心线作为上夹持点和下夹持点，分别用于J1，J2表示，且至少一组夹持组件可相对躯干作平行躯干行进路径的往复位移动作，P为角钢主材，F为虚拟障碍物，虚拟障碍物可以看做是螺栓或者斜材造成的不可夹持区域；所述作业机械臂上固定设有相机，所述夹持组件、相机、背部滑台均与机器人上位机控制系统信号连接。

在上述规划出作业机器人的最优路径后，需进行作业机器人的步态规划，以高效率利用作业机器人自身机械结构特性，实现作业机器人的自主运动步态规划：首先进行可夹持区域和不可夹持区域的判断，造成不可夹持区域的原因主要分为两种，一种斜材，一种是螺栓区，如图9所示。

在一实施例中，如图10至图11所示，所述步骤S22，具体包括以下步骤：

S221、从所述电力杆塔三维模型的原始数据中确定避障区域集[(E₁₁,E₁₂)，(E₂₁,E₂₂),...,(E_t1,E_t2),...,(E_T1,E_T2)]，(E_t1,E_t2)表示第t个障碍物对应的避障区域。

需要说明的是，沿主材方向为Y轴，以电力杆塔中主材脚部为原点建立基坐标系O₂，沿竖直方向为Y轴，以主材脚步为原点O₁建立世界坐标系；具体到本实施例中，所述避障区域(E_t1,E_t2)定义为基坐标系下沿主材方向为Y轴，约定节点板形状在主材方向上的长度与上、下两个夹爪宽度之和。

比如：障碍物位置范围用(y′₁,y′₂)表示，避障区域用(E_t1,E_t2)表示，作业机器人的夹爪沿主材方向的长度为l，考虑到机器人爪子具有一定的宽度，避障区域比障碍物区域上下各留出一个爪子宽度的余量，则：

E_t1＝y′₁-l

E_t2＝y′₂+l

通过将避障区域的范围设置比障碍物位置范围上下各留出爪子宽度的余量，可进一步防止抓手碰触到障碍物导致抓手损坏。

S222、计算所述作业机器人三维模型从起始位置移动到目标螺栓工作点所需距离d。

S223、将距离d和最大步距D进行比较，得出整数步及余数。

S224、每一次行走一个整数步或者最后一次行走一个余数步后，求出所述作业机器人三维模型上夹持点的位置和下夹持点的位置，所述上夹持点和下夹持点分别为所述上夹爪和下夹爪的中心线。

S225、基于所述上夹持点的位置、下夹持点的位置和避障区域集，规划所述作业机器人三维模型从起始位置到目标螺栓工作点的夹持点序列。

本实施例先通过电力杆塔三维模型进行避障区域的计算，然后将作业机器人从目前位置到目标螺栓工作点所需移动的距离与作业机器人的最大步距进行比较，得出整数步以及余数，在攀爬机器人每一次行走一个整数步或者最后一次行走一个余数步后，求出作业机器人上、下夹持点位置；判断上、下夹持点的位置是否在避障区域内，若在避障区域内，若在则将障碍区域的下界作为该次夹持点，并在此基础上重新计算整数步和余数步，直至抵达目标螺栓工作点位置时，输出夹持点序列；使用动态规划找出最优轨迹的基础上设计的作业机器人步态规划算法可高效率利用作业机器人自身机械结构特性，实现作业机器人的自主运动步态规划，在行进至目标螺栓工作点及除冰位置时进行螺栓紧固及除冰等作业任务。

在一实施例中，所述步骤S222中，计算所述作业机器人三维模型从起始位置移动到目标螺栓工作点所需距离d，具体包括以下步骤：

获取作业机器人三维模型的下夹持点在所述电力杆塔三维模型中主材上到底部的距离L₃；

根据所述初始落脚点L₃和所述电力杆塔作业平台三维模型中下夹爪距离作业机器人螺栓紧固工具点的距离L₂，计算初始时刻上夹持点距离底部挡板距离L₁；

根据目标螺栓工作点的位置与当前螺栓打击位置，计算所述电力杆塔作业平台三维模型移动到该目标螺栓工作点需要移动的位移d：

d＝(Z_target-Z_work)/sin(θ)

式中：θ为所述电力杆塔三维模型的倾斜角；Z_target为从所述电力杆塔作业平台三维模型中解析出的目标螺栓工作点高度坐标；Z_work为当前螺栓打击点的高度坐标。

然后，设计机器人最大行走步态长度为D，通过判断正常行走跨越值与位移之间的关系d，规划机器人到目标工作点的步态为：N＝d/D，则机器人行走N个完全正常行走步态值后还需要行走的余数距离reminder＝d-N*D。

如图8所示，该机器人的上、下夹爪之间的初始状态时相距为D₁，此为两夹爪最小安全距离，所以此时上下夹爪之间可以运动到最大行程D₂，D夹爪距离作业机器人螺栓打击器的距离是L₂，该作业机器人在主材上的初始落脚点可以通过激光测距获得距离为L₃，通过L₃可进行计算初始时刻上夹持点距离底部挡板距离L₁＝L₂+L₃。

在一实施例中，所述步骤S225，包括以下步骤：

每行走一步，判断所述上夹持点的位置或下夹持点的位置是否在所述避障区域集中任一避障区域(E_t1,E_t2)内；

需要说明的是，每行走一步判断是否在斜材和螺栓区对主材形成的避障区域内，即是判断L₁+i*D与避障区域(E_t1,E_t2)之间的关系，其中0<i<＝N，i表示已经走过的步数，N表示最大整数步；

若是，则将避障区域的下界作为该次夹持点，下夹爪攀爬至E_t1-D₁处，并更新所述距离L₃＝E_t1-L₅+(i-1)*D，其中，L₅＝L₃+D₁；

若否，则所述上、下夹爪同时沿着主材移动的所述最大步距D；

在抵达所述目标螺栓工作点位置时，输出所述夹持点序列。

需要说明的是，步态规划仿真结果图如图12所示，作业机器人越障步态图如图13所示，当前夹爪经计算后落入避障区域，则将该避障区域的下界限作为该夹爪的落点，并计算本次上夹爪的移动距离，并按照该距离移动下夹爪，更新L₃后必然可以一个完成行程跨越。

假设作业机器人夹爪以22mm/s完成一次夹持运动，则按上述算法仿真结果示意图如图14所示，其中黑点表示动态规划的求解结果，实线表示上夹爪在O₁坐标系Y轴方向上的距离。

在一实施例中，所述方法还包括：

判断所述最优路径中是否存在相邻两个所述节点板的高度一致；

若是，则将原theta、d值存入程序堆栈，并更新theta＝0、d＝||y₁-y₂||，y₁、y₂为相邻两个所述节点板的坐标；

若否，则重新从程序堆栈获取原theta、d值。

需要说明的是，在规划出最优路径后，判断动态规划得出的节点板序列中是否存在相邻节点板的高度一致，若某两个相邻节点板之间高度一致，即高度差Z＝0，则表明前方攀爬路径出现转折点，作业机器人前进路径与坐标系O₁的Y轴平行。此时，先将原来的theta、d值存入程序堆栈，再将其更新为theta＝0，d＝||y₁-y₂||；若高度差Z≠0，则表明后续的节点板序列重新分布在主材上，重新从程序堆栈中获取原theta、d值。

实施例三

针对作业机器人在电力杆塔行进至一定高度后，定位不准确，且实际场景中各种状态信息难以及时获取，反馈难度大，机器人实体和机器人虚拟模型作业难以保持一致等问题，本实施例在上述实施例一公开的内容的基础上，再利用作业机器人携带各种传感器设备获取作业机器人的实际状态信息，闭环反馈到虚拟平台，重新判断是否进行路径规划及步态规划，实现作业机器人虚实交互，自主导航与反馈，具体地，包括以下步骤：

S40、获取所述电力杆塔作业平台发送的实际状态信息，并计算所述实际状态信息与目标状态信息的差值，产生修正指令；

S50、向所述电力杆塔作业平台发送所述修正指令。

具体地，电力杆塔作业平台反馈的信息实际状态信息包括所述作业机器人的实际位置信息和实际位姿信息，作业机器人的上、下夹爪的实际位置及位姿信息，螺栓打击器的位置信息等。

进一步地，如图15至图16所示，所述步骤S40中，获取所述作业机器人实体在电力杆塔上的实际状态信息，具体包括以下步骤：

S41、在所述作业机器人在作业过程中，利用相机进行实时扫描检测，采集电力杆塔图像。

S42、基于所述电力杆塔图像，对所扫描到的实体特征点进行计数，并获取对应的实体特征点ID信息。

S43、对所述相机与所述作业机器人本体躯干进行标定，求解相机与所述作业机器人之间的相对位姿矩阵

需要说明的是，对固定于高空作业机器人背部工作机械臂的相机进行标定，确定相机的内参矩阵K以及畸变参数，根据相机标定模型，通过得到的特征点中心点位置，求得相机坐标系与标记坐标系之间的变换矩阵

(C：相机，R：高空作业)。

S44、基于所述实体特征点ID信息，在预先构建的匹配信息数据库中进行检索，获得当前扫描到的特征点所在坐标系与电力杆塔所在的世界坐标系之间的转换矩阵

(B：bolt，T：Tower)。

S45、采用相机模型对相机进行标定，并对实时扫描得到的所述图像信息中的实体特征点进行位姿估计，求解相机坐标系与特征点所在坐标系之间的转换矩阵

S46、基于所述相对位姿矩阵

所述转换矩阵

和所述转换矩阵

求解作业机器人在所述电力杆塔上的实际位姿状态信息

为：

需要说明的是，本实施例包括实现空间定位包括离线计算和实时运行两个部分，其中离线计算包括对施工建筑三维信息模型进行信息提取与运算，以电力杆塔三维信息模型为例，分割出螺栓区域、节点板区域、斜材区域等特征点并求解对应特征点坐标系与角钢塔(世界)坐标系之间的变换矩阵并生成特征点标记匹配信息数据库；实时运行时，固定好高空作业机器人的初始位姿，使其两个夹爪夹持住铁塔的斜材杆件，并给其带有相机的机械臂模块一个初始位姿，使得相机可以正对着角钢塔一条腿的一端；然后高空作业机器人开始沿着角钢塔一条腿进行作业，作业过程中相机进行实时扫描检测，通过所述相机采集图像信息，经过图传模块将信息传输至机器人控制系统，判断抓取到的图像中是否包含特征点，识别出特征点之后进行标记检测和位姿估计，之后根据特征点标记信息数据库进行检索从而求解高空作业机器人在电力杆塔行进过程中的实时位姿信息。离线计算和实时运行相结合，简化了高空作业机器人定位算法的计算过程。

本实施例通过根据作业机器人作业过程中扫描到的电力杆塔实体中的特征点与电力杆塔三维模型中的特征点进行匹配实现辅助定位，简化了定位方式，提高了定位精度，实现高空作业机器人在电力杆塔任意高度位置作业时的精准定位；将定位到的作业机器人在电力杆塔上的实际位姿反馈值虚拟空间，并与虚拟空间规划的目标位姿进行比较修正，通过闭环反馈调整使得作业机器人的作业运动和电力杆塔作业平台三维模型作业保持一致。

在一实施例中，所述步骤S44中，所述匹配信息数据库的构建过程包括：

基于所述电力杆塔三维模型，求解特征点的坐标系与所述电力杆塔所在世界坐标系之间的位姿信息，所述特征点包括螺栓、节点板和斜材；

对所述特征点进行标号，赋予每个特征点唯一ID，ID沿着电力杆塔的一条腿从下至上依次递增；

基于各特征点的位姿信息和特征点ID，构建所述匹配信息数据库。

进一步地，沿着角钢塔的一条腿从上到下的特征点都有一个相对同类型特征点递增的唯一ID，根据ID可以快速的从特征点标记信息数据库中读取到当前相机检测到的特征点的位姿信息，从而实现了高空作业机器人在作业过程中的远程空间定位并且提高了在一定高度以上的定位精度。

需要说明的是，以角钢塔为例，将角钢塔模型导入信息化模型软件；基于信息化模型软件中的角钢塔三维信息模型进行信息提取和运算，分割出螺栓区域、节点板区域、斜材区域等特征点并求解对应特征点坐标系与角钢塔(世界)坐标系之间的变换矩阵并生成特征点标记匹配信息数据库，获得电力铁塔四条腿上各个特征点的位姿矩阵并将其标号，赋予每个特征点一个唯一ID，ID沿着角钢塔的一条腿从下至上依次递增；根据从角钢塔三维信息模型中读取出来的特征点位姿信息构建基于固定角钢塔三维信息模型的特征点标记匹配位姿信息数据库，如表1所示(以角钢塔A号腿下半部为例如图17)：

表1

在一实施例中，所述步骤S44，具体包括以下步骤：

基于所述实体特征点的ID信息，从所述匹配信息数据库中搜索得到对应的位姿信息

和

分别为相对于所述电力杆塔所在世界坐标系的位置矢量数据和旋转矩阵；

基于对应的所述位姿信息，使用齐次变换矩阵来求解所述实体特征点相对与所述电力杆塔所在的世界坐标系的转换矩阵

公式表示为：

其中，从电力铁塔三维信息数据模型中读取到特征点的姿态信息是一组固定的角度α、β、γ，其中α表示以特征点质心为原点的坐标系相对于电力铁塔基(世界)坐标系的X轴旋转的角度，β表示其绕Y轴旋转的角度，γ表示其绕Z轴旋转的角度。

规定这种姿态的表示法为X-Y-Z固定角，因为其是在固定参考坐标系中确定的，将他们定义为回转角、俯仰角和偏转角；根据这三个角度可以直接推导特征点坐标系相当于电力铁塔(世界)坐标系的等价旋转矩阵

式中：α、β、γ为从所述电力杆塔三维模型中读取到的特征点的姿态信息，α为以所述特征点质心为原点的坐标系相对于所述电力杆塔所在世界坐标系的X轴旋转的角度，β为其绕Y轴旋转的角度，γ为其绕Z轴旋转的角度；cα为cosα；sα为sinα。

本实施例中，从机器人作业开始计数，针对不同类型的特征点，将相机扫描到的第一个特征点ID置为1，后续扫描到的特征点按照其类型依次赋予一个递增1的ID，采集到特征点信息之后，将扫描得到的特征点进行计数然后传输至机器人控制系统，然后机器人控制系统去特征点标记匹配信息数据库进行检索匹配获得当前特征点的位姿信息。之后求出机器人当前的位姿信息，并查看是否和预定攀爬路径的位姿信息一样，若不一样，计算预期位姿和当前位姿之间的差值，反馈给机器人控制系统使其可以至电力铁塔的预定位置进行作业。

在实现高空作业机器人高空定位的时候融入了电力三维信息数据模型，从而减少了传感器和激光测量器等直接测距器材的使用，直接采用在提前建立好的特征点标记匹配位姿信息数据库进行检索匹配的方式，与传统的直接定位方法相比简化定位方式并且提高了定位精度，实现稳定精确的高空作业机器人在作业电力铁塔时的空间定位。

在一实施例中，所述步骤S42，具体包括以下步骤：

使用高斯滤波器对所述电力杆塔图像进行滤波去噪后，采用直方图均衡化进行图像增强处理，得到增强图像；

采用Canny边缘检测器对所述增强图像进行有效的边缘提取，得到边缘特征；

使用Hough变换，将所述边缘特征转换到Hough空间，识别所述实体特征点的中心位置，并对所扫描到的实体特征点进行计数。

具体地，如图18所示，本实施例使用一个高斯滤波器平滑输入图像，对原始图像进行卷积操作，然后计算梯度幅值图像和角度图像，对梯度幅值图像应用非最大抑制用以排除非边缘信息，最后用双阈值处理和连接分析来检测并连接边缘，高阈值和低阈值的比率设置为3:1；使用Hough变换来完成特征点的识别。将得到的图像信息转换到Hough空间中，然后在Hough空间中找到若干组六边形点集。六边形的每组对边斜率相同，且三组对边斜率角度相差60度，这一特征也相应的呈现与Hough空间中峰值的分布，得到六边形信息之后计算六边形中心点可得到特征点中心位置。

实施例四

针对角钢塔放样软件建立的信息化模型与传统的机器人仿真平台模型无法兼容这一问题，本实施例在上述实施例一已通过对复杂的电力杆塔进行可视化建模，生成标准的角钢塔信息化模型数据格式的基础上，验证电力杆塔原始数据与在机器人仿真软件中模型数据的精确度，在满足要求后进行作业机器人的仿真作业计算。

本实施例中，如图19所示，在所述步骤S10之后，还包括以下步骤：

S01、从所述电力杆塔三维模型的原始数据中提取出有效数据；

需要说明的是，从原始数据中提取有效数据的方式包括但不限于类群分析，关联分析等。

S02、对所述有效数据进行加工处理，输出标准DAE格式的数据；

S03、基于所述机器人仿真软件坐标系，对所述标准DAE格式的数据进行坐标转换，得到所述机器人仿真软件坐标系下的数据；

S04、采用匹配算法验证所述标准DAE格式的数据与所述机器人仿真软件坐标系下的数据之间的精确度满足要求。

本发明在建模得到作业机器人对应的电力杆塔作业平台三维模型和电力杆塔对应的电力杆塔三维模型后，由于电力杆塔较为复杂，通过生成标准的信息化模型数据格式，并采用匹配算法验证电力杆塔的原始数据与在机器人仿真软件中模型数据的匹配精确度，满足电力杆塔作业平台三维模型在电力杆塔三维模型上仿真任务作业的需求。

应当理解的是，本实施例采用的模型匹配算法包括但不限于有：

ICP算法：迭代最近点算法(iterative closest point algorithm)

NDT算法：正态分布变换算法(Normal Distribution Transform algorithm)

IDC算法：Iterative dual correspondences algorithm

PIC算法：Probabilistic iterative correspondence method

RPM算法：Robust point matching algorithm

KC算法：Kernel correlation algorithm

CPD算法：Coherent point drift。

在一实施例中，所述步骤S01，包括以下步骤：

对所述原始数据进行筛选，确定原始数据的关键参数以及关键参数之间的关联规则，并根据关键参数提取所述有效数据，其中，所述有效数据包括电力杆塔信息和零件属性信息；

根据所述关键参数之间的关联规则，建立所述有效数据之间的关系模型，所述关系模型为电力杆塔零部件层级的父子关系，其中，关联规则为零件间的父子从属关系；

其中，电力杆塔零部件层级的父子关系模型如图20所示。

根据所述有效数据和所述有效数据之间的关系模型，确定用于存储有效数据的数据类型，所述数据类型为数组T_i＝(Position,Rotation,Scale)，其中，Position,Rotation,Scale分别表示数组中零件坐标的位置，姿态，比例信息，各零部件层级结构以拓扑树结构存储。

需要说明的是，数据类型以数组的方式存储，可节省空间方便顺序查找。

在一实施例中，所述步骤S02，包括以下步骤：

对所述有效数据进行轻量化处理，得到轻量化数据；

对所述轻量化数据进行加工处理和渲染，输出所述标准DAE格式的数据。

需要说明的是，由于电力杆塔一般采用的是网格模型，这种模型结构压缩较小，电力杆塔中螺栓、节点板以及斜材众多属性信息中，除序列号，位置以及姿态等一些属性不同，有些属性数据大量冗余，因此本实施例对其轻量化处理，遍历每一个零件，判断是否存在，若有则只保留关键信息，多余则删除。在轻量化处理后，针对提取的有效数据所形成的模型进行渲染，生成支持在机器人仿真终端运行的DAE格式的三维可视化模型。

进一步地，在Unity3d虚拟空间中，电力杆塔三维模型上的每一个零部件的Transform组件即代表电力杆塔初始化建立坐标系时的位姿，而不是代表在世界坐标系Unity3d下的坐标，第二建模软件初始化建立坐标系是以Z轴向上，但在Unity3d机器人仿真平台的世界坐标系下，是以Y轴向上，需考虑将塔自身坐标系向Unity3d世界坐标系角度旋转。

在一实施例中，所述步骤S03中，基于所述机器人仿真软件坐标系，对所述标准DAE格式的数据进行坐标转换，得到所述机器人仿真软件坐标系下的数据，转换公式为：

式中：P_unity为所述标准DAE格式的数据中一个零件中心点P在所述机器人仿真软件坐标系下的坐标点；R_x(θ)为旋转矩阵；θ为所述电力杆塔三维模型导入所述机器人仿真软件中时的倾斜角度；P_tower为所述标准DAE格式的数据中一个零件中心点P的坐标。

具体地，设角钢塔上其中一个零件中心点P，已知基于角钢塔初始化建立坐标系中的向量

为(x,y,z)^T，其在直角坐标系中的图21所示。其中点P在xy平面、xz平面、yz平面的投影分别为点M、点P、点N。

那么在虚拟空间Unity3d中

绕X轴逆旋转θ角：

绕X轴旋转，相当于

在yz平面的投影ON绕原点旋转，如下图22所示，ON旋转θ角到ON′，旋转后的坐标为(x′,y′,z′)^T，则点N的坐标为(z,y)^T，点N′的坐标为(y′,z′)^T。由此可得：

z＝ON cosφ

y＝ON sinφ

z′＝N′cos(φ+θ)

y′＝N′sin(φ+θ)

ON＝ON′

对于z′和y′进行三角展开可得：

z′＝ON(cosφcosθ-sinφsinθ)＝z cosθ-y sinθ

y′＝ON(sinφsinθ+cosφcosθ)＝y cosθ+z sinθ

且有x′可得绕X轴旋转θ角的旋转矩阵为：

在机器人仿真软件Unity3d的世界坐标系下，其Y轴朝上，如图22示。

零件

是绕X轴逆时针旋转θ，P点在Unity3d世界坐标下为：

由此知道角钢塔模型每一个零件在Unity3d下的坐标，式中：P_unity为所述标准DAE格式的数据中一个零件中心点P在所述机器人仿真软件坐标系下的坐标点；R_x(θ)为旋转矩阵；θ为所述电力杆塔三维模型导入所述机器人仿真软件中时的倾斜角度；P_tower为所述标准DAE格式的数据中一个零件中心点P的坐标。

在一实施例中，如图23所示，本实施例具体采用迭代最近点(iterative closestpoint,ICP)算法验证所述标准DAE格式的数据与所述机器人仿真软件坐标系下的数据之间的精确度，所述步骤S04，包括以下步骤：

a)从所述标准DAE格式的数据中选取部分数据，构建标准点集X＝{x₁,x₂,...,x_N}；

b)从所述机器人仿真软件坐标系下的数据中选取部分数据，构建待匹配点集Y＝{y₁,y₂,...,y_N}；

c)对于对应点集合{(x₁,y₁),(x₂,y₂),...,(x_Ny_N)}，确定旋转矩阵R和平移矩阵t，使得误差函数的误差最小，所述误差函数为：

式中：E(R,t)为目标函数；||||为范数符号；

d)判断所述误差函数的最小误差是否满足精度阈值要求，若是则执行步骤e)，若否则执行步骤f)；

e)确定所述标准DAE格式的数据与所述机器人仿真软件坐标系下的数据之间的精确度满足要求；

f)利用旋转矩阵R和平移矩阵t，对所述待匹配点集Y进行旋转和平移变换，更新所述待匹配点集Y；

g)利用更新后的所述待匹配点集Y和所述标准点集X，更新所述对应点集合，然后执行步骤c)，直至误差满足精度阈值要求或超过最大迭代次数时，退出迭代循环。

在一实施例中，所述步骤c)对于对应点集合{(x₁,y₁),(x₂,y₂),...,(x_Ny_N)}，确定旋转矩阵R和平移矩阵t，使得误差函数的误差最小，具体包括以下步骤：

c1)分别计算所述标准点集X和所述待匹配点集Y的质心，得到质心

和质心

c2)分别对所述标准点集X和所述待匹配点集Y进行去中心化处理，得到去中心化的点集x′_i＝x_i-u_x和点集y′_i＝y_i-u_y；

c3)基于所述去中心化的点集x′_i和点集y′_i，构建矩阵

c4)对所述矩阵H进行svd分解，得到分解矩阵H＝U∑V^T；

c5)基于所述分解矩阵，计算旋转矩阵R＝VU^T；

c6)基于所述旋转矩阵R、质心u_x和质心u_y，计算平移矩阵t＝μ_x-Rμ_y。

需要说明的是，本实施例通过计算转移矩阵R和平移矩阵t，最终判断误差函数是否满足精度阈值要求，若不满足精度阈值要求，则对待匹配点集Y使用上一步求得的旋转矩阵R和平移矩阵t进行旋转和平移变换，得到新的对应点集，再次计算新的误差函数E(R,t)，若新的误差满足精度阈值要求，则停止迭代，得出最优解，若超过最大迭代次数则退出循环。图24为ICP匹配验证迭代过程，灰色数据点集为角钢塔真实数据坐标，白色数据对点集为带匹配点集，每次迭代计算新的误差函数E(R,t)，生成新的带匹配数据，重新计算匹配原始数据点集。最终，满足要求后在机器人仿真软件下进行角钢塔的作业任务。

本实施例可以将复杂的电力杆塔以三维的角度全方位展示给目标人群，生成标准的角钢塔信息化模型数据格式，支持导入机器人领域的仿真软件，直接获取角钢塔关键参数属性信息，在主流的机器人仿真平台如ROS系统以及三维游戏引擎Unity3D下，形成1:1的忠实映射，进而开展作业机器人在角钢塔上仿真任务作业。解决了存在角钢塔放样软件建立的信息化模型与传统的机器人仿真平台模型无法兼容一大研究难点。

实施例五

如图25所示，本实施例提出了与上述实施例一提出的电力杆塔作业平台的控制方法对应的电力杆塔作业平台的控制系统，该系统包括：虚拟控制平台10和电力杆塔作业平台20，所述虚拟控制平台10包括作业机器人三维模型11、电力杆塔三维模型12和决策器13；所述决策器13包括控制指令产生模块131和信息传输模块132：

所述控制指令产生模块131，用于将所述作业机器人三维模型11和所述电力杆塔三维模型12进行耦合，产生控制指令；

所述信息传输模块132，用于向所述电力杆塔作业平台20发送所述控制指令。

需要说明的是，本实施例可以采用3dMAX软件分别将所述电力杆塔作业平台三维模型和所述电力杆塔三维模型转换为Unity3d虚拟空间下支持的格式文件，得到对应的第一格式文件和第二格式文件；将所述第一格式文件和所述第二格式文件导入到untiy3d虚拟空间，进行动作交互，搭建得到虚拟控制平台。

需要说明的是，本实施例将电力杆塔作业平台三维模型和电力杆塔三维模型导入Unity3d虚拟空间，即电力杆塔三维模型的三维模型格式.3ds通过3dMAX软件进行渲染，导出Unity3d下支持的.fbx格式文件；将电力杆塔作业平台三维模型的格式.stl经过3dMAX软件位姿居中设置等操作后导出.fbx格式文件，最终实现将不同领域软件格式导入到untiy3d虚拟空间，电力杆塔作业平台三维模型和电力杆塔三维模型在虚拟Unity3d空间进行动作交互。

进一步地，本实施例定义作业机器人的所需要采集信息的模型如螺栓紧固信息、缺陷信息、本体位姿信息等，定义通讯数据格式，并采用了OPC UA/Socket等通讯协议，同时采用多线程加速有效数据的采集、传输与反馈，实现在虚拟平台上联动作业机器人物理实体，实现虚拟Unity3d空间与物理作业机器人实体平台虚实交互，如图26所示。

进一步地，所述电力杆塔作业平台20包括控制器21和作业机器人22，所述控制指令产生模块131产生的控制指令发送至控制器21，由控制器21控制所述作业机器人22进行自主行进任务。

在一实施例中，所述控制指令包括最优路径和夹持点序列，所述控制指令产生模块131包括：

路径规划单元1311，用于利用所述电力杆塔作业平台三维模型在所述电力杆塔三维模型上进行任务仿真，采用动态规划算法，确定节点板序列作为所述电力杆塔作业平台的最优路径；

步态规划单元1312，用于基于所述最优路径，采用步态规划算法求取所述电力杆塔作业平台的夹持点位置，得到所述夹持点序列。

在一实施例中，所述路径规划单元1311包括：

距离计算子单元，用于以所述电力杆塔三维模型中节点板上某一个螺栓位置点代表该节点板在空间中的位置，并基于各所述节点板在空间中的位置，计算任意两所述节点板之间的欧式距离；

加权图构造子单元，用于以各所述节点板的位置作为有向图的点，以任意两所述节点板之间的欧式距离作为点到点之间的弧，构造二维有向加权图；

状态划分子单元，用于将所述节点板的高度作为每一个阶段的状态的划分依据，确定状态转移规划方程；

路径规划子单元，用于基于所述状态转移规划方程在所述二维有向加权图中采用顺序和/或逆序方式进行动态规划搜索解空间，形成一条从初始点E到终止点F的节点板序列作为最优路径。

在一实施例中，所述步态规划单元1312，包括：

避障区域确定子单元，用于从所述电力杆塔三维模型的原始数据中确定避障区域集[(E₁₁,E₁₂)，(E₂₁,E₂₂),...,(E_t1,E_t2),...,(E_T1,E_T2)]，(E_t1,E_t2)表示第t个障碍物对应的避障区域；

目标距离计算子单元，用于计算所述作业机器人三维模型从起始位置移动到目标螺栓工作点所需距离d；

比较子单元，用于将距离d和最大步距D进行比较，得出整数步及余数；

夹持位置确定子单元，用于每一次行走一个整数步或者最后一次行走一个余数步后，求出所述作业机器人三维模型上夹持点的位置和下夹持点的位置，所述上夹持点和下夹持点分别为所述上夹爪和下夹爪的中心线；

步态规划子单元，用于基于所述上夹持点的位置、下夹持点的位置和避障区域集，规划所述作业机器人三维模型从起始位置到目标螺栓工作点的夹持点序列。

在一实施例中，所述决策器还包括：

所述信息传输模块132，还用于获取所述作业机器人22上所搭载的传感器23发送的实际状态信息，并由所述决策器13计算所述实际状态信息与目标状态信息的差值，产生修正指令；

所述信息传输模块132，还用于向所述电力杆塔作业平台发送所述修正指令。

在一实施例中，所述控制系统还包括匹配验证模块，用于：

从所述电力杆塔三维模型的原始数据中提取出有效数据；

对所述有效数据进行加工处理，输出标准DAE格式的数据；

基于机器人仿真软件坐标系，对所述标准DAE格式的数据进行坐标转换，得到所述机器人仿真软件坐标系下的数据；

采用匹配算法验证所述标准DAE格式的数据与所述机器人仿真软件坐标系下的数据之间的精确度满足要求。

需要说明的是，在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行指令的定序列表，可以具体实现在任何计算机可读介质中，以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用，或结合这些指令执行系统、装置或设备而使用。就本说明书而言，“计算机可读介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。计算机可读介质的更具体的示例(非穷尽性列表)包括以下：具有一个或多个布线的电连接部(电子装置)，便携式计算机盘盒(磁装置)，随机存取存储器(RAM)，只读存储器(ROM)，可擦除可编辑只读存储器(EPROM或闪速存储器)，光纤装置，以及便携式光盘只读存储器(CDROM)。另外，计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序，然后将其存储在计算机存储器中。

应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(PGA)，现场可编程门阵列(FPGA)等。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (15)

1.一种电力杆塔作业平台的控制方法，其特征在于，所述方法包括：

分别对电力杆塔作业平台和电力杆塔进行建模，得到电力杆塔作业平台三维模型和电力杆塔三维模型；

将所述电力杆塔作业平台三维模型和所述电力杆塔三维模型进行耦合，产生控制指令；

向所述电力杆塔作业平台发送所述控制指令。

2.如权利要求1所述的电力杆塔作业平台的控制方法，其特征在于，所述控制指令包括最优路径信息和夹持点序列信息，将所述电力杆塔作业平台三维模型和所述电力杆塔三维模型进行耦合，产生控制指令，包括：

利用所述电力杆塔作业平台三维模型在所述电力杆塔三维模型上进行任务仿真，采用动态规划算法，确定节点板序列作为所述电力杆塔作业平台的最优路径；

基于所述最优路径，采用步态规划算法求取所述电力杆塔作业平台的夹持点位置，得到所述夹持点序列。

3.如权利要求2所述的电力杆塔作业平台的控制方法，其特征在于，所述利用所述电力杆塔作业平台三维模型在所述电力杆塔三维模型上进行任务仿真，采用动态规划算法，确定节点板序列作为所述电力杆塔作业平台的最优路径，包括：

以所述电力杆塔三维模型中节点板上某一个螺栓位置点代表该节点板在空间中的位置，并基于各所述节点板在空间中的位置，计算任意两所述节点板之间的欧式距离；

以各所述节点板的位置作为有向图的点，以任意两所述节点板之间的欧式距离作为点到点之间的弧，构造二维有向加权图；

将所述节点板的高度作为每一个阶段的状态的划分依据，确定状态转移规划方程；

基于所述状态转移规划方程在所述二维有向加权图中采用顺序和/或逆序方式进行动态规划搜索解空间，形成一条从初始点E到终止点F的节点板序列作为最优路径。

4.如权利要求3所述的电力杆塔作业平台的控制方法，其特征在于，将所述节点板的高度作为状态每一个阶段的划分依据，确定状态转移规划方程，包括：

令所述电力杆塔作业平台三维模型经过各所述节点板的航迹点的坐标为(x_i,y_i,z_i)，以高度z_i为离散化状态阶段的划分依据，将所述输电塔体的整体高度划分为J个阶段，令第k阶段的状态为S_k，状态S_k的决策变量表示为T_k(s_k)，则第k+1阶段的状态表示为：S_k+1＝C(S_k,T_k(S_k))，k＝1,2,......K；状态S_k和状态S_k+1之间的代价定义为两点的欧式距离d(S_k+1,S_k)。

5.如权利要求4所述的电力杆塔作业平台的控制方法，其特征在于，在所述二维有向加权图中采用顺序方式进行动态路径规划的公式表示为：

式中：u_k+1(s_k+1)为从初始点E到第k+1阶段末状态S_k+1的最优指标函数；u₀(s₀)为初始点E的初始状态；u_k(s_k)为初始点E到第k阶段状态的最优指标函数；d(s_k,s_k+1)为状态S_k与状态S_k+1之间的代价；

在所述二维有向加权图中采用逆序方式进行动态路径规划的公式表示为：

式中：u_k(s_k)为从第k阶段末状态S_k到终止点F的最优指标函数；u_K(s_K)为终止点F的结束状态；u_k+1(s_k+1)为结束点F到第k+1阶段状态的最优指标函数；d(s_k,s_k+1)为状态S_k与状态S_k+1之间的代价。

6.如权利要求2所述的电力杆塔作业平台的控制方法，其特征在于，所述电力杆塔作业平台包括作业机器人，所述电力杆塔作业平台三维模型为作业机器人三维模型，该作业机器人包括上夹爪、下夹爪和螺栓打击器，所述基于所述最优路径，采用步态规划算法求取所述电力杆塔作业平台的夹持点位置，得到所述夹持点序列，包括：

从所述电力杆塔三维模型的原始数据中确定避障区域集[(E₁₁,E₁₂)，(E₂₁,E₂₂),...,(E_t1,E_t2),...,(E_T1,E_T2)]，(E_t1,E_t2)表示第t个障碍物对应的避障区域；

计算所述作业机器人三维模型从起始位置移动到目标螺栓工作点所需距离d；

将距离d和最大步距D进行比较，得出整数步及余数；

每一次行走一个整数步或者最后一次行走一个余数步后，求出所述作业机器人三维模型上夹持点的位置和下夹持点的位置，所述上夹持点和下夹持点分别为所述上夹爪和下夹爪的中心线；

基于所述上夹持点的位置、下夹持点的位置和避障区域集，规划所述作业机器人三维模型从起始位置到目标螺栓工作点的夹持点序列。

7.如权利要求6所述的电力杆塔作业平台的控制方法，其特征在于，所述计算所述作业机器人三维模型从起始位置移动到目标螺栓工作点所需距离d，包括：

获取所述作业机器人三维模型的下夹持点在所述电力杆塔三维模型中主材上到底部的距离L₃；

根据所述距离L₃和所述作业机器人三维模型的下夹持点到所述螺栓打击器的距离L₂，计算上夹持点到底部挡板的距离L₁；

根据所述目标螺栓工作点的位置与当前螺栓打击位置，计算所述作业机器人三维模型从当前螺栓打击位置移动到所述目标螺栓工作点位置需要移动的位移d：

d＝(Z_target-Z_work)/sin(theta)

式中：theta为所述电力杆塔三维模型的倾斜角；Z_target为目标螺栓工作点的高度坐标；Z_work为当前螺栓打击点的高度坐标。

8.如权利要求6所述的电力杆塔作业平台的控制方法，其特征在于，所述基于所述上夹持点的位置、下夹持点的位置和避障区域集，规划所述作业机器人三维模型从起始位置到目标螺栓工作点的夹持点序列，包括：

每行走一步，判断所述上夹持点或下夹持点的位置是否在所述避障区域集中任一避障区域(E_t1,E_t2)内；

若是，则将避障区域的下界作为该次夹持点，下夹爪攀爬至E_t1-D₁处，并更新所述距离L₃＝E_t1-L₅+(i-1)*D，其中，L₅＝L₃+D₁；

若否，则所述上夹爪和下夹爪同时沿着主材移动所述最大步距D；

在抵达所述目标螺栓工作点位置时，输出所述夹持点序列。

9.如权利要求7所述的电力杆塔作业平台的控制方法，其特征在于，所述方法还包括：

判断所述最优路径中是否存在相邻两个所述节点板的高度一致；

若是，则将原theta、d值存入程序堆栈，并更新theta＝0、d＝||y₁-y₂||，y₁、y₂为相邻两个所述节点板的坐标；

若否，则重新从程序堆栈获取原theta、d值。

10.如权利要求1所述的电力杆塔作业平台的控制方法，其特征在于，所述方法还包括：

获取所述电力杆塔作业平台发送的实际状态信息，并计算所述实际状态信息与目标状态信息的差值，产生修正指令；

向所述电力杆塔作业平台发送所述修正指令。

11.如权利要求10所述的电力杆塔作业平台的控制方法，其特征在于，所述电力杆塔作业平台包括作业机器人，所述获取所述电力杆塔作业平台发送的实际状态信息，包括：

在所述作业机器人进行作业过程中，利用相机进行实时扫描检测，采集电力杆塔图像；

基于所述电力杆塔图像，对所扫描到的特征点进行计数，并获取对应的特征点ID信息；

对所述相机与所述作业机器人的躯干进行标定，求解相机与所述作业机器人之间的相对位姿矩阵

基于所述特征点ID信息，在预先构建的匹配信息数据库中进行检索，获得当前扫描到的特征点所在坐标系与电力杆塔所在的世界坐标系之间的转换矩阵

其中，所述匹配信息数据库包括各特征点的位姿信息和特征点ID；

采用相机模型对相机进行标定，并对扫描得到的特征点进行位姿估计，求解相机坐标系与特征点所在坐标系之间的转换矩阵

基于所述相对位姿矩阵

所述转换矩阵

和所述转换矩阵

求解作业机器人在电力杆塔上的实际位姿状态信息

为：

12.如权利要求1所述的电力杆塔作业平台的控制方法，其特征在于，在所述分别对电力杆塔作业平台和电力杆塔进行建模，得到电力杆塔作业平台三维模型和电力杆塔三维模型之后，所述方法还包括：

从所述电力杆塔三维模型的原始数据中提取有效数据；

对所述有效数据进行加工处理，输出标准DAE格式的数据；

基于机器人仿真软件坐标系，对所述标准DAE格式的数据进行坐标转换，得到所述机器人仿真软件坐标系下的数据；

采用匹配算法验证所述标准DAE格式的数据与所述机器人仿真软件坐标系下的数据之间的精确度满足要求。

13.如权利要求12所述的电力杆塔作业平台的控制方法，其特征在于，所述采用ICP匹配算法验证所述标准DAE格式的数据与所述机器人仿真软件坐标系下的数据之间的精确度满足要求，包括：

a)从所述标准DAE格式的数据中选取部分数据，构建标准点集X＝{x₁,x₂,...,x_N}；

b)从所述机器人仿真软件坐标系下的数据中选取部分数据，构建待匹配点集Y＝{y₁,y₂,...,y_N}；

c)对于对应点集合{(x₁,y₁),(x₂,y₂),...,(x_Ny_N)}，确定旋转矩阵R和平移矩阵t，使得误差函数的误差最小，所述误差函数为：

式中：E(R,t)为目标函数；|| ||为范数符号；

d)判断所述误差函数的最小误差是否满足精度阈值要求，若是则执行步骤e)，若否则执行步骤f)；

e)确定所述标准DAE格式的数据与所述机器人仿真软件坐标系下的数据之间的精确度满足要求；

f)利用旋转矩阵R和平移矩阵t，对所述待匹配点集Y进行旋转和平移变换，更新所述待匹配点集Y；

g)利用更新后的所述待匹配点集Y和所述标准点集X，更新所述对应点集合，然后执行步骤c)，直至误差满足精度阈值要求或超过最大迭代次数时，退出迭代循环。

14.如权利要求13所述的电力杆塔作业平台的控制方法，其特征在于，所述对于对应点集合{(x₁,y₁),(x₂,y₂),...,(x_Ny_N)}，确定旋转矩阵R和平移矩阵t，使得误差函数的误差最小，包括：

分别计算所述标准点集X和所述待匹配点集Y的质心，得到质心u_x和质心u_y；

分别对所述标准点集X和所述待匹配点集Y进行去中心化处理，得到去中心化的点集x_i′和点集y_i′；

基于所述去中心化的点集x_i′和点集y_i′，构建矩阵H；

对所述矩阵H进行svd分解，得到分解矩阵；

基于所述分解矩阵，计算旋转矩阵R；

基于所述旋转矩阵R、质心u_x和质心u_y，计算平移矩阵t。

15.一种电力杆塔作业平台的控制系统，其特征在于，所述系统包括：虚拟控制平台和电力杆塔作业平台，所述虚拟控制平台包括作业机器人三维模型、电力杆塔三维模型和决策器；所述决策器包括控制指令产生模块和信息传输模块：

所述控制指令产生模块，用于将所述作业机器人三维模型和所述电力杆塔三维模型进行耦合，产生控制指令；

所述信息传输模块，用于向所述电力杆塔作业平台发送所述控制指令。

Images