一种基于多分辨率碰撞检测的机器人控制方法及系统

技术领域

本发明涉及机器人的控制技术领域，具体地涉及一种基于多分辨率碰撞检测的机器人控制方法及系统。

背景技术

随着机器人技术的快速发展，在配网带电操作这一危险度较高的工作中，逐渐开始引入机器人进行现场操作，从而保障工作人员的生命安全。但是由于配网现场的复杂环境，机器人在实际实施的过程中很难完全避开所有障碍物来完成配网带电操作。现有技术中虽然存在一些路径算法，但是实际的实施效果并不理想。

发明内容

本发明实施例的目的是提供一种基于多分辨率碰撞检测的机器人控制方法及系统，该控制方法及系统能够生成机器人避障的有效路径。

为了实现上述目的，本发明实施例提供一种基于多分辨率碰撞检测的机器人控制方法，包括：

获取配网带电机器人的机械臂结构；

根据所述机械臂结构确定位姿变换矩阵；

获取当前的路径任务；

采用预设的改进RRT算法根据所述路径任务和所述位姿变换矩阵生成有效路径；

根据所述有效路径控制所述配网带电机器人的机械臂从当前位置移动至目标点位。

可选地，所述机械臂结构为六自由度机械臂。

可选地，根据所述机械臂结构确定位姿变换矩阵，包括：

根据公式(1)至公式(3)确定所述位姿变换矩阵，

其中，

可选地，采用预设的改进RRT算法根据所述路径任务和所述位姿变换矩阵生成有效路径，包括：

将当前所述配网带电机器人所在的空间描述为六维空间；

以所述配网带电机器人的当前位置为原点；

根据所述原点更新随机树；

在所述六维空间中随机选择一个点作为扩展点；

在所述随机树上寻找与所述扩展点最近的点作为最近点；

连接所述最近点和扩展点，以形成待扩展路径；

对所述待扩展路径进行碰撞检测操作；

在所述待扩展路径未通过所述碰撞检测操作的情况下，返回执行在所述六维空间中随机选择一个点作为扩展点的步骤；

在所述待扩展路径通过所述碰撞检测操作的情况下，将所述待扩展路径加入随机树中；

判断所述扩展点是否位于目标点的预设范围内；

在判断所述扩展点位于所述预设范围外的情况下，将所述扩展点作为新的原点，并返回执行根据所述原点更新随机树的步骤；

在判断所述扩展点位于所述预设范围内的情况下，输出所述随机树，以作为所述有效路径。

可选地，所述碰撞检测操作包括：

根据公式(4)分解所述配网带电机器人的三维模型，

其中，p1、r1、p2、r2、p3、p4、r3、p5、r4、p6、r5、p7、p8、r6、p9、r7、p10、r8、p11、p12以及r9为中心坐标和对应的尺寸值；

采用规划器根据三维模型和所述待扩展路径判断是否通过碰撞检测操作。

另一方面，本发明还提供一种基于多分辨率碰撞检测的机器人控制系统，所述控制系统包括控制器，所述控制器用于：

获取配网带电机器人的机械臂结构；

根据所述机械臂结构确定位姿变换矩阵；

获取当前的路径任务；

采用预设的改进RRT算法根据所述路径任务和所述位姿变换矩阵生成有效路径；

根据所述有效路径控制所述配网带电机器人的机械臂从当前位置移动至目标点位。

可选地，所述机械臂结构为六自由度机械臂。

可选地，根据所述机械臂结构确定位姿变换矩阵，包括：

根据公式(1)至公式(3)确定所述位姿变换矩阵，

其中，

可选地，采用预设的改进RRT算法根据所述路径任务和所述位姿变换矩阵生成有效路径，包括：

将当前所述配网带电机器人所在的空间描述为六维空间；

以所述配网带电机器人的当前位置为原点；

根据所述原点更新随机树；

在所述六维空间中随机选择一个点作为扩展点；

在所述随机树上寻找与所述扩展点最近的点作为最近点；

连接所述最近点和扩展点，以形成待扩展路径；

对所述待扩展路径进行碰撞检测操作；

在所述待扩展路径未通过所述碰撞检测操作的情况下，返回执行在所述六维空间中随机选择一个点作为扩展点的步骤；

在所述待扩展路径通过所述碰撞检测操作的情况下，将所述待扩展路径加入随机树中；

判断所述扩展点是否位于目标点的预设范围内；

在判断所述扩展点位于所述预设范围外的情况下，将所述扩展点作为新的原点，并返回执行根据所述原点更新随机树的步骤；

在判断所述扩展点位于所述预设范围内的情况下，输出所述随机树，以作为所述有效路径。

可选地，所述碰撞检测操作包括：

根据公式(4)分解所述配网带电机器人的三维模型，

其中，p1、r1、p2、r2、p3、p4、r3、p5、r4、p6、r5、p7、p8、r6、p9、r7、p10、r8、p11、p12以及r9为中心坐标和对应的尺寸值；

采用规划器根据三维模型和所述待扩展路径判断是否通过碰撞检测操作。

通过上述技术方案，本发明提供的一种基于多分辨率碰撞检测的机器人控制方法及系统通过构建配网带电机器人的位姿变换矩阵，然后针对该位姿变换矩阵通过RRT算法来生成有效路径。相较于现有技术而言，本发明提供的控制方法由于结合了现场的环境以及配网带电机器人本身的结构特点，使得生成的有效路径能够准确控制配网带电机器人避开障碍物。

本发明实施例的其它特征和优点将在随后的具体实施方式部分予以详细说明。

附图说明

附图是用来提供对本发明实施例的进一步理解，并且构成说明书的一部分，与下面的具体实施方式一起用于解释本发明实施例，但并不构成对本发明实施例的限制。在附图中：

图1是根据本发明的一个实施方式的一种基于多分辨率碰撞检测的机器人控制方法的流程图；

图2是根据本发明的一个实施方式的一种六自由度机械臂的示意图；

图3是根据本发明的一个实施方式的改进RRT算法的流程图；

图4是根据本发明的一个实施方式的分解六自由度机械臂的示意图；

图5是根据本发明的一个实施方式的基于分解方法构建的建模的示意图。

具体实施方式

以下结合附图对本发明实施例的具体实施方式进行详细说明。应当理解的是，此处所描述的具体实施方式仅用于说明和解释本发明实施例，并不用于限制本发明实施例。

如图1所示是根据本发明的一个实施方式的一种基于多分辨率碰撞检测的机器人控制方法的流程图。在该图1中，该控制方法可以包括以下步骤：

在步骤S10中，获取配网带电机器人的机械臂结构；

在步骤S11中，根据机械臂结构确定位姿变换矩阵；

在步骤S12中，获取当前的路径任务；

在步骤S13中，采用预设的改进RRT算法根据路径任务和位姿变换矩阵生成有效路径；

在步骤S14中，根据有效路径控制配网带电机器人的机械臂从当前位置移动至目标点位。

在该如图1所示出的方法中，步骤S10可以用于获取配网带电机器人的机械臂结构。该机械臂结构可以是本领域人员所知的多种形式。在本发明的一个示例中，该机械臂结构可以是如图2所示。在该图2中，该机械臂结构可以包括6个伺服转动轴，也即该机械臂结构可以是六自由度机械臂。该六自由度机械臂的DH参数表可以是如下表1所示，

表1

进一步地，基于该机械臂结构，步骤S11所构建的位姿变换矩阵可以是采用以下公式(1)至公式(3)来确定，

其中，

步骤S12可以用于获取当前的路径任务。其中，该路径任务可以是在场景中的当前位置和目标位置(目标点)。

步骤S13可以用于采用预设的改进RRT算法根据路径任务和位姿变换矩阵生成有效路径。对于该改进RRT算法，可以是本领域人员所知的多种形式。在本发明的一个示例中，该改进RRT算法可以是包括如图3中所示出的方法。在该图3中，该改进RRT算法可以是包括以下步骤：

在步骤S20中，将当前配网带电机器人所在的空间描述为六维空间；

在步骤S21中，以配网带电机器人的当前位置为原点；

在步骤S22中，根据原点更新随机树；

在步骤S23中，在六维空间中随机选择一个点作为扩展点；

在步骤S24中，在随机树上寻找与扩展点最近的点作为最近点；

在步骤S25中，连接最近点和扩展点，以形成待扩展路径；

在步骤S26中，对待扩展路径进行碰撞检测操作；

在待扩展路径未通过碰撞检测操作的情况下，返回执行在六维空间中随机选择一个点作为扩展点的步骤，即返回执行步骤S23；

在步骤S27中，在待扩展路径通过碰撞检测操作的情况下，将待扩展路径加入随机树中；

在步骤S28中，判断扩展点是否位于目标点的预设范围内；

在步骤S29中，在判断扩展点位于预设范围外的情况下，将扩展点作为新的原点，并返回执行根据原点更新随机树的步骤，即返回执行步骤S22；

在步骤S30中，在判断扩展点位于预设范围内的情况下，输出随机树，以作为有效路径。

在该如图1所示出的方法中，由于前述本发明提供的配网带电机器人为六自由度机械臂，那么为了便于确定位置坐标，可以通过步骤S20将当前配网带电机器人所在的空间描述为六维空间。然后在此基础上，通过步骤S21至步骤S30来逐个寻找扩展点，从而扩展随机树，并最终在步骤S30中判断到达目标点的预设范围内，从而输出有效路径。

在该实施方式中，对于步骤S26的碰撞检测的具体方法，可以是本领域人员所知的多种方式。考虑到碰撞检测的精度以及灵敏度直接与所构建的三维模型的方式相关，现有技术中常用的三角网格法虽然精度较高，但是需要处理的计算量非常大，因此并不能够满足本发明的配网带电机器人的路径生成要求。因此，在本发明的一个示例中，可以是首先采用以下公式(4)来分解配网带电机器人的三维模型，

其中，p1、r1、p2、r2、p3、p4、r3、p5、r4、p6、r5、p7、p8、r6、p9、r7、p10、r8、p11、p12以及r9为中心坐标和对应的尺寸值。然后再采用规划器根据三维模型和待扩展路径判断是否通过碰撞检测操作。其中，对于该公式(4)中所示出的分解方式，举例来说，可以是将具有单个圆头且长度很短的特征的连杆1分解为一个球1，而对于具有两个圆头且长度较长的特征的连杆2则可以分解为两个球和一个圆柱，如图4所示。基于该公式(4)的分解方式，对应的建模图如图5所示。

在生成有效路径后，最终可以是采用步骤S14根据该有效路径控制配网带电机器人的机械臂从当前位置移动至目标点位。

另一方面，本发明还提供一种基于多分辨率碰撞检测的机器人控制系统，控制系统包括控制器，控制器用于执行如图1中所示出的控制方法。具体地，在该图1中，该控制器可以用于执行以下步骤：

在步骤S10中，获取配网带电机器人的机械臂结构；

在步骤S11中，根据机械臂结构确定位姿变换矩阵；

在步骤S12中，获取当前的路径任务；

在步骤S13中，采用预设的改进RRT算法根据路径任务和位姿变换矩阵生成有效路径；

在步骤S14中，根据有效路径控制配网带电机器人的机械臂从当前位置移动至目标点位。

在该如图1所示出的方法中，步骤S10可以用于获取配网带电机器人的机械臂结构。该机械臂结构可以是本领域人员所知的多种形式。在本发明的一个示例中，该机械臂结构可以是如图2所示。在该图2中，该机械臂结构可以包括6个伺服转动轴，也即该机械臂结构可以是六自由度机械臂。该六自由度机械臂的DH参数表可以是如下表1所示，

表1

进一步地，基于该机械臂结构，步骤S11所构建的位姿变换矩阵可以是采用以下公式(1)至公式(3)来确定，

其中，

步骤S12可以用于获取当前的路径任务。其中，该路径任务可以是在场景中的当前位置和目标位置(目标点)。

步骤S13可以用于采用预设的改进RRT算法根据路径任务和位姿变换矩阵生成有效路径。对于该改进RRT算法，可以是本领域人员所知的多种形式。在本发明的一个示例中，该改进RRT算法可以是包括如图3中所示出的方法。在该图3中，该改进RRT算法可以是包括以下步骤：

在步骤S20中，将当前配网带电机器人所在的空间描述为六维空间；

在步骤S21中，以配网带电机器人的当前位置为原点；

在步骤S22中，根据原点更新随机树；

在步骤S23中，在六维空间中随机选择一个点作为扩展点；

在步骤S24中，在随机树上寻找与扩展点最近的点作为最近点；

在步骤S25中，连接最近点和扩展点，以形成待扩展路径；

在步骤S26中，对待扩展路径进行碰撞检测操作；

在待扩展路径未通过碰撞检测操作的情况下，返回执行在六维空间中随机选择一个点作为扩展点的步骤，即返回执行步骤S23；

在步骤S27中，在待扩展路径通过碰撞检测操作的情况下，将待扩展路径加入随机树中；

在步骤S28中，判断扩展点是否位于目标点的预设范围内；

在步骤S29中，在判断扩展点位于预设范围外的情况下，将扩展点作为新的原点，并返回执行根据原点更新随机树的步骤，即返回执行步骤S22；

在步骤S30中，在判断扩展点位于预设范围内的情况下，输出随机树，以作为有效路径。

在该如图1所示出的方法中，由于前述本发明提供的配网带电机器人为六自由度机械臂，那么为了便于确定位置坐标，可以通过步骤S20将当前配网带电机器人所在的空间描述为六维空间。然后在此基础上，通过步骤S21至步骤S30来逐个寻找扩展点，从而扩展随机树，并最终在步骤S30中判断到达目标点的预设范围内，从而输出有效路径。

在该实施方式中，对于步骤S26的碰撞检测的具体方法，可以是本领域人员所知的多种方式。考虑到碰撞检测的精度以及灵敏度直接与所构建的三维模型的方式相关，现有技术中常用的三角网格法虽然精度较高，但是需要处理的计算量非常大，因此并不能够满足本发明的配网带电机器人的路径生成要求。因此，在本发明的一个示例中，可以是首先采用以下公式(4)来分解配网带电机器人的三维模型，

其中，p1、r1、p2、r2、p3、p4、r3、p5、r4、p6、r5、p7、p8、r6、p9、r7、p10、r8、p11、p12以及r9为中心坐标和对应的尺寸值。然后再采用规划器根据三维模型和待扩展路径判断是否通过碰撞检测操作。其中，对于该公式(4)中所示出的分解方式，举例来说，可以是将具有单个圆头且长度很短的特征的连杆1分解为一个球1，而对于具有两个圆头且长度较长的特征的连杆2则可以分解为两个球和一个圆柱，如图4所示。基于该公式(4)的分解方式，对应的建模图如图5所示。

在生成有效路径后，最终可以是采用步骤S14根据该有效路径控制配网带电机器人的机械臂从当前位置移动至目标点位。

通过上述技术方案，本发明提供的一种基于多分辨率碰撞检测的机器人控制方法及系统通过构建配网带电机器人的位姿变换矩阵，然后针对该位姿变换矩阵通过RRT算法来生成有效路径。相较于现有技术而言，本发明提供的控制方法由于结合了现场的环境以及配网带电机器人本身的结构特点，使得生成的有效路径能够准确控制配网带电机器人避开障碍物。

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

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

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

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

在一个典型的配置中，计算设备包括一个或多个处理器(CPU)、输入/输出接口、网络接口和内存。

存储器可能包括计算机可读介质中的非永久性存储器，随机存取存储器(RAM)和/或非易失性内存等形式，如只读存储器(ROM)或闪存(flash RAM)。存储器是计算机可读介质的示例。

计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存(PRAM)、静态随机存取存储器(SRAM)、动态随机存取存储器(DRAM)、其他类型的随机存取存储器(RAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、快闪记忆体或其他内存技术、只读光盘只读存储器(CD-ROM)、数字多功能光盘(DVD)或其他光学存储、磁盒式磁带，磁带磁磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。按照本文中的界定，计算机可读介质不包括暂存电脑可读媒体(transitory media)，如调制的数据信号和载波。

还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括要素的过程、方法、商品或者设备中还存在另外的相同要素。

以上仅为本申请的实施例而已，并不用于限制本申请。对于本领域技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原理之内所作的任何修改、等同替换、改进等，均应包含在本申请的权利要求范围之内。