一种机械臂有效工作空间计算方法及其控制方法技术

本发明专利技术公开了一种机械臂有效工作空间计算方法及其控制方法，所述机械臂的机械臂末端通过两平行设置的移动关节实现俯仰，通过翻转关节实现翻转，且俯仰轴线与翻转轴线相互垂直；求解机械臂各关节坐标系与机器人基坐标系的变换关系；获取影像坐标系与机器人基坐标系的变换关系，据此得到影像坐标系中的规划通道在机器人基坐标系下的方向向量，据此得到机械臂末端的俯仰角及翻转角；结合上述机械臂各关节坐标系与机器人基坐标系的变换关系及机械臂的设计参数计算得到机械臂末端的最大运动范围。本发明专利技术的机械臂有效工作空间计算方法计算简单，并可基于此结合规划通道的位姿计算得到CT床所要移动的距离，医生据此移动CT床，操作简便。作简便。作简便。

【技术实现步骤摘要】
一种机械臂有效工作空间计算方法及其控制方法


[0001]本专利技术涉及手术机器人
，尤其涉及一种机械臂有效工作空间计算方法及其控制方法。

技术介绍

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

技术实现思路

[0004]专利技术目的：针对上述问题，本专利技术提出了一种结构简单、操作控制简便的手术机器人的机械臂有效工作空间计算方法及其控制方法。
[0005]技术方案：
[0006]一种机械臂有效工作空间计算方法，所述机械臂的机械臂末端通过两平行设置的移动关节实现俯仰，通过翻转关节实现翻转，且俯仰轴线与翻转轴线相互垂直；
[0007]求解机械臂各关节坐标系与机器人基坐标系的变换关系；
[0008]获取影像坐标系与机器人基坐标系的变换关系，据此得到影像坐标系中的规划通道在机器人基坐标系下的方向向量，据此得到机械臂末端的俯仰角及翻转角；
[0009]结合上述机械臂各关节坐标系与机器人基坐标系的变换关系及机械臂的设计参数计算得到机械臂末端的最大运动范围。
[0010]计算俯仰角和翻转角具体如下：
[0011]规划通道在机器人基坐标系下的方向向量为cd(x,y,z)，计算得到俯仰角θ＝90
°‑
arccos(cd.x)，其中，arccos为反余弦函数，cd.x表示规划通道在机器人基座标下的方向向量cd(x,y,z)在x轴方向上的标量；
[0012]令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)。
[0013]两平行设置的移动关节分别为第六关节及设置于第六关节下方的第五关节，第六关节通过连接件与机械臂末端铰接，铰接点为B，第五关节与机械臂末端的铰接点为A，第五、第六关节处于初始位置时，铰接点A和铰接点B之间的连线与第五、第六关节的移动方向
垂直。
[0014]计算机械臂末端的最大运动范围具体如下：
[0015]已知俯仰角θ、已知两平行关节在初始位置时其末端之间距离为m、垂直距离为L，两平行关节异步移动后，末端之间的距离为s，H1为连接件的长度，H2为铰接点A至铰接点B的距离；
[0016]当机械臂末端处于仰状态时，此时两平行关节末端的连线与两关节的移动方向所成的锐角为θ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为机械臂末端本文档来自技高网...

【技术保护点】

【技术特征摘要】
结合机械臂各关节坐标系与机器人基坐标系的变换关系计算得到机械臂末端处于俯状态时机械臂各关节的最小极限位置和最大极限位置。5.根据权利要求4所述的机械臂有效工作空间计算方法，其特征在于：所述机械臂末端从俯状态的最大极限位置运动到仰状态的最大极限位置所涵盖的空间即为机械臂的最大运动范围。6.根据权利要求4所述的机械臂有效工作空间计算方法，其特征在于：所述机械臂还包括沿与第五关节的移动方向垂直的第一方向移动的第一关节、沿与第一方向及第五关节的移动方向均垂直的第二方向移动的第二、第三关节；则机械臂末端处于仰状态时机械臂末端的最小极限位置对应的关节参数为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最大运动行程)，α为翻转角；机械臂末端处于俯状态时机械臂末端的最小极限位置对应的关节参数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)，α为翻转角。7.根据权利要求6所述的机械臂有效工作空间计算方法，其特征在于：机械臂各关节坐标系与机器人基坐标系的变换关系计算如下：定义机器人基坐标系为：则：则：则：
其中，dz1为第一关节的行程，dy2为第二关节的行程；y2为第二关节的坐标系原点相对于第一关节的坐标系原点的y方向的零位差值，z2为第二关节的坐标系原点相对于第一关节的坐标系原点的z方向的零位差值，x5为第五关节的坐标系原点相对于第四关节的坐标系原点的x方向的零位差值，dx5为第五关节的行程，z5为第五关节的...

【专利技术属性】
技术研发人员：曹红洋，刘孝波，倪庆亮，
申请(专利权)人：南京佗道医疗科技有限公司，
类型：发明
国别省市：