具体实施方式

下面结合附图对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。

实施例1

本发明的一种提高配网带电作业机器人的精准作业方法，参见图 1所示，包括以下过程：

第一步：绝缘斗移动到水平激光雷达和垂直激光雷达无障碍物遮挡扫描作业环境的最佳位姿。采用水平和垂直的激光雷达扫描作业环境得到对应的作业环境点云数据，并将两个激光雷达扫描的作业环境点云数据的坐标系转换到绝缘斗坐标系下，此时作业环境点云数据记为C₀。

现有技术中，参见图2-图6所示，配网带电作业机器人固定安装在绝缘斗内，定义绝缘斗坐标系81和配网带电作业机器人底座坐标系重合。水平激光雷达2安装在配网带电作业机器人底座上方，垂直激光雷达3安装在配网带电作业机器人底座正前方。其中，水平激光雷达2扫描范围为底座正上方范围；垂直激光雷达3扫描范围是底座正前方范围。

控制绝缘斗移动到初始位姿位置，即水平激光雷达和垂直激光雷达无障碍物遮挡扫描作业环境的最佳位姿。保持绝缘斗平面始终水平，使得安装在机器人底座上的水平和垂直激光雷达能够扫描完整作业环境，主要扫描行线、引线和杆塔4，另还会有杆塔周边的树枝等障碍物。行线有三根，以机器人底座正前方方向的顺序分别为近边线51、中间线52、远边线53。扫描后得到对应的作业环境点云数据，

水平激光雷达和垂直激光雷达扫描作业环境示意参见图5所示，其中，在水平激光雷达扫描面中包括：水平激光雷达扫描面中近边线对应的点61，水平激光雷达扫描面中中间线对应的点62，水平激光雷达扫描面中远边线对应的点63，垂直激光雷达扫描面中包括：垂直激光雷达扫描面中杆塔对应的点7。

点云数据主要是物体表面的点坐标，是基于各自激光雷达坐标系下的。其中水平激光雷达坐标系以转台安装位置的中心为原点，转台旋转轴为Z轴(底座正上方)，底座右侧方向为X轴方向，底座正前方方向为Y轴方向。垂直激光雷达坐标系以转台安装位置的中心为原点，转台旋转轴为Z轴(底座正前方)，底座右侧方向为X轴方向，底座正下方方向为Y轴方向。将两个不同坐标系的点云合并到统一坐标系下，并且合并为一个点云。

本发明中主要涉及作业环境坐标系82和绝缘斗坐标系81，其具体方向示意参见图6所示。坐标系的具体定义为：

定义作业环境坐标系82为：杆塔与行线的远边线和近边线组成平面的交点作为作业环境坐标系原点，Z轴方向沿杆塔朝上，Y轴方向平行近边线，X轴方向由近边线朝向远边线。此作业环境坐标系是固定不动的，相当于世界坐标系作用。

定义绝缘斗坐标系81为：以指定的机械臂安装位置中心为原点，底座正上方为Z轴正，底座正前方为X轴正向，底座左侧方向为Y 轴正向。

第二步：绝缘斗位姿初次测量：记录采集作业环境点云数据时绝缘斗位姿M₀。

垂直激光雷达转台转动角度，该角度是转台自身的旋转角度，转台角度0刻度定义是以底座右方(也就是对应激光雷达的坐标系X 轴正方向，与X轴正向夹角)，逆时针增大，顺时针减小。

水平激光雷达转台转动角度，该角度是转台自身的旋转角度，转台角度0刻度定义是以底座右方(也就是对应激光雷达的坐标系X 轴正方向，与X轴正向夹角)，逆时针增大，顺时针减小。

绝缘斗位姿测量包括一下步骤：

1)控制垂直激光雷达转台转动到指定角度θ_HR使得能够扫到杆塔位置，得到杆塔在垂直激光雷达坐标系下坐标，根据垂直激光雷达坐标系与绝缘斗坐标系的关系，进一步得到杆塔在绝缘斗坐标系下X 轴与Y轴坐标，也就是绝缘斗相对于环境坐标系的X轴与Y轴坐标，分别记为X₀，Y₀。

2)控制水平激光雷达转台转动到θ_VR＝PI/2弧度，其中，PI为圆周角。

测量得到行线在水平激光雷达坐标系下的Z轴坐标Z₀，也是绝缘斗坐标系下行线与机器人底座高度距离Z₀，则得到绝缘斗坐标相对于作业环境坐标系位置为

3)控制水平激光雷达转台转动，指定角度测量远边线和近边线距离。

控制水平激光雷达转台转动到θ_VR＝PI/2+θ弧度，其中θ为经验值，记录行线的远边线和近边线两个点连成直线AB的距离记为L₁，该直线AB与行线夹角记为θ₁；

控制水平激光雷达转台转动到θ_VR＝PI/2-θ弧度，记录行线的远边线和近边线两个点连成直线CD距离记为L₂。该直线CD与行线夹角记为θ₂。

4)计算当前绝缘斗转动角度，进而得到绝缘斗的姿态。

比较L₁和L₂值的大小。

如果L₁>L₂，说明绝缘斗绕Z轴逆时针转动，则绝缘斗绕Z轴逆时针旋转弧度角记Angle＝θ_Z，3×3旋转矩阵记为RZ₀(θ_Z)，其中RZ₀()表示绕Z轴旋转矩阵，其中旋转矩阵具体为

绝缘斗相对作业环境坐标系绕Z轴逆时针旋转作业场景俯视图参见图3所示。图中标记21为水平激光雷达转台转动到θ_VR＝PI/2+θ弧度时扫描面，22为水平激光雷达转台转动到θ_VR＝PI/2弧度时扫描面， 23为水平激光雷达转台转动到θ_VR＝PI/2-θ弧度时扫描面，θ₁为水平激光雷达转台转动到θ_VR＝PI/2+θ弧度时扫描面21与作业环境的近边线 51之间的夹角，θ₂为水平激光雷达转台转动到θ_VR＝PI/2-θ弧度时扫描面22与作业环境的近边线51之间的夹角。

通过L₁、L₂、θ进行求解L和θ₁。具体计算过程为：根据几何原理，得到关系式：L＝L₁*sin(PI-θ₁)，θ₁＝θ₂+2*θ，L＝L₂*sin(θ₂)。由此推导得到：那么远边线和近边线实际平行距离L＝L₁*sin(PI-θ₁)，绝缘斗绕Z轴逆时针旋转弧度角θ_Z＝θ₁-θ-PI/2。将上述推导出θ₁的公式带入，即可得到距离L和绝缘斗弧度角θ_Z关于θ角度的表达式。

则得到此时绝缘斗坐标相对于作业环境坐标系旋转矩阵为 RZ₀(θ_Z)，位姿为其中E为3×3单位矩阵。

如果L₁<L₂，说明绝缘斗绕Z轴顺时针转动，则绝缘斗绕Z轴顺时针旋转弧度角记为Angle＝-θ_Z，3×3旋转矩阵记为RZ₀(-θ_Z)。

绝缘斗相对作业环境坐标系绕Z轴顺时针旋转作业场景俯视图参见图2所示。

根据几何原理，得到关系式：L＝L₁*sin(θ₁)，θ₁＝θ₂+2*θ， L＝L₂*sin(θ₂)。由此推导得到：那么远边线和近边线实际距离L＝L₁*sin(PI-θ₁)，绝缘斗绕Z轴顺时针旋转弧度角 Angle＝-θ_Z＝(θ₁-θ)-PI/2。

则得到此时绝缘斗坐标相对于作业环境坐标系旋转矩阵为 RZ₀(-θ_Z)，位姿为其中E为3×3单位矩阵。

如果L₁＝L₂，即转台弧度在PI/2左右转动相同角度θ时测量远近边线的距离相等，说明转台弧度在PI/2的扫描面是和行线垂直的，又因为该位置扫描面是和绝缘斗的正面相互垂直，所以绝缘斗的正面是和行线平行的。说明绝缘斗没有绕Z轴旋转，则绝缘斗绕Z轴旋转弧度角为零，Angle＝0，记为RZ₀(0)。则得到此时绝缘斗坐标相对于作业环境坐标系旋转矩阵为RZ₀(0)，位姿为其中E为3×3单位矩阵。

绝缘斗相对作业环境坐标系绕Z轴旋转弧度角为零作业场景俯视图参见图4所示。根据几何原理，得到远边线和近边线实际距离： L＝L₁*cos(θ)。

第三步：移动绝缘斗到达作业目标位姿，使得配网带电作业机器人在最佳作业空间，机器人可以进行直接对行线或引线作业。控制水平激光雷达转台转动角度θ_VR＝PI/2，记录实时获取到的近边线和远边线距离记为L_C，行线与机器人底座实时高度距离为Z_C。垂直激光雷达实时获取得到杆塔相对绝缘斗坐标系下X轴与Y轴实时距离，也就是绝缘斗相对于环境坐标系的X轴与Y轴坐标，分别记为X_C，Y_C；则得到绝缘斗坐标相对于作业环境坐标系位置为

其中近边线和远边线距离，由远边线坐标(XYZ)和近边线坐标 (XYZ)通过欧拉距离求得两点之间距离。

第四步：实时测量绝缘斗目标位姿M。

判断L_C与L是否相等。

如果L_C＝L，说明此时水平激光雷达扫描面与行线垂直，读取此时水平激光雷达转台的角度传感器值θ_VRS，则绝缘斗绕Z轴逆时针旋转弧度角实时值θ_CZ＝PI/2-θ_VRS，得到此时绝缘斗坐标相对于作业环境坐标系旋转矩阵为RZ₀(θ_CZ)，目标位姿为其中E为3×3单位矩阵。

如果L_C≠L，控制水平激光雷达转台以当前位置基础上逆时针转动测试角度θ_TZ，记录测试此时得到近边线和远边线距离L_CT，其中θ_TZ为转台测试角度，此数值依据经验决定(数值较小)。如果L_CT≤L，继续控制水平激光雷达转台以当前位置基础上逆时针转动测试角度θ_TZ，如果L_CT>L，继续控制水平激光雷达转台以当前位置基础上顺时针转动测试角度θ_TZ，直至测量得到近边线和远边线距离L_CT＝L，水平激光雷达扫描面和行线垂直，读取此时水平激光雷达转台的角度传感器值θ_VRS，则绝缘斗绕Z轴逆时针旋转弧度角实时值θ_CZ＝PI/2-θ_VRS，得到此时绝缘斗坐标相对于作业环境坐标系旋转矩阵为RZ₀(θ_CZ)，目标位姿为其中E为3×3单位矩阵。

第五步：求点云数据误差矩阵，实时修正作业环境点云数据。

根据采集作业环境点云数据时绝缘斗初始位姿和绝缘斗当前实时目标位姿，求得两者之间的误差矩阵T_E＝inv(M₀)·M，其中inv() 表示矩阵的求逆运算。绝缘斗位姿之间的误差矩阵也就是初始点云数据C₀与实际作业环境中点云数据存在的误差矩阵。

利用误差矩阵，对作业环境点云数据进行误差修正，实时修正作业环境点云数据公式为C＝C₀·T_E。

第六步：在修正后的点云数据中选择作业点，继续完成配网带电作业任务。

由于绝缘斗震动或移动，导致绝缘斗相对作业环境坐标系的位置或姿态发生变化，造成作业环境点云与作业环境之间位置数据发生变化。后续只要重复测量绝缘斗当前目标位姿，通过利用误差矩阵，对作业环境点云数据进行误差修正，提高机器人精准作业，减少激光雷达对作业环境点云数据采集次数，缩短作业时间。

本发明能够根据采集作业环境点云数据时绝缘斗位姿和当前实时绝缘斗位姿，快速建立作业环境与作业环境点云数据之间的实时误差矩阵，只需初始时对作业环境进行一次激光点云数据采集，即可依据误差矩阵对作业环境点云数据进行实时修正，解决了绝缘斗移动导致实际作业环境与作业环境点云数据存在偏差，作业精度低问题，提高了作业精准度，减少激光雷达对作业环境扫描采集次数，大大缩短配网带电作业时间，给居民企业用电带来方便，减少作业过程中带来的不便影响，具有较高经济使用价值。

实施例2

基于与实施例1同样的发明构思，本发明的一种提高配网带电作业机器人的精准作业装置，包括：

初始数据采集模块，用于扫描作业环境得到初始的作业环境点云数据；

初始位姿计算模块，用于计算扫描作业环境时绝缘斗的初始位姿；

目标位姿计算模块，用于调整绝缘斗位姿使得配网带电作业机器人达到最佳作业空间，计算此时绝缘斗的目标位姿；

数据误差计算模块，用于计算绝缘斗的初始位姿和目标位姿之间的误差矩阵；

目标数据修正模块，用于根据误差矩阵修正初始的作业环境点云数据，以使配网带电作业机器人基于修正后的作业环境点云数据作业。

本发明装置中各模块的具体实现方案参见方法中各步骤处理过程。

相应的，基于与实施例1同样的发明构思，本发明还提供了如下终端。

一种终端，所述终端包括处理器、与所述处理器耦接的存储器，其中，所述存储器存储有用于执行上述一种提高配网带电作业机器人的精准作业方法的程序指令；

所述处理器用于执行所述存储器存储的所述程序指令以控制执行上述一种提高配网带电作业机器人的精准作业方法。

一种存储介质，存储有处理器可运行的程序指令，所述程序指令用于执行上述一种提高配网带电作业机器人的精准作业方法。

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

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

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

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

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变型，这些改进和变型也应视为落入本发明的保护范围。