[发明专利]基于光纤应变传感器与单目相机融合的体内SLAM方法、系统

说明书

本发明提供了一种基于光纤应变传感器与单目相机融合的体内SLAM方法和系统，包括：视觉光纤时空对齐步骤；数据预处理步骤；系统初始化步骤；软组织运动形变剔除步骤；位姿计算与优化步骤。本发明还提供相应的计算机存储介质与手术机器人。本发明能适应动态、光照不足以及存在大量无纹理区域的体内环境，在使用相机图像进行位姿估计失败或者图像特征点不足时，能够使用光纤数据进行末端位姿估计，避免重定位环节。此外，光纤数据能够提供图像帧间的位姿约束，得到更加精确的位姿和地图点坐标，完成体内环境下对连续体机器人进行精确的末端定位和体内环境建图的任务。

技术领域

本发明涉及手术机器人技术领域，具体地，涉及基于光纤应变传感器与单目相机融合的体内SLAM方法和系统。具体而言，涉及一种基于光纤应变传感器与单目相机融合的体内SLAM方法和系统及其手术机器人。

背景技术

肺部微小结节，医学上称为孤立性肺结节，是一种常见的胸外科疾病。其病因复杂，临床表现缺乏特异性，直到结节在人体内潜伏到一定阶段并生长到一定大小后，才能够通过胸部X光等手段发现，诊治具有一定的困难。

近年来，连续体机器人在微创手术中已得到广泛临床应用，与传统手术相比展示出充分的优越性。使用连续体机器人进行肺部微小结节取样手术，有助于提升肺部微小结节诊断能力。其中，手术机器人的末端的精确定位与体内环境的三维建图是完成肺部微小结节定位与采样的关键技术，对提高肺部微小结节的精确诊断水平具有重要意义。

对于SLAM技术，比较常用的传感器为单目相机。由于肺部结构的高度复杂性、手术操作的干扰加之人体的呼吸运动等生理运动的干扰，为手术机器人的末端精确定位与体内环境的三维建图带来了更高的技术难度。

对此，由于光纤传感器其本身耐腐蚀、体积小、抗电磁干扰等优点，使其在手术机器人的末端定位中得到广泛的应用。另外，光纤应变传感器与单目相机融合，可以弥补如图5所示的体内环境下由于光照变化、无纹理区域或者动态场景造成的末端定位误差增大或者失败，从而显著提高末端定位的精度和三维建图的准确性。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种基于光纤应变传感器与单目相机融合的体内SLAM方法和系统。

根据本发明提供的一种基于光纤应变传感器与单目相机融合的体内SLAM方法，包括如下任一个或任多个步骤：

步骤S1，视觉光纤时空对齐步骤：将单目相机与光纤传感器进行时钟源对齐和时间戳对齐；估计相机坐标系和光纤坐标系之间齐次变换矩阵；确定单目相机的尺度；

步骤S2，数据预处理步骤：对单目相机获取的图像，进行特征提取和特征追踪；通过光纤应变传感器的测量数据，获得连续体机器人中心线的曲率曲线和挠率曲线；

步骤S3，系统初始化步骤：根据所述曲率曲线和挠率曲线，获取连续体机器人的末端位姿；根据特征点和光纤数据得到的末端位姿，进行三角剖分，建立初始地图；

步骤S4，软组织运动形变剔除步骤：将单目图像划分为N×M个矩形块，计算每个矩形块内离群点的比例r_s，将动态软组织的特征点及特征点对应的地图点移除；

步骤S5，位姿计算与优化步骤：根据单目图像，计算当前帧的位姿并优化；根据视觉光纤损失函数，对相机位姿和地图点进行全局优化。

优选地，在所述步骤S2中，所述进行特征提取和特征追踪的步骤包括探测并估计当前帧的特征点在下一帧的位置；

在所述步骤S3中，通过形状重建算法，获取连续体机器人的末端位姿，检测特征点数量，数量大于设定阈值选定为关键帧，根据光纤得到的末端位姿，对特征点进行运动一致性检验，剔除离群点；

在所述步骤S4中，对于每个矩阵块，计算其中特征点到对应极线的距离D，根据距离D与设定阈值的关系，计算每个矩形块内离群点的比例r_s；