CN113681543B - 一种基于模型预测的机械臂零力控制方法 - Google Patents

Abstract

本发明公开了一种基于模型预测的机械臂零力控制方法，包括步骤：(1)设计目标函数使得机械臂末端的负载力矩与直接控制力矩之和最小，且机械臂系统状态与期望状态偏差最小，并在关节位置和角速度限定范围内构建约束；(2)读取当前机械臂各关节的位置和角速度信息，得到当前机械臂系统状态，并计算得到机械臂末端的负载力矩；(3)求解步骤(1)的目标函数得到下一时刻机械臂各关节的直接控制力矩，据此控制机械臂，返回步骤(2)；(4)重复步骤(3)，并据此控制机械臂。本发明通过模型预测进行机械臂零力控制，能够通过直接转矩进行零力控制并且保证安全、控制器性能调参数方式直观。

Description

一种基于模型预测的机械臂零力控制方法

技术领域

本发明涉及机械臂控制领域，具体涉及一种基于模型预测的机械臂零力控制方法。

背景技术

协作机器人为简化人机交互往往采用拖动示教的编程方式进行部署。所以通过零力控制实现拖动示教的技术非常重要。

基于机械臂各个关节的力矩信息，目前存在两种常用的机械臂零力控制技术方案：基于位置的零力控制技术、基于直接转矩的零力控制技术。基于直接转矩控制的零力控制技术由于缺乏位置闭环控制，使得整机系统运行过程中具有安全隐患。在机械臂负载较大的时候，依靠拖动难以克服系统的静摩擦力，难以取得良好的效果。基于位置控制的零力控制技术需要将外力与经动力学模型计算的力相加并转换为相应的位置指令，外力需要通过传感器直接或者间接测量，另外约束分解较为复杂。关节的外力力矩通常可以采用基于电机驱动器电流测量、基于关节力矩传感器测量或者基于末端6维力矩传感器的测量获得。

机械臂力/位置混合控制受限于实际场景多变且约束分解复杂，难以付诸实施。机械臂阻抗控制能够根据机器人机械臂末端作用力与位置偏差之间的动态关系，通过控制机械臂位移间接地控制机器人与环境间的作用力，此种控制方式可以大幅度降低机械臂末端力控的实现难度，但控制效果需要修改控制器的阻尼系数，如遇到需要示教的位置较远，需要大幅降低阻尼系数，难以直观调节性能。

另外，在传统机械臂零力控制方法中，基于力矩控制的机械臂零力控制缺乏位置与速度的保护，且在起始拖动时需要克服较大的静摩擦力。

发明内容

发明目的：为了改进现有技术的不足之处，本发明提出一种基于模型预测的机械臂零力控制方法，能够通过直接转矩进行零力控制并且保证安全、控制器性能调参数方式直观。

技术方案：

一种基于模型预测的机械臂零力控制方法，包括步骤：

(1)设计目标函数使得机械臂末端的负载力矩与直接控制力矩之和最小，且机械臂系统状态与期望状态偏差最小，并在关节位置和角速度限定范围内构建约束；

(2)读取当前机械臂各关节的位置和角速度信息，得到当前机械臂系统状态，并计算得到机械臂末端的负载力矩；

(3)求解步骤(1)的目标函数得到下一时刻机械臂各关节的直接控制力矩，据此控制机械臂，返回步骤(2)；

(4)重复步骤(3)，并据此控制机械臂。

所述步骤(1)具体为：

(11)n自由度机械臂的动力学模型如下：

x_i+1＝H(x_i,u_i)

其中

表示i时刻机械臂系统状态，取各个关节在i时刻的位置和角速度；q_i表示i时刻机械臂各关节的位置；

表示i时刻机械臂各关节的角速度；u_i表示i时刻各关节的直接控制力矩；

(12)针对给定i时刻的机械臂系统状态x_i，通过各关节驱动电机的力矩/电流及编码器读取各个关节的位置和角速度信息，据此得到机械臂末端的负载力矩：

τ_load[i]＝τ_gravity[i]+τ_friction[i]

τ_gravity[i]＝G(q_i)

τ_load[i]表示i时刻机械臂各个关节的负载力矩，τ_gravity[i]表示i时刻机械臂各关节由重力产生的负载力矩，τ_friction[i]表示i时刻机械臂各关节由摩擦力产生的负载力矩；G(q_i)表示i时刻各关节重力在机械臂末端的分量，

表示i时刻各关节摩擦力在机械臂末端的分量，二者均可根据获取得到的各关节驱动电机的力矩/电流，通过重力和摩擦力辨识得到；

(13)构建目标函数：

其中，对角矩阵Q和R分别表示机械臂系统状态的轨迹权重矩阵和控制轨迹权重矩阵；

(14)构建约束：

X_min＜x_i＜X_max

U_min＜u_i＜U_max

ΔX_min＜|x_i+1-x_i|＜ΔX_max

ΔU_min＜|u_i+1-u_i|＜ΔU_max

其中，X_min、X_max分别表示最小和最大允许的机械臂系统状态；U_min、U_max分别表示机械臂系统允许输入的最小关节力矩和最大关节力矩；ΔX_min,ΔX_max分别表示最小和最大允许的机械臂系统状态变化步长，分别对应最小和最大的关节位置变化步长及关节角度变化步长；ΔU_min,ΔU_max分别表示最小和最大允许的直接力矩变化步长。

所述步骤(13)中，通过调整Q,R对角线上元素的相对大小，以控制机械臂特定的关节完成控制目标。

所述步骤(3)中具体为：

在某个控制周期的i时刻，求解步骤(1)构建的机械臂零力控制目标函数，得到控制量序列u^★＝[u(1)，u(2)，...u(N)]；

取u(1)下发到机械臂底层控制器中，在该周期内采集机械臂系统状态信息

更新机械臂系统约束方程x_i＝X₀，取i时刻的机械臂系统输入序列u＝[u₁,u₂,...u_N]，得到i+1时刻机械臂系统的状态，求解步骤(1)构建的目标函数即得到第i+2时刻的机械臂各关节的控制量。

有益效果：

1)本发明通过模型预测进行机械臂零力控制，能够通过直接转矩进行零力控制并且保证安全、控制器性能调参数方式直观。

2)本发明中的机械臂零力控制方案本质上属于力矩控制，但是在约束中加入了关节位置和关节速度的约束，保障了零力控制的安全性。

3)本发明的系统状态设计取各关节的位置和速度，并不包括电流信息，控制器计算的力矩下发给驱动器电流环，保障了控制的稳定性。

4)本发明的目标函数中，可以通过权重矩阵的设置，让求解倾向于使用指定的关节，不需要机械臂的逆运动学求解，避免了运动过程中机械臂的奇异位型问题

5)本发明的目标函数可以根据机械臂零力控制的实际需求进行直观调整。

附图说明

图1为本发明的机械臂零力控制原理简图。

图2为本发明的机械臂模型预测零力控制流程图。

具体实施方式

下面结合附图和具体实施例，进一步阐明本发明。

图1为本发明的机械臂零力控制原理简图。如图1所示，本发明的机械臂零力控制方法采用模型预测控制器进行预测控制，其中定义机械臂的自由度为n个自由度。考虑到实际的应用场景，通常我们假设该n自由度机械臂在拖动时，整体运行速度较低，因此由关节加速度和惯量产生的负载τ_inertia以及由关节位置、速度产生的科氏效应负载τ_coriolis予以忽略不计，在系统模型中，相关项予以去除。

图2为本发明的机械臂模型预测零力控制流程图。如图2所示，本发明基于模型预测的机械臂零力控制方法包括如下步骤：

(1)n自由度机械臂的动力学模型可用下列抽象函数描述：

x_i+1＝H(x_i,u_i)

其中

表示i时刻机械臂系统状态，取各个关节在i时刻的位置和角速度；q_i表示i时刻机械臂各关节的位置；

表示i时刻机械臂各关节的角速度；u_i＝τ_control表示i时刻各关节的直接控制力矩；该抽象函数的意义是在当前时刻通过当前机械臂各关节的位置和角速度，在给定力矩τ_control输入时，计算下一时刻机械臂各关节的位置和角速度，即下一时刻机械臂系统状态；

(2)在给定机械臂系统状态x_i时，通过各关节驱动电机的力矩/电流及编码器读取各个关节的位置和角速度信息，据此得到机械臂末端的负载力矩：

τ_load[i]＝τ_gravity[i]+τ_friction[i]

其中，

τ_gravity[i]＝G(q_i)

τ_load[i]表示i时刻机械臂末端的负载力矩，τ_gravity[i]表示i时刻机械臂末端由重力产生的负载力矩，τ_friction[i]表示i时刻机械臂末端由摩擦力产生的负载力矩；G(q_i)表示i时刻各关节重力在机械臂末端的分量，

表示i时刻各关节摩擦力在机械臂末端的分量，二者均可根据获取得到的各关节驱动电机的力矩/电流，通过重力和摩擦力辨识得到；

(3)构建拖拽示教的机械臂零力控制目标函数：

其中，第一项的目标是尽力维持机械臂零力，期望找到合适的控制量序列[u(1),u(2),...u(N)]克服机械臂由重力和摩擦力产生的关节力矩负载，N表示预测控制总时间步长，控制量序列中各时刻控制量的初始值均为0；第二项的目标是辅助推动机械臂系统到示教点位，在拖拽示教模式下，机械臂系统的状态需要尽量接近拖拽示教模式下检测到的外部输入

即使得预测窗口内计算得到的负载力矩与直接控制力矩之和最小，另外使机械臂系统状态与期望状态偏差最小(即机械臂各关节的位置和角速度与期望位置和角速度之差的平方和最小)；

对角矩阵Q和R分别表示机械臂系统状态的轨迹权重矩阵和控制轨迹权重矩阵，分别表示机械臂系统状态量与期望状态之间的偏差权重和控制量与期望控制量之间的偏差权重；根据需要调节其相对大小，可实现在两个前述目标之间的权衡；另外，可以通过调整Q,R对角线上元素的相对大小，可实现控制机械臂特定的关节完成控制目标，例如用户倾向于使用前三轴，可以加大R₁₁ R₂₂ R₃₃三个元素的权重；

下面考虑约束项，首先机械臂零力控制需要满足下列等式约束：

x_i＝X₀

x_i+1＝H(x_i,u_i)

即当前机械臂系统状态x_i需要赋值成当前机械臂系统的状态测量值X₀，并对机械臂的动态系统模型约束，在给定机械臂系统状态和力矩输入后，下一个时刻的机械臂系统状态需要满足此方程约束；

除此以外，在零力拖动示教过程中，由于是直接力矩控制，为保障用户和设备安全，需要加入下列不等式约束，

X_min＜x_i＜X_max

U_min＜u_i＜U_max

ΔX_min＜|x_i+1-x_i|＜ΔX_max

ΔU_min＜|u_i+1-u_i|＜ΔU_max

其中，前两个不等式分别约束机械臂系统状态量和机械臂系统的输入量，这里对应是机械臂的关节位置限位、以及最大和最小允许的关节角速度，X_min、X_max分别表示最小和最大允许的机械臂系统状态；系统输入量是允许的最大关节力矩U_max和最小关节力矩U_min；后两个不等式分别约束机械臂系统状态的变化步长以及机械臂系统输入的变化步长，ΔX_min,ΔX_max分别表示最小和最大允许的机械臂系统状态变化步长，分别对应最小和最大的关节位置变化步长及关节角度变化步长；ΔU_min,ΔU_max分别表示最小和最大允许的直接力矩变化步长；

经过这两个不等式约束之后，求得的机械臂系统状态轨迹与机械臂系统输入轨迹会变得更为平滑；在实际机械臂系统中，如果后面两个约束导致求解时间变长或者找不到可行解，则可以放松相关约束，即调整前两个不等式中的最大值和最小值的大小，使其范围增大，使其变成系统目标函数的一部分，以降低求解难度；

(4)在某个控制周期的i时刻，求解步骤(3)构建的机械臂零力控制目标函数，得到控制量序列u^★＝[u(1),u(2),...u(N)]；则取u(1)下发到机械臂底层控制器中，在该周期内采集机械臂系统状态信息

更新机械臂系统约束方程x_i＝X₀，取i时刻的机械臂系统输入序列u＝[u₁,u₂,...u_N]，得到i+1时刻机械臂系统的状态，求解步骤(3)构建的机械臂零力控制目标函数即得到第i+2时刻的机械臂各关节的控制量；

(5)重复步骤(4)，则控制系统可以完成机械臂的零力控制。

以上详细描述了本发明的优选实施方式，但是本发明并不限于上述实施方式中的具体细节，在本发明的技术构思范围内，可以对本发明的技术方案进行多种等同变换(如数量、形状、位置等)，这些等同变换均属于本发明的保护范围。

Claims (2)

1.一种基于模型预测的机械臂零力控制方法，其特征在于：包括步骤：

(1)设计目标函数使得机械臂末端的负载力矩与直接控制力矩之和最小，且机械臂系统状态与期望状态偏差最小，并在关节位置和角速度限定范围内构建约束；

具体为：

(11)n自由度机械臂的动力学模型如下：

x_i+1＝H(x_i,u_i)

其中

表示i时刻机械臂系统状态，取各个关节在i时刻的位置和角速度；q_i表示i时刻机械臂各关节的位置；

表示i时刻机械臂各关节的角速度；u_i表示i时刻各关节的直接控制力矩；

(12)针对给定i时刻的机械臂系统状态x_i，通过各关节驱动电机的力矩/电流及编码器读取各个关节的位置和角速度信息，据此得到机械臂末端的负载力矩：

τ_load[i]＝τ_gravity[i]+τ_friction[i]

τ_gravity[i]＝G(q_i)

τ_load[i]表示i时刻机械臂各个关节的负载力矩，τ_gravity[i]表示i时刻机械臂各关节由重力产生的负载力矩，τ_friction[i]表示i时刻机械臂各关节由摩擦力产生的负载力矩；G(q_i)表示i时刻各关节重力在机械臂末端的分量，

表示i时刻各关节摩擦力在机械臂末端的分量，二者均可根据获取得到的各关节驱动电机的力矩/电流，通过重力和摩擦力辨识得到；

(13)构建目标函数：

其中，对角矩阵Q和R分别表示机械臂系统状态的轨迹权重矩阵和控制轨迹权重矩阵，x_cmd表示拖拽示教模式下检测到的外部输入，N表示预测控制总时间步长；

(14)构建约束：

X_min＜x_i＜X_max

U_min＜u_i＜U_max

ΔX_min＜|x_i+1-x_i|＜ΔX_max

ΔU_min＜|u_i+1-u_i|＜ΔU_max

其中，X_min、X_max分别表示最小和最大允许的机械臂系统状态；U_min、U_max分别表示机械臂系统允许输入的最小关节力矩和最大关节力矩；ΔX_min、ΔX_max分别表示最小和最大允许的机械臂系统状态变化步长，分别对应最小和最大的关节位置变化步长及关节角度变化步长；ΔU_min、ΔU_max分别表示最小和最大允许的直接力矩变化步长；

(2)读取当前机械臂各关节的位置和角速度信息，得到当前机械臂系统状态，并计算得到机械臂末端的负载力矩；

(3)求解步骤(1)的目标函数得到下一时刻机械臂各关节的直接控制力矩，据此控制机械臂，返回步骤(2)；

具体为：

在某个控制周期的i时刻，求解步骤(1)构建的机械臂零力控制目标函数，得到控制量序列u^＊＝[u(1),u(2),...u(N)]；

取u(1)下发到机械臂底层控制器中，在该周期内采集机械臂系统状态信息

更新机械臂系统约束方程x_i＝X₀，取i时刻的机械臂系统输入序列u＝[u₁,u₂,...u_N]，得到i+1时刻机械臂系统的状态，求解步骤(1)构建的目标函数即得到第i+2时刻的机械臂各关节的控制量；

(4)重复步骤(3)，并据此控制机械臂。

2.根据权利要求1所述的机械臂零力控制方法，其特征在于：所述步骤(13)中，通过调整Q,R对角线上元素的相对大小，以控制机械臂特定的关节完成控制目标。

Images