主从机器人的柔顺控制方法、系统、电子设备及存储介质

技术领域

本发明涉及机器人控制技术领域，特别涉及一种主从机器人的柔顺控制方法、系统、电子设备及存储介质。

背景技术

随着医疗技术的发展以及偏远地区的救治需求，催生了迫切的辅助手术机器人具有主从远程遥操作的手术需求，医疗机器人为了满足人机交互的安全性需求，一般都选择协作机器人作为执行机构，以便辅助医生手术操作的要求。一般主从手术机器人系统，主要由操作者、主机器人系统、网络信息传输通道、从机器人系统和外界工作环境组成。

在实际的应用场景中，通常采用机器人位置实时采集，通过末端位置或者关节位置姿态，映射给从端机器人的方案，通讯时率及采集周期的因素，会产生延迟位置姿态或者丢失过程姿态的问题，严重影响手术机器人的随动效果及跟随精度，因此，需要设计的控制方法不仅能使机器人完成工作目标，同时也要避免指令传输过程中抖动的问题及振动问题，保证系统的稳定性。

发明内容

本发明要解决的技术问题是为了克服现有技术中主从机器人系统由于通讯时率及采集周期的因素，会产生延迟位置姿态或者丢失过程姿态的问题的缺陷，提供一种主从机器人的柔顺控制方法、系统、电子设备及存储介质。

本发明是通过下述技术方案来解决上述技术问题：

本发明提供一种主从机器人的柔顺控制方法，所述柔顺控制方法包括：

获取主端机器人的环境力；

将所述环境力输入到所述主端机器人的关节状态预测模型以得到所述主端机器人的主关节位置增量；所述关节状态预测模型基于阻抗控制模型和机器人动力学模型构建得到；

将所述主关节位置增量作为从关节位置增量映射到从端机器人的关节以控制所述从机器人。

较佳地，获取主端机器人的环境力的步骤包括：

利用六维力传感器获取所述主端机器人末端的环境力和环境力矩；所述六维力传感器设置于所述主端机器人的末端。

较佳地，在利用六维力传感器获取所述主端机器人末端的环境力和环境力矩的步骤之前，所述柔顺控制方法包括：

获取设置所述六维力传感器后的所述主端机器人的初始末端负载、空间位姿、多组测得力和测得力矩以计算所述六维力传感器的初始力和初始力矩；

所述利用六维力传感器获取所述主端机器人末端的环境力和环境力矩的步骤包括：

根据当前测得力和测得力矩、所述初始力和初始力矩确定当前环境力和当前环境力矩。

较佳地，将所述环境力输入到所述主端机器人的关节状态预测模型的步骤之前，所述柔顺控制方法还包括：

基于阻抗控制模型和逆雅可比矩阵得到关节动力学模型；

基于所述关节动力学模型构建所述机器人动力学模型；

基于关节边界约束条件对所述机器人动力学模型进行模型预测控制以得到所述关节状态预测模型。

较佳地，通过以下公式得到所述关节动力学模型：

e

其中，f

通过以下公式得到所述机器人动力学模型：

q

其中，q为角位移，

通过以下公式得到所述关节状态预测模型：

其中，J

通过以下公式对所述关节状态预测模型进行矩阵化：

Q

其中，

τ＝[τ

A＝[A A

较佳地，在将所述环境力输入到所述主端机器人的关节状态预测模型的步骤之后，所述柔顺控制方法还包括：

构建所述关节状态预测模型的代价函数并加入约束条件；

求解所述代价函数以得到所述主关节位置增量。

较佳地，通过以下公式构建所述代价函数和约束条件：

s.t.P*Q

本发明还提供一种主从机器人的柔顺控制系统，其特征在于，所述柔顺控制系统包括：

环境力获取模块，用于获取主端机器人的环境力；

位置增量确定模块，用于将所述环境力输入到所述主端机器人的关节状态预测模型以得到所述主端机器人的主关节位置增量；所述关节状态预测模型基于阻抗控制模型和机器人动力学模型构建得到；

位置增量映射模块，用于将所述主关节位置增量作为从关节位置增量映射到从端机器人的关节以控制所述从机器人。

较佳地，所述环境力获取模块具体用于利用六维力传感器获取所述主端机器人末端的环境力和环境力矩；所述六维力传感器设置于所述主端机器人的末端。

较佳地，所述柔顺控制系统包括：

初始值计算模块，用于获取设置所述六维力传感器后的所述主端机器人的初始末端负载、空间位姿、多组测得力和测得力矩以计算所述六维力传感器的初始力和初始力矩；

所述环境力获取模块具体用于根据当前测得力和测得力矩、所述初始力和初始力矩确定当前环境力和当前环境力矩。

较佳地，所述柔顺控制系统还包括预测模型构建模块；

所述预测模型构建模块用于基于阻抗控制模型和逆雅可比矩阵得到关节动力学模型；

所述预测模型构建模块还用于基于所述关节动力学模型构建所述机器人动力学模型；

所述预测模型构建模块还用于基于关节边界约束条件对所述机器人动力学模型进行模型预测控制以得到所述关节状态预测模型。

较佳地，通过以下公式得到所述关节动力学模型：

e

其中，f

通过以下公式得到所述机器人动力学模型：

q

其中，q为角位移，

通过以下公式得到所述关节状态预测模型：

其中，J

通过以下公式对所述关节状态预测模型进行矩阵化：

Q

其中，

τ＝[τ

A＝[A A

较佳地，所述柔顺控制系统还包括：

代价函数构建模块，用于构建所述关节状态预测模型的代价函数并加入约束条件；

代价函数解析模块，用于求解所述代价函数以得到所述主关节位置增量。

较佳地，通过以下公式构建所述代价函数和约束条件：

s.t.P*Q

本发明还提供一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如上所述的主从机器人的柔顺控制方法。

本发明还提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现如上所述的主从机器人的柔顺控制方法。

本发明的积极进步效果在于：

本发明提供的主从机器人的柔顺控制方法，采用基于外部交互力的阻抗控制策略，通过采集主机器人的速度与力，预测下一步主机器人的位置姿态，使主端机器人符合医疗应用场景要求，将该位姿信息发给从端机器人，实现主从机器人的柔顺控制功能的同时，实现从机器人的跟随控制。

附图说明

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

图1为本发明实施例1中的主从机器人的柔顺控制方法的第一流程图。

图2为本发明实施例1中的主从机器人的柔顺控制方法的原理图。

图3为本发明实施例1中的主从机器人的柔顺控制方法的第二流程图。

图4为本发明实施例2中的主从机器人的柔顺控制系统的第一结构示意图。

图5为本发明实施例2中的主从机器人的柔顺控制系统的第一结构示意图。

图6为本发明实施例3中的电子设备的结构示意图。

具体实施方式

下面通过实施例的方式进一步说明本发明，但并不因此将本发明限制在所述的实施例范围之中。

在本文提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本申请的至少一个实施例中。在文中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是，本文所描述的实施例可以与其它实施例相结合。

应当理解，本文使用的“系统”、“装置”、“单元”和/或“模组”是用于区分不同级别的不同组件、元件、部件、部分或装配的一种方法。然而，如果其他词语可实现相同的目的，则可通过其他表达来替换所述词语。

如本文中所示，除非上下文明确提示例外情形，“一”、“一个”、“一种”和/或“该”等词并非特指单数，也可包括复数。一般说来，术语“包括”与“包含”仅提示包括已明确标识的步骤和元素，而这些步骤和元素不构成一个排它性的罗列，方法或者设备也可能包含其它的步骤或元素。

本文中的包括的定义，如这里所使用的术语“具有”、“可以具有”、“包括”或“可以包括”指示本文的相应功能、操作、元件等的存在，并且不限制其它的一个或多个功能、操作、元件等的存在。此外应当理解到，如这里所使用的术语“包括”或“具有”是指示在说明书中所描述的特点、数字、步骤、操作、元件、部件或其组合的存在，而不排除一个或多个其它特点、数字、步骤、操作、元件、部件或其组合的存在或增加。

本文中和/或的定义，如这里所使用的术语“A或B”、“A和/或B的至少之一”或“A和/或B的一个或多个”包括与其一起列举的单词的任意和所有组合。例如，“A或B”、“A和B的至少之一”或“A或B的至少之一”意味着(1)包括至少一个A，(2)包括至少一个B，或(3)包括至少一个A和至少一个B两者。

本文中使用了流程图用来说明根据本文的实施例的系统所执行的操作。应当理解的是，前面或后面操作不一定按照顺序来精确地执行。相反，可以按照倒序或同时处理各个步骤。同时，也可以将其他操作添加到这些过程中，或从这些过程移除某一步或数步操作。

实施例1

请参考图1，其为本实施例中的主从机器人的柔顺控制方法的第一流程图。具体的，如图1所示，所述柔顺控制方法包括：

S101、获取主端机器人的环境力。

S102、将环境力输入到主端机器人的关节状态预测模型以得到主端机器人的主关节位置增量；关节状态预测模型基于阻抗控制模型和机器人动力学模型构建得到。

S103、将主关节位置增量作为从关节位置增量映射到从端机器人的关节以控制从机器人。

请参考图2，其为本实施例中的主从机器人的柔顺控制方法的原理图。具体的，如图2所示，基于阻抗控制器和机器人动力学模型构建机器人预测控制器，通过采集主机器人的速度与环境力，预测下一步主机器人的位置姿态，实现主端机器人对环境的自适应性控制，并将该位姿信息发给从机器人，实现主从机器人的柔顺控制功能的同时，实现从机器人的跟随控制。

在一种可选的实施方式中，步骤S101包括：

利用六维力传感器获取主端机器人末端的环境力和环境力矩；六维力传感器设置于主端机器人的末端。具体地，六维力传感器又叫六维力和力矩传感器，是一种能够在笛卡尔坐标系中同时测量力和力矩并且可以各三个分量的转换成为电信号的器件。

请参考图3，其为本实施例中的主从机器人的柔顺控制方法的第二流程图。具体的，如图3所示，在另一种可选的实施方式中，在步骤S101之前，柔顺控制方法包括：

S001、获取设置六维力传感器后的主端机器人的初始末端负载、空间位姿、多组测得力和测得力矩以计算六维力传感器的初始力和初始力矩。具体地，对机器人末端的力传感器进行预处理步骤如下：

A.通过称重方法测量出机器人末端负载(安装在末端六维力传感器之后的负载)的重量为Mg。

B.通过运动学关系得到传感器在机器人基坐标系中的位姿：

Psensor(Psensor.x,Psensor.y,Psensor.z,Psensor.rx,Psensor.ry,Psensor.rz)；

C.计算末端负载重力在传感器坐标系X、Y、Z方向上的分量：Mgx＝(-cos(Psensor.rx)×sin(Psensor.ry)×cos(Psensor.rz)+sin(Psensor.rx)×sin(Psensor.rz))×(-Mg)Mgy＝(cos(Psensor.rx)×sin(Psensor.ry)×sin(Psensor.rz)+sin(Psensor.rx)×cos(Psensor.rz))×(-Mg)Mgz＝(cos(Psensor.rx)×cos(Psensor.ry))×(-Mg)。

D.六维力传感器测得力Sensor.F/力矩Sensor.T为：

(Sensor.Fx,Sensor.Fy,Sensor.Fz,Sensor.Tx,Sensor.Ty,Sensor.Tz)。

E.计算六维力传感器力初始化值InF：

InFx＝Sensor.Fx-Mgx

InFy＝Sensor.Fy-Mgy

InFz＝Sensor.Fz-Mgz

因为末端负载重力产生的重力矩对传感器力矩的初始化标定影响很小，所以六维力传感器初始化值力矩值InT就直接用测得的力矩值Sensor.T：

InTx＝Sensor.Tx

InTy＝Sensor.Ty

InTz＝Sensor.Tz

最后用若干组均值来提高六维力传感器力/力矩初始化值的准确定。

此时，步骤S101包括：

S1011、根据当前测得力和测得力矩、初始力和初始力矩确定当前环境力和当前环境力矩。具体地，根据以下公式确定环境力ExF。

ExFx＝Sensor.Fx-InFx

ExFy＝Sensor.Fy-InFy

ExFz＝Sensor.Fz-InFz

在一种可选的实施方式中，在步骤S102之前，柔顺控制方法还包括：

S002、基于阻抗控制模型和逆雅可比矩阵得到关节动力学模型；

S003、基于关节动力学模型构建机器人动力学模型；

S004、基于关节边界约束条件对机器人动力学模型进行模型预测控制以得到关节状态预测模型。

具体地，通过以下公式得到关节动力学模型：

e

其中，f

通过以下公式得到机器人动力学模型：

q

其中，q为角位移，

由于各个关节都是有边界约束，通过采用MPC(Model Predictive Control，模型预测控制)的预测控制方法，预测下一步的关节变化量△q

上述阻抗控制动力学模型，可以转化为：

整理可以获得矩阵形式，作为预测控制的状态迭代方程：

其中，J

通过以下公式对关节状态预测模型进行矩阵化：

Q

其中，

τ＝[τ

A＝[A A

在一种可选的实施方式中，在步骤S102之后，柔顺控制方法还包括：

S005、构建关节状态预测模型的代价函数并加入约束条件；

S006、求解代价函数以得到主关节位置增量。

具体地，通过以下公式构建代价函数：

约束条件为：

s.t.P*Q

其中，Q

本实施例提供的主从机器人的柔顺控制方法，采用基于外部交互力的阻抗控制策略，通过采集主机器人的速度与力，预测下一步主机器人的位置姿态，使主端机器人符合医疗应用场景要求，将该位姿信息发给从端机器人，实现主从机器人的柔顺控制功能的同时，实现从机器人的跟随控制；基于六维力传感器的数据补偿方法对传感器进行校正，精确计算各个方向的力和力矩的数据。

实施例2

请参考图4，其为本实施例中的主从机器人的柔顺控制系统的第一结构示意图。具体的，如图4所示，所述柔顺控制系统包括：

环境力获取模块1，用于获取主端机器人的环境力；

位置增量确定模块2，用于将环境力输入到主端机器人的关节状态预测模型以得到主端机器人的主关节位置增量；关节状态预测模型基于阻抗控制模型和机器人动力学模型构建得到；

位置增量映射模块3，用于将主关节位置增量作为从关节位置增量映射到从端机器人的关节以控制从机器人。

请参考图2，其为本实施例中的主从机器人的柔顺控制系统的原理图。具体的，如图2所示，基于阻抗控制器和机器人动力学模型构建机器人预测控制器，通过采集主机器人的速度与环境力，预测下一步主机器人的位置姿态，实现主端机器人对环境的自适应性控制，并将该位姿信息发给从机器人，实现主从机器人的柔顺控制功能的同时，实现从机器人的跟随控制。

在一种可选的实施方式中，环境力获取模块1具体用于利用六维力传感器获取主端机器人末端的环境力和环境力矩；六维力传感器设置于主端机器人的末端。具体地，六维力传感器又叫六维力和力矩传感器，是一种能够在笛卡尔坐标系中同时测量力和力矩并且可以各三个分量的转换成为电信号的器件。

请参考图5，其为本实施例中的主从机器人的柔顺控制系统的第二结构示意图。具体的，如图5所示，在另一种可选的实施方式中，柔顺控制系统包括：

初始值计算模块4，用于获取设置六维力传感器后的主端机器人的初始末端负载、空间位姿、多组测得力和测得力矩以计算六维力传感器的初始力和初始力矩；

具体地，对机器人末端的力传感器进行预处理步骤如下：

A.通过称重方法测量出机器人末端负载(安装在末端六维力传感器之后的负载)的重量为Mg。

B.通过运动学关系得到传感器在机器人基坐标系中的位姿：

Psensor(Psensor.x,Psensor.y,Psensor.z,Psensor.rx,Psensor.ry,Psensor.rz)；

C.计算末端负载重力在传感器坐标系X、Y、Z方向上的分量：Mgx＝(-cos(Psensor.rx)×sin(Psensor.ry)×cos(Psensor.rz)+sin(Psensor.rx)×sin(Psensor.rz))×(-Mg)Mgy＝(cos(Psensor.rx)×sin(Psensor.ry)×sin(Psensor.rz)+sin(Psensor.rx)×cos(Psensor.rz))×(-Mg)Mgz＝(cos(Psensor.rx)×cos(Psensor.ry))×(-Mg)。

D.六维力传感器测得力/力矩为：

(Sensor.Fx,Sensor.Fy,Sensor.Fz,Sensor.Tx,Sensor.Ty,Sensor.Tz)。

E.计算六维力传感器力/力矩初始化值：

InFx＝Sensor.Fx-Mgx

InFy＝Sensor.Fy-Mgy

InFz＝Sensor.Fz-Mgz

因为末端负载重力产生的重力矩对传感器力矩的初始化标定影响很小，所以六维力传感器初始化值力矩值就直接用测得的力矩值：

InTx＝Sensor.Tx

InTy＝Sensor.Ty

InTz＝Sensor.Tz

最后用若干组均值来提高六维力传感器力/力矩初始化值的准确定。

此时，环境力获取模块1具体用于根据当前测得力和测得力矩、初始力和初始力矩确定当前环境力和当前环境力矩。具体地，根据以下公式确定环境力。

ExFx＝Sensor.Fx-InFx

ExFy＝Sensor.Fy-InFy

ExFz＝Sensor.Fz-InFz

在一种可选的实施方式中，柔顺控制系统还包括预测模型构建模块5；

预测模型构建模块5用于基于阻抗控制模型和逆雅可比矩阵得到关节动力学模型；

预测模型构建模块5还用于基于关节动力学模型构建机器人动力学模型；

预测模型构建模块5还用于基于关节边界约束条件对机器人动力学模型进行模型预测控制以得到关节状态预测模型。

具体地，通过以下公式得到关节动力学模型：

e

其中，f

通过以下公式得到机器人动力学模型：

q

其中，q为角位移，

由于各个关节都是有边界约束，通过采用MPC(Model Predictive Control，模型预测控制)的预测控制方法，预测下一步的关节变化量△q

上述阻抗控制动力学模型，可以转化为：

整理可以获得矩阵形式，作为预测控制的状态迭代方程：

其中，J

通过以下公式对关节状态预测模型进行矩阵化：

Q

其中，

τ＝[τ

A＝[A A

在一种可选的实施方式中，柔顺控制系统还包括：

代价函数构建模块6，用于构建关节状态预测模型的代价函数并加入约束条件；

代价函数解析模块7，用于求解代价函数以得到主关节位置增量。

较佳地，通过以下公式构建代价函数：

约束条件为：

s.t.P*Q

其中，Q

本实施例提供的主从机器人的柔顺控制系统，采用基于外部交互力的阻抗控制策略，通过采集主机器人的速度与力，预测下一步主机器人的位置姿态，使主端机器人符合医疗应用场景要求，将该位姿信息发给从端机器人，实现主从机器人的柔顺控制功能的同时，实现从机器人的跟随控制；基于六维力传感器的数据补偿方法对传感器进行校正，精确计算各个方向的力和力矩的数据。

实施例3

图6为本发明实施例3提供的一种电子设备的结构示意图。所述电子设备包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现实施例1的主从机器人的柔顺控制方法。图6显示的电子设备30仅仅是一个示例，不应对本发明实施例的功能和使用范围带来任何限制。

如图6所示，电子设备30可以以通用计算设备的形式表现，例如其可以为服务器设备。电子设备30的组件可以包括但不限于：上述至少一个处理器31、上述至少一个存储器32、连接不同系统组件(包括存储器32和处理器31)的总线33。

总线33包括数据总线、地址总线和控制总线。

存储器32可以包括易失性存储器，例如随机存取存储器(RAM)321和/或高速缓存存储器322，还可以进一步包括只读存储器(ROM)323。

存储器32还可以包括具有一组(至少一个)程序模块324的程序/实用工具325，这样的程序模块324包括但不限于：操作系统、一个或者多个应用程序、其它程序模块以及程序数据，这些示例中的每一个或某种组合中可能包括网络环境的实现。

处理器31通过运行存储在存储器32中的计算机程序，从而执行各种功能应用以及数据处理，例如本发明实施例1的主从机器人的柔顺控制方法。

电子设备30也可以与一个或多个外部设备34(例如键盘、指向设备等)通信。这种通信可以通过输入/输出(I/O)接口35进行。并且，模型生成的设备30还可以通过网络适配器36与一个或者多个网络(例如局域网(LAN)，广域网(WAN)和/或公共网络，例如因特网)通信。如图所示，网络适配器36通过总线33与模型生成的设备30的其它模块通信。应当明白，尽管图中未示出，可以结合模型生成的设备30使用其它硬件和/或软件模块，包括但不限于：微代码、设备驱动器、冗余处理器、外部磁盘驱动阵列、RAID(磁盘阵列)系统、磁带驱动器以及数据备份存储系统等。

应当注意，尽管在上文详细描述中提及了电子设备的若干单元/模块或子单元/模块，但是这种划分仅仅是示例性的并非强制性的。实际上，根据本发明的实施方式，上文描述的两个或更多单元/模块的特征和功能可以在一个单元/模块中具体化。反之，上文描述的一个单元/模块的特征和功能可以进一步划分为由多个单元/模块来具体化。

实施例4

本实施例提供了一种计算机可读存储介质，其上存储有计算机程序，所述程序被处理器执行时实现实施例1的主从机器人的柔顺控制方法。

其中，可读存储介质可以采用的更具体可以包括但不限于：便携式盘、硬盘、随机存取存储器、只读存储器、可擦拭可编程只读存储器、光存储器件、磁存储器件或上述的任意合适的组合。

在可能的实施方式中，本发明还可以实现为一种程序产品的形式，其包括程序代码，当所述程序产品在终端设备上运行时，所述程序代码用于使所述终端设备执行实现实施例1的主从机器人的柔顺控制方法。

其中，可以以一种或多种程序设计语言的任意组合来编写用于执行本发明的程序代码，所述程序代码可以完全地在用户设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户设备上部分在远程设备上执行或完全在远程设备上执行。

虽然以上描述了本发明的具体实施方式，但是本领域的技术人员应当理解，这仅是举例说明，本发明的保护范围是由所附权利要求书限定的。本领域的技术人员在不背离本发明的原理和实质的前提下，可以对这些实施方式做出多种变更或修改，但这些变更和修改均落入本发明的保护范围。