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

技术特征：

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

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

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

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

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

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

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

${}_o{}^{b}T={}_c{}^{b}T\·{}_o{}^{c}T---\left(1\right)$

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

$D_{eo}={\[{}_e{}^{b}x-{}_o{}^{b}x,{}_e{}^{b}y-{}_o{}^{b}y,{}_e{}^{b}z-{}_o{}^{b}z\]}^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}}_{r\max }-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执行，直到所述障碍物不在所述无碰撞区域内，从而实现避开所述障碍物的功能。

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{\α}=\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{\λ}=\left\{\begin{array}{cc}-\mathrm{\π}/2& r_x=0,r_y\<0\\ \mathrm{\π}/2& r_x=0,r_y\>0\\ \arctan (r_y/r_x)& r_x\>0\\ \mathrm{\π}+\arctan (r_y/r_x)& r_x\<0\end{array}\right.---\left(9\right)$

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

$\hat{a}={\[\left|\right|r_{x-y}\left|\right|cos(\mathrm{\α}+\mathrm{\λ}),\left|\right|r_{x-y}\left|\right|sin(\mathrm{\α}+\mathrm{\λ}),r_z\]}^T---\left(10\right).$