CN103034123B - 基于动力学模型参数辨识的并联机器人控制方法 - Google Patents

Abstract

本发明提供了一种并联机器人的控制方法，包括如下步骤：S1、建立所述并联机器人的动力学模型；S2、根据所述并联机器人的动力学模型建立用于描述动力学辨识参数的最小二乘方程；S3、根据所述最小二乘方程，建立激励轨迹的优化准则，并且采用有限傅里叶级数来描述激励轨迹的数学模型；S4、控制并联机器人以最优激励轨迹作为期望运动轨迹，测量并计算实际运动轨迹；S5、利用辨识算法和实际运动轨迹对动力学模型参数进行辨识；S6、基于辨识动力学模型控制并联机器人的运动。本发明能够建立精确完整的动力学模型，从而能够精确地控制并联机器人的运动。

Description

基于动力学模型参数辨识的并联机器人控制方法

技术领域

本发明涉及机器人系统的控制方法，具体涉及基于动力学模型参数辨识的并联机器人控制方法。

背景技术

并联机器人是指基座和末端执行器之间包含多条运动链的机器人。由于拥有多条运动链，并联机器人的机械结构要比传统的串联机器人复杂得多，这使得并联机器人的运动学和动力学关系都非常复杂，而多条运动链对末端执行器运动的协调操作，更是给并联机器人的运动控制提出了挑战。在对并联机器人进行精确的运动控制时，往往要借助精确的动力学模型进行控制。并联机器人的动力学模型描述了并联机器人运动和各个关节力矩之间的关系，它是求解并联机器人前向动力学问题和反向动力学问题的基础。并联机器人前向动力学问题是在已知关节驱动力的情况下求解并联机器人的运动，而反向动力学问题则是在已知并联机器人运动的情况下求解各个关节的驱动力。由于并联机器人的多运动链结构和闭链约束，推导其动力学模型是比较复杂的。并联机器人动力学建模的主要方法归结为Newton-Euler法、Lagrange法，以及虚功原理(theprincipleofvirtualwork)三种方法，这三种方法在描述并联机器人的动态特性时是彼此等价的。有关并联机器人动力学的理论建模研究工作很多，但在理论模型基础上进一步实现动力学模型参数辨识的研究工作却很少。由于机械机构的复杂性所导致的装配与加工过程的复杂性，使得并联机器人的名义动力学模型参数往往不准确，特别是在考虑到关节摩擦力影响时，则必须通过实验来辨识并联机器人的模型参数。所谓名义动力学模型参数是指并联机器人关节连杆的质量、长度、质心、转动惯量的理论设计参数。因此，动力学模型参数辨识成为获得并联机器人精确动力学模型的唯一有效方法。

当前，人们充分认识到实现可靠、精确、有效的动力学辨识需要特别设计辨识实验。在进行辨识实验时，需要设计并联机器人的激励轨迹，所谓激励轨迹是指用于动力学辨识的并联机器人末端执行器的运动轨迹。在设计并联机器人的辨识实验时，当出现测量误差和驱动器扰动时，激励轨迹必须充分从而提供精确的参数估计。因此，为了改善辨识精度，并联机器人执行的激励轨迹需要在一定的准则下进行最优设计。目前，传统串联机器人动力学辨识的原理比较成熟，相应的激励轨迹设计方法也相对简单，这主要是由串联机器人的机械结构决定的。串联机器人的关节坐标是独立的，每一个关节的运动轨迹可以预先自由设计、互不约束。然而，对于并联机器人来说，由于其有效工作空间有限，并且存在多运动链构成的闭环约束，因此并联机器人的激励轨迹设计是一个挑战性问题，从而导致动力学模型参数辨识成为一个难以解决的技术问题。这个问题的复杂性主要体现在：首先，动力学模型方程非常复杂，难以设计出合适的参数辨识方法；其次，动力学模型参数的数值大小差别很大，某些小数值的参数的实际辨识结果不具有物理可实现性；第三，并联机器人末端执行器的有效工作空间非常小，在狭小的工作空间内难以设计充分激励轨迹，因此辨识的精度不高。为了实现并联机器人的精确动力学模型参数辨识，需要考虑其包含闭环运动链约束这一特殊机械结构。由于这种闭环运动链约束，并联机器人的关节坐标是彼此约束且耦合的，这也就是说，并不是所有的关节坐标都可以预先自由设计，因此必须利用广义坐标的独立性，在任务空间中进行激励轨迹的参数化。

发明内容

(一)要解决的技术问题

针对并联机器人包含多个运动学链的闭环约束，且末端执行器的有效工作空间有限，导致激励轨迹设计和动力学模型参数辨识非常困难，无法精确辨识全部动力学模型参数而导致的并联机器人控制精度不高的问题，本发明提供了一种基于动力学模型参数辨识的控制方法实现并联机器人的运动控制，该方法中利用的动力学模型参数的辨识方法能够在工作空间中统一地实现激励轨迹的优化和动力学模型参数的辨识。

(二)技术方案

本发明的并联机器人的控制方法用于控制并联机器人的运动，包括如下步骤：S1、建立所述并联机器人的动力学模型；S2、根据所述并联机器人的动力学模型建立用于描述动力学辨识参数的最小二乘方程；S3、根据所述最小二乘方程，建立激励轨迹的优化准则，并且采用有限傅里叶级数来描述激励轨迹的数学模型；S4、控制并联机器人以最优激励轨迹作为期望运动轨迹，测量并计算实际运动轨迹；S5、利用辨识算法和实际运动轨迹对动力学模型参数进行辨识；S6、基于辨识动力学模型控制并联机器人的运动。

所述激励轨迹的优化准则通过优化加权最小二乘方程中信息矩阵的D-optimality指标，获得最优的动力学辨识精度，所述信息矩阵完全取决于并联机器人的实际激励轨迹，从而将最优动力学辨识问题转化为激励轨迹的优化问题。

所述激励轨迹的数学模型用于描述工作空间的参数化激励轨迹，并且采用有限傅里叶级数来表示，从而得到多频率多幅值的参数化激励轨迹。

所述激励轨迹的参数最优化算法用于求解参数化激励轨迹中的轨迹参数，从而得到可实现最优动力学辨识的充分激励轨迹。

根据一种具体实施方式，一加权最小二乘辨识模块用于根据充分激励轨迹下的实验数据实现动力学辨识，估计并联机器人的惯性参数和摩擦力参数。

根据一种具体实施方式，一动力学模型的力矩验证模块用于实现对最优动力学辨识参数的初步验证，设计不同于激励轨迹的运动轨迹，分别基于名义的动力学模型参数和辨识的动力学模型参数计算驱动力矩，通过计算力矩和实际驱动力矩的比较来验证辨识的动力学模型参数。

根据一种具体实施方式，一动力学控制验证模块用于实现对最优动力学模型参数的最终验证，设计动力学前馈控制器，分别采用名义动力学模型参数和辨识动力学模型参数进行动力学补偿，通过比较不同参数下的动力学控制精度来验证辨识的动力学模型参数。

(三)有益效果

本发明基于并联机器人的动力学模型的全部惯性参数和摩擦力参数的最优辨识，从而能够建立精确完整的动力学模型，从而能够精确地控制并联机器人的运动。

附图说明

图1是本发明的并联机器人控制方法的流程图；

图2是本发明的并联机器人控制方法中的动力学前馈控制的原理示意图；

图3是本发明的并联机器人控制方法中获得的最优激励轨迹曲线图；

图4是本发明的一个实施例的并联机器人的主动关节力矩曲线，所述主动关节力矩包括由名义动力学模型或辨识动力学模型结合实际运动轨迹计算得到的关节驱动力矩，以及实际运动过程中并联机器人的关节控制力矩，其中图4A表示关节1的力矩，其中图4B表示关节2的力矩，其中图4C表示关节3的力矩；

图5是本发明的一个实施例的并联机器人的末端执行器的直线轨迹跟踪误差曲线，其中图5A表示X轴运动方向的轨迹跟踪误差，图5B表示Y轴运动方向的轨迹跟踪误差；

图6是本发明的一个实施例的并联机器人的末端执行器的圆周轨迹跟踪误差曲线，其中图6A表示X轴运动方向的轨迹跟踪误差，图6B表示Y轴运动方向的轨迹跟踪误差。

具体实施方式

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

图1是本发明的并联机器人控制方法的流程图。如图1所示，本发明的方法包括如下步骤：

S1、建立并联机器人的动力学模型；

S2、根据所述并联机器人的动力学模型建立用于描述动力学辨识参数的最小二乘方程；

S3、根据所述最小二乘方程，建立激励轨迹的优化准则，并且采用有限傅里叶级数来描述激励轨迹的数学模型；

S4、控制并联机器人以最优激励轨迹作为期望运动轨迹，测量并计算实际运动轨迹；

S5、利用辨识算法和实际运动轨迹对动力学模型参数进行辨识；

S6、基于辨识动力学模型控制并联机器人的运动。

优选地，在步骤S6完成之后还可以包括步骤S7，即对辨识动力学模型参数进行验证的步骤。

根据本发明，所述辨识算法是加权最小二乘辨识算法。

根据本发明的优选实施方式，所述辨识动力学模型参数的验证步骤包括力矩的计算验证和最终的实际控制实验验证。

对激励轨迹的优化时需要建立优化准则，根据本发明的优选实施方式，所述激励轨迹的优化准则是通过优化加权最小二乘方程中信息矩阵的D-optimality指标，获得最优的动力学辨识精度，所述信息矩阵完全取决于并联机器人的实际激励轨迹，从而将最优动力学模型参数辨识问题转化为激励轨迹的优化问题。

根据本发明的优选实施方式，所述激励轨迹的数学模型采用有限傅里叶级数来描述工作空间的激励轨迹，从而得到多频率多幅值的参数化激励轨迹。

根据本发明的优选实施方式，所述激励轨迹的参数最优化算法用于求解参数化激励轨迹中的轨迹参数，从而得到可实现最优动力学辨识的充分激励轨迹。

根据本发明的优选实施方式，所述加权最小二乘辨识算法用于根据充分激励轨迹下的实验数据实现动力学辨识，得到包括全部惯性和摩擦力参数的最优动力学模型参数。

根据本发明的优选实施方式，所述动力学模型的力矩验证是对最优动力学模型参数的初步验证，本发明通过设计不同于激励轨迹的运动轨迹，分别基于名义动力学模型参数和辨识动力学模型参数计算驱动力矩，通过比较计算的驱动力矩和实际的驱动力矩来验证最优动力学模型参数辨识的结果。

根据本发明的优选实施方式，所述动力学模型的实际控制验证是对最优动力学模型参数的最终验证。本发明设计动力学前馈控制器，分别采用名义动力学模型参数和辨识动力学模型参数进行动力学补偿，通过比较不同参数下的动力学控制精度来验证最优动力学参数辨识的结果。

下面分别对上述步骤S1～S7进行更加详细的说明，以体现本发明的优点和进步性。

步骤S1、建立并联机器人的动力学模型。

针对并联机器人机械结构的闭链约束，并联机器人的动力学模型可以通过“切分”的思想将闭链结构转化为带约束的开链结构来建立，而开链结构由一系列的串联机器人组成。

对于一个无约束的串联机器人而言，运动的动力学方程可写为

$M_i{\stackrel{\·\·}{q}}_i+C_i{\stackrel{\·}{q}}_i+f_i={\mathrm{\τ}}_i---\left(1\right)$

其中，q_i，和分别表示关节的角度、角速度和角加速度，τ_i和f_i表示关节驱动力和摩擦力，M_i是惯性矩阵，C_i是科里奥利矩阵，下标i＝1，...，n表示第n个串联机器人。

将所有无约束的串联机器人的动力学模型组合到一起，从而得到开链结构的动力学模型为

$M\stackrel{\·\·}{q}+C\stackrel{\·}{q}+f=\mathrm{\τ}---\left(2\right)$

方程(2)中符号的定义与方程(1)类似，唯一不同的是方程(2)中的符号代表的是整个开链结构而不是单个串联机器人。即，q，和分别表示并联机器人的关节的角度、角速度和角加速度，τ和f表示关节驱动力和摩擦力，M是惯性矩阵，C是科里奥利矩阵。考虑由闭链机械结构引起的约束力，并联机器人的动力学模型可写为

$M\stackrel{\·\·}{q}+C\stackrel{\·}{q}+f-A^T\mathrm{\λ}=\mathrm{\τ}---\left(3\right)$

方程(3)中的A^Tλ表示约束力向量，这里A表示闭环约束方程的微分，λ表示未知的约束力幅值。实际上，通过计算矩阵A的零空间可以消除约束力A^Tλ。

并联机器人中的关节摩擦力f通常需要考虑静摩擦力、库仑摩擦力和粘滞摩擦力的影响。一般地，摩擦力f的数学模型可写为

其中，函数的表达式和参数向量的选择取决于摩擦力模型。如果摩擦力模型中仅仅考虑库仑和粘滞摩擦力，则是关节角速度的线性函数。

S2、根据所述并联机器人的动力学模型建立用于描述动力学辨识参数的最小二乘方程。

虽然并联机器人的动力学模型的方程关于轨迹变量q， 是非线性的，但它关于模型参数是线性的。因此，将动力学模型参数从动力学模型方程中分离出来，可以得到关于这些模型参数的线性矩阵形式

其中，可以通过运动轨迹计算得到，模型参数向量包含惯性参数向量和摩擦力参数向量

在一段采样时间n_m内获取τ，q，和等变量信息，线性矩阵形式(5)可以用于估计模型参数从而得到最小二乘方程

其中， $Z:=\left[\begin{array}{c}D(q\left(t_1\right),\stackrel{\·}{q}\left(t_1\right),\stackrel{\·\·}{q}\left(t_1\right))\\ \·\\ \·\\ \·\\ D(q\left(t_{n_m}\right),\stackrel{\·}{q}\left(t_{n_m}\right),\stackrel{\·\·}{q}\left(t_{n_m}\right))\end{array}\right]$ 是观测矩阵， $y:={[\mathrm{\τ}\left(t_1\right)\·\·\·\mathrm{\τ}\left(t_{n_m}\right)]}^T$ 是测量向量，ρ表示零均值测量噪声向量，n_m＝r是采样数据的数目。

考虑到估计的模型参数向量只包含并联机器人独立的基本参数，因此，观测矩阵Z是列满秩的。通过最小化||ρ||₂可以获得模型参数的解。最小二乘解的灵敏度与观测矩阵Z最大和最小奇异值之比(即条件数)成反比。因此，观测矩阵Z的条件数Cond(Z)可以作为激励轨迹设计的一个基本准则。

如果测量噪声的某些性质可以获得，则辨识问题可以基于统计框架求解，采用极大似然估计方法计算。在统计框架中，关节坐标的测量是无噪声的，如果重新定义ρ为零均值的高斯噪声，则极大似然估计简化为加权最小二乘方法。通常假设ρ是零均值且相互独立的噪声信号，其标准差σ_ρ为

$C_{\mathrm{\ρ\ρ}}=E\left({\mathrm{\ρ}}^T\mathrm{\ρ}\right)={\mathrm{\σ}}_{\mathrm{\ρ}}^2{I}_r---\left(7\right)$

其中，E是期望符号，I_r为单位矩阵。误差协方差矩阵Q可以定义为

Q＝diag(s)，C_ρρ＝(Q^TQ)^-1(8)

其中，Q是一个(n×r)×(n×r)阶的对角矩阵，其对角元素向量s可以定义为

$s=\left[\begin{array}{ccc}{s}^1& \·\·\·& s^n\end{array}\right],s^i=\left[\begin{array}{ccc}\frac{1}{{\mathrm{\σ}}_{\mathrm{\ρ}}^i}& \·\·\·& \frac{1}{{\mathrm{\σ}}_{\mathrm{\ρ}}^i}\end{array}\right],i=1,\·\·\·,n---\left(10\right)$

其中，sⁱ是一个1×r阶的行向量。

采用协方差矩阵Q对最小二乘方程(6)进行加权得到

式中，y_w＝Qy，Z_w＝QZ，ρ_w＝Qρ。加权最小二乘解可以通过计算方程(11)的最小二乘解得到。加权最小二乘解使得加权误差的欧式范数最小即min[ρ^TQ^TQρ]。

S3、根据所述最小二乘方程，建立激励轨迹的优化准则，并且采用有限傅里叶级数来描述激励轨迹的数学模型。

步骤S31、根据所述最小二乘方程，建立激励轨迹参数的优化准则。

根据本发明，为了减小加权最小二乘解对于实际数据y_w和Z_w中误差的敏感度，观测矩阵Z_w的条件数应该尽可能地接近于1。条件数cond(Z_w)＝1意味着所有参数估计的绝对精度相同。因此，根据条件数Z_w可以定义优化准则其中Λ为实际驱动力矩的对角协方差矩阵。通过求解优化准则J来获得最优的激励轨迹，这一方法称之为D-optimality。D-optimality方法是求解激励轨迹最优参数的一个重要准则。

步骤S32、采用有限傅里叶级数来描述激励轨迹的数学模型，基于激励轨迹参数的优化准则，获得最优激励轨迹。

求解最优激励轨迹的过程，实质就是使得最小二乘方程观测矩阵的优化准则J最小。在满足优化准则J的条件下，激励轨迹的最优参数集ξ^*为

${\mathrm{\ξ}}^{*}=\underset{\mathrm{\ξ}}{\min }J\left(Z_w(q,\stackrel{\·}{q},\stackrel{\·\·}{q})\right)$ (12)

$\mathrm{subjecttog}(q,\stackrel{\·}{q},\stackrel{\·\·}{q})\≤0$

其中，是关于并联机器人驱动器力矩极限和工作空间约束的函数。最优激励轨迹的求解可以看作是一个带约束的优化过程，通过非线性优化方法来求解。

从最优参数集ξ^*的表达式(12)可知，观测矩阵Z_w的输入取决于关节角度q。不过，由于并联机器人机械结构的闭链约束，各个关节的运动是相互约束且相关的，导致关节角度向量q中的各分量不能预先独立定义。因此，基于运动学关系，关节角度向量q可以转化为任务空间中的广义坐标q_e，而广义坐标q_e中的各个运动分量是独立的。因此，仅仅与广义坐标q_e有关的优化问题可以写为

${\mathrm{\ξ}}^{*}=\underset{\mathrm{\ξ}}{\min }J\left(Z_w(q_e,{\stackrel{\·}{q}}_e,{\stackrel{\·\·}{q}}_e)\right)$ (13)

$\mathrm{subjecttog}(q_e,{\stackrel{\·}{q}}_e,{\stackrel{\·\·}{q}}_e)\≤0$

可见，方程(13)仅仅与独立的广义坐标q_e有关，坐标q_e成为需要优化的激励轨迹。因此，需要定义坐标q_e的数学模型，使得激励轨迹处于有限的工作空间内，且满足关节约束。为此，利用傅里叶级数来定义坐标q_e的参数化模型，从而得到多频率多幅值的参数化激励轨迹为

$q_{\mathrm{ej}}\left(t\right)=q_{\mathrm{ej}}^0+\underset{k=1}{\overset{n_H}{\mathrm{\Σ}}}[\frac{a_{\mathrm{jk}}}{2\mathrm{\πf}\·k}\sin (2\mathrm{\πf}\·k\·t)-\frac{b_{\mathrm{jk}}}{2\mathrm{\πf}\·k}\cos (2\mathrm{\πf}\·k\·t)]---\left(14\right)$

其中，t表示时间，j＝1，...，m表示激励轨迹的第j个广义坐标，n_H表示谐波级数的数目，f表示基本频率。在激励轨迹的参数化表达式(14)中，a_jk和b_jk是傅里叶级数的系数，即为优化过程中的变量。数值仿真实验表明谐波数目低至n_H＝3就足以确定充分的激励轨迹，因为更大的谐波数目n_H并不能改善最优的目标。

优化准则J可以保证参数估计的最小不确定性，在满足优化准则J的条件下，激励轨迹的最优参数可以采用非线性优化方法求解。在每一次优化过程中，激励轨迹利用包括三个谐波函数的傅里叶级数来参数化。实际优化过程中，将实际并联机器人关节运动和驱动器力矩的物理限制引入作为优化过程中的约束条件，采用Matlab^TM中的非线性约束优化函数fmincon来求解方程(14)中的最优参数，从而获得最优的激励轨迹。

S4、控制并联机器人以最优激励轨迹作为期望运动轨迹，测量并计算实际运动轨迹。

将最优激励轨迹作为并联机器人的期望跟踪轨迹，从而实现充分激励，基于充分激励条件下的实验数据实现加权最小二乘辨识。在辨识过程中，需要获得实验数据，如关节角度、角速度、角加速度和驱动力矩。对于并联机器人来说，只有主动关节转角可以通过绝对式光电编码器进行测量，关节的角速度则采用滤波器和数值差分算法来估计。为此，设计一个低通Butterworth滤波器来滤除主动关节转角中的噪声，并且采用Matlab提供的filtfilt函数来消除滤波信号的失真。之后，基于滤波后的关节转角，关节角速度则采用Matlab中的中心差分算法gradient来估计。为了进一步获得主动关节的角加速度，可以对滤波后的关节角度使用两次中心差分算法gradient计算得到。最后，基于并联机器人的运动学模型，被动关节的转角和角速度可以通过主动关节的转角和角速度计算得到，进一步，末端执行器的实际速度和加速度也可以计算得到。

采用与关节角度相同的离散时间滤波器对控制输入进行在线滤波，可以重构驱动器的力矩。假设并联机器人的激励轨迹重复运动了L次，在每次运动过程中进行n_m次数据采样。对于第j个广义运动坐标来说，相应驱动力矩测量值的采样方差可以计算为

${\mathrm{\σ}}_j^2=\frac{1}{n_m(L-1)}\underset{k=1}{\overset{n_m}{\mathrm{\Σ}}}\underset{l=1}{\overset{L}{\mathrm{\Σ}}}{({\mathrm{\τ}}_{\mathrm{jl}}\left(k\right)-{\stackrel{\‾}{\mathrm{\τ}}}_j\left(k\right))}^2,---\left(15\right)$

其中，τ_jl(k)表示在第l次运动过程中，第j个运动轴的驱动力矩在第k时刻的测量值，则表示在L次重复运动过程中，第j个运动轴的驱动力矩在第k时刻的均值。

S5、利用辨识算法和实际运动轨迹对动力学模型参数进行辨识。

根据实际运动轨迹，可以实现并联机器人的运动速度、加速度的估计，以及驱动器力矩的重构，从而获得用于动力学模型参数辨识的实验数据。在此基础上，基于加权最小二乘方程(11)，进行最小二乘求解，获得包括惯性参数和摩擦力参数在内的动力学模型参数。将辨识的动力学模型参数代入动力学模型(5)，从而建立实际的动力学模型。

S6、基于辨识动力学模型控制并联机器人的运动。

基于辨识动力学模型，加入期望速度和期望加速度，从而得到动力学前馈补偿模块。将动力学前馈补偿模块与传统的比例-微分(Proportional-Differential，PD)控制相结合，从而得到动力学前馈控制，并联机器人在动力学前馈控制作用下，可以实现有效的运动控制。图2是本发明提供的并联机器人控制方法中的动力学前馈控制的原理示意图。动力学前馈控制在PD控制的基础上，加入一个包含期望速度与期望加速度的前馈控制项，从而得到动力学前馈控制的控制律设计为

${\mathrm{\τ}}_a={\left(S^T\right)}^{+}(M_e{\stackrel{\·\·}{q}}_e^d+C_e{\stackrel{\·}{q}}_e^d+K_{p}e+K_v\stackrel{\·}{e})+f_a,---\left(16\right)$

其中，末端执行器的位置误差可写为(S^T)⁺表示速度雅克比矩阵的伪逆，满足S^T(S^T)⁺＝I。在方程(16)中，控制增益K_p和K_v是对称正定矩阵。考虑到动力学前馈控制和传统PD控制都包括类似的PD控制项，因此采用传统的误差试凑法来调整动力学前馈控制的增益如下：K_p＝diag(35000，35000)，K_v＝diag(300，300)。此外，方程(16)中的动力学矩阵M_e和C_e可以采用辨识的惯性参数计算得到，而摩擦力补偿项f_a可以采用辨识的摩擦力参数计算得到。

S7、对辨识动力学模型进行验证。

根据本发明，优选地，为了验证辨识得到的动力学模型参数，本发明设计了不同于最优激励轨迹的力矩验证轨迹，进行并联机器人的运动控制实验，并且采集实验过程中的运动轨迹数据。通过计算主动关节的驱动力矩，比较辨识模型和名义模型的精度。基于动力学模型方程(5)，利用辨识得到的惯性参数、摩擦力参数，以及实际轨迹数据，可以计算得到主动关节的力矩。如果辨识参数足够准确，那么通过动力学模型计算得到的主动关节力矩和实际控制力矩一致。否则的话，如果辨识得到的惯性和摩擦力参数不准确，那么计算得到的主动关节力矩和实际的控制力矩之间的误差就很大。

此外，在相同的动力学控制模块作用下，通过分析辨识参数和名义参数的不同运动结果，可以验证辨识模型的精度，从而进一步评价激励轨迹的优化准则。

实施例

下面通过一个具体实施例来说明本发明的方法。

在该实施例中，首先需要建立并联机器人的动力学模型，并且将该模型转化为加权最小二乘方程的形式。在加权最小二乘方程基础上，最优激励轨迹的计算可以基于优化准则得到。力矩测量值的方差可以根据前面给出的方程(15)来估计，得到和从而得到矩阵Λ＝diag{2.17，1.05}。优化准则J的最小值通过非线性优化迭代35次后获得，此时，得到优化准则的最小值J＝-101.139。

通过采用优化准则J，方程(14)定义的激励轨迹的参数优化结果如表I所示，对应的轨迹称之为最优激励轨迹。并联机器人最优激励轨迹的曲线图如图3所示，图中实线表示末端执行器X-轴方向的轨迹，虚线表示末端执行器Y-轴方向的轨迹。在最优动力学辨识方法的实现过程中，并联机器人重复执行最优激励轨迹10次，每5秒采样2500个运动位形点。基于最优激励轨迹获得的数据，采用加权最小二乘方法来估计惯性参数和摩擦力参数。惯性参数的估计结果如表II所示，同时也将惯性参数的名义值给出作为比较。摩擦力参数的估计值如表III所示，由表II和表III中参数构成的动力学模型称之为辨识的动力学模型。

表I最优激励轨迹的参数

表II动力学模型参数的辨识结果(kg·m2)

表III

摩擦力参数的辨识结果

在动力学模型的力矩验证实验中，并联机器人末端执行器的运动轨迹设计为包含多条直线的复杂曲线。实际轨迹的起始点为(0.21，0.30)，之后通过点(0.37，0.18)，(0.37，0.30)，(0.21，0.18)，最后回到起始点(0.21，0.30)。由惯性和摩擦力参数计算得到的驱动力矩和三个主动关节的实际控制力矩如图4所示。从图4中我们可以看出，与名义模型相比，由辨识模型得到的惯性和摩擦力参数计算出的驱动力矩更接近于实际的控制力矩。

为了进一步验证辨识得到的惯性和摩擦力参数组成的实际动力学模型，设计了动力学控制算法用于并联机器人的运动控制。在实际控制实验中，控制算法运行在PentiumIIICPU733MHz的计算机上，系统采样周期为2ms。选取工作空间中的直线和圆周作为期望轨迹来验证辨识结果。直线用来验证线性轨迹的跟踪精度，而圆周用来验证复杂连续轨迹的跟踪效果。对于直线来说，起始点为(0.22，0.29)，终止点为(0.37，0.21)。选取梯形曲线作为直线运动的速度轮廓，实现加减速运动过程，其中，最大速度为0.5m/s，加速度为10m/s²。对于圆周运动来讲，运动速度为0.5m/s，圆周的圆心为(0.29，0.25)，半径为0.04m。

在并联机器人的动力学控制实验中，为了与实验获得的辨识模型进行比较，我们还测试了名义模型。基于实际辨识动力学模型和名义动力学模型的跟踪误差曲线如图5和图6所示。末端执行器的直线轨迹跟踪误差如图5所示，其中实线表示由辨识动力学模型得到的跟踪误差，虚线表示由名义动力学模型得到的跟踪误差。从图5中我们可以看出，基于辨识动力学模型的跟踪误差更小。末端执行器的圆周轨迹跟踪误差曲线如图6所示，从图中可以发现基于辨识动力学模型得到的跟踪精度更高。从实际的轨迹跟踪控制实验可以知道，辨识动力学模型比名义动力学模型准确，得到的控制效果也更好。因此，本发明所建立的辨识动力学模型是准确的，这也就说明了本发明所提出的最优动力学辨识方法是有效的。

采用本发明提供的技术方案，能够实现以下的技术效果：

首先，本发明的方法能实现并联机器人全部动力学模型参数的精确辨识：包括惯性参数和摩擦力参数，从而建立精确、完整、可物理实现的动力学模型。在已有的并联机器人动力学辨识方法中，由于激励轨迹设计不充分以及辨识算法求解能力的限制，动力学辨识结果往往不准确甚至不具有物理可实现性，因此精确的动力学模型无法建立。

其次，本发明的方法中采用傅里叶级数来定义独立运动坐标的参数化模型，从而得到多频率多幅值的参数化激励轨迹，从而在理论模型上建立可行的充分激励轨迹。

第三，本发明的方法以加权最小二乘方程为基础，建立激励轨迹的优化准则，从而获得最优的激励轨迹，在最优激励轨迹作用下，并联机器人的动力学模型参数能够被充分激励，从而为实现最优动力学辨识提供必要条件。

第四，本发明的方法中采用加权最小二乘辨识算法实现惯性参数和摩擦力参数的估计，从而使得参数估计的绝对精度相同。在现有动力学辨识方法中，忽略了惯性参数在数值上大大小于摩擦力参数的这一物理特性，因此惯性参数的估计精度远小于摩擦力参数。

第五，本发明的方法中设计了动力学模型验证模块，能够对辨识结果进行双重的精确检验：一方面，力矩验证模块通过比较由最优动力学辨识参数得到的计算力矩和实际驱动力矩，实现对最优动力学辨识参数构建的辨识模型的精度验证；动力学控制验证模块分别采用名义动力学模型参数和最优动力学辨识参数实现动力学前馈控制器，通过比较不同参数下的动力学控制精度来验证最优动力学辨识的控制效果。

第六，本发明的方法整体结构清晰、合理，且辨识算法具有很好的扩展性：从结构上看，本发明的方法主要由激励轨迹的优化准则、激励轨迹的数学模型、激励轨迹的参数最优化算法、加权最小二乘辨识模块、动力学模型的力矩验证模块、动力学模型的实际控制验证模块六大部分组成，每一部分有其相应的功能：激励轨迹的优化准则、数学模型和参数最优化算法建立最优激励轨迹的三大功能模块，利用傅里叶级数来定义多频多幅的参数化轨迹，在激励轨迹的优化准则条件下，计算最优的轨迹参数，从而得到充分激励轨迹；加权最小二乘辨识模块包含了完整的数据采集、滤波和算法部分，能够实现精确的参数估计；动力学模型的验证模块具有双重的精确验证功能，既能验证关节力矩，又能验证最终的动力学控制精度。因此，本发明的方法各模块的功能明确，结构合理。

需要注意的是，上述系统实施例中，所包括的各个模块只是按照功能逻辑进行划分的，但并不局限于上述的划分，只要能够实现相应的功能即可；另外，各功能模块的具体名称也只是为了便于相互区分，并不用于限制本发明的保护范围。

以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种并联机器人的控制方法，用于控制并联机器人的运动，其特征在于，包括如下步骤：

S1、建立所述并联机器人的动力学模型：其中A^Tλ表示约束力向量，A表示闭环约束方程的微分，λ表示未知的约束力幅值，q，和分别表示并联机器人的关节的角度、角速度和角加速度，τ和f表示关节驱动力和摩擦力，M是惯性矩阵，C是科里奥利矩阵；

S2、根据所述并联机器人的动力学模型建立用于描述动力学辨识参数的最小二乘方程：其中， $Z=\left[\begin{array}{c}D\left(q\right(t_1),\stackrel{\·}{q}(t_1),\stackrel{\·\·}{q}(t_1\left)\right)\\ .\\ .\\ .\\ D\left(q\right(t_{n_m}),\stackrel{\·}{q}(t_{n_m}),\stackrel{\·\·}{q}(t_{n_m}\left)\right)\end{array}\right]$ 是观测矩阵，D表示动力学轨迹矩阵，是测量向量，是模型参数向量，ρ表示零均值测量噪声向量，n_m＝r是采样数据的数目；

步骤S31、根据所述最小二乘方程，建立激励轨迹参数的优化准则其中Λ为实际驱动力矩的对角协方差矩阵，Z_w是加权观测矩阵；

步骤S32、采用有限傅里叶级数来描述激励轨迹的数学模型，基于激励轨迹参数的优化准则，获得最优激励轨迹，所述激励轨迹的最优参数集ξ^*为， ${\mathrm{\ξ}}^{*}=\underset{{}_{\mathrm{\ξ}}}{\min }J\left(Z_w\right(q,\stackrel{\·}{q},\stackrel{\·\·}{q}\left)\right),$ 约束条件 $g(q,\stackrel{\·}{q},\stackrel{\·\·}{q})\≤0,$ 其中，是激励轨迹参数的优化准则，是激励轨迹参数的优化准则，ξ为激励轨迹参数的集合，ξ^*是该集合中最优的激励轨迹参数，是关于所述并联机器人驱动器力矩极限和工作空间约束的函数，

参数化激励轨迹为：

$q_{ej}\left(t\right)=q_{ej}^0+\underset{k=1}{\overset{n_H}{\Σ}}\[\frac{a_{jk}}{2{\mathrm{\πf}}_r\·k}\sin (2{\mathrm{\πf}}_r\·k\·t)-\frac{b_{jk}}{2{\mathrm{\πf}}_r\·k}\cos (2{\mathrm{\πf}}_r\·k\·t)\]$

其中，t表示时间，j＝1,…,m表示激励轨迹的第j个广义坐标，k表示谐波阶次变量，n_H表示谐波级数的数目，f_r表示基本频率，a_jk和b_jk是傅里叶级数的系数，即为优化过程中的变量；

S4、控制并联机器人以最优激励轨迹作为期望运动轨迹，测量并计算实际运动轨迹；

S5、利用辨识算法和实际运动轨迹对动力学模型参数进行辨识；

S6、基于辨识动力学模型控制并联机器人的运动。

2.如权利要求1所述的并联机器人的控制方法，其特征在于，在步骤S4中，所述并联机器人的主动关节转角通过测量而获得，所述关节的角速度则采用滤波器和数值差分算法来估计。

3.如权利要求1所述的并联机器人的控制方法，其特征在于，在步骤S6中，动力学前馈控制的控制律设计为

${\mathrm{\τ}}_a={\left(S^T\right)}^{+}(M_e{\stackrel{\·\·}{q}}_e^d+C_e{\stackrel{\·}{q}}_e^d+K_{p}e+K_v\stackrel{\·}{e})+f_a,$ 其中，末端执行器的位置误差可写为q_e表示末端执行器的实际位置坐标，表示末端执行器的期望位置坐标；M_e是由辨识的动力学参数计算得到的惯性矩阵，C_e是由辨识的动力学参数计算得到的科里奥利矩阵，f_a是由辨识的摩擦力参数计算得到的摩擦力补偿；(S^T)⁺表示速度雅克比矩阵的伪逆，满足S^T(S^T)⁺＝I，且控制增益K_p和K_v是对称正定矩阵。

4.如权利要求1所述的并联机器人的控制方法，其特征在于，在步骤S6之后还包括步骤S7：对辨识动力学模型进行验证。

5.如权利要求4所述的并联机器人的控制方法，其特征在于，所述步骤S7包括：进行并联机器人的运动控制实验，并且采集实验过程中的运动轨迹数据，通过计算主动关节的驱动力矩，比较辨识动力学模型和名义动力学模型的精度；并且，设计动力学前馈控制的控制律，分别采用名义动力学模型参数和辨识动力学模型参数进行动力学补偿，通过比较不同参数下的动力学控制精度来验证辨识的动力学模型参数。