CN114589702B - 基于动力学参数辨识和导纳控制的协作机器人拖动方法 - Google Patents

Abstract

本发明公开了基于动力学参数辨识和导纳控制的协作机器人拖动方法，主要包括以下步骤：利用牛顿‑欧拉法构建协作机器人的动力学方程，基于牛顿‑欧拉动力学方程动力学方程找出线性相关项，将动力学模型线性化处理；基于有限项傅里叶级数的形式设计激励轨迹，采集协作机器人负载端电流数值，利用最小二乘法求解动力学最小参数集；对参数辨识所得动力学最小参数集结果结果进行验证。本发明实现了协作机器人基于电流控制的力/位混合柔顺拖动，通过对动力学参数进行辨识得到更为准确的动力学模型，相较于传统的通过调参对机器人重力以及摩擦力进行补偿从而实现拖动的方式更为精确，也大大提高了拖动作业的安全性。

Description

基于动力学参数辨识和导纳控制的协作机器人拖动方法

技术领域

本发明涉及协作机器人柔顺控制技术领域，具体为基于动力学参数辨识和导纳控制的协作机器人拖动方法。

背景技术

协作机器人在生产制造领域的应用越来越广泛，越来越多的场景都需要与环境进行交互。传统的控制方法主要通过减小误差来实现更好的控制效果，这样做的结果可能加大协作机器人的损耗，同时也伴随有一定的危险性，通过给协作机器人加装末端六维力传感器，使协作机器人能够感知外界环境力变化，可以改善这种现象，但是目前市面上大多六维力传感器价格较高，成本高昂，因此使用电流实现协作机器人的柔顺控制不仅可以节约成本，而且可以提高与环境交互过程中的安全性。

实现协作机器人柔顺控制主要采用阻抗/导纳控制算法，传统的阻抗导纳控制算法是通过将协作机器人等价为一个二阶的弹簧阻尼系统，通过由环境力产生的当前位置与期望位置，当前速度与期望速度之间的误差来调节机械臂运动，但协作机械臂连续运动过程之中，位置与速度误差除了由环境力产生外，还应包含自身期望运动产生的偏差。因此需要制定新的控制策略，同时环境力的计算的准确性依赖于构建准确的协作机器人动力学方程。

为了能够构建准确的协作机器人动力学方程，我们需要获取协作机械臂准确的动力学参数，传统的物理分析获得协作机器人动力学参数的方法计算量大，分析过程复杂，且不能保证准确度，现阶段一些通过迭代计算以及深度学习的方法进行动力学参数辨识获得的结果较为准确，但由于计算量大，因此对硬件要求较高，且迭代结果不能保证收敛，可能会导致辨识不成功，因此一种简单，适用性强的基于动力学参数辨识和导纳控制的协作机器人柔顺拖动方法对协作做机器人环境交互作业场景具有重要意义。

发明内容

本发明的目的在于提供基于动力学参数辨识和导纳控制的协作机器人拖动方法，以解决上述背景技术中提出传统的物理分析获得协作机器人动力学参数的方法计算量大，分析过程复杂，且不能保证准确度，现阶段一些通过迭代计算以及深度学习的方法进行动力学参数辨识获得的结果较为准确，但由于计算量大，因此对硬件要求较高，且迭代结果不能保证收敛，可能会导致辨识不成功的问题。

为实现上述目的，本发明提供如下技术方案：基于动力学参数辨识和导纳控制的协作机器人拖动方法，该方法包括以下步骤：

步骤1：建立协作机器人基于牛顿-欧拉形式的动力学方程，基于牛顿-欧拉动力学方程动力学方程找出线性相关项，将动力学模型线性化处理；

步骤2：基于有限项傅里叶级数的形式设计激励轨迹，采集协作机器人负载端电流数值，利用最小二乘法求解动力学最小参数集；

步骤3：对参数辨识所得动力学最小参数集结果结果进行验证，基于动力学参数辨识结果，提出导纳柔顺拖动算法，实现柔顺控制。

优选的，所述的步骤1中，构建协作机器人基于基于牛顿-欧拉形式的动力学方程，基于牛顿-欧拉动力学方程找出线性相关项，将动力学模型线性化处理，具体步骤如下：

动力学模型构建如下，先利用运动学公式，前向迭代，求得机器人各连杆的线速度ⁱv_i、线加速度角速度ⁱω_i和角加速度/>计算各连杆上所需的力ⁱF_i和力矩ⁱN_i；

然后从末端连杆开始，根据力矩和力平衡方程，向内递推，求得各关节所需的力ⁱf_i、力矩ⁱn_i；

其中m_i为第i根连杆质量，为第i根连杆绕其质心坐标系{C_i}的惯性矩阵，/>为第i根连杆质心坐标系C_i相对于坐标系{i}的平移偏量，/>为沿第i个关节轴线的单位向量；

基于牛顿-欧拉动力学方程找出线性相关项，将动力学模型线性化处理如下：

分别为第i连杆绕坐标系{i}和{C_i}的惯性张量矩阵，P_ci为其质心在坐标系{i}中的位置，则有：

且有代表末端角速度矩阵,/>为负载惯性张量

则可得：

则由牛顿-欧拉动力学动力学方程可得：

若记其中/>表示末端所受外力、力矩；/>为第i根连杆的动力学参数,则上式可写为

其中：

若将组成一列，则递推可得：

U_ii＝B_i,U_ij＝A_iA_i+1...B_j,i<j

由于我们只关心Z轴方向的力矩：

则变换得：

J,K分别通过将V,U矩阵的每一个块矩阵(V_ij、U_ij)左乘向量[0 0 0 0 0 1]得到；

若考虑摩擦力影响，

采用正反库仑粘滞摩擦力+电机惯性补偿模型

则有：

其中为连杆角速度，/>为连杆角加速度，r_ci和r_vi分别为库伦摩擦系数和粘性摩擦系数。

综合考虑有：

同时，假设不存在外力和外力矩，则：

其中K_total为动力学方程线性回归矩阵，ψ_total为需要辨识的动力学最小参数集,每根杆有10个连杆动力学参数，6*n_f个摩擦力参数，n_f为摩擦力系数项数，6根连杆则共有60+6*n_f个参数。

优选的，所述的步骤2中，基于有限项傅里叶级数的形式设计激励轨迹，采集协作机器人负载端电流数值，利用最小二乘法求解动力学最小参数集，具体步骤如下：

激励轨迹采用有限项傅里叶级数的形式：

其中，N为傅里叶级数项数，基础频率ω_f＝2πf_f，f_f为采样频率，a_l,i、b_l,i为正余弦函数的幅值，q_i0为常数项；

本文采用的激励轨迹优化任务为最小化信息矩阵K的条件数即/>σ_max,σ_min分别为K的最大、小奇异值；

与此同时，任务需要满足关节角度、速度和加速度约束：

q_min≤q(t)≤q_max

根据上述想法，使用Matlab的fmincon函数对激励轨迹参数进行优化；取傅里叶级数的系数a_l,i,b_l,i,为待优化参数，在满足约束的条件下，将/>的大小设为优化指标；

将生成的激励轨迹点下发给协作机器人控制器，采集协作机器人在运动过程中各关节末端电流I，位置q(t)，与速度信号带入/> 中，其中τ_total＝K_f×I，K_f为力矩系数，则动力学最小参数集ψ_total＝(K_total)^-1×τ_total；

优选的，所述的步骤3中，对参数辨识所得动力学最小参数集结果结果进行验证，基于动力学参数辨识结果，提出导纳柔顺拖动算法，实现柔顺控制，具体步骤如下：

基于参数辨识得到动力学最小参数集ψ_total，则我们的期望力矩τ_e＝K_totalψ_total；柔顺拖动过程中所受外力矩τ_ex为τ_ex＝K_f×I，K_f为力矩系数，I为电流大小，其中τ_ex＝τ_e+τ，τ为环境力，则τ＝τ_ex-τ_e，通用形式的导纳控制器为：

其中x_e是实际位置与期望位置之差，是实际速度与期望速度之差，/>是实际加速度与期望加速度之差，τ为环境力矩。M为惯性系数矩阵，B为阻尼系数矩阵，K为刚度系数矩阵；

由于柔顺拖动需要根据环境力调整协作机器人运动，位置与速度误差除了由环境力产生外，还应包含自身期望运动产生的偏差，因此将导纳控制器改写成如下形式：

将当前时刻位置作为期望位置，则期望速度与期望位置为：

将导纳控制器写成二阶系统传递函数的形式可得：

则B/M＝2ξω_n，K/M＝ω_n ²,可得其中ξ为阻尼，当0<ξ<1是为欠阻尼，当ξ>1是为过阻尼，完成协作机器人的力/位转换柔顺控制，最后导纳控制器输出的是关节角度，将控制器输出发送给机械臂实现柔顺拖动。

与现有技术相比，本发明的有益效果是：本发明实现了协作机器人基于电流控制的力/位混合柔顺拖动，通过对动力学参数进行辨识得到更为准确的动力学模型，相较于传统的通过调参对机器人重力以及摩擦力进行补偿从而实现拖动的方式更为精确，也大大提高了拖动作业的安全性；通过对柔顺拖动过程中的位置与速度误差进行分析，得出更为准确的导纳控制模型；同时基于各关节实时电流变化来计算协作机器人所受外力大小，相较于基于末端力传感器的柔顺控制策略省去了复杂的环境力在坐标系下的转换，也节约了成本。

附图说明

图1为本发明的方法总体框图；

图2为动力学参数辨识方法框图；

图3为本发明实例提供的动力学参数辨识的激励轨迹优化图；

图4为本发明实例提供的动力学参数辨识的关节力矩图；

图5为本发明实例提供的动力学参数辨识的误差均方根拟合图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参阅图1-5，本发明提供一种技术方案：基于动力学参数辨识和导纳控制的协作机器人拖动方法，该方法包括以下步骤：

步骤1：建立协作机器人基于牛顿-欧拉形式的动力学方程，基于牛顿-欧拉动力学方程找出线性相关项，将动力学模型线性化处理；

步骤2：基于有限项傅里叶级数的形式设计激励轨迹，采集协作机器人负载端电流数值，利用最小二乘法求解动力学最小参数集；

步骤3：对参数辨识所得动力学最小参数集结果结果进行验证，基于动力学参数辨识结果，提出导纳柔顺拖动算法，实现柔顺控制。

本发明中：所述的步骤1中，构建协作机器人基于基于牛顿-欧拉形式的动力学方程，基于牛顿-欧拉动力学方程找出线性相关项，将动力学模型线性化处理，具体步骤如下：

动力学模型构建如下，先利用运动学公式，前向迭代，求得机器人各连杆的线速度ⁱv_i、线加速度角速度ⁱω_i和角加速度/>计算各连杆上所需的力ⁱF_i和力矩ⁱN_i；

然后从末端连杆开始，根据力矩和力平衡方程，向内递推，求得各关节所需的力ⁱf_i、力矩ⁱn_i；

其中m_i为第i根连杆质量，为第i根连杆绕其质心坐标系{C_i}的惯性矩阵，/>为第i根连杆质心坐标系C_i相对于坐标系{i}的平移偏量，/>为沿第i个关节轴线的单位向量；

基于牛顿-欧拉动力学方程找出线性相关项，将动力学模型线性化处理如下：

分别为第i连杆绕坐标系{i}和{C_i}的惯性张量矩阵，P_ci为其质心在坐标系{i}中的位置，则有：

且有代表末端角速度矩阵,/>为负载惯性张量

则可得：

则由牛顿-欧拉动力学动力学方程可得：

若记其中/>表示末端所受外力、力矩；/>为第i根连杆的动力学参数,则上式可写为

其中：

若将组成一列，则递推可得：

U_ii＝B_i,U_ij＝A_iA_i+1...B_j,i<j

由于我们只关心Z轴方向的力矩：

则变换得：

J,K分别通过将V,U矩阵的每一个块矩阵(V_ij、U_ij)左乘向量[0 0 0 0 0 1]得到；

若考虑摩擦力影响，

采用正反库仑粘滞摩擦力+电机惯性补偿模型

则有：

其中为连杆角速度，/>为连杆角加速度，r_ci和r_vi分别为库伦摩擦系数和粘性摩擦系数。

综合考虑有：

同时，假设不存在外力和外力矩，则：

其中K_total为动力学方程线性回归矩阵，ψ_total为需要辨识的动力学最小参数集,每根杆有10个连杆动力学参数，6*n_f个摩擦力参数，n_f为摩擦力系数项数，6根连杆则共有60+6*n_f个参数。

本发明中：所述的步骤2中，基于有限项傅里叶级数的形式设计激励轨迹，采集协作机器人负载端电流数值，利用最小二乘法求解动力学最小参数集，具体步骤如下：

激励轨迹采用有限项傅里叶级数的形式：

其中，N为傅里叶级数项数，基础频率ω_f＝2πf_f，f_f为采样频率，a_l,i、b_l,i为正余弦函数的幅值，q_i0为常数项；

本文采用的激励轨迹优化任务为最小化信息矩阵K的条件数即σ_max,σ_min分别为K的最大、小奇异值；

与此同时，任务需要满足关节角度、速度和加速度约束：

q_min≤q(t)≤q_max

根据上述想法，使用Matlab的fmincon函数对激励轨迹参数进行优化；取傅里叶级数的系数a_l,i,b_l,i,为待优化参数，在满足约束的条件下，将/>的大小设为优化指标；

将生成的激励轨迹点下发给协作机器人控制器，采集协作机器人在运动过程中各关节末端电流I，位置q(t)，与速度信号带入/> 中，其中τ_total＝K_f×I，K_f为力矩系数，则动力学最小参数集ψ_total＝(K_total)^-1×τ_total；

本发明中：所述的步骤3中，对参数辨识所得动力学最小参数集结果结果进行验证，基于动力学参数辨识结果，提出导纳柔顺拖动算法，实现柔顺控制，具体步骤如下：

基于参数辨识得到动力学最小参数集ψ_total，则我们的期望力矩τ_e＝K_totalψ_total；柔顺拖动过程中所受外力矩τ_ex为τ_ex＝K_f×I，K_f为力矩系数，I为电流大小，其中τ_ex＝τ_e+τ，τ为环境力，则τ＝τ_ex-τ_e，通用形式的导纳控制器为：

其中x_e是实际位置与期望位置之差，是实际速度与期望速度之差，/>是实际加速度与期望加速度之差，τ为环境力矩。M为惯性系数矩阵，B为阻尼系数矩阵，K为刚度系数矩阵；

由于柔顺拖动需要根据环境力调整协作机器人运动，位置与速度误差除了由环境力产生外，还应包含自身期望运动产生的偏差，因此将导纳控制器改写成如下形式：

将当前时刻位置作为期望位置，则期望速度与期望位置为：

将导纳控制器写成二阶系统传递函数的形式可得：

/>

则B/M＝2ξω_n，K/M＝ω_n ²,可得其中ξ为阻尼，当0<ξ<1是为欠阻尼，当ξ>1是为过阻尼，完成协作机器人的力/位转换柔顺控制，最后导纳控制器输出的是关节角度，将控制器输出发送给机械臂实现柔顺拖动。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。

Claims (1)

1.基于动力学参数辨识和导纳控制的协作机器人拖动方法，其特征在于，该方法包括以下步骤：

步骤1：建立协作机器人基于牛顿-欧拉形式的动力学方程，基于牛顿-欧拉动力学方程找出线性相关项，将动力学模型线性化处理；

步骤2：基于有限项傅里叶级数的形式设计激励轨迹，采集协作机器人负载端电流数值，利用最小二乘法求解动力学最小参数集；

步骤3：对参数辨识所得动力学最小参数集结果进行验证，基于动力学参数辨识结果，提出导纳柔顺拖动算法，实现柔顺控制；

其中，步骤1中，构建协作机器人基于牛顿-欧拉形式的动力学方程，基于牛顿-欧拉动力学方程找出线性相关项，将动力学模型线性化处理，具体步骤如下：

动力学模型构建如下，先利用运动学公式，前向迭代，求得机器人各连杆的线速度ⁱv_i、线加速度角速度ⁱω_i和角加速度/>计算各连杆上所需的力ⁱF_i和力矩ⁱN_i；

然后从末端连杆开始，根据力矩和力平衡方程，向内递推，求得各关节所需的力ⁱf_i、力矩ⁱn_i；

其中m_i为第i根连杆质量，为第i根连杆绕其质心坐标系{C_i}的惯性矩阵，/>为第i根连杆质心坐标系C_i相对于坐标系{i}的平移偏量，/>为沿第i个关节轴线的单位向量；

基于牛顿-欧拉动力学方程找出线性相关项，将动力学模型线性化处理如下：

分别为第i连杆绕坐标系{i}和{C_i}的惯性张量矩阵，P_ci为其质心在坐标系{i}中的位置，则有：

且有代表末端角速度矩阵,/>为负载惯性张量

则可得：

则由牛顿-欧拉动力学方程可得：

若记其中/>表示末端所受外力、力矩；/>为第i根连杆的动力学参数,则上式可写为

其中：

若将组成一列，则递推可得：

U_ii＝B_i,U_ij＝A_iA_i+1...B_j,i<j

由于只关心Z轴方向的力矩：

则变换得：

J,K分别通过将V,U矩阵的每一个块矩阵(V_ij、U_ij)左乘向量[0 0 0 0 0 1]得到；

若考虑摩擦力影响，

采用正反库仑粘滞摩擦力+电机惯性补偿模型

则有：

其中为连杆角速度，/>为连杆角加速度，r_ci和r_vi分别为库伦摩擦系数和粘性摩擦系数；

综合考虑有：

同时，假设不存在外力和外力矩，则：

其中K_total为动力学方程线性回归矩阵，ψ_total为需要辨识的动力学最小参数集,每根杆有10个连杆动力学参数，6*n_f个摩擦力参数，n_f为摩擦力系数项数，6根连杆则共有60+6*n_f个参数；

步骤2中，基于有限项傅里叶级数的形式设计激励轨迹，采集协作机器人负载端电流数值，利用最小二乘法求解动力学最小参数集，具体步骤如下：

激励轨迹采用有限项傅里叶级数的形式：

其中，N为傅里叶级数项数，基础频率ω_f＝2πf_f，f_f为采样频率，a_l,i、b_l,i为正余弦函数的幅值，q_i0为常数项；

激励轨迹优化任务为最小化信息矩阵K的条件数即σ_max,σ_min分别为K的最大、小奇异值；

与此同时，任务需要满足关节角度、速度和加速度约束：

根据上述，使用Matlab的fmincon函数对激励轨迹参数进行优化；取傅里叶级数的系数a_l,i,b_l,i,为待优化参数，在满足约束的条件下，将/>的大小设为优化指标；

将生成的激励轨迹点下发给协作机器人控制器，采集协作机器人在运动过程中各关节末端电流I，位置q(t)，与速度信号带入/> 中，其中τ_total＝K_f×I，K_f为力矩系数，则动力学最小参数集ψ_total＝(K_total)^-1×τ_total；

步骤3中，对参数辨识所得动力学最小参数集结果进行验证，基于动力学参数辨识结果，提出导纳柔顺拖动算法，实现柔顺控制，具体步骤如下：

基于参数辨识得到动力学最小参数集ψ_total，则的期望力矩τ_e＝K_totalψ_total；柔顺拖动过程中所受外力矩τ_ex为τ_ex＝K_f×I，K_f为力矩系数，I为电流大小，其中τ_ex＝τ_e+τ，τ为环境力，则τ＝τ_ex-τ_e，通用形式的导纳控制器为：

其中x_e是实际位置与期望位置之差，是实际速度与期望速度之差，/>是实际加速度与期望加速度之差，τ为环境力矩，M为惯性系数矩阵，B为阻尼系数矩阵，K为刚度系数矩阵；

由于柔顺拖动需要根据环境力调整协作机器人运动，位置与速度误差除了由环境力产生外，还应包含自身期望运动产生的偏差，因此将导纳控制器改写成如下形式：

将当前时刻位置作为期望位置，则期望速度与期望位置为：

将导纳控制器写成二阶系统传递函数的形式可得：

则B/M＝2ξω_n，K/M＝ω_n ²,可得其中ξ为阻尼，当0<ξ<1是为欠阻尼，当ξ>1是为过阻尼，完成协作机器人的力/位转换柔顺控制，最后导纳控制器输出的是关节角度，将控制器输出发送给机械臂实现柔顺拖动。