CN115179287A - 一种机械臂的路径规划方法 - Google Patents

Abstract

本发明公开了一种机械臂的路径规划方法，用于解决机械臂路径规划耗时长的问题。包括：对于多个目标堆放在一起的场景，先利用快速搜索随机树算法，建立不少于一条从目标到机械臂起始位置的抓取路径，并将抓取路径作为后续路径规划的参考地图；检测参考地图中各抓取路径的畅通性，对于被阻塞的抓取路径，只保留从机械臂起点到第一个阻塞位置的路径段；新规划路径通过两种方式实现路径规划选择，具有弯路少的特点，使得新规划路径呈现缩短的趋势；有效地提高路径规划的计算效率和成功率。

Description

一种机械臂的路径规划方法

技术领域

本发明涉及机械臂的路径规划技术领域，尤其涉及一种基于自建地图的机械臂的路径规划方法。

背景技术

在已获取场景3D点云信息的情况下，为了实现机械臂对场景中目标物体的自动抓取，除了要使机械臂具备识别目标位姿的功能外，还需具备自主场景避障的功能，这样才能确保整个抓取过程能够顺利完成。这涉及到机器视觉、机械臂运动学等方面的技术。机械臂自主避障功能要实现的是：在已识别出抓取目标位姿的情况下，利用机械臂运动学原理，制定出机械臂到达目标位姿所需的往返轨迹，且必须保证机械臂沿该轨迹往返过程中，不会与场景中的任何障碍物发生碰撞，完全由计算机随机搜索出无碰撞的抓取路线非常耗时。

路径规划领域目前被广泛使用且行之有效的是快速搜索随机树算法 (RRT)，该技术在每轮迭代中，随机在机械臂运动空间中定位一个状态点，根据机械臂运动学，该状态由机械臂各关节的旋转自由度组成，对于常用的6 自由度旋转角的机械臂，其状态就是6维向量，以机械臂当前状态到新定位状态的向量，作为机械臂状态的变化方向，并人为给定一个小的变化幅度，作为本轮中机械臂运动的步长。如果机械臂沿新方向运动指定的步长后，不会与场景中的障碍物放生碰撞，那么新状态就可以作为组成抓取路径的一个可行路径点，否则重新随机搜索新的状态点。如此循环，直至机械臂到达目标位置状态为止。全体搜索出的可行路径点就组成了一条由机械臂起始位置通向抓取目标的路径，且该路径不会与场景中的障碍发生碰撞。分析RRT技术的原理，可以发现该技术是在整个状态空间中随机地搜索状态点来进行该点是否为可行路径点的检测，因此该技术在规划路径时，会频繁地搜索到非可行路径点，从而极大拖慢了路径规划的时间。

为此，需要开发改进机械臂的路径规划方法，缩减确定无碰撞抓取路径的规划时间。

发明内容

本发明实施例提供了一种机械臂的路径规划方法，用于解决路径规划耗时长的问题。

本发明实施例提供了一种机械臂的路径规划方法，其特征在于，包括：

步骤A：对于多个目标堆放在一起的场景，先利用快速搜索随机树算法，建立不少于一条从目标到机械臂起始位置的抓取路径，并将抓取路径作为后续路径规划的参考地图；

步骤B：检测参考地图中各抓取路径的畅通性，对于被阻塞的抓取路径，只保留从机械臂起始位置到第一个阻塞位置的路径段；

步骤C：对于场景中的待抓取目标，从参考地图的各抓取路径中选取一路径点作为中转站，中转站的选取条件满足待抓取目标到中转站的距离与中转站到机械臂起始位置的距离之和最小；

步骤D：规划从待抓取目标到中转站的路径，设定一个概率阈值，随机获得一个概率数，当概率数小于等于概率阈值时，执行快速搜索随机树算法来获取从待抓取目标到中转站的机械臂运动向量状态；当概率数大于概率阈值时，由机械臂的当前状态向中转站前进一个步长，获得机械臂运动向量状态；若机械臂运动向量状态没有与场景发生碰撞，则机械臂运动向量状态是待抓取目标到中转站之间抓取路径的可行路径点；

步骤E：若可行路径点与中转站之间的距离小于等于给定阈值，抓取路径的规划结束，由待抓取目标位置到中转站的每一个机械臂运动向量状态形成的全体离散化状态点集，以及由中转站到机械臂起始位置的全体离散化状态点集，共同形成新规划的抓取路径，并存储在参考地图中；否则，跳转到步骤B；

步骤F：新规划的抓取路径，如果它与参考地图中原有的一条抓取路径存在部分重合，则比较这两条路径的长度，并在参考地图中用路径较短者替换，实现参考地图更新。

一种较佳的实施方式，概率阈值是根据实际工程对路径规划计算时间和成功率这两个因素的倾向而决定的，概率阈值越大，成功率越高；概率阈值越小，路径规划计算时间越快；概率阈值较佳的为0.8。

一种较佳的实施方式，步骤E中给定的阈值为机械臂运动一步的步长。

一种较佳的实施方式，步骤E中的离散化状态点是一个描述机械臂位姿的维度向量信息，每个维度向量信息对应着机械臂一个关节的旋转角。

一种较佳的实施方式，步长是由两个相邻的机械臂运动向量状态中的坐标向量偏差的∞范数来计算，即求取向量元素绝对值的最大值；较佳的步长为 1度。

本发明实施例的有益效果包括：基于自建地图的机械臂路径规划技术，只有前几次在建立地图时所使用的RRT路径搜索技术耗时较大，后续的路径规划以参考地图的抓取路径为参照，可以快速地规划出对新目标的抓取路径。同时新规划路径具有弯路少的特点，地图中路径长度也会随着新路径的规划而被尽可能压缩，使得新规划路径呈现缩短的趋势，这有利于机械臂快速抓取目标。另外，由于机械臂起始位置周边比较空旷，几乎不需考虑避障，而目标堆放在一起时，周边障碍物更稠密，避障情况变得很复杂。所以从场景目标向出发点进行路径规划，使机械臂尽早避开复杂区域，这样不易出现已规划好的路径因碰撞到场景而被舍弃的情况，从而有效地提高路径规划的计算效率和成功率。

附图说明

图1为本发明实施例的机械臂的路径规划方法；

具体实施方式

下面结合附图1，对本发明实施例提供的一种机械臂的路径规划方法的具体实施方式进行详细地说明。

本发明实施例提供的一种机械臂的路径规划方法，其特征在于，包括：

步骤A：对于多个目标堆放在一起的场景，先利用快速搜索随机树算法，建立不少于一条从目标到机械臂起始位置的抓取路径，并将抓取路径作为后续路径规划的参考地图；

步骤B：检测参考地图中各抓取路径的畅通性，对于被阻塞的抓取路径，只保留从机械臂起始位置到第一个阻塞位置的路径段；

步骤C：对于场景中的待抓取目标，从参考地图的各抓取路径中选取一路径点作为中转站，中转站的选取条件满足待抓取目标到中转站的距离与中转站到机械臂起始位置的距离之和最小；

步骤D：规划从待抓取目标到中转站的路径，设定一个概率阈值，随机获得一个概率数，当概率数小于等于概率阈值时，执行快速搜索随机树算法来获取从待抓取目标到中转站的机械臂运动向量状态；当概率数大于概率阈值时，由机械臂的当前状态向中转站前进一个步长，获得机械臂运动向量状态；若机械臂运动向量状态没有与场景发生碰撞，则机械臂运动向量状态是待抓取目标到中转站之间抓取路径的可行路径点；

步骤E：若可行路径点与中转站之间的距离小于等于给定阈值，抓取路径的规划结束，由待抓取目标位置到中转站的每一个机械臂运动向量状态形成的全体离散化状态点集，以及由中转站到机械臂起始位置的全体离散化状态点集，共同形成新规划的抓取路径，并存储在参考地图中；否则，跳转到步骤B；

步骤F：新规划的抓取路径，如果它与参考地图中原有的一条抓取路径存在部分重合，则比较这两条路径的长度，并在参考地图中用路径较短者替换，实现参考地图更新。

概率阈值是根据实际工程对路径规划计算时间和成功率这两个因素的倾向而决定的，概率阈值越大，成功率越高；概率阈值越小，路径规划计算时间越快。是按照RRT技术从当前位置沿随机朝向运动一个步长，还是由当前位置向中转站直线运动一个步长，这两种方案二选一时，按照一个人为指定的概率p来选择的，方案一的概率为p，则方案二的概率为1-p。方案一是为了尽量避开路径障碍的，方案二是为了快速形成路线的。p的设定是根据实际工程对路径规划计算时间和成功率这两个因素的倾向而决定的，如果希望成功率高，p就选得大一点，如果希望快速输出规划结果，p就选的小一点。在复杂场景下，如：目标大量堆叠，机械臂运行过程中会有料箱，支架等障碍物干扰的场景下，p选大点，可以保证足够的规划成功率，但规划速度会较慢。反之，如果场景较简单，p可以选的小一点，这样规划速度会很快，而且由于环境简单，路径阻断的可能性较低，导致每次规划的成功率也很高。目前实验中概率阈值取值为0.8。

步骤E中给定的阈值为机械臂运动一步的步长。阈值设定为机械臂的一个移动步长，即当机械臂靠近某个中转站的距离，在机械臂每次移动步长的范围内时，可认为机械臂到达了中转站，随后的运动就可按地图上中转站到起点的已有路线运动。对于6自由度的机械臂，机械臂的运动位置是由各关节旋转角决定的，所以在6维状态空间中，步长距离指的是两个6维坐标向量偏差的∞范数，即向量元素绝对值的最大者。步骤E中的离散化状态点是一个描述机械臂位姿的维度向量信息，每个维度向量信息对应着机械臂一个关节的旋转角。对于6自由度机械臂，点集的每个点都是一个描述机械臂位姿的6维向量点，向量的每个元素对应着机械臂一个关节的旋转角。

步长是由两个相邻的机械臂运动向量状态中的坐标向量偏差的∞范数来计算，即求取向量元素绝对值的最大值；较佳的步长为1度。

本发明实施例基本原理是：机械臂从起点到场景中目标的路径规划，等价于反向的从场景中目标到起点的路径规划。对于多个目标堆放在一起的场景，将已获得的机械臂从场景中某些目标到起点去的若干路径作为抓取路径，之后在规划从新的目标到起点的路径时，只需要从参考地图中已有抓取路线中找出一路径点作为中转站，并规划出从目标到中转站的这一小段支线路径即可，这样极大地减少了机械臂状态空间的搜索区域，而整个无碰撞抓取路径，就是目标到中转站，和中转站到起点这两段路径的拼接之和。同时，为了使规划路径少走弯路，中转站的选取，满足目标到中转站的距离与中转站到起点间的距离之和最小，这样新生成的抓取路径就被尽量缩短了。比较新抓取路径和参考地图中与之有部分重合的路径的长度，若新抓取路径更短，则用新抓取路径替换从而使得参考地图中的路径被不断优化缩短。

本发明实施例的机械臂的路径规划方法，新规划路径具有弯路少的特点，且参考地图中抓取路径长度也会随着新路径的规划而被尽可能压缩，使得新规划路径呈现缩短的趋势，有利于机械臂快速抓取目标；另外，从场景目标向出发点进行路径规划，使机械臂尽早避开复杂区域，这样不易出现已规划好的路径因碰撞到场景而被舍弃的情况，从而有效地提高路径规划的计算效率和成功率。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (7)

1.一种机械臂的路径规划方法，其特征在于，包括：

步骤A：对于多个目标堆放在一起的场景，先利用快速搜索随机树算法，建立不少于一条从目标到机械臂起始位置的抓取路径，并将所述抓取路径作为后续路径规划的参考地图；

步骤B：检测所述参考地图中各所述抓取路径的畅通性，对于被阻塞的所述抓取路径，只保留从所述机械臂起始位置到第一个阻塞位置的路径段；

步骤C：对于场景中的待抓取目标，从所述参考地图的各所述抓取路径中选取一路径点作为中转站，所述中转站的选取条件满足所述待抓取目标到所述中转站的距离与所述中转站到所述机械臂起始位置的距离之和最小；

步骤D：规划从所述待抓取目标到所述中转站的路径，设定一个概率阈值，随机获得一个概率数，当所述概率数小于等于所述概率阈值时，执行快速搜索随机树算法来获取从所述待抓取目标到所述中转站的机械臂运动向量状态；当所述概率数大于所述概率阈值时，由机械臂的当前状态向所述中转站前进一个步长，获得机械臂运动向量状态；若所述机械臂运动向量状态没有与场景发生碰撞，则所述机械臂运动向量状态是所述待抓取目标到所述中转站之间抓取路径的可行路径点；

步骤E：若可行路径点与中转站之间的距离小于等于给定阈值，所述抓取路径的规划结束，由待抓取目标位置到所述中转站的每一个所述机械臂运动向量状态形成的全体离散化状态点集，以及由所述中转站到所述机械臂起始位置的全体离散化状态点集，共同形成新规划的抓取路径，并存储在所述参考地图中；否则，跳转到步骤B；

步骤F：新规划的抓取路径，如果它与所述参考地图中原有的一条所述抓取路径存在部分重合，则比较这两条路径的长度，并在参考地图中用路径较短者替换，实现所述参考地图更新。

2.根据权利要求1所述的方法，其特征在于，所述概率阈值是根据实际工程对路径规划计算时间和成功率这两个因素的倾向而决定的，所述概率阈值越大，成功率越高；所述概率阈值越小，路径规划计算时间越快。

3.根据权利要求1或2所述的方法，其特征在于，所述概率阈值为0.8。

4.根据权利要求1所述的方法，其特征在于，步骤E中给定的所述阈值为机械臂运动一步的步长。

5.根据权利要求1所述的方法，其特征在于，步骤E中的所述离散化状态点是一个描述机械臂位姿的维度向量信息，每个所述维度向量信息对应着机械臂一个关节的旋转角。

6.根据权利要求1或4所述的方法，其特征在于，所述步长是由两个相邻的所述机械臂运动向量状态中的坐标向量偏差的∞范数来计算，即求取向量元素绝对值的最大值。

7.根据权利要求1或4所述的方法，其特征在于，所述步长为1度。

Images