CN112743541B - 一种无力/力矩传感器机械臂软浮动控制方法 - Google Patents

Abstract

本发明公开了一种无力/力矩传感器机械臂软浮动控制方法，包括：在每一个伺服控制周期内，通过离线建立的机械臂最小惯性参数集动力学模型，在线计算机械臂的模型参数集合；根据机械臂关节角度、关节输入转矩以及模型参数集合，构造外部作用耦合力矩的估算单元；根据外部作用耦合力矩的估算值，计算机械臂末端受到的外部作用力/力矩；根据外部作用力/力矩，计算机械臂在笛卡尔空间内期望的位姿矩阵；将笛卡尔空间期望的位姿矩阵发送至机器人运动控制单元，完成一个伺服控制周期内的软浮动控制。本发明可提供笛卡尔空间内6自由度任意方向上的软浮动功能，提高外力“软测量”精度，适用于不同的应用场合。

Description

一种无力/力矩传感器机械臂软浮动控制方法

技术领域

本发明涉及工业机器人控制技术领域，具体涉及一种无力/力矩传感器机械臂软浮动控制方法。

背景技术

目前，工业机器人已被广泛应用于电子、物流、化工等各个领域。当机器人在执行上下料任务过程中，由于工件摆放的位置偏差，采用传统的位置控制方法取件时必然会使得机械臂承受额外的附加外力，如果工件偏移量较大并强行取件，相当于与机械臂发生碰撞，导致作业失败。为了解决上述问题，工业机器人会提供软浮动功能，即当机械臂受外力作用时，可以顺应外力，偏离指定示教点。

中国专利CN109773781A公开了一种无传感器的工业机器人直线软浮动控制方法，该专利从笛卡尔空间位置控制角度提出了一种简单实用的软浮动直线控制策略，但这种思路需要预先调节机械臂各关节的伺服驱动器性能参数，使得机械臂在位置控制模式下的刚性降低。

从外力测量与控制的角度，一种简单易行的做法是在机械臂末端法兰上加装力/力矩传感器以测量外部作用力，然后采用基于位置的导纳控制策略实现柔顺控制。然而，力/力矩传感器的高昂价格限制了其广泛应用。在对测量精度要求不高的应用场合，另一种实现思路是采用力/力矩的“软测量”方法。

目前，在工业上被广泛应用的是基于机械臂逆动力学模型的直接外力估计方法(S.Haddadin，Robot collisions:A survey on detection,isolation,andidentification，IEEE Transactions on Robotics，2017)，这种方法需要利用关节转矩、角度、速度和加速度信息进行数值运算。但是，采用双重微分计算加速度信号会放大噪声，使得有效信号被噪声淹没。通过设计滤波器可以解决上述问题，但是滤波器的引入会带来信号输出的时滞，影响外力估计性能。另一种方法是通过设计观测器来实现外部作用力的估计，文献(G.Sebastian，Interaction force estimation using extended stateobservers:An application to impedance-based assistive and rehabilitationrobotics，IEEE Robotics and Automation Letters，2019)设计了扩张状态观测器(Extended State Observer,ESO)来估计外力，但是，这种方法仅适用于估计恒定或缓慢变化的相互作用力，适用于康复运动的助力训练，但是在工业机器人高动态的作业工况下将会失效。

发明内容

为此，本发明提供一种无力/力矩传感器机械臂软浮动控制方法，通过离线建立的机械臂最小惯性参数集动力学模型，在每个伺服控制周期内计算机械臂关节空间惯量矩阵、科氏向心力矩、重力矩以及摩擦力矩，并结合机械臂关节角度和关节输入转矩，构造外部作用耦合力矩的非线性估算单元，最终结合机械臂的运动学设计笛卡尔空间的期望位姿控制器，实现机械臂在笛卡尔空间内六自由度任意轴线上的软浮动控制。

本发明采用的技术方案是：

一种无力/力矩传感器机械臂软浮动控制方法，包括以下步骤：

步骤1，在每一个伺服控制周期T_s内，通过离线建立的机械臂最小惯性参数集动力学模型Σ_n，在线计算机械臂动力学模型参数集合{P_n}，包括关节空间惯量矩阵

科氏向心力矩

重力矩

及摩擦力矩

其中，q∈R^n×1表示机械臂关节角度，

表示关节角速度，n表示机械臂自由度；

步骤2，根据机械臂关节角度q，关节输入转矩τ以及步骤1得到的模型参数集合{P_n}，构造外部作用耦合力矩τ_m的估算单元Σ_o，满足：

其中，k∈N⁺表示采样时刻，z₀表示关节角度q的估算值，z₁表示关节角速度

的估算值，z₂表示外部作用耦合力矩τ_m的估算值，z₃表示外部作用耦合力矩变化量

的估算值，ξ_i(i＝0,...,3)为中间变量，ξ_i设计为：

其中，参数λ_i(i＝0,...,3)为估算性能微调参数，参数矩阵L为估算性能粗调参数，L＝diag(l_i),(i＝1,...,n)；

步骤3，根据步骤2得到的外部作用耦合力矩的估算值z₂，计算机械臂末端受到的外部作用力/力矩

其中，J∈R^6×n为机械臂雅可比矩阵；

步骤4，根据步骤3得到的外部作用力/力矩

计算机械臂在笛卡尔空间内期望的位姿矩阵X_d，X_d∈R^6×1满足：

其中，dX_d为中间变量，用于存储X_d在当前控制周期内的变化信息，M_l∈R^6×6为惯性矩阵，B_l∈R^6×6为阻尼矩阵，M_l及B_l用于调节软浮动控制的柔顺性能；

步骤5，将步骤4得到的笛卡尔空间期望的位姿矩阵X_d，发送至机器人运动控制单元，完成一个伺服控制周期内的软浮动控制，并重新转至步骤1执行。

进一步地，步骤1中所述的最小惯性参数集动力学模型Σ_n满足以下形式：

其中，

表示机械臂关节角加速度，

表示回归矩阵，μ∈R^p×1表示最小参数集，p表示最小参数集内元素数目。

进一步地，步骤1中所述的摩擦力矩

基于库伦摩擦加粘滞摩擦模型，

根据动力学模型Σ_n可直接获得其解析解形式，所述的关节空间惯量矩阵

科氏向心力矩

及重力矩

均为数值解形式，

及

由下式在每个控制周期在线计算获得；

其中，

表示

的第i行元素，

进一步地，步骤2中所述的估算性能微调参数λ_i(i＝0,...,3)的选取需满足s⁴+λ₀s³+λ₁s²+λ₂s+λ₃＝0的根均在复平面的左半平面内，所述的估算性能粗调参数L的选取应满足：

进一步地，在步骤4中，首先通过调节惯性矩阵M_l的大小，控制机械臂在笛卡尔空间内6自由度方向上的刚性强度，M_l越小，刚性越弱。

进一步地，其次通过调节B_l的大小，实现软浮动控制的阻尼性能，B_l越小，阻尼越小。

本发明的有益效果：

(1)与现有的基于无传感器的相互作用力估计方法，例如基于逆动力学模型的直接力估计和基于ESO的外力估计方法相比，本发明可提高外力“软测量”精度；

(2)与现有的软浮动控制方法相比，本方法可以提供笛卡尔空间内6自由度任意方向上的软浮动功能；

(3)本发明针对工业机械臂提出一套通用的软浮动控制方法，通过调节不同的性能参数可实现不同应用场合下的需要。

附图说明

图1是无力/力矩传感器机械臂软浮动控制设计框图

图2是六自由度工业机器人系统框图。

图3是本发明实验例1的六自由度工业机器人的1号关节外部作用力矩实际值与估算值对比图。

图4是本发明实施例1的六自由度工业机器人的2号关节外部作用力矩实际值与估算值对比图。

图5是本发明实施例1的六自由度工业机器人的3号关节外部作用力矩实际值与估算值对比图。

图6是本发明实施例1的六自由度工业机器人的4号关节外部作用力矩实际值与估算值对比图。

图7是本发明实施例1的六自由度工业机器人的5号关节外部作用力矩实际值与估算值对比图。

图8是本发明实施例1的六自由度工业机器人的6号关节外部作用力矩实际值与估算值对比图。

图9是本发明实施例1的六自由度工业机器人在笛卡尔空间沿X轴线的软浮动控制效果对比图。

图10是本发明实施例1的六自由度工业机器人在笛卡尔空间沿Y轴线的软浮动控制效果对比图。

具体实施方式

为了更好地理解本发明，下面结合附图及实施例进一步阐明本发明的内容，但本发明的内容不仅仅局限于下面的实施例。

实施例1

参阅图1，本实施例提出一种无力/力矩传感器机械臂软浮动控制方法，具体实现步骤如下：

步骤1，在每一个伺服控制周期T_s内，通过离线建立的机械臂最小惯性参数集动力学模型Σ_n，在线计算机械臂动力学模型参数集合{P_n}，包括关节空间惯量矩阵

科氏向心力矩

重力矩

及摩擦力矩

所述的最小惯性参数集动力学模型Σ_n满足以下形式：

其中，q∈R^n×1表示机械臂关节角度，

表示关节角速度，

表示机械臂关节角加速度，n表示机械臂自由度，

表示回归矩阵，μ∈R^p×1表示最小参数集，p表示最小参数集内元素数目。

所述的摩擦力矩

基于库伦摩擦加粘滞摩擦模型，

根据动力学模型Σ_n可直接获得其解析解形式，所述的关节空间惯量矩阵

科氏向心力矩

及重力矩

均为数值解形式，

及

由下式在每个控制周期在线计算获得；

其中，

表示

的第i行元素，

步骤2，根据机械臂关节角度q，关节输入转矩τ以及步骤1得到的模型参数集合{P_n}，构造外部作用耦合力矩τ_m的估算单元Σ_o，满足：

其中，k∈N⁺表示采样时刻，z₀表示关节角度q的估算值，z₁表示关节角速度

的估算值，z₂表示外部作用耦合力矩τ_m的估算值，z₃表示外部作用耦合力矩变化量

的估算值，ξ_i(i＝0,...,3)为中间变量，ξ_i设计为：

其中，参数λ_i(i＝0,...,3)为估算性能微调参数，参数矩阵L为估算性能粗调参数，L＝diag(l_i),(i＝1,...,n)。

所述的估算性能微调参数λ_i(i＝0,...,3)的选取需满足s⁴+λ₀s³+λ₁s²+λ₂s+λ₃＝0的根均在复平面的左半平面内，所述的估算性能粗调参数L的选取应满足：

步骤3，根据步骤2得到的外部作用耦合力矩的估算值z₂，计算机械臂末端受到的外部作用力/力矩

其中，J∈R^6×n为机械臂雅可比矩阵。

步骤4，根据步骤3得到的外部作用力/力矩

计算机械臂在笛卡尔空间内期望的位姿矩阵X_d，X_d∈R^6×1满足：

其中，dX_d为中间变量，用于存储X_d在当前控制周期内的变化信息，M_l∈R^6×6为惯性矩阵，B_l∈R^6×6为阻尼矩阵，M_l及B_l用于调节软浮动控制的柔顺性能；

首先通过调节惯性矩阵M_l的大小，控制机械臂在笛卡尔空间内6自由度方向上的刚性强度，M_l越小，刚性越弱；进一步，通过调节B_l的大小，实现软浮动控制的阻尼性能，B_l越小，阻尼越小。

步骤5，将步骤4得到的笛卡尔空间期望的位姿矩阵X_d，发送至机器人运动控制单元，完成一个伺服控制周期内的软浮动控制，并重新转至步骤1执行。

实验例1

采用实施例1的控制方法，对图2所示的一款6自由度工业机器人应用系统给出具体的实施方案，所采用的六自由度工业机器人标准DH参数如表1所示：

表1

i	d<sub>i</sub>	a<sub>i</sub>	α<sub>i</sub>	θ<sub>i</sub>
					1	0.3215	0.05	90	q<sub>1</sub>
2	0	0.27	0	q<sub>2</sub>
					3	0	0.07	90	q<sub>3</sub>
4	0.299	0	90	q<sub>4</sub>
					5	0	0	-90	q<sub>5</sub>
6	0.7085	0	0	q<sub>6</sub>

首先，离线辨识工业机器人的最小惯性参数集动力学模型Σ_n，根据辨识模型在每个控制周期T_s内计算关节空间惯量矩阵

科氏向心力矩

及重力矩

将所得到的数值解输入至估算单元Σ_o，Σ_o在机器人控制器中采用欧拉离散化形式实现：

其中，T_s＝0.001s,λ₀＝5,λ₁＝3,λ₂＝1.5,λ₃＝1.1,L＝diag{400,400,350,300,300,300}。

其次，在当前控制周期内，根据估算值z₂计算机器人末端所受外力

最后，完成笛卡尔空间内期望的位姿矩阵X_d的计算；

比较例1

采用与实施例1基本相同的流程方法对图2所示的一款6自由度工业机器人应用系统给出具体的实施方案，与实验例1不同的地方在于，在本比较例中，外部作用力矩“软测量”的实现采用文献(G.Sebastian，Interaction force estimation using extendedstate observers:An application to impedance-based assistive andrehabilitation robotics，IEEE Robotics and Automation Letters，2019)所提出的观测方法，即ESO观测器设计为：

其中，k₁＝diag(8,8,5,5,5,5)，k₂＝diag(50,50,40,40,40,40)，

k₃＝diag(300,300,200,200,200,200)。

效果检测：

在实验例1的六自由度机器人的末端施加外力，实验例1的机器人关节端受到外部作用力的实测效果与估计效果，以及与比较例1的估计效果的对比如图3～图8所示，在图3～图8中，“实际值”是通过传感器直接测量的机器人的关节外部作用力矩的值，“估计值”是按照实验例1的流程方法计算所得的关节外部作用力矩的值，“对比方法”是按照对比例1的流程方法计算所得的关节外部作用力矩的值，从图3～图8中可以看出，采用本发明提出的方法可以更有效的提高“软测量”的精度。

在实验例1的六自由度机器人的末端施加外力，实验例1的六自由度机器人在笛卡尔空间沿X轴线和Y轴线的软浮动控制效果对比如图9～图10所示，在图9～图10中，“实际值”是指通过传感器直接测量的机器人的末端外部作用力的值，“估计值”是指按照实验例1的流程方法计算所得的末端外部作用力的值，从图9～图10中可以看出，与基于力/力矩传感器测量的方法相比，本发明亦表现出良好的软浮动效果。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (6)

1.一种无力/力矩传感器机械臂软浮动控制方法，其特征在于，包括以下步骤：

步骤1，在每一个伺服控制周期T_s内，通过离线建立的机械臂最小惯性参数集动力学模型Σ_n，在线计算机械臂动力学模型参数集合{P_n}，包括关节空间惯量矩阵

科氏向心力矩

重力矩

及摩擦力矩

其中，q∈R^n×1表示机械臂关节角度，

表示关节角速度，n表示机械臂自由度；

步骤2，根据机械臂关节角度q，关节输入转矩τ以及步骤1得到的模型参数集合{P_n}，构造外部作用耦合力矩τ_m的估算单元Σ_o，满足：

其中，k∈N⁺表示采样时刻，z₀表示关节角度q的估算值，z₁表示关节角速度

的估算值，z₂表示外部作用耦合力矩τ_m的估算值，z₃表示外部作用耦合力矩变化量

的估算值，ξ_i(i＝0,...,3)为中间变量，ξ_i设计为：

其中，参数λ_i(i＝0,...,3)为估算性能微调参数，参数矩阵L为估算性能粗调参数，L＝diag(l_i),(i＝1,...,n)；

步骤3，根据步骤2得到的外部作用耦合力矩的估算值z₂，计算机械臂末端受到的外部作用力/力矩

其中，J∈R^6×n为机械臂雅可比矩阵；

步骤4，根据步骤3得到的外部作用力/力矩

计算机械臂在笛卡尔空间内期望的位姿矩阵X_d，X_d∈R^6×1满足：

其中，dX_d为中间变量，用于存储X_d在当前控制周期内的变化信息，M_l∈R^6×6为惯性矩阵，B_l∈R^6×6为阻尼矩阵，M_l及B_l用于调节软浮动控制的柔顺性能；

步骤5，将步骤4得到的笛卡尔空间期望的位姿矩阵X_d，发送至机器人运动控制单元，完成一个伺服控制周期内的软浮动控制，并重新转至步骤1执行。

2.根据权利要求1所述的无力/力矩传感器机械臂软浮动控制方法，其特征在于，步骤1中所述的最小惯性参数集动力学模型Σ_n满足以下形式：

其中，

表示机械臂关节角加速度，

表示回归矩阵，μ∈R^p×1表示最小参数集，p表示最小参数集内元素数目。

3.根据权利要求1所述的无力/力矩传感器机械臂软浮动控制方法，其特征在于，步骤1中所述的摩擦力矩

基于库伦摩擦加粘滞摩擦模型，

根据动力学模型Σ_n可直接获得其解析解形式，所述的关节空间惯量矩阵

科氏向心力矩

及重力矩

均为数值解形式，

及

由下式在每个控制周期在线计算获得；

其中，

表示

的第i行元素，

4.根据权利 要求1所述的无力/力矩传感器机械臂软浮动控制方法，其特征在于，步骤2中所述的估算性能微调参数λ_i(i＝0,...,3)的选取需满足s⁴+λ₀s³+λ₁s²+λ₂s+λ₃＝0的根均在复平面的左半平面内，所述的估算性能粗调参数L的选取应满足：

5.根据权利 要求1所述的无力/力矩传感器机械臂软浮动控制方法，其特征在于，在步骤4中，首先通过调节惯性矩阵M_l的大小，控制机械臂在笛卡尔空间内6自由度方向上的刚性强度，M_l越小，刚性越弱。

6.根据权利 要求5所述的无力/力矩传感器机械臂软浮动控制方法，其特征在于，其次通过调节B_l的大小，实现软浮动控制的阻尼性能，B_l越小，阻尼越小。

Images