CN110370274A - 一种基于快速行进树的机械臂动态避障控制方法 - Google Patents

Abstract

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

Description

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

技术领域

本发明属于机器人运动规划领域，涉及了一种基于快速行进树的六自由度工业机械臂动态避障控制方法，适用于具有可移动障碍物的机械臂作业环境的动态避障控制。

背景技术

工业机械臂是一种模仿人手臂动作的自动操作装置，作为工业机器人的一个分支，其具有通用性强、运动灵活、避障能力强、易于控制等优点，因而被广泛应用于搬运、焊接、喷涂、切割等领域。工业机械臂不仅将人类从危险、重复的工作中解放出来，而且推动了生产力的发展，提高了社会生产水平。

通常，工业机械臂的工作场景并不是自由的空间，而是包含较多障碍物的约束空间，因此在含障碍物约束的空间进行路径规划一直是学者们研究的热点，避障规划技术作为其中的关键技术，其效果的好坏直接影响着机械臂的控制效果。随着生产需求的扩大和应用场景的复杂化，工作场景中经常会含有一些运动的障碍物，这对工业机械臂的作业规划提出了更高的需求，即要求机械臂在执行任务的同时不与环境中运动的障碍物发生碰撞，一方面传统的静态规划算法无法解决现有问题，另一方面目前对于机械臂的动态避障规划研究还比较少。相比静态环境下的避障规划，动态环境下的避障规划对算法的实时性要求更高，对于六自由度机械臂的动态避障规划，规划空间较大、大量的在线的碰撞检测等成为限制算法规划效率的瓶颈，如何根据障碍物的运动情况及时调整避障规划路径，是实现机械臂在动态障碍物环境中安全作业的关键。

发明内容

为了解决背景技术中存在的问题，本发明提出一种基于快速行进树的动态避障控制方法，能够有效解决六自由度机械臂在含有动态障碍物环境中的避障规划问题，并且通过检验验证了方法的有效性，为机械臂动态避障规划提供了一种新的处理控制方式。

为了实现上述目的，本发明采用的技术方案是包括以下步骤：

1)机械臂在具有动态障碍物的场景环境中运动，机械臂底部固定于机座上，已知机械臂运动的起始位置和目标位置，根据起始位置和目标位置，求解出机械臂的起始关节构形和目标关节构形，在关节构形空间中采集不与障碍物发生干涉的构形作为样本点，关节构形空间是由机械臂的各个关节可能的构形组合而成，所有样本点组成了样本点集合V；

2)根据动态障碍物的初始位置，从目标关节构形开始采用快速随机树算法进行树的生长，获得一棵树结构及其对应的初始避障路径；

3)在机械臂从起始关节构形沿着初始避障路径运动过程中，当因动态障碍物的运动导致路径失效时，通过动态快速行进树算法对路径进行重新调整，暂停机械臂的运动，根据动态障碍物的当前位置对树结构进行重新调整，生成新路径，控制机械臂沿着新路径重新运动。

4)重复步骤3)，直至机械臂到达目标关节构形。

所述的步骤1)采用逆运动学算法求解出起始关节构形x_init和目标关节构形x_goal。

所述的机械臂为用于抓取物件的六自由度工业机械臂。

所述的步骤1)采用随机采样的方式采集一定数量的样本点构成样本点集V。

所述步骤2)具体为：

2.1)初始化树结构τ：将起始关节构形x_init和目标关节构形x_goal加入样本点集合V，建立用于存储样本点中间状态的三个集合V_unvisited、V_open和V_closed，分别为未访问集合V_unvisited、活跃集合V_open和不活跃集合V_closed，将目标关节构形x_goal加入到活跃集合V_open中，以目标关节构形x_goal作为树结构的根样本点，初始化将所有样本点集合V中的样本点放入未访问集合V_unvisited，将目标关节构形x_goal从未访问集合V_unvisited取出放入到活跃集合V_open中，建立用于存储结束搜索的终止样本集合V_final；其中V_unvisited表示样本点中还未添加到树中的样本点集合，V_open表示目前树结构上仍活跃的样本点集合，V_closed表示树结构上不活跃的样本点集合；

2.2)选取活跃集合V_open中到根样本点距离最小的样本点z作为第一参考样本点，距离是指样本点对应构形之间的欧拉距离，并进行以下判断：

当第一参考样本点不在终止样本集合中V_final时，执行步骤2.3)的操作；

当第一参考样本点在终止样本集合中V_final时，则根据树结构生成从起始关节构形x_init到目标关节构形x_goal的路径σ；

2.3)获得第一参考样本点的邻居点构成第一邻居点集合X，对第一邻居点集合X与未访问集合V_unvisited求交集得到第一集合X_near，以第一集合X_near中的一个样本点z作为处理点x，获得处理点x的邻居点构成第二邻居点集合Y，对第二邻居点集合Y与活跃集合V_open求交集得到第二集合Y_near；再计算第二集合Y_near中到根样本点距离最小的样本点z作为第二参考样本点y_min，判断每一个处理点x与第二参考样本点y_min之间的边e(即两个构形的变化过程)是否与障碍物发生干涉；

若无干涉，则将该处理点x通过边e连接到树τ的第二参考样本点y_min上，更新计算处理点x与根样本点之间的距离c(x)，将处理点x从未访问集合V_unvisited中移除并加入到活跃集合V_open中；

若干涉，则不对处理点x进行处理；

2.4)重复上述步骤2.3)遍历完第一集合X_near中所有的样本点z后，将第一参考样本点从活跃集合V_open中移除到不活跃集合V_closed，再回到步骤2.2)重复实施直到步骤2.2)中找到的第一参考样本点为起始关节构形x_init，则根据最终的树结构生成从起始关节构形x_init到目标关节构形x_goal的路径σ，作为初始避障路径。

所述步骤2.3)中，邻居点为以样本点z为圆心在半径r范围内包含的其他样本点，采用以下公式计算半径r：

式中，d是机械臂关节的总数，n是样本点集合V中的样本点总数，η是自适应调整参数，η＞1；r表示半径。

所述步骤3)具体为：

3.1)在路径σ上，开始取起始关节构形x_init作为当前目标构形x_current，取当前目标构形x_current及其下一个构形之间的边e_current离散化处理，获得当前目标构形x_current和下一个构形之间的k个关节构形传给机械臂控制单元驱动机械臂运动；

3.2)进行以下判断：

如果到达目标关节构形x_goal，即下一个构形为目标关节构形x_goal的样本点，则规划成功，结束算法；

如果未到达目标关节构形x_goal，则重复进行3.1)；

3.3)在步骤3.2)的机械臂运动过程中，实时检测判断路径σ和动态障碍物是否发生干涉；若因动态障碍物运动造成路径σ中的某段发生干涉，则将暂停机械臂的运动，采用分离树方法将树结构τ分解为以目标关节构形x_goal为根构形的子树τ1和以失效目标构形x_seperate为根构形的子树τ2的两棵子树，以路径σ中当前目标构形x_current到失效目标构形x_seperate之间的路径作为终止样本集合V_final，其中失效目标构形x_seperate表示干涉段中距离当前目标构形x_current最近的路径σ中的构形；

3.4)采用重生长树方法，以终止样本集合V_final为搜索结束区域，采用步骤2)的方式进行树的生长，生成新的从当前目标构形x_current到失效目标构形x_seperate的路径σ，重启机械臂的运动，并返回步骤3.1)。

所述步骤3.3)具体为：

3.3.1)从目标关节构形x_goal开始遍历路径σ上的每个路径构形x_i，对路径构形x_i做干涉检查，若发生干涉，则将路径构形x_i及所有的后代构形加入到未访问集合V_unvisited，并将路径构形和其后代构形从活跃集合V_open或不活跃集合V_closed中移除，活跃集合V_open和不活跃集合V_closed集合中剩余的构形组成以x_goal为根的子树τ1；

3.3.2)从当前关节构形x_current开始遍历路径σ上的每个路径构形x_i，对路径构形x_i做干涉检查，若不发生干涉，则将路径构形x_i加入到目标区域V_goal中，若发生干涉，则遍历结束，跳转到步骤3.4)。

本发明方法的控制在关节构形空间中进行，分为离线规划和在线规划两个阶段，在离线阶段，首先利用逆运动学算法求解出初始和目标关节构形，并在障碍物位于初始状态且暂不运动的情况下，利用逆向快速行进树方法规划出一条初始的避障规划路径，在线阶段，机械臂沿着初始的规划路径进行运动，随着障碍物的运动，关节构形空间下的地图环境不断更新，原有路径可能与障碍物发生干涉，此时，利用动态快速行进树方法根据障碍物的影响将树结构分成两棵子树，并采取两种方式进行树的重连接，之后更新的路径，机械臂沿着新路径运动。

本发明的有益效果如下：

本发明方法，是在快速行进树的基础上，针对动态障碍物的影响进行了适应性的修改，针对快速行进树算法中节点生长信息与根节点密切相关的特点，提出逆向快速行进树方法，以目标构形为根节点开始树的生长来配合动态环境中避障规划的实现。

本发明方法在线规划过程中，，利用了障碍物发生干涉前的路径信息，对树结构进行分离与重新连接，加快动态避障过程中重新规划路径的速度，能有效应用与机械臂的动态避障规划。

附图说明

图1为本发明方法的流程示意图。

图2为实施例的作业场景图。

图3为实施例中的机械臂离线规划的路径关节角变化图。

图4为实施例中的机械臂在线规划的路径关节角变化图。

图5为实施例中的机械臂避障作业过程图。

具体实施方式

为了更好的理解本发明，下面结合附图和实施例对本发明的技术方案作详细的描述。

如图1所示，本发明内容处方法完整实施的具体实施例及其实施过程如下：

图2所示为具有运动障碍物的机械臂作业场景图。A是机械臂末端执行器初始位置，B是机械臂末端执行器目标位置，C是障碍物初始位置。

下面具体详细叙述方法实施：

1.分别对起始位置A和目标位置B采用逆运动学算法进行求解，求解出A和B的关节构形x_init和x_goal，在关节构形空间中采集N个不与障碍物发生干涉的样本点，构成样本点集V。

本实例中，位置A的坐标为[0.4,-0.4,0.2]，(单位为m，下同)位置B的坐标为[0.4,0.5,0.15]，经过逆运动学算法转换后的A和B的关节构形x_init和x_goal分别为：

x_init＝[-44.1341°，66.0713°，-12.9624°，7.4674°，-53.3429°，-4.4745°]，

x_goal＝[51.0526°，96.5446°，-62.8643°，-4.0459°，-33.7463°，3.3659°]。

障碍物的尺寸为(0.1×0.1×0.3)，中心位置坐标为[0.35，0.15，0.15]，样本点数N＝1000。

2.将x_init和x_goal加入样本点集合V，初始化树结构τ，初始化用于存储样本点中间状态的集合V_unvisited、V_open和V_closed，将x_goal加入到V_open中，选取V_open中代价最小的节点z，当z不为x_init时重复如下操作：生成z的邻居点集合X，对X与V_unvisited求并集得到集合X_near，对于X_near中的每一个节点x，生成x的邻居点集合Y，对Y与V_open求并集得到集合Y_near，计算Y_near中到x代价最小的节点y_min，判断x与y_min之间边e是否与障碍物发生干涉，若无干涉则将节点x通过边e连接到树τ上，更新x的代价c(x)，将x从V_unvisited中移除并加入到集合V_open中，遍历完X_near中所有的节点后，将z从集合V_open中移除；当z为x_init时跳出循环，并生成从x_init到x_goal的路径σ，以链表形式存储并返回。

在本例中，生成的初始路径σ的关节角变化如图3所示。

3.障碍物沿着一定方向一定的速度开始运动，同时机械臂沿着路径σ开始运动，当机械臂未运动到终点时，执行以下过程：取σ中第一个节点作为x_current节点，取x_current的下一个节点作为x_next节点，将x_current和x_next之间的边e_current离散化成k个关节构形点，传给机械臂控制单元驱动机械臂运动。

若因障碍物运动造成路径σ中的某段发生干涉，暂停机械臂的运动，从目标节点x_goal开始对初始避障路径σ进行干涉检查，若发现路径中有节点或边与障碍物发生干涉，则将树结构从该处断开，将原树***成两棵子树τ和τ_leave，其中τ以目标节点x_goal为根，原树剩余部分组成树τ_leave，并去除σ中属于τ中的节点；从当前目标构形x_current开始对剩余的初始避障路径σ进行干涉检查，若发现路径中有节点或边与障碍物发生干涉，则将树结构从该处断开，断开处节点记为x_seperate，将原树***成两棵子树τ′和τ_leave，其中τ以目标节点为根x_seperate，原树剩余部分组成树τ_leave，去除σ中属于τ_leave中的节点，将τ_leave中的节点从集合V_open和V_closed中去除；

对于剩余路径σ中的每个节点u，在树τ中找到u的所有邻居点集合V_near，对V_near中的节点v，判断u和v之间的边e与障碍物是否发生干涉，若无干涉，则利用边e将树τ和树τ′重连，生成新的树τ，删除树中τ中从x_seperate到u段的所有节点及其子树，如果考察完V_near中的所有节点，树τ和树τ′仍没有重连成功，将标志ReconnectFailed设置为true，并返回，若有一个节点重连成功，则停止对V_near中剩余节点的考察，生成重连后的树τ和路径σ，将标志ReconnectFailed设置为false，重启机械臂的运动并返回；

若ReconnectFailed为true，将路径σ中的节点加入到集合V_σ中，对于树τ′中的每个节点u，判断是否位于集合V_σ中，如不属于集合V_σ，则将节点u释放到样本集合V中，否则的话继续考察V_σ中的下一节点，当V_σ中的节点全部考察完后，在以x_goal为根的树τ的基础上，以集合V_σ为目标区域，按照快速随机树的生长方式继续树的生长，搜索成功时的目标节点为x_conn，删除路径σ从x_seperate到x_conn段的所有节点及其子树，生成新的树τ以及初始避障路径σ。

在本例中，障碍物沿着-y方向以0.05m/s的速度运动，机械臂的关节角的变化如图4所示，机械臂末端执行器的轨迹对比如图5所示，其中青色曲线是初始规划的轨迹曲线，红色曲线是机械臂动态避开障碍物的末端轨迹图，从a点到b点段的路径并未受障碍物移动的影响而发生改变，b点以后的路径，由于受障碍物的运动导致与原有路径干涉，执行重新规划后的路径，从结果中可以看出，机械臂成功避开了运动的障碍物，验证了本方法的有效性。

Claims (8)

1.一种基于快速行进树的机械臂动态避障控制方法，其特征在于包含以下步骤：

1)机械臂在具有动态障碍物的场景环境中运动，机械臂底部固定于机座上，已知机械臂运动的起始位置和目标位置，根据起始位置和目标位置，求解出机械臂的起始关节构形和目标关节构形，在关节构形空间中采集不与障碍物发生干涉的构形作为样本点，关节构形空间是由机械臂的各个关节可能的构形组合而成，所有样本点组成了样本点集合V；

2)根据动态障碍物的初始位置，从目标关节构形开始采用快速随机树算法进行树的生长，获得一棵树结构及其对应的初始避障路径；

3)在机械臂从起始关节构形沿着初始避障路径运动过程中，当因动态障碍物的运动导致路径失效时，通过动态快速行进树算法对路径进行重新调整，暂停机械臂的运动，根据动态障碍物的当前位置对树结构进行调整，生成新路径，控制机械臂沿着新路径重新运动。

4)重复步骤3)，直至机械臂到达目标关节构形。

2.根据权利要求1所述的一种基于快速行进树的机械臂动态避障控制方法，其特征在于：所述的步骤1)采用逆运动学算法求解出起始关节构形x_init和目标关节构形x_goal。

3.根据权利要求1所述的一种基于快速行进树的机械臂动态避障控制方法，其特征在于：所述的机械臂为用于抓取物件的六自由度工业机械臂。

4.根据权利要求1所述的一种基于快速行进树的机械臂动态避障控制方法，其特征在于：所述的步骤1)采用随机采样的方式采集一定数量的样本点构成样本点集V。

5.根据权利要求1所述的一种基于快速行进树的机械臂动态避障控制方法，其特征在于：所述步骤2)具体为：

2.1)初始化树结构τ：将起始关节构形x_init和目标关节构形x_goal加入样本点集合V，建立用于存储样本点中间状态的三个集合V_unvisited、V_open和V_closed，分别为未访问集合V_unvisited、活跃集合V_open和不活跃集合V_closed，将目标关节构形x_goal加入到活跃集合V_open中，以目标关节构形x_goal作为树结构的根样本点，初始化将所有样本点集合V中的样本点放入未访问集合V_unvisited，将目标关节构形x_goal从未访问集合V_unvisited取出放入到活跃集合V_open中，建立用于存储结束搜索的终止样本集合V_final；

2.2)选取活跃集合V_open中到根样本点距离最小的样本点z作为第一参考样本点，距离是指样本点对应构形之间的欧拉距离，并进行以下判断：

当第一参考样本点不在终止样本集合中V_final时，执行步骤2.3)的操作；

当第一参考样本点在终止样本集合中V_final时，则根据树结构生成从起始关节构形x_init到目标关节构形x_goal的路径σ；

2.3)获得第一参考样本点的邻居点构成第一邻居点集合X，对第一邻居点集合X与未访问集合V_unvisited求交集得到第一集合X_near，以第一集合X_near中的一个样本点z作为处理点x，获得处理点x的邻居点构成第二邻居点集合Y，对第二邻居点集合Y与活跃集合V_open求交集得到第二集合Y_near；再计算第二集合Y_near中到根样本点距离最小的样本点z作为第二参考样本点y_min，判断每一个处理点x与第二参考样本点y_min之间的边e是否与障碍物发生干涉；

若无干涉，则将该处理点x通过边e连接到树τ的第二参考样本点y_min上，更新计算处理点x与根样本点之间的距离c(x)，将处理点x从未访问集合V_unvisited中移除并加入到活跃集合V_open中；

若干涉，则不对处理点x进行处理；

2.4)重复上述步骤2.3)遍历完第一集合X_near中所有的样本点z后，将第一参考样本点从活跃集合V_open中移除到不活跃集合V_closed，再回到步骤2.2)重复实施直到步骤2.2)中找到的第一参考样本点为起始关节构形x_init，则根据最终的树结构生成从起始关节构形x_init到目标关节构形x_goal的路径σ，作为初始避障路径。

6.根据权利要求5所述的一种基于快速行进树的机械臂动态避障控制方法，其特征在于：所述步骤2.3)中，邻居点为以样本点z为圆心在半径r范围内包含的其他样本点，采用以下公式计算半径r：

式中，d是机械臂关节的总数，n是样本点集合V中的样本点总数，η是自适应调整参数，η>1；r表示半径。

7.根据权利要求1所述的一种基于快速行进树的机械臂动态避障控制方法，其特征在于：所述步骤3)具体为：

3.1)在路径σ上取当前目标构形x_current及其下一个构形之间的边e_current离散化处理，获得当前目标构形x_current和下一个构形之间的k个关节构形传给机械臂控制单元驱动机械臂运动；

3.2)进行以下判断：

如果到达目标关节构形x_goal，则规划成功，结束方法；

如果未到达目标关节构形x_goal，则重复进行3.1)；

3.3)在步骤3.2)的机械臂运动过程中，实时检测判断路径σ和动态障碍物是否发生干涉；

若因动态障碍物运动造成路径σ中的某段发生干涉，则将暂停机械臂的运动，采用分离树方法将树结构τ分解为以目标关节构形x_goal为根构形的子树τ1，以路径σ中当前目标构形x_current到失效目标构形x_seperate之间的路径作为终止样本集合V_final，其中失效目标构形x_seperate表示干涉段中距离当前目标构形x_current最近的路径σ中的构形；

3.4)采用重生长树方法，以终止样本集合V_final为搜索结束区域，采用步骤2)的方式进行树的生长，生成新的从当前目标构形x_current到失效目标构形x_seperate的路径σ，重启机械臂的运动，并返回步骤3.1)。

8.根据权利要求书7所述的一种基于快速行进树的机械臂动态避障控制方法，其特征在于：所述步骤3.3)具体为：

3.3.1)从目标关节构形x_goal开始遍历路径σ上的每个路径构形x_i，对路径构形x_i做干涉检查，若发生干涉，则将路径构形x_i及所有的后代构形加入到未访问集合V_unvisited，并将路径构形和其后代构形从活跃集合V_open或不活跃集合V_closed中移除，活跃集合V_open和不活跃集合V_closed集合中剩余的构形组成以x_goal为根的子树τ1；

3.3.2)从当前关节构形x_current开始遍历路径σ上的每个路径构形x_i，对路径构形x_i做干涉检查，若不发生干涉，则将路径构形x_i加入到目标区域V_goal中，若发生干涉，则遍历结束，跳转到步骤3.4)。