CN114290332A - 一种应用于gis管道检测的串联机械臂路径规划方法及系统 - Google Patents

Abstract

本发明提出了一种应用于GIS管道检测的串联机械臂路径规划系统及其控制方法；根据检测机械臂的实际杆件参数和关节配置，建立机械臂的数学模型，得到机械臂的正向运动学方程，根据检测机械臂在GIS管道中的具体位置以及预先输入的管道内部环境模型，得到机械臂周围管道环境信息，建立检测机械臂以及周围管道环境的包围盒模型，在检测机械臂自身各杆件不发生碰撞的关节运动范围内，确定机械臂检测任务的起始位姿与目标位姿，并建立路径规划的关节搜索空间，采用改进的双树随机扩展树方法得到检测机械臂从起始位姿到目标位姿的无碰撞关节空间轨迹，进而得到机械臂末端轨迹；能够使机械臂在GIS管道中规划出有效避障路径，实现机械臂在GIS管道中检测作业。

Description

一种应用于GIS管道检测的串联机械臂路径规划方法及系统

技术领域

本发明属于机械臂路径规划技术领域，具体地，涉及一种应用于GIS管道检测的串联机械臂路径规划方法及系统。

背景技术

气体绝缘金属封闭开关设备(Gas Insulated Switchgear，简称GIS)是内部充满作为绝缘介质的压力气体的高压配电装置，主要由断路隔离开接地开关和母线等元器件组成，由于结构紧凑可靠、配置和安装便捷、安全性好而在电网中广泛使用。为了达到结构紧凑的要求GIS管道将许多重要组成单元封装在一定的结构空间中，其结构如图1所示。如果一个部件发生意外故障，会带来较为复杂的排除步骤和检修作业。GIS设备故障原因多样，不同的故障需要相应的检修方式，会导致更长时间的停电断电状况。GIS内部发生故障轻则损失负荷、影响设备安全，重则造成大面积停电，对整个电网的安全稳定运行造成很大影响，设备的检修和日常维护得到了高度重视。

传统的人工检测方法会造成较长时间的停电，可能危害人员人身安全，检修效率低。因此，使用现阶段发展较为成熟的机械臂对管道进行智能化检测是行之有效的方法，不但能够提高类似管道的检修效率，而且可以减少人员伤害，降低人力资源的投入，具有很好的研究意义和实际应用价值。

使用机械臂完成GIS管道内检测作业时，需要采用合理有效的方法规划出机械臂执行目标检测时的一条无碰撞路径。目前机械臂的路径规划方法大多应用于工业场景中如搬运、喷涂、焊接等，无法在机械臂检测GIS管道中直接使用，通过结合现有的路径规划算法如A*算法、人工势场法、快速随机扩展树法和概率路标图算法等，并根据实际应用的场景和检测要求进行改进，得到机械臂在GIS管道检测作业的有效无碰撞路径，具有重要的发明意义。

发明内容

本发明针对现有路径规划方法进行改进，针对机械臂在GIS管道中检测的作业要求，提供一种计算量小、搜索效率高、实时性强的一种应用于GIS管道检测的串联机械臂路径规划方法及系统。

本发明是通过以下方案实现的：

一种应用于GIS管道检测的串联机械臂路径规划方法：所述方法具体包括以下步骤：

步骤1：根据检测机械臂的实际杆件参数和关节配置，建立机械臂的数学模型，得到机械臂的正向运动学方程；

步骤2：根据检测机械臂在GIS管道中的具体位置，根据预先输入的管道内部环境模型，得到机械臂周围管道环境信息；

步骤3：建立检测机械臂以及周围管道环境的包围盒模型，采用基于包围盒的碰撞检测算法用于路径规划过程中碰撞检测，判断在路径搜索和扩展的过程中机械臂是否与管道环境发生碰撞；

步骤4：在检测机械臂自身各杆件不发生碰撞的关节运动范围内，确定机械臂检测任务的起始位姿与目标位姿，根据机械臂关节数n建立n维路径规划的关节搜索空间；

步骤5：采用改进的双树随机扩展树方法，在关节搜索空间起始节点和目标节点各生成一棵节点扩展树，在每次迭代过程中进行随机搜索并交替向对方扩展，直到两棵树的节点相连，得到检测机械臂从起始位姿到目标位姿的无碰撞关节空间轨迹，进而得到机械臂末端轨迹。

进一步地，在步骤1中，所述机械臂的正向运动学方程具体为：

采用标准D-H方法建立关节坐标系，求得相邻关节i-1和i之间的齐次变换矩阵为：

式中θ_i代表第i个关节的杆件回转角，对于旋转关节是变量而对于移动关节是常量，d_i代表第i个关节的杆件偏移量，对于旋转关节是常量而对于移动关节是变量；

通过各个关节之间的齐次变换矩阵连续相乘即可得到检测机械臂末端相对于基坐标系的齐次变换矩阵为：

式中(n_x，n_y，n_z)为末端坐标系的x轴在基坐标系中的矢量表示，(o_x，o_y，o_z)为末端坐标系的y轴在基坐标系中的矢量表示，(a_x，a_y，a_z)为末端坐标系的z轴在基坐标系中的矢量表示；

(p_x,p_y,p_z)即代表检测机械臂末端在基坐标系中的位置坐标。

进一步地，在步骤3中，使用简化的简单形状几何模型代替机械臂和管道的复杂形状几何模型，对于检测机械臂的每根连杆使用圆柱包围盒进行包络，对于管道内部分连接件和电气元件封装管道用球包围盒进行包络；

将每根机械臂连杆的圆柱包围盒两端圆形底面以连杆两端关节上的D-H坐标系原点为圆心，以圆心到连杆截面边缘的最大距离为半径，同时圆柱包围盒以连杆两端关节上的D-H坐标系原点的距离为圆柱高度，从而建立每根机械臂连杆的圆柱包围盒；

管道内球形元器件和障碍物的球包围盒直接以该元器件或障碍物的具体几何形状为准；

在得到包围盒和球包围盒后，将圆柱包围盒的半径值r₁叠加到球包围盒上，将圆柱简化为线段，把每个球包围盒的半径值r₂都加上该连杆圆柱包围盒的半径值r₁，即简化后的球包围盒的半径为R＝r₁+r₂，将碰撞检测转化为线段与球包围盒是否发生相交。

进一步地，在步骤3中，线段与球包围盒是否发生相交的具体判断方法为：

将线段的两端分别设为A和B，简化后的球包围盒的圆心设为C，则有：

若

或者

时，即线段有端点在球包围盒内部，会发生碰撞；

若

并且

即线段两个端点都在球外，首先求得球心C到西安段的距离：

如果d>R，则不发生碰撞；

如果d<R则进行下一步判断；

当线段两个端点都在球外并且球心到线段的距离d<R，则计算∠CAB和∠CBA：

如果两个角度都为锐角则线段与球包围盒相交，发生了碰撞，否则不发生碰撞。

进一步地，在步骤4中，所述关节搜索空间的范围即从起始位姿对应的各关节角度到目标位姿对应的各关节角度。

进一步地，在步骤5中，具体包括如下步骤：

步骤5.1：路径搜索开始时在关节搜索空间起始节点q_start生成一棵随机扩展节点树T_a，在目标节点q_goal生成另一棵随机扩展节点树T_b；

步骤5.2：对T_a进行扩展，在扩展前生成一个0到1之间的随机数a与概率p进行比较，如果a小于p则在搜索空间内生成一个随机节点q_rand，否则以另一个节点树的初始节点(q_start或q_goal)为随机节点q_rand；

步骤5.3：遍历T_a中所有节点，找到与随机节点q_rand欧式距离最短的节点q_nearesta，从q_nearest到q_rand根据该两点连线的矢量方向以一定步长step扩展一个采样点，其中扩展的距离为q_n＝step·(q_rand-q_nearesta)/||q_rand-q_nearesta||，返回扩展的采样点为q_newa＝q_nearesta+q_n，同时如果此时的||q_rand-q_nearesta||＜step则返回q_newa＝q_rand；

步骤5.4：对q_newa进行碰撞检测判断其是否有效，具体为根据q_newa中各个连杆关节的角度得到各个关节D-H坐标系的原点在基坐标系中的位置，然后根据步骤3建立此时机械臂各个连杆的圆柱包围盒，并与管道环境内的元器件和障碍物进行碰撞检测，

如果此时机械臂的每根连杆都不与管道环境内的元器件和障碍物发生碰撞，则将q_newa加入到节点树T_a中，并在节点索引矩阵E中记录下此时q_newa对应的父节点q_nearesta，如果q_newa＝q_rand则进入到步骤5.5，否则返回步骤5.3并且将q_newa作为q_nearest，如果此时机械臂的任意一根连杆与管道环境内的元器件和障碍物发生碰撞，同样进入到步骤5.5；

步骤5.5：对T_b进行扩展，找到最新加入T_a中的节点q_newa，并且遍历T_b中所有节点找到与q_newa欧式距离最短的节点q_nearestb，如果从q_nearestb到q_newa根据该两点连线的矢量方向以一定步长step扩展一个采样点，其中扩展的距离为q_n＝step·(q_newa-q_nearestb)/||q_newa-q_nearestb||，返回扩展的采样点为q_newb＝q_nearestb+q_n，同时如果此时的||q_newa-q_nearestb||＜step则返回q_newb＝q_newa；

步骤5.6：如果q_newb＝q_newa，则表示机械臂在GIS管道内从起始位姿到目标位姿的路径规划成功，对于T_a从q_newa开始搜索其父节点，找到父节点后继续搜索该节点的父节点，循环搜索直到搜索到T_a的初始节点，对于T_b从q_nearestb搜索其父节点，找到父节点后继续搜索该节点的父节点，循环搜索直到搜索到T_b的初始节点，将此时T_a和T_b的初始节点与q_start和q_goal进行比较；

如果T_a的初始节点为q_start，则得到的规划路径为从q_start到其子节点一直到q_newa，再从q_nearestb到其父节点一直到q_goal；

如果T_b的初始节点为q_start，则得到的规划路径为从q_start到其子节点一直到q_nearestb，再从q_newa到其父节点一直到q_goal；

得到规划路径后对路径中相邻的两个关节节点进行平滑处理，对相邻两个关节节点之间各个关节的速度和加速度施加一定的运动规律和变化方式，处理后得到机械臂在GIS管道内无碰撞的有效避障路径；

如果q_newb≠q_newa，则路径搜索和规划仍然在进行，进入到步骤5.7中；

步骤5.7：对q_newb进行碰撞检测判断其是否有效，具体为根据q_newb中各个连杆关节的角度得到各个关节D-H坐标系的原点在基坐标系中的位置，然后根据步骤3建立此时机械臂各个连杆的圆柱包围盒，并与管道环境内的元器件和障碍物进行碰撞检测；

如果此时机械臂的每根连杆都不与管道环境内的元器件和障碍物发生碰撞，则将q_newb加入到节点树T_b中，并在节点索引矩阵E中记录下此时q_newb对应的父节点q_nearestb，返回步骤5.5并且将此时的q_newb作为q_nearestb，

如果此时机械臂的任意一根连杆与管道环境内的元器件和障碍物发生碰撞，则进入到步骤5.8；

步骤5.8：交换此时T_a和T_b的所有节点以及每个节点对应的父节点信息，然后返回到步骤5.2。

一种应用于所述GIS管道检测的串联机械臂路径规划方法的系统，其特征在于：

所述系统具体包括管道环境信息感知模块，数据收集模块，运动学解算模块，碰撞检测模块，扩展节点树生成模块和无碰撞路径提取模块；

所述管道环境信息感知模块根据当前机械臂在管道内的具体位置，根据预先输入的管道内部环境模型，得到机械臂周围管道环境的元器件和障碍物具体形状和位置信息；

所述数据收集模块用于获取管道环境信息感知模块感知的信息，机械臂起始位姿以及目标位姿；

所述运动学解算模块用于实现机械臂的正运动学解算，获得各关节D-H坐标系在基坐标系中的位姿；

所述碰撞检测模块用于实现机械臂与管道环境中元器件和障碍物的碰撞检测；

所述扩展节点树生成模块用于建立随机扩展节点树T_a和T_b，在每次迭代的过程中进行随机搜索并交替向对方扩展，直到节点树T_a和T_b相连，得到检测机械臂从起始位姿到目标位姿的无碰撞关节空间轨迹，进而得到机械臂末端轨迹；

所述无碰撞路径提取模块根据扩展节点树生成模块产生的随机扩展节点树T_a和T_b，将两棵节点树的初始节点与搜索空间起始节点进行比较，然后获得从起始节点到目标节点经过的所有节点作为规划路径，对相邻两个节点进行平滑处理，得到机械臂在GIS管道内无碰撞的有效避障路径。

一种电子设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现上述方法的步骤。

一种计算机可读存储介质，用于存储计算机指令，所述计算机指令被处理器执行时实现上述方法的步骤。

本发明有益效果

本发明在复杂的环境中成功生成了扩展树，并且规划时间短，循环次数少，扩展效率高；通过父节点的唯一性从目标点到起始点生成了有效避障路径；

本发明能够使机械臂在GIS管道中高效率、高实时性地规划出有效避障路径，使机械臂从起始位姿合理运动到目标位姿，从而实现机械臂在GIS管道中检测作业，弥补现阶段GIS维护检测方式的不足。

附图说明

图1为本发明GIS管道的结构示意图；

图2为碰撞检测模型示意图；

图3为本发明一个实施方式的流程图；

图4为本发明一个实施方式的系统结构示意图；

图5为本发明一个实施方式的应用场景图；

图6为本发明一个实施方式的圆柱体罐装障碍物的球包围盒示意图

图7为本发明的路径规划仿真结果示意图。

具体实施方式

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

结合图1至图7，

一种应用于GIS管道检测的串联机械臂路径规划方法，

所述方法具体包括以下步骤：如图3所示，

步骤1：根据检测机械臂的实际杆件参数和关节配置，建立机械臂的数学模型，得到机械臂的正向运动学方程；

步骤2：根据检测机械臂在GIS管道中的具体位置，根据预先输入的管道内部环境模型，得到机械臂周围管道环境信息；

步骤3：建立检测机械臂以及周围管道环境的包围盒模型，采用基于包围盒的碰撞检测算法用于路径规划过程中碰撞检测，判断在路径搜索和扩展的过程中机械臂是否与管道环境发生碰撞；

步骤4：在检测机械臂自身各杆件不发生碰撞的关节运动范围内，确定机械臂检测任务的起始位姿与目标位姿，根据机械臂关节数n建立n维路径规划的关节搜索空间；

步骤5：采用改进的双树随机扩展树方法，在关节搜索空间起始节点和目标节点各生成一棵节点扩展树，在每次迭代过程中进行随机搜索并交替向对方扩展，直到两棵树的节点相连，得到检测机械臂从起始位姿到目标位姿的无碰撞关节空间轨迹，进而得到机械臂末端轨迹。

在步骤1中，所述机械臂的正向运动学方程具体为：

采用标准D-H方法建立关节坐标系，求得相邻关节i-1和i之间的齐次变换矩阵为：

式中θ_i代表第i个关节的杆件回转角，对于旋转关节是变量而对于移动关节是常量，d_i代表第i个关节的杆件偏移量，对于旋转关节是常量而对于移动关节是变量；

通过各个关节之间的齐次变换矩阵连续相乘即可得到检测机械臂末端相对于基坐标系的齐次变换矩阵为：

式中(n_x，n_y，n_z)为末端坐标系的x轴在基坐标系中的矢量表示，(o_x，o_y，o_z)为末端坐标系的y轴在基坐标系中的矢量表示，(a_x，a_y，a_z)为末端坐标系的z轴在基坐标系中的矢量表示；

(p_x,p_y,p_z)即代表检测机械臂末端在基坐标系中的位置坐标。

在步骤3中，使用简化的简单形状几何模型代替机械臂和管道的复杂形状几何模型，对于检测机械臂的每根连杆使用圆柱包围盒进行包络，对于管道内部分连接件和电气元件封装管道用球包围盒进行包络；

将每根机械臂连杆的圆柱包围盒两端圆形底面以连杆两端关节上的D-H坐标系原点为圆心，以圆心到连杆截面边缘的最大距离为半径，同时圆柱包围盒以连杆两端关节上的D-H坐标系原点的距离为圆柱高度，从而建立每根机械臂连杆的圆柱包围盒；

管道内球形元器件和障碍物的球包围盒直接以该元器件或障碍物的具体几何形状为准，管道内圆柱体管状障碍物可以使用一系列球包围盒进行包络，将管道环境内的元器件和障碍物都建立了球包围盒；

如图2所示，对其中一根连杆进行分析时，在得到包围盒和球包围盒后，将圆柱包围盒的半径值r₁叠加到球包围盒上，将圆柱简化为线段，把每个球包围盒的半径值r₂都加上该连杆圆柱包围盒的半径值r₁，即简化后的球包围盒的半径为R＝r₁+r₂，将碰撞检测转化为线段与球包围盒是否发生相交。

如图2所示，在步骤3中，线段与球包围盒是否发生相交的具体判断方法为：

将线段的两端分别设为A和B，简化后的球包围盒的圆心设为C，则有：

若

或者

时，即线段有端点在球包围盒内部，会发生碰撞；

若

并且

即线段两个端点都在球外，首先求得球心C到西安段的距离：

如果d>R，则不发生碰撞；

如果d<R则进行下一步判断；

当线段两个端点都在球外并且球心到线段的距离d<R，则计算∠CAB和∠CBA：

如果两个角度都为锐角则线段与球包围盒相交，发生了碰撞，否则不发生碰撞。

在步骤4中，所述关节搜索空间的范围即从起始位姿对应的各关节角度到目标位姿对应的各关节角度。

在步骤5中，具体包括如下步骤：

步骤5.1：路径搜索开始时在关节搜索空间起始节点q_start生成一棵随机扩展节点树T_a，在目标节点q_goal生成另一棵随机扩展节点树T_b；

步骤5.2：对T_a进行扩展，在扩展前生成一个0到1之间的随机数a与概率p进行比较，如果a小于p则在搜索空间内生成一个随机节点q_rand，否则以另一个节点树的初始节点(q_start或q_goal)为随机节点q_rand；

步骤5.3：遍历T_a中所有节点，找到与随机节点q_rand欧式距离最短的节点q_nearesta，从q_nearest到q_rand根据该两点连线的矢量方向以一定步长step扩展一个采样点，其中扩展的距离为q_n＝step·(q_rand-q_nearesta)/||q_rand-q_nearesta||，返回扩展的采样点为q_newa＝q_nearesta+q_n，同时如果此时的||q_rand-q_nearesta||＜step则返回q_newa＝q_rand；

步骤5.4：对q_newa进行碰撞检测判断其是否有效，具体为根据q_newa中各个连杆关节的角度得到各个关节D-H坐标系的原点在基坐标系中的位置，然后根据步骤3建立此时机械臂各个连杆的圆柱包围盒，并与管道环境内的元器件和障碍物进行碰撞检测，

如果此时机械臂的每根连杆都不与管道环境内的元器件和障碍物发生碰撞，则将q_newa加入到节点树T_a中，并在节点索引矩阵E中记录下此时q_newa对应的父节点q_nearesta，如果q_newa＝q_rand则进入到步骤5.5，否则返回步骤5.3并且将q_newa作为q_nearest，如果此时机械臂的任意一根连杆与管道环境内的元器件和障碍物发生碰撞，同样进入到步骤5.5；

步骤5.5：对T_b进行扩展，找到最新加入T_a中的节点q_newa，并且遍历T_b中所有节点找到与q_newa欧式距离最短的节点q_nearestb，如果从q_nearestb到q_newa根据该两点连线的矢量方向以一定步长step扩展一个采样点，其中扩展的距离为q_n＝step·(q_newa-q_nearestb)/||q_newa-q_nearestb||，返回扩展的采样点为q_newb＝q_nearestb+q_n，同时如果此时的||q_newa-q_nearestb||＜step则返回q_newb＝q_newa；

步骤5.6：如果q_newb＝q_newa，则表示机械臂在GIS管道内从起始位姿到目标位姿的路径规划成功，对于T_a从q_newa开始搜索其父节点，找到父节点后继续搜索该节点的父节点，循环搜索直到搜索到T_a的初始节点，对于T_b从q_nearestb搜索其父节点，找到父节点后继续搜索该节点的父节点，循环搜索直到搜索到T_b的初始节点，将此时T_a和T_b的初始节点与q_start和q_goal进行比较；

如果T_a的初始节点为q_start，则得到的规划路径为从q_start到其子节点一直到q_newa，再从q_nearestb到其父节点一直到q_goal；

如果T_b的初始节点为q_start，则得到的规划路径为从q_start到其子节点一直到q_nearestb，再从q_newa到其父节点一直到q_goal；

得到规划路径后对路径中相邻的两个关节节点进行平滑处理，对相邻两个关节节点之间各个关节的速度和加速度施加一定的运动规律和变化方式，处理后得到机械臂在GIS管道内无碰撞的有效避障路径；

如果q_newb≠q_newa，则路径搜索和规划仍然在进行，进入到步骤5.7中；

步骤5.7：对q_newb进行碰撞检测判断其是否有效，具体为根据q_newb中各个连杆关节的角度得到各个关节D-H坐标系的原点在基坐标系中的位置，然后根据步骤3建立此时机械臂各个连杆的圆柱包围盒，并与管道环境内的元器件和障碍物进行碰撞检测；

如果此时机械臂的每根连杆都不与管道环境内的元器件和障碍物发生碰撞，则将q_newb加入到节点树T_b中，并在节点索引矩阵E中记录下此时q_newb对应的父节点q_nearestb，返回步骤5.5并且将此时的q_newb作为q_nearestb，

如果此时机械臂的任意一根连杆与管道环境内的元器件和障碍物发生碰撞，则进入到步骤5.8；

步骤5.8：交换此时T_a和T_b的所有节点以及每个节点对应的父节点信息，然后返回到步骤5.2。

本实施例还提供了一种应用于GIS管道检测的串联机械臂路径规划系统，如图4所示，所述系统具体包括管道环境信息感知模块，数据收集模块，运动学解算模块，碰撞检测模块，扩展节点树生成模块和无碰撞路径提取模块；

所述管道环境信息感知模块根据当前机械臂在管道内的具体位置，根据预先输入的管道内部环境模型，得到机械臂周围管道环境的元器件和障碍物具体形状和位置信息；

所述数据收集模块用于获取管道环境信息感知模块感知的信息，机械臂起始位姿以及目标位姿；

所述运动学解算模块用于实现机械臂的正运动学解算，获得各关节D-H坐标系在基坐标系中的位姿；

所述碰撞检测模块用于实现机械臂与管道环境中元器件和障碍物的碰撞检测；

所述扩展节点树生成模块用于建立随机扩展节点树T_a和T_b，在每次迭代的过程中进行有一定概率的随机搜索并交替向对方扩展，直到节点树T_a和T_b相连，得到检测机械臂从起始位姿到目标位姿的无碰撞关节空间轨迹，进而得到机械臂末端轨迹；

所述无碰撞路径提取模块根据扩展节点树生成模块产生的随机扩展节点树T_a和T_b，将两棵节点树的初始节点与搜索空间起始节点进行比较，然后获得从起始节点到目标节点经过的所有节点作为规划路径，对相邻两个节点进行平滑处理，得到机械臂在GIS管道内无碰撞的有效避障路径。

一种电子设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现上述方法的步骤。

一种计算机可读存储介质，用于存储计算机指令，所述计算机指令被处理器执行时实现上述方法的步骤。

实施例1：

根据本发明所描述的应用于GIS管道检测的串联机械臂路径规划方法，将三自由度机械臂搭载到管道移动机器人中对管道进行检测，如图5所示，求得相邻关节之间的齐次变换矩阵为：

通过各个关节之间的齐次变换矩阵连续相乘即可得到检测机械臂末端相对于基坐标系的齐次变换矩阵为：

式中(p_x,p_y,p_z)即代表检测机械臂末端在基坐标系中的位置坐标。

根据移动机器人的传感器信息，确定检测机械臂在GIS管道中的具体位置，根据预先输入的管道内部环境模型，得到机械臂周围管道环境信息。

建立检测机械臂以及周围管道环境的包围盒模型，每根机械臂连杆建立圆柱包围盒，而管道内元器件和障碍物建立球包围盒，其中圆柱体管状障碍物使用一系列球包围盒进行包络，如图6所示。

在检测机械臂自身各杆件不发生碰撞的关节运动范围内，确定机械臂检测任务的起始位姿为q_start＝(0°,90°,0°,0°,0°)，目标位姿为q_goal＝(110°,122°,53.1°,134°,65°)，建立3维路径规划的搜索空间。

如图7所示，采用改进的双树随机扩展树方法，其中初始步长step＝0.1，比较概率p＝0.95，在关节搜索空间起始节点和目标节点各生成一棵节点扩展树，在每次迭代过程中进行随机搜索并交替向对方扩展，直到两棵树的节点相连，得到检测机械臂从起始位姿到目标位姿的无碰撞关节空间轨迹，进而得到机械臂末端轨迹，使用该方法仿真10次，得到平均循环次数为159.3。

本实施例对检测机械臂在GIS管道内的一种情况进行路径规划，该方法在复杂的环境中成功生成了扩展树，并且规划时间短，循环次数少，扩展效率高。通过父节点的唯一性从目标点到起始点生成了有效避障路径，根据机械臂的各连杆的位姿可以看到机械臂在运动途中没有发生碰撞。

本说明书中说明了大量具体细节，然而能够理解，本发明的实施例可以在没有这些具体细节的情况下进行实践。在一些实例中，并未详细示出公知的方法、结构和技术，以便不模糊对本说明书的理解。

以上对本发明所提出的一种应用于GIS管道检测的串联机械臂路径规划方法及系统，进行了详细介绍，对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (9)

1.一种应用于GIS管道检测的串联机械臂路径规划方法，其特征在于：

所述方法具体包括以下步骤：

步骤1：根据检测机械臂的实际杆件参数和关节配置，建立机械臂的数学模型，得到机械臂的正向运动学方程；

步骤2：根据检测机械臂在GIS管道中的具体位置，根据预先输入的管道内部环境模型，得到机械臂周围管道环境信息；

步骤3：建立检测机械臂以及周围管道环境的包围盒模型，采用基于包围盒的碰撞检测算法用于路径规划过程中碰撞检测，判断在路径搜索和扩展的过程中机械臂是否与管道环境发生碰撞；

步骤4：在检测机械臂自身各杆件不发生碰撞的关节运动范围内，确定机械臂检测任务的起始位姿与目标位姿，根据机械臂关节数n建立n维路径规划的关节搜索空间；

步骤5：采用改进的双树随机扩展树方法，在关节搜索空间起始节点和目标节点各生成一棵节点扩展树，在每次迭代过程中进行随机搜索并交替向对方扩展，直到两棵树的节点相连，得到检测机械臂从起始位姿到目标位姿的无碰撞关节空间轨迹，进而得到机械臂末端轨迹。

2.根据权利要求1所述方法，其特征在于：

在步骤1中，所述机械臂的正向运动学方程具体为：

采用标准D-H方法建立关节坐标系，求得相邻关节i-1和i之间的齐次变换矩阵为：

式中θ_i代表第i个关节的杆件回转角，对于旋转关节是变量而对于移动关节是常量，d_i代表第i个关节的杆件偏移量，对于旋转关节是常量而对于移动关节是变量；

通过各个关节之间的齐次变换矩阵连续相乘即可得到检测机械臂末端相对于基坐标系的齐次变换矩阵为：

式中(n_x，n_y，n_z)为末端坐标系的x轴在基坐标系中的矢量表示，(o_x，o_y，o_z)为末端坐标系的y轴在基坐标系中的矢量表示，(a_x，a_y，a_z)为末端坐标系的z轴在基坐标系中的矢量表示；

(p_x,p_y,p_z)即代表检测机械臂末端在基坐标系中的位置坐标。

3.根据权利要求2所述方法，其特征在于：

在步骤3中，使用简化的简单形状几何模型代替机械臂和管道的复杂形状几何模型，对于检测机械臂的每根连杆使用圆柱包围盒进行包络，对于管道内部分连接件和电气元件封装管道用球包围盒进行包络；

将每根机械臂连杆的圆柱包围盒两端圆形底面以连杆两端关节上的D-H坐标系原点为圆心，以圆心到连杆截面边缘的最大距离为半径，同时圆柱包围盒以连杆两端关节上的D-H坐标系原点的距离为圆柱高度，从而建立每根机械臂连杆的圆柱包围盒；

管道内球形元器件和障碍物的球包围盒直接以该元器件或障碍物的具体几何形状为准；

在得到包围盒和球包围盒后，将圆柱包围盒的半径值r₁叠加到球包围盒上，将圆柱简化为线段，把每个球包围盒的半径值r₂都加上该连杆圆柱包围盒的半径值r₁，即简化后的球包围盒的半径为R＝r₁+r₂，将碰撞检测转化为线段与球包围盒是否发生相交。

4.根据权利要求3所述方法，其特征在于：

在步骤3中，线段与球包围盒是否发生相交的具体判断方法为：

将线段的两端分别设为A和B，简化后的球包围盒的圆心设为C，则有：

若

或者

时，即线段有端点在球包围盒内部，会发生碰撞；

若

并且

即线段两个端点都在球外，首先求得球心C到西安段的距离：

如果d>R，则不发生碰撞；

如果d<R则进行下一步判断；

当线段两个端点都在球外并且球心到线段的距离d<R，则计算∠CAB和∠CBA：

如果两个角度都为锐角则线段与球包围盒相交，发生了碰撞，否则不发生碰撞。

5.根据权利要求3所述方法，其特征在于：

在步骤4中，所述关节搜索空间的范围即从起始位姿对应的各关节角度到目标位姿对应的各关节角度。

6.根据权利要求5所述方法，其特征在于：

在步骤5中，具体包括如下步骤：

步骤5.1：路径搜索开始时在关节搜索空间起始节点q_start生成一棵随机扩展节点树T_a，在目标节点q_goal生成另一棵随机扩展节点树T_b；

步骤5.2：对T_a进行扩展，在扩展前生成一个0到1之间的随机数a与概率p进行比较，如果a小于p则在搜索空间内生成一个随机节点q_rand，否则以另一个节点树的初始节点(q_start或q_goal)为随机节点q_rand；

步骤5.3：遍历T_a中所有节点，找到与随机节点q_rand欧式距离最短的节点q_nearesta，从q_nearest到q_rand根据该两点连线的矢量方向以一定步长step扩展一个采样点，其中扩展的距离为q_n＝step·(q_rand-q_nearesta)/||q_rand-q_nearesta||，返回扩展的采样点为q_newa＝q_nearesta+q_n，同时如果此时的||q_rand-q_nearesta||＜step则返回q_newa＝q_rand；

步骤5.4：对q_newa进行碰撞检测判断其是否有效，具体为根据q_newa中各个连杆关节的角度得到各个关节D-H坐标系的原点在基坐标系中的位置，然后根据步骤3建立此时机械臂各个连杆的圆柱包围盒，并与管道环境内的元器件和障碍物进行碰撞检测，

如果此时机械臂的每根连杆都不与管道环境内的元器件和障碍物发生碰撞，则将q_newa加入到节点树T_a中，并在节点索引矩阵E中记录下此时q_newa对应的父节点q_nearesta，如果q_newa＝q_rand则进入到步骤5.5，否则返回步骤5.3并且将q_newa作为q_nearest，如果此时机械臂的任意一根连杆与管道环境内的元器件和障碍物发生碰撞，同样进入到步骤5.5；

步骤5.5：对T_b进行扩展，找到最新加入T_a中的节点q_newa，并且遍历T_b中所有节点找到与q_newa欧式距离最短的节点q_nearestb，如果从q_nearestb到q_newa根据该两点连线的矢量方向以一定步长step扩展一个采样点，其中扩展的距离为q_n＝step·(q_newa-q_nearestb)/||q_newa-q_nearestb||，返回扩展的采样点为q_newb＝q_nearestb+q_n，同时如果此时的||q_newa-q_nearestb||＜step则返回q_newb＝q_newa；

步骤5.6：如果q_newb＝q_newa，则表示机械臂在GIS管道内从起始位姿到目标位姿的路径规划成功，对于T_a从q_newa开始搜索其父节点，找到父节点后继续搜索该节点的父节点，循环搜索直到搜索到T_a的初始节点，对于T_b从q_nearestb搜索其父节点，找到父节点后继续搜索该节点的父节点，循环搜索直到搜索到T_b的初始节点，将此时T_a和T_b的初始节点与q_start和q_goal进行比较；

如果T_a的初始节点为q_start，则得到的规划路径为从q_start到其子节点一直到q_newa，再从q_nearestb到其父节点一直到q_goal；

如果T_b的初始节点为q_start，则得到的规划路径为从q_start到其子节点一直到q_nearestb，再从q_newa到其父节点一直到q_goal；

得到规划路径后对路径中相邻的两个关节节点进行平滑处理，对相邻两个关节节点之间各个关节的速度和加速度施加一定的运动规律和变化方式，处理后得到机械臂在GIS管道内无碰撞的有效避障路径；

如果q_newb≠q_newa，则路径搜索和规划仍然在进行，进入到步骤5.7中；

步骤5.7：对q_newb进行碰撞检测判断其是否有效，具体为根据q_newb中各个连杆关节的角度得到各个关节D-H坐标系的原点在基坐标系中的位置，然后根据步骤3建立此时机械臂各个连杆的圆柱包围盒，并与管道环境内的元器件和障碍物进行碰撞检测；

如果此时机械臂的每根连杆都不与管道环境内的元器件和障碍物发生碰撞，则将q_newb加入到节点树T_b中，并在节点索引矩阵E中记录下此时q_newb对应的父节点q_nearestb，返回步骤5.5并且将此时的q_newb作为q_nearestb，

如果此时机械臂的任意一根连杆与管道环境内的元器件和障碍物发生碰撞，则进入到步骤5.8；

步骤5.8：交换此时T_a和T_b的所有节点以及每个节点对应的父节点信息，然后返回到步骤5.2。

7.一种应用于GIS管道检测的串联机械臂路径规划系统，其特征在于：

所述系统具体包括管道环境信息感知模块，数据收集模块，运动学解算模块，碰撞检测模块，扩展节点树生成模块和无碰撞路径提取模块；

所述管道环境信息感知模块根据当前机械臂在管道内的具体位置，根据预先输入的管道内部环境模型，得到机械臂周围管道环境的元器件和障碍物具体形状和位置信息；

所述数据收集模块用于获取管道环境信息感知模块感知的信息，机械臂起始位姿以及目标位姿；

所述运动学解算模块用于实现机械臂的正运动学解算，获得各关节D-H坐标系在基坐标系中的位姿；

所述碰撞检测模块用于实现机械臂与管道环境中元器件和障碍物的碰撞检测；

所述扩展节点树生成模块用于建立随机扩展节点树T_a和T_b，在每次迭代的过程中进行随机搜索并交替向对方扩展，直到节点树T_a和T_b相连，得到检测机械臂从起始位姿到目标位姿的无碰撞关节空间轨迹，进而得到机械臂末端轨迹；

所述无碰撞路径提取模块根据扩展节点树生成模块产生的随机扩展节点树T_a和T_b，将两棵节点树的初始节点与搜索空间起始节点进行比较，然后获得从起始节点到目标节点经过的所有节点作为规划路径，对相邻两个节点进行平滑处理，得到机械臂在GIS管道内无碰撞的有效避障路径。

8.一种电子设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至6中任一项所述方法的步骤。

9.一种计算机可读存储介质，用于存储计算机指令，其特征在于，所述计算机指令被处理器执行时实现权利要求1至6中任一项所述方法的步骤。

Images