一种基于超声伺服手术机器人的坐标标定方法

1.本发明属于手术机器人相关技术领域，更具体地，涉及一种基于超声伺服手术机器人的坐标标定方法。


背景技术：

2.良性前列腺增生及引起男性排尿障碍最常见的病因，良性前列腺在老年人当中发病率最高，据统计显示在50～65岁男性中，患病率约为16％～25％；70岁以上男性患病率高达30％～46％，随着年龄的增大而增加，经尿道前列腺电切术是现在前列腺增生治疗的主要方式，但该手术存在安全性、有效性和一致性等问题。随着手术机器人技术的逐渐成熟，前列腺手术机器人为前列腺增生的治疗提供了全新思路，但现有的技术中超声探头用于实时探测图像，手术机器人进行手术执行二者并不能很好的同步，探测影像并不能及时的反馈至机器人执行末端，使得手术机器人的操作较慢。


技术实现要素：

3.针对现有技术的以上缺陷或改进需求，本发明提供了一种基于超声伺服手术机器人的坐标标定方法，可以直观快速的实现探测端和手术执行末端的坐标标定，实现二者坐标的统一。
4.为实现上述目的，按照本发明的一个方面，提供了一种基于超声伺服手术机器人的坐标标定方法，所述超声伺服手术机器人包括机械臂、设置于机械臂的末端的手术执行末端、引导架、设置于引导架上的电机和探头，所述电机带动所述探头旋转，所述方法包括：s1：在所述手术执行末端和探头的斜上方设置立体视觉高精度定位相机，并获取所述立体视觉高精度定位相机的坐标系，即相机坐标系；s2：采用所述立体视觉高精度定位相机分别对所述引导架、探头、机械臂末端以及手术执行末端进行拍照，获得相机坐标系下的引导架坐标系、探头坐标系、机械臂坐标系以及手术执行末端坐标系，进一步分别获取探头坐标系与引导架坐标系、所述引导架坐标系与机械臂基坐标系、手术执行末端坐标系与所述机械臂末端坐标系两两坐标系之间的变换关系，将以上变换关系进行相乘逐级变换即可获得探头坐标系与所述手术执行末端坐标系的变换关系。
5.优选地，所述方法还包括：s3：获取二维图像坐标系与所述探头坐标系的变换关系，其中，所述二维图像坐标系与所述探头坐标系的一坐标平面重合，将探头坐标系与所述手术执行末端坐标系的变换关系和所述二维图像坐标系与所述探头坐标系的变换关系进行相乘获得所述二维图像坐标系与所述手术执行末端坐标系的变换关系。
6.优选地，所述步骤s2包括：s21：采用所述立体视觉高精度定位相机对所述探头和引导架进行拍照获得在相机坐标系下任意位姿下的探头坐标系和引导架坐标系其中t为4
×
4齐次矩阵；s22：基于所述探头坐标系和引导架坐标系获得探头坐标系与引导架坐标系之间的变换关系导架坐标系之间的变换关系s23：采用所述立体视觉高精度定位相机对
所述机械臂的末端进行拍照获得所述相机坐标系与机械臂基坐标系之间的变换关系进而获得所述机械臂基坐标系与所述引导架坐标系的变换关系进而获得所述机械臂基坐标系与所述引导架坐标系的变换关系s24：采用所述立体视觉高精度定位相机对所述手术执行末端进行拍照获得所述手术执行末端坐标系则获得所述手术执行末端坐标系与所述机械臂末端坐标系的变换关系则获得所述手术执行末端坐标系与所述机械臂末端坐标系的变换关系其中，为机械臂末端坐标系与机械臂基坐标系的变换关系，根据机械臂的几何结构直接获取；s25：根据以上则获得探头坐标系与手术执行末端坐标系的变换关系端坐标系的变换关系
7.优选地，所述步骤s3包括：定义所述二维图像坐标系与所述探头坐标系的一坐标平面重合，则获取所述二维图像坐标系与探头坐标系的变换关系将所述探头坐标系与手术执行末端坐标系的变换关系和二维图像坐标系与探头坐标系的变换关系进行乘积则得到二维图像坐标系与所述手术执行末端坐标系的变换关系得到二维图像坐标系与所述手术执行末端坐标系的变换关系
8.优选地，步骤s21具体包括：步骤1，在所述探头和引导架上分别设置第一反光靶标球和第二反光靶标球；步骤2，保持引导架不进行前后移动，启动电机带动所述探头旋转一周，同时采用所述立体视觉高精度定位相机记录所述第一反光靶标球运动轨迹上的多个点，并采用所述立体视觉高精度定位相机记录旋转后所述第一反光靶标球和第二反光靶标球的位置；步骤3，对所述多个点进行拟合后获得圆心作为所述引导架坐标系和初始探头坐标系的原点，拟合所得圆轴线为所述引导架坐标系和初始探头坐标系的z轴；结合旋转后所述第一反光靶标球和第二反光靶标球的位置获得在相机坐标系下任意位姿下的探头坐标系和引导架坐标系
9.优选地，所述步骤3中结合旋转后所述第一反光靶标球和第二反光靶标球的位置获得在相机坐标系下任意位姿下的探头坐标系和引导架坐标系具体包括：根据所述原点、z轴以及所述第一反光靶标球和第二反光靶标球的位置获得探头的初始探头坐标系和引导架坐标系根据所述初始探头坐标系和引导架坐标系获得探头在初始位姿时初始探头坐标系和引导架坐标系的初始转角；在所述探头运动至某一位姿时，获取其前后运动和绕z轴转动的运动变量，采用所述初始转角对所述运动变量进行修正即获得所述探头在任意位姿下的探头坐标系
10.优选地，步骤s23具体包括：在所述机械臂的末端设置第三反光靶标球，采用所述立体视觉高精度定位相机对所述机械臂的末端的第三反光靶标球进行拍照获得所述机械臂末端在相机坐标系中的坐标，同时根据机械臂末端坐标系与机械臂基坐标系的变换关系获得所述相机坐标系与机械臂基坐标系之间的变换关系将所述相机坐标系与机械臂基坐标系之间的变换关系和引导架坐标系进行乘积，即可获得所述机械臂基坐标系与所述引导架坐标系的变换关系与所述引导架坐标系的变换关系
11.优选地，步骤s24具体为：在所述手术执行末端设置第四反光靶标球，采用所述立体视觉高精度定位相机对所述手术执行末端的第四反光靶标球进行拍照获得所述手术执
行末端在所述相机坐标系中的手术执行末端坐标系将所述手术执行末端坐标系和以及进行转换获得手术执行末端坐标系与所述机械臂末端坐标系的变换关系
12.总体而言，通过本发明所构思的以上技术方案与现有技术相比，本发明提供的一种基于超声伺服手术机器人的坐标标定方法具有如下有益效果：
13.1.通过在机器人的工作空间中设置相机并将该相机坐标系作为基准，采用相机采集不同手术机器人不同位置的坐标系进行相互转换即可获得探头与执行末端的坐标标定，结构简单，易于实现。
14.2.由于二维图像坐标系与探头坐标系的一平面重合所以根据探头与执行末端的变换关系很容易获得二维图像与手术执行末端的变化关系，进而实现二维图像坐标系与手术执行末端坐标系的统一标定，方法简单巧妙，中间阶段的坐标变化都比较精准无省略，所以可以获得精确的标定结果。
附图说明
15.图1是本实施例基于超声伺服手术机器人的坐标标定示意图；
16.图2是本实施例基于超声伺服手术机器人的坐标标定方法的步骤图；
17.图3是本实施例坐标标定方法的原理图。
具体实施方式
18.为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
19.请参阅图1，本发明提供了一种基于超声伺服手术机器人的坐标标定方法，所述超声伺服手术机器人包括机械臂、设置于机械臂末端的手术执行末端、引导架、设置于引导架上的电机和探头，工作时电机带动探头旋转。本技术标定坐标系涉及二维图像坐标系(image)、探头坐标系(probe)、引导架坐标系(guide)、机械臂基坐标系(base)、机械臂末端坐标系(end)、连续体执行坐标系(work)以及相机坐标系(camera)，各坐标系的关系如下图1所示。其中，二维图像坐标系和三维探头坐标系yz平面或xz平面重合，故图中未直接标出。
20.如图2所示，所述方法包括如下步骤s1～s2。
21.s1：在所述手术执行末端和探头的斜上方设置立体视觉高精度定位相机，并获取所述立体视觉高精度定位相机的坐标系，即相机坐标系。
22.将该相机坐标系设定为绝对坐标系。
23.s2：采用所述立体视觉高精度定位相机分别对所述引导架、探头、机械臂末端以及手术执行末端进行拍照，获得相机坐标系下的引导架坐标系、探头坐标系、机械臂坐标系以及手术执行末端坐标系，进一步分别获取探头坐标系与引导架坐标系、所述引导架坐标系与机械臂基坐标系、手术执行末端坐标系与所述机械臂末端坐标系两两坐标系之间的变换关系，将以上变换关系进行相乘逐级变换即可获得探头坐标系与所述手术执行末端坐标系的变换关系。
24.具体的包括步骤s21～s25。
25.s21：采用所述立体视觉高精度定位相机对所述探头和引导架进行拍照获得在相机坐标系下任意位姿下的探头坐标系和引导架坐标系其中t为4
×
4齐次矩阵。具体包括以下步骤1～3。
26.步骤1，在所述探头和引导架上分别设置第一反光靶标球和第二反光靶标球；
27.步骤2，保持引导架不进行前后移动，启动电机带动所述探头旋转一周，同时采用所述立体视觉高精度定位相机记录所述第一反光靶标球运动轨迹上的多个点，并采用所述立体视觉高精度定位相机记录旋转后所述第一反光靶标球和第二反光靶标球的位置；
28.步骤3，对所述多个点进行拟合后获得圆心作为所述引导架坐标系和初始探头坐标系的原点，拟合所得圆轴线为所述引导架坐标系和初始探头坐标系的z轴；结合旋转后所述第一反光靶标球和第二反光靶标球的位置获得在相机坐标系下任意位姿下的探头坐标系和引导架坐标系
29.具体的，根据所述原点、z轴以及所述第一反光靶标球和第二反光靶标球的位置获得探头的初始探头坐标系和引导架坐标系由于原点确定反光靶标球的位置确定，z轴确定，因此可确定探头的初始探头坐标系和引导架坐标系
30.根据所述初始探头坐标系和引导架坐标系获得探头在初始位姿时初始探头坐标系和引导架坐标系的初始转角；
31.在所述探头运动至某一位姿时，获取其前后运动和绕z轴转动的运动变量，采用所述初始转角对所述运动变量进行修正即获得所述探头在任意位姿下的探头坐标系
32.s22：基于所述探头坐标系和引导架坐标系获得探头坐标系与引导架坐标系之间的变换关系之间的变换关系
33.s23：采用所述立体视觉高精度定位相机对所述机械臂的末端进行拍照获得所述相机坐标系与机械臂基坐标系之间的变换关系进而获得所述机械臂基坐标系与所述引导架坐标系的变换关系引导架坐标系的变换关系
34.具体的，在所述机械臂的末端设置第三反光靶标球，采用所述立体视觉高精度定位相机对所述机械臂的末端的第三反光靶标球进行拍照获得所述机械臂末端在相机坐标系中的坐标，同时根据机械臂末端坐标系与机械臂基坐标系的变换关系获得所述相机坐标系与机械臂基坐标系之间的变换关系
35.将所述相机坐标系与机械臂基坐标系之间的变换关系和引导架坐标系进行乘积，即可获得所述机械臂基坐标系与所述引导架坐标系的变换关系所述机械臂基坐标系与所述引导架坐标系的变换关系
36.s24：采用所述立体视觉高精度定位相机对所述手术执行末端进行拍照获得所述手术执行末端坐标系则获得所述手术执行末端坐标系与所述机械臂末端坐标系的变换关系换关系其中，为机械臂末端坐标系与机械臂基坐标系的变换关系，根据机械臂的连杆参数直接获取。
37.具体的，在所述手术执行末端设置第四反光靶标球，采用所述立体视觉高精度定位相机对所述手术执行末端的第四反光靶标球进行拍照获得所述手术执行末端在所述相机坐标系中的手术执行末端坐标系
38.将所述手术执行末端坐标系和以及进行转换获得手术执行末端坐标系与所述机械臂末端坐标系的变换关系
39.s25：根据以上则获得探头坐标系与手术执行末端坐标系的变换关系变换关系
40.本技术中的方法还包括将探头坐标系与手术执行末端坐标系的变换关系转变为二维图像坐标系与手术执行末端坐标系的变换关系，也即如下步骤s3。
41.s3：获取二维图像坐标系与所述探头坐标系的变换关系，其中，所述二维图像坐标系与所述探头坐标系的一坐标平面重合，将探头坐标系与所述手术执行末端坐标系的变换关系和所述二维图像坐标系与所述探头坐标系的变换关系进行相乘获得所述二维图像坐标系与所述手术执行末端坐标系的变换关系。
42.本技术通过借助同一点在不同坐标系的表示标定处坐标系转换关系。如图3所示，具体方法为已知两坐标系a和b，分别记录n个同一空间点在a、b坐标系中的坐标和i＝1
…
n，利用所记录点集构建辅助坐标系c，由点集的映射关系可以获得坐标系a、b和辅助坐标系c的变换关系和再根据公式可得到即可标定处坐标系a和b的变换关系
43.当经由医学影像获取到二维图像坐标系中某一点后，使用本技术所述标定方法可将其变换至手术执行末端坐标系，进而引导执行末端完成相应手术。
44.本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。