CN106182019A - 工业机器人抓取过程的动态避障系统及其方法 - Google Patents

Abstract

本发明提供了一种工业机器人抓取过程中的动态避障系统及其方法，包括：排斥向量模块、涡流算法模块、动态避障算法模块。排斥向量模块用于构建末端执行器和障碍物之间的相对运动关系，得到排斥向量；涡流算法模块用于根据涡流算法来更新排斥向量的方向；动态避障算法模块用于根据排斥向量得到末端执行器的位置，从而避开障碍物。本发明采用动态避障算法实现了在工业复杂环境中有运动障碍物的情况下，成功躲避障碍物完成抓取。

Description

工业机器人抓取过程的动态避障系统及其方法

技术领域

本发明涉及机器人系统的物体操作技术，具体涉及工业机器人抓取过程中的动态避障系统及其方法。

背景技术

传统的工业领域通常需要机器人在静态环境中重复完成一些任务，比如说装配、运输等，其中最为基本和重要的任务就是物体抓取。然而，当工业环境发生改变的时候，比如出现了一些动态的变化，那么机器人也要实时地做出相应的调整，当障碍物开始运动的时候，避开障碍物进行抓取就成了一项比较艰难的任务。为此，工业机器人抓取过程中的动态避障方法也就成为了当前工业环境中急需的一项关键技术。

对于在抓取过程中出现了运动障碍物的情况，就需要产生无碰撞的路径保证抓取动作的安全。一个实时的避免碰撞的方法通常包括以下三个方面：

(1)环境中障碍物的感知技术；

(2)避免碰撞的算法；

(3)机器人控制律。

目前，环境中障碍物的感知技术是比较成熟的，能够很快的检测环境中的障碍物，然而，寻求一个合理的躲避运动障碍物的算法还是一个具有挑战性的问题。这个问题的挑战性体现在：首先，需要找到一个构建机器人末端执行器和运动障碍物之间关系的方法；其次，在设计避免碰撞的算法时，需要考虑到当障碍物运动速度较快的时候，如果末端执行器躲避的方向不够合理的话，会造成最终依然相撞的结果；第三，设计一个合理的避障算法，得到末端执行器躲避障碍物时应当运动到的位置。为了保证工业机器人抓取过程中的动态避障，需要考虑到以上三点。

发明内容

本发明针对复杂非结构化环境中工业机器人的动态避障难题，提供一种工业机器人抓取过程的动态避障系统及其方法，以期能成功躲避碰撞区域内的运动障碍物，从而实现动态避障抓取。

本发明为达到上述发明目的，采用如下技术方案：

本发明一种工业机器人抓取过程的动态避障系统，是应用于由工业机器人、摄像头、实验台、待抓取物体和障碍物所组成的作业环境中，在所述实验台的一侧设置有所述工业机器人，在所述工业机器人的一侧设置有所述摄像头；所述摄像头的视场为所述工业机器人的作业范围；在所述实验台上设置有所述待抓取物；在所述工业机器人的作业范围内存在动态的障碍物；选取所述工业机器人的末端执行器为中心，并以ρ为半径的半球作为所述工业机器人的无碰撞区域；其特点是，所述动态避障系统包括：排斥向量模块、涡流算法模块、动态避障算法模块；

所述排斥向量模块根据所述障碍物在所述摄像头视场中的位置构建所述工业机器人的末端执行器与所述障碍物之间的距离向量；再根据所述无碰撞区域与所述相对位置关系得到所述工业机器人的末端执行器与所述障碍物之间的排斥向量；

所述涡流算法模块以所述末端执行器作为所述排斥向量的轴心，根据所述排斥向量在相邻时刻的变化，得到所述排斥向量的变化速度，以所述排斥向量的变化速度作为围绕所述排斥向量轴心的涡流；根据所述涡流更新所述排斥向量的方向；从而得到更新的排斥向量；

所述动态避障算法模块根据所述更新的排斥向量，计算所述末端执行器的位置，从而避开所述障碍物。

本发明一种工业机器人抓取过程的动态避障方法，是应用于由工业机器人、摄像头、实验台、待抓取物体和障碍物所组成的作业环境中，在所述实验台的一侧设置有所述工业机器人，在所述工业机器人的一侧设置有所述摄像头；所述摄像头的视场为所述工业机器人的作业范围；在所述实验台上设置有所述待抓取物；在所述工业机器人的作业范围内存在动态的障碍物；选取所述工业机器人的末端执行器为中心，并以ρ为半径的半球作为所述工业机器人的无碰撞区域；其特点是，所述动态避障方法是按如下步骤进行：

步骤1、对所述摄像头进行标定，得到所述摄像头坐标系F_c关于机器人基坐标系F_b的变换矩阵

步骤2、利用所述摄像头获取所述障碍物在所述摄像头坐标系F_c的变换矩阵从而利用式(1)得到所述末端执行器关于基坐标系的变换矩阵

$T_o^b=T_c^b\·T_o^c---\left(1\right)$

步骤3、利用式(2)获得所述工业机器人的末端执行器与所述障碍物之间的距离向量D_eo：

$D_{eo}={\[x_e^b-x_o^b,y_e^b-y_o^b,z_e^b-z_o^b\]}^T---\left(2\right)$

式(2)中分别表示所述障碍物在所述机器人基坐标系F_b中的位置；分别表示所述末端执行器在所述机器人基坐标系F_b中的位置；

步骤4、利用式(3)获得所述工业机器人的末端执行器与所述障碍物之间的排斥向量V_rep：

$V_{rep}=\left|\right|V_{rep}\left|\right|\frac{D_{eo}}{\left|\right|D_{eo}\left|\right|}---\left(3\right)$

式(3)中，||D_eo||表示所述距离向量D_eo的范数；||V_rep||表示排斥向量的范数，并有：

$\left|\right|V_{rep}\left|\right|\frac{V_{\max }}{1+e^{\left(2\right|\left|D_{eo}\right||/\mathrm{\ρ}-1)\mathrm{\μ}}}---\left(4\right)$

式(4)中，V_max表示所述末端执行器的最大线速度；μ表示大于零的常量；

步骤5、以所述末端执行器作为所述排斥向量V_rep的轴心，根据所述排斥向量V_rep在相邻时刻的变化，得到所述排斥向量V_rep的变化速度

步骤6、利用式(5)获得所述排斥向量V_rep与所述变化速度之间的夹角δ：

$\mathrm{\δ}=\mathrm{\π}/2-\arcsin \[{\left(\frac{{\stackrel{\·}{V}}_{rep}}{\left|\right|{\stackrel{\·}{V}}_{rep}\left|\right|}\right)}^T\frac{V_{rep}}{\left|\right|V_{rep}\left|\right|}\]---\left(5\right)$

式(5)中，表示所述变化速度的范数；

步骤7、更新所述排斥向量V_rep：

步骤7.1、判断所述夹角δ≥π/2是否成立，若成立，表示所述障碍物的运动方向远离所述末端执行器的中心，则不更新所述排斥向量V_rep；否则，执行步骤7.2；

步骤7.2、判断所述夹角δ＜π/2是否成立，若成立，表示所障碍物的运动方向靠近所述末端执行器的中心，则执行步骤7.3；否则，表示所述排斥向量V_rep和变化速度平行，即所述夹角δ＝0；则执行步骤7.5；

步骤7.3、建立坐标系(a,v,n)；其中，a表示所述变化速度的单位向量；n表示垂直于a-r平面的单位向量；r表示所述排斥向量的单位向量；v表示垂直于n-a平面的单位向量；以所述单位向量a、v和n的交点作为所述坐标系(a,v,n)的原点；

步骤7.4、利用式(6)获得更新后的排斥向量V′_rep：

$V_{rep}^{\′}=\left|\right|V_{rep}\left|\right|(cos\mathrm{\γ}\frac{{\stackrel{\·}{V}}_{rep}}{\left|\right|{\stackrel{\·}{V}}_{rep}\left|\right|}+sin\mathrm{\γ}\frac{V_{rep}}{\left|\right|V_{rep}\left|\right|})---\left(6\right)$

式(6)中，γ表示更新后的排斥向量V′_rep的角度；并有：

$\mathrm{\γ}=\frac{\mathrm{\π}/2+{\mathrm{\δe}}^{-\left(2\right|\left|{\stackrel{\·}{V}}_{rep}\right||/{\stackrel{\·}{V}}_{rmax}-1)c}}{1+e^{-\left(2\right|\left|{\stackrel{\·}{V}}_{rep}\right||/{\stackrel{\·}{V}}_{r\max }-1)c}}---\left(7\right)$

式(7)中，c表示大于零的常量；表示所设定的排斥向量变化速度的最大值；

步骤7.5、使所述单位向量a和单位向量r之间产生偏移角度从而获得更新的单位向量并代入步骤7.4中获得更新后的排斥向量V′_rep；

步骤7.6、以所述更新的排斥向量V′_rep作为所述末端执行器的期望线速度，计算获得在所述设定的间隔时间Δt内所述末端执行器的位置；

步骤7.7、根据所述工业机器人的运动学反解得到所述工业机器人的各个关节的转角，从而控制所述工业机器人运动到相应的位置；

步骤7.8、在所述工业机器人运动到相应的位置后，返回步骤2执行，直到所述障碍物不在所述无碰撞区域内，从而实现避开所述障碍物的功能。

本发明所述的工业机器人抓取过程的动态避障方法的特点也在于，所述步骤7.5中更新的单位向量是按如下过程获得：

步骤7.5.1、建立与所述机器人基坐标系F_b的三个轴的方向相同的坐标系O-XYZ，所述坐标系O-XYZ的原点O与所述坐标系(a,v,n)的原点相同；假设所述单位向量r在所述坐标系O-XYZ中的坐标位置为(r_x,r_y,r_z)；

步骤7.5.2、利用式(8)获得更新的单位向量与所述单位向量r分别在坐标系O-XYZ中X-Y平面上所形成的投影之间的夹角：

$\mathrm{\α}=\mathrm{\π}-2\arccos \frac{sin(\mathrm{\δ}/2)}{\left|\right|r_{x-y}\left|\right|}---\left(8\right)$

式(8)中，||r_x-y||表示所述单位向量r在X-Y平面上投影的范数；

步骤7.5.3、利用式(9)获得所述单位向量r在X-Y平面上的投影角度λ：

$\mathrm{\&lambda;}=\left\{\begin{array}{cc}-\mathrm{\&pi;}/2& r_x=0,r_y<0\\ \mathrm{\&pi;}/2& r_x=0,r_y>0\\ \arctan (r_y/r_x)& r_x>0\\ \mathrm{\&pi;}+\arctan (r_y/r_x)& r_x<0\end{array}\right.---\left(9\right)$

步骤7.5.4、利用式(10)获得更新的单位向量

$\hat{a}={\&lsqb;\left|\right|r_{x-y}\left|\right|cos(\mathrm{\&alpha;}+\mathrm{\&lambda;}),\left|\right|r_{x-y}\left|\right|sin(\mathrm{\&alpha;}+\mathrm{\&lambda;}),r_z\&rsqb;}^T---\left(10\right).$

与已有技术相比，本发明的有益效果体现在：

1、本发明采用了排斥向量和涡流算法，克服了动态障碍物在工业机器人的作业环境中运动时，出现干扰影响工业机器人的正常作业的情况，使得工业机器人能够有效的避开动态障碍物，继而完成抓取作业，实现了低成本、高精度、快速灵活的动态避障。

2、本发明构建末端执行器和障碍物之间的排斥向量，使得二者能产生排斥行为，从而确保了末端执行器能够远离动态障碍物。

3、本发明使用了涡流算法，用来更新上述排斥向量的方向，使得当动态障碍物朝向工业机器人末端执行器运动，且速度高于末端执行器速度时，末端执行器能够朝向更新的排斥向量方向运动，顺利的躲避动态障碍物，提高了躲避障碍物的准确性。

4、本发明采用了动态避障算法，基于上述使用涡流算法更新之后的排斥向量，得到工业机器人末端执行器实时的目标位置，利用运动学反解得到工业机器人各关节角，从而使得工业机器人运动到相应的目标位置，保证了末端执行器能实时躲避。

5、本发明利用了放置在机器人本体外的摄像头获取障碍物信息，解决了安装在机器人本体上会减小视场的问题，实现了更高精度的动态避障。

附图说明

图1是本发明动态避障方法的流程图；

图2是本发明排斥向量的示意图；

图3是本发明末端执行器无碰撞区域的示意图；

图4是本发明涡流算法模块更新排斥向量原理示意图；

图5是本发明实际抓取过程中末端执行器和障碍物的运动轨迹示意图。

具体实施方式

本实施方式提供的技术方案能够实现工业机器人在动态环境中的避障和抓取。一方面，由排斥向量和涡流算法得到机器人末端执行器和运动障碍物之间的相对运动关系以及排斥向量的更新方向；另一方面，采用动态避障算法这种高效的避障算法来求解得到末端执行器应该运动到的位置，成功地躲避障碍物。所以，需要构建末端执行器和障碍物之间的排斥向量，并且利用涡流算法更新排斥向量的方向，从而设计基于涡流算法的动态避障方法，以实现避开动态障碍物完成抓取。具体的说，

本实施例中，一种工业机器人抓取过程的动态避障系统，是应用于由工业机器人、摄像头、实验台、待抓取物体和障碍物所组成的作业环境中，在实验台的一侧设置有工业机器人，在工业机器人的一侧设置有摄像头；摄像头的视场为工业机器人的作业范围；在实验台上设置有待抓取物；在工业机器人的作业范围内存在动态的障碍物；选取工业机器人的末端执行器为中心，并以ρ为半径的半球作为工业机器人的无碰撞区域；动态避障系统包括：排斥向量模块、涡流算法模块、动态避障算法模块；

排斥向量模块根据障碍物在摄像头视场中的位置构建工业机器人的末端执行器与障碍物之间的距离向量；再根据无碰撞区域与相对位置关系得到工业机器人的末端执行器与障碍物之间的排斥向量；

涡流算法模块以末端执行器作为排斥向量的轴心，根据排斥向量在相邻时刻的变化，得到排斥向量的变化速度，以排斥向量的变化速度作为围绕排斥向量轴心的涡流；根据涡流更新排斥向量的方向；从而得到更新的排斥向量；

动态避障算法模块根据更新的排斥向量，计算末端执行器的位置，从而避开障碍物。

本实施例中，如图1所示，一种工业机器人抓取过程的动态避障方法，也应用于由工业机器人、摄像头、实验台、待抓取物体和障碍物所组成的作业环境中，并按如下步骤进行：

步骤1、对设置在工业机器人一侧的摄像头进行标定，在实验台放置有目标物一个，利用此摄像头识别定位得到目标物在此摄像头坐标系F_c中的变换矩阵再利用固定于工业机器人末端的摄像头识别定位得到目标物在工业机器人基坐标系F_b中的变换矩阵从而得到摄像头坐标系F_c关于机器人基坐标系F_b的变换矩阵机器人基坐标系、机器人末端执行器坐标系、摄像头坐标系和障碍物坐标系如图2所示，其原点分别为机器人基座中心、机器人末端执行器几何中心、摄像头光心和障碍物几何中心。

步骤2、利用摄像头获取障碍物在摄像头坐标系F_c的变换矩阵从而利用式(1)得到末端执行器关于基坐标系的变换矩阵

$T_o^b=T_c^b\&CenterDot;T_o^c---\left(1\right)$

步骤3、利用式(2)获得工业机器人的末端执行器与障碍物之间的距离向量D_eo：

$D_{eo}={\&lsqb;x_e^b-x_o^b,y_e^b-y_o^b,z_e^b-z_o^b\&rsqb;}^T---\left(2\right)$

对于动态避障，只需要考虑位置信息，不需要考虑姿态，故变换矩阵和可以表示成和式(2)中，分别表示障碍物在机器人基坐标系F_b中的位置；分别表示末端执行器在机器人基坐标系F_b中的位置；

步骤4、还需要定义一个无碰撞区域才能计算得到排斥向量，定义无碰撞区域为以末端执行器中心为球心，以ρ为半径的一个半球，如图3所示；利用式(3)获得工业机器人的末端执行器与障碍物之间的排斥向量V_rep：

$V_{rep}=\left|\right|V_{rep}\left|\right|\frac{D_{eo}}{\left|\right|D_{eo}\left|\right|}---\left(3\right)$

式(3)中，||D_eo||表示距离向量D_eo的范数；||V_rep||表示排斥向量的范数，并有：

$\left|\right|V_{rep}\left|\right|\frac{V_{\max }}{1+e^{\left(2\right|\left|D_{eo}\right||/\mathrm{\&rho;}-1)\mathrm{\&mu;}}}---\left(4\right)$

式(4)中，V_max表示末端执行器的最大线速度；μ表示大于零的常量。由以上两式可以看出，当障碍物在无碰撞区域外的时候，排斥向量的值||V_rep||为零；当障碍物离球心越近的时候，排斥向量的值越大；当障碍物位于球心的时候，排斥向量的值||V_rep||达到最大值；

步骤5、排斥向量并不能解决所有情况下的动态避障问题，比如，当障碍物快速向机器人末端执行器运动，且速度超过末端执行器的最大线速度的时候，如果末端执行器按照排斥向量的方向运动，那么最终仍然会发生碰撞。所以为了解决这个问题，引入了涡流算法，将当前新的排斥向量的方向修改为垂直于之前的排斥向量的方向，确保末端执行器沿着修改后的排斥向量运动，躲避障碍物。当障碍物在末端执行器周围运动的时候，考虑末端执行器作为排斥向量的轴心，根据排斥向量V_rep在相邻时刻的变化，得到排斥向量V_rep的变化速度将排斥向量的变化速度看作围绕此轴心的涡流；

步骤6、利用式(5)获得排斥向量V_rep与变化速度之间的夹角δ：

$\mathrm{\&delta;}=\mathrm{\&pi;}/2-\arcsin \&lsqb;{\left(\frac{{\stackrel{\&CenterDot;}{V}}_{rep}}{\left|\right|{\stackrel{\&CenterDot;}{V}}_{rep}\left|\right|}\right)}^T\frac{V_{rep}}{\left|\right|V_{rep}\left|\right|}\&rsqb;---\left(5\right)$

式(5)中，表示变化速度的范数；

步骤7、更新排斥向量V_rep：

步骤7.1、判断夹角δ≥π/2是否成立，若成立，表示障碍物的运动方向远离末端执行器的中心，则不更新排斥向量V_rep；否则，执行步骤7.2；

步骤7.2、判断夹角δ＜π/2是否成立，若成立，表示所障碍物的运动方向靠近末端执行器的中心，则执行步骤7.3；否则，表示排斥向量V_rep和变化速度平行，即夹角δ＝0；则执行步骤7.5；

步骤7.3、建立坐标系(a,v,n)；其中，a表示变化速度的单位向量；n表示垂直于a-r平面的单位向量；r表示排斥向量V_rep的单位向量；v表示垂直于n-a平面的单位向量；以单位向量a、v和n的交点作为坐标系(a,v,n)的原点；

步骤7.4、利用式(6)获得更新后的排斥向量V′_rep：

$V_{rep}^{\&prime;}=\left|\right|V_{rep}\left|\right|(cos\mathrm{\&gamma;}\frac{{\stackrel{\&CenterDot;}{V}}_{rep}}{\left|\right|{\stackrel{\&CenterDot;}{V}}_{rep}\left|\right|}+sin\mathrm{\&gamma;}\frac{V_{rep}}{\left|\right|V_{rep}\left|\right|})---\left(6\right)$

式(6)中，γ表示更新后的排斥向量V′_rep的角度；并有：

$\mathrm{\&gamma;}=\frac{\mathrm{\&pi;}/2+{\mathrm{\&delta;e}}^{-\left(2\right|\left|{\stackrel{\&CenterDot;}{V}}_{rep}\right||/{\stackrel{\&CenterDot;}{V}}_{rmax}-1)c}}{1+e^{-\left(2\right|\left|{\stackrel{\&CenterDot;}{V}}_{rep}\right||/{\stackrel{\&CenterDot;}{V}}_{r\max }-1)c}}---\left(7\right)$

式(7)中，c表示大于零的常量；表示所设定的排斥向量变化速度的最大值；当等于零时，γ等于δ，也即排斥向量的方向没有改变；当等于时，γ等于π/2，也即排斥向量的方向和v同向；当从零向变化时，其方向也从与V_rep同向变化到与v同向。

步骤7.5、使单位向量a和单位向量r之间产生偏移角度从而获得更新的单位向量并代入步骤7.4中获得更新后的排斥向量V′_rep；

步骤7.5.1、如图4所示，建立与机器人基坐标系F_b的三个轴的方向相同的坐标系O-XYZ，坐标系O-XYZ的原点O与坐标系(a,v,n)的原点相同；假设单位向量r在坐标系O-XYZ中的坐标位置为(r_x,r_y,r_z)；

步骤7.5.2、利用式(8)获得更新的单位向量与单位向量r分别在坐标系O-XYZ中X-Y平面上所形成的投影之间的夹角：

式(8)中，||r_x-y||表示单位向量r在X-Y平面上投影的范数；

步骤7.5.3、利用式(9)获得单位向量r在X-Y平面上的投影角度λ：

$\mathrm{\&lambda;}=\left\{\begin{array}{cc}-\mathrm{\&pi;}/2& r_x=0,r_y<0\\ \mathrm{\&pi;}/2& r_x=0,r_y>0\\ \arctan (r_y/r_x)& r_x>0\\ \mathrm{\&pi;}+\arctan (r_y/r_x)& r_x<0\end{array}\right.---\left(9\right)$

步骤7.5.4、利用式(10)获得更新的单位向量

$\hat{a}={\&lsqb;\left|\right|r_{x-y}\left|\right|cos(\mathrm{\&alpha;}+\mathrm{\&lambda;}),\left|\right|r_{x-y}\left|\right|sin(\mathrm{\&alpha;}+\mathrm{\&lambda;}),r_z\&rsqb;}^T---\left(10\right).$

若出现以下两种特殊情况，由式(10)分别可以得到以下两种结果；

(1)如果r位于x-y平面，也即r_z＝0，则α＝δ，且可得到新的单位向量

(2)如果r是在z轴上，也即将单位向量a在y-z上和单位向量r之间产生偏移角度则可得到在y-z平面上新的单位向量

步骤7.6、以更新的排斥向量V′_rep作为末端执行器的期望线速度，计算获得在设定的间隔时间Δt内末端执行器的位置；

步骤7.7、根据工业机器人的运动学反解得到工业机器人的各个关节的转角，从而控制工业机器人运动到相应的位置；

步骤7.8、在工业机器人运动到相应的位置后，返回步骤2执行，直到障碍物不在无碰撞区域内，从而实现避开障碍物的功能。

通过以上的几个步骤，得到障碍物和末端执行器之间的排斥向量，然后利用涡流算法更新排斥向量的方向，再由动态避障算法实时控制机器人运动到相应的位形，最终成功地避开障碍物。

图5是本具体实施方式提供的实际抓取过程中末端执行器和障碍物的运动轨迹示意图，其中，P₀到P₁为动态障碍物运动的轨迹，P₂到P₃为末端执行器躲避动态障碍物的运动轨迹，P₃到P₄为躲避障碍物之后抓取目标物的运动轨迹，P₄到P₅为抓取完成后向上举起的的轨迹。由图5可以看出，末端执行器是远离障碍物运动的，本发明所提出的工业机器人抓取过程中的动态避障方法是有效的。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明实施例揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。

Claims (3)

1.一种工业机器人抓取过程的动态避障系统，是应用于由工业机器人、摄像头、实验台、待抓取物体和障碍物所组成的作业环境中，在所述实验台的一侧设置有所述工业机器人，在所述工业机器人的一侧设置有所述摄像头；所述摄像头的视场为所述工业机器人的作业范围；在所述实验台上设置有所述待抓取物；在所述工业机器人的作业范围内存在动态的障碍物；选取所述工业机器人的末端执行器为中心，并以ρ为半径的半球作为所述工业机器人的无碰撞区域；其特征是，所述动态避障系统包括：排斥向量模块、涡流算法模块、动态避障算法模块；

所述排斥向量模块根据所述障碍物在所述摄像头视场中的位置构建所述工业机器人的末端执行器与所述障碍物之间的距离向量；再根据所述无碰撞区域与所述相对位置关系得到所述工业机器人的末端执行器与所述障碍物之间的排斥向量；

所述涡流算法模块以所述末端执行器作为所述排斥向量的轴心，根据所述排斥向量在相邻时刻的变化，得到所述排斥向量的变化速度，以所述排斥向量的变化速度作为围绕所述排斥向量轴心的涡流；根据所述涡流更新所述排斥向量的方向；从而得到更新的排斥向量；

所述动态避障算法模块根据所述更新的排斥向量，计算所述末端执行器的位置，从而避开所述障碍物。

2.一种工业机器人抓取过程的动态避障方法，是应用于由工业机器人、摄像头、实验台、待抓取物体和障碍物所组成的作业环境中，在所述实验台的一侧设置有所述工业机器人，在所述工业机器人的一侧设置有所述摄像头；所述摄像头的视场为所述工业机器人的作业范围；在所述实验台上设置有所述待抓取物；在所述工业机器人的作业范围内存在动态的障碍物；选取所述工业机器人的末端执行器为中心，并以ρ为半径的半球作为所述工业机器人的无碰撞区域；其特征是，所述动态避障方法是按如下步骤进行：

步骤1、对所述摄像头进行标定，得到所述摄像头坐标系F_c关于机器人基坐标系F_b的变换矩阵

步骤2、利用所述摄像头获取所述障碍物在所述摄像头坐标系F_c的变换矩阵从而利用式(1)得到所述末端执行器关于基坐标系的变换矩阵

$T_o^b=T_c^b\&CenterDot;T_o^c---\left(1\right)$

步骤3、利用式(2)获得所述工业机器人的末端执行器与所述障碍物之间的距离向量D_eo：

$D_{eo}={\&lsqb;x_e^b-x_o^b,y_e^b-y_o^b,z_e^b-z_o^b\&rsqb;}^T---\left(2\right)$

式(2)中，分别表示所述障碍物在所述机器人基坐标系F_b中的位置；分别表示所述末端执行器在所述机器人基坐标系F_b中的位置；

步骤4、利用式(3)获得所述工业机器人的末端执行器与所述障碍物之间的排斥向量V_rep：

$V_{rep}=\left|\right|V_{rep}\left|\right|\frac{D_{eo}}{\left|\right|D_{eo}\left|\right|}---\left(3\right)$

式(3)中，||D_eo||表示所述距离向量D_eo的范数；||V_rep||表示排斥向量的范数，并有：

$\left|\right|V_{rep}\left|\right|=\frac{V_{\max }}{1+e^{\left(2\right|\left|D_{eo}\right||/\mathrm{\&rho;}-1)\mathrm{\&mu;}}}---\left(4\right)$

式(4)中，V_max表示所述末端执行器的最大线速度；μ表示大于零的常量；

步骤5、以所述末端执行器作为所述排斥向量V_rep的轴心，根据所述排斥向量V_rep在相邻时刻的变化，得到所述排斥向量V_rep的变化速度

步骤6、利用式(5)获得所述排斥向量V_rep与所述变化速度之间的夹角δ：

$\mathrm{\&delta;}=\mathrm{\&pi;}/2-\arcsin \&lsqb;{\left(\frac{{\stackrel{\&CenterDot;}{V}}_{rep}}{\left|\right|{\stackrel{\&CenterDot;}{V}}_{rep}\left|\right|}\right)}^T\frac{V_{rep}}{\left|\right|V_{rep}\left|\right|}\&rsqb;---\left(5\right)$

式(5)中，表示所述变化速度的范数；

步骤7、更新所述排斥向量V_rep：

步骤7.1、判断所述夹角δ≥π/2是否成立，若成立，表示所述障碍物的运动方向远离所述末端执行器的中心，则不更新所述排斥向量V_rep；否则，执行步骤7.2；

步骤7.2、判断所述夹角δ＜π/2是否成立，若成立，表示所障碍物的运动方向靠近所述末端执行器的中心，则执行步骤7.3；否则，表示所述排斥向量V_rep和变化速度平行，即所述夹角δ＝0；则执行步骤7.5；

步骤7.3、建立坐标系(a,v,n)；其中，a表示所述变化速度的单位向量；n表示垂直于a-r平面的单位向量；r表示所述排斥向量的单位向量；v表示垂直于n-a平面的单位向量；以所述单位向量a、v和n的交点作为所述坐标系(a,v,n)的原点；

步骤7.4、利用式(6)获得更新后的排斥向量V′_rep：

$V_{rep}^{\&prime;}=\left|\right|V_{rep}\left|\right|(cos\mathrm{\&gamma;}\frac{{\stackrel{\&CenterDot;}{V}}_{rep}}{\left|\right|{\stackrel{\&CenterDot;}{V}}_{rep}\left|\right|}+sin\mathrm{\&gamma;}\frac{V_{rep}}{\left|\right|V_{rep}\left|\right|})---\left(6\right)$

式(6)中，γ表示更新后的排斥向量V′_rep的角度；并有：

$\mathrm{\&gamma;}=\frac{\mathrm{\&pi;}/2+{\mathrm{\&delta;e}}^{-\left(2\right|\left|{\stackrel{\&CenterDot;}{V}}_{rep}\right||/{\stackrel{\&CenterDot;}{V}}_{r\max }-1)c}}{1+e^{-\left(2\right|\left|{\stackrel{\&CenterDot;}{V}}_{rep}\right||/{\stackrel{\&CenterDot;}{V}}_{r\max }-1)c}}---\left(7\right)$

式(7)中，c表示大于零的常量；表示所设定的排斥向量变化速度的最大值；

步骤7.5、使所述单位向量a和单位向量r之间产生偏移角度从而获得更新的单位向量并代入步骤7.4中获得更新后的排斥向量V′_rep；

步骤7.6、以所述更新的排斥向量V′_rep作为所述末端执行器的期望线速度，计算获得在所述设定的间隔时间Δt内所述末端执行器的位置；

步骤7.7、根据所述工业机器人的运动学反解得到所述工业机器人的各个关节的转角，从而控制所述工业机器人运动到相应的位置；

步骤7.8、在所述工业机器人运动到相应的位置后，返回步骤2执行，直到所述障碍物不在所述无碰撞区域内，从而实现避开所述障碍物的功能。

3.根据权利要求2所述的工业机器人抓取过程的动态避障方法，其特征是，所述步骤7.5中更新的单位向量是按如下过程获得：

步骤7.5.1、建立与所述机器人基坐标系F_b的三个轴的方向相同的坐标系O-XYZ，所述坐标系O-XYZ的原点O与所述坐标系(a,v,n)的原点相同；假设所述单位向量r在所述坐标系O-XYZ中的坐标位置为(r_x,r_y,r_z)；

步骤7.5.2、利用式(8)获得更新的单位向量与所述单位向量r分别在坐标系O-XYZ中X-Y平面上所形成的投影之间的夹角：

$\mathrm{\&alpha;}=\mathrm{\&pi;}-2\arccos \frac{sin(\mathrm{\&delta;}/2)}{\left|\right|r_{x-y}\left|\right|}---\left(8\right)$

式(8)中，||r_x-y||表示所述单位向量r在X-Y平面上投影的范数；

步骤7.5.3、利用式(9)获得所述单位向量r在X-Y平面上的投影角度λ：

$\mathrm{\&lambda;}=\left\{\begin{array}{cc}-\mathrm{\&pi;}/2& r_x=0,r_y<0\\ \mathrm{\&pi;}/2& r_x=0,r_y>0\\ \arctan (r_y/r_x)& r_x>0\\ \mathrm{\&pi;}+\arctan (r_y/r_x)& r_x<0\end{array}\right.---\left(9\right)$

步骤7.5.4、利用式(10)获得更新的单位向量

$\hat{a}={\&lsqb;\left|\right|r_{x-y}\left|\right|cos(\mathrm{\&alpha;}+\mathrm{\&lambda;}),\left|\right|r_{x-y}\left|\right|sin(\mathrm{\&alpha;}+\mathrm{\&lambda;}),r_z\&rsqb;}^T---\left(10\right).$