一种力反馈末端夹持器导纳控制方法技术

本发明专利技术公开了一种力反馈末端夹持器导纳控制方法，首先通过控制六自由度机械臂的运动，求解出末端执持器的等效质量，从而排除末端执持器的自重干扰，然后通过末端夹持器采集穿刺针末端力信号，作为导纳控制器的输入信号，利用龙格—库塔方法求解零力补偿位置变化量，通过逆运动学计算相应的关节角姿态，利用位置控制器驱动机械臂运动至相应的位置，实现零力跟随。本发明专利技术方法可以实现对穿刺手术机器人末端穿刺针的零力跟随控制，减少因组织生理运动对穿刺手术造成的干扰，提升手术的精准性与安全性。与安全性。与安全性。

【技术实现步骤摘要】
一种力反馈末端夹持器导纳控制方法


[0001]本专利技术涉及穿刺手术机器人末端夹持器设计及机器人柔顺控制的
，尤其是指一种力反馈末端夹持器导纳控制方法。

技术介绍

[0002]随着机器人技术的不断发展与医学治疗手段的不断丰富，医疗机器人应运而生。医疗机器人是指各种应用于外科手术、康复治疗、残障人士辅助等医学用途的机器人设备，具有微创，精准，高效的特点。医疗机器人可以细分为手术机器人，康复机器人，辅助机器人等。其中穿刺手术机器人是手术机器人的一个重要分支，主要用于进行外科穿刺手术，如腰椎穿刺，血管穿刺，脏器穿刺等，对于相关疾病的诊断与恶性肿瘤的消融治疗有着重要作用。穿刺手术机器人一般由机械臂、空间定位系统、成像设施、工作站四大部分以及相应的软件组成。目前，穿刺手术机器人技术已经较为成熟，被广泛应用到临床中。机器人可辅助人工操作，降低穿刺手术难度，提高手术可靠性：所采用的光学导航系统可以辅助医生进行影像分析、路径规划，并对目标靶区进行精确定位与穿刺。手术机器人状态稳定，克服了传统手术易受医生个人操作和经验影响的缺点，提高手术的安全性和成功率。
[0003]然而，目前市场上的穿刺手术机器人仍然存在一定的局限性。目前成熟的穿刺手术机器人主要用于神经外科与骨科的穿刺手术，均无法对受呼吸运动影响的靶区，如肝，肺等进行精准的穿刺。因为这些部位的器官组织会随人体的呼吸运动发生位移和形变，从而导致目标靶区位置发生变化，降低系统的定位精度。此外，器官组织的生理运动还将给穿刺针造成挤压力，导致穿刺针发生弹性形变，偏离预定穿刺位置，导致穿刺失败。同时，器官组织与穿刺针之间的相互作用力还有可能对周围正常的人体器官组织造成损伤，甚至造成器官撕裂，降低手术的安全性，因此需设计可以适时调整的手术导航系统。仅依赖光学导航系统的穿刺机器人无法感知力信号并做出调整；并且由于穿刺针较长，仅使用一个三维力传感器并不能精准检测出穿刺针受力，因此需采用两个三维力传感器进行检测。目前，主流手术机器人系统和现行的穿刺手术技术均不能很好地解决上述问题。

技术实现思路

[0004]本专利技术的目的在于克服现有技术的缺点与不足，提出了一种力反馈末端夹持器导纳控制方法，可以实现对穿刺手术机器人末端穿刺针的零力跟随控制，减少因组织生理运动对穿刺手术造成的干扰，提高穿刺精准度，减少对周边器官组织的损伤，从而提升手术的精准性与安全性。
[0005]为实现上述目的，本专利技术所提供的技术方案为：一种力反馈末端夹持器导纳控制方法，该方法需要用到带有下部引导端三维力传感器和上部夹持端三维力传感器的力反馈末端夹持器及穿刺针手术工具，所述力反馈末端夹持器固定于六自由度机械臂末端，所述穿刺针手术工具固定在力反馈末端夹持器上，用于检测外界施加的作用力，当穿刺针手术工具受力时，力反馈末端夹持器上的两个三维力传感器能够实时采集到相应的力信号；该
方法包括以下步骤：
[0006]S1、设计自重偏置滤除算法，该算法是以六自由度机械臂基座坐标系为基准，通过六自由度机械臂的运动排除力反馈末端执持器的重力干扰；在力反馈末端执持器的重力干扰滤除后，取上部夹持端三维力传感器的Z轴与下部引导端三维力传感器的X轴和Y轴构成组合坐标系O
c
；
[0007]S2、将穿刺针手术工具在组合坐标系O
c
下的受力[F
cx F
cy F
cz
]输入到基于位置控制的导纳控制器中，获得期望位置与实际位置之差X
w
，再与初始位置X
o
进行叠加，求得下一时刻六自由度机械臂的期望位置X
f
；
[0008]S3、结合六自由度机械臂的结构参数和DH表，将上述求得的六自由度机械臂的期望位置X
f
进行逆向运动学转换，求得六自由度机械臂期望的关节角度信息q，再通过脚本函数控制六自由度机械臂到达期望位置，从而实现穿刺针手术工具的零力补偿。
[0009]进一步，所述S1包括以下步骤：
[0010]S101、根据六自由度机械臂的结构参数、位置信息和DH表，运用正向运动学求得六自由度机械臂基座坐标系下等效重力加速度g
b
和六自由度机械臂末端坐标系下等效重力加速度g
e
即力反馈末端执持器坐标系下等效重力加速度g
t
之间的转换关系：
[0011][0012]其中，g
t
为力反馈末端执持器的等效重力加速度，g
e
为六自由度机械臂末端的等效重力加速度，g
b
为六自由度机械臂基座的等效重力加速度，R
be
表示六自由度机械臂基座坐标系和六自由度机械臂末端坐标系之间的转换关系；
[0013]其中，X
be
,Y
be
,Z
be
分别为绕X轴，Y轴，Z轴的旋转分量；为X
be
的X轴坐标，为X
be
的Y轴坐标，是X
be
的Z轴坐标；为Y
be
的X轴坐标，为Y
be
的Y轴坐标，是Y
be
的Z轴坐标，为Z
be
的X轴坐标，为Z
be
的Y轴坐标，是Z
be
的Z轴坐标；具体计算方法如下：
[0014][0015]其中，θ1,θ2,θ3,θ4,θ5,θ6均为六自由度机械臂当前位置的关节角度；
[0016]S102、将六自由度机械臂末端关节角度设置为10度，六自由度机械臂腕关节角度
设置为300度，六自由度机械臂腕关节运动步长设置为5度，六自由度机械臂腕关节角度依次减去运动步长，并赋值给六自由度机械臂腕关节角度；
[0017]S103、将步骤S102求得的六自由度机械臂腕关节角度通过六自由度机械臂脚本函数控制六自由度机械臂运动至相应位置；
[0018]S104、在步骤S103的基础上，将组合坐标系下采集到的力信号拼接成向量F
t
，将每一时刻力反馈末端执持器的等效重力加速度g
t
拼接成向量G
t
；
[0019]S105、六自由度机械臂末端关节运动步长设置为5度，六自由度机械臂腕关节角度依次加上运动步长，并赋值给六自由度机械臂末端关节角度；
[0020]S106、将步骤S105求得的六自由度机械臂末端关节角度通过六自由度机械臂脚本函数控制六自由度机械臂运动至相应位置；
[0021]S107、结合步骤S103和S106，将组合坐标系下采集到的力信号拼接成向量F
t
，将每一时刻力反馈末端执持器的等效重力加速度g
t
拼接成向量G
t
；
[0022]S108、通过步骤S107求得的F
t
和G
t
，计算能够得力反馈末端执持器自身的重力干扰值的等效质量M...

【技术保护点】

【技术特征摘要】
1.一种力反馈末端夹持器导纳控制方法，其特征在于：该方法需要用到带有下部引导端三维力传感器和上部夹持端三维力传感器的力反馈末端夹持器及穿刺针手术工具，所述力反馈末端夹持器固定于六自由度机械臂末端，所述穿刺针手术工具固定在力反馈末端夹持器上，用于检测外界施加的作用力，当穿刺针手术工具受力时，力反馈末端夹持器上的两个三维力传感器能够实时采集到相应的力信号；该方法包括以下步骤：S1、设计自重偏置滤除算法，该算法是以六自由度机械臂基座坐标系为基准，通过六自由度机械臂的运动排除力反馈末端执持器的重力干扰；在力反馈末端执持器的重力干扰滤除后，取上部夹持端三维力传感器的Z轴与下部引导端三维力传感器的X轴和Y轴构成组合坐标系O
c
；S2、将穿刺针手术工具在组合坐标系O
c
下的受力[F
cx F
cy F
cz
]输入到基于位置控制的导纳控制器中，获得期望位置与实际位置之差X
w
，再与初始位置X
o
进行叠加，求得下一时刻六自由度机械臂的期望位置X
f
；S3、结合六自由度机械臂的结构参数和DH表，将上述求得的六自由度机械臂的期望位置X
f
进行逆向运动学转换，求得六自由度机械臂期望的关节角度信息q，再通过脚本函数控制六自由度机械臂到达期望位置，从而实现穿刺针手术工具的零力补偿。2.根据权利要求1所述的一种力反馈末端夹持器导纳控制方法，其特征在于，所述S1包括以下步骤：S101、根据六自由度机械臂的结构参数、位置信息和DH表，运用正向运动学求得六自由度机械臂基座坐标系下等效重力加速度g
b
和六自由度机械臂末端坐标系下等效重力加速度g
e
即力反馈末端执持器坐标系下等效重力加速度g
t
之间的转换关系：其中，g
t
为力反馈末端执持器的等效重力加速度，g
e
为六自由度机械臂末端的等效重力加速度，g
b
为六自由度机械臂基座的等效重力加速度，R
be
表示六自由度机械臂基座坐标系和六自由度机械臂末端坐标系之间的转换关系；其中，X
be
，Y
be
，Z
be
分别为绕X轴，Y轴，Z轴的旋转分量；为X
be
的X轴坐标，为X
be
的Y轴坐标，是X
be
的Z轴坐标；为Y
be
的X轴坐标，为Y
be
的Y轴坐标，是Y
be
的Z轴坐标，为Z
be
的X轴坐标，为Z
be
的Y轴坐标，是Z
be
的Z轴坐标；具体计算方法如下：
其中，θ1，θ2，θ3，θ4，θ5，θ6均为六自由度机械臂当前位置的关节角度；S102、将六自由度机械臂末端关节角度设置为10度，六自由度机械臂腕关节角度设置为300度，六自由度机械臂腕关节运动步长设置为5度，六自由度机械臂腕关节角度依次减去运动步长，并赋值给六自由度机械臂腕关节角度；S103、将步骤S102求得的六自由度机械臂腕关节角度通过六自由度机械臂脚本函数控制六自由度机械臂运动至相应位置；S104、在步骤S103的基础上，将组合坐标系下采集到的力信号拼接成向量F
t
，将每一时刻力反馈末端执持器的等效重力加速度g
t
拼接成向量G
t
；S105、六自由度机械臂末端关节运动步长设置为5度，六自由度机械臂腕关节角度依次加上运动步长，并赋值给六自由度机械臂末端关节角度；S106、将步骤S105求得的六自由度机械臂末端关节角度通过六自由度机械臂脚本函数控制六自由度机械臂运动至相应位置；S107、结合步骤S103和S106，将组合坐标系下采集到的力信号拼接成向量F
t
，将每一时刻力反馈末端执持器的等效重力加速度g
t
拼接成向量G
t
；S108、通过步骤S107求得的F
t
和G
t
，计算能够...

【专利技术属性】
技术研发人员：杨荣骞，张文龙，朱云龙，晏猋，
申请(专利权)人：华南理工大学，
类型：发明
国别省市：