介入手术机器人同步定位与三维地图构建方法和系统

技术领域

本发明涉及智能医疗器械技术领域，更具体地，涉及一种介入手术机器人同步定位与三维地图构建方法和系统。

背景技术

血管介入手术是指通过穿刺血管，导丝、导管等器械进入体内对血管内病变进行诊治的一种方式，具有操作精确、手术时间短、手术创伤小、术后恢复时间短、患者遭受的痛苦轻等优势，属于微创手术的一种。血管介入手术机器人主要是在血管介入手术中进行导管的推进及导航。目前介入手术机器人的导航技术大多通过体外造影或空间坐标配准获取导管的位置状态信息，这些技术通常即时性较差，准确度不高。血管内的位置状态信息具有更加准确、更加即时、更加直接的特点，然而导管所在的血管内信息提供的状态信息目前尚未被运用于介入手术机器人的导航。

血管内超声(intravenous ultrasound，IVUS)是指无创性的超声技术和有创性的导管技术相结合，使用末端连接有超声探针的特殊导管进行的医学成像技术。IVUS利用超声原理来探测血管内、血管壁及周围组织的结构，可用于指导血管介入治疗，堪称心血管介入术中医生的“火眼金睛”。

同步定位与地图构建系统(simultaneous localization and mapping,SLAM)，主要用于解决机器人在未知环境运动时的定位与地图构建问题。根据移动机器人搭载的传感器不同，可以将SLAM分为激光SLAM和视觉SLAM。其中，视觉SLAM使用摄像头获取场景的图片信息，由此来完成环境的感知工作。

Beatriz FarolaBarata等提出了一种基于IVUS的血管内机器人导航的局部血管估计方法(IVUS-Based Local Vessel Estimation for Robotic IntravascularNavigation,IEEE ROBOTICS ANDAUTOMATION LETTERS,2021)，通过一个圆柱体模型来近似导管尖端附近的血管几何形状，通过形状估计来实现血管介入机器人避障功能。然而，该研究并未实现利用IVUS的成像信息实现血管内的定位与建图。

发明内容

由于现有技术存在上述缺陷，本发明提供了一种利用IVUS的血管介入手术机器人SLAM的方法和系统，能够结合电磁定位提升血管介入手术机器人的自主定位与导航性能，有助于智能医疗机器人的发展。

为实现上述目的，一方面，本发明提供一种介入手术机器人同步定位与三维地图构建方法，其特征在于，包括以下步骤：

步骤S1、同步定位和建图系统通过介入手术机器人内的超声探头获取血管内超声图像作为输入帧，获得地图点；

步骤S2、对所述输入帧进行预处理；

步骤S3、采用ORB特征点提取算法，提取特征点；

步骤S4、通过运动模型对超声探头的位姿进行估计；

步骤S5、通过非极大值抑制算法保留区域中的最优特征点，剔除相邻位置的多个特征点；

步骤S6、同步定位和建图系统通过介入手术机器人内的电磁定位线圈获取图像的深度，并结合所述输入帧的特征点，生成对应于最优特征点的三维地图点信息；每个所述地图点信息包括世界坐标系中的3D坐标、视图方向；所述视图方向是指连接该点云和其对应观测关键帧光心的射线方向；

步骤S7、对每幅新来的图像用算法提取ORB特征点进行实时跟踪，并与最近的关键帧进行比较，计算特征点的位置并粗略的估计相机位姿，用于判断当前帧是否需要加入关键帧；

步骤S8、局部地图构建：根据加入的关键帧，构建局部地图，并根据局部空间内的特征点与相机位姿，求解局部Bundle Adjustment最小化重投影误差，以获得更精细的相机位姿与特征点空间位置，同时剔除地图点能够被其他关键帧所观察的多余关键帧；

步骤S8、通过运动模型对超声探头的位姿进行估计；

步骤S9、使用卡尔曼滤波法，对机器人的移动进行观测和估计；

步骤S10、不断更新地图与位置，实现机器人的同步定位与地图构建。

所述同步定位与三维地图构建方法有效地将IVUS、电磁定位系统和SLAM结合起来，能实现血管介入手术机器人的血管内自主定位与导航，提升了介入手术中机器人的导航和位置感知能力，有效提升手术的精确性与手术效果。

进一步地，所述同步定位和建图系统不引入用于检测机器人运动路径重复的回环检测模块。

进一步地，所述步骤S2中所述预处理包括将超声图像转化为灰度图像。

进一步地，所述步骤S3中所述ORB特征点提取算法包括FAST算法。此特征使得同步定位和建图系统具有计算速度快、效率高的特点，能够有效提高系统的运行效率。

进一步地，所述步骤S5中，对每个关键帧Ki保存以下信息：相机位姿T(i,w)，从世界坐标系转换到相机坐标系下的变换矩阵相机参数，包括主点和焦距。

进一步地，所述步骤S8中所述运动模型为PnP算法。

另一方面，本发明还提供一种介入手术机器人同步定位与三维地图构建系统，用以实现上述的介入手术机器人同步定位与三维地图构建方法，其特征在于，包括：导管组件、设置在所述导管组件内部前端的超声传感器和电磁定位线圈；所述超声传感器与电磁定位线圈之间设有电磁屏蔽层；

还包括被手术体的体外部分，所述体外部分包括：所述超声传感器的近端驱动模块和超声成像系统、与所述电磁定位线圈配套的电磁定位系统以及数据处理的同步定位和建图系统；所述同步定位和建图系统获取血管内超声的2D图像和所述电磁定位线圈的空间位置信息，进行所述介入手术机器人的同步定位与三维地图构建。

进一步地，所述电磁定位系统选用NDI Aurora V3系列医用磁场发生器。

进一步地，所述电磁屏蔽层包括所述电磁定位线圈的固定装置与超声传感器之间的两层分隔层以及所述分隔层之间的屏蔽体。

进一步地，所述分隔层为薄片状，由可吸收电磁波的材料制成；所述屏蔽体的材料为低电阻金属材料，所述屏蔽体接地。

与现有技术相比，上述发明具有如下优点或者有益效果：

1、有效地将IVUS、电磁定位系统和SLAM结合起来，能实现血管介入手术机器人的血管内自主定位与导航，提升了介入手术中机器人的导航和位置感知能力，有效提升手术的精确性与手术效果；

2、电磁定位系统为单目IVUS图像增加了空间尺度，弥补了传统单目SLAM中缺少空间尺度的缺点；

3、位置信息图像通过血管内超声获得，减少了对X射线成像、造影剂的依赖，减少了医生与病患所受的辐射剂量；

4、同步定位与三维地图构建系统设计合理，通过电磁屏蔽层尽可能减少电磁信号对超声传感器的干扰。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明及其特征、外形和优点将会变得更加明显。在全部附图中相同的标记指示相同的部分。并未可以按照比例绘制附图，重点在于示出本发明的主旨。

图1为本发明一实施例中的同步定位与三维地图构建方法的流程图；

图2为本发明一实施例中SLAM系统的处理流程图；

图3为本发明一实施例中介入手术导管的端部布局剖视图；

图4为本发明一实施例中同步定位与三维地图构建系统架构图；

其中，1、导管；2、超声传感器；3、电磁定位线圈固定位；4、分隔层；5、屏蔽体。

具体实施方式

下面结合附图和具体的实施例对本发明中的结构和方法作进一步的说明，显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。可以理解的是，省略对众所周知的装置、算法、电路以及方法的详细说明，以免不必要的细节妨碍本发明的描述。

实施例1

本实施例提供了一种基于血管内超声与电磁定位的介入机器人同步定位与地图构建方法，能够实现血管介入机器人的自主定位与导航，具体流程参见图1。本系统获取的图像信息来源于介入机器人头部的血管内超声传感器，该传感器能够获取并传输血管内的超声图像，空间位置信息来源于介入机器人配套的电磁定位系统，电磁定位系统能够获取并传输介入机器人上特定点的空间坐标信息，相邻时刻坐标的差，即可作为图像的深度，用于SLAM系统中的建图部分。这两处传感器获取的信息均是介入机器人导管部分头部的信息，有助于引导机器人在人体血管内的运动。图像与位置信息在获取后，传输进入SLAM系统。SLAM模块的具体执行流程如图2所示。首先SLAM系统对输入帧进行预处理，预处理包括将超声图像转化为灰度图像；同时空间坐标信息也通过解算转化为深度距离信息。

之后，对进行预处理后的超声图像，采用ORB特征点提取算法中的FAST算法。FAST角点定义为：若某像素点与其周围领域内足够多的像素点处于不同的区域，则该像素点可能为角点。在本实施例使用的灰度图像中，即若该点的灰度值比其周围领域内足够多的像素点的灰度值大或者小，则该点可能为角点。

具体步骤：

1.从图片中选取一个像素P，下面我们将判断它是否是一个特征点。我们首先把它的度值设为lp；

2.设定一个合适的阈值t；

3.考虑以该像素点为中心的一个半径等于3像素的离散化的Bresenham圆，这个圆的边界上有16个像素；

4.如果在这个大小为16个像素的圆上有n个连续的像素点，它们的像素值要么都比lp+t大，要么都比lp-t小，那么它就是一个角点。通常n＝9时效果最佳。

之后，使用非极大值抑制算法(NMS)保留最优特征点，同时剔除相邻位置的特征点，减小运算的数据量。具体过程是在图像上取一个3*3的窗口，如果在窗口内存在多个特征点，根据FAST特征点的响应值的大小，删除响应值较小的特征点，只保留响应值最大的特征点，实现筛选特征点的目的。同时，通过运动模型对IVUS传感器的位姿进行估计，使用的运动模型为PnP算法，根据相机模型，可以建立空间点位置与像素位置关系，考虑n个三维空间点P及其投影p，计算相机的姿态R,t。它的李群表示为T。用i表示第i个像素点，假设某空间点的坐标P

其中S

之后，将空间位置信息与超声图像结合，即可生成对应于最优特征点的三维地图信息。也就是说，同步定位和建图系统通过介入手术机器人内的电磁定位线圈获取图像的深度，并结合所述输入帧的特征点，生成对应于最优特征点的三维地图信息。每个地图点具有以下信息：在世界坐标系中的3D坐标、视图方向，即所有视图方向的平均单位向量(该方向是指连接该点云和其对应观测关键帧光心的射线方向)；对每个关键帧Ki保存以下信息：相机位姿T(i,w)，从世界坐标系转换到相机坐标系下的变换矩阵相机参数，包括主点和焦距。

具体地，由电磁定位传感器可得到相机在世界坐标系W中的坐标(x

我们可以通过位置差补偿的累加，以及多个补偿后空间坐标构成的向量集，获得特征点i在世界坐标系中的具体位置与其变化规律。

之后，对每幅新来的图像提取ORB特征点进行实时跟踪，并与最近的关键帧进行比较，计算特征点的位置并粗略的估计相机位姿。用于判断当前帧是否需要加入关键帧。

之后，结合运动模型估计的超声传感器位姿，根据加入的关键帧，进而构建局部地图，根据局部空间内的特征点与相机位姿，求解局部Bundle Adjustment最小化重投影误差，以获得更精细的相机位姿与特征点空间位置，同时剔除地图点能够被其他关键帧所观察的多余关键帧。

之后，使用卡尔曼滤波法，对移动进行观测与估计，由此实现了机器人的同步定位与地图构建。

需要说明的是，通常血管介入机器人在血管内通过的路径不会出现重复，因此本实施例所述方法中不必引入用于检测机器人运动路径重复的回环检测模块。

实施例2

本实施例提供了一种介入手术机器人同步定位与三维地图构建系统，用以实现实施例1中的介入手术机器人同步定位与三维地图构建方法，参见图3，包括：导管组件1、设置在导管组件内部前端的超声传感器2和电磁定位线圈3；超声传感器2与电磁定位线圈3之间设有电磁屏蔽层；还包括被手术体的体外部分，所述体外部分包括：所述超声传感器的近端驱动模块和超声成像系统、与所述电磁定位线圈配套的电磁定位系统以及数据处理的同步定位和建图系统；所述同步定位和建图系统获取血管内超声的2D图像和所述电磁定位线圈的空间位置信息，进行所述介入手术机器人的同步定位与三维地图构建。

通过IVUS进行血管成像时，首先将含超声传感器的导管通过桡动脉穿刺送入至目标血管中，到达病变位置后，软件端首先向近端驱动模块发送开始采集的信号，近端驱动模块带动导管尖端的超声传感器进行旋转扫描，在超声激励电路作用下，由超声传感器中的超声换能器发出超声波，超声波进入血管组织后，经过血管各个层次的组织结构反射形成超声回波，超声成像系统对换能器接收到的超声回波信号进行处理后，得到超声信号图像，再上传回超声成像系统主机。主机端在软件中对采集到的信号图像进行处理，并进行坐标变换(极坐标-->笛卡尔坐标)，最终将特定范围内的血管的管腔和管壁的横切面与纵切面分别以图像的形式展现出来。

作为一个优选的实施例，所述电磁定位系统选用NDIAuroraV3系列医用磁场发生器。所述电磁屏蔽层包括所述电磁定位线圈的固定装置与超声传感器之间的两层分隔层4以及所述分隔层之间的屏蔽体5。

作为一个优选的实施例，分隔层4为薄片状，由可吸收电磁波的材料制成；可吸收电磁波的材料包括且不限于铁氧体、纳米微粉吸波材料。屏蔽体5的材料为低电阻金属材料，屏蔽体5通过外接电缆接地。

介入手术机器人同步定位与三维地图构建系统模块之间的架构参见图4。介入机器人端的IVUS血管内超声模块与电磁定位系统作为信息获取模块，获取介入机器人端的图像与位置信息，进而传输到上位机，进行SLAM相关的数据与信息处理。所述数据与信息处理的控制器为TI公司的DSP28335型高性能快速处理器。

综上所述，本发明提供了一种介入手术机器人同步定位与三维地图构建方法和系统。所述系统包括导管组件、设置在导管组件内部前端的超声传感器和电磁定位线圈；超声传感器与电磁定位线圈之间设有电磁屏蔽层；还包括所述超声传感器的近端驱动模块和超声成像系统、与所述电磁定位线圈配套的电磁定位系统以及数据处理的同步定位和建图系统(SLAM)；SLAM获取血管内超声的2D图像和所述电磁定位线圈的空间位置信息，进行所述介入手术机器人的同步定位与三维地图构建。本发明有效地将血管内超声、电磁定位系统和SLAM结合起来，能实现血管介入手术机器人的血管内自主定位与导航，提升了介入手术中机器人的导航和位置感知能力，有效提升手术的精确性与手术效果。

所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。

本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成。其中，计算机程序包括计算机程序代码，计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。计算机可读介质可以包括：能够携带计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，Random Access Memory)、电载波信号、电信信号以及软件分发介质等。

以上对本发明的较佳实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，其中未尽详细描述的设备和方法应该理解为用本领域中的普通方式予以实施；任何熟悉本领域的技术人员，在不脱离本发明技术方案范围情况下，都可利用上述揭示的方法和技术内容对本发明技术方案做出许多可能的变动和修饰，或修改为等同变化的等效实施例，这并不影响本发明的实质内容。因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所做的任何简单修改、等同变化及修饰，均仍属于本发明技术方案保护的范围内。