技术领域
[0001] 本
发明涉及一种穿刺机器人导航系统，特别是涉及基于双目视觉 的动态补偿功能的穿刺机器人导航系统，属于导航技术领域。
背景技术
[0002] 近年来，穿刺手术在微创外科领域得到了迅速发展，具有创伤小、
疼痛轻、恢复快和手术并发症小的特点，是传统开放性手术的一次重 大变革，代表了医学手术发展的必然趋势。传统的经皮穿刺手术主要 是在医学图像引导下，由医生手持穿刺针等工具对病人做穿刺介入， 手术的效果很大程度上依赖于操作者的经验和技能，因此手术
治疗效 果差异较大，存在着穿刺病灶靶点
定位不准、穿刺路径规划困难和穿 刺精准度低的问题，特别的是患者是柔性的，由于患者的互相或者患 者疼痛翻转引起贴的靶点在术中不规则的运动，在这种辅助情景下， 无疑增加了医生穿刺的难度。
[0003] 在医疗机器人导航系统中，研究设计双目视觉动态补偿功能的穿 刺机器人导航系统是整个手术的关键，其作用就是实时得到靶点在机 器人中的空间
位置。首先通过术前与术中医学
图像配准以及术中医学 图像与患者、手术器械之间的配准关系，经过相应的坐标转换，控制 手术器械达到要求的部位后，通过对特定的标记点进行
跟踪，得到标 记点的运动轨迹，通过智能分析将运动轨迹的坐标点实时反馈给机器 人，让机器人在一定的范围内跟踪目标点。
[0004] 研究具有双目视觉动态补偿功能的穿刺机器人导航系统，具有极 其重要的意义，这套系统不仅大大提高了医生在术中穿刺的精准度， 也能够减轻病人的痛苦，将医生做手术的时间大大缩短了，提高了效 率。本系统首先将医学图像
坐标系通过3D视觉，利用三
角测量法原 理获取此标记点在机器人中的位置和
姿态，并将信息反馈给机器人， 让机器人到达初始的位置点，然后利用视觉显著性的跟踪
算法，视觉 显著性的算法是：拟人类视觉注意机制的显著性检测对深层理解图像 的过程有重要作用，注意是人类信息加工过程中的一项重要的心理调 节机制，它将信息资源分配，让
感知具备选择能
力。在海量的视频数 据下，如果将这种机制引入目标跟踪领域，优先对场景突出目标区域 处理，对标记点进行跟踪，将每
帧得到的标记点运动的空间位置通过 tcp协议通讯实时反馈给机器人，通过均值来校正穿刺的靶点位置， 可以消除人为的误差操作，具有极其重要的意义。
发明内容
[0005] 本发明的主要目的是为了提供基于双目视觉的动态补偿功能的 穿刺机器人导航系统，可实现机器人的远程编程，降低机器人的使用 维护成本。
[0006] 本发明的目的可以通过采用如下技术方案达到：
[0007] 基于双目视觉的动态补偿功能的穿刺机器人导航系统，包括底 座，所述底座的顶部安装有
支撑柱，且支撑柱的顶部交接有第一链接
块，所述第一链接块的外侧安装有第一交接臂，且第一交接臂的一端 交接有第二链接块，所述第二链接块的外侧安装有第二交接臂，且第 二交接臂的外侧交接有第三链接块，所述第三链接块的外侧安装有第 三交接臂，且第三交接臂的一端交接有第四链接块，所述第四链接块 的外侧设置有穿刺针，所述底座的内部设置有
机械臂控制设备。
[0008] 进一步的，该穿刺机器人导航系统的基于标记点的空间映射过程 包含四个步骤：
[0009] 1)医学
图像空间内对病灶靶点、穿刺入针点以及穿刺路径
位姿 的定位；
[0010] 2)双目摄像机图像空间内对患者
皮肤表层标记弹定位；
[0011] 3)双目摄像机图像空间内对机器人空间坐标的定位；
[0012] 4)计算出机器人空间内病灶靶点、穿刺入针点和穿刺路径位姿 的定位。
[0013] 进一步的，将靶点映射到机器人的
基础坐标系，通过机器人的逆 运动学，将机器人运动到穿刺点的上方，通过视觉显著性的跟踪算法 完成的，其跟踪算法如下：
[0014] 初始化，跟踪标志初值设定flag，计数量conti；
[0015] 读入视频文件，获取有关信息，视频总帧数N；
[0016] 求每一帧的显著图；
[0017] 对显著图进行处理，去除边界；
[0018] 相邻两帧显著图的
像素差，突出视频运动信息；
[0019] 自由状态下(flag＝＝0)，当前帧显著图像素差的最大值对应在 原rgb
视频帧中的位置信息(x，y)其中第一帧位置信息(x_1,y_1)；
[0020] 对得出的变量，求得dx，当前帧的位置信息与上一帧的位置信 息作差；
[0021] 假如dx小于设定的
阈值，此时计数量conti+1，连续6帧dx都 是小于设定的阈值，此时flag＝＝1，conti清0，画出框，框的中心位 置(x_1，y_1)保留；
[0022] 在跟踪状态下，求得此时帧中前200个最大显著图像素的位置信 息，分别计算与上一帧显著中心(x_1,y_1)的城市距离dist；
[0023] 将dist进行升序排列，如果它的最小的距离大于预定值A(80)， 放弃本次位置信息，依然保留上一帧的位置信息，同时出界计数器 (limit)+1；
[0024] 如果出界计数器超过2，此时变为自由太，画红色框；
[0025] 如果dist的最小的距离小于等于预定值A(80)，我们逐步筛选 出距离最近的点，求得这些距离最近点的均值坐标(x_m,y_m)；
[0026] 判断均值位置信息与上一帧跟踪点的距离的差d_distance小于设 定值B，以此时的均值坐标为中心画框，同时将跟踪点位置更新；
[0027] 如果d_distance大于设定的值B，跟踪点依然是上一帧的位置， 位置更新状态标识(over)+1，if over是大于1次的，变为自由太。
[0028] 进一步的，所述机械臂控制设备(11)的内部设置有机器人坐标 系系统。
[0029] 进一步的，所述图像坐标系系统与3D模型仿射坐标系系统对应； 其过程如下：
[0030] 利用在3D模型上粘贴不少于4个标记点建立一个3D模型仿射坐 标系；
[0031] 利用不在同一平面内的4个标记点建立参考坐标系，任选其中一 个标记点M0(xm0,ym0,zm0)作为参考坐标系原点；
[0032] 且平行于CT图像坐标系的三个坐标轴方向，和其他三个标记点 M1(xm1,ym1,zm1)、M2(xm2,ym,2,zm2)、M3(xm3,ym3,zm3)建立仿射 关系；
[0033] 从3D模型放射坐标系到图像坐标系的映射矩阵可表示为如下：
[0034]
[0035] 这样3D模型仿射坐标系和图像坐标系建立了一一对应的关系， 则图像坐标系中的任意一点P(xmp,ymp,zmp)在3D模型上都有唯一 的一点与之对应，其转换公式可表示为：
[0036]
[0037] 则3D模型仿射坐标系相对于CT图像坐标系发生了平移和旋转， 其分别在x、y、z轴上平移距离为xm0、ym0、zm0，其沿x、y、z轴旋 转为0。
[0038] 进一步的，所述3D模型仿射坐标系系统与双目摄像机光学坐标 系系统对应；其过程如下：
[0039] 通过双目摄像机做光学定位计算，求得四个标记点在光学坐标系 下的坐标位置，建立摄像机光学坐标系到3D模型仿射坐标系的转换 矩阵：
[0040]
[0041] 3D模型上的任意一点在双目摄像机光学坐标系上都有唯一的一 点与之对应，其转换公式可表示为：
[0042]
[0043] 求得图像坐标系中的点对应到摄像机光学坐标系下的映射关系 为：
[0044]
[0045] 其中， 是图像坐标系到摄像机坐标系的映射矩阵。
[0046] 进一步的，所述机器人坐标系系统与机器人仿射坐标系系统对 应；其过程如下：
[0047] 在机器人末端选取四个标记点，作为机器人的标记点，建立机器 人坐标系到机器人仿射坐标系之间的转换矩阵：
[0048]
[0049] 其转换公式可表示为：
[0050]
[0051] 机器人的任意一点在机器人仿射坐标系上都有唯一的一点与之 对应。
[0052] 进一步的，所述机器人仿射坐标系系统与双目摄像机光学坐标系 系统对应；其过程如下：
[0053] 可建立机器人仿射坐标系和双目摄像机光学坐标系之间的对应 关系，机器人上的每个点都可以唯一的映射到双目摄像机光学坐标系 中，其转换关系矩阵为：
[0054]
[0055] 其转换公式可表示为：
[0056]
[0057] 由以上关系可求得机器人坐标系中的点对应到摄像机光学坐标 系下的映射关系为：
[0058]
[0059] 其中， 是光学坐标系到摄像机坐标系的映射矩阵。
[0060] 进一步的，所述图像坐标系系统与机器人坐标系系统对应；其过 程如下：
[0061] 已经得出图像坐标系到摄像机光学坐标系的转换关系T3，光学 坐标系到机器人坐标系的转换关系T6；
[0062] 求得图像坐标系上的点到机器人坐标系的转换关系：
[0063] P机器人＝T6T3P图像＝T7P图像
[0064] 其中，T7＝T6T3，是图像坐标系到机器人坐标系的转换关系矩阵， 可实现3D模型上的任意点位置坐标映射到机器人坐标系下。
[0065] 本发明的有益技术效果：
[0066] 1、本发明提供的基于双目视觉的动态补偿功能的穿刺机器人导 航系统，通过双目相机识别患者皮肤表面的圆形标记点和手术机器人 末端的标记点，得出他们在视觉坐标系内的空间位置坐标，完成穿刺 手术中患者医学图像空间坐标系位置到手术机器人空间坐标系的映 射，将机器人运动到初始穿刺的点，然后打开相机连续工作模式，跟 踪标记点，在一定帧数内得到标记点的运动轨迹，通过均值来进行校 正穿刺的靶点，可以减少人为穿刺带来的误差，借助以机器人定位精 度高、运行稳定、安全可靠的特点，完成医学外科微创穿刺手术中辅 助精准定位的问题。
[0067] 2、本发明提供的基于双目视觉的动态补偿功能的穿刺机器人导 航系统，视觉算法的处理和推导较为简便。
附图说明
[0068] 图1为按照本发明的基于双目视觉的动态补偿功能的穿刺机器 人导航系统的一优选
实施例的机械臂结构立体图；
[0069] 图2为按照本发明的基于双目视觉的动态补偿功能的穿刺机器 人导航系统的一优选实施例的计算机图像坐标系到机器人坐标系映 射图；
[0070] 图3为按照本发明的基于双目视觉的动态补偿功能的穿刺机器 人导航系统的一优选实施例的基于标记点的坐标系映射关系图；
[0071] 图4为按照本发明的基于双目视觉的动态补偿功能的穿刺机器 人导航系统的一优选实施例的视觉显著性跟踪算法的原理图。
[0072] 图中：1-底座，2-支撑柱，3-第一链接块，4-第一交接臂，5-第 二链接块，6-第二交接臂，7-第三链接块，8-第三交接臂，9-第四链 接块，10-穿刺针，11-机械臂控制设备。
具体实施方式
[0073] 为使本领域技术人员更加清楚和明确本发明的技术方案，下面结 合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式 不限于此。
[0074] 如图1所示，基于双目视觉的动态补偿功能的穿刺机器人导航系 统，包括底座1，所述底座1的顶部安装有支撑柱2，且支撑柱2的 顶部交接有第一链接块3，所述第一链接块3的外侧安装有第一交接 臂4，且第一交接臂4的一端交接有第二链接块5，所述第二链接块
5的外侧安装有第二交接臂6，且第二交接臂6的外侧交接有第三链 接块7，所述第三链接块
7的外侧安装有第三交接臂8，且第三交接 臂8的一端交接有第四链接块9，所述第四链接块
9的外侧设置有穿 刺针10，所述底座1的内部设置有机械臂控制设备11。
[0075] 在实施例中，如图1所示，所述机械臂控制设备11的内部设置 有机器人坐标系系统，从而可以通过机器人坐标系系统对机械臂实现 导航。
[0076] 在实施例中，如图2和图3所示，所述图像坐标系系统与3D模 型仿射坐标系系统对应，利用在3D模型上粘贴不少于4个标记点建 立一个3D模型仿射坐标系，利用不在同一平面内的4个标记点建立 参考坐标系，任选其中一个标记点M0(xm0,ym0,zm0)作为参考坐标系 原点，且平行于CT图像坐标系的三个坐标轴方向，则3D模型仿射坐 标系相对于CT图像坐标系发生了平移和旋转，其分别在x、y、z轴 上平移距离为xm0、ym0、zm0，其沿x、y、z轴旋转为0。和其他三个标 记点M1(xm1,ym1,zm1)、M2(xm2,ym,2,zm2)、M3(xm3,ym3,zm3)建立 仿射关系，从3D模型放射坐标系到图像坐标系的映射矩阵可表示为 如下：
[0077]
[0078] 这样3D模型仿射坐标系和图像坐标系建立了一一对应的关系， 则图像坐标系中的任意一点P(xmp,ymp,zmp)在3D模型上都有唯一 的一点与之对应，其转换公式可表示为：
[0079]
[0080] P图像＝T1P3D仿射
[0081] 在实施例中，如图1所示，所述3D模型仿射坐标系系统与双目 摄像机光学坐标系系统对应，通过双目摄像机做光学定位计算，同样 求得四个标记点在光学坐标系下的坐标位置，同样可以建立摄像机光 学坐标系到3D模型仿射坐标系的转换矩阵：
[0082]
[0083] 这样双目摄像机光学坐标系和3D模型仿射坐标系建立了一一对 应的关系，则3D模型上的任意一点在双目摄像机光学坐标系上都有 唯一的一点与之对应，其转换公式可表示为：
[0084]
[0085] P3D仿射＝T2P光学
[0086] 由公式XX和公式XX关系可求得图像坐标系中的点对应到摄像机 光学坐标系下的映射关系为：
[0087]
[0088] 其中 是图像坐标系到摄像机坐标系的映射矩阵。
[0089] 在实施例中，如图1所示，所述机器人坐标系系统与机器人仿射 坐标系系统对应，仿照以上的转换关系，在机器人末端选取四个标记 点，作为机器人的标记点，可建立机器人坐标系到机器人仿射坐标系 之间的转换矩阵：
[0090]
[0091] 这样机器人坐标系和机器人仿射坐标系建立了一一对应的关系， 则机器人的任意一点在机器人仿射坐标系上都有唯一的一点与之对 应，其转换公式可表示为：
[0092]
[0093] P机器人＝T4P机器人仿射
[0094] 在实施例中，如图1所示，所述机器人仿射坐标系系统与双目摄 像机光学坐标系系统对应，同理，可建立机器人仿射坐标系和双目摄 像机光学坐标系之间的对应关系，机器人上的每个点都可以唯一的映 射到双目摄像机光学坐标系中，其转换关系矩阵为：
[0095]
[0096] 其转换公式可表示为：
[0097]
[0098] P光学＝T2P机器人仿射
[0099] 由以上关系可求得机器人坐标系中的点对应到摄像机光学坐标 系下的映射关系为：
[0100]
[0101] 其中 是光学坐标系到摄像机坐标系的映射矩阵。
[0102] 在实施例中，如图1所示，所述图像坐标系系统与机器人坐标系 系统对应，综上所述，已经得出图像坐标系到摄像机光学坐标系的转 换关系T3，光学坐标系到机器人坐标系的转换关系T6，故可求得图 像坐标系上的点到机器人坐标系的转换关系：
[0103] P机器人＝T6T3P图像＝T7P图像
[0104] 其中T7＝T6T3，是图像坐标系到机器人坐标系的转换关系矩阵，可 实现3D模型上的任意点位置坐标映射到机器人坐标系下。
[0105] 通过双目摄像机识别圆形标记点在双目图像平面内的图像坐标 值，利用精确推导出圆形标记点的圆心在视觉空间坐标系内的精确坐 标值；同时双目摄像机通过识别
机器人本体的圆形标记点坐标，推导 得出机器人在视觉空间坐标系内的坐标值。这样，标记点在图像空间 和机器人空间的坐标位置就能够确定，并且他们之间存在着一一对应 的关系。
[0106] 如图4所示，上述能够将靶点映射到机器人的基础坐标系，通过 机器人的逆运动学，将机器人运动到穿刺点的上方。此时依据视觉显 著性的跟踪算法：
[0107] 初始化(跟踪标志初值设定flag，计数量conti，)。
[0108] 读入视频文件，获取有关信息(视频总帧数N)。
[0109] 求每一帧的显著图。
[0110] 对显著图进行处理(去除边界)。
[0111] 相邻两帧显著图的像素差，突出视频运动信息。
[0112] 自由状态下(flag＝＝0)，当前帧显著图像素差的最大值对应 在原rgb视频帧中的位置信息(x，y)其中第一帧位置信息(x_1,y_1)。
[0113] 对step6得出的变量，求得dx，(当前帧的位置信息与上一帧 的位置信息作差)。
[0114] 假如dx小于设定的阈值(当前帧显著点位置和上一帧位置信息 距离近，跳动小)，此时计数量conti+1，(不满足dx小于设定的 阈值，计数清0)连续6帧dx都是小于设定的阈值，(conti大于 等于5)此时flag＝＝1，conti清0，画出框(最大显著点位置，框 的大小40*40)，框的中心位置(x_1，y_1)保留。
[0115] 在跟踪状态下，求得此时帧中前200个最大显著图像素的位置信 息，分别计算与上一帧显著中心(x_1,y_1)的城市距离dist。
[0116] 将dist进行升序排列，如果它的最小的距离大于预定值A(80)， 我们放弃本次位置信息，依然保留上一帧的位置信息，同时出界计数 器(limit)+1。
[0117] 如果出界计数器超过2，此时变为自由太，画红色框。
[0118] 如果dist的最小的距离小于等于预定值A(80)，我们逐步筛选 出距离最近的点，求得这些距离最近点的均值坐标(x_m,y_m)
[0119] 判断均值位置信息与上一帧跟踪点的距离的差d_distance小于设 定值B(每一帧运动都是细微的。如果这个跳动很小我们认为跟踪的 目标是不变化的)，那么我们以此时的均值坐标为中心画框，同时将 跟踪点位置更新。
[0120] 当然如果d_distance大于设定的值B，跟踪点依然是上一帧的 位置，位置更新状态标识(over)+1，if over是大于1次的，那么 变为自由太。
[0121] 我们首先将我们的标记点利用opencv的
鼠标框框出，采用的策 略是取帧差显著图前300个像素点位置(后续章节
流程图不同之处加 黑表示)，取得的点依次与上一帧跟踪目标确定中心点利用公式：
[0122]
[0123] 作欧式距离.找到欧氏距离最小的显著点位置，而此时最小的欧 式距离在设定阈值内(本次阈值可以与搜寻状态下一样)，可以充分 说明这300个显著点位置坐标都是目标特征值。为了让跟踪效果更为 精确，我们利用主成分分析原理逐步删选到距离跟踪点最为接近的 点，丢弃距离较远的显著点，利用公式求得删选出的这些点的均值。
[0124]
[0125] 公式中，(x1,x2…xn)表示删选出的显著点位置横坐标。同理可以 求得纵坐标。以为中心跟踪点，画出跟踪框。
[0126] 如图1所示，本实施例提供的基于双目视觉的动态补偿功能的穿 刺机器人导航系统的工作过程如下：
[0127] 步骤1：医学图像空间内对病灶靶点、穿刺入针点以及穿刺路径 位姿的定位；
[0128] 步骤2：双目摄像机图像空间内对患者皮肤表层标记弹定位；
[0129] 步骤3：双目摄像机图像空间内对机器人空间坐标的定位；
[0130] 步骤4：计算出机械臂空间内病灶靶点、穿刺入针点和穿刺路径 位姿的定位；
[0131] 步骤5：从而通过机械臂控制设备11控制机械臂进行微创穿刺。
[0132] 综上所述，在本实施例中，医学图像空间内对病灶靶点、穿刺入 针点以及穿刺路径位姿的定位，双目摄像机图像空间内对患者皮肤表 层标记弹定位，双目摄像机图像空间内对机器人空间坐标的定位，计 算出机械臂空间内病灶靶点、穿刺入针点和穿刺路径位姿的定位，计 算出机械臂空间内病灶靶点、穿刺入针点和穿刺路径位姿的定位，可 以减少人为穿刺带来的误差，借助以机器人定位
精度高、运行稳定、 安全可靠的特点，完成医学外科微创穿刺手术中辅助精准定位的问 题。
[0133] 以上所述，仅为本发明进一步的实施例，但本发明的保护范围并 不局限于此，任何熟悉
本技术领域的技术人员在本发明所公开的范围 内，根据本发明的技术方案及其构思加以等同替换或改变，都属于本 发明的保护范围。
