CN109249397B - 一种六自由度机器人动力学参数辨识方法和系统 - Google Patents

Abstract

本申请实施例提供了一种六自由度机器人动力学参数辨识方法和系统，其中，该方法的步骤包括：基于利用能量原理构建的六自由度机器人能量守恒方程，获得六自由度机器人的功率模型；获取六自由度机器人跟踪预定周期性激励轨迹的运动数据；利用所述功力模型和运动数据，计算六自由度机器人动力学参数的估计值。本方案从能量原理出发，推导基于功率的参数辨识模型，过程简单、直观，并且可以通过迭代的形式计算，明显减小计算量；本方案不需要计算关节角加速度，降低了噪声对辨识结果的影响，且能够辨识六自由度机器人动力学参数(包括惯性参数、关节摩擦力矩、关节偏置力矩等)。

Description

一种六自由度机器人动力学参数辨识方法和系统

技术领域

本申请涉及机器人动力学领域，特别涉及一种六自由度机器人动力学参数辨识方法和系统。

背景技术

六自由度串联型工业机器人具有高度非线性、耦合性的特点。基于动力学的控制，提升其运动性能，一直是机器人控制领域的研究重点。获得精确的动力学参数，建立动力学模型，是对其机器人进行控制和仿真的关键。由于机器人机构的复杂性，在设计阶段无法对每个零件进行准确的建模，而在装配好之后也无法通过直接测量的手段获得精确的动力学参数。因此，通过对机器人进行动力学辨识实验，是获取其动力学参数精确估计值的有效方法。

大多数应用中，常用的机器人动力学参数辨识方法是基于逆动力学模型的方法。基于逆动力学模型的辨识方法的基本原理与过程概括为：在牛顿-欧拉方程的基础上，推导机器人动力学参数(惯性参数、动摩擦参数、关节偏置力矩)与各个关节力矩之间的线性关系，建立逆动力学辨识模型；使机器人跟踪特定的激励轨迹，同时对轨迹上的点进行采样，带入逆动力学模型得到以动力学参数为未知数的超定线性方程组；利用最小二乘法或其他方法求解动力学参数的估计值。

基于逆动力学模型的辨识方法，可以对机器人的惯性参数、动摩擦等动力学参数进行辨识。但是作为机器人动力学辨识方法，存在以下问题：

首先，虽然利用迭代牛顿-欧拉法计算关节力矩的效率很高，但是对于动力学参数辨识模型的推导过程十分繁琐，符号表达式冗长，而且会随着机器人关节数量的增加变得更加复杂。

此外，在逆动力学辨识模型的计算过程中，需要同时利用的关节角度、角速度以及角加速度值。对于关节角加速度，一般无法通过直接测量获得，只能通过对关节角度进行两次差分的方法获得角加速度的估计值，在实际应用中，将引入很大的噪声，影响辨识结果的准确性。

发明内容

为解决上述问题之一，本申请提供了一种六自由度机器人动力学参数辨识方法和系统。

根据本申请实施例的第一个方面，提供了一种六自由度机器人动力学参数辨识方法，该方法的步骤包括：

基于利用能量原理构建的六自由度机器人能量守恒方程，获得六自由度机器人的功率模型；

获取六自由度机器人跟踪预定周期性激励轨迹的运动数据；

利用所述功力模型和运动数据，计算六自由度机器人动力学参数的估计值。

根据本申请实施例的第二个方面，提供了一种六自由度机器人动力学参数辨识系统，该系统包括：

构建模块，基于利用能量原理构建的六自由度机器人能量守恒方程，获得六自由度机器人的功率模型；

采集模块，获取六自由度机器人跟踪预定周期性激励轨迹的运动数据；

计算模块，利用所述功力模型和运动数据，计算六自由度机器人动力学参数的估计值。

本申请所述技术方案从能量原理出发，推导基于功率的参数辨识模型，过程简单、直观，并且可以通过迭代的形式计算，明显减小计算量；本方案不需要计算关节角加速度，降低了噪声对辨识结果的影响，且能够辨识六自由度机器人动力学参数(包括惯性参数、关节摩擦力矩、关节偏置力矩)。

附图说明

此处所说明的附图用来提供对本申请的进一步理解，构成本申请的一部分，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：

图1示出本方案所述六自由度机器人动力学参数辨识方法的示意图；

图2示出本方案所述机器人连杆坐标系定义以及连杆参数定义示意图；

图3示出本方案所述参数辨识过程原理的示意图；

图4示出本方案所述验证实验原理的示意图；

具体实施方式

为了使本申请实施例中的技术方案及优点更加清楚明白，以下结合附图对本申请的示例性实施例进行进一步详细的说明，显然，所描述的实施例仅是本申请的一部分实施例，而不是所有实施例的穷举。需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。

本方案的核心思路是该方法从能量原理出发，推导基于功率的参数辨识模型，实现对机器人动力学参数的辨识。该方法推导过程简单直观，计算量明显减小，同时不需要计算关节角加速度，降低了噪声对辨识结果的影响。

如图1所示，本方案公开了一种六自由度机器人动力学参数辨识方法，该方法首先根据能量原理，建立六自由度机器人能量守恒方程；基于能量守恒方程，推导六自由度机器人的功率模型，用于动力学参数辨识；生成周期性激励轨迹；通过实时控制器控制机器人跟踪激励轨迹进行辨识实验，同时在控制周期内对机器人进行数据采集和处理；利用功率模型以及实验数据得到动力学参数估计值；利用辨识得到的动力学估计值计算关节力矩预测值，与采样得到的关节力矩实际值进行比较，验证参数辨识结果的准确性。

一、根据能量原理，建立六自由度机器人能量守恒方程。

如图2所示，为本方案所述机器人连杆坐标系定义以及连杆参数定义示意图。

机器人跟踪特定轨迹

运动时，满足能量守恒定理。即在在没有外力作用的条件下，相同时间内，关节力矩、动摩擦力矩及偏移力矩做功之和等于系统能量总和的变化量，于是得到的六自由度机器人能量守恒方程为：

其中，τ_dyn是机器人6个关节力矩构成的列向量；τ_fc和τ_fv分别是库伦摩擦力矩和粘性摩擦力矩向量；τ_off是6个关节驱动系统偏置力矩构成的列向量，包括功放的力矩偏移量和库伦摩擦力矩非对称因素；

是6个关节角速度构成的列向量；E(·)代表动能，E(t2)和E(t1)分别代表机器人在t2和t1时刻的动能；U(·)代表势能，U(t2)和U(t1)分别代表机器人在t2和t1时刻的势能。L(·)＝E(·)+U(·)代表机器人的总能量。每个关节上库伦摩擦力矩τ_fc和粘性摩擦力矩τ_fv分别为：

其中，Fc_j是关节j上的库伦摩擦力矩系数；Fv_j是关节j上的粘性摩擦力矩系数。Fc_j和Fv_j均属于待辨识的动力学参数。

二、基于能量守恒方程，推导六自由度机器人的功率模型，用于动力学参数辨识。具体推导过程如下：

1、计算机器人动能E(·)和势能U(·)及总能量L(·)表达式。

设机器人第j个连杆的关节角度为q_j，连杆动能E_j的表达式为：

其中^jω_j是连杆j的角速度在坐标系{j}中的描述，^jv_j是连杆j的线速度在坐标系{j}中的描述。^jω_j和^jv_j可以应用牛顿-欧拉方程向外迭代计算得到：

其中R和P分别是机器人动力学齐次变换中的旋转矩阵和平移向量。以连杆坐标系{0}为参考点，连杆势能U_j的表达式为：

式(3)至(5)表明，机器人的各个连杆的动能和势能表达式是每个连杆惯性参数的线性组合。因此，机器人动能E(·)和势能U(·)可以表示成机器人惯性参数的线性组合形式：

其中，Ψ_In＝[X₁，X₂，…，X_m]^T是机器人的全部m个惯性参数构成的列向量；

DE＝[DE₁，DE₂，…，DE_m]^T、DU＝[DU₁，DU₂，…，DU_m]^T分别是机器人动能和势能表达式中各个惯性参数前面的系数构成的向量；DL＝DE+DU是总能量表达式中各个惯性参数前面的系数构成的向量，可以表示为时间的函数DL(·)。

2、将能量守恒方程改写为动力学参数的线性方程形式。

利用式(2)和式(6)，将S1中的能量守恒方程改写为：

其中

Fv＝[Fv₁，Fv₂，…，Fv_n]^T，Fc＝[Fc₁，Fc₂，…，Fc_n]^T，τ_off＝[τ_off1，τ_off2，…，τ_offn]^T

方程(14)等号左边表示在[t1，t2]时间内关节力矩做功的总和，右边是机器人全部惯性参数、库伦摩擦力矩系数、粘性摩擦力矩系数以及偏置力矩的线性组合。

3、对能量守恒方程进行微分。

对于第2步中的能量守恒方程，令Δt＝t2-t1，且Δt足够小到等于机器人的1个控制周期，方程两边同时除以Δt，可以得到

当Δt足够小时，可以近似消掉含有积分计算的项，得到

其中

式(10)在于：在某一时刻，关节力矩、摩擦力矩、偏置力矩的总功率等于机器人总能量的变化率，这也是能量守恒方程在单位时间内的表现形式。

4、整理得到可以用于动力学参数辨识的功率模型。

在式(10)中令某时刻机器人6个关节总功率

得到基于功率的辨识模型：

P＝K_P·Ψ (12)

其中，

为机器人所有的动力学参数(包括惯性参数、动摩擦力矩系数、偏置力矩)构成的列向量；

是相应的观测矩阵。

式(12)表示了一个关于动力学参数Ψ和关节力矩功率的线性方程组，即基于功率的辨识模型。功率模型本质上是能量模型的一种表现形式，都是以能量守恒原理为基础进行推导的。辨识过程中，令Δt等于机器人控制周期，即相邻的两个控制周期内对关节力矩、角度、角速度进行两次连续采样，通过数值差分计算得到观测矩阵K_P中dDL^T，关节功率P以及dDFv^T，dDFc^T，

则可以利用采样结果直接计算得到。

三、生成周期性激励轨迹。

在机器人控制器内对机器人各个关节设置激励轨迹，使机器人跟踪设定的激励轨迹。本发明所述六自由度机器人动力学参数辨识方法使用的周期性激励轨迹为各个关节角度的组合正弦波，其表达式为：

其中，q_rj(t)为第j个关节角度；Δf_j为组合正弦波的基频；A_j为幅值。实际应用中，应根据实际机器人各个关节角度、角速度、角加速度的限制，设定合理的轨迹参数。

6个关节的激励轨迹表示为

四、通过实时控制器控制机器人跟踪激励轨迹进行辨识实验，同时在控制周期内对机器人进行数据采集和处理。

如图2所示，为本发明所述参数辨识过程原理示意图。

辨识实验过程中，需要在控制周期内采集的数据包括：

机器人6个关节的实际角度

利用角度传感器测量直接得到；

机器人6个关节的实际角速度

通过对角度差分和滤波进行估计；

机器人6个关节的力矩

依实际机器人硬件条件，可以利用相应的力矩传感器测量得到，也可以通过驱动系统电流测量估计得到。

对采集的数据进行辨识采样，比如控制周期为1mm时，可以选择间隔10mm进行一次辨识采样，每次采样需要提取相邻两个控制周期的数据，并且令S23中的Δt等于1个控制周期，然后利用采样数据计算S24中的总功率P和观测矩阵K_P。

对于采样时刻k，带入式(12)的辨识模型，可以得到以Ψ为未知数的线性方程

P(k)＝K_P(k)·Ψ (14)

观测矩阵的计算描述如下：

本发明所述的六自由度机器人动力学参数辨识方法，计算观测矩阵K_P的过程中，关节功率P以及dDFv^T，dDFc^T，

可以由采样结果直接计算得到，需要进行推导的，只有式(6)中连杆惯性参数在能量表达式中的系数，即DE_i和DU_i(i＝1，2，…，m)。将这些系数按连杆j＝1，2，…，6分组，表示为DE_i和DU_i

设牛顿-欧拉方程向外迭代计算得到的

^jω_i＝[ω₁，ω₂，ω₃]^T，^jv_j＝[v₁，v₂，v₃]^T (16)

带入式(3)中的连杆动能表达式，可以得到

根据式(5)的势能表达式，可以得到

通过式(17)、(18)可以看出，功率模型的推导过程简单、直观，并且可以通过迭代的形式计算，计算效率较高。同时，机器人的总功率是标量，因此基于功率的辨识方法中的观测矩阵的行数为1，而具有逆动力学模型的辨识方法中，由于计算的是关节力矩的广义向量，因此其观测矩阵的行数等于关节数量n。这说明基于功率模型的辨识方法相比于基于逆动力学模型的方法，推导过程简单直观，计算量明显减小。

五、利用功率模型以及实验数据得到动力学参数估计值。

参数辨识过程具体指：对于整个轨迹上的足够多的采样点，将数据带入上述基于功率的辨识模型，构成超定线性方程组

P＝K_P·Ψ (19)

利用最小二乘方法得到动力学参数估计值：

其中，P所有辨识采样时刻机器人的总功率构造的列向量；K_P是所有辨识采样时刻机器人功率观测矩阵按列排列组成的矩阵；

是机器人动力学参数的估计值。

六、利用辨识得到的动力学估计值计算关节力矩预测值，与采样得到的关节力矩实际值进行比较，验证参数辨识结果的准确性。

如图3所示，为本发明所述验证实验原理示意图。为了验证动力学参数辨识的正确性，一般选取一条不同的激励轨迹作为验证轨迹。利用辨识得到的动力学参数和轨迹模型以及逆动力学模型计算关节力矩的预测值，并与机器人跟踪验证轨迹得到的实际力矩实测值进行对比。

本方案进一步公开了一种六自由度机器人动力学参数辨识系统，该系统包括：

构建模块，基于利用能量原理构建的六自由度机器人能量守恒方程，获得六自由度机器人的功率模型；

采集模块，获取六自由度机器人跟踪预定周期性激励轨迹的运动数据；

计算模块，利用所述功力模型和运动数据，计算六自由度机器人动力学参数的估计值；

验证模块，利用所述动力学参数的估计值计算关节力矩预测值，并与采样得到的关节力矩实际值进行比较，验证参数辨识结果的准确性。

本方案中，所述六自由度机器人动力学参数辨识方法也可以通过例如关节位置控制器等电子设备实现其辨识功能，所述电子设备包括：存储器，一个或多个处理器；存储器与处理器通过通信总线相连；处理器被配置为执行存储器中的指令；所述存储介质中存储有用于执行如上所述方法中各个步骤的指令。

本方案中，所述六自由度机器人动力学参数辨识方法也可以记载于计算机可读存储介质中，通过计算机可读存储介质上存储有计算机程序实现辨识功能，该程序被处理器执行时实现如上所述方法的步骤。

本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

以上仅为本发明的实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均包含在申请待批的本发明的权利要求范围之内。

Claims (9)

1.一种六自由度机器人动力学参数辨识方法，其特征在于，该方法的步骤包括：

基于利用能量原理构建的六自由度机器人能量守恒方程，获得六自由度机器人的功率模型；

所述六自由度机器人的功率模型为：

P＝K_P·Ψ，

其中，P是某时刻机器人6个关节总功率；

为机器人所有的动力学参数构成的列向量，其中

表示机器人的全部m个惯性参数构成的列向量；F_v ^T表示机器人各关节上粘性摩擦力矩系数构成的列向量；F_c ^T表示机器人各关节上库伦摩擦力矩系数构成的列向量；τ_off ^T表示各关节上电机的偏置力矩；

是相应的观测矩阵；相邻的两个控制周期内对关节力矩、角度、角速度进行两次连续采样，通过数值差分计算得到观测矩阵K_P中dDL^T，关节功率P以及dDFv^T，dDFc^T，

则可以利用采样结果直接计算得到；

获取六自由度机器人跟踪预定周期性激励轨迹的运动数据；

利用所述功率模型和运动数据，计算六自由度机器人动力学参数的估计值。

2.根据权利要求1所述的六自由度机器人动力学参数辨识方法，其特征在于，所述利用能量原理构建的六自由度机器人能量守恒方程为：

其中，τ_dyn是机器人6个关节力矩构成的列向量；τ_fc和τ_fv分别是库伦摩擦力矩和粘性摩擦力矩向量；τ_off是6个关节驱动系统偏置力矩构成的列向量，包括功放的力矩偏移量和库伦摩擦力矩非对称因素；

是6个关节角速度构成的列向量；E(·)代表动能，E(t2)和E(t1)分别代表机器人在t2和t1时刻的动能；U(·)代表势能，U(t2)和U(t1)分别代表机器人在t2和t1时刻的势能；L(·)＝E(·)+U(·)代表机器人的总能量；每个关节上库伦摩擦力矩τ_fc和粘性摩擦力矩τ_fv分别为：

其中，Fc_j是关节j上的库伦摩擦力矩系数；Fv_j是关节j上的粘性摩擦力矩系数；Fc_j和Fv_j均属于待辨识的动力学参数。

3.根据权利要求1所述的六自由度机器人动力学参数辨识方法，其特征在于，所述预定周期性激励轨迹为各个关节角度的组合正弦波，其表达式为：

其中，q_r，j(t)为第j个关节角度；Δf_j为组合正弦波的基频；A_j为幅值；实际应用中，应根据实际机器人各个关节角度、角速度、角加速度的限制，设定轨迹参数；

6个关节的机器人跟踪激励轨迹表示为

其中q_r为各个关节角度，

为各个关节角速度，

为各个关节角加速度。

4.根据权利要求1所述的六自由度机器人动力学参数辨识方法，其特征在于，所述利用所述功率模型和运动数据，计算六自由度机器人动力学参数的估计值的步骤包括：

将运动数据带入功率模型，构建超定线性方程；

利用最小二乘法，获得动力学参数的估计值：

其中，P是所有辨识采样时刻机器人的总功率构造的列向量；K_P是所有辨识采样时刻机器人功率观测矩阵按列排列组成的矩阵；

是机器人动力学参数的估计值。

5.根据权利要求1所述的六自由度机器人动力学参数辨识方法，其特征在于，所述利用所述功率模型和运动数据，计算六自由度机器人动力学参数的估计值的步骤之后包括：

利用所述动力学参数的估计值计算关节力矩预测值，并与采样得到的关节力矩实际值进行比较，验证参数辨识结果的准确性。

6.一种执行权利要求1-5中任一项所述方法的六自由度机器人动力学参数辨识系统，其特征在于，该系统包括：

构建模块，基于利用能量原理构建的六自由度机器人能量守恒方程，获得六自由度机器人的功率模型；

采集模块，获取六自由度机器人跟踪预定周期性激励轨迹的运动数据；

计算模块，利用所述功率模型和运动数据，计算六自由度机器人动力学参数的估计值。

7.根据权利要求6所述的六自由度机器人动力学参数辨识系统，其特征在于，所述六自由度机器人的功率模型为：

P＝K_P·Ψ，

其中，P是某时刻机器人6个关节总功率；

为机器人所有的动力学参数构成的列向量，其中

表示机器人的全部m个惯性参数构成的列向量；F_v ^T表示机器人各关节上粘性摩擦力矩系数构成的列向量；F_c ^T表示机器人各关节上库伦摩擦力矩系数构成的列向量；τ_off ^T表示各关节上电机的偏置力矩；

是相应的观测矩阵；相邻的两个控制周期内对关节力矩、角度、角速度进行两次连续采样，通过数值差分计算得到观测矩阵K_P中dDL^T，关节功率P以及dDFv^T，dDFc^T，

则可以利用采样结果直接计算得到。

8.根据权利要求6所述的六自由度机器人动力学参数辨识系统，其特征在于，所述动力学参数的估计值：

其中，P是所有辨识采样时刻机器人的总功率构造的列向量；K_P是所有辨识采样时刻机器人功率观测矩阵按列排列组成的矩阵；

是机器人动力学参数的估计值。

9.根据权利要求7所述的六自由度机器人动力学参数辨识系统，其特征在于，该系统还包括：验证模块，利用所述动力学参数的估计值计算关节力矩预测值，并与采样得到的关节力矩实际值进行比较，验证参数辨识结果的准确性。

Images