CN113352322B - 一种基于最优导纳参数的自适应人机协作控制方法 - Google Patents

Abstract

一种基于最优导纳参数的自适应人机协作控制方法，包括如下步骤：描述具有HRC任务的机器人动力学模型和用于机器人导纳控制的机器人导纳模型；设计基于BLF的机器人自适应约束控制器的内环；步骤3：设计基于积分强化学习的任务优化的外环；步骤4：利用积分强化学习求解LQR问题。本发明通过使用BLF考虑了机器人的运动约束，从而提高了HRC任务的安全性；然后，在面向任务的外环中获得最优导纳参数，以使任务跟踪误差和交互力最小；提出了一种基于积分强化学习的无模型方法来寻找最优导纳参数；此外，设计了基于特定任务轨迹的辅助力，保证了良好的任务跟踪。

Description

一种基于最优导纳参数的自适应人机协作控制方法

技术领域

本发明涉及人机协作任务中一种基于最有导纳参数的自适应控制方法。

背景技术

如今，机器人不仅需要完成高精度的重复性任务，还被广泛应用于修复、搬运、装配等人-机协作任务。因此，有必要开发人-机器人协作(HRC)系统，使人与机器人的优势互补。例如，人类有处理未知环境的分析能力，而机器人可以在危险的环境中不知疲倦地工作。

为了完成HRC任务，操作者需要通过安装在机器人上的力/扭矩传感器与机器人进行交互。近年来，阻抗控制和导纳控制因其鲁棒性和灵活性被广泛应用于HRC任务中。在阻抗控制方面，提出了一个阻尼-弹簧-质量模型来建立环境与机器人之间的动态关系。然后通过测量机器人的位置和适当的阻抗参数，得到机器人的输入力/转矩。导纳控制的目的是通过测量相互作用的力和适当的导纳参数来获得所需的机器人轨迹。而对于阻抗控制和导纳控制，则必须根据不同的操作人员和工作环境选择合适的模型参数。结合生物反馈策略和自适应控制策略，根据患者的肌肉活动自适应调整阻抗参数。叶伯生提出了一种基于动力学模型的机器人自适应阻抗控制系统，对机器人动力学模型进行优化，进而将优化后的动力学模型传输给阻抗控制器来实现机器人末端的力和位置的柔顺控制(叶伯生,陶婕妤,谢鹏,饶阿龙,张文彬,谢远龙,谭朝,帅思远.一种基于动力学模型的机器人自适应阻抗控制系统[P].CN110065070B,2020-09-18.)但在实际应用中，多数机器人的模型是未知的，基于模型的方法应用的场景较少。尤波等提出一种基于意图识别的人机协作系统控制方法，利用神经网络识别系统估计人的意图，使机械臂产生控制输入，该方法既减小了交互力，又改善了运动的柔顺性(尤波,焦龙,李佳钰.一种基于意图识别的人机协作系统控制方法[P].CN112276944A,2021-01-29.)但该方法未考虑机器人工作在一个安全的工作空间内，在人机协作的任务中，会存在机器人工作超出安全空间的隐患。

发明内容

本发明要克服现有技术的上述缺点，提出了一种基于最优导纳参数的自适应人机协作控制方法。

本发明首先建立了由内环和外环组成的整体控制结构。内环和外环的任务分别是机器人控制和任务优化。在此基础上，提出了一种结合障碍李雅普诺夫函数(BLF)和径向基函数神经网络(RBFNN)的机器人内环控制器，使具有未知动力学的机器人安全地表现为操作者感知的规定的机器人导纳模型。然后，在外环中获得机器人导纳模型的最优参数，以使任务跟踪误差和交互力最小。通过构建人-机器人协作系统模型，将机器人导纳模型的优化问题转化为线性二次型调节问题。该模型包含了操作员的未知动态和任务执行细节。为了放宽对系统模型的要求，采用积分强化学习的方法来解决线性二次调节问题。此外，还设计了辅助力，帮助操作者更好地完成特定的任务。与传统的控制方案相比，提高了人机协作系统的安全性能和交互性能。

本发明的一种基于最优导纳参数的自适应人机协作控制方法，具体步骤如下：

步骤1：描述具有HRC任务的机器人动力学模型和用于机器人导纳控制的导纳模型。动力学模型为：

其中q，

分别为关节角、速度和加速度矢量。x,

分别为末端执行器位置、速度和加速度矢量。M(q)∈R^p×p为对称惯性矩阵，

为科氏力和向心力的矢量，G(q)∈R^p表示重力矢量，J(q)∈R^n×p为雅可比矩阵，τ∈R^p为机器人控制输入力矩，f_h∈Rⁿ为由操作者产生的相互作用力，G_h∈Rⁿ表示人的输入力增益，κ(·)为逆运动学的解，p为机器人关节的数量，n表示笛卡尔空间中的维数。

本发明将导纳控制用于机器人与操作者的交互，导纳模型描述为：

其中x_m是期望参考轨迹，即导纳模型和x_d的输出是预定的任务轨迹，M_d，B_d和K_d分别代表期望的惯性、阻尼和刚度系数。

是后面要设计的辅助力，对应于x_m和x_d的关节坐标可以表示为q_m和q_d。

步骤2：设计基于BLF的机器人自适应约束控制器的内环；

内环设计的目的是使机器人的行为像规定的机器人导纳模型，并保持在一个安全工作空间内。不需要任务信息，只需关注于最下化规定的机器人导纳模型输出与机器人实际运动之间的跟踪误差。具体步骤如下：

步骤2-1：将节点约束设为|q|＜k_l，其中k_l＝[k_l1,...,k_lp]为约束边界。取q₁＝q，

机器人动力学(1)可以表示为：

定义跟踪误差e₁＝q₁-q_m，e₂＝q₂-α，其中，

其中，k_i(i＝1,…,p)是正常数，

代表跟踪误差的约束条件，

其中是由q_m的最大绝对值组成的向量。对e₂相对于时间求微分：

步骤2-2：构造一个log型BLF候选V₁，如下所示：

对V₁相对于时间求微分得到：

步骤2-3：设计另一个BLF候选V₂：

对V₂相对于时间求微分得到：

根据摩尔-彭罗斯伪逆性质可以得到：

当e₂＝[0 0 ... 0]^T时，等式(11)满足

因此系统渐进稳定。然而，e₂＝[0 0 ... 0]^T的情况在实际中几乎是不可能的。

步骤2-4：对于其他情况，设计基于模型的控制器如下：

其中，

代表

的伪逆，K₂是满足

的增益矩阵。将(13)代入(11)得到：

可见控制器(13)可以使系统稳定。另外，根据BLF的性质，误差e_1i满足-k_bi＜e_1i＜k_bi。因此，关节状态保持在区间|q_1i|＜k_li。

步骤2-5：将控制器(13)改写为：

由于在实际应用中，(1)中机器人模型总是未知的。考虑到神经网络可以逼近任何未知的连续函数，故将控制器改为上述形式。其中

是神经网络的输入，S(·)表示径向基函数，

是估计得神经网络权重，l是神经网络节点数量，

表示理想的

的近似。

和

之间的关系由下给出：

其中W^*表示理想的神经网络权重，ε(Z)是近似误差。设计神经网络权值的更新规则为

其中

表示

的第i列，

是常值增益矩阵，σ_i是提高系统鲁棒性的一个小的正常数。

步骤2-6：给出候选BLFV₃为：

其中

对V₃相对时间微分并结合等式(14)-(16)，可以得到：

进一步根据(11)的性质，我们得到：

其中，

λ_min(·)和λ_max(·)分别表示矩阵的极小特征值和极大特征值。所有误差信号都是半全局一致最终有界(SGUUB)。

步骤3：设计基于积分强化学习的任务优化外环：

在外环设计中，对导纳模型的参数进行优化，以使任务跟踪误差和交互力最小。为此，本发明将导纳参数优化问题转化为LQR问题，在不考虑系统模型的情况下，采用积分强化学习的方法求解。此外，需要注意的是，完全依靠操作者很难实现良好的任务跟踪。因此，设计一个名为辅助力的前馈项来协助操作者。具体步骤如下：

步骤3-1：设计系统辅助力：

当手臂执行简单任务时，定义人手臂阻抗模型为：

(K_us+K_p)f_h＝k_ee_d, (22)

其中K_u，K_p，k_e表示不同操作者和任务的位置矩阵，e_d为跟踪误差，定义为：

e_d＝x_d-x_m. (23)

让

其中

x_d和x_m分别表示特定任务轨迹和期望轨迹，式(22)可以写成：

将导纳模型(3)改写为如下状态空间方程：

其中，

从式(28)，可以得到：

为了设计辅助力，假设系统是稳定的，即x_m→x_d。因此可以得到：

步骤3-2：设计最优控制器：

我们将HRC系统的性能函数定义为该系统的状态空间模型：

其中Q＝diag(Q_d,Q_h)，

R＝R^T＞0。可见，性能函数优化问题可以转化为LQR问题。因此，最优控制器设计为：

其中P是代数黎卡提方程的解：

PA+A^TP-PBR^-1B^TP+Q＝0. (35)

矩阵R^-1B^TP包含了机器人导纳模型的参数和人的增益G_h。结合方程(27)和(34)可以得到

通过求解上式，可以得到最优导纳参数。

步骤4：利用积分强化学习求解LQR问题：

由于系统矩阵A和B中存在未知参数，用传统方法求解LQR问题比较困难。积分强化学习作为一种在线离轨算法，可以不需要系统模型的先验知识，利用在线收集的系统信息解决LQR问题。本发明采用一种基于值迭代(VI)的积分强化学习算法来估计最优控制器。导纳参数优化的具体算法如下：

步骤4-1：从允许控制策略u＝K_kX+e开始，其中K_k包含初始导纳参数，e为探索噪声，选择

设置迭代参数k,q为0。

步骤4-2：在时间间隔t∈[t_h-1,t_h]，h＝1,2,...内收集系统状态；

步骤4-3：收集到足够的系统状态后，用下面的贝尔曼方程求解H_k和K_k：

其中H_k＝A^TP_k+P_kA；

步骤4-4：更新

为

其中∈_k是步长；

步骤4-5：如果

则将P_k+1更新为P₀，q更新为q+1，k更新为k+1，执行第3步，其中B_q是有界的非空集合。

步骤4-6：如果

其中ε是一个小的常数，使用u^*＝-K_kX作为估计的最优策略。然后求解

得到估计的最优导纳参数c。否则，将P_k+1更新为

k更新为k+1，执行步骤3。

本发明提出了一种基于最优导纳参数的人机协作方法。建立了包括内外环在内的控制结构。设计了面向机器人的内环，使具有未知动力学的机器人表现为一个规定的导纳模型。

本发明的优点：通过使用BLF考虑了机器人的运动约束，从而提高了HRC任务的安全性；然后，在面向任务的外环中获得最优导纳参数，以使任务跟踪误差和交互力最小；提出了一种基于积分强化学习的无模型方法来寻找最优导纳参数；此外，设计了基于特定任务轨迹的辅助力，保证了良好的任务跟踪。

附图说明

图1是本发明的人机交互场景。

图2是本发明的系统控制结构。

图3是本发明的机器人内环设计图。

图4是本发明的机器人外环设计图。

图5是本发明的实验过程说明。

具体实施方式

下面结合附图进一步说明本发明的技术方案。

本发明的一种基于最优导纳参数的自适应人机协作控制方法，具体过程如下：

步骤1：本发明使用的是Franka Emika Panda机器人。它有7个柔性关节，每个关节都有角度和扭矩传感器。设置关节1-3的最大扭矩设置为25Nm，关节4-7的最大扭矩设置为20Nm。机器人的通信频率为1000Hz。操作系统是Ubuntu 16.04，开发平台是ROS Kinetic。由于计算机的计算速度有限，我们使用如图5所示的两个ROS节点来控制机器人并进行计算。第一个节点可以计算出自适应约束控制器，并将结果传递给第二个节点。第二个节点接收来自机器人的信息，并发布控制信号来控制机器人。两个节点之间的通信频率为200Hz。

步骤2：为了验证自适应约束控制器的有效性，进行了一项没有人机协作的机器人轨迹跟踪任务。期望轨迹预先生成。将S(·)设为高斯函数。神经网络节点个数为16个，在[-1,1]区域内选择每个节点的中心。中心方差设为1.5。初始权重

为0。控制参数K₂＝diag(12,12,12,10,10,10,10)。跟踪误差约束设为k_b1＝4.5，k_b2＝4.7，k_b3＝4.7，k_b4＝2.7，k_b5＝2.5，k_b6＝2.5，k_b7＝2.7，k_1i＝3，σ_i＝0.02，Γ_i＝200I_16×16(i＝1,2...7)

步骤3：在Franka机器人上进行HRC任务。操作者手持机器人的末端执行器，在起点和终点之间进行有规律的点对点运动。末端执行器的旋转和高度是恒定的，这意味着我们只关注x方向和y方向。该自适应约束控制器的参数与之前的步骤一致。具体的任务轨迹在x方向和y方向均设置为0.106sin(0.00125πt)。随后,初始参数M_d、B_d和K_d的导纳模型设置为M_d＝diag(5,5)、B_d＝diag(7,7)和K_d＝diag(3,3)，增益矩阵设为G_h＝diag(0.2,0.2)。将探测噪声加入到相互力中，设为

其中ω_i∈[-10,10]。

步骤4：为了优化HRC任务的性能，设计了外环。积分强化学习参数设为Q＝diag(5,5,5,5,10,10)，R＝I₂，T＝0.01s，采样时间为t＝5s。取P₀＝0.001I₆，∈_k＝(k+1)^-1，B_q＝10(q+1)，ε＝0.01。

本发明提出了一种基于最优导纳参数的人机协作方法。建立了包括内外环在内的控制结构。设计了面向机器人的内环，使具有未知动力学的机器人表现为一个规定的导纳模型。与以往的机器人控制方法相比，该方法通过使用BLF考虑了机器人的运动约束。从而提高了HRC任务的安全性。然后，在面向任务的外环中获得最优导纳参数，以使任务跟踪误差和交互力最小。提出了一种基于积分强化学习的无模型方法来寻找最优导纳参数。此外，设计了基于特定任务轨迹的辅助力，保证了良好的任务跟踪。

本说明书实施例所述的内容仅仅是对发明构思的实现形式的列举，本发明的保护范围不应当被视为仅限于实施例所陈述的具体形式，本发明的保护范围也及于本领域技术人员根据本发明构思所能够想到的等同技术手段。

Claims (1)

1.一种基于最优导纳参数的自适应人机协作控制方法，具体步骤如下：

步骤1：描述具有HRC任务的机器人动力学模型和用于机器人导纳控制的机器人导纳模型；

机器人动力学模型为：

其中q，

分别为关节角、速度和加速度矢量；x,

分别为末端执行器位置、速度和加速度矢量；M(q)∈R^p×p为对称惯性矩阵，

为科氏力和向心力的矢量，G(q)∈R^p表示重力矢量，J(q)∈R^n×p为雅可比矩阵，τ∈R^p为机器人控制输入力矩，f_h∈Rⁿ为由操作者产生的相互作用力，G_h∈Rⁿ表示人的输入力增益，κ(·)为逆运动学的解，p为机器人关节的数量，n表示笛卡尔空间中的维数；

机器人导纳模型描述为：

其中x_m是期望参考轨迹，即机器人导纳模型和x_d的输出是预定的任务轨迹，M_d，B_d和K_d分别代表期望的惯性、阻尼和刚度系数；

是系统辅助力，对应于x_m和x_d的关节坐标可以表示为q_m和q_d；

步骤2：设计基于BLF的机器人自适应约束控制器的内环；

内环设计的目的是使机器人的行为跟踪规定的机器人导纳模型的输出，并保持在一个安全工作空间内；不需要任务信息，只需关注于最小化规定的机器人导纳模型输出与机器人实际运动之间的跟踪误差；具体步骤如下：

步骤2-1：将节点约束设为|q|＜k_l，其中k_l＝[k_l1,...,k_lp]为约束边界；取q₁＝q，

机器人动力学(1)可以表示为：

定义跟踪误差e₁＝q₁-q_m，e₂＝q₂-α，其中，

其中，k_i(i＝1,…,p)是正常数，

代表跟踪误差的约束条件，

其中是由q_m的最大绝对值组成的向量；对e₂相对于时间求微分：

步骤2-2：构造一个log型BLF候选V₁，如下所示：

对V₁相对于时间求微分得到：

步骤2-3：设计另一个BLF候选V₂：

对V₂相对于时间求微分得到：

根据摩尔-彭罗斯伪逆性质可以得到：

当e₂＝[0 0...0]^T时，等式(11)满足

因此系统渐进稳定；然而，e₂＝[0 0...0]^T的情况在实际中几乎是不可能的；

步骤2-4：对于其他情况，设计基于模型的控制器如下：

其中，

代表

的伪逆，K₂是满足

的增益矩阵；将(13)代入(11)得到

可见控制器(13)可以使系统稳定；另外，根据BLF的性质，误差e_1i满足-k_bi＜e_1i＜k_bi；因此，关节状态保持在区间|q_1i|＜k_li；

步骤2-5：将控制器(13)改写为：

由于在实际应用中，公式(1)中机器人模型总是未知的；考虑到神经网络可以逼近任何未知的连续函数，故将控制器改为公式(15)；其中

是神经网络的输入，S(·)表示径向基函数，

是估计的神经网络权重，l是神经网络节点数量，

表示理想的W^*TS(Z)的近似；

和W^*TS(Z)之间的关系由下给出：

其中W^*表示理想的神经网络权重，ε(Z)是近似误差；设计神经网络权值的更新规则为：

其中

表示

的第i列，

是常值增益矩阵，σ_i是提高系统鲁棒性的一个小的正常数；

步骤2-6：给出候选BLF的V₃为：

其中

对V₃相对时间微分并结合等式(14)-(16)，可以得到：

进一步根据(11)的性质，得到：

其中，

λ_min(·)和λ_max(·)分别表示矩阵的极小特征值和极大特征值；所有误差信号都是半全局一致最终有界；

步骤3：设计基于积分强化学习的任务优化的外环：

在外环设计中，对导纳模型的参数进行优化，以使任务跟踪误差和交互力最小；将导纳参数优化问题转化为LQR问题，在不考虑系统模型的情况下，采用积分强化学习的方法求解；系统辅助力的设计具体步骤如下：

步骤3-1：设计系统辅助力：

当手臂执行简单任务时，定义人手臂阻抗模型为(K_us+K_p)f_h＝k_ee_d, (22)

其中K_u，K_p，k_e表示不同操作者和任务的位置矩阵，e_d为跟踪误差，定义为

e_d＝x_d-x_m, (23)

让

其中

x_d和x_m分别表示特定任务轨迹和期望轨迹，式(22)可以写成：

将导纳模型(3)改写为如下状态空间方程：

其中

从式(28)，可以得到：

为了设计辅助力，假设系统是稳定的，即x_m→x_d；因此可以得到：

步骤3-2：设计最优控制器：

将HRC系统的性能函数定义为该系统的状态空间模型

其中Q＝diag(Q_d,Q_h)，

R＝R^T＞0；将性能函数优化问题转化为LQR问题，最优控制器设计为

其中P是代数黎卡提方程的解；

PA+A^TP-PBR^-1B^TP+Q＝0, (35)

矩阵R^-1B^TP包含了机器人导纳模型的参数和人的增益G_h；结合方程(27)和(34)可以得到

通过求解式(36)，可以得到最优导纳参数；

步骤4：利用积分强化学习求解LQR问题：

由于系统矩阵A和B中存在未知参数，采用一种基于值迭代的积分强化学习算法来估计最优控制器；导纳参数优化的具体算法如下：

步骤4-1：从允许控制策略u＝K_kX+e开始，其中K_k包含初始导纳参数，e为探索噪声，选择

设置迭代参数k,q为0；

步骤4-2：在时间间隔t∈[t_h-1,t_h]，h＝1,2,...内收集系统状态；

步骤4-3：收集到足够的系统状态后，用下面的贝尔曼方程求解H_k和K_k：

其中H_k＝A^TP_k+P_kA

步骤4-4：更新

为

其中∈_k是步长；

步骤4-5：如果

则将P_k+1更新为P₀，q更新为q+1，k更新为k+1，执行第3步，其中B_q是有界的非空集合；

步骤4-6：如果

其中ε是一个小的常数，使用u^*＝-K_kX作为估计的最优策略；然后求解

得到估计的最优导纳参数c；否则，将P_k+1更新为

k更新为k+1，执行步骤3。

Images