[发明专利]一种基于柔体动力学模型的机器人柔性力矩前馈控制方法

权利要求书

1.一种基于柔体动力学模型的机器人柔性力矩前馈控制方法，其特征在于，包括步骤：

步骤1、建立机器人柔性关节的可辨识最小惯性参数模型；具体包括：

步骤11、建立具有柔性关节的机器人动力学模型；所述的步骤11具体包括：

步骤111、根据如式(1)所示刚体动力学方程，确定了机器人各个连杆的几何参数以及动力学参数:

式中，D_ij为关节i和关节j的惯性项，I_ai为关节i的惯量项，D_ijk为哥氏力、向心力项，G_i为作用于关节i的重力项，为关节j的角速度和角加速度；

步骤112、改进式(1)，将机器人关节的柔性因素考虑在内，利用柔体动力学建模思想，建立实验平台的柔性关节的动力学方程，准确描述机器人在运动过程中的位置信息、关节的柔性信息与关节力矩之间的关系，则具有柔性关节的机器人动力学模型可以表述为式(2)：

式(2)中，M(q_link)为n×n的惯性矩阵，为n×1的哥氏力、向心力以及重力项矩阵，K为n×1的扭转刚度系数矩阵，q_link，分别为n×1关节位置、速度、加速度矩阵，q_motor，分别为n×1电机端位置、速度、加速度，J_motor表示n×1的电机转子惯量矩阵，f_v,f_s为n×1的粘性摩擦力系数矩阵和库伦摩擦力系数矩阵，其中电机端期望的关节角度矩阵q＝q_motor/N，N为减速比，τ为n×1关节力矩矩阵；

步骤12、采用单轴激励进行辨识的方法，求得第一轴的最小惯性参数模型；所述的步骤12具体包括：

步骤121、将机器人单关节的柔性因素等价于电机-弹簧-质量的两惯量系统，其中电机端输出力矩为u，旋转角度为q_m，电机转子旋转惯量为J_m，减速比为N；关节端关节旋转惯量为J_l，旋转角度为q_l；

步骤122、设单轴运动中减速比为N，重力项为G(q_l)，哥氏力、向心力项为0，建立如式(3)至式(5)的单轴动力学模型：

G(q_l)＝MgX_lcos(q_l)+MgY_lsin(q_l) (5)

其中q＝q_m/N，f_vj、f_sj分别是关节端的粘性摩擦力系数和库伦摩擦力系数，k表示关节刚度系数，f_vm、f_sm分别表示电机端的粘性摩擦力系数以及库伦摩擦力系数，sign(·)表示符号函数，X_l、Y_l表示关节的质心位置信息；

步骤123、对所述单轴动力学模型式(3)至式(5)进行线性形式的变换,得到则如式(6)所示的辨识的线性模型:

τ＝DX (6)

其中X＝[J_l f_vj f_sj MX_l MY_l k J_m f_vm f_sm]；

步骤124、采用电机输出端的位置信息q_m作为计算未知变量，将式(4)进行变换，得到如式(7)至式(9)：

柔性端位置角度计算公式如下：

柔性端位置角速度与角加速度计算公式如下：

步骤125、设重力项式(5)为0，并将式(7)、(8)、(9)代入式(6)进行化简，得到如下的最小惯性参数模型：

τ_m＝W_sX_s (10)

其中，τ_m为电机端的力矩值，

步骤13、重复步骤11，求得其他所有轴的最小惯性参数模型；

步骤2、以采样周期为T实时对机器人柔性关节运动过程中的各个关节运动参数进行数据采样和预处理；

步骤3、将预处理后的各个关节运动参数代入所述可辨识最小惯性参数模型，并利用最小二乘估计法辨识得到柔性动力学参数，并回代计算出柔性关节下所需的力矩值；

步骤4、将计算的力矩值作为前馈量，并以周期T的形式发送到伺服驱动器的底层，实时刷新驱动器；

步骤5、采用补偿的形式与电流环输出量进行叠加，从而实现机器人的柔性控制。