CN113977602A - 一种力反馈末端夹持器导纳控制方法 - Google Patents

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

Info

Publication number
CN113977602A
CN113977602A CN202111263805.1A CN202111263805A CN113977602A CN 113977602 A CN113977602 A CN 113977602A CN 202111263805 A CN202111263805 A CN 202111263805A CN 113977602 A CN113977602 A CN 113977602A
Authority
CN
China
Prior art keywords
mechanical arm
degree
freedom mechanical
force
tail end
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202111263805.1A
Other languages
English (en)
Other versions
CN113977602B (zh
Inventor
杨荣骞
张文龙
朱云龙
晏猋
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
South China University of Technology SCUT
Original Assignee
South China University of Technology SCUT
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by South China University of Technology SCUT filed Critical South China University of Technology SCUT
Priority to CN202111263805.1A priority Critical patent/CN113977602B/zh
Publication of CN113977602A publication Critical patent/CN113977602A/zh
Application granted granted Critical
Publication of CN113977602B publication Critical patent/CN113977602B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J11/00—Manipulators not otherwise provided for
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
    • B25J13/088—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices with position, velocity or acceleration sensors
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J17/00—Joints
    • B25J17/02—Wrist joints
    • B25J17/0258—Two-dimensional joints
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J18/00—Arms

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Human Computer Interaction (AREA)
  • Manipulator (AREA)

Abstract

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

Description

一种力反馈末端夹持器导纳控制方法
技术领域
本发明涉及穿刺手术机器人末端夹持器设计及机器人柔顺控制的技术领域，尤其是指一种力反馈末端夹持器导纳控制方法。
背景技术
随着机器人技术的不断发展与医学治疗手段的不断丰富，医疗机器人应运而生。医疗机器人是指各种应用于外科手术、康复治疗、残障人士辅助等医学用途的机器人设备，具有微创，精准，高效的特点。医疗机器人可以细分为手术机器人，康复机器人，辅助机器人等。其中穿刺手术机器人是手术机器人的一个重要分支，主要用于进行外科穿刺手术，如腰椎穿刺，血管穿刺，脏器穿刺等，对于相关疾病的诊断与恶性肿瘤的消融治疗有着重要作用。穿刺手术机器人一般由机械臂、空间定位系统、成像设施、工作站四大部分以及相应的软件组成。目前，穿刺手术机器人技术已经较为成熟，被广泛应用到临床中。机器人可辅助人工操作，降低穿刺手术难度，提高手术可靠性：所采用的光学导航系统可以辅助医生进行影像分析、路径规划，并对目标靶区进行精确定位与穿刺。手术机器人状态稳定，克服了传统手术易受医生个人操作和经验影响的缺点，提高手术的安全性和成功率。
然而，目前市场上的穿刺手术机器人仍然存在一定的局限性。目前成熟的穿刺手术机器人主要用于神经外科与骨科的穿刺手术，均无法对受呼吸运动影响的靶区，如肝，肺等进行精准的穿刺。因为这些部位的器官组织会随人体的呼吸运动发生位移和形变，从而导致目标靶区位置发生变化，降低系统的定位精度。此外，器官组织的生理运动还将给穿刺针造成挤压力，导致穿刺针发生弹性形变，偏离预定穿刺位置，导致穿刺失败。同时，器官组织与穿刺针之间的相互作用力还有可能对周围正常的人体器官组织造成损伤，甚至造成器官撕裂，降低手术的安全性，因此需设计可以适时调整的手术导航系统。仅依赖光学导航系统的穿刺机器人无法感知力信号并做出调整；并且由于穿刺针较长，仅使用一个三维力传感器并不能精准检测出穿刺针受力，因此需采用两个三维力传感器进行检测。目前，主流手术机器人系统和现行的穿刺手术技术均不能很好地解决上述问题。
发明内容
本发明的目的在于克服现有技术的缺点与不足，提出了一种力反馈末端夹持器导纳控制方法，可以实现对穿刺手术机器人末端穿刺针的零力跟随控制，减少因组织生理运动对穿刺手术造成的干扰，提高穿刺精准度，减少对周边器官组织的损伤，从而提升手术的精准性与安全性。
为实现上述目的，本发明所提供的技术方案为：一种力反馈末端夹持器导纳控制方法，该方法需要用到带有下部引导端三维力传感器和上部夹持端三维力传感器的力反馈末端夹持器及穿刺针手术工具，所述力反馈末端夹持器固定于六自由度机械臂末端，所述穿刺针手术工具固定在力反馈末端夹持器上，用于检测外界施加的作用力，当穿刺针手术工具受力时，力反馈末端夹持器上的两个三维力传感器能够实时采集到相应的力信号；该方法包括以下步骤：
S1、设计自重偏置滤除算法，该算法是以六自由度机械臂基座坐标系为基准，通过六自由度机械臂的运动排除力反馈末端执持器的重力干扰；在力反馈末端执持器的重力干扰滤除后，取上部夹持端三维力传感器的Z轴与下部引导端三维力传感器的X轴和Y轴构成组合坐标系Oc；
S2、将穿刺针手术工具在组合坐标系Oc下的受力[Fcx Fcy Fcz]输入到基于位置控制的导纳控制器中，获得期望位置与实际位置之差Xw，再与初始位置Xo进行叠加，求得下一时刻六自由度机械臂的期望位置Xf；
S3、结合六自由度机械臂的结构参数和DH表，将上述求得的六自由度机械臂的期望位置Xf进行逆向运动学转换，求得六自由度机械臂期望的关节角度信息q，再通过脚本函数控制六自由度机械臂到达期望位置，从而实现穿刺针手术工具的零力补偿。
进一步，所述S1包括以下步骤：
S101、根据六自由度机械臂的结构参数、位置信息和DH表，运用正向运动学求得六自由度机械臂基座坐标系下等效重力加速度gb和六自由度机械臂末端坐标系下等效重力加速度ge即力反馈末端执持器坐标系下等效重力加速度gt之间的转换关系：
Figure BDA0003323634920000031
其中，gt为力反馈末端执持器的等效重力加速度，ge为六自由度机械臂末端的等效重力加速度，gb为六自由度机械臂基座的等效重力加速度，Rbe表示六自由度机械臂基座坐标系和六自由度机械臂末端坐标系之间的转换关系；
其中，Xbe,Ybe,Zbe分别为绕X轴，Y轴，Z轴的旋转分量；
Figure BDA0003323634920000032
为Xbe的X轴坐标，
Figure BDA0003323634920000033
为Xbe的Y轴坐标，
Figure BDA0003323634920000034
是Xbe的Z轴坐标；
Figure BDA0003323634920000035
为Ybe的X轴坐标，
Figure BDA0003323634920000036
为Ybe的Y轴坐标，
Figure BDA0003323634920000037
是Ybe的Z轴坐标，
Figure BDA0003323634920000038
为Zbe的X轴坐标，
Figure BDA0003323634920000039
为Zbe的Y轴坐标，
Figure BDA00033236349200000310
是Zbe的Z轴坐标；具体计算方法如下：
Figure BDA0003323634920000041
其中，θ1,θ2,θ3,θ4,θ5,θ6均为六自由度机械臂当前位置的关节角度；
S102、将六自由度机械臂末端关节角度设置为10度，六自由度机械臂腕关节角度设置为300度，六自由度机械臂腕关节运动步长设置为5度，六自由度机械臂腕关节角度依次减去运动步长，并赋值给六自由度机械臂腕关节角度；
S103、将步骤S102求得的六自由度机械臂腕关节角度通过六自由度机械臂脚本函数控制六自由度机械臂运动至相应位置；
S104、在步骤S103的基础上，将组合坐标系下采集到的力信号拼接成向量Ft，将每一时刻力反馈末端执持器的等效重力加速度gt拼接成向量Gt；
S105、六自由度机械臂末端关节运动步长设置为5度，六自由度机械臂腕关节角度依次加上运动步长，并赋值给六自由度机械臂末端关节角度；
S106、将步骤S105求得的六自由度机械臂末端关节角度通过六自由度机械臂脚本函数控制六自由度机械臂运动至相应位置；
S107、结合步骤S103和S106，将组合坐标系下采集到的力信号拼接成向量Ft，将每一时刻力反馈末端执持器的等效重力加速度gt拼接成向量Gt；
S108、通过步骤S107求得的Ft和Gt，计算能够得力反馈末端执持器自身的重力干扰值的等效质量Mt为：
Figure BDA0003323634920000042
其中，
Figure BDA0003323634920000051
为Gt的转置向量；
S109、根据步骤S108求得的力反馈末端执持器自身的重力干扰值的等效质量，能够求得力反馈末端执持器自身的重力干扰值G为：
Figure BDA0003323634920000052
进一步，所述S2包括以下步骤：
S201、对于导纳控制器，其运动模型为：
Figure BDA0003323634920000053
其中M为惯性系数对角矩阵，B为阻力系数对角矩阵，K为刚度系数对角矩阵，Fc为所受外力，Xw为期望位置与实际位置之差，
Figure BDA0003323634920000054
分别为Xw的一阶微分与二阶微分；取力反馈末端夹持器下方引导端三维力传感器在Z轴与Y轴检测到的横向力作为Fcy,Fcz，力反馈末端夹持器上方引导端三维力传感器在X轴检测到的力为Fcx，此时Fc＝[Fcx Fcy Fcz]；
S202、零力矫正姿态与当前姿态的差值Δq作为位置控制器的输入信号，位置控制器根据Δq对六自由度机械臂各关节的转角进行控制直至Δq趋向于零，输出此时的关节角度信息q，并利用正运动学f(q)来求解出当前空间位置X；
接着，利用机器人的雅克比矩阵求解每一时刻姿态即关节角度信息q所对应的六自由度机械臂的末端运动速度
Figure BDA0003323634920000055
有：
Figure BDA0003323634920000056
Figure BDA0003323634920000057
其中，J(q)为机器人雅可比矩阵，
Figure BDA0003323634920000058
为六自由度机械臂末端的运动速度向量，
Figure BDA0003323634920000061
为六自由度机械臂各关节运动速度向量，
Figure BDA0003323634920000062
当中用
Figure BDA0003323634920000063
表示第m个关节的空间位置对第n个关节角度的偏导数，m、n＝1,2,3,4,5,6；当前空间位置X与六自由度机械臂末端运动速度
Figure BDA0003323634920000064
作为两个反馈信号输入到导纳控制器中；
S203、导纳控制是输入力Fc，输出期望位置与实际位置之差Xw；由
Figure BDA0003323634920000065
能够推导出方程
Figure BDA0003323634920000066
Figure BDA0003323634920000067
为Xw在n时刻的迭代值；对该方程进行降阶处理，采用变量替换的方法，令Yw为Xw的一阶微分，即六自由度机械臂的末端运动速度，
Figure BDA0003323634920000068
为Yw的一阶微分，即六自由度机械臂的末端运动加速度，有：
Figure BDA0003323634920000069
其中，M-1为M的逆矩阵；
S204、上式为一阶微分方程组，对方程进行离散化处理，采用龙格—库塔方法，求解步进值为Δt和Δt/2时两方程的四阶斜率估计值An、Bn、Cn、Dn、En、Fn、Gn、Hn，代入下式：
Figure BDA00033236349200000610
Figure BDA0003323634920000071
为Xw在n+1时刻的迭代值；
Figure BDA0003323634920000072
和
Figure BDA0003323634920000073
分别为
Figure BDA0003323634920000074
的一阶微分；由此求出某一时刻的期望位置与实际位置之差Xw，并与初始位置Xo进行叠加，进而求得期望位置Xf；
S205、应用导纳控制器的运动模型时，需确定模型的M、B、K的数值；对模型方程应用拉普拉斯变换，得到该模型的传递函数为：
Figure BDA0003323634920000075
其中，
Figure BDA0003323634920000076
ξ为阻尼比，惯性系数对角矩阵M的数值越大，响应越难发生突变，即作用力发生变化时，六自由度机械臂无法及时发生改变；M小于阈值则会导致六自由度机械臂易受微小扰动影响而不稳定；弹性系数对角矩阵K的数值越大，则六自由度机械臂的回弹现象越严重，难以实现零力补偿的效果；阻尼系数对角矩阵B有利于减弱反弹现象和抑制响应突变；通过在实际操作中不断调整这三个矩阵的参数，能够控制六自由度机械臂达到期望的零力补偿状态。
进一步，所述S3包括以下步骤：
S301、结合六自由度机械臂的结构参数、六自由度机械臂的位置信息和DH表将S2求得的下一时刻六自由度机械臂的期望位置Xf通过逆向运动学脚本函数控制机械臂实时求得六自由度机械臂的关节角度信息q；
S302、通过机械臂通讯协议实时传输六自由度机械臂的位置信息，并和目标位置信息进行作差运算，若差值的绝对值小于0.5，表示六自由度机械臂到达目标位置，否则一直通过机械臂的脚本函数控制机械臂运动。
本发明与现有技术相比，具有如下优点与有益效果：
1、本发明采用的自重偏置滤除算法有效滤除了夹持器自重对力信号采集的干扰，提高穿刺时针体受力采集的准确度。
2、本发明可以实现对穿刺手术机器人末端穿刺针的零力跟随控制。机器人可根据穿刺针受力情况自动进行受力补偿，调节姿态角度，从而避免穿刺针受到挤压发生弹性形变而偏离规划路径，进而提高穿刺精度。
3、零力跟随控制可减弱穿刺针与靶区周边组织的相互作用力，减少针体与器官组织之间的相互拉扯，避免对正常组织造成损伤，提高了手术的安全性。
附图说明
图1为本发明方法的逻辑流程示意图。
图2为本发明方法的实际应用场景图。
具体实施方式
下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。
参见图1和图2所示，本实施例所提供的力反馈末端夹持器导纳控制方法，需要用到带有下部引导端三维力传感器1和上部夹持端三维力传感器2的力反馈末端夹持器及穿刺针手术工具4，所述力反馈末端夹持器固定于六自由度机械臂3末端，所述穿刺针手术工具4固定在力反馈末端夹持器上，用于检测外界施加的作用力，当穿刺针手术工具4受力时，力反馈末端夹持器上的两个三维力传感器能够实时采集到相应的力信号；其具体包括以下步骤：
S1、设计自重偏置滤除算法，该算法是以六自由度机械臂基座坐标系为基准，通过六自由度机械臂的运动排除力反馈末端执持器的重力干扰；在力反馈末端执持器的重力干扰滤除后，取上部夹持端三维力传感器的Z轴与下部引导端三维力传感器的X轴和Y轴构成组合坐标系Oc，具体包括以下步骤：
S101、根据六自由度机械臂的结构参数、位置信息和DH表，运用正向运动学求得六自由度机械臂基座坐标系下等效重力加速度gb和六自由度机械臂末端坐标系下等效重力加速度ge即力反馈末端执持器坐标系下等效重力加速度gt之间的转换关系：
Figure BDA0003323634920000091
其中，gt为力反馈末端执持器的等效重力加速度，ge为六自由度机械臂末端的等效重力加速度，gb为六自由度机械臂基座的等效重力加速度，Rbe表示六自由度机械臂基座坐标系和六自由度机械臂末端坐标系之间的转换关系；
其中，Xbe,Ybe,Zbe分别为绕X轴，Y轴，Z轴的旋转分量；
Figure BDA0003323634920000092
为Xbe的X轴坐标，
Figure BDA0003323634920000093
为Xbe的Y轴坐标，
Figure BDA0003323634920000094
是Xbe的Z轴坐标；
Figure BDA0003323634920000095
为Ybe的X轴坐标，
Figure BDA0003323634920000096
为Ybe的Y轴坐标，
Figure BDA0003323634920000097
是Ybe的Z轴坐标，
Figure BDA0003323634920000098
为Zbe的X轴坐标，
Figure BDA0003323634920000099
为Zbe的Y轴坐标，
Figure BDA00033236349200000910
是Zbe的Z轴坐标；具体计算方法如下：
Figure BDA00033236349200000911
其中，θ1,θ2,θ3,θ4,θ5,θ6均为六自由度机械臂当前位置的关节角度；
S102、将六自由度机械臂末端关节角度设置为10度，六自由度机械臂腕关节角度设置为300度，六自由度机械臂腕关节运动步长设置为5度，六自由度机械臂腕关节角度依次减去运动步长，并赋值给六自由度机械臂腕关节角度；
S103、将步骤S102求得的六自由度机械臂腕关节角度通过六自由度机械臂脚本函数控制六自由度机械臂运动至相应位置；
S104、在步骤S103的基础上，将组合坐标系下采集到的力信号拼接成向量Ft，将每一时刻力反馈末端执持器的等效重力加速度gt拼接成向量Gt；
S105、六自由度机械臂末端关节运动步长设置为5度，六自由度机械臂腕关节角度依次加上运动步长，并赋值给六自由度机械臂末端关节角度；
S106、将步骤S105求得的六自由度机械臂末端关节角度通过六自由度机械臂脚本函数控制六自由度机械臂运动至相应位置；
S107、结合步骤S103和S106，将组合坐标系下采集到的力信号拼接成向量Ft，将每一时刻力反馈末端执持器的等效重力加速度gt拼接成向量Gt；
S108、通过步骤S107求得的Ft和Gt，计算能够得力反馈末端执持器自身的重力干扰值的等效质量Mt为：
Figure BDA0003323634920000101
其中，
Figure BDA0003323634920000102
为Gt的转置向量；
S109、根据步骤S108求得的力反馈末端执持器自身的重力干扰值的等效质量，能够求得力反馈末端执持器自身的重力干扰值G为：
Figure BDA0003323634920000103
S2、将穿刺针手术工具在组合坐标系Oc下的受力[Fcx Fcy Fcz]输入到基于位置控制的导纳控制器中，获得期望位置与实际位置之差Xw，再与初始位置Xo进行叠加，求得下一时刻六自由度机械臂的期望位置Xf，具体包括以下步骤：
S201、对于导纳控制器，其运动模型为：
Figure BDA0003323634920000104
其中M为惯性系数对角矩阵，B为阻力系数对角矩阵，K为刚度系数对角矩阵，Fc为所受外力，Xw为期望位置与实际位置之差，
Figure BDA0003323634920000111
分别为Xw的一阶微分与二阶微分；取力反馈末端夹持器下方引导端三维力传感器在Z轴与Y轴检测到的横向力作为Fcy,Fcz，力反馈末端夹持器上方引导端三维力传感器在X轴检测到的力为Fcx，此时Fc＝[Fcx Fcy Fcz]；
S202、零力矫正姿态与当前姿态的差值Δq作为位置控制器的输入信号，位置控制器根据Δq对六自由度机械臂各关节的转角进行控制直至Δq趋向于零，输出此时的关节角度信息q，并利用正运动学f(q)来求解出当前空间位置X；
接着，利用机器人的雅克比矩阵求解每一时刻姿态即关节角度信息q所对应的关节速度
Figure BDA0003323634920000112
有：
Figure BDA0003323634920000113
Figure BDA0003323634920000114
其中，J(q)为机器人雅可比矩阵，
Figure BDA0003323634920000115
为六自由度机械臂末端的运动速度向量，
Figure BDA0003323634920000116
为六自由度机械臂各关节运动速度向量，
Figure BDA0003323634920000117
当中用
Figure BDA0003323634920000118
表示第m个关节的空间位置对第n个关节角度的偏导数，m、n＝1,2,3,4,5,6；当前空间位置X与六自由度机械臂末端运动速度
Figure BDA0003323634920000119
作为两个反馈信号输入到导纳控制器中；
S203、导纳控制是输入力Fc，输出期望位置与实际位置之差Xw；由
Figure BDA0003323634920000121
能够推导出方程
Figure BDA0003323634920000122
Figure BDA0003323634920000123
为Xw在n时刻的迭代值；对该方程进行降阶处理，采用变量替换的方法，令Yw为Xw的一阶微分，即六自由度机械臂的末端运动速度，
Figure BDA0003323634920000124
为Yw的一阶微分，即六自由度机械臂的末端运动加速度，有：
Figure BDA0003323634920000125
其中，M-1为M的逆矩阵；
S204、上式为一阶微分方程组，对方程进行离散化处理，采用龙格—库塔方法，求解步进值为Δt和Δt/2时两方程的四阶斜率估计值An、Bn、Cn、Dn、En、Fn、Gn、Hn，代入下式：
Figure BDA0003323634920000126
Figure BDA0003323634920000127
为Xw在n+1时刻的迭代值；
Figure BDA0003323634920000128
和
Figure BDA0003323634920000129
分别为
Figure BDA00033236349200001210
的一阶微分；由此求出某一时刻的期望位置与实际位置之差Xw，并与初始位置Xo进行叠加，进而求得期望位置Xf；
S205、应用导纳控制器的运动模型时，需确定模型的M、B、K的数值；对模型方程应用拉普拉斯变换，得到该模型的传递函数为：
Figure BDA0003323634920000131
其中，
Figure BDA0003323634920000132
ξ为阻尼比，惯性系数对角矩阵M的数值越大，响应越难发生突变，即作用力发生变化时，六自由度机械臂无法及时发生改变；M过小则会导致六自由度机械臂易受微小扰动影响而不稳定；弹性系数对角矩阵K的数值越大，则六自由度机械臂的回弹现象越严重，难以实现零力补偿的效果；阻尼系数对角矩阵B有利于减弱反弹现象和抑制响应突变；通过在实际操作中不断调整这三个矩阵的参数，能够控制六自由度机械臂达到期望的零力补偿状态。
S3、结合六自由度机械臂的结构参数和DH表，将上述求得的六自由度机械臂的期望位置Xf进行逆向运动学转换，求得六自由度机械臂期望的关节角度信息q，再通过脚本函数控制六自由度机械臂到达期望位置，从而实现穿刺针手术工具的零力补偿，具体包括以下步骤：
S301、结合六自由度机械臂的结构参数、六自由度机械臂的位置信息和DH表将S2求得的下一时刻六自由度机械臂的期望位置Xf通过逆向运动学脚本函数控制机械臂实时求得六自由度机械臂的关节角度信息q；
S302、通过机械臂通讯协议实时传输六自由度机械臂的位置信息，并和目标位置信息进行作差运算，若差值的绝对值小于0.5，表示六自由度机械臂到达目标位置，否则一直通过机械臂的脚本函数控制机械臂运动。
上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (4)

1.一种力反馈末端夹持器导纳控制方法，其特征在于：该方法需要用到带有下部引导端三维力传感器和上部夹持端三维力传感器的力反馈末端夹持器及穿刺针手术工具，所述力反馈末端夹持器固定于六自由度机械臂末端，所述穿刺针手术工具固定在力反馈末端夹持器上，用于检测外界施加的作用力，当穿刺针手术工具受力时，力反馈末端夹持器上的两个三维力传感器能够实时采集到相应的力信号；该方法包括以下步骤：
S1、设计自重偏置滤除算法，该算法是以六自由度机械臂基座坐标系为基准，通过六自由度机械臂的运动排除力反馈末端执持器的重力干扰；在力反馈末端执持器的重力干扰滤除后，取上部夹持端三维力传感器的Z轴与下部引导端三维力传感器的X轴和Y轴构成组合坐标系Oc；
S2、将穿刺针手术工具在组合坐标系Oc下的受力[Fcx Fcy Fcz]输入到基于位置控制的导纳控制器中，获得期望位置与实际位置之差Xw，再与初始位置Xo进行叠加，求得下一时刻六自由度机械臂的期望位置Xf；
S3、结合六自由度机械臂的结构参数和DH表，将上述求得的六自由度机械臂的期望位置Xf进行逆向运动学转换，求得六自由度机械臂期望的关节角度信息q，再通过脚本函数控制六自由度机械臂到达期望位置，从而实现穿刺针手术工具的零力补偿。
2.根据权利要求1所述的一种力反馈末端夹持器导纳控制方法，其特征在于，所述S1包括以下步骤：
S101、根据六自由度机械臂的结构参数、位置信息和DH表，运用正向运动学求得六自由度机械臂基座坐标系下等效重力加速度gb和六自由度机械臂末端坐标系下等效重力加速度ge即力反馈末端执持器坐标系下等效重力加速度gt之间的转换关系：
Figure FDA0003323634910000021
其中，gt为力反馈末端执持器的等效重力加速度，ge为六自由度机械臂末端的等效重力加速度，gb为六自由度机械臂基座的等效重力加速度，Rbe表示六自由度机械臂基座坐标系和六自由度机械臂末端坐标系之间的转换关系；
其中，Xbe，Ybe，Zbe分别为绕X轴，Y轴，Z轴的旋转分量；
Figure FDA0003323634910000022
为Xbe的X轴坐标，
Figure FDA0003323634910000023
为Xbe的Y轴坐标，
Figure FDA0003323634910000024
是Xbe的Z轴坐标；
Figure FDA0003323634910000025
为Ybe的X轴坐标，
Figure FDA0003323634910000026
为Ybe的Y轴坐标，
Figure FDA0003323634910000027
是Ybe的Z轴坐标，
Figure FDA0003323634910000028
为Zbe的X轴坐标，
Figure FDA0003323634910000029
为Zbe的Y轴坐标，
Figure FDA00033236349100000210
是Zbe的Z轴坐标；具体计算方法如下：
Figure FDA00033236349100000211
其中，θ1，θ2，θ3，θ4，θ5，θ6均为六自由度机械臂当前位置的关节角度；
S102、将六自由度机械臂末端关节角度设置为10度，六自由度机械臂腕关节角度设置为300度，六自由度机械臂腕关节运动步长设置为5度，六自由度机械臂腕关节角度依次减去运动步长，并赋值给六自由度机械臂腕关节角度；
S103、将步骤S102求得的六自由度机械臂腕关节角度通过六自由度机械臂脚本函数控制六自由度机械臂运动至相应位置；
S104、在步骤S103的基础上，将组合坐标系下采集到的力信号拼接成向量Ft，将每一时刻力反馈末端执持器的等效重力加速度gt拼接成向量Gt；
S105、六自由度机械臂末端关节运动步长设置为5度，六自由度机械臂腕关节角度依次加上运动步长，并赋值给六自由度机械臂末端关节角度；
S106、将步骤S105求得的六自由度机械臂末端关节角度通过六自由度机械臂脚本函数控制六自由度机械臂运动至相应位置；
S107、结合步骤S103和S106，将组合坐标系下采集到的力信号拼接成向量Ft，将每一时刻力反馈末端执持器的等效重力加速度gt拼接成向量Gt；
S108、通过步骤S107求得的Ft和Gt，计算能够得力反馈末端执持器自身的重力干扰值的等效质量Mt为：
Figure FDA0003323634910000031
其中，
Figure FDA0003323634910000032
为Gt的转置向量；
S109、根据步骤S108求得的力反馈末端执持器自身的重力干扰值的等效质量，能够求得力反馈末端执持器自身的重力干扰值G为：
Figure FDA0003323634910000033
3.根据权利要求1所述的一种力反馈末端夹持器导纳控制方法，其特征在于，所述S2包括以下步骤：
S201、对于导纳控制器，其运动模型为：
Figure FDA0003323634910000034
其中M为惯性系数对角矩阵，B为阻力系数对角矩阵，K为刚度系数对角矩阵，Fc为所受外力，Xw为期望位置与实际位置之差，
Figure FDA0003323634910000035
分别为Xw的一阶微分与二阶微分；取力反馈末端夹持器下方引导端三维力传感器在Z轴与Y轴检测到的横向力作为Fcy，Fcz，力反馈末端夹持器上方引导端三维力传感器在X轴检测到的力为Fcx，此时Fc＝[Fcx Fcy Fcz]；
S202、零力矫正姿态与当前姿态的差值Δq作为位置控制器的输入信号，位置控制器根据Δq对六自由度机械臂各关节的转角进行控制直至Δq趋向于零，输出此时的关节角度信息q，并利用正运动学f(q)来求解出当前空间位置X；
接着，利用机器人的雅克比矩阵求解每一时刻姿态即关节角度信息q所对应的六自由度机械臂的末端运动速度
Figure FDA0003323634910000041
有：
Figure FDA0003323634910000042
Figure FDA0003323634910000043
其中，J(q)为机器人雅可比矩阵，
Figure FDA0003323634910000044
为六自由度机械臂末端的运动速度向量，
Figure FDA0003323634910000045
为六自由度机械臂各关节运动速度向量，
Figure FDA0003323634910000046
当中用
Figure FDA0003323634910000047
表示第m个关节的空间位置对第n个关节角度的偏导数，m、n＝1，2，3，4，5，6；当前空间位置X与六自由度机械臂末端运动速度
Figure FDA0003323634910000048
作为两个反馈信号输入到导纳控制器中；
S203、导纳控制是输入力Fc，输出期望位置与实际位置之差Xw；由
Figure FDA0003323634910000049
能够推导出方程
Figure FDA00033236349100000410
Figure FDA00033236349100000411
为Xw在n时刻的迭代值；对该方程进行降阶处理，采用变量替换的方法，令Yw为Xw的一阶微分，即六自由度机械臂的末端运动速度，
Figure FDA00033236349100000412
为Yw的一阶微分，即六自由度机械臂的末端运动加速度，有：
Figure FDA0003323634910000051
其中，M-1为M的逆矩阵；
S204、上式为一阶微分方程组，对方程进行离散化处理，采用龙格-库塔方法，求解步进值为Δt和Δt/2时两方程的四阶斜率估计值An、Bn、Cn、Dn、En、Fn、Gn、Hn，代入下式：
Figure FDA0003323634910000052
Figure FDA0003323634910000053
为Xw在n+1时刻的迭代值；
Figure FDA0003323634910000054
和
Figure FDA0003323634910000055
分别为
Figure FDA0003323634910000056
的一阶微分；由此求出某一时刻的期望位置与实际位置之差Xw，并与初始位置Xo进行叠加，进而求得期望位置Xf；
S205、应用导纳控制器的运动模型时，需确定模型的M、B、K的数值；对模型方程应用拉普拉斯变换，得到该模型的传递函数为：
Figure FDA0003323634910000057
其中，
Figure FDA0003323634910000058
ξ为阻尼比，惯性系数对角矩阵M的数值越大，响应越难发生突变，即作用力发生变化时，六自由度机械臂无法及时发生改变；M小于阈值则会导致六自由度机械臂易受微小扰动影响而不稳定；弹性系数对角矩阵K的数值越大，则六自由度机械臂的回弹现象越严重，难以实现零力补偿的效果；阻尼系数对角矩阵B有利于减弱反弹现象和抑制响应突变；通过在实际操作中不断调整这三个矩阵的参数，能够控制六自由度机械臂达到期望的零力补偿状态。
4.根据权利要求1所述的一种力反馈末端夹持器导纳控制方法，其特征在于，所述S3包括以下步骤：
S301、结合六自由度机械臂的结构参数、六自由度机械臂的位置信息和DH表将S2求得的下一时刻六自由度机械臂的期望位置Xf通过逆向运动学脚本函数控制机械臂实时求得六自由度机械臂的关节角度信息q；
S302、通过机械臂通讯协议实时传输六自由度机械臂的位置信息，并和目标位置信息进行作差运算，若差值的绝对值小于0.5，表示六自由度机械臂到达目标位置，否则一直通过机械臂的脚本函数控制机械臂运动。
CN202111263805.1A 2021-10-27 2021-10-27 一种力反馈末端夹持器导纳控制方法 Active CN113977602B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111263805.1A CN113977602B (zh) 2021-10-27 2021-10-27 一种力反馈末端夹持器导纳控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111263805.1A CN113977602B (zh) 2021-10-27 2021-10-27 一种力反馈末端夹持器导纳控制方法

Publications (2)

Publication Number Publication Date
CN113977602A true CN113977602A (zh) 2022-01-28
CN113977602B CN113977602B (zh) 2023-03-21

Family

ID=79743506

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111263805.1A Active CN113977602B (zh) 2021-10-27 2021-10-27 一种力反馈末端夹持器导纳控制方法

Country Status (1)

Country Link
CN (1) CN113977602B (zh)

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114770459A (zh) * 2022-05-18 2022-07-22 南京佗道医疗科技有限公司 一种主控制臂运动控制方法
CN114800442A (zh) * 2022-03-22 2022-07-29 华南理工大学 基于肌电信号和运动学冗余的机器人拖动示教系统与方法
CN116160300A (zh) * 2023-03-03 2023-05-26 东北大学 一种协作机器人磨抛工艺控制方法
CN116370810A (zh) * 2023-06-06 2023-07-04 浙江伽奈维医疗科技有限公司 远程遥控粒子植入装置
CN116442240A (zh) * 2023-05-26 2023-07-18 中山大学 一种基于高通滤波解耦的机器人零力控制方法及装置
CN116966416A (zh) * 2023-07-04 2023-10-31 上海交通大学医学院附属第九人民医院 一种用于远心点控制的人工耳蜗微电极植入系统及方法
CN117084794A (zh) * 2023-10-20 2023-11-21 北京航空航天大学 一种呼吸随动控制方法、装置和控制器
CN117204955A (zh) * 2023-11-06 2023-12-12 华东交通大学 柔性手术机器人的力控制系统及其装置
CN117464691A (zh) * 2023-12-27 2024-01-30 中国科学院自动化研究所 防反弹控制方法、装置、电子设备和存储介质

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20150366624A1 (en) * 2014-06-19 2015-12-24 KB Medical SA Systems and methods for performing minimally invasive surgery
US20200078097A1 (en) * 2016-10-25 2020-03-12 GYS Tech, LLC d/b/a Cardan Robotics Methods and Systems for Robot-Assisted Surgery
CN111249005A (zh) * 2020-03-20 2020-06-09 苏州新医智越机器人科技有限公司 一种穿刺手术机器人柔顺控制系统
CN111430022A (zh) * 2020-03-20 2020-07-17 苏州新医智越机器人科技有限公司 一种穿刺手术机器人穿刺算法
CN112057141A (zh) * 2020-08-19 2020-12-11 华南理工大学 一种智能反馈穿刺针执持器
CN112336461A (zh) * 2020-11-05 2021-02-09 苏州微创畅行机器人有限公司 手术机器人、控制方法、系统及可读存储介质
CN113305843A (zh) * 2021-05-28 2021-08-27 深圳亿嘉和科技研发有限公司 一种机械臂零力控制方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20150366624A1 (en) * 2014-06-19 2015-12-24 KB Medical SA Systems and methods for performing minimally invasive surgery
US20200078097A1 (en) * 2016-10-25 2020-03-12 GYS Tech, LLC d/b/a Cardan Robotics Methods and Systems for Robot-Assisted Surgery
CN111249005A (zh) * 2020-03-20 2020-06-09 苏州新医智越机器人科技有限公司 一种穿刺手术机器人柔顺控制系统
CN111430022A (zh) * 2020-03-20 2020-07-17 苏州新医智越机器人科技有限公司 一种穿刺手术机器人穿刺算法
CN112057141A (zh) * 2020-08-19 2020-12-11 华南理工大学 一种智能反馈穿刺针执持器
CN112336461A (zh) * 2020-11-05 2021-02-09 苏州微创畅行机器人有限公司 手术机器人、控制方法、系统及可读存储介质
CN113305843A (zh) * 2021-05-28 2021-08-27 深圳亿嘉和科技研发有限公司 一种机械臂零力控制方法

Cited By (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114800442A (zh) * 2022-03-22 2022-07-29 华南理工大学 基于肌电信号和运动学冗余的机器人拖动示教系统与方法
CN114800442B (zh) * 2022-03-22 2023-07-25 华南理工大学 基于肌电信号和运动学冗余的机器人拖动示教系统与方法
CN114770459A (zh) * 2022-05-18 2022-07-22 南京佗道医疗科技有限公司 一种主控制臂运动控制方法
CN114770459B (zh) * 2022-05-18 2023-12-08 佗道医疗科技有限公司 一种主控制臂运动控制方法
CN116160300A (zh) * 2023-03-03 2023-05-26 东北大学 一种协作机器人磨抛工艺控制方法
CN116160300B (zh) * 2023-03-03 2024-04-16 东北大学 一种协作机器人磨抛工艺控制方法
CN116442240B (zh) * 2023-05-26 2023-11-14 中山大学 一种基于高通滤波解耦的机器人零力控制方法及装置
CN116442240A (zh) * 2023-05-26 2023-07-18 中山大学 一种基于高通滤波解耦的机器人零力控制方法及装置
CN116370810B (zh) * 2023-06-06 2023-09-26 浙江伽奈维医疗科技有限公司 远程遥控粒子植入装置
CN116370810A (zh) * 2023-06-06 2023-07-04 浙江伽奈维医疗科技有限公司 远程遥控粒子植入装置
CN116966416A (zh) * 2023-07-04 2023-10-31 上海交通大学医学院附属第九人民医院 一种用于远心点控制的人工耳蜗微电极植入系统及方法
CN117084794A (zh) * 2023-10-20 2023-11-21 北京航空航天大学 一种呼吸随动控制方法、装置和控制器
CN117084794B (zh) * 2023-10-20 2024-02-06 北京航空航天大学 一种呼吸随动控制方法、装置和控制器
CN117204955A (zh) * 2023-11-06 2023-12-12 华东交通大学 柔性手术机器人的力控制系统及其装置
CN117464691A (zh) * 2023-12-27 2024-01-30 中国科学院自动化研究所 防反弹控制方法、装置、电子设备和存储介质
CN117464691B (zh) * 2023-12-27 2024-03-22 中国科学院自动化研究所 防反弹控制方法、装置、电子设备和存储介质

Also Published As

Publication number Publication date
CN113977602B (zh) 2023-03-21

Similar Documents

Publication Publication Date Title
CN113977602B (zh) 一种力反馈末端夹持器导纳控制方法
US11679499B2 (en) Systems and methods for controlling a robotic manipulator or associated tool
US11712798B2 (en) Systems and methods for controlling a robotic manipulator or associated tool
EP3684280B1 (en) Systems and methods for controlling a robotic manipulator or associated tool
KR102438357B1 (ko) 컴퓨터 보조 의료 시스템 및 방법
Zemiti et al. Mechatronic design of a new robot for force control in minimally invasive surgery
Tavakoli et al. Haptics for teleoperated surgical robotic systems
Ganji et al. Catheter kinematics for intracardiac navigation
EP3685787B1 (en) Surgical robot system
TWI695765B (zh) 機械手臂
EP2706943A1 (en) Medical master/slave type device for minimally invasive surgery
CN113742992B (zh) 基于深度学习的主从控制方法及应用
US20230028689A1 (en) System and method for inter-arm registration
Yan et al. UKF-based motion estimation of cable-driven forceps for robot-assisted surgical system
KR20180035669A (ko) 상지 다관절 임피던스 측정 방법 및 그 장치
Ren et al. The critical technologies of vascular interventional robotic catheterization: A Review
CN111358659B (zh) 一种机器人的助力控制方法、系统及下肢康复机器人
CN115300110A (zh) 内窥镜手术控制系统
CN114452004B (zh) 一种手术机器人末端位置和姿态的控制方法
Bai et al. A humanoid robotic hand capable of internal assembly and measurement in spacesuit gloves
CN110584790B (zh) 基于手臂刚度的手术机器人遥操作比例控制方法
JP2022549347A (ja) 異常なセンサ信号の存在下で可撓性デバイスを制御するための方法及びシステム
JP2003211377A (ja) マニピュレーション動作支援装置及びその支援方法
Guo et al. A study on telecommunication technology and remote-control algorithm in minimally invasive surgical robotic system
CN114948586B (zh) 一种下肢康复机器人的人机系统运动学参数辨识及控制方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant