CN113858198B - 一种考虑关节电机动力学的机械臂力跟踪阻抗控制方法 - Google Patents

Abstract

本发明公开了一种考虑关节电机动力学的机械臂力跟踪阻抗控制方法，包括以下步骤：步骤一、建立力跟踪偏差的微分方程，根据力跟踪偏差的微分方程获得状态空间方程；在状态空间方程下，利用反步法结合自适应神经网络，得到环境未知时的位置二次修正律，计算笛卡尔空间中的柔顺位置x_c，步骤二、通过运动学反解，将步骤一中得到的x_c转换成关节空间的柔顺角位置q_c；步骤三、设计位置跟踪控制器，得到跟踪q_c所需要的控制力矩τ；步骤四、设计电流跟踪控制器，在电流跟踪控制器中利用自适应神经网络来逼近关节电机的动力学模型，进而得到应当施加在各个关节电机上的控制电压。本发明提升了轨迹跟踪精度，最终减小了在未知环境下的力跟踪偏差。

Description

一种考虑关节电机动力学的机械臂力跟踪阻抗控制方法

技术领域

本发明涉及机械臂控制技术领域，特别是一种考虑关节电机动力学的机械臂力跟踪阻抗控制方法。

背景技术

阻抗控制是机械臂柔顺控制中常用的方法之一，它建立了力与位置之间的动态关系，通过调整位置可以间接调整机械臂末端所受到的环境力的大小。如果环境信息可以准确获得，阻抗模型能够较为准确地将力跟踪偏差转化为位置的修正量，从而跟踪期望力。然而，在环境信息无法准确获取时，阻抗模型的有限调节作用无法得到合适的轨迹，较大的力跟踪偏差则可能导致任务失败或者设备的损坏。因此，如何在未知环境下实现机械臂的力跟踪阻抗控制成为了当前的一个研究热点。目前较多的研究集中在通过调节阻抗参数实现对期望力的跟踪，然而由于待调节参数较多以及参数的时变性，系统的稳定性和动态性能难以得到保证。也有研究提出将阻抗模型的刚度系数设为零来提高力跟踪精度，这种策略对于力跟踪精度确实有很大的提升，然而阻抗模型可以看作是一个二阶滤波器，将刚度系数设为零，可能会影响到系统的响应速度。

此外，当阻抗控制产生了合适的柔顺轨迹之后，机械臂的轨迹跟踪性能就决定了力跟踪的精度。然而目前的研究成果大多只考虑了在力矩层面的控制，实际上，控制力矩是由关节电机提供的，这些研究忽视了控制过程中关节电机的特性对于跟踪效果的影响。综上存在未知环境中阻抗模型控制精度不足的问题以及机械臂轨迹跟踪控制未考虑关节电机特性的问题。

发明内容

本发明所要解决的技术问题是克服现有技术的不足而提供一种考虑关节电机动力学的机械臂力跟踪阻抗控制方法，通过提高轨迹修正精度和轨迹跟踪精度，最终提高机械臂的力跟踪控制精度；其中，针对阻抗模型无法在未知环境中跟踪期望力的问题，采用反步法对阻抗模型产生的轨迹作二次修正，在保证系统稳定性的前提下得到合适的柔顺位置，从而为减小力跟踪偏差提供必要的条件；针对机械臂关节电机的动力学对于轨迹跟踪性能的影响，将关节电机的动力学模型以及电机的输出力矩饱和特性引入，使控制方案更加贴近于机械臂的实际模型，提高控制精度，以获得更高的力跟踪精度。

本发明为解决上述技术问题采用以下技术方案：

根据本发明提出的一种考虑关节电机动力学的机械臂力跟踪阻抗控制方法，包括以下步骤：

步骤一、建立环境的动力学模型，由环境的动力学模型和预先给定的期望力f_d得到力跟踪偏差e_f；

根据阻抗模型和力跟踪偏差e_f，建立力跟踪偏差的微分方程；

根据力跟踪偏差的微分方程获得状态空间方程；

在状态空间方程下，利用反步法结合自适应神经网络，得到环境未知时的位置二次修正律Δx_f；

计算笛卡尔空间中的柔顺位置x_c，x_c＝x_d+x_f+Δx_f，其中，x_d为期望轨迹，x_f为由阻抗模型得到的位置修正量；

步骤二、通过运动学反解，将步骤一中得到的x_c转换成关节空间的柔顺角位置q_c；

步骤三、考虑到机械臂的动力学模型存在不确定性以及关节电机输出力矩的饱和特性，设计力矩层面上的位置跟踪控制器，并在位置跟踪控制器中引入自适应神经网络和辅助系统，得到跟踪柔顺角位置q_c所需要的控制力矩τ；

步骤四、建立关节电机的动力学模型，并通过关节电机的力矩常数K_T将所需的控制力矩τ转换成期望电流i_d；根据实际电流大小与期望电流大小的偏差，定义出电流跟踪偏差e_i；设计电流跟踪控制器，在电流跟踪控制器中利用自适应神经网络来逼近关节电机的动力学模型，进而得到应当施加在各个关节电机上的控制电压u。

作为本发明所述的一种考虑关节电机动力学的机械臂力跟踪阻抗控制方法进一步优化方案，步骤一中，

其中，k_a2是大于零的系数，x_a1、x_a2分别是力跟踪偏差及力跟踪偏差的变化率，z_a1＝x_a1，z_a1是实际力跟踪偏差与期望的力跟踪偏差之间的差值，期望的力跟踪偏差总是为0，α_a1＝-k_a1z_a1是二次修正律中选定的虚拟控制律，k_a1是大于零的系数，z_a2＝x_a2-α_a1，z_a2表示力跟踪偏差的变化率与虚拟控制律的差值，x_d是给定的期望轨迹，设定神经网络含有m个隐含层节点，是对二次修正律中自适应神经网络的最优权值的估计值，/>是/>的导数，表示径向基函数的值；/>表示二次修正律中自适应神经网络的输入，k_r是大于零的鲁棒项系数，/>是α_a1的导数，上标T表示转置，/>表示m维列向量，Γ₁、σ₁分别是二次修正律中神经网络权重更新律的正定对角矩阵和正定系数。

作为本发明所述的一种考虑关节电机动力学的机械臂力跟踪阻抗控制方法进一步优化方案，步骤三中，

其中，J(q)为非奇异雅克比矩阵，F_e表示机械臂末端受到的外力，e₁是关节空间中实际角位置与柔顺角位置的差值，e₂表示关节空间中实际角速度与给定虚拟控制律之间的差值，是对位置跟踪控制器中自适应神经网络的最优权值W₂ ^*的估计值，S(Z₂)为位置跟踪控制器中自适应神经网络的径向基函数的值，K_b2是正系数，ζ是定义的辅助系统中的状态变量，Z₂是位置跟踪控制器中自适应神经网络的输入；

为/>的导数，Γ₂、σ₂分别是位置跟踪控制器中神经网络权重更新律的正定对角矩阵和正定系数。

作为本发明所述的一种考虑关节电机动力学的机械臂力跟踪阻抗控制方法进一步优化方案，步骤四中，

其中，表示期望电流的导数，/>表示对关节电机的电感常数矩阵L的估计，/>表示对关节电机的电阻常数矩阵R的估计，/>表示对关节电机反电动势的常数矩阵K_b的估计，/>表示对电流跟踪控制器中自适应神经网络最优权值W₃ ^*的估计，并且满足Γ₃、σ₃分别是电流跟踪控制器中神经网络权重更新律的正定对角矩阵和正定系数，/>表示关节电机的电枢电流矢量，/>是n维列向量，/>为关节的速度，K_c是正定对角的系数方阵，e_i为电流跟踪误差，S(Z₃)是电流跟踪控制器中自适应神经网络的径向基函数的值。

本发明采用以上技术方案与现有技术相比，具有以下技术效果：

本发明解决了当环境信息未知时，阻抗控制具有较大的力跟踪偏差的问题；以反步法为基础，结合自适应神经网络，在保证稳定性的前提下设计机械臂末端位置的二次修正律，得到合适的位置，并且将关节电机的动力学特性考虑在内，提升了轨迹跟踪精度；最终减小了在未知环境下的力跟踪偏差。

附图说明

图1是本发明的设计流程图。

图2是本发明的控制框图。

图3是在本控制策略下机械臂对期望力的跟踪性能。

图4是环境位置与机械臂末端位置的比较。

图5是关节1的位置跟踪偏差。

图6是关节2的位置跟踪偏差。

图7是关节1的速度跟踪偏差。

图8是关节2的速度跟踪偏差。

图9是关节1在饱和约束下的控制力矩。

图10是关节2在饱和约束下的控制力矩。

具体实施方式

为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图及具体实施例对本发明进行详细描述。

本发明提供了一种基于二次修正阻抗模型并且考虑关节电机动力学特性的机械臂力跟踪阻抗控制策略，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。如图1所示，本方法如下：

步骤1：建立环境的动力学模型，并定义力跟踪偏差，具体如下：

将环境的动力学模型表示为刚度模型，即f_e＝k_e(x-x_e)，则可得到方程：

f_d-f_e＝f_d+k_ex_e-k_ex＝f_d+k_ex_e-k_e(x_d+Δx_f+h(s)e_f)

其中，f_d、x_d是期望力和期望轨迹，f_e是机械臂末端实际受到的交互力，k_e、x_e是未知的环境刚度和未知的环境位置，并且满足环境位置始终在一个有限的空间内，即满足x_e有界，Δx_f是由自适应神经网络产生的轨迹二次修正律，h(s)是导纳系统的传递函数，e_f＝f_d-f_e是期望力与环境力的偏差。

步骤2：建立力跟踪偏差的微分方程，具体如下：

将代入，得到/>

其中，m、b、k分别是阻抗模型的惯性系数、阻尼系数和刚度系数，表示e_f的一阶导数，/>表示e_f的二阶导数。

步骤3：将步骤2中力跟踪偏差的微分方程写成状态空间方程的形式。具体如下：

定义状态为x_a1＝e_f、

将力跟踪偏差的微分方程改写成状态空间方程的形式为：

其中，表示x_a1的导数，/>表示x_a2的导数。

步骤4：利用反步法设计基于环境信息的位置二次修正律Δx_f。具体如下：

定义位置二次修正律的跟踪偏差z_a1＝x_a1，z_a2＝x_a2-α_a1，其中，α_a1是设计二次修正律时选定的虚拟控制律并且满足α_a1＝-k_a1z_a1，k_a1是大于零的系数。

取第一个Lyapunov函数为并且将V₁对时间求导，得到

取第二个Lyapunov函数为将V₂对时间求导，得到：

其中，表示α_a1的导数。

选择其中k_a2是大于零的系数。

将Δx_f代入得到/>

步骤5：考虑环境信息的不确定性，利用自适应神经网络改造步骤4中的位置二次修正律Δx_f，具体如下：

利用自适应神经网络逼近Δx_f中含有未知环境参数的部分，即：

其中，对于含有m个隐含层节点的神经网络，为二次修正律中自适应神经网络的最优权值；/>表示二次修正律中自适应神经网络的径向基函数的值；上标T表示转置，/>表示m维列向量；/>表示二次修正律中神经网络的输入；表示二次修正律中神经网络的逼近误差。

因此，二次修正量可以表示为

其中，k_r≥||x_e||_max+||ε₁||_max表示鲁棒项系数，是对/>的估计值，并且满足/>的更新律/>Γ₁、σ₁分别是二次修正律中神经网络的权值更新律的正定对角矩阵和正定系数，以及二次修正律中自适应神经网络的权值偏差/>

步骤6：计算笛卡尔空间中的柔顺位置x_c，并将其转换成关节空间中的柔顺角位置q_c，具体如下：

笛卡尔空间中的柔顺位置x_c可以表示为

x_c＝x_d+x_f+Δx_f

通过机械臂的运动学反解，得到柔顺角位置q_c。

步骤7：建立n关节机械臂的动力学模型为：

其中，q、分别表示关节的位置、速度和加速度；/> 分别表示正定的惯性矩阵、离心力项和科氏力项、重力项；/>表示关节摩擦力；τ_d表示外部扰动；J(q)为非奇异雅可比矩阵；τ表示关节的控制力矩；/>表示机械臂末端受到的外力。

步骤8：将n关节机械臂的动力学模型写成状态空间方程。具体如下：

定义x_b1＝q、将动力学微分方程写成状态空间方程形式：

其中，表示x_b1的导数，/>表示x_b2的导数。

步骤9：利用反步法设计力矩层面上的位置跟踪控制器。具体如下：

定义位置跟踪控制器的第一个跟踪误差和第二个跟踪误差分别为：

e₁＝x_b1-q_c

e₂＝x_b2-α_b1

其中，α_b1为位置跟踪控制器中设计的虚拟控制律，并且满足K_b1是正定对角矩阵。则e₁的导数/>

定义P(·)是关节电机的实际输出力矩，则有Δτ＝P(τ)-τ，设计在力矩层面的基于模型的控制器为：

其中，K_b2是正定对角方阵；ζ是辅助系统的状态，用于减小关节电机输出力矩饱和导致的系统不稳定，并且将其设计为：

并且满足ξ⁺ξ＝I和μ是一个非常小的数，ζ⁺是ζ的伪逆，K_ζ是正定对角方阵，上标T表示转置，||·||表示欧氏距离。

步骤10：设计力矩层面的位置跟踪控制器。具体如下：

由于M、C、G均为未知，故上述控制律难以实现，利用神经网络的万能逼近特性估计未知部分，即：

设定神经网络含有m个隐含层节点，为位置跟踪控制器中神经网络的最优权值；/>表示位置跟踪控制器中神经网络的径向基函数的值；上标T表示转置，/>表示m维列向量；

因此，基于自适应神经网络的控制律设计为：

其中，是对W₂ ^*的估计值，/>Γ₂、σ₂分别是位置跟踪控制器中神经网络权重更新律的正定对角矩阵和正定系数，/>并且对于这个神经网络，权重估计值与权重最优值之间的差值/>

步骤11：建立关节电机的动力学模型为：

K_Ti＝τ

其中，表示关节电机的电感常数矩阵，/>表示关节电机的电枢电流矢量，/>表示i的导数，/>表示关节电机的电阻常数矩阵，/>是表示关节电机反电动势的常数矩阵，/>为关节电机的控制输入电压，/>表示可逆的关节电机力矩常数矩阵。并且，L、R、K_b均为未知。

步骤12：设计电压层面的基于自适应神经网络的电流跟踪控制器，具体如下：

由步骤9的基于自适应神经网络的力矩层面的控制器，可以得到期望的电流大小为：

定义电流跟踪误差为e_i＝i-i_d，设计电压层面的基于模型的控制器为：

其中，表示i_d的一阶导数，K_c是正定对角方阵。

由于关节电机的动力学参数未知，故利用神经网络逼近上述控制器中的未知非线性，即其中，/>为电流跟踪控制器中自适应神经网络逼近非线性的最优权值，/>表示电流跟踪控制器中自适应神经网络的径向基函数的值，表示电流跟踪控制器中自适应神经网络的输入，设定神经网络含有m个隐含层节点，上标T表示转置，/>表示m维列向量。

因此，控制律改写为：

其中，表示对L的估计，/>表示对R的估计，/>表示对K_b的估计，/>表示对W₃ ^*的估计，并且满足电流跟踪控制器中神经网络权重更新律/>Γ₃、σ₃分别是电流跟踪控制器中神经网络权重更新律的正定对角矩阵和正定系数。

通过以下步骤验证所发明控制器的稳定性：

首先验证二次修正阻抗模型的稳定性。

定义Lyapunov函数为

将V₃对时间求导，得到：

根据和k_r≥||ε||_max+||x_e||_max，可以得到

根据杨氏不等式，有

将上式代入得到

其中，C₁＝(σ₁/2)W₁ ^*TW₁ ^*。

为了保证阻抗闭环系统的稳定性，必须满足K₁＞0，此时将负定。

下面验证机械臂跟踪控制系统的稳定性：

定义Lyapunov函数为

将V₄对时间求导，得到

考虑到杨氏不等式及以下不等式成立

对应用上述不等式，并整理，得到

其中，

为了保证闭环系统的稳定性，需要满足K_b1＞0、2K_b2-2I＞0以及2K_c-I＞0，此时/>将负定。

下面以具体仿真实验，验证本实施例提供的力跟踪阻抗控制方法的有效性。

在仿真中，设计机械臂的力跟踪方向为环境的法线方向。在力控制方向上的环境实际位置为x_e＝0.3m，估计的环境位置为环境刚度为k_e＝5000N/m，期望跟踪的力为-50N。对于环境法线方向上的阻抗模型，取参数分别为m＝1、b＝100以及k＝20。对于关节电机，取其力矩常数矩阵为/>电感常数矩阵为/>电阻常数矩阵为/>反电动势常数矩阵为/>机械臂两个关节电机的控制力矩饱和上限和饱和下限分别为5N·m和-5N·m。对于本发明设计的阻抗修正器，取K₁＝10、K₂＝10、k_r＝0.33、Γ₁＝20、σ₁＝10，取神经网络的隐含层节点5个。对于关节电机的控制器，取/> 两个神经网络的隐含层节点数量都选择为7个。仿真时间设置为5秒。

图2和图3展示了采用本发明进行修正时产生的环境力以及机械臂末端与环境的相对位置，当环境信息不能准确获得时，由于对力跟踪偏差及其变化率进行约束，可以有效地将力跟踪偏差控制在2N左右，并且具有较快的调节速度。图4至图8展示了考虑关节电机额的动力学以及关节电机输出力矩饱和时，对于阻抗控制生成的柔顺轨迹的跟踪性能，经过调整后，两个关节的位置和速度均能够较为准确地跟踪柔顺位置和速度，并且将跟踪误差限制在一个较小的范围内。图9和图10展示了两个关节电机的输出力矩，均未超出所能提供的最大力矩。因此本发明提供的自适应神经网络二次修正阻抗模型以及考虑关节电机动力学特性的轨迹跟踪策略能够有效消除未知环境信息的影响，能够达到较好的力跟踪性能。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围内。

Claims (4)

1.一种考虑关节电机动力学的机械臂力跟踪阻抗控制方法，其特征在于，包括以下步骤：

步骤一、建立环境的动力学模型，由环境的动力学模型和预先给定的期望力f_d得到力跟踪偏差e_f；

根据阻抗模型和力跟踪偏差e_f，建立力跟踪偏差的微分方程；

根据力跟踪偏差的微分方程获得状态空间方程；

在状态空间方程下，利用反步法结合自适应神经网络，得到环境未知时的位置二次修正律Δx_f；

计算笛卡尔空间中的柔顺位置x_c，x_c＝x_d+x_f+Δx_f，其中，x_d为期望轨迹，x_f为由阻抗模型得到的位置修正量；

步骤二、通过运动学反解，将步骤一中得到的x_c转换成关节空间的柔顺角位置q_c；

步骤三、考虑到机械臂的动力学模型存在不确定性以及关节电机输出力矩的饱和特性，设计力矩层面上的位置跟踪控制器，并在位置跟踪控制器中引入自适应神经网络和辅助系统，得到跟踪柔顺角位置q_c所需要的控制力矩τ；

步骤四、建立关节电机的动力学模型，并通过关节电机的力矩常数K_T将所需的控制力矩τ转换成期望电流i_d；根据实际电流大小与期望电流大小的偏差，定义出电流跟踪偏差e_i；设计电流跟踪控制器，在电流跟踪控制器中利用自适应神经网络来逼近关节电机的动力学模型，进而得到应当施加在各个关节电机上的控制电压u。

2.根据权利要求1所述的一种考虑关节电机动力学的机械臂力跟踪阻抗控制方法，其特征在于，步骤一中，

其中，k_a2是大于零的系数，x_a1、x_a2分别是力跟踪偏差及力跟踪偏差的变化率，z_a1＝x_a1，z_a1是实际力跟踪偏差与期望的力跟踪偏差之间的差值，期望的力跟踪偏差总是为0，α_a1＝-k_a1z_a1是二次修正律中选定的虚拟控制律，k_a1是大于零的系数，z_a2＝x_a2-α_a1，z_a2表示力跟踪偏差的变化率与虚拟控制律的差值，x_d是给定的期望轨迹，设定神经网络含有m个隐含层节点，是对二次修正律中自适应神经网络的最优权值的估计值，/>是/>的导数，表示径向基函数的值；/>表示二次修正律中自适应神经网络的输入，k_r是大于零的鲁棒项系数，/>是α_a1的导数，上标T表示转置，/>表示m维列向量，Γ₁、σ₁分别是二次修正律中神经网络权重更新律的正定对角矩阵和正定系数。

3.根据权利要求2所述的一种考虑关节电机动力学的机械臂力跟踪阻抗控制方法，其特征在于，步骤三中，

其中，J(q)为非奇异雅克比矩阵，F_e表示机械臂末端受到的外力，e₁是关节空间中实际角位置与柔顺角位置的差值，e₂表示关节空间中实际角速度与给定虚拟控制律之间的差值，是对位置跟踪控制器中自适应神经网络的最优权值W₂ ^*的估计值，S(Z₂)为位置跟踪控制器中自适应神经网络的径向基函数的值，K_b2是正系数，ζ是定义的辅助系统中的状态变量，Z₂是位置跟踪控制器中自适应神经网络的输入；

为/>的导数，Γ₂、σ₂分别是位置跟踪控制器中神经网络权重更新律的正定对角矩阵和正定系数。

4.根据权利要求2所述的一种考虑关节电机动力学的机械臂力跟踪阻抗控制方法，其特征在于，步骤四中，

其中，表示期望电流的导数，/>表示对关节电机的电感常数矩阵L的估计，/>表示对关节电机的电阻常数矩阵R的估计，/>表示对关节电机反电动势的常数矩阵K_b的估计，/>表示对电流跟踪控制器中自适应神经网络最优权值W₃ ^*的估计，并且满足/>Γ₃、σ₃分别是电流跟踪控制器中神经网络权重更新律的正定对角矩阵和正定系数，/>表示关节电机的电枢电流矢量，/>是n维列向量，/>为关节的速度，K_c是正定对角的系数方阵，e_i为电流跟踪误差，S(Z₃)是电流跟踪控制器中自适应神经网络的径向基函数的值。