CN112828885A - 混联主从映射方法、机械臂系统和计算机设备 - Google Patents

Abstract

本申请涉及一种混联主从映射方法、机械臂系统和计算机设备。其中，该方法包括：获取机械臂的末端点的当前位置和目标位置，机械臂包括被动臂和搭载在被动臂的末端的多自由度并联平台；将当前位置到目标位置的位移分别分解到竖直方向和水平平面上，得到竖直位移位置和水平位移位置；根据竖直位移位置确定多自由度并联平台的第一控制量，以及根据水平位移位置确定被动臂的第二控制量；根据第一控制量控制多自由度并联平台，以及根据第二控制量控制被动臂。通过本申请，解决了手术机器人在术中运动范围小的问题，提高了手术机器人的运动范围。

Description

混联主从映射方法、机械臂系统和计算机设备

技术领域

本申请涉及控制领域，特别是涉及混联主从映射方法、机械臂系统和计算机设备。

背景技术

目前具有多自由度并联平台和被动臂的手术机器人被应用到各种手术过程。这种手术机器人的被动臂末端搭载有多自由度并联平台，通过在多自由度并联平台的动平台上设置手术器械来实现手术器械的位置和姿态控制。但是一方面，虽然被动臂的运动范围比较大，但由于被动臂的体积和重量大，在术中被动臂的所有关节均被锁死，以防止被动臂在术中运动砸伤患者；另一方面，由于多自由度并联平台的工作空间较小，仅依靠并联平台的运动无法在患者体表大范围移动。因此，手术机器人在术中的运动范围小，目前尚未提出有效的解决方案。

发明内容

本申请实施例提供了一种混联主从映射方法、机械臂系统和计算机设备，以解决相关技术中手术机器人在术中运动范围小的问题。

第一方面，本实施例提供了一种混联主从映射方法，包括：获取机械臂的末端点的当前位置和目标位置，所述机械臂包括被动臂和搭载在所述被动臂的末端的多自由度并联平台；将所述当前位置到所述目标位置的位移分别分解到竖直方向和水平平面上，得到竖直位移位置和水平位移位置；根据所述竖直位移位置确定所述多自由度并联平台的第一控制量，以及根据所述水平位移位置确定所述被动臂的第二控制量；根据所述第一控制量控制所述多自由度并联平台，以及根据所述第二控制量控制所述被动臂。

在其中一些实施例中，所述机械臂由主操作手控制；获取所述机械臂的末端点的当前位置包括：在所述主操作手开始被操作的时刻，将所述机械臂的末端点的位置记录为所述当前位置。

在其中一些实施例中，所述机械臂由主操作手控制；获取所述机械臂的末端点的目标位置包括：在所述主操作手开始被操作的时刻，初始化所述主操作手的初始位置；在所述主操作手被操作后，确定所述主操作手的当前位置，并根据所述主操作手的当前位置和初始位置，确定所述主操作手的位移；根据预设位移比例系数，将所述主操作手的位移转换为所述机械臂的目标位移，并根据所述目标位移和所述机械臂的末端点的所述当前位置确定所述目标位置。

在其中一些实施例中，所述预设位移比例系数可调节。

在其中一些实施例中，所述被动臂包括多个关节；根据所述水平位移位置确定所述被动臂的第二控制量包括：确定所述多个关节中至少一个在水平平面内产生位移的目标移动关节和至少一个绕竖直方向转动的目标旋转关节；根据所述水平位移位置确定所述被动臂的第二控制量，其中，所述第二控制量包括所述目标移动关节的移动量和所述目标旋转关节的旋转量。

在其中一些实施例中，根据所述水平位移位置确定所述被动臂的第二控制量包括：根据所述水平位移位置逆解得到所述被动臂的控制量；在逆解得到的所述被动臂的控制量为多组结果的情况下，确定与所述目标旋转关节需要旋转的角度最小的一组结果为所述第二控制量；在逆解得到的所述被动臂的控制量为一组结果的情况下，确定这一组结果为所述第二控制量。

第二方面，本实施例提供了一种机械臂系统，所述机械臂系统包括计算机设备和机械臂，所述机械臂包括被动臂和搭载在所述被动臂的末端的多自由度并联平台，所述计算机设备分别与所述被动臂和所述多自由度并联平台电性连接，所述计算机设备包括处理器和存储介质，所述存储介质中存储有计算机程序，所述计算机程序被设置为运行时执行如第一方面所述的混联主从映射方法。

在其中一些实施例中，所述被动臂包括多个关节，所述多个关节中能够使得所述机械臂的末端点在竖直方向产生位移的关节被锁定。

在其中一些实施例中，所述多个关节中能够使得所述机械臂的末端点在竖直方向产生位移的关节包括：具有沿竖直方向位移的移动关节，和/或具有绕非竖直方向转动的旋转关节。

第三方面，本实施例提供了一种计算机设备，所述计算机设备包括处理器和存储介质，所述存储介质中存储有计算机程序，所述计算机程序被设置为运行时执行如第一方面所述的混联主从映射方法。

相比于相关技术，本申请实施例提供的混联主从映射方法、机械臂系统和计算机设备，解决了相关技术中手术机器人在术中运动范围小的问题，提高了手术机器人的运动范围。

本申请的一个或多个实施例的细节在以下附图和描述中提出，以使本申请的其他特征、目的和优点更加简明易懂。

附图说明

此处所说明的附图用来提供对本申请的进一步理解，构成本申请的一部分，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：

图1是本申请实施例提供的机械臂系统的结构框图。

图2是本申请实施例提供的混联主从映射方法的流程图。

图3是本申请实施例的多关节被动臂的坐标转换方法的流程图。

图4是本申请优选实施例提供的机械臂的结构示意图。

图5是本申请优选实施例提供的机械臂的机械坐标系和关节坐标系的示意图。

图6是本申请优选实施例提供的主操作手与超声探头末端点在用户坐标系下的运动映射示意图。

具体实施方式

为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行描述和说明。应当理解，此处所描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。基于本申请提供的实施例，本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。此外，还可以理解的是，虽然这种开发过程中所作出的努力可能是复杂并且冗长的，然而对于与本申请公开的内容相关的本领域的普通技术人员而言，在本申请揭露的技术内容的基础上进行的一些设计，制造或者生产等变更只是常规的技术手段，不应当理解为本申请公开的内容不充分。

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

除非另作定义，本申请所涉及的技术术语或者科学术语应当为本申请所属技术领域内具有一般技能的人士所理解的通常意义。本申请所涉及的“一”、“一个”、“一种”、“该”等类似词语并不表示数量限制，可表示单数或复数。本申请所涉及的术语“包括”、“包含”、“具有”以及它们任何变形，意图在于覆盖不排他的包含；例如包含了一系列步骤或模块(单元)的过程、方法、系统、产品或设备没有限定于已列出的步骤或单元，而是可以还包括没有列出的步骤或单元，或可以还包括对于这些过程、方法、产品或设备固有的其它步骤或单元。本申请所涉及的“连接”、“相连”、“耦接”等类似的词语并非限定于物理的或者机械的连接，而是可以包括电气的连接，不管是直接的还是间接的。本申请所涉及的“多个”是指大于或者等于两个。“和/或”描述关联对象的关联关系，表示可以存在三种关系，例如，“A和/或B”可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。本申请所涉及的术语“第一”、“第二”、“第三”等仅仅是区别类似的对象，不代表针对对象的特定排序。

本实施例提供了一种机械臂系统，图1是本申请实施例提供的机械臂系统的结构框图，如图1所示，该机械臂系统包括计算机设备101和机械臂102，机械臂102包括被动臂1021和搭载在被动臂1021的末端的多自由度并联平台1022。

该被动臂1021可以包括多个关节，关节的类型包括但不限于移动关节和旋转关节，以保证该被动臂1021能够在较大的范围内运动。被动臂1021的各个关节都具有锁定装置，可以将各个关节的姿态锁定，以保持固定的位移或者旋转角度。

在术前摆位时，可以通过人为拉动被动臂1021运动到相应的手术位置附近。在相关技术中，手术开始之前会将被动臂1021的各个关节锁定，以防止被动臂1021在术中下坠导致患者被砸伤等意外。

与相关技术不同之处在于，在本实施例中，并不将被动臂1021的全部关节都锁定，而仅将能够使机械臂的末端点在竖直方向产生位移的关节锁定，这些能够使得机械臂的末端点在竖直方向产生位移的关节包括但不限于：具有沿竖直方向位移的移动关节，和/或具有绕非竖直方向转动的旋转关节。

通常而言，为了运算方便，被动臂1021的各关节的位移方向或者旋转轴都平行于正交坐标系的某一条坐标轴。以正交坐标系的Z轴竖直向上，X轴水平向右，Y轴垂直于XZ平面，坐标轴设置符合右手法则为例，在这种情况下，具有沿Z轴方向位移的移动关节，以及绕X轴或者绕Y轴方向转动的旋转关节都有可能使得机械臂的末端点竖直向下运动，从而存在安全隐患。

仍以上述坐标轴为例，若某个移动关节平行于X轴或Y轴方向，或者某个旋转关节的旋转轴平行于Z轴，则这样的移动关节或旋转关节不会使得机械臂的末端点竖直向下运动，因此，这样的移动关节或旋转关节即使未被锁定，也可以保证手术机器人的安全性。除了上述的移动关节和旋转关节之外，被动臂1021中按照其他方向设置的移动关节和旋转关节都应当在术中被锁定，以保障手术安全。

本实施例的多自由度并联平台可以实现多个自由度的运动，以具有六自由度的Stewart并联平台为例，Stewart并联平台包括静平台、动平台和多个设置在静平台与动平台之间的伸缩组件，可以实现空间上六个自由度的运动，分别为沿X轴位移、沿Y轴位移、沿Z轴位移、绕X轴转动、绕Y轴转动和绕Z轴转动。Stewart并联平台由6根伸缩组件支撑，与采用串联的悬臂梁结构的被动臂相比刚度大，结构稳定，并且由于刚度大，并联结构较串联结构在相同的自重或体积下，有较高的承载能力。采用串联的悬臂梁结构的被动臂末端的误差是各个关节误差的积累和放大，因而误差大、精度低，而并联平台则没有那样的误差积累和放大关系，微动精度高，更适宜执行高精度的手术操作。此外，在位置求解上，并联平台的逆解非常容易，容易根据坐标位置求得并联平台各伸缩组件的运动姿态。

在上述的机械臂系统中，计算机设备101分别与被动臂1021和多自由度并联平台1022电性连接，计算机设备101包括处理器1011和存储介质1012，存储介质1012中存储有计算机程序10121，计算机程序10121被处理器运行时执行机械臂系统的控制方法。

图2是本申请实施例提供的混联主从映射方法的流程图，如图2所示，该流程包括如下步骤：

步骤S201，获取机械臂的末端点的当前位置和目标位置，机械臂包括被动臂和搭载在被动臂的末端的多自由度并联平台。

步骤S202，将当前位置到目标位置的位移分别分解到竖直方向和水平平面上，得到竖直位移位置和水平位移位置。

步骤S203，根据竖直位移位置确定多自由度并联平台的第一控制量，以及根据水平位移位置确定被动臂的第二控制量。

步骤S204，根据第一控制量控制多自由度并联平台，以及根据第二控制量控制被动臂。

相较于相关技术中被动臂的各个关节都必须锁死而言，在本实施例中可以仅锁死在竖直方向能够产生位移的关节，通过将机械臂的末端点的位移分解到竖直方向和水平平面内。由于被动臂能够在水平平面内的更大范围内运动，相较于多自由度并联平台在水平平面内的运动范围大得多，因此增加了手术机器人的运动范围。

本实施例的机械臂系统中，可以采用主从映射方式来控制被动臂和多自由度并联平台。在其中一些实施例中，机械臂系统还包括主操作手。相较于相关技术中的主操作手只能用来控制多自由度并联平台不同之处在于，在本实施例中，主操作手通过混联映射的方式实现被动臂和多自由度并联平台的共同控制。

在其中一些实施例中，为了获得机械臂的末端点的当前位置，可以在主操作手开始被操作的时刻，将机械臂的末端点的位置记录为当前位置。

在其中一些实施例中，在主操作手开始被操作的时刻，初始化主操作手的初始位置，也就是将主操作手在开始被操作的时刻所处的位置设为初始位置。在主操作手被操作后，主操作手的位置运动到新的位置，这个新的位置称为主操作手的当前位置。那么，根据主操作手的初始位置到主操作手的当前位置，就能够确定主操作手在被操作过程中的位移，该位移将被映射为机械臂的位移，从而使得机械臂的末端点根据由主操作手的位移确定的机械臂的位移，从当前位置移动到目标位置。

在本实施例中，主操作手的位移与机械臂的末端点的位移之间的关系根据预设位移比例系数确定。该预设位移比例系数可调节。例如，当该预设位移比例系数小于1的情况下，则主操作手的位移数值大于机械臂的末端点的位移数值，因此，在需要进行高精度的手术的情形下，可以将预设位移比例系数设置为小于1的数值。当该预设位移比例系数大于1的情况下，则主操作手的位置数值小于机械臂的末端点的位移数值，因此，在需要机械臂的末端点在较大范围内运动的情形下，可以将预设位移比例系数设置为大于1的数值，以提高操作体验。

另外，在上述实施例中，多自由度并联平台的第一控制量是根据机械臂末端点的目标位置在竖直方向上分解得到的竖直位移位置逆解得到的，由于多自由度并联平台的逆解运算非常容易，因此上述方式能够减少运算量，提高机械臂的控制效率。

在其中一些实施例中，根据水平位移位置确定被动臂的第二控制量包括：确定多个关节中至少一个在水平平面内产生位移的目标移动关节和至少一个绕竖直方向转动的目标旋转关节；根据水平位移位置确定被动臂的第二控制量，其中，第二控制量包括目标移动关节的移动量和目标旋转关节的旋转量。

在采用串联结构的被动臂中，逆解运算相较于正解运算要复杂，为了降低运算复杂度，在其中一些实施例中，可以在满足运动范围要求的情况下，尽量少地选取目标移动关节和目标旋转关节，并保证除选取的目标移动关节和目标旋转关节之外的其他关节都被锁定。在其他关节被锁定后，其他关节之间的坐标转换关系也就固定下来了，那么串联结构的被动臂中逆解运算未知的坐标转换关系仅涉及到与被选取的目标移动关节和目标旋转关节之间的坐标转换关系，大大地降低了运算的复杂度。

在一些实施例中，选取的目标移动关节的数量为1个，选取的目标旋转关节的数量也为1个。其中，被选取的这一个目标移动关节可以是被动臂中在水平平面内运动的移动关节中能够移动的范围最大的移动关节，从而保证最大的运动范围。

具有串联结构的被动臂的逆解可能为多个。在其中一些实施例中，根据水平位移位置确定被动臂的第二控制量时，在根据水平位移位置逆解得到被动臂的控制量之后，如果逆解得到的被动臂的控制量为多组结果，则确定与目标旋转关节需要旋转的角度最小的一组结果为第二控制量；如果逆解得到的被动臂的控制量为一组结果，则确定这一组结果为第二控制量。

下面通过优选实施例对本申请进行描述和说明。

针对相关技术中被动臂的控制复杂的问题，在本实施例提供了一种被动臂的坐标系建立和转换方法，应用于包括旋转关节和移动关节的被动臂，这些旋转关节和移动关节依次串联连接。为了实现被动臂在空间中多个自由度的运动，旋转关节和移动关节的数量可以根据需要的自由度和运动范围进行设置，即旋转关节和移动关节的数量都可以是一个或者多个。

图3是本申请实施例的多关节被动臂的坐标转换方法的流程图，如图3所示，该流程包括如下步骤S301至步骤S303。

步骤S301，在被动臂处于初始状态时，建立被动臂的机械坐标系和被动臂的各关节的关节坐标系。

被动臂的控制通常基于DH参数进行坐标系的转换。相连接的两个关节为相邻关节。相邻两个关节的关节坐标系的变换通常采用DH参数或改进的DH参数来表示。以DH参数为例，相邻两个关节坐标系通过绕Z轴旋转θ、平移d后，再绕X轴旋转α、平移a，就能够相互重合，上述的θ、d、α和a即为DH参数。由此可知，如果DH参数越简洁，则相邻两个关节坐标系的转换越简单。

为了简化DH参数，在本实施例中，旋转关节的关节坐标系的Z轴沿旋转轴设置，移动关节的关节坐标系的Z轴沿移动方向设置，机械坐标系与各关节的关节坐标系同为左手系或右手系，且当旋转关节之前的关节为移动关节时，旋转关节的关节坐标系的原点与该移动关节的关节坐标系的原点重合。

上述的机械坐标系又可以称为全局坐标系、参考坐标系或者世界坐标系，该机械坐标系通常设置于被动臂的底座中心，并且位于机械臂的第一个关节的正下方，这样可以使得机械坐标系与第一个关节的转换关系尽可能地简单。

在多关节被动臂中，在大多数情况下旋转关节在接收到转角为0或2π的指令时，可能不需要区分二者，而是根据预先的设定保持不旋转或者以设定方向旋转2π。但在一些情况下，需要对这两个转角进行区分。在需要对0或2π转角进行区分的情形下，旋转关节的DH参数中Z轴的旋转角度不为0或2π，以避免混淆0和2π的位置。

步骤S302，获取被动臂的机械坐标系和被动臂的各关节的关节坐标系的DH参数，并根据DH参数，确定被动臂的机械坐标系到被动臂的末端关节的关节坐标系之间的转换关系。

在建立各关节的关节坐标系之后，就可以依次获得相邻关节之间的DH参数，并根据DH参数，确定被动臂的机械坐标系到被动臂的末端关节的关节坐标系之间的转换关系。

其中，根据DH法则，第i-1关节的坐标到第i关节的坐标的齐次变换被构造为具有两个旋转和两个变换的序列，采用矩阵可表示如下：

其中，i＝2,3,4,…,n；n为被动臂的旋转关节和移动关节的总数量。被动臂的第1个关节的DH参数是表示的该第一个关节与机械坐标系之间的坐标系的转换，记为

则机械坐标系与第1个关节的关节坐标系的转换关系：

可见，该变换关系与第i-1关节的坐标到第i关节的坐标的齐次变换形式完全相同。

在得到

与

之后，从第0个坐标系(机械坐标系)到第N关节的关节坐标系的转换矩阵可表示为：

其中，第N个关节为末端关节。

步骤S303，根据转换关系进行机械坐标系与末端关节的关节坐标系之间的坐标转换。

上述步骤S302中确定的

表示的就是机械坐标系与末端关节的关节坐标系之间的坐标转换关系，根据该转换关系就能够实现机械坐标系与末端关节的关节坐标系之间的坐标转换。

图4是本申请优选实施例提供的机械臂的结构示意图，如图4所示的机械臂依次包括被动臂和多自由度并联平台，被动臂包括：移动关节1、旋转关节2、移动关节3、旋转关节4、旋转关节5、移动关节6、旋转关节7、移动关节8、旋转关节9、移动关节10。多自由度并联平台固定在移动关节10的关节臂末端。该机械臂还包括与移动关节1固连的基座11。

该优选实施例的机械臂的坐标转换方法包括步骤1至步骤3，分别对应于图3中的步骤S301至步骤S303。

步骤1，依照世界坐标系的规则在被动臂的基座处建立机械坐标系，以及各关节的关节坐标系。

图5是本申请优选实施例提供的机械臂的机械坐标系和关节坐标系的示意图。

参考图5，机械坐标系F₀-X₀Y₀Z₀坐标系原点F₀固连于被动臂基座处，Z₀轴由F₀指向移动关节1，Y₀轴由基座F₀点指向被动臂，X₀轴指向符合右手坐标系。

移动关节1的关节坐标系L₁-X₁Y₁Z₁的原点L₁固连在移动关节1上，各坐标轴的指向和机械坐标系的对应轴指向相同。

旋转关节2的关节坐标系R₂-X₂Y₂Z₂的原点R₂固连在旋转关节2上且与L₁重合，Z₂指向和Z₁轴指向相同，X₂轴和Y₂轴的指向分别与X₁轴和Y₁轴指向相反。

移动关节3的关节坐标系L₃-X₃Y₃Z₃原点L₃固连在移动关节3上，Z₃轴由L₁点指向L₃点，X₃轴和Y₃轴分别与X₂轴和Z₂轴指向相同。

旋转关节4的关节坐标系R₄-X₄Y₄Z₄原点R₄固连在旋转关节4上且与L₃点重合(图5中为清晰标明移动关节3的关节坐标系和旋转关节4的关节坐标系将L₃和R₄分开标出，下同)，Z₄轴指向和Y₃轴指向相反，初始时，X₄轴和Y₄轴指向分别与X₃轴和Z₃轴指向相反。

旋转关节5的关节坐标系R₅-X₅Y₅Z₅原点R₅固连在旋转关节5上，Z₅轴方向和Z₄轴方向相同，初始时，X₅轴和Y₅轴指向分别与X₄轴和Y₄轴指向相反。

移动关节6的关节坐标系L₆-X₆Y₆Z₆原点L₆固连在移动关节6上，Z₆轴由R₅点指向L₆点，X₆轴和Y₆轴指向分别与X₅轴和Z₅轴指向相同。

旋转关节7的关节坐标系R₇-X₇Y₇Z₇原点R₇固连在旋转关节7上且和L₆点重合，初始时，Z₇轴和Y₇轴的指向分别与Y₆轴和X₆轴的指向相反，X₇轴和Z₆轴的指向相同。

移动关节8的关节坐标系L₈-X₈Y₈Z₈的原点L₈固连在移动关节8上，Z₈轴由L₈点指向R₇点，X₈和Y₈轴的指向分别与X₇和Y₇轴的指向相同。

旋转关节9的关节坐标系R₉-X₉Y₉Z₉的原点R₉点固连在旋转关节9上且和L₈点重合，Z₉轴的方向与Y₈轴的方向相反，初始时，X₉轴的方向与Z₈轴的方向相反，Y₉轴的方向与X8轴的方向相同。

移动关节10的关节坐标系L₁₀-X₁₀Y₁₀Z₁₀的原点L₁₀固连在移动关节10上，Z₁₀轴由R₉点指向L₁₀点，X₁₀轴和Y₁₀轴的指向分别与X₉轴和Z₉轴的指向相同。

F₀R₂的长度为l₁，R₂R₄的长度为l₂，R₄R₅的长度为l₃，R₅R₇的长度为l₄，R₇R₉的长度为l₅，R₉L₁₀的长度为l₆，点L₁、R₂、L₃、R₄、R₅、L₆和R₇位于同一水平面上。

步骤2，获取DH参数，并计算机械坐标系到多关节被动臂的末端关节的转换关系，该DH参数可以为传统的DH参数或改进的DH参数。

DH参数如表1所示。

表1图5所示的多关节被动臂的DH参数表

在图4所示的多关节机械臂的末端关节上还可以搭载并联平台，并联平台包括静平台、动平台和多个设置在静平台与动平台之间的伸缩组件，并联平台的计算坐标系与多关节机械臂的末端关节的关节坐标系完全重合，超声探测装置设于动平台。

本实施例的并联平台可以实现多个自由度的运动，以具有六自由度的Stewart并联平台为例，Stewart并联平台包括静平台、动平台和多个设置在静平台与动平台之间的伸缩组件，可以实现空间上六个自由度的运动，分别为沿X轴位移、沿Y轴位移、沿Z轴位移、绕X轴转动、绕Y轴转动和绕Z轴转动。Stewart并联平台由6根伸缩组件支撑，与采用串联的悬臂梁结构的被动臂相比刚度大，结构稳定，并且由于刚度大，并联结构较串联结构在相同的自重或体积下，有较高的承载能力。采用串联的悬臂梁结构的被动臂末端的误差是各个关节误差的积累和放大，因而误差大、精度低，而并联平台则没有那样的误差积累和放大关系，微动精度高，更适宜执行高精度的手术操作。此外，在位置求解上，并联平台的逆解非常容易，容易根据坐标位置求得并联平台各伸缩组件的运动姿态。

正是基于并联平台逆解非常容易的特点，在上述实施例中，可以通过在静平台上建立从用户坐标系，将主操作手的位移量映射到从用户坐标系内，再通过从用户坐标系与计算坐标系的转换从而得到目标位置在计算坐标系内的位置信息，根据该位置信息通过逆解就能够很容易地得到并联平台各伸缩组件的运动姿态，相对于相关技术中通过正运动学求解并联平台末端的位置信息，上述方式大大降低了运算复杂度，提高了控制效率，节约了运算资源。

解算Stewart计算坐标系到机械坐标系的转换矩阵，即移动关节坐标系L₁₀-X₁₀Y₁₀Z₁₀到机械坐标系F₀-X₀Y₀Z₀的转换矩阵与逆矩阵：

根据机器人正运动学可知两相邻关节之间的转换矩阵为

从第m关节(包含m关节的自由度)到第n关节的转换矩阵可表示为：

可解算得单条被动臂从基座到Stewart平台静平台的转换矩阵

即机械坐标系到Stewart计算坐标系的转换矩阵，命名为T_{trans_mach_st}，而Stewart计算坐标系到机械坐标系的转换矩阵则为逆矩阵

步骤3，根据转换矩阵和转换矩阵的逆矩阵进行机械坐标系与Stewart计算坐标系的坐标转换。

在采用主从控制的机械臂系统中，为了简化主从控制的运动映射，还可以在机械臂建立用户坐标系。

在机械臂为单臂工作的情况下，图3所示的机械臂的坐标转换方法还可以包括如下步骤：

步骤1，建立用户坐标系，其中，用户坐标系的XY坐标平面平行于机械坐标系的XY平面，且用户坐标系的原点与末端关节的关节坐标系的原点重合。

步骤2，获取用户输入的视角值，并根据视角值和机械坐标系与末端关节的关节坐标系之间的转换关系，确定用户坐标系与机械坐标系之间的转换关系。

在步骤2中建立的用户坐标系的XY坐标平面绕Z轴旋转角度为视角值。单臂工作时，视角值由用户根据视角自行输入，命名为θ_{theta_mach_user}，根据用户坐标的定义方式，可得到用户坐标系相对于机械坐标系的转换矩阵：

其中

表示上述

中第1行第4列的数据。

下面以在多自由度并联平台的动平台上搭载超声探测装置为例对本优选实施例的混联主从映射方法进行说明。由于超声探测装置需要在患者体表大范围移动来获取全面的影像，因此采用本优选实施例的机械臂系统的控制方法来实现机械臂的混联映射，实现大范围的运动。

在本优选实施例中，超声探测装置的末端点位于多自由度并联平台的动平台的轴线上。

在临床应用中，操控超声探头时需要在患者体表大范围的移动，即机械坐标系XY平面内的大范围移动，而垂直于患者体表的方向不需要大范围的移动，即机械坐标系Z轴方向上不需要大范围移动。由于被动臂体积较大，质量较重，在手术过程中必须将沿Z轴方向的移动关节和绕X轴、Y轴方向的转动关节锁死，才能保证手术的安全性。因此，主手沿机械坐标系Z轴方向(根据用户坐标系建立准则，机械坐标系的Z轴与用户坐标系Z轴平行)的移动映射到Stewart并联平台上完成，主手在机械坐标系XY平面(根据用户坐标系建立准则，机械坐标系的XY平面与用户坐标系XY平面平行)内的移动映射到被动臂的一个转动关节和一个移动关节上。

本优选实施例的混联映射方法包括如下步骤：

步骤1，选取被动臂在机械坐标系下XY平面内的一个移动关节d₆和一个绕Z轴的转动关节θ₅，作为混联控制中串联部分的两个控制对象。Stewart并联平台作为并联部分的控制对象。

步骤2，术者握持主操作手开始手术操作时刻起，至手离开主操作手停止的手术操作期间视为一个工作周期T。

步骤3，术者握持主操作手开始手术操作时刻即T(0)时刻，将该时刻的主操作手的位置坐标设为原点M₀(0，0，0)；该时刻器械末端点在用户坐标系的位置坐标为S₀(X₀，Y₀，Z₀)，系统会计算并保存S₀(X₀，Y₀，Z₀)作为已知值，记为C_{coord_now_user}。

步骤4，在工作周期内的任一时刻t，设主操作手的位置坐标为M_t(X_mt，Y_mt，Z_mt)，记为C_{coord_offset_mas}：

C_{coord_offset_mas}＝[cx_{coord_offset_mas} cy_{coord_offset_mas} cz_{coord_offset_mas}]。

此时器械末端点在用户坐标系中运动目标点位置坐标S_t(X_t，Y_t，Z_t)可通过M_t(X_mt，Y_mt，Z_mt)经位移比例放大系数K₁为获得：

Xt＝X₀+K₁*X_mt；

Yt＝Y₀+K₁*Y_mt；

Zt＝Z₀+K₁*Z_mt。

基于T(0)时刻器械末端点在用户坐标系的位置坐标为S₀(X₀，Y₀，Z₀)，在每一个运动执行周期，主操作手会发送当下的坐标M_t(X_mt，Y_mt，Z_mt)来解算该时刻器械末端点在用户坐标系下的坐标S_t(X_t，Y_t，Z_t)，记做C_{coord_new_user}：

C_{coord_new_user}＝C_{coord_now_user}+K₀×C_{coord_offset_mas}。

步骤5，将主手的运动在用户坐标系内分解为在Z轴方向上的位移和在XY平面内的位移，初始点S₀(X₀，Y₀，Z₀)到目标点S_t(X_t，Y_t，Z_t)的运动分解点S’(X₀，Y₀，Z_t)记为C_{coord_znew_user}：

C_{coord_znew_user}＝[C_{coord_now_user}(1) C_{coord_now_user}(2) C_{coord_new_user}(3) 1]^T

步骤6，将运动分解点S’(X₀，Y₀，Z_t)在用户坐标系下的坐标通过用户坐标系到Stewart计算坐标系的转换矩阵转换到Stewart计算坐标系下，记做C_{coord_znew_st}：

C_{coord_znew_st}＝T_{trans_st_user}·C_{coord_znew_user}。

其中T_{trans_st_user}为用户坐标系到Stewart计算坐标系的转换矩阵。

步骤7，已知器械末端点在Stewart计算坐标系下的坐标，即可通过Stewart平台的逆运动学解算出平台各个关节的运动量，完成主手在机械坐标Z轴方向的运动到Stewart并联平台的运动映射。

步骤8，接下来解算主手在机械坐标XY平面上的运动到被动臂的运动映射：由于被动臂的运动不影响超声探头末端点Stewart计算坐标系下的坐标，一个运动执行周期后，超声探头末端点在Stewart计算坐标系下的坐标依然为C_{coord_znew_st}，而在用户坐标系下的坐标变为C_{coord_new_user}，因此可得到坐标变换公式：

其中T_{trans_user_mach}为机械坐标系到用户坐标系的转换矩阵。

其中

表示被动臂上第j关节到第i关节的转换矩阵。

将上式转化为：

由于在被动臂除了θ₅和d₆两个运动量参与混联控制，其余运动关节均被锁定，因此其余关节的运动量都是确定的，关节之间的转换矩阵也都是已知的，故方程左边均为已知量，将其用[x₂，y₂，z₂，1]^T代替；超声探头末端点在Stewart计算坐标系下的坐标C_{coord_znew_st}在上一步中已求出，且没有因被动臂的运动而改变，故方程右边的

也是已知量，将其用[x₁，y₁，z₁，1]^T代替。

再将关节5和关节6的DH参数带入到转换矩阵

中，可将上述方程简化为以下形式：

求解该方程可得：

对于上一步中逆解多解的情况，采用就近原则，取最接近的值作为终解，方法为用两个解分别与θ₅的初始值相减并取绝对值，取绝对值较小的那个解，则被动臂的转动关节能转动最小的角度到达目标点。

在已知θ₅的情况下，可得移动关节d₆：

至此完成主手在机械坐标XY平面上的运动到被动臂的运动映射。

主操作手与超声探头末端点在用户坐标系下的运动映射示意图如图6所示。

其中探头从S₀到S’的运动是通过Stewart平台的运动实现的，从S’到S_t的运动是由被动臂的转动关节及移动关节运动实现的。值得注意的是，S’为将探头在Z轴方向上的运动与XY平面内运动分解开的假想参考点，并非探头在实际工作中的轨迹点。

在本实施例中还提供了一种计算机设备，包括存储介质和处理器，该存储介质中存储有计算机程序，该处理器被设置为运行计算机程序以执行上述任一项方法实施例中的步骤。

可选地，上述计算机设备还可以包括传输设备以及输入输出设备，其中，该传输设备和上述处理器连接，该输入输出设备和上述处理器连接。

可选地，在本实施例中，上述处理器可以被设置为通过计算机程序执行以下步骤：

S1，获取机械臂的末端点的当前位置和目标位置，机械臂包括被动臂和搭载在被动臂的末端的多自由度并联平台。

S2，将当前位置到目标位置的位移分别分解到竖直方向和水平平面上，得到竖直位移位置和水平位移位置。

S3，根据竖直位移位置确定多自由度并联平台的第一控制量，以及根据水平位移位置确定被动臂的第二控制量。

S4，根据第一控制量控制多自由度并联平台，以及根据第二控制量控制被动臂。

需要说明的是，在本实施例中的具体示例可以参考上述实施例及可选实施方式中所描述的示例，在本实施例中不再赘述。

另外，结合上述实施例中的方法，本申请实施例还提供一种存储介质来实现。该介质上存储有计算机程序指令；该计算机程序指令被处理器执行时实现上述实施例中的任意一种混联主从映射方法。

需要说明的是，本实施例中的具体示例可以参考上述实施例及可选实施方式中所描述的示例，本实施例在此不再赘述。

本领域的技术人员应该明白，以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。

Claims (10)

1.一种混联主从映射方法，其特征在于包括：

获取机械臂的末端点的当前位置和目标位置，所述机械臂包括被动臂和搭载在所述被动臂的末端的多自由度并联平台；

将所述当前位置到所述目标位置的位移分别分解到竖直方向和水平平面上，得到竖直位移位置和水平位移位置；

根据所述竖直位移位置确定所述多自由度并联平台的第一控制量，以及根据所述水平位移位置确定所述被动臂的第二控制量；

根据所述第一控制量控制所述多自由度并联平台，以及根据所述第二控制量控制所述被动臂。

2.根据权利要求1所述的方法，其特征在于，所述机械臂由主操作手控制；获取所述机械臂的末端点的当前位置包括：

在所述主操作手开始被操作的时刻，将所述机械臂的末端点的位置记录为所述当前位置。

3.根据权利要求1所述的方法，其特征在于，所述机械臂由主操作手控制；获取所述机械臂的末端点的目标位置包括：

在所述主操作手开始被操作的时刻，初始化所述主操作手的初始位置；

在所述主操作手被操作后，确定所述主操作手的当前位置，并根据所述主操作手的当前位置和初始位置，确定所述主操作手的位移；

根据预设位移比例系数，将所述主操作手的位移转换为所述机械臂的目标位移，并根据所述目标位移和所述机械臂的末端点的所述当前位置确定所述目标位置。

4.根据权利要求3所述的方法，其特征在于，所述预设位移比例系数可调节。

5.根据权利要求1所述的方法，其特征在于，所述被动臂包括多个关节；根据所述水平位移位置确定所述被动臂的第二控制量包括：

确定所述多个关节中至少一个在水平平面内产生位移的目标移动关节和至少一个绕竖直方向转动的目标旋转关节；

根据所述水平位移位置确定所述被动臂的第二控制量，其中，所述第二控制量包括所述目标移动关节的移动量和所述目标旋转关节的旋转量。

6.根据权利要求5所述的方法，其特征在于，根据所述水平位移位置确定所述被动臂的第二控制量包括：

根据所述水平位移位置逆解得到所述被动臂的控制量；

在逆解得到的所述被动臂的控制量为多组结果的情况下，确定与所述目标旋转关节需要旋转的角度最小的一组结果为所述第二控制量；

在逆解得到的所述被动臂的控制量为一组结果的情况下，确定这一组结果为所述第二控制量。

7.一种机械臂系统，其特征在于，所述机械臂系统包括计算机设备和机械臂，所述机械臂包括被动臂和搭载在所述被动臂的末端的多自由度并联平台，所述计算机设备分别与所述被动臂和所述多自由度并联平台电性连接，所述计算机设备包括处理器和存储介质，所述存储介质中存储有计算机程序，所述计算机程序被设置为运行时执行如权利要求1至6中任一项所述的混联主从映射方法。

8.根据权利要求7所述的机械臂系统，其特征在于，所述被动臂包括多个关节，所述多个关节中能够使得所述机械臂的末端点在竖直方向产生位移的关节被锁定。

9.根据权利要求8所述的机械臂系统，其特征在于，所述多个关节中能够使得所述机械臂的末端点在竖直方向产生位移的关节包括：具有沿竖直方向位移的移动关节，和/或具有绕非竖直方向转动的旋转关节。

10.一种计算机设备，其特征在于，所述计算机设备包括处理器和存储介质，所述存储介质中存储有计算机程序，所述计算机程序被设置为运行时执行如权利要求1至6中任一项所述的混联主从映射方法。

Images