CN110228069A - 一种机械臂在线避障运动规划方法 - Google Patents

Abstract

本发明提供一种机械臂在线避障运动规划方法，涉及机械臂运动规划技术领域。该方法对机械臂和障碍物进行建模，通过逆解确定目标节点；预规划阶段，在关节空间内进行采样生成新节点，对新节点进行碰撞检测确定其是否添加进搜索树中，将搜索树中使得新节点成本最小的节点作为其父节点；从搜索树中规划出的前k个节点并送给轨迹规划器，直至搜索到目标节点进入重规划；重规划阶段，在路径节点附近进行采样以优化路径，当障碍物移动与路径冲突时则重新规划路径，同时将规划节点送给轨迹规划器，直至机械臂运动到目标点。本发明方法路径规划与机械臂运动同步进行，实现了在线避障运动，即使目标节点位置发生变化，也能够重新采样连接路径到达目标点。

Description

一种机械臂在线避障运动规划方法

技术领域

本发明涉及机械臂运动规划技术领域，尤其涉及一种机械臂在线避障运动规划方法。

背景技术

运动规划是机器人研究的基本问题，早期的运动规划主要针对移动机器人，将移动机器人看做二维平面的一个质点，寻找一条从起始点到目标点的无碰撞路径。目前针对移动机器人的路径规划方法已经非常丰富，目前该类方法主要有Dijkstra算法、A*算法、人工势场法、群智能算法等。针对机械臂这样的多输入多输出、非线性、强耦合的高维复杂系统，这些方法分别存在相应时间长、计算量大、易陷入局部最优等缺点，不能实现机械臂工作的在线避障。因此机械臂在线实时避障运动规划是机械臂控制的一个关键问题。

基于随机采样的方法PRM(Probabilistic Road-Map)和RRT(Rapidly-exploringRandom Tree)算法凭借强大的搜索能力和不需要对障碍物进行精确建模的优点广受学者们的喜爱，也更适合于机械臂这样的高维空间。PRM算法分为两个步骤，首先在可行解空间内随机采样一定数量的点，然后将这些采样点连接并寻找最短路径。采样点过多会降低最短路径的寻找速度，难以达到机械臂在线实时运动的要求。RRT算法速度较快，RRT*对其节点连接方式进行了改进，使得RRT*具有渐进寻优的能力。

发明内容

本发明要解决的技术问题是针对上述现有技术的不足，提供一种机械臂在线避障运动规划方法，通过预规划和重规划两个阶段，在规划过程中将若干个节点输入给轨迹规划器，宏观上实现机械臂在线实时避障运动。

为解决上述技术问题，本发明所采取的技术方案是：一种机械臂在线避障运动规划方法，包括以下步骤：

步骤1：利用D-H法对六自由度机械臂进行建模，对障碍物使用轴向包围盒和球形包围盒技术建立模型，给出笛卡尔空间的起始点和目标点坐标；

步骤2：初始化机械臂的工作环境和参数，确定障碍物位置，逆解确定关节空间中的机械臂运动路径的起始点q_init和目标点q_goal，确定扩展步长r，邻域半径R，并初始化仅包含起始节点的搜索树和最大迭代次数maxAttempts，进入预规划阶段；

所述关节空间，对于一个具有n个自由度的的操作臂来说，它的所有连杆位置由一组n个关节变量来确定，这样的一组变量通常被称为n×1的关节矢量，所有关节矢量组成的空间称为关节空间；

步骤3：预规划阶段，在关节空间内以概率α向目标点采样，以概率1-α在自由空间内随机采样，得到新采样点q_rand，对搜索树中节点遍历求得距离q_rand最近的节点q_nearest，从q_nearest向q_rand以扩展步长r扩展得到新节点q_new；

步骤4：计算距离新节点q_new小于R的邻域集合Q_near，在Q_near内找出节点成本与该节点到q_new距离之和最小的节点作为q_new的父节点q_parent；

所述节点成本描述了两个节点q_1c和q_2c间的距离信息，具体计算方式为：

其中，

其中，d₁为关节空间的欧式距离，d₂为关节角对应机械臂末端位置的欧式距离，Δθ为关节角对应机械臂末端姿态的欧式距离，(x₁，y₁，z₁)和(x₂，y₂，z₂)分别为两个节点对应机械臂末端在笛卡尔空间的坐标，r′₁、p₁、y′₁和r′₂、p₂、y′₂分别为绕z、y、x轴的旋转角，α、β、γ均为权重系数；

步骤5：对新节点q_new与其父节点q_parent形成的路径做碰撞检测，如果发生碰撞，则返回步骤3重新采样；否则将新节点q_new与其父节点q_parent形成的路径添加进搜索树中，并对邻域集合Q_near中每个节点进行遍历，如果邻域集合Q_near中节点q_near的成本小于新节点q_new的成本与两者形成路径距离之和，则用q_new代替q_near原来的父节点，并更新其对应的成本；

所述碰撞检测的具体方法为：对障碍物使用轴向包围盒或球形包围盒包络，对机械臂连杆使用圆柱体进行包络，然后根据轴向包围盒是否与圆柱体相交、球心与圆柱体中心线的直线距离是否小于两者半径之和来判断是否发生碰撞；

步骤6：判断新节点q_new与目标节点之间的距离q_goal小于扩展步长r是否成立，如果成立并且q_goal与q_new形成的路径没有发生碰撞，则说明已经找到可到达目标点的路径，将q_goal添加进搜索树中并转到步骤8进入重规划阶段，否则执行步骤7；

步骤7：判断搜索树中的节点数是否大于等于根据起点到目标点的距离及扩展步长确定的机械臂运动路径点数k，如果是，则在建立好的搜索树中规划出k个节点，形成机械臂运动的路径点序列，并将路径点序列送到轨迹规划器中控制机械臂运动，循环执行步骤3至步骤7直到跳出循环进入重规划阶段或达到最大迭代次数中止，否则，重新执行步骤3；

步骤8：重规划阶段，更新目标点q_goal、机械臂位置q_a和障碍物位置；

步骤9：在搜索树中从q_goal反向检索得到通向目标节点的路径，在此路径上选取一个节点作为信标q_beacon，以概率α在以R为半径，q_beacon为圆心的超椭球内随机采样，以概率1-α在自由空间内随机采样，按照步骤3-5的方法生成新节点并连接，规划出新路径；

步骤10：按照三角形定理“两边之和大于第三边”去掉新路径中的中间点使得整条路径的总成本更小，对整条路径重新布线以优化路径；

步骤11：在建立好的搜索树中重新规划出k个节点，形成机械臂运动的新路径点序列，当机械臂q_a运动到新路径的起始节点q₀时，将k个节点序列送到轨迹规划器中控制机械臂运动，并使用新路径的最后一个节点更新起始节点，即q₀＝q_k，循环执行步骤8至步骤11直到机械臂q_a运动到目标节点q_goal。

进一步地，步骤7和步骤11所述在建立好的搜索树中规划出k个节点的具体方法为：

(1)对路径点序列中机械臂所在的当前路径点q_i-1，i＝1，…，k，选择代价f_c＝cost(q_i)+H(q_i)最小的子节点作为下一路径点q_i，H(q_i)描述的是q_i节点到目标节点的代价，

(2)如果下一路径点q_i是搜索树的叶子节点或其子节点已被标记，返回路径点序列并标记q_i，否则执行(3)；

(3)如果新路径点q_i比之前的路径点q_i-1更接近目标点，则将新路径点q_i加入路径点序列，更新路径点序列；

(4)重复执行(1)至(3)，直至得到k个路径点，并返回路径点序列。

采用上述技术方案所产生的有益效果在于：本发明提供的一种机械臂在线避障运动规划方法，对RRT*进行改进并应用到机械臂空间，使其能够根据障碍物位置进行剪枝和重新搜索路径，规划过程中将节点送给机械臂轨迹规划器，实现在线避障运动规划；同时，在整个规划过程中机械臂始终处于移动状态，即使障碍物和目标节点发生移动，依然能够重新搜索路径，到达目标节点。

附图说明

图1为本发明实施例提供的一种机械臂在线避障运动规划方法的流程图；

图2为本发明实施例提供的6自由度机械臂模型示意图；

图3为本发明实施例提供的不同障碍物所采用的包围盒示意图，其中，(a)为采用轴向包围盒的障碍物A，(b)为采用球形包围盒的障碍物B，(c)为采用两种包围盒都可以的障碍物C；

图4为本发明实施例提供的RRT*添加节点与重新布线过程示意图；

图5为本发明实施例提供的碰撞检测过程的示意图；

图6为本发明实施例提供的三维空间内采用三种算法的搜索结果对比图，其中，(a)为采用RRT算法，(b)为采用RRT*算法，(c)为采用本发明方法；

图7为本发明实施例提供的重规划添加节点与路径优化示意图，其中，(a)为生成新节点并连接，(b)为对整条路径重新布线以优化路径；

图8为本发明实施例提供的预规划路径与重规划路径的对比图，其中，(a)为预规划路径，(b)为重规划路径；

图9为本发明实施例提供的实际环境下机械臂避障过程图，其中，(a)为机械臂的起始点位置，(b)为机械臂越过障碍物，(c)为机械臂到达目标点。

图中：1、障碍物1；2、障碍物2；3、连杆i；4、连杆i-1。

具体实施方式

下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。以下实施例用于说明本发明，但不用来限制本发明的范围。

本实施例中，一种机械臂在线避障运动规划方法，如图1所示，包括以下步骤：

步骤1：利用D-H法对六自由度机械臂进行建模，对障碍物使用轴向包围盒和球形包围盒技术建立模型，给出笛卡尔空间的起始点和目标点坐标；

本实施例中，首先建立6自由度机械臂模型，如图2所示，每个关节都是旋转关节，绕z轴旋转，其中(X₀，Y₀，Z₀)是基坐标系，(X₆，Y₆，Z₆)是末端坐标系。机械臂的D-H参数如表1所示，使用D-H参数就可以完成坐标变换过程：

(1)将Z_i-1轴绕X_i-1轴旋转α_i-1，使得Z_i-1与Z_i平行或重合；

(2)沿X_i-1轴平移a_i-1的距离，使得i-1轴坐标系与i轴坐标系原点重合；

(3)绕Z_i轴旋转θ_i，使得X_i-1轴与X_i轴相互平行或重合；

(4)沿Z_i轴平移d_i的距离，使得i-1轴坐标系与i轴坐标系完全重合。

根据上述变换可将坐标系i-1变换到坐标系i，如下公式(1)所示：

其中，sθ为sin(θ)简写，cθ为cos(θ)简写。

分别计算机械臂各个连杆的变换矩阵，将所有的变换矩阵连乘就得到机械臂正运动学模型，即坐标系{n}相对于坐标系{0}的变换矩阵，如公式(2)所示：

表1机械臂D-H参数

本实施例中，不同类型的障碍物包围盒示意图如图3所示，障碍物A采用轴向包围盒，障碍物B采用球形包围盒，对于障碍物C，两种包围盒都可以，选择较为简单的球形包围盒。因此在对障碍物选择包围盒时，首先使用轴向包围盒，当其边长满足一定条件时，说明包围盒的边长值相近，此时改用球形包围盒更为合理。

步骤2：初始化机械臂的工作环境和参数，确定障碍物位置，逆解确定关节空间中的机械臂运动路径的起始点q_init和目标点q_goal，确定扩展步长r，邻域半径R，并初始化仅包含起始节点的搜索树和最大迭代次数maxAttempts，进入预规划阶段；

所述关节空间，对于一个具有n个自由度的的操作臂来说，它的所有连杆位置由一组n个关节变量来确定，这样的一组变量通常被称为n×1的关节矢量，所有关节矢量组成的空间称为关节空间；

本实施例中，在100×100×100的三维空间内，在不考虑机械臂姿态，只考虑空间位置的情况下，设置起始点q_init(10，10，10)，目标点q_goal(90，90，90)，扩展步长r＝5，邻域半径R＝15。

步骤3：预规划阶段，在关节空间内以概率α向目标点采样，以概率1-α在自由空间内随机采样，得到新采样点q_rand，对搜索树中节点遍历求得距离q_rand最近的节点q_nearest，从q_nearest向q_rand以扩展步长r扩展得到新节点q_new；

本实施例中，在关节空间内以概率α向目标节点进行采样得到q_rand，如式(3)所示，对搜索树中节点遍历求得距离q_rand最近的节点q_nearest，从q_nearest向q_rand以步长r扩展得到新节点q_new，如式(4)所示；

其中，P_r是一个0～1的随机值，α是人为设定的0～1之间的值；

步骤4：计算距离新节点q_new小于R的邻域集合Q_near，在Q_near内找出节点成本与该节点到q_new距离之和最小的节点作为q_new的父节点q_parent；

所述节点成本描述了两个节点q_1c和q_2c间的距离信息，具体计算方式为：

其中，

其中，d₁为关节空间的欧式距离，d₂为关节角对应机械臂末端位置的欧式距离，Δθ为关节角对应机械臂末端姿态的欧式距离，(x₁，y₁，z₁)和(x₂，y₂，z₂)分别为两个节点对应机械臂末端在笛卡尔空间的坐标，r′₁、p₁、y′₁和r′₂、p₂、y′₂分别为绕z、y、x轴的旋转角，α、β、γ均为权重系数。

步骤5：对新节点q_new与其父节点q_parent形成的路径做碰撞检测，如果发生碰撞，则返回步骤3重新采样；否则将新节点q_new与其父节点q_parent形成的路径添加进搜索树中，并对邻域集合Q_near中每个节点进行遍历，如果邻域集合Q_near中节点q_near的成本小于新节点q_new的成本与两者形成路径距离之和，则用q_new代替q_near原来的父节点，并更新其对应的成本，如图4所示；

所述碰撞检测的具体方法为：对障碍物使用轴向包围盒或球形包围盒包络，对机械臂连杆使用圆柱体进行包络，然后根据轴向包围盒是否与圆柱体相交、球心与圆柱体中心线的直线距离是否小于两者半径之和来判断是否发生碰撞；

本实施例中，碰撞检测的示意图如图5所示，A_i，B_i是连杆i的两端，ω_i是连杆i的半径，障碍物1使用球形包围盒，障碍物2使用轴向包围盒。针对球形包围盒的碰撞检测，如果障碍物1包络球球心P到连杆i的距离dis_i＜r+ω_i，说明连杆i与障碍物1发生碰撞，反之，则没有发生碰撞。针对轴向包围盒的碰撞检测，线段AB上任一点在障碍物的包围盒内，则说明连杆i与障碍物2发生碰撞。分别检测每个连杆，确定整个机械臂与障碍物是否发生碰撞。

步骤6：判断新节点q_new与目标节点之间的距离q_goal小于扩展步长r是否成立，如果成立并且q_goal与q_new形成的路径没有发生碰撞，则说明已经找到可到达目标点的路径，将q_goal添加进搜索树中并转到步骤8进入重规划阶段，否则执行步骤7；

步骤7：判断搜索树中的节点数是否大于等于根据起点到目标点的距离及扩展步长确定的机械臂运动路径点数k，如果是，则在建立好的搜索树中规划出k个节点，形成机械臂运动的路径点序列，并将路径点序列送到轨迹规划器中控制机械臂运动，循环执行步骤3至步骤7直到跳出循环进入重规划阶段或达到最大迭代次数中止，否则，重新执行步骤3；

本实施例中，给出了如图6预规划阶段的搜索路径图，设置了7个障碍物，并与RRT、RRT*算法做了对比，可以看出，RRT搜索路径不够光滑，RRT*的搜索点几乎布满了整个空间，很多都是没必要的，本发明方法Pre-planning在预规划阶段以概率α向目标点进行搜索，轨迹较为光滑，同时在自由空间也有一些搜索点，便于重规划进行扩展和重新布线。表2给出了三种算法运行的不同指标，可以看出针对7个障碍物的复杂环境，三种算法都有着较高的成功率。RRT*由于对父节点的选择做了改进，路径长度小于RRT，但运行时间较长，迭代次数更多。而本发明方法Pre-planning在RRT*的基础上对采样方式做了改进，同时将前k个节点送给机械臂控制器，使其路径长度更短，成功率更高，迭代次数也最短，而运行时间与RRT相比变化不大。

表2三种算法性能比较

算法	成功次数	迭代次数	运行时间/ms	路径长度/cm
					RRT	8	807	101	190.10
RRT*	9	1053	570	170.82
					Pre-planning	10	270	116	153.41

步骤8：重规划阶段，更新目标点q_goal、机械臂位置q_a和障碍物位置；

步骤9：在搜索树中从q_goal反向检索得到通向目标节点的路径，在此路径上选取一个节点作为信标q_beacon，以概率α在以R为半径，q_beacon为圆心的超椭球内随机采样，如公式(9)所示，以概率1-α在自由空间内随机采样，按照步骤3-5的方法生成新节点并连接，如图7(a)所示，规划出新路径；

步骤10：按照三角形定理“两边之和大于第三边”去掉新路径中的中间点使得整条路径的总成本更小，对整条路径重新布线以优化路径，如图7(b)所示；

步骤11：在建立好的搜索树中重新规划出k个节点，形成机械臂运动的新路径点序列，当机械臂q_a运动到新路径的起始节点q₀时，将k个节点序列送到轨迹规划器中控制机械臂运动，并使用新路径的最后一个节点更新起始节点，即q₀＝q_k，循环执行步骤8至步骤11直到机械臂q_a运动到目标节点q_goal。

步骤7和步骤11所述在建立好的搜索树中规划出k个节点的具体方法为：

(1)对路径点序列中机械臂所在的当前路径点q_i-1，i＝1，…，k，选择代价f_c＝cost(q_i)+H(q_i)最小的子节点作为下一路径点q_i，H(q_i)描述的是q_i节点到目标节点的代价，

(2)如果下一路径点q_i是搜索树的叶子节点或其子节点已被标记，返回路径点序列并标记q_i，否则执行(3)；

(3)如果新路径点q_i比之前的路径点q_i-1更接近目标点，则将新路径点q_i加入路径点序列，更新路径点序列；

(4)重复执行(1)至(3)，直至得到k个路径点，并返回路径点序列。

本实施例给出了如图8所示的预规划与重规划的对比图，图8(a)是预规划给出的路径，当中间障碍物的变大后与规划路径发生冲突，在此基础上重新规划修改后面的路径，依然可以顺利完成避障，图8(b)所示。说明本发明方法可以很好地适应动态环境，能够完成动态避障任务。

本实施例还给出了如图9所示的实际机械臂避障过程图，利用ROS(RobotOperating System)中的运动规划库MoveIt！控制机械臂进行避障运动规划。长方体盒子为障碍物，图9(a)中方块位置为起始点，任务是将其移动到障碍物的另一侧，如图9(c)所示。通过对障碍物建模，对每一个路径点进行碰撞检测实现避障运动。在规划过程中，将k个节点送给轨迹规划器控制机械臂运动，实现了在线避障。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明权利要求所限定的范围。

Claims (5)

1.一种机械臂在线避障运动规划方法，其特征在于：包括以下步骤：

步骤1：利用D-H法对六自由度机械臂进行建模，对障碍物使用轴向包围盒和球形包围盒技术建立模型，给出笛卡尔空间的起始点和目标点坐标；

步骤2：初始化机械臂的工作环境和参数，确定障碍物位置，逆解确定关节空间中的机械臂运动路径的起始点q_init和目标点q_goal，确定扩展步长r，邻域半径R，并初始化仅包含起始节点的搜索树和最大迭代次数maxAttempts，进入预规划阶段；

步骤3：预规划阶段，在关节空间内以概率α向目标点采样，以概率1-α在自由空间内随机采样，得到新采样点q_rand，对搜索树中节点遍历求得距离q_rand最近的节点q_nearest，从q_nearest向q_rand以扩展步长r扩展得到新节点q_new；

步骤4：计算距离新节点q_new小于R的邻域集合Q_near，在Q_near内找出节点成本与该节点到q_new距离之和最小的节点作为q_new的父节点q_parent；

步骤5：对新节点q_new与其父节点q_parent形成的路径做碰撞检测，如果发生碰撞，则返回步骤3重新采样；否则将新节点q_new与其父节点q_parent形成的路径添加进搜索树中，并对邻域集合Q_near中每个节点进行遍历，如果邻域集合Q_near中节点q_near的成本小于新节点q_new的成本与两者形成路径距离之和，则用q_new代替q_near原来的父节点，并更新其对应的成本；

步骤6：判断新节点q_new与目标节点之间的距离q_goal小于扩展步长r是否成立，如果成立并且q_goal与q_new形成的路径没有发生碰撞，则说明已经找到可到达目标点的路径，将q_goal添加进搜索树中并转到步骤8进入重规划阶段，否则执行步骤7；

步骤7：判断搜索树中的节点数是否大于等于根据起点到目标点的距离及扩展步长确定的机械臂运动路径点数k，如果是，则在建立好的搜索树中规划出k个节点，形成机械臂运动的路径点序列，并将路径点序列送到轨迹规划器中控制机械臂运动，循环执行步骤3至步骤7直到跳出循环进入重规划阶段或达到最大迭代次数中止，否则，重新执行步骤3；

步骤8：重规划阶段，更新目标点q_goal、机械臂位置q_a和障碍物位置；

步骤9：在搜索树中从q_goal反向检索得到通向目标节点的路径，在此路径上选取一个节点作为信标q_beacon，以概率α在以R为半径，q_beacon为圆心的超椭球内随机采样，以概率1-α在自由空间内随机采样，按照步骤3-5的方法生成新节点并连接，规划出新路径；

步骤10：按照三角形定理“两边之和大于第三边”去掉新路径中的中间点使得整条路径的总成本更小，对整条路径重新布线以优化路径；

步骤11：在建立好的搜索树中重新规划出k个节点，形成机械臂运动的新路径点序列，当机械臂q_a运动到新路径的起始节点q₀时，将k个节点序列送到轨迹规划器中控制机械臂运动，并使用新路径的最后一个节点更新起始节点，即q₀＝q_k，循环执行步骤8至步骤11直到机械臂q_a运动到目标节点q_goal。

2.根据权利要求1所述的一种机械臂在线避障运动规划方法，其特征在于：步骤2所述关节空间，对于一个具有n个自由度的的操作臂来说，它的所有连杆位置由一组n个关节变量来确定，这样的一组变量通常被称为n×1的关节矢量，所有关节矢量组成的空间称为关节空间。

3.根据权利要求1所述的一种机械臂在线避障运动规划方法，其特征在于：步骤4所述节点成本描述了两个节点q_1c和q_2c间的距离信息，具体计算方式为：

其中，

其中，d₁为关节空间的欧式距离，d₂为关节角对应机械臂末端位置的欧式距离，Δθ为关节角对应机械臂末端姿态的欧式距离，(x₁，y₁，z₁)和(x₂，y₂，z₂)分别为两个节点对应机械臂末端在笛卡尔空间的坐标，r′₁、p₁、y′₁和r₂′、p₂、y′₂分别为绕z、y、x轴的旋转角，α、β、γ权重系数。

4.根据权利要求1所述的一种机械臂在线避障运动规划方法，其特征在于：步骤5所述碰撞检测的具体方法为：对障碍物使用轴向包围盒或球形包围盒包络，对机械臂连杆使用圆柱体进行包络，然后根据轴向包围盒是否与圆柱体相交、球心与圆柱体中心线的直线距离是否小于两者半径之和来判断是否发生碰撞。

5.根据权利要求1所述的一种机械臂在线避障运动规划方法，其特征在于：步骤7和步骤11所述在建立好的搜索树中规划出k个节点的具体方法为：

(1)对路径点序列中机械臂所在的当前路径点q_i-1，i＝1，…，k，选择代价f_c＝cost(q_i)+H(q_i)最小的子节点作为下一路径点q_i，H(q_i)描述的是q_i节点到目标节点的代价，

(2)如果下一路径点q_i是搜索树的叶子节点或其子节点已被标记，返回路径点序列并标记q_i，否则执行(3)；

(3)如果新路径点q_i比之前的路径点q_i-1更接近目标点，则将新路径点q_i加入路径点序列，更新路径点序列；

(4)重复执行(1)至(3)，直至得到k个路径点，并返回路径点序列。