CN109397265B - 一种基于动力学模型的关节型工业机器人拖拽示教方法 - Google Patents

Abstract

本发明属于工业机器人示教领域，并具体公开了一种基于动力学模型的关节型工业机器人拖拽示教方法，包括：建立多关节工业机器人动力学模型，并辨识出动力学模型中的动力学参数；机器人进入拖拽示教模式，获取各关节实际力矩及理论力矩，计算当前周期机器人末端广义力矢量差值；计算当前周期机器人末端的更新速度；根据当前周期机器人末端的更新速度计算当前周期机器人各关节速度值，将各关节速度值传给各关节对应的伺服电机实现机器人拖拽示教；判断多关节工业机器人是否保持拖拽示教模式，若是，重复关节速度更新过程，若否，则结束。本发明具有位姿控制精确、拖拽过程平稳等优点。

Description

一种基于动力学模型的关节型工业机器人拖拽示教方法

技术领域

本发明属于工业机器人示教领域，更具体地，涉及一种基于动力学模型的关节型工业机器人拖拽示教方法。

背景技术

在工业自动化生产过程中，特别是在汽车、电子、医疗及航天等领域，机器人得到了广泛的应用。为保证工艺质量，需要对机器人进行示教。传统示教方法包括：手持示教盒示教和离线编程示教，上述方法都需要较长的部署时间，过程繁琐，对操作人员的专业技术要求较高，难以满足一次生产或者小批量生产对于示教效率的要求。为了改善传统机器人的示教过程，可以采用拖拽示教方式，直接拖拽机器人到达指定位姿或者按特定轨迹运动，记录此过程的位姿信息从而实现机器人的快速示教。

根据对拖拽力矩感知方式的不同，拖拽示教可以分为：基于力矩传感器拖拽示教方法和无力矩传感器拖拽示教方法。基于力矩传感器拖拽示教方法包括末端力矩传感器拖拽示教和关节力矩传感器拖拽示教，末端传感器拖拽示教实现简单，但是只能感知末端力矩；关节传感器拖拽示教需要在各关节安装传感器，拖拽灵活，但提高了成本及结果复杂度，降低了机器人刚度。根据电机控制模式不同，无力传感器拖拽示教方法分为力矩控制模式和位置控制模式，力矩控制模式下，电机对机器人重力矩和摩擦力矩进行补偿，可以实现拖拽示教，但是在静止时需要克服较大摩擦力；位置控制模式下，基于机器人动力学模型计算理论关节力矩得到拖拽力矩，轻松实现拖拽示教。

无力矩传感器的拖拽示教方法相比基于力矩传感器的拖拽示教方法，不需要安装力矩传感器，降低了成本、提高了机器人刚度。但现有无力矩拖拽示教方法往往是各轴独立拖拽，没有实现机器人位姿控制，同时拖拽力矩撤销时，难以快速停止。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种基于动力学模型的关节型工业机器人拖拽示教方法，其基于动力学模型得到理论力矩，并根据力矩雅克比矩阵获取机器人末端广义力矢量差值，在位置控制模式下拖拽机器人末端到达指定位姿，具有位姿控制精确、拖拽过程平稳等优点。

为实现上述目的，本发明提出了一种基于动力学模型的关节型工业机器人拖拽示教方法，其，包括如下步骤：

1)建立多关节工业机器人动力学模型，使多关节工业机器人各关节按照预设的激励轨迹运动，以辨识出动力学模型中的动力学参数；

2)使多关节工业机器人进入拖拽示教模式，获取各关节实际力矩，并根据辨识出动力学参数的机器人动力学模型获取各关节理论力矩，根据各关节实际力矩和理论力矩利用力矩雅克比矩阵获取当前周期机器人末端广义力矢量差值；

3)利用当前周期机器人末端广义力矩差值及上周期机器人末端速度采用分数阶级数计算当前周期机器人末端速度，并获得当前周期机器人末端的更新速度；

4)根据当前周期机器人末端的更新速度利用速度雅克比矩阵计算当前周期机器人各关节速度值，将各关节速度值传给各关节对应的伺服电机实现机器人拖拽示教；

5)判断多关节工业机器人是否保持拖拽示教模式，若是，重复步骤2)-4)，若否，则结束。

作为进一步优选的，步骤1)中采用拉格朗日法建立多关节工业机器人动力学模型。

作为进一步优选的，步骤2)动力学模型中需辨识的动力学参数包括m_i,s_i,x,s_i,y,s_i,z,I_i,xx,I_i,yy,I_i,zz,I_i,xy,I_i,xz和I_i,yz，其中，m_i表示连杆i的质量，s_i,x,s_i,y,s_i,z分别表示连杆i在x,y,z方向的质心位置，I_i,xx,I_i,yy,I_i,zz分别表示连杆i绕连杆坐标轴x_i,y_i,z_i的质量惯性矩，I_i,xy,I_i,xz,I_i,yz表示惯性积，i＝1,2,...,n。

作为进一步优选的，步骤2)中根据各关节实际力矩和理论力矩并利用力矩雅克比矩阵获取当前周期机器人末端广义力矢量差值具体为：

2.1)计算力矩差值：

式中，

表示当前周期机器人末端广义力矢量差值，

表示机器人关节i的理论力矩，

表示机器人关节i的实际力矩，τ_i,min表示机器人关节i的拖拽力矩下限值，τ_i,max表示机器人关节i的拖拽力矩上限值，i＝1,2,...,n；

2.2)计算机器人末端广义力矢量差值：

F_k＝[J^T(q)]^-1·dτ_k

式中，

J^T(q)表示力矩雅克比矩阵，n为机器人关节数。

作为进一步优选的，其特征在于，步骤3)具体包括如下子步骤：

3.1)以分数阶级数计算当前周期k的机器人末端速度：

式中，γ表示速度增益，v_k-1表示第k-1周期的机器人末端速度，v_max表示机器人末端速度最大值，η表示分数阶级数，δ表示力矩增益，κ表示速度力矩比，F_k表示机器人末端广义力矢量差值，

分别表示第k周期机器人末端沿机器人末端坐标系x，y，z轴的移动速度，

分别表示第k周期机器人末端绕机器人末端坐标系x，y，z轴的转动速度；

3.2)更新当前周期k机器人末端速度中各项的值：

式中,

分别表示更新后的第k周期沿x，y，z轴的移动速度，

分别表示更新后的第k周期绕x，y，z轴的转动速度，d_x,min,d_y,min,d_z,min分别表示机器人末端沿机器人末端坐标系x，y，z轴的移动速度的最小值，δ_x,min,δ_y,min,δ_z,min分别表示机器人末端绕机器人末端坐标系x，y，z轴的转动速度的最小值，d_x,max,d_y,max,d_z,max分别表示机器人末端沿机器人末端坐标系x，y，z轴的移动速度的最大值，δ_x,max,δ_y,max,δ_z,max分别表示机器人末端绕机器人末端坐标系x，y，z轴的转动速度的最大值；

3.3)根据更新的当前周期机器人末端速度中各项的值获得当前周期k机器人末端的更新速度v_k′为：

作为进一步优选的，其特征在于，步骤4)中机器人各关节速度值采用如下公式计算：

式中，

表示第k周期关节速度矩阵，

表示第k周期关节i的速度，J(q)表示速度雅克比矩阵。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，主要具备以下的技术优点：

1.本发明与传统带力矩传感器拖拽示教方法相比，克服了机器人静止时需要较大摩擦力的问题，提高机器人对拖拽力的感知能力，使拖拽示教更加轻松。

2.本发明根据力矩雅克比矩阵计算机器人末端广义力矢量差值，以机器人末端广义力矢量差值及上周期速度分数阶作为输入，获取机器人本周期速度值，该方法不需要外接力矩传感器，采用分数阶的方式可以实现拖拽力矩衰减，从而保证拖拽过程的平稳性，实现机器人位姿控制。

3.本发明采用力矩衰减的方式，保证外力撤销后机器人可以快速感知并停止运动。

附图说明

图1是华中数控605型机器人的结构图，图中标注单位为mm；

图2是本发明的基于动力学模型的关节型工业机器人拖拽示教方法的流程图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

本实施例以华中数控605型机器人为例对本发明的方法进行说明，机器人结构如图1所示，关节数n＝6，机器人的连杆参数如表1所示：

表1连杆参数

具体实施步骤如下所示：

1)建立多关节工业机器人动力学模型，使多关节工业机器人各关节按照预设的激励轨迹运动，以辨识出动力学模型中的动力学参数：

首先建立多关节工业机器人动力学模型，譬如采用拉格朗日法建立多关节工业机器人动力学模型，具体模型如下：

式中，τ_i表示关节i的理论力矩，i＝1,2,...,n，n表示机器人的关节数，

分别表示关节i的角度、角速度和角加速度，T_i是4×4矩阵，表示连杆坐标系i相对于基座的坐标变换矩阵，大小只和关节角度有关，m_i表示连杆i的质量，r_i＝[s_i,x s_i,y s_i,z]^T，s_i,x,s_i,y,s_i,z分别表示连杆i在连杆坐标系i的x,y,z方向的质心位置，g^T＝[0 0 9.8]表示重力方向矢量，T_j表示连杆坐标j相对于基座坐标系的位姿变换矩阵，c,m,j均为中间参量，c取值为1-j，m取值为1-j，j取值为i-n或1-n，

分别表示第c关节的角度和角加速度，

分别表示第m关节的角度和角速度，

表示第j关节的角速度，I_j表示伪惯性矩阵，j在1-n取值，可以表示为：

式中，

分别表示机器人连杆绕连杆坐标轴x_i,y_i,z_i的质量惯性矩，

表示惯性积，

分别表示连杆i的质量m_i在连杆坐标系x_i,y_i,z_i方向的分量，即I_j＝1＝I_i＝1，I_j＝2＝I_i＝2，…，I_j＝n＝I_i＝n；

然后，使多关节工业机器人各关节按照预设的激励轨迹运动以辨识出动力学模型中的动力学参数，具体如下：

选择有限项周期性傅里叶级数作为激励轨迹，使机器人各关节按照激励轨迹运动，在激励过程中，记录机器人按激励轨迹运动过程中的6个关节的角度、角速度、角加速度及电流信息，利用最小二乘法辨识出动力学模型中的动力学参数，以提高机器人动力学模型精度，利用最小二乘法辨识动力学参数为现有技术，在此不赘述，其中，待辨识连杆i的动力学参数表示为：

P_i＝[m_i,s_i,x,s_i,y,s_i,z,I_i,xx,I_i,yy,I_i,zz,I_i,xy,I_i,xz,I_i,yz]^T

式中，m_i表示连杆i的质量，s_i,x,s_i,y,s_i,z分别表示连杆i在x,y,z方向的质心位置，I_i,xx,I_i,yy,I_i,zz分别表示连杆i绕连杆坐标轴x_i,y_i,z_i的质量惯性矩，I_i,xy,I_i,xz,I_i,yz表示惯性积。

2)使多关节工业机器人进入拖拽示教模式，获取各关节实际力矩，并根据建立的机器人动力学模型获取各关节理论力矩，根据各关节实际力矩和理论力矩并利用力矩雅克比矩阵获取当前周期机器人末端广义力矢量差值，具体的：

首先，605机器人进入拖拽示教模式后，获取各关节角度、角速度和角加速度，根据辨识出动力学参数的动力学模型计算第k周期(即当前周期)机器人各关节理论力矩

同时获取各关节对应伺服电机的电流并采用卡尔曼滤波对电流进行处理，得到第k周期机器人各关节的实际力矩

根据各关节理论力矩

和实际力矩

得到力矩差值

可以表示为：

式中，τ_min表示605机器人各关节的拖拽力矩下限值，本实施例τ_min＝[0.3 0.3 0.30.05 0.05 0.05]^T，当实际力矩与理论力矩之差小于拖拽力矩差下限值时，表示不存在外力，τ_max表示605机器人各关节拖拽力矩上限值，本实施例τ_max＝[1.2 1.2 1.2 0.2 0.20.2]^T，当实际力矩与理论力矩之差大于拖拽力矩差上限值时，力矩差值取拖拽力矩差的上限值。

然后，根据矢量积法获取605机器人的力矩雅克比矩阵J^T(q)，其为现有技术在此不赘述，也可采用现有其他方法获得机器人的力矩雅克比矩阵J^T(q)，然后获取第k周期机器人末端广义力矢量差值，可以表示为：

式中，

分别表示第k周期机器人末端沿机器人末端坐标系x,y,z方向的受力差值，

分别表示第k周期机器人末端绕机器人末端坐标系x,y,z方向的力矩差值，J^T(q)为w×n阶矩阵，w表示操作空间的维度，本实施例J^T(q)为6×6阶矩阵；

3)利用当前周期机器人末端广义力矩差值及上周期机器人末端速度采用分数阶级数计算当前周期机器人末端速度，以保证外力撤消时机器人能够快速停止，具体的：

3.1)每周期根据上一周期机器人末端速度及末端广义力矢量差值计算机器人末端速度，即周期性计算605机器人末端速度，在第k周期机器人末端速度可以表示为：

式中，

表示第k周期机器人末端沿机器人末端坐标系x，y，z轴的移动速度，

表示第k周期机器人末端绕机器人末端坐标系x，y，z轴的转动速度，机器人末端初始速度为零；

以分数阶级数计算机器人末端速度，第k周期机器人末端速度可以表示为：

式中，γ表示速度增益，范围在[0,1]，本实施例γ＝0.5，κ表示速度力矩比，本实施例κ＝0.1，δ表示力矩增益，范围在[0,1]，本实施例δ＝0.3，η表示分数阶级数，范围在[0.5,1]，本实施例η＝0.75，保证拖拽速度以一定比例衰减，能够快速停止，当对拖拽速度平稳性要求较高时，γ,η取较大值，相应δ取较小值，v_max表示机器人末端速度最大值，

v_max＝[d_x,max d_y,max d_z,max δ_x,max δ_y,max δ_z,max]^T，本实施例中

v_max＝[0.06 0.06 0.06 0.03 0.03 0.03]^T，v_min表示机器人末端速度最小值，

v_min＝[d_x,min d_y,min d_z,min δ_x,min δ_y,min δ_z,min]^T，本实施例中

v_min＝[0.02 0.02 0.02 0.01 0.01 0.01]^T；

即，根据第k周期机器人末端广义力矢量差值F_k利用下式可得到末端速度v_k中的各项

3.2)更新第k周期机器人末端速度v_k中的各项：

式中，d_x,min,d_y,min,d_z,min,δ_x,min,δ_y,min,δ_z,min分别表示机器人末端沿机器人末端坐标系x，y，z轴的移动速度和绕机器人末端坐标系x，y，z轴的转动速度的最小值，本实施例中d_x,min＝0.02，d_y,min＝0.02，d_z,min＝0.02，δ_x,min＝0.01，δ_y,min＝0.01，δ_z,min＝0.01，d_x,max,d_y,max,d_z,max,δ_x,max,δ_y,max,δ_z,max分别表示机器人末端沿机器人末端坐标系x，y，z轴的移动速度和绕机器人末端坐标系x，y，z轴的转动速度的最大值，本实施例中d_x,max＝0.06，d_y,max＝0.06，d_z,max＝0.06，δ_x,max＝0.03，δ_y,max＝0.03，δ_z,max＝0.03；

3.3)根据末端速度中各项更新后的值即可获得当前周期k机器人末端的更新速度v_k′为：

4)根据当前周期机器人末端的更新速度v_k′利用速度雅克比矩阵获取第k周期机器人各关节速度值，将各关节速度值传给各关节对应的伺服电机实现机器人拖拽示教，关节速度表示为：

式中，J(q)表示速度雅克比矩阵，其为力矩雅克比矩阵的转置J^T(q)，

表示第k周期关节速度矩阵，

表示第k周期关节i的速度；

J^T(q)其为w×n阶矩阵，w表示操作空间的维度，力矩雅克比矩阵根据实际需要进行选择，本实施例J^T(q)为6×6阶矩阵；

5)判断605机器人是否保持拖拽示教模式，若是，重复步骤2)-4)，若否，则停止速度更新过程，以此完成关节型工业机器人拖拽示教，具体的，譬如根据示教器中示教操作按键状态判断605机器人是否保持拖拽示教模式，当然也可以采用现有的其他判断方式进行判断。

本发明提供的基于动力学模型的关节型工业机器人拖拽示教方法，无需外接传感器，可解决基于传感器的拖拽示教方式在机器人静止时需要克服较大摩擦及拖拽力矩大的问题，本发明不需要安装力矩传感器，使用力矩雅克比矩阵将机器人关节控制转换为机器人末端控制，可以实现对机器人位姿的精确控制，同时采用周期性速度更新与衰减策略，在调高机器人拖拽稳定性的同时可以保证外力撤销后机器人快速停止。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (6)

1.一种基于动力学模型的关节型工业机器人拖拽示教方法，其特征在于，包括如下步骤：

1)建立多关节工业机器人动力学模型，使多关节工业机器人各关节按照预设的激励轨迹运动，以辨识出动力学模型中的动力学参数；

2)使多关节工业机器人进入拖拽示教模式，获取各关节实际力矩，并根据辨识出动力学参数的机器人动力学模型获取各关节理论力矩，根据各关节实际力矩和理论力矩利用力矩雅克比矩阵获取当前周期机器人末端广义力矢量差值；

3)利用当前周期机器人末端广义力矢量差值及上周期机器人末端速度，采用分数阶级数计算当前周期机器人末端速度，并获得当前周期机器人末端的更新速度；

4)根据当前周期机器人末端的更新速度利用速度雅克比矩阵计算当前周期机器人各关节速度值，将各关节速度值传给各关节对应的伺服电机实现机器人拖拽示教；

5)判断多关节工业机器人是否保持拖拽示教模式，若是，重复步骤2)-4)，若否，则结束。

2.如权利要求1所述的基于动力学模型的关节型工业机器人拖拽示教方法，其特征在于，步骤1)中采用拉格朗日法建立多关节工业机器人动力学模型。

3.如权利要求1所述的基于动力学模型的关节型工业机器人拖拽示教方法，其特征在于，步骤2)动力学模型中需辨识的动力学参数包括m_i,s_i,x,s_i,y,s_i,z,I_i,xx,I_i,yy,I_i,zz,I_i,xy,I_i,xz和I_i,yz，其中，m_i表示连杆i的质量，s_i,x,s_i,y,s_i,z分别表示连杆i在x,y,z方向的质心位置，I_i,xx,I_i,yy,I_i,zz分别表示连杆i绕连杆坐标轴x_i,y_i,z_i的质量惯性矩，I_i,xy,I_i,xz,I_i,yz表示惯性积，i＝1,2,...,n。

4.如权利要求1所述的基于动力学模型的关节型工业机器人拖拽示教方法，其特征在于，步骤2)中根据各关节实际力矩和理论力矩并利用力矩雅克比矩阵获取当前周期机器人末端广义力矢量差值具体为：

2.1)计算力矩差值：

式中，

表示当前周期机器人末端广义力矢量差值，

表示机器人关节i的理论力矩，

表示机器人关节i的实际力矩，τ_i,min表示机器人关节i的拖拽力矩下限值，τ_i,max表示机器人关节i的拖拽力矩上限值，i＝1,2,...,n；

2.2)计算机器人末端广义力矢量差值：

F_k＝[J^T(q)]^-1·dτ_k

式中，

J^T(q)表示力矩雅克比矩阵，n为机器人关节数。

5.如权利要求1所述的基于动力学模型的关节型工业机器人拖拽示教方法，其特征在于，步骤3)具体包括如下子步骤：

3.1)以分数阶级数计算当前周期k的机器人末端速度：

式中，γ表示速度增益，v_k-1表示第k-1周期的机器人末端速度，v_max表示机器人末端速度最大值，δ表示力矩增益，κ表示速度力矩比，F_k表示机器人末端广义力矢量差值，

分别表示第k周期机器人末端沿机器人末端坐标系x，y，z轴的移动速度，

分别表示第k周期机器人末端绕机器人末端坐标系x，y，z轴的转动速度；

3.2)更新当前周期k机器人末端速度中各项的值：

式中,

分别表示更新后的第k周期沿x，y，z轴的移动速度，

分别表示更新后的第k周期绕x，y，z轴的转动速度，d_x,min,d_y,min,d_z,min分别表示机器人末端沿机器人末端坐标系x，y，z轴的移动速度的最小值，δ_x,min,δ_y,min,δ_z,min分别表示机器人末端绕机器人末端坐标系x，y，z轴的转动速度的最小值，d_x,max,d_y,max,d_z,max分别表示机器人末端沿机器人末端坐标系x，y，z轴的移动速度的最大值，δ_x,max,δ_y,max,δ_z,max分别表示机器人末端绕机器人末端坐标系x，y，z轴的转动速度的最大值；

3.3)根据更新的当前周期机器人末端速度中各项的值获得当前周期k机器人末端的更新速度v_k′为：

。

6.如权利要求1所述的基于动力学模型的关节型工业机器人拖拽示教方法，其特征在于，步骤4)中机器人各关节速度值采用如下公式计算：

式中，

表示第k周期关节速度矩阵，

表示第k周期关节i的速度，J(q)表示速度雅克比矩阵。

Images