[发明专利]一种6R型工业机器人负载识别方法

摘要

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

主权项

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型工业机器人中相邻连杆坐标系间的齐次变换矩阵为：Tii-1=Rotx(αi-1)Transx(αi-1)Rotz(θi)Transz(di)=cosθi-sinθi0ai-1sinθicosαi-1cosθic osαi-1-sinαi-1-sinαi-1disinθisinαi-1cosθisinαi-1cosαi-1cosi-1di0001=Rii-1Pii-10001---(1)]]>式（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的力和力矩平衡方程为：fii-fi+1i+miig=0Mii-Mi+1i-Pi+1i×fi+1i+Pcii×miig=0---(3)]]>其中，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）进行整理，得到相邻连杆间的静力“传递”表达式：fii=fi+1i-miig=Ri+1i+1ifi+1-Rmi0igMii=Ri+1i+1iMi+1+Pi+1i×(Ri+1i+1ifi+1)-Pcii×(Rmi0ig)---(4)]]>其中，为坐标系{i+1}相对于坐标系{i}的旋转矩阵；为坐标系{0}相对于坐标系{i}的旋转矩阵；对于连杆6来说，连杆6的重力为m₆g，包含末端执行器的重力；设末端执行器所抓持的负载为mg；通过坐标系{0}与坐标系{6}之间的坐标变换矩阵，得到连杆6在坐标系{6}中的重力矢量为末端执行器所抓持的负载在坐标系{6}中的重力矢量为即为坐标系{6}相对于坐标系{0}的旋转矩阵；设末端执行器所抓持的负载在坐标系{6}中的质心偏移量为l，则连杆6的力和力矩平衡方程为：f66+(Rmg60)+(Rm660g)=0M66+((Rmg60)×l(Rm660g)×Pc66)=0---(5)]]>其中，⁶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=ΔτiXig---(11)]]>从式（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=ΔτMiiMiηMiXig---(13)]]>其中，驱动电机的输出转矩可以从电机的控制器上直接得到，无需附加额外的力和力矩传感器来测量，减速比与传动效率均可从设备型号上得知，系数X_i可由机器人D-H参数及机器人位姿计算得到；由此通过式（13）可分别得到6个负载质量值；因此针对不同型号不同参数的6R型工业机器人，通过验证已知负载的实际质量与通过上述方法识别得到的6个负载质量值进行对比，选取最接近实际负载质量值所对应的关节的数据作为当前型号6R型工业机器人后续再次进行未知负载识别运算时所应用的数据。