CN112247984B - 一种变刚度关节轨迹跟踪控制方法 - Google Patents

Abstract

本发明提供了一种变刚度关节轨迹跟踪控制方法，步骤包括：根据被控制的变刚度关节机械臂建立动力学模型的状态方程，并拆分为连杆位置子系统和刚度电机子系统两部分，分别对各子系统建立虚拟控制律进行控制，最后对各子系统输出的位置电机力矩和刚度电机力矩做饱和补偿以及干扰补偿；本发明的控制方法，接收到外部的指令后，基于反馈回来的变刚度关节机械臂实际状态信息、受饱和约束后的电机力矩信息、扰动信息，对输出的电机力矩做实时修正，提高了跟踪控制精度。

Description

一种变刚度关节轨迹跟踪控制方法

技术领域

本发明属于仿生机器人领域，具体涉及一种变刚度关节轨迹跟踪的控制方法。

背景技术

在工业制造领域，机器人已有着广泛的应用，目前工业机器人主要采用刚性结构，在结构化的环境中，重复执行指定的单一动作。但是随着工业自动化需求范围的拓展和应用场景的进一步挖掘，例如零配件装配、工件打磨以及人机协作等，传统机器人的高机械刚度和控制刚度具有较大的局限性，因此刚度可调节的柔性机器人成为未来重要的发展方向。

变刚度关节是实现柔性机器人的重要组成部分，机器人通过关节获得柔性。目前变刚度关节通常采用双驱动结构，即采用位置电机和刚度电机共同驱动，其中位置电机调节关节位置、刚度电机调节关节刚度。

因变刚度关节存在强耦合、强非线性以及低阻尼高柔性的特点，如何实现较高的轨迹跟踪精度控制是亟待解决的问题。

发明内容

针对现有技术中存在不足，本发明提供了一种变刚度关节轨迹跟踪控制方法，提高对变刚度关节机械臂的控制精度。

本发明是通过以下技术手段实现上述技术目的的。

一种变刚度关节轨迹跟踪控制方法，具体为：

建立连杆位置子系统状态方程为：

针对连杆位置子系统状态方程，定义系统跟踪误差为：

e₁＝q-q^d

e₃＝τ_s-α₂

其中α₁、α₂、α₃是连杆位置子系统虚拟控制律，且虚拟控制律满足如下表达式：

对位置电机力矩τ_pos进行饱和补偿和干扰补偿，输出位置电机力矩u₁；

建立刚度电机子系统状态方程为：

针对刚度电机子系统状态方程，采用PD控制器对刚度电机子系统跟踪控制，定义系统跟踪误差为：

则刚度电机子系统的虚拟控制律满足如下表达式：

对刚度电机力矩进行饱和补偿和干扰补偿，输出刚度电机力矩u₂；

上述式中

q为变刚度关节机械臂角度位置，q^d是变刚度关节机械臂角度位置指令，

是变刚度关节机械臂的角速度，

是变刚关节机械臂角速度指令，τ_s是变刚度关节机械臂的弹性力矩，

是弹性力矩估计值，δ_τ是弹性力矩扰动，

是弹性力矩变化率的扰动，δ_stiff是刚度电机扰动，τ_cam是刚度电机侧的弹性力矩分量，θ_pos是位置电机角度位置，

是位置电机角速度，

是位置电机角速度指令，u₁是位置电机力矩通用表达符号，θ_stiff是刚度电机角度位置，

是刚度电机角度位置指令，

是刚度电机角速度，

是刚度电机角速度

的导数，u₂是刚度电机力矩通用表达符号，

是变刚度关节形变指令值，

是τ_s相对

的偏微分，J_pos是位置电机惯量矩阵，J_stiff是刚度电机惯量矩阵，M(q)是变刚度关节机械臂的惯性矩阵，

是变刚度关节机械臂的科里奥利矩阵，

是变刚度关节机械臂的库伦摩擦矩阵，G(q)是变刚度关节机械臂的重力矩矩阵；

是变刚度关节械臂角速度指令

的转置、τ_pos是未考虑饱和特性时的位置电机力矩；

是PD控制器的控制刚度系数，

是PD控制的控制阻尼系数，c₁、c₂、c₃、c₄是控制增益，

是e_stiff的导数。

进一步地，所述连杆位置子系统虚拟控制律的表达式，采用指令滤波方法求解其中的导数，二阶指令滤波器的动态方程为：

其中，

是滤波器的阻尼系数、

是滤波器的自然频率、Φ是滤波器的输入值，z₁、z₂是滤波中间变量，

分别是z₁、z₂的导数。

进一步地，所述采用指令滤波方法求解导数，还需引入误差补偿；

经指令滤波后，所述连杆位置子系统跟踪误差重新定义为：

其中α_2f和α_3f分别是α₂和α₃由指令滤波方法求得的值；

经指令滤波后，所述连杆位置子系统虚拟控制律变为：

其中

为

的逆矩阵，

分别是α₂、α₃经指令滤波方法求得的导数，

是滤波方法求导引入的滤波器估计误差，其具体定义为

且满足如下动态方程：

进一步地，所述位置电机力矩饱和补偿为-k₃ε_pos，其中ε_pos是位置电机的饱和补偿，k₃是位置电机的饱和补偿系数，且ε_pos由如下动态方程求得：

其中k₄是位置电机的补偿增益矩阵，

是考虑饱和特性后的位置电机力矩。

进一步地，所述位置电机力矩的干扰补偿，采用基于动量的干扰观测器获得，所述连杆位置子系统虚拟控制律算上饱和补偿以及干扰补偿后，其表达式变为：

其中

是

的逆矩阵，

弹性力矩扰动估计值，且

是弹性力矩变化率扰动估计值；

和

由干扰观测器得到，其表达式为：

其中

是干扰观测器的增益对角矩阵，

是干扰观测器的增益。

进一步地，所述刚度电机力矩饱和补偿为-k₅ε_stiff，其中ε_stiff是刚度电机的饱和补偿，

是刚度电机的饱和补偿系数，且ε_stiff由如下动态方程求得：

其中

是刚度电机饱和补偿增益矩阵，τ_stiff是未考虑饱和特性的刚度电机力矩，

是考虑饱和特性后刚度电机力矩。

进一步地，所述刚度电机力矩干扰补偿，采用基于动量的干扰观测器获得，所述刚度电机子系统虚拟控制律算上饱和补偿以及干扰补偿后，其表达式变为：

其中，

刚度电机扰动估计值，由干扰观测器得到，其表达式为：

其中

是干扰观测器观测刚度电机时的增益对角矩阵。

本发明的有益效果为：

(1)本发明通过将变刚度关节机械臂动力学状态方程解耦拆分为连杆位置子系统和刚度电机子系统两部分，将高阶的控制系统降阶为对各个子系统的控制，简化控制难度；通过根据反馈回来的实际状态与设定状态进行比较确定跟踪误差，进而设计各子系统的虚拟控制律，提高控制精度。

(2)本发明采用指令滤波法对连杆位置子系统虚拟控制律的表达式求解，避免了直接对虚拟控制律求导而造成的维数爆炸问题；并且还针对采用指令滤波法求解而引入的滤波器估计误差，进行补偿，从而提高求解的精度。

(3)本发明采用干扰观测器对扰动进行观测估计，并根据观测得到的扰动估计值进行干扰补偿，提高控制进度；并采用基于动量的干扰观测器，从而避免或减小测量噪声和计算误差，提高干扰观测器的观测精度。

(4)针对电机力矩信号，在实际输出过程中存在饱和约束特性，本发明提出了饱和模型，并给出相应的饱和补偿的动态方程，为电机力矩做饱和补偿，提高控制精度。

附图说明

图1为本发明变刚度关节轨迹跟踪控制流程图；

图2为二自由度变刚度关节机械臂简图；

图3为本发明变刚度关节轨迹跟踪控制方法实施步骤流程图；

具体实施方式

下面详细描述本发明的实施例，所示实施例的示例在附图中示出，其中自始至终相通或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。

如图1所示的变刚度关节轨迹跟踪控制流程，经操作人员操作设定后，轨迹生成器输出指令给控制系统，轨迹生成器输出的指令包括变刚度关节机械臂的角度位置指令q^d、角速度指令

位置电机的角度位置指令

位置电机的角速度指令

刚度电机的角度位置指令

刚度电机的角速度指令

控制系统根据接收到的指令，输出位置电机力矩u₁和刚度电机力矩u₂分别控制驱动系统中的位置电机和刚度电机运作。

本发明变刚度关节轨迹跟踪控制方法，通过变刚度关节机械臂实际状态的反馈、受饱和约束的力矩反馈、扰动反馈，修正输出的位置电机力矩u₁和刚度电机力矩u₂，实现精确控制。

本发明变刚度关节轨迹跟踪控制方法具体包括如下步骤：

S1，建立变刚度关节模型参数：

如图2所示的二自由度变刚度关节机械臂，由第一连杆和第二连杆组成，机械臂两关节采用平行布置方式，处于垂直平面并受重力作用，机械臂末端为集中质量的负载，机械臂由位置电机和刚度电机共同驱动；第一连杆质量为m₁、质心距离关节距离为L_c1、杆长为L₁、惯量为I₁、摩擦系数为B₁、角度位置为q₁、角速度为

第二连杆质量为m₂、质心距离关节距离为L_c2、杆长为L₂、惯量为I₂、摩擦系数为B₂、角度位置为q₂、角速度为

机械臂末端的负载质量为m₃。

变刚度关节机械臂角度位置定义为：q＝[q₁,q₂]；

变刚度关节机械臂动力学惯性矩阵定义为：

变刚度关节机械臂科里奥利矩阵定义：

变刚度关节机械臂的库伦摩擦矩阵定义：

变刚度关节机械臂的重力矩矩阵定义：G(q)＝[g₁,g₂]

上述定义中，各参数通过如下表达式计算得到：

g₁＝g(m₁L_c1+m₂L₁+m₃L₁)cosq₁+g(m₂L_c2+m₃L₂cos(q₁+q₂))

g₂＝g(m₂L_c2+m₃L₂)cos(q₁+q₂)

S2，建立变刚度关节动力学模型的状态方程

根据S1建立并定义的参数，将机器人动力学模型转换成状态方程的表达式：

其中，

是变刚度关节机械臂的角速度、τ_s是变刚度关节机械臂的弹性力矩、τ_cam是刚度电机侧的弹性力矩分量、θ_pos是位置电机角度位置、

是位置电机角速度、θ_stiff是刚度电机角度位置、

是刚度电机角速度、

是变刚度关节形变指令值、

是τ_s相对

的偏微分、

是τ_s相对θ_stiff的偏微分、J_pos是位置电机惯量矩阵、

是J_pos的逆矩阵、J_stiff是刚度电机惯量矩阵、

是J_stiff的逆矩阵，M(q)是变刚度关节机械臂的惯性矩阵，

是变刚度关节机械臂的科里奥利矩阵，

是变刚度关节机械臂的库伦摩擦矩阵，G(q)是变刚度关节机械臂的重力矩矩阵；

算上扰动时，上述弹性力矩τ_s可以写成如下表达式：

其中

是弹性力矩估计值，δ_τ是弹性力矩扰动、

是刚度电机角度位置指令。

S3，拆分变刚度关节动力学模型的状态方程

根据S2中建立的状态方程，将变刚度关节动力学模型拆分为连杆位置子系统和刚度电机子系统两部分，并且算上有扰动的情况：

连杆位置子系统：

其中

是弹性力矩变化率的扰动；

刚度电机子系统：

其中

是位置电机角度位置指令、q^d是变刚度关节机械臂角度位置指令、δ_stiff是刚度电机扰动。

S4，连杆位置子系统控制

S4.1，定义连杆位置子系统的跟踪误差如下：

e₁＝q-q^d

e₃＝τ_s-α₂

其中α₁、α₂、α₃是连杆位置子系统虚拟控制律。

S4.2，根据反步法控制律标准的设计流程，设计连杆位置子系统虚拟控制律的表达式为：

其中

为

的逆矩阵，

是变刚度关节械臂角速度指令的转置、τ_pos是位置电机力矩；c₁、c₂、c₃、c₄是控制增益，由操作人员自行定义。

S4.3，指令滤波

对虚拟控制律的求导需要对状态变量高阶导数的求解，因此采用指令滤波的方式求取虚拟控制律中的导数，避免直接对虚拟控制律求导而产生的维数爆炸问题。

二阶指令滤波器的动态方程如下：

其中，

是滤波器的阻尼系数、

是滤波器的自然频率、Φ是滤波器的输入值，z₁、z₂是滤波中间变量，

分别是z₁、z₂的导数。

通过上述指令滤波器可求得一阶导数

和二阶导数

将S4.2中的虚拟控制律带入指令滤波器求得各自相应导数。但是由指令滤波的方法求导会引入滤波器估计误差，因此求导后还需对误差进行补偿。

经指令滤波后，S4.1连杆位置子系统的跟踪误差重新定义为：

其中α_2f和α_3f分别是α₂和α₃由S4.3指令滤波方法求得的值；

经指令滤波后，S4.2连杆位置子系统虚拟控制律变为：

其中

分别是α₂、α₃经S4c指令滤波方法求得的导数；

是指令滤波方法求导引入的滤波器估计误差，其具体定义为：

且滤波器估计误差满足以下动态方程：

S5，刚度电机子系统控制

将S3中得到的刚度电机子系统(4)变形为：

其中

是刚度电机角速度

的导数，即刚度电机角加速度。

刚度电机子系统采用PD控制器跟踪控制，定义PD控制器对于刚度电机的跟踪误差

则得到刚度电机的虚拟控制律：

其中

是PD控制器的控制刚度系数，

是PD控制的控制阻尼系数，

是e_stiff的导数；

根据

可变形为：

S6，饱和补偿算法

控制系统的电机力矩输出饱和模型：

其中u_i为未考虑饱和特性时的电机力矩、

为电机力矩输出饱和约束的上限值、

为电机力矩输出饱和约束的下限值。

考虑饱和特性后，应在电机力矩上附加-k₁ε作为补偿，其中ε为饱和补偿，k₁是饱和补偿系数，且ε采用如下动态方程求得：

其中k₂是补偿增益矩阵，τ是未考虑饱和特性时的力矩，τ^sat是考虑饱和特性时的力矩。

S7，干扰观测器

通常电机的角加速度信息是采用对脉冲数二次差分计算得到，但是差分信号会带来测量噪声以及计算误差；为提高干扰观测器的精度，本发明采用基于动量的干扰观测器对系统扰动进行估计。

变刚度关节的位置电机动力学表达式为：

将式(2)带入式(17)得到：

其中

是位置电机的角加速度；

根据式(18)，可得到弹性力矩扰动δ_τ：

其中

定义位置电机侧动量p，且

则有：

定义干扰观测器观测出的弹性力矩扰动估计值

且

则有：

其中

是干扰观测器的增益对角矩阵，K_r的具体值由控制系统使用者自行设定，

是位置电机侧动量估计值，且根据式(20)得到：

将式(18)和式(22)带入式(21)得到：

当增益K_r值设定越大时，越能有效减少高频信号，例如噪声等；对(23)求导得到：

弹性力矩的变化率的扰动

表达式：

根据(24)和(25)，得到弹性力矩变化率扰动估计值

的公式为：

其中

是干扰观测器的增益。

S8，位置电机力矩

基于S6和S7的分别得到连杆位置子系统的饱和补偿和干扰补偿，将S4中得到的连杆位置子系统虚拟控制率表达式(9)加上饱和补偿以及干扰补偿，可得：

其中

是

的逆矩阵，

是考虑饱和特性后的位置电机力矩，

和

由S7干扰观测器得到，ε_pos为位置电机的饱和补偿，k₃是位置电机的饱和补偿系数，且根据S6，ε_pos由以下动态方程求得：

其中k₄是位置电机的补偿增益矩阵。

S9，刚度电机力矩

基于S6和S7的分别得到刚度电机的饱和补偿和干扰补偿，将S5中得到的刚度电机的虚拟控制律(14)加上饱和补偿得到：

其中

是考虑饱和特性时刚度电机力矩，

是刚度电机的饱和补偿系数，ε_stiff是刚度电机的饱和补偿；

刚度电机力矩u₂的表达式(29)中

是刚度电机扰动估计值，由S7干扰观测器得到，其具体表达式为：

其中

是干扰观测器观测刚度电机时的增益对角矩阵；

刚度电机力矩u₂的表达式(29)中ε_stiff由S6饱和补偿得到，其具体动态方程为：

其中τ_stiff是未考虑饱和特性时刚度电机力矩，

是刚度电机饱和补偿增益矩阵。

如图3所示为步骤S1-S9的流程框图。本发明变刚度关节轨迹跟踪控制方法，首先经过步骤S1-S3根据被控制的变刚度关节机械臂建立动力学模型的状态方程，并拆分为连杆位置子系统和刚度电机子系统两部分；步骤S4和S5分别对连杆位置子系统和刚度电机子系统建立虚拟控制律进行控制，其中为避免连杆位置子系统内对虚拟控制直接求导而造成维数爆炸问题，引入S4.3指令滤波的方法求解连杆位置子系统的虚拟控制律；最后通过步骤S6饱和补偿算法以及步骤S7干扰观测器，分别对连杆位置子系统和刚度电机子系统进行饱和补偿与干扰补偿；通过以上步骤，得到并输出位置电机力矩u₁和刚度电机力矩u₂。

本发明变刚度关节轨迹跟踪控制方法，通过获得反馈回来的变刚度关节机械臂的实际状态，即变刚度关节机械臂的角度位置q、角速度

位置电机的角度位置θ_pos、角速度

刚度电机的角度位置θ_stiff、角速度

通过比较实际值与指令值确定跟踪误差，并根据虚拟控制律修正输出的电机力矩；通过干扰观测器对驱动系统实时观测得到弹性力矩扰动和刚度电机扰动，并将干扰观测器获得的弹性力矩扰动估计值

和刚度电机扰动估计值

反馈给控制器做干扰补偿；通过获得反馈回来的受饱和约束的位置电机力矩

和刚度电机力矩

与未考虑饱和特性时的位置电机力矩τ_pos和刚度电机力矩τ_stiff比较，进而对输出的电机力矩做饱和补偿。本发明通过以上方法提高跟踪控制精度。

本发明并不限于上述实施方式，在不背离本发明的实质内容的情况下，本领域技术人员能够做出的任何显而易见的改进、替换或变型均属于本发明的保护范围。

Claims (7)

1.一种变刚度关节轨迹跟踪控制方法，其特征在于，具体为：

建立连杆位置子系统状态方程为：

针对连杆位置子系统状态方程，定义系统跟踪误差为：

e₁＝q-q^d

e₃＝τ_s-α₂

其中α₁、α₂、α₃是连杆位置子系统虚拟控制律，且虚拟控制律满足如下表达式：

对位置电机力矩τ_pos进行饱和补偿和干扰补偿，输出位置电机力矩u₁；

建立刚度电机子系统状态方程为：

针对刚度电机子系统状态方程，采用PD控制器对刚度电机子系统跟踪控制，定义系统跟踪误差为：

则刚度电机子系统的虚拟控制律满足如下表达式：

对刚度电机力矩进行饱和补偿和干扰补偿，输出刚度电机力矩u₂；

上述式中

q为变刚度关节机械臂角度位置，q^d是变刚度关节机械臂角度位置指令，

是变刚度关节机械臂的角速度，

是变刚关节机械臂角速度指令，τ_s是变刚度关节机械臂的弹性力矩，

是弹性力矩估计值，δ_τ是弹性力矩扰动，

是弹性力矩变化率的扰动，δ_stiff是刚度电机扰动，τ_cam是刚度电机侧的弹性力矩分量，θ_pos是位置电机角度位置，

是位置电机角度位置指令，

是位置电机角速度，u₁是位置电机力矩通用表达符号，θ_stiff是刚度电机角度位置，

是刚度电机角度位置指令，

是刚度电机角速度，

是刚度电机角速度

的导数，u₂是刚度电机力矩通用表达符号，

是变刚度关节形变指令值，

是τ_s相对

的偏微分，J_pos是位置电机惯量矩阵，J_stiff是刚度电机惯量矩阵，M(q)是变刚度关节机械臂的惯性矩阵，

是变刚度关节机械臂的科里奥利矩阵，

是变刚度关节机械臂的库伦摩擦矩阵，G(q)是变刚度关节机械臂的重力矩矩阵；

是变刚度关节械臂角速度指令

的转置、τ_pos是未考虑饱和特性时的位置电机力矩；

是PD控制器的控制刚度系数，

是PD控制的控制阻尼系数，c₁、c₂、c₃、c₄是控制增益，

是e_stiff的导数。

2.根据权利要求1所述的变刚度关节轨迹跟踪控制方法，其特征在于，所述连杆位置子系统虚拟控制律的表达式，采用指令滤波方法求解其中的导数，二阶指令滤波器的动态方程为：

其中，

是滤波器的阻尼系数、

是滤波器的自然频率、Φ是滤波器的输入值，z₁、z₂是滤波中间变量，

分别是z₁、z₂的导数。

3.根据权利要求2所述的变刚度关节轨迹跟踪控制方法，其特征在于，所述采用指令滤波方法求解导数，还需引入误差补偿；

经指令滤波后，所述连杆位置子系统跟踪误差重新定义为：

其中α_2f和α_3f分别是α₂和α₃由指令滤波方法求得的值；

经指令滤波后，所述连杆位置子系统虚拟控制律变为：

其中

为

的逆矩阵，

分别是α₂、α₃经指令滤波方法求得的导数，

是滤波方法求导引入的滤波器估计误差，其具体定义为

且满足如下动态方程：

4.根据权利要求3所述的变刚度关节轨迹跟踪控制方法，其特征在于，所述位置电机力矩饱和补偿为-k₃ε_pos，其中ε_pos是位置电机的饱和补偿，k₃是位置电机的饱和补偿系数，且ε_pos由如下动态方程求得：

其中k₄是位置电机的补偿增益矩阵，

是考虑饱和特性后的位置电机力矩。

5.根据权利要求4所述的变刚度关节轨迹跟踪控制方法，其特征在于，所述位置电机力矩的干扰补偿，采用基于动量的干扰观测器获得，所述连杆位置子系统虚拟控制律算上饱和补偿以及干扰补偿后，其表达式变为：

其中

是

的逆矩阵，

弹性力矩扰动估计值，且

是弹性力矩变化率扰动估计值；

和

由干扰观测器得到，其表达式为：

其中

是干扰观测器的增益对角矩阵，

是干扰观测器的增益。

6.根据权利要求1所述的变刚度关节轨迹跟踪控制方法，其特征在于，所述刚度电机力矩饱和补偿为-k₅ε_stiff，其中ε_stiff是刚度电机的饱和补偿，

是刚度电机的饱和补偿系数，且ε_stiff由如下动态方程求得：

其中

是刚度电机饱和补偿增益矩阵，τ_stiff是未考虑饱和特性的刚度电机力矩，

是考虑饱和特性后刚度电机力矩。

7.根据权利要求6所述的变刚度关节轨迹跟踪控制方法，其特征在于，所述刚度电机力矩干扰补偿，采用基于动量的干扰观测器获得，所述刚度电机子系统虚拟控制律算上饱和补偿以及干扰补偿后，其表达式变为：

其中，

刚度电机扰动估计值，由干扰观测器得到，其表达式为：

其中

是干扰观测器观测刚度电机时的增益对角矩阵。

Images