CN111730598B - 一种机器人力位连锁控制方法、装置及系统 - Google Patents

Abstract

本发明涉及机器人控制技术领域，公开了一种机器人力位连锁控制方法，包括以下步骤：获取基于机器人内部传感器检测得到的末端工具的第一受力信息；获取基于机器人内部传感器检测得到的末端工具坐标系与机器人基座坐标系的第一变换矩阵；获取基于机器人外部传感器检测得到的末端工具的第二受力信息，获取基于机器人外部传感器检测得到的末端工具坐标系与机器人基座坐标系的第二变换矩阵；判断所述第一受力信息与所述第二受力信息的偏差是否小于受力偏差阈值，判断所述第一变换矩阵与所述第二变换矩阵的偏差是否小于变换偏差阈值，如果均小于，则启动受限区域控制，否则不启动受限区域控制。本发明具有受限区域内协作配合稳定性高的技术效果。

Description

一种机器人力位连锁控制方法、装置及系统

技术领域

本发明涉及机器人控制技术领域，具体涉及一种机器人力位连锁控制方法、装置、系统及计算机存储介质。

背景技术

机器人在协作环境中往往需要对末端工具的受力和位置进行一些约束，例如骨科手术机器人在手术中，用户经常会进行一些人机交互动作，即拖动机械臂在受限区域中进行运动，机器人末端安装的工具可能会与病人身体部位进行接触，此时就需要对末端工具的受力和位置进行约束，防止误操作导致病人收到末端工具的伤害。受限区域指需要进行力约束和位置约束的位置范围，受限区域可以设置为圆锥体型、圆锥体加圆柱体型、四面体型等，在手术机器人中受限区域与移动物体的位置相关，跟随移动物体的移动而实时变化，移动物体指需要进行手术的人体部位，例如髋关节、膝关节等。

在对机器人进行受限区域控制时，必然需要对末端工具的受力情况和位置信息进行检测，如果受力信息和位置信息的检测不精确，就会导致机器人协作配合时稳定性差的问题。

发明内容

本发明的目的在于克服上述技术不足，提供一种机器人力位连锁控制方法、装置、系统及计算机存储介质，解决现有技术中机器人末端工具受力信息与位置信息检测不精确，影响机器人在受限区域中协作配合的稳定性的技术问题。

为达到上述技术目的，本发明的技术方案提供一种机器人力位连锁控制系统，包括以下步骤：

获取基于机器人内部传感器检测得到的末端工具的第一受力信息；获取基于机器人内部传感器检测得到的末端工具坐标系与机器人基座坐标系的第一变换矩阵；

获取基于机器人外部传感器检测得到的末端工具的第二受力信息，获取基于机器人外部传感器检测得到的末端工具坐标系与机器人基座坐标系的第二变换矩阵；

判断所述第一受力信息与所述第二受力信息的偏差是否小于受力偏差阈值，判断所述第一变换矩阵与所述第二变换矩阵的偏差是否小于变换偏差阈值，如果均小于，则启动受限区域控制，否则不启动受限区域控制。

本发明还提供一种机器人力位连锁控制装置，包括处理器以及存储器，所述存储器上存储有计算机程序，所述计算机程序被所述处理器执行时，实现所述机器人力位连锁控制方法。

本发明还提供一种机器人力位连锁控制系统，包括所述机器人力位连锁控制装置，还包括机器人、末端力矩传感器、末端工具、第一光学阵列、第二光学阵列以及光学设备：

所述末端工具安装于所述机器人的末端；所述末端力矩传感器安装于所述机器人与所述末端工具之间，并用于检测末端工具的第二受力信息；所述第一光学阵列安装于所述末端工具上，所述第二光学阵列安装于所述机器人上，所述光学设备用于追踪所述第一光学阵列与所述第二光学阵列的位姿关系矩阵；所述机器人力位连锁控制装置用于根据所述第一光学阵列与所述第二光学阵列的位姿关系矩阵，计算末端工具坐标系与机器人基座坐标系的第一变换矩阵。

本发明还提供一种计算机存储介质，其上存储有计算机程序，所述计算机该程序被处理器执行时，实现所述机器人力位连锁控制方法。

与现有技术相比，本发明的有益效果包括：本发明对机器人进行受限区域控制之前，先对末端工具的受力信息以及位置信息检测精度进行校验，如果检测精度达到要求，再启动受限区域控制，如果没有达到要求，则不启动受限区域控制，保证受限区域控制时的力/位置的检测精度，进而提高机器人在受限区域中进行协作配合的稳定性。

附图说明

图1是本发明提供的机器人力位连锁控制方法一实施方式的流程图；

图2是本发明提供的机器人力位连锁控制系统一实施方式的结构示意图；

附图标记：

1、机器人；11、关节；2、末端力矩传感器；3、末端工具；31、法兰盘；41、第一光学阵列；42、第二光学阵列；43、第三光学阵列；44、光学设备；10、移动物体；20、受限区域。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

实施例1

如图1所示，本发明的实施例1提供了机器人力位连锁控制方法，包括以下步骤：

S1、获取基于机器人内部传感器检测得到的末端工具的第一受力信息；获取基于机器人内部传感器检测得到的末端工具坐标系与机器人基座坐标系的第一变换矩阵；

S2、获取基于机器人外部传感器检测得到的末端工具的第二受力信息，获取基于机器人外部传感器检测得到的末端工具坐标系与机器人基座坐标系的第二变换矩阵；

S3、判断所述第一受力信息与所述第二受力信息的偏差是否小于受力偏差阈值，判断所述第一变换矩阵与所述第二变换矩阵的偏差是否小于变换偏差阈值，如果均小于，则启动受限区域控制，否则不启动受限区域控制。

机器人的末端工具在受限区域内具有位置约束和力约束。受限区域与移动物体固定连接，移动物体移动时受限区域随动，移动物体包括但不限于髋关节膝关节等人体部位，受限区域包括但不限于圆锥体、圆锥体加圆柱体、四面体等。

本实施例对机器人在受限区域内协作过程中的力/位置进行双重联锁设计，从而确保力检测和位置检测的准确性。具体的，首先获取末端工具的第一受力信息，即受力检测数据；然后获取末端工具坐标系与机器人基座坐标系的第一变换矩阵，即位置检测数据。检测数据获取完成后，获取第二受力信息和第二变换矩阵，然后进行比较，只有受力信息偏差和变换矩阵偏差均在一定偏差范围内时，才会置信力检测和位置检测结果的准确性，启动受限区域控制模式，从而保证机器人进入受限区域控制模式后，进行力约束控制和位置约束控制时的控制精确度，提高机器人在受限区域协作配合时的稳定性。

本实施例对机器人进行受限区域控制之前，先对末端工具的受力信息以及位置信息的检测精度进行校验，如果检测精度达到要求，再启动受限区域控制，如果没有达到要求，则不启动受限区域控制，保证受限区域控制时的力/位置的检测精度，进而提高机器人在受限区域中进行协作配合的稳定性。

优选的，获取末端工具的第一受力信息，具体为：

获取机器人各关节的关节扭矩值，根据所述关节扭矩值计算所述第一受力信息。

优选的，通过所述关节扭矩值计算所述第一受力信息，具体为：

FT₁＝(J^T)⁺*τ

其中，FT₁为第一受力信息，(J^T)⁺为机器人的机械臂雅可比矩阵转置的伪逆，τ为机器人各关节的关节扭矩矩阵，τ＝[τ_i]，τ_i为机器人第i个关节的关节扭矩值，i＝1,2,L,n，n为机器人的关节数量。

由关节扭矩值推算出末端工具的第一受力信息。从理论上看，第一受力信息应该等于第二受力信息，但是第一受力信息一般都存在一定的检测偏差，因此两者之间存在偏差。本实施例通过获取该偏差判断第一受力信息的检测精确度。

优选的，获取末端工具坐标系与机器人基座坐标系的第一变换矩阵，具体为：

获取机器人各关节的角度值，采用机器人正运动学方法计算第一变换矩阵。

优选的，采用机器人正运动学方法计算第一变换矩阵，具体为：

T₁＝FKin(q₁,L,q_n)

其中，T₁为第一变换矩阵，FKin()为机器人正运动学函数，q_i为机器人第i个关节的角度值，i＝1,2,L,n，n为机器人的关节数量。

由关节角度值推算出末端工具坐标系与机器人基座坐标系的第一变换矩阵。从理论上看，第一变换矩阵应该等于第二变换矩阵，但是第一变换矩阵一般都存在一定的检测偏差，因此两者之间存在偏差。本实施例通过获取该偏差判断第一变换矩阵的检测精确度。

在推算出第一变换矩阵和第一受力信息，检测得到第二变换矩阵和第二受力信息后，分别计算受力信息和变换矩阵的偏差，并判断以下条件是否符合。

具体的，对于受力信息偏差的判断，需要判断条件(1)是否符合：

其中，FT₁为第一受力信息，FT₂为第二受力信息，||F₁-F₂||_∞表示向量差的无穷范数，ε_f表示力的最大容许偏差，||T₁-T₂||_∞表示力矩向量差的无穷范数，ε_τ表示力矩的最大容许偏差。上述实例是将六维力向量分为力向量和力矩向量，并分别求无穷范数衡量力/力矩的偏差。也可以使用其他形式的偏差判断来衡量力/力矩的偏差。

对于变换矩阵偏差的判断，需要判断条件(2)是否符合：

其中，T₁是第一变换矩阵，表示为位置向量p₁和欧拉角姿态向量r₁，即(p₁,r₁)^T；T₂是第二变换矩阵，表示为位置向量p₂和欧拉角姿态向量r₂，即(p₂,r₂)^T；→表示位姿态变换矩阵变换成位置向量和欧拉角姿态向量的变换方式。ε_p表示位置的最大容许偏差，ε_r表示欧拉角姿态向量的最大容许偏差。||p₁-p₂||_∞表示位置向量差的无穷范数，||r₁-r₂||_∞表示欧拉角姿态向量差的无穷范数。

上述实例是将位姿矩阵变换为位置向量和欧拉角姿态向量，并分别求无穷范数进行偏差判断。也可以使用其他形式的偏差判断来衡量位姿的偏差。

当上述两个条件(1)、(2)均满足时，置信力和位置的检测数据准确性，启动受限区域控制模式。

T₁是机器人根据机械臂本身属性推算出来的一个位置坐标变换矩阵，T₂是检测计算出来的一个位置坐标变换矩阵，两者理论上应该相同，但由于检测偏差存在，两者应在一定可接受偏差内才能去启动受限区域控制；同时，Ft₁是根据机械臂本身属性推算出来的末端工具受力信息，FT₂是检测得的受力信息，两者理论上应该相同，但由于检测偏差存在，两者应在一定可接受偏差内才能去启动受限区域控制。这是因为，受限区域内力约束和位置约束对力检测和位置检测的精度均有较高要求，以保证手术安全，因此在启动受限区域控制时，先对检测精度进行校验。

优选的，所述受限区域控制，具体为：

获取移动物体的位置信息，根据所述位置信息更新受限区域

判断末端工具是否位于受限区域内，如果位于，则对所述末端工具进行力约束和/或位置约束，如果不位于，则不对所述末端工具进行力约束和位置约束。

通过上述方法确保了力的检测数据和位置的追踪数据的数据来源准确性后，再决定是否进入受限区域控制模式。进入受限区域控制模式后，由于末端检测精度已经进行了校验，因此在进行力约束和位置约束时，检测的末端工具实时受力信息和末端工具的实时位置信息，均为置信度较高的数据，基于该数据进行力约束和位置约束，可以提高机器人力/位控制的精确性，提高机器人在受限区域中进行协作配合时的稳定性。机器人在受限区域中可以单独具有力约束或单独具有位置约束，也可以同时具有力约束和位置约束。

同时，本实施例考虑了移动物体移动时受限区域实时变化对力约束和位置约束的安全性的影响，从而先获取移动物体的位置信息，即可获取实时的受限区域。

实施例2

本发明的实施例2提供了机器人力位连锁控制装置，包括处理器以及存储器，所述存储器上存储有计算机程序，所述计算机程序被所述处理器执行时，实现实施例1提供的机器人力位连锁控制方法。

本发明实施例提供的机器人力位连锁控制装置，用于实现机器人力位连锁控制方法，因此，机器人力位连锁控制方法所具备的技术效果，机器人力位连锁控制装置同样具备，在此不再赘述。

实施例3

如图2所示，本发明的实施例3提供了机器人力位连锁控制系统，以下简称本系统，包括实施例2提供的机器人力位连锁控制装置，还包括机器人1、末端力矩传感器2、末端工具3、第一光学阵列41、第二光学阵列42以及光学设备44：

所述末端工具3安装于所述机器人1的末端；所述末端力矩传感器2安装于所述机器人1与所述末端工具3之间，并用于检测末端工具3的第二受力信息；所述第一光学阵列41安装于所述末端工具3上，所述第二光学阵列42安装于所述机器人1上，所述光学设备44用于追踪所述第一光学阵列41与所述第二光学阵列42的位姿关系矩阵；所述机器人力位连锁控制装置用于根据所述第一光学阵列41与所述第二光学阵列42的位姿关系矩阵，计算末端工具坐标系与机器人基座坐标系的第二变换矩阵。

机器人1的末端工具3在受限区域20内具有位置约束和力约束。受限区域20与移动物体10固定连接，移动物体10移动时受限区域20随动，移动物体10包括但不限于髋关节膝关节等人体部位，受限区域20包括但不限于圆锥体、圆锥体加圆柱体、四面体等，图2中示出的首先区域即为圆锥体形状。

本实施例中，对机器人协作过程中的力/位置进行双重联锁设计，从而确保力检测和位置检测的准确性和安全性。具体的，首先通过末端力矩传感器2直接检测末端工具3的受力，得到第二受力信息；末端工具3通过法兰盘31安装于机器人末端，末端力矩传感器2安装于法兰盘31与机器人末端之间。然后通过光学设备44实时追踪第一光学阵列41与第二光学阵列42的位置关系，由于第一光学阵列41安装于末端工具3上，第二光学阵列42安装于机器人1上，因此通过第一光学阵列41与第二光学阵列42的位置关系，可以间接获取末端工具3坐标系与机器人基座坐标系的第二变换矩阵。检测完成后，通过控制器获取第一受力信息和第一变换矩阵，然后进行比较，只有受力信息偏差和变换矩阵偏差均在一定偏差范围内时，才会置信力检测和位置检测结果的准确性，启动受限区域控制模式，从而保证机器人1进入受限区域控制模式后，进行力约束控制和位置约束控制时的控制精确度，提高机器人1在受限区域20协作配合时的稳定性。

优选的，根据所述第一光学阵列41与所述第二光学阵列42的位姿关系矩阵，计算末端工具坐标系与机器人基座坐标系的第二变换矩阵，具体为：

^TT_R＝^TT_maker1×^maker1T_maker2×^maker2T_R

其中，^TT_R为所述第二变换矩阵，^maker1T_maker2为所述第一光学阵列41与所述第二光学阵列42的位姿关系矩阵，^TT_maker1为末端工具3与第一光学阵列41的位姿关系矩阵，^maker2T_R为第二光学阵列42与机器人基座的位姿关系矩阵。

在光学设备44坐标下，光学设备44可以检测追踪第一光学阵列41和第二光学阵列42的位置，从而计算出第一光学阵列41和第二光学阵列42的位姿关系矩阵。而第一光学阵列41安装于末端工具3上，第二光学阵列42安装于机器人1上，因此获取了第一光学阵列41与第二光学阵列42的位姿关系矩阵后，可以根据该位姿关系矩阵间接计算出末端工具坐标系与机器人基座坐标系的第二变换矩阵。位姿关系矩阵^TT_maker1和位姿关系矩阵^maker2T_R分别通过对第一光学阵列41和第二光学阵列42进行标定获取即可。

优选的，如图2所示，本系统还包括第三光学阵列43，所述第三光学阵列43安装于移动物体10上，所述光学设备44用于追踪所述第三光学阵列43，并检测移动物体10的位置信息。

本实施例增加了第三光学阵列43，用于检测移动物体10的位置信息。第三光学阵列43与移动物体10固定连接，因此当移动物体10移动引起受限区域20移动时，第三光学阵列43也发生移动，通过光学设备44追踪第三光学阵列43的位置信息，即可获取实时受限区域20。本实施例中信任光学设备44的追踪数据具备可靠性的前提是条件(2)，故只有当此条件成立时系统才会置信移动物体10的移动，从而置信受限区域20的移动。

优选的，所述机器人1的各关节11分别安装有关节力矩传感器，所述关节力矩传感器与所述控制器电连接，并用于检测机器人1各关节11的关节扭矩值。

用于计算第一受力信息的关节扭矩值通过在机器人1关节11上安装的关节力矩传感器检测获取。

优选的，所述机器人1的各关节11处分别安装有角度传感器，所述角度传感器与所述控制器电连接，并用于检测机器人1各关节角度值。

用于计算第一变换矩阵的关节角度值，由安装在机器人关节11处的角度传感器检测获取。

实施例4

本发明的实施例4提供了计算机存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时，实现实施例1提供的机器人力位连锁控制方法。

本发明实施例提供的计算机存储介质，用于实现机器人力位连锁控制方法，因此，机器人力位连锁控制方法所具备的技术效果，计算机存储介质同样具备，在此不再赘述。

以上所述本发明的具体实施方式，并不构成对本发明保护范围的限定。任何根据本发明的技术构思所做出的各种其他相应的改变与变形，均应包含在本发明权利要求的保护范围内。

Claims (12)

1.一种机器人力位连锁控制方法，其特征在于，包括以下步骤：

获取基于机器人内部传感器检测得到的末端工具的第一受力信息；获取基于机器人内部传感器检测得到的末端工具坐标系与机器人基座坐标系的第一变换矩阵；

获取基于机器人外部传感器检测得到的末端工具的第二受力信息，获取基于机器人外部传感器检测得到的末端工具坐标系与机器人基座坐标系的第二变换矩阵；

判断所述第一受力信息与所述第二受力信息的偏差是否小于受力偏差阈值，判断所述第一变换矩阵与所述第二变换矩阵的偏差是否小于变换偏差阈值，如果均小于，则启动受限区域控制，否则不启动受限区域控制。

2.根据权利要求1所述的机器人力位连锁控制方法，其特征在于，获取末端工具的第一受力信息，具体为：

获取机器人各关节的关节扭矩值，根据所述关节扭矩值计算所述第一受力信息。

3.根据权利要求2所述的机器人力位连锁控制方法，其特征在于，通过所述关节扭矩值计算所述第一受力信息，具体为：

FT₁＝(J^T)⁺*τ

其中，FT₁为第一受力信息，(J^T)⁺为机器人的机械臂雅可比矩阵转置的伪逆，τ为机器人各关节的关节扭矩矩阵，τ＝[τ_i]，τ_i为机器人第i个关节的关节扭矩值，i＝1,2,L,n，n为机器人的关节数量。

4.根据权利要求1所述的机器人力位连锁控制方法，其特征在于，获取末端工具坐标系与机器人基座坐标系的第一变换矩阵，具体为：

获取机器人各关节的角度值，采用机器人正运动学方法计算第一变换矩阵。

5.根据权利要求4所述的机器人力位连锁控制方法，其特征在于，通过机器人正运动学方法计算第一变换矩阵，具体为：

T₁＝FKin(q₁,L,q_n)

其中，T₁为第一变换矩阵，FKin()为机器人正运动学函数，q_i为机器人第i个关节的角度值，i＝1,2,L,n，n为机器人的关节数量。

6.根据权利要求1所述的机器人力位连锁控制方法，其特征在于，所述受限区域控制，具体为：

获取移动物体的位置信息，根据所述位置信息更新受限区域；

判断末端工具是否位于受限区域内，如果位于，则对所述末端工具进行力约束和/或位置约束，如果不位于，则不对所述末端工具进行力约束和位置约束。

7.一种机器人力位连锁控制装置，其特征在于，包括处理器以及存储器，所述存储器上存储有计算机程序，所述计算机程序被所述处理器执行时，实现如权利要求1-6任一所述的机器人力位连锁控制方法。

8.一种机器人力位连锁控制系统，其特征在于，包括如权利要求7所述的机器人力位连锁控制装置，还包括机器人、末端力矩传感器、末端工具、第一光学阵列、第二光学阵列以及光学设备：

所述末端工具安装于所述机器人的末端；所述末端力矩传感器安装于所述机器人与所述末端工具之间，并用于检测末端工具的第二受力信息；所述第一光学阵列安装于所述末端工具上，所述第二光学阵列安装于所述机器人上，所述光学设备用于追踪所述第一光学阵列与所述第二光学阵列的位姿关系矩阵；所述机器人力位连锁控制装置用于根据所述第一光学阵列与所述第二光学阵列的位姿关系矩阵，计算末端工具坐标系与机器人基座坐标系的第二变换矩阵。

9.根据权利要求8所述的机器人力位连锁控制系统，其特征在于，根据所述第一光学阵列与所述第二光学阵列的位姿关系矩阵，计算末端工具坐标系与机器人基座坐标系的第二变换矩阵，具体为：

^TT_R＝^TT_maker1×^maker1T_maker2×^maker2T_R

其中，^TT_R为所述第二变换矩阵，^maker1T_maker2为所述第一光学阵列与所述第二光学阵列的位姿关系矩阵，^TT_maker1为末端工具与第一光学阵列的位姿关系矩阵，^maker2T_R为第二光学阵列与机器人基座的位姿关系矩阵。

10.根据权利要求8所述的机器人力位连锁控制系统，其特征在于，还包括第三光学阵列，所述第三光学阵列安装于移动物体上，所述光学设备用于追踪所述第三光学阵列，并检测移动物体的位置信息。

11.根据权利要求8所述的机器人力位连锁控制系统，其特征在于，所述机器人的各关节分别安装有关节力矩传感器，所述关节力矩传感器用于检测机器人各关节的关节扭矩值；所述机器人的各关节处分别安装有角度传感器，所述角度传感器用于检测机器人各关节的角度值。

12.一种计算机存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时，实现如权利要求1-6任一所述的机器人力位连锁控制方法。

Images