CN102323822A - 一种避免工业机器人碰撞工人的方法 - Google Patents

Abstract

本发明公开了一种避免工业机器人碰撞工人的方法，该方法由监控计算机将视觉单元采集到的工作区域图像输入至工人运动特性识别单元中进行场景建模、区域分析和工人运状态势预测，之后，工人运动特性识别单元将据此生成的工人运动特性参数输入至安全防护区域计算单元中；安全防护区域计算单元根据工人运动特性参数检测工业机器人的当前运动特性，计算出工人的安全防护区域，并将其输入至工业机器人目标位置和速度调整单元中；工业机器人目标位置和速度调整单元根据安全防护区域计算出工业机器人的安全运动范围，判断工业机器人的每个关节的运动是否进入该安全防护区域，在线调整工业机器人的目标位置和速度，以避免工业机器人碰撞工人。

Description

一种避免工业机器人碰撞工人的方法

技术领域

本发明涉及一种避免工人和工业机器人在工作区域发生碰撞的方法，特别涉及一种通过判别工人的运动趋势，避免工业机器人碰撞工人的方法。

背景技术

工业机器人应用中存在的一个问题为：工业机器人需要在一个封闭的、隔离的工作区域内操作，为了避免工人意外遭到工业机器人的伤害，工人需要远离工业机器人的该工作区域。如何让工业机器人能够判别工人的运动趋势，避免碰撞工人，是使工业机器人和工人能够在同一个工作区域内协作完成任务的关键问题。

目前，已经提出了许多基于不同原理的防止碰撞的技术和方法，以避免工业机器人和同属一个工作区域的移动物体或是静止物体发生碰撞。现有方法主要是通过对工业机器人和物体的碰撞时间和距离的估计，确定工业机器人的停止时间。

例如，专利号为US 6,212,444的美国专利公开了一种通过设置公共区域防止碰撞的方法，在该公共区域内，工业机器人的操作区域和其它工业机器人或装置的操作区域彼此重叠。当该工业机器人上的预定参考点在公共区域内时，会向其它工业机器人或装置输出第一进入禁止信号，以禁止其它工业机器人或装置进入该公共区域；当其它工业机器人或装置的可移动部分在公共区域内时，又会向该工业机器人或装置输出第二进入禁止信号，以禁止该工业机器人或装置的预定参考点进入该公共区域。

公开号为CN 101512453A的中国专利申请公开了一种防止碰撞的方法，其通过估计工业机器人与物体之间在发生碰撞之前的剩余时间确定工业机器人的停止时间，将估计的发生碰撞之前的剩余时间与工业机器人的停止时间进行比较，以及在所估计的发生碰撞之前的剩余时间接近工业机器人的停止时间时，停止工业机器人或者物体。该种方法的不足之处是：工人在工作区域内的运动轨迹或运动趋势是无法预知的，因而难以估计工业机器人与工人之间在发生碰撞之前的剩余时间，也就无法通过估计工业机器人与物体之间在发生碰撞之前的剩余时间确定工业机器人的停止时间。

由于现有方法没有考虑工人的运动特性，如速度和方向等非确定性因素，因而，工人仍然存在和工业机器人发生碰撞的风险。

发明内容

为了解决上述问题，本发明以保证工人的安全性和增强工人运动的自由性为目标，提供一种通过判别工人的运动状态避免工业机器人碰撞工人的方法。

本发明采用的技术方案为：一种避免工业机器人碰撞工人的方法为，在工人正常进入工作区域后，完成如下步骤：

步骤a2：监控计算机通过视觉单元实时监控工人的运动状态，并将视觉单元采集到的工作区域图像输入至工人运动特性识别单元中计算工人运动特性参数，并由安全防护区域计算模块根据所述工人运动特性参数计算工人的安全防护区域，所述安全防护区域计算模块将获得的安全防护区域输入至工业机器人目标位置和速度调整单元中，其中，所述安全防护区域为工人在运动状态或静止状态保证其安全的最小区域；

步骤a3：安全防护区域计算模块根据工业机器人的预定的运动轨迹，计算其防护区域，并将其输入至工业机器人目标位置和速度调整单元中，其中，所述防护区域表示工业机器人的每个关节均不会碰撞到其它物体的空间区域；

步骤a4：当安全防护区域和防护区域有重叠部分时，表示工业机器人的关节存在碰撞工人的危险，则所述工业机器人目标位置和速度调整单元在线调整工业机器人各个关节的目标位置和速度，避免工业机器人的任何一个关节碰撞到工作区域内的工人。

优选地，在所述步骤a2之前完成如下步骤：

步骤a1：监控计算机通过视觉单元实时监测工作区域的边界，判断工人是否正常进入工作区域；

步骤a11：若工人没有进入工作区域，则监控计算机控制工业机器人按照预定的程序完成操作，并返回步骤a1；

步骤a12：若工人正常进入工作区域，则监控计算机转换为工人和工业机器人共同在工作区域内工作的协作工作模式，并进入步骤a2；

步骤a13：如果工人非正常进入工作区域，则监控计算机根据工人所进入的工作区域的防护等级，通过报警单元给出与防护等级相对应的报警信息，并返回步骤a1。

优选地，所述视觉单元包括安装于工作区域的周边上的传感器阵列，通过传感器阵列形成位于工作区域外、且位于工作区域周边的第一检测区域和位于工作区域内、且位于工作区域周边的第二检测区域；步骤a1中监控计算机通过视觉单元实时监测工作区域的边界的方法为：

步骤b1：监控计算机通过传感器阵列判断工人是否进入第一检测区域，如果位于第一检测区域上的传感器的第一检测信号受阻，则受控于第一检测信号的报警装置向工人给出语音提示，并将工人已进入第一检测区域的信息通过总线传达给监控计算机，并继续进入步骤b2；

步骤b2：监控计算机通过传感器阵列判断工人是否进入第二检测区域，如果位于第二检测区域上的传感器的第二检测信号受阻，则受控于第二检测信号的报警装置向工人给出警报提示，并将工人已进入第二检测区域的信息通过总线传达给监控计算机，并继续进入步骤b3；

步骤b3：监控计算机根据工业机器人的任务情况，判断工人是否按照需求正常进入工作区域；如果不是正常进入，控制工业机器人停止运动，并完成步骤a13；如果是正常进入，则完成步骤a12。

优选地，所述的工人运动特性识别单元计算工人运动特性参数的步骤为：

步骤c1：对输入的工作区域图像建立工作区域背景建模，获得工作区域背景模型；

步骤c2：检测工作区域背景模型中的工人运动变化，获得工人区域；

步骤c3：输入工人区域，根据预先假设的工人的预测位置分布，分析工人的当前位置分布，预测工人的运动趋势；

步骤c4：输入预测的工人的运动趋势，通过工人朝向的验证，确定工人的运动趋势，并对工人的下一位置和下一朝向进行预测。

优选地，工人的安全防护区域的确定方法为：为以当前位置为中心，以下一位置到当前位置的距离为半径，以当前朝向与下一朝向之间的夹角为圆心角的扇形底面所形成的高度为工人高度的柱形区域。

优选地，工业机器人地防护区域的确定方法为：

工业机器人的第j关节的防护区域S_j是以r_j为半径的球形区域；

$S_j=\left\{s_j\right|{(s_j-R_j\left(i\right))}^2<r_j^2\}$

其中，r_j＝|R_j(i+1)-R_j(i)|，s_j表示第j关节在空间中能到达的位置，R_j(i)表示第j关节的当前位置，R_j(i+1)表示第j关节的下一位置；

所述防护区域A2为工业机器人的所有关节的防护区域的集合：

A2＝S₁∪S₂∪...S_n

其中，n为工业机器人的关节的数量，n为自然数，1≤j≤n。

优选地，所述工业机器人目标位置和速度调整单元在线调整工业机器人运动轨迹的目标位置和速度的方法为：

步骤d1：判断第j关节的防护区域S_j与安全防护区域是否有重叠的部分，如果有，则调整第j关节的下一位置R_j(i+1)和在当前位置R_j(i)处的关节速度V_j(i)；

步骤d2：计算第j关节的当前位置R_j(i)和安全防护区域间的最短距离d_j(i)：

d_j(i)＝min|R_j(i)-p_k|

其中，p_k为安全防护区域的柱形区域的表面上的点；

步骤d3：调整工业机器人的第j关节的当前速度V_j(i)，新的当前速度为V′_j(i)：

$V_j^{\&prime;}\left(i\right)=\frac{d_j\left(i\right)}{T}$

其中，T为在步骤c4得到的工人的下一位置与当前位置在时间轴上的时间间隔；

步骤d4：调整工业机器人的第j关节的下一位置R_j(i+1)，新的下一位置为R′_j(i+1)：

R′_j(i+1)＝R_j(i)+d_j(i)。

本发明的有益之处在于：从保障工人的工作自由度和人身安全的角度出发，以工人为核心，实时调整工业机器人的运动，避免工业机器人危害工人。本发明能够增加工业机器人和工人协作完成任务可能性。

附图说明

图1示出了本发明的避免工业机器人碰撞工人的方法的一种实施方式；

图2示出了本发明的避免工业机器人碰撞工人的方法的流程图；

图3示出了监控计算机监测工作区域的边界的工作流程图；

图4示出了工人运动特性识别模块的工作流程图；

图5示出了工人的安全防护区域；

图6示出了工业机器人的防护区域；

图7示出了工业机器人运动轨迹目标位置和速度调整模块的工作流程图。

具体实施方式

下面结合附图对本发明的具体实施方式进行详细的说明。

本发明的避免工业机器人碰撞工人的方法主要应用了四个单元，分别为视觉单元、工人运动特性识别单元、安全防护区域计算单元和工业机器人目标位置和速度调整单元，该视觉单元包括图1中的内部监控摄像头3、外部监控摄像头5和传感器阵列4。图1中的工作区域1为工业机器人2完成某个任务时设定的场地范围，工业机器人2在工作区域1内形成其防护区域21，该防护区域21表示工人可能会被工业机器人的关节碰撞的空间区域，即工业机器人的每个关节均不会碰撞到其它物体的空间区域；内部监控摄像头3和外部监控摄像头5分别设置在工作区域1的内部和外部；传感器阵列4安装于工作区域1的周边上，通过传感器阵列4形成基本上位于工作区域1外、且位于工作区域1周边的第一检测区域41和基本上位于工作区域1内、且位于工作区域1周边的第二检测区域42。工人6在工作区域1内形成自己的安全防护区域61，该安全防护区域61表示工人在运动状态或静止状态保证其安全的最小区域。

该工业机器人2可以是六自由度的串联型工业机器人，当然也可以是其它类型的工业机器人；内部监控摄像头3和外部监控摄像头5可以是CCD摄像头，也可以是其它类型的摄像头，只要可以满足图像采集的要求即可；传感器阵列4可以由多个红外光幕式传感器构成，也可由其它类型的通过判断信号是否被物体遮挡以检测物体是否进入指定区域的传感器构成。

四个单元之间的连接关系为：

监控计算机将视觉单元(主要为其中的内部监控摄像头3)采集到的工作区域图像输入至工人运动特性识别单元中进行场景建模、区域分析和工人运状态势预测，之后，工人运动特性识别单元将据此生成的工人运动特性参数(工人的当前位置、当前运动速度和当前运动方向)输入至安全防护区域计算单元中。安全防护区域计算单元根据工人运动特性参数检测工业机器人的当前运动特性，计算出工人的安全防护区域61，并将其输入至工业机器人目标位置和速度调整单元中。工业机器人目标位置和速度调整单元根据安全防护区域61计算出工业机器人的安全运动范围，判断工业机器人的每个关节的运动是否进入该安全防护区域，在线调整工业机器人的目标位置和速度。

监控计算机可以工业计算机也可以是其它有图像、信号处理和计算功能的***。在此，该工人运动特性识别单元、安全防护区域计算单元和工业机器人目标位置和速度调整单元均可为通过监控计算机实现的处理单元，当然也可以通过独立于监控计算机、并与监控计算机通讯连接的处理器实现。

图2示出了本发明的避免工业机器人碰撞工人的方法的流程图，具体为：

步骤a1：监控计算机通过外部监控摄像头5和传感器阵列4实时监测工作区域1的边界，判断工人是否正常进入工作区域1；

步骤a11：若工人没有进入工作区域1，则监控计算机控制工业机器人按照预定的程序完成操作，并返回步骤a1；

步骤a12：若工人正常进入工作区域1，则监控计算机转换为工人和工业机器人共同在工作区域内工作的协作工作模式，并进入步骤a2；

步骤a13：如果工人非正常进入工作区域，则监控计算机根据工人所进入的工作区域的防护等级，通过报警单元给出与防护等级相对应的报警信息，并返回步骤a1；

步骤a2：监控计算机通过内部监控摄像头3实时监控工人的运动状态，并将内部监控摄像头3采集到的工作区域图像输入至工人运动特性识别单元中计算工人运动特性参数；安全防护区域计算模块再根据该工人运动特性参数计算工人的安全防护区域61，并将其输入至工业机器人目标位置和速度调整单元中；

步骤a3：安全防护区域计算模块根据工业机器人的预定的运动轨迹，计算其防护区域21，并将其输入至工业机器人目标位置和速度调整单元中；

步骤a4：当安全防护区域61和防护区域21有重叠部分时，表示工业机器人的关节存在碰撞工人的危险，则该工业机器人目标位置和速度调整单元在线调整工业机器人各个关节的目标位置和速度，避免工业机器人的任何一个关节碰撞到工作区域内的工人。

图3示出了步骤a1中监控计算机通过外部监控摄像头5和传感器阵列4实时监测工作区域1的边界的工作方法流程图，具体为：

步骤b1：监控计算机通过传感器阵列4判断工人是否进入第一检测区域41，如果位于第一检测区域上的传感器的第一检测信号受阻，则受控于第一检测信号的报警装置向工人给出语音提示，并将工人已进入第一检测区域的信息通过总线传达给监控计算机，并继续进入步骤b2；

步骤b2：监控计算机通过传感器阵列4判断工人是否进入第二检测区域，如果位于第二检测区域上的传感器的第二检测信号受阻，则受控于第二检测信号的报警装置向工人给出警报提示，并将工人已进入第二检测区域的信息通过总线传达给监控计算机，并继续进入步骤b3；

步骤b3：监控计算机根据工业机器人的任务情况，判断工人是否按照需求正常进入工作区域；如果不是正常进入，控制工业机器人停止运动，并完成步骤a13；如果是正常进入，则完成步骤a12。

上述的受控于第一检测信号的报警装置和受控于第二检测信号的报警装置可以为同一报警装置，该报警装置针对第一和第二检测信号产生不同的响应，当然，二者也可以为不同的报警装置。另外，该报警装置可以属于上述的报警单元，步骤a13中的报警信号可由该报警装置执行，优选为通过属于报警单元的其它报警装置执行。

图4示出了工人运动特性识别单元的工作流程图，具体为：

步骤c1：工作区域的背景建模

工人运动特性识别单元根据输入的工作区域图像(优选为视频图像)建立基于时间坐标系的高斯统计模型，然后对高斯统计模型进行去噪和平滑，得到工作区域的初始背景模型，然后采用贝叶斯方法对初始背景模型中的样本点进行实时计算更新，以消除复杂变化的背景及光照变化带来的影响，得到工作区域的背景模型。

步骤c2：分析目标(工人)区域

对工作区域的背景模型进行运动检测与背景减除，获得包含有工人的区域；对包含有工人的区域进行噪声剔除、区域融合与平滑和形态学处理，最终获得工人区域。

步骤c3：分析目标(工人)

工人运动特性识别单元根据获得的工人区域和已预先储存于其中的工人的预测位置分布，分析工人的当前位置分布，利用树形结构分类器判断工人的朝向(朝向机器人的正面还是侧面，即是否与工业机器人正对)。

步骤c4：预测目标(工人)的运动趋势

对工人的每一时刻的图像进行卡尔曼滤波，同时通过工人朝向的验证，确定工人的运动趋势，并对工人的下一位置和朝向进行预测，其中，目标工人的下一位置与当前位置在时间轴上的时间间隔为T。

在步骤c4中主要通过卡尔曼和粒子滤波结合的方法建立工人运动轨迹预测模型，并通过该预测模型判断工人的下一位置和下一朝向，具体为：

首先，根据工人的工作任务和状态建立工人的初始运动模型(即预测位置分布)，在工人运动初期，基于该初始运动模型采用卡拉曼预测方法对工人的下一位置和下一朝向进行预测和判断；

然后，通过对已获取的连续帧中工人的位置的学习和建模，得出工人运动的轨迹和运动参数(即分析工人的当前位置分布)，修正卡尔曼预测的模型参数，并据此判断工人的下一位置和下一朝向；

最后，为处理目标工人运动的不确定性和非线性问题，引入粒子滤波算法对卡尔曼预测的工人的位置分布进行重要性概率采样，通过视频测量函数和目标权重均值计算，得到下一帧目标最可能的区域以及运动方向。

如图5所示，工人的安全防护区域61为以当前位置P(i)为中心，以下一位置P(i+1)到当前位置P(i)的距离R为半径，以当前朝向O(i)与下一朝向O(i+1)之间的夹角θ为圆心角的扇形底面所形成的高度为工人高度H的柱形区域。

图6示出了工业机器人的防护区域21，其中，

工业机器人的第j关节的防护区域S_j是以为半径r_j的球形区域：

$S_j=\left\{s_j\right|{(s_j-R_j\left(i\right))}^2<r_j^2\}$

其中，r_j＝|R_j(i+1)-R_j(i)|，s_j表示第j关节在空间中能到达的位置，R_j(i)表示第j关节的当前位置，R_j(i+1)表示第j关节的下一位置，即下一时刻的位置。

防护区域A2(21)为工业机器人的所有关节的防护区域的集合(并集)，即：

A2＝S₁∪S₂∪...S_n

其中，S₁为第一关节的防护区域，S₂为第二关节的防护区域，S_n为第n关节的防护区域，其中，n为自然数，表示工业机器人关节的数量，1≤j≤n。如图6中所示的工业机器人具有三个关节，即n等于3，各自的防护区域分别为S₁、S₂和S₃。

图7为工业机器人目标位置和速度调整单元在线调整工业机器人运动轨迹的目标位置和速度的工作方法流程图，具体为：

步骤d1：判断第j关节的防护区域S_j与安全防护区域61是否有重叠的部分，如果有，则调整第j关节的下一位置R_j(i+1)和在当前位置R_j(i)处的关节速度V_j(i)。

步骤d2：计算第j关节的当前位置R_j(i)和安全防护区域61间的最短距离d_j(i)：

d_j(i)＝min|R_j(i)-p_k|

其中，p_k为安全防护区域的柱形区域的表面上的点。

步骤d3：调整工业机器人的第j关节的当前速度V_j(i)，新的当前速度为V′_j(i)：

$V_j^{\&prime;}\left(i\right)=\frac{d_j\left(i\right)}{T}$

步骤d4：调整工业机器人的第j关节的下一位置R_j(i+1)，新的下一位置为R′_j(i+1)：

R′_j(i+1)＝R_j(i)+d_j(i)

监控计算机负责将新的目标位置(即新的下一位置)和新的当前速度发送给工业机器人的控制器，并由控制器控制工业机器人完成新的动作。

综上所述仅为本发明较佳的实施例，并非用来限定本发明的实施范围。即凡依本发明申请专利范围的内容所作的等效变化及修饰，皆应属于本发明的技术范畴。

Claims (9)

1.一种避免工业机器人碰撞工人的方法，其特征在于，在工人正常进入工作区域后，完成如下步骤：

步骤a2：监控计算机通过视觉单元实时监控工人的运动状态，并将视觉单元采集到的工作区域图像输入至工人运动特性识别单元中计算工人运动特性参数，并由安全防护区域计算模块根据所述工人运动特性参数计算工人的安全防护区域，所述安全防护区域计算模块将获得的安全防护区域输入至工业机器人目标位置和速度调整单元中，其中，所述安全防护区域为工人在运动状态或静止状态保证其安全的最小区域；

步骤a3：安全防护区域计算模块根据工业机器人的预定的运动轨迹，计算其防护区域，并将其输入至工业机器人目标位置和速度调整单元中，其中，所述防护区域表示工业机器人的每个关节均不会碰撞到其它物体的空间区域；

步骤a4：当安全防护区域和防护区域有重叠部分时，表示工业机器人的关节存在碰撞工人的危险，则所述工业机器人目标位置和速度调整单元在线调整工业机器人各个关节的目标位置和速度，避免工业机器人的任何一个关节碰撞到工作区域内的工人。

2.根据权利要求1所述的避免工业机器人碰撞工人的方法，其特征在于，在所述步骤a2之前完成如下步骤：

步骤a1：监控计算机通过视觉单元实时监测工作区域的边界，判断工人是否正常进入工作区域；

步骤all：若工人没有进入工作区域，则监控计算机控制工业机器人按照预定的程序完成操作，并返回步骤a1；

步骤a12：若工人正常进入工作区域，则监控计算机转换为工人和工业机器人共同在工作区域内工作的协作工作模式，并进入步骤a2；

步骤a13：如果工人非正常进入工作区域，则监控计算机根据工人所进入的工作区域的防护等级，通过报警单元给出与防护等级相对应的报警信息，并返回步骤a1。

3.根据权利要求2所述的避免工业机器人碰撞工人的方法，其特征在于，所述视觉单元包括安装于工作区域的周边上的传感器阵列，通过传感器阵列形成位于工作区域外、且位于工作区域周边的第一检测区域和位于工作区域内、且位于工作区域周边的第二检测区域；步骤a1中监控计算机通过视觉单元实时监测工作区域的边界的方法为：

步骤b1：监控计算机通过传感器阵列判断工人是否进入第一检测区域，如果位于第一检测区域上的传感器的第一检测信号受阻，则受控于第一检测信号的报警装置向工人给出语音提示，并将工人已进入第一检测区域的信息通过总线传达给监控计算机，并继续进入步骤b2；

步骤b2：监控计算机通过传感器阵列判断工人是否进入第二检测区域，如果位于第二检测区域上的传感器的第二检测信号受阻，则受控于第二检测信号的报警装置向工人给出警报提示，并将工人已进入第二检测区域的信息通过总线传达给监控计算机，并继续进入步骤b3；

步骤b3：监控计算机根据工业机器人的任务情况，判断工人是否按照需求正常进入工作区域；如果不是正常进入，控制工业机器人停止运动，并完成步骤a13；如果是正常进入，则完成步骤a12。

4.根据权利要求3所述的避免工业机器人碰撞工人的方法，其特征在于，所述视觉单元还包括安装于工作区域外的外部监测摄像头，在步骤a1中，监控计算机通过所述外部监测摄像头和传感器阵列实时监控工作区域的边界。

5.根据权利要求1所述的避免工业机器人碰撞工人的方法，其特征在于，所述视觉单元包括安装于工作区域内的内部监测摄像头，在步骤a2中，监控计算机通过所述内部监测摄像头实时监控工人的运动状态。

6.根据权利要求1至5中任一项所述的避免工业机器人碰撞工人的方法，其特征在于，所述的工人运动特性识别单元计算工人运动特性参数的步骤为：

步骤c1：对输入的工作区域图像建立工作区域背景建模，获得工作区域背景模型；

步骤c2：检测工作区域背景模型中的工人运动变化，获得工人区域；

步骤c3：输入工人区域，根据预先假设的工人的预测位置分布，分析工人的当前位置分布，预测工人的运动趋势；

步骤c4：输入预测的工人的运动趋势，通过工人朝向的验证，确定工人的运动趋势，并对工人的下一位置和下一朝向进行预测。

7.根据权利要求6所述的一种通过判别工人的运动状态以避免工业机器人碰撞工人的方法，其特征在于，工人的安全防护区域的确定方法为：为以当前位置为中心，以下一位置到当前位置的距离为半径，以当前朝向与下一朝向之间的夹角为圆心角的扇形底面所形成的高度为工人高度的柱形区域。

8.根据权利要求7所述的一种避免工业机器人碰撞工人的方法，其特征在于，工业机器人地防护区域的确定方法为：

工业机器人的第j关节的防护区域S_j是以r_j为半径的球形区域；

$S_j=\left\{s_j\right|{(s_j-R_j\left(i\right))}^2<r_j^2\}$

其中，r_j＝|R_j(i+1)-R_j(i)|，s_j表示第j关节在空间中能到达的位置，R_j(i)表示第j关节的当前位置，R_j(i+1)表示第j关节的下一位置；

所述防护区域A2为工业机器人的所有关节的防护区域的集合：

A2＝S₁∪S₂∪...S_n

其中，n为工业机器人的关节的数量，n为自然数，1≤j≤n。

9.根据权利要求8所述的一种避免工业机器人碰撞工人的方法，其特征在于，所述工业机器人目标位置和速度调整单元在线调整工业机器人运动轨迹的目标位置和速度的方法为：

步骤d1：判断第j关节的防护区域S_j与安全防护区域是否有重叠的部分，如果有，则调整第j关节的下一位置R_j(i+1)和在当前位置R_j(i)处的关节速度V_j(i)；

步骤d2：计算第j关节的当前位置R_j(i)和安全防护区域间的最短距离d_j(i)：

d_j(i)＝min|R_j(i)-p_k|

其中，p_k为安全防护区域的柱形区域的表面上的点；

步骤d3：调整工业机器人的第j关节的当前速度V_j(i)，新的当前速度为V′_j(i)：

$V_j^{\&prime;}\left(i\right)=\frac{d_j\left(i\right)}{T}$

其中，T为在步骤c4得到的工人的下一位置与当前位置在时间轴上的时间间隔；

步骤d4：调整工业机器人的第j关节的下一位置R_j(i+1)，新的下一位置为R′_j(i+1)：

R′_j(i+1)＝R_j(i)+d_j(i)。

Images