一种外科手术的主动导航系统及其控制方法

1.本发明涉及医疗设备技术领域，具体涉及外科手术机器人领域，尤其涉及一种外科手术的主动导航系统及其控制方法。


背景技术：

2.依靠图像导航技术，辅助手术系统可以对手术部位以及操作工具进行精确的定位，以辅助医生开展微创手术、远程手术或者由机器人辅助执行外科手术。目前，手术导航依赖光学导航设备通过检测识别光学定位工具，并进行图像以及位置的结算实现对手术部位或者手术工具的定位功能。实际操作时，手术导航设备由辅助手术的医生根据手术需要进行人手调整。具体的，通过拖动设备的手柄，把光学导航设备调整到合适的观测位置。然而，这种交互方式在实际手术过程中带来诸多不便，而且对于一些特殊的手术位置设计，单靠人手难以调整出合适的测量位置，无法保证位置精度。
3.赋予光学导航设备运动能力，成为新的趋势。需要实现光学导航的主动导航，不仅要求机器人具备用于定位的光学导航传感器，还需要具备其他环境感知功能的传感器，来感知手术室内发生的人为或者设备位置变动的事件发生，由此触发响应的主动运动。因此需要特定的硬件构成系统；同时，机器人主动调整到的目标位姿需要综合考虑多种因素，包括但仅限于：测量的精度、靶标定位的可测量条件、机器人的可达性等，并且在术中调整位姿时还不能丢失任何一个光学定位工具，因此需要特定的机器人位姿优化以及路径规划的控制算法。


技术实现要素：

4.考虑到上述因素，本发明提供一种外科手术的主动导航系统及其控制方法。本发明的技术方案解决了手术导航定位的机器人最优的观测位姿的获取，以及实时主动调整位置，避免导航靶标定位器被遮挡，提高导航过程的定位精度等问题。
5.一种外科手术的主动导航系统，该系统包括：控制主机、任意自由度的串联机械臂、定位传感器及其适配的一个或多个定位工具、环境感知传感器；环境感知传感器与定位传感器的重合测量区域为外科手术的主动导航系统的可测量区域；定位工具的数量为一个或多个；每个定位工具上有k个按照一定位置关系分布形成的定位部件；所述定位部件为能够反光或发光的特异性标志物、和/或由若干个特定图案按一定位置关系排列后形成的部件；能够反光的特异性标志物至少包括：表面覆盖有高反光度涂层的小球；能够发光的特异性标志物至少包括：led灯；所述特定图案为经过专门编码设计的图案，至少包括二维码、格雷码；每个定位工具上各个定位部件的位置与/或数量不相同，用以区分定位工具；同一个定位工具的k个定位部件的质心都在同一个平面上；每个定位工具中心处设计有特异的形状特征，把特征轴线与定位部件质心所在的平面交点作为坐标原点；所述形状特征至少可以为圆孔、半球、凸台、圆锥；以所述坐标原点
为球心，为每个定位工具构造包络该定位工具上k个定位部件的最小外接球，所述最小外接球的半径为；以k个定位部件的质心所在平面的法线方向为z轴方向；且朝k个定位部件附着的一侧的方向为z轴正向；以垂直于z轴且指向离坐标原点最远的定位部件的方向为x轴正向，建立三维直角坐标系；将所有定位工具的集合记为s，对于第个定位工具，其坐标系圆心为，即。
6.实际应用中，会在的基础上增加一定的裕度，即球面大小估算时设定比稍大一些，例如：将乘以一个大于1的裕度系数，得到，以避免实际操作中的一些细小误差导致方法失效。
7.一种用于上述外科手术的主动导航系统的控制方法，所述控制方法包括以下步骤：步骤1、测量视角多目标优化：输入定位工具的位置参数并设置其他相关参数，通过多目标优化求解出最优测量视角的集合；步骤2、机械臂位姿的多目标决策：根据所述最优测量视角的集合，采用多目标决策算法向用户推荐手术各环节中机械臂的最优位姿方案；或者根据用户根据偏好选择手术各环节中机械臂的最优位姿方案；步骤3、机械臂路径规划与执行：根据所选择的手术各环节中机械臂的最优位姿方案，规划出机械臂从当前位姿到达最优位姿方案的路径。
8.可选地，所述步骤1包括以下步骤：步骤1.1、获取手术过程中各个环节的所有定位工具的信息及其所在的位置，建立基于决策变量x的多目标最小化问题：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（公式1）上式中：关节变量；n为关节变量的数量；决策变量x表示机械臂的n个关节变量组成的向量，其取值范围为机械臂各关节可实现的关节值范围q，即；步骤1.2、定义最小化优化的至少两个目标函数和，具体如下：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（公式2）
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（公式3）其中，表示第m个定位工具的坐标原点与定位传感器的坐标原点之间的距离；表示所有定位工具的坐标原点与定位传感器的坐标原点之间的最大距离；表示对于给定的一对定位工具j与k，其在定位传感器各个相机坐标中较小的无遮挡裕度函数；表示在决策变量x所决定的机械臂位姿下，在定位传感器所有相机中，测量到的所有定位工具的二元组合中，最小的无遮挡裕度函数值；
通过以下公式计算所述较小的无遮挡裕度函数：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（公式4）
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（公式5） ， 且
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（公式6）
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（公式7） ， 且
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（公式8）
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（公式9）
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（公式10）
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（公式11）=min(,)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（公式12）上述公式中：g为定位传感器中左侧或右侧摄像头的坐标原点；l、r分别为定位传感器中左侧、右侧摄像头的坐标原点；和分别为任意两个定位工具与的半径分别为、的最小外接球的球心，即定位工具与的坐标原点；和分别为定位工具与的扩展半径；裕度系数为大于1的常数；向量长度和通过定位传感器测量获得；表示向量点乘；步骤1.3、设置以下约束条件，在保证以下约束条件的到满足的同时，使至少两个目标函数和同时实现最小化：约束条件1：约束条件2：约束条件3：
ꢀꢀꢀ
其中，约束条件1表示任意定位工具都要处于定位传感器与环境感知传感器共同可检测的范围内；约束条件2表示从定位传感器的任意一侧摄像头向任意一个定位工具的连线与该
定位工具的z轴方向之间的夹角不能大于既定的阈值；表示第i个定位工具的坐标原点向指向定位传感器中左侧或右侧摄像头坐标原点的向量与第i个定位工具的z轴方向向量之间的夹角；th为预设阈值；约束条件3表示任意两个定位工具之间互不遮挡，即任意两个定位工具之间的无遮挡裕度函数的最小值为非负。
9.可选地，所述步骤2中，所述根据所述最优测量视角的集合，采用多目标决策算法向用户推荐手术各环节中机械臂的最优位姿方案，包括以下步骤：步骤2.1：找出所述最优测量视角的集合中，在单一目标上最优的解，并计算出所述最优测量视角的集合对应曲线的两个端点所在的直线方程：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（公式13）步骤2.2：计算所述最优测量视角的集合对应曲线中每个点到上述直线的垂直距离d，将每个点的目标值代入以下公式：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（公式14）步骤2.3：将垂直距离d的最大值所对应最优测量视角的解作为所述机械臂关节值的多目标决策的推荐值；其中，a、b、c通过单目标最优解的目标值求解线性方程获得。
10.可选地，所述步骤3包括以下步骤：步骤3.1：手术过程中，在进入指定的手术环节以后，根据手术前机械臂位姿的优化求解以及多目标决策所获得的最优位姿方案以及手术进行情况中机械臂的最优位姿方案，获取当前手术环节的目标位姿；步骤3.2：环境感知传感器获取手术机器人周围环境的三维信息，生成周围环境的点云图像，并通过以下公式获得环境点云在定位传感器的坐标下的点云位置信息：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（公式15）其中，为4*4常数转换矩阵；步骤3.3：随机生成候选路径点；步骤3.4：判断路径点是否会碰到障碍物；如果会，则返回步骤3.3；否则继续下一步；步骤3.5：判断在该位姿上是否所有定位工具都可以被检测；如果不可以，则返回步骤3.3；否则继续下一步；其中，所述判断在该位姿上是否所有定位工具都可以被检测的步骤中，需要定位工具符合上述约束条件1~3；步骤3.6：将当前的候选路径点加入路径目录中，用以生成合理的路径规划；步骤3.7：判断是否到达了目标位姿，如果还未达到，则返回步骤3.3；否则找出当前路径目录中最短的路径，作为机械臂运动的路径；步骤3.8：执行上述路径位姿，使手术机器人的机械臂到达目标位姿。
11.有益效果：本发明提供一种外科手术的主动导航系统及其控制方法。本发明的技术方案解决了手术导航定位的机器人最优的观测位姿的获取，以及实时主动调整位置，避免导航靶标定位器被遮挡，提高导航过程的定位精度等问题。
附图说明
12.图1为本发明的外科手术的主动导航系统的总体结构图；图2为本发明的外科手术的主动导航系统的实施方案图；图3为本发明的外科手术的主动导航系统中坐标系的建立示意图；图4为本发明的定位工具及其坐标系建立图；图5为本发明的无遮挡裕度函数设计的示意图；图6为本发明的观测角度的示意图；图7为本发明的测量视角多目标优化的最优解图；图8为本发明的多目标决策算法提供的最优解推荐方法图。
具体实施方式
13.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。
14.本发明提供一种外科手术的主动导航系统及其控制方法。
15.图1为本发明的外科手术的主动导航系统的总体结构图。如图1所示，所述系统包括：手术操作规划系统、用于数据处理和机器人控制的控制主机、机械臂、定位传感器及其适配的定位工具、环境感知传感器；所述环境感知传感器实现手术环境，例如：潜在遮挡物和/或障碍物的感知。所述机械臂为7自由度的串联机械臂；所述定位传感器和/或环境感知传感器连接在所述机械臂的末端法兰上。
16.定位传感器可以采用多种不同的模态方式：如基于可见光的双目深度摄像头、基于近红外光双目定位摄像头等，对应的定位工具是与定位传感器相匹配的光学二维码或者其他编码图案，或者表面覆盖有特定涂料的光学小球组成的定位工具等。
17.环境感知传感器同样可以是多种模态方式的：如基于可见光的双目深度摄像头、激光雷达、超声波传感器等。
18.环境感知传感器与定位传感器可以是两种的设备载体的组合，如：基于近红外光双目定位摄像头+激光雷达的方案；也可以是同一类传感器，如：基于可见光的双目深度摄像头，既可以用于定位，同时可以实现手术环境感知。但是，无论是何种方式，环境感知传感器与定位传感器测量的空间区域必须要相互重合的区域，并且相互重合的区域是本系统的可测量区域。
19.图2为本发明的外科手术的主动导航系统的实施方案图。如图2所示，实施方案如下：系统由一台7自由度机械臂以及连接在机械臂末端法兰上的近红外光学定位系统（作为“定位传感器”）和双目摄像头（作为“环境感知传感器”），以及用于数据处理和机器人控制的计算机、近红外光学定位系统适配的定位工具组成。
20.这里的近红外光学定位系统包括两个红外发射灯、以及用于检测反射红外光的红外光摄像头。其工作原理是：左右两个红外发射灯发射特定红外光，投射在定位工具上的反光小球表面。反光小球反射红外光，并被红外光摄像头检测到，其根据接收到的反射红外光推算出近红外光学定位系统与各个小球之间的相对位置，并根据预先标定好的定位关系模型计算出各个定位工具相对于近红外光学定位系统的相对位置。
21.机械臂的基座坐标为，第k个关节的关节角为，末端法兰的坐标系原点为。近红外光学定位系统中心坐标为，左右两边的摄像头坐标分别为与。在机械臂处于位置p时，近红外光学定位系统可测量区域空间为。双目摄像头的坐标系为c。
22.如图2中的附图标记含义如下：1—七自由度机械臂，2—近红外光学定位系统，3—双目摄像头，4—定位工具，5—计算机。
23.图3为本发明的外科手术的主动导航系统中坐标系的建立示意图。所有定位工具的集合为s，对于第个工具，其坐标系圆心为，即。光学定位系统中心坐标为n，左右两边的摄像头坐标分别为r与l。在机械臂处于位置p时，光学定位系统与环境感知传感器重合的可测量区域空间为a(p)，即无遮挡条件下，在机械臂处于位置p时，定位工具可以被正常测量的所有可能位置的集合。双目摄像头的坐标系为c。
24.图4为本发明的定位工具及其坐标系建立图。定位工具选用与近红外光学定位系统（即“定位传感器”）相匹配的定位工具，如图4所示。每一个定位工具上有4个表面覆盖有高反光度涂层的小球，按一定的位置关系分布形成的。同一个定位工具的4个小球的球心都在同一个平面上，以k个定位部件的质心所在平面的法线方向为z轴方向；且朝k个定位部件附着的一侧的方向为z轴正向。每一个定位工具小球的位置与/或数量不相同，用以区分定位工具。每个定位工具采用小球球心所在平面与定位工具连杆中心小孔（即外形特征的一种实例）的中心轴相交点作为坐标原点，并以相交点指向离原点距离最远的小球方向为x轴方向。以交点为圆心，建立包络所有小球的最小外接球，该外接球半径为。所有定位工具的集合为s，对于第的工具，其坐标系圆心为，即。
25.本发明提供一种外科手术机器人主动导航的控制方法。这种控制方法的实现，包括：“测量视角多目标优化
”‑“
机械臂位姿的多目标决策
”‑“
机械臂路径规划与执行”三部分组成。具体如下：
·
测量视角多目标优化：通过把定位工具的情况及位置等输入程序，设置好相关的参数后，通过多目标优化，求解出最优测量视角的集合。
26.·
机械臂位姿的多目标决策：基于上一个步骤中通过优化获得最优解集合，采用多目标决策算法，向用户推荐方案，或者由用户根据偏好选择手术各环节中合适的手术导航机械臂的位姿方案。
27.·
机械臂路径规划与执行：基于上一步骤获得的手术各环节的最优位姿方案，机械臂通过算法规划出从当前位姿，到达最优位姿方案。在此过程中需要考虑在运动过程中定位传感器始终可以正常对该手术环节需要的所有定位工具进行定位以及在此过程中出现的非预期的障碍，并最终到达合适的最优位姿。
28.上述三部分的内容具体介绍如下：
（一）测量视角多目标优化：通过手术操作规划系统获取手术过程中各个环节的所有定位工具的信息以及所在的位置。建立如下多目标最小化问题：决策变量：，上式中：关节变量；n为关节变量的数量；决策变量x表示机械臂的n个关节变量组成的向量，其取值范围为机械臂各关节可实现的关节值范围q，即；优化目标如下（至少两个目标函数和同时最小化）：优化目标1：最小化定位工具与近红外光学定位系统的最大距离：其中，表示第m个定位工具的坐标原点与近红外光学定位系统的坐标原点之间的距离；优化目标2：表示定位工具之间最小的无遮挡裕度函数值。通过取其数值的相反数，转化成最小化优化问题：其中，表示对于给定的一对定位工具j与k，其在定位传感器各个相机坐标中较小的无遮挡裕度函数；表示在q所决定的机械臂位姿下，在定位传感器所有相机中，测量到的所有定位工具的二元组合中，最小的无遮挡裕度函数值；定位工具与之间的无遮挡裕度函数定义如图5所示：图5为本发明的无遮挡裕度函数设计的示意图，其描述了无遮挡裕度函数的定义。具体地，图5中描述的是任意两个定位工具以及定位传感器左或者右边其中一侧摄像头的几何关系。因此如果对于定位工具数量大于2时，任意两个定位工具和任意一侧摄像头就会产生一个特定的值，例如：3个定位工具可以产生6个值，即：，，，，，其中，g是指定位传感器左或右其中一侧摄像头坐标系原点。和分别指任意两个定位工具抽象成球体以后的球心，同时也是定位工具坐标系原点。和是指定位工具抽象成球体的半径。选择每个定位工具采用小球球心所在平面与定位工具连杆中心小孔
（即外形特征的一种实例）的中心轴相交点作为坐标原点。以坐标原点为球心的最小外接球半径为。考虑到实际操作时的误差影响，在的基础上进行裕度倍的扩展，获得定位工具抽象成球体的半径。（这里定位工具的特点是，以一个中心延伸出4条或以上共平面的连杆，连杆末端设置有小球。在一套导航设备内，每一个定位工具的小球之间相对位置是唯一的）。其中>1。
29.因此，和大小已知。向量长度和可以通过定位传感器测量获得。和可以通过以下关系求得：可以通过以下关系求得：而可以通过向量计算：其中，表示向量点乘。
30.最后，计算其中，表示把定位工具抽象简化后的球面的半径；其中，。
31.约束条件如下：约束条件1：约束条件2：约束条件3：其中，约束条件1表示任意定位工具都要处于定位传感器与环境感知传感器共同可检测的范围内；约束条件2表示从定位传感器的任意一侧摄像头向任意一个定位工具的连线与该定位工具的z轴方向之间的夹角不能大于既定的阈值；表示第i个定位工具的坐标原点向指向定位传感器中左侧或右侧摄像头坐标原点的向量与第i个定位工具的z轴方向向量之间的夹角；th为预设阈值，例如：th=π/2；
约束条件3表示任意两个定位工具之间互不遮挡，即任意两个定位工具之间的无遮挡裕度函数的最小值为非负。
32.图6为本发明的观测角度的示意图。观测角度是指：左或右摄像头原点与任一个定位工具的z轴（定位工具向上指的法向方向固定为定位工具坐标z轴）之间的夹角。
33.如图6所示：其中，g是指定位传感器左或者右边其中一侧摄像头坐标系原点。为在g坐标系下定位工具z轴单位向量。通过定位传感器可以求得和。代入公式计算即可。另外，需要说明的是，任意一侧相机对于任意一个定位工具，均会有一个观测角大小值。
34.综上所述，即需要优化以下优化问题：决策变量：同时最小化：同时最小化：并同时考虑以下约束：(i)(ii)(iii)通过约束多目标优化算法，可以对上述的优化问题进行求解。在本实施例中采用moea/d
‑
cdp算法，可以获得上述优化问题的帕累托最优解。
35.图7为本发明的测量视角多目标优化的最优解图。
36.如图7所示：图中每一个点都对应着一种最优化的位姿方案。这些方案互不支配，都是最优解。
37.（二）机械臂位姿的多目标决策：在获得如图7所示的测量视角多目标优化的最优解以后，用户可以直接根据自身偏好，选择上述的任意一个最优解；或者基于系统所提供的多目标决策算法进行推荐后，再进行选择。
38.图8为本发明的多目标决策算法提供的最优解推荐方法图。
39.所述最优解推荐方法的具体步骤如下：步骤1：找出最优解集合中，在单一个目标上最优的解，并计算出两个端点所在的
直线方程为：步骤2：计算各个点到这条直线的垂直距离d.把每个点的目标值代入公式步骤3：根据用户的需要，推荐d值最大的一个最优解作为推荐值，直接使用；或者若干个最优解，由用户进行选择。
40.（三）机械臂路径规划与执行：步骤1：在具体手术过程中，进入了指定的手术环节步骤以后，根据手术前机械臂位姿的优化求解以及多目标决策所获得的最优位姿方案以及手术进行情况中机械臂的最优位姿方案，获取当前手术环节的目标位姿（即在机械臂位姿的多目标决策环节中选定的最优目标）。
41.步骤2：双目摄像头获取机器人周边环境的三维信息。生成周围环境的点云图像，并通过以下公式：获得环境点云在光学定位系统坐标下的点云位置信息；其中为4*4常数转换矩阵，其值与双目相机与光学定位系统相对位置有关。
42.步骤3：算法随机生成候选路径点。
43.步骤4：判断路径点是否会碰到障碍物，如果会，则返回步骤3；否则继续进行下一步判断；步骤5：判断是否在该位姿上所有定位工具都可以被检测，如果不可以，则返回步骤3；否则继续下一步；其中，判断定位工具是否可以被检测，需要定位工具符合以下约束条件：(i)(ii),(iii)步骤6：把当前的候选路径点加入路径目录当中，用于最后生成合理的路径规划；步骤7：是否到达了目标的位姿，如果还未达到，则返回步骤3，否则找出当前目录中最短的路径，作为机械臂运动的路径。
44.步骤8：执行上述路径位姿，使机器人到达目标位姿。
45.以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。