CN113812902A - 柔性器械末端的控制方法、装置、电子设备及存储介质 - Google Patents

Abstract

本发明实施例公开了一种柔性器械末端的控制方法、装置、设备和介质。所述方法包括：确定柔性器械末端的可运动空间的极限位置，接收柔性器械末端对应的控制手柄的输入指令，当柔性器械末端达到可运动空间的极限位置时，根据柔性器械末端的第一当前空间位置和输入指令，将超出可运动空间的极限位置的柔性器械末端的运动路径修正，以得到修正后的运动路径，控制柔性器械末端，按照修正后的运动路径进行运动，使得对于柔性器械末端控制的过程中出现错误的输入指令后，对柔性器械末端在极限位置的运动路径进行了修正，使得柔性器械末端在极限位置时尽可能保证较好的跟随控制手柄的操控，并且防止出现机械损伤和不可预期的情况，更加安全可靠。

Description

柔性器械末端的控制方法、装置、电子设备及存储介质

技术领域

本发明涉及柔性器械末端的控制技术领域，具体涉及一种柔性器械末端的控制方法、一种柔性器械末端的控制装置、一种电子设备、一种可读存储介质。

背景技术

医疗器械的准确操作起着至关重要的作用。比如，依照人体自然腔道或操作便利性等因素需主动改变形状的柔性器械，例如内窥镜、多功能通道器等软组织机器人。

如果控制手柄的输入超出柔性器械末端的机械固有运动范围，可能会导致对机械结构造成不可逆的损伤，甚至会将钢索拉断。

发明内容

鉴于上述问题，提出了本发明以便提供一种克服上述问题或者至少部分地解决上述问题的柔性器械末端的控制方法、柔性器械末端的控制装置、电子设备以及可读存储介质。

依据本发明的一个方面，提供了一种柔性器械末端的控制方法，所述方法包括：

确定所述柔性器械末端的可运动空间的极限位置；

接收所述柔性器械末端对应的控制手柄的输入指令；

当所述柔性器械末端达到所述可运动空间的极限位置时，根据所述柔性器械末端的第一当前空间位置和输入指令，将超出所述可运动空间的极限位置的所述柔性器械末端的运动路径修正，以得到修正后的运动路径；

控制所述柔性器械末端，按照所述修正后的运动路径进行运动。

可选地，所述确定所述柔性器械末端的可运动空间的极限位置包括：

控制所述柔性器械末端分别向偏航的两个极限位置和俯仰的两个极限位置进行运动，确定所述偏航的两个极限位置和俯仰的两个极限位置；

根据所述偏航的两个极限位置和俯仰的两个极限位置，以及所述柔性器械末端的预设空间模型，确定所述柔性器械末端的可运动空间的极限位置。

可选地，所述控制所述柔性器械末端分别向偏航的两个极限位置和俯仰的两个极限位置进行运动，确定所述偏航的两个极限位置和俯仰的两个极限位置包括：

在所述柔性器械末端分别向偏航的两个极限位置和俯仰的两个极限位置进行运动过程中，检测所述柔性器械末端的控制电机的运转情况；

若所述柔性器械末端在预设运动空间内的运动过程中，所述运转情况出现堵转，则根据所述柔性器械末端的第二当前空间位置，确定所述极限位置；

若所述柔性器械末端在所述预设运动空间内的运动过程中，所述运转情况未出现堵转，则将所述预设运动空间对应的极限位置作为所述极限位置。

可选地，所述根据所述柔性器械末端的第二当前空间位置，确定所述极限位置包括：

若所述第二当前空间位置与所述预设运动空间的极限位置的最小距离小于预设阈值，则将所述第二当前空间位置作为所述极限位置；

若所述第二当前空间位置与所述预设运动空间的极限位置的最小距离不小于预设阈值，则上报错误。

可选地，所述柔性器械末端在工装平面上运动，所述工装平面上安装有陀螺仪，所述工装平面的上方安装有摄像头，在所述确定所述柔性器械末端的可运动空间的极限位置之前，所述方法还包括：

利用所述陀螺仪和摄像头，采集所述柔性器械末端的多个空间位置样本；

根据所述多个空间位置样本，使用最小二乘法进行拟合，以生成所述预设空间模型。

可选地，所述预设空间模型为椭球的部分空间模型；所述根据所述偏航的两个极限位置和俯仰的两个极限位置，以及所述柔性器械末端的预设空间模型，确定所述柔性器械末端的可运动空间的极限位置包括：

根据所述偏航的两个极限位置和所述俯仰的两个极限位置，确定所述柔性器械末端在所述椭球中可到达的位置限制信息；

根据所述位置限制信息和所述椭球的部分空间模型，确定所述柔性器械末端的可运动空间的极限位置。

可选地，所述根据所述偏航的两个极限位置和所述俯仰的两个极限位置，确定所述柔性器械末端在所述椭球中可到达的位置限制信息包括：

计算所述偏航的两个极限位置之间的第一距离，以及所述俯仰的两个极限位置之间的第二距离；

所述根据所述位置限制信息和所述椭球的部分空间模型，确定所述柔性器械末端的可运动空间的极限位置包括：

以所述第一距离的一半和第二距离的一半作为椭圆截面的两个半径，生成椭圆截面公式；其中，所述椭圆截面为所述柔性器械末端在所述椭球上可到达的最大截面；

根据所述椭圆截面公式，将所述椭圆截面的边缘点确定为所述柔性器械末端的可运动空间的极限位置。

可选地，所述当所述柔性器械末端达到所述可运动空间的极限位置时，根据所述柔性器械末端的第一当前空间位置和输入指令，将超出所述可运动空间的极限位置的所述柔性器械末端的运动路径修正，以得到修正后的运动路径包括：

当所述柔性器械末端达到所述可运动空间的极限位置时，根据所述第一当前空间位置和输入指令，确定所述输入指令对应的运动路径；

确定所述输入指令对应的运动路径超出所述可运动空间的极限位置；

根据所述输入指令对应的运动路径、第一当前空间位置和可运动空间的极限位置，将所述输入指令对应的运动路径修正，得到修正后的运动路径；其中，所述修正后的运动路径为以所述第一当前空间位置为起点、沿所述可运动空间的极限位置的运动路径，且所述修正后的运动路径与所述输入指令对应的运动路径的方向一致。

可选地，所述柔性器械末端包括两个，所述柔性器械末端与对应的柔性器械中端连接，所述方法还包括：

获取两个所述柔性器械中端所形成的工作三角区的开合角度；

在所述柔性器械末端的运动过程中，获取两个所述柔性器械末端的第三当前空间位置，以及至少一个所述柔性器械末端对应的控制手柄的输入指令；

根据所述开合角度、两个所述柔性器械末端的第三当前空间位置和至少一个所述柔性器械末端对应的控制手柄的输入指令，将具有碰撞风险的所述柔性器械末端的运动路径修正，以得到修正后的运动路径。

可选地，所述根据所述开合角度、两个所述柔性器械末端的第三当前空间位置和至少一个所述柔性器械末端对应的控制手柄的输入指令，将具有碰撞风险的所述柔性器械末端的运动路径修正，以得到修正后的运动路径包括：

根据所述开合角度，两个所述柔性器械末端的第三当前空间位置和至少一个所述柔性器械末端对应的控制手柄的输入指令，确定至少一个所述柔性器械末端在执行所述输入指令的情况下的预测空间位置；

根据两个所述柔性器械末端的预测空间位置，或者一个所述柔性器械末端的预测空间位置和另一个所述柔性器械末端的第三当前空间位置，确定两个所述柔性器械末端具有碰撞风险；

将具有碰撞风险的至少一个所述柔性器械末端的运动路径修正，以得到修正后的运动路径。

可选地，所述根据两个所述柔性器械末端的预测空间位置，或者一个所述柔性器械末端的预测空间位置和另一个所述柔性器械末端的第三当前空间位置，确定两个所述柔性器械末端具有碰撞风险包括：

根据两个所述柔性器械末端的预测空间位置，或者一个所述柔性器械末端的预测空间位置和另一个所述柔性器械末端的第三当前空间位置，确定两个所述柔性器械末端之间的预测相撞点；

所述将具有碰撞风险的至少一个所述柔性器械末端的运动路径修正，以得到修正后的运动路径包括：

若所述预测相撞点位于一个或两个所述柔性器械末端的前端区域，对所述一个或两个所述柔性器械末端的运动路径进行修正，使两个所述柔性器械末端在运动时相互绕开，得到所述修正后的运动路径；

若所述预测相撞点位于两个所述柔性器械末端的后端区域，对所述两个所述柔性器械末端的运动路径进行修正，使两个所述柔性器械末端在相撞之前停止运动，得到所述修正后的运动路径。

相应的，本发明还提供了一种柔性器械末端的控制装置，包括：

位置确定模块，用于确定所述柔性器械末端的可运动空间的极限位置；

指令接收模块，用于接收所述柔性器械末端对应的控制手柄的输入指令；

路径修正模块，用于当所述柔性器械末端达到所述可运动空间的极限位置时，根据所述柔性器械末端的第一当前空间位置和输入指令，将超出所述可运动空间的极限位置的所述柔性器械末端的运动路径修正，以得到修正后的运动路径；

运动控制模块，用于控制所述柔性器械末端，按照所述修正后的运动路径进行运动。

可选地，所述位置确定模块包括：

第一位置确定子模块，用于控制所述柔性器械末端分别向偏航的两个极限位置和俯仰的两个极限位置进行运动，确定所述偏航的两个极限位置和俯仰的两个极限位置；

第二位置确定子模块，用于根据所述偏航的两个极限位置和俯仰的两个极限位置，以及所述柔性器械末端的预设空间模型，确定所述柔性器械末端的可运动空间的极限位置。

可选地，所述第一位置确定子模块包括：

运转情况检测单元，用于在所述柔性器械末端分别向偏航的两个极限位置和俯仰的两个极限位置进行运动过程中，检测所述柔性器械末端的控制电机的运转情况；

第一位置确定单元，用于若所述柔性器械末端在预设运动空间内的运动过程中，所述运转情况出现堵转，则根据所述柔性器械末端的第二当前空间位置，确定所述极限位置；

第二位置确定单元，用于若所述柔性器械末端在所述预设运动空间内的运动过程中，所述运转情况未出现堵转，则将所述预设运动空间对应的极限位置作为所述极限位置。

可选地，所述第一位置确定单元包括：

位置确定子单元，用于若所述第二当前空间位置与所述预设运动空间的极限位置的最小距离小于预设阈值，则将所述第二当前空间位置作为所述极限位置；

错误上报子单元，用于若所述第二当前空间位置与所述预设运动空间的极限位置的最小距离不小于预设阈值，则上报错误。

可选地，所述柔性器械末端在工装平面上运动，所述工装平面上安装有陀螺仪，所述工装平面的上方安装有摄像头，所述装置还包括：

样本采集模块，用于在所述确定所述柔性器械末端的可运动空间的极限位置之前，利用所述陀螺仪和摄像头，采集所述柔性器械末端的多个空间位置样本；

模型生成模块，用于根据所述多个空间位置样本，使用最小二乘法进行拟合，以生成所述预设空间模型。

可选地，所述预设空间模型为椭球的部分空间模型；所述第二位置确定子模块包括：

信息确定单元，用于根据所述偏航的两个极限位置和所述俯仰的两个极限位置，确定所述柔性器械末端在所述椭球中可到达的位置限制信息；

第三位置确定单元，用于根据所述位置限制信息和所述椭球的部分空间模型，确定所述柔性器械末端的可运动空间的极限位置。

可选地，所述信息确定单元包括：

距离计算子单元，用于计算所述偏航的两个极限位置之间的第一距离，以及所述俯仰的两个极限位置之间的第二距离；

所述第三位置确定单元包括：

公式生成子单元，用于以所述第一距离的一半和第二距离的一半作为椭圆截面的两个半径，生成椭圆截面公式；其中，所述椭圆截面为所述柔性器械末端在所述椭球上可到达的最大截面；

位置确定子单元，用于根据所述椭圆截面公式，将所述椭圆截面的边缘点确定为所述柔性器械末端的可运动空间的极限位置。

可选地，所述路径修正模块包括：

路径确定子模块，用于当所述柔性器械末端达到所述可运动空间的极限位置时，根据所述第一当前空间位置和输入指令，确定所述输入指令对应的运动路径；

超出位置确定子模块，用于确定所述输入指令对应的运动路径超出所述可运动空间的极限位置；

修正子模块，用于根据所述输入指令对应的运动路径、第一当前空间位置和可运动空间的极限位置，将所述输入指令对应的运动路径修正，得到修正后的运动路径；其中，所述修正后的运动路径为以所述第一当前空间位置为起点、沿所述可运动空间的极限位置的运动路径，且所述修正后的运动路径与所述输入指令对应的运动路径的方向一致。

可选地，所述柔性器械末端包括两个，所述柔性器械末端与对应的柔性器械中端连接，所述装置还包括：

角度获取模块，用于获取两个所述柔性器械中端所形成的工作三角区的开合角度；

位置指令获取模块，用于在所述柔性器械末端的运动过程中，获取两个所述柔性器械末端的第三当前空间位置，以及至少一个所述柔性器械末端对应的控制手柄的输入指令；

风险路径修正模块，用于根据所述开合角度、两个所述柔性器械末端的第三当前空间位置和至少一个所述柔性器械末端对应的控制手柄的输入指令，将具有碰撞风险的所述柔性器械末端的运动路径修正，以得到修正后的运动路径。

可选地，所述风险路径修正模块包括：

预测位置确定子模块，用于根据所述开合角度，两个所述柔性器械末端的第三当前空间位置和至少一个所述柔性器械末端对应的控制手柄的输入指令，确定至少一个所述柔性器械末端在执行所述输入指令的情况下的预测空间位置；

风险确定子模块，用于根据两个所述柔性器械末端的预测空间位置，或者一个所述柔性器械末端的预测空间位置和另一个所述柔性器械末端的第三当前空间位置，确定两个所述柔性器械末端具有碰撞风险；

风险路径修正子模块，用于将具有碰撞风险的至少一个所述柔性器械末端的运动路径修正，以得到修正后的运动路径。

可选地，所述风险确定子模块包括：

相撞点确定单元，用于根据两个所述柔性器械末端的预测空间位置，或者一个所述柔性器械末端的预测空间位置和另一个所述柔性器械末端的第三当前空间位置，确定两个所述柔性器械末端之间的预测相撞点；

所述风险路径修正子模块包括：

第一修正单元，用于若所述预测相撞点位于一个或两个所述柔性器械末端的前端区域，对所述一个或两个所述柔性器械末端的运动路径进行修正，使两个所述柔性器械末端在运动时相互绕开，得到所述修正后的运动路径；

第二修正单元，用于若所述预测相撞点位于两个所述柔性器械末端的后端区域，对所述两个所述柔性器械末端的运动路径进行修正，使两个所述柔性器械末端在相撞之前停止运动，得到所述修正后的运动路径。

相应的，本发明还提供了一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如上任一所述的方法。

相应的，本发明还提供了一种可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如上任一所述的方法。

依据本发明实施例，通过确定所述柔性器械末端的可运动空间的极限位置，接收所述柔性器械末端对应的控制手柄的输入指令，当所述柔性器械末端达到所述可运动空间的极限位置时，根据所述柔性器械末端的第一当前空间位置和输入指令，将超出所述可运动空间的极限位置的所述柔性器械末端的运动路径修正，以得到修正后的运动路径，控制所述柔性器械末端，按照所述修正后的运动路径进行运动，使得对于柔性器械末端控制的过程中出现错误的输入指令后，对柔性器械末端在极限位置的运动路径进行了修正，使得柔性器械末端在极限位置时尽可能保证较好的跟随控制手柄的操控，并且防止出现机械损伤和不可预期的情况，更加安全可靠。

上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，而可依照说明书的内容予以实施，并且为了让本发明的上述和其它目的、特征和优点能够更明显易懂，以下特举本发明的具体实施方式。

附图说明

通过阅读下文优选实施方式的详细描述，各种其他的优点和益处对于本领域普通技术人员将变得清楚明了。附图仅用于示出优选实施方式的目的，而并不认为是对本发明的限制。而且在整个附图中，用相同的参考符号表示相同的部件。在附图中：

图1示出了根据本发明实施例的一种柔性器械末端的控制方法实施例的流程图；

图2示出了柔性器械末端的示意图；

图3示出了柔性器械的整体示意图；

图4示出了基座与附加器械的示意图；

图5示出了根据本发明实施例的一种柔性器械末端的控制方法实施例的流程图；

图6示出了柔性器械末端的正视图；

图7示出了机械工装的示意图；

图8示出了柔性器械末端的运动空间正视图；

图9示出了上位机的主要功能模块的示意图；

图10示出了模型建立流程的示意图；

图11示出了编码器编码值和器械末端位置之间关系的示意图；

图12示出了柔性器械末端的可运动空间模型的示意图；

图13示出了柔性器械末端处于极限位置运动的示意图；

图14示出了工作三角区完全撑开的示意图；

图15示出了工作三角区未完全撑开的示意图；

图16示出了两个柔性器械末端的可运动空间的极限位置的示意图；

图17示出了两个柔性器械末端的相撞位置的示意图；

图18示出了根据本发明实施例的一种柔性器械末端的控制装置实施例的结构框图；

图19示出了本发明实施例提供的一种电子设备的结构框图。

具体实施方式

下面将参照附图更详细地描述本公开的示例性实施例。虽然附图中显示了本公开的示例性实施例，然而应当理解，可以以各种形式实现本公开而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了能够更透彻地理解本公开，并且能够将本公开的范围完整的传达给本领域的技术人员。

参照图1，示出了根据本发明实施例的一种柔性器械末端的控制方法实施例的流程图，该方法具体可以包括以下步骤：

步骤101，确定所述柔性器械末端的可运动空间的极限位置。

在本发明实施例中，柔性器械末端是指柔性器械中可柔性运动的末端部位。例如，在如图2所示的柔性器械末端的示意图中，柔性器械包括可调椎骨，可调椎骨包括四向椎骨组件4291和双向椎骨组件4292，其中，四向椎骨组件4291为柔性器械末端。例如，如图3所示的柔性器械的整体示意图。

在本发明实施例中，由于器械机械运动空间的物理限制，每个柔性器械末端存在可运动空间，即器械本身机械结构的显示，若控制器控制器械超过此可运动空间的范围限制，可能会导致机械结构永久损伤。例如，因图2中的四向椎骨组件4291的运动靠钢索牵拉实现，四向椎骨组件4291均可实现弯曲，即在弯曲时圆心是不固定的，因为当钢索被拉动时四向椎骨组件4291最软的部位会先被拉动，所以在拉动钢索控制四向椎骨组件4291弯曲时，随着弯曲角度的变化，产生弯曲的圆心会在四向椎骨组件4291上变化，所以四向椎骨组件4291的运动范围受钢索结构和四向椎骨组件4291结构的限制。如果输入超出四向椎骨组件4291的机械固有的可运动空间，且控制程序不针对此输入进行四向椎骨组件4291运动的范围限制，可能会导致对机械结构造成不可逆的损伤，甚至会将钢索拉断。

在本发明实施例中，对于一个柔性器械末端，确定其可运动空间的极限位置时，可以根据已有的空间模型，计算出本柔性器械末端的可运动空间的极限位置。比如，已知柔性器械末端的可运动空间的空间模型为椭球的部分空间模型，通过控制柔性器械末端向多个方向进行弯曲动作，直到柔性器械末端达到极限位置，获取多个极限位置，根据多个极限位置的坐标，并带入已知的椭球的部分空间模型的公式中，以得到该柔性器械末端的可运动空间的极限位置。具体可以采用任意适用的方式，分别确定柔性器械末端的可运动空间的极限位置，本发明实施例对此不做限制。

在本发明实施例中，两个柔性器械末端在开始工作时，可以通过控制达到初始位置，继而得到两个柔性器械末端的相对位置关系。根据相对位置关系，分别确定两个柔性器械末端的可运动空间，从而使得确定的两个可运动空间处于相同的参照系内，以便后续协同工作时对两个柔性器械末端进行控制。

步骤102，接收所述柔性器械末端对应的控制手柄的输入指令。

在本发明实施例中，为了控制柔性器械末端进行运动，需要通过对应的控制手柄来接收输入指令。在控制手柄控制柔性器械末端运动时，采用了增量式的控制，即在控制手柄使能后根据控制手柄在按下使能键后，以控制手柄当前位置作为起始点，处理器程序采集其在各方向的增量运动进行器械端的运动控制，在控制手柄失能后，控制手柄任意动作均不会影响柔性器械末端的运动。这种增量控制方式控制下处理器程序需要在检测到控制手柄使能后，时刻监控控制手柄的输入是否有增量产生。因控制手柄的位置和柔性器械末端的运动位置无对应关系，所以不可用控制手柄处的极限位置去限制柔性器械末端的位置，需要先对柔性器械末端的极限位置进行确定。

例如，如图4所示的基座与附加器械的示意图，图中示出了控制手柄4801、手柄俯仰偏航运动关节4802、手动旋钮4681、附加器械4682等。

步骤103，当所述柔性器械末端达到所述可运动空间的极限位置时，根据所述柔性器械末端的第一当前空间位置和输入指令，将超出所述可运动空间的极限位置的所述柔性器械末端的运动路径修正，以得到修正后的运动路径。

在本发明实施例中，在柔性器械末端的运动过程中，可以获取到柔性器械末端的当前空间位置，记为第一当前空间位置。例如，因为图2中的四向椎骨组件4291是由电机牵拉钢索控制的，处理器程序通过读取电机传动轴两侧安装的磁传感编码器的值可以获取到四向椎骨组件4291的当前的位置，即第一当前空间位置，也就相当于处理器程序可以时刻知道左右两个四向椎骨组件4291柔性段的实时位置。

在本发明实施例中，当第一当前空间位置是可运动空间的极限位置时，即柔性器械末端达到可运动空间的极限位置。如果控制手柄的输入指令超出柔性器械末端的机械固有运动范围，即可运动空间的极限位置，且控制程序不针对此输入进行柔性器械末端的运动路径的规划和范围限制，可能会导致对机械结构造成不可逆的损伤，甚至会将钢索拉断。

在本发明实施例中，根据柔性器械末端的第一当前空间位置和输入指令，将超出可运动空间的极限位置的柔性器械末端的运动路径修正，以得到修正后的运动路径。具体实现方式可以包括多种。例如，当柔性器械末端达到可运动空间的极限位置时，根据第一当前空间位置和输入指令，确定输入指令对应的运动路径，确定输入指令对应的运动路径超出可运动空间的极限位置，根据输入指令对应的运动路径、第一当前空间位置和可运动空间的极限位置，将输入指令对应的运动路径修正，得到修正后的运动路径；其中，修正后的运动路径为以第一当前空间位置为起点、沿可运动空间的极限位置的运动路径，且所述修正后的运动路径与输入指令对应的运动路径的方向一致。

步骤104，控制所述柔性器械末端，按照所述修正后的运动路径进行运动。

在本发明实施例中，在柔性器械末端的运动过程中，为避免柔性器械末端因超出可运动空间而发生机械结构永久损伤，需通过控制，使得柔性器械末端按照所述修正后的运动路径进行运动。

依据本发明实施例，通过确定所述柔性器械末端的可运动空间的极限位置，接收所述柔性器械末端对应的控制手柄的输入指令，当所述柔性器械末端达到所述可运动空间的极限位置时，根据所述柔性器械末端的第一当前空间位置和输入指令，将超出所述可运动空间的极限位置的所述柔性器械末端的运动路径修正，以得到修正后的运动路径，控制所述柔性器械末端，按照所述修正后的运动路径进行运动，使得对于柔性器械末端控制的过程中出现错误的输入指令后，对柔性器械末端在极限位置的运动路径进行了修正，使得柔性器械末端在极限位置时尽可能保证较好的跟随控制手柄的操控，并且防止出现机械损伤和不可预期的情况，更加安全可靠。

在本发明的一种可选实施例中，如图5所示，上述步骤101包括：

步骤1011，控制所述柔性器械末端分别向偏航的两个极限位置和俯仰的两个极限位置进行运动，确定所述偏航的两个极限位置和俯仰的两个极限位置。

步骤1012，根据所述偏航的两个极限位置和俯仰的两个极限位置，以及所述柔性器械末端的预设空间模型，确定所述柔性器械末端的可运动空间的极限位置。

对于柔性器械末端的运动描述，基于柔性器械末端的机械结构，将一个平面上的运动称为偏航，将与垂直于前一平面的平面上的运动称为俯仰。对于偏航，向一侧偏航会存在一个极限位置，向另一侧偏航也会存在一个极限位置。对于俯仰，向一侧俯仰会存在一个极限位置，向另一侧俯仰也会存在一个极限位置。偏航和俯仰联合运动可产生复合运动。

如图6所示，在柔性器械末端的正视图上建立XY坐标，其X轴和Y轴方向与手柄端的偏航和俯仰的方向一致。如图6所示，A、B、C和D是4根钢索，斜对角两根，由同一个控制电机控制，设A、B由控制电机1控制，C、D由控制电机2控制，所以想要使柔性器械末端在X轴（偏航）上运动，需要控制电机1和控制电机2同时运动，若向X正方向弯曲需要控制电机1控制A钢索前拉B钢索后缩，控制电机2控制C钢索前拉D钢索后缩，且控制电机1和控制电机2拉动范围相同，在Y轴（俯仰）上运动亦然。

针对每个柔性器械末端，控制柔性器械末端分别向偏航的两个极限位置和俯仰的两个极限位置进行运动，从而确定偏航的两个极限位置和俯仰的两个极限位置。具体实现方式可以包括多种，例如，当运动到极限位置时，电机会发生堵转，获取柔性器械末端的当前空间位置，作为极限位置。

柔性器械末端的预设空间模型是预先得到的。预设空间模型通常为描述可运动空间/可运动空间的极限位置的数学表达式。其中，已知量可以预先通过测定和数学方法确定。未知量则可以根据偏航的两个极限位置、俯仰的两个极限位置确定。确定对应的所述柔性器械末端的可运动空间的实现方式可以包括多种，例如，根据偏航的两个极限位置、俯仰的两个极限位置，确定出预设空间模型所需的未知量，然后将未知量代入预设空间模型，就可以得到可运动空间的极限位置。

在本发明的一种可选实施例中，所述控制所述柔性器械末端分别向偏航的两个极限位置和俯仰的两个极限位置进行运动，确定所述偏航的两个极限位置和俯仰的两个极限位置的一种具体实现方式可以包括：

在所述柔性器械末端分别向偏航的两个极限位置和俯仰的两个极限位置进行运动过程中，检测所述柔性器械末端的控制电机的运转情况；

若所述柔性器械末端在预设运动空间内的运动过程中，所述运转情况出现堵转，则根据所述柔性器械末端的第二当前空间位置，确定所述极限位置；

若所述柔性器械末端在所述预设运动空间内的运动过程中，所述运转情况未出现堵转，则将所述预设运动空间对应的极限位置作为所述极限位置。

柔性器械末端的运动由控制电机来控制，柔性器械末端是否达到极限位置，可以根据控制电机的运转情况来判定。在柔性器械末端分别向偏航的两个极限位置和俯仰的两个极限位置进行运动过程中，检测柔性器械末端的控制电机的运转情况。

柔性器械末端具有预设运动空间，此范围在产品机械设计阶段确定。在柔性器械末端的运动过程中，获取柔性器械末端的第二当前空间位置。柔性器械末端在预设运动空间内的运动过程中，即第二当前空间位置在预设运动空间内，若运转情况出现堵转，则根据柔性器械末端的第二当前空间位置，确定极限位置，若运转情况未出现堵转，则将预设运动空间对应的极限位置作为极限位置。分别对偏航的两个极限位置和俯仰的两个极限位置都采用上述确定方式。

在本发明的一种可选实施例中，所述根据所述柔性器械末端的第二当前空间位置，确定所述极限位置的一种具体实现方式可以包括：

若所述第二当前空间位置与所述预设运动空间的极限位置的最小距离小于预设阈值，则将所述第二当前空间位置作为所述极限位置；

若所述第二当前空间位置与所述预设运动空间的极限位置的最小距离不小于预设阈值，则上报错误。

例如，处理器程序在开机自检阶段会通过控制电机控制柔性器械末端进行X轴和Y轴极限位置的探测，即处理器程序先根据系统的预设运动空间（此范围在产品机械设计阶段确定）去通过控制电机拉动柔性器械末端进行运动，并通过控制电机输出轴上安装的磁传感编码器来获取柔性器械末端的运动。若在此范围内处理器程序通过ADC（Analog-to-Digital Converter，模拟数字转换器）采集检测到控制电机驱动的电流增大超过阈值则判定为电机堵转，认为此处为此器械的极限位置即比系统的预设运动空间要小，若此范围与系统的预设运动空间的差值在百分之五之内视为正常，可能由生产安装误差导致，并将实际测出的极限范围用于之后的计算，若与系统的预设运动空间的差别较大，则上报错误；若在系统的预设运动空间内未出现电机堵转现象则将系统预设运动空间用于之后的运算。

在本发明的一种可选实施例中，所述柔性器械末端在工装平面上运动，所述工装平面上安装有陀螺仪，所述工装平面的上方安装有摄像头，在所述确定所述柔性器械末端的可运动空间的极限位置之前，还可以包括：

利用所述陀螺仪和摄像头，采集所述柔性器械末端的多个空间位置样本；

根据所述多个空间位置样本，使用最小二乘法进行拟合，以生成所述预设空间模型。

预设空间模型是预先生成的，具体需要使用相应的工装，对柔性器械末端在空间上的运动范围进行大量采样，并且使用最小二乘法进行拟合建模，建模完成后在需求实际满足的情况下对模型做出近似化处理修正，使得此模型更加的简单易用。

例如，特有机械工装如图7所示，通过此工装平面01将四向椎骨组件4291固定，使其在由此工装确定的平面上运动。同时在此工装平面水平上安装陀螺仪用以测量工装平面所处的空间位置；在此工装平面上方垂直安装摄像头，用于测量四向椎骨组件4291实时运动的角度；通过电机输出齿轮轴上安装的磁传感编码器测量电机牵引电机牵拉钢索的长度。

通过柔性器械末端的机械结构可以初步分析出柔性器械末端的运动空间为一个椭球的一部分，其大致运动空间正视图如图8所示，即为一个大致的椭圆形，为了可以测量整个运动空间的数据，所以应该过椭球心截取不同的平面进行测量，即如图8中双箭头线段所示。

上位机负责整合所有传感器数据，并且接收使用者输入的参数，自动控制电机带动柔性器械末端运动，通过运动中采集的数据，自动整合建模，生成最终补偿公式。

上位机的主要功能模块如图9所示，其中摄像头数据接收处理模块用于接收布置在工装垂直平面上摄像头的影像信息，并且通过视觉识别算法，对器械末端进行跟踪，实时获取器械末端运动的角度。

传感器数据接收模块主要用于接收水平贴附在工装平面上的陀螺仪传感器的数据，用于检测工装平面在柔性器械运动空间所处的截面位置；接收电机输出轴处安装的磁传感编码器的数据，从而获取电机牵拉钢索实时运动情况。

参数设置模块可以根据实际需求设置测试过程中的采样精度，比如选取如图6所示的X轴所表示的平面作为测试平面，则上位机程序需要获取从中间初始位置开始，柔性器械末端在此测试平面上每向左运动x度，需要电机牵拉钢索运动多少，选取的x度数越小则精度越高；并且参数设置模块可以对进行建模方式的选择，比如是使用插值方式还是拟合方式，对每种方式预留建模算法，如分段插值、三次样条插值、线性最小二乘法拟合等。

数据处理与运动控制模块根据陀螺仪获取的运动平面信息，控制电机牵拉钢索运动，并通过摄像头实时监控的器械末端运行的角度，每过x度记录一次控制电机的磁传感编码器的数据，将柔性器械末端每动x度，磁传感编码器对应的增量值存储到数据库中，直到柔性器械末端到达极限位置。

建模运算模块在程序控制电机牵拉器械运动完毕后，根据存储在数据库中的数据，结合用户输入的建模方式，对本平面测试的柔性器械末端运动与电机运动进行关联建模，建模完成后输出并存储补偿公式和相关数据，再对更多的运动平面进行相同的操作，获取补偿公式和相关数据，最后本模块可以将所有模块的补偿公式进行整合运算，将获得整个柔性器械末端运动空间的补偿公式。柔性器械末端的多个空间位置样本，使用最小二乘法进行拟合，以生成所述预设空间模型。

通过上述工装设计，配套程序算法，获取到更加精准的空间位置样本，继而生成更加精确的预设空间模型。

如图10所示的模型建立流程的示意图。

基本参数设定：设定测试过程中的采样精度，此精度决定柔性器械末端每运行多少度对控制电机的磁传感编码器的数据进行一次采集；对进行建模方式的选择。

建立通讯连接，检测通讯是否正常，显示通讯错误状态：开启与陀螺仪传感器、摄像头、磁传感编码器等各种测试必要器件建立通讯连接，并通过是否接收到正确的数据判断通讯是否成功，若成功则进行下一步，若不成功则显示通讯错误。

获取陀螺仪传感器数据，确定柔性器械末端所在运动平面：程序通过获取陀螺仪传感器的数据，因为陀螺仪水平贴附在工装平面上，所以程序可以知道当前工装相对于水平角度偏了多少度，由此可以判断出当前测试柔性器械末端应该如何运动，并最终解算到电机应该如何运动。

根据设置参数规划柔性器械末端采样点，柔性器械末端所在运动平面解算电机运动路径：根据基本参数设定中设置的采样精度和确定的柔性器械末端所在的运动平面，可以拟定出程序控制器械末端所要经过的采样点，在摄像头检测到器械末端运动到采样点时，程序采集并存储控制电机的磁传感编码器的值。

具体实现时，工装平面上方的摄像头始终垂直于工装平面设置，并对准工装平面进行拍摄。为了检测柔性器械末端是否运动到目标采样点，可以每隔2ms采集一张图像。然后对拍摄到的图像进行二值化，由于工装平板为浅色（例如白色），而柔性器械末端为深色（例如黑色），因此经过二值化后，可以确定出柔性器械末端所在的像素位置。然后将柔性器械末端的像素点与柔性器械根部像素点，建立连线，得到连线相对于工装平面对称轴的夹角，如果该夹角等于目标采样点的对应夹角，则确定柔性器械末端运动到目标采样点。

驱动电机带动器械按预定路径运动，判断器械末端是否到达极限位置：按照上述步骤中的信息，程序驱动控制电机带动器械按预定路径运动，并且通过实时检测控制电机时候有过流堵转现象出现从而判定柔性器械末端是否达到极限位置。

判断末端器械是否到达目标采样点：通过摄像头检测柔性器械末端是否达到之前预测的目标采样点。

将目标点位置电机编码器数据写入数据库，将极限位置电机编码器数据写入数据库：在柔性器械末端运行到预测的目标采样点和到达极限位置时，程序将会读出控制电机的磁传感编码器数据，并将其写入数据库中以供后续步骤建模使用。

设置下一个规划采样点为目标采样点：程序在驱动控制电机带动柔性器械末端运动时，到达一个预测的目标采样点后，获取并保存控制电机的磁传感编码器的数据，且此时柔性器械末端并没有到达极限位置，则程序会继续驱动控制电机运动，使其牵拉钢索带动柔性器械末端到达下一个目标采样点。

根据数据库中数据和初始参数设置进行建模运算：在柔性器械末端运动到极限位置时，根据以上步骤所采集到的信息和参数设置时的输入，进行自动建模运算。

根据模型进行角度校正补偿运算：在建模运算之后，根据模型，可以获取在不同的弯曲角度下柔性器械末端想要运动1度需要驱动控制电机运动的不同的圈数，即得到了驱动控制电机运动与器械末端运动所对应的动态关系。

如图11所示编码器编码值和器械末端位置之间关系的示意图。针对椭球的一个截面，拟合出了曲线。在使用该曲线驱动控制电机期间，当知道了柔性器械末端的当前空间位置和目标空间位置后，可以根据曲线，确定出控制电机还需要转动的圈数。

输出并存储补偿公式和相关数据：根据以上步骤，本工装已经将本运动平面补偿公式计算出来，程序将把本平面的信息和补偿公式的信息一同显示出来，并存储到数据库，用于将更多平面进行同样的操作获取补偿公式后将整个柔性器械末端的可运动空间的补偿公式计算整合出来。

在本发明的一种可选实施例中，预设空间模型为椭球的部分空间模型；根据所述偏航的两个极限位置和俯仰的两个极限位置，以及所述柔性器械末端的预设空间模型，确定所述柔性器械末端的可运动空间的极限位置的一种具体实现方式中，可以包括：

根据所述偏航的两个极限位置和所述俯仰的两个极限位置，确定所述柔性器械末端在所述椭球中可到达的位置限制信息；

根据所述位置限制信息和所述椭球的部分空间模型，确定所述柔性器械末端的可运动空间的极限位置。

由于柔性器械末端的机械结构，柔性器械末端在椭球中可到达的极限是一个椭圆。而所述偏航的两个极限位置和所述俯仰的两个极限位置在该椭圆上。因此将所述偏航的两个极限位置和所述俯仰的两个极限位置可以确定该椭圆在椭球中的位置，可以用位置限制信息来表征。例如，位置限制信息包括偏航的两个极限位置之间的距离，以及俯仰的两个极限位置之间的距离。

将位置限制信息代入椭球的部分空间模型中，从而确定对应的柔性器械末端的可运动空间，得到可运动空间的极限位置为椭球的部分空间的最大椭圆。最后确定柔性器械末端的可运动空间的建模如图12所示，为椭球的一部分。

在本发明的一种可选实施例中，根据所述偏航的两个极限位置和所述俯仰的两个极限位置，确定所述柔性器械末端在所述椭球中可到达的位置限制信息的一种具体实现方式中，可以包括：

计算所述偏航的两个极限位置之间的第一距离，以及所述俯仰的两个极限位置之间的第二距离；

所述根据所述位置限制信息和所述椭球的部分空间模型，确定所述柔性器械末端的可运动空间的极限位置包括：

以所述第一距离的一半和第二距离的一半作为椭圆截面的两个半径，生成椭圆截面公式；其中，所述椭圆截面为所述柔性器械末端在所述椭球上可到达的最大截面；

根据所述椭圆截面公式，将所述椭圆截面的边缘点确定为所述柔性器械末端的可运动空间的极限位置。

根据柔性器械末端自身机械结构运动特性，将柔性器械末端的空间建模如图12所示，在确定的X轴的极限位置（即偏航的两个极限位置）和Y轴的极限位置（即俯仰的两个极限位置）的平面取截面，建立如图13所示坐标系，假设测得X轴的极限运动行程为2a，即第一距离，Y轴的极限运动行程为2b，即第二距离，可知在此坐标系下柔性器械末端的可运动空间的椭圆截面的公式为：

其中，椭圆截面为所述柔性器械末端在所述椭球上可到达的最大截面，a和b为椭圆截面的两个半径。

根据上述椭圆截面公式，将椭圆截面的边缘点确定为柔性器械末端的可运动空间的极限位置。

在本发明的一种可选实施例中，当所述柔性器械末端达到所述可运动空间的极限位置时，根据所述柔性器械末端的第一当前空间位置和输入指令，将超出所述可运动空间的极限位置的所述柔性器械末端的运动路径修正，以得到修正后的运动路径的一种具体实现方式可以包括：

当所述柔性器械末端达到所述可运动空间的极限位置时，根据所述第一当前空间位置和输入指令，确定所述输入指令对应的运动路径；

确定所述输入指令对应的运动路径超出所述可运动空间的极限位置；

根据所述输入指令对应的运动路径、第一当前空间位置和可运动空间的极限位置，将所述输入指令对应的运动路径修正，得到修正后的运动路径；其中，所述修正后的运动路径为以所述第一当前空间位置为起点、沿所述可运动空间的极限位置的运动路径，且所述修正后的运动路径与所述输入指令对应的运动路径的方向一致。

根据第一当前空间位置和输入指令，确定若按照输入指令执行柔性器械末端的运动路径。由于在此之前已经确定可运动空间的极限位置，因此可以判定输入指令对应的运动路径是否超出可运动空间的极限位置，若确定输入指令对应的运动路径超出可运动空间的极限位置，则需要对运动路径进行修正。

然后，根据输入指令对应的运动路径、第一当前空间位置和可运动空间的极限位置，对输入指令对应的运动路径进行修正，修正时，以第一当前空间位置为起点、沿可运动空间的极限位置运动，且与输入指令对应的运动路径的方向一致，得到新的运动路径作为修正后的运动路径。

例如，若控制手柄控制柔性器械末端运动到如图13所示的a点，即为X轴正方向的极限位置，此时若控制手柄在保持控制X轴位置不变的情况下，使用控制手柄控制Y轴向Y轴正方向运行，即为图12所示的ac方向，这时处理器程序将分析输入手柄的输入指令，根据机械结构本身的限制，柔性器械末端无法按照ac方向运动。判定为控制手柄执行此操作的目的为沿弧ab进行运动，并控制控制电机按照此路径规划执行。Y轴同理。

在本发明的一种可选实施例中，所述柔性器械末端包括两个，所述柔性器械末端与对应的柔性器械中端连接，还可以包括：

获取两个所述柔性器械中端所形成的工作三角区的开合角度；

在所述柔性器械末端的运动过程中，获取两个所述柔性器械末端的第三当前空间位置，以及至少一个所述柔性器械末端对应的控制手柄的输入指令；

根据所述开合角度、两个所述柔性器械末端的第三当前空间位置和至少一个所述柔性器械末端对应的控制手柄的输入指令，将具有碰撞风险的所述柔性器械末端的运动路径修正，以得到修正后的运动路径。

柔性器械末端包括两个，两个柔性器械末端可以协同工作。柔性器械末端与对应的柔性器械中端连接。例如，如图2所示，其中，双向椎骨组件4292为柔性器械中端。在两个柔性器械末端进行协同工作前，会先将两个柔性器械中端撑起一个工作三角区，如图14所示的工作三角区完全撑开的示意图和如图15所示的工作三角区未完全撑开的示意图，使得两个柔性器械末端有着足够的空间进行协同工作，因为工作三角区撑开的大小不同，两个柔性器械末端的协同的范围也是不一样的，需要根据当前的工作三角区撑开的范围对两个柔性器械末端的可运动空间进行限制。在控制柔性器械末端运动时，通常不再控制柔性器械中端运动，即工作三角区是不变的。如果再控制柔性器械中端运动，导致工作三角区发生变化，则重新获取工作三角区的开合角度，并对两个所述柔性器械末端的可运动空间进行更新。

柔性器械中端所形成的工作三角区的开合角度可以直接获取到。例如，柔性器械在开始工作时可以通过旋转图3所示的三角区调整机构468中的工作三角区的手动旋钮来控制工作三角区的开合大小，同时此旋钮为一个电位计，处理器程序通过采集此电位计的电压大小来获取旋钮旋转的角度，进而获取工作三角区的开合角度的度数。工作三角区不同的开合度数如图14和图15所示。如图16建立坐标系，从图中可以看出两个柔性器械末端可能出现碰撞的位置即为两个椭圆相交的部分，并且根据工作三角区打开的大小不同两个椭圆相交的范围也会随之变化。

两个柔性器械末端的可运动空间可以分别确定，再结合开合角度，就可以在以工作三角区建立的参照系中，确定出两个柔性器械末端的可运动空间的所占的空间位置。具体实现方式可以包括多种。

在柔性器械末端的运动过程中，可以分别获取到两个柔性器械末端的当前空间位置，记为第三当前空间位置，以及至少一个柔性器械末端对应的控制手柄的输入指令。

根据开合角度、两个柔性器械末端的第三当前空间位置和至少一个柔性器械末端对应的控制手柄的输入指令，确定两个柔性器械末端是否具有碰撞风险，若两个柔性器械末端具有碰撞风险，则将柔性器械末端的运动路径修正，以避免两个柔性器械末端发生碰撞。

例如，如图6所示的柔性器械末端的正视图，由图6可知柔性器械末端的正视图为一个圆，柔性器械末端可看作为一个高度变化的圆柱体，两个柔性器械末端只能在步骤101所确定的可运动空间中运动。设柔性器械末端正视图圆的半径为r，则两个圆柱体的中心线线段之间的空间距离大于2r才能保证不会相撞。

因为柔性器械末端是由控制电机牵拉钢索控制的，处理器程序通过读取控制电机的输出轴的磁传感编码器的值可以获取到柔性器械末端的第三当前空间位置，也就相当于处理器程序可以时刻知道左右两个柔性器械末端在坐标系中的实时位置，所以处理器程序可以根据上述两个柔性器械末端不相撞的判定条件，实行更新运动路径规划。

在处理器程序知道两个柔性器械末端所有的限制范围时，为了在运动过程中使两个柔性器械末端不产生碰撞，需要对两个柔性器械末端的运动路径进行规划，确定是停止运动防撞还是避让防撞。

在本发明的一种可选实施例中，根据所述开合角度、两个所述柔性器械末端的第三当前空间位置和至少一个所述柔性器械末端对应的控制手柄的输入指令，将具有碰撞风险的所述柔性器械末端的运动路径修正，以得到修正后的运动路径的一种具体实现方式可以包括：

根据所述开合角度，两个所述柔性器械末端的第三当前空间位置和至少一个所述柔性器械末端对应的控制手柄的输入指令，确定至少一个所述柔性器械末端在执行所述输入指令的情况下的预测空间位置；

根据两个所述柔性器械末端的预测空间位置，或者一个所述柔性器械末端的预测空间位置和另一个所述柔性器械末端的第三当前空间位置，确定两个所述柔性器械末端具有碰撞风险；

将具有碰撞风险的至少一个所述柔性器械末端的运动路径修正，以得到修正后的运动路径。

根据开合角度，两个柔性器械末端的第三当前空间位置和一个柔性器械末端对应的控制手柄的输入指令，可以确定该柔性器械末端在执行输入指令的情况下的预测空间位置。根据开合角度，两个柔性器械末端的第三当前空间位置和两个柔性器械末端对应的控制手柄的输入指令，可以确定对应的两个柔性器械末端在执行输入指令的情况下的预测空间位置。

若两个柔性器械末端都接收到了输入指令，则根据两个柔性器械末端的预测空间位置，确定两个柔性器械末端是否具有碰撞风险，即两个柔性器械末端的预测空间位置具有重合部分，则确定具有碰撞风险。或者若一个柔性器械末端接收到了输入指令，则根据该所述柔性器械末端的预测空间位置和另一个柔性器械末端的第三当前空间位置，确定两个柔性器械末端是否具有碰撞风险，即预测空间位置和第三当前空间位置具有重合部分，则确定具有碰撞风险。

若两个柔性器械末端都接收到了输入指令，则将具有碰撞风险的两个柔性器械末端的运动路径修正，以得到修正后的运动路径。若一个柔性器械末端接收到了输入指令，则将具有碰撞风险的一个柔性器械末端的运动路径修正，以得到修正后的运动路径。

例如，在控制手柄控制柔性器械末端运动时，控制程序实时检测控制手柄的输入指令并计算出此输入指令会使柔性器械末端产生的下一步动作的位置。

在本发明的一种可选实施例中，根据两个所述柔性器械末端的预测空间位置，或者一个所述柔性器械末端的预测空间位置和另一个所述柔性器械末端的第三当前空间位置，确定两个所述柔性器械末端具有碰撞风险的一种具体实现方式可以包括：

根据两个所述柔性器械末端的预测空间位置，或者一个所述柔性器械末端的预测空间位置和另一个所述柔性器械末端的第三当前空间位置，确定两个所述柔性器械末端之间的预测相撞点；

相应的，将具有碰撞风险的至少一个所述柔性器械末端的运动路径修正，以得到修正后的运动路径的一种具体实现方式可以包括：

若所述预测相撞点位于一个或两个所述柔性器械末端的前端区域，对所述一个或两个所述柔性器械末端的运动路径进行修正，使两个所述柔性器械末端在运动时相互绕开，得到所述修正后的运动路径；

若所述预测相撞点位于两个所述柔性器械末端的后端区域，对所述两个所述柔性器械末端的运动路径进行修正，使两个所述柔性器械末端在相撞之前停止运动，得到所述修正后的运动路径。

若两个柔性器械末端都接收到了输入指令，则根据两个柔性器械末端的预测空间位置，确定两个柔性器械末端之间的预测相撞点。或者若一个柔性器械末端接收到了输入指令，则根据该所述柔性器械末端的预测空间位置和另一个柔性器械末端的第三当前空间位置，确定两个柔性器械末端之间的预测相撞点。

例如，实时根据空间两条中心线线段之间的距离最短的距离公式算出两条柔性器械末端中心线线段的实时最短距离和两个中心线线段的最短距离点在线段中的位置。根据两个柔性器械末端的中心线线段最短距离点（用于计算预测相撞点）在中心线线段中的位置和控制手柄的输入指令判断具体防止碰撞的方式。

柔性器械末端的前端区域和后端区域的划分可以根据实际需要进行设定，本发明实施例对此不做限制。

若预测相撞点位于一个或两个柔性器械末端的前端区域，对一个或两个柔性器械末端的运动路径进行修正，使两个柔性器械末端在运动时相互绕开，得到修正后的运动路径。由于让前端区域相互绕开的动作不会太大，因此该躲避动作不会产生太大影响，对运动路径的影响不会太大。

例如，如果根据控制手柄的输入指令和目前两个柔性器械末端中心线线段最短距离点的预测相撞点，如图17左侧和中间两幅图所示，位于一个或两个柔性器械末端的前端区域（前端区域判断存在一个预设阈值），控制程序会控制两个柔性器械末端做出轻微远离动作，进行相互躲避，相互绕过对方后按照控制手柄的输入指令继续运动。

若预测相撞点位于两个柔性器械末端的后端区域，对两个柔性器械末端的运动路径进行修正，使两个柔性器械末端在相撞之前停止运动，得到修正后的运动路径。由于让后端区域相互绕开的动作太大，因此该躲避动作会产生太大影响，对运动路径的影响太大。

例如，如果根据控制手柄的输入指令和目前两个柔性器械末端中心线线段最短距离点的预测相撞点，如图17右侧的图所示，位于两个柔性器械末端的后端区域（后端区域判断存在一个预设阈值），控制程序会控制两个柔性器械末端做出及时减速至停止的动作，防止因为控制手柄的输入指令产生碰撞。

参照图18，示出了根据本发明实施例的一种柔性器械末端的控制装置实施例的结构框图，具体可以包括：

位置确定模块201，用于确定所述柔性器械末端的可运动空间的极限位置；

指令接收模块202，用于接收所述柔性器械末端对应的控制手柄的输入指令；

路径修正模块203，用于当所述柔性器械末端达到所述可运动空间的极限位置时，根据所述柔性器械末端的第一当前空间位置和输入指令，将超出所述可运动空间的极限位置的所述柔性器械末端的运动路径修正，以得到修正后的运动路径；

运动控制模块204，用于控制所述柔性器械末端，按照所述修正后的运动路径进行运动。

可选地，所述位置确定模块包括：

第一位置确定子模块，用于控制所述柔性器械末端分别向偏航的两个极限位置和俯仰的两个极限位置进行运动，确定所述偏航的两个极限位置和俯仰的两个极限位置；

第二位置确定子模块，用于根据所述偏航的两个极限位置和俯仰的两个极限位置，以及所述柔性器械末端的预设空间模型，确定所述柔性器械末端的可运动空间的极限位置。

可选地，所述第一位置确定子模块包括：

运转情况检测单元，用于在所述柔性器械末端分别向偏航的两个极限位置和俯仰的两个极限位置进行运动过程中，检测所述柔性器械末端的控制电机的运转情况；

第一位置确定单元，用于若所述柔性器械末端在预设运动空间内的运动过程中，所述运转情况出现堵转，则根据所述柔性器械末端的第二当前空间位置，确定所述极限位置；

第二位置确定单元，用于若所述柔性器械末端在所述预设运动空间内的运动过程中，所述运转情况未出现堵转，则将所述预设运动空间对应的极限位置作为所述极限位置。

可选地，所述第一位置确定单元包括：

位置确定子单元，用于若所述第二当前空间位置与所述预设运动空间的极限位置的最小距离小于预设阈值，则将所述第二当前空间位置作为所述极限位置；

错误上报子单元，用于若所述第二当前空间位置与所述预设运动空间的极限位置的最小距离不小于预设阈值，则上报错误。

可选地，所述柔性器械末端在工装平面上运动，所述工装平面上安装有陀螺仪，所述工装平面的上方安装有摄像头，所述装置还包括：

样本采集模块，用于在所述确定所述柔性器械末端的可运动空间的极限位置之前，利用所述陀螺仪和摄像头，采集所述柔性器械末端的多个空间位置样本；

模型生成模块，用于根据所述多个空间位置样本，使用最小二乘法进行拟合，以生成所述预设空间模型。

可选地，所述预设空间模型为椭球的部分空间模型；所述第二位置确定子模块包括：

信息确定单元，用于根据所述偏航的两个极限位置和所述俯仰的两个极限位置，确定所述柔性器械末端在所述椭球中可到达的位置限制信息；

第三位置确定单元，用于根据所述位置限制信息和所述椭球的部分空间模型，确定所述柔性器械末端的可运动空间的极限位置。

可选地，所述信息确定单元包括：

距离计算子单元，用于计算所述偏航的两个极限位置之间的第一距离，以及所述俯仰的两个极限位置之间的第二距离；

所述第三位置确定单元包括：

公式生成子单元，用于以所述第一距离的一半和第二距离的一半作为椭圆截面的两个半径，生成椭圆截面公式；其中，所述椭圆截面为所述柔性器械末端在所述椭球上可到达的最大截面；

位置确定子单元，用于根据所述椭圆截面公式，将所述椭圆截面的边缘点确定为所述柔性器械末端的可运动空间的极限位置。

可选地，所述路径修正模块包括：

路径确定子模块，用于当所述柔性器械末端达到所述可运动空间的极限位置时，根据所述第一当前空间位置和输入指令，确定所述输入指令对应的运动路径；

超出位置确定子模块，用于确定所述输入指令对应的运动路径超出所述可运动空间的极限位置；

修正子模块，用于根据所述输入指令对应的运动路径、第一当前空间位置和可运动空间的极限位置，将所述输入指令对应的运动路径修正，得到修正后的运动路径；其中，所述修正后的运动路径为以所述第一当前空间位置为起点、沿所述可运动空间的极限位置的运动路径，且所述修正后的运动路径与所述输入指令对应的运动路径的方向一致。

可选地，所述柔性器械末端包括两个，所述柔性器械末端与对应的柔性器械中端连接，所述装置还包括：

角度获取模块，用于获取两个所述柔性器械中端所形成的工作三角区的开合角度；

位置指令获取模块，用于在所述柔性器械末端的运动过程中，获取两个所述柔性器械末端的第三当前空间位置，以及至少一个所述柔性器械末端对应的控制手柄的输入指令；

风险路径修正模块，用于根据所述开合角度、两个所述柔性器械末端的第三当前空间位置和至少一个所述柔性器械末端对应的控制手柄的输入指令，将具有碰撞风险的所述柔性器械末端的运动路径修正，以得到修正后的运动路径。

可选地，所述风险路径修正模块包括：

预测位置确定子模块，用于根据所述开合角度，两个所述柔性器械末端的第三当前空间位置和至少一个所述柔性器械末端对应的控制手柄的输入指令，确定至少一个所述柔性器械末端在执行所述输入指令的情况下的预测空间位置；

风险确定子模块，用于根据两个所述柔性器械末端的预测空间位置，或者一个所述柔性器械末端的预测空间位置和另一个所述柔性器械末端的第三当前空间位置，确定两个所述柔性器械末端具有碰撞风险；

风险路径修正子模块，用于将具有碰撞风险的至少一个所述柔性器械末端的运动路径修正，以得到修正后的运动路径。

可选地，所述风险确定子模块包括：

相撞点确定单元，用于根据两个所述柔性器械末端的预测空间位置，或者一个所述柔性器械末端的预测空间位置和另一个所述柔性器械末端的第三当前空间位置，确定两个所述柔性器械末端之间的预测相撞点；

所述风险路径修正子模块包括：

第一修正单元，用于若所述预测相撞点位于一个或两个所述柔性器械末端的前端区域，对所述一个或两个所述柔性器械末端的运动路径进行修正，使两个所述柔性器械末端在运动时相互绕开，得到所述修正后的运动路径；

第二修正单元，用于若所述预测相撞点位于两个所述柔性器械末端的后端区域，对所述两个所述柔性器械末端的运动路径进行修正，使两个所述柔性器械末端在相撞之前停止运动，得到所述修正后的运动路径。

依据本发明实施例，通过确定所述柔性器械末端的可运动空间的极限位置，接收所述柔性器械末端对应的控制手柄的输入指令，当所述柔性器械末端达到所述可运动空间的极限位置时，根据所述柔性器械末端的第一当前空间位置和输入指令，将超出所述可运动空间的极限位置的所述柔性器械末端的运动路径修正，以得到修正后的运动路径，控制所述柔性器械末端，按照所述修正后的运动路径进行运动，使得对于柔性器械末端控制的过程中出现错误的输入指令后，对柔性器械末端在极限位置的运动路径进行了修正，使得柔性器械末端在极限位置时尽可能保证较好的跟随控制手柄的操控，并且防止出现机械损伤和不可预期的情况，更加安全可靠。

对于装置实施例而言，由于其与方法实施例基本相似，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。

图19为本发明实施例提供的一种电子设备的结构框图。如图19所示，该电子设备800可以包括一个或多个处理器801以及与处理器801连接的一个或多个存储器802。电子设备800还可以包括输入接口803和输出接口804，用于与另一装置或系统进行通信。被处理器801的CPU执行的程序代码可存储在存储器802中。

电子设备800中的处理器801调用存储在存储器802的程序代码，以执行上述实施例中的柔性器械末端的控制方法。

处理器为上述服务器的控制中心，并提供处理装置，用于执行指令，进行中断操作，提供计时功能以及多种其他功能。处理器可为单核(单CPU )处理器或多核(多CPU )处理器。除非另有声明，描述为用于执行任务的例如处理器或存储器的部件可实现为通用部件，其暂时用于在给定时间执行任务，或实现为专门制造用于执行该任务的特定部件。此处所用的术语“处理器”指一个或多个装置，电路和/或处理核，用于处理数据，例如计算机程序指令。

上述服务器中的上述元件可通过总线彼此连接，总线例如数据总线、地址总线、控制总线、扩展总线和局部总线之一或其任意组合。

根据本发明的一个实施例，还提供了一种可读存储介质，所述可读存储介质上存储有计算机程序，存储介质可以是只读存储器( Read-Only Memory，ROM )，或是可读写的，例如硬盘、闪存。所述计算机程序被处理器执行时实现前述实施例的柔性器械末端的控制方法。

本说明书中的各个实施例均采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似的部分互相参见即可。

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

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

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

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

尽管已描述了本发明实施例的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例做出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明实施例范围的所有变更和修改。

最后，还需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者终端设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者终端设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者终端设备中还存在另外的相同要素。

以上对本发明所提供的一种柔性器械末端的控制方法、装置、电子设备及可读存储介质，进行了详细介绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (15)

1.一种柔性器械末端的控制方法，其特征在于，所述方法包括：

确定所述柔性器械末端的可运动空间的极限位置；

接收所述柔性器械末端对应的控制手柄的输入指令；

当所述柔性器械末端达到所述可运动空间的极限位置时，根据所述柔性器械末端的第一当前空间位置和输入指令，将超出所述可运动空间的极限位置的所述柔性器械末端的运动路径修正，以得到修正后的运动路径；

控制所述柔性器械末端，按照所述修正后的运动路径进行运动。

2.根据权利要求1所述的方法，其特征在于，所述确定所述柔性器械末端的可运动空间的极限位置包括：

控制所述柔性器械末端分别向偏航的两个极限位置和俯仰的两个极限位置进行运动，确定所述偏航的两个极限位置和俯仰的两个极限位置；

根据所述偏航的两个极限位置和俯仰的两个极限位置，以及所述柔性器械末端的预设空间模型，确定所述柔性器械末端的可运动空间的极限位置。

3.根据权利要求2所述的方法，其特征在于，所述控制所述柔性器械末端分别向偏航的两个极限位置和俯仰的两个极限位置进行运动，确定所述偏航的两个极限位置和俯仰的两个极限位置包括：

在所述柔性器械末端分别向偏航的两个极限位置和俯仰的两个极限位置进行运动过程中，检测所述柔性器械末端的控制电机的运转情况；

若所述柔性器械末端在预设运动空间内的运动过程中，所述运转情况出现堵转，则根据所述柔性器械末端的第二当前空间位置，确定所述极限位置；

若所述柔性器械末端在所述预设运动空间内的运动过程中，所述运转情况未出现堵转，则将所述预设运动空间对应的极限位置作为所述极限位置。

4.根据权利要求3所述的方法，其特征在于，所述根据所述柔性器械末端的第二当前空间位置，确定所述极限位置包括：

若所述第二当前空间位置与所述预设运动空间的极限位置的最小距离小于预设阈值，则将所述第二当前空间位置作为所述极限位置；

若所述第二当前空间位置与所述预设运动空间的极限位置的最小距离不小于预设阈值，则上报错误。

5.根据权利要求2所述的方法，其特征在于，所述柔性器械末端在工装平面上运动，所述工装平面上安装有陀螺仪，所述工装平面的上方安装有摄像头，在所述确定所述柔性器械末端的可运动空间的极限位置之前，所述方法还包括：

利用所述陀螺仪和摄像头，采集所述柔性器械末端的多个空间位置样本；

根据所述多个空间位置样本，使用最小二乘法进行拟合，以生成所述预设空间模型。

6.根据权利要求2所述的方法，其特征在于，所述预设空间模型为椭球的部分空间模型；所述根据所述偏航的两个极限位置和俯仰的两个极限位置，以及所述柔性器械末端的预设空间模型，确定所述柔性器械末端的可运动空间的极限位置包括：

根据所述偏航的两个极限位置和所述俯仰的两个极限位置，确定所述柔性器械末端在所述椭球中可到达的位置限制信息；

根据所述位置限制信息和所述椭球的部分空间模型，确定所述柔性器械末端的可运动空间的极限位置。

7.根据权利要求6所述的方法，其特征在于，所述根据所述偏航的两个极限位置和所述俯仰的两个极限位置，确定所述柔性器械末端在所述椭球中可到达的位置限制信息包括：

计算所述偏航的两个极限位置之间的第一距离，以及所述俯仰的两个极限位置之间的第二距离；

所述根据所述位置限制信息和所述椭球的部分空间模型，确定所述柔性器械末端的可运动空间的极限位置包括：

以所述第一距离的一半和第二距离的一半作为椭圆截面的两个半径，生成椭圆截面公式；其中，所述椭圆截面为所述柔性器械末端在所述椭球上可到达的最大截面；

根据所述椭圆截面公式，将所述椭圆截面的边缘点确定为所述柔性器械末端的可运动空间的极限位置。

8.根据权利要求1所述的方法，其特征在于，所述当所述柔性器械末端达到所述可运动空间的极限位置时，根据所述柔性器械末端的第一当前空间位置和输入指令，将超出所述可运动空间的极限位置的所述柔性器械末端的运动路径修正，以得到修正后的运动路径包括：

当所述柔性器械末端达到所述可运动空间的极限位置时，根据所述第一当前空间位置和输入指令，确定所述输入指令对应的运动路径；

确定所述输入指令对应的运动路径超出所述可运动空间的极限位置；

根据所述输入指令对应的运动路径、第一当前空间位置和可运动空间的极限位置，将所述输入指令对应的运动路径修正，得到修正后的运动路径；其中，所述修正后的运动路径为以所述第一当前空间位置为起点、沿所述可运动空间的极限位置的运动路径，且所述修正后的运动路径与所述输入指令对应的运动路径的方向一致。

9.根据权利要求1所述的方法，其特征在于，所述柔性器械末端包括两个，所述柔性器械末端与对应的柔性器械中端连接，所述方法还包括：

获取两个所述柔性器械中端所形成的工作三角区的开合角度；

在所述柔性器械末端的运动过程中，获取两个所述柔性器械末端的第三当前空间位置，以及至少一个所述柔性器械末端对应的控制手柄的输入指令；

根据所述开合角度、两个所述柔性器械末端的第三当前空间位置和至少一个所述柔性器械末端对应的控制手柄的输入指令，将具有碰撞风险的所述柔性器械末端的运动路径修正，以得到修正后的运动路径。

10.根据权利要求9所述的方法，其特征在于，所述根据所述开合角度、两个所述柔性器械末端的第三当前空间位置和至少一个所述柔性器械末端对应的控制手柄的输入指令，将具有碰撞风险的所述柔性器械末端的运动路径修正，以得到修正后的运动路径包括：

根据所述开合角度，两个所述柔性器械末端的第三当前空间位置和至少一个所述柔性器械末端对应的控制手柄的输入指令，确定至少一个所述柔性器械末端在执行所述输入指令的情况下的预测空间位置；

根据两个所述柔性器械末端的预测空间位置，或者一个所述柔性器械末端的预测空间位置和另一个所述柔性器械末端的第三当前空间位置，确定两个所述柔性器械末端具有碰撞风险；

将具有碰撞风险的至少一个所述柔性器械末端的运动路径修正，以得到修正后的运动路径。

11.根据权利要求10所述的方法，其特征在于，所述根据两个所述柔性器械末端的预测空间位置，或者一个所述柔性器械末端的预测空间位置和另一个所述柔性器械末端的第三当前空间位置，确定两个所述柔性器械末端具有碰撞风险包括：

根据两个所述柔性器械末端的预测空间位置，或者一个所述柔性器械末端的预测空间位置和另一个所述柔性器械末端的第三当前空间位置，确定两个所述柔性器械末端之间的预测相撞点；

所述将具有碰撞风险的至少一个所述柔性器械末端的运动路径修正，以得到修正后的运动路径包括：

若所述预测相撞点位于一个或两个所述柔性器械末端的前端区域，对所述一个或两个所述柔性器械末端的运动路径进行修正，使两个所述柔性器械末端在运动时相互绕开，得到所述修正后的运动路径；

若所述预测相撞点位于两个所述柔性器械末端的后端区域，对所述两个所述柔性器械末端的运动路径进行修正，使两个所述柔性器械末端在相撞之前停止运动，得到所述修正后的运动路径。

12.一种柔性器械末端的控制装置，其特征在于，所述装置包括：

位置确定模块，用于确定所述柔性器械末端的可运动空间的极限位置；

指令接收模块，用于接收所述柔性器械末端对应的控制手柄的输入指令；

路径修正模块，用于当所述柔性器械末端达到所述可运动空间的极限位置时，根据所述柔性器械末端的第一当前空间位置和输入指令，将超出所述可运动空间的极限位置的所述柔性器械末端的运动路径修正，以得到修正后的运动路径；

运动控制模块，用于控制所述柔性器械末端，按照所述修正后的运动路径进行运动。

13.根据权利要求12所述的装置，其特征在于，所述位置确定模块包括：

第一位置确定子模块，用于控制所述柔性器械末端分别向偏航的两个极限位置和俯仰的两个极限位置进行运动，确定所述偏航的两个极限位置和俯仰的两个极限位置；

第二位置确定子模块，用于根据所述偏航的两个极限位置和俯仰的两个极限位置，以及所述柔性器械末端的预设空间模型，确定所述柔性器械末端的可运动空间的极限位置。

14.一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1-11中任一所述的方法。

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

Images