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

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

背景技术：

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

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

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

技术实现思路

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

16、一个或多个处理器；

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

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

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

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

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

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

技术特征：

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

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

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

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

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

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

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

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

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

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

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

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

技术研发人员：江维,江国豪,蔺志伟
受保护的技术使用者：北京云力境安科技有限公司
技术研发日：
技术公布日：2024/5/6