[发明专利]一种机械臂的路径规划方法

说明书

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

技术领域

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

背景技术

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

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

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

发明内容

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

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

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

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

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

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

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