CN115793647A - 一种机器人避障路径规划方法、系统及介质 - Google Patents

Abstract

本申请公开了一种机器人避障路径规划方法、系统及介质，方法包括：确定场景中机器人的类型及机器人工作范围内的静态障碍物的类型，根据所述机器人及静态障碍物对场景进行三维重建，确定需要执行任务的机器人模型及当前状态，对机器人进行标记；使用八叉树构建并压缩初始状态机器人点云及障碍物点云；对装配物起始位姿到结束位姿进行规划；使用包围盒模型构建碰撞检测对，判断当前状态机器人是否与场景中障碍物碰撞；确定优化目标，得到当前最优路径轨迹；更新点云，并对机器人的机械臂的各个状态进行碰撞检测，确定当前路径的有效性，根据机械臂是否发生碰撞，输出机器人运动路径。本发明减少了传统机器人路径规划中示教成本，提升了规划效率。

Description

一种机器人避障路径规划方法、系统及介质

技术领域

本申请涉及机器人避障技术领域，具体而言，涉及一种机器人避障路径规划方法、系统及介质。

背景技术

在工业生产线中，机器人的参与程度越趋频繁，根据任务情形的不同，使用的机器人类型及型号也有差异，如何帮助方案工程师快速高效的设搭建机器人产线，快速规划机器人执行任务的无碰撞运动路径，提供可行的解决方案亟待解决。现阶段，机器人主要还是以离线的方式参与任务，即需要提前为机器人的运动示教可行的任务路径，这需要耗费大量的时间去调试示教，规划合理的任务路径。在多障碍物的场景中示教更频繁，人工示教的效率十分低下。因此，提供一种新的技术方案改善上述问题，是本领域技术人员急需解决的问题。

发明内容

本申请的目的在于提供一种机器人避障路径规划方法、系统及介质，可以对机器人各关节的点云与障碍物的点云进行碰撞检测，能够在机器人参与生产线的场景中，快速规划出机器人的无碰撞任务轨迹。

本申请还提供了机器人避障路径规划方法，包括以下步骤：

种机器人避障路径规划方法，其特征在于，所述方法包括：

确定场景中机器人的类型及机器人工作范围内的静态障碍物的类型，根据所述机器人及静态障碍物对场景进行三维重建，确定需要执行任务的机器人模型及当前状态，对机器人进行标记；

使用八叉树构建并压缩初始状态机器人点云及障碍物点云；

对装配物起始位姿到结束位姿进行规划，得到一组装配物从起始点到结束点的位姿点序列，根据所述位姿点序列得到机器人的机械臂在当前状态的关节运动量，同时更新机械臂点云；

使用包围盒模型构建碰撞检测对，判断当前状态机器人的机械臂是否与场景中障碍物碰撞，以不发生碰撞时对应的位姿点作为初始规划路径；

以机器人执行任务时运动的最短路径作为优化目标，在初始规划路径基础上得到当前最优路径轨迹；

利用当前最优路径轨迹更新点云，并对机器人的机械臂各个状态进行碰撞检测，确定当前路径的有效性，根据机械臂是否发生碰撞，输出机器人运动路径。

可选地，确定场景中机器人的类型及机器人工作范围内的静态障碍物的类型，根据所述机器人及静态障碍物对场景进行三维重建，具体为：

确定场景中机器人的类型、机器人的工作范围内的静态障碍物模型，获取机器人、夹具、装配物及障碍物的空间坐标及网格信息，根据所述空间坐标和网格信息得到位姿信息，根据位姿信息对场景进行三维重建。

可选地，确定需要执行任务的机器人模型及当前状态，对机器人进行标记还包括：

对机器人工作范围内的其他模型进行识别，若所选择模型为夹具或装配物，将其标识为会跟随机器人运动的模型，否则将其标记为障碍物，确定装配物的任务起点及任务终点。

可选地，使用八叉树构建并压缩初始状态机器人点云及障碍物点云，具体为：设置八叉树构建碰撞状态时点云的精度为Xcm,即在根节点Xcm范围内的会被融合为同一个坐标点。

可选地，对装配物起始位姿到结束位姿进行规划，得到一组装配物从起始点到结束点的位姿点序列，根据所述位姿点序列得到机器人的机械臂在当前状态的关节运动量，同时更新机械臂点云；具体为：

根据机器人运动学约束条件，设置机器人的工作空间的可达范围，采用均匀分布的方式对规划空间进行采样，使用RRTConnect算法搜索机器人从任务起点到任务终点的可行状态；根据当前状态位置和姿态计算对应的旋转和平移矩阵，根据旋转和平移矩阵更新当前状态下的机器人机械臂点云位置和姿态，更新机器人机械臂的点云。

可选地，使用包围盒模型构建碰撞检测对，判断当前状态机器人是否与场景中障碍物碰撞，以不发生碰撞时对应的位姿点作为初始规划路径；具体为：

使用AABB包围盒模型对机器人以及障碍物的点云进行碰撞简化处理，根据碰撞检测对的检测状态结果，确定当前搜索节点是否有效，所述碰撞检测对即机器人与障碍物组成的检测对；

若当前节点无效，更新上一节点为下一次搜索的初始节点，重新进行搜索，如果在当前状态及机器人与障碍物不发生碰撞，则将当前状态标记为可行状态，下一次更新的起始节点为当前状态的节点，直到满足终止条件。

可选地，以机器人执行任务时运动的最短路径作为优化目标，在初始规划路径基础上得到当前最优路径轨迹，具体为：

在初始规划的路径基础上，计算当前节点与有效节点之间的距离，记录运动量最小的状态节点，组成新的轨迹，即当前最优路径轨迹。

可选地，利用当前最优路径轨迹更新点云，并对机器人的机械臂的各个状态进行碰撞检测，确定当前路径的有效性，根据机械臂是否发生碰撞，输出机器人运动路径，具体为：

计算机器人在当前最优路径轨迹中各节点的关节角，计算点云旋转和平移矩阵，更新点云，对机器人的机械臂的各个状态进行碰撞检测，确定当前路径的有效性，如果机械臂发生碰撞则返回对装配物起始位姿到结束位姿进行规划，否则停止规划，输出机器人运动路径。

第二方面，本申请提供了一种机器人避障路径规划系统，该系统包括：存储器及处理器，所述存储器中包括一种机器人避障路径规划方法的程序，所述一种机器人避障路径规划方法的程序被所述处理器执行时实现以下步骤：

确定场景中机器人的类型及机器人工作范围内的静态障碍物的类型，根据所述机器人及静态障碍物对场景进行三维重建，确定需要执行任务的机器人模型及当前状态，对机器人进行标记；

使用八叉树构建并压缩初始状态机器人点云及障碍物点云；

对装配物起始位姿到结束位姿进行规划，得到一组装配物从起始点到结束点的位姿点序列，根据所述位姿点序列得到机器人的机械臂在当前状态的关节运动量，同时更新机械臂点云；

使用包围盒模型构建碰撞检测对，判断当前状态机器人的机械臂是否与场景中障碍物碰撞，以不发生碰撞时对应的位姿点作为初始规划路径；

以机器人执行任务时运动的最短路径作为优化目标，在初始规划路径基础上得到当前最优路径轨迹；

利用当前最优路径轨迹更新点云，并对机器人的机械臂各个状态进行碰撞检测，确定当前路径的有效性，根据机械臂是否发生碰撞，输出机器人运动路径。

第三方面，本申请还提供了一种计算机可读存储介质，所述计算机可读存储介质中包括机器人避障路径规划方法程序，所述机器人避障路径规划方法程序被处理器执行时，实现如上述任一项所述的机器人避障路径规划方法的步骤。

由上可知，本申请提供的一种机器人避障路径规划方法、系统及介质通过确定场景中机器人的类型及机器人工作范围内的静态障碍物的类型，进而根据所述机器人及静态障碍物对场景进行三维重建，确定需要执行任务的机器人模型及当前状态，对机器人进行标记，然后使用八叉树构建并压缩初始状态机器人点云及障碍物点云；对装配物起始位姿到结束位姿进行规划，得到一组装配物从起始点到结束点的位姿点序列，根据所述位姿点序列得到机器人的机械臂在当前状态的关节运动量，同时更新机械臂点云；使用包围盒模型构建碰撞检测对，判断当前状态机器人的机械臂是否与场景中障碍物碰撞，以不发生碰撞时对应的位姿点作为初始规划路径；以机器人执行任务时运动的最短路径作为优化目标，在初始规划路径基础上得到当前最优路径轨迹；利用当前最优路径轨迹更新点云，并对机器人的机械臂的各个状态进行碰撞检测，确定当前路径的有效性，根据机械臂是否发生碰撞，输出机器人运动路径，本发明减少了传统机器人路径规划中示教成本，提出的碰撞检测算法避免了在规划过程中对机械臂所有关节的碰撞检测，大大降低了检测时间，提升了规划效率。

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

附图说明

为了更清楚地说明本申请实施例的技术方案，下面将对本申请实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本申请的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1为本申请实施例提供的机器人避障路径规划方法的流程图；

图2为本申请实施例提供的使用包围盒模型构建碰撞检测的流程图；

图3为本申请实施例提供的机器人避障路径规划系统框图；

具体实施方式

下面将结合本申请实施例中附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。

应注意到，相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。同时，在本申请的描述中，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

请参照图1，为本申请一些实施例中的机器人避障路径规划方法的流程图。该机器人避障路径规划方法用于机器人中，例如用于工业生成中的数字机器人、用于物流行业中的机器人等。该机器人避障路径规划方法，包括以下步骤：

S101、确定场景中机器人的类型及机器人工作范围内的静态障碍物的类型，根据所述机器人及静态障碍物对场景进行三维重建，确定需要执行任务的机器人模型及当前状态，对机器人进行标记；

S102、使用八叉树构建并压缩初始状态机器人点云及障碍物点云；

S103、对装配物起始位姿到结束位姿进行规划，得到一组装配物从起始点到结束点的位姿点序列，根据所述位姿点序列得到机器人的机械臂在当前状态的关节运动量，同时更新机械臂点云；

S104、使用包围盒模型构建碰撞检测对，判断当前状态机器人的机械臂是否与场景中障碍物碰撞，以不发生碰撞时对应的位姿点作为初始规划路径；

S105、以机器人执行任务时运动的最短路径作为优化目标，在初始规划路径基础上得到当前最优路径轨迹；

S106、利用当前最优路径轨迹更新点云，并对机器人的机械臂各个状态进行碰撞检测，确定当前路径的有效性，根据机械臂是否发生碰撞，输出机器人运动路径。

需要说明的是，本技术目的为通过确定场景中机器人的类型及机器人工作范围内的静态障碍物的类型，进而根据所述机器人及静态障碍物对场景进行三维重建，确定需要执行任务的机器人模型及当前状态，对机器人进行标记，然后使用八叉树构建并压缩初始状态机器人点云及障碍物点云；对装配物起始位姿到结束位姿进行规划，得到一组装配物从起始点到结束点的位姿点序列，根据所述位姿点序列得到机器人的机械臂在当前状态的关节运动量，同时更新机械臂点云；使用包围盒模型构建碰撞检测对，判断当前状态机器人的机械臂是否与场景中障碍物碰撞，以不发生碰撞时对应的位姿点作为初始规划路径；以机器人执行任务时运动的最短路径作为优化目标，在初始规划路径基础上得到当前最优路径轨迹；利用当前最优路径轨迹更新点云，并对机器人的机械臂的各个状态进行碰撞检测，确定当前路径的有效性，根据机械臂是否发生碰撞，输出机器人运动路径的技术。

需要说明的是，本发明对于机器人没有关节数量和运动方式的限制，适用于多旋转关节、多平移关节以及旋转与平移关节组合的机器人。

根据本发明实施例，确定场景中机器人的类型及机器人工作范围内的静态障碍物的类型，根据所述机器人及静态障碍物对场景进行三维重建，具体为：

确定场景中机器人的类型、机器人的工作范围内的静态障碍物模型，获取机器人、夹具、装配物及障碍物的空间坐标及网格信息，根据所述空间坐标和网格信息得到位姿信息，根据位姿信息对场景进行三维重建。

需要说明的是，首先需要确定工作场景中机器人的类型，同时还需要确认在机器人工作范围内的静态障碍物模型，确定上述信息后，进而获取机器人、夹具、装配物及障碍物的空间坐标及网格信息，根据空间坐标和网格信息得到位姿信息，根据位姿信息对场景进行三维重建。

根据本发明实施例，确定需要执行任务的机器人模型及当前状态，对机器人进行标记还包括：

对机器人工作范围内的其他模型进行识别，若所选择模型为夹具或装配物，将其标识为会跟随机器人运动的模型，否则将其标记为障碍物，确定装配物的任务起点及任务终点。

需要说明的是，本发明基于数字机器人模型网格信息和骨架信息以及场景其他物体数字模型网格信息，自主确定避障规划空间类型及避障规划范围，只需确定场景中各机器人作业的装配物的无碰撞起始点和结束点的位置和姿态，就可以快速规划出产线上各机器人在各自工位上执行任务时的无碰撞避障轨迹，减少需要大量示教各个机器人运动轨迹所消耗成本。

根据本发明实施例，使用八叉树构建并压缩初始状态机器人点云及障碍物点云，具体为：设置八叉树构建碰撞状态时点云的精度为Xcm,即在根节点Xcm范围内的会被融合为同一个坐标点。

需要说明的是，点云由巨大的数据集组成，数据集描述了与距离、颜色、法线等附加信息相关的三维点。此外，点云可以以很高的速度创建，因此会占用大量内存资源。一旦点云必须在速率受限的通信信道上存储或传输，就必须想办法对这类数据进行压缩。采用八叉树数据结构能够有效地合并来自多个数据源的点云数据。在一个具体的实施例中，设置八叉树构建碰撞状态时点云的精度可以为1cm，即在根节点1cm范围内的会被融合为同一个坐标点。

根据本发明实施例，对装配物起始位姿到结束位姿进行规划，得到一组装配物从起始点到结束点的位姿点序列，根据所述位姿点序列得到机器人的机械臂在当前状态的关节运动量，同时更新机械臂点云；具体为：

根据机器人运动学约束条件，设置机器人的工作空间的可达范围，采用均匀分布的方式对规划空间进行采样，使用RRTConnect算法搜索机器人从任务起点到任务终点的可行状态；根据当前状态位置和姿态计算对应的旋转和平移矩阵，根据旋转和平移矩阵更新当前状态下的机器人点云位置和姿态，更新机器人的点云。

需要说明是，路径规划领域目前被广泛使用且行之有效的是快速搜索随机树算法(RRT)，该算法随机在给定的工作范围内定位起始状态点，根据机器人运动学，该状态可以由机器人各关节的运动量组成，对于多自由度的机器人，其状态就是多维向量，以机器人当前状态到新定位状态的向量，作为机器人状态的变化方向，并设定算法搜索步长。如果机器人沿新方向运动指定的步长后，不会与场景中的障碍物放生碰撞，那么该状态被标记为有效状态点，否则重新随机搜索新的状态点。直到满足算法迭代的终止条件。所有可行状态组成机器人的可达路径，并且该路径不会和场景中的障碍物发生碰撞。本发明中首先只考虑对装配物的避障规划，使用RRTconnect算法规划出一条可行路径，并对该条路径进行路径最短优化处理，其次再在此基础上对机器人在该路径的各节点进行碰撞检测，判断这条路径的有效性，避免了在规划过程中对机械臂所有关节的碰撞检测，大大降低了检测时间，提升了规划效率。

请参照图2，为本申请实施例提供的使用包围盒模型构建碰撞检测的流程图。

根据本发明实施例，使用包围盒模型构建碰撞检测对，判断当前状态机器人是否与场景中障碍物碰撞，以不发生碰撞时对应的位姿点作为初始规划路径；具体为：

S201、使用AABB包围盒模型对机器人以及障碍物的点云进行碰撞简化处理，根据碰撞检测对的检测状态结果，确定当前搜索节点是否有效，所述碰撞检测对即机器人与障碍物组成的检测对；

S202、若当前节点无效，更新上一节点为下一次搜索的初始节点，重新进行搜索，如果在当前状态及机器人与障碍物不发生碰撞，则将当前状态标记为可行状态，下一次更新的起始节点为当前状态的节点，直到满足终止条件。

需要说明的是，本发明采用AABB包围盒模型对机器人以及障碍物的点云进行碰撞简化处理，所述AABB包围盒就是采用一个长方体将物体包裹起来，进行两个物体的相交性检测时仅检测物体对应包围盒(包裹物体的长方体)的相交性。AABB包围盒对应的长方体每一个面都是与某个坐标轴平面平行的，因此，AABB包围盒又称轴对齐包围盒。AABB包围盒构造比较简单，存储空间小。

根据本发明实施例，以机器人执行任务时运动的最短路径作为优化目标，在初始规划路径基础上得到当前最优路径轨迹；具体为：

在初始规划的路径基础上，计算当前节点与有效节点之间的距离，记录运动量最小的状态节点，组成新的轨迹，即当前最优路径轨迹。

根据本发明实施例，利用当前最优路径轨迹更新点云，并对机器人的机械臂的各个状态进行碰撞检测，确定当前路径的有效性，根据机械臂是否发生碰撞，输出机器人运动路径，具体为：

计算机器人在当前最优路径轨迹中各节点的关节角，计算点云旋转和平移矩阵，更新点云，对机器人的机械臂的各个状态进行碰撞检测，确定当前路径的有效性，如果机械臂发生碰撞则返回对装配物起始位姿到结束位姿进行规划，否则停止规划，输出机器人运动路径。

图3示出了一种机器人避障路径规划系统框图。

如图3所示，第二方面，本申请提供了一种机器人避障路径规划系统，该系统包括：存储器301及处理器302，所述存储器中包括一种机器人避障路径规划方法的程序，所述一种机器人避障路径规划方法的程序被所述处理器执行时实现以下步骤：

需要说明的是，本技术目的为通过确定场景中机器人的类型及机器人工作范围内的静态障碍物的类型，进而根据所述机器人及静态障碍物对场景进行三维重建，确定需要执行任务的机器人模型及当前状态，对机器人进行标记，然后使用八叉树构建并压缩初始状态机器人点云及障碍物点云；对装配物起始位姿到结束位姿进行规划，得到一组装配物从起始点到结束点的位姿点序列，根据所述位姿点序列得到机器人的机械臂在当前状态的关节运动量，同时更新机械臂点云；使用包围盒模型构建碰撞检测对，判断当前状态机器人的机械臂是否与场景中障碍物碰撞，以不发生碰撞时对应的位姿点作为初始规划路径；以机器人执行任务时运动的最短路径作为优化目标，在初始规划路径基础上得到当前最优路径轨迹；利用当前最优路径轨迹更新点云，并对机器人的机械臂的各个状态进行碰撞检测，确定当前路径的有效性，根据机械臂是否发生碰撞，输出机器人运动路径的技术。

需要说明的是，本发明对于机器人没有关节数量和运动方式的限制，适用于多旋转关节、多平移关节以及旋转与平移关节组合的机器人。

根据本发明实施例，确定场景中机器人的类型及机器人工作范围内的静态障碍物的类型，根据所述机器人及静态障碍物对场景进行三维重建，具体为：

确定场景中机器人的类型、机器人的工作范围内的静态障碍物模型，获取机器人、夹具、装配物及障碍物的空间坐标及网格信息，根据所述空间坐标和网格信息得到位姿信息，根据位姿信息对场景进行三维重建。

需要说明的是，首先需要确定工作场景中机器人的类型，同时还需要确认在机器人工作范围内的静态障碍物模型，确定上述信息后，进而获取机器人、夹具、装配物及障碍物的空间坐标及网格信息，根据空间坐标和网格信息得到位姿信息，根据位姿信息对场景进行三维重建。

根据本发明实施例，确定需要执行任务的机器人模型及当前状态，对机器人进行标记还包括：

对机器人工作范围内的其他模型进行识别，若所选择模型为夹具或装配物，将其标识为会跟随机器人运动的模型，否则将其标记为障碍物，确定装配物的任务起点及任务终点。

需要说明的是，本发明基于数字机器人模型网格信息和骨架信息以及场景其他物体数字模型网格信息，自主确定避障规划空间类型及避障规划范围，只需确定场景中各机器人作业的装配物的无碰撞起始点和结束点的位置和姿态，就可以快速规划出产线上各机器人在各自工位上执行任务时的无碰撞避障轨迹，减少需要大量示教各个机器人运动轨迹所消耗成本。

根据本发明实施例，使用八叉树构建并压缩初始状态机器人点云及障碍物点云，具体为：设置八叉树构建碰撞状态时点云的精度为Xcm,即在根节点Xcm范围内的会被融合为同一个坐标点。

需要说明的是，点云由巨大的数据集组成，数据集描述了与距离、颜色、法线等附加信息相关的三维点。此外，点云可以以很高的速度创建，因此会占用大量内存资源。一旦点云必须在速率受限的通信信道上存储或传输，就必须想办法对这类数据进行压缩。采用八叉树数据结构能够有效地合并来自多个数据源的点云数据。在一个具体的实施例中，设置八叉树构建碰撞状态时点云的精度可以为1cm，即在根节点1cm范围内的会被融合为同一个坐标点。

根据本发明实施例，对装配物起始位姿到结束位姿进行规划，得到一组装配物从起始点到结束点的位姿点序列，根据所述位姿点序列得到机器人的机械臂在当前状态的关节运动量，同时更新机械臂点云；具体为：

根据机器人运动学约束条件，设置机器人的工作空间的可达范围，采用均匀分布的方式对规划空间进行采样，使用RRTConnect算法搜索机器人从任务起点到任务终点的可行状态；根据当前状态位置和姿态计算对应的旋转和平移矩阵，根据旋转和平移矩阵更新当前状态下的机器人点云位置和姿态，更新机器人的点云。

需要说明是，路径规划领域目前被广泛使用且行之有效的是快速搜索随机树算法(RRT)，该算法随机在给定的工作范围内定位起始状态点，根据机器人运动学，该状态可以由机器人各关节的运动量组成，对于多自由度的机器人，其状态就是多维向量，以机器人当前状态到新定位状态的向量，作为机器人状态的变化方向，并设定算法搜索步长。如果机器人沿新方向运动指定的步长后，不会与场景中的障碍物放生碰撞，那么该状态被标记为有效状态点，否则重新随机搜索新的状态点。直到满足算法迭代的终止条件。所有可行状态组成机器人的可达路径，并且该路径不会和场景中的障碍物发生碰撞。本发明中首先只考虑对装配物的避障规划，使用RRTconnect算法规划出一条可行路径，并对该条路径进行路径最短优化处理，其次再在此基础上对机器人在该路径的各节点进行碰撞检测，判断这条路径的有效性，避免了在规划过程中对机械臂所有关节的碰撞检测，大大降低了检测时间，提升了规划效率。

请参照图2，为本申请实施例提供的使用包围盒模型构建碰撞检测的流程图。

根据本发明实施例，使用包围盒模型构建碰撞检测对，判断当前状态机器人是否与场景中障碍物碰撞，以不发生碰撞时对应的位姿点作为初始规划路径；具体为：

S201、使用AABB包围盒模型对机器人以及障碍物的点云进行碰撞简化处理，根据碰撞检测对的检测状态结果，确定当前搜索节点是否有效，所述碰撞检测对即机器人与障碍物组成的检测对；

S202、若当前节点无效，更新上一节点为下一次搜索的初始节点，重新进行搜索，如果在当前状态及机器人与障碍物不发生碰撞，则将当前状态标记为可行状态，下一次更新的起始节点为当前状态的节点，直到满足终止条件。

需要说明的是，本发明采用AABB包围盒模型对机器人以及障碍物的点云进行碰撞简化处理，所述AABB包围盒就是采用一个长方体将物体包裹起来，进行两个物体的相交性检测时仅检测物体对应包围盒(包裹物体的长方体)的相交性。AABB包围盒对应的长方体每一个面都是与某个坐标轴平面平行的，因此，AABB包围盒又称轴对齐包围盒。AABB包围盒构造比较简单，存储空间小。

根据本发明实施例，以机器人执行任务时运动的最短路径作为优化目标，在初始规划路径基础上得到当前最优路径轨迹；具体为：

在初始规划的路径基础上，计算当前节点与有效节点之间的距离，记录运动量最小的状态节点，组成新的轨迹，即当前最优路径轨迹。

根据本发明实施例，利用当前最优路径轨迹更新点云，并对机器人的机械臂的各个状态进行碰撞检测，确定当前路径的有效性，根据机械臂是否发生碰撞，输出机器人运动路径，具体为：

计算机器人在当前最优路径轨迹中各节点的关节角，计算点云旋转和平移矩阵，更新点云，对机器人的机械臂的各个状态进行碰撞检测，确定当前路径的有效性，如果机械臂发生碰撞则返回对装配物起始位姿到结束位姿进行规划，否则停止规划，输出机器人运动路径。

第三方面，本申请还提供了一种计算机可读存储介质，所述计算机可读存储介质中包括机器人避障路径规划方法程序，所述机器人避障路径规划方法程序被处理器执行时，实现如上述任一项所述的机器人避障路径规划方法的步骤。

本发明公开的一种机器人避障路径规划方法、系统及介质通过确定场景中机器人的类型及机器人工作范围内的静态障碍物的类型，进而根据所述机器人及静态障碍物对场景进行三维重建，确定需要执行任务的机器人模型及当前状态，对机器人进行标记，然后使用八叉树构建并压缩初始状态机器人点云及障碍物点云；对装配物起始位姿到结束位姿进行规划，得到一组装配物从起始点到结束点的位姿点序列，根据所述位姿点序列得到机器人的机械臂在当前状态的关节运动量，同时更新机械臂点云；使用包围盒模型构建碰撞检测对，判断当前状态机器人的机械臂是否与场景中障碍物碰撞，以不发生碰撞时对应的位姿点作为初始规划路径；以机器人执行任务时运动的最短路径作为优化目标，在初始规划路径基础上得到当前最优路径轨迹；利用当前最优路径轨迹更新点云，并对机器人的机械臂的各个状态进行碰撞检测，确定当前路径的有效性，根据机械臂是否发生碰撞，输出机器人运动路径，本发明减少了传统机器人路径规划中示教成本，提出的碰撞检测算法避免了在规划过程中对机械臂所有关节的碰撞检测，大大降低了检测时间，提升了规划效率。

在本申请所提供的几个实施例中，应该理解到，所揭露的设备和方法，可以通过其它的方式实现。以上所描述的设备实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，如：多个单元或组件可以结合，或可以集成到另一个系统，或一些特征可以忽略，或不执行。另外，所显示或讨论的各组成部分相互之间的耦合、或直接耦合、或通信连接可以是通过一些接口，设备或单元的间接耦合或通信连接，可以是电性的、机械的或其它形式的。

上述作为分离部件说明的单元可以是、或也可以不是物理上分开的，作为单元显示的部件可以是、或也可以不是物理单元；既可以位于一个地方，也可以分布到多个网络单元上；可以根据实际的需要选择其中的部分或全部单元来实现本实施例方案的目的。

另外，在本发明各实施例中的各功能单元可以全部集成在一个处理单元中，也可以是各单元分别单独作为一个单元，也可以两个或两个以上单元集成在一个单元中；上述集成的单元既可以采用硬件的形式实现，也可以采用硬件加软件功能单元的形式实现。

本领域普通技术人员可以理解：实现上述方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成，前述的程序可以存储于可读取存储介质中，该程序在执行时，执行包括上述方法实施例的步骤；而前述的存储介质包括：移动存储设备、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

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

Claims (10)

1.一种机器人避障路径规划方法，其特征在于，所述方法包括：

确定场景中机器人的类型及机器人工作范围内的静态障碍物的类型，根据所述机器人及静态障碍物对场景进行三维重建，确定需要执行任务的机器人模型及当前状态，对机器人进行标记；

使用八叉树构建并压缩初始状态机器人点云及障碍物点云；

对装配物起始位姿到结束位姿进行规划，得到一组装配物从起始点到结束点的位姿点序列，根据所述位姿点序列得到机器人的机械臂在当前状态的关节运动量，同时更新机械臂点云；

使用包围盒模型构建碰撞检测对，判断当前状态机器人的机械臂是否与场景中障碍物碰撞，以不发生碰撞时对应的位姿点作为初始规划路径；

以机器人执行任务时运动的最短路径作为优化目标，在初始规划路径基础上得到当前最优路径轨迹；

利用当前最优路径轨迹更新点云，并对机器人的机械臂各个状态进行碰撞检测，确定当前路径的有效性，根据机械臂是否发生碰撞，输出机器人运动路径。

2.根据权利要求1所述的一种机器人避障路径规划方法，其特征在于，确定场景中机器人的类型及机器人工作范围内的静态障碍物的类型，根据所述机器人及静态障碍物对场景进行三维重建，具体为：

确定场景中机器人的类型、机器人的工作范围内的静态障碍物模型，获取机器人、夹具、装配物及障碍物的空间坐标及网格信息，根据所述空间坐标和网格信息得到位姿信息，根据位姿信息对场景进行三维重建。

3.根据权利要求1所述的一种机器人避障路径规划方法，其特征在于，确定需要执行任务的机器人模型及当前状态，对机器人进行标记还包括：对机器人工作范围内的其他模型进行识别，若所选择模型为夹具或装配物，将其标识为会跟随机器人运动的模型，否则将其标记为障碍物，确定装配物的任务起点及任务终点。

4.根据权利要求1所述的一种机器人避障路径规划方法，其特征在于，使用八叉树构建并压缩初始状态机器人点云及障碍物点云，具体为：设置八叉树构建碰撞状态时点云的精度为Xcm,即在根节点Xcm范围内的会被融合为同一个坐标点。

5.根据权利要求1所述的一种机器人避障路径规划方法，其特征在于，对装配物起始位姿到结束位姿进行规划，得到一组装配物从起始点到结束点的位姿点序列，根据所述位姿点序列得到机器人的机械臂在当前状态的关节运动量，同时更新机械臂点云；具体为：

根据机器人运动学约束条件，设置机器人的工作空间的可达范围，采用均匀分布的方式对规划空间进行采样，使用RRTConnect算法搜索机器人从任务起点到任务终点的可行状态；根据当前状态位置和姿态计算对应的旋转和平移矩阵，根据旋转和平移矩阵更新当前状态下的机器人机械臂点云位置和姿态，更新机器人机械臂的点云。

6.根据权利要求1所述的一种机器人避障路径规划方法，其特征在于，使用包围盒模型构建碰撞检测对，判断当前状态机器人是否与场景中障碍物碰撞，以不发生碰撞时对应的位姿点作为初始规划路径；具体为：

使用AABB包围盒模型对机器人以及障碍物的点云进行碰撞简化处理，根据碰撞检测对的检测状态结果，确定当前搜索节点是否有效，所述碰撞检测对即机器人与障碍物组成的检测对；

若当前节点无效，更新上一节点为下一次搜索的初始节点，重新进行搜索，如果在当前状态及机器人与障碍物不发生碰撞，则将当前状态标记为可行状态，下一次更新的起始节点为当前状态的节点，直到满足终止条件。

7.根据权利要求1所述的一种机器人避障路径规划方法，其特征在于，以机器人执行任务时运动的最短路径作为优化目标，在初始规划路径基础上得到当前最优路径轨迹，具体为：

在初始规划的路径基础上，计算当前节点与有效节点之间的距离，记录运动量最小的状态节点，组成新的轨迹，即当前最优路径轨迹。

8.根据权利要求1所述的一种机器人避障路径规划方法，其特征在于，

利用当前最优路径轨迹更新点云，并对机器人的机械臂的各个状态进行碰撞检测，确定当前路径的有效性，根据机械臂是否发生碰撞，输出机器人运动路径，具体为：

计算机器人在当前最优路径轨迹中各节点的关节角，计算点云旋转和平移矩阵，更新点云，对机器人的机械臂的各个状态进行碰撞检测，确定当前路径的有效性，如果机械臂发生碰撞则返回对装配物起始位姿到结束位姿进行规划，否则停止规划，输出机器人运动路径。

9.一种机器人避障路径规划系统，其特征在于，该系统包括：存储器及处理器，所述存储器中包括一种机器人避障路径规划方法的程序，所述一种机器人避障路径规划方法的程序被所述处理器执行时实现以下步骤：

确定场景中机器人的类型及机器人工作范围内的静态障碍物的类型，根据所述机器人及静态障碍物对场景进行三维重建，确定需要执行任务的机器人模型及当前状态，对机器人进行标记；

使用八叉树构建并压缩初始状态机器人点云及障碍物点云；

对装配物起始位姿到结束位姿进行规划，得到一组装配物从起始点到结束点的位姿点序列，根据所述位姿点序列得到机器人的机械臂在当前状态的关节运动量，同时更新机械臂点云；

使用包围盒模型构建碰撞检测对，判断当前状态机器人的机械臂是否与场景中障碍物碰撞，以不发生碰撞时对应的位姿点作为初始规划路径；

以机器人执行任务时运动的最短路径作为优化目标，在初始规划路径基础上得到当前最优路径轨迹；

利用当前最优路径轨迹更新点云，并对机器人的机械臂各个状态进行碰撞检测，确定当前路径的有效性，根据机械臂是否发生碰撞，输出机器人运动路径。

10.计算机可读存储介质，其特征在于，所述计算机可读存储介质中包括机器人避障路径规划方法程序，所述机器人避障路径规划方法程序被处理器执行时，实现如权利要求1至8中任一项所述的机器人避障路径规划方法的步骤。

Images