一种光力融合末端夹持器及其定位控制方法与流程

1.本发明涉及光学导航及力反馈控制手术机器人的技术领域，尤其是指一种光力融合末端夹持器及其定位控制方法。


背景技术：

2.伴随着机器人技术的进步与医疗技术的革新，医学和机器人学的结合已成为医疗行业的发展趋势之一。医疗机器人是指各种应用于外科手术、医学培训、康复治疗、假体和残障人士辅助等的机器人设备。根据服务对象的不同可分为医疗外科机器人、康复机器人和助老助残机器人等。而穿刺手术机器人作为医疗外科机器人的分支之一，则是专用于进行微创外科手术的医疗机器人，其一般由机械臂、成像设备、空间定位系统及工作站四部分以及相应的软件组成。现在，穿刺手术机器人技术的发展已经比较成熟，应用到临床中，代替人工操作，降低穿刺手术难度，提高手术可靠性；特别是应用其中的光学导航系统，代替医生进行影像分析，规划好路径，在计算机手术导航系统的引导下，自主对目标靶区进行精确定位，状态稳定，灵巧性强，工作范围大，克服了医生身体疲劳和恶劣环境对手术造成的影响，提高手术的安全性和成功率。但是基于目前的穿刺手术机器人的应用情况，还存在两个问题。第一个问题是在穿刺手术过程中由于人体器官或组织的生理性运动导致目标靶点会运动从而产生位置变化，导致基于光学引导的穿刺手术机器人仅仅依靠光学跟踪很难命中靶点，比如进行胸部及腹部穿刺时，呼吸运动会导致内脏穿刺靶点移动，病人疼痛反应时肌肉收缩及不自主肢动也会影响定位及进针穿刺。第二个问题是所使用的光学跟踪系统只能通过安装在机械臂末端的光学标记点来判断穿刺针在人体内的情况，进而来调整机器人位姿；但事实上穿刺针本身过长容易发生弹性形变，在穿刺过程中会与周围组织产生相互作用力，而光学跟踪系统并不能检测到这个相互作用力，所以没有相应的力补偿，对机器人位姿的调整不够准确，反馈的光学信息也不能得到精准的规划和控制，从而导致实际的穿刺轨迹与计划的轨迹严重偏离，使穿刺针与周围组织的相互作用力更大，这样在皮肤里面和在皮肤外面的穿刺针会发生弹性形变，严重的会有肉眼可见的弯曲，最终可能出现人体组织皮肤或者器官被损伤的现象。


技术实现要素：

3.本发明的目的在于克服现有技术的不足与缺点，为穿刺手术机器人提供一种光力融合末端夹持器及其定位控制方法，使穿刺针可以按需求进行固定和移动，并且穿刺针受到的作用力可以及时反馈，实现穿刺手术机器人位姿的实时调整，再通过近红外光学定位仪判断并调整机器人位姿的基础上进一步提高穿刺手术机器人在手术中的操作精确性和安全性。
4.为实现上述目的，本发明提供的技术方案为：一种光力融合末端夹持器，包括近红外光学定位仪及安装在六自由度机械臂末端的夹持机构，所述夹持机构包括穿刺针受力采集装置、穿刺针夹持装置、封装盒及封装于封装盒内的电机滑动模组；所述穿刺针受力采集
装置包括穿刺针横向力采集端及位于穿刺针横向力采集端上方的穿刺针径向力采集端，所述穿刺针横向力采集端包括第一三维力传感器和第一连接构件，所述第一连接构件接近电机滑动模组的电机，所述第一三维力传感器通过第一连接构件固定在第一三维力传感器上，用于采集穿刺针穿刺过程中受到的横向力；所述穿刺针径向力采集端包括第二三维力传感器和第二连接构件，所述第二连接构件固定在电机滑动模组的滑块上，能够随滑块上下滑动，所述第二三维力传感器固定在第二连接构件上，用于采集穿刺针穿刺过程中受到的径向力；所述穿刺针夹持装置包括两个分别固定在穿刺针横向力采集端和穿刺针径向力采集端上的夹持组件；所述近红外光学定位仪配置有均带反光标记点的第一定位工具、第二定位工具和定位板，所述第一定位工具和第二定位工具分别通过支架固定在穿刺针横向力采集端和穿刺针径向力采集端上，其中，所述第一定位工具用于实时确定穿刺针的进针深度，所述第二定位工具用于实时确定穿刺针的针尖位置信息，所述定位板用于与第一定位工具配合进行工具注册，进而精准计算穿刺针的中间位置信息。
5.进一步，所述夹持组件包括连接板、固定夹块、活动夹块、导轨、螺母、手拧螺丝和挡板；所述连接板固定在相应的三维力传感器上，所述固定夹块和挡板分别固定在连接板的左、右两端，所述导轨横设于固定夹块和挡板之间，并固定在连接板上，所述活动夹块滑动安装在导轨上，其与固定夹块相向的面上开有相互匹配的槽位，所述固定夹块和活动夹块相向的槽位能够组合形成供穿刺针通过的槽孔；所述挡板上开有一个供手拧螺丝穿过的螺孔，所述手拧螺丝穿过挡板抵触于活动夹块，并采用螺母固定，通过手拧螺丝的旋进推动活动夹块的运动来调整夹针的力度和深度。
6.本发明也提供了上述光力融合末端夹持器的定位控制方法，包括以下步骤：
7.s1、近红外光学定位仪实时跟踪定位第二定位工具和定位板上的反光标记点位置信息，构建第二定位工具坐标系和定位板坐标系，计算出两者的坐标转换关系，从而精准计算穿刺针的针尖位置信息；
8.s2、近红外光学定位仪实时跟踪定位第一定位工具上的反光标记点位置信息，构建第一定位工具坐标系，并基于第一定位工具坐标系和第二定位工具坐标系精准计算穿刺针的进针深度；
9.s3、结合步骤s2构建的第一定位工具坐标系、及基于六自由度机械臂自身结构构建的六自由度机械臂基座坐标系和六自由度机械臂末端坐标系、及基于近红外光学定位仪内外参构建的近红外光学定位仪坐标系，计算各坐标系之间的位姿转换关系，实时获取穿刺针在空间中的姿态信息。
10.进一步，所述s1包括以下步骤：
11.s101、将定位板置于穿刺针的针尖处，近红外光学定位仪实时跟踪定位第二定位工具和定位板上四个反光标记点的位置信息；
12.s102、基于步骤s101识别到的反光标记点，以第二定位工具中心为原点构建第二定位工具坐标系，基于第二定位工具坐标系和近红外光学定位仪坐标系之间的位姿变换关系，将定位板上四个反光标记点的位置信息变换到第二定位工具坐标系下的位置信息，坐标系构建方法如下：
13.任意选取第二定位工具上的三个点中心点为
[0014][0015]
其中，分别表示第二定位工具坐标系的x、y、z轴单位向量，分别表示第二定位工具上点到和的单位向量；
[0016]
s103、基于步骤s101识别到的反光标记点，以定位板中心为原点可建立定位板坐标系，坐标系构建方式如下：
[0017]
任意选取定位板上的三个点中心点为
[0018][0019]
其中，分别表示定位板坐标系的x、y、z轴单位向量，分别表示定位板坐标系的x、y、z轴单位向量，分别表示标定板上点到和的单位向量；
[0020]
基于定位板上四个反光标记点在第二定位工具坐标系下的位置信息，利用奇异值分解法，求出第二定位工具坐标系和定位板坐标系之间的位姿转换关系；
[0021]
s104、基于第二定位工具坐标系和定位板坐标系之间的位姿转换关系以及针孔在定位板坐标系下的位置信息，能够实时求得针尖在第二定位工具坐标系下的位置信息，即求得穿刺针的针尖在空间中的位置信息，具体求解过程如下：
[0022][0023]
其中，p
tip
和分别为穿刺针的针尖在空间中的位置信息和第二定位工具坐标系下的位置信息，表示针孔在定位板坐标系下的位置信息，r
cs
和tcs分别表示定位板坐标系和第二定位工具坐标系之间的旋转矩阵和平移矩阵，r
os
和t
os
分别表示近红外光学定位仪坐标系和第二定位工具坐标系之间的旋转矩阵和平移矩阵，r
co
和t
co
分别表示定位板坐标系和近红外光学定位仪坐标系之间的旋转矩阵和平移矩阵。
[0024]
进一步，所述s2包括以下步骤：
[0025]
s201、近红外光学定位仪实时跟踪定位第一定位工具上四个反光标记点的位置信息，构建第一定位工具坐标系，结合构建的第二定位工具坐标系，求得任意一点在第一定位工具坐标系下和第二定位工具坐标系之间的位姿转换关系；
[0026][0027]
其中，分别表示反光标记点p
a
在第一定位工具坐标系和第二定位工具坐标系下的位置信息，r
sf
和t
sf
分别表示第二定位工具坐标系和第一定位工具坐标系之间的旋转矩阵和平移矩阵，r
of
和t
of
分别表示近红外光学定位仪坐标系和第一定位工具坐标系
之间的旋转矩阵和平移矩阵，r
so
和t
so
分别表示第二定位工具坐标系和近红外光学定位仪坐标系之间的旋转矩阵和平移矩阵；
[0028]
s202、结合穿刺针针尖在第二定位工具坐标系下的位置信息及第一定位工具坐标系和第二定位工具坐标系之间的位姿转换关系，精准求得穿刺针的针尖在第一定位工具坐标系下的位置信息；
[0029]
s203、在穿刺针进针或退针运动前后，通过步骤s202能够分别求得穿刺针针尖在第一定位工具坐标系下的位置信息，计算前后两次位置信息的差值即可得出穿刺针实时进针或退针深度。
[0030]
进一步，所述s3包括以下步骤：
[0031]
s301、基于六自由度机械臂自身结构设计参数dh值，结合正逆向运动学和六自由度机械臂在空间中的实时位姿，求得六自由度机械臂末端坐标系和六自由度机械臂基座坐标系之间的位姿转换关系；
[0032]
s302、六自由度机械臂末端带动夹持机构分别沿着六自由度机械臂基座坐标系各坐标轴正向、六自由度机械臂末端坐标系各坐标轴正向多次移动相同的小段距离，并由近红外光学定位仪实时跟踪定位第一定位工具上四个反光标记点的位置信息；
[0033]
s303、任意选取第一定位工具上的一个反光标记点，当六自由度机械臂末端带动夹持机构沿六自由度机械臂基座坐标系各坐标轴正向运动至新的位置时，计算选取的那个反光标记点从初始位置到最新位置的单位向量，求取多次运动后的算术平均值，得到六自由度机械臂基座坐标系与近红外光学定位仪坐标系之间的位姿变换关系；
[0034]
s304、任意选取第一定位工具上的一个反光标记点，以步骤s303求得的六自由度机械臂实时运动后的位姿为此处的初始位置，使六自由度机械臂末端带动夹持机构沿六自由度机械臂末端坐标系各坐标轴正向运动至新的位置，计算选取的那个反光标记点从初始位置到最新位置的单位向量，求取多次运动后的算术平均值，得到六自由度机械臂末端坐标系与近红外光学定位仪坐标系之间的位姿变换关系；
[0035]
s305、基于上述求得的各坐标系之间的转换关系，求得任意一点在第一定位工具坐标系下和六自由度机械臂基座坐标系的位姿转换关系：
[0036][0037]
其中，分别表示该反光标记点在六自由度机械臂基座坐标系下的位置信息和第一定位工具坐标系下的位置信息，r
fb
、t
fb
分别表示第一定位工具坐标系和六自由度机械臂基座坐标系之间的旋转矩阵和平移矩阵，r
ob
、t
ob
分别表示近红外光学定位仪坐标系和六自由度机械臂基座坐标系之间的旋转矩阵和平移矩阵，r
fo
、t
fo
分别表示第一定位工具坐标系和近红外光学定位仪坐标系之间的旋转矩阵和平移矩阵；
[0038]
s306、基于上述求得的各坐标系之间的转换关系，求得任意一点在第一定位工具坐标系下和六自由度机械臂末端坐标系下的位姿转换关系，具体位姿转换关系如下：
[0039]
[0040]
其中，分别表示该反光标记点在六自由度机械臂末端坐标系下的位置信息和第一定位工具坐标系下的位置信息，r
fe
、t
fe
分别表示第一定位工具坐标系和六自由度机械臂末端坐标系之间的旋转矩阵和平移矩阵，r
oe
、t
oe
分别表示近红外光学定位仪坐标系和六自由度机械臂末端坐标系之间的旋转矩阵和平移矩阵，r
fo
、t
fo
分别表示第一定位工具坐标系和近红外光学定位仪坐标系之间的旋转矩阵和平移矩阵；
[0041]
其中，六自由度机械臂基座和六自由度机械臂末端的位姿决定着夹持机构的空间位姿，穿刺针的空间位姿也随之确定。
[0042]
本发明与现有技术相比，具有如下优点与有益效果：
[0043]
1、本发明解决了目前穿刺手术机器人只适用于神经外科和骨科等刚性静态组织手术场景中的问题，适用于受呼吸运动影响较大的胸腹部、肝脏等软组织动态非刚性穿刺手术场景，有助于拓展穿刺手术机器人临床适用场景。
[0044]
2、与其它采用螺丝、紧固件或者压把式装夹结构不同，本发明采用手拧螺母、导轨和槽孔的方式进行穿刺针的夹持，可精准夹紧毫米级别的多种类型的穿刺针；采用上下两级夹持结构，比仅采用一级夹持结构夹持穿刺针更安全、稳定、可靠；在实际穿刺过程中，采用该方式也可减少过程中对三维力传感器的干扰，有助于延长传感器的寿命，同时夹持拆卸操作更加方便快捷。
[0045]
3、与采用一级传感器监测受力不同，由于穿刺针针体较长，只采用一个三维力传感器往往存在较大误差；本发明采用两个三维力传感器形成上下两级传感器，可精准监测穿刺针受力，为后续软件开发及实时补偿穿刺针受力方法提供了研究数据基础。
[0046]
4、本发明采用第一定位工具、第二定位工具和定位板配合进行穿刺针的跟踪识别定位，可精准检测穿刺针的针尖位置和穿刺针进针或退针深度，减少穿刺手术过程中的偶然性误差，提高穿刺手术的成功率，减少对病人造成的创伤，加快病人恢复速度。
[0047]
5、利用构建的第一定位工具坐标系、六自由度机械臂基座坐标系、六自由度机械臂末端坐标系和近红外光学定位仪坐标系，实时求得各坐标系之间的位姿变换关系，进而精准求得穿刺针在空间中的位姿信息。
[0048]
6、采用近红外光学定位仪及其配置工具和自主设计的反光标记球进行空间位置定位，具有较高的鲁棒性和精度，定位均方根精度误差可达0.12mm，数据置信度达95％。
[0049]
7、本发明提出的定位控制方法，计算步骤简洁，所需要的数据量较少，且方法精度高，抗干扰能力强，用于多种复杂的手术场景。
附图说明
[0050]
图1为光力融合末端夹持器的安装示意图。
[0051]
图2为夹持机构的结构示意图。
[0052]
图3为穿刺针横向力采集装置的安装示意图。
[0053]
图4为穿刺针径向力采集装置的安装示意图。
[0054]
图5为穿刺针夹持装置的结构示意图。
[0055]
图6为定位板的结构示意图。
具体实施方式
[0056]
下面结合具体实施实例对本发明做进一步说明，但本发明的实施方式不限于此。
[0057]
参见图1至图6所示，本实施例提供了一种光力融合末端夹持器，包括近红外光学定位仪3及安装在六自由度机械臂7末端的夹持机构，所述夹持机构包括穿刺针受力采集装置、穿刺针夹持装置、封装盒4及封装于封装盒4内的电机滑动模组5；所述穿刺针受力采集装置包括穿刺针横向力采集端201及位于穿刺针横向力采集端201上方的穿刺针径向力采集端202，所述穿刺针横向力采集端201包括第一三维力传感器2011和第一连接构件2012，所述第一连接构件2012接近电机滑动模组5的电机501，所述第一三维力传感器2011通过第一连接构件2012固定在第一三维力传感器2011上，用于采集穿刺针6穿刺过程中受到的横向力；所述穿刺针径向力采集端202包括第二三维力传感器2021和第二连接构件2022，所述第二连接构件2022固定在电机滑动模组5的滑块502上，能够随滑块502上下滑动，所述第二三维力传感器2021固定在第二连接构件2022上，用于采集穿刺针6穿刺过程中受到的径向力；所述穿刺针夹持装置包括两个分别固定在穿刺针横向力采集端201和穿刺针径向力采集端202上的夹持组件101；所述近红外光学定位仪3配置有均带四个反光标记点的第一定位工具301、第二定位工具302和定位板303，所述第一定位工具301和第二定位工具302分别通过支架固定在穿刺针横向力采集端201和穿刺针径向力采集端202上，其中，所述第一定位工具301用于实时确定穿刺针6的进针深度，所述第二定位工具302用于实时确定穿刺针6的针尖位置信息，所述定位板303用于与第一定位工具301配合进行工具注册，进而精准计算穿刺针6的中间位置信息。
[0058]
优选的，所述夹持组件101包括连接板1011、固定夹块1012、活动夹块1013、导轨1014、螺母1015、手拧螺丝1016和挡板1017；所述连接板1011固定在相应的三维力传感器上，所述固定夹块1012和挡板1017分别固定在连接板1011的左、右两端，所述导轨1014横设于固定夹块1012和挡板1017之间，并固定在连接板1011上，所述活动夹块1013滑动安装在导轨1014上，其与固定夹块1012相向的面上开有相互匹配的槽位，所述固定夹块1012和活动夹块1013相向的槽位能够组合形成供穿刺针6通过的槽孔；所述挡板1017上开有一个供手拧螺丝1016穿过的螺孔，所述手拧螺丝1016穿过挡板1017抵触于活动夹块1013，并采用螺母1015固定，通过手拧螺丝1016的旋进推动活动夹块1013的运动来调整夹针的力度和深度。
[0059]
下面为本实施例上述光力融合末端夹持器的定位控制方法，包括以下步骤：
[0060]
s1、近红外光学定位仪实时跟踪定位第二定位工具和定位板上的反光标记点位置信息，构建第二定位工具坐标系和定位板坐标系，计算出两者的坐标转换关系，从而精准计算穿刺针的针尖位置信息；其包括以下步骤：
[0061]
s101、将定位板置于穿刺针的针尖处，近红外光学定位仪实时跟踪定位第二定位工具和定位板上四个反光标记点的位置信息；
[0062]
s102、基于步骤s101识别到的反光标记点，以第二定位工具中心为原点构建第二定位工具坐标系，基于第二定位工具坐标系和近红外光学定位仪坐标系之间的位姿变换关系，将定位板上四个反光标记点的位置信息变换到第二定位工具坐标系下的位置信息，坐标系构建方法如下：
[0063]
任意选取第二定位工具上的三个点中心点为
[0064][0065]
其中，分别表示第二定位工具坐标系的x、y、z轴单位向量，分别表示第二定位工具上点到和的单位向量；
[0066]
s103、基于步骤s101识别到的反光标记点，以定位板中心为原点可建立定位板坐标系，坐标系构建方式如下：
[0067]
任意选取定位板上的三个点中心点为
[0068][0069]
其中，分别表示定位板坐标系的x、y、z轴单位向量，分别表示定位板坐标系的x、y、z轴单位向量，分别表示标定板上点到和的单位向量；
[0070]
基于定位板上四个反光标记点在第二定位工具坐标系下的位置信息，利用奇异值分解法，求出第二定位工具坐标系和定位板坐标系之间的位姿转换关系；
[0071]
s104、基于第二定位工具坐标系和定位板坐标系之间的位姿转换关系以及针孔在定位板坐标系下的位置信息，能够实时求得针尖在第二定位工具坐标系下的位置信息，即求得穿刺针的针尖在空间中的位置信息，具体求解过程如下：
[0072][0073]
其中，p
tip
和分别为穿刺针的针尖在空间中的位置信息和第二定位工具坐标系下的位置信息，表示针孔在定位板坐标系下的位置信息，r
cs
和tcs分别表示定位板坐标系和第二定位工具坐标系之间的旋转矩阵和平移矩阵，r
os
和t
os
分别表示近红外光学定位仪坐标系和第二定位工具坐标系之间的旋转矩阵和平移矩阵，r
co
和t
co
分别表示定位板坐标系和近红外光学定位仪坐标系之间的旋转矩阵和平移矩阵。
[0074]
s2、近红外光学定位仪实时跟踪定位第一定位工具上的反光标记点位置信息，构建第一定位工具坐标系，并基于第一定位工具坐标系和第二定位工具坐标系精准计算穿刺针的进针深度；其包括以下步骤：
[0075]
s201、近红外光学定位仪实时跟踪定位第一定位工具上四个反光标记点的位置信息，构建第一定位工具坐标系，结合构建的第二定位工具坐标系，求得任意一点在第一定位工具坐标系下和第二定位工具坐标系之间的位姿转换关系；
[0076][0077]
其中，分别表示反光标记点p
a
在第一定位工具坐标系和第二定位工具
坐标系下的位置信息，r
sf
和t
sf
分别表示第二定位工具坐标系和第一定位工具坐标系之间的旋转矩阵和平移矩阵，r
of
和t
of
分别表示近红外光学定位仪坐标系和第一定位工具坐标系之间的旋转矩阵和平移矩阵，r
so
和t
so
分别表示第二定位工具坐标系和近红外光学定位仪坐标系之间的旋转矩阵和平移矩阵；
[0078]
s202、结合穿刺针针尖在第二定位工具坐标系下的位置信息及第一定位工具坐标系和第二定位工具坐标系之间的位姿转换关系，精准求得穿刺针的针尖在第一定位工具坐标系下的位置信息；
[0079]
s203、在穿刺针进针或退针运动前后，通过步骤s202能够分别求得穿刺针针尖在第一定位工具坐标系下的位置信息，计算前后两次位置信息的差值即可得出穿刺针实时进针或退针深度。
[0080]
s3、结合步骤s2构建的第一定位工具坐标系、及基于六自由度机械臂自身结构构建的六自由度机械臂基座坐标系和六自由度机械臂末端坐标系、及基于近红外光学定位仪内外参构建的近红外光学定位仪坐标系，计算各坐标系之间的位姿转换关系，实时获取穿刺针在空间中的姿态信息；其包括以下步骤：
[0081]
s301、基于六自由度机械臂自身结构设计参数dh值，结合正逆向运动学和六自由度机械臂在空间中的实时位姿，求得六自由度机械臂末端坐标系和六自由度机械臂基座坐标系之间的位姿转换关系；
[0082]
s302、六自由度机械臂末端带动夹持机构分别沿着六自由度机械臂基座坐标系各坐标轴正向、六自由度机械臂末端坐标系各坐标轴正向多次移动相同的小段距离，并由近红外光学定位仪实时跟踪定位第一定位工具上四个反光标记点的位置信息；
[0083]
s303、任意选取第一定位工具上的一个反光标记点，当六自由度机械臂末端带动夹持机构沿六自由度机械臂基座坐标系各坐标轴正向运动至新的位置时，计算选取的那个反光标记点从初始位置到最新位置的单位向量，求取多次运动后的算术平均值，得到六自由度机械臂基座坐标系与近红外光学定位仪坐标系之间的位姿变换关系；
[0084]
s304、任意选取第一定位工具上的一个反光标记点，以步骤s303求得的六自由度机械臂实时运动后的位姿为此处的初始位置，使六自由度机械臂末端带动夹持机构沿六自由度机械臂末端坐标系各坐标轴正向运动至新的位置，计算选取的那个反光标记点从初始位置到最新位置的单位向量，求取多次运动后的算术平均值，得到六自由度机械臂末端坐标系与近红外光学定位仪坐标系之间的位姿变换关系；
[0085]
s305、基于上述求得的各坐标系之间的转换关系，求得任意一点在第一定位工具坐标系下和六自由度机械臂基座坐标系的位姿转换关系：
[0086][0087]
其中，分别表示该反光标记点在六自由度机械臂基座坐标系下的位置信息和第一定位工具坐标系下的位置信息，r
fb
、t
fb
分别表示第一定位工具坐标系和六自由度机械臂基座坐标系之间的旋转矩阵和平移矩阵，r
ob
、t
ob
分别表示近红外光学定位仪坐标系和六自由度机械臂基座坐标系之间的旋转矩阵和平移矩阵，r
fo
、t
fo
分别表示第一定位工具坐标系和近红外光学定位仪坐标系之间的旋转矩阵和平移矩阵；
[0088]
s306、基于上述求得的各坐标系之间的转换关系，求得任意一点在第一定位工具坐标系下和六自由度机械臂末端坐标系下的位姿转换关系，具体位姿转换关系如下：
[0089][0090]
其中，分别表示该反光标记点在六自由度机械臂末端坐标系下的位置信息和第一定位工具坐标系下的位置信息，r
fe
、t
fe
分别表示第一定位工具坐标系和六自由度机械臂末端坐标系之间的旋转矩阵和平移矩阵，r
oe
、t
oe
分别表示近红外光学定位仪坐标系和六自由度机械臂末端坐标系之间的旋转矩阵和平移矩阵，r
fo
、t
fo
分别表示第一定位工具坐标系和近红外光学定位仪坐标系之间的旋转矩阵和平移矩阵；
[0091]
其中，六自由度机械臂基座和六自由度机械臂末端的位姿决定着夹持机构的空间位姿，穿刺针的空间位姿也随之确定。
[0092]
上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。