本发明公开一种手术机器人导航定位系统及测量视角多目标优化方法。所述系统包括：手术操作规划系统、用于数据处理和机器人控制的控制主机、任意自由度的串联机械臂、定位传感器及其适配的定位工具、环境感知传感器。所述方法获取手术过程中各个环节的所有定位工具的信息及其所在的位置，建立基于决策变量的多目标最小化问题；为每个定位工具建立三维直角坐标系；通过定义定位工具之间的无遮挡裕度函数、以及最小化优化的至少两个目标函数；并且设置约束条件，使所述至少两个目标函数同时实现最小化。本发明针对手术机器人导航定位系统中的环境感知传感器与定位传感器测量的空间区域存在相互重合的区域（相互重合的区域为系统的可测量区域），定义了定位工具之间的无遮挡裕度函数，通过约束多目标优化算法，对优化问题进行求解，较好地解决了手术机器人导航定位系统中的测量视角多目标优化问题。
1.一种用于手术机器人导航定位系统的测量视角多目标优化方法，其特征在于，该方法通过手术操作规划系统获取手术过程中各个环节的所需定位工具的数量、编号以及所在位置，建立基于决策变量x的多目标最小化问题：x＝[q1，q2，q3，...，qN] (公式1)上式中：q1，q2，q3，...，qN为关节变量；N为关节变量的数量；决策变量x表示机械臂的N个关节变量组成的向量，其取值范围为机械臂各关节可实现的关节值范围Q，即x∈Q；
步骤2、定义最小化优化的至少两个目标函数f1和f2；
步骤3、设置约束条件，使至少两个目标函数f1和f2同时实现最小化；
步骤1.1、每个定位工具中心处设计有特异的形状特征，把特征轴线与定位部件质心所在的平面交点作为坐标原点；所述形状特征至少可以为圆孔、半球、凸台、圆锥；以所述坐标原点为球心，为每个定位工具构造包络该定位工具上K个定位部件的最小外接球，所述最小外接球的半径为li；
步骤1.2、以K个定位部件的质心所在平面的法线方向为z轴方向；且朝K个定位部件附着的一侧的方向为z轴正向；以垂直于z轴且指向离坐标原点最远的定位部件的方向为x轴正向，建立三维直角坐标系；
步骤1.3、将所有定位工具的集合记为S，对于第i个定位工具，其坐标系圆心为Mi，即Mi∈S；
其中，所述步骤2中最小化优化的至少两个目标函数f1和f2的定义如下：f2＝‑minj，k∈SOmin(j，k) (公式3)其中， 表示第m个定位工具的坐标原点与定位传感器的坐标原点之间的距离；f1表示所有定位工具的坐标原点与定位传感器的坐标原点之间的最大距离；Omin(j，k)表示对于给定的一对定位工具j与k，其在定位传感器各个相机坐标中较小的无遮挡裕度函数；
minj，k∈SOmin(j，k)表示在q所决定的机械臂位姿下，在定位传感器所有相机中，测量到的所有定位工具的二元组合中，最小的无遮挡裕度函数值；
通过以下公式计算所述较小的无遮挡裕度函数Omin(j，k)：rj＝ωlj，且ω＞1 (公式6)
Omin(j，k)＝min(O(j，k，L),O(j，k，R)) (公式12)上述公式中：G为定位传感器中左侧或右侧摄像头的坐标原点；L、R分别为定位传感器中左侧、右侧摄像头的坐标原点；Mj和Mk分别为任意两个定位工具j与k的半径分别为li、lk的最小外接球的球心，即定位工具j与k的的坐标原点；rj和rk分别为定位工具j与k的扩展半径；裕度系数ω为大于1的常数；向量长度 和 通过定位传感器测量获得；■表示向量点乘。
2.根据权利要求1所述的用于手术机器人导航定位系统的测量视角多目标优化方法，其特征在于，步骤3中所述约束条件如下：约束条件1：
约束条件1表示任意定位工具都要处于定位传感器与环境感知传感器共同可检测的范围内；
约束条件2表示从定位传感器的任意一侧摄像头向任意一个定位工具的连线与该定位工具的z轴方向之间的夹角不能大于既定的阈值；αG，i表示第i个定位工具的坐标原点向指向定位传感器中左侧或右侧摄像头坐标原点的向量与第i个定位工具的z轴方向向量之间的夹角；Th为预设阈值；
约束条件3表示任意两个定位工具之间互不遮挡，即任意两个定位工具之间的无遮挡裕度函数O(j，k，G)的最小值为非负。
3.一种手术机器人导航定位系统，该系统可执行如权利要求1或2所述的用于手术机器人导航定位系统的测量视角多目标优化方法，其特征在于，该系统包括：手术操作规划系统、用于数据处理和机器人控制的控制主机、任意自由度的串联机械臂、定位传感器及其适配的定位工具、环境感知传感器；所述定位传感器和/或环境感知传感器连接在所述机械臂的末端法兰上；
所述定位传感器为光学定位传感器，包括：基于可见光的双目深度摄像头、和/或基于近红外光双目定位摄像头；
所述环境感知传感器，包括：基于可见光的双目深度摄像头、激光雷达、超声波传感器；
所述环境感知传感器与定位传感器可以是组合设置的两个不同类型的传感器；也可以由同一个传感器实现，该传感器同时用于环境感知和定位；
所述环境感知传感器与定位传感器测量的空间区域存在相互重合的区域，所述相互重合的区域为所述手术机器人导航定位系统的可测量区域；
定位工具的数量为一个或多个；每个定位工具上有K个按照一定位置关系分布形成的定位部件；根据每个定位工具上K个定位部件的位置与/或数量不相同，可以唯一确定定位工具；所述定位部件为能够反光或发光的特异性标志物、和/或由若干个特定图案按一定位置关系排列后形成的部件；
能够反光的特异性标志物至少包括：表面覆盖有高反光度涂层的小球；能够发光的特异性标志物至少包括：LED灯；
所述特定图案为经过专门编码设计的图案，至少包括二维码、格雷码。
一种手术机器人导航定位系统及测量视角多目标优化方法
技术领域
[0001] 本发明涉及医疗设备技术领域，具体涉及外科手术机器人领域，尤其涉及一种手术机器人导航定位系统及测量视角多目标优化方法。
背景技术
[0002] 移动手术机器人由于其精确性和安全性的优点，为各类外科手术提供了一种全新的选择。经过一段时间的发展，手术机器人的相关技术日趋成熟，其在临床手术上的使用频率也逐年提高。
[0003] 具备光学导航系统的手术机器人在辅助手术方面有着广泛的应用，依靠图像导航技术，辅助手术系统可以对手术部位以及操作工具进行精确的定位，以辅助医生开展微创手术、远程手术或者由机器人辅助执行外科手术。目前，手术导航依赖光学导航设备通过检测识别光学定位工具，并进行图像以及位置的解算实现对手术部位或者手术工具的定位功能。
[0004] 现有技术的实际操作中，手术导航设备由辅助手术的医生根据手术需要进行人手调整。具体的，通过拖动设备的手柄，把光学导航设备调整到合适的观测位置。然而，这种交互方式在实际手术过程中带来诸多不便，而且对于一些特殊的手术位置设计，单靠人手难以调整出合适的测量位置，无法保证位置精度。
[0005] 因此，赋予光学导航设备运动能力，成为新的趋势。需要实现光学导航的主动导航，不仅要求机器人具备用于定位的光学导航传感器，还需要具备其他环境感知功能的传感器，来感知手术室内发生的人为或者设备位置变动的事件发生，由此触发响应的主动运动。因此需要特定的硬件构成系统。同时，如何实现上述系统中测量视角多目标优化，也是一个需要考虑的问题。
[0006] 考虑到上述因素，本发明提出一种手术机器人导航定位系统及测量视角多目标优化方法，该方法能够较好地解决手术机器人导航定位系统的测量视角多目标优化问题。
发明内容
[0007] 为了解决上述问题，本发明提供一种手术机器人导航定位系统及测量视角多目标优化方法。
[0008] 一种手术机器人导航定位系统，该系统包括：手术操作规划系统、用于数据处理和机器人控制的控制主机、任意自由度的串联机械臂、定位传感器及其适配的定位工具、环境感知传感器；所述定位传感器和/或环境感知传感器连接在所述机械臂的末端法兰上；
[0009] 所述定位传感器为光学定位传感器，包括：基于可见光的双目深度摄像头、和/或基于近红外光双目定位摄像头；
[0010] 所述环境感知传感器包括：基于可见光的双目深度摄像头、激光雷达、超声波传感器；
[0011] 所述环境感知传感器与定位传感器可以是组合设置的两个不同类型的传感器；也可以由同一个传感器实现，该传感器同时用于环境感知和定位；
[0012] 所述环境感知传感器与定位传感器测量的空间区域存在相互重合的区域，所述相互重合的区域为所述手术机器人导航定位系统的可测量区域；
[0013] 定位工具的数量为一个或多个；每个定位工具上有K个按照一定位置关系分布形成的定位部件；根据每个定位工具上K个定位部件的位置与/或数量不相同，可以唯一确定定位工具；所述定位部件为能够反光或发光的特异性标志物、和/或由若干个特定图案按一定位置关系排列后形成的部件；
[0014] 能够反光的特异性标志物至少包括：表面覆盖有高反光度涂层的小球；能够发光的特异性标志物至少包括：LED灯；
[0015] 所述特定图案为经过专门编码设计的图案，至少包括二维码、格雷码。
[0016] 一种用于如上所述的手术机器人导航定位系统的测量视角多目标优化方法，其中，该方法通过所述手术操作规划系统获取手术过程中各个环节的所需定位工具的数量、编号以及所在位置，建立基于决策变量x的多目标最小化问题：
[0017] x＝[q1，q2，q3，...，qN] (公式1)
[0018] 上式中：q1，q2，q3，...，qN为关节变量；N为关节变量的数量；决策变量x表示机械臂的N 个关节变量组成的向量，其取值范围为机械臂各关节可实现的关节值范围Q，即x∈Q。
[0019] 可选地，所述方法包括以下步骤：
[0020] 步骤1、为每个定位工具建立三维直角坐标系；
[0021] 步骤2、定义最小化优化的至少两个目标函数f1和f2；
[0022] 步骤3、设置约束条件，使至少两个目标函数f1和f2同时实现最小化。
[0023] 可选地，所述步骤1包括以下步骤：
[0024] 步骤1.1、每个定位工具中心处设计有特异的形状特征，把特征轴线与定位部件质心所在的平面交点作为坐标原点；所述形状特征至少可以为圆孔、半球、凸台、圆锥；以所述坐标原点为球心，为每个定位工具构造包络该定位工具上K个定位部件的最小外接球，所述最小外接球的半径为li；实际应用中，会在li的基础上增加一定的裕度，即球面大小估算时设定比 1i稍大一些，例如：将li乘以一个大于1的裕度系数ω，得到ri，以避免实际操作中的一些细小误差导致方法失效；
[0025] 步骤1.2、以K个定位部件的质心所在平面的法线方向为z轴方向；且朝K个定位部件附着的一侧的方向为z轴正向；以垂直于z轴且指向离坐标原点最远的定位部件的方向为x轴正向，建立三维直角坐标系；
[0026] 步骤1.3、将所有定位工具的集合记为S，对于第i个定位工具，其坐标系圆心为Mi，即 Mi∈S。
[0027] 可选地，所述步骤2中最小化优化的至少两个目标函数f1和f2的定义如下：
[0028]
[0029] f2＝‑minj，k∈Somin(j，k) (公式3)
[0030] 其中， 表示第m个定位工具的坐标原点与定位传感器的坐标原点之间的距离；f1表示所有定位工具的坐标原点与定位传感器的坐标原点之间的最大距离；Omin(j，k)表示对于给定的一对定位工具j与k，其在定位传感器各个相机坐标中较小的无遮挡裕度函数； minj，k∈S Omin(j，k)表示在q所决定的机械臂位姿下，在定位传感器所有相机中，测量到的所有定位工具的二元组合中，最小的无遮挡裕度函数值；
[0031] 通过以下公式计算所述较小的无遮挡裕度函数Omin(j，k)：
[0032]
[0033]
[0034] rj＝ωlj，且ω＞1 (公式6)
[0035]
[0036] rk＝ωlk，且ω＞1 (公式8)
[0037]
[0038]
[0039]
[0040] Omin(j，k)＝min(O(j，k，L)，O(j，k，R)) (公式12)
[0041] 上述公式中：G为定位传感器中左侧或右侧摄像头的坐标原点；L、R分别为定位传感器中左侧、右侧摄像头的坐标原点；Mj和Mk分别为任意两个定位工具j与k的半径分别为lj、lk的最小外接球的球心，即定位工具j与k的坐标原点；rj和rk分别为定位工具j与k的扩展半径；裕度系数ω为大于1的常数；向量长度 和 通过定位传感器测量获得；■表示向量点乘。
[0042] 可选地，步骤3中所述约束条件如下：
[0043] 约束条件1：
[0044] 约束条件2：
[0045] 约束条件3： 其中，
[0046] 约束条件1表示任意定位工具都要处于定位传感器与环境感知传感器共同可检测的范围内；约束条件2表示从定位传感器的任意一侧摄像头向任意一个定位工具的连线与该定位工具的 z轴方向之间的夹角不能大于既定的阈值；αG，i表示第i个定位工具的坐标原点向指向定位传感器中左侧或右侧摄像头坐标原点的向量与第i个定位工具的z轴方向向量之间的夹角；Th 为预设阈值；
[0047] 约束条件3表示任意两个定位工具之间互不遮挡，即任意两个定位工具之间的无遮挡裕度函数O(j，k，G)的最小值为非负。
[0048] 本发明技术方案带来的有益效果：
[0049] 本发明提供一种手术机器人导航定位系统及测量视角多目标优化方法。首先，本发明对手术机器人导航定位系统进行了优化设计；其次，针对手术机器人导航定位系统中的环境感知传感器与定位传感器测量的空间区域存在相互重合的区域(相互重合的区域为系统的可测量区域)，本发明定义了定位工具之间的无遮挡裕度函数，通过约束多目标优化算法，对优化问题进行求解，较好地解决了手术机器人导航定位系统中的测量视角多目标优化问题。
附图说明
[0050] 图1为根据本发明实施例的手术机器人导航定位系统的总体结构图；
[0051] 图2为根据本发明实施例的手术机器人导航定位系统的实施方案图；
[0052] 图3为根据本发明实施例的手术机器人导航定位系统中坐标系的建立示意图；
[0053] 图4为根据本发明实施例的定位工具及其坐标系建立图；
[0054] 图5为根据本发明实施例的无遮挡裕度函数O(j，k，G)设计的示意图；
[0055] 图6为根据本发明实施例的观测角度αG，i的示意图。
具体实施方式
[0056] 下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，可以理解的是，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0057] 本发明提供一种手术机器人导航定位系统及测量视角多目标优化方法。
[0058] 图1为本发明的手术机器人导航定位系统的总体结构图。如图1所示，所述手术机器人导航定位系统包括：手术操作规划系统、用于数据处理和机器人控制的控制主机、机械臂、定位传感器及其适配的定位工具、环境感知传感器；所述环境感知传感器实现手术环境，例如：潜在遮挡物和/或障碍物的感知。所述机械臂为7自由度的串联机械臂；所述定位传感器和/或环境感知传感器连接在所述机械臂的末端法兰上。
[0059] 图2为本发明的手术机器人导航定位系统的实施方案图。如图2所示，实施方案如下：系统由一台7自由度机械臂以及连接在机械臂末端法兰上的近红外光学定位系统(即“定位传感器”)和双目摄像头，以及用于数据处理和机器人控制的计算机、近红外光学定位系统适配的定位工具组成。
[0060] 这里的近红外光学定位系统包括两个红外发射灯、以及用于检测反射红外光的红外光摄像头。其工作原理是：左右两个红外发射灯发射特定红外光，投射在定位工具上的反光小球表面。反光小球反射红外光，并被红外光摄像头检测到，其根据接收到的反射红外光推算出近红外光学定位系统与各个小球之间的相对位置，并根据预先标定好的定位关系模型计算出各个定位工具相对于近红外光学定位系统的相对位置。
[0061] 机械臂的基座坐标为O，第k个关节的关节角为qk，末端法兰的坐标系原点为E。近红外光学定位系统中心坐标为N，左右两边的摄像头坐标分别为R与L。在机械臂处于位置p 时，近红外光学定位系统与环境感知传感器重合的可测量区域空间为A(p)。双目摄像头的坐标系为C。
[0062] 如图2中的附图标记含义如下：1‑七自由度机械臂，2‑近红外光学定位系统，3‑ 双目摄像头，4‑定位工具，5‑计算机。
[0063] 图3为本发明的手术机器人导航定位系统中坐标系的建立示意图。所有定位工具的集合为S，对于第i个工具，其坐标系圆心为Mi，即Mi∈S。光学定位系统中心坐标为N，左右两边的摄像头坐标分别为R与L。在机械臂处于位置p时，光学定位系统可测量区域空间为A(p)，即无遮挡条件下，在机械臂处于位置p时，定位工具可以被正常测量的所有可能位置的集合。双目摄像头的坐标系为C。
[0064] 图4为本发明的定位工具及其坐标系建立图。定位工具选用与近红外光学定位系统 (即“定位传感器”)相匹配的定位工具，如图4所示。每一个定位工具上有4个表面覆盖有高反光度涂层的小球，按一定的位置关系分布形成的。同一个定位工具的4个小球的球心都在同一个平面上，以K个定位部件的质心所在平面的法线方向为z轴方向；且朝K个定位部件附着的一侧的方向为z轴正向。每一个定位工具小球的位置与/或数量不相同，用以区分定位工具。每个定位工作采用小球球心所在平面，与定位工具连杆中心小孔(即外形特征的一种实例)的中心轴相交点作为坐标原点，并以相交点指向离原点距离最远的小球方向为x轴方向。以交点为圆心，建立包络所有小球的最小外接球，该外接球半径为li。所有定位工具的集合为S，对于第i的工具，其坐标系圆心为Mi，即Mi∈S。
[0065] 测量视角多目标优化：通过手术操作规划系统获取手术过程中各个环节的所有定位工具的信息以及所在的位置。建立如下多目标最小化问题：
[0066] 决策变量：x＝[q1，q2，q3，...，q7]
[0067] 表示机械臂的7个关节变量组成的向量，其取值范围为机械臂各关节可实现的关节值范围 Q，即x∈Q。这里，机械臂的关节的数量可以是合理的任意多个。
[0068] 优化目标如下(至少将目标函数f1和f2同时最小化)：
[0069] 优化目标1：最小化定位工具与近红外光学定位系统的最大距离：
[0070]
[0071] 其中， 表示第m个定位工具的坐标原点与近红外光学定位系统的坐标原点之间的距离；
[0072] 优化目标2：minj，k∈SO(j，k)表示定位工具之间最小的无遮挡裕度函数值。通过取其数值的相反数，转化成最小化优化问题：
[0073]
[0074] 其中，Omin(j，k)表示对于给定的一对定位工具j与k，其在定位传感器各个相机坐标中较小的无遮挡裕度函数；minj，k∈SOmin(j，k)表示在q所决定的机械臂位姿下，在定位传感器所有相机中，测量到的所有定位工具的二元组合中，最小的无遮挡裕度函数值；
[0075] 定位工具j与k之间的无遮挡裕度函数O(j，k，G)定义如图5所示：
[0076] 图5为本发明的无遮挡裕度函数O(j，k，G)设计的示意图，其描述了无遮挡裕度函数O(j，k，G) 的定义。具体地，图5中描述的是任意两个定位工具以及定位传感器左或者右边其中一侧摄像头的几何关系。因此如果对于定位工具数量大于2时，任意两个定位工具和任意一侧摄像头就会产生一个特定的O(j，k，G)值，例如：3个定位工具可以产生6个O(j，k，G)值，即： O(1，2，L)，O(1，3，L)，O(2，3，L)，O(1，2，R)，O(1，3，R)，O(2，3，R)。
[0077] 其中，G是指定位传感器左或右其中一侧摄像头坐标系原点。Mj和Mk分别指任意两个定位工具抽象成球体以后的球心，同时也是定位工具坐标系原点。rj和rk是指定位工具抽象成球体的半径。选择每个定位工具采用小球球心所在平面与定位工具连杆中心小孔的中心轴相交点作为坐标原点。以坐标原点为球心的最小外接球半径为li。考虑到实际操作时的误差影响，在li的基础上进行裕度ω倍的扩展，获得定位工具抽象成球体的半径ri。(这里定位工具的特点是，以一个中心延伸出4条或以上共平面的连杆，连杆末端设置有小球。在一套导航设备内，每一个定位工具的小球之间相对位置是唯一的)。其中ω＞1。
[0078] 因此，rj和rk大小已知。向量长度 和 可以通过定位传感器测量获得。βG，j和βG，k可以通过以下关系求得：
[0079]
[0080]
[0081] 而αG，j，k可以通过向量计算：
[0082]
[0083] 其中，■表示向量点乘。
[0084] 最后，计算
[0085]
[0086] 其中，ri＝ωli，表示把定位工具抽象简化后的球面的半径；其中，ω＞1。
[0087] 约束条件如下：
[0088] 约束条件1：
[0089] 约束条件2：
[0090] 约束条件3： 其中，
[0091] 约束条件1表示任意定位工具都要处于定位传感器与环境感知传感器共同可检测的范围内；
[0092] 约束条件2表示从定位传感器的任意一侧摄像头向任意一个定位工具的连线与该定位工具的 z轴方向之间的夹角不能大于既定的阈值；αG，i表示第i个定位工具的坐标原点向指向定位传感器中左侧或右侧摄像头坐标原点的向量与第i个定位工具的z轴方向向量之间的夹角；Th 为预设阈值，例如：Th＝π/2；
[0093] 约束条件3表示任意两个定位工具之间互不遮挡，即任意两个定位工具之间的无遮挡裕度函数O(j，k，G)的最小值为非负。
[0094] 图6为本发明的观测角度αG，i的示意图。观测角度是指：左或右摄像头原点与任一个定位工具的Z轴(定位工具向上指的法向方向固定为定位工具坐标Z轴)之间的夹角。
[0095]
[0096] 如图6所示：其中，G是指定位传感器左或者右边其中一侧摄像头坐标系原点。为在G坐标系下定位工具Z轴单位向量。通过定位传感器可以求得 和 带入公式计算即可。另外，需要说明的是，任意一侧相机对于任意一个定位工具，均会有一个观测角大小值。
[0097] 综上所述，即需要优化以下优化问题：
[0098] 决策变量：x＝[q1，q2，q3，...，qN]
[0099] 同时最小化：
[0100]
[0101]
[0102] 并同时考虑以下约束：
[0103] (i)
[0104] (ii)
[0105] (iii)
[0106] 通过约束多目标优化算法，可以对上述的优化问题进行求解。在本例子当中采用MOEA/D‑ CDP算法，可以获得上述优化问题的帕累托最优解。
[0107] 尽管结合优选实施方案具体展示和介绍了本发明，但所属领域的技术人员应该明白，在不脱离所附权利要求书所限定的本发明的精神和范围内，在形式上和细节上可以对本发明做出各种变化，均为本发明的保护范围。
