CN110125943B - 多自由度机械臂避障路径规划方法 - Google Patents

Abstract

本发明公开了一种多自由度机械臂避障路径规划方法，包括如下步骤：采用机械臂路径规划方法得到初始路径；采集初始路径中每个关节在每一预设时刻的实时信息；对所述初始路径中的实时信息进行数学建模并映射，得到Q_ec，在Q_ec中随机选择q条路径，按预设的约束条件计算可行度、适应度，将适应度最大的路径标记为合格路径；重复，直至合格路径的数量达到G，将G条合格路径记为第二代路径集合Q′_ec；对第二代路径集合Q′_ec进行交叉、变异、融合；得到第三代路径集合Q″_ec将得到的第三代路径集合Q″_ec作为Q_ec,重复，直至迭代次数为D；将最后一次迭代中得到适应度最大的路径作为最终目标路径，本方法对初始路径进行进一步优化，获得更加高效的机械臂运行路径。

Description

多自由度机械臂避障路径规划方法

技术领域

本发明涉及机械臂避障路径规划和优化方法，具体涉及一种多自由度机械臂避障路径规划方法。

背景技术

在工业机器人领域，确保机械臂运动轨迹安全性和合理性具有重要的研究意义，现有的机械臂路径规划方法有：A*，D*，人工势场法，粒子群算法，蚁群算法等，如专利CN107953334A中公开了一种基于A星算法的工业机械臂无碰撞路径规划方法，采用基于分离轴的碰撞检测算法在搜索的每一步中判断机械臂是否与环境发生碰撞，根据关节转动角度之和最小且不发生碰撞原则在机械臂的六维关节空间中进行搜索，得到无碰撞的路径；

专利CN106166750A中公开了一种改进型D*机械臂动态避障路径规划方法，根据空间机械臂特征，获得基于启发函数的路径搜索算法；将应用于七自由度机械臂D*避障路径规划进行改进，完成七自由度机械臂动态避障路径规划；

专利CN104029203A公开了一种实现空间机械臂避障的路径规划方法，通过建立窗口型障碍，利用人工势场方法找出一条机械臂末端避障路径；

这些方法存在：建模以及求解上需要花费很长的时间，场景变化需要重新建模的劣势；且都是基于空间位置考虑，并未考虑机械臂在执行路径时，关节速度、加速度、力矩等情况。

发明内容

因此本发明提出了一种多自由度机械臂避障路径规划方法，通过设置机械臂运行的优化指标(驱动***性能指标)，对初始路径进行进一步优化，获得更加高效的机械臂运行路径。

一种多自由度机械臂避障路径规划方法，包括如下步骤：

S1、采用机械臂路径规划方法对机械臂进行路径规划，得到初始路径，所述机械臂的关节数为m；

S2、所述机械臂沿初始路径运动，采集每个关节在每一预设时刻的实时信息；

所述实时信息包括：当前运动时刻t_ij、关节空间信息Z_ij、速度v_ij、加速度a_ij以及驱动力矩信息τ_ij，i＝1,2,3……m；j表示当前时刻，j＝1,2,3……n；n为预设时刻的总数；所述关节空间信息Z_ij为当前关节的角度信息；

S3、对所述初始路径中的实时信息进行数学建模并映射，得到包含多条路径信息的集合，单条路径信息包括基于预设的运动时间和各关节的角度计算得到的特定时刻各关节的速度、加速度和驱动力矩；

S4、记所述包含多条路径信息的集合为Q_ec，e对应第几条路径，c对应第几次迭代；e＝1,2,3……r，r为路径的总条数；c＝1,2,3……D，D为预设的迭代总数；Q_ec包含信息为：第c次迭代第e条路径第i个关节在第j时刻所对应的时间t′_eijc、角度Z′_eijc、速度v′_eijc、加速度a′_eijc以及驱动力矩信息τ′_eijc；

在Q_ec中随机选择q条路径，按预设的约束条件计算其对应Q_ec的可行度，当可行度不满足预设的可行度阈值时，剔除当前路径；遍历q条路径，将所有剩下的路径组成的集合记为q’；计算q’中各条路径的适应度，将适应度最大的路径标记为合格路径；

将Q_ec中合格路径剔除后的集合作为新的Q_ec，采用相同方法筛选合格路径，直至合格路径的数量达到G，将G条合格路径组成的集合标记为第二代路径集合Q'_ec；其中G＝10％～40％r；

S5、对第二代路径集合Q'_ec随机进行交叉处理，得到杂交后的路径，对杂交后的路径进行变异处理；同时，将发生变异的和未发生变异的路径融合；得到第三代路径集合Q”_ec，其包含的路径条数为90％～110％r；

优选，所述交叉处理采用模拟二进制交叉方法、所述变异处理采用多项式变异方法。

S6、将得到的第三代路径集合Q'_ec作为步骤S4中的Q_ec，在其中筛选q条路径，再采用相同方法获取第二代路径集合Q'_ec，再利用步骤S5方法获取第三代路径集合Q”_ec,重复本步骤，直至迭代次数为D；将最后一次迭代中得到的第三代路径集合Q”_ec中适应度最大的路径作为最终目标路径。

优选D＝15～30。

进一步，还包括S7、机械臂沿所述目标路径运动，进行碰撞检测，若机械臂与环境发生碰撞，则重新设置S4约束条件中的机器人可达工作空间，重新进行步骤S4。

进一步，模型映射采用Logistic映射或McDill-amateis模型；

进一步，采用Logistic映射时，计算Logistic映射的输入参数：混沌变量β_j+1、路径种群空间波动范围Δt_j；

β_j+1＝μβ_j(1-β_j)

人为设定：初始混沌变量β₁，取值：0≤β₁≤1，混沌系数μ，取值：0≤μ≤4；β_j为所述初始路径上当前时刻的混沌变量，β_j+1所述初始路径上下一时刻的混沌变量；

和

为对应的Δt_j的变化区间，通过人为设定，用于限制每个路径种群Q_m中每个关节关节空间信息相对于初始路径中每个关节关节空间信息的波动范围。

进一步，步骤S4中的q＝[10％～30％r]，其中r为c＝1时，Q_ec的路径总条数；

可行度

的计算如下：

x’_eijc为实时信息中的一项参数，min{0,g(x’_eijc)}表示：当输入参数x_eijc满足约束条件时，输出0，反之输出x′_eijc与约束条件中的预设上限值x′_eijc-max的差值；

h_k(Z’_eijc)表示：当输入参数Z’_eijc满足Z’_eijc∈Z’_eijc-max时，输出0，反之，输出无穷大，Z’_eijc-max表示每个关节可以到达角度的上限，为机械臂的固有参数；

当所述差值＜0.2～0.5x′_eijc-max，且h_k(Z’_eijc)＝0时，可行度

满足预设的可行度阈值。

进一步，所述约束条件为：根据机器人型号，运行要求，人为设置实时信息的上限值：v′_eijc-max＝1.2～1.5v_ij、a′_eijc-max＝1.2～1.5a_ij、τ′_eijc-max＝1.2～1.5τ_ij。

即：关节力矩：τ′_eijc≤τ′_eijc-max、关节加速度：a′_eijc≤a′_eijc-max、关节加加速度：a″_eijc≤a″_eijc-max、关节速度：v′_eijc≤v′_eijc-max。

进一步，步骤S4的适应度:f计算如下：

S_pijc为优化指标，根据所述路径种群q’的实时信息计算，p＝1,2,3……L,L为优化指标的个数：k_p为人为设定的每个优化指标对应的权重，k_p＝0表示该项指标不参与优化。

优选，L＝6，所述优化指标包括：

运动时间指标S_1ijc，用于衡量机器人的运动效率：

关节的平均加速度指标S_2ij，用于衡量机器人关节的能耗：

关节的平均波动指标S_3ij，用于衡量的是轨迹的平滑度:

关节平均力矩指标S_4ij:

关节平均消耗的能量指标S_5ij:

力矩指标S_6ij，用于衡量执行机构的平稳性:

其中，T为机械臂沿初始路径运动的总时间，A′_eijc为加速度的导数，加加速度。

进一步，所述预设时刻的设置方式为：设定时间间隔t,从初始路径的起始位置开始，每间隔时间t，采集当前时刻的实时信息；

优选，t＝0.5ms～4ms；

进一步，步骤S1中利用深度相机获取机器人运动空间的环境模型、采用混合层次包围盒方法简化所述环境模型，采用机械臂路径规划方法对机械臂进行路径规划，得到初始路径；

所述机械臂路径规划方法包括：A*、D*、RRT*、PRM*、人工势场法、遗传算法、粒子群算法或蚁群算法。

利用本发明方法对现有路径规划方法得到的初始路径规进行优化，结果如下：

由上表可以看出，经过本发明方案的优化，机械臂路径长度缩短，运行时间减少，规划出了更加优化的路径。

附图说明

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

具体实施方式

以下结合具体实施方式对本发明的技术方案进行详细描述。

一种多自由度机械臂避障路径规划方法，包括如下步骤：

S1、利用深度相机获取机器人运动空间的环境模型、采用OBB包围盒方法简化环境模型，采用PRM*对机械臂进行路径规划，得到初始路径；

记机械臂的关节数为m；

S2、机械臂沿初始路径运动，采集每个关节在每一预设时刻的实时信息；

预设时刻的设置方式为：设定时间间隔0.1ms,从初始路径的起始位置开始，每间隔时间0.1ms，采集一次当前时刻的实时信息；

实时信息包括：当前运动时刻t_ij、关节空间信息Z_ij、速度v_ij、加速度a_ij以及驱动力矩信息τ_ij，i＝1,2,3……m；j表示当前时刻，j＝1,2,3……n；n为预设时刻的总数；关节空间信息Z_ij为当前关节的角度信息；

S3、对初始路径中的实时信息进行数学建模并采用Logistic映射，计算Logistic映射的输入参数：混沌变量β_j+1、路径种群空间波动范围Δt_j；

β_j+1＝μβ_j(1-β_j)

人为设定：初始混沌变量β₁＝1，混沌系数μ＝4；β_j为初始路径上当前时刻的混沌变量，β_j+1初始路径上下一时刻的混沌变量；

和

为对应的Δt_j的变化区间，通过人为设定，用于限制每个路径种群Q_m中每个关节关节空间信息相对于初始路径中每个关节关节空间信息的波动范围。

通过模型映射得到包含500条路径信息的集合，单条路径信息包括基于预设的运动时间和各关节的角度计算得到的特定时刻各关节的速度、加速度和驱动力矩；

S4、记包含多条路径信息的集合为Q_ec，e对应第几条路径，c对应第几次迭代；e＝1,2,3……500；c＝1,2,3……20，20为预设的迭代总数；Q_ec包含信息为：第c次迭代第e条路径第i个关节在第j时刻所对应的时间t′_eijc、角度Z′_eijc、速度v′_eijc、加速度a′_eijc以及驱动力矩信息τ′_eijc；

在Q_ec中随机选择q＝100条路径，按预设的约束条件计算其对应Q_ec的可行度

当可行度不满足预设的可行度阈值时，剔除当前路径；遍历q条路径，将所有剩下的路径组成的集合记为q’；计算q’中各条路径的适应度，将适应度最大的路径标记为合格路径；

将Q_ec中合格路径剔除后的集合作为新的Q_ec，采用相同方法筛选合格路径，直至合格路径的数量达到G＝150条，将150条合格路径组成的集合标记为第二代路径集合Q'_ec；

S5、对第二代路径集合Q'_ec随机进行交叉处理，得到杂交后的路径，对杂交后的路径进行变异处理；同时，将发生变异的和未发生变异的路径融合；得到第三代路径集合Q”_ec，其包含的路径条数为480；

优选，交叉处理采用模拟二进制交叉方法、变异处理采用多项式变异方法。

S6、将得到的第三代路径集合Q”_ec作为步骤S4中的Q_ec，在其中筛选q＝100条路径，再采用相同方法获取第二代路径集合Q'_ec，再利用步骤S5方法获取第三代路径集合Q”_ec,重复本步骤，直至迭代次数为20；将最后一次迭代中得到的第三代路径集合Q”_ec中适应度最大的路径作为最终目标路径。

S7、机械臂沿目标路径运动，进行碰撞检测，若机械臂与环境发生碰撞，则重新设置S4约束条件中的机器人可达工作空间，重新进行步骤S4。

其中，可行度

的计算如下：

x’_eijc为实时信息中的一项参数，min{0,g(x’_eijc)}表示：当输入参数x_eijc满足约束条件时，输出0，反之输出x′_eijc与约束条件中的预设上限值x′_eijc-max的差值；本实施例中，约束条件为：人为设置的实时信息的上限值：v′_eijc-max＝1.3v_ij、a′_eijc-max＝1.3a_ij、τ′_eijc-max＝1.3τ_ij；

即：关节力矩：τ′_eijc≤τ′_eijc-max、关节加速度：a′_eijc≤a′_eijc-max、关节加加速度：a″_eijc≤a″_eijc-max、关节速度：v′_eijc≤v′_eijc-max。

h_k(Z’_eijc)表示：当输入参数Z’_eijc满足Z’_eijc∈Z’_eijc-max时，输出0，反之，输出无穷大，Z’_eijc-max表示每个关节可以到达角度的上限，为机械臂的固有参数；

当差值＜0.3x′_eijc-max，且h_k(Z’_eijc)＝0时，可行度

满足预设的可行度阈值。

其中，适应度:f计算如下：

S_pijc为优化指标，根据路径种群q’的实时信息计算，p＝1,2,3……L,L为优化指标的个数：k_p为人为设定的每个优化指标对应的权重，k_p＝0表示该项指标不参与优化。

本实施例中，L＝6，k₁取值为120，k₂取值为30，k₃取值为50,k₄取值为100，k₅取值为30，k₆取值为60,优化指标包括：

运动时间指标S_1ijc，用于衡量机器人的运动效率：

关节的平均加速度指标S_2ij，用于衡量机器人关节的能耗：

关节的平均波动指标S_3ij，用于衡量的是轨迹的平滑度:

关节平均力矩指标S_4ij:

关节平均消耗的能量指标S_5ij:

力矩指标S_6ij，用于衡量执行机构的平稳性:

其中，T为机械臂沿初始路径运动的总时间，A′_eijc为加速度的导数，加加速度。

实验表明：采用RRT*方法规划的初始路径长度为1576.8mm，机械臂单次运行时间3476ms，采用本方法优化后的路径长度为：1512.2mm，机械臂单次运行时间为3019ms；优化后的路径长度缩短，耗时短。

前面对本发明具体示例性实施方案所呈现的描述是出于说明和描述的目的。前面的描述并不想要成为毫无遗漏的，也不是想要把本发明限制为所公开的精确形式，显然，根据上述教导很多改变和变化都是可能的。选择示例性实施方案并进行描述是为了解释本发明的特定原理及其实际应用，从而使得本领域的其它技术人员能够实现并利用本发明的各种示例性实施方案及其不同选择形式和修改形式。本发明的范围旨在由所附权利要求书及其等价形式所限定。

Claims (10)

1.一种多自由度机械臂避障路径规划方法，其特征在于，包括如下步骤：

S1、采用机械臂路径规划方法对机械臂进行路径规划，得到初始路径，所述机械臂的关节数为m；

S2、所述机械臂沿初始路径运动，采集每个关节在每一预设时刻的实时信息；

所述实时信息包括：当前运动时刻t_ij、关节空间信息Z_ij、速度v_ij、加速度a_ij以及驱动力矩信息τ_ij，i＝1,2,3……m；j表示当前时刻，j＝1,2,3……n；n为预设时刻的总数；所述关节空间信息Z_ij为当前关节的角度信息；

S3、对所述初始路径中的实时信息进行数学建模并映射，得到包含多条路径信息的集合，单条路径信息包括基于预设的运动时间和各关节的角度计算得到的特定时刻各关节的速度、加速度和驱动力矩；

S4、记所述包含多条路径信息的集合为Q_ec，e对应第几条路径，c对应第几次迭代；e＝1,2,3……r，r为路径的总条数；c＝1,2,3……D，D为预设的迭代总数；Q_ec包含信息为：第c次迭代第e条路径第i个关节在第j时刻所对应的时间t′_eijc、角度Z′_eijc、速度v′_eijc、加速度a′_eijc以及驱动力矩信息τ′_eijc；

在Q_ec中随机选择q条路径，按预设的约束条件计算其对应Q_ec的可行度，当可行度不满足预设的可行度阈值时，剔除当前路径；遍历q条路径，将所有剩下的路径组成的集合记为q’；计算q’中各条路径的适应度，将适应度最大的路径标记为合格路径；

将Q_ec中合格路径剔除后的集合作为新的Q_ec，采用相同方法筛选合格路径，直至合格路径的数量达到G，将G条合格路径组成的集合标记为第二代路径集合Q′_ec；其中G＝10％～40％r；

S5、对第二代路径集合Q′_ec随机进行交叉处理，得到杂交后的路径，对杂交后的路径进行变异处理；同时，将发生变异的和未发生变异的路径融合；得到第三代路径集合Q″_ec，其包含的路径条数为90％～110％r；

S6、将得到的第三代路径集合Q″_ec作为步骤S4中的Q_ec，在其中筛选q条路径，再采用相同方法获取第二代路径集合Q′_ec，再利用步骤S5方法获取第三代路径集合Q″_ec,重复本步骤，直至迭代次数为D；将最后一次迭代中得到的第三代路径集合Q″_ec中适应度最大的路径作为最终目标路径。

2.如权利要求1所述多自由度机械臂避障路径规划方法，其特征在于：还包括S7、机械臂沿所述目标路径运动，进行碰撞检测，若机械臂与环境发生碰撞，则重新设置S4约束条件中的机器人可达工作空间，重新进行步骤S4。

3.如权利要求1所述多自由度机械臂避障路径规划方法，其特征在于：模型映射采用Logistic映射或McDill-amateis模型。

4.如权利要求3所述多自由度机械臂避障路径规划方法，其特征在于：模型映射采用Logistic映射，计算Logistic映射的输入参数：混沌变量β_j+1、路径空间波动范围Δt_j；

β_j+1＝μβ_j(1-β_j)

人为设定：初始混沌变量β₁，取值：0≤β₁≤1，混沌系数μ，取值：0≤μ≤4；β_j为所述初始路径上当前时刻的混沌变量，β_j+1所述初始路径上下一时刻的混沌变量；

和

为对应的Δt_j的变化区间，通过人为设定，用于限制每个路径种群Q_m中每个关节空间信息相对于初始路径中每个关节空间信息的波动范围。

5.如权利要求1所述多自由度机械臂避障路径规划方法，其特征在于：步骤S4中的q＝[10％～30％r]，其中r为c＝1时，Q_ec的路径总条数；

可行度

的计算如下：

x’_eijc为实时信息中的一项参数，min{0,g(x’_eijc)}表示：当输入参数x_eijc满足约束条件时，输出0，反之输出x′_eijc与约束条件中的预设上限值x′_eijc-max的差值；

h_k(Z’_eijc)表示：当输入参数Z’_eijc满足Z’_eijc∈Z’_eijc-max时，输出0，反之，输出无穷大，Z’_eijc-max表示每个关节可以到达角度的上限，为机械臂的固有参数；当所述差值＜0.2～0.5x′_eijc-max，且h_k(Z’_eijc)＝0时，可行度

满足预设的可行度阈值。

6.如权利要求1或5所述多自由度机械臂避障路径规划方法，其特征在于：所述约束条件为：根据机器人型号、运行要求，人为设置实时信息的上限值：v′_eijc-max＝1.2～1.5v_ij、a′_eijc-max＝1.2～1.5a_ij、τ′_eijc-max＝1.2～1.5τ_ij。

7.如权利要求4所述多自由度机械臂避障路径规划方法，其特征在于：步骤S4的适应度f计算如下：

S_pijc为优化指标，根据所述路径种群q’的实时信息计算，p＝1,2,3……L,L为优化指标的个数：k_p为人为设定的每个优化指标对应的权重，k_p＝0表示该项指标不参与优化。

8.如权利要求7所述多自由度机械臂避障路径规划方法，其特征在于：L＝6，所述优化指标包括：

运动时间指标S_1ijc，用于衡量机器人的运动效率：

关节的平均加速度指标S_2ij，用于衡量机器人关节的能耗：

关节的平均波动指标S_3ij，用于衡量的是轨迹的平滑度:

关节平均力矩指标S_4ij:

关节平均消耗的能量指标S_5ij:

力矩指标S_6ij，用于衡量执行机构的平稳性:

其中，T为机械臂沿初始路径运动的总时间，A′_eijc为加速度的导数，加加速度。

9.如权利要求1所述多自由度机械臂避障路径规划方法，其特征在于：所述预设时刻的设置方式为：设定时间间隔t,从初始路径的起始位置开始，每间隔时间t，采集当前时刻的实时信息；t＝0.5ms～4ms。

10.如权利要求1所述多自由度机械臂避障路径规划方法，其特征在于：步骤S1中利用深度相机获取机器人运动空间的环境模型、采用混合层次包围盒方法简化所述环境模型，采用机械臂路径规划方法对机械臂进行路径规划，得到初始路径；

所述机械臂路径规划方法包括：A*、D*、RRT*、PRM*、人工势场法、遗传算法、粒子群算法或蚁群算法。

Images