CN114767274A - 立体定位微型手术机器人 - Google Patents

Abstract

本发明公开一种立体定位微型手术机器人，包括第一位置控制单元、第二位置控制单元、深度控制单元、穿刺针连接装置、第一活动单元、第二活动单元以及第一基座；第一基座提供安装基础，第一、第二位置控制单元分别具备2个主动作动自由度，分别以串联、并联形式组成，末端位置工作空间为平面，深度控制单元具备1个主动作动自由度的直线进给，第一、第二活动单元分别具有俯仰、偏航的被动自由度，两者之间还具有1个被动自由度的翻滚运动和1个被动自由度的直线运动，穿刺针连接装置固定针；本发明体积小，安装方便，末端执行机构有至少5个自由度，能达到空间所需位置，能配合不同器械完成不同手术，可为医生提供实时影像信息，保证手术顺利完成。

Description

立体定位微型手术机器人

技术领域

本发明为一种立体定位微型手术机器人，涉及微创手术机器人领域，特别涉及基于图像引导的立体定位靶向穿刺手术机器人系统。

背景技术

随着精准微创医疗器械的发展，外科手术效果显著提升。立体定位是微创手术机器人的重要需求，在穿刺活检、消融、冷冻、粒子植入等微创治疗手术方式中，都需要术中根据医学影像的判断，将器械准确地植入到病灶部位，并稳定工作。在甲状腺、肝胆、肺、子宫、前列腺等肿瘤的诊断与治疗中，通常利用经皮穿刺来进行，在医学影像设备(B超、MRI和CT等)的引导下，将手术器械(例如穿刺针、活检针、消融针等)快速准确置入病人体内器官进行组织提取或消融治疗。这种手术具有创伤小、并发症少及术后恢复快的特点，是传统开放式手术的重大变革。传统的穿刺介入手术主要依赖于医生经验，医生手持针插入体内，存在手术精度不高、术中医患双方受辐射、术后并发症多，还存在穿刺过程中定位不准、夹持不稳、路径规划困难等问题。

随着机器人技术和医学图像技术的发展，机器人辅助医生手术已成为机器人领域重要的分支，机器人辅助医生手术不仅可以克服手术精度不高、术中医患双方受辐射、术后并发症多等局限性，还可以增强医生做手术的能力，充分发挥机器人重复定位精度高和医生经验丰富的优点。首先，通过医学影像对组织器官的解剖结构进行图像处理与分析，计算得到病灶(靶标)的位置，根据体表穿刺不动点的设计，控制机器人将末端位置和姿态到达一定值，使得穿刺针经过该位置和姿态插入人体时，可以通过穿刺不动点直接到达病灶处，并稳定工作，获得穿刺位置精度高、夹持稳定、入路灵活的特点，也避免了术中在床旁受到辐射及感染等问题。

目前常见的定位系统有框架式神经外科定位装置、无框架式神经外科装置、基于传统工业机器人的骨科机器人、肺部活检机器人以及专用化的腹腔穿刺机器人等。这些机器人多是采用工业6自由度机械臂、被动机械臂或者框架定位系统。构型可以分为串联或者并联。串联构型的机器人容易导致关节的误差传播路径长，在末端的定位精度低并且机器人的刚性差，并联构型的机器人容易导致机器人的工作空间范围小，机器人的体积大。而定位机器人系统常常需要安装或者放置在特定的手术室，基于工业机器人的系统移动或者携带非常不方便，难以在不同层级的医院中普及，也不适应于多种的适应症。

发明内容

本发明的目的在于提供一种立体定位微型手术机器人，以针对目前手术机器人体积庞大、机械臂刚性不足、定位精度不足、末端柔性自由度不足、功能单一等问题。该机器人采用串并联混合机构，适应于不同组织器官的靶向穿刺或者立体定向治疗。该立体定位手术机器人体积小，安装方便，末端执行机构具有至少5个自由度，能达到空间所需的位置，末端执行机构能配合不同的手术器械(例如穿刺针、活检针、消融针等)来完成不同的手术，与医学影像设备(例如B超、核磁、CT等)为医生提供实时的影像信息，保证手术的顺利完成。

本发明采用如下的技术方案：

一种立体定位微型手术机器人，包括多自由度立体定位机器人和床旁定位臂；其特征在于：所述的多自由度立体定位机器人，包括第一位置控制单元、第二位置控制单元、深度控制单元、穿刺针连接装置、第一活动单元、第二活动单元以及第一基座；第一基座提供安装基础，第一、第二位置控制单元分别具备2个主动作动自由度，分别以串联、并联形式组成，末端位置的工作空间为平面，深度控制单元具备1个主动作动自由度的直线进给，第一、第二活动单元分别具有至少俯仰、偏航的被动自由度，两者之间还具有1个被动自由度的翻滚运动和1个被动自由度的直线运动，穿刺针连接装置固定针；

所述的第一位置控制单元、第二位置控制单元通过第一活动单元、第二活动单元以并联机构形式与深度控制单元、穿刺针连接装置连接，从而允许所述的穿刺针连接装置在三维空间内的定位以及俯仰、翻转定姿运动，多自由度立体定位机器人至少具有5个自由度，使末端穿刺针运动到空间目标位置；

所述的第一位置控制单元、第二位置控制单元一端基座固定安装在所述的第一基座上，另一端输出分别与第一活动单元、第二活动单元连接；所述的第一活动单元输出与所述的深度控制单元基座固定连接；所述的穿刺针连接装置固定安装在所述的深度控制单元的输出上。

作为另一种结构形式，一种立体定位微型手术机器人，包括多自由度立体定位机器人和床旁定位臂；所述的多自由度立体定位机器人，包括第一位置控制单元、第二位置控制单元、深度控制单元、穿刺针连接装置、第一活动单元、第二活动单元以及第一基座；第一基座提供安装基础，第一、第二位置控制单元分别具备2个主动作动自由度，分别以串联、并联形式组成，末端位置的工作空间为平面，深度控制单元具备1个主动作动自由度的直线进给，第一、第二活动单元分别具有至少俯仰、偏航的被动自由度，两者之间还具有1个被动自由度的翻滚运动和1个被动自由度的直线运动，穿刺针连接装置固定针；

所述的第一位置控制单元、第二位置控制单元通过第一活动单元、第二活动单元以并联机构形式与深度控制单元、穿刺针连接装置连接，从而允许所述的穿刺针连接装置在三维空间内的定位以及俯仰、翻转定姿运动，多自由度立体定位机器人至少具有5个自由度，使末端穿刺针运动到空间目标位置；

第一基座固定安装在深度控制单元的输出上；第一位置控制单元、第二位置控制单元的一端固定安装在第一基座上，另一端输出分别与第一活动单元、第二活动单元的一端连接；穿刺针连接装置固定安装在第一活动单元的输出上。

其中，所述的第一位置控制单元、第二位置控制单元均采用模块化设计，具有相同的结构，每一个位置控制单元的各个主动作动自由度通过作动器直接驱动，或者通过作动器前置安装在基座而后通过连杆传动来驱动；每一个位置控制单元均由第一关节、第二关节、驱动系统和末端连杆组成；每个关节均由单独的电机驱动，传动机构采用连杆传动，每个关节在电机作用下可转动，控制末端的二自由度位置；每一个位置控制单元各关节铰接处采用双轴连杆设计，以增加第一位置控制单元的定位精度；第二关节的一个连杆设计为弯曲杆，以提高第二关节的运动范围；

所述第一关节由第一关节输出杆、第一电机通过第一转接杆、第五连杆、第六连杆、主轴彼此铰接构成曲柄滑块机构，将第一电机的上下竖直运动转换为第一关节的转动；

所述第二关节由第二电机通过主轴绕过第一关节并通过第三连杆、第四连杆、曲杆、第七连杆、第八连杆和第一关节彼此铰接组成的四连杆机构以及第九连杆、第十连杆将第二电机的竖直上下运动转换为第二关节输出杆的转动；

所述驱动系统，电机与传动机构相结合，组成曲柄滑块机构，使其驱动电机可以保持竖直方向运动；

所述末端连杆，在第二关节输出杆上设计了两个可调节运动范围的档位调节孔。

所述的第一位置控制单元、第二位置控制单元的另一种结构形式为：通过两个电机分别驱动丝杆上的两个螺母沿直线平行运动，通过两个平行四连杆机构带动末端的第二连接底座在平面内进行水平和竖直运动，且第二连接底座的姿态在运动过程中与水平面保持平行不变。

其中，所述的第一活动单元和第二活动单元各自俯仰、偏航的旋转轴正交，第一、第二活动单元之间还具备1个被动自由度的翻滚运动和1个被动自由度的直线运动，翻滚运动中心轴与直线运动方向轴或者称中心轴平行，并且与第一、第二活动单元的俯仰旋转轴正交，与第一、第二活动单元的偏航旋转轴相交；第一、第二活动单元为末端提供至少5个自由度的被动活动度，

所述的第一活动单元的一种结构形式，由第一偏航轴，第一U型块，第一俯仰轴，第一支撑台组成，所述的第一偏航轴一端与第一位置控制单元一端连接，相对于第一位置控制单元一端可转动，转动中心为第一偏航旋转轴；所述的第一偏航轴另一端与第一U型块固定连接；所述的第一俯仰轴与第一U型块连接，相对于第一U型块可转动，转动中心轴为第一俯仰旋转轴；所述的第一支撑台与第一俯仰轴固定连接，并且沿着翻滚运动中心轴方向在中间设计有螺纹孔；

所述的第二活动单元的一种结构形式，由第二俯仰轴孔，第二U型块，第二偏航轴组成；所述的第二偏航轴与第二位置控制单元一端连接，同时与第二U型块固定连接；所述的第二俯仰轴孔与第二U型块连接，相对于第二U型块可转动，转动中心轴为第二俯仰旋转轴；第二俯仰轴孔沿着翻滚运动中心轴方向在中间设计有光孔；所述的第一活动单元与第二活动单元通过一个第一滑杆连接，且第一滑杆一端与第一俯仰轴连接，另一端与第二俯仰轴孔的光孔配合；第一滑杆相对于第二俯仰轴孔能在第一滑杆轴线方向相对直线滑动、绕第一滑杆周向相对翻滚转动。

其中，所述的第一活动单元、第二活动单元的一种相对位置组装形式为：第一俯仰轴、第一偏航轴垂直正交于点O₁；第二俯仰轴孔、第二偏航轴垂直正交于点O₂；第一滑杆旋转轴分别与第一俯仰轴、第二俯仰轴孔的轴线垂直正交于点O₁和点O₂；穿刺针方向与第一滑杆旋转轴方向平行，并偏移距离u₁，典型偏移所在面仍为第一偏航轴和第二偏航轴所形成的平面内。

所述的第一活动单元、第二活动单元的另一种种相对位置组装形式为：第一支撑台为L形状，将深度控制单元向内侧安装，所述第一俯仰轴、所述的第一偏航轴垂直正交于点O₁；第二俯仰轴孔、第二偏航轴垂直正交于点O₂；第一滑杆的旋转轴与穿刺针轴线重合，两者均分别与第一俯仰轴、第二俯仰轴孔的轴线垂直正交于点O₁和点O₂。

其中，所述的第一活动单元和第二活动单元各自俯仰、偏航的旋转轴正交，第一、第二活动单元之间还具备1个被动自由度的翻滚运动和1个被动自由度的直线运动，翻滚运动中心轴与直线运动方向轴或者称中心轴平行，并且与第一、第二活动单元的俯仰旋转轴正交，与第一、第二活动单元的偏航旋转轴相交；第一、第二活动单元为末端提供至少5个自由度的被动活动度；

所述的第一活动单元，由第一偏航轴，第一U型块，第一俯仰轴，第一支撑台组成，所述的第一偏航轴一端与第一位置控制单元一端连接，相对于第一位置控制单元一端可转动，转动中心为第一偏航旋转轴；所述的第一偏航轴另一端与第一U型块固定连接；所述的第一俯仰轴与第一U型块连接，相对于第一U型块可转动，转动中心轴为第一俯仰旋转轴；所述的第一支撑台与第一俯仰轴固定连接，并且沿着翻滚运动中心轴方向在中间设计有螺纹孔；

所述的第二活动单元的有第二种结构形式，包括第五偏航轴、球面外圈、球面内圈三部分；所述的第五偏航轴一端与第二位置控制单元一端连接，第五偏航轴相对于第二位置控制单元一端可转动，转动中心轴为偏航旋转轴；第五偏航轴另一端与球面外圈固定连接；所述的球面外圈与球面内圈形成球铰；球面内圈中空设置，形成光孔；球面内圈与第一滑杆一端连接，且第一滑杆相对于球面内圈的光孔的轴线可直线滑动和翻滚转动。

其中，所述的深度控制单元，包括深度控制单元底座，第三电机，第一联轴器，第一丝杆，螺母座，光杆，所述的深度控制单元底座固定安装在第一支撑台上；所述的第三电机固定安装在深度控制单元底座上；所述的第三电机与第一丝杆一端通过第一联轴器连接；所述的第一丝杆另一端与深度控制单元底座连接；所述的螺母座与第一丝杆、光杆连接，并可沿着光杆滑动；所述的光杆与第一丝杠轴向平行，两端连接到深度控制单元底座上，约束螺母座的旋转运动；所述的第三电机的输出轴转动，通过第一联轴器带动第一丝杆旋转，从而带动所述的螺母座在第一丝杆的长度方向上做前进和后退运动；所述的第三电机的输出轴的两个旋转方向对应所述的螺母座的前进和后退两个动作。

其中，所述的穿刺针连接装置，包括针连接底座，穿刺针，套管，器械固定装置；所述的针连接底座固定安装在所述的深度控制单元的螺母座或者第一活动单元上；所述的针连接底座上设计有两个安装器械固定装置的定位滑槽，所述的器械固定装置根据需求安装在针连接底座至少其中之一的定位滑槽上，所述的套管与器械固定装置连接，所述的穿刺针与套管连接且同轴心；所述的深度控制单元的第三电机的输出轴的两个旋转方向对应螺母座的前进和后退两个动作，从而带动针连接底座前进和后退，进而带动穿刺针运动，完成穿刺任务。

一种所述的立体定位微型手术机器人的设计方法，包括：所述第一位置控制单元、第二位置控制单元的末端工作空间W₁、W₂、深度控制单元控制穿刺针的行程L，由精准穿刺手术的工作方式确定；病灶点与体表切口决定第一、第二位置控制单元和深度控制单元的设计目标；由病灶(g_i)和穿刺点(t_j)计算针路，由针路在第一、二位置控制单元的末端工作空间所在平面XY₁、XY₂上的交集计算第一位置控制单元、第二位置控制单元的末端工作空间W₁、W₂；由病灶(g_i)到穿刺点(t_j)距离中的最大值计算深度控制单元控制穿刺针的行程L；具体过程如下：

设计时假设穿刺针与翻滚运动中心轴重合，建立全局坐标系为O_W，假设目标器官所在空间为W_o，目标器官中的病灶为g_i(i＝1，2，3...)；体表切口的分布空间为W_t，体表切口的穿刺点为t_j(j＝1，2，3...)；第一位置控制单元的末端工作空间为W₁，W₁所在平面为XY₁，第二位置控制单元的末端工作空间为W₂，W₂所在平面为XY₂，平面XY₁和平面XY₂与全局坐标系O_W的X_WY_W平面平行，W₁与W₂之间的距离为d，W₂与W_t之间的距离为h，W_t与W_o之间的距离为k，l_j为穿刺针所在直线；

设计时假设穿刺针轴线与翻滚运动中心轴重合，第一、第二位置控制单元末端与穿刺针连接点分别为O_d1、O_d2，分别在XY₁、XY₂平面内；穿刺针沿穿刺方向的进给深度即穿刺针的行程L由深度控制单元控制；为实现针路覆盖器官内尽量多的病灶(g_i)以及穿刺针精准穿中病灶，并避开危险组织器官，同时减少切口数量，根据手术的样式确定合理的穿刺点(t_j)位置和数量；

目标器官中的病灶g_i的坐标为P_gi(x_i，y_i，z_i)(i＝1，2，3.....)，穿刺点t_j的坐标为P_tj(x_j，y_j，z_j)(j＝1，2，3.....)，则l_j所在的空间直线方程为：

XY₁、XY₂平面与全局坐标系O_wX_wY_w平面平行，则XY₁、XY₂的平面方程分别为：

z₁＝E₁ (2)

z₂＝E₁-d (3)

其中，E₁为全局坐标系中平面XY₁与Z_W轴交点的z坐标值；

设穿刺针所在直线方程l_j与XY₁、XY₂平面的交点分别为O_1j、O_2j，则联立方程(1)、(2)可以求得交点O_1j的坐标为：

联立方程(1)、(3)可以求得交点O_2j的坐标为：

基于公式(4)和(5)，通过目标器官中的所有病灶坐标P_gi(x_i,y_i,z_i)(其中i＝1,2,3…)与所有穿刺点t_j的坐标P_tj(x_j,y_j,z_j)(其中j＝1,2,3…)，可以求得所有的与平面XY₁和XY₂的交点O_1j和O_2j的坐标，假设所有的点O_1j坐标分布空间为W_s1，所有的点O_2j坐标分布空间为W_s2，空间点分布W_s1和W_s2分别为第一位置控制单元、第二位置控制单元的末端O_d1、O_d2的工作空间设计输入，则W_s1∈W₁和W_s2∈W₂，根据此设计所述的第一位置控制单元、第二位置控制单元的末端O_d1、O_d2的工作空间W₁和W₂应分别不小于W_s1和W_s2，即W₁≥W_s1和W₂≥W_s2，这样第一位置控制单元和第二位置控制单元才能控制穿刺针运动到体表所有穿刺点位置，完成穿刺任务；

目标器官中的病灶坐标P_gi(x_i,y_i,z_i)(i＝1,2,3…)与穿刺点t_j的坐标P_tj(x_j,y_j,z_j)(j＝1,2,3.....)之间的最大距离为：

为使穿刺针能到达目标器官中所有的病灶位置，所述的深度控制单元控制穿刺针的行程L应不小于L_P,即L≥L_P，这样深度控制单元才能控制穿刺针到达所有的病灶位置。

一种所述的立体定位微型手术机器人的运动学控制方法：包括正运动学和逆运动学，传动支链环节包括穿刺针、针连接装置、深度控制单元、活动单元和位置控制单元的运动学；

其中，正运动学控制具体过程如下：

根据D-H参数法则建立各组成部分坐标系及变量；设全局坐标系为W_O；建立第一位置控制单元基座b₁的坐标系W_b1；在第一活动单元俯仰偏航正交点O₁处，建立第一位置控制单元的末端输出坐标系为W_o1，第一位置控制单元的末端输出坐标为

第一活动单元偏航旋转坐标系为W_yaw1,第一活动单元俯仰旋转轴坐标系为W_pitch1；穿刺针轴线与第一俯仰轴的公垂线相交于第一滑杆翻滚运动中心轴的点为r₁，穿刺针轴线与第一俯仰轴的公垂线相交于穿刺针轴线的点为p₁，在点r₁建立连接深度控制单元基座的坐标系为W_r1；同理建立第二位置控制单元相关变量：穿刺针轴线与第二俯仰轴的公垂线相交于第一滑杆翻滚运动中心轴的点为r₂，穿刺针轴线与第二俯仰轴的公垂线相交于穿刺针轴线的点为p₂，在点r₂建立连接深度控制单元基座的坐标系为W_r2；r₁与r₂之间距离为n；穿刺针与第一滑杆翻滚运动中心轴的距离为d₁；第一滑杆轴线距离第一活动单元俯仰偏航正交点O₁的距离为d₂，距离第二活动单元俯仰偏航正交点O₂的距离为d'₂，穿刺针末端e的坐标P_e；第一、第二位置控制单元的基座和末端输出的坐标分别为P_b1、P_b2、P_o1、P_o2；深度控制单元的初始位置为h₀，位移为h₁，为h＝h₀+h₁；第一滑杆翻滚运动中心轴的转角为c；s₁₁、s₁₂分别为第一位置控制单元的两个作动器控制变量；

所述立体定位手术机器人的正运动学计算如下：

S1：从第一位置控制单元侧计算运动学

第一位置控制单元的基座坐标系W_b1相对于全局坐标系W_O的齐次坐标变换为：T_b1；根据第一位置控制单元的运动学，第一位置控制单元的末端输出坐标系W_o1相对于第一位置控制单元的基座坐标系W_b1的齐次坐标变换为：

T₁＝T_f(s₁₁,s₁₂) (7)

点r₁所在坐标系W_r1在经过第一活动单元的偏航和俯仰运动后，相对于第一位置控制单元的末端输出坐标系W_o1的齐次坐标变换为：

T_h1＝Rot(Y_o1,a₁)Rot(X_yaw1,b₁)Trans(Y_pitch1,d₂) (8)

其中，Rot(Y_o1,a₁)为绕Y_o1轴旋转a₁角度的4×4齐次旋转矩阵；Rot(X_yaw1,b₁)为绕X_yaw1轴旋转b₁角度的4×4齐次旋转矩阵；Trans(Y_pitch1,d₂)表示为沿着Y_pitch1轴平移d₂距离的4×4齐次平移矩阵；

将点r₁所在坐标系W_r1从第一活动单元、第一位置控制单元的第一支链机构计算得到相对于全局坐标系W_O的齐次坐标变换为：T₁₁＝T_b1T₁T_h1；

S2：从第二位置控制单元测计算运动学

W_b2相对于W_O的齐次坐标变换为：T_b2

根据第二位置控制单元的运动学，W_o2相对W_b2的齐次坐标变换为：

T₂＝T_f(s₂₁,s₂₂) (9)

W_r2相对于W_o2的齐次坐标变换为：

T_h2＝Rot(Y_o2,a₂)Rot(X_yaw2,b₂)Trans(Y_pitch2,d_2') (10)

其中，Rot(Y_o2,a₂)为绕Y_o2轴旋转a₂角度的4×4齐次旋转矩阵；Rot(X_yaw2,b₂)为绕X_yaw2轴旋转b₂角度的4×4齐次旋转矩阵；Trans(Y_pitch2,d_2')表示为沿着Y_pitch2轴平移d_2'距离的4×4齐次平移矩阵；

当点r₂所在的坐标系W_r2绕第一滑杆翻滚运动中心轴转动c角度，再沿着第一滑杆运动中心轴移动距离n，W_r2坐标系与W_r1坐标系重合，则坐标变换为：

T_r2r1＝Rot(X,c)Trans(X,-n) (11)

则将坐标系W_r1经过坐标系W_r2，从深度控制单元、第二活动单元、第二位置控制单元的第二支链计算得到的全局坐标变换为：T₁₂＝T_b2T₂T_h2T_r2r1

S3：同一坐标系经过两个支链的变换后，得到等式方程：

或者

利用以下步骤A1-A3迭代法求解，令公式(12)的6个变量为a₁、b₁、a₂、b₂、n、c＝x₁、x₆；

A1：给定X⁰＝[x₁、x₆]'初始值；

A2：根据公式(12)分别计算F＝[f₁、f₉]'和T＝[t₁、t₃]'；

A3：如果f_i<e₁和t_j<e₂，计算完成，否则按照如下计算迭代值，带入所述A2循环：

式中，

a与β为给定的收敛系数，非负数，和为1；

S4：计算穿刺针上坐标系

根据D-H参数法则建立各组成部分坐标系，穿刺针轴线分别与第一、第二俯仰轴的公垂线相交于穿刺针轴线点p₁和点p₂，刺针末端点为e，分别以该三个点为原点建立连接在穿刺针上的坐标系W_p1、W_p2、W_e；

W_p1相对W_r1的齐次坐标变换为：T_p1＝Trans(Y_r1,d₁)，W_p2相对W_p1的齐次坐标变换为：T_p1p2＝Trans(Z_p1,-n)，W_e相对W_p1的齐次坐标变换为：T_e＝Trans(Z_p1,-h)；

W_e在全局坐标系W_O的齐次坐标变换为：T_We＝T_b1T₁T_h1T_p1T_e；

W_e下的e点坐标P_e在全局坐标系W_O下的坐标为：P_we＝T_WeP_e，从T_We得到的旋转矩阵R_e为穿刺针的姿态矩阵。

所述的立体定位手术机器人的逆运动学：

H1:设定a₁、b₁、a₂、b₂、n、c六个被动关节对应的变量以及s₁₁、s₁₂、s₂₁、s₂₂四个作动器的控制变量为未知变量，组合形成向量X＝[x_i]_10×1；

已知目标器官中病灶g_i的坐标为P_gi(x_i,y_i,z_i),(i＝1,2,3....)，穿刺点t_j的坐标为P_tj(x_j,y_j,z_j),(j＝1,2,3......)，则穿刺针所在的空间直线方程如公式(1)，

点p₁、p₂的坐标系W_p1和W_p2在全局坐标系W_O的齐次坐标变换分别为：

T_Wp1＝T_b1T₁T_h1T_p1

T_Wp2＝T_Wp1T_p1p2 (13)

点p₁、p₂分别在坐标系W_p1、W_p2下的坐标分别为p_p1、p_p2，则点p₁、p₂在全局坐标系W_O下的坐标为：

p_w1＝T_wp1p_p1

p_w2＝T_wp2p_p2 (14)

H2:求解变量X

B1：点p₁(p₂)位于穿刺针轴线上，所以满足空间直线方程(1)，因此，将方程(14)代入方程(1)，形成4个独立的方程；

B2：再根据所述步骤S1、S2、S3和S4的方法得到公式(12)，计算12个方程；

B3：同理参照所述步骤A1-A3迭代法求解，得到X，X向量后四个元素为s₁₁、s₁₂、s₂₁、s₂₂；

H3：根据正运动学计算P_e，深度控制单元控制穿刺针运动的行程为：

本发明一种立体定位微型手术机器人，的优点在于：提供了两种优选的立体定位机器人组装形式、三种位置控制单元、立体定位机器人位置控制单元的参数设计方法以及立体定位机器人的运动学计算方法。从手术机器人的结构来看，该机器人体积非常小，非常方便的携带或者移动到想要的位置。立体定位机器人具有至少5个自由度，通过两个位置控制单元来给末端穿刺针提供至少4个自由度的定位定姿，通过深度控制单元提供1自由度进给，大大增加了末端穿刺针的灵活度，能使穿刺针精准的运动到目标靶点位置，完成穿刺。

该立体定位微型手术机器人体积小，安装方便，末端执行机构具有至少5个自由度，能达到空间所需的位置，末端执行机构能配合不同的手术器械(例如穿刺针、活检针、消融针等)来完成不同的手术，与医学影像设备(例如B超、核磁、CT等)为医生提供实时的影像信息，保证手术的顺利完成。

附图说明

图1所示为本发明的一种立体定位微型手术机器人整体结构图

图2a所示为本发明实施例中多自由度立体定位机器人的一种组装形式Ⅰ的整体结构图

图2b所示为图2a的结构爆炸图

图3所示为本发明的一种多自由度立体定位机器人的工作空间和穿刺路径示意图

图4a所示为本发明实施例中多自由度立体定位机器人的一种组装形式Ⅱ的整体结构图

图4b所示为图4a的结构爆炸图

图5所示为本发明实施例中第一位置控制单元的一种结构形式A结构爆炸图

图6所示为图5所示的第一位置控制单元的驱动系统局部结构图

图7所示为图5所示的第一位置控制单元的第二关节输出杆档位调节孔示意图

图8所示为图5、6、7所示的第一位置控制单元的一档运动范围示意图

图9所示为图5、6、7所示的第一位置控制单元的二挡运动范围示意图

图10所示为本发明的第一活动单元、第二活动单元的一种相对位置组装形式的整体结构图

图11所示为本发明的一种深度控制单元的整体结构图

图12所示为本发明的一种穿刺针连接装置的整体结构图

图13所示为本发明的一种床旁定位臂的整体结构图

图14所示为本发明实施例中第一位置控制单元的一种结构形式B的整体结构图

图15所示为本发明实施例中多自由度立体定位机器人的一种组装形式Ⅲ的整体结构图

图16所示为本发明的实施例中第一位置控制单元的一种结构形式C的整体结构图

图17所示为图16的结构组成图和爆炸图

图18所示为本发明实施例中多自由度立体定位机器人的一种组装形式Ⅳ的整体结构图

图19所示为本发明的第一活动单元、第二活动单元的另一种相对位置组装形式的整体结构图

图20所示为本发明实施例中多自由度立体定位机器人的一种组装形式Ⅴ的整体结构图

图21所示为图20第一、第二活动单元局部放大结构图和各旋转轴关系示意图

图22所示为本发明的一种第二活动单元D的整体结构图

图23所示为本发明实施例中多自由度立体定位机器人的一种组装形式ⅥI的整体结构图

图24所示为图23第一、第二活动单元局部放大结构图和各旋转轴关系示意图

图25所示为本发明实施例中多自由度立体定位机器人的一种组装形式VII的整体结构图

图26所示为图25第一、第二活动单元局部放大结构图和各旋转轴关系示意图

图27为立体定位微型手术机器人运动学坐标系定义与坐标变换示意图

图27a、b、c为图27的拆分示意图

图27d为图27的流程图

图28所示为位置控制单元A的运动学坐标定义与参数变量

图29a、b所示为位置控制单元B的运动学坐标定义与参数变量

图30a、b所示为位置控制单元C的运动学坐标定义与参数变量

图中编号说明如下列表：

具体实施方式

下面结合附图，对本发明的技术方案做进一步的说明。

一种立体定位微型手术机器人，如图1所示，立体定位微型手术机器人主要由多自由度立体定位机器人1和床旁定位臂2两部分组成。所述的多自由度立体定位机器人1，如图2a，图2b所示，其中图2b为图2a的爆炸图，包括第一位置控制单元3、第二位置控制单元4、深度控制单元5、穿刺针连接装置6、第一活动单元7、第二活动单元8以及第一基座9。第一基座9提供安装基础。第一、第二位置控制单元分别具备2个主动作动自由度，末端位置的工作空间为平面。深度控制单元具备1个主动作动自由度的直线进给。第一、第二活动单元分别具有至少俯仰、偏航的被动自由度，两者之间还具有1个被动自由度的翻滚运动和1个被动自由度的直线运动。穿刺针连接装置6固定针(包括并不限于穿刺针、活检针、消融针等任一种靶向穿刺相关针，或具备末端组织采样、射频消融、微波消融、激光消融、冷冻或者射流的器械)。多自由度立体定位机器人1提供不少于5个自由度的定位、定(向)姿与深度控制能力，能够通过身体上的不同切口(穿刺点)穿刺进入人体达到预定病灶点。

所述的第一位置控制单元3、第二位置控制单元4分别至少具有两个主动作动自由度，分别以串联、并联形式组成，各个主动作动自由度通过作动器(电机)直接驱动或者更优地为降低远端负载，通过电机前置安装在基座而后通过连杆传动来驱动。所述的第一位置控制单元3、第二位置控制单元4通过第一活动单元7、第二活动单元8以并联机构形式与深度控制单元5、穿刺针连接装置6连接，从而允许所述的穿刺针连接装置6在三维空间内的定位以及俯仰、翻转定姿(向)运动，多自由度立体定位机器人1至少具有5个自由度，从而能使末端穿刺针运动到空间目标位置。

所述第一位置控制单元3、第二位置控制单元4的末端工作空间W₁、W₂、深度控制单元5控制穿刺针的行程L，由精准穿刺手术的工作方式确定；病灶点与体表切口(入针点)决定第一、第二位置控制单元和深度控制单元的设计目标；由病灶(g_i)和穿刺点(t_j)计算针路，由针路在第一、二位置控制单元的末端工作空间所在平面XY₁、XY₂上的交集计算第一位置控制单元3、第二位置控制单元4的末端工作空间W₁、W₂；由病灶(g_i)到穿刺点(t_j)距离中的最大值计算深度控制单元5控制穿刺针的行程L。

如图3所示，设计时假设穿刺针与翻滚运动中心轴重合，建立全局坐标系为O_W，假设目标器官所在空间为W_o，目标器官中的病灶为g_i(i＝1，2，3...)；体表切口的分布空间为W_t，体表切口的穿刺点为t_j(j＝1，2，3...)；第一位置控制单元的末端工作空间为W₁，W₁所在平面为XY₁，第二位置控制单元的末端工作空间为W₂，W₂所在平面为XY₂，平面XY₁和平面XY₂与全局坐标系O_W的X_WY_W平面平行，W₁与W₂之间的距离为d，W₂与W_t之间的距离为h，W_t与W_o之间的距离为k(距离可为最短欧式距离)，l_j为穿刺针所在直线；

设计时假设穿刺针轴线与翻滚运动中心轴重合，第一、第二位置控制单元末端与穿刺针连接点分别为O_d1、O_d2，分别在XY₁、XY₂平面内；穿刺针沿穿刺方向的进给深度即穿刺针的行程L由深度控制单元5控制；针路决定穿刺手术有效性、精准性以及微创化；为实现针路覆盖器官内尽量多的病灶(g_i)以及穿刺针精准穿中病灶，并避开危险组织器官，同时减少切口数量，根据手术的样式确定合理的穿刺点(t_j)位置和数量。

目标器官中的病灶g_i的坐标为P_gi(x_i,y_i,z_i)(i＝1,2,3…)，穿刺点t_j的坐标为P_tj(x_j,y_j,z_j)(j＝1,2,3.....)，则l_j所在的空间直线方程为：

XY₁、XY₂平面与全局坐标系O_wX_wY_w平面平行，则XY₁、XY₂的平面方程分别为：

z₁＝E₁ (2)

z₂＝E₁-d (3)

其中，E₁为全局坐标系中平面XY₁与Z_W轴交点的z坐标值。

设穿刺针所在直线方程l_j与XY₁、XY₂平面的交点分别为O_1j、O_2j，则联立方程(1)、(2)可以求得交点O_1j的坐标为：

联立方程(1)、(3)可以求得交点O_2j的坐标为：

基于公式(4)和(5)，通过目标器官中的所有病灶坐标P_gi(x_i,y_i,z_i)(其中i＝1,2,3…)与所有穿刺点t_j的坐标P_tj(x_j,y_j,z_j)(其中j＝1,2,3…)，可以求得所有的与平面XY₁和XY₂的交点O_1j和O_2j的坐标，假设所有的点O_1j坐标分布空间为W_s1，所有的点O_2j坐标分布空间为W_s2，空间点分布W_s1和W_s2分别为第一位置控制单元、第二位置控制单元的末端O₁、O₂的工作空间设计输入，则W_s1∈W₁和W_s2∈W₂，根据此设计所述的第一位置控制单元、第二位置控制单元的末端O₁、O₂的工作空间W₁和W₂应分别不小于W_s1和W_s2，即W₁≥W_s1和W₂≥W_s2，这样第一位置控制单元和第二位置控制单元才能控制穿刺针运动到体表所有穿刺点位置，完成穿刺任务。

目标器官中的病灶坐标P_gi(x_i,y_i,z_i)(i＝1,2,3…)与穿刺点t₍的坐标P_tj(x_j,y_j,z_j)(j＝1,2,3.....)之间的最大距离为：

为使穿刺针能到达目标器官中所有的病灶位置，设计所述的深度控制单元5控制穿刺针的行程L应不小于L_P,即L>L_P，这样深度控制单元才能控制穿刺针到达所有的病灶位置。

作为本发明的一个实施例，所述的多自由度立体定位机器人1的一种组装形式Ⅰ，如图2a、2b所示，其中图2b为图2a的爆炸图，所述的第一位置控制单元3、第二位置控制单元4一端基座固定安装在所述的第一基座9上，另一端输出分别与第一活动单元7、第二活动单元8连接；所述的第一活动单元7输出与所述的深度控制单元5基座固定连接；所述的穿刺针连接装置6固定安装在所述的深度控制单元5的输出(螺母座)上。

作为本发明的一个实施例，所述的多自由度立体定位机器人1的另一种组装形式Ⅱ，如图4a，4b所示，其中图4b为图4a的爆炸图，第一基座11固定安装在深度控制单元10的输出(螺母座)上；第一位置控制单元12、第二位置控制单元13的一端固定安装在第一基座11上，另一端输出分别与第一活动单元16、第二活动单元14的一端连接；穿刺针连接装置15固定安装在第一活动单元16的输出上。

所述的第一位置控制单元与第二位置控制单元之间的距离为d。

所述的第一位置控制单元、第二位置控制单元均采用模块化设计，具有相同的结构，每个位置控制单元至少具有两个主动作动自由度。每一个位置控制单元的各个主动作动自由度通过作动器(电机)直接驱动或者更优地为降低远端负载，通过电机前置安装在基座而后通过连杆传动来驱动；因为两个位置控制单元的结构完全一样，所以下面只详细介绍第一位置控制单元。

所述的第一位置控制单元由两个关节构成，分别为第一关节和第二关节，每个关节均由单独的电机驱动，传动机构采用连杆传动，每个关节在电机作用下可转动，控制末端的二自由度位置。一是为提高第一位置控制单元的传力性能以及强度和刚度等，第一位置控制单元各关节铰接处采用了双轴连杆设计，同时还可以增加第一位置控制单元的定位精度；二是为了提高第一位置控制单元的运动范围，将第二关节的一个连杆计为弯曲杆，能显著提高第一位置控制单元第二关节的运动范围。

第一位置控制单元结构如图5、6、7所示，第一位置控制单元由第一关节、第二关节、驱动系统和末端连杆组成。

所述第一关节由第一关节输出杆33、第一电机19通过第一转接杆32、第五连杆31、第六连杆45、主轴28彼此铰接构成曲柄滑块机构，将第一电机的上下竖直运动转换为第一关节的转动。具体描述：第一电机19通过第一电机固定座17、第一电机底座21将其连接在外壳24内侧，并且由内六角螺栓23与外壳24预留的螺栓孔位将其完全固定；第一电机19与第一电机轴26通过螺纹固连，第一转接杆32套在第一电机轴26上；第一转接杆32和第五连杆31、第六连杆45均与第三轴29铰接，第三轴29一端设计有螺纹并与六角型螺母41连接；第五连杆31、第六连杆45另一端与第一关节输出杆33铰接，同时第一关节输出杆33还与主轴28铰接，主轴28与外壳24铰接，并且固定在外壳24上。通过上述描述的结构特征，第一电机驱动第一关节进行转动。

所述第二关节由第二电机20通过主轴28绕过第一关节并通过第三连杆46、第四连杆48、曲杆44、第七连杆36、第八连杆43和第一关节彼此铰接组成的四连杆机构以及第九连杆38、第十连杆42将第二电机的竖直上下运动转换为第二关节输出杆39的转动。具体描述：第二电机20通过第二电机固定座18、第二电机底座22将其连接在外壳24内侧，并且由内六角螺栓与外壳预留的螺栓孔位将其完全固定；第二电机20与第二电机轴51通过螺纹固连，第二转接杆50套在第二电机轴51上；第二转接杆50和第一连杆47、第二连杆49均与第一轴25铰接，第一轴25一端设计有螺纹并与六角型螺母连接；第一连杆47、第二连杆49与第三连杆46、第四连杆48和曲杆44均通过第二轴27铰接，第三连杆46、第四连杆48、轴套52和主轴28铰接；曲杆44另一端与第七连杆36、第八连杆43和第九连杆38、第十连杆42通过第六轴35铰接，第九连杆38、第十连杆42和第二关节输出杆39的两个档位调节孔其中之一铰接，第七连杆36、第八连杆43另一端通过第五轴34和第一关节输出杆33、第二关节输出杆39同时铰接。通过上述描述的结构特征，当第二电机在竖直方向运动时，通过主轴带动第一连杆与第二连杆、第三连杆、第四连杆、曲杆、第七连杆、第八连杆和第一关节输出杆彼此铰接组成的四连杆机构以及第九连杆、第十连杆，将运动传递给第二关节输出杆使其绕第五轴34转动。

所述驱动系统，如图6所示，电机与传动机构相结合，组成曲柄滑块机构，使其驱动电机可以保持竖直方向运动。该驱动系统结构如下：第一电机19上通过螺纹连接有第一电机轴26，第一电机轴26与第一转接杆32连接，第一转接杆32套在第一电机轴26上；第二电机20上通过螺纹连接有第二电机轴51，第二电机轴51与第二转接杆50连接，第二转接杆50套在第二电机轴51上。第一转接杆32与第五连杆31、第六连杆45一端铰接，第五连杆31,、第六连杆45另一端和第一关节输出杆33铰接从而带动第一关节转动。运动过程：第一电机进行竖直方向伸缩，带动第一电机轴和第五连杆、第六连杆等构件运动进而带动第一关节输出杆转动。同时，第二电机进行竖直方向伸缩，带动第二电机轴和第一连杆、第二连杆以及第三连杆、第四连杆、曲杆、第七连杆、第八连杆和第一关节输出杆彼此铰接组成的四连杆机构以及第九连杆、第十连杆，从而驱动第二关节输出杆转动。

所述末端连杆，如图7所示，在保证连杆体积尽量小的情况下，在第二关节输出杆39上设计了两个可调节运动范围的档位调节孔，分别为第一挡调节孔53和第二档调节孔54。当第九连杆、第十连杆连接到第一档调节孔时，连杆末端处于伸直状态，运动范围较小。当第九连杆、第十连杆连接到第二档调节孔时，连杆末端处于微屈状态，运动范围较大，提高了第一位置控制单元在运动范围方面的适应能力，而且档位调节过程简单，方便操作。第一位置控制单元的第一档和第二档两种调节状态运动范围分别如图8、图9所示，其中深黑色区域为第一位置控制单元末端运动范围。

所述第一电机19与第一电机轴26固连，第一转接杆32套在第一电机轴26上；第一转接杆32和第五连杆31、第六连杆45均与第三轴29铰接，第三轴29一端设计有螺纹并与六角型螺母41连接；第五连杆32、第六连杆45的另一端与第一关节输出杆33通过第四轴30铰接，同时第一关节输出杆33还与主轴28铰接，主轴28与外壳24铰接并固定；

所述的第二电机20与第二电机轴51通过螺纹固连，第二转接杆50套在第二电机轴51上；第二转接杆50和第一连杆47、第二连杆49均与第一轴25铰接，第一轴25一端设计有螺纹并与六角型螺母41连接；第一连杆47、第二连杆49与第三连杆46、第四连杆48和曲杆44均通过第二轴27铰接，第三连杆46、第四连杆48、轴套52和主轴28铰接；曲杆44另一端与第八连杆43、第七连杆36和第九连杆38、第十连杆42通过第六轴35铰接，第九连杆38、第十连杆42和第二关节输出杆39的两个档位调节孔其中之一通过第七轴37铰接，第七连杆36、第八连杆43的另一端通过第五轴34和第一关节输出杆33、第二关节输出杆39同时铰接；通过上述结构，当第二电机20在竖直方向运动时，通过第二转接杆50带动第一连杆与第二连杆、第三连杆、第四连杆、曲杆、第七连杆、第八连杆和第一关节输出杆彼此铰接组成的四连杆机构以及第九连杆、第十连杆，将运动传递给第二关节输出杆使其绕第五轴转动。

第一位置控制单元运动过程如下：由于第一位置控制单元分别是由两个电机独立驱动的，因此在第一位置控制单元在竖直状态下，第一关节保持不动，第二关节通过第二电机驱动也可进行转动。两个关节分别由两个电机独立驱动，驱动部分设计成曲柄滑块机构使得第一电机和第二电机能够保持在竖直方向上移动，保证了运动的可靠性。当第一位置控制单元开始运动时，第一关节由第一电机通过转接杆、第五连杆、第六连杆、第一关节输出杆、主轴彼此铰接构成曲柄滑块机构，将第一电机的上下竖直运动转换为第一关节转动，使得第一关节开始弯曲；同时第二关节由第二电机通过主轴绕过第一关节并通过第三连杆、第四连杆、曲杆、第七连杆、第八连杆和第一关节输出杆彼此铰接组成的四连杆机构以及第九连杆、第十连杆将第二电机的竖直上下运动转换为第二关节转动，使得第二关节实现弯曲运动。当第一电机完全收缩时，第一关节停止转动，此时第一关节弯曲达到最大值；由于考虑到第二关节末端运动范围要尽可能的大，因此选取的第二电机行程大于第一电机的行程。此时第二电机继续伸出，第二关节不断扩大运动范围，当第二电机完全伸出时，第二关节停止弯曲，并且达到最大运动范围，同时配合第一关节完成运动过程。所述的第一位置控制单元、第二位置控制单元的末端连杆，均设计有安装孔位，与所述的第一活动单元、第二活动单元连接。

作为本发明的一个实施例，所述的两个位置控制单元的另外一种结构形式B，如图14所示，同样的；两个位置控制单元的结构完全相同，均由第四电机82，第十一连杆83，第十二连杆84，第一连接底座85，第八轴86，第十三连杆87，第九轴88，第十四连杆89，第五电机90，第二基座91，第十轴92等组成。所述的第四电机82与第五电机90固定安装在所述的第二基座91上，且第四电机82与第五电机90之间是有距离的；所述的第十一连杆83与所述的第四电机82的输出轴连接；所述的第十一连杆83与所述的第十二连杆84通过所述的第十轴92连接；所述的第十二连杆84、第一连接底座85和第十三连杆87通过所述的第八轴86连接，且第一连接底座85通过紧固件与第十二连杆84或者第十三连杆87刚性连接；所述的第十三杆87与所述的第十四杆89通过所述的第九轴88连接；所述的第十四连杆89与所述的第五电机90的输出轴连接。第四电机82的输出轴、第八轴86、第九轴88，第五电机90的输出轴、第十轴92平行。所述的第四电机、第五电机的输出轴的旋转分别驱动所述的第十一连杆83、第十四连杆89的转动，从而控制末端的第一连接底座85在XY平面(工作空间)内的位置。作为一个实施例，所述的第二基座91、第一连接底座85上设计有安装孔位，使两个位置控制单元按照工作空间的平面平行与第一基座9、深度控制单元5、第一、第二活动单元7、8以及穿刺针连接装置6等连接，可实现针定位定姿，如图15所示。

作为本发明的一个实施例，本发明提供了所述的两个位置控制单元的再一种结构形式C，如图16、17所示；该结构通过两个电机分别驱动丝杆上的两个螺母沿直线平行运动，通过平行四连杆机构带动末端第二连接底座在平面内进行水平和竖直运动且第二连接底座姿态在运动过程中与水平面保持平行不变；该结构包括：第一支撑底座93，第一支撑滑块94，滑杆104-1，第一支撑螺母座95，第二丝杆96，第二联轴器97，第六电机98，第三基座99，第七电机100，第三联轴器101，第二支撑螺母座102，第二支撑滑块103，滑杆104-2，第三丝杆105，第二支撑底座106，第十五连杆107，第十六连杆108，第二连接底座109，第十七连杆110，第十八连杆111，第十九连杆112，第二十连杆113等。所述的第一支撑底座93和第二支撑底座106固定安装在所述的第三基座99内，所述的第六电机98、第七电机100分别固定安装在所述的第一支撑底座93和第二支撑底座106上，所述的第二丝杆96、第三丝杆105分别通过所述的第二联轴器97、第三联轴器101分别与所述的第六电机98、第七电机100的输出轴连接；所述的第一支撑螺母座95、第二支撑螺母座102分别与所述的第二丝杆96、第三丝杆105、滑杆104-1和104-2连接，并沿着滑杆104-1和104-2直线滑动。所述的滑杆104-1和104-2分别与第二丝杠96、第三丝杆105轴向平行，两端分别与第一支撑底座93、第二支撑底座106连接，分别约束第一支撑螺母座95、第二支撑螺母座102的旋转。所述的第一支撑滑块94、第二支撑滑块103分别固定安装在所述的第一支撑螺母座95、第二支撑螺母座102上；所述的第一支撑滑块94与所述的第十七连杆110、第十八连杆111、第二十连杆113的一端以铰接形式连接，所述的第十七连杆110、第十八连杆111、第二十连杆113的另一端与所述的第二连接底座109一端以铰接形式连接，构成铰接的平行四边形连杆机构；所述的第二支撑滑块103与所述的第十五连杆107、第十六连杆108、第十九连杆112的一端以铰接形式连接，所述的第十五连杆107、第十六连杆108、第十九连杆112的另一端与所述的第二连接底座109另一端以铰接形式连接，构成铰接的平行四边形连杆机构；所述的第六电机98、第七电机100的输出轴转动，分别通过第二联轴器97、第三联轴器101带动所述的第二丝杆96、第三丝杆105旋转，从而分别带动所述的第一支撑螺母座95、第二支撑螺母座102在所述的第二丝杆、第三丝杆的长度方向上做前进和后退运动，进而分别带动所述的第一支撑滑块94、第二支撑滑块103在所述的第二丝杆、第三丝杆的长度方向上做前进和后退运动，进而控制末端第二连接底座109在XY平面内的位置，且第二连接底座109的姿态，在运动过程中与水平面保持平行不变；采用铰接的平行四边形连杆机构，能保证第二连接底座109姿态始终不变。作为一个实施例，所述的第三基座99、第二连接底座109上设计有安装孔位，使两个位置控制单元按照工作空间的平面平行且与所述的第一基座9、所述的深度控制单元5、第一、第二活动单元7、8以及穿刺针连接装置6等连接，可实现穿刺针定位定姿，如图18所示。

所述的第一活动单元7和第二活动单元8，如图2a和图10所示。第一、第二活动单元的各自俯仰、偏航的旋转轴正交。第一、第二活动单元之间还具备1个被动自由度的翻滚运动和1个被动自由度的直线运动，翻滚运动中心轴与直线运动方向轴(或者称中心轴)平行，并且与第一、第二活动单元的俯仰旋转轴正交，与第一、第二活动单元的偏航旋转轴相交；第一、第二活动单元为末端提供至少5个自由度的被动活动度，这样就能实现位移和姿态的传递。

如图10所示，所述的第一活动单元7的一种结构形式A，主要由第一偏航轴55，第一U型块56，第一俯仰轴57，第一支撑台58等组成，所述的第一偏航轴55一端与所述的第一位置控制单元一端连接，第一偏航轴55相对于第一位置控制单元一端可转动，转动中心为第一偏航旋转轴；所述的第一偏航轴55另一端与所述的第一U型块56固定连接；所述的第一俯仰轴57与所述的第一U型块56连接，第一俯仰轴57相对于第一U型块56可转动，转动中心轴为第一俯仰旋转轴；所述的第一支撑台58与所述的第一俯仰轴57固定连接，并且沿着翻滚运动中心轴方向在中间设计有螺纹孔。

如图10所示，所述的第二活动单元8的一种结构形式A，主要由第二俯仰轴孔59，第二U型块60，第二偏航轴61等组成；所述的第二偏航轴61与所述的第二位置控制单元一端连接，第二偏航轴61相对于第二位置控制单元一端可转动，转动中心为第二偏航旋转轴；所述的第二偏航轴61与所述的第二U型块60固定连接；所述的第二俯仰轴孔59与所述的第二U型块60连接，第二俯仰轴孔59相对于第二U型块60可转动，转动中心轴为第二俯仰旋转轴。第二俯仰轴孔59沿着翻滚运动中心轴方向在中间设计有光孔。所述的第一活动单元7与第二活动单元8通过一个第一滑杆62连接，且第一滑杆62一端设计有螺纹与所述的第一俯仰轴57的螺纹孔通过螺纹固定连接，第一滑杆62的另一端与所述的第二俯仰轴孔59的光孔配合；第一滑杆62相对于第二俯仰轴孔59能在第一滑杆62轴线方向相对直线滑动、绕第一滑杆62周向相对翻滚转动。

作为一个实施例，所述的第一活动单元7、第二活动单元8的一种相对位置组装形式如图10所示。所述第一俯仰轴57、所述的第一偏航轴55垂直正交于点O₁；第二俯仰轴孔59、第二偏航轴61垂直正交于点O₂；第一滑杆62旋转轴(翻滚运动中心轴、直线运动方向轴)分别与第一俯仰轴57、第二俯仰轴孔59的轴线垂直正交于点O₁和点O₂；穿刺针方向与第一滑杆62旋转轴方向平行，并偏移距离u₁，典型偏移所在面仍为第一偏航轴55和第二偏航轴61所形成的平面内，如图2a所示。

作为一个实施例，所述的第一活动单元7、第二活动单元8的另一种相对位置组装形式，如图19所示；第二支撑台117的螺纹孔距第一偏航轴轴线方向水平距离为u₂，距第一俯仰轴轴线方向竖直距离为u₃；第二俯仰轴孔119的光孔通过支撑圆柱118与第三支撑台122距第二俯仰轴孔的轴线竖直距离为u₄；所述第一俯仰轴116、所述的第一偏航轴114垂直正交于点O₁；第二俯仰轴孔119、第二偏航轴120垂直正交于点O₂；第一滑杆123的旋转轴(翻滚运动中心轴、直线运动方向轴)分别与第一俯仰轴116、第二俯仰轴孔119的轴线垂直相交，但相对点O₁和点O₂偏移d2和d2’，如图19和27a所示。所述的第一、第二位置控制单元，深度控制单元上设计有安装孔位，使得穿刺针方向与第一滑杆123的旋转轴方向平行，并偏移距离u₅，典型偏移所在面仍为第一偏航轴114和第二偏航轴120所形成的平面内，如图20和21所示。

作为一个实施例，所述的第一活动单元7、第二活动单元8的第三种相对位置组装形式，如图26所示，第一支撑台58为L形状，将深度控制单元向内侧安装。所述第一俯仰轴57、所述的第一偏航轴55垂直正交于点O₁；第二俯仰轴孔59、第二偏航轴61垂直正交于点O₂；第一滑杆62的旋转轴(翻滚运动中心轴、直线运动方向轴)与穿刺针轴线重合，两者均分别与第一俯仰轴57、第二俯仰轴孔59的轴线垂直正交于点O₁和点O₂。

作为一个实施例，本发明所述的第二活动单元的第二种结构形式，如图22所示，主要由第五偏航轴124、球面外圈125、球面内圈126等三部分组成；所述的第五偏航轴124一端与所述的第二位置控制单元一端连接，第五偏航轴124相对于第二位置控制单元一端是可转动的，转动中心轴为偏航旋转轴；第五偏航轴124另一端与所述的球面外圈125固定连接；所述的球面外圈125与所述的球面内圈126形成球铰，球面内圈126相对于球面外圈125形成球面运动副；球面内圈126中空设置，形成光孔。球面内圈126与所述的第一滑杆62一端连接，且第一滑杆62相对于球面内圈126的光孔轴线可直线滑动和翻滚转动。作为一个实施例，所述的第一、二位置控制单元、深度控制单元上设计有安装孔位，使第一活动单元7、所述的第二活动单元的第二种结构形式与所述的第一、第二位置控制单元、所述的深度控制单元、第一基座以及穿刺针连接装置6等连接，可实现针定位定姿，如图23、24所示，其中图24为图23中第一、第二活动单元的放大视图。

所述的深度控制单元5，如图11所示，主要由深度控制单元底座68、第三电机63，第一联轴器64，第一丝杆66，螺母座67，光杆65等组成，所述的深度控制单元底座66固定安装在所述的第一支撑台58上；所述的第三电机63固定安装在深度控制单元底座68上。所述的第三电机63与第一丝杆66一端通过第一联轴器64连接，所述的第一丝杆66另一端与深度控制单元底座连接。所述的螺母座67与第一丝杆66、光杆65连接，并可沿着光杆滑动。所述的光杆与第一丝杠轴向平行，两端连接到深度控制单元底座68上，约束螺母座的旋转运动。所述的第三电机63的输出轴转动，通过第一联轴器64带动第一丝杆66旋转，从而带动所述的螺母座67在第一丝杆的长度方向上做前进和后退运动；所述的第三电机的输出轴的两个旋转方向对应所述的螺母座的前进和后退两个动作。

所述的穿刺针连接装置，如图12所示，主要由针连接底座69，穿刺针70，套管71，器械固定装置72等组成。所述的针连接底座69固定安装在所述的深度控制单元的螺母座67或者第一活动单元7上；所述的针连接底座69上设计有两个安装所述的器械固定装置72的定位滑槽，所述的器械固定装置72根据需求安装在所述的针连接底座至少其中之一的定位滑槽上，所述的套管71与所述的器械固定装置72连接，所述的穿刺针70与套管71连接且同轴心。套管71和器械固定装置72根据穿刺针的外形和参数调整结构。所述的深度控制单元5的第三电机63的输出轴的两个旋转方向对应所述的螺母座67的前进和后退两个动作，从而带动所述针连接底座69前进和后退，进而带动所述的穿刺针运动，完成穿刺任务。

所述的床旁定位臂如图13所示，主要由第一固定螺栓73，定位滑块74，第一杆75，第一万向节76，第二杆77，第二固定螺栓78，第三杆79，第二万向节80，第四杆81等组成。所述的定位滑块74可以固定安装在手术床旁的滑动轨道上，定位滑块一侧设计有滑槽，可以方便的与手术床旁的轨道安装连接。所述的定位滑块74上设计有安装孔位，与所述的第一杆75连接，第一杆可以相对于定位滑块上下移动；第一杆75竖直方向的运动通过所述的第一固定螺栓73来限制，所述的定位滑块74连接设计有螺纹孔，与所述的第一固定螺栓73连接；第一固定螺栓的松开运动可以使第一杆相对定位滑块上下移动，第一固定螺栓的拧紧运动可以固定第一杆，使第一杆不能相对定位滑块上下移动。所述的第一万向节76与所述的第一杆75、第二杆77固定连接，所述的第一万向节76至少具有两个自由度，从而允许所述的第二杆在三维空间内的俯仰和滚转运动。所述的第二杆77一端与所述的第三杆79一端均设计有螺纹孔，通过所述的第二固定螺栓78连接，所述的第三杆79相对于第二杆能在三维空间内做俯仰运动，具有至少一个自由度；第三杆调节到合适的位置时，可以通过所述的第二固定螺栓来固定第二杆与第三杆的相对运动，从而保证第三杆处于相对静止状态。所述的第二万向节80与所述的第三杆79、第四杆81固定连接，所述的第二万向节80至少具有两个自由度，从而允许所述的第四杆在三维空间内的俯仰和滚转。所述的第四杆81另一端与所述的多自由度立体定位机器人1固定连接。

本发明的一种立体定位微型手术机器人，其安装过程为：将床旁定位臂安装在手术床旁边合适的位置，通过调整第一固定螺栓来调节床旁定位臂的高度，调节到合适高度时拧紧第一固定螺栓，使第一杆保持相对静止状态。然后通过调整第一万向节、第二固定螺栓、第二万向节使立体定位机器人运动到目标位置上方，医生通过遥操作手柄来操作多自由度立体定位机器人，通过医学影像设备和空间定位系统提供的信息，第一位置控制单元，第二位置控制单元开始控制深度控制单元和穿刺针连接装置在空间中运动，运动到目标病灶位置上方时，深度控制单元控制穿刺针连接装置上的穿刺针向目标病灶位置进针穿刺，根据医学影像信息，穿刺针末端利用空间定位系统建立手术空间的映射关系，实时监控手术器械所在位置，根据系统规划的运动步骤和计算的运动参数，准确地按照规划的运动路径进行运动，精准完成穿刺。

所述立体定位微型手术机器人通过运动学控制，包括正运动学和逆运动学，传动支链环节包括穿刺针、针连接装置、深度控制单元、活动单元和位置控制单元的运动学。

作为一个实施例，本发明提供立体定位微型手术机器人如图20所示的组装形式Ⅴ的运动学的求解，包括正运动学和逆运动学，如图27所示，求解的流程图如图27d所示。

如图27a所示，根据D-H参数法则建立各组成部分坐标系及变量；设全局坐标系为W_O；建立第一位置控制单元基座b₁的坐标系W_b1,第一位置控制单元的基座坐标为

在第一活动单元俯仰偏航正交点O₁处，建立第一位置控制单元的末端输出坐标系为W_o1,第一位置控制单元的末端输出坐标为

第一活动单元偏航旋转坐标系为W_yaw1,第一活动单元俯仰旋转轴坐标系为W_pitch1；穿刺针轴线与第一俯仰轴的公垂线相交于第一滑杆翻滚运动中心轴的点为r₁，穿刺针轴线与第一俯仰轴的公垂线相交于穿刺针轴线的点为p₁，在点r₁建立连接深度控制单元基座的坐标系为W_r1。同理建立第二位置控制单元相关变量：穿刺针轴线与第二俯仰轴的公垂线相交于第一滑杆翻滚运动中心轴的点为r₂，穿刺针轴线与第二俯仰轴的公垂线相交于穿刺针轴线的点为p₂，在点r₂建立连接深度控制单元基座的坐标系为W_r2；r₁与r₂之间距离为n；穿刺针与第一滑杆翻滚运动中心轴的距离为d₁；第一滑杆轴线距离第一活动单元俯仰偏航正交点o₁的距离为d₂，距离第二活动单元俯仰偏航正交点o₂的距离为d'₂，穿刺针末端e的坐标P_e；第一、第二位置控制单元的基座和末端输出的坐标分别为P_b1、P_b2、P_o1、P_o2；深度控制单元的初始位置为h₀，位移为h₁，为h＝h₀+h₁；第一滑杆翻滚运动中心轴的转角为c；s₁₁、s₁₂分别为第一位置控制单元的两个作动器控制变量。

所述立体定位手术机器人组装形式Ⅴ的正运动学计算如下：

S1：从第一位置控制单元侧计算运动学。

第一位置控制单元的基座坐标系W_b1相对于全局坐标系W_O的齐次坐标变换为：T_b1；根据第一位置控制单元的运动学，第一位置控制单元的末端输出坐标系W_o1相对于第一位置控制单元的基座坐标系W_b1的齐次坐标变换为：

T₁＝T_f(s₁₁,s₁₂) (7)

根据图27a，点r₁所在坐标系W_r1在经过第一活动单元的偏航和俯仰运动后，相对于第一位置控制单元的末端输出坐标系W_o1的齐次坐标变换为：

T_h1＝Rot(Y_o1,a₁)Rot(X_yaw1,b₁)Trans(Y_pitch1,d₂) (8)

其中，Rot(Y_o1,a₁)为绕Y_o1轴旋转a₁角度的4×4齐次旋转矩阵；Rot(X_yaw1,b₁)为绕X_yaw1轴旋转b₁角度的4×4齐次旋转矩阵；Trans(Y_pitch1,d₂)表示为沿着Y_pitch1轴平移d₂距离的4×4齐次平移矩阵。

将点r₁所在坐标系W_r1从第一活动单元、第一位置控制单元的第一支链机构计算得到相对于全局坐标系W_O的齐次坐标变换为：T₁₁＝T_b1T₁T_h1。

S2：从第二位置控制单元侧计算运动学。

如图27b所示，W_b2相对于W_O的齐次坐标变换为：T_b2

根据第二位置控制单元的运动学，W_o2相对W_b2的齐次坐标变换为：

T₂＝T_f(s₂₁,s₂₂) (9)

W_r2相对于W_o2的齐次坐标变换为：

T_h2＝Rot(Y_o2,a₂)Rot(X_yaw2,b₂)Trans(Y_pitch2,d_2') (10)

其中，Rot(Y_o2,a₂)为绕Y_o2轴旋转a₂角度的4×4齐次旋转矩阵；Rot(X_yaw2,b₂)为绕X_yaw2轴旋转b₂角度的4×4齐次旋转矩阵；Trans(Y_pitch2,d_2')表示为沿着Y_pitch2轴平移d_2'距离的4×4齐次平移矩阵。

当点r₂所在的坐标系W_r2绕第一滑杆翻滚运动中心轴转动c角度，再沿着第一滑杆运动中心轴移动距离n，W_r2坐标系与W_r1坐标系重合，则坐标变换为：

T_r2r1＝Rot(X,c)Trans(X,-n) (11)

则将坐标系W_r1经过坐标系W_r2，从深度控制单元、第二活动单元、第二位置控制单元的第二支链计算得到的全局坐标变换为：T₁₂＝T_b2T₂T_h2T_r2r1

S3：同一坐标系经过两个支链的变换后，得到等式方程(两种方程均可)：

或者

利用A1-A3迭代法求解，令公式(12)的6个变量为a₁、b₁、a₂、b₂、n、c＝x₁、x₆；

A1：给定X⁰＝[x₁、x₆]'初始值；

A2：根据公式(12)分别计算F＝[f₁、f₉]'和T＝[t₁、t₃]'；

A3:如果f_i<e₁和t_j<e₂，计算完成，否则按照如下计算迭代值，带入所述A2循环。

式中，

a与β为给定的收敛系数，非负数，和为1。

S4：计算穿刺针上坐标系。

如图27c所示，根据D-H参数法则建立各组成部分坐标系，穿刺针轴线分别与第一、第二俯仰轴的公垂线相交于穿刺针轴线点p₁和点p₂，刺针末端点为e，分别以该三个点为原点建立连接在穿刺针上的坐标系W_p1、W_p2、W_e。

W_p1相对W_r1的齐次坐标变换为：T_p1＝Trans(Y_r1,d₁)，W_p2相对W_p1的齐次坐标变换为：T_p1p2＝Trans(Z_p1,-n)，W_e相对W_p1的齐次坐标变换为：T_e＝Trans(Z_p1,-h)。

W_e在全局坐标系W_O的齐次坐标变换为：T_We＝T_b1T₁T_h1T_p1T_e。

W_e下的e点坐标P_e在全局坐标系W_O下的坐标为：P_we＝T_WeP_e，从T_We得到的旋转矩阵R_e为穿刺针的姿态矩阵。

所述的立体定位手术机器人组装形式Ⅴ的逆运动学。

H1:设定a₁、b₁、a₂、b₂、n、c六个被动关节对应的变量以及s₁₁、s₁₂、s₂₁、s₂₂四个作动器(主动关节)的控制变量为未知变量，组合形成向量X＝[x_i]_10×1。

已知目标器官中病灶g_i的坐标为P_gi(x_i,y_i,z_i),(i＝1,2,3....)，穿刺点t_j的坐标为P_tj(x_j,y_j,z_j),(j＝1,2,3......),则穿刺针所在的空间直线方程如公式(1)

点p₁、p₂的坐标系W_p1和W_p2在全局坐标系W_O的齐次坐标变换分别为：

T_Wp1＝T_b1T₁T_h1T_p1

T_Wp2＝T_Wp1T_p1p2 (13)

点p₁、p₂分别在坐标系W_p1、W_p2下的坐标分别为p_p1、p_p2，则点p₁、p₂在全局坐标系W_O下的坐标为：

p_w1＝T_wp1p_p1

p_w2＝T_wp2p_p2 (14)

H2:求解变量X。

B1：点p₁(p₂)位于穿刺针轴线上，所以满足空间直线方程(1)，因此，将方程(14)代入方程(1)，形成4个独立的方程；

B2：再根据S1、S2、S3和S4方法得到公式(12)，计算12个方程。

B3：同理参照A1-A3迭代法求解，得到X，X向量后四个元素为s₁₁、s₁₂、s₂₁、s₂₂。

H3：根据正运动学计算P_e，深度控制单元控制穿刺针运动的行程为：

所述的立体定位微型手术机器人如图4a、b所示的第二种组装形式的运动学，第一基座11安装在深度控制单元5上，如图4a所示，则h＝h₀。第一、第二位置控制单元的基座坐标系W_b1、W_b2相对于全局坐标系W_O跟随深度控制单元的直线滑动行程h₁发生沿Z_W轴的平移，则齐次坐标变换分别为：T_b1＝Trans(Z_W,h₁)T_b10和T_b2＝Trans(Z_W,h₁)T_b20，其中，T_b10、T_b20为初始状态值。

正运动学计算方法和立体定位机器人组装形式Ⅴ的正运动学一样。

逆运动学计算方法为，设定a₁、b₁、a₂、b₂、n、c六个被动关节对应的变量以及s₁₁、s₁₂、s₂₁、s₂₂四个作动器(主动关节)以及直线滑动行程h₁的控制变量为未知变量，组合形成向量X＝[x_i]_11×1。穿刺针末端点e的坐标P_we＝P_gi，其中P_we＝T_WeP_e，T_We可由公式T_We＝T_b1T₁T_h1T_p1T_e得到表达式；再联合逆运动学H2步骤中的B1、B2中的16个方程，形成19个方程；同理参照A1-A3迭代法求解，得到X，X向量后五个元素为s₁₁、s₁₂、s₂₁、s₂₂、h₁。

如图28所示，所述的位置控制单元A的运动学，所述的末端输出坐标点o₁(o₂)和第一、第二电机的控制关系可通过正逆运动学计算，利用正运动学计算末端输出坐标点o₁(o₂)和工作空间，利用逆运动学计算得到末端输出坐标点o₁(o₂)所需的第一、第二电机的伸缩量；位置控制单元A的正运动学计算方法为，先计算S1，再计算S2，然后计算S3；逆运动学计算方法为，先计算S3，再计算S1和S2。

S1:建立第一电机轴的伸缩量L₁与第一关节的弯曲角度θ₁之间的解析关系式；

以主轴与外壳的交点A为原点(坐标P_A＝(0,0,0,1)’，以下坐标矢量末位补1，与齐次坐标变换矩阵对齐)，第一电机轴伸缩方向为X轴，手指工作空间所在平面为XY面，垂直该面为Z轴，建立固定到位置控制单元外壳的右手坐标系{O_A}～XYZ；B点为第五连杆、第六连杆一端与第一转接杆的交点；初始时，B点距离A点在X轴方向的距离为L_1init，横坐标y_B固定不变，则B点坐标：

P_B＝(x_B,y_B,z_B,1)'＝(L_1init+L₁,y_B,0,1)' (15)

C点为第五连杆、第六连杆另一端与第一关节输出杆的交点；在C点处绕着垂直XY平面的Z轴的旋转角度为θ₁，沿着X_AC轴平移L_AC，得到C点坐标：

P_C＝(x_C,y_C,z_C,1)'＝Rot(Z,θ₁)·Trans(X_AC,L_AC)P_A (16)

以AC为X轴，A为原点建立的连接到第一关节输出杆的随动坐标系，X_AC为随动坐标系的X轴；L_AC为AC杆长。BC间距离固定不变，则得到约束方程：

将方程(15)和(16)带入方程(17)，得到方程(18)：f(L₁,θ₁)＝0。

正运动学中，方程(18)解析解为，根据L₁计算θ₁:

逆运动学中，方程(18)解析解为，根据θ₁计算L₁:

根据第一电机轴的伸缩量L₁的实际取值，对方程(20)的正负号进行取舍,在X轴方向上，A介于B、C之间时，取负号。

S2：根据步骤S1中的L₁和θ₁，建立第二电机轴的伸缩量L₂与第二关节的弯曲角度θ₂之间的解析关系式；D点为第二转接杆和第一连杆、第二连杆一端的交点，P_D为D点的坐标，初始时，D点距离A点在X轴方向的距离为L_2init，横坐标y_D固定不变，则D点坐标：

P_D＝(x_D,y_D,z_D,1)'＝(L_2init+L₂,y_D,0,1)' (21)

F点为第一连杆、第二连杆与第三连杆、第四连杆和曲杆的交点，P_F为F点的坐标，L_DF为D点与F点之间的坐标距离，a₁为AF绕A点的旋转角度；与方程(16)同理，得到F点坐标：

P_F＝(x_F,y_F,z_F,1)'＝Rot(Z,a₁)·Trans(X_AF,L_AF)P_A (22)

与方程(17)同理，DF间距离固定不变，则得到约束方程：

将方程(21)和(22)带入方程(23)，得到方程(24)：f(L₂,a₁)＝0；与方程(19)和方程(20)同理，分别得到正逆运动学中L₂和a₁的计算关系。

正运动学中，方程(24)解析解为，根据L₂计算a₁:

逆运动学中，方程(24)解析解为，根据a₁计算L₂:

根据第二电机轴的伸缩量L₂的实际取值，对方程(26)的正负号进行取舍，在X轴方向上，A介于D、F之间时，取负号。

H点为第一关节输出杆与第二关节输出杆的交点，P_H为H点的坐标，G点为曲杆另一端与第七连杆、第八连杆和第九连杆、第十连杆的交点，P_G为G点的坐标，L_GF为G点与F点之间的坐标距离，I点为第九连杆、第十连杆和第二关节输出杆的交点，a₂为GH绕A点旋转角度。

H点坐标为：

P_H＝(x_H,y_H,z_H,1)'＝Rot(Z,θ₁)·Trans(X_AH,L_AH)P_A (27)

G点坐标为：

P_G＝(x_G,y_G,z_G,1)'＝Rot(Z,a₂)·Trans(Y_GH,L_GH)P_H (28)

与方程(17)同理，GF间距离固定不变，则得到约束方程：

HI相对AH转角为：

θ₂＝a₂+∠GHI (30)

将方程(22)、(27)、(28)和(30)带入方程(29)，得到方程(31)：f(a₁,θ₁,θ₂)＝0。

正运动学中，方程(31)解析解为，根据a₁、θ₁计算θ₂(θ₂＝a₂+∠GHI):

逆运动学中，方程(31)解析解为方程(33)，根据θ₁、θ₂(a₂＝θ₂-∠GHI)，计算a₁，:

再基于方程(26)计算L₂。

S3：计算位置控制单元A运动学中θ₁、θ₂和末端输出K的坐标P_K＝(x_K,y_K,z_K,1)'(z_K为固定值)的解析式。

正运动学中，根据θ₁、θ₂计算P_K：

逆运动学中，根据P_K计算θ₁、θ₂：

所述Rot(Z,θ)为绕Z轴旋转θ角度的4×4齐次旋转矩阵；所述Trans(X,d)为沿着X轴平移d距离的4×4齐次平移矩阵。

所述方程(18)、(24)、(31)的另一种解算方法为将已知量带入，未知量在给定初始值后，利用迭代牛顿欧拉法、龙格库塔法或者非线性优化方法得到数值解。

所述的位置控制单元B的运动学。如图29a和图29b所示，其中图29b为位置控制单元B正逆运动学求解的流程图，所述的位置控制单元B的运动学，利用D-H参数法建立各组成部分坐标系。设全局坐标系为W_b1，第四电机基座处的坐标系为W_s120，第十一连杆一端与第四电机交点处的坐标系为W_s12，第五电机基座处的坐标系为W_s110，第十四连杆一端与第五电机交点处的坐标系为W_s11，第十轴处的坐标系为W_N，第八轴与第一连接底座交点处的坐标系为W_K，第一连接底座上的坐标系为W_o1，第九轴处的坐标系为W_M；设第十一连杆长度为l₁,第十二连杆长度为l₂,第十三连杆长度为l₃,第十四连杆长度为l₄；第十轴被动关节角度为nn，第九轴被动关节角度为mm；第五电机的输出转角为s₁₁，第四电机的输出转角为s₁₂。

则第十轴所在坐标系W_N相对第四电机基座坐标系W_s120的齐次坐标变换为：T_N＝Rot(Z_s120,s₁₂)Trans(Y₁₂,l₁)Rot(Z_N,nn)，第九轴所在坐标系W_M相对第五电机基座坐标系W_s110的齐次坐标变换为：T_M＝Rot(Z_s110,s₁₁)Trans(Y₁₁,l₄)Rot(Z_M,mm)，其中，Rot(Z,θ)表示为绕Z轴旋转θ角度的4×4齐次旋转矩阵；Trans(X,d)表示为沿着X轴平移d距离的4×4齐次平移矩阵。因为位置控制单元仅在XY平面内运动，上述矩阵展开后可以得到两个方程组成的方程组，记为：B₁。坐标系W_K相对坐标系W_I的齐次坐标变换为：T_NK＝Trans(Y_K,l₂)。假设第一连接底座通过紧固件与第十二连杆(第十三连杆)刚性连接，则利用坐标系W_o1相对坐标系W_K的齐次坐标变换为定值：T_Ko1。

第四电机所在坐标系W_s120相对全局坐标系W_b1的齐次坐标变换为：T_s12，第五电机所在坐标系W_s110相对全局坐标系W_b1的齐次坐标变换为：T_s11，第八轴在XY平面的点在全局坐标系W_b1的坐标为p_k，点p_k的坐标可分别从左右两侧的支链求得，左侧支链计算方程B₂₁为：p_k＝T_s12T_N[0,l₂,0,1]'，右侧支链计算方程B₂₂为：p_k＝T_s11T_M[0,l₃,0,1]'。

位置控制单元B的正运动学，令方程B₂₁与方程B₂₂相等，则矩阵展开后得到XY平面上的2个方程(两边X坐标相等，两边Y坐标相等)，记为方程组B₃。正运动学s₁₁、s₁₂为已知的，nn，mm两个变量未知，通过解方程组B3，可以求得nn和mm，继而可以求出T_N，则坐标系W_o1相对全局坐标系W_b1的齐次坐标变换为：T₁＝T_s12T_NT_NKT_ko1。

位置控制单元B的逆运动学，坐标系W_o1相对坐标系W_b1的坐标变换T₁为已知的，T_N＝(T_s12)^-1T₁(T_NKT_ko1)^-1，根据方程组B₁计算nn和s₁₂。根据方程B₂₁，计算点p_k的坐标，将求得的点p_k的坐标带入方程B₂₂，得到两个独立方程，从而求解s₁₁和mm两个位置变量，从而求得第五电机和第六电机的转角s₁₁和s₁₂。

所述的位置控制单元C的运动学。如图30a和图30b所示，其中图30b为位置控制单元C正逆运动学求解的流程图，所述的位置控制单元C的运动学，利用D-H参数法建立各组成部分坐标系。设全局坐标系为W_b1，第七电机基座处的坐标系为W_s120，第六电机基座处的坐标系为W_s110，第十九连杆一端与第二支撑滑块滑动轴相交点为N，在N点处的坐标系为W_N，第十七连杆一端与第一支撑滑块滑动轴相交点为M，在点M处的坐标系为W_G，在第二连接底座上的坐标系为W_o1；第六电机的输出转角经过丝杠单元后直线滑动行程为s₁₁，第七电机的输出转角经过丝杠单元后直线滑动行程为s₁₂；第二支撑滑块的初始位置为l₁，第一支撑滑块的初始位置为l₄，第十九连杆相对第二支撑滑块滑动方向的转角为nn，第十七连杆相对第一支撑滑块滑动方向的转角为mm；第十九连杆的杆长为l₂，第十七连杆的杆长为l₃，第二连接底座与第十七连杆，第十九连杆的交点为K，交点K处与第十九连杆固定连接的坐标系为W_K1，与第二连接底座固定连接的坐标系为W_K。

则坐标系W_N相对位置控制单元的第七电机基座坐标系W_s120的坐标变换为：T_N＝Trans(Y_s120,l₁+s₁₂)Rot(Z_s12,nn)，坐标系W_G相对位置控制单元的第六电机基座坐标系W_s110的坐标变换为：T_M＝Trans(Y_s110,l₄+s₁₁)Rot(Z_s11,mm)。因为位置控制单元C的运动仅在XY平面内运动，上述矩阵展开后可以得到两个方程组成的方程组为：B₁。坐标系W_K1相对坐标系W_N的齐次坐标变换为：T_NK1＝Trans(Y_N,l₂)，因为坐标系W_o1、W_K均与第二连接底座固定连接，所以坐标系W_o1相对坐标系W_K的齐次坐标变换为定值：T_Ko1。由于位置控制单元C采用铰接的平行四边形连杆机构，所以坐标系W_K的姿态是不变的。

坐标系W_s120相对全局坐标系W_b1的坐标变换为：T_s12，坐标系W_s110相对全局坐标系W_b1的坐标变换为：T_s11。假设点K在全局坐标系W_b1的坐标为p_k，则p_k的坐标可分别从左右两侧支链计算求得。左侧支链计算方程B₂₁为：p_k＝T_s12T_N[0,l₂,0,1]'；右侧支链计算方程B₂₂为：p_k＝T_s11T_M[0,l₂,0,1]'。

位置控制单元C的正运动学，令方程B₂₁与方程B₂₂相等，则矩阵展开后得到XY平面上的2个方程(两边X坐标相等，两边Y坐标相等)，记为方程组B₃。当求解位置控制单元C的正运动学时s₁₁和s₁₂是已知的，nn和mm两个变量未知，通过求解方程组B₃，可求得nn，mm，继而可以求解出T_N。则坐标系W_k1相对全局坐标系W_b1的坐标变换为：T_b1k1＝T_s12T_NT_NK1＝[R_3×3；t_3×1；0,0,0,1]，令T_b1k1中的R＝[0]_3×3，从而求解出T₁＝T_b1k1T_ko1。或者根据s₁₁和nn，坐标系W_K相对坐标系W_s120在Y_s120轴方向平移距离y＝l₁+s₁₂+l₂cos(nn)，在X_s120轴方向平移距离x＝l₂sin(nn)，从而求解出T₁＝T_s120Trans(Y_s120,y)Trans(X_s120,x)T_ko1。

位置控制单元C的逆运动学，坐标系W_o1相对全局坐标系W_b1的坐标变换T₁为已知的，坐标系W_K相对坐标系W_s120的坐标变换为：T_s120k＝(T_s12)^-1T₁(T_ko1)^-1＝[R_3×3；t_3×1；0,0,0,1]，则

s₁₂＝t(2)-l₂cos(nn)；又坐标系W_s120和坐标系W_s110的Y轴之间的距离为d₁，则

s₁₁＝t(2)-l₄-l₃cos(mm)；综上，可求得逆解滑动行程s₁₁和s₁₂。

所述第一活动单元、第二活动单元的运动学计算优化，可根据第一活动单元7、第二活动单元8的相对位置组装形式，通过判断穿刺针、滑杆与翻滚运动中心轴是否重合，简化运动学模型。

所述的第一活动单元7、第二活动单元8的相对位置组装形式B1的运动学中模型参数，d₂＝d_2'。

所述的第一活动单元7、第二活动单元8的相对位置组装形式B3的运动学模型参数，d₁＝d₂＝d_2'＝0。

所述的第二活动单元D的运动学模型参数，d_2'＝0。

综上，本发明一种立体定位手术机器人系统，并提供了两种优选的立体定位机器人组装形式和三种位置控制单元。从手术机器人的结构来看，该机器人体积非常小，非常方便的携带或者移动到想要的位置。立体定位机器人具有至少5个自由度，通过两个位置控制单元来给末端穿刺针提供至少4个自由度的定位定姿，通过深度控制单元提供进给，大大增加了末端穿刺针的灵活度，能使穿刺针精准的运动到目标靶点位置，完成穿刺。

该立体定位手术机器人体积小，安装方便，末端执行机构具有至少5个自由度，能达到空间所需的位置，末端执行机构能配合不同的手术器械(例如穿刺针、活检针、消融针等)来完成不同的手术，与医学影像设备(例如B超、核磁、CT等)为医生提供实时的影像信息，保证手术的顺利完成。

Claims (13)

1.一种立体定位微型手术机器人，包括多自由度立体定位机器人和床旁定位臂；其特征在于：所述的多自由度立体定位机器人，包括第一位置控制单元、第二位置控制单元、深度控制单元、穿刺针连接装置、第一活动单元、第二活动单元以及第一基座；第一基座提供安装基础，第一、第二位置控制单元分别具备2个主动作动自由度，分别以串联、并联形式组成，末端位置的工作空间为平面，深度控制单元具备1个主动作动自由度的直线进给，第一、第二活动单元分别具有至少俯仰、偏航的被动自由度，两者之间还具有1个被动自由度的翻滚运动和1个被动自由度的直线运动，穿刺针连接装置固定针；

所述的第一位置控制单元、第二位置控制单元通过第一活动单元、第二活动单元以并联机构形式与深度控制单元、穿刺针连接装置连接，从而允许所述的穿刺针连接装置在三维空间内的定位以及俯仰、翻转定姿运动，多自由度立体定位机器人至少具有5个自由度，使末端穿刺针运动到空间目标位置；

所述的第一位置控制单元、第二位置控制单元一端基座固定安装在所述的第一基座上，另一端输出分别与第一活动单元、第二活动单元连接；所述的第一活动单元输出与所述的深度控制单元基座固定连接；所述的穿刺针连接装置固定安装在所述的深度控制单元的输出上。

2.一种立体定位微型手术机器人，包括多自由度立体定位机器人和床旁定位臂；其特征在于：所述的多自由度立体定位机器人，包括第一位置控制单元、第二位置控制单元、深度控制单元、穿刺针连接装置、第一活动单元、第二活动单元以及第一基座；第一基座提供安装基础，第一、第二位置控制单元分别具备2个主动作动自由度，分别以串联、并联形式组成，末端位置的工作空间为平面，深度控制单元具备1个主动作动自由度的直线进给，第一、第二活动单元分别具有至少俯仰、偏航的被动自由度，两者之间还具有1个被动自由度的翻滚运动和1个被动自由度的直线运动，穿刺针连接装置固定针；

所述的第一位置控制单元、第二位置控制单元通过第一活动单元、第二活动单元以并联机构形式与深度控制单元、穿刺针连接装置连接，从而允许所述的穿刺针连接装置在三维空间内的定位以及俯仰、翻转定姿运动，多自由度立体定位机器人至少具有5个自由度，使末端穿刺针运动到空间目标位置；

第一基座固定安装在深度控制单元的输出上；第一位置控制单元、第二位置控制单元的一端固定安装在第一基座上，另一端输出分别与第一活动单元、第二活动单元的一端连接；穿刺针连接装置固定安装在第一活动单元的输出上。

3.根据权利要求1或2所述的立体定位微型手术机器人，其特征在于：所述的第一位置控制单元、第二位置控制单元均采用模块化设计，具有相同的结构，每一个位置控制单元的各个主动作动自由度通过作动器直接驱动，或者通过作动器前置安装在基座而后通过连杆传动来驱动；每一个位置控制单元均由第一关节、第二关节、驱动系统和末端连杆组成；每个关节均由单独的电机驱动，传动机构采用连杆传动，每个关节在电机作用下可转动，控制末端的二自由度位置；每一个位置控制单元各关节铰接处采用双轴连杆设计，以增加第一位置控制单元的定位精度；第二关节的一个连杆设计为弯曲杆，以提高第二关节的运动范围；

所述第一关节由第一关节输出杆、第一电机通过第一转接杆、第五连杆、第六连杆、主轴彼此铰接构成曲柄滑块机构，将第一电机的上下竖直运动转换为第一关节的转动；

所述第二关节由第二电机通过主轴绕过第一关节并通过第三连杆、第四连杆、曲杆、第七连杆、第八连杆和第一关节彼此铰接组成的四连杆机构以及第九连杆、第十连杆将第二电机的竖直上下运动转换为第二关节输出杆的转动；

所述驱动系统，电机与传动机构相结合，组成曲柄滑块机构，使其驱动电机可以保持竖直方向运动；

所述末端连杆，在第二关节输出杆上设计了两个可调节运动范围的档位调节孔。

4.根据权利要求1或2所述的立体定位微型手术机器人，其特征在于：所述的第一位置控制单元、第二位置控制单元的另一种结构形式为：通过两个电机分别驱动丝杆上的两个螺母沿直线平行运动，通过两个平行四连杆机构带动末端的第二连接底座在平面内进行水平和竖直运动，且第二连接底座的姿态在运动过程中与水平面保持平行不变。

5.根据权利要求1或2所述的立体定位微型手术机器人，其特征在于：所述的第一活动单元和第二活动单元各自俯仰、偏航的旋转轴正交，第一、第二活动单元之间还具备1个被动自由度的翻滚运动和1个被动自由度的直线运动，翻滚运动中心轴与直线运动方向轴或者称中心轴平行，并且与第一、第二活动单元的俯仰旋转轴正交，与第一、第二活动单元的偏航旋转轴相交；第一、第二活动单元为末端提供至少5个自由度的被动活动度，

所述的第一活动单元的一种结构形式，由第一偏航轴，第一U型块，第一俯仰轴，第一支撑台组成，所述的第一偏航轴一端与第一位置控制单元一端连接，相对于第一位置控制单元一端可转动，转动中心为第一偏航旋转轴；所述的第一偏航轴另一端与第一U型块固定连接；所述的第一俯仰轴与第一U型块连接，相对于第一U型块可转动，转动中心轴为第一俯仰旋转轴；所述的第一支撑台与第一俯仰轴固定连接，并且沿着翻滚运动中心轴方向在中间设计有螺纹孔；

所述的第二活动单元的一种结构形式，由第二俯仰轴孔，第二U型块，第二偏航轴组成；所述的第二偏航轴与第二位置控制单元一端连接，同时与第二U型块固定连接；所述的第二俯仰轴孔与第二U型块连接，相对于第二U型块可转动，转动中心轴为第二俯仰旋转轴；第二俯仰轴孔沿着翻滚运动中心轴方向在中间设计有光孔；所述的第一活动单元与第二活动单元通过一个第一滑杆连接，且第一滑杆一端与第一俯仰轴连接，另一端与第二俯仰轴孔的光孔配合；第一滑杆相对于第二俯仰轴孔能在第一滑杆轴线方向相对直线滑动、绕第一滑杆周向相对翻滚转动。

6.根据权利要求5所述的立体定位微型手术机器人，其特征在于：所述的第一活动单元、第二活动单元的一种相对位置组装形式为：第一俯仰轴、第一偏航轴垂直正交于点O₁；第二俯仰轴孔、第二偏航轴垂直正交于点O₂；第一滑杆旋转轴分别与第一俯仰轴、第二俯仰轴孔的轴线垂直正交于点O₁和点O₂；穿刺针方向与第一滑杆旋转轴方向平行，并偏移距离u₁，典型偏移所在面仍为第一偏航轴和第二偏航轴所形成的平面内。

7.根据权利要求5所述的立体定位微型手术机器人，其特征在于：所述的第一活动单元、第二活动单元的第三种相对位置组装形式为：第一支撑台为L形状，将深度控制单元向内侧安装，所述第一俯仰轴、所述的第一偏航轴垂直正交于点O₁；第二俯仰轴孔、第二偏航轴垂直正交于点O₂；第一滑杆的旋转轴与穿刺针轴线重合，两者均分别与第一俯仰轴、第二俯仰轴孔的轴线垂直正交于点O₁和点O₂。

8.根据权利要求1或2所述的立体定位微型手术机器人，其特征在于：所述的第一活动单元和第二活动单元各自俯仰、偏航的旋转轴正交，第一、第二活动单元之间还具备1个被动自由度的翻滚运动和1个被动自由度的直线运动，翻滚运动中心轴与直线运动方向轴或者称中心轴平行，并且与第一、第二活动单元的俯仰旋转轴正交，与第一、第二活动单元的偏航旋转轴相交；第一、第二活动单元为末端提供至少5个自由度的被动活动度；

所述的第一活动单元，由第一偏航轴，第一U型块，第一俯仰轴，第一支撑台组成，所述的第一偏航轴一端与第一位置控制单元一端连接，相对于第一位置控制单元一端可转动，转动中心为第一偏航旋转轴；所述的第一偏航轴另一端与第一U型块固定连接；所述的第一俯仰轴与第一U型块连接，相对于第一U型块可转动，转动中心轴为第一俯仰旋转轴；所述的第一支撑台与第一俯仰轴固定连接，并且沿着翻滚运动中心轴方向在中间设计有螺纹孔；

所述的第二活动单元的有第二种结构形式，包括第五偏航轴、球面外圈、球面内圈三部分；所述的第五偏航轴一端与第二位置控制单元一端连接，第五偏航轴相对于第二位置控制单元一端可转动，转动中心轴为偏航旋转轴；第五偏航轴另一端与球面外圈固定连接；所述的球面外圈与球面内圈形成球铰；球面内圈中空设置，形成光孔；球面内圈与第一滑杆一端连接，且第一滑杆相对于球面内圈的光孔的轴线可直线滑动和翻滚转动。

9.根据权利要求1或2所述的立体定位微型手术机器人，其特征在于：所述的深度控制单元，包括深度控制单元底座，第三电机，第一联轴器，第一丝杆，螺母座，光杆，所述的深度控制单元底座固定安装在第一支撑台上；所述的第三电机固定安装在深度控制单元底座上；所述的第三电机与第一丝杆一端通过第一联轴器连接；所述的第一丝杆另一端与深度控制单元底座连接；所述的螺母座与第一丝杆、光杆连接，并可沿着光杆滑动；所述的光杆与第一丝杠轴向平行，两端连接到深度控制单元底座上，约束螺母座的旋转运动；所述的第三电机的输出轴转动，通过第一联轴器带动第一丝杆旋转，从而带动所述的螺母座在第一丝杆的长度方向上做前进和后退运动；所述的第三电机的输出轴的两个旋转方向对应所述的螺母座的前进和后退两个动作。

10.根据权利要求1或2所述的立体定位微型手术机器人，其特征在于：所述的穿刺针连接装置，包括针连接底座，穿刺针，套管，器械固定装置；所述的针连接底座固定安装在所述的深度控制单元的螺母座或者第一活动单元上；所述的针连接底座上设计有两个安装器械固定装置的定位滑槽，所述的器械固定装置根据需求安装在针连接底座至少其中之一的定位滑槽上，所述的套管与器械固定装置连接，所述的穿刺针与套管连接且同轴心；所述的深度控制单元的第三电机的输出轴的两个旋转方向对应螺母座的前进和后退两个动作，从而带动针连接底座前进和后退，进而带动穿刺针运动，完成穿刺任务。

11.一种如权利要求1或2所述的立体定位微型手术机器人的设计方法，其特征在于：所述第一位置控制单元、第二位置控制单元的末端工作空间W₁、W₂、深度控制单元控制穿刺针的行程L，由精准穿刺手术的工作方式确定；病灶点与体表切口决定第一、第二位置控制单元和深度控制单元的设计目标；由病灶(g_i)和穿刺点(t_j)计算针路，由针路在第一、二位置控制单元的末端工作空间所在平面XY₁、XY₂上的交集计算第一位置控制单元、第二位置控制单元的末端工作空间W₁、W₂；由病灶(g_i)到穿刺点(t_j)距离中的最大值计算深度控制单元控制穿刺针的行程L；具体过程如下：

设计时假设穿刺针与翻滚运动中心轴重合，建立全局坐标系为O_W，假设目标器官所在空间为W_o，目标器官中的病灶为g_i(i＝1,2,3…)；体表切口的分布空间为W_t，体表切口的穿刺点为t_j(j＝1,2,3…)；第一位置控制单元的末端工作空间为W₁，W₁所在平面为XY₁，第二位置控制单元的末端工作空间为W₂，W₂所在平面为XY₂，平面XY₁和平面XY₂与全局坐标系O_W的X_WY_W平面平行，W₁与W₂之间的距离为d，W₂与W_t之间的距离为h，W_t与W_o之间的距离为k，l_j为穿刺针所在直线；

设计时假设穿刺针轴线与翻滚运动中心轴重合，第一、第二位置控制单元末端与穿刺针连接点分别为O_d1、O_d2，分别在XY₁、XY₂平面内；穿刺针沿穿刺方向的进给深度即穿刺针的行程L由深度控制单元控制；为实现针路覆盖器官内尽量多的病灶(g_i)以及穿刺针精准穿中病灶，并避开危险组织器官，同时减少切口数量，根据手术的样式确定合理的穿刺点(t_j)位置和数量；

目标器官中的病灶g_i的坐标为P_gi(x_i,y_i,z_i)(i＝1,2,3.....)，穿刺点t_j的坐标为P_tj(x_j,y_j,z_j)(j＝1,2,3.....)，则l_j所在的空间直线方程为：

XY₁、XY₂平面与全局坐标系O_wX_wY_w平面平行，则XY₁、XY₂的平面方程分别为：

z₁＝E₁ (2)

z₂＝E₁-d (3)

其中，E₁为全局坐标系中平面XY₁与Z_W轴交点的z坐标值；

设穿刺针所在直线方程l_j与XY₁、XY₂平面的交点分别为O_1j、O_2j，则联立方程(1)、(2)可以求得交点O_1j的坐标为：

联立方程(1)、(3)可以求得交点O_2j的坐标为：

基于公式(4)和(5)，通过目标器官中的所有病灶坐标P_gi(x_i,y_i,z_i)(其中i＝1,2,3…)与所有穿刺点t_j的坐标P_tj(x_j,y_j,z_j)(其中j＝1,2,3…)，可以求得所有的与平面XY₁和XY₂的交点O_1j和O_2j的坐标，假设所有的点O_1j坐标分布空间为W_s1，所有的点O_2j坐标分布空间为W_s2，空间点分布W_s1和W_s2分别为第一位置控制单元、第二位置控制单元的末端O₁、O₂的工作空间设计输入，则W_s1∈W₁和W_s2∈W₂，根据此设计所述的第一位置控制单元、第二位置控制单元的末端O₁、O₂的工作空间W₁和W₂应分别不小于W_s1和W_s2，即W₁≥W_s1和W₂≥W_s2，这样第一位置控制单元和第二位置控制单元才能控制穿刺针运动到体表所有穿刺点位置，完成穿刺任务；

目标器官中的病灶坐标P_gi(x_i,y_i,z_i)(i＝1,2,3…)与穿刺点t_j的坐标P_tj(x_j,y_j,z_j)(j＝1,2,3.....)之间的最大距离为：

为使穿刺针能到达目标器官中所有的病灶位置，所述的深度控制单元控制穿刺针的行程L应不小于L_P,即L≥L_P，这样深度控制单元才能控制穿刺针到达所有的病灶位置。

12.一种如权利要求1所述的立体定位微型手术机器人的运动学控制方法，其特征在于：包括正运动学和逆运动学，传动支链环节包括穿刺针、针连接装置、深度控制单元、活动单元和位置控制单元的运动学；

其中，正运动学控制具体过程如下：

根据D-H参数法则建立各组成部分坐标系及变量；设全局坐标系为W_O；建立第一位置控制单元基座b₁的坐标系W_b1；在第一活动单元俯仰偏航正交点O₁处，建立第一位置控制单元的末端输出坐标系为W_o1，第一位置控制单元的末端输出坐标为P_o1，第一活动单元偏航旋转坐标系为W_yaw1,第一活动单元俯仰旋转轴坐标系为W_pitch1；穿刺针轴线与第一俯仰轴的公垂线相交于第一滑杆翻滚运动中心轴的点为r₁，穿刺针轴线与第一俯仰轴的公垂线相交于穿刺针轴线的点为p₁，在点r₁建立连接深度控制单元基座的坐标系为W_r1；同理建立第二位置控制单元相关变量：穿刺针轴线与第二俯仰轴的公垂线相交于第一滑杆翻滚运动中心轴的点为r₂，穿刺针轴线与第二俯仰轴的公垂线相交于穿刺针轴线的点为p₂，在点r₂建立连接深度控制单元基座的坐标系为W_r2；r₁与r₂之间距离为n；穿刺针与第一滑杆翻滚运动中心轴的距离为d₁；第一滑杆轴线距离第一活动单元俯仰偏航正交点O₁的距离为d₂，距离第二活动单元俯仰偏航正交点O₂的距离为d'₂，穿刺针末端e的坐标P_e；第一、第二位置控制单元的基座和末端输出的坐标分别为P_b1、P_b2、P_o1、P_o2；深度控制单元的初始位置为h₀，位移为h₁，为h＝h₀+h₁；第一滑杆翻滚运动中心轴的转角为c；s₁₁、s₁₂分别为第一位置控制单元的两个作动器控制变量；

所述立体定位手术机器人的正运动学计算如下：

S1：从第一位置控制单元侧计算运动学

第一位置控制单元的基座坐标系W_b1相对于全局坐标系W_O的齐次坐标变换为：T_b1；根据第一位置控制单元的运动学，第一位置控制单元的末端输出坐标系W_o1相对于第一位置控制单元的基座坐标系W_b1的齐次坐标变换为：

T₁＝T_f(s₁₁,s₁₂) (7)

点r₁所在坐标系W_r1在经过第一活动单元的偏航和俯仰运动后，相对于第一位置控制单元的末端输出坐标系W_o1的齐次坐标变换为：

T_h1＝Rot(Y_o1,a₁)Rot(X_yaw1,b₁)Trans(Y_pitch1,d₂) (8)

其中，Rot(Y_o1,a₁)为绕Y_o1轴旋转a₁角度的4×4齐次旋转矩阵；Rot(X_yaw1,b₁)为绕X_yaw1轴旋转b₁角度的4×4齐次旋转矩阵；Trans(Y_pitch1,d₂)表示为沿着Y_pitch1轴平移d₂距离的4×4齐次平移矩阵；

将点r₁所在坐标系W_r1从第一活动单元、第一位置控制单元的第一支链机构计算得到相对于全局坐标系W_O的齐次坐标变换为：T₁₁＝T_b1T₁T_h1；

S2：从第二位置控制单元测计算运动学

W_b2相对于W_O的齐次坐标变换为：T_b2

根据第二位置控制单元的运动学，W_o2相对W_b2的齐次坐标变换为：

T₂＝T_f(s₂₁,s₂₂) (9)

W_r2相对于W_o2的齐次坐标变换为：

T_h2＝Rot(Y_o2,a₂)Rot(X_yaw2,b₂)Trans(Y_pitch2,d_2') (10)

其中，Rot(Y_o2,a₂)为绕Y_o2轴旋转a₂角度的4×4齐次旋转矩阵；Rot(X_yaw2,b₂)为绕X_yaw2轴旋转b₂角度的4×4齐次旋转矩阵；Trans(Y_pitch2,d_2')表示为沿着Y_pitch2轴平移d_2'距离的4×4齐次平移矩阵；

当点r₂所在的坐标系W_r2绕第一滑杆翻滚运动中心轴转动c角度，再沿着第一滑杆运动中心轴移动距离n，W_r2坐标系与W_r1坐标系重合，则坐标变换为：

T_r2r1＝Rot(X,c)Trans(X,-n) (11)

则将坐标系W_r1经过坐标系W_r2，从深度控制单元、第二活动单元、第二位置控制单元的第二支链计算得到的全局坐标变换为：T₁₂＝T_b2T₂T_h2T_r2r1

S3：同一坐标系经过两个支链的变换后，得到等式方程：

或者

利用以下步骤A1-A3迭代法求解，令公式(12)的6个变量为a₁、b₁、a₂、b₂、n、c＝x₁、x₆；

A1：给定X⁰＝[x₁、x₆]'初始值；

A2：根据公式(12)分别计算F＝[f₁、f₉]'和T＝[t₁、t₃]'；

A3：如果f_i<e₁和t_j<e₂，计算完成，否则按照如下计算迭代值，带入所述A2循环：

式中，

α与β为给定的收敛系数，非负数，和为1；

S4：计算穿刺针上坐标系

根据D-H参数法则建立各组成部分坐标系，穿刺针轴线分别与第一、第二俯仰轴的公垂线相交于穿刺针轴线点p₁和点p₂，刺针末端点为e，分别以该三个点为原点建立连接在穿刺针上的坐标系W_p1、W_p2、W_e；

W_p1相对W_r1的齐次坐标变换为：T_p1＝Trans(Y_r1,d₁)，W_p2相对W_p1的齐次坐标变换为：T_p1p2＝Trans(Z_p1,-n)，W_e相对W_p1的齐次坐标变换为：T_e＝Trans(Z_p1,-h)；

W_e在全局坐标系W_O的齐次坐标变换为：T_We＝T_b1T₁T_h1T_p1T_e；

W_e下的e点坐标P_e在全局坐标系W_O下的坐标为：P_we＝T_WeP_e，从T_We得到的旋转矩阵R_e为穿刺针的姿态矩阵。

13.根据权利要求1所述的控制方法，其特征在于：所述的立体定位手术机器人的逆运动学：

H1:设定a₁、b₁、a₂、b₂、n、c六个被动关节对应的变量以及s₁₁、s₁₂、s₂₁、s₂₂四个作动器的控制变量为未知变量，组合形成向量X＝[x_i]_10×1；

已知目标器官中病灶g_i的坐标为P_gi(x_i,y_i,z_i),(i＝1,2,3....)，穿刺点t_j的坐标为P_tj(x_j,y_j,z_j),(j＝1,2,3......)，则穿刺针所在的空间直线方程如公式(1)，

点p₁、p₂的坐标系W_p1和W_p2在全局坐标系W_O的齐次坐标变换分别为：

T_Wp1＝T_b1T₁T_h1T_p1

T_Wp2＝T_Wp1T_p1p2 (13)

点p₁、p₂分别在坐标系W_p1、W_p2下的坐标分别为p_p1、p_p2，则点p₁、p₂在全局坐标系W_O下的坐标为：

p_w1＝T_wp1p_p1

p_w2＝T_wp2p_p2 (14)

H2:求解变量X

B1：点p₁(p₂)位于穿刺针轴线上，所以满足空间直线方程(1)，因此，将方程(14)代入方程(1)，形成4个独立的方程；

B2：再根据所述步骤S1、S2、S3和S4的方法得到公式(12)，计算12个方程；

B3：同理参照所述步骤A1-A3迭代法求解，得到X，X向量后四个元素为s₁₁、s₁₂、s₂₁、s₂₂；

H3：根据正运动学计算P_e，深度控制单元控制穿刺针运动的行程为：

Images