一种基于改进快速扩展随机树的机械臂路径规划方法

本发明属于工业机器人控制，具体涉及一种基于改进快速扩展随机树的机械臂路径规划方法。

背景技术：

1、路径规划是工业机器人实现智能化的过程之一，良好的路径规划算法能够节约机器人的运动时间，降低关节损耗。区别于移动机器人，工业机器人在空间中的路径规划问题涉及到多次机器人运动学求解，计算量大，效率低下。目前针对机器人的路径规划问题，常用的方法有：a*算法、人工势场法、仿生算法等。但是在机械臂的路径规划中，大部分算法并不适用，主要体现在空间维度增加，导致算法计算量激增或陷入局部最小值无法逃离等问题。快速扩展随机树算法改善了这一问题，其作为一种基于采样的规划方法，有效的避免了对空间环境的建模且具备概率完备性，因此引得了广泛的使用。但是该算法也存在一定缺陷，如盲目生成随机点导致算法收敛时间过长，生成路径冗余，不平滑等。因此针对rrt算法在机械臂路径规划中的缺点进行改进是有必要的。

技术实现思路

1、为解决机械臂路径规划速度过慢、路径冗余等问题，本发明公开了一种基于改进快速扩展随机树的机械臂路径规划方法，该方法根据设定的安全距离，快速获得一条从起点运动到终点的无碰撞六轴关节值序列集合。在初始路径搜索中，使用空间预置树策略，保证节点与起点的联系，同时令机械臂的不同位姿尽可能遍布空间，为后续路径搜索奠定基础，在路径搜索算法中，利用了rrt-connect算法，以及目标偏置、贪婪策略、变步长策略等加快算法收敛；路径优化方面利用循环剪枝算法大幅度减少了冗余路径，在迭代过程中保证了路径的局部最优性；基于改进快速扩展随机树的机械臂路径规划方法提高了机械臂在三维空间中的路径规划效率,解决了多维空间中，机械臂运动学求解时间过长，路径规划后生成路径冗余的问题，成功实现了关节空间中的无碰撞运动任务。

2、为达到上述目的，本发明的技术方案如下：

3、一种基于改进快速扩展随机树的机械臂路径规划方法，包括以下步骤：

4、步骤一、读取空间障碍物位置信息，对障碍物进行简化球模型包络，设定机械臂终点位姿信息，建立机械臂d-h参数模型，同时根据给定机械臂末端执行器的终点位姿进行机器人运动学逆解，判断该状态下的关节值是否在机械臂可运动范围内，设置与障碍物的安全距离r；

5、步骤二、在关节空间内设置机械臂预置树，将机械臂可能存在的位姿，在保持与初末位姿具备一定联系的前提下，尽可能地遍布整个空间，接着在关节空间下建立机械臂双向快速随机搜索树，在起点位姿与终点位姿同时生成随机树，并利用贪婪策略保证第二棵树新生成的节点始终向第一棵树中距离最近的节点进行扩展；在双向树随机生成节点的过程中，使用目标偏置策略，根据双树交换次数t以及目标偏置系数p与随机生成数q的关系，决定第一棵随机扩展树的终点指向以及第二棵随机扩展树的终点指向；在双向树的探索过程中，采用变步长策略，大步长探索，小步长搜寻，快速获得一条由起点指向终点的无碰撞路径；

6、步骤三、利用循环剪枝算法，不断将搜寻到的节点进行优化，从起点位姿与终点位姿交替进行剪枝操作，获得一段局部最优的无碰撞路径。

7、本发明的有益效果为：

8、本申请使用的一种基于改进快速扩展随机树的机械臂路径规划方法，在机械臂规划路径时提出了预置树方法，并且在rrt-connect算法的基础上结合目标偏置、贪婪策略、变步长策略，同时利用循环剪枝方案，实现六自由度机械臂避障，提高了算法的规划速度与效率。

技术特征：

1.一种基于改进快速扩展随机树的机械臂路径规划方法，其特征在于，包括以下步骤：

2.根据权利要求1所述的基于改进快速扩展随机树的机械臂路径规划方法，其特征在于，所述步骤一具体包括以下步骤：

3.根据权利要求2所述的基于改进快速扩展随机树的机械臂路径规划方法，其特征在于，所述步骤二中在关节空间内设置机械臂预置树，将机械臂可能存在的位姿，在保持与初末位姿具备一定联系的前提下，尽可能地遍布整个空间，接着在关节空间下建立机械臂双向快速随机搜索树，在起点位姿与终点位姿同时生成随机树，并利用贪婪策略保证第二棵树新生成的节点始终向第一棵树中距离最近的节点进行扩展，具体的：记起点六轴关节值θs并存入rrtree1，记终点六轴关节值θg并存入rrtree2，预置树指向位k＝1，表示此时指向rrtree1，当k＝2时，表示当前状态指向rrtree2，步进长度step1＝0.5，为六轴关节值移动的总欧拉距离，计算出起点六轴关节值θs和终点六轴关节值θg之间的方向向量θm，根据sign函数获取单位向量θm符号的正负性，并在该方向向量的各个元素后随机加上一个与θm同向的波动值randi作为新的方向向量，其中randi是一个一行六列的数组，且各元素为0到1内的随机值，其组成的新方向向量如式(4)所示：

4.根据权利要求3所述的基于改进快速扩展随机树的机械臂路径规划方法，其特征在于，所述步骤二中在双向树随机生成节点的过程中，使用目标偏置策略，根据双树交换次数t以及目标偏置系数p与随机生成数q的关系，决定第一棵随机扩展树的终点指向以及第二棵随机扩展树的终点指向具体为：设目标偏置参数为p，取p＝0.7～0.8，随机生成一个[0,1]内的数q，若q＜p，则在关节软限位内随机生成采样节点，反之生成固定目标，作为采样节点；固定目标的生成取决于双树失败交换次数t，记初始双树交换次数t为1，每交换一次t加一，当t为奇数时，固定目标指向终点θg，当t为偶数时，固定目标指向起点θs；

5.根据权利要求4所述的基于改进快速扩展随机树的机械臂路径规划方法，其特征在于，所述步骤二中在双向树的探索过程中，采用变步长策略，大步长探索，小步长搜寻，快速获得一条由起点指向终点的无碰撞路径，包括以下步骤：

6.根据权利要求5所述的基于改进快速扩展随机树的机械臂路径规划方法，其特征在于，所述步骤三包括以下步骤：

技术总结
本发明公开了一种基于改进快速扩展随机树的机械臂路径规划方法，属于工业机器人控制技术领域。根据设定的安全距离，快速获得一条从起点运动到终点的无碰撞六轴关节值序列集合。在初始路径搜索中，使用空间预置树策略，保证探索机械臂位姿与初末机械臂位姿的联系，同时令不同位姿尽可能遍布空间；在路径搜索算法中，釆用双向搜索树算法，以及目标偏置、贪婪策略、变步长策略等加快算法收敛；路径优化釆用循环剪枝算法大幅度减少了冗余路径，在迭代过程中保证了路径的局部最优性。本方法提高了机械臂在三维空间中的路径规划效率，解决了多维空间中，机械臂运动学求解时间过长、路径冗余的问题，实现了关节空间中的无碰撞运动任务。

技术研发人员：潘丰,黄杰
受保护的技术使用者：江南大学
技术研发日：
技术公布日：2024/1/15