CN113305843A - 一种机械臂零力控制方法 - Google Patents

Abstract

本发明公开了一种机械臂零力控制方法，包括步骤：一、在机械臂基座处安装6维力矩传感器；二、根据基座作用力与机械臂末端作用力之间的关系计算得到机械臂末端等效外力，并据此建立机械臂笛卡尔空间的导纳控制模型；三、根据机械臂各关节驱动器的编码器得到的机械臂各关节角度，通过正运动学计算得到机械臂的末端位姿，并结合导纳控制模型计算得到机械臂末端的期望位置与期望运动速度；四、通过机械臂逆运动学与速度雅克比变换计算得到机械臂各关节的期望关节位置与期望关节速度；五、对机械臂各关节进行PD控制。本发明在机械臂基座布置6维力矩传感器，通过导纳控制改善机械臂的目标刚性，实现机械臂的零力控制，增强机械臂的柔顺功能。

Description

一种机械臂零力控制方法

技术领域

本发明本发明涉及机械臂控制领域，具体涉及一种机械臂零力控制方法。

背景技术

机械臂零力控制技术作为机械臂拖动示教的实现方式备受重视。目前存在两种常用的机械臂零力控制技术方案：基于位置的零力控制技术和基于直接转矩的零力控制技术。

基于直接转矩控制的零力控制技术由于缺乏位置环与速度环的保护，使得整机系统存在一定的安全隐患；而且在拖动启动时要克服静摩擦力，这往往难以在重载机械臂上取得良好的效果。基于位置控制的零力控制技术则将外力与经动力学模型计算的力相加并转换为相应的位置指令。由于需要测得机械臂各个关节的外力力矩，通常采用3种实现方式：基于电机驱动器电流测量、基于关节力矩传感器测量，基于末端6维力矩传感器的测量，但这三种测量方式都具有各自的局限性：利用机械臂关节驱动器电流方法测量外力严重依赖机械臂关节驱动器性能；通过关节力矩传感器测量外力则代价昂贵，需要在每一个关节布置扭矩传感器；使用机械臂末端6维力矩传感器测量外力则限制了拖动外力产生的位置。

考虑现有机械臂可行的柔顺控制方案实现零力控制，其基本可归结为两类：机械臂力/位置混合控制与机械臂阻抗控制。囿于实际场景多变且约束分解复杂，通过力/位置混合控制方法实现机械臂零力控制变得难以付诸实施。机械臂阻抗控制能够根据机器人末端作用力与位置偏差之间的动态关系,通过控制机械臂位移从而间接地控制机器人与环境间的作用力，此种控制方式可以大幅度降低机械臂末端力控的实现难度。

发明内容

发明目的：为了改进现有技术的不足之处，本发明提出一种新型的机械臂零力控制方法，能够提高人机交互水平、控制过程可靠柔顺。

技术方案：

一种机械臂零力控制方法，包括步骤：

步骤一、在机械臂基座处安装用于测量机械臂受力的6维力矩传感器，并通过机械臂各关节驱动器的编码器数据获取得到各关节角度、各关节速度及各关节加速度信息；

步骤二、通过6维力矩传感器测量得到机械臂基座受力信息，并根据基座作用力与机械臂末端作用力之间的关系计算得到机械臂末端等效外力，并据此建立机械臂笛卡尔空间的导纳控制模型；

步骤三、根据步骤一得到的机械臂各关节角度，通过正运动学计算得到机械臂的末端位姿，并根据步骤二得到的导纳控制模型计算得到机械臂末端在机械臂基座坐标系下的期望位置与期望运动速度；

步骤四、通过机械臂逆运动学与速度雅克比变换计算得到机械臂各关节的期望关节位置与期望关节速度；

步骤五、结合步骤四及当前关节位置与当前关节速度，对机械臂各关节进行PD控制，实现机械臂的末端的零力控制。

所述步骤二中，机械臂末端等效外力计算如下：

式中：M_c(q)为机械臂连杆相对机械臂基座坐标系的惯性矩阵，

为机械臂连杆相对机械臂基座坐标系的张量矩阵，g_c(q)为机械臂连杆相对机械臂基座坐标系的重力项，J^T为机械臂连杆相对机械臂基座坐标系的雅克比矩阵转置；

进而得到：

式中，(J^T)^*为J^T的广义逆矩阵。

所述步骤二中，建立机械臂笛卡尔空间的导纳控制模型如下所示：

式中，M_d为机械臂本体的目标惯性矩阵；D_d为目标阻尼矩阵；Z_d为目标刚度矩阵；x为当前机械臂末端在机械臂基座坐标系下的位置；

为当前机械臂末端在机械臂基座坐标系下的运动速度；

为当前机械臂末端在机械臂基座坐标系下的运动加速度；

将上式改写为：

式中，x_d为机械臂基座坐标系下的期望位置；

为机械臂基座坐标系下的期望运动速度；

为机械臂基座坐标系下的期望运动加速度；F_d为机械臂末端的期望外力；

由于在机械臂末端进行零力控制，此时机械臂末端的期望外力为0，因此F_d＝0，则上式改写为：

式中，Z_d为目标刚度矩阵；运动位置调节量ΔX＝x_d-x，

与

分别为ΔX的一阶导数和二阶导数，分别表示机械臂末端在机械臂基座坐标系下的速度调节量和加速度调节量。

所述步骤五中，对机械臂各关节进行PD控制具体如下：

(1)采用逆运动学将目标位姿转化为关节角度，得到各关节期望位置，并用如下式表示：

q_d＝f^-1(X_d)＝f^-1(X+ΔX)

式中，f^-1(X_d)为机械臂末端位姿矩阵的逆运动学求解函数；

(2)根据机械臂末端笛卡尔速度雅克比转换，结合步骤三得到机械臂末端在机械臂基座坐标系下的速度调节量，进而得到关节空间中各关节的期望速度，如下式表示：

式中，

为机械臂末端在机械臂基座坐标系中的速度，J为相应机械臂连杆相对机械臂基座的雅克比变换矩阵，J^*为J的广义逆矩阵；

(3)建立机械臂关节空间中各关节的PD控制器，以计算各驱动器电机的期望力矩：

式中：K_p为PD控制器中的比例环节系数，K_d为PD控制器中微分环节的系数，q_d与

的定义见步骤四中的变量定义，根据PD控制器生成关节电机的力矩命令，直接驱动关节电机运行。

本发明的有益效果在于：

1)本发明中的机械臂零力控制方案本质上属于位置控制，避开了基于直接转矩的零力控制时的启动静摩擦力补偿问题。

2)本发明在机械臂基座布置6维力矩传感器用于测量整个机械臂的受力，从而囊括了机械臂非末端受力的情况，可以根据传感器的实测值计算出机械臂末端的等效受力大小，拓宽了拖动外力及接触外力的施加位置。

3)该种外力的等效计算克服了采用机械臂关节电机驱动器电流测量外力的弊端。

4)通过导纳控制改善机械臂本体的目标刚性，实现机械臂本体末端的零力控制，增强机械臂的柔顺功能。

5)在关节空间上采用具有位置与速度保护的PD控制方式，且由于控制量为位置差与速度差的函数，增强了控制系统的鲁棒性与抗干扰能力。

6)机械臂本体的目标导纳特性可以根据机械臂零力控制的实际需求进行调整，即让机械臂本体表现出不同程度的柔性特性，可以极大的增强机械臂与人机交互的效果。

附图说明

图1为本发明的机械臂零力控制方法流程图。

图2为本发明的机械臂零力控制系统结构示意图。

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

具体实施方式

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

图1为本发明的机械臂零力控制方法流程图，如图1所示，本发明的机械臂零力控制方法包括如下步骤：

步骤一、在机械臂的基座处安装6维力矩传感器用以测量整个机械臂的受力情况，通过6维力矩传感器采集机器人机械臂基座处的受力信息F_b；同时通过读取机械臂各关节驱动器的编码器数据获取得到各关节角度q(即所有关节角度)、各关节速度

及各关节加速度信息

步骤二、当机械臂处于外力F作用时，对6维力矩传感器的测量值F_b进行均值滤波处理，并根据基座作用力与机械臂末端作用力之间的关系计算得到机械臂末端等效外力大小F_ext，如图1所示；计算公式如下所示：

式中：M_c(q)为机械臂相对机械臂基座坐标系的惯性矩阵，

为机械臂相对机械臂基座坐标系的张量矩阵，g_c(q)为机械臂相对机械臂基座坐标系的重力项，J^T为机械臂相对机械臂基座坐标系的雅克比矩阵转置；

可以得到：

式中，(J^T)^*为J^T的广义逆矩阵；

步骤三、根据机械臂驱动器关节角度q通过正运动学计算得到机械臂的末端位姿：

X＝f(q)

式中，X为机械臂末端在机械臂基座坐标系中的位姿，f(q)为机械臂关于关节角度q的正运动学变换公式函数；

步骤四、设计机械臂笛卡尔空间的导纳控制器，根据拖动的柔顺要求及经验值调整导纳控制器的目标惯性矩阵、目标阻尼矩阵，目标刚度矩阵；具体如下：

根据步骤二中得到的机械臂末端等效外力F_ext，建立机械臂笛卡尔空间的导纳控制模型如下式所示：

式中，M_d为机械臂本体的目标惯性矩阵；D_d为目标阻尼矩阵；Z_d为目标刚度矩阵；x为当前机械臂末端在机械臂基座坐标系下的位置；

为当前机械臂末端在机械臂基座坐标系下的运动速度；

为当前机械臂末端在机械臂基座坐标系下的运动加速度；

将上式改写为：

式中，x_d为机械臂基座坐标系下的期望位置；

为机械臂基座坐标系下的期望运动速度；

为机械臂基座坐标系下的期望运动加速度；F_d为机械臂末端的期望外力；

由于在机械臂末端进行零力控制，此时机械臂末端的期望外力为0，因此F_d＝0，则上式改写为：

式中，运动位置调节量ΔX＝x_d-x，

与

分别为ΔX的一阶导数和二阶导数，分别表示机械臂末端在机械臂基座坐标系下的速度调节量和加速度调节量；

步骤五、根据步骤四计算得到机械臂末端在机械臂基座坐标系下的位置调节量ΔX与速度调节量

并据此计算得到机械臂末端在机械臂基座坐标系下的期望位置X_d与期望运动速度

步骤六、针对机械臂想要运动到的期望位姿，根据机械臂逆运动学与速度雅克比变换得到机械臂各关节的期望关节位置q_d与期望关节速度

(1)采用逆运动学将目标位姿转化为关节角度，得到各关节期望位置q_d，调整后的得到机器人关节位置作为重塑后的期望位置，用如下式表示：

q_d＝f^-1(X_d)＝f^-1(X+ΔX)

式中，f^-1(X_d)为机械臂末端位姿矩阵的逆运动学求解函数；

(2)根据机械臂末端笛卡尔速度雅克比转换，结合步骤三得到机械臂末端在机械臂基座坐标系下的速度调节量，进而得到关节空间中各关节的期望速度如下式表示：

式中，

为机械臂末端在机械臂基座坐标系中的速度，J为相应机械臂连杆相对机械臂基座的雅克比变换矩阵，J^*为J的广义逆矩阵；

步骤七、根据步骤六计算得到的期望关节位置q_d与期望关节速度

结合当前关节位置q与当前关节速度

在机械臂关节空间对机械臂各关节进行PD控制，将计算得到的力矩τ下发至各机械臂关节驱动器，从而实现机械臂的末端的零力控制；具体为：

考虑关节力矩命令下发过程中的安全保护，建立机械臂关节空间中各关节的PD控制器，以计算各驱动器电机的期望力矩：

式中：K_p为PD控制器中的比例环节系数，K_d为PD控制器中微分环节的系数，q_d与

的定义见步骤四中的变量定义，根据PD控制器生成关节电机的力矩命令，直接驱动关节电机运行。

本发明中的机械臂零力控制方案本质上属于位置控制，避开了基于直接转矩的零力控制时的启动静摩擦力补偿问题；在机械臂基座布置6维力矩传感器用于测量整个机械臂的受力，从而囊括了机械臂非末端受力的情况，可以根据传感器的实测值计算出机械臂末端的等效受力大小，拓宽了拖动外力及接触外力的施加位置；通过导纳控制改善机械臂本体的目标刚性，实现机械臂本体末端的零力控制，增强机械臂的柔顺功能；在关节空间上采用具有位置与速度保护的PD控制方式，且由于控制量为位置差与速度差的函数，增强了控制系统的鲁棒性与抗干扰能力。

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

Claims (4)

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

步骤一、在机械臂基座处安装用于测量机械臂受力的6维力矩传感器，并通过机械臂各关节驱动器的编码器数据获取得到各关节角度、各关节速度及各关节加速度信息；

步骤二、通过6维力矩传感器测量得到机械臂基座受力信息，并根据基座作用力与机械臂末端作用力之间的关系计算得到机械臂末端等效外力，并据此建立机械臂笛卡尔空间的导纳控制模型；

步骤三、根据步骤一得到的机械臂各关节角度，通过正运动学计算得到机械臂的末端位姿，并根据步骤二得到的导纳控制模型计算得到机械臂末端在机械臂基座坐标系下的期望位置与期望运动速度；

步骤四、通过机械臂逆运动学与速度雅克比变换计算得到机械臂各关节的期望关节位置与期望关节速度；

步骤五、结合步骤四及当前关节位置与当前关节速度，对机械臂各关节进行PD控制，实现机械臂的末端的零力控制。

2.根据权利要求1所述的机械臂零力控制方法，其特征在于：所述步骤二中，机械臂末端等效外力计算如下：

式中：M_c(q)为机械臂连杆相对机械臂基座坐标系的惯性矩阵，

为机械臂连杆相对机械臂基座坐标系的张量矩阵，g_c(q)为机械臂连杆相对机械臂基座坐标系的重力项，J^T为机械臂连杆相对机械臂基座坐标系的雅克比矩阵转置；

进而得到：

式中，(J^T)^*为J^T的广义逆矩阵。

3.根据权利要求1所述的机械臂零力控制方法，其特征在于：所述步骤二中，建立机械臂笛卡尔空间的导纳控制模型如下所示：

式中，M_d为机械臂本体的目标惯性矩阵；D_d为目标阻尼矩阵；Z_d为目标刚度矩阵；x为当前机械臂末端在机械臂基座坐标系下的位置；

为当前机械臂末端在机械臂基座坐标系下的运动速度；

为当前机械臂末端在机械臂基座坐标系下的运动加速度；

将上式改写为：

式中，x_d为机械臂基座坐标系下的期望位置；

为机械臂基座坐标系下的期望运动速度；

为机械臂基座坐标系下的期望运动加速度；F_d为机械臂末端的期望外力；

由于在机械臂末端进行零力控制，此时机械臂末端的期望外力为0，因此F_d＝0，则上式改写为：

式中，Z_d为目标刚度矩阵；运动位置调节量ΔX＝x_d-x，

与

分别为ΔX的一阶导数和二阶导数，分别表示机械臂末端在机械臂基座坐标系下的速度调节量和加速度调节量。

4.根据权利要求1所述的机械臂零力控制方法，其特征在于：所述步骤五中，对机械臂各关节进行PD控制具体如下：

(1)采用逆运动学将目标位姿转化为关节角度，得到各关节期望位置，并用如下式表示：

q_d＝f^-1(X_d)＝f^-1(X+ΔX)

式中，f^-1(X_d)为机械臂末端位姿矩阵的逆运动学求解函数；

(2)根据机械臂末端笛卡尔速度雅克比转换，结合步骤三得到机械臂末端在机械臂基座坐标系下的速度调节量，进而得到关节空间中各关节的期望速度，如下式表示：

式中，

为机械臂末端在机械臂基座坐标系中的速度，J为相应机械臂连杆相对机械臂基座的雅克比变换矩阵，J^*为J的广义逆矩阵；

(3)建立机械臂关节空间中各关节的PD控制器，以计算各驱动器电机的期望力矩：

式中：K_p为PD控制器中的比例环节系数，K_d为PD控制器中微分环节的系数，q_d与

的定义见步骤四中的变量定义，根据PD控制器生成关节电机的力矩命令，直接驱动关节电机运行。

Images