基于轴角表示的阻抗控制方法、装置、设备及存储介质

技术领域

本发明涉及机器人自动化领域，尤其涉及基于轴角表示的阻抗控制方法、装置、设备及存储介质。

背景技术

随着自动化程度的提高，机器人的应用场景不再只是搬运等在自由空间上运动的任务，机器人也被要求完成一些接触性的操作，比如抛光等。此外，随着人机共融概念的提出，机器人进一步被要求能和人协作地完成一些任务，这就要求机器人具有安全特性。阻抗控制是一个能实现上述两个要求的一种控制算法。

阻抗控制是使机器人末端坐标系误差和外界力之间形成一种动态关系，这种动态关系是指定的，且通常具有二阶微分方程的形式。机器人末端坐标系误差包括位置误差和角度误差。在一般的阻抗控制中，所指定的动态关系中的一阶项和二阶项中的角度误差部分通常使用末端的角速度误差和角加速度误差，而动态关系中的零阶项的角度误差部分则通常使用实际末端坐标系和期望末端坐标系之间的欧拉角或轴角表示，这就导致了角度误差部分的零阶项和一二阶项之间并不构成导数关系，这实际上是在动态关系中引入了非线性因素，会导致阻抗控制在角度控制层面上的困难。

发明内容

针对现有技术中存在的问题，本发明提供了基于轴角表示的阻抗控制方法、装置、设备及存储介质，其目的在于消除阻抗控制中动态关系的非线性因素。

为了解决上述技术问题，本发明通过以下技术方案予以实现：

一种基于轴角表示的阻抗控制方法，包括：

获取机器人关节角度和关节角速度，根据所述关节角度和所述关节角速度计算机器人雅克比矩阵和机器人实际末端坐标系旋转矩阵；

根据所述机器人实际末端坐标系旋转矩阵和预设期望末端坐标系旋转矩阵计算旋转角度和三维方向向量；

根据所述旋转角度和三维方向向量计算旋转角度和三维方向向量关于关节角度的雅可比矩阵以及旋转角度和三维方向向量的乘积关于时间的二阶导数中与关节角速度不相关的矩阵；

根据所述机器人雅克比矩阵、所述旋转角度和三维方向向量关于关节角度的雅可比矩阵、所述旋转角度和三维方向向量的乘积关于时间的二阶导数中与关节角速度不相关的矩阵、所述旋转角度和所述三维方向向量构建机器人关节指令力矩的数学关系式；

根据预设的约束函数，对所述机器人关节指令力矩的数学关系进行求解得到机器人关节指令力矩。

进一步地，所述根据所述机器人实际末端坐标系旋转矩阵和预设期望末端坐标系旋转矩阵计算旋转角度和三维方向向量，具体如下：

式中：

进一步地，所述根据所述旋转角度和三维方向向量计算旋转角度和三维方向向量关于关节角度的雅可比矩阵以及旋转角度和三维方向向量的乘积关于时间的二阶导数中与关节角速度不相关的矩阵，具体如下：

式中：J

进一步地，所述机器人关节指令力矩的数学关系式如下：

式中，

进一步地，所述约束函数具体如下：

一种基于轴角表示的阻抗控制装置，包括：

第一计算模块，用于获取机器人关节角度和关节角速度，根据所述关节角度和所述关节角速度计算机器人雅克比矩阵和机器人实际末端坐标系旋转矩阵；

第二计算模块，用于根据所述机器人实际末端坐标系旋转矩阵和预设期望末端坐标系旋转矩阵计算旋转角度和三维方向向量；

第三计算模块，用于根据所述旋转角度和三维方向向量计算旋转角度和三维方向向量关于关节角度的雅可比矩阵以及旋转角度和三维方向向量的乘积关于时间的二阶导数中与关节角速度不相关的矩阵；

第四计算模块，用于根据所述机器人雅克比矩阵、所述旋转角度和三维方向向量关于关节角度的雅可比矩阵、所述旋转角度和三维方向向量的乘积关于时间的二阶导数中与关节角速度不相关的矩阵、所述旋转角度和所述三维方向向量构建机器人关节指令力矩的数学关系式；

第五计算模块，用于根据预设的约束函数，对所述机器人关节指令力矩的数学关系进行求解得到机器人关节指令力矩。

一种设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现所述的一种基于轴角表示的阻抗控制方法的步骤。

一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现所述的一种基于轴角表示的阻抗控制方法的步骤。

与现有技术相比，本发明至少具有以下有益效果：

本发明提供的一种基于轴角表示的阻抗控制方法，获取机器人关节角度和关节角速度，根据关节角度和关节角速度计算机器人雅克比矩阵和机器人实际末端坐标系旋转矩阵；根据机器人实际末端坐标系旋转矩阵和预设期望末端坐标系旋转矩阵计算旋转角度和三维方向向量；根据旋转角度和三维方向向量计算旋转角度和三维方向向量关于关节角度的雅可比矩阵以及旋转角度和三维方向向量的乘积关于时间的二阶导数中与关节角速度不相关的矩阵；根据机器人雅克比矩阵、旋转角度和三维方向向量关于关节角度的雅可比矩阵、旋转角度和三维方向向量的乘积关于时间的二阶导数中与关节角速度不相关的矩阵、旋转角度和三维方向向量构建机器人关节指令力矩的数学关系式；根据预设的约束函数，对机器人关节指令力矩的数学关系进行求解得到机器人关节指令力矩，求解得到机器人关节指令力矩实现了指定的二阶动态关系，消除了阻抗控制中动态关系的非线性因素，便于后续阻抗控制的应用。

为使本发明的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。

附图说明

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

图1是本发明一种基于轴角表示的阻抗控制方法流程图；

图2是实施例中基于轴角表示的阻抗控制装置示意图；

图3是实施例中基于轴角表示的阻抗控制方法的计算流程图。

图中：1-机械臂；2-力觉传感器；3-上位机。

具体实施方式

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

作为本发明的某一具体实施方式，结合图1所示，一种基于轴角表示的阻抗控制方法，具体如下：

a.获取机器人关节角度和关节角速度，根据所述关节角度和所述关节角速度计算机器人雅克比矩阵和机器人实际末端坐标系旋转矩阵。

b.根据所述机器人实际末端坐标系旋转矩阵和预设期望末端坐标系旋转矩阵计算旋转角度和三维方向向量，具体如下：

式中：

c.根据所述旋转角度和三维方向向量计算旋转角度和三维方向向量关于关节角度的雅可比矩阵以及旋转角度和三维方向向量的乘积关于时间的二阶导数中与关节角速度不相关的矩阵，具体如下：

式中：J

d.根据所述机器人雅克比矩阵、所述旋转角度和三维方向向量关于关节角度的雅可比矩阵、所述旋转角度和三维方向向量的乘积关于时间的二阶导数中与关节角速度不相关的矩阵、所述旋转角度和所述三维方向向量构建机器人关节指令力矩的数学关系式，具体如下：

式中，

e.根据预设的约束函数，对所述机器人关节指令力矩的数学关系进行求解得到机器人关节指令力矩，约束函数具体如下：

下面提供更加详细的解释说明。

如图3所示，一种基于轴角表示的阻抗控制方法，包括如下步骤：

步骤一、根据机器人关节角度和关节角速度计算机器人雅克比矩阵、机器人雅克比矩阵导数、机器人实际末端坐标系旋转矩阵和机器人实际末端坐标系旋转矩阵导数。

步骤二、根据机器人实际末端坐标系旋转矩阵和给定的期望末端坐标系旋转矩阵计算θ和l的值。具体地说，计算方式如下：

式中：

步骤三、基于上述的计算结果，计算α和β，计算α和β的算法不唯一，但所计算的结果应满足：α和β是任取的相互垂直且与l垂直的单位向量，且有α×l＝β。

步骤四、基于上述的计算结果，计算

/>

步骤五、基于上述的计算结果，计算

步骤六、基于上述的计算结果，计算J

式中：J

步骤七、基于上述的计算结果，结合力觉传感器的测量值，构建机器人关节指令力矩τ应满足的数学关系，具体的，数学关系式如下：

式中，

步骤八、基于上述的计算结果，根据所得到的机器人关节指令力矩τ应满足的数学关系求解出关节指令力矩τ的值，此处的求解方法不唯一，但所求解的力矩指令τ使如下的数学关系成立：

如图2所示，一种基于轴角表示的阻抗控制系统，包括机械臂1、力觉传感器2和上位机3，力觉传感器2安装在机械臂1的末端，上位机3与机械臂1和力觉传感器2通讯连接。

力觉传感器2用于测量环境施加在机器人上的力和扭矩。

上位机3用于控制机械臂1运动；还用于根据机器人关节角度和关节角速度计算机器人雅克比矩阵、机器人雅克比矩阵导数、实际末端坐标系旋转矩阵、实际末端坐标系导数；还用于根据实际末端坐标系旋转矩阵和期望的末端坐标系旋转矩阵计算θ和l的值；还用于计算α和β的值；还用于计算

本实施例中，机械臂1为七自由度机械臂；上位机3为计算机系统；力觉传感器2为六维力/力矩传感器。上位机3通过通讯协议与机械臂1进行通讯，六维力/力矩传感器通过通讯协议将数据反馈回上位机3，运行过程中，机械臂1在上位机3的指令下进行运动。

本发明在一个实施例中，提供了一种计算机设备，该计算机设备包括处理器以及存储器，所述存储器用于存储计算机程序，所述计算机程序包括程序指令，所述处理器用于执行所述计算机存储介质存储的程序指令。处理器可以是中央处理单元(CentralProcessing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital SignalProcessor、DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable GateArray，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等，其是终端的计算核心以及控制核心，其适于实现一条或一条以上指令，具体适于加载并执行一条或一条以上指令从而实现相应方法流程或相应功能；本发明实施例所述的处理器可以用于实现一种基于轴角表示的阻抗控制方法的操作。

本发明在一个实施例中，一种基于轴角表示的阻抗控制方法如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。计算机可读存储介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。

所述计算机存储介质可以是计算机能够存取的任何可用介质或数据存储设备，包括但不限于磁性存储器(例如软盘、硬盘、磁带、磁光盘(MO)等)、光学存储器(例如CD、DVD、BD、HVD等)、以及半导体存储器(例如ROM、EPROM、EEPROM、非易失性存储器(NANDFLASH)、固态硬盘(SSD))等。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本申请是参照本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

最后应说明的是：以上所述实施例，仅为本发明的具体实施方式，用以说明本发明的技术方案，而非对其限制，本发明的保护范围并不局限于此，尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，其依然可以对前述实施例所记载的技术方案进行修改或可轻易想到变化，或者对其中部分技术特征进行等同替换；而这些修改、变化或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的精神和范围，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。