一种基于快速行进树的机械臂动态避障控制方法技术

本发明专利技术公开了一种基于快速行进树的机械臂动态避障控制方法。机械臂在具有动态障碍物的场景环境中运动，机械臂底部固定于机座上，根据机械臂运动的起始位置和目标位置解出起始和目标关节构形，在关节构形空间中采集样本点；从目标关节构形开始采用快速随机树算法进行树的生长，获得一棵树结构及其初始避障路径；运动中对路径进行重新调整，暂停机械臂的运动，根据动态障碍物的当前位置对树结构进行调整生成新路径，控制机械臂沿着新路径重新运动；重复步骤直至机械臂到达目标关节构形。本发明专利技术方法利用了障碍物发生干涉前的路径信息，对树结构进行分离与重新连接，加快动态避障过程中重新规划路径的速度，能有效应用与机械臂的动态避障规划。

A dynamic obstacle avoidance control method of manipulator based on fast moving tree

【技术实现步骤摘要】
一种基于快速行进树的机械臂动态避障控制方法
本专利技术属于机器人运动规划领域，涉及了一种基于快速行进树的六自由度工业机械臂动态避障控制方法，适用于具有可移动障碍物的机械臂作业环境的动态避障控制。
技术介绍
工业机械臂是一种模仿人手臂动作的自动操作装置，作为工业机器人的一个分支，其具有通用性强、运动灵活、避障能力强、易于控制等优点，因而被广泛应用于搬运、焊接、喷涂、切割等领域。工业机械臂不仅将人类从危险、重复的工作中解放出来，而且推动了生产力的发展，提高了社会生产水平。通常，工业机械臂的工作场景并不是自由的空间，而是包含较多障碍物的约束空间，因此在含障碍物约束的空间进行路径规划一直是学者们研究的热点，避障规划技术作为其中的关键技术，其效果的好坏直接影响着机械臂的控制效果。随着生产需求的扩大和应用场景的复杂化，工作场景中经常会含有一些运动的障碍物，这对工业机械臂的作业规划提出了更高的需求，即要求机械臂在执行任务的同时不与环境中运动的障碍物发生碰撞，一方面传统的静态规划算法无法解决现有问题，另一方面目前对于机械臂的动态避障规划研究还比较少。相比静态环境下的避障规划，动态环境下的避障规划对算法的实时性要求更高，对于六自由度机械臂的动态避障规划，规划空间较大、大量的在线的碰撞检测等成为限制算法规划效率的瓶颈，如何根据障碍物的运动情况及时调整避障规划路径，是实现机械臂在动态障碍物环境中安全作业的关键。
技术实现思路
为了解决
技术介绍
中存在的问题，本专利技术提出一种基于快速行进树的动态避障控制方法，能够有效解决六自由度机械臂在含有动态障碍物环境中的避障规划问题，并且通过检验验证了方法的有效性，为机械臂动态避障规划提供了一种新的处理控制方式。为了实现上述目的，本专利技术采用的技术方案是包括以下步骤：1)机械臂在具有动态障碍物的场景环境中运动，机械臂底部固定于机座上，已知机械臂运动的起始位置和目标位置，根据起始位置和目标位置，求解出机械臂的起始关节构形和目标关节构形，在关节构形空间中采集不与障碍物发生干涉的构形作为样本点，关节构形空间是由机械臂的各个关节可能的构形组合而成，所有样本点组成了样本点集合V；2)根据动态障碍物的初始位置，从目标关节构形开始采用快速随机树算法进行树的生长，获得一棵树结构及其对应的初始避障路径；3)在机械臂从起始关节构形沿着初始避障路径运动过程中，当因动态障碍物的运动导致路径失效时，通过动态快速行进树算法对路径进行重新调整，暂停机械臂的运动，根据动态障碍物的当前位置对树结构进行重新调整，生成新路径，控制机械臂沿着新路径重新运动。4)重复步骤3)，直至机械臂到达目标关节构形。所述的步骤1)采用逆运动学算法求解出起始关节构形xinit和目标关节构形xgoal。所述的机械臂为用于抓取物件的六自由度工业机械臂。所述的步骤1)采用随机采样的方式采集一定数量的样本点构成样本点集V。所述步骤2)具体为：2.1)初始化树结构τ：将起始关节构形xinit和目标关节构形xgoal加入样本点集合V，建立用于存储样本点中间状态的三个集合Vunvisited、Vopen和Vclosed，分别为未访问集合Vunvisited、活跃集合Vopen和不活跃集合Vclosed，将目标关节构形xgoal加入到活跃集合Vopen中，以目标关节构形xgoal作为树结构的根样本点，初始化将所有样本点集合V中的样本点放入未访问集合Vunvisited，将目标关节构形xgoal从未访问集合Vunvisited取出放入到活跃集合Vopen中，建立用于存储结束搜索的终止样本集合Vfinal；其中Vunvisited表示样本点中还未添加到树中的样本点集合，Vopen表示目前树结构上仍活跃的样本点集合，Vclosed表示树结构上不活跃的样本点集合；2.2)选取活跃集合Vopen中到根样本点距离最小的样本点z作为第一参考样本点，距离是指样本点对应构形之间的欧拉距离，并进行以下判断：当第一参考样本点不在终止样本集合中Vfinal时，执行步骤2.3)的操作；当第一参考样本点在终止样本集合中Vfinal时，则根据树结构生成从起始关节构形xinit到目标关节构形xgoal的路径σ；2.3)获得第一参考样本点的邻居点构成第一邻居点集合X，对第一邻居点集合X与未访问集合Vunvisited求交集得到第一集合Xnear，以第一集合Xnear中的一个样本点z作为处理点x，获得处理点x的邻居点构成第二邻居点集合Y，对第二邻居点集合Y与活跃集合Vopen求交集得到第二集合Ynear；再计算第二集合Ynear中到根样本点距离最小的样本点z作为第二参考样本点ymin，判断每一个处理点x与第二参考样本点ymin之间的边e(即两个构形的变化过程)是否与障碍物发生干涉；若无干涉，则将该处理点x通过边e连接到树τ的第二参考样本点ymin上，更新计算处理点x与根样本点之间的距离c(x)，将处理点x从未访问集合Vunvisited中移除并加入到活跃集合Vopen中；若干涉，则不对处理点x进行处理；2.4)重复上述步骤2.3)遍历完第一集合Xnear中所有的样本点z后，将第一参考样本点从活跃集合Vopen中移除到不活跃集合Vclosed，再回到步骤2.2)重复实施直到步骤2.2)中找到的第一参考样本点为起始关节构形xinit，则根据最终的树结构生成从起始关节构形xinit到目标关节构形xgoal的路径σ，作为初始避障路径。所述步骤2.3)中，邻居点为以样本点z为圆心在半径r范围内包含的其他样本点，采用以下公式计算半径r：式中，d是机械臂关节的总数，n是样本点集合V中的样本点总数，η是自适应调整参数，η＞1；r表示半径。所述步骤3)具体为：3.1)在路径σ上，开始取起始关节构形xinit作为当前目标构形xcurrent，取当前目标构形xcurrent及其下一个构形之间的边ecurrent离散化处理，获得当前目标构形xcurrent和下一个构形之间的k个关节构形传给机械臂控制单元驱动机械臂运动；3.2)进行以下判断：如果到达目标关节构形xgoal，即下一个构形为目标关节构形xgoal的样本点，则规划成功，结束算法；如果未到达目标关节构形xgoal，则重复进行3.1)；3.3)在步骤3.2)的机械臂运动过程中，实时检测判断路径σ和动态障碍物是否发生干涉；若因动态障碍物运动造成路径σ中的某段发生干涉，则将暂停机械臂的运动，采用分离树方法将树结构τ分解为以目标关节构形xgoal为根构形的子树τ1和以失效目标构形xseperate为根构形的子树τ2的两棵子树，以路径σ中当前目标构形xcurrent到失效目标构形xseperate之间的路径作为终止样本集合Vfinal，其中失效目标构形xseperate表示干涉段中距离当前目标构形xcurrent最近的路径σ中的构形；3.4)采用重生长树方法，以终止样本集合Vfinal为搜索结束区域，采用步骤2)的方式进行树的生长，生成新的从当前目标构形xcurrent到失效目标构形xseperate的路径σ，重启机械臂的运动，并返回步骤3.1)。所述步骤3.3)具体为：3.3.1)从目标关节构形xgoal开始遍历路径σ上的每个路径构形xi，对路径构形xi做干涉检查，若本文档来自技高网...

【技术保护点】
1.一种基于快速行进树的机械臂动态避障控制方法，其特征在于包含以下步骤：1)机械臂在具有动态障碍物的场景环境中运动，机械臂底部固定于机座上，已知机械臂运动的起始位置和目标位置，根据起始位置和目标位置，求解出机械臂的起始关节构形和目标关节构形，在关节构形空间中采集不与障碍物发生干涉的构形作为样本点，关节构形空间是由机械臂的各个关节可能的构形组合而成，所有样本点组成了样本点集合V；2)根据动态障碍物的初始位置，从目标关节构形开始采用快速随机树算法进行树的生长，获得一棵树结构及其对应的初始避障路径；3)在机械臂从起始关节构形沿着初始避障路径运动过程中，当因动态障碍物的运动导致路径失效时，通过动态快速行进树算法对路径进行重新调整，暂停机械臂的运动，根据动态障碍物的当前位置对树结构进行调整，生成新路径，控制机械臂沿着新路径重新运动。4)重复步骤3)，直至机械臂到达目标关节构形。

【技术特征摘要】
1.一种基于快速行进树的机械臂动态避障控制方法，其特征在于包含以下步骤：1)机械臂在具有动态障碍物的场景环境中运动，机械臂底部固定于机座上，已知机械臂运动的起始位置和目标位置，根据起始位置和目标位置，求解出机械臂的起始关节构形和目标关节构形，在关节构形空间中采集不与障碍物发生干涉的构形作为样本点，关节构形空间是由机械臂的各个关节可能的构形组合而成，所有样本点组成了样本点集合V；2)根据动态障碍物的初始位置，从目标关节构形开始采用快速随机树算法进行树的生长，获得一棵树结构及其对应的初始避障路径；3)在机械臂从起始关节构形沿着初始避障路径运动过程中，当因动态障碍物的运动导致路径失效时，通过动态快速行进树算法对路径进行重新调整，暂停机械臂的运动，根据动态障碍物的当前位置对树结构进行调整，生成新路径，控制机械臂沿着新路径重新运动。4)重复步骤3)，直至机械臂到达目标关节构形。2.根据权利要求1所述的一种基于快速行进树的机械臂动态避障控制方法，其特征在于：所述的步骤1)采用逆运动学算法求解出起始关节构形xinit和目标关节构形xgoal。3.根据权利要求1所述的一种基于快速行进树的机械臂动态避障控制方法，其特征在于：所述的机械臂为用于抓取物件的六自由度工业机械臂。4.根据权利要求1所述的一种基于快速行进树的机械臂动态避障控制方法，其特征在于：所述的步骤1)采用随机采样的方式采集一定数量的样本点构成样本点集V。5.根据权利要求1所述的一种基于快速行进树的机械臂动态避障控制方法，其特征在于：所述步骤2)具体为：2.1)初始化树结构τ：将起始关节构形xinit和目标关节构形xgoal加入样本点集合V，建立用于存储样本点中间状态的三个集合Vunvisited、Vopen和Vclosed，分别为未访问集合Vunvisited、活跃集合Vopen和不活跃集合Vclosed，将目标关节构形xgoal加入到活跃集合Vopen中，以目标关节构形xgoal作为树结构的根样本点，初始化将所有样本点集合V中的样本点放入未访问集合Vunvisited，将目标关节构形xgoal从未访问集合Vunvisited取出放入到活跃集合Vopen中，建立用于存储结束搜索的终止样本集合Vfinal；2.2)选取活跃集合Vopen中到根样本点距离最小的样本点z作为第一参考样本点，距离是指样本点对应构形之间的欧拉距离，并进行以下判断：当第一参考样本点不在终止样本集合中Vfinal时，执行步骤2.3)的操作；当第一参考样本点在终止样本集合中Vfinal时，则根据树结构生成从起始关节构形xinit到目标关节构形xgoal的路径σ；2.3)获得第一参考样本点的邻居点构成第一邻居点集合X，对第一邻居点集合X与未访问集合Vunvisited求交集得到第一集合Xnear，以第一集合Xnear中的一个样本点z作为处理点x，获得处理点x的邻居点构成第二邻居点集合Y，对第二邻居点集合Y与活跃集合Vopen求交集得到第二集合Ynear；再计算第二集合Ynear中到根样本点距离最小的样本点z作为第二参考样本点ymin，判断每一个处理点x与第二参考样本点...

【专利技术属性】
技术研发人员：段桂芳，吴向东，刘振宇，谭建荣，
申请(专利权)人：浙江大学，
类型：发明
国别省市：浙江,33