CN111624941A - 一种面向未知环境的六自由度机器人力控方法 - Google Patents

Abstract

本发明公开了一种面向未知环境的六自由度机器人力控方法，属于机器人运动的柔顺控制领域，所述控制方法具体步骤如下：首先，获取力传感器零点数据、机器人腕部工具重力及重心坐标，确定机器人腕部工具与环境接触时产生的真实力与力矩；然后，通过机器人腕部工具与环境接触时产生的真实力与力矩，确定机器人末端运动的位控方向和力控方向；最后，获取机器人运动的参考轨迹，确定阻抗控制模型，完成机器人力控作业。本发明提出了通过对力传感器在线标定的方式，提高了力传感器的测量精度，为机器人的力控作业提供了准确的力觉感知信息，采用阻抗控制策略，来控制机器人的运动轨迹，在力控与位控之间的转换稳定，适应能力和鲁棒性较好。

Description

一种面向未知环境的六自由度机器人力控方法

技术领域

本发明公开了一种面向未知环境的六自由度机器人力控方法，属于机器人运动的柔顺控制领域。

背景技术

目前工业机器人的作业模式主要为仅基于位置的运动控制，缺乏在作业过程中对接触力的控制，为了更好地完成比如打磨抛光、恒力跟踪、精密装配等任务，需要机器人与接触环境间有力控制的能力。

通过在机器人末端安装六维力传感器，使机器人具备力觉感知能力，通过获取的力觉信息实现机器人与外界环境的接触力控制，但在力传感器的实际工作过程中，需要考虑力传感的测量误差，误差的产生主要为力传感器零点数据和机器人腕部工具重力的影响，故需要预先对力传感器进行在线标定，消除其零点和机器人腕部工具重力对读数的干扰，从而提高力传感器的测量精度。

机器人作业过程中的力控制，根据控制方式的不同，主要可以分为两种：力/位混合控制和阻抗控制。力/位混合控制将机器人在某些自由度上设置为位置控制模式，而在其它自由度上设置为力控制模式，但该控制方法必须建立精确的环境约束方程来实现确定的雅克比矩阵并计算坐标系，通过实时地反应任务要求的选择矩阵来决定力和位置的方向，故力/位混合控制很难实现未知环境下的力控制任务。阻抗控制通过力与位置之间的动态关系实现力控制任务，将机器人末端受到的力等效为“质量-阻尼-弹簧”模型，通过对惯性、阻尼、刚度的参数调节实现对力与位置的调整。阻抗控制根据控制原理的不同可分为基于力的阻抗控制和基于位置的阻抗控制两种。阻抗控制不直接控制机器人与接触环境的接触力，而是根据机器人末端位置和接触力之间的动态关系，来控制机器人末端的运动轨迹。阻抗控制不需要对自由空间的运动规划和算法进行调整，在自由运动和约束运动之间的转换比较稳定，适应能力和鲁棒性较强，在面对未知环境时，也能表现出良好的稳定性。

因此，有必要对力传感器进行在线标定，获取准确的力觉信息，然后得到未知环境的约束条件，确定机器人运动的参考轨迹，并结合阻抗控制，实现机器人在运动过程中对未知环境的力控制，使机器人可以应对更加复杂多变的工作环境。

发明内容

本发明的目的在于解决目前六自由度机器人在主动柔顺控制中，末端安装的力传感器存在较大的力觉感知误差，且机器人无法应对未知环境的力控作业，提出一种面向未知环境的六自由度机器人力控制方法，通过力传感器标定算法获取其零点及机器人腕部工具重力对力传感器产生的测量误差，使机器人可以获取准确的环境接触力，并通过反馈的力觉信息，得到未知环境的约束条件，最后采用阻抗控制实现机器人面对未知环境的力控作业。

本发明所要解决的问题是由以下技术方案实现的：

一种面向未知环境的六自由度机器人力控方法，所述控制方法具体步骤如下：

步骤S10，获取力传感器零点数据、机器人腕部工具重力及重心坐标，确定机器人腕部工具与环境接触时产生的真实力与力矩；

步骤S20，通过机器人腕部工具与环境接触时产生的真实力与力矩，确定机器人末端运动的位控方向和力控方向；

步骤S30，获取机器人运动的参考轨迹，确定阻抗控制模型，完成机器人力控作业。

优选的是，所述步骤S10的具体过程如下：

步骤S101，在机器人腕部安装工具后，获取机器人在N(N＞3)个不同位姿下的力传感器实际测得的力与力矩；

步骤S102，根据所述机器人的安装位置，确定机器人基坐标系，根据所述力传感器相对于所述机器人基坐标系的位置，确定力传感器坐标系，根据所述力传感器坐标系和所述机器人基坐标系，确定所述力传感器坐标系与机器人基坐标系的对应关系；

步骤S103，根据所述机器人腕部工具重力在机器人基坐标系的分力和所述力传感器坐标系与机器人基坐标系的对应关系，确定所述机器人腕部工具重力在力传感器坐标系上分力的对应关系；

步骤S104，根据力传感器实际测得的力和机器人腕部工具重力在力传感器坐标系上分力的对应关系，确定力传感器的零点力和所述机器人腕部工具重力分别在力传感器坐标系上的分力；

步骤S105，根据所述机器人腕部工具重力分别在力传感器坐标系上的分力和力传感器实际测得的力，确定所述机器人腕部工具重力；

步骤S106，根据所述力传感器实际测得的力与力矩和所述机器人腕部工具重心在所述力传感器上的坐标，确定所述机器人腕部工具重心在所述力传感器坐标系上的坐标；

步骤S107，根据所述力传感器的零点力和机器人腕部工具重心在所述力传感器坐标系上的坐标，确定力传感器的零点力矩；

步骤S108，根据所述力传感器的零点力与力矩和机器人腕部工具重力在传感器产生的力与力矩，确定机器人腕部工具与外界环境接触时产生的真实力与力矩。

优选的是，所述步骤S20的具体过程如下：

步骤S201，根据机器人腕部工具与外界环境接触时产生的真实力与力矩，确定机器人腕部工具与环境接触时产生的法向接触力、切向接触力以及所述法向接触力与竖直方向的夹角；

步骤S202，根据所述法向接触力与竖直方向的夹角，确定接触点处的单位法向量和单位切向量。

优选的是，所述步骤S30的具体过程如下：

步骤S301，根据所述接触点处的单位法向量和单位切向量，确定机器人运动时的位置控制方向和力控制方向；

步骤S302，根据所述法向接触力与法向位移，确定未知环境的刚度数据；

步骤S303，根据所述位置控制方向、力控制方向、法向期望接触力和法向接触力，确定机器人力控作业所需的参考轨迹；

步骤S304，根据所述机器人力控作业所需的参考轨迹，确定机器人力控作业的阻抗控制模型，并选取合适的模型参数，使机器人产生满足要求的运动序列。

本发明相对于现有而言具有的有益效果：

1.本发明提出了通过对力传感器在线标定的方式，提高了力传感器的测量精度，为机器人的力控作业提供了准确的力觉感知信息。

2.本发明提出了通过力传感器检测的力觉信息对未知环境的约束条件进行在线估计，获得了未知环境的法线方向和切向方向，确定了机器人的运动参考轨迹，使机器人可以应对更加复杂的作业环境。

3.本发明采用阻抗控制策略，不直接控制机器人与接触环境的接触力，而是根据机器人腕部工具位置和接触力之间的关系，来控制机器人的运动轨迹，在力控与位控之间的转换稳定，适应能力和鲁棒性较好。

附图说明

图1是本发明的机器人模型及坐标系示意图。

图2是本发明的机器人腕部工具重力在力传感器上产生力与力矩示意图。

图3是本发明的机器人腕部工具与环境的接触关系示意图。

图4是本发明的机器人力控方法结构图。

具体实施方式

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

在本发明的描述中，需要说明的是，术语“中心”、“上”、“下”、“左”、“右”、“竖直”、“水平”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。

如图1-4所示，本发明第一实施例在现有技术的基础上提供了一种面向未知环境的六自由度机器人力控方法，包括：控制方法具体步骤如下：

步骤S10，获取力传感器零点数据、机器人腕部工具重力及重心坐标，确定机器人腕部工具与环境接触时产生的真实力与力矩，对力传感器进行标定，具体步骤如下：

步骤S101，在机器人腕部安装工具后，获取机器人在N(N＞3)个不同位姿下的力传感器实际测得的力与力矩。将力传感器实际测得的力记为：F′＝[F′_x F′_y F′_z]^T，测得的力矩记为M′＝[M′_x M′_y M′_z]^T，力传感器的零点力记为：F₀＝[F_x0 F_y0 F_z0]^T，零点力矩记为： M₀＝[M_x0 M_y0 M_z0]^T，机器人腕部工具重力记为G(包括工具重力及力传感器敏感端的自身重力)，将机器人腕部工具重力作用在力传感器上的力记为：G_s＝[G_x G_y G_z]^T，力矩记为：M_s＝[M_Gx M_Gy M_Gz]^T。

步骤S102，如图1所述，根据机器人的位置，确定机器人基坐标系O₀_X₀Y₀Z₀位置(机器人基坐标系与世界坐标系重合)，令其Z₀轴为重力的反方向。根据力传感器相对于机器人基坐标系O₀_X₀Y₀Z₀的位置，确定力传感器坐标系，将力传感器坐标系记为O_s_X_sY_sZ_s，可由机器人基坐标系O₀_X₀Y₀Z₀先绕Z₀旋转角度W，再绕当前Y轴旋转角度y，最后绕当前X轴旋转角度U得到，机器人基坐标系O₀_X₀Y₀Z₀与力传感器坐标系O_s_X_sY_sZ_s的对应关系如公式(1) 所示。

步骤S103，根据机器人腕部工具重力在机器人基坐标系O_{0_}X₀Y₀Z₀的分力和机器人基坐标系O_{0_}X₀Y₀Z₀与力传感器坐标系O_s_X_sY_sZ_s的对应关系，将工具重力在机器人基坐标系O_{0_}X₀Y₀Z₀上的分力记为G₀＝[0 0 -G]^T，确定机器人腕部工具重力在力传感器坐标系 O_{s_}X_sY_sZ_s上分力的对应关系如下：

步骤S104，根据力传感器在仅安装机器人腕部工具的情况下，测量的数据主要由零点分力和机器人腕部工具重力G在力传感器上产生的分力组成，再根据公式(2)，将力传感器实际测得的力F′表示为：

其中，I为3×3的单位矩阵，记α＝[0 0 -G F_x0 F_y0 F_z0]^T，公式(3)可表示为 F′＝R·α，可整理为：

α＝(R^T·R)^-1·R^T·F′ (4)

步骤S105，将获取的N个不同位姿下的力传感器实际测得的力数据代入公式(4)中，可计算出α中的机器人腕部工具重力G和力传感器的零点力F₀。

步骤S106，根据力传感器实际测得的力与力矩和机器人腕部工具重心在所述力传感器上的坐标，将机器人腕部工具重心在力传感器O_{s_}X_sY_sZ_s上的坐标记为P＝[P_x P_y P_z]^T，根据末端工具重力G在坐标系O_{s_}X_sY_sZ_s上产生的力与力矩关系，如图2所述，可得机器人腕部工具重力G在力传感器上的力矩的计算公式如下：

根据公式(3)和公式(5)，可得力传感器实际测得的力矩的计算公式为：

记

根据公式(7)，整理公式(6)可得：

记β＝[P_x P_y P_z β_x β_y β_z]^T，公式(8)可表示为M′＝F′·β，整理可得：

β＝(F′^T·F′)^-1·F′^T·M′ (9)

把机器人在N(N＞3)个不同位姿下的力传感器实际测得的力与力矩带入公式(9)，可计算出机器人腕部工具重心在力传感器坐标系O_{s_}X_sY_sZ_s上的坐标P_x、P_y、P_z和常数β_x、β_y、β_z的值。

步骤S107，根据公式(7)确定力传感器的零点力矩M₀如下：

步骤S108，根据上述获得的力传感器的零点数据和机器人腕部工具重力在力传感器上产生的力与力矩，确定机器人腕部工具与环境接触时产生的真实力与力矩表示为：

步骤S20，通过机器人腕部工具与环境接触时产生的真实力与力矩，确定机器人末端运动的位控方向和力控方向，具体步骤如下：

步骤S201，根据机器人腕部工具与外界环境接触时产生的真实力与力矩，确定机器人腕部工具与环境接触时产生的法向接触力、切向接触力以及所述法向接触力与竖直方向的夹角，如图3所示。

根据力传感器标定后，可以实时并且准确地测量出机器人腕部工具与环境接触时的真实力与力矩信息，计算机器人腕部工具与环境接触时产生的法向力与切向力和真实力与力矩之间的关系如下：

其中，F_n为机器人腕部工具与环境的法向接触力，F_t机器人腕部工具与环境的切向接触力，θ为法向接触力与竖直方向的夹角，l为力传感器到机器人腕部工具末端半球的距离，r为工具末端半球的半径；

根据公式(11)，将力传感器实际测得的力与力矩代入公式(12)，确定法向接触力F_n、切向接触力F_t和法向接触力与竖直方向的夹角θ；

步骤S202，根据得取的法向接触力与竖直方向的夹角θ可确定未知环境在该接触点的单位法向量K_n和单位切向量K_t，计算公式如下：

步骤S30，获取机器人运动的参考轨迹，确定阻抗控制模型，完成机器人力控作业，具体步骤如下：

S301，根据公式(13)，确定未知环境下机器人腕部工具运动的切向方向为位置控制方向，法向方向为力控制方向。

S302，根据获取的机器人与环境接触产生的法向接触力F_n和机器人的法向位移ΔX，通过胡克定律F_n＝k_m·ΔX，确定未知环境的刚度参数k_m。

S303，根据机器人的位置控制方向与力控制方向以及法向期望力与实际力的偏差，确定机器人力控作业所需的参考轨迹X_r：

其中，k为采样时刻，T为采样时间，V为机器人末端速度，k_m为未知环境的刚度， K_n和K_t分别为运动的单位切向量和法向量，为避免产生累计误差，公式中的X(k-1)为上一次采样周期与环境接触的实际位置信息。

S304，根据公式(14)，将获取的机器人运动参考轨迹和机器人腕部工具与环境的期望力与实际接触力的偏差代入公式(15)的阻抗控制模型，并选取合适的惯性、阻尼、刚度参数产生满足要求的机器人力控运动序列。

式中：M_d——惯性矩阵；B_d——阻尼矩阵；K_d——刚度矩阵；

E_f——机器人末端与环境的实际接触力与期望力误差；

X(t)、

——机器人末端实际的位置、速度、加速度；

X_r(t)、

——机器人末端期望的位置、速度、加速度；

尽管本发明的实施方案已公开如上，但其并不仅仅限于说明书和实施方式中所列运用。它完全可以被适用于各种适合本发明的领域。对于熟悉本领域的人员而言，可容易地实现另外的修改。因此在不背离权利要求及等同范围所限定的一般概念下，本发明并不限于特定的细节和这里示出与描述的图例。

Claims (4)

1.一种面向未知环境的六自由度机器人力控方法，其特征在于，所述控制方法具体步骤如下：

步骤S10，获取力传感器零点数据、机器人腕部工具重力及重心坐标，确定机器人腕部工具与环境接触时产生的真实力与力矩；

步骤S20，通过机器人腕部工具与环境接触时产生的真实力与力矩，确定机器人末端运动的位控方向和力控方向；

步骤S30，获取机器人运动的参考轨迹，确定阻抗控制模型，完成机器人力控作业。

2.根据权利要求1所述的一种面向未知环境的六自由度机器人力控方法，其特征在于，所述步骤S10的具体过程如下：

步骤S101，在机器人腕部安装工具后，获取机器人在N(N＞3)个不同位姿下的力传感器实际测得的力与力矩；

步骤S102，根据所述机器人的安装位置，确定机器人基坐标系，根据所述力传感器相对于所述机器人基坐标系的位置，确定力传感器坐标系，根据所述力传感器坐标系和所述机器人基坐标系，确定所述力传感器坐标系与机器人基坐标系的对应关系；

步骤S103，根据所述机器人腕部工具重力在机器人基坐标系的分力和所述力传感器坐标系与机器人基坐标系的对应关系，确定所述机器人腕部工具重力在力传感器坐标系上分力的对应关系；

步骤S104，根据力传感器实际测得的力和机器人腕部工具重力在力传感器坐标系上分力的对应关系，确定力传感器的零点力和所述机器人腕部工具重力分别在力传感器坐标系上的分力；

步骤S105，根据所述机器人腕部工具重力分别在力传感器坐标系上的分力和力传感器实际测得的力，确定所述机器人腕部工具重力；

步骤S106，根据所述力传感器实际测得的力与力矩和所述机器人腕部工具重心在所述力传感器上的坐标，确定所述机器人腕部工具重心在所述力传感器坐标系上的坐标；

步骤S107，根据所述力传感器的零点力和机器人腕部工具重心在所述力传感器坐标系上的坐标，确定力传感器的零点力矩；

步骤S108，根据所述力传感器的零点力与力矩和机器人腕部工具重力在传感器产生的力与力矩，确定机器人腕部工具与外界环境接触时产生的真实力与力矩。

3.请根据权利要求1所述的一种面向未知环境的六自由度机器人力控方法，其特征在于，所述步骤S20的具体过程如下：

步骤S201，根据机器人腕部工具与外界环境接触时产生的真实力与力矩，确定机器人腕部工具与环境接触时产生的法向接触力、切向接触力以及所述法向接触力与竖直方向的夹角；

步骤S202，根据所述法向接触力与竖直方向的夹角，确定接触点处的单位法向量和单位切向量。

4.请根据权利要求1所述的一种面向未知环境的六自由度机器人力控方法，其特征在于，所述步骤S30的具体过程如下：

步骤S301，根据所述接触点处的单位法向量和单位切向量，确定机器人运动时的位置控制方向和力控制方向；

步骤S302，根据所述法向接触力与法向位移，确定未知环境的刚度数据；

步骤S303，根据所述位置控制方向、力控制方向、法向期望接触力和法向接触力，确定机器人力控作业所需的参考轨迹；

步骤S304，根据所述机器人力控作业所需的参考轨迹，确定机器人力控作业的阻抗控制模型，并选取合适的模型参数，使机器人产生满足要求的运动序列。

Images