CN113977602B - 一种力反馈末端夹持器导纳控制方法 - Google Patents
一种力反馈末端夹持器导纳控制方法 Download PDFInfo
- Publication number
- CN113977602B CN113977602B CN202111263805.1A CN202111263805A CN113977602B CN 113977602 B CN113977602 B CN 113977602B CN 202111263805 A CN202111263805 A CN 202111263805A CN 113977602 B CN113977602 B CN 113977602B
- Authority
- CN
- China
- Prior art keywords
- degree
- mechanical arm
- freedom mechanical
- force
- freedom
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 26
- 230000001133 acceleration Effects 0.000 claims description 27
- 239000011159 matrix material Substances 0.000 claims description 27
- 239000012636 effector Substances 0.000 claims description 21
- 210000003857 wrist joint Anatomy 0.000 claims description 16
- 230000005484 gravity Effects 0.000 claims description 11
- 238000006243 chemical reaction Methods 0.000 claims description 9
- 230000008859 change Effects 0.000 claims description 7
- 238000013016 damping Methods 0.000 claims description 6
- 230000004044 response Effects 0.000 claims description 6
- 238000004422 calculation algorithm Methods 0.000 claims description 5
- 238000001914 filtration Methods 0.000 claims description 5
- 238000004364 calculation method Methods 0.000 claims description 3
- 238000004891 communication Methods 0.000 claims description 3
- 238000012937 correction Methods 0.000 claims description 3
- 230000000694 effects Effects 0.000 claims description 3
- 230000036961 partial effect Effects 0.000 claims description 3
- 238000012546 transfer Methods 0.000 claims description 3
- 230000003313 weakening effect Effects 0.000 claims description 3
- 230000009286 beneficial effect Effects 0.000 claims description 2
- 230000002441 reversible effect Effects 0.000 claims 2
- 241001123248 Arma Species 0.000 claims 1
- 230000004069 differentiation Effects 0.000 claims 1
- 230000002401 inhibitory effect Effects 0.000 claims 1
- 230000035772 mutation Effects 0.000 claims 1
- 238000001356 surgical procedure Methods 0.000 abstract description 14
- 210000000056 organ Anatomy 0.000 description 8
- 238000013461 design Methods 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 3
- 238000006467 substitution reaction Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 230000005489 elastic deformation Effects 0.000 description 2
- 230000003993 interaction Effects 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 230000001629 suppression Effects 0.000 description 2
- 238000011282 treatment Methods 0.000 description 2
- 238000011298 ablation treatment Methods 0.000 description 1
- 210000004204 blood vessel Anatomy 0.000 description 1
- 201000011510 cancer Diseases 0.000 description 1
- 230000007812 deficiency Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000003745 diagnosis Methods 0.000 description 1
- 201000010099 disease Diseases 0.000 description 1
- 208000037265 diseases, disorders, signs and symptoms Diseases 0.000 description 1
- 238000001125 extrusion Methods 0.000 description 1
- 238000010191 image analysis Methods 0.000 description 1
- 238000003384 imaging method Methods 0.000 description 1
- 210000004185 liver Anatomy 0.000 description 1
- 238000009593 lumbar puncture Methods 0.000 description 1
- 210000004072 lung Anatomy 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000000399 orthopedic effect Effects 0.000 description 1
- 230000000241 respiratory effect Effects 0.000 description 1
- 230000029058 respiratory gaseous exchange Effects 0.000 description 1
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之间的转换关系：
其中，gt为力反馈末端执持器的等效重力加速度，ge为六自由度机械臂末端的等效重力加速度，gb为六自由度机械臂基座的等效重力加速度，Rbe表示六自由度机械臂基座坐标系和六自由度机械臂末端坐标系之间的转换关系；
其中，Xbe,Ybe,Zbe分别为绕X轴，Y轴，Z轴的旋转分量；
为Xbe的X轴坐标，
为Xbe的Y轴坐标，
是Xbe的Z轴坐标；
为Ybe的X轴坐标，
为Ybe的Y轴坐标，
是Ybe的Z轴坐标，
为Zbe的X轴坐标，
为Zbe的Y轴坐标，
是Zbe的Z轴坐标；具体计算方法如下：
其中，θ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为：
其中，
为Gt的转置向量；
S109、根据步骤S108求得的力反馈末端执持器自身的重力干扰值的等效质量，能够求得力反馈末端执持器自身的重力干扰值G为：
进一步，所述S2包括以下步骤：
S201、对于导纳控制器，其运动模型为：
其中M为惯性系数对角矩阵，B为阻力系数对角矩阵，K为刚度系数对角矩阵，Fc为所受外力，Xw为期望位置与实际位置之差，
分别为Xw的一阶微分与二阶微分；取力反馈末端夹持器下方引导端三维力传感器在Z轴与Y轴检测到的横向力作为Fcy,Fcz，力反馈末端夹持器上方引导端三维力传感器在X轴检测到的力为Fcx，此时Fc＝[Fcx Fcy Fcz]；
S202、零力矫正姿态与当前姿态的差值Δq作为位置控制器的输入信号，位置控制器根据Δq对六自由度机械臂各关节的转角进行控制直至Δq趋向于零，输出此时的关节角度信息q，并利用正运动学f(q)来求解出当前空间位置X；
接着，利用机器人的雅克比矩阵求解每一时刻姿态即关节角度信息q所对应的六自由度机械臂的末端运动速度
有：
其中，J(q)为机器人雅可比矩阵，
为六自由度机械臂末端的运动速度向量，
为六自由度机械臂各关节运动速度向量，
当中用
表示第m个关节的空间位置对第n个关节角度的偏导数，m、n＝1,2,3,4,5,6；当前空间位置X与六自由度机械臂末端运动速度
作为两个反馈信号输入到导纳控制器中；
S203、导纳控制是输入力Fc，输出期望位置与实际位置之差Xw；由
能够推导出方程
为Xw在n时刻的迭代值；对该方程进行降阶处理，采用变量替换的方法，令Yw为Xw的一阶微分，即六自由度机械臂的末端运动速度，
为Yw的一阶微分，即六自由度机械臂的末端运动加速度，有：
其中，M-1为M的逆矩阵；
S204、上式为一阶微分方程组，对方程进行离散化处理，采用龙格—库塔方法，求解步进值为Δt和Δt/2时两方程的四阶斜率估计值An、Bn、Cn、Dn、En、Fn、Gn、Hn，代入下式：
S205、应用导纳控制器的运动模型时，需确定模型的M、B、K的数值；对模型方程应用拉普拉斯变换，得到该模型的传递函数为：
其中，
ξ为阻尼比，惯性系数对角矩阵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之间的转换关系：
其中，gt为力反馈末端执持器的等效重力加速度，ge为六自由度机械臂末端的等效重力加速度，gb为六自由度机械臂基座的等效重力加速度，Rbe表示六自由度机械臂基座坐标系和六自由度机械臂末端坐标系之间的转换关系；
其中，Xbe,Ybe,Zbe分别为绕X轴，Y轴，Z轴的旋转分量；
为Xbe的X轴坐标，
为Xbe的Y轴坐标，
是Xbe的Z轴坐标；
为Ybe的X轴坐标，
为Ybe的Y轴坐标，
是Ybe的Z轴坐标，
为Zbe的X轴坐标，
为Zbe的Y轴坐标，
是Zbe的Z轴坐标；具体计算方法如下：
其中，θ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为：
其中，
为Gt的转置向量；
S109、根据步骤S108求得的力反馈末端执持器自身的重力干扰值的等效质量，能够求得力反馈末端执持器自身的重力干扰值G为：
S2、将穿刺针手术工具在组合坐标系Oc下的受力[Fcx Fcy Fcz]输入到基于位置控制的导纳控制器中，获得期望位置与实际位置之差Xw，再与初始位置Xo进行叠加，求得下一时刻六自由度机械臂的期望位置Xf，具体包括以下步骤：
S201、对于导纳控制器，其运动模型为：
其中M为惯性系数对角矩阵，B为阻力系数对角矩阵，K为刚度系数对角矩阵，Fc为所受外力，Xw为期望位置与实际位置之差，
分别为Xw的一阶微分与二阶微分；取力反馈末端夹持器下方引导端三维力传感器在Z轴与Y轴检测到的横向力作为Fcy,Fcz，力反馈末端夹持器上方引导端三维力传感器在X轴检测到的力为Fcx，此时Fc＝[Fcx Fcy Fcz]；
S202、零力矫正姿态与当前姿态的差值Δq作为位置控制器的输入信号，位置控制器根据Δq对六自由度机械臂各关节的转角进行控制直至Δq趋向于零，输出此时的关节角度信息q，并利用正运动学f(q)来求解出当前空间位置X；
接着，利用机器人的雅克比矩阵求解每一时刻姿态即关节角度信息q所对应的关节速度
有：
其中，J(q)为机器人雅可比矩阵，
为六自由度机械臂末端的运动速度向量，
为六自由度机械臂各关节运动速度向量，
当中用
表示第m个关节的空间位置对第n个关节角度的偏导数，m、n＝1,2,3,4,5,6；当前空间位置X与六自由度机械臂末端运动速度
作为两个反馈信号输入到导纳控制器中；
S203、导纳控制是输入力Fc，输出期望位置与实际位置之差Xw；由
能够推导出方程
为Xw在n时刻的迭代值；对该方程进行降阶处理，采用变量替换的方法，令Yw为Xw的一阶微分，即六自由度机械臂的末端运动速度，
为Yw的一阶微分，即六自由度机械臂的末端运动加速度，有：
其中，M-1为M的逆矩阵；
S204、上式为一阶微分方程组，对方程进行离散化处理，采用龙格—库塔方法，求解步进值为Δt和Δt/2时两方程的四阶斜率估计值An、Bn、Cn、Dn、En、Fn、Gn、Hn，代入下式：
S205、应用导纳控制器的运动模型时，需确定模型的M、B、K的数值；对模型方程应用拉普拉斯变换，得到该模型的传递函数为：
其中，
ξ为阻尼比，惯性系数对角矩阵M的数值越大，响应越难发生突变，即作用力发生变化时，六自由度机械臂无法及时发生改变；M过小则会导致六自由度机械臂易受微小扰动影响而不稳定；弹性系数对角矩阵K的数值越大，则六自由度机械臂的回弹现象越严重，难以实现零力补偿的效果；阻尼系数对角矩阵B有利于减弱反弹现象和抑制响应突变；通过在实际操作中不断调整这三个矩阵的参数，能够控制六自由度机械臂达到期望的零力补偿状态。
S3、结合六自由度机械臂的结构参数和DH表，将上述求得的六自由度机械臂的期望位置Xf进行逆向运动学转换，求得六自由度机械臂期望的关节角度信息q，再通过脚本函数控制六自由度机械臂到达期望位置，从而实现穿刺针手术工具的零力补偿，具体包括以下步骤：
S301、结合六自由度机械臂的结构参数、六自由度机械臂的位置信息和DH表将S2求得的下一时刻六自由度机械臂的期望位置Xf通过逆向运动学脚本函数控制机械臂实时求得六自由度机械臂的关节角度信息q；
S302、通过机械臂通讯协议实时传输六自由度机械臂的位置信息，并和目标位置信息进行作差运算，若差值的绝对值小于0.5，表示六自由度机械臂到达目标位置，否则一直通过机械臂的脚本函数控制机械臂运动。
上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。
Claims (3)
1.一种力反馈末端夹持器导纳控制方法，其特征在于：该方法需要用到带有下部引导端三维力传感器和上部夹持端三维力传感器的力反馈末端夹持器及穿刺针手术工具，所述力反馈末端夹持器固定于六自由度机械臂末端，所述穿刺针手术工具固定在力反馈末端夹持器上，用于检测外界施加的作用力，当穿刺针手术工具受力时，力反馈末端夹持器上的两个三维力传感器能够实时采集到相应的力信号；该方法包括以下步骤：
S1、设计自重偏置滤除算法，该算法是以六自由度机械臂基座坐标系为基准，通过六自由度机械臂的运动排除力反馈末端执持器的重力干扰；在力反馈末端执持器的重力干扰滤除后，取上部夹持端三维力传感器的Z轴与下部引导端三维力传感器的X轴和Y轴构成组合坐标系Oc；包括以下步骤：
S101、根据六自由度机械臂的结构参数、位置信息和DH表，运用正向运动学求得六自由度机械臂基座坐标系下等效重力加速度gb和六自由度机械臂末端坐标系下等效重力加速度ge即力反馈末端执持器坐标系下等效重力加速度gt之间的转换关系：
其中，gt为力反馈末端执持器的等效重力加速度，ge为六自由度机械臂末端的等效重力加速度，gb为六自由度机械臂基座的等效重力加速度，Rbe表示六自由度机械臂基座坐标系和六自由度机械臂末端坐标系之间的转换关系；
其中，Xbe,Ybe,Zbe分别为绕X轴，Y轴，Z轴的旋转分量；
为Xbe的X轴坐标，
为Xbe的Y轴坐标，
是Xbe的Z轴坐标；
为Ybe的X轴坐标，
为Ybe的Y轴坐标，
是Ybe的Z轴坐标，
为Zbe的X轴坐标，
为Zbe的Y轴坐标，
是Zbe的Z轴坐标；具体计算方法如下：
其中，θ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为：
其中，
为Gt的转置向量；
S109、根据步骤S108求得的力反馈末端执持器自身的重力干扰值的等效质量，能够求得力反馈末端执持器自身的重力干扰值G为：
S2、将穿刺针手术工具在组合坐标系Oc下的受力[Fcx Fcy Fcz]输入到基于位置控制的导纳控制器中，获得某一时刻的期望位置与实际位置之差Xw，再与初始位置Xo进行叠加，求得下一时刻六自由度机械臂的期望位置Xf；
S3、结合六自由度机械臂的结构参数和DH表，将上述求得的六自由度机械臂的期望位置Xf进行逆向运动学转换，求得六自由度机械臂期望的关节角度信息q，再通过脚本函数控制六自由度机械臂到达期望位置，从而实现穿刺针手术工具的零力补偿。
2.根据权利要求1所述的一种力反馈末端夹持器导纳控制方法，其特征在于，所述S2包括以下步骤：
S201、对于导纳控制器，其运动模型为：
其中M为惯性系数对角矩阵，B为阻力系数对角矩阵，K为刚度系数对角矩阵，Fc为所受外力，Xw为期望位置与实际位置之差，
分别为Xw的一阶微分与二阶微分；取力反馈末端夹持器下方引导端三维力传感器在Z轴与Y轴检测到的横向力作为Fcy,Fcz，力反馈末端夹持器上方引导端三维力传感器在X轴检测到的力为Fcx，此时Fc＝[Fcx Fcy Fcz]；
S202、零力矫正姿态与当前姿态的差值Δq作为位置控制器的输入信号，位置控制器根据Δq对六自由度机械臂各关节的转角进行控制直至Δq趋向于零，输出此时的关节角度信息q，并利用正运动学f(q)来求解出当前空间位置X；
接着，利用机器人的雅克比矩阵求解每一时刻姿态即关节角度信息q所对应的六自由度机械臂的末端运动速度
有：
其中，J(q)为机器人雅可比矩阵，
为六自由度机械臂末端的运动速度向量，
为六自由度机械臂各关节运动速度向量，
当中用
表示第m个关节的空间位置对第n个关节角度的偏导数，m、n＝1,2,3,4,5,6；当前空间位置X与六自由度机械臂末端运动速度
作为两个反馈信号输入到导纳控制器中；
S203、导纳控制是输入力Fc，输出期望位置与实际位置之差Xw；由
能够推导出方程
为Xw在n时刻的迭代值；对该方程进行降阶处理，采用变量替换的方法，令Yw为Xw的一阶微分，即六自由度机械臂的末端运动速度，
为Yw的一阶微分，即六自由度机械臂的末端运动加速度，有：
其中，M-1为M的逆矩阵；
S204、上式为一阶微分方程组，对方程进行离散化处理，采用龙格—库塔方法，求解步进值为Δt和Δt/2时两方程的四阶斜率估计值An、Bn、Cn、Dn、En、Fn、Gn、Hn，代入下式：
S205、应用导纳控制器的运动模型时，需确定模型的M、B、K的数值；对模型方程应用拉普拉斯变换，得到该模型的传递函数为：
其中，
ξ为阻尼比，惯性系数对角矩阵M的数值越大，响应越难发生突变，即作用力发生变化时，六自由度机械臂无法及时发生改变；M小于阈值则会导致六自由度机械臂易受微小扰动影响而不稳定；弹性系数对角矩阵K的数值越大，则六自由度机械臂的回弹现象越严重，难以实现零力补偿的效果；阻尼系数对角矩阵B有利于减弱反弹现象和抑制响应突变；通过在实际操作中不断调整这三个矩阵的参数，能够控制六自由度机械臂达到期望的零力补偿状态。
3.根据权利要求1所述的一种力反馈末端夹持器导纳控制方法，其特征在于，所述S3包括以下步骤：
S301、结合六自由度机械臂的结构参数、六自由度机械臂的位置信息和DH表将S2求得的下一时刻六自由度机械臂的期望位置Xf通过逆向运动学脚本函数控制机械臂实时求得六自由度机械臂的关节角度信息q；
S302、通过机械臂通讯协议实时传输六自由度机械臂的位置信息，并和目标位置信息进行作差运算，若差值的绝对值小于0.5，表示六自由度机械臂到达目标位置，否则一直通过机械臂的脚本函数控制机械臂运动。
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 CN113977602A (zh) | 2022-01-28 |
| CN113977602B true 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) |
Families Citing this family (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114800442B (zh) * | 2022-03-22 | 2023-07-25 | 华南理工大学 | 基于肌电信号和运动学冗余的机器人拖动示教系统与方法 |
| CN114770459B (zh) * | 2022-05-18 | 2023-12-08 | 佗道医疗科技有限公司 | 一种主控制臂运动控制方法 |
| CN115446832B (zh) * | 2022-08-29 | 2025-02-07 | 北京空间飞行器总体设计部 | 一种可重构机械臂操作力突变抑制控制方法 |
| CN116160300B (zh) * | 2023-03-03 | 2024-04-16 | 东北大学 | 一种协作机器人磨抛工艺控制方法 |
| CN116442240B (zh) * | 2023-05-26 | 2023-11-14 | 中山大学 | 一种基于高通滤波解耦的机器人零力控制方法及装置 |
| CN116370810B (zh) * | 2023-06-06 | 2023-09-26 | 浙江伽奈维医疗科技有限公司 | 远程遥控粒子植入装置 |
| CN116966416B (zh) * | 2023-07-04 | 2024-06-21 | 上海交通大学医学院附属第九人民医院 | 一种用于远心点控制的人工耳蜗微电极植入系统及方法 |
| CN117084794B (zh) * | 2023-10-20 | 2024-02-06 | 北京航空航天大学 | 一种呼吸随动控制方法、装置和控制器 |
| CN117204955A (zh) * | 2023-11-06 | 2023-12-12 | 华东交通大学 | 柔性手术机器人的力控制系统及其装置 |
| CN117464691B (zh) * | 2023-12-27 | 2024-03-22 | 中国科学院自动化研究所 | 防反弹控制方法、装置、电子设备和存储介质 |
Family Cites Families (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP3157446B1 (en) * | 2014-06-19 | 2018-08-15 | KB Medical SA | Systems for performing minimally invasive surgery |
| WO2018081136A2 (en) * | 2016-10-25 | 2018-05-03 | Eugene Gregerson | Methods and systems for robot-assisted surgery |
| CN111249005A (zh) * | 2020-03-20 | 2020-06-09 | 苏州新医智越机器人科技有限公司 | 一种穿刺手术机器人柔顺控制系统 |
| CN111430022A (zh) * | 2020-03-20 | 2020-07-17 | 苏州新医智越机器人科技有限公司 | 一种穿刺手术机器人穿刺算法 |
| CN112057141B (zh) * | 2020-08-19 | 2021-08-10 | 华南理工大学 | 一种智能反馈穿刺针执持器 |
| CN115153852A (zh) * | 2020-11-05 | 2022-10-11 | 苏州微创畅行机器人有限公司 | 手术机器人、控制方法、系统及可读存储介质 |
| CN113305843A (zh) * | 2021-05-28 | 2021-08-27 | 深圳亿嘉和科技研发有限公司 | 一种机械臂零力控制方法 |
-
2021
- 2021-10-27 CN CN202111263805.1A patent/CN113977602B/zh active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN113977602A (zh) | 2022-01-28 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113977602B (zh) | 一种力反馈末端夹持器导纳控制方法 | |
| US12133700B2 (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 | |
| US12194635B2 (en) | Systems and methods for controlling a robotic manipulator or associated tool | |
| JP7522777B2 (ja) | コンピュータ支援医療システム及び方法 | |
| KR102252641B1 (ko) | 모드 전환 시의 진동을 감쇠시키기 위한 명령 성형 | |
| Zemiti et al. | Mechatronic design of a new robot for force control in minimally invasive surgery | |
| Tavakoli | Haptics for teleoperated surgical robotic systems | |
| CN111773027B (zh) | 一种柔性驱动的手功能康复机器人控制系统及控制方法 | |
| KR102145236B1 (ko) | 영-공간을 이용하여 매니퓰레이터 암 대 환자의 충돌을 회피하는 방법 및 시스템 | |
| KR101901580B1 (ko) | 수술 로봇 및 그 제어 방법 | |
| US20100161129A1 (en) | System and method for adjusting an image capturing device attribute using an unused degree-of-freedom of a master control device | |
| CN105073058A (zh) | 用于通过在零垂直空间内进行咬合同时发生零空间移动而定位操纵器臂的系统和方法 | |
| KR20150045468A (ko) | 로봇 암의 수동식 운동에 의해 제어되는 이동가능한 수술용 장착 플랫폼 | |
| Wang et al. | Mirror therapy with an exoskeleton upper-limb robot based on IMU measurement system | |
| WO2012153152A1 (en) | Medical master/slave type device for minimally invasive surgery | |
| Peng et al. | A novel force feedback interventional surgery robotic system | |
| Ren et al. | The critical technologies of vascular interventional robotic catheterization: A Review | |
| CN113742992B (zh) | 基于深度学习的主从控制方法及应用 | |
| CN113171177A (zh) | 可捕捉腰椎穿刺组织层突破感的人机交互控制方法及系统 | |
| Guo et al. | A study on telecommunication technology and remote-control algorithm in minimally invasive surgical robotic system | |
| CN119139009A (zh) | 一种手术机器人、控制方法及计算机可读存储介质 | |
| CN119139010A (zh) | 一种手术机器人、控制方法及计算机可读存储介质 | |
| CN115139299A (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 | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20241219 Address after: 510641 Industrial Building, Wushan South China University of Technology, Tianhe District, Guangzhou City, Guangdong Province Patentee after: Guangzhou South China University of Technology Asset Management Co.,Ltd. Country or region after: China Address before: 510640 No. five, 381 mountain road, Guangzhou, Guangdong, Tianhe District Patentee before: SOUTH CHINA University OF TECHNOLOGY Country or region before: China |
|
| TR01 | Transfer of patent right | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20250103 Address after: Room 101, No. 30-6 Kaitai Avenue, Huangpu District, Guangzhou City, Guangdong Province 510530 Patentee after: GUANGZHOU AIMUYI TECHNOLOGY CO.,LTD. Country or region after: China Address before: 510641 Industrial Building, Wushan South China University of Technology, Tianhe District, Guangzhou City, Guangdong Province Patentee before: Guangzhou South China University of Technology Asset Management Co.,Ltd. Country or region before: China |
|
| TR01 | Transfer of patent right |