CN111645079B - 一种带电作业机器人机械臂路径规划控制装置及其方法 - Google Patents

Abstract

本发明公开了一种带电作业机器人机械臂路径规划控制装置及其方法，其中控制装置包括状态感知器、动作选择器、评价信号发生器和动态更新机构；方法主要基于正方体空间划分方法、强化学习算法和逆向运动学求解，并依靠控制装置，来生成最优路径。本发明能够准确规划出一条路径，使机器人可以从初始位置到达目标位置，并且绕过周围障碍物，从而实现带电作业机器人的自主作业。

Description

一种带电作业机器人机械臂路径规划控制装置及其方法

技术领域

本发明涉及电力线路带电作业机器人领域，尤其是一种带电作业机器人机械臂路径规划控制装置及其方法。

背景技术

当通过带电作业机器人进行配电线路接、断引线等带电作业时，需要人为设置路径并控制绝缘斗臂车将机器人的机械臂移至初步作业位置，其中人为设定的路径仅是根据作业现场情况估算出来的，并非最优路径，这样会导致机器人机械臂难以到达最佳的作业位置，影响其精准操作，而且人为设定路径的方式使得机器人的自动化程度低，致使工作效率低下。

发明内容

本发明的目的在于弥补现有技术的不足之处，提供一种带电作业机器人机械臂路径规划控制装置及其方法，其能够准确规划出一条路径，使机器人可以从初始位置到达目标位置，并且绕过周围障碍物（如其他导线、周围存在的金具、树枝等），从而实现带电作业机器人的自主作业。

为了解决上述技术问题，本发明采用如下技术方案：

一种带电作业机器人机械臂路径规划控制装置，其特征在于，包括状态感知器、动作选择器、评价信号发生器和动态更新机构。

状态感知器：通过传感器读取用于带动末端设备移动的各连杆间的关节转角θ，确定各连杆的状态S_k；然后，用坐标变换公式得到末端设备每次移动后的位置；最后，给出能够到达的下一时刻状态S_k+1。

动作选择器：存储过去通过强化学习获得的历史学习经验数据，根据这些历史学习经验数据和状态感知器给出的下一时刻状态S_k+1，应用动作选择策略选择出能够获得最佳执行效果的动作a_k，并执行该动作。

评价信号发生器：当执行完被选中的动作a_k后，评价信号发生器要对动作的执行效果给出返馈值r_k，用来影响今后选中这个动作的概率。

动态更新机构：根据返馈值r_k对动作选择器中的历史学习经验数据进行更新。

一种带电作业机器人机械臂路径规划方法，包括以下步骤：

步骤1、获取环境信息，确定作业空间，将作业空间看作正方体并均匀划分为多个小方块，其中依据工控机的计算能力和可承受的时间来确定小方块的数量。

步骤2、对每个小方块分别赋予一个特定名称。

步骤3、对空间范围内包含有障碍物的小方块进行标定并统一定义为障碍物方块。

步骤4、确定机械臂的初始位置和目标位置，将初始位置和目标位置对应至相应的小方块中并定义为初始方块和目标方块；确定状态集和动作集，状态集为所有小方块的位置；设置返馈值函数。

步骤5、将初始方块和目标方块的位置、状态集、动作集和返馈值函数输入至带电作业机器人机械臂路径规划控制装置，运用强化学习算法，躲避障碍物方块并生成从初始方块至目标方块的最优的路径，且该路径用多个子目标点表示。

步骤6、对子目标点进行合并得到若干个分目标点，以获得可执行的最简化路径。

步骤7、按最简化路径对机械臂求运动学逆解解集，再由包络碰撞检测法和加权最短行程法得到最优解。

步骤8、向操控装置下达控制指令，使机械臂到达目标位置。

步骤9、以机械臂末端执行器的最终目标位置为基准，兼顾机械臂完成步骤8之后的剩余自由度，如有必要，在接近最终目标位置的更小空间内重复步骤1至步骤8进行二次或多次更加精细的空间划分和路径规划。

进一步地，在步骤4中，反馈值函数采用下面的优化方式设置，以避免绕路现象：

式中，m>n>p>q，m典型值为100，n典型值为5，s典型值为2，p典型值为-15，q典型值为-100，将状态为S_k的小方块定义为S_k方块，将状态为S_k+1的小方块定义为S_k+1方块，d(S_k)表示S_k方块距离目标方块的距离，d(S_k+1)表示S_k+1方块距离目标方块的距离，D(S_k+1)表示S_k+1方块距离障碍物方块的距离；条件S定义为：S_k+1方块的周围若存在障碍物方块，则过S_k+1方块中心点与障碍物方块中心点作连线，垂直于该连线的障碍物方块平面如果正好将S_k+1方块与障碍物方块隔开，则条件S成立。

进一步地，在步骤6中，对子目标点进行合并是指合并在一条直线上的各子目标点。

进一步地，在步骤7中，包络碰撞检测法是指圆柱包络碰撞检测法。

进一步地，在步骤7中，加权最短行程法是指在求最短行程解时需要加权，设每个关节转角变化量△θ_i的加权系数为ω_i，则最短行程解的约束为min∑|ω_i·△θ_i|。

本发明的有益效果是：

1、实现了机械臂自主路径规划。通过运用机器学习的方法，自主规划出从初始位置至作业目标位置的路径。

2、使机械臂能够有效的躲避周围障碍物。既保证了运动路径所经过线路上没有障碍物，又保证了机械臂在运动过程中不会碰到障碍物。

3、将带电作业机器人的动作过程划分为多个步骤，按照动作范围的大小逐步利用正方体空间划分方法及强化学习等求得最优路径，可提升计算效率、节约时间。

附图说明

为了使本发明的优点更容易理解，将通过参考在附图中示出的具体实施方式更详细地描述上文简要描述的本发明。可以理解这些附图只描绘了本发明的典型实施方式，因此不应认为是对其保护范围的限制，通过附图以附加的特性和细节描述和解释本发明。

图1为本发明中带电作业机器人机械臂路径规划控制装置的结构框图。

图2为本发明中机器学习过程示意图。

图3为本发明中正方体空间划分方法的示意图。

图4为本发明中带电作业机器人机械臂路径规划方法的流程图。

图5为本发明中圆柱包络碰撞检测法的示意图。

其中，1-障碍物方块, 2-初始方块, 3-目标方块。

具体实施方式

在下文的描述中，给出了大量具体的细节以便提供对本发明更为彻底的理解。然而，对于本领域技术人员来说显而易见的是，本发明实施方式可以无需一个或多个这些细节而得以实施。在其他的例子中，为了避免与本发明实施方式发生混淆，对于本领域公知的一些技术特征未进行描述。

为了彻底了解本发明实施方式，将在下列的描述中提出详细的结构。显然，本发明实施方式的施行并不限定于本领域的技术人员所熟习的特殊细节。本发明的较佳实施方式详细描述如下，然而除了这些详细描述外，本发明还可以具有其他实施方式。

在本发明的描述中，术语“内侧”、“外侧”、“纵向”、“横向”、“上”、“下”、“顶”、“底”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明而不是要求本发明必须以特定的方位构造和操作，因此不能理解为对本发明的限制。

下面结合附图对本发明的一种实施方式进行详细说明，主要是先对带电作业机器人的机械臂进行粗略路径规划，再对机械臂末端执行器进行微观路径规划，最后将两个路径合一，得到总路径；如同人的手臂，先由胳膊移动到手能触及到的物体附近，胳膊不再移动，再由手腕、手指定位到物体进行抓取。

本发明提供的一种带电作业机器人机械臂路径规划控制装置（以下简称“路径规划控制装置”），主要由状态感知器、动作选择器、评价信号发生器和动态更新机构四部分组成，并运用机器学习中的强化学习方法进行路径规划。如图1所示，这四部分的功能如下：

状态感知器：通过传感器读取用于带动末端设备移动的各连杆间的关节转角θ，确定各连杆的状态S_k；然后，用坐标变换公式得到末端设备每次移动后的位置；最后，给出能够到达的下一时刻状态S_k+1。其中在粗略路径规划中，末端设备是处于绝缘斗臂车末端的机械臂，在微观路径规划中，末端设备是机械臂末端执行器。

动作选择器：存储过去通过强化学习获得的历史学习经验数据，根据这些历史学习经验数据和状态感知器给出的下一时刻状态S_k+1，应用动作选择策略选择出能够获得最佳执行效果的动作a_k，并执行该动作。

评价信号发生器：当执行完被选中的动作a_k后，评价信号发生器要对动作的执行效果给出返馈值r_k，用来影响今后选中这个动作的概率。如果某个动作导致返馈值r_k低，那么产生这个动作被选中的概率将会逐渐减小；反之，如果某个动作执行后效果非常好，即得到的返馈值r_k高，则在将来的运动中选择该动作的概率将会逐渐增加。

动态更新机构：根据返馈值r_k对动作选择器中的历史学习经验数据进行更新。

通过路径规划控制装置，并运用机器学习中的强化学习算法，通过迭代计算最后收敛，得到决策数值表，从而可规划出最佳路径，过程如图2所示,其中，S_k、S_k+1、S_k+2分别表示末端设备的第k个状态、第k+1个状态和第k+2个状态，k为自然数；r_k、r_k+1、r_k+2分别为执行完每步动作后获得的返馈值；a_k,1、a_k,2、a_k,3、a_k,4为在S_k状态下各种可选择的动作，a_k+1,1、a_k+1,2、a_k+1,3、a_k+1,4为在S_k+1状态下各种可选择的动作，a_k+2,1、a_k+2,2、a_k+2,3、a_k+2,4为在S_k+2状态下各种可选择的动作；Q(S_k，a_k,1)为在状态S_k下选择第1种动作所对应的值，其他同理，在同一状态下，所对应的值越大，表明选择该动作的概率越大。机械臂的路径规划以及机械臂末端执行器的路径规划均是基于路径规划控制装置来实现的。

为了提高运算速度，根据工况的难易程度，可以选择运用正方体空间划分方法对和作业空间进行划分。如图3所示，对正方体空间划分方法作进一步说明：将作业空间看作成正方体，将正方体划分成3×3×3个小方块，当然也可以划分成4×4×4个小方块或更多个小方块，依据机器人系统中工控机的计算能力和可承受的时间来确定，其中可承受的时间是指为提高效率等由人为设定的操作时间，划分的小方块少可以节约操作时间，但精度低，反之，划分的小方块多，可以提高精度，但是耗费时间长，根据作业现场实际情况，可设定一个合理的操作时间。在图3中，大正方体视作机械臂的作业空间，大正方体中的各个小方块的位置构成了机械臂的状态集，基于各个小方块确定机械臂的动作集为{向上移动，向下移动，向左移动，向右移动，向前移动，向后移动}；对包含有障碍物的小方块定义为障碍物方块1并标定，如图3中黑色的方块，在大正方体中还可以直接确认出初始方块2和目标方块3；依据各个小方块距离目标方块的距离以及是否为障碍物方块、是否为边缘方块，给出做出某个动作后的返馈值，并基于此归类总结成返馈值函数。其中，初始方块2和目标方块3的位置、状态集、动作集和返馈值函数是强化学习算法所需的输入参数，运用强化学习算法且通过参数的输入可生成机械臂的最优路径。在图3中，目标方块3可视作机械臂末端执行器的作业空间，可依照上述机械臂路径规划中利用的正方体空间划分方法对机械臂末端执行器的作业空间进行进一步地划分，并最终得到强化学习算法所需的输入参数，并生成机械臂末端执行器的最优路径。正方体空间划分方法为强化学习算法提供了必要的参数基础，由粗到细的逐步利用正方体空间划分方法还可提高运算速度，减轻机器人系统的计算压力。

一种带电作业机器人机械臂路径规划方法（以下简称“路径规划方法”），主要基于正方体空间划分方法、强化学习算法和逆向运动学求解来实现的。具体结合图4对路径规划方法作如下说明：

步骤1、通过激光雷达扫描获取环境信息，确定机械臂的作业空间，将作业空间看作正方体并均匀划分为3×3×3个小方块，共27个小方块。

步骤2、对每个小方块分别赋予一个特定名称，特定名称可以是每个小方块的三维坐标系，分别记为（1，1，1）、（1，1，2）、（1，1，3）、（2，1，1）、（2，1，2）、（2，1，3）、。。。。。。、（3，3，3），以用于各个小方块之间的区分。

步骤3、依据激光雷达的扫描结果，对空间范围内包含有障碍物的小方块进行标定并统一定义为障碍物方块。如经扫描障碍物只涉及（1，1，3）和（2，1，3）这两个小方块的空间范围，则标定这两个小方块为障碍物方块，后面算法在处理时将对这两个障碍物方块设定特别的反馈值。

步骤4、确定机械臂的初始位置和目标位置，将初始位置和目标位置对应至相应的小方块中并定义为初始方块和目标方块；如初始方块为（1，1，1），目标方块为（3，1，3）。

确定状态集和动作集，状态集为27个小方块的空间位置，即每个小方块的空间位置作为状态集的一个元素，动作集优选为{向上移动，向下移动，向左移动，向右移动，向前移动，向后移动}，包含6个元素，当然动作集还可以是向中心小方块周围26个小方块移动的动作集，包含26个元素，选哪种动作集取决于工控机的计算能力。

设置返馈值函数，其中，反馈值函数采用下面的优化方式设置，以避免绕路现象：

式中，m>n>p>q，m典型值为100，n典型值为5，s典型值为2，p典型值为-15，q典型值为-100，将状态为S_k的小方块定义为S_k方块，将状态为S_k+1的小方块定义为S_k+1方块，d(S_k)表示S_k方块距离目标方块的距离，d(S_k+1) 表示S_k+1方块距离目标方块的距离，D(S_k+1)表示S_k+1方块距离障碍物方块的距离；条件S定义为：S_k+1方块的周围若存在障碍物方块，则过S_k+1方块中心点与障碍物方块中心点作连线，垂直于该连线的障碍物方块平面如果正好将S_k+1方块与障碍物方块隔开，则条件S成立。

步骤5、将初始方块和目标方块的位置、状态集、动作集和返馈值函数输入至路径规划控制装置，运用强化学习算法，躲避障碍物方块并生成从初始方块至目标方块的最优的路径，且该路径用多个子目标点表示。如经计算得到路径为：（1，1，1）→（2，1，1）→（3，1，1）→（3，1，2）→（3，1，3）。

步骤6、对子目标点进行合并，主要是合并在一条直线上的各子目标点，得到若干个分目标点，其中分目标点比子目标点的数量少得多，故可以获得可执行的最简化路径。如步骤5中所得的路径为例，将（1，1，1）→（2，1，1）→（3，1，1）合并为（1，1，1）→（3，1，1），将（3，1，1）→（3，1，2）→（3，1，3）合并为（3，1，1）→（3，1，3），得到的最简化路径为：（1，1，1）→（3，1，1）→（3，1，3）。

步骤7、按最简化路径对机械臂求运动学逆解解集，再由圆柱包络碰撞检测法和加权最短行程法得到最优解。

步骤8、向操控装置下达控制指令，使机械臂由初始位置到达目标位置。

步骤9、以机械臂末端执行器的最终目标位置为基准，兼顾机械臂完成步骤8之后的剩余自由度，如有必要，在接近最终目标位置的更小空间内重复步骤1至步骤8进行二次或多次更加精细的空间划分和路径规划。

利用路径规划方法先对机械臂进行粗略路径规划，再对机械臂末端执行器进行微观路径规划，在微观路径规划中可根据实际需求进行多次更加精细的空间划分和路径规划，之后将所有路径合一，即得到总路径。为保障执行动作的连续性，可待所有路径规划生成后再一起执行动作；当然也可以是每条路径生成后，就立即执行相应的动作，并逐一完成后续的路径生成及动作的执行。

在路径规划方法中，以机械臂连杆和障碍物为例对圆柱包络碰撞检测法进行说明：将机械臂连杆和障碍物用合适直径及高度的圆柱进行包络，将其转化为圆柱体的方法。通过该方法，可将机械臂各连杆与障碍物之间的碰撞问题转化为判断圆柱与圆柱间位置关系的问题。

如图5所示，假设空间中机械臂连杆i首末端的位置坐标分别为P_i=(x_i，y_i，z_i)、P_i+1=(x_i+1，y_i+1，z_i+1)，其包络圆柱的半径为r_i；障碍物首、末端的位置坐标为P_j=(x_j，y_j，z_j)、P_j+1=(x_j+1，y_j+1，z_j+1)，其包络圆柱的半径为r_j。

机械臂及障碍物圆柱包络中心线方向向量分别为L_i=P_i+1-P_i、L_j=P_j+1-P_j。两中心线上任意取两点，这里不妨取P_i和P_j，得到向量L_ij= P_i- P_j，若机械臂与障碍物的圆柱包络中心线平行，则两中心线间的最短距离可表示为d_ij=|L_i×L_ij|/|L_i|。

若机械臂与障碍物的圆柱包络的中心线空间相交，则两中心线的公垂线向量可表示为n_ij=| L_i×L_j |，那么两中心线之间的距离可表示为d_ij=|n_ij·L_ij|/|n_ij|。当d_ij>r_i+r_j时，两圆柱体不会发生干涉，即该机械臂不会与障碍物发生碰撞；当d_ij≤r_i+r_j时，两圆柱体发生干涉，即该机械臂与障碍物发生碰撞。

同理，运用空间解析几何公式，同理可以采取球体包络、方块体包络，这要依据实际障碍物具体形状，以上仅以圆柱包络为例进行说明。

在路径规划方法中，以机械臂为例对加权最短行程法进行说明：机械臂从分目标点p_k移动到下一个分目标点p_k+1时，要使机械臂各关节转角的变化量△θ_i尽可能小，越长的连杆，转过相同角度移动距离越大，所以求最短行程解时需要加权，设每个关节转角变化量△θ_i的加权系数为ω_i，则最短行程解的约束为min∑|ω_i·△θ_i|。

综上所述，本发明的内容并不局限在上述的实施例中，本领域的技术人员可以在本发明的技术指导思想之内提出其他的实施例，但这些实施例都包括在本发明的范围之内。

Claims (6)

1.一种带电作业机器人机械臂路径规划方法，其特征在于，包括以下步骤：

步骤1、获取环境信息，确定作业空间，将作业空间看作正方体并均匀划分为多个小方块，其中依据工控机的计算能力和可承受的时间来确定小方块的数量；

步骤2、对每个小方块分别赋予一个特定名称；

步骤3、对空间范围内包含有障碍物的小方块进行标定并统一定义为障碍物方块；

步骤4、确定机械臂的初始位置和目标位置，将初始位置和目标位置对应至相应的小方块中并定义为初始方块和目标方块；确定状态集和动作集，状态集为所有小方块的位置；设置返馈值函数；

步骤5、将初始方块和目标方块的位置、状态集、动作集和返馈值函数输入至带电作业机器人机械臂路径规划控制装置，运用强化学习算法，躲避障碍物方块并生成从初始方块至目标方块的最优的路径，且该路径用多个子目标点表示；

步骤6、对子目标点进行合并得到若干个分目标点，以获得可执行的最简化路径；

步骤7、按最简化路径对机械臂求运动学逆解解集，再由包络碰撞检测法和加权最短行程法得到最优解；

步骤8、向操控装置下达控制指令，使机械臂到达目标位置；

步骤9、以机械臂末端执行器的最终目标位置为基准，兼顾机械臂完成步骤8之后的剩余自由度，如有必要，在接近最终目标位置的更小空间内重复步骤1至步骤8进行二次或多次更加精细的空间划分和路径规划。

2.根据权利要求1所述的带电作业机器人机械臂路径规划方法，其特征在于，在步骤4中，反馈值函数采用下面的优化方式设置，以避免绕路现象：

式中，m>n>p>q，m典型值为100，n典型值为5，s典型值为2，p典型值为-15，q典型值为-100，将状态为S_k的小方块定义为S_k方块，将状态为S_k+1的小方块定义为S_k+1方块，d(S_k)表示S_k方块距离目标方块的距离，d(S_k+1)表示S_k+1方块距离目标方块的距离，D(S_k+1)表示S_k+1方块距离障碍物方块的距离；条件S定义为：S_k+1方块的周围若存在障碍物方块，则过S_k+1方块中心点与障碍物方块中心点作连线，垂直于该连线的障碍物方块平面如果正好将S_k+1方块与障碍物方块隔开，则条件S成立。

3.根据权利要求1所述的带电作业机器人机械臂路径规划方法，其特征在于，在步骤6中，对子目标点进行合并是指合并在一条直线上的各子目标点。

4.根据权利要求1所述的带电作业机器人机械臂路径规划方法，其特征在于，在步骤7中，包络碰撞检测法是指圆柱包络碰撞检测法。

5.根据权利要求1所述的带电作业机器人机械臂路径规划方法，其特征在于，在步骤7中，加权最短行程法是指在求最短行程解时需要加权，设每个关节转角变化量△θ_i的加权系数为ω_i，则最短行程解的约束为min∑|ω_i·△θ_i|。

6.一种应用于权利要求1至5中任一项所述的带电作业机器人机械臂路径规划方法的控制装置，其特征在于，包括

状态感知器：通过传感器读取用于带动末端设备移动的各连杆间的关节转角θ，确定各连杆的状态S_k；然后，用坐标变换公式得到末端设备每次移动后的位置；最后，给出能够到达的下一时刻状态S_k+1；

动作选择器：存储过去通过强化学习获得的历史学习经验数据，根据这些历史学习经验数据和状态感知器给出的下一时刻状态S_k+1，应用动作选择策略选择出能够获得最佳执行效果的动作a_k，并执行该动作；

评价信号发生器：当执行完被选中的动作a_k后，评价信号发生器要对动作的执行效果给出返馈值r_k，用来影响今后选中这个动作的概率；以及

动态更新机构：根据返馈值r_k对动作选择器中的历史学习经验数据进行更新。

Images