CN105894120A - 一种基于姿态控制的鞋底喷胶路径的规划方法 - Google Patents

Abstract

本发明一种基于姿态控制的鞋底喷胶路径的规划方法，主要包括机器人、激光轮廓测量仪、胶枪及工控机，通过激光轮廓测量仪扫描鞋底，得到鞋底曲面的三维点云，数据处理后进行喷胶路径规划，在工控机上生成控制程序导入机器人，根据不同的鞋型控制机器人末端胶枪的姿态，完成鞋底的喷胶工序，能够使胶枪姿态跟随鞋底轮廓拟合曲线的斜率进行变化，可以根据不同的鞋型控制机器人的末端胶枪姿态，且由于本发明的喷胶路径经过平滑处理，能够减少机器人电机换向的次数，提高机器人的运行速度，从而提高喷胶效率，并延长机器人的工作寿命。

Description

一种基于姿态控制的鞋底喷胶路径的规划方法

技术领域

本发明设计一种基于姿态控制的鞋底喷胶路径的规划方法。

背景技术

在机器人鞋底喷胶的工序中，由于鞋底为各种型号的不规则曲面，其喷胶路径需要根据不同的型号的鞋底进行自适应规划。目前的鞋底喷胶路径的规划，主要是根据鞋底轮廓获取的，由于鞋底轮廓的不规则性，鞋底喷胶路径存在不平滑的缺点，对喷胶的连续性造成影响；另外，喷胶路径规划中缺少对机器人末端的胶枪进行姿态控制，则机器人末端的胶枪的不适当姿态会造成喷胶不均匀及不完整。

发明内容

本发明的目的在于提供一种基于姿态控制的鞋底喷胶路径的规划方法，能根据各种不同的鞋型，对鞋底喷胶路径进行规划，控制机器人末端的胶枪姿态，使其跟随鞋底轮廓拟合曲线的斜率进行变化，快速、完整地完成机器人鞋底喷胶。

本发明一种基于姿态控制的鞋底喷胶路径的规划方法，具体包括如下步骤：

步骤1、通过激光轮廓测量仪扫描得到鞋底曲面信息，根据曲面信息提取鞋底轮廓曲线，对鞋底轮廓曲线进行偏置规划，生成喷胶轨迹线：

步骤2、对步骤1得到的喷胶轨迹曲线采用分段曲线拟合法进行平滑处理，即在喷胶轨迹曲线的每段区间上进行局部最小二乘法拟合：

步骤3、根据喷胶轨迹曲线的斜率控制机器人末端胶枪的姿态，结合机器人喷胶时的运动模型，通过机器人逆运动学逆解得到机器人在喷胶过程中每个关节转动的序列角度；

步骤4、依据步骤3得到的机器人在喷胶过程中每个关节转动的序列角度,在工控机上生成控制程序导入机器人，根据不同的鞋型控制机器人的末端胶枪姿态，完成鞋底的喷胶工序。

该步骤1具体为：通过激光轮廓测量仪扫描得到由一组三维点云数据构成的鞋底曲面信息，将该鞋底三维点云数据投影到XOY平面上，提取鞋底最外圈轮廓曲线，该XOY平面上的鞋底轮廓曲线以参数形式表示为r(u)＝(x(u)，y(u))，其参数形式的偏置曲线为：其中，d是偏置的距离，N(u)为单位法向量，对轮廓曲线r(u)上的每个数据点及两个相邻点进行分段处理，近似求得轮廓曲线r(u)上每个数据点对应的法线，设置偏置的距离d，由式(1)得到偏置的喷胶轨迹曲线。

该步骤3具体为：建立机器人各个连杆坐标系，得到机器人连杆参数，建立坐标系{i}相对于坐标系{i-1}的变换，从而得到机器人末端胶枪相对于基础坐标系的变换矩阵T_6,0：

$T_{6,0}=T_{1,0}{T}_{2,1}{T}_{3,2}{T}_{4,3}{T}_{5,4}{T}_{6,5}=\left[\begin{array}{cccc}N& O& A& P\\ 0& 0& 0& 1\end{array}\right]---\left(2\right)$

式中O为规定安装有胶枪的法兰的自身方向的矢量，A为所述法兰接近工件的矢量，N为由方向矢量O和接近矢量A并结合右手法则得到的机器人末端所述法兰的法线矢量，P为所述法兰中心点的位置矢量,则机器人变换矩阵表示为：

$T_{6,0}=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]---\left(3\right)$

在鞋底喷胶的过程中，使胶枪在鞋底的投影始终垂直于喷胶轨迹曲线，结合拟合后的喷胶轨迹曲线的方程，确定机器人变换矩阵T_6,0，根据机器人运动学方程逆解得到式(4)，相应得到机器人关节1到关节6转动的角度θ₁至θ₆：

$\left\{\begin{array}{c}{\mathrm{\θ}}_1=\arctan \frac{p_y}{p_x}\\ {\mathrm{\θ}}_3=\arctan \frac{a_3}{d_4}-\arctan \frac{K}{\sqrt{{a_3}^2+{d_4}^2-K^2}}\\ {\mathrm{\θ}}_2=\arctan \frac{(-a_3-a_2{c}_3)p_z-(s_1{p}_y+c_1{p}_x)(d_4-a_2{s}_3)}{(a_2{s}_3-d_4)p_z-(a_3+a_2{c}_3)(s_1{p}_y+c_1{p}_x)}-{\mathrm{\θ}}_3\\ {\mathrm{\θ}}_4=\arctan \frac{-a_x{s}_1+a_y{c}_1}{-a_x{c}_1{c}_{23}-a_y{s}_1{c}_{23}+a_z{s}_{23}}\\ {\mathrm{\θ}}_5=\arctan \frac{a_x(c_1{c}_{23}{c}_4+s_1{s}_4)+a_y(s_1{c}_{23}{c}_4-c_1{s}_4)-a_z{s}_{23}{c}_4}{a_x{c}_1{s}_{23}+a_y{s}_1{s}_{23}+a_z{c}_{23}}\\ {\mathrm{\θ}}_6=\arctan \frac{s_6}{c_6}\end{array}\right.---\left(4\right)$

其中：

s₆＝-n_x(c₁c₂₃s₄-s₁c₄)-n_y(s₁c₂₃s₄+c₁c₄)-n_zs₂₃s₄，

c₆＝n_x[(c₁c₂₃c₄+s₁s₄)c₅-c₁s₂₃s₅]+n_y[(s₁c₂₃c₄-c₁s₄)c₅-s₁s₂₃s₅]a_xc₁s₂₃-n_z(s₂₃c₄c₅+c₂₃s₅)

上述s_j为sinθ_j的简写，c_j为cosθ_j的简写，j＝1、2，…，6，s₂₃为sin(θ₂+θ₃)的简写，c₂₃为cos(θ₂+θ₃)的简写。

本发明能够使胶枪姿态跟随鞋底轮廓拟合曲线的斜率进行变化，可以根据不同的鞋型控制机器人的末端胶枪姿态，且由于本发明的喷胶路径经过平滑处理，能够减少机器人电机换向的次数，提高机器人的运行速度，从而提高喷胶效率，并延长机器人的工作寿命。

附图说明

图1为本发明的流程框图；

图2为本发明中安川Motoman-MH5F机器人连杆坐标系示意图；

图3为本发明中机器人连杆参数；

图4为实施例中激光轮廓测量仪扫描得到鞋底三维点云数据；

图5为实施例中提取的鞋底的外圈轮廓曲线；

图6为实施例中的鞋底喷胶轨迹曲线；

图7为实施例中的平滑处理后的鞋底喷胶轨迹曲线；

图8为实施例中机器人各关节转动的角度。

以下结合附图和实施例对本发明作进一步详述。

具体实施方式

本发明一种基于姿态控制的鞋底喷胶路径的规划方法，主要包括机器人、激光轮廓测量仪、胶枪及工控机，通过激光轮廓测量仪扫描鞋底，得到鞋底曲面的三维点云，数据处理后进行喷胶路径规划，在工控机上生成控制程序导入机器人，根据不同的鞋型控制机器人末端胶枪的姿态，完成鞋底的喷胶工序，如图1所示，具体包括如下步骤：

步骤1、通过激光轮廓测量仪扫描得到鞋底曲面信息，根据曲面信息提取鞋底轮廓曲线，对鞋底轮廓曲线进行偏置规划，生成喷胶轨迹线：

通过激光轮廓测量仪扫描得到的鞋底曲面信息，是由一组三维点云构成的数据，将该鞋底三维点云数据投影到XOY平面上，提取鞋底最外圈轮廓曲线，该XOY平面上的鞋底轮廓曲线以参数形式表示为r(u)＝(x(u)，y(u))，其参数形式的偏置曲线为：其中，d是偏置的距离，N(u)为单位法向量，对轮廓曲线r(u)上的每个数据点及两个相邻点进行分段处理，近似求得轮廓曲线r(u)上每个数据点对应的法线，设置偏置的距离d，由式(1)得到偏置的喷胶轨迹曲线；

步骤2、对步骤1得到的喷胶轨迹曲线采用分段曲线拟合法进行平滑处理，即在喷胶轨迹曲线的每段区间上进行局部最小二乘法拟合：

根据鞋底外圈轮廓曲线偏置得到的喷胶轨迹曲线斜率变化剧烈，这会导致机器人喷胶过程中需要剧烈变换电机的转向，会降低机器人的运行速度，并对机器人的寿命产生影响，本发明采用最小二乘法多项式拟合的方法对喷胶轨迹曲线进行平滑处理，由于鞋底喷胶轨迹曲线形状不规则且数据点较多，拟合的多项式阶数太低，拟合精度和效果不太理想。要提高拟合精度和效果就需要提高曲线阶数，但阶数太高又带来计算上的复杂性及其他方面的不利。因此，采用分段曲线拟合，在每段区间上进行局部最小二乘法拟合。

步骤3、根据喷胶轨迹曲线的斜率控制机器人末端胶枪的姿态，结合机器人喷胶时的运动模型(姿态)，通过机器人逆运动学逆解得到机器人在喷胶过程中每个关节转动的序列角度；

在鞋底喷胶过程中，机器人末端胶枪的姿态只有跟随喷胶轨迹曲线斜率的改变而改变，才能保证鞋底的完整均匀喷胶。本发明中的胶枪安装在6自由度的工业机器人上，以安川Motoman-MH5F机械臂为例阐述胶枪姿态的控制方法，如图2建立机器人各个连杆坐标系，机器人连杆参数如图3所示。安川Motoman-MH5F机械臂有6个自由度，关节1绕基础坐标系{0}中的Z轴转动，关节2、3、5绕X轴转动，关节4、6绕Y轴转动。

建立坐标系{i}相对于坐标系{i-1}的变换，从而得到机器人末端胶枪相对于基础坐标系的变换矩阵T_6,0：

$T_{6,0}=T_{1,0}{T}_{2,1}{T}_{3,2}{T}_{4,3}{T}_{5,4}{T}_{6,5}=\left[\begin{array}{cccc}N& O& A& P\\ 0& 0& 0& 1\end{array}\right]---\left(2\right)$

式中O为规定安装有胶枪的法兰的自身方向的矢量，A为所述法兰接近工件的矢量，N为由方向矢量O和接近矢量A并结合右手法则得到的机器人末端所述法兰的法线矢量，P为所述法兰中心点的位置矢量,则机器人变换矩阵表示为：

$T_{6,0}=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]---\left(3\right)$

在鞋底喷胶的过程中，使胶枪在鞋底的投影始终垂直于喷胶轨迹曲线，结合拟合后的喷胶轨迹曲线的方程，确定机器人变换矩阵T_6,0，根据机器人运动学方程逆解得到式(4)，相应得到机器人关节1到关节6转动的角度θ₁至θ₆：

$\left\{\begin{array}{c}{\mathrm{\θ}}_1=\arctan \frac{p_y}{p_x}\\ {\mathrm{\θ}}_3=\arctan \frac{a_3}{d_4}-\arctan \frac{K}{\sqrt{{a_3}^2+{d_4}^2-K^2}}\\ {\mathrm{\θ}}_2=\arctan \frac{(-a_3-a_2{c}_3)p_z-(s_1{p}_y+c_1{p}_x)(d_4-a_2{s}_3)}{(a_2{s}_3-d_4)p_z-(a_3+a_2{c}_3)(s_1{p}_y+c_1{p}_x)}-{\mathrm{\θ}}_3\\ {\mathrm{\θ}}_4=\arctan \frac{-a_x{s}_1+a_y{c}_1}{-a_x{c}_1{c}_{23}-a_y{s}_1{c}_{23}+a_z{s}_{23}}\\ {\mathrm{\θ}}_5=\arctan \frac{a_x(c_1{c}_{23}{c}_4+s_1{s}_4)+a_y(s_1{c}_{23}{c}_4-c_1{s}_4)-a_z{s}_{23}{c}_4}{a_x{c}_1{s}_{23}+a_y{s}_1{s}_{23}+a_z{c}_{23}}\\ {\mathrm{\θ}}_6=\arctan \frac{s_6}{c_6}\end{array}\right.---\left(4\right)$

其中：

s₆＝-n_x(c₁c₂₃s₄-s₁c₄)-n_y(s₁c₂₃s₄+c₁c₄)-n_zs₂₃s₄，

c₆＝n_x[(c₁c₂₃c₄+s₁s₄)c₅-c₁s₂₃s₅]+n_y[(s₁c₂₃c₄-c₁s₄)c₅-s₁s₂₃s₅]a_xc₁s₂₃-n_z(s₂₃c₄c₅+c₂₃s₅)

上述s_j为sinθ_j的简写，c_j为cosθ_j的简写，j＝1、2，…，6，s₂₃为sin(θ₂+θ₃)的简写，c₂₃为cos(θ₂+θ₃)的简写；

步骤4、依据步骤3得到的机器人在喷胶过程中每个关节转动的序列角度,在工控机上生成控制程序导入机器人，根据不同的鞋型控制机器人的末端胶枪姿态，完成鞋底的喷胶工序。

实施例

激光轮廓测量仪扫描得到鞋底三维点云数据，见图4，并对三维点云数据进行处理，提取出鞋底的外圈轮廓曲线，如图5所示，以鞋底的外圈轮廓曲线为基础进行偏置规划，根据胶枪的喷胶量及鞋底的轮廓宽度，设置偏置的距离为d＝5mm(向内法线偏置)，偏置次数为4次，由式(1)得到的鞋底喷胶轨迹曲线，如图6所示；采用二阶多项式拟合的方法对鞋底喷胶轨迹曲线进行平滑处理，分别以4圈喷胶轨迹为对象，将每圈轨迹曲线近似十等分，拟合后得到的喷胶轨迹曲线如图7所示，喷胶轨迹曲线相对于拟合前的曲线，曲线斜率变化平缓。为了判断拟合程度的好坏，计算拟合数据和原始数据对应点的均方根误差：式中f(x_i)为拟合数据，y_i为原始数据，根据式(5)计算得到的均方根误差RMSE＝0.0049，说明曲线的拟合程度较好。取鞋底最外圈喷胶轨迹，控制机器人的末端胶枪沿着喷胶轨迹运动。在拟合轨迹曲线的基础上，计算得到曲线的斜率，结合机器人空间坐标变换求得机器人位姿矩阵T_6,0，代入式(4)中，逆解得到机器人关节1到关节6转过的角度θ₁至θ₆，如图8所示，从图8可以看出机器人六个关节转动角度呈现连续性变化，避免机器人电机频繁换向，实现快速喷胶，保证机器人的工作寿命。

以上所述，仅是本发明较佳实施例而已，并非对本发明的技术范围作任何限制，故凡是依据本发明的技术实质对以上实施例所作的任何细微修改、等同变化与修饰，均仍属于本发明技术方案的范围内。

Claims (3)

1.一种基于姿态控制的鞋底喷胶路径的规划方法，其特征在于包括如下步骤：

步骤1、通过激光轮廓测量仪扫描得到鞋底曲面信息，根据曲面信息提取鞋底轮廓曲线，对鞋底轮廓曲线进行偏置规划，生成喷胶轨迹线：

步骤2、对步骤1得到的喷胶轨迹曲线采用分段曲线拟合法进行平滑处理，即在喷胶轨迹曲线的每段区间上进行局部最小二乘法拟合：

步骤3、根据喷胶轨迹曲线的斜率控制机器人末端胶枪的姿态，结合机器人喷胶时的运动模型，通过机器人逆运动学逆解得到机器人在喷胶过程中每个关节转动的序列角度；

步骤4、依据步骤3得到的机器人在喷胶过程中每个关节转动的序列角度,在工控机上生成控制程序导入机器人，根据不同的鞋型控制机器人的末端胶枪姿态，完成鞋底的喷胶工序。

2.根据权利要求1所述的一种基于姿态控制的鞋底喷胶路径的规划方法，其特征在于该步骤1具体为：通过激光轮廓测量仪扫描得到由一组三维点云数据构成的鞋底曲面信息，将该鞋底三维点云数据投影到XOY平面上，提取鞋底最外圈轮廓曲线，该XOY平面上的鞋底轮廓曲线以参数形式表示为r(u)＝(x(u)，y(u))，其参数形式的偏置曲线为：其中，d是偏置的距离，N(u)为单位法向量，对轮廓曲线r(u)上的每个数据点及两个相邻点进行分段处理，近似求得轮廓曲线r(u)上每个数据点对应的法线，设置偏置的距离d，由式(1)得到偏置的喷胶轨迹曲线。

3.根据权利要求1所述的一种基于姿态控制的鞋底喷胶路径的规划方法，其特征在于该步骤3具体为：

建立机器人各个连杆坐标系，得到机器人连杆参数，建立坐标系{i}相对于坐标系{i-1}的变换，从而得到机器人末端胶枪相对于基础坐标系的变换矩阵T_6,0：

$T_{6,0}=T_{1,0}{T}_{2,1}{T}_{3,2}{T}_{4,3}{T}_{5,4}{T}_{6,5}=\left[\begin{array}{cccc}N& O& A& P\\ 0& 0& 0& 1\end{array}\right]---\left(2\right)$

式中O为规定安装有胶枪的法兰的自身方向的矢量，A为所述法兰接近工件的矢量，N为由方向矢量O和接近矢量A并结合右手法则得到的机器人末端所述法兰的法线矢量，P为所述法兰中心点的位置矢量,则机器人变换矩阵表示为：

$T_{6,0}=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]---\left(3\right)$

在鞋底喷胶的过程中，使胶枪在鞋底的投影始终垂直于喷胶轨迹曲线，结合拟合后的喷胶轨迹曲线的方程，确定机器人变换矩阵T_6,0，根据机器人运动学方程逆解得到式(4)，相应得到机器人关节1到关节6转动的角度θ₁至θ₆：

$\left\{\begin{array}{c}{\mathrm{\θ}}_1=\arctan \frac{p_y}{p_x}\\ {\mathrm{\θ}}_3=\arctan \frac{a_3}{d_4}-\arctan \frac{K}{\sqrt{{a_3}^2+{d_4}^2-K^2}}\\ {\mathrm{\θ}}_2=\arctan \frac{\left(-a_3-a_2{c}_3\right)p_z-\left(s_1{p}_y+c_1{p}_x\right)\left(d_4-a_2{s}_3\right)}{\left(a_2{s}_3-d_4\right)p_z-\left(a_3+a_2{c}_3\right)\left(s_1{p}_y+c_1{p}_x\right)}-{\mathrm{\θ}}_3\\ {\mathrm{\θ}}_4=\arctan \frac{-a_x{s}_1+a_y{c}_1}{-a_x{c}_1{c}_{23}-a_y{s}_1{c}_{23}+a_z{s}_{23}}\\ {\mathrm{\θ}}_5=\arctan \frac{a_x\left(c_1{c}_{23}{c}_4+s_1{s}_4\right)+a_y\left(s_1{c}_{23}{c}_4-c_1{s}_4\right)-a_z{s}_{23}{c}_4}{a_x{c}_1{s}_{23}+a_y{s}_1{s}_{23}+a_z{c}_{23}}\\ {\mathrm{\θ}}_6=\arctan \frac{s_6}{c_6}\end{array}\right.---\left(4\right)$

其中：

s₆＝-n_x(c₁c₂₃s₄-s₁c₄)-n_y(s₁c₂₃s₄+c₁c₄)-n_zs₂₃s₄，

c₆＝n_x[(c₁c₂₃c₄+s₁s₄)c₅-c₁s₂₃s₅]+n_y[(s₁c₂₃c₄-c₁s₄)c₅-s₁s₂₃s₅]a_xc₁s₂₃-n_z(s₂₃c₄c₅+c₂₃s₅)

上述s_j为sinθ_j的简写，c_j为cosθ_j的简写，j＝1、2，…，6，s₂₃为sin(θ₂+θ₃)的简写，c₂₃为cos(θ₂+θ₃)的简写。