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

Abstract

本发明实施例公开了一种柔性器械末端的控制方法、装置、设备和介质。所述方法包括：分别确定两个柔性器械末端的可运动空间，控制柔性器械末端，在可运动空间中进行运动，在柔性器械末端的运动过程中，分别获取两个柔性器械末端的第一当前空间位置，当柔性器械末端的第一当前空间位置与另一个柔性器械末端的可运动空间具有重合部分时，将另一个柔性器械末端的可运动空间中重合部分剔除，使得柔性器械末端可以稳定的在可运动空间内运动，避免机械结构损伤，并且根据实时位置对可运动空间进行实时更新，避免两个柔性器械末端运动到对方所处的位置，从而避免相撞，提高了可运动空间的合理性，继而在实时更新的可运动空间内运动时更安全可靠。

Description

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

技术领域

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

背景技术

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

在两个柔性器械进行协同工作时，两个协作的柔性器械在运行过程中可能会发生碰撞。

发明内容

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

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

分别确定两个所述柔性器械末端的可运动空间；

控制所述柔性器械末端，在所述柔性器械末端的可运动空间中进行运动；

在所述柔性器械末端的运动过程中，分别获取两个所述柔性器械末端的第一当前空间位置；

当所述柔性器械末端的第一当前空间位置与另一个所述柔性器械末端的可运动空间具有重合部分时，将所述另一个柔性器械末端的可运动空间中所述重合部分剔除。

可选地，所述柔性器械末端与对应的柔性器械中端连接，所述分别确定两个所述柔性器械末端的可运动空间包括：

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

根据所述开合角度，分别确定两个所述柔性器械末端的可运动空间。

可选地，所述根据所述开合角度，分别确定两个所述柔性器械末端的可运动空间包括：

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

根据所述开合角度、所述偏航的两个极限位置、所述俯仰的两个极限位置，以及所述柔性器械末端的预设空间模型，分别确定对应的所述柔性器械末端的可运动空间。

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

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

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

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

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

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

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

可选地，所述柔性器械末端在工装平面上运动，所述工装平面上安装有陀螺仪，所述工装平面的上方安装有摄像头，在所述根据所述开合角度，分别确定两个所述柔性器械末端的可运动空间之前，所述方法还包括：

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

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

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

根据所述开合角度，确定所述柔性器械末端在没有进行运动时与所述工作三角区的中心点之间的偏移距离；

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

根据所述偏移距离、位置限制信息和所述椭球的部分空间模型，分别确定对应的所述柔性器械末端的可运动空间。

可选地，以所述工作三角区的中心点为原点建立XYZ三维坐标系，所述根据所述偏航的两个极限位置和所述俯仰的两个极限位置，确定所述柔性器械末端在所述椭球中可到达的位置限制信息包括：

针对每个所述柔性器械末端，计算所述偏航的两个极限位置与Z轴之间的距离的最小值，作为偏航最小值，以及所述俯仰的两个极限位置与Z轴之间的距离的最小值，作为俯仰最小值；

针对左侧的所述柔性器械末端，所述根据所述偏移距离、位置限制信息和所述椭球的部分空间模型，分别确定对应的所述柔性器械末端的可运动空间包括：

针对右侧的所述柔性器械末端，所述根据所述偏移距离、位置限制信息和所述椭球的部分空间模型，分别确定对应的所述柔性器械末端的可运动空间包括：

其中，x为所述XYZ三维坐标系中对应X轴的坐标值，y为所述XYZ三维坐标系中对应Y轴的坐标值，z为所述XYZ三维坐标系中对应Z轴的坐标值，n为所述偏移距离，k为左侧的所述柔性器械末端的所述偏航的两个极限位置与Z轴之间的距离的偏航最小值，d为右侧的所述柔性器械末端的所述偏航的两个极限位置与Z轴之间的距离的偏航最小值，L为左侧的所述柔性器械末端的所述俯仰的两个极限位置与Z轴之间的距离的俯仰最小值，e为右侧的所述柔性器械末端的所述俯仰的两个极限位置与Z轴之间的距离的俯仰最小值，a和b为所述椭球的部分空间模型确定的椭球的赤道半径，c为所述椭球的部分空间模型确定的所述柔性器械末端的长度。

可选地，所述控制所述柔性器械末端，在所述柔性器械末端的可运动空间中进行运动包括：

获取所述柔性器械末端的当前线速度；

计算所述柔性器械末端从对应的所述当前线速度降低至零所需的第一减速距离，以及所述柔性器械末端在与所述可运动空间的极限位置的不同距离处的第一最大允许速度；

在所述柔性器械末端的运动过程中，若所述柔性器械末端与所述可运动空间的极限位置之间的距离达到所述第一减速距离，则根据所述第一最大允许速度，对所述柔性器械末端的运动速度进行限速，使所述柔性器械末端足以在达到所述可运动空间的极限位置时运动速度降低至零。

可选地，当两个所述柔性器械末端同向运动时，所述控制所述柔性器械末端，在所述柔性器械末端的可运动空间中进行运动包括：

分别获取两个所述柔性器械末端的当前线速度；

当位于后方的所述柔性器械末端的当前线速度大于位于前方的所述柔性器械末端的当前线速度，计算将位于后方的所述柔性器械末端的当前线速度降低至位于前方的所述柔性器械末端的当前线速度所需的第二减速距离，以及位于后方的所述柔性器械末端在与所述位于前方的所述柔性器械末端的不同距离处的第二最大允许速度；

在所述柔性器械末端的运动过程中，若位于后方的所述柔性器械末端与位于前方的所述柔性器械末端之间的距离达到所述第二减速距离，则根据所述第二最大允许速度，对位于后方的所述柔性器械末端的运动速度进行限速，使两个所述柔性器械末端避免相撞。

可选地，当两个所述柔性器械末端相向运动时，所述控制所述柔性器械末端，在所述柔性器械末端的可运动空间中进行运动包括：

分别获取两个所述柔性器械末端的当前线速度；

分别计算两个所述柔性器械末端从对应的所述当前线速度降低至零所需的第三减速距离，以及两个所述柔性器械末端在不同距离处的第三最大允许速度；

在所述柔性器械末端的运动过程中，若两个所述柔性器械末端之间的距离达到所述第三减速距离，则根据所述第三最大允许速度，对两个所述柔性器械末端的运动速度进行限速，使两个所述柔性器械末端避免相撞。

可选地，所述在所述柔性器械末端的运动过程中，分别获取两个所述柔性器械末端的第一当前空间位置包括：

分别获取两个所述柔性器械末端的控制电机的运行信息；

根据所述控制电机的运行信息，以及所述柔性器械末端的空间位置和所述控制电机的运行信息的关联关系，分别确定两个所述柔性器械末端的第一当前空间位置。

可选地，在所述在所述柔性器械末端的运动过程中，分别获取两个所述柔性器械末端的第一当前空间位置之前，所述方法还包括：

采集所述柔性器械末端的控制电机的运行信息样本，以及对应的所述柔性器械末端的空间位置样本；

根据所述运行信息样本和空间位置样本，建立所述柔性器械末端的空间位置和所述控制电机的运行信息的关联关系。

可选地，所述柔性器械末端在工装平面上运动，所述工装平面上安装有陀螺仪，所述工装平面上方安装有摄像头，所述控制电机上安装有磁传感编码器，所述采集所述柔性器械末端的控制电机的运行信息样本，以及对应的所述柔性器械末端的空间位置样本包括：

获取所述陀螺仪的数据，确定所述柔性器械末端运动时所在平面的平面位置信息；

通过所述摄像头，检测所述柔性器械末端是否达到预设的目标采样点；

当所述柔性器械末端的控制电机出现堵转，则确定所述柔性器械末端达到极限位置；

在所述柔性器械末端达到所述目标采样点和/或极限位置时，将所述平面位置信息和所述目标采样点和/或极限位置的位置信息作为所述空间位置样本，并读出所述磁传感编码器的数据，作为所述运行信息样本。

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

空间确定模块，用于分别确定两个所述柔性器械末端的可运动空间；

运动控制模块，用于控制所述柔性器械末端，在所述柔性器械末端的可运动空间中进行运动；

位置获取模块，用于在所述柔性器械末端的运动过程中，分别获取两个所述柔性器械末端的第一当前空间位置；

剔除模块，用于当所述柔性器械末端的第一当前空间位置与另一个所述柔性器械末端的可运动空间具有重合部分时，将所述另一个柔性器械末端的可运动空间中所述重合部分剔除。

可选地，所述柔性器械末端与对应的柔性器械中端连接，所述空间确定模块包括：

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

空间确定子模块，用于根据所述开合角度，分别确定两个所述柔性器械末端的可运动空间。

可选地，所述空间确定子模块包括：

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

空间确定单元，用于根据所述开合角度、所述偏航的两个极限位置、所述俯仰的两个极限位置，以及所述柔性器械末端的预设空间模型，分别确定对应的所述柔性器械末端的可运动空间。

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

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

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

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

可选地，所述第一位置确定子单元具体用于：

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

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

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

样本采集模块，用于在所述根据所述开合角度，分别确定两个所述柔性器械末端的可运动空间之前，针对每个所述柔性器械末端，利用所述陀螺仪和摄像头，采集所述柔性器械末端的多个空间位置样本；

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

可选地，所述预设空间模型为椭球的部分空间模型；所述空间确定单元包括：

距离确定子单元，用于根据所述开合角度，确定所述柔性器械末端在没有进行运动时与所述工作三角区的中心点之间的偏移距离；

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

空间确定子单元，用于根据所述偏移距离、位置限制信息和所述椭球的部分空间模型，分别确定对应的所述柔性器械末端的可运动空间。

可选地，以所述工作三角区的中心点为原点建立XYZ三维坐标系，所述信息确定子单元具体用于：

针对每个所述柔性器械末端，计算所述偏航的两个极限位置与Z轴之间的距离的最小值，作为偏航最小值，以及所述俯仰的两个极限位置与Z轴之间的距离的最小值，作为俯仰最小值；

针对左侧的所述柔性器械末端，所述空间确定子单元具体用于：

针对右侧的所述柔性器械末端，所述空间确定子单元具体用于：

其中，x为所述XYZ三维坐标系中对应X轴的坐标值，y为所述XYZ三维坐标系中对应Y轴的坐标值，z为所述XYZ三维坐标系中对应Z轴的坐标值，n为所述偏移距离，k为左侧的所述柔性器械末端的所述偏航的两个极限位置与Z轴之间的距离的偏航最小值，d为右侧的所述柔性器械末端的所述偏航的两个极限位置与Z轴之间的距离的偏航最小值，L为左侧的所述柔性器械末端的所述俯仰的两个极限位置与Z轴之间的距离的俯仰最小值，e为右侧的所述柔性器械末端的所述俯仰的两个极限位置与Z轴之间的距离的俯仰最小值，a和b为所述椭球的部分空间模型确定的椭球的赤道半径，c为所述椭球的部分空间模型确定的所述柔性器械末端的长度。

可选地，所述运动控制模块包括：

第一速度获取子模块，用于获取所述柔性器械末端的当前线速度；

第一速度计算子模块，用于计算所述柔性器械末端从对应的所述当前线速度降低至零所需的第一减速距离，以及所述柔性器械末端在与所述可运动空间的极限位置的不同距离处的第一最大允许速度；

第一限速子模块，用于在所述柔性器械末端的运动过程中，若所述柔性器械末端与所述可运动空间的极限位置之间的距离达到所述第一减速距离，则根据所述第一最大允许速度，对所述柔性器械末端的运动速度进行限速，使所述柔性器械末端足以在达到所述可运动空间的极限位置时运动速度降低至零。

可选地，当两个所述柔性器械末端同向运动时，所述运动控制模块包括：

第二速度获取子模块，用于分别获取两个所述柔性器械末端的当前线速度；

第二速度计算子模块，用于当位于后方的所述柔性器械末端的当前线速度大于位于前方的所述柔性器械末端的当前线速度，计算将位于后方的所述柔性器械末端的当前线速度降低至位于前方的所述柔性器械末端的当前线速度所需的第二减速距离，以及位于后方的所述柔性器械末端在与所述位于前方的所述柔性器械末端的不同距离处的第二最大允许速度；

第二限速子模块，用于在所述柔性器械末端的运动过程中，若位于后方的所述柔性器械末端与位于前方的所述柔性器械末端之间的距离达到所述第二减速距离，则根据所述第二最大允许速度，对位于后方的所述柔性器械末端的运动速度进行限速，使两个所述柔性器械末端避免相撞。

可选地，当两个所述柔性器械末端相向运动时，所述运动控制模块包括：

第三速度获取子模块，用于分别获取两个所述柔性器械末端的当前线速度；

第三速度计算子模块，用于分别计算两个所述柔性器械末端从对应的所述当前线速度降低至零所需的第三减速距离，以及两个所述柔性器械末端在不同距离处的第三最大允许速度；

第三限速子模块，用于在所述柔性器械末端的运动过程中，若两个所述柔性器械末端之间的距离达到所述第三减速距离，则根据所述第三最大允许速度，对两个所述柔性器械末端的运动速度进行限速，使两个所述柔性器械末端避免相撞。

可选地，所述位置获取模块包括：

运行信息获取子模块，用于分别获取两个所述柔性器械末端的控制电机的运行信息；

空间位置确定子模块，用于根据所述控制电机的运行信息，以及所述柔性器械末端的空间位置和所述控制电机的运行信息的关联关系，分别确定两个所述柔性器械末端的第一当前空间位置。

可选地，所述装置还包括：

样本采集模块，用于在所述在所述柔性器械末端的运动过程中，分别获取两个所述柔性器械末端的第一当前空间位置之前，采集所述柔性器械末端的控制电机的运行信息样本，以及对应的所述柔性器械末端的空间位置样本；

关系建立模块，用于根据所述运行信息样本和空间位置样本，建立所述柔性器械末端的空间位置和所述控制电机的运行信息的关联关系。

可选地，所述柔性器械末端在工装平面上运动，所述工装平面上安装有陀螺仪，所述工装平面上方安装有摄像头，所述控制电机上安装有磁传感编码器，所述样本采集模块包括：

平面位置信息确定子模块，用于获取所述陀螺仪的数据，确定所述柔性器械末端运动时所在平面的平面位置信息；

检测子模块，用于通过所述摄像头，检测所述柔性器械末端是否达到预设的目标采样点；

极限位置确定子模块，用于当所述柔性器械末端的控制电机出现堵转，则确定所述柔性器械末端达到极限位置；

样本获得子模块，用于在所述柔性器械末端达到所述目标采样点和/或极限位置时，将所述平面位置信息和所述目标采样点和/或极限位置的位置信息作为所述空间位置样本，并读出所述磁传感编码器的数据，作为所述运行信息样本。

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

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

依据本发明实施例，通过分别确定两个所述柔性器械末端的可运动空间，控制所述柔性器械末端，在所述柔性器械末端的可运动空间中进行运动，在所述柔性器械末端的运动过程中，分别获取两个所述柔性器械末端的第一当前空间位置，当所述柔性器械末端的第一当前空间位置与另一个所述柔性器械末端的可运动空间具有重合部分时，将所述另一个柔性器械末端的可运动空间中所述重合部分剔除，使得两个柔性器械末端可以稳定的在可运动空间内运动，不会超出机械结构的物理限制，避免机械结构损伤，并且根据两个柔性器械末端的实时位置对可运动空间进行实时更新，避免两个柔性器械末端运动到对方所处的位置，从而避免两个柔性器械末端相撞，提高了可运动空间的合理性，继而在实时更新的可运动空间内运动时更安全可靠。

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

附图说明

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

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

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

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

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

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

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

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

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

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

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

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

图12示出了两个柔性器械末端的可运动空间所在椭球的示意图；

图13示出了速度分解的示意图；

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

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

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

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

具体实施方式

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

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

步骤101，分别确定两个所述柔性器械末端的可运动空间。

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

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

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

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

步骤102，控制所述柔性器械末端，在所述柔性器械末端的可运动空间中进行运动。

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

在本发明实施例中，控制所述柔性器械末端，在所述柔性器械末端的可运动空间中进行运动的具体实现方式可以包括多种。例如，在柔性器械末端的运动过程中，若柔性器械末端与可运动空间的极限位置之间的距离达到最小减速距离，则对柔性器械末端的运动速度进行限速，使柔性器械末端足以在达到可运动空间的极限位置时运动速度降低至零，以使柔性器械末端在可运动空间中进行运动。又例如，在柔性器械末端的运动过程中，当柔性器械末端的运动路径将超出可运动空间的范围对运动路径进行修正，以得到修正后的运动路径，控制柔性器械末端，按照修正后的运动路径进行运动，以使柔性器械末端在可运动空间中进行运动。具体可以采用任意适用的方式，控制所述柔性器械末端，在所述柔性器械末端的可运动空间中进行运动，本发明实施例对此不做限制。

步骤103，在所述柔性器械末端的运动过程中，分别获取两个所述柔性器械末端的第一当前空间位置。

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

步骤104，当所述柔性器械末端的第一当前空间位置与另一个所述柔性器械末端的可运动空间具有重合部分时，将所述另一个柔性器械末端的可运动空间中所述重合部分剔除。

在本发明实施例中，根据步骤102所得到的可运动空间，分析两个柔性器械末端可能相撞的情况，为了防止相撞的情况，分别对两个柔性器械末端的可运动空间在步骤102所得的可运动空间的基础上添加限制。

例如，如图4所示的柔性器械末端的正视图，由图4可知柔性器械末端的正视图为一个圆，柔性器械末端可看作为一个高度变化的圆柱体，两个柔性器械末端只能在步骤102所确定的可运动空间中运动。设柔性器械末端正视图圆的半径为r，则两个圆柱体的中心线线段之间的空间距离大于2r才能保证不会相撞。设左侧柔性器械末端圆心在坐标系中的坐标为Q1（x1，y1，z1），椭球球心的坐标为Q2（-n，0，0），可得椭球心到此点Q1的中心线线段L1的表达式为：

右侧柔性器械末端圆心在坐标系中的坐标为P1（x2，y2，z2），椭球球心的坐标为P2（-n，0，0），可得椭球心到此点P1的中心线线段L2的表达式为：

其中，

表示左侧柔性器械末端的中心线线段，

表示右侧柔性器械末端 的中心线线段，

是表示线段

中的点的变量，

是表示线段

中的点的变量，

表示从点Q2到点Q1的向量，

表示从点P2到点P1的向量。

若两个柔性器械末端不相撞则需要上述两条中心线线段最短空间距离不得小于2r。

将空间两条中心线线段最短距离转换为有约束的最优化问题:

由极小值条件：

，

可得：

则

。

因为处理器程序可以获取到柔性器械末端的第一当前空间位置，所以处理器程序可以根据上述两个柔性器械末端不相撞的判定条件，实时把左侧柔性器械末端所在的第一当前空间位置从右侧柔性器械末端的可运动空间中剔除，把右侧柔性器械所在的第一当前空间位置从左侧柔性器械末端的可运动空间中剔除，从而实时更新两个柔性器械末端的可运动空间。也就是说，当柔性器械末端的第一当前空间位置与另一个柔性器械末端的可运动空间具有重合部分时，将另一个柔性器械末端的可运动空间中重合部分剔除。

依据本发明实施例，通过分别确定两个所述柔性器械末端的可运动空间，控制所述柔性器械末端，在所述柔性器械末端的可运动空间中进行运动，在所述柔性器械末端的运动过程中，分别获取两个所述柔性器械末端的第一当前空间位置，当所述柔性器械末端的第一当前空间位置与另一个所述柔性器械末端的可运动空间具有重合部分时，将所述另一个柔性器械末端的可运动空间中所述重合部分剔除，使得两个柔性器械末端可以稳定的在可运动空间内运动，不会超出机械结构的物理限制，避免机械结构损伤，并且根据两个柔性器械末端的实时位置对可运动空间进行实时更新，避免两个柔性器械末端运动到对方所处的位置，从而避免两个柔性器械末端相撞，提高了可运动空间的合理性，继而在实时更新的可运动空间内运动时更安全可靠。

在本发明的一种可选实施例中，如图5所示，所述柔性器械末端与对应的柔性器械中端连接，上述步骤101包括：

步骤1011，获取两个所述柔性器械中端所形成的工作三角区的开合角度。

步骤1012，根据所述开合角度，分别确定两个所述柔性器械末端的可运动空间。

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

柔性器械中端所形成的工作三角区的开合角度可以直接获取到。例如，柔性器械在开始工作时可以通过旋转图3所示的三角区调整机构468中的工作三角区的手动旋钮来控制工作三角区的开合大小，同时此旋钮为一个电位计，处理器程序通过采集此电位计的电压大小来获取旋钮旋转的角度，进而获取工作三角区的开合角度的度数。工作三角区不同的开合度数如图6和图7所示。

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

例如，根据开合角度和已知的工作三角区的机械长度，控制程序可以计算得出两个柔性器械末端在工作三角区打开后的距离，即图6所示线段ab的距离，假设为2n，也就是两个柔性器械末端在没有进行弯曲运动时初始位置的距离为2n。在以工作三角区的中心点处建立如图7所示的坐标系XYZ，则在该坐标系中，两个柔性器械末端的可运动空间的中心点在X轴上的距离为2n。

在本发明的一种可选实施例中，根据所述开合角度，分别确定两个所述柔性器械末端的可运动空间的一种具体实现方式可以包括：

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

根据所述开合角度、所述偏航的两个极限位置、所述俯仰的两个极限位置，以及所述柔性器械末端的预设空间模型，分别确定对应的所述柔性器械末端的可运动空间。

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

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

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

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

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

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

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

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

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

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

在本发明的一种可选实施例中，根据所述柔性器械末端的第二当前空间位置，确定所述极限位置的一种具体实现方式可以包括：若第二当前空间位置与预设运动空间的极限位置的最小距离小于预设阈值，则将第二当前空间位置作为极限位置；若第二当前空间位置与预设运动空间的极限位置的最小距离不小于预设阈值，则上报错误。

在本发明的一种可选实施例中，所述柔性器械末端在工装平面上运动，所述工装平面上安装有陀螺仪，所述工装平面的上方安装有摄像头，在所述根据所述开合角度，分别确定两个所述柔性器械末端的可运动空间之前，还可以包括：

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

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

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

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

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

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

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

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

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

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

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

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

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

根据所述开合角度，确定所述柔性器械末端在没有进行运动时与所述工作三角区的中心点之间的偏移距离；

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

根据所述偏移距离、位置限制信息和所述椭球的部分空间模型，分别确定对应的所述柔性器械末端的可运动空间。

由于已知工作三角区的机械长度，根据开合角度和机械长度，可以计算出柔性器械末端在没有进行运动时与工作三角区的中心点之间的距离，记为偏移距离。例如，已经通过读取工作三角区的开合角度，获取了左右两个柔性器械末端在没有进行弯曲运动时初始位置的距离为2n，则两个柔性器械末端在没有进行运动时与工作三角区的中心点之间的偏移距离为n。

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

将偏移距离和位置限制信息代入椭球的部分空间模型中，从而确定对应的柔性器械末端的可运动空间，得到可运动空间的预设空间模型为椭球的部分空间模型。最后确定柔性器械末端的可运动空间的建模如图11所示，为椭球的一部分。如图12所示的两个柔性器械末端的可运动空间所在椭球的示意图，两个可运动空间存在共同部分。

在本发明的一种可选实施例中，以所述工作三角区的中心点为原点建立XYZ三维坐标系，所述根据所述偏航的两个极限位置和所述俯仰的两个极限位置，确定所述柔性器械末端在所述椭球中可到达的位置限制信息包括：

针对每个所述柔性器械末端，计算所述偏航的两个极限位置与Z轴之间的距离的最小值，作为偏航最小值，以及所述俯仰的两个极限位置与Z轴之间的距离的最小值，作为俯仰最小值。

例如，如图7所示坐标系XYZ，首先控制器程序将下发指令给控制电机，通过控制电机牵拉钢索控制左侧的柔性器械末端沿X轴负坐标轴处进行弯曲运动，直到控制器程序检测到控制电机出现堵转现象，则判定为到达极限位置，同时控制器程序将读取此位置控制电机的传动轴的磁传感编码器的值，计算出极限位置时柔性器械末端的最末端距离Z轴的距离，比如为k1,然后控制器下发指令控制控制电机牵拉钢索控制左侧的柔性器械末端沿X轴正坐标轴处进行弯曲运动，直到控制器程序检测到控制电机出现堵转现象，则判定为到达极限位置，同时控制器程序将读取此位置控制电机的传动轴的磁传感编码器的值，计算出极限位置时柔性器械末端的最末端距离Z轴的距离，比如为k2，控制程序将选取k1与k2中较小的值k（即偏航最小值）进行运动空间计算。同样的，控制器程序控制柔性器械末端沿Y轴运动测试极限位置获取在Y轴方向极限位置距Z轴的距离为L（即俯仰最小值）。同理右侧的柔性器械末端也通过同样的方法测试，设最终获取X轴上极限位置到Z轴的距离为d（即偏航最小值），Y轴上极限位置到Z轴的距离为e（即俯仰最小值）。

针对左侧的所述柔性器械末端，所述根据所述偏移距离、位置限制信息和所述椭球的部分空间模型，分别确定对应的所述柔性器械末端的可运动空间包括：

针对右侧的所述柔性器械末端，所述根据所述偏移距离、位置限制信息和所述椭球的部分空间模型，分别确定对应的所述柔性器械末端的可运动空间包括：

其中，x为所述XYZ三维坐标系中对应X轴的坐标值，y为所述XYZ三维坐标系中对应Y轴的坐标值，z为所述XYZ三维坐标系中对应Z轴的坐标值，n为偏移距离，k为左侧的所述柔性器械末端的所述偏航的两个极限位置与Z轴之间的距离的偏航最小值，d为右侧的所述柔性器械末端的所述偏航的两个极限位置与Z轴之间的距离的偏航最小值，L为左侧的所述柔性器械末端的所述俯仰的两个极限位置与Z轴之间的距离的俯仰最小值，e为右侧的所述柔性器械末端的所述俯仰的两个极限位置与Z轴之间的距离的俯仰最小值，a和b为所述椭球的部分空间模型确定的椭球的赤道半径，c为所述椭球的部分空间模型确定的所述柔性器械末端的长度。

在本发明的一种可选实施例中，控制所述柔性器械末端，在所述柔性器械末端的可运动空间中进行运动的一种具体实现方式可以包括：

获取所述柔性器械末端的当前线速度；

计算所述柔性器械末端从对应的所述当前线速度降低至零所需的第一减速距离，以及所述柔性器械末端在与所述可运动空间的极限位置的不同距离处的第一最大允许速度；

在所述柔性器械末端的运动过程中，若所述柔性器械末端与所述可运动空间的极限位置之间的距离达到所述第一减速距离，则根据所述第一最大允许速度，对所述柔性器械末端的运动速度进行限速，使所述柔性器械末端足以在达到所述可运动空间的极限位置时运动速度降低至零。

由于柔性器械末端的当前空间位置可以被直接获取，那么通过当前时刻之前的一段时间的空间位置，就可以推算出柔性器械末端在运动过程中的当前线速度。柔性器械末端在减速时的最大加速度是已知的，具体可以通过测定得到。因此，可以根据柔性器械末端的当前线速度，以及减速时的最大加速度，计算出从对应的所述当前线速度降低至零所需的第一减速距离，以及柔性器械末端在与可运动空间的极限位置的不同距离处的最大允许速度，记为第一最大允许速度。

当柔性器械末端与可运动空间的极限位置之间的距离达到第一减速距离，则对柔性器械末端进行减速，具体为根据柔性器械末端与可运动空间的极限位置之间的距离对应的第一最大允许速度，对柔性器械末端的运动速度进行限速。若根据当前的输入指令，柔性器械末端的运动速度会超过第一最大允许速度，则将运动速度限制在第一最大允许速度。若根据当前的输入指令，柔性器械末端的运动速度不会超过第一最大允许速度，则无需对运动速度进行限速。

例如，在处理器程序控制柔性器械末端运动时，通过磁传感编码器的实时反馈，可以得到柔性器械末端的当前空间位置，以及当前线速度。处理器程序会实时检测当前空间位置与极限位置的距离，即当前空间位置距X和Y两个变量极限位置的距离。如图13所示的速度分解的示意图，并将当前线速度矢量AB分解到XYZ坐标轴上，分别得到X轴和Y轴方向的速度矢量。根据X轴和Y轴方向的速度，通过速度公式可以计算得出为了不超出可运动空间限定每个坐标轴的第一减速距离，即也确定了在距离极限位置不同距离处的不同的第一最大允许速度。比如在距离极限位置距离为x1，在这个距离内只有速度在v1之内才能确保在超出可运动空间之内停止，所以在x1距离处，柔性器械末端可以达到的最大速度为v1。

如果一个柔性器械末端运动，另一个柔性器械末端静止，此场景与上述处理方式一致，不再赘述。

通过对柔性器械末端的运动速度进行限速，实现对柔性器械末端的运动速度进行预判处理，保证柔性器械末端可以稳定的在可运动空间内运行，不会超限，避免机械结构的永久损伤，而且避免了两个柔性器械末端因速度过快，来不及减速导致的相撞。

在本发明的一种可选实施例中，当两个所述柔性器械末端同向运动时，所述控制所述柔性器械末端，在所述柔性器械末端的可运动空间中进行运动的一种具体实现方式可以包括：

分别获取两个所述柔性器械末端的当前线速度；

当位于后方的所述柔性器械末端的当前线速度大于位于前方的所述柔性器械末端的当前线速度，计算将位于后方的所述柔性器械末端的当前线速度降低至位于前方的所述柔性器械末端的当前线速度所需的第二减速距离，以及位于后方的所述柔性器械末端在与所述位于前方的所述柔性器械末端的不同距离处的第二最大允许速度；

在所述柔性器械末端的运动过程中，若位于后方的所述柔性器械末端与位于前方的所述柔性器械末端之间的距离达到所述第二减速距离，则根据所述第二最大允许速度，对位于后方的所述柔性器械末端的运动速度进行限速，使两个所述柔性器械末端避免相撞。

当位于后方的所述柔性器械末端的当前线速度大于位于前方的所述柔性器械末端的当前线速度，那么当位于后方的所述柔性器械末端追上位于前方的所述柔性器械末端时，两者将相撞。

根据位于后方的柔性器械末端的当前线速度，以及减速时的最大加速度，计算出从位于后方的所述柔性器械末端的当前线速度降低至位于前方的所述柔性器械末端的当前线速度所需的第二减速距离，以及位于后方的柔性器械末端在与位于前方的柔性器械末端的不同距离处的最大允许速度，记为第二最大允许速度。

当位于后方的柔性器械末端与位于前方的柔性器械末端之间的距离达到所述第二减速距离，则对位于后方的柔性器械末端进行减速，具体为根据两个柔性器械末端之间的距离对应的第二最大允许速度，对位于后方的柔性器械末端的运动速度进行限速。若根据当前的输入指令，位于后方的柔性器械末端的运动速度会超过第二最大允许速度，则将运动速度限制在第二最大允许速度。若根据当前的输入指令，柔性器械末端的运动速度不会超过第二最大允许速度，则无需对运动速度进行限速。

例如，两个柔性器械末端同向运动，将当前线速度矢量分解到XYZ坐标轴上，分别得到X轴和Y轴方向的速度矢量。分别从X轴和Y轴上分析，后方柔性器械末端的运动速度大于前方的柔性器械末端，此种情况要防止“追尾”情况发生，所以必须在两个柔性器械末端之间的最短空间距离小于2r之前，使后方的柔性器械末端速度小于或者等于前方的柔性器械末端，由速度公式可以求得后方的柔性器械末端减速到和前方的柔性器械末端速度相同时需要的时间，可以求得后方的柔性器械末端的第二减速距离。即也确定了在不同位置距离处的不同最大允许速度。

通过对柔性器械末端的运动速度进行限速，实现对柔性器械末端的运动速度进行预判处理，避免了两个柔性器械末端因速度过快，来不及减速导致的相撞。

在本发明的一种可选实施例中，当两个所述柔性器械末端相向运动时，所述控制所述柔性器械末端，在所述柔性器械末端的可运动空间中进行运动的一种具体实现方式可以包括：

分别获取两个所述柔性器械末端的当前线速度；

分别计算两个所述柔性器械末端从对应的所述当前线速度降低至零所需的第三减速距离，以及两个所述柔性器械末端在不同距离处的第三最大允许速度；

在所述柔性器械末端的运动过程中，若两个所述柔性器械末端之间的距离达到所述第三减速距离，则根据所述第三最大允许速度，对两个所述柔性器械末端的运动速度进行限速，使两个所述柔性器械末端避免相撞。

根据两个柔性器械末端的当前线速度，以及减速时的最大加速度，计算出两个柔性器械末端从当前线速度降低至0所需的第三减速距离，以及两个柔性器械末端在不同距离处的最大允许速度，记为第三最大允许速度。

当两个柔性器械末端之间的距离达到所述第三减速距离，则对两个性器械末端进行减速，具体为根据两个柔性器械末端之间的距离对应的第三最大允许速度，对两个柔性器械末端的运动速度进行限速。若根据当前的输入指令，两个柔性器械末端的运动速度会超过第三最大允许速度，则将运动速度限制在第三最大允许速度。若根据当前的输入指令，柔性器械末端的运动速度不会超过第三最大允许速度，则无需对运动速度进行限速。

例如，两个柔性器械末端相向运动时，将当前线速度矢量分解到XYZ坐标轴上，分别得到X轴和Y轴方向的速度矢量。为了防止两个器械相撞，必须在两个柔性器械末端的最短空间距离小于2r之前两个柔性器械末端的速度都减为0，分别计算出两个柔性器械末端从当前线速度减速到0所需要的距离，如第三减速距离分别为S5、S6，则两个柔性器械必须在相距S5+S6时同时进行减速。

通过对柔性器械末端的运动速度进行限速，实现对柔性器械末端的运动速度进行预判处理，避免了两个柔性器械末端因速度过快，来不及减速导致的相撞。

在本发明的一种可选实施例中，在所述柔性器械末端的运动过程中，分别获取两个所述柔性器械末端的第一当前空间位置的一种具体实现方式可以包括：

分别获取两个所述柔性器械末端的控制电机的运行信息；

根据所述控制电机的运行信息，以及所述柔性器械末端的空间位置和所述控制电机的运行信息的关联关系，分别确定两个所述柔性器械末端的第一当前空间位置。

运行信息包括控制电机的磁传感编码器的数据等，或者其他任意适用的信息，本发明实施例对此不做限制。柔性器械末端的空间位置和控制电机的运行信息的关联关系可以预先确定。在柔性器械末端的运动过程中，分别获取两个柔性器械末端的控制电机的运行信息，然后根据预先确定的关联关系，查找对应的位置信息，作为第一当前空间位置。

在本发明的一种可选实施例中，在所述在所述柔性器械末端的运动过程中，分别获取两个所述柔性器械末端的当前空间位置之前，所述方法还包括：

采集所述柔性器械末端的控制电机的运行信息样本，以及对应的所述柔性器械末端的空间位置样本；

根据所述运行信息样本和空间位置样本，建立所述柔性器械末端的空间位置和所述控制电机的运行信息的关联关系。

控制柔性器械末端进行运动，大量采集柔性器械末端的控制电机的运行信息样本以及对应的空间位置样本。然后拟合出柔性器械末端的空间位置和控制电机的运行信息之间的关联关系。

在本发明的一种可选实施例中，柔性器械末端在工装平面上运动，所述工装平面上安装有陀螺仪，所述工装平面上方安装有摄像头，所述控制电机上安装有磁传感编码器，所述采集所述柔性器械末端的控制电机的运行信息样本，以及对应的所述柔性器械末端的空间位置样本包括：

获取所述陀螺仪的数据，确定所述柔性器械末端运动时所在平面的平面位置信息；

通过所述摄像头，检测所述柔性器械末端是否达到预设的目标采样点；

当所述柔性器械末端的控制电机出现堵转，则确定所述柔性器械末端达到极限位置；

在所述柔性器械末端达到所述目标采样点和/或极限位置时，将所述平面位置信息和所述目标采样点和/或极限位置的位置信息作为所述空间位置样本，并读出所述磁传感编码器的数据，作为所述运行信息样本。

为了提高空间位置样本的准确性，通过使用相应的工装，来对柔性器械末端在空间上的运动范围进行大量采样。让柔性器械末端在工装平面上运动，通过陀螺仪的数据，确定柔性器械末端运动时所在平面的平面位置信息，通过摄像头，检测柔性器械末端是否达到预设的目标采样点，当柔性器械末端的控制电机出现堵转，则确定柔性器械末端达到极限位置。然后在柔性器械末端达到目标采样点和/或极限位置时，将平面位置信息和目标采样点和/或极限位置的位置信息作为空间位置样本。

例如，由于柔性器械末端的机械结构属性问题，柔性器械末端的弯曲部位随着弯曲度数的变化会有些许变化，这将会导致通过控制电机转动牵拉钢索的长度与柔性器械末端实际产生弯曲的度数比例呈现非固定比例的问题，使得在控制程序中，对柔性器械末端角度的预估出现偏差。其中，工装如图8所示。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

空间确定模块201，用于分别确定两个所述柔性器械末端的可运动空间；

运动控制模块202，用于控制所述柔性器械末端，在所述柔性器械末端的可运动空间中进行运动；

位置获取模块203，用于在所述柔性器械末端的运动过程中，分别获取两个所述柔性器械末端的第一当前空间位置；

剔除模块204，用于当所述柔性器械末端的第一当前空间位置与另一个所述柔性器械末端的可运动空间具有重合部分时，将所述另一个柔性器械末端的可运动空间中所述重合部分剔除。

可选地，所述柔性器械末端与对应的柔性器械中端连接，所述空间确定模块包括：

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

空间确定子模块，用于根据所述开合角度，分别确定两个所述柔性器械末端的可运动空间。

可选地，所述空间确定子模块包括：

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

空间确定单元，用于根据所述开合角度、所述偏航的两个极限位置、所述俯仰的两个极限位置，以及所述柔性器械末端的预设空间模型，分别确定对应的所述柔性器械末端的可运动空间。

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

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

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

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

可选地，所述第一位置确定子单元具体用于：

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

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

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

样本采集模块，用于在所述根据所述开合角度，分别确定两个所述柔性器械末端的可运动空间之前，针对每个所述柔性器械末端，利用所述陀螺仪和摄像头，采集所述柔性器械末端的多个空间位置样本；

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

可选地，所述预设空间模型为椭球的部分空间模型；所述空间确定单元包括：

距离确定子单元，用于根据所述开合角度，确定所述柔性器械末端在没有进行运动时与所述工作三角区的中心点之间的偏移距离；

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

空间确定子单元，用于根据所述偏移距离、位置限制信息和所述椭球的部分空间模型，分别确定对应的所述柔性器械末端的可运动空间。

可选地，以所述工作三角区的中心点为原点建立XYZ三维坐标系，所述信息确定子单元具体用于：

针对每个所述柔性器械末端，计算所述偏航的两个极限位置与Z轴之间的距离的最小值，作为偏航最小值，以及所述俯仰的两个极限位置与Z轴之间的距离的最小值，作为俯仰最小值；

针对左侧的所述柔性器械末端，所述空间确定子单元具体用于：

针对右侧的所述柔性器械末端，所述空间确定子单元具体用于：

其中，x为所述XYZ三维坐标系中对应X轴的坐标值，y为所述XYZ三维坐标系中对应Y轴的坐标值，z为所述XYZ三维坐标系中对应Z轴的坐标值，n为所述偏移距离，k为左侧的所述柔性器械末端的所述偏航的两个极限位置与Z轴之间的距离的偏航最小值，d为右侧的所述柔性器械末端的所述偏航的两个极限位置与Z轴之间的距离的偏航最小值，L为左侧的所述柔性器械末端的所述俯仰的两个极限位置与Z轴之间的距离的俯仰最小值，e为右侧的所述柔性器械末端的所述俯仰的两个极限位置与Z轴之间的距离的俯仰最小值，a和b为所述椭球的部分空间模型确定的椭球的赤道半径，c为所述椭球的部分空间模型确定的所述柔性器械末端的长度。

可选地，所述运动控制模块包括：

第一速度获取子模块，用于获取所述柔性器械末端的当前线速度；

第一速度计算子模块，用于计算所述柔性器械末端从对应的所述当前线速度降低至零所需的第一减速距离，以及所述柔性器械末端在与所述可运动空间的极限位置的不同距离处的第一最大允许速度；

第一限速子模块，用于在所述柔性器械末端的运动过程中，若所述柔性器械末端与所述可运动空间的极限位置之间的距离达到所述第一减速距离，则根据所述第一最大允许速度，对所述柔性器械末端的运动速度进行限速，使所述柔性器械末端足以在达到所述可运动空间的极限位置时运动速度降低至零。

可选地，当两个所述柔性器械末端同向运动时，所述运动控制模块包括：

第二速度获取子模块，用于分别获取两个所述柔性器械末端的当前线速度；

第二速度计算子模块，用于当位于后方的所述柔性器械末端的当前线速度大于位于前方的所述柔性器械末端的当前线速度，计算将位于后方的所述柔性器械末端的当前线速度降低至位于前方的所述柔性器械末端的当前线速度所需的第二减速距离，以及位于后方的所述柔性器械末端在与所述位于前方的所述柔性器械末端的不同距离处的第二最大允许速度；

第二限速子模块，用于在所述柔性器械末端的运动过程中，若位于后方的所述柔性器械末端与位于前方的所述柔性器械末端之间的距离达到所述第二减速距离，则根据所述第二最大允许速度，对位于后方的所述柔性器械末端的运动速度进行限速，使两个所述柔性器械末端避免相撞。

可选地，当两个所述柔性器械末端相向运动时，所述运动控制模块包括：

第三速度获取子模块，用于分别获取两个所述柔性器械末端的当前线速度；

第三速度计算子模块，用于分别计算两个所述柔性器械末端从对应的所述当前线速度降低至零所需的第三减速距离，以及两个所述柔性器械末端在不同距离处的第三最大允许速度；

第三限速子模块，用于在所述柔性器械末端的运动过程中，若两个所述柔性器械末端之间的距离达到所述第三减速距离，则根据所述第三最大允许速度，对两个所述柔性器械末端的运动速度进行限速，使两个所述柔性器械末端避免相撞。

可选地，所述位置获取模块包括：

运行信息获取子模块，用于分别获取两个所述柔性器械末端的控制电机的运行信息；

空间位置确定子模块，用于根据所述控制电机的运行信息，以及所述柔性器械末端的空间位置和所述控制电机的运行信息的关联关系，分别确定两个所述柔性器械末端的第一当前空间位置。

可选地，所述装置还包括：

样本采集模块，用于在所述在所述柔性器械末端的运动过程中，分别获取两个所述柔性器械末端的第一当前空间位置之前，采集所述柔性器械末端的控制电机的运行信息样本，以及对应的所述柔性器械末端的空间位置样本；

关系建立模块，用于根据所述运行信息样本和空间位置样本，建立所述柔性器械末端的空间位置和所述控制电机的运行信息的关联关系。

可选地，所述柔性器械末端在工装平面上运动，所述工装平面上安装有陀螺仪，所述工装平面上方安装有摄像头，所述控制电机上安装有磁传感编码器，所述样本采集模块包括：

平面位置信息确定子模块，用于获取所述陀螺仪的数据，确定所述柔性器械末端运动时所在平面的平面位置信息；

检测子模块，用于通过所述摄像头，检测所述柔性器械末端是否达到预设的目标采样点；

极限位置确定子模块，用于当所述柔性器械末端的控制电机出现堵转，则确定所述柔性器械末端达到极限位置；

样本获得子模块，用于在所述柔性器械末端达到所述目标采样点和/或极限位置时，将所述平面位置信息和所述目标采样点和/或极限位置的位置信息作为所述空间位置样本，并读出所述磁传感编码器的数据，作为所述运行信息样本。

依据本发明实施例，通过分别确定两个所述柔性器械末端的可运动空间，控制所述柔性器械末端，在所述柔性器械末端的可运动空间中进行运动，在所述柔性器械末端的运动过程中，分别获取两个所述柔性器械末端的第一当前空间位置，当所述柔性器械末端的第一当前空间位置与另一个所述柔性器械末端的可运动空间具有重合部分时，将所述另一个柔性器械末端的可运动空间中所述重合部分剔除，使得两个柔性器械末端可以稳定的在可运动空间内运动，不会超出机械结构的物理限制，避免机械结构损伤，并且根据两个柔性器械末端的实时位置对可运动空间进行实时更新，避免两个柔性器械末端运动到对方所处的位置，从而避免两个柔性器械末端相撞，提高了可运动空间的合理性，继而在实时更新的可运动空间内运动时更安全可靠。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Claims (20)

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

分别确定两个所述柔性器械末端的可运动空间；

控制所述柔性器械末端，在所述柔性器械末端的可运动空间中进行运动；

在所述柔性器械末端的运动过程中，分别获取两个所述柔性器械末端的第一当前空间位置；

当所述柔性器械末端的第一当前空间位置与另一个所述柔性器械末端的可运动空间具有重合部分时，将所述另一个柔性器械末端的可运动空间中所述重合部分剔除。

2.根据权利要求1所述的方法，其特征在于，所述柔性器械末端与对应的柔性器械中端连接，所述分别确定两个所述柔性器械末端的可运动空间包括：

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

根据所述开合角度，分别确定两个所述柔性器械末端的可运动空间。

3.根据权利要求2所述的方法，其特征在于，所述根据所述开合角度，分别确定两个所述柔性器械末端的可运动空间包括：

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

根据所述开合角度、所述偏航的两个极限位置、所述俯仰的两个极限位置，以及所述柔性器械末端的预设空间模型，分别确定对应的所述柔性器械末端的可运动空间。

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

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

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

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

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

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

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

6.根据权利要求3所述的方法，其特征在于，所述柔性器械末端在工装平面上运动，所述工装平面上安装有陀螺仪，所述工装平面的上方安装有摄像头，在所述根据所述开合角度，分别确定两个所述柔性器械末端的可运动空间之前，所述方法还包括：

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

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

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

根据所述开合角度，确定所述柔性器械末端在没有进行运动时与所述工作三角区的中心点之间的偏移距离；

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

根据所述偏移距离、位置限制信息和所述椭球的部分空间模型，分别确定对应的所述柔性器械末端的可运动空间。

8.根据权利要求7所述的方法，其特征在于，以所述工作三角区的中心点为原点建立XYZ三维坐标系，所述根据所述偏航的两个极限位置和所述俯仰的两个极限位置，确定所述柔性器械末端在所述椭球中可到达的位置限制信息包括：

针对每个所述柔性器械末端，计算所述偏航的两个极限位置与Z轴之间的距离的最小值，作为偏航最小值，以及所述俯仰的两个极限位置与Z轴之间的距离的最小值，作为俯仰最小值；

针对左侧的所述柔性器械末端，所述根据所述偏移距离、位置限制信息和所述椭球的部分空间模型，分别确定对应的所述柔性器械末端的可运动空间包括：

针对右侧的所述柔性器械末端，所述根据所述偏移距离、位置限制信息和所述椭球的部分空间模型，分别确定对应的所述柔性器械末端的可运动空间包括：

其中，x为所述XYZ三维坐标系中对应X轴的坐标值，y为所述XYZ三维坐标系中对应Y轴的坐标值，z为所述XYZ三维坐标系中对应Z轴的坐标值，n为所述偏移距离，k为左侧的所述柔性器械末端的所述偏航的两个极限位置与Z轴之间的距离的偏航最小值，d为右侧的所述柔性器械末端的所述偏航的两个极限位置与Z轴之间的距离的偏航最小值，L为左侧的所述柔性器械末端的所述俯仰的两个极限位置与Z轴之间的距离的俯仰最小值，e为右侧的所述柔性器械末端的所述俯仰的两个极限位置与Z轴之间的距离的俯仰最小值，a和b为所述椭球的部分空间模型确定的椭球的赤道半径，c为所述椭球的部分空间模型确定的所述柔性器械末端的长度。

9.根据权利要求1所述的方法，其特征在于，所述控制所述柔性器械末端，在所述柔性器械末端的可运动空间中进行运动包括：

获取所述柔性器械末端的当前线速度；

计算所述柔性器械末端从对应的所述当前线速度降低至零所需的第一减速距离，以及所述柔性器械末端在与所述可运动空间的极限位置的不同距离处的第一最大允许速度；

在所述柔性器械末端的运动过程中，若所述柔性器械末端与所述可运动空间的极限位置之间的距离达到所述第一减速距离，则根据所述第一最大允许速度，对所述柔性器械末端的运动速度进行限速，使所述柔性器械末端足以在达到所述可运动空间的极限位置时运动速度降低至零。

10.根据权利要求1所述的方法，其特征在于，当两个所述柔性器械末端同向运动时，所述控制所述柔性器械末端，在所述柔性器械末端的可运动空间中进行运动包括：

分别获取两个所述柔性器械末端的当前线速度；

当位于后方的所述柔性器械末端的当前线速度大于位于前方的所述柔性器械末端的当前线速度，计算将位于后方的所述柔性器械末端的当前线速度降低至位于前方的所述柔性器械末端的当前线速度所需的第二减速距离，以及位于后方的所述柔性器械末端在与所述位于前方的所述柔性器械末端的不同距离处的第二最大允许速度；

在所述柔性器械末端的运动过程中，若位于后方的所述柔性器械末端与位于前方的所述柔性器械末端之间的距离达到所述第二减速距离，则根据所述第二最大允许速度，对位于后方的所述柔性器械末端的运动速度进行限速，使两个所述柔性器械末端避免相撞。

11.根据权利要求1所述的方法，其特征在于，当两个所述柔性器械末端相向运动时，所述控制所述柔性器械末端，在所述柔性器械末端的可运动空间中进行运动包括：

分别获取两个所述柔性器械末端的当前线速度；

分别计算两个所述柔性器械末端从对应的所述当前线速度降低至零所需的第三减速距离，以及两个所述柔性器械末端在不同距离处的第三最大允许速度；

在所述柔性器械末端的运动过程中，若两个所述柔性器械末端之间的距离达到所述第三减速距离，则根据所述第三最大允许速度，对两个所述柔性器械末端的运动速度进行限速，使两个所述柔性器械末端避免相撞。

12.根据权利要求1所述的方法，其特征在于，所述在所述柔性器械末端的运动过程中，分别获取两个所述柔性器械末端的第一当前空间位置包括：

分别获取两个所述柔性器械末端的控制电机的运行信息；

根据所述控制电机的运行信息，以及所述柔性器械末端的空间位置和所述控制电机的运行信息的关联关系，分别确定两个所述柔性器械末端的第一当前空间位置。

13.根据权利要求12所述的方法，其特征在于，在所述在所述柔性器械末端的运动过程中，分别获取两个所述柔性器械末端的第一当前空间位置之前，所述方法还包括：

采集所述柔性器械末端的控制电机的运行信息样本，以及对应的所述柔性器械末端的空间位置样本；

根据所述运行信息样本和空间位置样本，建立所述柔性器械末端的空间位置和所述控制电机的运行信息的关联关系。

14.根据权利要求13所述的方法，其特征在于，所述柔性器械末端在工装平面上运动，所述工装平面上安装有陀螺仪，所述工装平面上方安装有摄像头，所述控制电机上安装有磁传感编码器，所述采集所述柔性器械末端的控制电机的运行信息样本，以及对应的所述柔性器械末端的空间位置样本包括：

获取所述陀螺仪的数据，确定所述柔性器械末端运动时所在平面的平面位置信息；

通过所述摄像头，检测所述柔性器械末端是否达到预设的目标采样点；

当所述柔性器械末端的控制电机出现堵转，则确定所述柔性器械末端达到极限位置；

在所述柔性器械末端达到所述目标采样点和/或极限位置时，将所述平面位置信息和所述目标采样点和/或极限位置的位置信息作为所述空间位置样本，并读出所述磁传感编码器的数据，作为所述运行信息样本。

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

空间确定模块，用于分别确定两个所述柔性器械末端的可运动空间；

运动控制模块，用于控制所述柔性器械末端，在所述柔性器械末端的可运动空间中进行运动；

位置获取模块，用于在所述柔性器械末端的运动过程中，分别获取两个所述柔性器械末端的第一当前空间位置；

剔除模块，用于当所述柔性器械末端的第一当前空间位置与另一个所述柔性器械末端的可运动空间具有重合部分时，将所述另一个柔性器械末端的可运动空间中所述重合部分剔除。

16.根据权利要求15所述的装置，其特征在于，所述柔性器械末端与对应的柔性器械中端连接，所述空间确定模块包括：

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

空间确定子模块，用于根据所述开合角度，分别确定两个所述柔性器械末端的可运动空间。

17.根据权利要求16所述的装置，其特征在于，所述空间确定子模块包括：

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

空间确定单元，用于根据所述开合角度、所述偏航的两个极限位置、所述俯仰的两个极限位置，以及所述柔性器械末端的预设空间模型，分别确定对应的所述柔性器械末端的可运动空间。

18.根据权利要求17所述的装置，其特征在于，所述柔性器械末端在工装平面上运动，所述工装平面上安装有陀螺仪，所述工装平面的上方安装有摄像头，所述装置还包括：

样本采集模块，用于在所述根据所述开合角度，分别确定两个所述柔性器械末端的可运动空间之前，针对每个所述柔性器械末端，利用所述陀螺仪和摄像头，采集所述柔性器械末端的多个空间位置样本；

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

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

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

Images