CN103495977B - 一种6r型工业机器人负载识别方法 - Google Patents

Abstract

本发明公开了一种6R型工业机器人负载识别方法，首先锁定所有的关节以使6R型工业机器人的结构固定，然后对这种结构中的连杆进行分析，找出力和力矩对于各个连杆坐标系的平衡关系，最后为了保持机器人连杆系统的静态平衡，计算出需要对各关节依次施加多大的力矩，通过这种方法求出为了使末端执行器支承住某个负载所需的一组关节驱动力矩。识别未知负载时，分别在6R型工业机器人空载和附加有未知负载时，利用6R型工业机器人各个关节的伺服电机控制器得到6R型工业机器人的各个关节驱动力矩，进而计算出负载。本发明的优点为：不需要额外附加力和力矩传感器来测量机器人的关节力和关节驱动力矩，并无需预先知道机器人各个连杆的质量和质心等惯性参数。

Description

一种6R型工业机器人负载识别方法

技术领域

本发明属于机器人技术领域，具体来说，是一种6R型工业机器人负载识别的方法。

背景技术

随着科技的进步和经济的发展，机器人技术不断进步，工业机器人的应用领域越来越广泛。在工业生产中，工业机器人广泛应用于搬运、焊接、喷漆、装配等领域。当工业机器人作用于不同的负载时，由于机器人驱动装置的输出功率与负载不匹配，不可避免的要引起振动，导致机器人的自身磨损并造成操作误差，降低机器人作业的精确性与安全性。随着工业机器人向着更加轻质结构的方向发展，机器人负载对执行机构所需的驱动力矩的贡献越来越大，对机器人的振动及精度的影响也越来越大，因此有必要实时识别机器人作用的负载，以调整工业机器人的驱动装置的各个参数，控制机器人输出功率与负载的相匹配。

机器人负载识别就是利用先进的测量手段和基于模型的参数识别方法辨识出机器人所作用的负载的质量，为调整机器人的各项参数与负载匹配做准备。目前对机器人负载识别的研究比较少。Paul^[1]提出了两种在机械臂处于静止状态时确定负载质量的方法，一种方法需要知道关节力矩的信息，另一种方法需要知道腕部的力和力矩的信息。Coiffe^t[2]利用关节力矩传感估计处于静止状态的机器人的负载质量和质心。Mukerjee^[3]和Mukerjee与Ballard^[4]利用传感器在动力学的基础上对机械臂负载进行识别，在识别过程中允许进行一般的运动。Olsen与Bekey^[5]假定所有的力和力矩传感都在腕部来识别负载。Christopher G.Atkeson^[6]在机器人动力学的基础上利用力和力矩传感器分别对机器人进行了动态和静态的负载识别，并且发现静态识别结果优于动态识别的结果。JAN SWEVERS和WALTER VERDONCK^[7]基于机器人动力学模型，提出了一种基于周期激励的工业机器人有效载荷的识别方法。虽然上述研究都对机器人负载进行了识别，但这些研究都基于附加额外的力和力矩传感器来测得机器人的关节力和力矩，并且要以识别机器人的动态模型各个参数为先验条件。从实际应用的角度来说，采用附加传感器进行辨识并不是比较好的方法，因为对机器人来说，增加额外的传感器不但成本昂贵，而且会额外增加机器人本身的重量，会对识别造成一定的误差，有时候也是无法实现的。

参考文献[1]：Paul,R.P.Robot manipulators:mathematics,programming,andcontrol.Cambridge:MIT Press,1981；

参考文献[2]：Coiffet,P.Robot technology:interaction with the environment,vol.2.Englewood Cliffs,N.J.:Prentice-Hall,1983；

参考文献[3]：Mukerjee,A.Adaptation in biological sensory-motor systems:amodel for robotic control.Proc.SPIE Conf.on Intelligent Robots and ComputerVision,Vol.521.Cambridge,Mass,1984；

参考文献[4]：Mukerjee,A,and Ballard,D.H.Self-calibration in robotmanipulators.Proc.IEEE Conf.Robotics and Automation pp.1050-1057,Mar1985；

参考文献[5]：Olsen,H.B.,and Bekey,G.A.Identification of parameters inmodels of robots with rotary joints.Proc.IEEE Conf.Robotics and Automation,pp.1045-1050,Mar1985；

参考文献[6]：C.G.Atkeson,C.H.An and J.M.Hollerbach,Estimation of inertialparameters of manipulator loads and links,Int.J.of Robotics Research,1986,Vol.5(3),pp.101-119；

参考文献[7]：J.Swevers,W.VerdonckE,and J.D.Schutter.Dynamic ModelIdentification for Industrial Robots.IEEE CONTROL SYSTEMS MAGAZINE,pp.58-71,October2007。

发明内容

为了解决上述问题，针对以上情况，本发明从工业生产的实用角度出发，对以6R型工业机器人模型为基础，通过静力学分析，提出了一种6R型工业机器人负载识别方法。

一种6R型工业机器人负载识别方法，其特征在于：通过下述步骤实现：

步骤1：根据D-H方法，建立6R型工业机器人的连杆坐标系；

6R型工业机器人是指全部关节为转动关节的6自由度工业机器人；6R型工业机器人可视为由连杆和关节组成。在6R型工业机器人的连杆中，将固定基座设为连杆0，然后由基座一侧到末端执行器一侧的各个可动连杆按照1、2、3、4、5、6的顺序依次编号，由基座一侧到末端执行器一侧的各个关节也按照1、2、3、4、5、6的顺序进行编号。为了描述每个连杆与相邻连杆之间的相对位置关系，需要在每个连杆上固定一个连杆坐标系，根据连杆坐标系所在的连杆的编号对连杆坐标系命名，因此固定在连杆i上的连杆坐标系为坐标系{i}，坐标系{i}为O_i（x_i,y_i,z_i），i=0、1、2、3、4、5、6；则根据D-H方法建立连杆坐标系的具体方法如下：

A、找出各个关节的关节轴，以相邻两个关节轴i与i+1的交点，或关节轴i与i+1的公垂线与关节轴i的交点作为连杆坐标系{i}的原点O_i，i=1、2、3、4、5；连杆坐标系{i}的z_i轴沿关节轴i的指向；当关节轴i与i+1的公垂线与关节轴i的交点作为连杆坐标系{i}的原点O_i时，连杆坐标系{i}的x_i轴沿关节轴i与i+1公垂线的指向；当关节轴i与i+1的交点与关节轴i的交点作为连杆坐标系{i}的原点O_i时，连杆坐标系{i}的x_i轴垂直于关节轴i与i+1所在的平面；连杆坐标系{i}的y_i轴则可按照右手原则确定，使y_i＝z_i×x_i；而连杆6作为6R型工业机器人的末端连杆，连杆坐标系{6}的原点O₆以及x₆轴的方向可任意选取；连杆坐标系{6}的z₆轴同样沿关节轴6的指向；连杆坐标系{6}的y₆轴是按右手原则确定。

B、建立固定于6R型工业机器人基座上的连杆坐标系{0}；

当6R型工业机器人中关节1的关节变量为0时，规定连杆坐标系{0}与连杆坐标系{1}重合；

步骤2：获得6R型工业机器人中相邻连杆间连杆坐标系{i}相对于连杆坐标系{i-1}的齐次变换矩阵；

连杆坐标系{i}相对于连杆坐标系{i-1}的变换，是由绕x_i-1旋转α_i-1角、再沿着旋转后坐标系的x_i-1轴平移a_i-1、再绕平移后的坐标系的Z_i-1轴旋转θ_i角、最后再沿旋转后的坐标系的Z_i-1轴平移d_i合成的，i=1、2、3、4、5、6；根据步骤1中建立的6R型工业机器人的连杆坐标系，得到6R型工业机器人中相邻连杆坐标系间的齐次变换矩阵为：

$\begin{array}{c}T_i^{i-1}={\mathrm{Rot}}_x\left({\mathrm{\α}}_{i-1}\right){\mathrm{Trans}}_x\left({\mathrm{\α}}_{i-1}\right){\mathrm{Rot}}_z\left({\mathrm{\θ}}_i\right){\mathrm{Trans}}_z\left(d_i\right)\\ =\left[\begin{array}{cccc}{\cos \mathrm{\θ}}_i& -{\sin \mathrm{\θ}}_i& 0& a_{i-1}\\ {\sin \mathrm{\θ}}_i{\cos \mathrm{\α}}_{i-1}& {\cos \mathrm{\θ}}_i{\mathrm{c\; os\α}}_{i-1}& {-\sin \mathrm{\α}}_{i-1}& {-\sin \mathrm{\α}}_{i-1}{d}_i\\ {\sin \mathrm{\θ}}_i{\sin \mathrm{\α}}_{i-1}& {\cos \mathrm{\θ}}_i{\sin \mathrm{\α}}_{i-1}& {\cos \mathrm{\α}}_{i-1}& {\cos }_{i-1}{d}_i\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}& & & \\ & R_i^{i-1}& & P_i^{i-1}\\ & & & \\ 0& 0& 0& 1\end{array}\right]\end{array}---\left(1\right)$

式（1）中，i=1、2、3、4、5、6；α_i-1、a_i-1、θ_i、d_i为6R型工业机器人的连杆参数，α_i-1为连杆转角，表示绕x_i-1轴，从z_i-1轴旋转到z_i轴的角度；a_i-1为连杆长度，表示沿x_i-1轴，从z_i-1轴移动到z_i轴的距离；θ_i为关节角，表示绕z_i轴，从x_i-1轴旋转到x_i轴的角度；d_i为连杆偏距，表示沿z_i轴，从x_i-1轴移动到x_i轴的距离；在以上定义的4个连杆参数中，对于旋转关节，θ_i表示关节动作的关节变量；^i- _i ¹R为连杆坐标系{i}相对于连杆坐标系{i-1}的旋转矩阵，^i-1P_i为连杆坐标系{i}相对于坐标系{i-1}的平移矩阵；

对6R型工业机器人进行正向运动学求解可以得到机器人末端连杆坐标系相对于基座连杆坐标系的齐次变换矩阵：

⁰T₆＝⁰T₁ ¹T₂ ²T₃ ³T₄ ⁴T₅ ⁵T₆                   （2）

步骤3：建立6R型工业机器人中各连杆的力和力矩平衡方程；

以6R型工业机器人中一个连杆i为对象进行静力分析，当连杆i处于平衡状态时，所受合力为零，连杆i的力和力矩平衡方程为：

$\left\{\begin{array}{c}f_i^i-f_{i+1}^i+m_i^{i}g=0\\ M_i^i-M_{i+1}^i-P_{i+1}^i\×f_{i+1}^i+P_{\mathrm{ci}}^i\×m_i^{i}g=0\end{array}\right.---\left(3\right)$

其中，i=1、2、3、4、5；f_i为连杆i-1施加在连杆i上的力，ⁱf_i为f_i在坐标系{i}中的矢量；M_i为连杆i-1施加在连杆i上的力矩，ⁱM_i为M_i在坐标系{i}中的矢量；m_ig为连杆i的重力，ⁱm_ig为坐标系{i}中连杆i的重力矢量，_g为重力加速度；P_ci为连杆i的质心位置，ⁱP_ci为连杆i的质心在坐标系{i}中的位置矢量；ⁱP_i+1为坐标系{i+1}相对于坐标系{i}的平移矩阵；

为便于从高编号连杆向低编号连杆进行迭代求解，用坐标系{i+1}和坐标系{0}相对于坐标系{i}的旋转矩阵进行变换，对式（3）进行整理，得到相邻连杆间的静力“传递”表达式：

$\left\{\begin{array}{c}f_i^i=f_{i+1}^i-m_i^{i}g=R_{i+1}^{i+1}{}_i{f}_{i+1}-\mathrm{Rm}_i^0{}_{i}g\\ M_i^i=R_{i+1}^{i+1}{}_i{M}_{i+1}+P_{i+1}^i\×\left(R_{i+1}^{i+1}{}_i{f}_{i+1}\right)-P_{\mathrm{ci}}^i\×\left(\mathrm{Rm}_i^0{}_{i}g\right)\end{array}\right.---\left(4\right)$

其中，为坐标系{i+1}相对于坐标系{i}的旋转矩阵；为坐标系{0}相对于坐标系{i}的旋转矩阵；

对于连杆6来说，连杆6的重力为m₆g，包含末端执行器的重力；设末端执行器所抓持的负载为mg；通过坐标系{0}与坐标系{6}之间的坐标变换矩阵，得到连杆6在坐标系{6}中的重力矢量为末端执行器所抓持的负载在坐标系{6}中的重力矢量为 即为坐标系{6}相对于坐标系{0}的旋转矩阵；设末端执行器所抓持的负载在坐标系{6}中的质心偏移量为l，则连杆6的力和力矩平衡方程为：

$\left\{\begin{array}{c}f_6^6+\left(\mathrm{Rmg}_6^0\right)+\left(\mathrm{Rm}_6^6{}_{0}g\right)=0\\ M_6^6+(\left(\mathrm{Rmg}_6^0\right)\×l\left(\mathrm{Rm}_6^6{}_{0}g\right)\×P_{c6}^6)=0\end{array}\right.---\left(5\right)$

其中，⁶f₆为在坐标系{6}中连杆5施加在连杆6上的力；⁶M₆为在坐标系{6}中连杆5施加在连杆6上的力矩；⁶P_c6为连杆6的质心在坐标系{6}中的位置矢量；

将式（5）代入式（4），从连杆6坐标系{6}到坐标系{1}依次进行迭代计算，得到作用于各个连杆上的力和力矩；

步骤4：得到6R型工业机器人中保持各连杆平衡的关节驱动力矩；

各个连杆对应的关节驱动力矩τ_i为：

τ_i=ⁱM_i·ⁱz_i=ⁱM_i ^Tiz_i             （6）

式（6）中，i=1、2、3、4、5、6；ⁱz_i为坐标系{i}中关节轴i的z_i矢量；

则由式（1）和式（6）可得：

τ_i＝X_img+Y_i                    （7）

其中，符号X_i为关节i的驱动力矩τ_i中与负载mg有关的系数，X_i与6R型工业机器人的D-H参数及位姿有关；Y_i表示τ_i中与各连杆重力有关、与负载mg无关的项；

当6R型工业机器人在某一位姿下空载时，根据式（7）可得到机器人空载时关节i的驱动力矩τ_i空为：

τ_i空＝Y_i             （8）

在6R型工业机器人所处的位姿不改变的情况下，当在机器人的末端执行器上附加负载mg时，根据式（7）可得到机器人附加负载时关节i的驱动力矩τ_i载为：

τ_i载=X_img+Y_i                 （9）

根据式（8）和（9）得到6R型工业机器人在末端执行器附加有负载mg时与机器人末端执行器空载时关节i的驱动力矩的差值Δτ_i为：

Δτ_i=τ_i载-τ_i空=X_img              （10）

则根据式（10）可以得到负载质量m为：

$m=\frac{{\mathrm{\Δ\τ}}_i}{X_{i}g}---\left(11\right)$

从式（11）可以看出，6R型工业机器人负载的质量与连杆的质量、质心等惯性参数无关；

上述Δτ_i通过下述方法得到：

6R型工业机器人的每个关节分别由一个伺服电机来驱动；设驱动电机的输出转矩为τ_M，减速比为i_M，传动效率为η_M；设关节i处的驱动电机的输出转矩为τ_Mi，减速比为i_Mi，传动效率为η_Mi，i=1、2、3、4、5、6；则根据电机拖动原理，机器人关节i的驱动力矩τ_i折算到电机轴上的输出转矩τ_Mi为：则有：τ_i=τ_Mii_Miη_Mi，由式（10）可得到：

Δτ_i=τ_Mi载i_Miη_Mi-τ_Mi空i_Miη_Mi=Δτ_Mii_Miη_Mi     （12）

其中，Δτ_Mi表示机器人在末端执行器附加有负载mg时与机器人末端执行器空载时关节i的驱动电机的输出转矩的差值；τ_Mi载为6R型工业机器人的末端执行器上附加负载mg时，关节i的驱动电机的输出转矩；τ_Mi空为6R型工业机器人的末端执行器空载时，关节i的驱动电机的输出转矩；

由式（11）和（12），得到负载的质量为：

$m=\frac{{\mathrm{\Δ\τ}}_{\mathrm{Mi}}{i}_{\mathrm{Mi}}{\mathrm{\η}}_{\mathrm{Mi}}}{X_{i}g}---\left(13\right)$

其中，驱动电机的输出转矩可以从电机的控制器上直接得到，无需附加额外的力和力矩传感器来测量，减速比与传动效率均可从设备型号上得知，系数X_i可由机器人D-H参数及机器人位姿计算得到；

由此通过式（13）可分别得到6个负载质量值；因此针对不同型号不同参数的6R型工业机器人，通过验证已知负载的实际质量与通过上述方法识别得到的6个负载质量值进行对比，选取最接近实际负载质量值所对应的关节的数据作为当前型号6R型工业机器人后续再次进行未知负载识别运算时所应用的数据。

本发明的优点在于：

1、本发明负载识别方法简单，能够快速、有效地识别负载质量；

2、本发明负载识别方法实用，不需要附加额外的传感器，并无需预先识别6R型工业机器人的各个惯性参数。

附图说明

图1为本发明中6R型工业机器人负载识别的实施步骤；

图2为本发明中6R型工业机器人结构示意图；

图3为本发明中6R型工业机器人连杆坐标系；

图4为6R型工业机器人中相邻连杆之间的作用力与作用力矩的受力分析和平衡关系示意图。

具体实施方式

下面将结合附图对本实用新型作进一步的详细说明。

本发明一种6R型工业机器人负载识别方法，如图1所示，通过下述步骤实现：

步骤1：根据D-H方法，建立6R型工业机器人的连杆坐标系；

如图2所示，6R型工业机器人是指全部关节为转动关节的6自由度工业机器人；6R型工业机器人可视为由连杆和关节组成。在6R型工业机器人的连杆中，将固定基座设为连杆0，然后由基座一侧到末端执行器一侧的各个可动连杆按照1、2、3、4、5、6的顺序依次编号，由基座一侧到末端执行器一侧的各个关节也按照1、2、3、4、5、6的顺序进行编号。为了描述每个连杆与相邻连杆之间的相对位置关系，需要在每个连杆上固定一个连杆坐标系，根据连杆坐标系所在的连杆的编号对连杆坐标系命名，因此固定在连杆i上的连杆坐标系为坐标系{i}，坐标系{i}为O_i（x_i,y_i,z_i），i=0、1、2、3、4、5、6；则根据D-H方法建立连杆坐标系的具体方法如下：

A、找出各个关节的关节轴，以相邻两个关节轴i与i+1的交点，或关节轴i与i+1的公垂线与关节轴i的交点作为连杆坐标系{i}的原点O_i，i=1、2、3、4、5；连杆坐标系{i}的z_i轴沿关节轴i的指向；当关节轴i与i+1的公垂线与关节轴i的交点作为连杆坐标系{i}的原点O_i时，连杆坐标系{i}的x_i轴沿关节轴i与i+1公垂线的指向；当关节轴i与i+1的交点与关节轴i的交点作为连杆坐标系{i}的原点O_i时，连杆坐标系{i}的x_i轴垂直于关节轴i与i+1所在的平面；连杆坐标系{i}的y_i轴则可按照右手原则确定，使y_i＝z_i×x_i；而连杆6作为6R型工业机器人的末端连杆，连杆坐标系{6}的原点O₆以及x₆轴的方向可任意选取；连杆坐标系{6}的z₆轴同样沿关节轴6的指向；连杆坐标系{6}的y₆轴是按右手原则确定。

B、建立固定于6R型工业机器人基座上的连杆坐标系{0}；

当6R型工业机器人中关节1的关节变量为0时，规定连杆坐标系{0}与连杆坐标系{1}重合；

根据上述方法建立的6R型工业机器人的连杆坐标系如图3所示。

步骤2：获得6R型工业机器人中相邻连杆间连杆坐标系{i}相对于连杆坐标系{i-1}的齐次变换矩阵；

连杆坐标系{i}相对于连杆坐标系{i-1}的变换，是由绕x_i-1旋转α_i-1角、再沿着旋转后坐标系的x_i-1轴平移a_i-1、再绕平移后的坐标系的z_i-1轴旋转θ_i角、最后再沿旋转后的坐标系的z_i-1轴平移d_i合成的，i=1、2、3、4、5、6；根据步骤1中建立的6R型工业机器人的连杆坐标系，得到6R型工业机器人中相邻连杆坐标系间的齐次变换矩阵为：

$\begin{array}{c}T_i^{i-1}={\mathrm{Rot}}_x\left({\mathrm{\α}}_{i-1}\right){\mathrm{Trans}}_x\left({\mathrm{\α}}_{i-1}\right){\mathrm{Rot}}_z\left({\mathrm{\θ}}_i\right){\mathrm{Trans}}_z\left(d_i\right)\\ =\left[\begin{array}{cccc}{\cos \mathrm{\θ}}_i& -{\sin \mathrm{\θ}}_i& 0& a_{i-1}\\ {\sin \mathrm{\θ}}_i{\cos \mathrm{\α}}_{i-1}& {\cos \mathrm{\θ}}_i{\mathrm{c\; os\α}}_{i-1}& {-\sin \mathrm{\α}}_{i-1}& {-\sin \mathrm{\α}}_{i-1}{d}_i\\ {\sin \mathrm{\θ}}_i{\sin \mathrm{\α}}_{i-1}& {\cos \mathrm{\θ}}_i{\sin \mathrm{\α}}_{i-1}& {\cos \mathrm{\α}}_{i-1}& {\cos }_{i-1}{d}_i\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}& & & \\ & R_i^{i-1}& & P_i^{i-1}\\ & & & \\ 0& 0& 0& 1\end{array}\right]\end{array}---\left(1\right)$

式（1）中，i=1、2、3、4、5、6；α_i-1、a_i-1、θ_i、d_i为6R型工业机器人的连杆参数，α_i-1为连杆转角，表示绕x_i-1轴，从z_i-1轴旋转到z_i轴的角度；a_i-1为连杆长度，表示沿x_i-1轴，从z_i-1轴移动到z_i轴的距离；θ_i为关节角，表示绕z_i轴，从x_i-1轴旋转到x_i轴的角度；d_i为连杆偏距，表示沿z_i轴，从x_i-1轴移动到x_i轴的距离；在以上定义的4个连杆参数中，对于旋转关节，θ_i表示关节动作的关节变量；为连杆坐标系{i}相对于连杆坐标系{i-1}的旋转矩阵，^i-1P_i为连杆坐标系{i}相对于坐标系{i-1}的平移矩阵；

对6R型工业机器人进行正向运动学求解可以得到机器人末端连杆坐标系相对于基座连杆坐标系的齐次变换矩阵：

⁰T₆＝⁰T₁ ¹T₂ ²T₃ ³T₄ ⁴T₅ ⁵T₆             （2）

步骤3：建立6R型工业机器人中各连杆的力和力矩平衡方程；

如图4所示，以6R型工业机器人中一个连杆i为对象进行静力分析，当连杆i处于平衡状态时，所受合力为零，连杆i的力和力矩平衡方程为：

$\left\{\begin{array}{c}f_i^i-f_{i+1}^i+m_i^{i}g=0\\ M_i^i-M_{i+1}^i-P_{i+1}^i\×f_{i+1}^i+P_{\mathrm{ci}}^i\×m_i^{i}g=0\end{array}\right.---\left(3\right)$

其中，i=1、2、3、4、5；f_i为连杆i-1施加在连杆i上的力，ⁱf_i为f_i在坐标系{i}中的矢量；M_i为连杆i-1施加在连杆i上的力矩，ⁱM_i为M_i在坐标系{i}中的矢量；m_ig为连杆i的重力，ⁱm_ig为坐标系{i}中连杆i的重力矢量，g为重力加速度；P_ci为连杆i的质心位置，ⁱP_ci为连杆i的质心在坐标系{i}中的位置矢量；ⁱP_i+1为坐标系{i+1}相对于坐标系{i}的平移矩阵；

为便于从高编号连杆向低编号连杆进行迭代求解，用坐标系{i+1}和坐标系{0}相对于坐标系{i}的旋转矩阵进行变换，对式（3）进行整理，得到相邻连杆间的静力“传递”表达式：

$\left\{\begin{array}{c}f_i^i=f_{i+1}^i-m_i^{i}g=R_{i+1}^{i+1}{}_i{f}_{i+1}-\mathrm{Rm}_i^0{}_{i}g\\ M_i^i=R_{i+1}^{i+1}{}_i{M}_{i+1}+P_{i+1}^i\×\left(R_{i+1}^{i+1}{}_i{f}_{i+1}\right)-P_{\mathrm{ci}}^i\×\left(\mathrm{Rm}_i^0{}_{i}g\right)\end{array}\right.---\left(4\right)$

其中，为坐标系{i+1}相对于坐标系{i}的旋转矩阵；为坐标系{0}相对于坐标系{i}的旋转矩阵；

对于连杆6来说，连杆6的重力为m₆g，包含末端执行器的重力；设末端执行器所抓持的负载为mg；通过坐标系{0}与坐标系{6}之间的坐标变换矩阵，得到连杆6在坐标系{6}中的重力矢量为末端执行器所抓持的负载在坐标系{6}中的重力矢量为 即为坐标系{6}相对于坐标系{0}的旋转矩阵；设末端执行器所抓持的负载在坐标系{6}中的质心偏移量为l，则连杆6的力和力矩平衡方程为：

$\left\{\begin{array}{c}f_6^6+\left(\mathrm{Rmg}_6^0\right)+\left(\mathrm{Rm}_6^6{}_{0}g\right)=0\\ M_6^6+(\left(\mathrm{Rmg}_6^0\right)\×l\left(\mathrm{Rm}_6^6{}_{0}g\right)\×P_{c6}^6)=0\end{array}\right.---\left(5\right)$

其中，⁶f₆为在坐标系{6}中连杆5施加在连杆6上的力；⁶M₆为在坐标系{6}中连杆5施加在连杆6上的力矩；⁶P_c6为连杆6的质心在坐标系{6}中的位置矢量；

将式（5）代入式（4），从连杆6坐标系{6}到坐标系{1}依次进行迭代计算，得到作用于各个连杆上的力和力矩；

步骤4：得到6R型工业机器人中保持各连杆平衡的关节驱动力矩；

各个连杆对应的关节驱动力矩τ_i为：

τ_i=ⁱM_i·ⁱz_i=ⁱM_i ^Tiz_i              （6）

式（6）中，i=1、2、3、4、5、6；ⁱz_i为坐标系{i}中关节轴i的z_i矢量；

则由式（1）和式（6）可得：

τ_i=X_img+Y_i               （7）

其中，符号X_i为关节i的驱动力矩τ_i中与负载mg有关的系数，X_i与6R型工业机器人的D-H参数及位姿有关；Y_i表示τ_i中与各连杆重力有关、与负载mg无关的项；

当6R型工业机器人在某一位姿下空载时，根据式（7）可得到机器人空载时关节i的驱动力矩τ_i空为：

τ_i空＝Y_i             （8）

在6R型工业机器人所处的位姿不改变的情况下，当在机器人的末端执行器上附加负载mg时，根据式（7）可得到机器人附加负载时关节i的驱动力矩τ_i载为：

τ_i载=X_img+Y_i                  （9）

根据式（8）和（9）得到6R型工业机器人在末端执行器附加有负载mg时与机器人末端执行器空载时关节i的驱动力矩的差值Δτ_i为：

Δτ_i=τ_i载-τ_i空=X_img                 （10）

则根据式（10）可以得到负载质量m为：

$m=\frac{{\mathrm{\Δ\τ}}_i}{X_{i}g}---\left(11\right)$

从式（11）可以看出，6R型工业机器人负载的质量与连杆的质量、质心等惯性参数无关；

上述Δτ_i通过下述方法得到：

6R型工业机器人的每个关节分别由一个伺服电机来驱动；设驱动电机的输出转矩为τ_M，减速比为i_M，传动效率为η_M；设关节i处的驱动电机的输出转矩为τ_Mi，减速比为i_Mi，传动效率为η_Mi，i=1、2、3、4、5、6；则根据电机拖动原理，机器人关节i的驱动力矩τ_i折算到电机轴上的输出转矩τ_Mi为：则有：τ_i=τ_Mii_Miη_Mi，由式（10）可得到：

Δτ_i=τ_Mi载i_Miη_Mi-τ_Mi空i_Miη_Mi=Δτ_Mii_Miη_Mi   （12）

其中，Δτ_Mi表示机器人在末端执行器附加有负载mg时与机器人末端执行器空载时关节i的驱动电机的输出转矩的差值；τ_Mi载为6R型工业机器人的末端执行器上附加负载mg时，关节i的驱动电机的输出转矩；τ_Mi空为6R型工业机器人的末端执行器空载时，关节i的驱动电机的输出转矩；

由式（11）和（12），可得到负载的质量为：

$m=\frac{{\mathrm{\Δ\τ}}_{\mathrm{Mi}}{i}_{\mathrm{Mi}}{\mathrm{\η}}_{\mathrm{Mi}}}{X_{i}g}---\left(13\right)$

其中，驱动电机的输出转矩可以从电机的控制器上直接得到，无需附加额外的力和力矩传感器来测量，减速比与传动效率均可从设备型号上得知，系数X_i可由机器人D-H参数及机器人位姿计算得到；

由此通过式（13）可分别得到6个负载质量值；因此针对不同型号不同参数的6R型工业机器人，通过验证已知负载的实际质量与通过上述方法识别得到的6个负载质量值进行对比，选取最接近实际负载质量值所对应的关节的数据作为当前型号6R型工业机器人后续再次进行未知负载识别运算时所应用的数据。例如：测得负载实际质量为200kg，通过上述方法得到关节轴1～6所对应的6个负载质量值分别为198、183、199、203、0、-200.3；因此，除去其中不合理数据0与-200.3后，在剩余数据中选取最接近实际负载质量值的199所对应的关节轴3的数据作为此6R型工业机器人后续再次进行负载识别运算的数据。

Claims (1)

1.一种6R型工业机器人负载识别方法，其特征在于：通过下述步骤实现：

步骤1：根据D-H方法，建立6R型工业机器人的连杆坐标系；

6R型工业机器人是指全部关节为转动关节的6自由度工业机器人；6R型工业机器人可视为由连杆和关节组成；在6R型工业机器人的连杆中，将固定基座设为连杆0，然后由基座一侧到末端执行器一侧的各个可动连杆按照1、2、3、4、5、6的顺序依次编号，由基座一侧到末端执行器一侧的各个关节也按照1、2、3、4、5、6的顺序进行编号；为了描述每个连杆与相邻连杆之间的相对位置关系，需要在每个连杆上固定一个连杆坐标系，根据连杆坐标系所在的连杆的编号对连杆坐标系命名，因此固定在连杆i上的连杆坐标系为坐标系{i}，坐标系{i}为O_i(x_i,y_i,z_i)，i＝0、1、2、3、4、5、6；则根据D-H方法建立连杆坐标系的具体方法如下：

A、找出各个关节的关节轴，以相邻两个关节轴i与i+1的交点，或关节轴i与i+1的公垂线与关节轴i的交点作为连杆坐标系{i}的原点O_i，i＝1、2、3、4、5；连杆坐标系{i}的z_i轴沿关节轴i的指向；当关节轴i与i+1的公垂线与关节轴i的交点作为连杆坐标系{i}的原点O_i时，连杆坐标系{i}的x_i轴沿关节轴i与i+1公垂线的指向；当关节轴i与i+1的交点与关节轴i的交点作为连杆坐标系{i}的原点O_i时，连杆坐标系{i}的x_i轴垂直于关节轴i与i+1所在的平面；连杆坐标系{i}的y_i轴则可按照右手原则确定，使y_i＝z_i×x_i；而连杆6作为6R型工业机器人的末端连杆，连杆坐标系{6}的原点O₆以及x₆轴的方向可任意选取；连杆坐标系{6}的z₆轴同样沿关节轴6的指向；连杆坐标系{6}的y₆轴是按右手原则确定；

B、建立固定于6R型工业机器人基座上的连杆坐标系{0}；

当6R型工业机器人中关节1的关节变量为0时，规定连杆坐标系{0}与连杆坐标系{1}重合；

步骤2：获得6R型工业机器人中相邻连杆间连杆坐标系{i}相对于连杆坐标系{i-1}的齐次变换矩阵；

连杆坐标系{i}相对于连杆坐标系{i-1}的变换，是由绕x_i-1旋转α_i-1角、再沿着旋转后坐标系的x_i-1轴平移a_i-1、再绕平移后的坐标系的Z_i-1轴旋转θ_i角、最后再沿旋转后的坐标系的Z_i-1轴平移d_i合成的，i＝1、2、3、4、5、6；根据步骤1中建立的6R型工业机器人的连杆坐标系，得到6R型工业机器人中相邻连杆坐标系间的齐次变换矩阵为：

$\begin{array}{c}T_i^{i-1}={\mathrm{Rot}}_x\left({\mathrm{\α}}_{i-1}\right){\mathrm{Trans}}_x\left(a_{i-1}\right){\mathrm{Rot}}_z\left({\mathrm{\θ}}_i\right){\mathrm{Trans}}_z\left(d_i\right)\\ =\left[\begin{array}{cccc}{\cos \mathrm{\θ}}_i& -{\sin \mathrm{\θ}}_i& 0& a_{i-1}\\ {\sin \mathrm{\θ}}_i{\cos \mathrm{\α}}_{i-1}& {\cos \mathrm{\θ}}_i{\cos \mathrm{\α}}_{i-1}& -{\sin \mathrm{\α}}_{i-1}& -{\sin \mathrm{\α}}_{i-1}{d}_i\\ {\sin \mathrm{\θ}}_i{\sin \mathrm{\α}}_{i-1}& {\cos \mathrm{\θ}}_i{\sin \mathrm{\α}}_{i-1}& {\cos \mathrm{\α}}_{i-1}& {\cos \mathrm{\α}}_{i-1}{d}_i\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}& & & \\ & R_i^{i-1}& & P_i^{i-1}\\ & & & \\ 0& 0& 0& 1\end{array}\right]\end{array}----\left(1\right)$

式(1)中，i＝1、2、3、4、5、6；α_i-1、a_i-1、θ_i、d_i为6R型工业机器人的连杆参数，α_i-1为连杆转角，表示绕x_i-1轴，从z_i-1轴旋转到z_i轴的角度；a_i-1为连杆长度，表示沿x_i-1轴，从z_i-1轴移动到z_i轴的距离；θ_i为关节角，表示绕z_i轴，从x_i-1轴旋转到x_i轴的角度；d_i为连杆偏距，表示沿z_i轴，从x_i-1轴移动到x_i轴的距离；在以上定义的4个连杆参数中，对于旋转关节，θ_i表示关节动作的关节变量；为连杆坐标系{i}相对于连杆坐标系{i-1}的旋转矩阵，^i-1P_i为连杆坐标系{i}相对于坐标系{i-1}的平移矩阵；

对6R型工业机器人进行正向运动学求解可以得到机器人末端连杆坐标系相对于基座连杆坐标系的齐次变换矩阵：

⁰T₆＝⁰T₁ ¹T₂ ²T₃ ³T₄ ⁴T₅ ⁵T₆    (2)

步骤3：建立6R型工业机器人中各连杆的力和力矩平衡方程；

以6R型工业机器人中一个连杆i为对象进行静力分析，当连杆i处于平衡状态时，所受合力为零，连杆i的力和力矩平衡方程为：

$\left\{\begin{array}{c}f_i^i-f_{i+1}^i+m_i^{i}g=0\\ M_i^i-M_{i+1}^i-P_{i+1}^i\×f_{i+1}^i+P_{\mathrm{ci}}^i\×m_i^{i}g=0\end{array}\right.---\left(3\right)$

其中，i＝1、2、3、4、5；f_i为连杆i-1施加在连杆i上的力，ⁱf_i为f_i在坐标系{i}中的矢量；M_i为连杆i-1施加在连杆i上的力矩，ⁱM_i为M_i在坐标系{i}中的矢量；m_ig为连杆i的重力，ⁱm_ig为坐标系{i}中连杆i的重力矢量，g为重力加速度；P_ci为连杆i的质心位置，ⁱP_ci为连杆i的质心在坐标系{i}中的位置矢量；ⁱP_i+1为坐标系{i+1}相对于坐标系{i}的平移矩阵；

为便于从高编号连杆向低编号连杆进行迭代求解，用坐标系{i+1}和坐标系{0}相对于坐标系{i}的旋转矩阵进行变换，对式(3)进行整理，得到相邻连杆间的静力“传递”表达式：

$\left\{\begin{array}{c}f_i^i=f_{i+1}^i-m_i^{i}g=R_{i+1}^{i+1}{}_i{f}_{i+1}-R{m}_i_0^{i}g\\ M_i^i=R_{i+1}^{i+1}{}_i{M}_{i^{+1}}P_{i+1}^i\×\left(R_{i+1}^{i+1}{}_i{f}_{i+1}\right)-P_{\mathrm{ci}}_i\left(R{m}_i_0^{i}g\right)\end{array},---\left(4\right)\right.$

其中，为坐标系{i+1}相对于坐标系{i}的旋转矩阵；为坐标系{0}相对于坐标系{i}的旋转矩阵；

对于连杆6来说，连杆6的重力为m₆g，包含末端执行器的重力；设末端执行器所抓持的负载为mg；通过坐标系{0}与坐标系{6}之间的坐标变换矩阵，得到连杆6在坐标系{6}中的重力矢量为末端执行器所抓持的负载在坐标系{6}中的重力矢量为即为坐标系{6}相对于坐标系{0}的旋转矩阵；设末端执行器所抓持的负载在坐标系{6}中的质心偏移量为l，则连杆6的力和力矩平衡方程为：

$\left\{\begin{array}{c}f_6^6+\left(\mathrm{Rmg}_6^0\right)+\left({\mathrm{Rm}}_{6}g_6^0\right)=0\\ M_6^6+(\left(\mathrm{Rmg}_6^0\right)\×l+\left({\mathrm{Rm}}_{6}g_6^0\right)\×P_{c6}^6)=0\end{array}\right.---\left(5\right)$

其中，⁶f₆为在坐标系{6}中连杆5施加在连杆6上的力；⁶M₆为在坐标系{6}中连杆5施加在连杆6上的力矩；⁶P_c6为连杆6的质心在坐标系{6}中的位置矢量；

将式(5)代入式(4)，从连杆6坐标系{6}到坐标系{1}依次进行迭代计算，得到作用于各个连杆上的力和力矩；

步骤4：得到6R型工业机器人中保持各连杆平衡的关节驱动力矩；

各个连杆对应的关节驱动力矩τ_i为：

τ_i＝ⁱM_i·ⁱz_i＝ⁱM_i ^Tiz_i    (6)

式(6)中，i＝1、2、3、4、5、6；ⁱz_i为坐标系{i}中关节轴i的z_i矢量；

则由式(1)和式(6)可得：

τ_i＝X_img+Y_i    (7)

其中，符号X_i为关节i的驱动力矩τ_i中与负载mg有关的系数，X_i与6R型工业机器人的D-H参数及位姿有关；Y_i表示τ_i中与各连杆重力有关、与负载mg无关的项；

当6R型工业机器人在某一位姿下空载时，根据式(7)可得到机器人末端执行器空载时关节i的驱动力矩τ_i空为：

τ_i空＝Y_i    (8)

在6R型工业机器人所处的位姿不改变的情况下，当在机器人的末端执行器上附加负载mg时，根据式(7)可得到机器人附加负载时关节i的驱动力矩τ_i载为：

τ_i载＝X_img+Y_i    (9)

根据式(8)和(9)得到6R型工业机器人在末端执行器附加有负载mg时与机器人末端执行器空载时关节i的驱动力矩的差值Δτ_i为：

Δτ_i＝τ_i载-τ_i空＝X_img    (10)

则根据式(10)可以得到负载质量m为：

$m=\frac{{\mathrm{\Δ\τ}}_i}{X_{i}g}---\left(11\right)$

从式(11)可以看出，6R型工业机器人负载的质量与连杆的质量、质心等惯性参数无关；

上述Δτ_i通过下述方法得到：

6R型工业机器人的每个关节分别由一个伺服电机来驱动；设驱动电机的输出转矩为τ_M，减速比为i_M，传动效率为η_M；设关节i处的驱动电机的输出转矩为τ_Mi，减速比为i_Mi，传动效率为η_Mi，i＝1、2、3、4、5、6；则根据电机拖动原理，机器人关节i的驱动力矩τ_i折算到电机轴上的输出转矩τ_Mi为：则有：τ_i＝τ_Mii_Miη_Mi，由式(10)可得到：

Δτ_i＝τ_Mi载i_Miη_Mi-τ_Mi空i_Miη_Mi＝Δτ_Mii_Miη_Mi    (12)

其中，Δτ_Mi表示机器人在末端执行器附加有负载mg时与机器人末端执行器空载时关节i的驱动电机的输出转矩的差值；τ_Mi载为6R型工业机器人的末端执行器上附加负载mg时，关节i的驱动电机的输出转矩；τ_Mi空为6R型工业机器人的末端执行器空载时，关节i的驱动电机的输出转矩；

由式(11)和(12)，得到负载的质量为：

$m=\frac{{\mathrm{\Δ\τ}}_{\mathrm{Mi}}{i}_{\mathrm{Mi}}{\mathrm{\η}}_{\mathrm{Mi}}}{X_{i}g}---\left(13\right)$

其中，驱动电机的输出转矩可以从电机的控制器上直接得到，无需附加额外的力和力矩传感器来测量，减速比与传动效率均可从设备型号上得知，系数X_i可由机器人D-H参数及机器人位姿计算得到；

由此通过式(13)可分别得到6个负载质量值；因此针对不同型号不同参数的6R型工业机器人，通过验证已知负载的实际质量与通过上述方法识别得到的6个负载质量值进行对比，选取最接近实际负载质量值所对应的关节的数据作为当前型号6R型工业机器人后续再次进行未知负载识别运算时所应用的数据。