CN114260896B - 一种协作机器人的柔顺力控制方法及系统 - Google Patents

Abstract

本发明公开了一种协作机器人的柔顺力控制方法及系统，其中，所述方法包括：建立协作机器人末端的稳定力控制策略模型；建立协作机器人的刚度优化策略模型；建立协作机器人的奇异规避模型；基于所述稳定力控制策略模型、所述刚度优化策略模型和所述奇异规避模型建立实时控制量求解模型，并进行实时控制量求解，获得所述协作机器人的实时控制量；基于所述实时控制量对所述协作机器人进行柔顺力的控制处理。在本发明实施例中，能够实现对接触力的高精度控制，并能够实时提升协作机器人系统的刚度，从而抑制颤振；能够有效规避系统陷入奇异位型，同时满足机器人的关节角度、角速度限制。

Description

一种协作机器人的柔顺力控制方法及系统

技术领域

本发明涉及机器人控制技术领域，尤其涉及一种协作机器人的柔顺力控制方法及系统。

背景技术

协作机器人具备柔性好、人机交互灵活的优势，在工业、医疗等领域已取得了广泛的应用。其中柔顺力控制是协作机器人实现复杂任务高质量作业以及人机交互等的核心。通过对接触力的实时感知以及控制量的在线生成，可以实现机器人末端执行器按照预期的接触力施加于作用对象上。现有针对具有冗余自由度的协作机器人的力控制方法，主要是基于机器人雅克比矩阵的伪逆计算实现。但是这种方法难以处理系统的物理约束。CN201911358551.4提出了一种无需机器算雅克比矩阵的伪逆，但是这种空间解耦的方法建模困难，同时没有考虑系统刚度不足的问题。

然而，由于协作机器人自身结构的限制，系统的刚度较传统工业机器人有明显不足，使得机器人在机交互过程中能够容易出现颤振、误差较低等现象，使系统的稳定性显著下降。CN202010141357.7提出了一种机器人的刚度优化算法，该类方法是在机器人的结构设计过程中，通过对本体结构的优化完成的。在机器人本体固化的前提下，该方法无法通过调整机器人位型实现系统的刚度优化。

现有机器人力控制方法，通常采用基于雅克比矩阵求逆的方法，即针对末端接触力，采用雅克比矩阵的逆矩阵算子进行空间变化，从而计算机器人的关节控制量。这种方法虽然直观，但是这种方法通常难以处理复杂的系统约束；在刚度优化方面，目前的主流方法一般采用离线优化，首先将机器人的运动轨迹离散化，然后建立系统的刚度优化指标，通过启发式算法对进行全局寻优。但是这种方法难以扩展到力控制的场景，其主要原因是由于实际力相应的不可预知性，导致机器人的期望运动轨迹获取困难，从而难以保证机器人的力控制精度。

发明内容

本发明的目的在于克服现有技术的不足，本发明提供了一种协作机器人的柔顺力控制方法及系统，能够实现对接触力的高精度控制，并能够实时提升协作机器人系统的刚度，从而抑制颤振；能够有效规避系统陷入奇异位型，同时满足机器人的关节角度、角速度限制。

为了解决上述技术问题，本发明实施例提供了一种协作机器人的柔顺力控制方法，所述方法包括：

建立协作机器人末端的稳定力控制策略模型；

建立协作机器人的刚度优化策略模型；

建立协作机器人的奇异规避模型；

基于所述稳定力控制策略模型、所述刚度优化策略模型和所述奇异规避模型建立实时控制量求解模型，并进行实时控制量求解，获得所述协作机器人的实时控制量；

基于所述实时控制量对所述协作机器人进行柔顺力的控制处理。

可选的，所述建立协作机器人末端的稳定力控制策略模型，包括：

利用组坑控制原理，建立所述协作机器人的末端相对运动与接触力之间的关系；

基于末端相对运动与接触力之间的关系建立协作机器人末端的稳定力控制策略模型。

可选的，所述端相对运动与接触力之间的关系如下：

所述协作机器人末端的稳定力控制策略模型如下：

其中，C_m表示组坑模型；G_m表示弹性系数；F表示接触力；x表示协作机器人末端执行器的位置；表示协作机器人末端执行器的速度；x_d表示协作机器人末端执行器的期望位置；/>表示协作机器人末端执行器的的期望速度；/>表示协作机器人末端执行器的反馈速度；F_d表示期望接触力；t表示时间；k表示大于0的控制参数。

可选的，所述建立协作机器人的刚度优化策略模型，包括：

建立所述协作机器人的刚度指标，并建立所述刚度指标与决策量之间的映射关系；

基于所述映射关系进行刚度指标重构处理，获得刚度优化策略模型。

可选的，所述映射关系如下：

det((JJ^T)^-1)； (3)

其中，det表示方阵对应的行列式的值；T表示矩阵的转置算子；-1表示矩阵的求逆算子。

可选的，所述基于所述映射关系进行刚度指标重构处理，获得刚度优化策略模型，如下：

对所述映射关系的式子进行简化，将性能指标改写为：det(JJ^T)，此时，所述协作机器人的刚度优化转为：

min det(JJ^T)； (4)

根据所述协作机器人的关节角度θ对det(JJ^T)进行展开，得到刚度优化与机器人关节运动之间的映射关系即为刚度优化策略模型，如下：

其中，det表示方阵对应的行列式的值；T表示矩阵的转置算子；-1表示矩阵的求逆算子；θ_i表示θ的第i个值，i＝1,2,3,…,n；/>表示协作机器人的关节角速度；J表示协作机器人的雅克比矩阵；vec表示将矩阵转化为列向量的算子。

可选的，所述建立协作机器人的奇异规避模型，包括：

建立所述协作机器人的灵活性指标

建立协作机器人的灵活性约束如下：

根据决策变量对式(6)进行改写如下：

其中，β、η均表示正常数；其中，‖ ‖_F表示求取矩阵Frobenoius范数的算子；表示J的伪逆；det表示方阵对应的行列式的值；trace表示矩阵的迹。

可选的，所述实时控制量求解模型如下：

其中，T表示矩阵的转置算子；-1表示矩阵的求逆算子；θ_i表示θ的第i个值，i＝1,2,3,…,n；/>表示协作机器人的关节角速度；J表示协作机器人的雅克比矩阵；vec表示将矩阵转化为列向量的算子；κ表示协作机器人的灵活性指标；β、η均表示正常数；θ^-、θ⁺分别表示θ的下界与上界；/>分别表示/>的下界与上界。

可选的，所述进行实时控制量求解，包括：

设置满足实时控制量求解模型的隐式解，对所述协作机器人的实时力控制与刚度优化如下：

其中，∈为大于0的神经网络参数；Proj表示一个饱和函数，定义如下：

表示为一个投影函数，定义为：

其中，表示协作机器人的关节角速度；J表示协作机器人的雅克比矩阵；T表示矩阵的转置算子；k表示协作机器人的灵活性指标；β、η均表示正常数；θ^-、θ⁺分别表示θ的下界与上界；/>分别表示/>的下界与上界；α表示正控制参数；/>表示协作机器人的关节角加速度；θ表示协作机器人的关节角度；/>表示协作机器人末端执行器的反馈速度；λ₁和λ₂均表示状态变量。

另外，本发明实施例还提供了一种协作机器人的柔顺力控制系统，所述系统包括：

第一建立模块：用于建立协作机器人末端的稳定力控制策略模型；

第二建立模块：用于建立协作机器人的刚度优化策略模型；

第三建立模块：用于建立协作机器人的奇异规避模型；

求解模块：用于基于所述稳定力控制策略模型、所述刚度优化策略模型和所述奇异规避模型建立实时控制量求解模型，并进行实时控制量求解，获得所述协作机器人的实时控制量；

控制模块：用于基于所述实时控制量对所述协作机器人进行柔顺力的控制处理。

在本发实施例中，能够实现对接触力的高精度控制，并能够实时提升协作机器人系统的刚度，从而抑制颤振；在协作机器人系统刚度较好时，通常会使机器人接近奇异位型，从而大大降低系统的灵活性，能够有效规避系统陷入奇异位型，同时满足机器人的关节角度、角速度限制。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见的，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。

图1是本发明实施例中的协作机器人的柔顺力控制方法的流程示意图；

图2是本发明实施例中的协作机器人的柔顺力控制系统的结构组成示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

实施例一

请参阅图1，图1是本发明实施例中的协作机器人的柔顺力控制方法的流程示意图。

如图1所示，一种协作机器人的柔顺力控制方法，所述方法包括：

S11：建立协作机器人末端的稳定力控制策略模型；

在本发明具体实施过程中，所述建立协作机器人末端的稳定力控制策略模型，包括：利用组坑控制原理，建立所述协作机器人的末端相对运动与接触力之间的关系；基于末端相对运动与接触力之间的关系建立协作机器人末端的稳定力控制策略模型。

进一步的，所述端相对运动与接触力之间的关系如下：

所述协作机器人末端的稳定力控制策略模型如下：

其中，C_m表示组坑模型；G_m表示弹性系数；F表示接触力；x表示协作机器人末端执行器的位置；表示协作机器人末端执行器的速度；x_d表示协作机器人末端执行器的期望位置；/>表示协作机器人末端执行器的的期望速度；/>表示协作机器人末端执行器的反馈速度；F_d表示期望接触力；t表示时间；k表示大于0的控制参数。

具体的，利用阻抗控制原理，建立机器人末端相对运动与接触力之间的关系，如下：

建立抗噪的协作机器人末端的稳定力控制策略模型如下：

其中，C_m表示组坑模型；G_m表示弹性系数；F表示接触力；x表示协作机器人末端执行器的位置；表示协作机器人末端执行器的速度；x_d表示协作机器人末端执行器的期望位置；/>表示协作机器人末端执行器的的期望速度；/>表示协作机器人末端执行器的反馈速度；F_d表示期望接触力；t表示时间；k表示大于0的控制参数。

S12：建立协作机器人的刚度优化策略模型；

在本发明具体实施过程中，所述建立协作机器人的刚度优化策略模型，包括：建立所述协作机器人的刚度指标，并建立所述刚度指标与决策量之间的映射关系；基于所述映射关系进行刚度指标重构处理，获得刚度优化策略模型。

进一步的，所述映射关系如下：

det((JJ^T)^-1)； (3)

其中，det表示方阵对应的行列式的值；T表示矩阵的转置算子；-1表示矩阵的求逆算子。

进一步的，所述基于所述映射关系进行刚度指标重构处理，获得刚度优化策略模型，如下：

对所述映射关系的式子进行简化，将性能指标改写为：det(JJ^T)，此时，所述协作机器人的刚度优化转为：

min det(JJ^T)； (4)

根据所述协作机器人的关节角度θ对det(JJ^T)进行展开，得到刚度优化与机器人关节运动之间的映射关系即为刚度优化策略模型，如下：

其中，det表示方阵对应的行列式的值；T表示矩阵的转置算子；-1表示矩阵的求逆算子；θ_i表示θ的第i个值，i＝1,2,3,…,n；/>表示协作机器人的关节角速度；J表示协作机器人的雅克比矩阵；vec表示将矩阵转化为列向量的算子。

具体的，建立机器人的刚度指标，并建立其与决策变量之间的映射关系如下：

det((JJ^T)^-1)； (3)

其中，det表示方阵对应的行列式的值；T表示矩阵的转置算子；-1表示矩阵的求逆算子。

对上述的式(3)进行简化，将性能指标改写为：det(JJ^T)，此时，所述协作机器人的刚度优化转为：

min det(JJ^T)； (4)

根据所述协作机器人的关节角度Δ对det(JJ^T)进行展开，得到刚度优化与机器人关节运动之间的映射关系即为刚度优化策略模型，如下：

其中，det表示方阵对应的行列式的值；T表示矩阵的转置算子；-1表示矩阵的求逆算子；θ_i表示θ的第i个值，i＝1,2,3,…,n；/>表示协作机器人的关节角速度；J表示协作机器人的雅克比矩阵；vec表示将矩阵转化为列向量的算子。

S13：建立协作机器人的奇异规避模型；

在本发明具体实施过程中，所述建立协作机器人的奇异规避模型，包括：

建立所述协作机器人的灵活性指标

建立协作机器人的灵活性约束如下：

根据决策变量对式(6)进行改写如下：

其中，β、η均表示正常数；其中，‖ ‖_F表示求取矩阵Frobenoius范数的算子；表示J的伪逆；det表示方阵对应的行列式的值；trace表示矩阵的迹。

具体的，3.1为避免机器人靠近奇异位型，建立机器人的灵活性指标

建立协作机器人的灵活性约束如下：

根据决策变量对式(6)进行改写如下：

其中，β、η均表示正常数；其中，‖ ‖_F表示求取矩阵Frobenoius范数的算子；表示J的伪逆；det表示方阵对应的行列式的值；trace表示矩阵的迹。

S14：基于所述稳定力控制策略模型、所述刚度优化策略模型和所述奇异规避模型建立实时控制量求解模型，并进行实时控制量求解，获得所述协作机器人的实时控制量；

在本发明具体实时过程中，所述实时控制量求解模型如下：

其中，T表示矩阵的转置算子；-1表示矩阵的求逆算子；θ_i表示θ的第i个值，i＝1,2,3,…,n；/>表示协作机器人的关节角速度；J表示协作机器人的雅克比矩阵；vec表示将矩阵转化为列向量的算子；κ表示协作机器人的灵活性指标；β、η均表示正常数；θ^-、θ⁺分别表示θ的下界与上界；/>分别表示/>的下界与上界。

进一步的，所述进行实时控制量求解，包括：

设置满足实时控制量求解模型的隐式解，对所述协作机器人的实时力控制与刚度优化如下：

其中，∈为大于0的神经网络参数；Proj表示一个饱和函数，定义如下：

表示为一个投影函数，定义为：

其中，表示协作机器人的关节角速度；J表示协作机器人的雅克比矩阵；T表示矩阵的转置算子；k表示协作机器人的灵活性指标；β、η均表示正常数；θ^-、θ⁺分别表示θ的下界与上界；/>分别表示/>的下界与上界；α表示正控制参数；/>表示协作机器人的关节角加速度；θ表示协作机器人的关节角度；/>表示协作机器人末端执行器的反馈速度；λ₁和λ₂均表示状态变量。

具体的，根据稳定力控制策略模型、刚度优化策略模型和奇异规避模型建立实时控制量求解模型如下：

其中，T表示矩阵的转置算子；-1表示矩阵的求逆算子；θ_i表示θ的第i个值，i＝1,2,3,…,n；/>表示协作机器人的关节角速度；J表示协作机器人的雅克比矩阵；vec表示将矩阵转化为列向量的算子；κ表示协作机器人的灵活性指标；β、η均表示正常数；θ^-、θ⁺分别表示θ的下界与上界；/>分别表示/>的下界与上界。

在对该实时控制量求解模型进行求解时，需要设计满足式(8)的隐式解，实现机器人的实时力控制与刚度优化如下：

其中，∈为大于0的神经网络参数；Proj表示一个饱和函数，定义如下：

表示为一个投影函数，定义为：

其中，表示协作机器人的关节角速度；J表示协作机器人的雅克比矩阵；T表示矩阵的转置算子；κ表示协作机器人的灵活性指标；β、η均表示正常数；θ^-、θ⁺分别表示θ的下界与上界；/>分别表示/>的下界与上界；α表示正控制参数；/>表示协作机器人的关节角加速度；θ表示协作机器人的关节角度；/>表示协作机器人末端执行器的反馈速度；λ₁和λ₂均表示状态变量。

S15：基于所述实时控制量对所述协作机器人进行柔顺力的控制处理。

在本发明具体实施过程中，最后根据该实时控制量对该协作机器人进行柔顺力的控制处理。

即，输入：控制器参数k、α、∈、β，协作机器人的机械臂的关节角限幅θ^-、θ⁺，角速度限幅机械臂初始状态θ(0)、/>期望轨迹x_d(t)，/>以及期望接触力F_d，在任务时长T，机械臂末端执行的位置反馈x(t)，接触力F(t)，关节反馈θ。

输出：机械臂的关节速度指令以实现位置-力控制与刚度优化：

1.初始化λ₁(0)，λ₂(0)；

Repeat

2.读取传感器测量值F，θ，x；

3.计算当前时刻的机器人雅克比矩阵J和H_i；

4.根据式(2)计算参考指令

5.根据式(4)-(7)计算不等式与优化指标参数M_F、

6.根据式(9b)(9c)更新状态变量λ₁、λ₂；

7.根据式(9a)更新控制量

Until(t>T)。

在本发实施例中，能够实现对接触力的高精度控制，并能够实时提升协作机器人系统的刚度，从而抑制颤振；在协作机器人系统刚度较好时，通常会使机器人接近奇异位型，从而大大降低系统的灵活性，能够有效规避系统陷入奇异位型，同时满足机器人的关节角度、角速度限制。

实施例二

请参阅图2，图2是本发明实施例中的协作机器人的柔顺力控制系统的结构组成示意图。

如图2所示，一种协作机器人的柔顺力控制系统，所述系统包括：

第一建立模块21：用于建立协作机器人末端的稳定力控制策略模型；

在本发明具体实施过程中，所述建立协作机器人末端的稳定力控制策略模型，包括：利用组坑控制原理，建立所述协作机器人的末端相对运动与接触力之间的关系；基于末端相对运动与接触力之间的关系建立协作机器人末端的稳定力控制策略模型。

进一步的，所述端相对运动与接触力之间的关系如下：

所述协作机器人末端的稳定力控制策略模型如下：

其中，C_m表示组坑模型；G_m表示弹性系数；F表示接触力；x表示协作机器人末端执行器的位置；表示协作机器人末端执行器的速度；x_d表示协作机器人末端执行器的期望位置；/>表示协作机器人末端执行器的的期望速度；/>表示协作机器人末端执行器的反馈速度；F_d表示期望接触力；t表示时间；k表示大于0的控制参数。

具体的，利用阻抗控制原理，建立机器人末端相对运动与接触力之间的关系，如下：

建立抗噪的协作机器人末端的稳定力控制策略模型如下：

/>

其中，C_m表示组坑模型；G_m表示弹性系数；F表示接触力；x表示协作机器人末端执行器的位置；表示协作机器人末端执行器的速度；x_d表示协作机器人末端执行器的期望位置；/>表示协作机器人末端执行器的的期望速度；/>表示协作机器人末端执行器的反馈速度；F_d表示期望接触力；t表示时间；k表示大于0的控制参数。

第二建立模块22：用于建立协作机器人的刚度优化策略模型；

在本发明具体实施过程中，所述建立协作机器人的刚度优化策略模型，包括：建立所述协作机器人的刚度指标，并建立所述刚度指标与决策量之间的映射关系；基于所述映射关系进行刚度指标重构处理，获得刚度优化策略模型。

进一步的，所述映射关系如下：

det((JJ^T)^-1)； (3)

其中，det表示方阵对应的行列式的值；T表示矩阵的转置算子；-1表示矩阵的求逆算子。

进一步的，所述基于所述映射关系进行刚度指标重构处理，获得刚度优化策略模型，如下：

对所述映射关系的式子进行简化，将性能指标改写为：det(JJ^T)，此时，所述协作机器人的刚度优化转为：

min det(JJ^T)； (4)

根据所述协作机器人的关节角度θ对det(JJ^T)进行展开，得到刚度优化与机器人关节运动之间的映射关系即为刚度优化策略模型，如下：

其中，det表示方阵对应的行列式的值；T表示矩阵的转置算子；-1表示矩阵的求逆算子；θ_i表示θ的第i个值，i＝1,2,3,…,n；/>表示协作机器人的关节角速度；J表示协作机器人的雅克比矩阵；vec表示将矩阵转化为列向量的算子。

具体的，建立机器人的刚度指标，并建立其与决策变量之间的映射关系如下：

det((JJ^T)^-1)； (3)

其中，det表示方阵对应的行列式的值；T表示矩阵的转置算子；-1表示矩阵的求逆算子。

对上述的式(3)进行简化，将性能指标改写为：det(JJ^T)，此时，所述协作机器人的刚度优化转为：

min det(JJ^T)； (4)

根据所述协作机器人的关节角度θ对det(JJ^T)进行展开，得到刚度优化与机器人关节运动之间的映射关系即为刚度优化策略模型，如下：

其中，det表示方阵对应的行列式的值；T表示矩阵的转置算子；-1表示矩阵的求逆算子；θ_i表示θ的第i个值，i＝1,2,3,…,n；/>表示协作机器人的关节角速度；J表示协作机器人的雅克比矩阵；vec表示将矩阵转化为列向量的算子。

第三建立模块23：用于建立协作机器人的奇异规避模型；

在本发明具体实施过程中，所述建立协作机器人的奇异规避模型，包括：

建立所述协作机器人的灵活性指标

建立协作机器人的灵活性约束如下：

根据决策变量对式(6)进行改写如下：

其中，β、η均表示正常数；其中，‖ ‖_F表示求取矩阵Frobenoius范数的算子；表示J的伪逆；det表示方阵对应的行列式的值；trace表示矩阵的迹。

具体的，3.1为避免机器人靠近奇异位型，建立机器人的灵活性指标

建立协作机器人的灵活性约束如下：

根据决策变量对式(6)进行改写如下：

/>

其中，β、η均表示正常数；其中，‖ ‖_F表示求取矩阵Frobenoius范数的算子；表示J的伪逆；det表示方阵对应的行列式的值；trace表示矩阵的迹。

求解模块24：用于基于所述稳定力控制策略模型、所述刚度优化策略模型和所述奇异规避模型建立实时控制量求解模型，并进行实时控制量求解，获得所述协作机器人的实时控制量；

在本发明具体实时过程中，所述实时控制量求解模型如下：

其中，T表示矩阵的转置算子；-1表示矩阵的求逆算子；θ_i表示θ的第i个值，i＝1,2,3,…,n；/>表示协作机器人的关节角速度；J表示协作机器人的雅克比矩阵；vec表示将矩阵转化为列向量的算子；κ表示协作机器人的灵活性指标；β、η均表示正常数；θ^-、θ⁺分别表示θ的下界与上界；/>分别表示/>的下界与上界。

进一步的，所述进行实时控制量求解，包括：

设置满足实时控制量求解模型的隐式解，对所述协作机器人的实时力控制与刚度优化如下：

其中，∈为大于0的神经网络参数；Proj表示一个饱和函数，定义如下：

表示为一个投影函数，定义为：

/>

其中，表示协作机器人的关节角速度；J表示协作机器人的雅克比矩阵；T表示矩阵的转置算子；k表示协作机器人的灵活性指标；β、η均表示正常数；θ^-、θ⁺分别表示θ的下界与上界；/>分别表示/>的下界与上界；α表示正控制参数；/>表示协作机器人的关节角加速度；θ表示协作机器人的关节角度；/>表示协作机器人末端执行器的反馈速度；λ₁和λ₂均表示状态变量。

具体的，根据稳定力控制策略模型、刚度优化策略模型和奇异规避模型建立实时控制量求解模型如下：

其中，T表示矩阵的转置算子；-1表示矩阵的求逆算子；θ_i表示θ的第i个值，i＝1,2,3,…,n；/>表示协作机器人的关节角速度；J表示协作机器人的雅克比矩阵；vec表示将矩阵转化为列向量的算子；κ表示协作机器人的灵活性指标；β、η均表示正常数；θ^-、θ⁺分别表示θ的下界与上界；/>分别表示/>的下界与上界。

在对该实时控制量求解模型进行求解时，需要设计满足式(8)的隐式解，实现机器人的实时力控制与刚度优化如下：

其中，∈为大于0的神经网络参数；Proj表示一个饱和函数，定义如下：

表示为一个投影函数，定义为：/>

其中，表示协作机器人的关节角速度；J表示协作机器人的雅克比矩阵；T表示矩阵的转置算子；k表示协作机器人的灵活性指标；β、η均表示正常数；θ^-、θ⁺分别表示θ的下界与上界；/>分别表示/>的下界与上界；α表示正控制参数；/>表示协作机器人的关节角加速度；θ表示协作机器人的关节角度；/>表示协作机器人末端执行器的反馈速度；λ₁和λ₂均表示状态变量。

控制模块25：用于基于所述实时控制量对所述协作机器人进行柔顺力的控制处理。

在本发明具体实施过程中，最后根据该实时控制量对该协作机器人进行柔顺力的控制处理。

即，输入：控制器参数k、α、∈、β，协作机器人的机械臂的关节角限幅θ^-、θ⁺，角速度限幅机械臂初始状态θ(0)、/>期望轨迹x_d(t)，/>以及期望接触力F_d，在任务时长T，机械臂末端执行的位置反馈x(t)，接触力F(t)，关节反馈θ。

输出：机械臂的关节速度指令以实现位置-力控制与刚度优化：

1.初始化λ₁(0)，λ₂(0)；

Repeat

2.读取传感器测量值F，θ，x；

3.计算当前时刻的机器人雅克比矩阵J和H_i；

4.根据式(2)计算参考指令

5.根据式(4)-(7)计算不等式与优化指标参数M_F、

6.根据式(9b)(9c)更新状态变量λ₁、λ₂；

7.根据式(9a)更新控制量

Until(t>T)。

在本发实施例中，能够实现对接触力的高精度控制，并能够实时提升协作机器人系统的刚度，从而抑制颤振；在协作机器人系统刚度较好时，通常会使机器人接近奇异位型，从而大大降低系统的灵活性，能够有效规避系统陷入奇异位型，同时满足机器人的关节角度、角速度限制。

本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于一计算机可读存储介质中，存储介质可以包括：只读存储器(ROM，ReadOnly Memory)、随机存取存储器(RAM，Random AccessMemory)、磁盘或光盘等。

另外，以上对本发明实施例所提供的一种协作机器人的柔顺力控制方法及系统进行了详细介绍，本文中应采用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (3)

1.一种协作机器人的柔顺力控制方法，其特征在于，所述方法包括：

建立协作机器人末端的稳定力控制策略模型；

建立协作机器人的刚度优化策略模型；

建立协作机器人的奇异规避模型；

基于所述稳定力控制策略模型、所述刚度优化策略模型和所述奇异规避模型建立实时控制量求解模型，并进行实时控制量求解，获得所述协作机器人的实时控制量；

基于所述实时控制量对所述协作机器人进行柔顺力的控制处理；

所述建立协作机器人末端的稳定力控制策略模型，包括：

利用组坑控制原理，建立所述协作机器人的末端相对运动与接触力之间的关系；

基于末端相对运动与接触力之间的关系建立协作机器人末端的稳定力控制策略模型；

所述末端相对运动与接触力之间的关系如下：

所述协作机器人末端的稳定力控制策略模型如下：

其中，C_m表示组坑模型；G_m表示弹性系数；F表示接触力；x表示协作机器人末端执行器的位置；表示协作机器人末端执行器的速度；x_d表示协作机器人末端执行器的期望位置；/>表示协作机器人末端执行器的的期望速度；/>表示协作机器人末端执行器的反馈速度；F_d表示期望接触力；t表示时间；k表示大于0的控制参数；

所述建立协作机器人的刚度优化策略模型，包括：

建立所述协作机器人的刚度指标，并建立所述刚度指标与决策量之间的映射关系；

基于所述映射关系进行刚度指标重构处理，获得刚度优化策略模型；

所述映射关系如下：

det((JJ^T)^-1)； (3)

其中，det表示方阵对应的行列式的值；T表示矩阵的转置算子；-1表示矩阵的求逆算子；

所述基于所述映射关系进行刚度指标重构处理，获得刚度优化策略模型，如下：

对所述映射关系的式子进行简化，将性能指标改写为：det(JJ^T)，此时，所述协作机器人的刚度优化转为：

min det(JJ^T)； (4)

根据所述协作机器人的关节角度θ对det(JJ^T)进行展开，得到刚度优化与机器人关节运动之间的映射关系即为刚度优化策略模型，如下：

其中，det表示方阵对应的行列式的值；T表示矩阵的转置算子；-1表示矩阵的求逆算子；θ_i表示θ的第i个值，i＝1,2,3,…,n；/>表示协作机器人的关节角速度；J表示协作机器人的雅克比矩阵；vec表示将矩阵转化为列向量的算子；

所述建立协作机器人的奇异规避模型，包括：

建立所述协作机器人的灵活性指标

建立协作机器人的灵活性约束如下：

根据决策变量对式(6)进行改写如下：

其中，β、η均表示正常数；其中，‖ ‖_F表示求取矩阵Frobenoius范数的算子；表示J的伪逆；det表示方阵对应的行列式的值；trace表示矩阵的迹；

所述实时控制量求解模型如下：

其中，T表示矩阵的转置算子；-1表示矩阵的求逆算子；θ_i表示θ的第i个值，i＝1,2,3,…,n；/>表示协作机器人的关节角速度；J表示协作机器人的雅克比矩阵；vec表示将矩阵转化为列向量的算子；κ表示协作机器人的灵活性指标；β、η均表示正常数；θ^-、θ⁺分别表示θ的下界与上界；/>分别表示/>的下界与上界。

2.根据权利要求1所述的柔顺力控制方法，其特征在于，所述进行实时控制量求解，包括：

设置满足实时控制量求解模型的隐式解，对所述协作机器人的实时力控制与刚度优化如下：

其中，∈为大于0的神经网络参数；Proj表示一个饱和函数，定义如下：

表示为一个投影函数，定义为：

其中，表示协作机器人的关节角速度；J表示协作机器人的雅克比矩阵；T表示矩阵的转置算子；κ表示协作机器人的灵活性指标；β、η均表示正常数；θ^-、θ⁺分别表示θ的下界与上界；/>分别表示/>的下界与上界；α表示正控制参数；/>表示协作机器人的关节角加速度；θ表示协作机器人的关节角度；/>表示协作机器人末端执行器的反馈速度；λ₁和λ₂均表示状态变量。

3.一种协作机器人的柔顺力控制系统，其特征在于，所述系统包括：

第一建立模块：用于建立协作机器人末端的稳定力控制策略模型；

第二建立模块：用于建立协作机器人的刚度优化策略模型；

第三建立模块：用于建立协作机器人的奇异规避模型；

求解模块：用于基于所述稳定力控制策略模型、所述刚度优化策略模型和所述奇异规避模型建立实时控制量求解模型，并进行实时控制量求解，获得所述协作机器人的实时控制量；

控制模块：用于基于所述实时控制量对所述协作机器人进行柔顺力的控制处理；

所述建立协作机器人末端的稳定力控制策略模型，包括：

利用组坑控制原理，建立所述协作机器人的末端相对运动与接触力之间的关系；

基于末端相对运动与接触力之间的关系建立协作机器人末端的稳定力控制策略模型；

所述末端相对运动与接触力之间的关系如下：

所述协作机器人末端的稳定力控制策略模型如下：

其中，C_m表示组坑模型；G_m表示弹性系数；F表示接触力；x表示协作机器人末端执行器的位置；表示协作机器人末端执行器的速度；x_d表示协作机器人末端执行器的期望位置；/>表示协作机器人末端执行器的的期望速度；/>表示协作机器人末端执行器的反馈速度；F_d表示期望接触力；t表示时间；k表示大于0的控制参数；

所述建立协作机器人的刚度优化策略模型，包括：

建立所述协作机器人的刚度指标，并建立所述刚度指标与决策量之间的映射关系；

基于所述映射关系进行刚度指标重构处理，获得刚度优化策略模型；

所述映射关系如下：

det((JJ^T)^-1)； (3)

其中，det表示方阵对应的行列式的值；T表示矩阵的转置算子；-1表示矩阵的求逆算子；

所述基于所述映射关系进行刚度指标重构处理，获得刚度优化策略模型，如下：

对所述映射关系的式子进行简化，将性能指标改写为：det(JJ^T)，此时，所述协作机器人的刚度优化转为：

min det(JJ^T)； (4)

根据所述协作机器人的关节角度θ对det(JJ^T)进行展开，得到刚度优化与机器人关节运动之间的映射关系即为刚度优化策略模型，如下：

其中，det表示方阵对应的行列式的值；T表示矩阵的转置算子；-1表示矩阵的求逆算子；θ_i表示θ的第i个值，i＝1,2,3,…,n；/>表示协作机器人的关节角速度；J表示协作机器人的雅克比矩阵；vec表示将矩阵转化为列向量的算子；

所述建立协作机器人的奇异规避模型，包括：

建立所述协作机器人的灵活性指标

建立协作机器人的灵活性约束如下：

根据决策变量对式(6)进行改写如下：

其中，β、η均表示正常数；其中，‖ ‖_F表示求取矩阵Frobenoius范数的算子；表示J的伪逆；det表示方阵对应的行列式的值；trace表示矩阵的迹；

所述实时控制量求解模型如下：

其中，T表示矩阵的转置算子；-1表示矩阵的求逆算子；θ_i表示θ的第i个值，i＝1,2,3,…,n；/>表示协作机器人的关节角速度；J表示协作机器人的雅克比矩阵；vec表示将矩阵转化为列向量的算子；κ表示协作机器人的灵活性指标；β、η均表示正常数；θ^-、θ⁺分别表示θ的下界与上界；/>分别表示/>的下界与上界。