基于机器人操作系统的机械臂辅助超声扫描系统研究*

张娟，李锐，程威，秦浩，吴诗尧，刘立，2，3，倪东，2，3，陈思平，2，3，钱建庭，2，3△

( 1.深圳大学医学部生物医学工程学院，深圳 518060;2.医学超声关键技术国家地方联合工程实验室，深圳 518060;3.广东省生物医学信息检测与超声成像重点实验室，深圳 518060)

1 引 言

超声检查是临床最为普及的常规检查方法之一，这就导致超声科室工作繁重，超声医师长期以非生理性的姿态工作，身体呈高负荷状态[1-2]。由此引发的职业损伤[3]，已严重影响超声医师的工作和家庭生活，亟待寻求一种新的超声诊疗方式来缓解这一情况。

1999年，法国Pierrot教授团队第一次挑战性地利用工业机器人(PA-10)突破了超声人工扫描的局限性[4]，之后类似机器人辅助超声诊疗的各种系统设计和临床应用都逐步开展起来[5]。2002年，不列颠哥伦比亚大学的Abolmaesumi团队将视觉伺服用于机器人辅助超声诊断的系统中，同时也证明了基于互联网的机器人辅助远程超声系统的可行性[6]。机器人辅助超声穿刺是超声机器人的主流方向之一，2004年，Hong团队将超声引导的穿刺机器人用于实时经皮胆囊造口术，并且能够跟随患者因非自主性呼吸运动而引起的器官移位，实时调节穿刺针的路径[7]。近年来，约翰·霍普金斯大学Nassir Navab团队提出了一种超声机器人运动轨迹规划算法，根据RGB-D相机数据事先分割出感兴趣区域，参数化生成单个或多个平行扫描轨迹并使之覆盖整个区域，自动进行机器人超声采集，该方法已在动物内脏上实现[8]。

2 基于ROS的系统框架

目前，超声机器人的研究因囊括多个硬件平台，都是较为分散地控制，大多的研究并没有去探究各模块如何整合。本研究基于ROS集成的平台，提出一种依据视觉导航，机械臂实施自动化超声扫描并获取实时超声图像的整体化系统，图1为系统框架。

图1基于ROS的系统框架

Fig1SystemframeworkbasedonROS

3 KinectV2机器人视觉

目前，基于视觉图像处理来进行场景的认知是机器人视觉导航的主要方法。微软KinectV2传感器利用RGB图像结合深度数据，引入了三维空间信息，能够使机器人更好地对环境进行认知、辨识，实现导航功能。

3.1 Kinect V2标定与配准

相机标定是KinectV2应用于本系统之前必要的一项工作，获得图像与物理空间的映射关系。本研究使用经典的张正友棋盘标定法[9]，内参标定过程见图2、图3，分别使用RGB和深度摄像头对20个不同的棋盘位置进行角点捕捉，内参标定结果见式(1)，MRGB、DistRGB表示RGB摄像头的内参矩阵和畸变系数，MIR、DistIR表示深度摄像头的内参矩阵和畸变系数。

图2RGB摄像头内参标定

Fig2InternalparametercalibrationofRGBcamera

图3IR深度摄像头内参标定

Fig3Internalparametercalibrationofdepthcamera

MRGB=

DistRGB=[0.011，0.063，-0.001，0.005，-0.152]

(1)

DistIR=[0.078，-0.275，-0.002，0.001，0.113]

(2)

KinectV2外参标定，即标定RGB和深度摄像头的相对物理空间位置，见图4，对同一棋盘放置位置，使用RGB和深度摄像头同时进行角点捕捉，得到外参的标定结果见式(3)，R、T表示RGB和IR深度摄像头之间的外参旋转矩阵和平移矩阵。

T=[-0.05056 -0.00116 0.00073]T

(3)

图4 (a).RGB角点捕捉；(b).IR角点标定

Fig4 (a).CornercaptureofRGB；(b).CornercaptureofIR

因RGB摄像头和深度摄像头位置不同且图像的分辨率不同，如果RGB和深度图像直接融合成彩色点云，两组数据会出现较大的偏差，将两个摄像头进行内外参标定后，可以实现彩色图和红外深度图之间的配准，配准前后效果对比见图5。本系统使用配准后的点云数据，作为系统视觉导航的源数据。

图5 (a).配准前彩色点云；(b).配准后彩色点云

Fig5 (a).Pre-registrationcolorpointcloud；(b).Registratedcolorpointcloud

3.2 点云图像处理

本研究机械臂的运动控制是围绕超声扫描目标位置的点云信息进行的，而点云源数据中包含大量冗余信息。本系统利用点云库(point cloud library，PCL)来进行前期的视觉点云图像处理。

3.2.1去噪 由于硬件设备的局限，获得的深度图像有较多的噪声信息，因此点云数据中含有大量的稀疏离群点，这会影响点云的局部特征估计(例如法向量)。本研究移除稀疏离群点的方法是基于点到临近点的距离分布来计算的，对每个点，计算它到其所有临近点的平均距离，将得到的结果假定为一个高斯分布，其形状由均值和标准差决定，平均距离在定义范围之外的点，即为离群点并从数据集中去除掉[10]。

3.2.2路径的获取 我们需要从点云中提取出目标路径点，让机械臂持超声探头沿着路径点运动。每个路径点都包含空间坐标和方向两部分，这决定探头到达的位置和实施扫描的姿态。

首先需分割出目标物体，然后才能在其上进行路径的提取。本研究使用半自动的分割方法，在点云可视化窗口中，鼠标选择目标物体上一点，以此为种子点，通过区域增长，将目标区域分割出来，再在其上自定义一个方向提取一条路径点，见图7、图8。

图6去除离群点前后

Fig6Beforeandaftercomparisonofoutlierremoval

图7分割出目标物体

Fig7Segmentedtargetobject

图8自定义路径点

Fig8Customizedpathpoints

探头的姿态取决于路径点的方向，即物体表面法向量，决定着超声声束的入射角度与目标物体表面的夹角。我们获取的点云数据集在真实物体的表面表现为一组定点样本，本研究的法向量计算方法是通过对表面点云切向平面的法线估计来表示此点的法线，见图9，因此这就转换为最小二乘法平面拟合估计问题[11-12]。

图9pk最小二乘法拟合平面

Fig9Aleast-squareplanefittingestimationinpk

(4)

(5)

(6)

4 UR5机械臂的建模与控制

MoveIt是ROS系统中运动控制的核心功能包，囊括运动规划、操作控制、3D感知、运动学算法、碰撞检测等，并且提供良好的界面交互。

4.1 机械臂模型建立

使用ROS中的MoveIt插件加载UR5机械臂的几何和运动学模型，见图10，通过TCP/IP通讯使得机械臂模型对应真实机械臂的实时姿态。

图10MoveIt添加UR5模型(对应右下角实际状态)

Fig10AddUR5modelinMoveit(Correspondstothelowerrightcorneroftheactualstateofrobotarm)

4.2 运动规划

已知机械臂的初始位姿和目标位姿，那么需要通过一定的算法，找到一条到达目标位姿的较优路径，这种算法就称为运动规划(Motion Planning)。

运动学算法是机械臂控制算法的核心，本研究使用逆向运动学(inverse kinematics，IK)算法，即已知机械臂工具末端在基座坐标系下的位置和姿态，求解对应的6个关节的角度。这一过程存在多解情况[14]，而我们只需要一个唯一确定的解。求解唯一性的过程有两个方向，一个是找到最佳解，使用最优规划；另一个是快速找到一个有效解，使用采样规划。我们使用MoveIt默认的开源运动规划库，其中包含有最优规划，但总体来说还是一个采样规划算法库，利于本系统的快速应答。

5 系统坐标系的转换

图11坐标系统

Fig11Coordinatesystem

图12 (a).机械臂手眼标定；(b).手眼标定转换关系

Fig12 (a).Hand-eyecalibrationofrobotarm；(b).Transformationinhand-eyecalibration

(7)

控制机械臂从位置(1)移动到位置(2)，则：

(8)

联立，得到：

(9)

(10)

qKB=[qwqxqyqz]

qw=0.01207

qx=-0.00133

qy=0.83050

qz=-0.55689

tKB=[0.13039 1.22668 0.49889]T

(11)

6 仿体实验结果及分析

本研究采用硅胶皮肤仿体验证所提出的扫描系统的可行性。在KinectV2视野下，仿体分别放置在三个不同的位置，每个位置采集5点数据。记录从视觉端鼠标选择目标点到机械臂开始运动的时间间隔，即系统响应时间；图13测量探头到达的位置与目标实际位置的偏差，即系统误差。

见表1，响应时间维持在3 s以下，证明本系统具有一定的实时性，符合医疗环境下的要求。系统误差见表2，稳定在8 mm以下，这与KinectV2的测量误差和手眼标定的误差相关，目前这一精度基本满足系统的要求，但我们仍需寻求方法来降低系统误差。

图13测量探头与目标实际位置的偏差

Fig13Measurethedeviationoftheprobefromtheactualpositionofthetarget

表1 系统响应时间

7 结语

本文提出一种基于ROS的机械臂辅助医学超声扫描系统，采用Kinect V2作为视觉导航，通过对扫描路径的提取，坐标系统转换，得到目标表面的运动轨迹规划，引导机械臂持超声探头实施扫描动作，采集实时超声影像。通过仿体实验验证了本系统的实时性与精度，结果表明，在Kinect视觉导航下，机械臂能够夹持超声探头实现自主超声扫描，并且有较高的效率和稳定性。从一定程度上来说，本系统有替代超声医生实施重复性、机械性扫描的可行性，使得超声医生有更充裕的时间去实施高阶的诊断和治疗工作，优化医疗资源。

当然，本系统仍有许多需要改进的地方，机械臂的动作需尽可能地模仿超声医生的扫查手法，进一步量化探头运动路径和探头扫描切角，智能化地针对某一个待测部位设计一系列连贯动作；系统还需添加力反馈传感器模块，量化探头与目标表面接触的压力和角度，解决系统安全性问题。