CN115609594A - 机械臂路径的规划方法、装置、上位控制端及存储介质 - Google Patents

Abstract

本发明提供了一种机械臂路径的规划方法、装置、上位控制端及存储介质，包括：如果接收到路径规划请求，则获取机械臂所处场景对应的实时障碍物场景；基于实时障碍物场景和路径规划请求携带的规划目标数据进行静态全局路径规划，得到机械臂对应的初始全局路径；如果路径规划请求携带的规划类型为避障类型，遍历初始全局路径中的每个轨迹点，并在下一轨迹点未通过碰撞检测时，基于当前轨迹点和规划目标数据进行实时全局路径规划，得到目标全局路径；在下一轨迹点通过碰撞检测时，继续执行下一个轨迹点。本发明可以显著提高实时路径的规划效率、规划成功率和轨迹合理性，还可以提供准确度更高的环境数据，以及提升环境适应性。

Description

机械臂路径的规划方法、装置、上位控制端及存储介质

技术领域

本发明涉及路径规划技术领域，尤其是涉及一种机械臂路径的规划方法、装置、上位控制端及存储介质。

背景技术

目前，机械臂路径规划主要采用的是静态路径规划的方法，静态规划的架构遵循的是“感知-规划-执行”的方法，此种架构在实际的生产环境中适应性比较差，真实环境下通常存在着动态的障碍物，如人在机械臂的移动过程中突然进入机械臂的工作空间内，容易出现碰撞事件，造成安全隐患。

相关技术提供了一种基于改进RRT（rapidly exploring random tree，快速扩展随机树）算法的机器人实时路径规划方法，该算法只是针对于移动机器人快要接近的区域进行实时路径规划，可以减小许多不必要的路径规划，但该算法不适用于机械臂的路径规划，主要原因为机械臂作业时，全局区域与局部区域差异并不大，在障碍物附近进行小范围的路径规划，规划得到的路径并非全局最优，并且规划的成功率下降明显。

发明内容

有鉴于此，本发明的目的在于提供一种机械臂路径的规划方法、装置、上位控制端及存储介质，可以显著提高实时路径的规划效率、规划成功率和轨迹合理性，还可以提供准确度更高的环境数据，以及提升环境适应性。

第一方面，本发明实施例提供了一种机械臂路径的规划方法，所述方法应用于机械臂的上位控制端，包括：如果接收到路径规划请求，则获取机械臂所处场景对应的实时障碍物场景；其中，所述实时障碍物场景是基于剔除机械臂视觉数据之后的实时视觉数据构建得到的；基于所述实时障碍物场景和所述路径规划请求携带的规划目标数据进行静态全局路径规划，得到所述机械臂对应的初始全局路径；如果所述路径规划请求携带的规划类型为避障类型，遍历所述初始全局路径中的每个轨迹点，并在当前轨迹点对应的下一轨迹点未通过碰撞检测时，基于所述当前轨迹点和所述规划目标数据进行实时全局路径规划，得到目标全局路径。

在一种实施方式中，所述上位控制端分别与激光雷达、视觉传感器通信连接，所述机械臂视觉数据包括机械臂点云数据；所述获取机械臂所处场景对应的实时障碍物场景的步骤，包括：获取所述激光雷达采集的场景点云数据，以及获取所述视觉传感器采集的图像数据；其中，所述实时视觉数据包括所述场景点云数据和/或所述图像数据；对所述实时视觉数据依次进行预处理，以降低所述实时视觉数据中的噪声数据；确定预处理后的实时视觉数据的语义分割结果，以基于所述语义分割结果确定所述场景点云数据中包含的机械臂点云数据；将所述机械臂点云数据从所述场景点云数据中剔除，并基于剔除所述机械臂点云数据的场景点云数据构建实时障碍物场景。

在一种实施方式中，所述在当前轨迹点对应的下一轨迹点未通过碰撞检测时，基于所述当前轨迹点和所述规划目标数据进行实时全局路径规划的步骤，包括：基于所述实时障碍物场景对当前轨迹点对应的下一轨迹点进行碰撞检测；如果所述下一轨迹点未通过所述碰撞检测，则将所述当前轨迹点作为轨迹起始点，以及将所述规划目标数据中的目标点作为轨迹终止点；基于所述轨迹起始点和所述轨迹终止点进行静态全局路径规划，得到更新后的初始全局路径，继续遍历更新后的初始全局路径中的每个轨迹点，直至达到末位轨迹点。

在一种实施方式中，所述上位控制端还与机械臂的下位控制端通信连接，所述方法还包括：如果所述下一轨迹点通过所述碰撞检测，则基于所述当前轨迹点生成第一机械臂控制指令；将所述第一机械臂控制指令发送至所述下位控制端，以通过所述下位控制端基于所述第一机械臂控制指令，控制所述机械臂在所述场景内移动；将所述下一轨迹点作为新的当前轨迹点，并在所述新的当前轨迹点对应的下一轨迹点通过所述碰撞检测时，基于所述新的当前轨迹点生成第一机械臂控制指令，直至执行至末位轨迹点。

在一种实施方式中，所述上位控制端还与力觉传感器、机械臂的下位控制端通信连接；所述方法还包括：如果所述路径规划请求携带的规划类型为力控类型，则获取所述力觉传感器采集的传感器数据；基于所述传感器数据，确定所述机械臂末端的受力数据；根据所述受力数据确定所述当前轨迹点对应的下一轨迹点的调整量，以基于所述调整量更新所述下一轨迹点，并基于更新后的下一轨迹点生成第二机械臂控制指令；将所述第二机械臂控制指令发送至所述下位控制端，以通过所述下位控制端基于所述第二机械臂控制指令，控制所述机械臂在所述场景内移动。

在一种实施方式中，所述基于所述实时障碍物场景和所述路径规划请求携带的规划目标数据进行静态全局路径规划，得到所述机械臂对应的初始全局路径的步骤，包括：采用快速扩展随机树算法，基于预设约束条件、所述实时障碍物场景、所述路径规划请求携带的规划目标数据进行静态全局路径规划，得到所述机械臂对应的初始全局路径。

在一种实施方式中，所述确定预处理后的实时视觉数据的语义分割结果的步骤，包括：通过预先训练的神经网络，对预处理后的实时视觉数据进行识别及语义分割处理，得到语义分割结果；其中，所述神经网络包括PointNet++网络。

第二方面，本发明实施例还提供一种机械臂路径的规划装置，所述装置应用于机械臂的上位控制端，包括：规划调度模块，用于如果接收到路径规划请求，则获取机械臂所处场景对应的实时障碍物场景；其中，所述实时障碍物场景是基于剔除机械臂视觉数据之后的实时视觉数据构建得到的；全局路径规划模块，用于基于所述实时障碍物场景和所述路径规划请求携带的规划目标数据进行静态全局路径规划，得到所述机械臂对应的初始全局路径；局部路径规划模块，用于如果所述路径规划请求携带的规划类型为避障类型，遍历所述初始全局路径中的每个轨迹点，并在当前轨迹点对应的下一轨迹点未通过碰撞检测时，基于所述当前轨迹点和所述规划目标数据进行实时全局路径规划，得到目标全局路径。

第三方面，本发明实施例还提供一种上位控制端，包括处理器和存储器，所述存储器存储有能够被所述处理器执行的计算机可执行指令，所述处理器执行所述计算机可执行指令以实现第一方面提供的任一项所述的方法。

第四方面，本发明实施例还提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机可执行指令，所述计算机可执行指令在被处理器调用和执行时，计算机可执行指令促使处理器实现第一方面提供的任一项所述的方法。

本发明实施例提供的一种机械臂路径的规划方法、装置、上位控制端及存储介质，如果接收到路径规划请求，则获取机械臂所处场景对应的实时障碍物场景，实时障碍物场景是基于剔除机械臂视觉数据之后的实时视觉数据构建得到的，再基于实时障碍物场景和路径规划请求携带的规划目标数据进行静态全局路径规划，得到机械臂对应的初始全局路径，如果所述路径规划请求携带的规划类型为避障类型，遍历初始全局路径中的每个轨迹点，并在当前轨迹点对应的下一轨迹点未通过碰撞检测时，基于当前轨迹点和规划目标数据进行实时全局路径规划，得到目标全局路径。上述方法实现了机械臂工作场景中障碍物的实时全局感知，并把机械臂视觉数据从实时视觉数据中剔除，为实时碰撞检测提供精确的实时障碍物场景，以及提升环境适应性，另外，本发明实施例通过全局路径静态规划、局部碰撞检测与实时全局规划，可以只针对当前轨迹点的后续少量轨迹点进行碰撞检测，并在碰撞检测未通过时使用当前轨迹点以及规划目标数据进行实时全局路径规划，可以有效提高计算效率、规划成功率以及轨迹的合理性。

本发明的其他特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点在说明书、权利要求书以及附图中所特别指出的结构来实现和获得。

为使本发明的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。

附图说明

为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例提供的一种机械臂路径的规划方法的流程示意图；

图2为本发明实施例提供的一种***硬件模块构成框图；

图3为本发明实施例提供的一种***软件模块构成框图；

图4为本发明实施例提供的一种点云数据示意图；

图5为本发明实施例提供的一种在线识别模块的工作流程图；

图6为本发明实施例提供的一种目标全局轨迹的示意图；

图7为本发明实施例提供的一种机械臂路径的规划方法的工作流程图；

图8为本发明实施例提供的一种机械臂路径的规划装置的结构示意图；

图9为本发明实施例提供的一种上位控制端的结构示意图。

具体实施方式

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

目前，相关技术提供的基于改进RRT算法的机器人实时路径规划方法，首先进行全局的静态路径规划，机器人按照运动路径开始运动，每运动到一个节点均以该节点为中心确定一定范围内是否由于障碍物的影响导致机器人不能通过，若不能通过则进行局部范围内的规划，最后把局部路径与全局路径进行连接，该方法不适用于机械臂规划，利用该方法规划得到的路径并非全局最优，而且规划的成功率下降明显。基于此，本发明实施提供了一种机械臂路径的规划方法、装置、上位控制端及存储介质，可以显著提高实时路径的规划效率、规划成功率和轨迹合理性，还可以提供准确度更高的环境数据，以及提升环境适应性。

为便于对本实施例进行理解，首先对本发明实施例所公开的一种机械臂路径的规划方法进行详细介绍，该方法应用于机械臂的上位控制端，参见图1所示的一种机械臂路径的规划方法的流程示意图，该方法主要包括以下步骤S102至步骤S106：

步骤S102，如果接收到路径规划请求，则获取机械臂所处场景对应的实时障碍物场景。其中，实时障碍物场景是基于剔除机械臂视觉数据之后的实时视觉数据构建得到的，实时视觉数据包括场景点云数据和图像数据，路径规划请求携带有规划类型和规划目标数据，规划类型包括避障类型和力控类型，规划目标数据至少包括目标点的坐标数据。在一种实施方式中，上位控制端与激光雷达、视觉传感器通信连接，可以获取激光雷达采集的场景点云数据以及视觉传感器采集的图像数据等实时视觉的数据，通过对实时视觉数据进行预处理、目标识别处理、语义分割处理等，可以得到场景点云数据的语义分割结果，基于该语义分割结果可以确定场景点云数据中包含的机械臂点云数据，通过将机械臂点云数据从场景点云数据中剔除，即可基于剔除机械臂点云数据之后的场景点云数据构建实时障碍物场景。

步骤S104，基于实时障碍物场景和路径规划请求携带的规划目标数据进行静态全局路径规划，得到机械臂对应的初始全局路径。在一种实施方式中，可以基于实时障碍物场景、规划目标数据中作业目标的坐标数据以及预先配置的约束条件，规划机械臂对应的初始全局路径。

步骤S106，如果所述路径规划请求携带的规划类型为避障类型，遍历初始全局路径中的每个轨迹点，并在当前轨迹点对应的下一轨迹点未通过碰撞检测时，基于当前轨迹点和规划目标数据进行实时全局路径规划，得到目标全局路径。在一种实施方式中，可以对当前轨迹点对应的下一轨迹点进行碰撞检测，并在下一轨迹点未通过碰撞检测时，将当前轨迹点作为起始点，将目标点作为终止点，重新规划全局路径，并重复上述操作，直至遍历到末位轨迹点（也即，最后一个轨迹点）。在另一种实施方式中，在下一轨迹点通过碰撞检测时，可以基于当前轨迹点生成机械臂控制指令，以控制机械臂在场景中进行移动，并对下一轨迹点重复上述操作，直至遍历到末位轨迹点。

本发明实施例提供的机械臂路径的规划方法，实现了机械臂工作场景中障碍物的实时全局感知，并把机械臂视觉数据从实时视觉数据中剔除，为实时碰撞检测提供精确的实时障碍物场景，以及提升环境适应性，另外，本发明实施例通过全局路径静态规划、局部碰撞检测与实时全局规划，可以只针对当前轨迹点的后续少量轨迹点进行碰撞检测，并在碰撞检测未通过时使用当前轨迹点以及规划目标数据进行实时全局路径规划，可以有效提高计算效率、规划成功率以及轨迹的合理性。

为便于对上述实施例进行理解，本发明实施例提供了一种机械臂路径的规划方法所涉及的硬件结构，参见图2所示的一种***硬件模块构成框图，包括机械臂上位控制端、机械臂下位控制端、机械臂、视觉传感器/激光雷达、力觉传感器。本实施例采用激光雷达作为现场障碍物的实时检测设备，力觉传感器采用6维力觉传感器，机械臂采用6自由度串联式机械臂。

在进行机械臂的动态规划的过程中，首先要实时检测动态的障碍物，在机械臂的工作场景中，感知设备如双目相机（也即，上述视觉传感器）或者激光雷达，一般布置在机械臂外，以获取全局的场景数据，因此感知设备的视野内存在着机械臂以及障碍物，并且障碍物的类型是不可预知的，机械臂并非障碍物，因此首先识别场景中的障碍物，以将机械臂从场景数据中剔除。其中，上位控制端分别与激光雷达、视觉传感器通信连接，机械臂视觉数据包括机械臂点云数据。

本发明实施例还提供了一种机械臂路径的规划方法所涉及的软件结构，参见图3所示的一种***软件模块构成框图，包括在线识别模块、力觉传感器数据处理模块、实时规划模块以及机械臂控制模块。具体的：

（a）力觉传感器模块读取6维力觉传感器数据，并对数据进行处理，计算出机械臂末端的受力情况f，f为6维矢量。

（b）在线识别模块使用深度学习网络模型PointNet++对点云进行语义分割，在场景点云数据的基础上，将分割得到的机械臂点云数据剔除，再将剔除机械臂点云数据的场景点云数据输出到规划场景，作为障碍物。

（c）如图3所示，实时规划模块包括规划调度模块，全局规划模块以及局部规划模块。

在一种实施方式中，规划调度模块创建有实时规划服务端，负责接收实时规划的请求，创建有全局规划客户端，负责发送全局规划请求，创建了局部规划客户端，负责发送局部规划请求，规划调度模块协调全局规划器以及局部规划器完成实时的规划以及动作的执行。

在一种实施方式中，全局规划模块的主要任务是解决全局的规划问题，采用的步骤是“感知-规划-执行”，规划算法是概率完备的但不能做到实时安全，主要是用于产生一条初始的轨迹。在具体实现时，全局规划模块创建了全局规划服务端，接受全局规划客户端的调用，规划结果发布器用于对外发布规划出来的轨迹，规划流水线是负责根据起始位置、目标位置、约束条件以及规划场景，完成全局路径的规划。

在一种实施方式中，局部规划模块处理全局轨迹的更新，基于机器人的实时状态、规划场景以及参考轨迹处理局部规划的问题，并且发送位置控/速度控指令到机械臂控制器。在具体实现时，局部规划模块由规划结果订阅器、局部规划服务端、轨迹处理模块、局部约束求解器以及关节命令发布器组成，规划结果订阅器负责监听全局规划模块发布出来的规划好的全局轨迹，通知轨迹处理模块把全局轨迹作为参考轨迹进行保存；局部规划服务端负责接收局部规划客户端的调用，局部约束求解器用于根据实时情况进行局部规划求解，关节命令发布器用于发布局部轨迹到机械臂控制模块，从而实现轨迹的执行。

（d）机械臂控制模块负责执行机械臂的控制指令，并上传机械臂的实时状态信息。

在上述硬件结构和软件结构的基础上，本发明实施例提供了一种获取机械臂所处场景对应的实时障碍物场景的实施方式，具体由在线识别模块获取实时障碍物场景，参见如下（1.1）至（1.4）：

（1.1）获取激光雷达采集的场景点云数据，以及获取视觉传感器采集的图像数据。其中，实时视觉数据包括场景点云数据和/或图像数据。在具体实现时，由在线识别模块获取激光雷达采集的场景点云数据，以及由在线识别模块获取视觉传感器采集的图像数据。示例性的，参见图4所示的一种点云数据示意图，原始点云数据中包含机械臂、绝缘子、水瓶、桌面等物体。

（1.2）对实时视觉数据依次进行预处理，以降低实时视觉数据中的噪声数据。其中，预处理包括直通滤波和去除离群点。在具体实现时，由在线识别模块对实时视觉数据依次进行直通滤波和去除离群点，以减少非必要的数据和噪声数据。

（1.3）确定预处理后的实时视觉数据的语义分割结果，以基于语义分割结果确定场景点云数据中包含的机械臂点云数据。在一种实施方式中，可以通过预先训练的神经网络，对预处理后的实时视觉数据进行识别及语义分割处理，得到语义分割结果，其中，神经网络包括PointNet++网络。在具体实现时，由在线识别模块使用深度学习神经网络架构PointNet++网络对场景数据进行在线识别以及语义分割，以基于语义分割结果确定场景点云数据中包含的机械臂点云数据。

（1.4）将机械臂点云数据从场景点云数据中剔除，并基于剔除机械臂点云数据的场景点云数据构建实时障碍物场景。在一种实施方式中，由在线识别模块对场景点云数据进行后处理，基于语义分割结果将机械臂点云数据从场景点云数据中去除，如图4所示，机械臂点云数据已被去除，再将实时障碍物场景发布到实时规划模块。

为便于理解，本发明实施例提供了如图5所示的一种在线识别模块的工作流程图，包括获取实时视觉数据、数据预处理、目标识别处理以及语义分割处理、点云后处理、点云发布。

在具体实现时，规划调度模块负责接受规划的请求，协调全局规划器以及局部规划器完成运动的规划以及执行。

示例性的，规划调度模块接收路径规划请求，并向全局路径规划模块发送规划请求，全局路径规划模块将执行基于实时障碍物场景和路径规划请求携带的规划目标数据进行静态全局路径规划，得到机械臂对应的初始全局路径的步骤，由全局规划模块采用快速扩展随机树（RRT-conect）算法，基于预设约束条件、实时障碍物场景、路径规划请求携带的规划目标数据进行静态全局路径规划，得到机械臂对应的初始全局路径。全局规划模块的主要任务是解决全局的规划问题，采用的步骤是“感知-规划-执行”，规划算法是概率完备的但不能做到实时安全，主要是用于产生一条初始全局轨迹。

示例性的，规划调度模块接收路径规划请求，并向局部路径规划模块发送规划请求，局部路径规划模块将执行前述步骤S106。其中，局部规划模块处理全局轨迹的更新，基于机器人的实时状态、规划场景以及参考轨迹处理局部规划的问题，并且发送位置控/速度控指令到机器人控制器。其中，上位控制端还与机械臂的下位控制端通信连接。为便于理解，本发明实施例提供了一种在当前轨迹点对应的下一轨迹点未通过碰撞检测时，基于当前轨迹点和规划目标数据进行实时全局路径规划的实施方式，参见如下步骤a至步骤c：

步骤a，基于实时障碍物场景对当前轨迹点对应的下一轨迹点进行碰撞检测。如果下一轨迹点未通过碰撞检测，则执行步骤b；如果下一轨迹点通过碰撞检测，则执行步骤c。

步骤b，将当前轨迹点作为轨迹起始点，以及将规划目标数据中的目标点作为轨迹终止点，基于轨迹起始点和轨迹终止点进行静态全局路径规划，得到更新后的初始全局路径，继续遍历更新后的初始全局路径中的每个轨迹点，直至达到末位轨迹点。在具体实现时，如果碰撞检测失败则以当前轨迹点为轨迹起始点，最终目标点为轨迹终止点进行全局路径规划。

步骤c，基于当前轨迹点生成第一机械臂控制指令，将第一机械臂控制指令发送至下位控制端，以通过下位控制端基于第一机械臂控制指令，控制机械臂在场景内移动；将下一轨迹点作为新的当前轨迹点，并在新的当前轨迹点对应的下一轨迹点通过碰撞检测时，基于新的当前轨迹点生成第一机械臂控制指令，直至执行至末位轨迹点。在具体实现时，如果碰撞检测成功，则发送第一机械臂控制指令到机械臂下位控制端，并移动至下一个轨迹点。参见图6所示的一种目标全局轨迹的示意图，采用实时全局规划相比于初始全局轨迹、局部避障规划均有明显的优势，由于在机械臂的工作场景中，如果采用局部避障规划，由于规划的目标点是近邻区域的下一个不碰撞点，规划的结果会现局部最优但全局不优的轨迹；如果采用实时全局的避障规划，由于目标点离障碍物远，可以大概率的得到全局较优的轨迹。本发明实施例通过上述步骤a至步骤c，可以根据场景中实时障碍物的改变，迅速的计算出可到达目标位置的路径，具有成功率高、实时性好等优点。

考虑到在实时路径规划场景中，除了要解决碰撞的问题以外，还需要解决力觉伺服的路径调整问题，在力觉伺服控制时，需要对全局的路径轨迹点进行实时的调整以满足柔性作业的要求。因此，在一种实施方式中，如果路径规划请求携带的规划类型为力控类型，则可以由局部规划模块执行如下步骤：（2.1）获取力觉传感器采集的传感器数据；（2.2）基于传感器数据确定机械臂末端的受力数据；（2.3）根据受力数据确定当前轨迹点对应的下一轨迹点的调整量，以基于调整量更新下一轨迹点，并基于更新后的下一轨迹点生成第二机械臂控制指令；（2.4）将第二机械臂控制指令发送至下位控制端，以通过下位控制端基于第二机械臂控制指令，控制机械臂在场景内移动。本发明实施例通过力控计算确定下一个轨迹点的调整量，并根据调整量更新下一个轨迹点，将相应的第二机械臂控制指令发送至机械臂下位控制端，并移动至下一个轨迹点，检查是否已经到达最后一个轨迹点，如果尚未达到则重复上述步骤（2.1）至（2.4），如果已经到达最后一个轨迹点，则返回规划成功的结果到规划调度模块。

本发明解决的主要问题是如何解决机械臂实时路径中存在的问题，可以通过使用深度学习神经网络，实现场景中物体的识别与分割，从而实现机械臂点云从规划场景中剔除；还可以通过对不同的实时规划类型，采用不同的实时规划策略，对于实时避障规划、采用全局路径静态规划、局部碰撞检测与实时全局规划的方法；对于力控类型的规划，采用的是全局路径静态规划与局部规划的方法，实现了实时性好、计算效率高、结果合理并且适用性广的实时路径规划方法与***。

为便于对前述实施例提供的机械臂路径的规划方法进行理解，本发明实施例提供了一种机械臂路径的规划方法的应用示例，参见图7所示的一种机械臂路径的规划方法的工作流程图，包括如下步骤1至步骤13：

步骤1：规划调度模块接受规划请求，规划请求包含规划目标以及规划类型；

步骤2：规划调度模块发送全局规划请求；

步骤3：全局规划模块根据规划目标以及规划场景进行全局路径规划；

步骤4：如果规划成功，全局规划模块返回规划成功的结果到规划调度模块，并且把计算得到的全局参考轨迹发送到局部规划器，以使局部规划期存储全局参考轨迹；如果规划失败，返回规划失败的结果到规划调度模块，实时路径规划结束；

步骤5：规划调度模块发送局部规划请求；

步骤6：局部规划器获取下一个轨迹点；

步骤7：局部规划器判断规划类型。如果规划类型为避障类型，则执行步骤8；如果规划类型为力控类型，则执行步骤9。

步骤8：获取实时障碍物场景，对下一个轨迹点进行碰撞检测。如果碰撞检测失败则以当前位置为起始点，最终目标点为终点进行全局路径规划，跳转到步骤2，如果碰撞检测成功，则发送指令到机械臂控制器，移动至下一个轨迹点；

步骤9：力控计算，通过力控计算以确定下一个轨迹点的调整量；

步骤10：更新局部轨迹，可以根据调整量更新下一个轨迹点；

步骤11：发送指令到机械臂控制器，移动至下一个轨迹点；

步骤12：检查是否已经到达最后一个轨迹点，如果尚未达到则跳转到步骤6，如果已经到达最后一个轨迹点，则返回规划成功的结果到规划调度模块；

步骤13：实时路径规划结束。

综上所述，本发明实施例提供的机械臂路径的规划方法，至少具有以下特点：

（1）实现了机械臂工作场景中障碍物的实时全局感知，并把机械臂模型从障碍物去除，为实时碰撞检测提供精确的环境数据；

（2）适用性好，实时路径规划不仅适用于避障场景，亦适用于力控场景；

（3）路径执行时，只针对当前位置的后续少量点进行碰撞检测，检测出碰撞时，使用当前位置以及最终目标位置进行实时全局路径规划的方法，可以提高计算效率，规划成功率以及轨迹的合理性；

（4）采用了在原有点云的基础上去除机械臂点云的方法来实时检测场景，进入工作场景中的障碍物可以不进行识别，提升了环境适应性。

对于前述实施例提供的机械臂路径的规划方法，本发明实施例提供了一种机械臂路径的规划装置，该装置应用于机械臂的上位控制端，参见图8所示的一种机械臂路径的规划装置的结构示意图，该装置主要包括以下部分：

规划调度模块802，用于如果接收到路径规划请求，则获取机械臂所处场景对应的实时障碍物场景；其中，实时障碍物场景是基于剔除机械臂视觉数据之后的实时视觉数据构建得到的；

全局路径规划模块804，用于基于实时障碍物场景和路径规划请求携带的规划目标数据进行静态全局路径规划，得到机械臂对应的初始全局路径；

局部路径规划模块806，用于如果所述路径规划请求携带的规划类型为避障类型，遍历初始全局路径中的每个轨迹点，并在当前轨迹点对应的下一轨迹点未通过碰撞检测时，基于当前轨迹点和规划目标数据进行实时全局路径规划，得到目标全局路径。

本发明实施例提供的机械臂路径的规划装置，实现了机械臂工作场景中障碍物的实时全局感知，并把机械臂视觉数据从实时视觉数据中剔除，为实时碰撞检测提供精确的实时障碍物场景，以及提升环境适应性，另外，本发明实施例通过全局路径静态规划、局部碰撞检测与实时全局规划，可以只针对当前轨迹点的后续少量轨迹点进行碰撞检测，并在碰撞检测未通过时使用当前轨迹点以及规划目标数据进行实时全局路径规划，可以有效提高计算效率、规划成功率以及轨迹的合理性。

在一种实施方式中，上位控制端分别与激光雷达、视觉传感器通信连接，机械臂视觉数据包括机械臂点云数据；上述装置还包括在线识别模块，用于：获取激光雷达采集的场景点云数据，以及获取视觉传感器采集的图像数据；其中，实时视觉数据包括场景点云数据和/或图像数据；对实时视觉数据依次进行预处理，以降低实时视觉数据中的噪声数据；确定预处理后的实时视觉数据的语义分割结果，以基于语义分割结果确定场景点云数据中包含的机械臂点云数据；将机械臂点云数据从场景点云数据中剔除，并基于剔除机械臂点云数据的场景点云数据构建实时障碍物场景。

在一种实施方式中，局部路径规划模块806还用于：基于实时障碍物场景对当前轨迹点对应的下一轨迹点进行碰撞检测；如果下一轨迹点未通过碰撞检测，则将当前轨迹点作为轨迹起始点，以及将规划目标数据中的目标点作为轨迹终止点；基于轨迹起始点和轨迹终止点进行静态全局路径规划，得到更新后的初始全局路径，继续遍历更新后的初始全局路径中的每个轨迹点，直至达到末位轨迹点。

在一种实施方式中，上位控制端还与机械臂的下位控制端通信连接，局部路径规划模块806还用于：如果下一轨迹点通过碰撞检测，则基于当前轨迹点生成第一机械臂控制指令；将第一机械臂控制指令发送至下位控制端，以通过下位控制端基于第一机械臂控制指令，控制机械臂在场景内移动将下一轨迹点作为新的当前轨迹点，并在新的当前轨迹点对应的下一轨迹点通过碰撞检测时，基于新的当前轨迹点生成第一机械臂控制指令，直至执行至末位轨迹点。

在一种实施方式中，上位控制端还与力觉传感器、机械臂的下位控制端通信连接；上述装置还包括力觉传感器数据处理模块，用于：如果路径规划请求携带的规划类型为力控类型，则获取力觉传感器采集的传感器数据；基于传感器数据，确定机械臂末端的受力数据；局部路径规划模块806还用于：根据受力数据确定当前轨迹点对应的下一轨迹点的调整量，以基于调整量更新下一轨迹点，并基于更新后的下一轨迹点生成第二机械臂控制指令；将第二机械臂控制指令发送至下位控制端，以通过下位控制端基于第二机械臂控制指令，控制机械臂在场景内移动。

在一种实施方式中，全局路径规划模块804还用于：采用快速扩展随机树算法，基于预设约束条件、实时障碍物场景、路径规划请求携带的规划目标数据进行静态全局路径规划，得到机械臂对应的初始全局路径。

在一种实施方式中，局部路径规划模块806还用于：通过预先训练的神经网络，对预处理后的实时视觉数据进行识别及语义分割处理，得到语义分割结果；其中，神经网络包括PointNet++网络。

本发明实施例所提供的装置，其实现原理及产生的技术效果和前述方法实施例相同，为简要描述，装置实施例部分未提及之处，可参考前述方法实施例中相应内容。

本发明实施例提供了一种上位控制端，具体的，该上位控制端包括处理器和存储装置；存储装置上存储有计算机程序，计算机程序在被所述处理器运行时执行如上所述实施方式的任一项所述的方法 。

图9为本发明实施例提供的一种上位控制端的结构示意图，该上位控制端100包括：处理器90，存储器91，总线92和通信接口93，所述处理器90、通信接口93和存储器91通过总线92连接；处理器90用于执行存储器91中存储的可执行模块，例如计算机程序。

其中，存储器91可能包含高速随机存取存储器（RAM，Random Access Memory），也可能还包括非不稳定的存储器（non-volatile memory），例如至少一个磁盘存储器。通过至少一个通信接口93（可以是有线或者无线）实现该***网元与至少一个其他网元之间的通信连接，可以使用互联网，广域网，本地网，城域网等。

总线92可以是ISA总线、PCI总线或EISA总线等。所述总线可以分为地址总线、数据总线、控制总线等。为便于表示，图9中仅用一个双向箭头表示，但并不表示仅有一根总线或一种类型的总线。

其中，存储器91用于存储程序，所述处理器90在接收到执行指令后，执行所述程序，前述本发明实施例任一实施例揭示的流过程定义的装置所执行的方法可以应用于处理器90中，或者由处理器90实现。

处理器90可能是一种集成电路芯片，具有信号的处理能力。在实现过程中，上述方法的各步骤可以通过处理器90中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器90可以是通用处理器，包括中央处理器(Central Processing Unit，简称CPU)、网络处理器(Network Processor，简称NP)等；还可以是数字信号处理器(Digital SignalProcessing，简称DSP)、专用集成电路(Application Specific Integrated Circuit，简称ASIC)、现成可编程门阵列(Field-Programmable Gate Array，简称FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。可以实现或者执行本发明实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。结合本发明实施例所公开的方法的步骤可以直接体现为硬件译码处理器执行完成，或者用译码处理器中的硬件及软件模块组合执行完成。软件模块可以位于随机存储器，闪存、只读存储器，可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器91，处理器90读取存储器91中的信息，结合其硬件完成上述方法的步骤。

本发明实施例所提供的可读存储介质的计算机程序产品，包括存储了程序代码的计算机可读存储介质，所述程序代码包括的指令可用于执行前面方法实施例中所述的方法，具体实现可参见前述方法实施例，在此不再赘述。

所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备（可以是个人计算机，服务器，或者网络设备等）执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器（ROM，Read-Only Memory）、随机存取存储器（RAM，Random Access Memory）、磁碟或者光盘等各种可以存储程序代码的介质。

最后应说明的是：以上所述实施例，仅为本发明的具体实施方式，用以说明本发明的技术方案，而非对其限制，本发明的保护范围并不局限于此，尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，其依然可以对前述实施例所记载的技术方案进行修改或可轻易想到变化，或者对其中部分技术特征进行等同替换；而这些修改、变化或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的精神和范围，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应所述以权利要求的保护范围为准。

Claims (10)

1.一种机械臂路径的规划方法，其特征在于，所述方法应用于机械臂的上位控制端，包括：

如果接收到路径规划请求，则获取机械臂所处场景对应的实时障碍物场景；其中，所述实时障碍物场景是基于剔除机械臂视觉数据之后的实时视觉数据构建得到的；

基于所述实时障碍物场景和所述路径规划请求携带的规划目标数据进行静态全局路径规划，得到所述机械臂对应的初始全局路径；

如果所述路径规划请求携带的规划类型为避障类型，则遍历所述初始全局路径中的每个轨迹点，并在当前轨迹点对应的下一轨迹点未通过碰撞检测时，基于所述当前轨迹点和所述规划目标数据进行实时全局路径规划，得到目标全局路径。

2.根据权利要求1所述的机械臂路径的规划方法，其特征在于，所述上位控制端分别与激光雷达、视觉传感器通信连接，所述机械臂视觉数据包括机械臂点云数据；所述获取机械臂所处场景对应的实时障碍物场景的步骤，包括：

获取所述激光雷达采集的场景点云数据，以及获取所述视觉传感器采集的图像数据；其中，所述实时视觉数据包括所述场景点云数据和/或所述图像数据；

对所述实时视觉数据依次进行预处理，以降低所述实时视觉数据中的噪声数据；

确定预处理后的实时视觉数据的语义分割结果，以基于所述语义分割结果确定所述场景点云数据中包含的机械臂点云数据；

将所述机械臂点云数据从所述场景点云数据中剔除，并基于剔除所述机械臂点云数据的场景点云数据构建实时障碍物场景。

3.根据权利要求1所述的机械臂路径的规划方法，其特征在于，所述在当前轨迹点对应的下一轨迹点未通过碰撞检测时，基于所述当前轨迹点和所述规划目标数据进行实时全局路径规划的步骤，包括：

基于所述实时障碍物场景对当前轨迹点对应的下一轨迹点进行碰撞检测；

如果所述下一轨迹点未通过所述碰撞检测，则将所述当前轨迹点作为轨迹起始点，以及将所述规划目标数据中的目标点作为轨迹终止点；

基于所述轨迹起始点和所述轨迹终止点进行静态全局路径规划，得到更新后的初始全局路径，继续遍历更新后的初始全局路径中的每个轨迹点，直至达到末位轨迹点。

4.根据权利要求3所述的机械臂路径的规划方法，其特征在于，所述上位控制端还与机械臂的下位控制端通信连接，所述方法还包括：

如果所述下一轨迹点通过所述碰撞检测，则基于所述当前轨迹点生成第一机械臂控制指令；

将所述第一机械臂控制指令发送至所述下位控制端，以通过所述下位控制端基于所述第一机械臂控制指令，控制所述机械臂在所述场景内移动；

将所述下一轨迹点作为新的当前轨迹点，并在所述新的当前轨迹点对应的下一轨迹点通过所述碰撞检测时，基于所述新的当前轨迹点生成第一机械臂控制指令，直至执行至末位轨迹点。

5.根据权利要求1所述的机械臂路径的规划方法，其特征在于，所述上位控制端还与力觉传感器、机械臂的下位控制端通信连接；所述方法还包括：

如果所述路径规划请求携带的规划类型为力控类型，则获取所述力觉传感器采集的传感器数据；

基于所述传感器数据，确定所述机械臂末端的受力数据；

根据所述受力数据确定所述当前轨迹点对应的下一轨迹点的调整量，以基于所述调整量更新所述下一轨迹点，并基于更新后的下一轨迹点生成第二机械臂控制指令；

将所述第二机械臂控制指令发送至所述下位控制端，以通过所述下位控制端基于所述第二机械臂控制指令，控制所述机械臂在所述场景内移动。

6.根据权利要求1所述的机械臂路径的规划方法，其特征在于，所述基于所述实时障碍物场景和所述路径规划请求携带的规划目标数据进行静态全局路径规划，得到所述机械臂对应的初始全局路径的步骤，包括：

采用快速扩展随机树算法，基于预设约束条件、所述实时障碍物场景、所述路径规划请求携带的规划目标数据进行静态全局路径规划，得到所述机械臂对应的初始全局路径。

7.根据权利要求2所述的机械臂路径的规划方法，其特征在于，所述确定预处理后的实时视觉数据的语义分割结果的步骤，包括：

通过预先训练的神经网络，对预处理后的实时视觉数据进行识别及语义分割处理，得到语义分割结果；其中，所述神经网络包括PointNet++网络。

8.一种机械臂路径的规划装置，其特征在于，所述装置应用于机械臂的上位控制端，包括：

规划调度模块，用于如果接收到路径规划请求，则获取机械臂所处场景对应的实时障碍物场景；其中，所述实时障碍物场景是基于剔除机械臂视觉数据之后的实时视觉数据构建得到的；

全局路径规划模块，用于基于所述实时障碍物场景和所述路径规划请求携带的规划目标数据进行静态全局路径规划，得到所述机械臂对应的初始全局路径；

局部路径规划模块，用于如果所述路径规划请求携带的规划类型为避障类型，遍历所述初始全局路径中的每个轨迹点，并在当前轨迹点对应的下一轨迹点未通过碰撞检测时，基于所述当前轨迹点和所述规划目标数据进行实时全局路径规划，得到目标全局路径。

9.一种上位控制端，其特征在于，包括处理器和存储器，所述存储器存储有能够被所述处理器执行的计算机可执行指令，所述处理器执行所述计算机可执行指令以实现权利要求1至7任一项所述的机械臂路径的规划方法。

10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机可执行指令，所述计算机可执行指令在被处理器调用和执行时，计算机可执行指令促使处理器实现权利要求1至7任一项所述的机械臂路径的规划方法。

Images