CN107590340A

CN107590340A - 一种机械臂外力估计方法及装置

Info

Publication number: CN107590340A
Application number: CN201710834869.XA
Authority: CN
Inventors: 熊蓉; 胡晋
Original assignee: Zhejiang University ZJU
Current assignee: Zhejiang University ZJU
Priority date: 2017-09-15
Filing date: 2017-09-15
Publication date: 2018-01-16

Abstract

本发明公开了一种机械臂外力估计方法及装置，包括存储器、处理器以及存储在存储器中并可在处理器上运行的计算机程序，处理器执行计算机程序时实现前述的机械臂外力估计方法的步骤。本发明通过对机械臂动力学建模以及使用卡尔曼滤波器进行外力估计，避免了额外力传感器的使用；通过监督学习模型进行补偿，机械臂动力学模型误差可以显著减小；另外通过引入卡尔曼滤波器,外力估计结果在观测噪声和模型误差存在的情况下的鲁棒性可以显著提升。

Description

一种机械臂外力估计方法及装置

技术领域

本发明涉及机械臂控制技术，尤其涉及一种机械臂外力估计方法和装置。

背景技术

近年来，随着工业领域应用机械臂的不断深入，机械臂感知环境接触力的能力越来远重要。现有方法主要依靠在机械臂末端加装额外力传感器。这会增加系统的成本并且减小系统的有效负荷；另外传感器输出带有很大噪声，且极易受到环境温湿度改变的影响，使得外力估计结果不可靠。

发明内容

为了克服现有技术的不足，本发明的目的之一在于提供一种机械臂外力估计方法，其能解决现有技术增加系统的成本并且减小系统的有效负荷，且外力测量结果含有较大噪声并极易受到环境温湿度干扰的问题。

本发明的目的之二在于提供一种机械臂外力估计装置，包括存储器、处理器以及存储在存储器中并可在处理器上运行的计算机程序，处理器执行计算机程序时实现前述的机械臂外力估计方法的步骤。

本发明的目的之一采用以下技术方案实现：

本发明公开了一种机械臂外力估计方法，包括以下步骤：

使用回归方法估计机械臂的刚体动力学模型；获取模型的估计误差；

使用监督学习方法补偿误差并得到完整动力学模型；

结合完整动力学模型与卡尔曼滤波器估计机械臂所受到外力。

作为进一步地改进，所述的使用回归方法估计机械臂的刚体动力学模型，具体为求解以下最优化问题：

其中，B表示待求解的机械臂刚体动力学模型参数，表示回归元矩阵，表示回归目标值。

作为进一步地改进，所述刚体动力学模型误差为其中，为动力学参数向量B的估计值。

作为进一步地改进，所述使用监督学习方法补偿误差并得到完整动力学模型，具体包含以下步骤：将机械臂关节位置、速度、加速度作为输入以及对应刚体动力学模型误差值作为期望值，采用监督学习方法进行训练得到补偿模型，根据刚体动力学模型以及补偿模型获得完整动力学模型。

作为进一步地改进，所述结合完整动力学模型与卡尔曼滤波器估计机械臂所受到外力，具体包含以下步骤：根据完整动力学模型生成过程方程和观测方程，根据过程方程和观测方程生成离散卡尔曼滤波器，利用卡尔曼滤波器估计外力的扰动力矩，根据扰动力矩计算外力。

本发明还公开了一种机械臂外力估计装置，包括：

第一建模模块，用于使用回归方法估计机械臂的刚体动力学模型；

第二建模模块，用于获取刚体动力学模型的估计误差，并使用监督学习方法补偿误差得到完整动力学模型，

外力估计模块，用于结合完整动力学模型与卡尔曼滤波器估计机械臂所受到外力。

作为进一步地改进，本发明所述第一建模模块包括

第一计算单元，用于计算回归元矩阵

第二计算单元，用于获得回归目标值

求解单元，用于计算的最优化解获得动力学模型参数B

作为进一步地改进，本发明所述第二建模模块具体包括

误差单元，用于获取刚体动力学模型的模型误差；

补偿模型单元，用于将机械臂关节位置、速度、加速度作为输入以及对应刚体动力学模型误差值作为期望值，采用监督学习方法进行训练得到补偿模型；

结合单元，用于结合刚体动力学模型以及补偿模型获得完整动力学模型。

作为进一步地改进，本发明所述外力估计模块具体包括

滤波器单元，根据完整动力学模型生成过程方程和观测方程，根据过程方程和观测方程生成离散卡尔曼滤波器；

第三计算单元，利用所述卡尔曼滤波器估计外力的扰动力矩，根据所述扰动力矩计算外力。

本发明还公开了一种机械臂外力估计装置，包括存储器、处理器以及存储在存储器中并可在处理器上运行的计算机程序，其特征在于，处理器执行计算机程序时实现如权利要求1-6中任一项所述的机械臂外力估计方法的步骤。

相比现有技术，本发明的有益效果在于：通过对机械臂动力学建模以及使用卡尔曼滤波器进行外力估计，避免了额外力传感器的使用；通过监督学习模型进行补偿，机械臂动力学模型误差可以显著减小；另外通过引入卡尔曼滤波器,外力估计结果在观测噪声和模型误差存在的情况下的鲁棒性可以显著提升。

附图说明

图1为本发明机械臂外力估计方法的流程示意图；

图2为本发明机械臂外力估计装置的结构示意图。

具体实施方式

本发明公开了一种机械臂外力估计方法和装置，其中该方法包括以下步骤：使用回归方法估计机械臂的刚体动力学模型；使用监督学习方法补偿误差并得到完整动力学模型；结合完整动力学模型与卡尔曼滤波器估计机械臂所受到外力。其中该装置包括存储器、处理器以及存储在存储器中并可在处理器上运行的计算机程序，处理器执行计算机程序时实现前述的机械臂外力估计方法的步骤。通过对机械臂动力学建模以及使用卡尔曼滤波器进行外力估计，避免了额外力传感器的使用。通过监督学习方法进行补偿，机械臂动力学模型误差可以显著减小。通过引入卡尔曼滤波器,外力估计结果在观测噪声和模型误差存在的情况下的鲁棒性可以显著提升。

具体地说，一种机械臂外力估计方法，包括以下步骤：

使用回归方法估计机械臂的刚体动力学模型；

使用监督学习方法补偿误差并得到完整动力学模型；

使用回归方法估计机械臂的刚体动力学模型，具体为求解以下最优化问题：

进一步地，所述回归元矩阵具体为：

其中分别代表时刻i的机械臂关节位置、速度及加速度测量值。

代表时刻i的回归元矩阵值。

进一步地，所述回归目标值具体为：

其中τ_i(i＝1…m)代表时刻i的机械臂关节力矩测量值。

使用监督学习方法补偿误差并得到完整动力学模型，具体包含以下步骤:

将机械臂关节位置、速度、加速度作为输入以及对应刚体动力学模型误差值作为期望值，采用监督学习方法进行训练得到补偿模型。

结合完整动力学模型与卡尔曼滤波器估计机械臂所受到外力，具体包含以下步骤：

根据刚体动力学模型和监督学习模型构成的完整动力学模型根据刚体动力学模型和监督学习模型构成的完整动力学模型生成过程方程和观测方程；根据过程方程和观测方程生成离散卡尔曼滤波器估计机械臂所受到外力。

一种机械臂外力估计装置，包括：

第一建模模块，用于计算机械臂的刚体动力学模型参数；

第二建模模块，用于补偿刚体动力学模型误差；

外力估计模块，用于估计机械臂所受到外力；

第一建模模块与第二建模模块，第二建模模块与外力估计模块信号连接。

进一步地，所述第一建模模块具体用于求解以下最优化问题：

第一建模模块包括：

第一计算单元，用于计算回归元矩阵具体为：

代表时刻i的回归元矩阵值。

第一建模模块还包括：

第二计算单元，用于获得回归目标值为：

其中τ_i(i＝1…m)代表时刻i的机械臂关节力矩测量值。

第二建模模块具体包含以下步骤:

外力估计模块具体包含以下步骤:

机械臂外力估计装置，包括存储器、处理器以及存储在存储器中并可在处理器上运行的计算机程序，处理器执行计算机程序时实现前述的机械臂外力估计方法的步骤。

下面，结合附图以及具体实施方式，对本发明做进一步描述，需要说明的是，在不相冲突的前提下，以下描述的各实施例之间或各技术特征之间可以任意组合形成新的实施例。

机械臂外力估计，是指通过关节处传感器估计机械臂末端与环境接触时产生的接触力；外力估计结果主要用于阻抗控制和混合力位控制等柔顺控制方法。用于估计外力的传感器为关节码盘，力矩传感器。传感器放置在机械臂的各关节内，获取各个时刻的关节位置、速度、力矩测量值。机械臂所受到外力利用关节位置、速度、力矩测量值以及机械臂动力学模型并设计相应观测器来计算。

实施例一

如图1为一种机械臂外力估计方法，包括以下步骤：

步骤S01、使用回归方法估计机械臂的刚体动力学模型。

根据刚体动力学理论，机械臂的刚体动力学模型可以表示为

其中分别代表关节位置、速度、加速度。M(q)，和G(q)分别代表惯性矩阵、科氏力\离心力矩阵和重力向量。估计刚体动力学模型实际上就是估计模型中的动力学参数。对于包含N个连杆的机械臂，第i个连杆(i＝1…N)的动力学参数由下面三部分组成：

1)质量：m_i

2)相对于连杆坐标系的质心坐标：r_i＝(r_ix,r_iy,r_iz)

3)相对于质心坐标的惯性张量：

为了估计模型中的动力学参数，基本步骤如下：

1、刚体动力学模型可以简化成只依赖于关节位置、速度、加速度的回归元方程与未知动力学参数向量B相乘的形式，有：

其中未知动力学参数向量B由原有的各连杆动力学参数(m_i，r_i，L_i，i＝1…N)重新线性组合得到。线性组合的目的是使得回归元方程满足列满秩的条件。

2、使用关节码盘和力矩传感器采集多个不同时刻的关节位置、速度、加速度、力矩测量值，构建回归方程：

求解未知参数B等价于求解以下最优化问题：

其中，B表示待求解的机械臂刚体动力学模型参数，表示回归元矩阵：

代表时刻i的回归元矩阵值

表示回归目标值：

其中τ_i(i＝1…m)代表时刻i的机械臂关节力矩测量值。

3、根据最小二乘理论，动力学参数向量B的估计值可采用以下公式计算：

模型的估计误差为

其中，为动力学参数向量B的估计值。

步骤S02、使用监督学习方法补偿误差并得到完整动力学模型。

步骤S01得到的刚体动力学模型与实际数据之间存在建模误差，通过引入监督学习模型进行补偿可以减小模型误差，提高力估计结果精度。

进一步地，步骤S02使用监督学习方法补偿误差并得到完整动力学模型具体包括以下步骤：

将不同时刻下机械臂关节位置、速度、加速度测量值作为输入以及对应刚体动力学模型估计误差值∈作为期望值，采用多层感知机神经网络进行训练得到补偿模型。多层感知机神经网络包括一个输入层，一个使用sigmond激活函数的隐含层以及一个使用线性激活函数的输出层。多层感知机神经网络的模型参数可以采用反向传播算法优化得到。反向传播算法根据现有技术可以实现，不再赘述。

进一步地，采用多层感知机神经网络进行训练得到补偿模型可以表示为下式：

进一步地，由刚体动力学模型叠加多层感知机模型构成的完整动力学模型可以表示为下式：

其中τ_RBD为步骤S01得到的刚体动力学模型，τ_MLP为步骤S02得到的多层感知机模型，表示均值为0、协方差矩阵为∑_m的高斯建模误差。

步骤S03、结合完整动力学模型与卡尔曼滤波器估计机械臂所受到外力。

机械臂传感器测量值包含噪声，采用卡尔曼滤波器进行外力估计可以显著提升外力估计结果在观测噪声和模型误差存在的情况下的鲁棒性。

进一步地，步骤S03结合完整动力学模型与卡尔曼滤波器估计机械臂所受到外力，具体包括以下步骤：首先根据刚体动力学模型和监督学习模型构成的完整动力学模型生成过程方程和观测方程；然后根据过程方程和观测方程生成离散卡尔曼滤波器估计机械臂所受到外力。下面详细叙述这两个步骤。

1、根据刚体动力学模型和监督学习模型构成的完整动力学模型生成过程方程和观测方程。

首先，机械臂关节力矩和外力扰动力矩的关系可以表示为下式：

τ_IDM＝τ_control+d

其中τ_IDM是步骤S02中得到的完整动力学模型，τ_control是机械臂控制器产生的已知控制力矩，d是由于外力作用带来的待估计扰动力矩。

进一步地，根据上面的机械臂关节力矩和外力扰动力矩的关系式生成的过程模型表示为下式：

x＝Ax+Bu+e_m

其中系统状态和d分别代表关节速度值和待估计扰动力矩值。系统矩阵C[I O]。系统输入τ_control代表已知的关节控制力矩值。系统过程噪声表示为协方差矩阵为∑_m的高斯噪声。

然后，由于关节速度可通过关节码盘传感器测量，观测模型可以根据以下公式生成：

y＝Cx+e_o

其中C[I O]。系统观测噪声表示为均值为0、协方差矩阵为∑_o的高斯噪声。

2、根据过程方程和观测方程生成离散卡尔曼滤波器估计机械臂所受到外力。

首先，将上一步中的过程方程和观测方程进行离散化，离散化后的过程方程和观测方程表示为下式：

x_k＝A_k-1x_k-1+B_k-1u_k-1+w_k-x

y_k＝C_kx_k+v_k

其中下标k，k-1代表不同时刻。离散化的系统方程A_k、B_k、C_k根据以下公式计算：

C_k＝C

其中T_S代表采样时间。

离散化的过程噪声w_k的协方差矩阵Q_k和观测噪声v_k的协方差矩阵

R_k根据以下公式计算：

Q_k＝M_3k ^TM_2k

然后，根据离散化的过程方程和观测方程可以设计卡尔曼滤波器进行外力估计，相应卡尔曼滤波器的具体更新步骤如下：

1)计算系统状态和协方差

2)计算最优卡尔曼增益

K_k＝P_k|k-1C_k ^T(C_kP_k|k-1C_k ^T+R_k)^-1

3)使用当前观测主更新系统状态

4)更新协方差

5)计算扰动力矩值

机械臂受到外力可以根据扰动力矩值和以下公式计算得到：

其中J^T代表机械臂雅克比矩阵的转置。

实施例二

如图2所示的机械臂外力估计装置，包括：

第一建模模块01，用于计算机械臂的刚体动力学模型参数；

第二建模模块02，用于补偿刚体动力学模型误差；

外力估计模块03，用于估计机械臂所受到外力。

进一步地，第一建模模块01具体用于求解以下最优化问题：

进一步地，第一建模模块01包括：

第一计算单元，用于计算回归元矩阵具体为：

代表时刻i的回归元矩阵值。

进一步地，第一建模模块01还包括：

第二计算单元，用于获得回归目标值为：

进一步地，第二建模模块02具体用于将机械臂关节位置、速度、加速度作为输入以及对应刚体动力学模型误差值作为期望值，采用监督学习方法进行训练得到补偿模型。

进一步地，外力估计模块03具体用于根据刚体动力学模型和监督学习模型构成的完整动力学模型生成过程方程和观测方程并根据过程方程和观测方程生成离散卡尔曼滤波器估计机械臂所受到外力。

本实施例中的装置与前述实施例中的方法是基于同一发明构思下的两个方面，在前面已经对方法实施过程作了详细的描述，所以本领域技术人员可根据前述描述清楚地了解本实施中的系统的结构及实施过程，为了说明书的简洁，在此就不再赘述。

为了描述的方便，描述以上装置时以功能分为各种模块分别描述。当然，在实施本发明时可以把各模块的功能在同一个或多个软件和/或硬件中实现。

通过以上的实施方式的描述可知，本领域的技术人员可以清楚地了解到本发明可借助软件加必需的通用硬件平台的方式来实现。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例或者实施例的某些部分所述的方法，如：一种存储介质，所述存储介质存储有计算机程序，所述计算机程序被处理器执行时实现前述机械臂外力估计方法的步骤。

描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的模块或单元可以是或者也可以不是物理上分开的，作为模块或单元示意的部件可以是或者也可以不是物理模块，既可以位于一个地方，或者也可以分布到多个网络模块上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。

本发明可用于众多通用或专用的计算系统环境或配置中。例如：个人计算机、服务器计算机、手持设备或便携式设备、平板型设备、多处理器系统、基于微处理器的系统、机顶盒、可编程的消费电子设备、网络PC、小型计算机、大型计算机、包括以上任何系统或设备的分布式计算环境等等。

上述实施方式仅为本发明的优选实施方式，不能以此来限定本发明保护的范围，本领域的技术人员在本发明的基础上所做的任何非实质性的变化及替换均属于本发明所要求保护的范围。

Claims

1.一种机械臂外力估计方法，其特征在于，包括以下步骤：

使用监督学习方法补偿误差并得到完整动力学模型；

2.如权利要求1所述的机械臂外力估计方法，其特征在于，所述的使用回归方法估计机械臂的刚体动力学模型，具体为求解以下最优化问题：

3.如权利要求2所述的机械臂外力估计方法，其特征在于，所述刚体动力学模型误差为其中，为动力学参数向量B的估计值。

4.如权利要求1所述的机械臂外力估计方法，其特征在于，所述使用监督学习方法补偿误差并得到完整动力学模型，具体包含以下步骤：将机械臂关节位置、速度、加速度作为输入以及对应刚体动力学模型误差值作为期望值，采用监督学习方法进行训练得到补偿模型，根据刚体动力学模型以及补偿模型获得完整动力学模型。

5.如权利要求1所述的机械臂外力估计方法，其特征在于，所述结合完整动力学模型与卡尔曼滤波器估计机械臂所受到外力，具体包含以下步骤：根据完整动力学模型生成过程方程和观测方程，根据过程方程和观测方程生成离散卡尔曼滤波器，利用所述卡尔曼滤波器估计外力的扰动力矩，根据所述扰动力矩计算外力。

6.一种机械臂外力估计装置，其特征在于，包括：

7.如权利要求9所述的机械臂外力估计装置，其特征在于，所述第一建模模块包括

第一计算单元，用于计算回归元矩阵

第二计算单元，用于获得回归目标值

求解单元，用于计算的最优化解获得动力学模型参数B。

8.如权利要求7所述的机械臂外力估计装置，其特征在于，所述第二建模模块具体包括

误差单元，用于获取刚体动力学模型的模型误差；

9.如权利要求7所述的机械臂外力估计装置，其特征在于，所述外力估计模块具体包括

10.一种机械臂外力估计装置，包括存储器、处理器以及存储在存储器中并可在处理器上运行的计算机程序，其特征在于，处理器执行计算机程序时实现如权利要求1-6中任一项所述的机械臂外力估计方法的步骤。