CN114366313B - 一种基于腹腔镜手术器械位姿的持镜机器人控制方法 - Google Patents

Abstract

本发明公开了一种基于腹腔镜手术器械位姿的持镜机器人控制方法，包括：持镜机器人包括：多个关节单元以及设置在所述关节单元的位置传感器；持镜机器人夹持有腹腔镜；在腹腔镜手术器械上布置腹腔镜手术器械位姿传感器，来获取腹腔镜手术器械的空间位姿；通过各个关节单元的位置传感器获取的各关节位置，计算当前腹腔镜的空间位姿；采集当前腹腔镜手术器械位姿传感器的空间位姿信息，根据腹腔镜手术器械移动过程中位姿变化，通过持镜机器人控制腹腔镜到目标空间位姿。本发明利用腔镜手术器械作为主动手，持镜机器人作为从动手，实现了主从控制及随动控制。

Description

一种基于腹腔镜手术器械位姿的持镜机器人控制方法

技术领域

本发明涉及医疗器械控制方法领域，具体涉及一种基于腹腔镜手术器械位姿的持镜机器人控制方法。

背景技术

腹腔镜手术是一门新发展起来的微创方法，是未来手术方法发展的一个必然趋势。随着工业制造技术的突飞猛进，相关学科的融合为开展新技术、新方法奠定了坚定的基础，加上医生越来越娴熟的操作，使得许多过去的开放性手术已被腔内手术取而代之，大大增加了手术选择机会。

腹腔镜手术就是利用腹腔镜及腹腔镜手术器械进行的手术：使用持镜机器人将腹腔镜镜头插入腹腔内，运用数字摄像技术使腹腔镜镜头拍摄到的图像通过光导纤维传导至后级信号处理系统，并且实时显示在专用监视器上。然后医生通过监视器屏幕上所显示患者器官不同角度的图像，对病人的病情进行分析判断，并且运用腹腔镜手术器械进行手术。

微创手术是外科发展的总趋势和追求目标。腹腔镜手术的金标准是胆囊切除术，一般地说，大部分普通外科的手术，腹腔镜手术都能完成。如阑尾切除术，胃、十二指肠溃疡穿孔修补术、疝气修补术、结肠切除术、脾切除术、肾上腺切除术，还有卵巢囊肿摘除、宫外孕、子宫切除等，随着腹腔镜技术的日益完善和腹腔镜医生操作水平的提高，几乎所有的外科手术都能采用这种手术。

传统腔镜手术，采用声控或踏板、手柄等控制持镜机器人，主刀医生双手需要离开手术器械去控制、手柄，或分散精力去通过语音或踏板控制持镜机器人。相比于在腹腔镜手术器械臂上安装标记点，通过布置能识别标记点的相机来获取并计算器械臂的位姿的方案，存在因相机被遮挡而无法跟踪器械位姿的情况出现。

发明内容

本发明提供了一种基于腹腔镜手术器械位姿的持镜机器人控制方法，本发明利用腔镜手术器械作为主动手，持镜机器人作为从动手，实现了主从控制及随动控制。

一种基于腹腔镜手术器械位姿的持镜机器人控制方法，包括以下步骤：

1）持镜机器人包括：多个关节单元以及设置在所述关节单元的位置传感器；

所述的持镜机器人夹持有腹腔镜；

2）在腹腔镜手术器械上布置腹腔镜手术器械位姿传感器，来获取腹腔镜手术器械的空间位姿；

3）通过各个关节单元的位置传感器获取的各关节位置，计算当前腹腔镜的空间位姿

；

4）采集当前腹腔镜手术器械位姿传感器的空间位姿信息

，根据腹腔镜手术器械移动过程中位姿变化，持镜机器人控制腹腔镜到目标空间位姿。

本发明利用腔镜手术器械作为主动手，持镜机器人作为从动手，实现了主从控制及随动控制，在腹腔镜手术器械在移动过程中，持镜机器人上的腹腔镜能够随着主从控制模式及随动控制模式跟随。

步骤1）中，进一步优选，持镜机器人包括：

旋转关节单元，所述的旋转关节单元上布置旋转关节位置传感器；

与所述旋转关节单元连接的俯仰关节单元，所述的俯仰关节单元布置俯仰关节位置传感器；

以及与所述俯仰关节单元连接的升降关节单元，所述的升降关节单元布置升降关节位置传感器；

所述的升降关节单元与腹腔镜连接；

通过所述旋转关节位置传感器、俯仰关节位置传感器及升降关节位置传感器获取的各关节位置，可以得到腹腔镜的空间位姿。

步骤2）中，所述的位姿包含位置和姿态两部分，位姿传感器安装位置不限于手柄，只要位姿传感器与器械的相对位姿固定，就可以得到手术器械的空间位姿。进一步优选，在腹腔镜手术器械的手柄上布置腹腔镜手术器械位姿传感器，来获取手柄的空间位姿。

在手柄上设置有模式的切换开关，如主从控制模式和随动控制模式的切换开关，开关位置可以设置在手柄上，方便医生操作。

在手柄上还设置有主从控制模式的离合开关，通过离合开关，主从模式按下离合，腹腔镜手术器械可以当成控制手柄用，来控制持镜机器人和持镜机器人上的腹腔镜的空间位姿；松开离合开关，持镜机器人不在随着腹腔镜手术器械动。

步骤3）中，通过旋转关节位置传感器、俯仰关节位置传感器、升降关节位置传感器获取的各关节位置，计算当前腹腔镜的空间位姿

。

步骤4）中，所述的控制可采用主从控制模式或随动控制模式。

主从控制模式包括：

4.1.1）采集相邻的下一个控制周期腹腔镜手术器械位姿传感器的空间位姿信息

；

4.1.2）计算

和

之间的位置变化ΔT^T，根据ΔT^T来计算腹腔镜空间位置信息变换量ΔT^E；

4.1.3）根据

及ΔT^E来计算腹腔镜下一控制周期的目标空间位姿信息

；

4.1.4）根据腹腔镜的目标空间位姿信息

，反推计算持镜机器人各关节的目标位置，调整持镜机器人各关节至目标位置，则持镜机器人夹持腹腔镜实现了一个控制周期内的主从控制。

重复上述步骤，则持镜机器人夹持腹腔镜实现了连续不断的主从控制。

所述离合开关用于建立或断开主从控制关系，在离合开关建立主从控制关系时，上述过程生效；离合开关断开主从控制关系时，则上述过程不生效，持镜机器人保持当前的空间位姿。

采用主从控制模式，手术过程中，主刀医生需要调节腹腔镜的空间位姿，则按下手术器械上附着安装的主从控制模式按钮，此时通过移动腹腔镜手术器械控制腹腔镜动作，腹腔镜手术器械（简称器械）与腹腔镜（简称腔镜）的动作趋势符合人的直观感受，从而实现器械动作对腔镜位姿的控制。当调整过程中器械位置达到极限位置，而无法继续移动，从而无法继续移动腔时，可通过器械手柄上附着安装的离合按钮，断开主从控制的连接，调整器械到合适位姿后，再次按下离合按钮，重新建立主从控制，从而可以继续通过器械调节主手位姿。

随动控制模式包括：

4.2.1）根据

、

以及腹腔镜景深参数、腹腔镜视场角参数计算得到腹腔镜手术器械的末端执行器在图像平面S中的位置P^T；

4.2.2）设定百分比形式的阈值t%，当P^T处于S中央t%的面积内（面积可用圆形、矩形等几何图像圈出），认为腔镜观察器械的角度合理，持镜机器人夹持腔镜保持当前位姿即可；

步骤4.2.2）中，所述的中央t%的面积是指图像平面S中心向外占图像平面S的t%的指定区域，该指定区域可用圆形、矩形等几何图像圈出， t%为指定区域与图像平面S的百分比；

4.2.3）当P^T处于S中央t%面积以外时，认为腔镜观察器械角度不合理，持镜机器人根据偏差的方向调整夹持的腹腔镜的空间位姿，使P_T重新回到S中央t%的面积内。

重复上述过程，实现随动控制模式。

随动控制模式中，获取器械位姿及腔镜位姿的方法与主从模式相同。

与上述主从模式的区别在于，主从模式腹腔镜动作完全取决于腹腔镜手术器械动作，腹腔镜手术器械不动则腹腔镜不动，而随动模式下，腹腔镜动作与腹腔镜手术器械动作有关，但不完全取决于腹腔镜手术器械动作，存在腹腔镜手术器械小范围运动，腹腔镜不动；腹腔镜手术器械大范围运动移到腔镜视野外，腹腔镜才开始跟随移动。

采用随动控制模式，手术过程中，主刀医生无需主动控制调节腹腔镜的空间位姿，只需要关注手术本身，持镜机器人会根据本发明提出的随动控制方法，自动根据手术器械上布置的腹腔镜手术器械位姿传感器，计算合适的腹腔镜的空间位姿，并通过持镜机器人夹持腹腔镜移动到此空间位姿，当器械在小范围内操作时，腹腔镜不进行跟随移动，即可保证器械在腹腔镜视野范围内，且视野稳定，不会跟随腹腔镜的高频率小范围移动而晃动；当器械进行牵拉、转移等操作时，器械大范围移动，会移出腔镜视野范围，此时根据本发明所述算法，使得腔镜自动跟随器械动作，腔镜视野中的器械持续存在。

本发明的控制可采用主从控制模式或随动控制模式，两种模式的必要性：在进行常规手术操作时，可使用随动模式。但手术接近结束时，需要大范围对腹腔内进行冲洗等清扫工作，此时需要大范围调整腔镜位姿进行观察，因此带有离合开关的主从模式也必不可少。

与现有技术相比，本发明具有如下优点：

本发明利用腔镜手术器械作为主动手，持镜机器人作为从动手，实现了主从控制及随动控制，与传统腔镜手术相比，可节省人力，无需人工助手；与声控或踏板、手柄等控制的持镜机器人相比，主刀医生双手无需离开手术器械去控制、手柄，或分散精力去通过语音或踏板控制持镜机器人，仅通过移动器械即可实现对腔镜观察角度及观察位置的控制。且与基于腔镜图像识别的控制方式相比，无需复杂的图像算法，能更加可靠的获得腔镜、器械等的空间位姿。而与在器械臂上安装标记点，通过布置能识别标记点的相机来获取并计算器械臂的位姿的方案相比，本发明的技术方案无需在手术室内布置用于识别器械位姿的相机，且不存在因相机被遮挡而无法跟踪器械位姿的情况出现。

附图说明

图1为本发明基于腹腔镜手术器械位姿的持镜机器人控制方法的示意图。

图2为本发明中主从控制的原理示意图。

图3为本发明中随动控制的原理示意图。

图4为本发明中持镜机器人通过连杆安装在支撑台上的结构示意图。

图5为本发明中持镜机器人的结构示意图。

具体实施方式

一、系统组成

整体系统可分为腹腔镜手术器械位姿采集子系统与持镜机器人子系统，其中器械位姿采集子系统可采用惯导、视觉等方式获取器械手持部分在该器械RCM处且与世界坐标系相对静止的坐标系中的位置与姿态数据，此位置与姿态数据经过本专利所述方法运算后，可用于引导持镜机器人子系统夹持腹腔镜完成跟随动作。而进行映射运算的处理系统可单独列出，或集成到持镜机器人系统中，整体系统采用周期控制模式，在一个周期内，完成采集数据、运算、下发、执行动作。

如图4、图5所示，持镜机器人通过连杆5安装在支撑台6上，连杆5安装在支撑台6上；连杆5为多节结构，连杆5至少包括三节，第一节连杆与第二节连杆之间通过转动结构连接，该转动结构的转动方向为第二节连杆的周向，第二节连杆与第三节连杆之间通过旋转结构连接，该旋转结构的转动以第三节连杆为径向，即该旋转结构的转动方向垂直于第三节连杆的轴向。持镜机器人主要包括三部分：旋转关节单元1，俯仰关节单元2，升降关节单元3，包括：安装在连杆5上的旋转关节单元1；与旋转关节单元1连接的俯仰关节单元2；以及与俯仰关节单元2连接的升降关节单元3；升降关节单元3与腹腔镜4连接，腹腔镜4安装在升降关节单元3上。旋转关节单元1上布置旋转关节位置传感器，俯仰关节单元2布置俯仰关节位置传感器；升降关节单元3布置升降关节位置传感器；升降关节单元3与腹腔镜4连接。

二、控制方法

如图1所示，一种基于腹腔镜手术器械位姿的持镜机器人控制方法，包括以下步骤：

1）在腹腔镜手术器械上布置腹腔镜手术器械位姿传感器，来获取腹腔镜手术器械的空间位姿；

在手柄上设置有主从控制模式和随动控制模式的切换开关。在手柄上还设置有主从控制模式的离合开关；

2）通过旋转关节位置传感器、俯仰关节位置传感器、升降关节位置传感器获取的各关节位置，计算当前腹腔镜的空间位姿

；

3）采集当前腹腔镜手术器械位姿传感器的空间位姿信息

，根据腹腔镜手术器械移动过程中位姿变化，持镜机器人控制腹腔镜到目标空间位姿。

步骤3）中，所述的控制可采用主从控制模式或随动控制模式；本发明是所述控制方法包括两种控制模式，其一为主从模式，即将手术器械位姿态变化用于调整腔镜位姿，其过程类似于使用手术机器人主手控制从手带动腔镜运动，腔镜动作完全取决于手术器械位姿变化即手术器械位姿与腔镜位姿映射关系；其二为随动模式，即腔镜动作取决于手术器械的行为而非直接由器械位姿映射而来。下文将分别从这两方面进行介绍。

1、主从控制：在主从控制模式中，获取手术器械位姿后，通过映射关系，得到腔镜的运动目标。由于无法保证手术器械的初始姿态与腔镜的初始姿态相同，故在姿态方面采用相对映射方式，即腔镜姿态变化量与器械姿态的变化量相同；而位置方面，由于器械位置尤其是插入Trocar深度受限，因此主从控制中不进行位置映射，亦即只通过器械姿态控制腔镜姿态调整，而不控制腔镜插入Trocar深度。如图2所示，主从控制的具体方法如下：

1）基于腔镜及器械的几何特征建立基坐标系，分别记为endo_base（腔镜基）坐标系及tool_base（器械基）坐标系，二者姿态相同，且在腔镜及器械运动过程中均相对于世界坐标系静止；

2）建立腔镜与器械的当前坐标系，分别记为endo_current（腔镜当前）坐标系及tool_current（器械当前）坐标系，在腔镜与器械分别处于初始姿态时，endo_current与endo_base重合，tool_current与tool_base重合，且在腔镜及器械运动过程中，此二坐标系分别固结在腔镜及器械上随之运动，但endo_current坐标系相对于endo_base坐标系的姿态变换可在持镜机器人子系统中获取到，而tool_current坐标系相对于tool_base的姿态变换可通过器械姿态采集子系统获得；

3）建立腔镜与器械的下一时刻坐标系，分别记为endo_next（腔镜下一时刻）坐标系及tool_next（器械下一时刻）坐标系，下一时刻指下一数据采集时刻（当前时刻与下一时刻分别处于前后相邻的两个控制周期中），对于器械来说，tool_next相对于tool_current的变化体现了手持器械运动的姿态变化，而对于腔镜来说，endo_next相对于endo_current的变化体现了经过映射后的器械运动作用于腔镜后的运动结果。

以上坐标系建立完成后，下面详细叙述主从控制的过程：

1）对于控制循环周期中的第n个周期，获取tool_current姿态，即^{tool_ base}R_{tool_current}，还需获取在第n+1个周期中，获取tool_next姿态，即^tool_baseR_{tool_next}；

其中R代表3×3的旋转矩阵，R的左上标代表参考坐标系，R的右下标代表当前坐标系。R中每一列均为3×1的单位列向量，第一列用于表示当前坐标系X轴在参考坐标系下的方向向量，第二列用于表示当前坐标系Y轴在参考坐标系下的方向向量，第三列用于表示当前坐标系Z轴在参考坐标系下的方向向量；

2）计算从tool_current到tool_next的变换矩阵^tool_currentR_{tool_next}，^{tool_ current}R_{tool_next}=(^tool_baseR_{tool_current})^-1tool_baseR_{tool_next}，-1表示矩阵的转置，公式为两矩阵为矩阵乘法运算；

3）将从tool_current到tool_next的变换映射到持镜机器人上，即：^{endo_ current}R_{endo_next}=^tool_currentR_{tool_next}；

4）计算endo_current的目标姿态：^endo_baseR_{endo_next}=^endo_baseR_{endo_current} ^{endo_ current}R_{endo_next}，式中^endo_baseR_{endo_current}可根据持镜机器人各关节位置传感器测得的关节位置结合已知的各关节单元设计参数计算得到，使用机器人学中D-H方法即可求解；式中^endo_currentR_{endo_next}由上面步骤3）得到；

5)将得到endoscop_next坐标系作为持镜机器人系统的输入目标，即可通过机器人学中D-H方法的逆运算得到持镜机器人各关节的目标角度，从而引导持镜机器人夹持腔镜完成跟随动作。

此外，由于器械在手术过程中运动姿态范围受限，可在器械上连接离合设备，用于断开主从映射即重新建立主从映射。由于采用了姿态相对映射的方式，可以实现器械运动到极限位置，断开映射调整位置并重新映射。

2、随动控制：除上文所述的通过器械姿态调整腔镜姿态的主从映射控制方式，本系统还具备手术过程中腔镜实时跟随器械的随动控制方式。但在手术过程中，不仅存在转移、牵拉等需要大范围移动器械的操作，还存在缝合等器械在小范围内反复移动的操作，若在器械小范围内反复移动的过程中始终进行随动控制，将导致视野晃动，映射手术操作者观察效果。除此之外，通常有两把器械配合完成手术，器械所跟随的具体器械也是值得关注的一点。下文将详细叙述综合考虑上述问题的随动控制算法。

如图3所示，随动控制的步骤及策略为：

1）首先对器械末端、器械Trocar、腔镜末端及腔镜Trocar建立坐标系，记为tool（器械末端）坐标系、tool_trocar（器械戳卡）坐标系、endoscope（腔镜末端坐标系）坐标系、endo_trocar（腔镜戳卡）坐标系，世界坐标系记为world（世界）坐标系；

2）对于1)中建立的各坐标系，tool_trocar及endo_trocar在世界坐标系world中的位姿可通过术前采用视觉或其他手段定位和测量，记为^worldT_{tool_trocar}、^worldT_{endo_trocar}；

3）对于上述建立的坐标系，tool_trocar中tool的位姿可通过集成在器械上的传感器获取，记为^tool_trocarT_tool，同理，endo_trocar中的endoscope的位姿可通过持镜机器人上的关节传感器数值解算得到，记为^endo_trocarT_endoscope；

其中T为4×4的齐次变换矩阵，矩阵左上角3×3区域与上文说明的R（3×3的旋转矩阵）相同，矩阵中第四列一到三行为表示位置的列向量，而其余的第四行用行向量（0，0，0，1）填充；

4）由2中的^worldT_{tool_trocar}、^worldT_{endo_trocar}及3）中的^tool_trocarT_tool、^endo_trocarT_endoscope得到^worldT_tool、^worldT_endoscope，公式为：^worldT_tool=^worldT_{tool_trocar} ^tool_trocarT_tool，^worldT_endoscope=^worldT_{endo_trocar} ^endo_trocarT_endoscope；

5）由^worldT_tool、^worldT_endscope计算得到endoscope坐标系中tool的位姿：^endoscopeT_tool，公式为：^endoscopeT_tool=(^worldT_endoscope)^-1worldT_tool；

6）由^endoscopeT_tool可以得到endoscope坐标系中表示器械末端点位置的向量^endoscopeP_tool；

7）根据^endoscopeP_tool及相机参数，可以计算器械末端点在腔镜图像平面中的坐标，记为P_tool

8）找到腔镜图像平面中心点，其坐标记为P_center，并以设定值R为半径做圆形区域，此区域记为S，而R的求法如下：R =

，其中A为图像平面面积，t%为用户设置的阈值，表示S区域占图像平面的百分比；

9）当P_tool与P_center之间的距离小于半径R时，认为器械处于腔镜图像中合适区域，即腔镜位姿合理，持镜机器人保持当前位姿即可；

10）当P_tool与P_center之间的距离大于半径R时，认为器械在腔镜图像中位置不合理，腔镜位姿不合理，持镜机器人应当按照向量P_centerP_tool所指方向调整腔镜位姿，从而使得器械重新回到视野中央区域，实现随动；

11）腔镜中央区域S也可用矩形等其他形状表示，但计算P_tool是否在此区域内的具体方法有所不同，不一一列举。

Claims (1)

1.一种基于腹腔镜手术器械位姿的持镜机器人控制方法，其特征在于,包括以下步骤：

1）持镜机器人包括：多个关节单元以及设置在所述关节单元的位置传感器；

所述的持镜机器人夹持有腹腔镜；

所述的持镜机器人包括：

旋转关节单元，所述的旋转关节单元上布置旋转关节位置传感器；

与所述旋转关节单元连接的俯仰关节单元，所述的俯仰关节单元布置俯仰关节位置传感器；

以及与所述俯仰关节单元连接的升降关节单元，所述的升降关节单元布置升降关节位置传感器；

所述的升降关节单元与腹腔镜连接；

2）在腹腔镜手术器械上布置腹腔镜手术器械位姿传感器，来获取腹腔镜手术器械的空间位姿；

在手柄上设置有主从控制模式和随动控制模式的切换开关；

在所述手柄上还设置有主从控制模式的离合开关；

3）通过各个关节单元的位置传感器获取的各关节位置，计算当前腹腔镜的空间位姿

；

所述的各个关节单元的位置传感器包括：旋转关节位置传感器、俯仰关节位置传感器、升降关节位置传感器；

4）采集当前腹腔镜手术器械位姿传感器的空间位姿信息

，根据腹腔镜手术器械移动过程中位姿变化，持镜机器人控制腹腔镜到目标空间位姿；

所述的控制采用主从控制模式和随动控制模式；

所述的主从控制模式包括：

4.1.1）采集相邻的下一个控制周期腹腔镜手术器械位姿传感器的空间位姿信息

；

4.1.2）计算

和

之间的位置变化ΔT^T，根据ΔT^T来计算腹腔镜空间位置信息变换量ΔT^E；

4.1.3）根据

及ΔT^E来计算腹腔镜下一控制周期的目标空间位姿信息

；

4.1.4）根据腹腔镜的目标空间位姿信息

，反推计算持镜机器人各关节的目标位置，调整持镜机器人各关节至目标位置，持镜机器人将夹持的腹腔镜移动到目标空间位姿，完成一个控制周期内的主从控制；

所述的随动控制模式包括：

4.2.1）根据

、

以及腹腔镜景深参数、腹腔镜视场角参数计算得到腹腔镜手术器械的末端执行器在图像平面S中的位置P^T；

4.2.2）设定百分比形式的阈值t%，当P^T处于S中央t%的面积内，认为腔镜观察器械的角度合理，持镜机器人夹持腔镜保持当前位姿即可；

所述的中央t%的面积是指图像平面S中心向外占图像平面S的t%的指定区域；

4.2.3）当P^T处于S中央t%面积以外时，认为腔镜观察器械角度不合理，持镜机器人根据偏差的方向调整夹持的腹腔镜的空间位姿，使P_T重新回到S中央t%的面积内；

重复上述过程，实现随动控制模式。

Images