一种机械臂有效工作空间计算方法及其控制方法与流程

1.本发明涉及手术机器人技术领域，尤其涉及一种机械臂有效工作空间计算方法及其控制方法。


背景技术：

2.肺癌是对人生命和健康威胁最大的癌症之一，肺部活检是肺部肿瘤诊断的金标准。肺部活检手术需要医生取出病灶进行病理分析，通常，医生采用机器人进行病灶的提取。为实现精确的穿刺手术，目前的手术机器人结构复杂、控制程序繁琐，因此，手术机器人制造成本较高且对手术室的空间要求较大。
3.为降低生产、制造成本和对手术室空间的要求，将手术机器人向小型化和简单化方向进行改进，针对改进后的手术机器人，其有效工作空间有所减小，为实现精确手术，有必要提供一种在实际手术操作前就能够明确得到手术机器人的有效工作空间和与之对应的控制流程的方法。


技术实现要素：

4.发明目的：针对上述问题，本发明提出了一种结构简单、操作控制简便的手术机器人的机械臂有效工作空间计算方法及其控制方法。
5.技术方案：
6.一种机械臂有效工作空间计算方法，所述机械臂的机械臂末端通过两平行设置的移动关节实现俯仰，通过翻转关节实现翻转，且俯仰轴线与翻转轴线相互垂直；
7.求解机械臂各关节坐标系与机器人基坐标系的变换关系；
8.获取影像坐标系与机器人基坐标系的变换关系，据此得到影像坐标系中的规划通道在机器人基坐标系下的方向向量，据此得到机械臂末端的俯仰角及翻转角；
9.结合上述机械臂各关节坐标系与机器人基坐标系的变换关系及机械臂的设计参数计算得到机械臂末端的最大运动范围。
10.计算俯仰角和翻转角具体如下：
11.规划通道在机器人基坐标系下的方向向量为cd(x,y,z)，计算得到俯仰角θ＝90
°‑
arccos(cd.x)，其中，arccos为反余弦函数，cd.x表示规划通道在机器人基座标下的方向向量cd(x,y,z)在x轴方向上的标量；
12.令dx为在x轴方向上的单位向量(1,0,0)，u为规划通道在机器人基座标下的方向向量cd(x,y,z)与向量dx的叉积的归一化向量；u.y为u在y轴方向上的标量，u.z为u在z轴方向上的标量，若u.z小于0，则翻转角α＝-arccos(u.y)，若u.z大于等于0，则翻转角α＝arccos(u.y)。
13.两平行设置的移动关节分别为第六关节及设置于第六关节下方的第五关节，第六关节通过连接件与机械臂末端铰接，铰接点为b，第五关节与机械臂末端的铰接点为a，第五、第六关节处于初始位置时，铰接点a和铰接点b之间的连线与第五、第六关节的移动方向
垂直。
14.计算机械臂末端的最大运动范围具体如下：
15.已知俯仰角θ、已知两平行关节在初始位置时其末端之间距离为m、垂直距离为l，两平行关节异步移动后，末端之间的距离为s，h1为连接件的长度，h2为铰接点a至铰接点b的距离；
16.当机械臂末端处于仰状态时，此时两平行关节末端的连线与两关节的移动方向所成的锐角为θ1，则得到两平行关节的移动差值d1为：
[0017][0018][0019]
结合机械臂各关节坐标系与机器人基坐标系的变换关系计算得到机械臂末端处于仰状态时机械臂末端的最小极限位置和最大极限位置；
[0020]
当机械臂末端处于俯状态时，此时两平行关节末端的连线与两关节的移动方向所成的锐角为θ2，则得到两平行关节的移动差值d2为：
[0021]
d2＝m-l
×
tanθ2[0022][0023]
结合机械臂各关节坐标系与机器人基坐标系的变换关系计算得到机械臂末端处于俯状态时机械臂各关节的最小极限位置和最大极限位置。
[0024]
所述机械臂末端从俯状态的最大极限位置运动到仰状态的最大极限位置所涵盖的空间即为机械臂的最大运动范围。
[0025]
所述机械臂还包括沿与第五关节的移动方向垂直的第一方向移动的第一关节、沿与第一方向及第五关节的移动方向均垂直的第二方向移动的第二、第三关节；
[0026]
则机械臂末端处于仰状态时机械臂末端的最小极限位置对应的关节参数为i
min
{0，0，0，α，d1，0}和最大极限位置对应的关节参数为i
max
，若第六关节z6最大运动行程减去第五关节z5最大运动行程小于等于d1，i
max
则为(第一关节z1最大运动行程，第二关节z2最大运动行程，第三关节z3最大运动行程，α，第五关节z5最大运动行程，第五关节z5最大运动行程 d1)，反之i
max
则为(第一关节z1最大运动行程，第二关节z2最大运动行程，第三关节z3最大运动行程，α，第六关节z6最大运动行程-d1，第六关节z6最大运动行程)，α为翻转角；
[0027]
机械臂末端处于俯状态时机械臂末端的最小极限位置对应的关节参数j
min
{0，0，0，α，d2，0}和最大极限位置对应的关节参数为j
max
，若第五关节z5最大运动行程减去第六关节z6最大运动行程大于等于d2，j
max
则为(第一关节z1最大运动行程，第二关节z2最大运动行程，第三关节z3最大运动行程，α，第六关节z6最大运动行程 d2，第六关节z6最大运动行程)，反之j
max
则为(第一关节z1最大运动行程，第二关节z2最大运动行程，第三关节z3最大运动行程，α，第五关节z5最大运动行程，第五关节z5最大运动行程-d2)，α为翻转角。
[0028]
机械臂各关节坐标系与机器人基坐标系的变换关系计算如下：
[0029]
定义机器人基坐标系为：
[0030][0031]
则：
[0032][0033][0034][0035][0036][0037][0038][0039]
其中，dz1为第一关节的行程，dy2为第二关节的行程；y2为第二关节的坐标系原点相对于第一关节的坐标系原点的y方向的零位差值，z2为第二关节的坐标系原点相对于第一关节的坐标系原点的z方向的零位差值，x5为第五关节的坐标系原点相对于第四关节的坐标系原点的x方向的零位差值，dx5为第五关节的行程，z5为第五关节的坐标系原点相对于第四关节的坐标系原点的z方向的零位差值，x6为第六关节的坐标系原点相对于第四关节
的坐标系原点的x方向的零位差值，dx6为第六关节的行程，z6为第六关节的坐标系原点相对于第四关节的坐标系原点的z方向的零位差值，x7为第七关节的坐标系原点相对于第六关节的坐标系原点的x方向的零位差值，z7为第七关节的坐标系原点相对于第六关节的坐标系原点的z方向的零位差值；t1为第一关节坐标系、t2为第二关节坐标系、t3为第三关节坐标系、t4为第四关节坐标系、t5为第五关节坐标系、t6为第六关节坐标系、t7为机械臂末端坐标系、θ为机械臂末端的俯仰角、α为机械臂末端的翻转角。
[0040]
获取影像坐标系与机器人基坐标系的变换关系具体为：
[0041]
根据机械臂的各个关节姿态参数求解得到机械臂末端相对于机器人基坐标系的变换关系，记为c
base_tool
；
[0042]
根据机械臂上安装的末端示踪器的安装参数得到末端示踪器与机械臂末端的变换关系，记为c
tool_et
；
[0043]
根据光学导航设备识别得到末端示踪器和患者患处的注册示踪器与光学导航设备之间的变换关系，分别记为c
ots_et
和c
ots_regist
；
[0044]
根据注册示踪器在ct影像中的位姿得到影像坐标系和注册示踪器的变换关系，记为c
regist_img
；
[0045]
计算得到影像坐标系与机器人基坐标系的变换关系c
base_img
：
[0046]cbase_img
＝c
base_tool
*c
tool_et
*(c
ots_et
)
inv
*c
ots_regist
*c
regist_img
[0047]
其中，inv为矩阵求逆运算。
[0048]
一种手术机器人的控制方法，包括：
[0049]
根据前述机械臂有效工作空间计算方法计算得到机械臂有效工作空间的中心点；
[0050]
根据影像坐标系与机器人基坐标系的变换关系计算得到影像坐标系下规划通道的起始点和终点在机器人基坐标系下的位置，并以起始点为起点向终点至起始点方向延长设定长度的点作为机械臂末端的目标点；
[0051]
计算得到的所述目标点与所述中心点在机器人基坐标系的x、y、z轴方向的差值，得到ct床所需要移动的距离；
[0052]
医生根据计算得到的所述距离移动ct床，ct床移动到位后执行规划结果。
[0053]
所述设定长度为150mm。
[0054]
有益效果：本发明提供了一种相应的机械臂有效工作空间计算方法，并基于此计算结果结合规划通道的位姿计算得到ct床所要移动的距离，医生只需据此移动ct床即可，操作简单便捷。
附图说明
[0055]
图1为本发明的手术机器人的结构示意图；
[0056]
图2为本发明的手术机器人机械臂的部分侧视图；
[0057]
图3为本发明的机械臂末端向上旋转的原理图；
[0058]
图4为本发明的机械臂末端向下旋转的原理图；
[0059]
图5为机械臂关节的运动学参数示意图。
[0060]
其中，1为底座，2为机械臂，21为机械臂末端，22为末端示踪器，23为执行器械，3为穿刺针。
具体实施方式
[0061]
下面结合附图和具体实施例，进一步阐明本发明。
[0062]
图1为本发明的手术机器人的结构示意图，结合图2所示，本发明的手术机器人包括底座1、机械臂2及机械臂末端21。在机械臂末端21设置有末端示踪器22和执行器械23，执行器械23上设有执行通道用于穿设穿刺针3。机械臂2包括与底座1连接的第一驱动部、由第一驱动部驱动而相对于底座1运动的第一连杆、与第一连杆连接的第二驱动部、由第二驱动部驱动的第二连杆、与第二连杆连接的第三驱动部、由第三驱动部驱动的第三连杆、与第三连杆连接的第四驱动部、由第四驱动部驱动的第四连杆、与第四连杆连接的第五驱动部和第六驱动部及分别与第五驱动部和第六驱动部连接的第五连杆和第六连杆。
[0063]
第一驱动部和第一连杆组成第一关节z1，第二驱动部与第二连杆组成第二关节z2，第三驱动部与第三连杆组成第三关节z3，第四驱动部与第四连杆组成第四关节z4，第五驱动部与第五连杆组成第六关节z5，第六驱动部与第六连杆组成第六关节z6，各连杆的运动量构成相应各关节的行程。
[0064]
继续参照图1和图2所示，第一关节z1、第二关节z2、第三关节z3、第五关节z5和第六关节z6均为直线平移关节，第四关节z4为旋转关节。具体地，第一驱动部驱动第一连杆沿第一方向移动，第二驱动部和第三驱动部分别驱动第二连杆和第三连杆沿与第一方向垂直的第二方向移动，第四驱动部驱动第四连杆绕垂直于第一方向和第二方向的第一轴线转动，第五驱动部和第六驱动部分别驱动第五连杆和第六连杆沿平行于第一轴线的第三方向移动。其中，第一方向与地面垂直，第二方向与地面平行。
[0065]
更具体的，第五关节z5和第六关节z6沿第一方向平行设置且第六关节z6位于第五关节z5的上方，二者之间的距离为l。
[0066]
结合图3所示，第五连杆的末端与机械臂末端21铰接，铰接点为a；第六连杆通过连接件与机械臂末端21铰接，铰接点为b。在初始位置，第五连杆和第六连杆的末端之间的距离为m，连接件的长度为h1，铰接点a至铰接点b的距离为h2，铰接点a和铰接点b的连线与地面垂直，此时，机械臂末端21处于水平状态，即机械臂末端21与地面平行；当第五关节z5和第六关节z6同步运动时，即第五连杆和第六连杆的移动量相同，机械臂末端21始终处于水平状态且进行平移运动；当第五关节z5和第六关节z6异步运动时，即第五连杆和第六连杆的移动量不同时，机械臂末端21向下俯或向上仰。具体地，在靠近机械臂末端21的方向上，当第六连杆的移动量大于第五连杆的移动量时，机械臂末端21向下俯；在沿远离机械臂末端21的方向上，当第六连杆的移动量小于第五连杆的移动量时，机械臂末端21向上仰。
[0067]
本发明手术机器人通过简单的结构实现5个自由度的运动，且通过第五关节z5和第六关节z6的配合，使机械臂末端21实现移动加俯仰的功能，整体结构简单、紧凑、制造成本低、手术需求空间小。
[0068]
为实现精确手术和确保病灶处于本发明手术机器人的有效工作空间内，本发明还提供一种手术机器人的有效工作空间计算方法，如图4、5所示，其包括步骤：
[0069]
(1)计算影像坐标系与机器人基坐标系t的变换关系；
[0070]
由机械臂2的各个关节姿态参数，求解出机械臂末端21相对于机器人基坐标系t的变换关系，记为c
base_tool
；机械臂末端21相对于其上安装的末端示踪器22的变换关系可以通过三维测量仪获取得到，记为c
tool_et
；其中，机器人基坐标系t以第一关节z1初始位置为原
点，其x轴、y轴及z轴分别平行于第四关节z4的旋转轴线、第二关节z2的运动方向及第一关节z1运动方向；
[0071]
机械臂末端21上安装的末端示踪器22可以被光学导航设备识别得到，这样就可以建立末端示踪器22相对于光学导航设备坐标系的变换关系，记为c
ots_et
；而患者患处的注册示踪器也可以被光学导航设备识别到，同样也可建立注册示踪器相对于光学导航设备坐标系的变换关系，记为c
ots_regist
；而注册示踪器可以在ct影像中被识别到，因而可以建立影像坐标系和注册示踪器的变换关系，记为c
regist_img
；
[0072]
由此可以计算得到影像坐标系与机器人基坐标系t的变换关系c
base_img
：
[0073]cbase_img
＝c
base_tool
*c
tool_et
*(c
ots_et
)
inv
*c
ots_regist
*c
regist_img
[0074]
其中，inv为矩阵求逆运算；
[0075]
通过以上的坐标系变换就可以建立影像坐标系相对于机器人基坐标系t的变换关系c
base_img
；
[0076]
(2)机械臂各关节坐标系与机器人基坐标系的变换关系；
[0077]
(21)定义各关节坐标系，其中，第一关节坐标系t1是以机器人基坐标系t为基准，以第一关节的移动距离为变换关系变换得到；第二关节坐标系t2是以第一关节坐标系t1为基准，以第二关节的移动距离为变换关系变换得到；第三关节坐标系t3是以第二关节坐标系t2为基准，以第三关节的移动距离为变换关系变换得到；第四关节坐标系t4是以第三关节坐标系t3为基准，以第四关节的旋转角度为变换关系变换得到；第五关节坐标系t5和第六关节坐标系t6均是以第四关节坐标系t4为基准，分别以第五关节和第六关节的移动距离为变换关系变换得到；
[0078]
具体的，定义机器人基坐标系为：
[0079][0080]
则：
[0081][0082][0083]
[0084][0085][0086][0087][0088]
其中，t7为机械臂末端坐标系，θ为机械臂末端的俯仰角、α为机械臂末端的翻转角，dz1为第一关节的行程，dy2为第二关节的行程；y2为第二关节的坐标系原点相对于第一关节的坐标系原点的y方向的零位差值，z2为第二关节的坐标系原点相对于第一关节的坐标系原点的z方向的零位差值，x5为第五关节的坐标系原点相对于第四关节的坐标系原点的x方向的零位差值，dx5为第五关节的行程，z5为第五关节的坐标系原点相对于第四关节的坐标系原点的z方向的零位差值，x6为第六关节的坐标系原点相对于第四关节的坐标系原点的x方向的零位差值，dx6为第六关节的行程，z6为第六关节的坐标系原点相对于第四关节的坐标系原点的z方向的零位差值，x7为第七关节的坐标系原点相对于第六关节的坐标系原点的x方向的零位差值，z7为第七关节的坐标系原点相对于第六关节的坐标系原点的z方向的零位差值。
[0089]
第六关节z6向远离机械臂末端21方向移动距离大于第五关节z5向远离机械臂末端21方向的移动距离，则可以实现机械臂末端21向上旋转，就是机械臂末端21处于仰状态；第六关节z6向靠近机械臂末端21的方向移动距离大于第五关节z5向靠近机械臂末端21方向的移动距离，则可以实现机械臂末端21的向下旋转，就是机械臂末端21处于俯状态；
[0090]
根据机械臂的设计参数及各关节的坐标系与机器人基座标系之间的变换关系计算得到机械臂各关节的运动学参数，图5为机械臂各关节的运动学参数；
[0091]
(3)机械臂运动范围求解；
[0092]
(31)求解机械臂末端的目标位姿：
[0093]
根据步骤(1)求解出的影像坐标系与机器人基座标系的变换关系c
base_img
，将在影像中规划的规划通道变换至机器人基坐标系下，据此计算得到规划通道的起始点p
t1
和终点p
t2
在机器人基坐标系t下的位置，定义机械臂末端21的目标点p
t
为以起始点p
t1
为起点并沿
终点p
t2
到起始点p
t1
的方向上延长150mm的点，据此计算得到机器人机械臂末端21的目标位姿，求解得到机械臂末端21处于其目标位姿时的方向向量cd(x,y,z)，即规划通道在机器人基坐标系下的方向向量；
[0094]
(32)求解机械臂末端处于目标位姿时对应的俯仰角和翻转角：
[0095]
本发明中的机械臂末端21只有两个方向的旋转角，即通过第五关节z5和第六关节z6异步移动产生的俯仰角和绕第四关节z4的旋转轴线旋转的翻转角，且俯仰角旋转轴线和翻转角的旋转轴线相互垂直，结合规划通道在机器人基座标下的方向向量cd(x,y,z)求解出机械臂末端在工作时的翻转角α和俯仰角θ；具体地，根据规划通道可以计算得到机械臂末端的目标位姿，机械臂末端的目标位姿位于规划通道的延长线上，因此机械臂末端的目标位姿的方向向量与规划通道的方向向量一致。
[0096]
则θ＝90
°‑
arccos(cd.x)，其中，arccos为反余弦函数，cd.x表示规划通道在机器人基座标下的方向向量cd(x,y,z)在x轴方向上的标量；
[0097]
令dx为在x轴方向上的单位向量(1,0,0)，u为规划通道在机器人基座标下的方向向量cd(x,y,z)与向量dx的叉积的归一化向量；u.y为u在y轴方向上的标量，u.z为u在z轴方向上的标量，若u.z小于0，则翻转角α＝-arccos(u.y)，若u.z大于等于0，则翻转角α＝arccos(u.y)。
[0098]
(33)求解机械臂末端的最大极限运动位置：
[0099]
已知俯仰角θ，第五关节z5和第六关节z6在初始位置的水平距离差为m、垂直距离差为l，铰接点ab之间的距离为h2，连接件的长度为h1，第五关节和第六关节异步移动之后，第五关节z5末端与第六关节z6的末端之间的距离为s，据此求得第五关节z5和第六关节z6的移动差值d；其中，计算根据机械臂末端21的俯仰极限位置分成如下两种情况：
[0100]
i)第五关节z5与机械臂末端21的铰接点为a点，第六关节z6与机械臂末端21的铰接点由b点运动至b'点，机械臂末端21向上旋转，也即机械臂末端21处于仰状态，如图3所示，俯仰角θ即为ab与ab'之间的夹角；此时，移动后的第六关节z6的末端与第五关节z5末端之间的连线与水平方向所成的锐角为θ1，则：
[0101][0102][0103]
化简得到：
[0104][0105][0106]
由此可以计算得到机械臂末端21处于仰状态时，机械臂末端21最小极限位置的关节参数i
min
{0，0，0，α，0，d1}和处于最大极限位置的关节参数i
max
。若第六关节z6最大运动行程减去第五关节z5最大运动行程小于等于d1，i
max
则为(第一关节z1最大运动行程，第二关
节z2最大运动行程，第三关节z3最大运动行程，α，第五关节z5最大运动行程，第五关节z5最大运动行程 d1)，反之i
max
则为(第一关节z1最大运动行程，第二关节z2最大运动行程，第三关节z3最大运动行程，α，第六关节z6最大运动行程-d1，第六关节z6最大运动行程)，α为翻转角。结合步骤(2)中各关节坐标系与机器人基坐标系的变换关系即可求得机械臂末端21仰状态时的最小极限位置和最大极限位置；
[0107]
ii)第六关节z6向靠近机械臂末端21的方向移动，实现机械臂末端21的向下旋转，就是机械臂末端21处于俯状态，此时，第五关节z5与机械臂末端21的铰接点为a点，第六关节z6与机械臂末端21的铰接点由b点运动至b”点，如图4所示，此时，移动后的第六关节z6的末端与第五关节z5的末端之间的连线与垂直方向所成的锐角为θ2，则：
[0108][0109][0110]
化简得到：
[0111][0112]
d2＝m-l
×
tanθ2[0113]
据此，得到机械臂末端21处于俯状态时机械臂末端21最小极限位置的关节参数j
min
{0，0，0，α，d2，0}和最大极限位置的关节参数j
max
，若第五关节z5最大运动行程减去第六关节z6最大运动行程大于等于d2，j
max
则为(第一关节z1最大运动行程，第二关节z2最大运动行程，第三关节z3最大运动行程，α，第六关节z6最大运动行程 d2，第六关节z6最大运动行程)，反之j
max
则为(第一关节z1最大运动行程，第二关节z2最大运动行程，第三关节z3最大运动行程，α，第五关节z5最大运动行程，第五关节z5最大运动行程-d2)，α为翻转角。结合步骤(2)中各关节坐标系与机器人基坐标系的变换关系即可求得机械臂末端21俯状态时的最小极限位置和最大极限位置；
[0114]
所述机械臂末端从俯状态的最大极限位置运动到仰状态的最大极限位置所涵盖的空间即为机械臂的最大运动范围，即有效工作空间。
[0115]
当在影像规划病灶通道后，如果病灶不在机械臂的有效工作空间内，此时，可以移动ct床或移动患者，使得病灶移动至机械臂的有效工作空间内。
[0116]
相应地，本发明还提供了一种基于前述机械臂有效工作空间计算方法的控制方法，具体如下：
[0117]
采用前述机械臂有效工作空间计算方法计算出机械臂有效工作空间的中心点pc；
[0118]
医生在影像中的规划通道，并根据影像坐标系与机器人基坐标系t的变换关系c
base_img
将规划通道变换至机器人基坐标系下，据此计算得到规划通道的起始点p
t1
和终点p
t2
在机器人基坐标系t下的位置，定义目标点为p
t
，其为以起始点p
t1
为起点并沿终点p
t2
到起始点p
t1
的方向上延长150mm的点；
[0119]
计算得到的目标点p
t
与机械臂有效工作空间的中心点pc在机器人基坐标系下的差
值，该差值即为ct床所需要移动的距离；
[0120]
医生根据计算得到的ct床所需要移动的距离移动ct床到位，机器人执行穿刺操作。
[0121]
以上详细描述了本发明的优选实施方式，但是本发明并不限于上述实施方式中的具体细节，在本发明的技术构思范围内，可以对本发明的技术方案进行多种等同变换(如数量、形状、位置等)，这些等同变换均属于本发明的保护范围。