CN116141330A - 机器人的运动控制方法、装置、机器人设备及存储介质 - Google Patents

Abstract

本申请涉及机器人的运动控制方法、装置、机器人设备及存储介质，该方法包括：建立从手柔性臂末端的欧拉变换信息、从手柔性臂末端的齐次变换矩阵与目标位姿信息之间的等式关系，确定待求解函数组；基于获取的各关节的当前关节参数信息，分别确定每个欧拉角函数的函数值和每个坐标位置函数的函数值，得到待校验的函数值集合；根据待校验的函数值集合和目标位姿信息，对各关节的当前关节参数信息进行迭代更新，得到迭代更新后的各关节的目标关节参数信息。本申请的机器人的运动控制方法，在应用于对机器人从手逆运动学求解时，可以提高算法的收敛速度和精度，进而有助于提升机器人的操作灵活度，提升微创手术机器人的精准度和安全性。

Description

机器人的运动控制方法、装置、机器人设备及存储介质

技术领域

本申请涉及机器人技术领域，特别涉及一种机器人的运动控制方法、装置、机器人设备及存储介质。

背景技术

随着机器人技术的发展，微创手术机器人技术逐渐成熟，并被广泛应用。微创手术机器人可以减轻医生在手术过程中的体力劳动，同时达到精准手术的目的，使患者微创伤、失血少、术后感染少、术后恢复快。

目前，微创手术机器人系统通常使用主从式控制模式：操作者在对主手进行操作时，手部运动会带动主手随之运动，通过主从控制算法将主手的运动映射到从手，进而带动末端的手术器械实现相应运动。

在主从控制算法中，根据当前时刻t的从手位姿矩阵、目标时刻t+1从手位置增量和姿态转换矩阵，计算出目标时刻t+1从手末端的位姿矩阵；然后，再根据目标时刻t+1从手末端的位姿矩阵，解算出从手各关节的关节变量，电机根据这些关节变量驱动相应关节运动。从目标时刻t+1从手末端的位姿矩阵，解算出从手各关节的关节变量，这一过程称为逆运动学建模。

由于机器人逆运动学问题的复杂性，传统解算方法存在收敛速度慢、收敛精度不高等不足，这导致主手和从手两端运动的趋势无法保持一致，不能保证主手和从手之间的一一对应，使得医生操作不够灵活，或者处于容易疲劳的姿势。为此，有必要对传统解算方法进行改进，提高算法的收敛速度和精度，提升机器人的操作灵活度，进而提升微创手术机器人的精准度和安全性。

发明内容

本申请实施例提供了一种机器人的运动控制方法、装置、机器人设备及存储介质，可以提高算法的收敛速度和精度，提升机器人的操作灵活度，进而提升微创手术机器人的精准度和安全性。

第一方面，本申请实施例提供了一种机器人的运动控制方法，包括：

获取机器人的从手柔性臂末端的目标位姿信息；

基于预设的欧拉角旋转顺序，确定从手柔性臂末端的欧拉变换信息；欧拉变换信息表征从手柔性臂末端的旋转姿态；

建立从手柔性臂末端的欧拉变换信息、从手柔性臂末端的齐次变换矩阵与目标位姿信息之间的等式关系，确定待求解函数组；待求解函数组包括基于末端坐标系的三个欧拉角函数和三个坐标位置函数；三个欧拉角函数中每个欧拉角函数和三个坐标位置函数中每个坐标位置函数分别与从手柔性臂的各关节的关节参数相关；

基于获取的各关节的当前关节参数信息，分别确定每个欧拉角函数的函数值和每个坐标位置函数的函数值，得到待校验的函数值集合；

根据待校验的函数值集合和目标位姿信息，对各关节的当前关节参数信息进行迭代更新，得到迭代更新后的各关节的目标关节参数信息；各关节的目标关节参数信息用于控制从手柔性臂进行运动。

在一些可能的实施例中，目标位姿信息包括目标姿态信息和目标位置信息；目标姿态信息用于描述末端坐标系中各个坐标轴的方向；目标位置信息包括末端坐标系的原点基于机器人基坐标系的三维位置坐标值；末端坐标系的原点与从手柔性臂末端的中心点重合；

根据待校验的函数值集合和目标位姿信息，对各关节的当前关节参数信息进行迭代更新，得到迭代更新后的各关节的目标关节参数信息，包括：

基于目标姿态信息确定三个欧拉角目标值；

基于三个欧拉角函数、三个坐标位置函数和各关节的当前关节参数信息，确定当前迭代轮次对应的当前雅克比矩阵；

基于当前雅克比矩阵、待校验的函数值集合、三维位置坐标值和三个欧拉角目标值，确定各关节的关节参数增量信息；

利用各关节的关节参数增量信息，对各关节的当前关节参数信息进行更新，得到更新后的各关节的当前关节参数信息；

若所述待校验的函数值集合和所述目标位姿信息之间不满足预设迭代结束条件，将所述更新后的各关节的当前关节参数信息作为所述各关节的当前关节参数信息；

对所述各关节的当前关节参数信息进行下一轮次迭代更新，直至满足所述预设迭代结束条件，得到迭代更新后的各关节的目标关节参数信息。

在一些可能的实施例中，若所述待校验的函数值集合和所述目标位姿信息之间不满足预设迭代结束条件，将所述更新后的各关节的当前关节参数信息作为所述各关节的当前关节参数信息，包括：

基于三个欧拉角目标值，确定每个欧拉角函数的函数值的误差值；

基于三维位置坐标值，确定每个坐标位置函数的函数值的误差值；

根据每个欧拉角函数的函数值的误差值和每个坐标位置函数的函数值的误差值，确定当前总误差值；

若当前总误差值大于等于预设值，利将所述更新后的各关节的当前关节参数信息作为所述各关节的当前关节参数信息。

在一些可能的实施例中，方法还包括：

若待校验的函数值集合和目标位姿信息之间满足预设迭代结束条件，将所述各关节的当前关节参数信息确定为所述迭代更新后的各关节的目标关节参数信息。

在一些可能的实施例中，获取机器人的从手柔性臂末端的目标位姿信息，包括：

基于机器人主手的运动增量信息进行主从映射，得到从手柔性臂末端的目标位置增量信息和从手柔性臂末端的目标姿态信息；

根据从手柔性臂末端的当前位置信息、从手柔性臂末端的目标位置增量信息和从手柔性臂末端的目标姿态信息，确定从手柔性臂末端的目标位姿信息。

在一些可能的实施例中，机器人主手的运动增量信息包括目标主手位置增量信息和目标主手姿态增量信息；

基于机器人主手的运动增量信息进行主从映射，得到从手柔性臂末端的目标位置增量信息和从手柔性臂末端的目标姿态信息，包括：

基于内窥镜视野下主从映射方法，对目标主手位置增量信息进行映射，得到从手柔性臂末端的目标位置增量信息；

根据目标主手姿态增量信息和获取的当前主手姿态信息，确定目标主手姿态信息；

基于内窥镜视野下主从映射方法，对目标主手姿态信息进行映射，得到从手柔性臂末端的的目标姿态信息。

在一些可能的实施例中，基于预设的欧拉角旋转顺序，确定从手柔性臂末端的欧拉变换信息，包括：

确定绕末端坐标系中Z轴旋转第一欧拉角的第一旋转矩阵；

确定绕末端坐标系中Y轴旋转第二欧拉角的第二旋转矩阵；

确定绕末端坐标系中X轴旋转第三欧拉角的第三旋转矩阵；

将第一旋转矩阵、第二旋转矩阵和第三旋转矩阵相乘，得到从手柔性臂末端的欧拉变换信息。

第二方面，本申请实施例提供了一种机器人的运动控制装置，包括：

获取模块，被配置为执行获取机器人的从手柔性臂末端的目标位姿信息；

第一确定模块，被配置为执行基于预设的欧拉角旋转顺序，确定从手柔性臂末端的欧拉变换信息；欧拉变换信息表征从手柔性臂末端的旋转姿态；

第二确定模块，用于建立从手柔性臂末端的欧拉变换信息、从手柔性臂末端的齐次变换矩阵与目标位姿信息之间的等式关系，确定待求解函数组；待求解函数组包括基于末端坐标系的三个欧拉角函数和三个坐标位置函数；三个欧拉角函数中每个欧拉角函数和三个坐标位置函数中每个坐标位置函数分别与从手柔性臂的各关节的关节参数相关；

第三确定模块，被配置为执行基于获取的各关节的当前关节参数信息，分别确定每个欧拉角函数的函数值和每个坐标位置函数的函数值，得到待校验的函数值集合；

第四确定模块，被配置为执行根据待校验的函数值集合和目标位姿信息，对各关节的当前关节参数信息进行迭代更新，得到迭代更新后的各关节的目标关节参数信息；各关节的目标关节参数信息用于控制从手柔性臂进行运动。

第三方面，本申请实施例提供了一种机器人设备，机器人设备包括处理器和存储器，存储器中存储有至少一条指令或至少一段程序，至少一条指令或至少一段程序由处理器加载并执行本申请实施例第一方面提供的机器人的运动控制方法。

第四方面，本申请实施例提供了一种计算机存储介质，存储介质中存储有至少一条指令或至少一段程序，至少一条指令或至少一段程序由处理器加载并执行以实现本申请实施例第一方面提供的机器人的运动控制方法。

本申请实施例提供的机器人的运动控制方法、装置、机器人设备及存储介质至少具有如下有益效果：

本申请实施例提供的机器人的运动控制方法，在对从手柔性臂进行逆运动学解算的过程中，使用欧拉角表示从手柔性臂末端姿态，并基于从手柔性臂末端的欧拉变换信息、从手柔性臂末端的齐次变换矩阵与目标位姿信息之间的等式关系，构建6个待求解函数，由于6个待求解函数最大限定地囊括已知信息，从而在迭代求解时，可以在保证解算精度的同时，缩短解算时间；在算法的收敛速度和精度的提升效果下，微创手术机器人的操作灵活度也有所提升，这非常有利于微创手术机器人的精准度和安全性。

附图说明

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

图1是本申请实施例提供的一种机器人的运动控制方法的流程示意图；

图2是本申请实施例提供的一种主从控制策略的示意图；

图3是本申请实施例提供的一种确定目标位姿信息的流程示意图；

图4是本申请实施例提供的一种主从映射的流程示意图；

图5是本申请实施例提供的一种运动学建模的示意图；

图6是本申请实施例提供的一种确定欧拉变换信息的流程示意图；

图7是本申请实施例提供的一种从手逆运动求解的迭代过程示意图；

图8是本申请实施例提供的一种迭代计算的流程示意图；

图9是本申请实施例提供的一种机器人的运动控制装置的结构示意图；

图10是本申请实施例提供的一种用于机器人的运动控制的电子设备的框图。

具体实施方式

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

需要说明的是，本申请的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本申请的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或服务器不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

本申请实施例提供的一种机器人的运动控制方法，主要是对微创手术机器人的从手柔性臂的运动进行控制，在控制从手柔性臂的运动过程中，涉及到机器人从手逆运动学求解问题，该问题本质上是机器人运动学方程的求解问题，指给定机器人末端执行器的期望位姿，求到达该位姿所需的各关节变量值。

由于机器人的运动学方程是一组关节变量强耦合、非线性的复杂超越方程组，导致逆运动学问题的求解比较困难。鉴于传统解算方法存在收敛速度慢、收敛精度不高等不足，本申请实施例提供了一种机器人的运动控制方法，基于机器人从手柔性臂末端的目标位姿，可以快速求解出准确度较高的从手柔性臂各关节的关节参数，以根据从手柔性臂各关节的关节参数控制从手柔性臂进行运动，使得从手柔性臂末端到达目标位姿。由于解算速度的提升和准确度的提高，使得机器人的操作灵活度有所提升，进而有助于提升微创手术机器人的精准度和安全性。

以下介绍本申请一种机器人的运动控制方法的具体实施例，图1是本申请实施例提供的一种机器人的运动控制方法的流程示意图，本说明书提供了如实施例或流程图的方法操作步骤，但基于常规或者无创造性的劳动可以包括更多或者更少的操作步骤。实施例中列举的步骤顺序仅仅为众多步骤执行顺序中的一种方式，不代表唯一的执行顺序。在实际中的系统或服务器产品执行时，可以按照实施例或者附图所示的方法顺序执行或者并行执行(例如并行处理器或者多线程处理的环境)。具体的如图1所示，该方法可以包括：

在步骤S101中，获取机器人的从手柔性臂末端的目标位姿信息。

本申请实施例中，机器人可以指微创手术机器人。微创手术机器人一般包括主手和从手，操作者操控主手，从手在控制系统的控制下跟随复现操作者的手部动作，控制系统可以通过主从控制策略实现该过程。上述从手柔性臂末端的目标位姿信息，可以在进行主从控制的过程中获得。

如图2所示，图2是本申请实施例提供的一种主从控制策略的示意图。下面结合图2对上述步骤S101进行进一步说明。

在一些可能的实施例中，上述步骤S101可以包括如图3所示的以下步骤：

在步骤S301中，基于机器人主手的运动增量信息进行主从映射，得到从手柔性臂末端的目标位置增量信息和从手柔性臂末端的目标姿态信息。

该步骤中，结合图2所示，医生在操作机器人主手时，主手设备可以输出主手的运动增量信息，主手的运动增量信息包括目标主手位置增量信息和目标主手姿态增量信息；其中，目标主手位置增量信息指的是目标时刻(t+1时刻)相对于当前(t时刻)主手的位置增量；目标主手姿态增量信息指的是目标时刻(t+1时刻)相对于当前(t时刻)主手的姿态角增量；然后，利用内窥镜视野下主从映射方法，分别对位置和姿态进行映射，得到从手柔性臂末端的目标位置增量信息和从手柔性臂末端的目标姿态信息；其中，从手柔性臂末端的目标位置增量信息指的是目标时刻(t+1时刻)从手柔性臂末端的位置增量；从手柔性臂末端的目标姿态信息指的是目标时刻(t+1时刻)从手柔性臂末端的姿态矩阵。具体的，步骤S301可以包括如图4所示的以下步骤：

在步骤S401中，基于内窥镜视野下主从映射方法，对目标主手位置增量信息进行映射，得到从手柔性臂末端的目标位置增量信息。

该步骤是对位置进行主从映射。具体的，基于内窥镜视野下主从映射方法，直接将t+1时刻主手的位置增量映射成t+1时刻从手柔性臂末端的位置增量。

后续，t+1时刻从手柔性臂末端的位置增量加上当前(t时刻)从手柔性臂末端的实际位置，即可获得t+1时刻从手柔性臂末端的位置。这里，内窥镜视野下主从映射方法为现有技术，具体可以参考相关记载实现，本申请不再展开赘述。

在步骤S403中，根据目标主手姿态增量信息和获取的当前主手姿态信息，确定目标主手姿态信息。

步骤S403～S405是对姿态进行主从映射。该步骤中，首先根据t+1时刻主手的姿态角增量，以及当前主手姿态信息即t时刻主手姿态矩阵，确定出t+1时刻主手的姿态矩阵。

具体的，如图2所示，通过主手设备中的传感器采集获得t时刻主手的姿态角，然后将t+1时刻主手的姿态角增量与t时刻主手的姿态角相加，将结果通过主手正运动学转换成t+1时刻主手的姿态矩阵。

在步骤S405中，基于内窥镜视野下主从映射方法，对目标主手姿态信息进行映射，得到从手柔性臂末端的目标姿态信息。

该步骤中，如图2所示，基于内窥镜视野下主从映射方法，对t+1时刻主手的姿态矩阵进行映射，得到t+1时刻从手柔性臂末端的姿态矩阵。

在步骤S303中，根据从手柔性臂末端的当前位置信息、从手柔性臂末端的目标位置增量信息和从手柔性臂末端的目标姿态信息，确定从手柔性臂末端的目标位姿信息。

该步骤中，要确定的从手柔性臂末端的目标位姿信息包括从手柔性臂末端的目标位置信息和从手柔性臂末端的目标姿态信息，从手柔性臂末端的目标姿态信息即t+1时刻从手柔性臂末端的姿态矩阵通过上述步骤S401～S405确定；从手柔性臂末端的当前位置信息即t时刻从手柔性臂末端的实际位置，可以直接获得，从而从手柔性臂末端的目标位置信息即t+1时刻从手柔性臂末端的位置通过以下方式确定：将t时刻从手柔性臂末端的实际位置和t+1时刻从手柔性臂末端的位置增量相加，即可得到t+1时刻从手柔性臂末端的位置。

上述实施例中，在主从映射控制策略中采用增量控制的方式，并在内窥镜视野下实现主从映射，可以保证主手和从手两端运动的趋势一致，保证计算得到的从手柔性臂末端的目标位姿信息的准确性，为后续从手逆运动学的求解提供高可靠的数据支撑。

目前微创手术机器人所采用的柔性臂由多个关节串联构成，在对从手逆运动学进行求解时，需要进行运动学建模。如图5所示，以单个关节进行运动学建模，设单个关节的长度为L，其坐标系及相应的关节参数定义如图5所示：以关节近端端面圆心为原点，建立基坐标系O_i；以关节远端端面圆心为原点，建立末端坐标系o_i+1；实际应用中，末端坐标系的原点与从手柔性臂末端的中心点重合；设关节的弯曲角为θ、弯曲半径为R，其中，弯曲角θ为关节变量；则基坐标系O_i到末端坐标系O_i+1的变换可通过如下过程实现：

1)O_i沿Z_i轴移动R sinθ，然后再沿X_i移动R(1-cosθ)，得到坐标系O_i1；

2)O_i1绕Y_i1旋转θ角度，得到坐标系O_i+1(R＝L/θ)。

基于以上变换过程可以得到O_i+1和O_i的齐次变换矩阵T_i ⁱ⁺¹，如下表达式(1):

这里，齐次变换矩阵

表示坐标系O_i+1在坐标系O_i中的描述；矩阵中左上角3*3表示旋转矩阵，也称姿态矩阵，用于描述末端坐标系中各个坐标轴的方向；第四列前三行表示坐标系O_i+1原点坐标。

实际应用时，步骤S101中获得的从手柔性臂末端的目标位姿信息也以矩阵形式表示，如下表达式(2)：

这里，从手末端的位姿矩阵A中左上角3*3表示旋转矩阵或姿态矩阵，用R(q)表示，R(q)∈SO(3)；第四列前三行表示从手末端中心点的坐标，用P(q)表示，P(q)∈R³；R(q)和P(q)中的元素用r_ij表示；q＝[q₁,q₂,…q_n]^T，表示关节列向量，描述从手柔性臂各关节(1～n)的关节参数信息(即各关节的弯曲角θ)。

由于上述齐次变换矩阵

与从手末端的位姿矩阵A均是用于描述从手末端坐标系在从手基坐标系中的位姿，因此各元素呈一一对应的关系，从而可以建立如下等式(3)：

显然，根据上述等式(3)可以建立一个以关节列向量q为未知数，由12个方程组成的n元非线性方程组。通过求解该非线性方程组，即可得到从手各关节的目标关节参数信息。

本申请实施例中，基于牛顿-拉夫森法，对该非线性方程组进行求解，其求解公式见式如下：

qⁱ⁺¹＝Δqⁱ+qⁱ (5)

其中，f(q)∈R^m为构造函数组，构造函数组中函数的个数为m，m≥n；i表示迭代轮次；J(q)表示雅克比矩阵，J(q)是一个m行n列的矩阵，对f(q)中的各个构造函数分别求导，其矩阵表达式如下：

当n＝6时，

当n<6时，/>

x_d描述从手末端的姿态和位置，可以根据目标姿态矩阵A计算得到，具体在下文展开描述，此处暂不赘述。

由上可见，在构造函数组f(q)时，若f(q)中函数个数过多(m>n)时，将使得雅克比矩阵J(q)过于复杂，将影响从手各关节的目标关节参数信息的求解速度；若f(q)中函数个数过少(m＝n)时，会导致f(q)中包含的r_ij元素过少，如只含12个r_ij元素中的6～8个，这会使得后续在迭代计算误差x_d-f(qⁱ)与原始误差x_d-f(q⁰)相比越来越大，最终导致求取的结果q＝[q₁,q₂,…q_n]^T发散而不收敛。

基于此，本申请通过步骤S103～S105，以从手末端中心点的三个欧拉角和三个位置坐标为构造函数，此时f(q)∈R⁶，且6个构造函数可以覆盖到矩阵A中的10个r_ij元素，如此，能够最大限度地应用已知条件，可以提升求解的速度和精度。

在步骤S103中，基于预设的欧拉角旋转顺序，确定从手柔性臂末端的欧拉变换信息；欧拉变换信息表征从手柔性臂末端的旋转姿态。

本申请实施例中，用三个欧拉角(ψ、θ和φ)来表示从手柔性臂末端的姿态，欧拉变换信息即欧拉变换矩阵，也可以用于描述从手柔性臂末端的姿态。一般地，可以根据任意欧拉角旋转顺序，确定对应的欧拉变换矩阵。下面提供一种可能的实施例。

在一些可能的实施例中，预设的欧拉角旋转顺序为Z(ψ)→Y(θ)→X(φ)；从而，上述步骤S103可以包括如图6所示的以下步骤：

在步骤S601中，确定绕末端坐标系中Z轴旋转第一欧拉角的第一旋转矩阵。

这里，第一旋转矩阵可以表示为Rot(z，ψ)。

在步骤S603中，确定绕末端坐标系中Y轴旋转第二欧拉角的第二旋转矩阵。

这里，第二旋转矩阵可以表示为Rot(y，θ)。

在步骤S605中，确定绕末端坐标系中X轴旋转第三欧拉角的第三旋转矩阵。

这里，第三旋转矩阵可以表示为Rot(x，φ)。

在步骤S607中，将第一旋转矩阵、第二旋转矩阵和第三旋转矩阵相乘，得到从手柔性臂末端的欧拉变换信息。

这里，将第一旋转矩阵、第二旋转矩阵和第三旋转矩阵按照右乘规则来相乘，如下式(6)所示：

其中，R表示最终得到的从手柔性臂末端的欧拉变换矩阵；Cθ代表Cosθ，Sθ代表sinθ，其余依此类推。

在步骤S105中，建立从手柔性臂末端的欧拉变换信息、从手柔性臂末端的齐次变换矩阵与目标位姿信息之间的等式关系，确定待求解函数组。

本申请实施例中，由于欧拉变换矩阵同样用于描述从手柔性臂末端的姿态，从而，鉴于从手柔性臂末端的欧拉变换信息、从手柔性臂末端的齐次变换矩阵与目标位姿信息之间的等式关系，构造待求解函数组；待求解函数组包括基于末端坐标系的三个欧拉角函数和三个坐标位置函数。

具体的，将式(6)中元素与R(q)中元素进行一一对应，并基于式(3)，可以构造待求解函数组如下：

由式(2)可见，r_ij与关节参数θ相关，是关节列向量q的函数；因此，上述三个欧拉角函数：ψ、θ、φ是关于关节列向量q的函数，与从手柔性臂的各关节的关节参数相关。三个坐标位置函数：r₁₄、r₂₄、r₃₄也是关于关节列向量q的函数，与从手柔性臂的各关节的关节参数相关。

由式(7)可见，本申请构造的6个待求解函数f₁(q)～f₆(q)，覆盖到矩阵A中的10个元素r₁₁、r₁₂、r₁₃、r₁₄、r₂₁、r₂₂、r₂₃、r₂₄、r₃₁、r₃₄。

在步骤S107中，基于获取的各关节的当前关节参数信息，分别确定每个欧拉角函数的函数值和每个坐标位置函数的函数值，得到待校验的函数值集合。

在步骤S109中，根据待校验的函数值集合和目标位姿信息，对各关节的当前关节参数信息进行迭代更新，得到迭代更新后的各关节的目标关节参数信息；各关节的目标关节参数信息用于控制从手柔性臂进行运动。

上述步骤S107～S109中，将从手各关节的当前关节参数信息，即当前时刻从手各关节的实际关节弯曲角，其可以从传感器的反馈中直接获得，作为迭代的初始值，对待求解函数组进行求解，结合由正运动学计算出来的从手柔性臂末端的目标位姿信息，判断函数值是否满足精度要求，不满足精度要求时，对各关节的当前关节参数进行迭代更新，通过多次迭代更新，直至满足精度要求时，完成各关节的关节参数的迭代，得到迭代更新后的各关节的目标关节参数信息；后续，根据求解得到的从手各关节的目标关节参数信息，驱动从手柔性臂进行运动。

简单来说，在进行从手逆运动求解时，按照如图7所示的流程进行迭代求解。首先，在步骤S701中，给定一组各关节的初始参数

例如可以是当前时刻从手各关节的实际关节弯曲角；然后，在步骤S703中，基于各关节的初始参数q⁰，依次运算式(7)、(4)和(6)；具体的，先将各关节的初始参数q⁰带入式(7)求解f(qⁱ)，然后将f(qⁱ)带入式(4)求解Δqⁱ，然后将Δqⁱ带入式(6)求解qⁱ⁺¹，i从0开始递增；然后，在步骤S705中，计算||x_d-f(qⁱ)||，判断||x_d-f(qⁱ)||<ε是否成立；若||x_d-f(qⁱ)||≥ε，表示不满足精度要求，用q^{i +1}替代qⁱ并返回步骤S703进行迭代计算；若||x_d-f(qⁱ)||<ε，表示满足精度要求，则在步骤S707中，输出/>

结束迭代；这里，ε根据实际应用进行设置，示例性的，ε＝10^-6。

下面对步骤S109中的迭代计算进行详细说明。在一些可能的实施例中，上述步骤S109可以包括如图8所示的以下步骤：

在步骤S801中，基于目标姿态信息确定三个欧拉角目标值。

这里，目标姿态信息对应目标姿态矩阵A，上文中提及x_d描述从手末端的姿态和位置，可以根据目标姿态矩阵A计算得到，这里，三个欧拉角目标值表征从手末端的姿态；相应的，根据目标姿态矩阵A计算得到的x_d的表达式如下：

其中，前三行分别为三个欧拉角目标值；p_x、p_y和p_z表征从手末端的位置，分别为末端坐标系的原点基于机器人基坐标系的三维位置坐标值；

在步骤S803中，基于三个欧拉角函数、三个坐标位置函数和各关节的当前关节参数信息，确定当前迭代轮次对应的当前雅克比矩阵。

该步骤中，基于上文关于雅克比矩阵J(q)的描述可知，分别对三个欧拉角函数、三个坐标位置函数进行求导，然后根据当前迭代轮次i对应的qⁱ，计算出当前雅克比矩阵J(qⁱ)。

在步骤S805中，基于当前雅克比矩阵、待校验的函数值集合、三维位置坐标值和三个欧拉角目标值，确定各关节的关节参数增量信息。

该步骤中，待校验的函数值集合在每个迭代轮次中需要根据各关节的当前关节参数信息qⁱ由式(7)计算得到；参照式(7)，待校验的函数值集合包括当前迭代轮次i对应的f₁(qⁱ)～f₆(qⁱ)；三维位置坐标值和三个欧拉角目标值见上式(8)，其在每个迭代轮次中是固定不变的；从而，根据当前雅克比矩阵J(qⁱ)、当前迭代轮次i对应的f₁(qⁱ)～f₆(qⁱ)、x_d，利用上式(4)，即可计算出当前迭代轮次i中各关节的关节参数增量信息Δqⁱ。

在步骤S807中，利用各关节的关节参数增量信息，对各关节的当前关节参数信息进行更新，得到更新后的各关节的当前关节参数信息。

该步骤中，基于上式(5)利用各关节的关节参数增量信息Δqⁱ，对各关节的当前关节参数信息qⁱ进行更新，得到更新后的各关节的当前关节参数信息qⁱ⁺¹。

上述步骤S803～S807即对步骤S703的展开描述。

在步骤S809中，判断待校验的函数值集合和目标位姿信息之间是否满足预设迭代结束条件。若待校验的函数值集合和目标位姿信息之间满足不预设迭代结束条件，执行步骤S811～S813；否则，执行步骤S815。

该步骤中，预设迭代结束条件为，若||x_d-f(qⁱ)||<ε，则结束迭代。这里，f(qⁱ)包含当前迭代轮次i对应的f₁(qⁱ)～f₆(qⁱ)。

详细来说，需要将f₁(qⁱ)～f₆(qⁱ)中的6个值分别与x_d中六个值一一对应进行误差计算，具体的，基于三个欧拉角目标值，确定每个欧拉角函数的函数值的误差值；基于三维位置坐标值，确定每个坐标位置函数的函数值的误差值；根据每个欧拉角函数的函数值的误差值和每个坐标位置函数的函数值的误差值，确定当前总误差值；实际算法中，||x_d-f(qⁱ)||即可表示当前总误差值。

在步骤S811中，将更新后的各关节的当前关节参数信息作为各关节的当前关节参数信息。

在步骤S813中，对各关节的当前关节参数信息进行下一轮次迭代更新，直至满足预设迭代结束条件，得到迭代更新后的各关节的目标关节参数信息。

上述步骤S811～S813中，当前迭代轮次i计算得到的当前总误差值大于等于预设值时，表示不满足精度要求，则用更新后的各关节的当前关节参数信息qⁱ⁺¹替换各关节的当前关节参数信息qⁱ，然后重复步骤S803～S807，对各关节的当前关节参数信息qⁱ进行下一轮次迭代更新。其中，预设值即上文ε。

在步骤S815中，将各关节的当前关节参数信息确定为迭代更新后的各关节的目标关节参数信息。

该步骤中，待校验的函数值集合和目标位姿信息之间满足预设迭代结束条件，即当前迭代轮次i计算得到的当前总误差值小于预设值时，表示满足精度要求，则直接将各关节的当前关节参数信息确定为迭代更新后的各关节的目标关节参数信息。

上述实施例中，由上述式(7)和式(8)可见，本申请构造的待求解函数组f(q)，以及描述目标位姿信息的x_d，均分别囊括了式矩阵A中的10个元素r_ij，从而，在计算误差||x_d-f(qⁱ)||时，最大限度地应用已知条件，如此，可以精确快速得到收敛解，减少求解从手各关节的目标关节参数信息的迭代次数，大大缩短计算时间，实际应用时，最长计算时间控制在1ms以内，平均计算时间为0.7ms左右；在实际机器人的操作过程中，十分有利于控制系统快速准确的控制从手末端到达目标位姿。

综上，本申请实施例提供的机器人的运动控制方法，在对从手柔性臂进行逆运动学解算的过程中，使用欧拉角表示从手柔性臂末端姿态，并基于从手柔性臂末端的欧拉变换信息、从手柔性臂末端的齐次变换矩阵与目标位姿信息之间的等式关系，构建6个待求解函数，由于6个待求解函数最大限定地囊括已知信息，从而在迭代求解时，可以在保证解算精度的同时，缩短解算时间；在算法的收敛速度和精度的提升效果下，微创手术机器人的操作灵活度也有所提升，这非常有利于微创手术机器人的精准度和安全性。

本申请实施例还提供了一种机器人的运动控制装置，图9是本申请实施例提供的一种机器人的运动控制装置的结构示意图，如图9所示，该装置包括：

获取模块901，被配置为执行获取机器人的从手柔性臂末端的目标位姿信息；

第一确定模块902，被配置为执行基于预设的欧拉角旋转顺序，确定从手柔性臂末端的欧拉变换信息；欧拉变换信息表征从手柔性臂末端的旋转姿态；

第二确定模块903，用于建立从手柔性臂末端的欧拉变换信息、从手柔性臂末端的齐次变换矩阵与目标位姿信息之间的等式关系，确定待求解函数组；待求解函数组包括基于末端坐标系的三个欧拉角函数和三个坐标位置函数；三个欧拉角函数中每个欧拉角函数和三个坐标位置函数中每个坐标位置函数分别与从手柔性臂的各关节的关节参数相关；

第三确定模块904，被配置为执行基于获取的各关节的当前关节参数信息，分别确定每个欧拉角函数的函数值和每个坐标位置函数的函数值，得到待校验的函数值集合；

第四确定模块905，被配置为执行根据待校验的函数值集合和目标位姿信息，对各关节的当前关节参数信息进行迭代更新，得到迭代更新后的各关节的目标关节参数信息；各关节的目标关节参数信息用于控制从手柔性臂进行运动。

在一些可能的实施例中，目标位姿信息包括目标姿态信息和目标位置信息；目标姿态信息用于描述末端坐标系中各个坐标轴的方向；目标位置信息包括末端坐标系的原点基于机器人基坐标系的三维位置坐标值；末端坐标系的原点与从手柔性臂末端的中心点重合；

第四确定模块905，还被配置为执行基于目标姿态信息确定三个欧拉角目标值；基于三个欧拉角函数、三个坐标位置函数和各关节的当前关节参数信息，确定当前迭代轮次对应的当前雅克比矩阵；基于当前雅克比矩阵、待校验的函数值集合、三维位置坐标值和三个欧拉角目标值，确定各关节的关节参数增量信息；利用各关节的关节参数增量信息，对各关节的当前关节参数信息进行更新，得到更新后的各关节的当前关节参数信息；若所述待校验的函数值集合和所述目标位姿信息之间不满足预设迭代结束条件，将所述更新后的各关节的当前关节参数信息作为所述各关节的当前关节参数信息；对所述各关节的当前关节参数信息进行下一轮次迭代更新，直至满足所述预设迭代结束条件，得到迭代更新后的各关节的目标关节参数信息。

在一些可能的实施例中，第四确定模块905，还被配置为执行基于三个欧拉角目标值，确定每个欧拉角函数的函数值的误差值；基于三维位置坐标值，确定每个坐标位置函数的函数值的误差值；根据每个欧拉角函数的函数值的误差值和每个坐标位置函数的函数值的误差值，确定当前总误差值；若当前总误差值大于等于预设值，利将所述更新后的各关节的当前关节参数信息作为所述各关节的当前关节参数信息。

在一些可能的实施例中，第四确定模块905，还被配置为执行若待校验的函数值集合和目标位姿信息之间满足预设迭代结束条件，将所述各关节的当前关节参数信息确定为所述迭代更新后的各关节的目标关节参数信息。

在一些可能的实施例中，获取模块901，还被配置为执行基于机器人主手的运动增量信息进行主从映射，得到从手柔性臂末端的目标位置增量信息和从手柔性臂末端的目标姿态信息；根据从手柔性臂末端的当前位置信息、从手柔性臂末端的目标位置增量信息和从手柔性臂末端的目标姿态信息，确定从手柔性臂末端的目标位姿信息。

在一些可能的实施例中，机器人主手的运动增量信息包括目标主手位置增量信息和目标主手姿态增量信息；获取模块901，还被配置为执行基于内窥镜视野下主从映射方法，对目标主手位置增量信息进行映射，得到从手柔性臂末端的目标位置增量信息；根据目标主手姿态增量信息和获取的当前主手姿态信息，确定目标主手姿态信息；基于内窥镜视野下主从映射方法，对目标主手姿态信息进行映射，得到从手柔性臂末端的目标姿态信息。

在一些可能的实施例中，第一确定模块902，还被配置为执行确定绕末端坐标系中Z轴旋转第一欧拉角的第一旋转矩阵；确定绕末端坐标系中Y轴旋转第二欧拉角的第二旋转矩阵；确定绕末端坐标系中X轴旋转第三欧拉角的第三旋转矩阵；将第一旋转矩阵、第二旋转矩阵和第三旋转矩阵相乘，得到从手柔性臂末端的欧拉变换信息。

关于上述实施例中的装置，其中各个模块执行操作的具体方式已经在有关该方法的实施例中进行了详细描述，此处将不做详细阐述说明。

图10是本申请实施例提供的一种用于机器人的运动控制的电子设备的框图，该电子设备可以是终端，其内部结构图可以如图10所示。该电子设备包括通过系统总线连接的处理器、存储器、网络接口、显示屏和输入装置。其中，该电子设备的处理器用于提供计算和控制能力。该电子设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统和计算机程序。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该电子设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种机器人的运动控制方法。该电子设备的显示屏可以是液晶显示屏或者电子墨水显示屏，该电子设备的输入装置可以是显示屏上覆盖的触摸层，也可以是电子设备外壳上设置的按键、轨迹球或触控板，还可以是外接的键盘、触控板或鼠标等。

本领域技术人员可以理解，图10中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的电子设备的限定，具体的电子设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。

在示例性实施例中，还提供了一种机器人设备，包括：处理器；用于存储该处理器可执行指令的存储器；其中，该处理器被配置为执行该指令，以实现如本申请实施例中的机器人的运动控制方法。

在示例性实施例中，还提供了一种计算机可读存储介质，当该存储介质中的指令由电子设备的处理器执行时，使得电子设备能够执行本申请实施例中的机器人的运动控制方法。

在示例性实施例中，还提供了一种包含指令的计算机程序产品，计算机程序产品包括计算机程序，计算机程序存储在可读存储介质中，计算机设备的至少一个处理器从可读存储介质读取并执行计算机程序，使得计算机设备执行本申请实施例的机器人的运动控制方法。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，该计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPROM)或闪存。易失性存储器可包括随机存取存储器(RAM)或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM(SRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据率SDRAM(DDRSDRAM)、增强型SDRAM(ESDRAM)、同步链路(Synchlink)DRAM(SLDRAM)、存储器总线(Rambus)直接RAM(RDRAM)、直接存储器总线动态RAM(DRDRAM)、以及存储器总线动态RAM(RDRAM)等。

本领域技术人员在考虑说明书及实践这里公开的发明后，将容易想到本申请的其它实施方案。本申请旨在涵盖本申请的任何变型、用途或者适应性变化，这些变型、用途或者适应性变化遵循本申请的一般性原理并包括本申请未公开的本技术领域中的公知常识或惯用技术手段。说明书和实施例仅被视为示例性的，本申请的真正范围和精神由下面的权利要求指出。

应当理解的是，本申请并不局限于上面已经描述并在附图中示出的精确结构，并且可以在不脱离其范围进行各种修改和改变。本申请的范围仅由所附的权利要求来限制。

Claims (10)

1.一种机器人的运动控制方法，其特征在于，包括：

获取机器人的从手柔性臂末端的目标位姿信息；

基于预设的欧拉角旋转顺序，确定所述从手柔性臂末端的欧拉变换信息；所述欧拉变换信息表征所述从手柔性臂末端的旋转姿态；

建立所述从手柔性臂末端的欧拉变换信息、所述从手柔性臂末端的齐次变换矩阵与所述目标位姿信息之间的等式关系，确定待求解函数组；所述待求解函数组包括基于末端坐标系的三个欧拉角函数和三个坐标位置函数；所述三个欧拉角函数中每个欧拉角函数和所述三个坐标位置函数中每个坐标位置函数分别与所述从手柔性臂的各关节的关节参数相关；

基于获取的所述各关节的当前关节参数信息，分别确定所述每个欧拉角函数的函数值和所述每个坐标位置函数的函数值，得到待校验的函数值集合；

根据所述待校验的函数值集合和所述目标位姿信息，对所述各关节的当前关节参数信息进行迭代更新，得到迭代更新后的各关节的目标关节参数信息；所述各关节的目标关节参数信息用于控制所述从手柔性臂进行运动。

2.根据权利要求1所述的机器人的运动控制方法，其特征在于，所述目标位姿信息包括目标姿态信息和目标位置信息；所述目标姿态信息用于描述所述末端坐标系中各个坐标轴的方向；所述目标位置信息包括所述末端坐标系的原点基于机器人基坐标系的三维位置坐标值；所述末端坐标系的原点与所述从手柔性臂末端的中心点重合；

所述根据所述待校验的函数值集合和所述目标位姿信息，对所述各关节的当前关节参数信息进行迭代更新，得到迭代更新后的各关节的目标关节参数信息，包括：

基于所述目标姿态信息确定三个欧拉角目标值；

基于所述三个欧拉角函数、所述三个坐标位置函数和所述各关节的当前关节参数信息，确定当前迭代轮次对应的当前雅克比矩阵；

基于所述当前雅克比矩阵、所述待校验的函数值集合、所述三维位置坐标值和所述三个欧拉角目标值，确定所述各关节的关节参数增量信息；

利用所述各关节的关节参数增量信息，对所述各关节的当前关节参数信息进行更新，得到更新后的各关节的当前关节参数信息；

若所述待校验的函数值集合和所述目标位姿信息之间不满足预设迭代结束条件，将所述更新后的各关节的当前关节参数信息作为所述各关节的当前关节参数信息；

对所述各关节的当前关节参数信息进行下一轮次迭代更新，直至满足所述预设迭代结束条件，得到迭代更新后的各关节的目标关节参数信息。

3.根据权利要求2所述的机器人的运动控制方法，其特征在于，所述若所述待校验的函数值集合和所述目标位姿信息之间不满足预设迭代结束条件，将所述更新后的各关节的当前关节参数信息作为所述各关节的当前关节参数信息，包括：

基于所述三个欧拉角目标值，确定所述每个欧拉角函数的函数值的误差值；

基于所述三维位置坐标值，确定所述每个坐标位置函数的函数值的误差值；

根据所述每个欧拉角函数的函数值的误差值和所述每个坐标位置函数的函数值的误差值，确定当前总误差值；

若所述当前总误差值大于等于预设值，利将所述更新后的各关节的当前关节参数信息作为所述各关节的当前关节参数信息。

4.根据权利要求3所述的机器人的运动控制方法，其特征在于，所述方法还包括：

若所述待校验的函数值集合和所述目标位姿信息之间满足所述预设迭代结束条件，将所述各关节的当前关节参数信息确定为所述迭代更新后的各关节的目标关节参数信息。

5.根据权利要求1所述的机器人的运动控制方法，其特征在于，所述获取机器人的从手柔性臂末端的目标位姿信息，包括：

基于机器人主手的运动增量信息进行主从映射，得到所述从手柔性臂末端的目标位置增量信息和所述从手柔性臂末端的目标姿态信息；

根据所述从手柔性臂末端的当前位置信息、所述从手柔性臂末端的目标位置增量信息和所述从手柔性臂末端的目标姿态信息，确定所述从手柔性臂末端的目标位姿信息。

6.根据权利要求5所述的机器人的运动控制方法，其特征在于，所述机器人主手的运动增量信息包括目标主手位置增量信息和目标主手姿态增量信息；

所述基于机器人主手的运动增量信息进行主从映射，得到所述从手柔性臂末端的目标位置增量信息和所述从手柔性臂末端的目标姿态信息，包括：

基于内窥镜视野下主从映射方法，对所述目标主手位置增量信息进行映射，得到所述从手柔性臂末端的目标位置增量信息；

根据所述目标主手姿态增量信息和获取的当前主手姿态信息，确定目标主手姿态信息；

基于所述内窥镜视野下主从映射方法，对所述目标主手姿态信息进行映射，得到所述从手柔性臂末端的目标姿态信息。

7.根据权利要求1所述的机器人的运动控制方法，其特征在于，基于预设的欧拉角旋转顺序，确定所述从手柔性臂末端的欧拉变换信息，包括：

确定绕所述末端坐标系中Z轴旋转第一欧拉角的第一旋转矩阵；

确定绕所述末端坐标系中Y轴旋转第二欧拉角的第二旋转矩阵；

确定绕所述末端坐标系中X轴旋转第三欧拉角的第三旋转矩阵；

将所述第一旋转矩阵、所述第二旋转矩阵和所述第三旋转矩阵相乘，得到所述从手柔性臂末端的欧拉变换信息。

8.一种机器人的运动控制装置，其特征在于，包括：

获取模块，被配置为执行获取机器人的从手柔性臂末端的目标位姿信息；

第一确定模块，被配置为执行基于预设的欧拉角旋转顺序，确定所述从手柔性臂末端的欧拉变换信息；所述欧拉变换信息表征所述从手柔性臂末端的旋转姿态；

第二确定模块，用于建立所述从手柔性臂末端的欧拉变换信息、所述从手柔性臂末端的齐次变换矩阵与所述目标位姿信息之间的等式关系，确定待求解函数组；所述待求解函数组包括基于末端坐标系的三个欧拉角函数和三个坐标位置函数；所述三个欧拉角函数中每个欧拉角函数和所述三个坐标位置函数中每个坐标位置函数分别与所述从手柔性臂的各关节的关节参数相关；

第三确定模块，被配置为执行基于获取的所述各关节的当前关节参数信息，分别确定所述每个欧拉角函数的函数值和所述每个坐标位置函数的函数值，得到待校验的函数值集合；

第四确定模块，被配置为执行根据所述待校验的函数值集合和所述目标位姿信息，对所述各关节的当前关节参数信息进行迭代更新，得到迭代更新后的各关节的目标关节参数信息；所述各关节的目标关节参数信息用于控制所述从手柔性臂进行运动。

9.一种机器人设备，其特征在于，所述机器人设备包括处理器和存储器，所述存储器中存储有至少一条指令或至少一段程序，所述至少一条指令或所述至少一段程序由所述处理器加载并执行如权利要求1-7任一项所述的机器人的运动控制方法。

10.一种计算机存储介质，其特征在于，所述存储介质中存储有至少一条指令或至少一段程序，所述至少一条指令或至少一段程序由处理器加载并执行以实现如权利要求1-7任一项所述的机器人的运动控制方法。

Images