CN117944057A - 一种机械臂轨迹规划方法、装置、设备及介质 - Google Patents

Abstract

本申请公开了一种机械臂轨迹规划方法、装置、设备及介质，可应用于软式内镜操作机器人系统中。获取软式内镜的预设剩余长度，以输送臂末端工具中心点为端点，基于预设剩余长度拟合目标圆弧。将目标圆弧的另一端点确定为操作臂轨迹分割点，基于目标圆弧提取圆弧插补点。在操作臂轨迹分割点与操作臂的初始位置点之间进行直线插补，获取直线插补点。操作臂轨迹分割点对应的软式内镜剩余长度为软式内镜总长度，获取其余各插补点对应的软式内镜剩余长度。由上述确定的各点组成操作臂的各个轨迹点。在运镜过程中，操作臂的轨迹点由对应的软式内镜剩余长度进行匹配。如此，考虑运行效率、内镜柔顺度来规划操作臂的轨迹点，能够提高软式内镜的输送效果。

Description

一种机械臂轨迹规划方法、装置、设备及介质

技术领域

本申请涉及机械臂控制技术领域，尤其涉及一种机械臂轨迹规划方法、装置、设备及介质。

背景技术

消化道、呼吸道等自然腔道是人类常见疾病好发部位。由于病灶处于人体自然腔道内部，因此需要通过软式内镜进行检查。

传统的软式内镜检查或手术，需要人双手配合持镜体、操作旋钮、手工输送镜体等动作以完成操作过程。而且，一些检查或手术需要在放射线的图像引导下进行，医务人员需要在长时间穿戴沉重的铅制防护服的情况下手工操作软式内镜。这对医务人员诊疗操作质量、体力和健康影响较大。

随着机器人辅助技术的发展，医生可以通过操作机器人进行送镜以及镜下操作，这可以大大降低医生的体力和手工操作疲劳，减少对医务人员的辐射。但是，目前的软式内镜操作机器人系统中机械臂的送镜轨迹由医务人员在送镜过程中自行判断，这可能会导致软式内镜的输送效果较差。

发明内容

为了解决上述技术问题，本申请提供了一种机械臂轨迹规划方法、装置、设备及介质，能够在送镜前进行操作臂轨迹规划，以使得在送镜过程中可以按照操作臂轨迹控制操作臂，提高软式内镜的输送效果。

为了实现上述目的，本申请提供的技术方案如下：

第一方面，本申请提供一种机械臂轨迹规划方法，应用于软式内镜操作机器人系统，所述软式内镜操作机器人系统包括输送臂和操作臂；所述方法包括：

获取软式内镜的预设剩余长度；

以输送臂末端工具中心点为端点，按照弧长为所述预设剩余长度拟合目标圆弧，将所述目标圆弧的另一端点确定为操作臂轨迹分割点；所述输送臂末端工具中心点在目标圆弧上的切线方向为所述输送臂的输送方向；所述操作臂轨迹分割点对应的软式内镜剩余长度为所述预设剩余长度；

基于所述目标圆弧提取圆弧插补点，获取每个所述圆弧插补点对应的软式内镜剩余长度；

在所述操作臂轨迹分割点与所述操作臂的初始位置点之间拟合目标直线，从所述目标直线中提取直线插补点，获取每个直线插补点对应的软式内镜剩余长度；所述初始位置点对应的软式内镜剩余长度为所述软式内镜的总长度；

由所述初始位置点、所述直线插补点、所述操作臂轨迹分割点、所述圆弧插补点以及所述输送臂末端工具中心点，组成所述操作臂的各个轨迹点；所述操作臂的轨迹点通过所述操作臂的位姿进行描述，在软式内镜的输送过程中，所述操作臂的轨迹点由对应的软式内镜剩余长度进行匹配。

第二方面，本申请提供一种机械臂轨迹规划装置，应用于软式内镜操作机器人系统，所述软式内镜操作机器人系统包括输送臂和操作臂；所述装置包括：

第一获取单元，用于获取软式内镜的预设剩余长度；

第一确定单元，用于以输送臂末端工具中心点为端点，按照弧长为所述预设剩余长度拟合目标圆弧，将所述目标圆弧的另一端点确定为操作臂轨迹分割点；所述输送臂末端工具中心点在目标圆弧上的切线方向为所述输送臂的输送方向；所述操作臂轨迹分割点对应的软式内镜剩余长度为所述预设剩余长度；

第一提取单元，用于基于所述目标圆弧提取圆弧插补点，获取每个所述圆弧插补点对应的软式内镜剩余长度；

第二提取单元，用于在所述操作臂轨迹分割点与所述操作臂的初始位置点之间拟合目标直线，从所述目标直线中提取直线插补点，获取每个直线插补点对应的软式内镜剩余长度；所述初始位置点对应的软式内镜剩余长度为所述软式内镜的总长度；

组成单元，用于由所述初始位置点、所述直线插补点、所述操作臂轨迹分割点、所述圆弧插补点以及所述输送臂末端工具中心点，组成所述操作臂的各个轨迹点；所述操作臂的轨迹点通过所述操作臂的位姿进行描述，在软式内镜的输送过程中，所述操作臂的轨迹点由对应的软式内镜剩余长度进行匹配。

第三方面，本申请提供一种电子设备，包括：

一个或多个处理器；

存储装置，其上存储有一个或多个程序，

当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如第一方面所述的机械臂轨迹规划方法。

第四方面，本申请提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现如第一方面所述的机械臂轨迹规划方法。

通过上述技术方案可知，本申请具有以下有益效果：

本申请提供了一种机械臂轨迹规划方法、装置、设备及介质，该方法可应用于软式内镜操作机器人系统中，软式内镜操作机器人系统包括输送臂和操作臂。先获取软式内镜的预设剩余长度，并以输送臂末端工具中心点为端点，按照弧长为预设剩余长度来拟合目标圆弧。其中，输送臂末端工具中心点在该目标圆弧上的切线方向为输送臂的输送方向，以此来限定唯一的目标圆弧。进而，将确定的目标圆弧的另一端点确定为操作臂轨迹分割点，并确定操作臂轨迹分割点对应的软式内镜剩余长度为预设剩余长度。进而，基于目标圆弧提取圆弧插补点，并获取每个圆弧插补点对应的软式内镜剩余长度。另外，在操作臂轨迹分割点与操作臂的初始位置点之间拟合目标直线，并从目标直线中提取直线插补点，获取每个直线插补点对应的软式内镜剩余长度。其中，初始位置点对应的软式内镜剩余长度为软式内镜的总长度。则，由初始位置点、直线插补点、操作臂轨迹分割点、圆弧插补点以及输送臂末端工具中心点，可组成操作臂的各个轨迹点，操作臂的轨迹点通过操作臂位姿进行描述，各个轨迹点处的操作臂位姿均是确定的。基于此，后续在软式内镜的输送过程中，便可根据软式内镜剩余长度确定对应的操作臂轨迹点。

可知，本申请确定了操作臂轨迹分割点，基于操作臂轨迹分割点划分了操作臂的轨迹。将软式内镜从总长度输送到预设剩余长度的过程中，根据软式内镜的剩余长度确定对应的直线插补点，并要求控制操作臂在初始位置点和操作臂轨迹分割点之间按照直线插补点进行运动，以提高操作臂的运行效率，使得操作臂更快到达操作臂轨迹分割点。由于操作臂轨迹分割点是根据软式内镜的预设剩余长度拟合目标圆弧后得到的轨迹点，反之，当控制操作臂到达操作臂轨迹分割点时，软式内镜会展示为圆弧的形态，这会使得软式内镜的输送状态为柔顺状态。进而，在将软式内镜从预设剩余长度输送到结束的过程中，根据软式内镜的剩余长度确定对应的圆弧插补点，并要求控制操作臂在初始位置点和操作臂轨迹分割点之间按照圆弧插补点进行运动，这同样也会使得软式内镜在输送过程中的输送状态保持为柔顺状态，以便于输送臂输送。而且，在后续输送软式内镜的过程中，每个操作臂轨迹点均根据软式内镜的剩余长度确定，确保了操作臂的定位准确。如此，本申请综合考虑操作臂的运行效率、内镜柔顺度、操作臂的位置准确度等来规划操作臂的送镜轨迹，提高了软式内镜的输送效果。

附图说明

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

图1a为本申请实施例提供的一种传统软式内镜的示意图；

图1b为本申请实施例提供的一种软式内镜操作机器人系统的结构示意图；

图2为本申请实施例提供的一种机械臂轨迹规划方法的流程图；

图3为本申请实施例提供的一种软式内镜输送状态的示意图；

图4a为本申请实施例提供的一种软式内镜达到预设剩余长度的示意图；

图4b为本申请实施例提供的另一种软式内镜达到预设剩余长度的示意图；

图5为本申请实施例提供的一种精确位置跟随段的轨迹点规划的示意图；

图6为本申请实施例提供的一种快速位置跟随段的轨迹点规划的示意图；

图7为本申请实施例提供的一种操作臂的轨迹点的示意图；

图8为本申请实施例提供的一种机械臂轨迹规划装置的结构示意图。

具体实施方式

为使本申请的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本申请实施例作进一步详细的说明。

为了便于理解和解释本申请实施例提供的技术方案，先对本申请实施例中所涉及到的背景技术进行介绍。

消化道、呼吸道等自然腔道是人类常见疾病好发部位。由于病灶处于人体自然腔道内部，因此需要通过软式内镜进行检查。

传统的软式内镜检查或手术，需要人双手配合持镜体、操作旋钮、手工输送镜体等动作以完成操作过程。而且，一些检查或手术需要在放射线的图像引导下进行，医务人员需要在长时间穿戴沉重的铅制防护服的情况下手工操作软式内镜。这对医务人员诊疗操作质量、体力和健康影响较大。

随着机器人辅助技术的发展，医生可以通过操作机器人进行送镜以及镜下操作，这可以大大降低医生的体力和手工操作疲劳，减少对医务人员的辐射。但是，目前的软式内镜操作机器人系统中机械臂的送镜轨迹由医务人员在送镜过程中自行判断，这可能会导致软式内镜的输送效果较差。

基于此，本申请实施例提供了一种机械臂轨迹规划方法，在送镜前预先确定操作臂轨迹。具体地，确定操作臂轨迹分割点，基于操作臂轨迹分割点划分了操作臂的轨迹。将软式内镜从总长度输送到预设剩余长度的过程中，根据软式内镜的剩余长度确定对应的直线插补点，并要求控制操作臂在初始位置点和操作臂轨迹分割点之间按照直线插补点进行运动，以提高操作臂的运行效率，使得操作臂更快到达操作臂轨迹分割点。由于操作臂轨迹分割点是根据软式内镜的预设剩余长度拟合目标圆弧后得到的轨迹点，反之，当控制操作臂到达操作臂轨迹分割点时，软式内镜会展示为圆弧的形态，这会使得软式内镜的输送状态为柔顺状态。进而，在将软式内镜从预设剩余长度输送到结束的过程中，根据软式内镜的剩余长度确定对应的圆弧插补点，并要求控制操作臂在初始位置点和操作臂轨迹分割点之间按照圆弧插补点进行运动，这同样也会使得软式内镜在输送过程中的输送状态保持为柔顺状态，以便于输送臂输送。而且，在后续输送软式内镜的过程中，每个操作臂轨迹点均根据软式内镜的剩余长度确定，确保了操作臂的定位准确。如此，本申请综合考虑操作臂的运行效率、柔顺度、位置准确度等来规划操作臂的送镜轨迹，提高了软式内镜的输送效果。

可以理解的是，针对以上方案所存在的缺陷，均是申请人在经过实践并仔细研究后得出的结果。因此，上述问题的发现过程以及本申请实施例中针对上述问题所提出的解决方案，都应该是申请人在本申请过程中对本申请实施例做出的贡献。

为了便于理解本申请实施例提供的机械臂轨迹规划方法，下面结合图1a和图1b进行说明。图1a为本申请实施例提供的一种传统软式内镜的示意图，图1b为本申请实施例提供的一种软式内镜操作机器人系统的结构示意图。

在本申请实施例中，以软式内镜为消化软式内镜为例，如图1a所示，软式内镜整体结构包括操作部11、插入部12和头端部13。操作部11包括内镜工作站接口110、大波轮111、小波轮112、功能按钮113、吸引阀按钮114、水汽阀按钮115以及器械通道116。在输送软式内镜时，先控制头端部13插入患者口腔内，后控制插入部12插入患者口腔内。

结合图1a，以消化软式内镜为例，传统的人工操作消化软式内镜的方式具体为，医护人员右手持软式内镜前端，左手持软式内镜操作部置于胸前，以拇指、中指和无名指调节大小旋钮，中指控制注气、注水，食指控制吸引阀按钮。控制软式内镜的进退时，需要旋转镜身，始终维持良好视野。到达病灶或者需要活检处，从器械通道下对应器械，经插入部内部的器械通道，至头端部。在镜下视野配合下，完成对应操作。

结合图1b，软式内镜操作机器人系统，通常可包括机器人台车100、医生端控制台200（用于对台车进行操控）、配套的工作站300、手术台400、输送臂（也可称为主臂）101、操作臂（也可称为从臂）102、输送装置103以及操作装置104。输送臂101和操作臂102统称为机械臂。其中，输送臂101和操作臂102均为多自由度机械臂。在软式内镜操作机器人系统中，通常需要在机械臂的末端固定一个工具。例如，操作臂的末端固定有操作装置104，操作装置104上固定有软式内镜的操作部11，输送臂101的末端固定有输送装置103。操作装置104和软式内镜的操作部11可共同看作是操作臂末端固定的工具，输送装置可看作是输送臂末端固定的工具，工具也可称为末端执行器。

在主从控制模式下，医护人员可在医生端控制台200生成操作命令，操作命令可转换为输送臂101、操作臂102、输送装置103和操作装置104的运动。其中，操作装置104操作软式内镜弯曲度弯曲，输送装置103控制软式内镜进入人体的长度，二者同时控制软式内镜旋转，亦可单独控制，另一个跟随。同时，操作装置104能驱动软式内镜配套的器械。

在本申请实施例中，为了提高对软式内镜的输送效果，可在控制台的上位机中预先生成操作臂的运动轨迹。医护人员后续在控制台上可按照操作臂的运动轨迹来控制操作臂102的运动。具体地，上位机可先获取软式内镜的预设剩余长度，并以输送臂末端工具中心点为端点，按照弧长为预设剩余长度来拟合目标圆弧。其中，输送臂末端工具中心点在该目标圆弧上的切线方向为输送臂的输送方向，由此可获得唯一的目标圆弧。进而，将确定的目标圆弧的另一端点确定为操作臂轨迹分割点，并确定操作臂轨迹分割点对应的软式内镜剩余长度为预设剩余长度。进而，基于目标圆弧提取圆弧插补点，并获取每个圆弧插补点对应的软式内镜剩余长度。另外，在操作臂轨迹分割点与操作臂的初始位置点之间拟合目标直线，并从目标直线中提取直线插补点，获取每个直线插补点对应的软式内镜剩余长度。其中，初始位置点对应的软式内镜剩余长度为软式内镜的总长度。则，由初始位置点、直线插补点、操作臂轨迹分割点、圆弧插补点以及输送臂末端工具中心点，可组成操作臂的各个轨迹点，操作臂的轨迹点通过操作臂位姿进行描述，各个轨迹点处的操作臂位姿均是确定的。基于此，后续在软式内镜的输送过程中，便可根据软式内镜剩余长度确定对应的操作臂轨迹点。

为了便于理解本申请，下面结合附图对本申请实施例提供的一种机械臂轨迹规划方法进行说明。

参见图2所示，该图为本申请实施例提供的一种机械臂轨迹规划方法的流程图。该方法可由终端设备或服务器执行，这里不进行限定。例如，可由控制台的上位机实现。如图2所示，该方法可以包括S201-S205：

S201：获取软式内镜的预设剩余长度。

在软式内镜操作机器人系统下，输送装置输送软式内镜通过自然腔道进入人体。送镜过程中，输送臂会保持输送位置停止，输送臂末端的输送装置输送软式内镜。操作臂末端支撑、固定软式内镜，操作软式内镜弯曲度弯曲，操作臂随软式内镜剩余长度进行位置跟随。剩余长度为输送装置中输送软式内镜的入口与操作装置中操作部之间的软式内镜的长度。在本申请实施例中，机械臂轨迹规划即操作臂轨迹规划。在送镜前预先进行操作臂的轨迹规划。

可以理解的是，本申请实施例考虑到软式内镜本体长度不同，初始设定位置软式内镜都有部分冗余，则为了使得软式内镜的输送效果更好，可在初始阶段的软式内镜输送过程中要求操作臂快速粗跟随，跟随精度可较低，以提高软式内镜的输送效率。操作臂快速跟随的阶段可称为快速位置跟随段。当软式内镜的输送剩余长度较短后，可控制操作臂进行精准跟随，以使得软式内镜保持柔顺状态，便于输送装置输送和长度检测。操作臂精准跟随的阶段可称为精确位置跟随段。因此，可在综合考虑操作臂的运行效率、柔顺度、位置准确度等方面的基础上，来规划操作臂的送镜轨迹。

基于上述内容，为了划分操作臂轨迹的快速位置跟随段和精确位置跟随段，先确定软式内镜的预设剩余长度，预设剩余长度小于软式内镜的总长度。预设剩余长度为预先设置的，例如，可为500mm。预设剩余长度用于划分操作臂的快速位置跟随段和精确位置跟随段。可以理解为，在将软式内镜从总长度输送到预设剩余长度之间，软式内镜的剩余长度较长，此时可控制操作臂进行快速跟随。在将软式内镜从预设剩余长度输送到结束之间，软式内镜的剩余长度较短，此时可控制操作臂进行精准跟随。

S202：以输送臂末端工具中心点为端点，按照弧长为预设剩余长度拟合目标圆弧，将目标圆弧的另一端点确定为操作臂轨迹分割点；输送臂末端工具中心点在目标圆弧上的切线方向为输送臂的输送方向；操作臂轨迹分割点对应的软式内镜剩余长度为预设剩余长度。

操作臂轨迹的快速位置跟随段和精确位置跟随段均由操作臂的轨迹点构成。操作臂的轨迹点可以理解为在操作臂运送软式内镜的过程中，操作臂末端的工具中心点的轨迹点。工具中心点可表示为TCP。在进行操作臂轨迹规划时，可先规划精确位置跟随段的轨迹点。

参见图3，图3为本申请实施例提供的一种软式内镜输送状态的示意图。如图3所示，在软式内镜的输送过程中，软式内镜通常存在冗余部分。在精确位置跟随段，需要消除软式内镜的冗余，使软式内镜处于柔顺状态，进行精准跟随，以便输送装置进行软式内镜的输送。

为了实现上述目的，在确定预设剩余长度后，以输送臂末端工具中心点为端点，按照弧长为预设剩余长度拟合目标圆弧，且要求输送臂末端工具中心点在目标圆弧上的切线方向为输送臂的输送方向。输送臂的输送方向即向自然腔道输送软式内镜的方向。由此，能够确定唯一的目标圆弧。目标圆弧应符合软式内镜的运送轨迹，目标圆弧用于拟合当软式内镜的剩余长度为预设剩余长度时的软式内镜。

进而，将目标圆弧的另一端点确定为操作臂轨迹分割点，可表示为P1。操作臂轨迹分割点为操作臂轨迹中的一个轨迹点，该轨迹点为划分快速位置跟随段和精确位置跟随段的轨迹点。由目标圆弧的拟合过程可知，操作臂轨迹分割点对应的软式内镜剩余长度为预设剩余长度。则，要求在软式内镜的剩余长度大于预设剩余长度时，控制操作臂快速跟随，以快速跟随到操作臂轨迹分割点。在软式内镜的剩余长度为预设剩余长度时，要求操作臂到达操作臂轨迹分割点，以使得软式内镜为柔顺状态，消除软式内镜冗余。在软式内镜的剩余长度小于预设剩余长度时，要求控制操作臂进行精准跟随。

参见图4a和图4b，图4a为本申请实施例提供的一种软式内镜达到预设剩余长度的示意图，图4b为本申请实施例提供的另一种软式内镜达到预设剩余长度的示意图。如图4a所示，当输送机构（即输送装置）和跟随部（即操作装置）之间的软式内镜的剩余程度达到预设剩余长度时，期望的软式内镜的状态如图4a所示，即软式内镜的状态为柔顺的，无冗余的。如图4b所示，图4b中的软式内镜与输送臂的输送方向的相切。本步骤中拟合的目标圆弧便可理解为用于拟合图4b中的软式内镜。图4a中的跟随部的位置（具体为跟随臂末端工具中心点）便为操作臂轨迹分割点。从图4b可知，当操作臂位于操作臂轨迹分割点时，操作臂姿态方向也与软式内镜相切，也可理解为操作臂姿态方向与拟合软式内镜的目标圆弧相切。

操作臂的轨迹点包括操作臂轨迹分割点。本申请实施例获取的操作臂的各个轨迹点均通过操作臂的位姿表示。即，每个轨迹点的位姿均是确定的。位姿包括空间位置P和姿态R。姿态包括俯仰角、偏转角和旋转角。操作臂的位姿可构成位姿矩阵。位姿矩阵为4×4的矩阵，具体为[R P；0 1]。其中，R为3×3的旋转矩阵，P为3维的平移矩阵，由空间位置（x,y,z）组成。

其中，旋转角表示操作臂绕自身轴线旋转的角度。由于操作臂的旋转角并不影响操作臂轨迹点的确定，则在确定操作臂轨迹点的过程中，可默认操作臂的旋转角为零度。如图4a所示，由于跟随部在运动时，通常朝输送机构的方向运动，则操作臂的俯仰角为跟随部的运动方向（也即姿态方向）与竖直方向的夹角。当俯仰角为90度时，跟随部和输送机构平行。偏转角为跟随部左右转动的角度。

基于此，本申请实施例提供了一种获取操作臂轨迹分割点的位姿的详细步骤，包括A1-A4：

A1：获取目标圆弧对应的弦与输送臂的输送方向之间的第一夹角，利用第一夹角计算操作臂的俯仰角。

如图4a所示，由于目标圆弧用于拟合操作臂到达操作臂轨迹分割点时柔顺状态下的软式内镜，因此，目标圆弧对应的弦可以理解为输送机构工具中心点（即输送臂末端工具中心点）与跟随部工具中心点（即操作臂末端工具中心点）之间的连线。

如图4a所示，输送臂的输送方向为输送软式内镜的方向。第一夹角即目标圆弧对应的弦与输送臂的输送方向之间的夹角，该夹角为锐角，可表示为φ。在实际应用中，可通过测量获得第一夹角的角度，这里不限定测量方法。

进而，可计算跟随部自身轴线与输送臂的输送方向之间的夹角，该夹角也为锐角，可表示为第二夹角。根据圆弧以及圆弧切线的特性可知，第二夹角为2φ。据此，可计算操作臂的俯仰角，表示为90度-2φ。

可知，俯仰角越大，跟随部处的软式内镜的斜率越大，软式内镜越陡，软式内镜越不柔顺。为了控制俯仰角不过于大，在本申请实施例中，可限制第一夹角最小为20度，以此来限制操作臂的俯仰角。另外，由于在输送软式内镜的过程中，输送臂的位置不变，操作臂随软式内镜剩余长度进行位置跟随，则操作臂的偏转角通常较小。基于此，还可限制偏转角为0度附近。

A2：利用预设剩余长度的目标圆弧与第一夹角，计算目标圆弧对应的弦的弦长。

在获知第一夹角后，便可计算得到目标圆弧对应的圆心角为180度-2（90度-φ）。由此，可基于圆心角和目标圆弧的弧长（即预设剩余长度）计算圆半径。弦长公式为2×圆半径×sin(圆心角/2)，由此，可计算得到目标圆弧对应的弦的弦长。

A3：利用弦长、第一夹角以及操作臂的偏转角，获取弦长在操作臂末端工具坐标系中的空间位置分量。

由于目标圆弧对应的弦在空间中，因此会在操作臂末端工具坐标系中存在控件位置分量，即在操作臂末端工具坐标系的x轴上的分量、y轴上的分量以及z轴上的分量。该分量由弦和弦分别与操作臂末端工具坐标系的x轴、y轴、z轴之间的夹角获得。

A4：基于输送臂末端工具中心点的位姿以及弦长在操作臂末端工具坐标系中的空间位置分量，获取操作臂轨迹分割点的空间位置，将操作臂轨迹分割点的空间位置、操作臂的俯仰角和操作臂的偏转角确定为操作臂轨迹分割点的位姿。

输送臂末端工具中心点的位姿可通过机械臂的机器运动学的方式进行获取。例如，在获取输送臂和输臂之间连杆长度等参数后，利用机械臂的机器运动学原理便可计算得到输送臂末端工具中心点的位姿。进而，便可结合于输送臂末端工具中心点的位姿以及弦长在操作臂末端工具坐标系中的空间位置分量，获取操作臂轨迹分割点的空间位置。

可知，操作臂的旋转角可设置为0。由此，将操作臂轨迹分割点的空间位置、操作臂的俯仰角和操作臂的偏转角组成操作臂轨迹分割点的位姿。

基于A1-A4的相关内容可知，由输送臂末端工具中心点的位姿以及目标圆弧的弧长可计算得到操作臂轨迹分割点的位姿。可以理解的是，在确定目标圆弧的弧长（即预设剩余长度）后，操作臂轨迹分割点的位姿只与输送机构的位置相关。输送机构变更位置后，输送臂末端工具中心点的位姿变化，则操作臂轨迹分割点的位姿变化。

S203：基于目标圆弧提取圆弧插补点，获取每个圆弧插补点对应的软式内镜剩余长度。

在确定操作臂轨迹分割点后，便将操作臂的轨迹点划分为了快速位置跟随段的轨迹点和精确位置跟随段的轨迹点。

由于对精确位置跟随段的轨迹点的要求度较高，则在输送臂末端工具中心点和操作臂轨迹分割点中间设置圆弧插补点，以使得后续输送臂按照圆弧插补点运动时，操作臂以圆弧的顺滑方式经过圆弧插补点，且使得软式内镜一直处于柔顺状态。

示例地，基于目标圆弧提取圆弧插补点，包括：

B1：按照每间隔第一长度对目标圆弧进行分割，基于输送臂末端工具中心点的位姿、操作臂轨迹分割点的位姿以及分割比例，计算圆弧分割点的位姿。

例如，第一长度为10mm，则每间隔10mm获取一个操作臂轨迹分割点。当预设剩余长度为500mm时，能够通过分割目标圆弧获得50个操作臂轨迹分割点。进而，可根据输送臂末端工具中心点的位姿、操作臂轨迹分割点的位姿以及分割比例，计算每个圆弧分割点的位姿。

位姿包括空间位置和姿态。例如，输送臂末端工具中心点的位姿包括空间位置(x1,y1,z1)、俯仰角1、偏转角1，操作臂轨迹分割点的位姿包括空间位置(x2,y2,z2)、俯仰角2、偏转角2。由于将目标圆弧进行了50份的分割，则对应地，将(x1-x2)、（y1-y2）以及（z1-z2）均划分为50份，将（俯仰角1-俯仰角2）、（偏转角1-偏转角2）也均划分为50份。若其中一个操作臂轨迹分割点的分割比例为1/50，则按照该分割比例，可确定其对应的空间位置、俯仰角和偏转角，这实现了每间隔10mm获取一组操作臂的位姿信息。在本申请实施例中，由于旋转角对确定操作臂的轨迹不产生影响，因此不对旋转角进行插补，可默认旋转角均为0。

B2：将圆弧分割点确定为圆弧插补点。

在获取各个圆弧分割点后，将每个圆弧分割点均确定为圆弧插补点。

基于上述B1-B2可知，在确定圆弧插补点的过程中，不仅进行了空间位置的插补，还进行了姿态（俯仰角、偏转角）的插补，完成了位姿的插补。这使得精确位置跟随段的轨迹点是准确的。

基于上述内容，为了增加圆弧插补点的数量，使得操作臂在精确位置跟随段中经过轨迹点时更加顺滑，在一种可能的实现方式中，还可包括如下步骤：

C1：基于圆弧插补点的位姿，生成圆弧插补点的位姿矩阵。

可知，圆弧插补点的位姿矩阵为4×4的矩阵，均可表示为SE(3)=[R P；0 1]。

C2：通过李代数运算将圆弧插补点的位姿矩阵转换成六维旋量坐标组，获取圆弧插补点的六维旋量坐标组。

通过李代数运算，可计算圆弧插补点的位姿矩阵的对数映射，获取对应的六维旋量坐标组。具体地，计算旋转矩阵R的对数映射，获得三维旋转向量w，w=log(R)。同时，计算三维平移向量v=R^-1·P。由三维旋转向量和三维平移向量组成六维旋量坐标组。

则，当圆弧插补点为50个时，转换得到的六维旋量坐标组的数量也为50个。

C3：以圆弧插补点的六维旋量坐标组为控制点进行二次B样条曲线插补，获取拟合的贝塞斯曲线；贝塞斯曲线的表达式中包括插值参数；贝塞斯曲线与操作臂的运动方向以及输送臂的输送方向相切。

获取圆弧插补点的六维旋量坐标组后，以圆弧插补点的六维旋量坐标组为控制点进行二次B样条曲线插补，拟合贝塞斯曲线。获得的贝塞斯曲线与操作臂的运动方向以及输送臂的输送方向相切，由此可使得获得的贝塞斯曲线的点也可看作是圆弧中的点。

在获得贝塞斯曲线后，便可根据确定的插值参数，从贝塞斯曲线中进行任一点的取样。插值参数的取值范围为[0,1]。例如，当插值参数为0和1时，可获得贝塞斯曲线中的两个端点的位姿；当插值参数为0.5时，便可获得贝塞斯曲线一半位置处的点的位置。

C4：利用插值参数对贝塞斯曲线进行曲线点取样，重新获取各个圆弧插补点。

可以理解的是，取样的点的数量通常大于前述实施例中获取的圆弧插补点的数量。由此，可获取数量更多的插补点，这些插补点也可称为圆弧插补点。

基于C1-C4可知，获取到精确位置跟随段的轨迹点后，可通过李代数运算将所有轨迹点的位姿矩阵转化为六维旋量坐标组，并对六维旋量坐标组进行二次B样条曲线插补，获得到柔顺的贝塞斯曲线。进而，对贝塞斯曲线中的点进行取样，获取精确位置跟随段中更多的插补点。如此，可精准拟合操作臂在精确位置跟随段中的空间位姿，让操作臂以准确的位置姿态到达每个轨迹点，实现精准位置控制。

参见图5，图5为本申请实施例提供的一种精确位置跟随段的轨迹点规划的示意图。如图5可知，在确定输送臂末端工具中心点以及操作臂轨迹分割点之后，在输送臂末端工具中心点以及操作臂轨迹分割点之间进行轨迹点插补。将输送臂末端工具中心点、操作臂轨迹分割点以及圆弧插补点确定为精确位置跟随段的各个轨迹点。

在确定圆弧插补点后，还可获取每个圆弧插补点对应的软式内镜剩余长度。如此，在后续在送镜过程中，可按照软式内镜的剩余长度确定对应的轨迹点。

具体地，获取每个圆弧插补点对应的软式内镜剩余长度，包括：

按照每个圆弧插补点在目标圆弧中的分割比例以及预设剩余长度，获取每个圆弧插补点对应的软式内镜剩余长度。

目标圆弧的长度为预设剩余长度。按照圆弧插补点的位置，确定圆弧插补点在目标圆弧中的分割比例，将该分割比例乘以预设剩余长度，便可获得该圆弧插补点对应的圆弧长度。示例地，若从跟随部向输送机构的方向进行分割，则圆弧插补点对应的圆弧长度为跟随部到圆弧插补点之间的圆弧长度。进而，由圆弧插补点对应的圆弧长度，便可获得圆弧插补点对应的软式内镜剩余长度。示例地，若从跟随部向输送机构的方向进行分割，圆弧插补点对应的软式内镜剩余长度为预设剩余长度减去圆弧插补点对应的圆弧长度。

例如，预设剩余长度为500mm。若圆弧插补点在目标圆弧中的分割比例为1/50（以从跟随部向输送机构的方向为例进行分割），则该圆弧插补点对应的圆弧长度为10mm，则该圆弧插补点对应的软式内镜剩余长度为490mm。若圆弧插补点在目标圆弧中的分割比例为49/50（以从跟随部向输送机构的方向为例进行分割），则该圆弧插补点对应的圆弧长度为490mm，则该圆弧插补点对应的软式内镜剩余长度为10mm。

在确定精确位置跟随段的各个轨迹点以及每个轨迹点对应的软式内镜剩余长度后，后续在输送装置输送软式内镜的过程中，输送臂位置不变，通过软式内镜剩余长度匹配操作臂在精确位置跟随段的轨迹点，控制操作臂到达对应的轨迹点进行位置跟随。基于上述内容可知，操作臂位于精确位置跟随段的轨迹点时，会使得软式内镜持续与输送臂的输送方向和操作臂的运动方向（姿态方向）相切，这会使得软式内镜持续处于柔顺状态，无冗余部分。

S204：在操作臂轨迹分割点与操作臂的初始位置点之间拟合目标直线，从目标直线中提取直线插补点，获取每个直线插补点对应的软式内镜剩余长度；初始位置点对应的软式内镜剩余长度为软式内镜的总长度。

快速位置跟随段可由操作臂轨迹分割点、操作臂的初始位置点以及操作臂轨迹分割点与操作臂的初始位置点之间的插补点组成。其中，操作臂的初始位置点的位姿也可由机器运动学原理获得，这里不再详述。

在快速位置跟随段中，对轨迹要求的准确度较低。因此，可采用直线插补的方式，获取操作臂轨迹分割点与操作臂的初始位置点之间的插补点。

具体地，在操作臂轨迹分割点与操作臂的初始位置点之间拟合目标直线，从目标直线中提取直线插补点。

示例地，从目标直线中提取直线插补点，包括：

D1：按照每间隔第二长度对目标直线进行分割，基于操作臂轨迹分割点的空间位置、操作臂初始位置点的空间位置以及分割比例，计算直线分割点的空间位置。

例如，第二长度为10mm。则在目标直线中，每间隔10mm进行目标直线的分割，确定一个直线分割点。

在进行直线插补时，只获取直线分割点的空间位置即可，可不用获取直线分割点的俯仰角和偏转角。具体地，基于操作臂轨迹分割点的空间位置、操作臂初始位置点的空间位置以及分割比例，计算直线分割点的空间位置。可知，获取直线分割点的空间位置的过程类似于获取圆弧插补点的空间位置的过程，这里不再赘述，可参考上述实施例。

D2：将直线分割点确定为直线插补点。

在确定直线分割点后，将每个直线分割点均确定为直线插补点。基于上述内容，也可理解为，在操作臂轨迹分割点与操作臂的初始位置点之间，每隔第二长度（例如10mm）进行一次直线插补，获取一个直线插补点。

基于D1-D2可知，在快速位置跟随段，可采用直线插补的方式，确定操作臂轨迹分割点与操作臂的初始位置点之间的轨迹点。快速位置跟随段的轨迹点包括操作臂的初始位置点和直线插补点。直线插补属于线性插补，则操作臂由初始位置点到操作臂轨迹分割点之间按照直线插补点运行时的效率更高，即以直线运动的方式可节约操作臂运动时间，使得操作臂能够更快速地到达操作臂轨迹分割点。

参见图6，图6为本申请实施例提供的一种快速位置跟随段的轨迹点规划的示意图。如图6所示，操作臂轨迹分割点表示为P1，操作臂的初始位置点表示为P0。操作臂轨迹分割点和操作臂的初始位置点之间的轨迹点为直线插补点。

在确定每个直线插补点后，还需获取每个直线插补点对应的软式内镜剩余长度。在运镜过程中，可根据软式内镜剩余长度，确定对应的直线插补点。

示例地，获取每个直线插补点对应的软式内镜剩余长度，包括：

计算软式内镜的总长度与预设剩余长度的差值长度；

按照每个直线插补点在目标直线中的分割比例以及差值长度，获取每个直线插补点对应的软式内镜剩余长度。

其中，操作臂将软式内镜从总长度输送到预设剩余长度时，软式内镜已经输送的长度为差值长度。在操作臂位于操作臂轨迹分割点以及按照精准位置跟随段的轨迹点运行之前，软式内镜需要被输送的长度为差值长度。即，在快速位置跟随段结束时，需要输送软式内镜的长度为差值长度。

基于此，可按照每个直线插补点在目标直线中的分割比例以及差值长度，获取每个直线插补点对应的软式内镜剩余长度。例如，可按照分割比例的比例关系，确定到达每个直线插补点时需要输送的软式内镜的长度，由此可确定对应的剩余长度。即，差值长度减去到达直线插补点时需要输送的软式内镜的长度即为该直线插补点对应的剩余长度。

S205：由初始位置点、直线插补点、操作臂轨迹分割点、圆弧插补点以及输送臂末端工具中心点，组成操作臂的各个轨迹点；操作臂的轨迹点通过操作臂的位姿进行描述，在软式内镜的输送过程中，操作臂的轨迹点由对应的软式内镜剩余长度进行匹配。

操作臂先经过快速位置跟随段，再经过精准位置跟随段。按照此顺序，可将精准位置跟随段中的各个轨迹点（操作臂轨迹分割点、圆弧插补点以及输送臂末端工具中心点）和快速位置跟随段中的各个轨迹点（初始位置点、直线插补点）进行拼接，获得操作臂的各个轨迹点，由此获得关于软式内镜剩余长度对应的操作臂位置跟随轨迹。后续，医护人员便可按照轨迹点控制操作臂以快速、精确的方式进行运动跟随。

如此，能够提高软式内镜的输送效果。另外，无需医护人员自行确定轨迹点，可降低医护人员的操作成本。而且，在输送软式内镜的过程中，每个操作臂轨迹点均根据软式内镜的剩余长度确定，确保了操作臂的定位准确，在一定程度上减缓了送镜过快或过慢的情况，尽可能避免了送镜过快或过慢导致的损坏内镜或系统执行机构的情况的发生。

参见图7，图7为本申请实施例提供的一种操作臂的轨迹点的示意图。如图7所示，操作臂的轨迹点由初始位置点P0、直线插补点、操作臂轨迹分割点P1、圆弧插补点以及输送臂末端工具中心点组成。

在一种可能的实现方式中，本申请实施例提供的机械臂轨迹规划方法还包括如下步骤：

E1：基于操作臂各关节的限位角度信息，验证操作臂的各个轨迹点是否可达。

在实际应用中，操作臂中的各关节均有限位角度，关节的活动角度不能超过限位角度。在获取操作臂的轨迹点后，可基于操作臂各关节的限位角度信息，验证操作臂的各个轨迹点是否可达。

E2：若各个轨迹点中的第一目标轨迹点不可达，调整第一目标轨迹点的俯仰角和/或偏转角。

通过验证操作臂各关节是否可达，来判断对应的轨迹点是否可用。若不可用，可通过调整操作臂姿态中的俯仰角和/或偏转角以匹配到可用的运动姿态。

例如，第一目标轨迹点不可达，则需要调整第一目标轨迹点的姿态。其中，第一目标轨迹点用位姿进行描述，位姿中包括姿态，即俯仰角和偏转角。第一目标轨迹点的俯仰角即用于描述第一目标轨迹点的位姿中的俯仰角。第一目标轨迹点的偏转角即用于描述第一目标轨迹点的位姿中的偏转角。

具体地，第一目标轨迹点的俯仰角与第一夹角相关，可根据第一夹角的限制情况来限制俯仰角的调整。第一目标轨迹点的偏转角通常限定在0度附近，则可按照此限制来调整第一目标轨迹点的偏转角。

在确定操作臂的轨迹点以及确定操作臂的每个轨迹点均可达时，便可在运镜过程中使用操作臂的轨迹点进行操作臂的辅助控制。

基于此，在一种可能的实现方式中，使用操作臂的轨迹点进行操作臂的辅助控制，包括如下步骤：

F1：确定软式内镜剩余长度为目标剩余长度时的目标剩余长度对应的第二目标轨迹点；

F2：按照第二目标轨迹点的位姿调整操作臂，以使软式内镜在到达第二目标轨迹点时的剩余长度为目标剩余长度。

在实际应用中，可由安装在输送装置中的传感器测量软式内镜被输送的长度，以此来计算软式内镜剩余长度。进而，将软式内镜剩余长度传到控制台上，控制台会根据操作臂的轨迹点和该软式内镜剩余长度，确定第二目标轨迹点。进而，医护人员可按照第二目标轨迹点的位姿控制操作臂到达第二目标轨迹点，此时，可使得软式内镜在到达第二目标轨迹点时的剩余长度为目标剩余长度。若操作臂运行速度过快，使得当操作臂到达第二目标轨迹点时的软式内镜的剩余长度不为目标剩余长度，则可等待输送装置输送软式内镜。当软式内镜的剩余长度为目标剩余长度时，再进行操作臂的下一轨迹点的控制。

基于上述内容可知，基于操作臂的轨迹点，可使得操作臂保持前段粗跟随、后段精准跟随的状态进行运行，能够保持软式内镜无大幅度抖动。在后段精准跟随的过程中，还可使软式内镜以顺滑、推拉力小且始终与两臂的运动方向相切的状态运动。

本领域技术人员可以理解，在具体实施方式的上述方法中，各步骤的撰写顺序并不意味着严格的执行顺序而对实施过程构成任何限定，各步骤的具体执行顺序应当以其功能和可能的内在逻辑确定。

基于上述方法实施例提供的一种机械臂轨迹规划方法，本申请实施例还提供了一种机械臂轨迹规划装置，下面将结合附图对机械臂轨迹规划装置进行说明。由于本公开实施例中的装置解决问题的原理与本申请实施例上述机械臂轨迹规划方法相似，因此装置的实施可以参见方法的实施，重复之处不再赘述。

参见图8所示，该图为本申请实施例提供的一种机械臂轨迹规划装置的结构示意图。该机械臂轨迹规划装置可应用于软式内镜操作机器人系统。软式内镜操作机器人系统包括输送臂和操作臂。机械臂轨迹规划装置800包括：

第一获取单元801，用于获取软式内镜的预设剩余长度；

第一确定单元802，用于以输送臂末端工具中心点为端点，按照弧长为所述预设剩余长度拟合目标圆弧，将所述目标圆弧的另一端点确定为操作臂轨迹分割点；所述输送臂末端工具中心点在目标圆弧上的切线方向为所述输送臂的输送方向；所述操作臂轨迹分割点对应的软式内镜剩余长度为所述预设剩余长度；

第一提取单元803，用于基于所述目标圆弧提取圆弧插补点，获取每个所述圆弧插补点对应的软式内镜剩余长度；

第二提取单元804，用于在所述操作臂轨迹分割点与所述操作臂的初始位置点之间拟合目标直线，从所述目标直线中提取直线插补点，获取每个直线插补点对应的软式内镜剩余长度；所述初始位置点对应的软式内镜剩余长度为所述软式内镜的总长度；

组成单元805，用于由所述初始位置点、所述直线插补点、所述操作臂轨迹分割点、所述圆弧插补点以及所述输送臂末端工具中心点，组成所述操作臂的各个轨迹点；所述操作臂的轨迹点通过所述操作臂的位姿进行描述，在软式内镜的输送过程中，所述操作臂的轨迹点由对应的软式内镜剩余长度进行匹配。

在一种可能的实现方式中，所述方法还包括：

第二获取单元，用于获取操作臂轨迹分割点的位姿的获取过程。

第二获取单元，包括：

第一计算子单元，用于获取所述目标圆弧对应的弦与所述输送臂的输送方向之间的第一夹角，利用所述第一夹角计算所述操作臂的俯仰角；

第二计算子单元，用于利用所述预设剩余长度的目标圆弧与所述第一夹角，计算所述目标圆弧对应的弦的弦长；

第一获取子单元，用于利用所述弦长、所述第一夹角以及所述操作臂的偏转角，获取所述弦长在操作臂末端工具坐标系中的空间位置分量；

第二获取子单元，用于基于所述输送臂末端工具中心点的位姿以及所述弦长在操作臂末端工具坐标系中的空间位置分量，获取所述操作臂轨迹分割点的空间位置，将所述操作臂轨迹分割点的空间位置、所述操作臂的俯仰角和所述操作臂的偏转角确定为操作臂轨迹分割点的位姿。

在一种可能的实现方式中，所述第一提取单元803，包括：

第三计算子单元，用于利用按照每间隔第一长度对所述目标圆弧进行分割，基于所述输送臂末端工具中心点的位姿、所述操作臂轨迹分割点的位姿以及分割比例，计算圆弧分割点的位姿；

第一确定子单元，用于将所述圆弧分割点确定为圆弧插补点。

在一种可能的实现方式中，所述装置还包括：

生成单元，用于基于所述圆弧插补点的位姿，生成所述圆弧插补点的位姿矩阵；

第三获取单元，用于通过李代数运算将所述圆弧插补点的位姿矩阵转换成六维旋量坐标组，获取所述圆弧插补点的六维旋量坐标组；

第四获取单元，用于以所述圆弧插补点的六维旋量坐标组为控制点进行二次B样条曲线插补，获取拟合的贝塞斯曲线；所述贝塞斯曲线的表达式中包括插值参数；所述贝塞斯曲线与所述操作臂的运动方向以及所述输送臂的输送方向相切；

第五获取单元，用于利用所述插值参数对所述贝塞斯曲线进行曲线点取样，重新获取各个圆弧插补点。

在一种可能的实现方式中，所述第二提取单元804，包括：

第四计算子单元，用于按照每间隔第二长度对所述目标直线进行分割，基于所述操作臂轨迹分割点的空间位置、所述操作臂初始位置点的空间位置以及分割比例，计算直线分割点的空间位置；

第二确定子单元，用于将所述直线分割点确定为直线插补点。

在一种可能的实现方式中，所述获取每个所述圆弧插补点对应的软式内镜剩余长度，包括：

按照每个所述圆弧插补点在所述目标圆弧中的分割比例以及所述预设剩余长度，获取每个所述圆弧插补点对应的软式内镜剩余长度；

所述获取每个直线插补点对应的软式内镜剩余长度，包括：

计算所述软式内镜的总长度与所述预设剩余长度的差值长度；

按照每个所述直线插补点在所述目标直线中的分割比例以及所述差值长度，获取每个所述直线插补点对应的软式内镜剩余长度。

在一种可能的实现方式中，所述装置还包括：

验证单元，用于基于所述操作臂各关节的限位角度信息，验证所述操作臂的各个轨迹点是否可达；

第一调整单元，用于若所述各个轨迹点中的第一目标轨迹点不可达，调整所述第一目标轨迹点的俯仰角和/或偏转角。

在一种可能的实现方式中，所述装置还包括：

第二确定单元，用于确定所述软式内镜剩余长度为目标剩余长度时的所述目标剩余长度对应的第二目标轨迹点；

第二调整单元，用于按照所述第二目标轨迹点的位姿调整所述操作臂，以使所述软式内镜在到达所述第二目标轨迹点时的剩余长度为所述目标剩余长度。

需要说明的是，本实施例中各个单元的具体实现可以参见上述方法实施例中的相关描述。本申请实施例中对单元的划分是示意性的，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式。本申请实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

通过以上的实施方式的描述可知，本领域的技术人员可以清楚地了解到上述实施例方法中的全部或部分步骤可借助软件加必需的通用硬件平台的方式来实现。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备（可以是个人计算机，服务器，或者诸如媒体网关等网络通信设备，等等）执行本申请各个实施例或者实施例的某些部分所述的方法。

需要说明的是，本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的方法而言，由于其与实施例公开的系统相对应，所以描述的比较简单，相关之处参见系统部分说明即可。

还需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本申请。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本申请的精神或范围的情况下，在其它实施例中实现。因此，本申请将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (11)

1.一种机械臂轨迹规划方法，其特征在于，应用于软式内镜操作机器人系统，所述软式内镜操作机器人系统包括输送臂和操作臂；所述方法包括：

获取软式内镜的预设剩余长度；

以输送臂末端工具中心点为端点，按照弧长为所述预设剩余长度拟合目标圆弧，将所述目标圆弧的另一端点确定为操作臂轨迹分割点；所述输送臂末端工具中心点在目标圆弧上的切线方向为所述输送臂的输送方向；所述操作臂轨迹分割点对应的软式内镜剩余长度为所述预设剩余长度；

基于所述目标圆弧提取圆弧插补点，获取每个所述圆弧插补点对应的软式内镜剩余长度；

在所述操作臂轨迹分割点与所述操作臂的初始位置点之间拟合目标直线，从所述目标直线中提取直线插补点，获取每个直线插补点对应的软式内镜剩余长度；所述初始位置点对应的软式内镜剩余长度为所述软式内镜的总长度；

由所述初始位置点、所述直线插补点、所述操作臂轨迹分割点、所述圆弧插补点以及所述输送臂末端工具中心点，组成所述操作臂的各个轨迹点；所述操作臂的轨迹点通过所述操作臂的位姿进行描述，在软式内镜的输送过程中，所述操作臂的轨迹点由对应的软式内镜剩余长度进行匹配。

2.根据权利要求1所述的方法，其特征在于，所述操作臂轨迹分割点的位姿的获取过程，包括：

获取所述目标圆弧对应的弦与所述输送臂的输送方向之间的第一夹角，利用所述第一夹角计算所述操作臂的俯仰角；

利用所述预设剩余长度的目标圆弧与所述第一夹角，计算所述目标圆弧对应的弦的弦长；

利用所述弦长、所述第一夹角以及所述操作臂的偏转角，获取所述弦长在操作臂末端工具坐标系中的空间位置分量；

基于所述输送臂末端工具中心点的位姿以及所述弦长在操作臂末端工具坐标系中的空间位置分量，获取所述操作臂轨迹分割点的空间位置，将所述操作臂轨迹分割点的空间位置、所述操作臂的俯仰角和所述操作臂的偏转角确定为操作臂轨迹分割点的位姿。

3.根据权利要求1所述的方法，其特征在于，所述基于所述目标圆弧提取圆弧插补点，包括：

按照每间隔第一长度对所述目标圆弧进行分割，基于所述输送臂末端工具中心点的位姿、所述操作臂轨迹分割点的位姿以及分割比例，计算圆弧分割点的位姿；

将所述圆弧分割点确定为圆弧插补点。

4.根据权利要求1-3任一项所述的方法，其特征在于，所述方法还包括：

基于所述圆弧插补点的位姿，生成所述圆弧插补点的位姿矩阵；

通过李代数运算将所述圆弧插补点的位姿矩阵转换成六维旋量坐标组，获取所述圆弧插补点的六维旋量坐标组；

以所述圆弧插补点的六维旋量坐标组为控制点进行二次B样条曲线插补，获取拟合的贝塞斯曲线；所述贝塞斯曲线的表达式中包括插值参数；所述贝塞斯曲线与所述操作臂的运动方向以及所述输送臂的输送方向相切；

利用所述插值参数对所述贝塞斯曲线进行曲线点取样，重新获取各个圆弧插补点。

5.根据权利要求1所述的方法，其特征在于，所述从所述目标直线中提取直线插补点，包括：

按照每间隔第二长度对所述目标直线进行分割，基于所述操作臂轨迹分割点的空间位置、所述操作臂初始位置点的空间位置以及分割比例，计算直线分割点的空间位置；

将所述直线分割点确定为直线插补点。

6.根据权利要求1所述的方法，其特征在于，所述获取每个所述圆弧插补点对应的软式内镜剩余长度，包括：

按照每个所述圆弧插补点在所述目标圆弧中的分割比例以及所述预设剩余长度，获取每个所述圆弧插补点对应的软式内镜剩余长度；

所述获取每个直线插补点对应的软式内镜剩余长度，包括：

计算所述软式内镜的总长度与所述预设剩余长度的差值长度；

按照每个所述直线插补点在所述目标直线中的分割比例以及所述差值长度，获取每个所述直线插补点对应的软式内镜剩余长度。

7.根据权利要求1所述的方法，其特征在于，所述方法还包括：

基于所述操作臂各关节的限位角度信息，验证所述操作臂的各个轨迹点是否可达；

若所述各个轨迹点中的第一目标轨迹点不可达，调整所述第一目标轨迹点的俯仰角和/或偏转角。

8.根据权利要求1所述的方法，其特征在于，所述方法还包括：

确定所述软式内镜剩余长度为目标剩余长度时的所述目标剩余长度对应的第二目标轨迹点；

按照所述第二目标轨迹点的位姿调整所述操作臂，以使所述软式内镜在到达所述第二目标轨迹点时的剩余长度为所述目标剩余长度。

9.一种机械臂轨迹规划装置，其特征在于，应用于软式内镜操作机器人系统，所述软式内镜操作机器人系统包括输送臂和操作臂；所述装置包括：

第一获取单元，用于获取软式内镜的预设剩余长度；

第一确定单元，用于以输送臂末端工具中心点为端点，按照弧长为所述预设剩余长度拟合目标圆弧，将所述目标圆弧的另一端点确定为操作臂轨迹分割点；所述输送臂末端工具中心点在目标圆弧上的切线方向为所述输送臂的输送方向；所述操作臂轨迹分割点对应的软式内镜剩余长度为所述预设剩余长度；

第一提取单元，用于基于所述目标圆弧提取圆弧插补点，获取每个所述圆弧插补点对应的软式内镜剩余长度；

第二提取单元，用于在所述操作臂轨迹分割点与所述操作臂的初始位置点之间拟合目标直线，从所述目标直线中提取直线插补点，获取每个直线插补点对应的软式内镜剩余长度；所述初始位置点对应的软式内镜剩余长度为所述软式内镜的总长度；

组成单元，用于由所述初始位置点、所述直线插补点、所述操作臂轨迹分割点、所述圆弧插补点以及所述输送臂末端工具中心点，组成所述操作臂的各个轨迹点；所述操作臂的轨迹点通过所述操作臂的位姿进行描述，在软式内镜的输送过程中，所述操作臂的轨迹点由对应的软式内镜剩余长度进行匹配。

10.一种电子设备，其特征在于，包括：

一个或多个处理器；

存储装置，其上存储有一个或多个程序，

当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如权利要求1-8中任一所述的机械臂轨迹规划方法。

11.一种计算机可读存储介质，其特征在于，其上存储有计算机程序，所述计算机程序被处理器执行时实现如权利要求1-8中任一所述的机械臂轨迹规划方法。