CN115998389A - 一种手术机器人的复合引导前端装置及定位方法 - Google Patents

Abstract

本发明属于手术机器人技术领域，公开了一种手术机器人的复合引导前端装置及定位方法，安装在手术机器人的机械臂的前端,包括电动夹爪引导机构和激光引导机构，电动夹爪引导机构包括夹持器，夹持器用于提供穿刺针引导通道并夹持穿刺针，激光引导机构设置在所述夹持器之间的空隙处，用于提供激光路径，穿刺针与激光路径重合从而定位。本发明将机械通道引导和激光引导两种引导方式复合集成在机械臂前端，可分别使用，两种引导结构互不干扰，同时在电动夹爪引导机构中设置了手动控制装置，紧急情况下能够手动松开穿刺针，提高了安全性能。同时本发明采用跟踪系统跟踪定位，逐渐调整机械臂位姿直到逼近目标值，提高了手术机器人穿刺精度。

Description

一种手术机器人的复合引导前端装置及定位方法

技术领域

本发明属于手术机器人技术领域，尤其涉及一种手术机器人的复合引导前端装置及定位方法。

背景技术

随着医疗水平的发展，手术机器人开始代替人工进行一些简单的穿刺操作，将穿刺针固定在手术机器人机械臂前端，通过控制机械臂进行穿刺手术。目前手术机器人机械臂前端穿刺通道引导常见的主要有通道夹持和激光导引两种形式：通道夹持因为有了物理夹持通道因此对操作者把持穿刺器械稳定性要求大大降低，具体又分成如下三种夹持方式：

1、固定孔通道夹持：机械臂前端具有安装穿刺针的固定孔，穿刺针穿过固定孔从而固定安装在机械臂前端，然而固定孔通道夹持在穿刺过程中若遇到紧急情况，穿刺针无法脱离，安全性最差。

2、电动夹爪通道开合夹持：机械臂前端具有电动夹爪，用于夹持穿刺针，电动夹爪通道在穿刺过程中若遇到紧急情况，可通过控制器控制夹爪打开，使用方便，但电动夹爪通道脱离需要外部指令脱开，不能直接徒手打开；一旦机械控制失灵或断电，将无法打开夹爪，安全性较差。

3、手动夹爪通道开合夹持：机械臂前端具有手动夹爪，通过手动调节控制夹爪张开或闭合，这种存机械的控制方式不依赖于电子设备，紧急情况可以直接徒手脱离；安全性较好。但手动控制使用较不方便。

采用激光通道导引的方式是利用激光束来导引，使用时操作者手持穿刺针，通过穿刺针的针尖、针尾与激光束重合的方法来实现穿刺通道导引，使用方便，不存在紧急情况脱离问题，安全性最好。但是对于操作者把持穿刺器械的稳定性要求提高。

手术机器人实际使用中，往往存在有的医生喜欢通道夹持的方式，有的医生喜欢激光导引的方式，而机械臂前端空间比较小，市面上尚没有激光导引和通道夹持结合在机械臂前端的方案。

发明内容

本发明目的在于提供一种手术机器人的复合引导前端装置及定位方法，以解决上述的技术问题。

为解决上述技术问题，本发明的一种手术机器人的复合引导前端装置及定位方法的具体技术方案如下：

一种手术机器人的复合引导前端装置，安装在手术机器人的机械臂的前端,包括电动夹爪引导机构和激光引导机构，所述电动夹爪引导机构包括夹持器，所述夹持器用于提供穿刺针引导通道并夹持穿刺针，所述激光引导机构设置在所述夹持器之间的空隙处，用于提供激光路径，所述穿刺针与激光路径重合从而定位。

进一步的，所述电动夹爪引导机构包括电机、传动机构、夹爪和手动控制装置，所述电机输出轴与传动机构连接，传动机构与夹爪连接，电机通过传动机构控制夹爪夹紧或松开，所述手动控制装置与传动机构固定连接，用于手动控制传动机构，从而手动控制夹爪夹紧或松开。

进一步的，所述电动夹爪引导机构包括丝杆，所述丝杆为正反牙双头丝杆，所述丝杆杆身左半部分为正牙，右半部分为反牙，所述夹爪包括第一夹爪和第二夹爪，所述第一夹爪中间具有贯穿的第一螺纹孔，所述第二夹爪中间具有贯穿的第二螺纹孔，第一夹爪的第一螺纹孔套入丝杆的正牙，第二夹爪的第二螺纹孔入丝杆的反牙，所述电机输出轴与传动机构连接，所述传动机构与丝杆一端连接。

进一步的，所述传动机构包括主动同步带轮、同步带和从动同步带轮，所述主动同步带轮与电机输出轴同轴固定连接，所述从动同步带轮与丝杆一端同轴固定连接，所述同步带连接在主动同步带轮和从动同步带轮上，所述手动控制装置固定连接在从动同步带轮外侧，用于手动旋转从动同步带轮。

进一步的，所述第一夹爪下端具有用于安装夹持器的第一夹爪输出端，所述第二夹爪下端具有用于安装夹持器的第二夹爪输出端，所述第一夹爪和第二夹爪的夹爪输出端相对设置，所述第一夹爪输出端上固定安装有第一夹持器，所述第二夹爪输出端上固定安装有第二夹持器，所述第一夹持器和第二夹持器用于夹持穿刺针，所述第一夹持器和第二夹持器之间夹持形成引导通道，用于穿过穿刺针。

进一步的，所述电动夹爪引导机构包括固定机架和导向装置，所述电机固定在固定机架上，所述导向装置固定连接在固定机架上，并与第一夹爪和第二夹爪直线滑动连接，用于提供夹爪的直线运动路径。

进一步的，所述电动夹爪引导机构包括夹爪传感器，所述夹爪传感器固定设置在固定机架内壁上，用于检测夹爪是张开还是闭合。

进一步的，所述电动夹爪引导机构包括参考标记，所述参考标记用于引导通道的定位。

进一步的，所述激光引导机构包括激光发射器，所述激光发射器设置在第一夹持器和第二夹持器之间的空隙处，所述激光发射器上具有激光发射口，用于发射激光。

本发明还公开了一种手术机器人的复合引导前端装置的定位方法，包括如下步骤：

步骤1：定义坐标系：已知任意坐标系X到坐标系Y的齐次坐标转移矩阵定义为 M_xy，定义跟踪系统坐标系为C，参考标记坐标系为T，机器人坐标系为R，机器人前端法兰坐标系为F，病人坐标系为P；

步骤2：术前标定：

步骤2.1：标定好跟踪系统坐标系C与病人坐标系P的关系，即已知M_pc；

步骤2.2：标定好夹爪引导通道在参考标记坐标系T里面的线段坐标L_t1或激光线在参考标记坐标系T里面的线段坐标L_t2；

步骤2.3：通过手眼标定，获得跟踪系统坐标系C与机器人坐标系R的初始关系 M_cr0，机器人前端法兰坐标系F与参考标记坐标系T的初始关系 M_ft0，这里机器人前端法兰坐标系F是手，跟踪系统坐标系C是眼，用的是eye to hand的方法；

步骤3：术中跟踪：

步骤3.1：通过手术规划获得病人坐标系下目标针道L_p1，通过已知矩阵M_pc获得跟踪系统下的夹爪引导通道线段坐标L_c1或激光线线段坐标L_c2，由步骤2.2已知的夹爪引导通道在参考标记坐标系T里面的线段坐标L_t1或激光线在参考标记坐标系T里面的线段坐标L_t2，可以获得目标参考坐标系M_tc0，即获得参考标记在跟踪系统下的姿态；

步骤3.2：获得机器人前端法兰初始姿态矩阵：M_fr0=M_cr0*M_tc0*M_ft0；

步骤3.3：控制机器人前端法兰走到初始目标位置M_fr0附近；

步骤3.4：通过跟踪系统，获得当前参考标记坐标系T与跟踪系统坐标系C的关系M_tc，控制机械臂法兰移动，使得M_tc逼近M_tc_目标，M_tc_目标为目标矩阵，当两者的误差小于设定阈值时，定位完成。

本发明的一种手术机器人的复合引导前端装置及定位方法具有以下优点：本发明将机械通道引导和激光引导两种引导方式复合集成在机械臂前端，激光发射器设置在夹持器之间的空隙处，合理利用了空间，使用时只要翻转前端装置即可分别使用，使得两种引导结构互不干扰，同时在电动夹爪引导机构中设置了手动控制装置，紧急情况下能够手动松开穿刺针，提高了安全性能。同时本发明针对现有技术机械定位精度低的缺陷，采用跟踪系统跟踪定位，逐渐调整机械臂位姿直到逼近目标值，提高了手术机器人穿刺精度。

附图说明

图1为本发明的手术机器人的复合引导前端装置结构示意图；

图2为本发明的电动夹爪引导机构内部结构示意图；

图3为本发明的电动夹爪引导机构结构示意图；

图4为本发明的丝杆结构示意图；

图5为本发明的激光引导示意图；

图中标记说明：1、电动夹爪引导机构；11、电机；12、传动机构；121、主动同步带轮；122、同步带；123、从动同步带轮；13、丝杆；131、正牙；132、反牙；14、夹爪；141、第一夹爪；1411、第一螺纹孔；1412、第一夹爪输出端；1421、第二螺纹孔；1422、第二夹爪输出端；142、第二夹爪；143、第一夹持器；144、第二夹持器；145、引导通道；15、手动控制装置；16、保护盖；17、导向装置；171、第一滑轨；172、第二滑轨；173、第一滑块；174、第二滑块；18、固定机架；19、夹爪传感器；20、参考标记；2、激光引导机构；21、激光发射器；211、激光发射口；3、机械臂；4、穿刺针；41、激光重合点。

具体实施方式

为了更好地了解本发明的目的、结构及功能，下面结合附图，对本发明一种手术机器人的复合引导前端装置及定位方法做进一步详细的描述。

如图1所示，本发明的一种手术机器人的复合引导前端装置，安装在手术机器人的机械臂3的前端,包括电动夹爪引导机构1和激光引导机构2，如图2、图3所示，电动夹爪引导机构1包括电机11、传动机构12、丝杆13和夹爪14，电机11固定在固定机架18上，固定机架18固定在机械臂前端壳体内，电机11输出轴与传动机构12连接，传动机构12与丝杆13连接，传动机构12将电机输出传送到丝杆13，带动丝杆13转动。如图4所示，丝杆13为正反牙双头丝杆，丝杆杆身左半部分为正牙131，右半部分为反牙132，夹爪14包括第一夹爪141和第二夹爪142，第一夹爪141中间具有贯穿的第一螺纹孔1411，第二夹爪142中间具有贯穿的第二螺纹孔1421，第一夹爪141的第一螺纹孔1411套入丝杆13的正牙131，第二夹爪142的第二螺纹孔1421套入丝杆13的反牙132。第一夹爪141下端具有用于安装夹持器的第一夹爪输出端1412，第二夹爪142下端具有用于安装夹持器的第二夹爪输出端1422，第一夹爪141和第二夹爪142的夹爪输出端相对设置，第一夹爪输出端1412上固定安装有第一夹持器143，第二夹爪输出端1422上固定安装有第二夹持器144，第一夹持器143和第二夹持器144用于夹持穿刺针4，第一夹持器143和第二夹持器144之间夹持形成引导通道145，用于穿过穿刺针4，并防止穿刺针4轴偏。电机11正转时通过传动机构12带动丝杆13转动，第一夹爪141和第二夹爪142在正反牙的作用下，第一夹爪输出端1412和第二夹爪输出端1422相互靠近，从而夹紧穿刺针4。电机11反转时通过传动机构12带动丝杆13反向转动，第一夹爪141和第二夹爪142在正反牙的作用下，第一夹爪输出端1412和第二夹爪输出端1422相互远离，从而松开穿刺针4。

传动机构12可以采用同步皮带传动，齿轮传动、涡轮涡杆传动、齿条传动等现有的机械传动方式，本发明实施例中的传动机构12为同步皮带传动。传动机构12包括主动同步带轮121、同步带122和从动同步带轮123，主动同步带轮121与电机11输出轴同轴固定连接，从动同步带轮123与丝杆13一端同轴固定连接，同步带122连接在主动同步带轮121和从动同步带轮123上，电机11转动带动主动同步带轮121转动，从而通过同步带122带动从动同步带轮123转动。传动机构12的设置目的是为了在传动机构12输出端加上手动控制装置15，用于手动控制夹爪14的开合。手动控制装置15固定连接在从动同步带轮123外侧，用于手动旋转从动同步带轮123，带动丝杆13转动，从而能够在紧急情况下手动打开夹爪14。优选的，手动控制装置15为手动旋钮，手动旋钮与从动同步带轮123同轴固定连接。

电动夹爪引导机构1外具有保护盖16，第一夹持器143、第二夹持器144和手动控制装置15露出于保护盖16，方便操作，保护盖16保护内部机械结构，同时防止灰层进入。

电动夹爪引导机构1还包括导向装置17，导向装置17包括第一滑轨171、第二滑轨172、第一滑块173和第二滑块174，第一滑轨171和第二滑轨172分别固定连接在固定机架18的前后内壁上，第一夹爪141上端与第一滑块173固定连接，第二夹爪142上端与第二滑块174固定连接，第一滑块173沿着第一滑轨171滑动，第二滑块174沿着第二滑轨172滑动，导向装置17用于保证第一夹爪141和第二夹爪142的直线运动精度，即保证夹紧状态的精度。导向装置17也可以采用导向轴和轴套等具有直线导向功能的机械机构来实现。

电动夹爪引导机构1还包括夹爪传感器19，夹爪传感器19固定设置在固定机架18内壁上，用于检测夹爪14是张开还是闭合。

激光引导机构2包括激光发射器21，激光发射器21设置在第一夹持器143和第二夹持器144之间的空隙处，激光发射器21上具有激光发射口211，用于发射激光。

如图5所示，进行激光导引时，翻转机械臂前端装置，使得激光发射口211向下（这样可以将夹爪位置排除在激光路径之外，防止穿刺针在操作过程中碰到夹爪），然后根据初始标定的相对位置关系确定穿刺部位的激光路径，激光发射器21发射激光到目标面（可以是皮肤），将穿刺针4的针尖对准目标面上的激光标记点A，然后保持针尖不动，移动针尾到激光路径上，穿刺针针尾具有一个与针同轴的激光重合点41，激光重合点41为圆点或圆孔，当激光与针尾的激光重合点41重合时，针与激光路径重合，医生手握穿刺针保持该路径进行穿刺。

本发明装置可以灵活选择穿刺针引导方式，两种方式使用时互不干扰，能够适应多种手术环境。

现有的针道引导方式无论是针道引导还是激光引导，其穿刺导航机器人都是使用纯机械方式进行控制，由于串联机械臂模型是由多个自由度串联而成，在经过多维度控制后在最前端的绝对精度不高，针道位置或激光路径位置往往存在偏差，穿刺位置并不准确。因此本发明采用光学定位跟踪法进行跟踪定位。

为了对引导通道进行定位跟踪，本发明装置还包括参考标记20，参考标记20设置在保护盖16上，参考标记20为光学引导支架或磁钉定位系统，本实施例中采用光学引导支架，光学引导支架上具有多个光学球201，用于夹爪14的引导通道145的定位。

夹爪定位跟踪实施例：

本发明的一种手术机器人的复合引导前端装置的定位方法，包括如下步骤：

步骤1：定义坐标系：已知任意坐标系X到坐标系Y的齐次坐标转移矩阵定义为 M_xy，由于转移矩阵必然可逆，因此已知M_xy则可计算Inv(M_xy)获得M_yx，定义跟踪系统坐标系为C，参考标记坐标系为T，机器人坐标系为R，机器人前端法兰坐标系为F，病人坐标系为P。

步骤2：术前标定：

步骤2.1：标定好跟踪系统坐标系C与病人坐标系P的关系，即已知M_pc；

步骤2.2：标定好夹爪引导通道在参考标记坐标系T里面的线段坐标L_t1；

步骤2.3：通过手眼标定，获得跟踪系统坐标系C与机器人坐标系R的初始关系 M_cr0，机器人前端法兰坐标系F与参考标记坐标系T的初始关系 M_ft0。这里机器人前端法兰坐标系F是手，跟踪系统坐标系C是眼，用的是eye to hand的方法。

步骤3：术中跟踪：

步骤3.1：通过手术规划获得病人坐标系下目标针道L_p1，通过已知矩阵M_pc获得跟踪系统下的夹爪引导通道线段坐标L_c1，由步骤2.2已知的夹爪引导通道在参考标记坐标系T里面的线段坐标L_t1，可以获得目标参考坐标系M_tc0，即获得参考标记在跟踪系统下的姿态；

步骤3.2：获得机器人前端法兰初始姿态矩阵：M_fr0=M_cr0*M_tc0*M_ft0；

步骤3.3：控制机器人前端法兰走到初始目标位置M_fr0附近；

步骤3.4：通过跟踪系统，获得当前参考标记坐标系T与跟踪系统坐标系C的关系M_tc，由于手眼标定有误差，机械臂正逆解有误差，可以控制机械臂法兰移动，使得M_tc逼近M_tc_目标，M_tc_目标为目标矩阵，可通过现有的优化算法获得。当两者的误差小于设定阈值时，定位完成，这里逼近的方法通常使用视觉伺服的方式。

激光线定位跟踪实施例：

本发明的一种手术机器人的复合引导前端装置的定位方法，包括如下步骤：

步骤1：定义坐标系：已知任意坐标系X到坐标系Y的齐次坐标转移矩阵定义为 M_xy，由于转移矩阵必然可逆，因此已知M_xy则可计算Inv(M_xy)获得M_yx，定义跟踪系统坐标系为C，参考标记坐标系为T，机器人坐标系为R，机器人前端法兰坐标系为F，病人坐标系为P。

步骤2：术前标定：

步骤2.1：标定好跟踪系统坐标系C与病人坐标系P的关系，即已知M_pc；

步骤2.2：标定好激光线在参考标记坐标系T里面的线段坐标L_t2；

步骤2.3：通过手眼标定，获得跟踪系统坐标系C与机器人坐标系R的初始关系 M_cr0，机器人前端法兰坐标系F与参考标记坐标系T的初始关系 M_ft0。这里机器人前端法兰坐标系F是手，跟踪系统坐标系C是眼，用的是eye to hand的方法。

步骤3：术中跟踪：

步骤3.1：通过手术规划获得病人坐标系下目标针道L_p1，通过已知矩阵M_pc获得跟踪系统下的激光线线段坐标L_c2，由步骤2.2已知的激光线在参考标记坐标系T里面的线段坐标L_t2，可以获得目标参考坐标系M_tc0，即获得参考标记在跟踪系统下的姿态；

步骤3.2：获得机器人前端法兰初始姿态矩阵：M_fr0=M_cr0*M_tc0*M_ft0；

步骤3.3：控制机器人前端法兰走到初始目标位置M_fr0附近；

步骤3.4：通过跟踪系统，获得当前参考标记坐标系T与跟踪系统坐标系C的关系M_tc，由于手眼标定有误差，机械臂正逆解有误差，可以控制机械臂法兰移动，使得M_tc逼近M_tc_目标，M_tc_目标为目标矩阵，可通过现有的优化算法获得。当两者的误差小于设定阈值时，定位完成，这里逼近的方法通常使用视觉伺服的方式。

本发明的跟踪系统可以为光学跟踪相机如双面光学相机，此时参考标记20为光学引导支架，跟踪系统也可以为磁钉定位系统，此时参考标记20为磁场传感器。

本发明采用视觉伺服进行跟踪，相较于机械定位精度更高。

可以理解，本发明是通过一些实施例进行描述的，本领域技术人员知悉的，在不脱离本发明的精神和范围的情况下，可以对这些特征和实施例进行各种改变或等效替换。另外，在本发明的教导下，可以对这些特征和实施例进行修改以适应具体的情况及材料而不会脱离本发明的精神和范围。因此，本发明不受此处所公开的具体实施例的限制，所有落入本申请的权利要求范围内的实施例都属于本发明所保护的范围内。

Claims (10)

1.一种手术机器人的复合引导前端装置，安装在手术机器人的机械臂（3）的前端,其特征在于，包括电动夹爪引导机构（1）和激光引导机构（2），所述电动夹爪引导机构（1）包括夹持器，所述夹持器用于提供穿刺针引导通道并夹持穿刺针（4），所述激光引导机构（2）设置在所述夹持器之间的空隙处，用于提供激光路径，所述穿刺针（4）与激光路径重合从而定位。

2.根据权利要求1所述的手术机器人的复合引导前端装置，其特征在于，所述电动夹爪引导机构（1）包括电机（11）、传动机构（12）、夹爪（14）和手动控制装置（15），所述电机（11）输出轴与传动机构（12）连接，传动机构（12）与夹爪（14）连接，电机（11）通过传动机构（12）控制夹爪（14）夹紧或松开，所述手动控制装置（15）与传动机构（12）固定连接，用于手动控制传动机构（12），从而手动控制夹爪（14）夹紧或松开。

3.根据权利要求2所述的手术机器人的复合引导前端装置，其特征在于，所述电动夹爪引导机构（1）包括丝杆（13），所述丝杆（13）为正反牙双头丝杆，所述丝杆杆身左半部分为正牙（131），右半部分为反牙（132），所述夹爪（14）包括第一夹爪（141）和第二夹爪（142），所述第一夹爪（141）中间具有贯穿的第一螺纹孔（1411），所述第二夹爪（142）中间具有贯穿的第二螺纹孔（1421），第一夹爪（141）的第一螺纹孔（1411）套入丝杆（13）的正牙（131），第二夹爪（142）的第二螺纹孔（1421）入丝杆（13）的反牙（132），所述电机（11）输出轴与传动机构（12）连接，所述传动机构（12）与丝杆（13）一端连接。

4.根据权利要求3所述的手术机器人的复合引导前端装置，其特征在于，所述传动机构（12）包括主动同步带轮（121）、同步带（122）和从动同步带轮（123），所述主动同步带轮（121）与电机（11）输出轴同轴固定连接，所述从动同步带轮（123）与丝杆（13）一端同轴固定连接，所述同步带（122）连接在主动同步带轮（121）和从动同步带轮（123）上，所述手动控制装置（15）固定连接在从动同步带轮（123）外侧，用于手动旋转从动同步带轮（123）。

5.根据权利要求3所述的手术机器人的复合引导前端装置，其特征在于，所述第一夹爪（141）下端具有用于安装夹持器的第一夹爪输出端（1412），所述第二夹爪（142）下端具有用于安装夹持器的第二夹爪输出端（1422），所述第一夹爪（141）和第二夹爪（142）的夹爪输出端相对设置，所述第一夹爪输出端（1412）上固定安装有第一夹持器（143），所述第二夹爪输出端（1422）上固定安装有第二夹持器（144），所述第一夹持器（143）和第二夹持器（144）用于夹持穿刺针（4），所述第一夹持器（143）和第二夹持器（144）之间夹持形成引导通道（145），用于穿过穿刺针（4）。

6.根据权利要求3所述的手术机器人的复合引导前端装置，其特征在于，所述电动夹爪引导机构（1）包括固定机架（18）和导向装置（17），所述电机（11）固定在固定机架（18）上，所述导向装置（17）固定连接在固定机架（18）上，并与第一夹爪（141）和第二夹爪（142）直线滑动连接，用于提供夹爪（14）的直线运动路径。

7.根据权利要求6所述的手术机器人的复合引导前端装置，其特征在于，所述电动夹爪引导机构（1）包括夹爪传感器（19），所述夹爪传感器（19）固定设置在固定机架（18）内壁上，用于检测夹爪（14）是张开还是闭合。

8.根据权利要求5所述的手术机器人的复合引导前端装置，其特征在于，所述电动夹爪引导机构（1）包括参考标记（20），所述参考标记（20）用于引导通道（145）的定位。

9.根据权利要求5所述的手术机器人的复合引导前端装置，其特征在于，所述激光引导机构（2）包括激光发射器（21），所述激光发射器（21）设置在第一夹持器（143）和第二夹持器（144）之间的空隙处，所述激光发射器（21）上具有激光发射口（211），用于发射激光。

10.一种如权利要求1-9任一项所述的手术机器人的复合引导前端装置的定位方法，其特征在于，包括如下步骤：

步骤1：定义坐标系：已知任意坐标系X到坐标系Y的齐次坐标转移矩阵定义为 M_xy，定义跟踪系统坐标系为C，参考标记坐标系为T，机器人坐标系为R，机器人前端法兰坐标系为F，病人坐标系为P；

步骤2：术前标定：

步骤2.1：标定好跟踪系统坐标系C与病人坐标系P的关系，即已知M_pc；

步骤2.2：标定好夹爪引导通道在参考标记坐标系T里面的线段坐标L_t1或激光线在参考标记坐标系T里面的线段坐标L_t2；

步骤2.3：通过手眼标定，获得跟踪系统坐标系C与机器人坐标系R的初始关系 M_cr0，机器人前端法兰坐标系F与参考标记坐标系T的初始关系 M_ft0，这里机器人前端法兰坐标系F是手，跟踪系统坐标系C是眼，用的是eye to hand的方法；

步骤3：术中跟踪：

步骤3.1：通过手术规划获得病人坐标系下目标针道L_p1，通过已知矩阵M_pc获得跟踪系统下的夹爪引导通道线段坐标L_c1或激光线线段坐标L_c2，由步骤2.2已知的夹爪引导通道在参考标记坐标系T里面的线段坐标L_t1或激光线在参考标记坐标系T里面的线段坐标L_t2，可以获得目标参考坐标系M_tc0，即获得参考标记在跟踪系统下的姿态；

步骤3.2：获得机器人前端法兰初始姿态矩阵：M_fr0=M_cr0*M_tc0*M_ft0；

步骤3.3：控制机器人前端法兰走到初始目标位置M_fr0附近；

步骤3.4：通过跟踪系统，获得当前参考标记坐标系T与跟踪系统坐标系C的关系M_tc，控制机械臂法兰移动，使得M_tc逼近M_tc_目标，M_tc_目标为目标矩阵，当两者的误差小于设定阈值时，定位完成。

Images