CN116392253A - 一种应用于手术机器人的主动摆位方法及系统 - Google Patents

一种应用于手术机器人的主动摆位方法及系统 Download PDF

Info

Publication number
CN116392253A
CN116392253A CN202310419921.0A CN202310419921A CN116392253A CN 116392253 A CN116392253 A CN 116392253A CN 202310419921 A CN202310419921 A CN 202310419921A CN 116392253 A CN116392253 A CN 116392253A
Authority
CN
China
Prior art keywords
mechanical arm
joint
gravity
term
angular velocity
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.)
Pending
Application number
CN202310419921.0A
Other languages
English (en)
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.)
Shanghai Zhuoxin Medical Technology Co ltd
Original Assignee
Shanghai Zhuoxin Medical Technology Co ltd
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 Shanghai Zhuoxin Medical Technology Co ltd filed Critical Shanghai Zhuoxin Medical Technology Co ltd
Priority to CN202310419921.0A priority Critical patent/CN116392253A/zh
Publication of CN116392253A publication Critical patent/CN116392253A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/30—Surgical robots
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/70—Manipulators specially adapted for use in surgery
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B90/00—Instruments, implements or accessories specially adapted for surgery or diagnosis and not covered by any of the groups A61B1/00 - A61B50/00, e.g. for luxation treatment or for protecting wound edges
    • A61B90/06—Measuring instruments not otherwise provided for
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/1633—Programme controls characterised by the control loop compliant, force, torque control, e.g. combined with position control
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/30—Surgical robots
    • A61B2034/305—Details of wrist mechanisms at distal ends of robotic arms
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B90/00—Instruments, implements or accessories specially adapted for surgery or diagnosis and not covered by any of the groups A61B1/00 - A61B50/00, e.g. for luxation treatment or for protecting wound edges
    • A61B90/06—Measuring instruments not otherwise provided for
    • A61B2090/067—Measuring instruments not otherwise provided for for measuring angles

Landscapes

  • Engineering & Computer Science (AREA)
  • Health & Medical Sciences (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Surgery (AREA)
  • Robotics (AREA)
  • Medical Informatics (AREA)
  • Biomedical Technology (AREA)
  • Heart & Thoracic Surgery (AREA)
  • Nuclear Medicine, Radiotherapy & Molecular Imaging (AREA)
  • Molecular Biology (AREA)
  • Animal Behavior & Ethology (AREA)
  • General Health & Medical Sciences (AREA)
  • Public Health (AREA)
  • Veterinary Medicine (AREA)
  • Mechanical Engineering (AREA)
  • Oral & Maxillofacial Surgery (AREA)
  • Pathology (AREA)
  • Manipulator (AREA)

Abstract

本发明涉及手术机器人技术领域，提供了一种应用于手术机器人的主动摆位方法，包括：S1：在机械臂运行过程中采集机械臂轴关节的关节角度，以及计算关节角速度；S2：依据关节角度计算重力辨识项，依据关节角速度计算库伦摩擦力辨识项，依据关节角速度计算粘滞摩擦力辨识项；S3：基于重力辨识项、库伦摩擦力辨识项以及粘滞摩擦力辨识项，计算为机械臂轴关节的电机进行补偿的电流，以减少机械臂摆位过程中使用者施加的力。上述技术方案，采用基于电流的方式进行拖动助力补偿，不需要额外的力矩传感器，只需要辨识拖动过程中产生的重力、库伦摩擦力以及粘滞摩擦力，就可以准确地获得外力与角度和角速度的关系，实现主动摆位助力功能。

Description

一种应用于手术机器人的主动摆位方法及系统
技术领域
本发明涉及手术机器人的技术领域，尤其涉及一种应用于手术机器人的主动摆位方法及系统。
背景技术
手术机器人是一种可以协助医生进行手术的机器人系统。它们通常由多个机器人机械臂组成，每个机械臂上都有一个小型工具，例如激光、剪刀、钳子等，可以通过小切口进入患者体内进行手术。手术机器人具有高度精确性和稳定性，可以减少手术期间的误差和颤动，从而减少手术风险和并发症的发生率。此外，手术机器人还可以提供更好的可视化和放大效果，让医生更好地观察手术区域，进行更加精细的手术操作。
手术机器人的远心机构通常是指手术机器人系统中的机械臂，也称为“手术臂”或“工具臂”，负责将手术工具送入患者体内进行手术操作。远心机构的设计通常采用了一些先进的技术，例如电机驱动、传感器反馈和计算机控制等。这些技术可以确保机械臂具有高度精准的运动和定位能力，可以准确地将手术工具送入患者体内并进行精细的手术操作。此外，手术机器人的远心机构还具有一些特殊的设计，例如可弯曲的末端部分，可以在狭小的手术空间内灵活操作；还有一些机械臂具有一定的自主运动能力，可以根据医生的指令进行自动调整和协作操作。总的来说，手术机器人的远心机构是其核心部件之一，其高度精准的运动和定位能力为手术操作提供了重要的支持和保障。
手术进行前，需要医生或者助手将手术机器人的远心机构摆位到相应的位置。出于机械臂大而笨重且需要反复摆动对齐的考虑，需要对手术机器人的远心机构进行主动摆位。
现有的主动摆位方案有：
基于双编码器：两个编码器分别安装在电机端和负载端，中间通过减速机相连。当关节负载端受力后，由于减速机产生弹性形变，电机端编码器和负载端编码器产生位置偏差，将该偏差值进行闭环控制可以减少摆位过程中用户需要施加的力。
基于关节力矩传感器：力矩传感器安装在机器人关节处，可以直接测量出关节力矩。将该力矩进行闭环控制可以补偿摆位过程中需要的力。
但是上述现有技术中的主动摆位方案存在以下缺陷：
基于双编码器：减速机动力学建模复杂，与柔轮弹性形变，波发生器变形量以及输入侧齿隙都有关系。其模型为非线性模型，不能准确的估算受到的外力，因此将其闭环控制也会产生控制的偏差，造成摆位助力效果较差。
基于关节力矩传感器：关节力矩传感器能够准确地测量电机输出力矩并进行力矩补偿。但是关节力传感器价格较贵，从经济的角度来看并不理想。
发明内容
针对上述问题，本发明的目的在于提供一种应用于手术机器人的主动摆位方法及系统，采用基于电流的方式进行拖动助力补偿，不需要额外的力矩传感器，只需要辨识拖动过程中产生的重力、库伦摩擦力以及粘滞摩擦力，就可以准确地获得外力与角度和角速度的关系，实现主动摆位助力功能。
本发明的上述发明目的是通过以下技术方案得以实现的：
一种应用于手术机器人的主动摆位方法，包括以下步骤：
S1：在机械臂运行过程中采集机械臂轴关节的关节角度，以及计算所述机械臂轴关节的关节角速度；
S2：依据所述关节角度计算重力辨识项，依据所述关节角速度计算库伦摩擦力辨识项，依据所述关节角速度计算粘滞摩擦力辨识项；
S3：基于所述重力辨识项、所述库伦摩擦力辨识项以及所述粘滞摩擦力辨识项，计算为所述机械臂轴关节的电机进行补偿的电流，以减少所述机械臂摆位过程中使用者施加的力。
进一步地，在步骤S2中，依据所述关节角度计算所述重力辨识项之前，还包括：建立所述重力辨识项的理论模型，具体为：
利用所述机械臂的每个连杆的DH参数建立基于所述DH参数的齐次变换矩阵；基于所述齐次变换矩阵，通过拉格朗日动力学得出重力项与所述关节角度的关系式，作为所述重力辨识项的所述理论模型的关系式。
进一步地，在步骤S2中，还包括：对重力项和库伦摩擦力项进行辨识拟合，获取所述重力辨识项和所述库伦摩擦力辨识项，具体为：
对所述机械臂做位置环控制，所述位置环控制包括设置所述机械臂运动时的穿越频率，输入正弦的电流激励，和所述关节角度进行对比，设计超前补偿器以补偿相位裕度，同时串联一个滞后补偿器以减小高频增益；设计好所述位置环控制后，对所述电机进行S运动规划，使得所述机械臂做正反两个方向的低速匀速运动，通过采集所述低速匀速运动过程中同一位置下正反两个方向运动时的电流，做差和做和分别得到所述库伦摩擦力辨识项和所述重力辨识项的曲线，其中，所述库伦摩擦力辨识项取常量，所述重力辨识项的关系式的系数按照所述理论模型的关系式进行拟合。
进一步地，在步骤S2中，还包括：对所述重力辨识项、所述库伦摩擦力辨识项和所述粘滞摩擦力辨识项计算补偿电流，具体为：
通过低通滤波器获得当前的所述关节角速度以及当前关节位置；通过所述当前关节位置计算出基于所述重力辨识项的重力补偿；设定所述关节角速度的阈值，若当前所述关节角速度在所述阈值范围内，增加高频方波的静态输出以实现初始的助力，若当前所述关节角速度在所述阈值范围外，依据所述关节角速度的方向计算出基于所述库伦摩擦力辨识项的库伦摩擦力补偿；基于粘滞摩擦力与所述关节角速度成正比，设置所述粘滞摩擦力与所述关节角速度的调节系数，以计算出基于所述粘滞摩擦力辨识项的粘滞摩擦力补偿。
进一步地，在步骤S3中，基于所述重力辨识项、所述库伦摩擦力辨识项以及所述粘滞摩擦力辨识项，计算为所述机械臂轴关节的电机进行补偿的电流，具体为：
将所述重力补偿、所述库伦摩擦力补偿和所述粘滞摩擦力补偿相加，得到为所述机械臂轴关节的所述电机进行补偿的目标输出电流。
进一步地，在步骤S3中，基于所述重力辨识项、所述库伦摩擦力辨识项以及所述粘滞摩擦力辨识项，计算为所述机械臂轴关节的所述电机进行补偿的电流，具体的计算公式为：
Figure BDA0004186366240000041
其中，G(θ)为所述重力辨识项，
Figure BDA0004186366240000042
为所述库伦摩擦力辨识项，/>
Figure BDA0004186366240000043
为所述粘滞摩擦力辨识项，θ为所述关节角度，/>
Figure BDA0004186366240000044
为所述关节角速度；
在尚未拖动所述机械臂时，因为所述重力辨识项的补偿，所述机械臂会保持在当前位置，并且由于所述高频方波的存在，为使用者在拖动时提供初始助力，以减少静摩擦力的影响，当开始拖动所述机械臂时，所述高频方波被所述库伦摩擦力辨识项的补偿常量代替，同时根据所述关节角速度补偿相应的所述粘滞摩擦力辨识项，以实现摆位助力的功能。
进一步地，计算所述重力辨识项、所述库伦摩擦力辨识项以及所述粘滞摩擦力辨识项的补偿电流，具体的计算公式为：
所述重力辨识项：
G(θ)＝A sinθ+B cosθ+C
其中，A、B和C为对所述重力项按照所述理论模型的关系式进行辨识拟合后的系数；
所述库伦摩擦力辨识项：
Figure BDA0004186366240000045
其中，k为辨识的所述库伦摩擦力辨识项的常量，a为所述关节角速度的阈值，SquareWave为当所述关节角速度在所述阈值范围内，增加的作为静态输出的所述高频方波，用以在所述机械臂运动初始时提供助力；
所述粘滞摩擦力辨识项：
Figure BDA0004186366240000046
其中，b为所述粘滞摩擦力与所述关节角速度的调节系数。
一种用于执行如上述的应用于手术机器人的主动摆位方法的应用于手术机器人的主动摆位系统，包括：
机械臂参数采集模块，用于在机械臂运行过程中采集机械臂轴关节的关节角度，以及计算所述机械臂轴关节的关节角速度；
辨识项计算模块，用于依据所述关节角度计算重力辨识项，依据所述关节角速度计算库伦摩擦力辨识项，依据所述关节角速度计算粘滞摩擦力辨识项；
补偿电流计算模块，用于基于所述重力辨识项、所述库伦摩擦力辨识项以及所述粘滞摩擦力辨识项，计算为所述机械臂轴关节的电机进行补偿的电流，以减少所述机械臂摆位过程中使用者施加的力。
一种计算机设备，包括存储器和一个或多个处理器，所述存储器中存储有计算机代码，所述计算机代码被所述一个或多个处理器执行时，使得所述一个或多个处理器执行如上述的方法。
一种计算机可读存储介质，所述计算机可读存储介质存储有计算机代码，当所述计算机代码被执行时，如上述的方法被执行。
与现有技术相比，本发明的有益效果是：
通过提供一种应用于手术机器人的主动摆位方法，包括：S1：在机械臂运行过程中采集机械臂轴关节的关节角度，以及计算所述机械臂轴关节的关节角速度；S2：依据所述关节角度计算重力辨识项，依据所述关节角速度计算库伦摩擦力辨识项，依据所述关节角速度计算粘滞摩擦力辨识项；S3：基于所述重力辨识项、所述库伦摩擦力辨识项以及所述粘滞摩擦力辨识项，计算为所述机械臂轴关节的电机进行补偿的电流，以减少所述机械臂摆位过程中使用者施加的力。上述技术方案，基于电流的方式进行拖动助力补偿，不需要额外的力矩传感器，只需要辨识拖动过程中产生的重力、库伦摩擦力以及粘滞摩擦力，就可以准确地获得外力与角度和角速度的关系，实现主动摆位助力功能。当使用者拖动机械臂时，给机械臂轴关节的电机输入电流补偿，尽可能减少摆位过程中使用者施加的力，以达到快速轻松省力的摆位目的。
附图说明
图1为本发明应用于手术机器人的主动摆位方法的整体流程图；
图2为本发明应用于手术机器人的主动摆位方法的细节流程图；
图3为本发明应用于手术机器人的主动摆位系统的整体结构图。
具体实施方式
为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
本技术领域技术人员可以理解，除非特意声明，这里使用的单数形式“一”、“一个”、“所述”和“该”也可包括复数形式。应该进一步理解的是，本发明的说明书中使用的措辞“包括”是指存在所述特征、整数、步骤、操作、元件和/或组件，但是并不排除存在或添加一个或多个其他特征、整数、步骤、操作、元件、组件和/或它们的组。
第一实施例
如图1和2所示，本实施例提供了一种应用于手术机器人的主动摆位方法，包括以下步骤：
S1：在机械臂运行过程中采集机械臂轴关节的关节角度，以及计算所述机械臂轴关节的关节角速度。
具体的，在本实施例中，我们需要实时采集机械臂轴关节的关节角度和关节角速度，以用于计算用于主动摆位电流补偿的重力辨识项、库伦摩擦力辨识项和粘滞摩擦力辨识项。
对于采集关节角度和计算关节角速度的方式为现有技术，可以采用现有技术中任意一种技术，在本发明中不做任何限制。
如对于机械臂轴关节的关节角度可以通过以下方法确定：(1)确定坐标系：机械臂坐标系通常采用右手坐标系，其中每个轴关节都对应一个坐标系。(2)定义关节角度：对于每个关节，定义其角度为从前一个坐标系到后一个坐标系的旋转角度，通常采用欧拉角或四元数表示。(3)机械臂正运动学计算：通过正运动学计算，将每个轴关节的角度转换为机械臂末端的位置和姿态，即机械臂末端执行器相对于基坐标系的位置和姿态。(4)机械臂逆运动学计算：根据机械臂末端执行器的位置和姿态，计算每个轴关节的角度，使得机械臂末端执行器能够到达指定的位置和姿态。
如对于机械臂轴关节的关节角速度可以通过以下方法确定：(1)机械臂正运动学计算：根据机械臂的几何结构和运动学模型，计算机械臂末端执行器相对于基坐标系的位置和姿态。(2)对机械臂正运动学方程求导：对机械臂正运动学方程求导，得到机械臂末端执行器的线速度和角速度。(3)基于链式法则计算关节角速度：根据链式法则，将机械臂末端执行器的角速度分解为各个轴关节的角速度，得到每个轴关节的角速度。
S2：依据所述关节角度计算重力辨识项，依据所述关节角速度计算库伦摩擦力辨识项，依据所述关节角速度计算粘滞摩擦力辨识项。
具体的，在进行手术机器人的主动摆位电流补偿之前，需要计算重力辨识项、库伦摩擦力辨识项和粘滞摩擦力辨识项。
对于重力辨识项，在计算重力辨识项之前，首先需要建立重力辨识项的理论模型，具体为：
(1)由于机械臂的每个连杆的长度，质量以及质心位置已知，利用所述机械臂的每个连杆的DH参数建立基于所述DH参数的齐次变换矩阵。
DH(Denavit-Hartenberg)参数是描述机械臂关节间相对位置和方向关系的一种方法，通过DH参数可以建立起机械臂的运动学模型，进而进行机械臂的正逆运动学计算。而基于DH参数的齐次变换矩阵则是将机械臂各个关节之间的坐标系相互转换的一种数学工具。
对于一个机械臂的第i个关节，其DH参数包括4个变量：theta_i、d_i、a_i和alpha_i，分别表示该关节的旋转角度、平移距离、前后相邻关节间的距离和前后相邻关节间的夹角。使用这些参数可以得到第i个关节的变换矩阵A_i，表示第i个坐标系相对于第i-1个坐标系的转换。
DH参数的标准定义方式中，将第i个坐标系的z轴取为第i-1个坐标系的x轴到第i个坐标系的交点，x轴取为第i-1个坐标系的x轴在z轴到第i个坐标系的交点的投影，y轴则是通过右手法则确定。因此，第i个坐标系的原点就是机械臂第i个关节的轴心点。基于DH参数，机械臂从第0个坐标系(通常是基坐标系)开始运动，根据每个关节的DH参数计算出各个坐标系之间的变换矩阵，最终得到机械臂末端执行器相对于基坐标系的变换矩阵，即机械臂的正运动学模型。
通常情况下，机械臂正运动学模型可以表示为以下形式的齐次变换矩阵：
T0n＝A1A2...An
其中，T0n表示机械臂末端执行器相对于基坐标系的变换矩阵，Ai表示第i个坐标系相对于第i-1个坐标系的变换矩阵。由于变换矩阵是齐次坐标系下的矩阵，因此可以方便地进行矩阵运算和坐标变换。
(2)基于所述齐次变换矩阵，通过拉格朗日动力学得出重力项与所述关节角度的关系式，作为所述重力辨识项的所述理论模型的关系式。
拉格朗日力学，分析力学中的一种，由拉格朗日在1788年建立，是对经典力学的一种的新的数学表述。经典力学，最初的表述形式由牛顿建立，它着重分析位移，速度，加速度，力等矢量间的关系，又称为矢量力学。拉格朗日引入了广义坐标的概念，运用达朗贝尔原理，得到和牛顿第二定律等价的拉格朗日方程。但拉格朗日方程具有更普遍的意义，适用范围更广泛。并且，选取恰当的广义坐标，可以使拉格朗日方程的求解大大简化。
基于齐次变换矩阵，通过拉格朗日动力学算出各个机械臂各个连杆的重心位置，重力势能＝重力*高度，再对各个连杆的重力势能对角度进行偏微分可以算出各个连杆作用在电机上的扭矩。获得理论模型关系式G(θ)＝A sinθ+B cosθ+C，但是具体的变量A、B、C需要进行辨识后才能更加准确。
建立完重力项的理论模型之后，需要对重力项和库伦摩擦力项进行辨识拟合，获取所述重力辨识项和所述库伦摩擦力辨识项，具体为：
对所述机械臂做位置环控制，所述位置环控制包括设置所述机械臂运动时的穿越频率，输入正弦的电流激励，和所述关节角度进行对比，设计超前补偿器以补偿相位裕度，同时串联一个滞后补偿器以减小高频增益。
设计好所述位置环控制后，对所述电机进行S运动规划，使得所述机械臂做正反两个方向的低速匀速运动，通过采集所述低速匀速运动过程中同一位置下正反两个方向运动时的电流，做差和做和分别得到所述库伦摩擦力辨识项和所述重力辨识项的曲线，其中，所述库伦摩擦力辨识项取常量(近似认为是一个常量)，所述重力辨识项的关系式的系数按照所述理论模型的关系式进行拟合(如：可以基于对测量的运动时的电流作差和做和得到的重力曲线，在MATLAB里面用cftool进行拟合辨识参数)，求得更准确的与关节角度的关系式。
在辨识完重力项和库伦摩擦力项之后，可以制定相应的电流补偿策略，具体为：
通过低通滤波器获得当前的所述关节角速度以及当前关节位置；通过所述当前关节位置计算出基于所述重力辨识项的重力补偿；设定所述关节角速度的阈值，若当前所述关节角速度在所述阈值范围内，增加高频方波的静态输出以实现初始的助力，若当前所述关节角速度在所述阈值范围外，依据所述关节角速度的方向计算出基于所述库伦摩擦力辨识项的库伦摩擦力补偿；基于粘滞摩擦力与所述关节角速度成近似正比，设置所述粘滞摩擦力与所述关节角速度的调节系数，以计算出基于所述粘滞摩擦力辨识项的粘滞摩擦力补偿。
S3：基于所述重力辨识项、所述库伦摩擦力辨识项以及所述粘滞摩擦力辨识项，计算为所述机械臂轴关节的电机进行补偿的电流，以减少所述机械臂摆位过程中使用者施加的力，具体为：
将所述重力补偿、所述库伦摩擦力补偿和所述粘滞摩擦力补偿相加，得到为所述机械臂轴关节的所述电机进行补偿的目标输出电流。
第二实施例
本实施例提供了基于第一实施例的应用于手术机器人的主动摆位方法的具体计算公式。具体为：
手术进行前，手术机器人的使用者需要将手术机器人的远心机构摆位到相应的位置。处于机械臂大而笨重且需要反复摆动对齐的考虑，我们需要给机械臂轴关节的电机输入合适的电流指令，以尽可能减少摆位过程中使用者施加的力，以达到快速轻松省力的摆位目的。本发明基于重力、库伦摩擦力及粘滞摩擦力进行摆位，机械臂在运行过程中利用编码器采集关节角度θ，并计算关节角速度
Figure BDA0004186366240000101
补偿的电流为：
Figure BDA0004186366240000102
其中，G(θ)为所述重力辨识项，
Figure BDA0004186366240000103
为所述库伦摩擦力辨识项，/>
Figure BDA0004186366240000104
为所述粘滞摩擦力辨识项，θ为所述关节角度，/>
Figure BDA0004186366240000105
为所述关节角速度；
在尚未拖动所述机械臂时，因为所述重力辨识项的补偿，所述机械臂会保持在当前位置，并且由于所述高频方波的存在，为使用者在拖动时提供初始助力，以减少静摩擦力的影响，当开始拖动所述机械臂时，所述高频方波被所述库伦摩擦力辨识项的补偿常量代替，同时根据所述关节角速度补偿相应的所述粘滞摩擦力辨识项，以实现摆位助力的功能。
以下为每一个辨识项的具体公式：
所述重力辨识项：
G(θ)＝A sinθ+B cosθ+C
其中，A、B和C为对所述重力项按照所述理论模型的关系式进行辨识拟合后的系数；
所述库伦摩擦力辨识项：
Figure BDA0004186366240000106
其中，k为辨识的所述库伦摩擦力辨识项的常量，a为所述关节角速度的阈值，SquareWave为当所述关节角速度在所述阈值范围内，增加的作为静态输出的所述高频方波，用以在所述机械臂运动初始时提供助力；
所述粘滞摩擦力辨识项：
Figure BDA0004186366240000107
其中，b为所述粘滞摩擦力与所述关节角速度的调节系数。
第三实施例
如图3所示，本实施例提供了一种用于执行如第一实施例中的应用于手术机器人的主动摆位方法的应用于手术机器人的主动摆位系统，包括：
机械臂参数采集模块1，用于在机械臂运行过程中采集机械臂轴关节的关节角度，以及计算所述机械臂轴关节的关节角速度；
辨识项计算模块2，用于依据所述关节角度计算重力辨识项，依据所述关节角速度计算库伦摩擦力辨识项，依据所述关节角速度计算粘滞摩擦力辨识项；
补偿电流计算模块3，用于基于所述重力辨识项、所述库伦摩擦力辨识项以及所述粘滞摩擦力辨识项，计算为所述机械臂轴关节的电机进行补偿的电流，以减少所述机械臂摆位过程中使用者施加的力。
一种计算机可读存储介质，计算机可读存储介质存储有计算机代码，当计算机代码被执行时，如上述方法被执行。本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于一计算机可读存储介质中，存储介质可以包括：只读存储器(ROM，Read Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁盘或光盘等。
以上所述仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。
以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
应当说明的是，上述实施例均可根据需要自由组合。以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (10)

1.一种应用于手术机器人的主动摆位方法，其特征在于，包括以下步骤：
S1：在机械臂运行过程中采集机械臂轴关节的关节角度，以及计算所述机械臂轴关节的关节角速度；
S2：依据所述关节角度计算重力辨识项，依据所述关节角速度计算库伦摩擦力辨识项，依据所述关节角速度计算粘滞摩擦力辨识项；
S3：基于所述重力辨识项、所述库伦摩擦力辨识项以及所述粘滞摩擦力辨识项，计算为所述机械臂轴关节的电机进行补偿的电流，以减少所述机械臂摆位过程中使用者施加的力。
2.根据权利要求1所述的应用于手术机器人的主动摆位方法，其特征在于，在步骤S2中，依据所述关节角度计算所述重力辨识项之前，还包括：建立所述重力辨识项的理论模型，具体为：
利用所述机械臂的每个连杆的DH参数建立基于所述DH参数的齐次变换矩阵；
基于所述齐次变换矩阵，通过拉格朗日动力学得出重力项与所述关节角度的关系式，作为所述重力辨识项的所述理论模型的关系式。
3.根据权利要求2所述的应用于手术机器人的主动摆位方法，其特征在于，在步骤S2中，还包括：对重力项和库伦摩擦力项进行辨识拟合，获取所述重力辨识项和所述库伦摩擦力辨识项，具体为：
对所述机械臂做位置环控制，所述位置环控制包括设置所述机械臂运动时的穿越频率，输入正弦的电流激励，和所述关节角度进行对比，设计超前补偿器以补偿相位裕度，同时串联一个滞后补偿器以减小高频增益；
设计好所述位置环控制后，对所述电机进行S运动规划，使得所述机械臂做正反两个方向的低速匀速运动，通过采集所述低速匀速运动过程中同一位置下正反两个方向运动时的电流，做差和做和分别得到所述库伦摩擦力辨识项和所述重力辨识项的曲线，其中，所述库伦摩擦力辨识项取常量，所述重力辨识项的关系式的系数按照所述理论模型的关系式进行拟合。
4.根据权利要求3所述的应用于手术机器人的主动摆位方法，其特征在于，在步骤S2中，还包括：对所述重力辨识项、所述库伦摩擦力辨识项和所述粘滞摩擦力辨识项计算补偿电流，具体为：
通过低通滤波器获得当前的所述关节角速度以及当前关节位置；
通过所述当前关节位置计算出基于所述重力辨识项的重力补偿；
设定所述关节角速度的阈值，若当前所述关节角速度在所述阈值范围内，增加高频方波的静态输出以实现初始的助力，若当前所述关节角速度在所述阈值范围外，依据所述关节角速度的方向计算出基于所述库伦摩擦力辨识项的库伦摩擦力补偿；
基于粘滞摩擦力与所述关节角速度成正比，设置所述粘滞摩擦力与所述关节角速度的调节系数，以计算出基于所述粘滞摩擦力辨识项的粘滞摩擦力补偿。
5.根据权利要求5所述的应用于手术机器人的主动摆位方法，其特征在于，在步骤S3中，基于所述重力辨识项、所述库伦摩擦力辨识项以及所述粘滞摩擦力辨识项，计算为所述机械臂轴关节的电机进行补偿的电流，具体为：
将所述重力补偿、所述库伦摩擦力补偿和所述粘滞摩擦力补偿相加，得到为所述机械臂轴关节的所述电机进行补偿的目标输出电流。
6.根据权利要求4所述的应用于手术机器人的主动摆位方法，其特征在于，在步骤S3中，基于所述重力辨识项、所述库伦摩擦力辨识项以及所述粘滞摩擦力辨识项，计算为所述机械臂轴关节的所述电机进行补偿的电流，具体的计算公式为：
Figure FDA0004186366230000021
其中，G(θ)为所述重力辨识项，
Figure FDA0004186366230000022
为所述库伦摩擦力辨识项，/>
Figure FDA0004186366230000023
为所述粘滞摩擦力辨识项，θ为所述关节角度，/>
Figure FDA0004186366230000024
为所述关节角速度；
在尚未拖动所述机械臂时，因为所述重力辨识项的补偿，所述机械臂会保持在当前位置，并且由于所述高频方波的存在，为使用者在拖动时提供初始助力，以减少静摩擦力的影响，当开始拖动所述机械臂时，所述高频方波被所述库伦摩擦力辨识项的补偿常量代替，同时根据所述关节角速度补偿相应的所述粘滞摩擦力辨识项，以实现摆位助力的功能。
7.根据权利要求6所述的应用于手术机器人的主动摆位方法，其特征在于，计算所述重力辨识项、所述库伦摩擦力辨识项以及所述粘滞摩擦力辨识项的补偿电流，具体的计算公式为：
所述重力辨识项：
G(θ)＝A sinθ+B cosθ+C
其中，A、B和C为对所述重力项按照所述理论模型的关系式进行辨识拟合后的系数；
所述库伦摩擦力辨识项：
Figure FDA0004186366230000031
其中，k为辨识的所述库伦摩擦力辨识项的常量，a为所述关节角速度的阈值，SquareWave为当所述关节角速度在所述阈值范围内，增加的作为静态输出的所述高频方波，用以在所述机械臂运动初始时提供助力；
所述粘滞摩擦力辨识项：
Figure FDA0004186366230000032
其中，b为所述粘滞摩擦力与所述关节角速度的调节系数。
8.一种用于执行如权利要求1-7中任意一项所述的应用于手术机器人的主动摆位方法的应用于手术机器人的主动摆位系统，包括：
机械臂参数采集模块，用于在机械臂运行过程中采集机械臂轴关节的关节角度，以及计算所述机械臂轴关节的关节角速度；
辨识项计算模块，用于依据所述关节角度计算重力辨识项，依据所述关节角速度计算库伦摩擦力辨识项，依据所述关节角速度计算粘滞摩擦力辨识项；
补偿电流计算模块，用于基于所述重力辨识项、所述库伦摩擦力辨识项以及所述粘滞摩擦力辨识项，计算为所述机械臂轴关节的电机进行补偿的电流，以减少所述机械臂摆位过程中使用者施加的力。
9.一种计算机设备，包括存储器和一个或多个处理器，所述存储器中存储有计算机代码，所述计算机代码被所述一个或多个处理器执行时，使得所述一个或多个处理器执行如权利要求1至7中任一项所述的方法。
10.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机代码，当所述计算机代码被执行时，如权利要求1至7中任一项所述的方法被执行。
CN202310419921.0A 2023-04-19 2023-04-19 一种应用于手术机器人的主动摆位方法及系统 Pending CN116392253A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310419921.0A CN116392253A (zh) 2023-04-19 2023-04-19 一种应用于手术机器人的主动摆位方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310419921.0A CN116392253A (zh) 2023-04-19 2023-04-19 一种应用于手术机器人的主动摆位方法及系统

Publications (1)

Publication Number Publication Date
CN116392253A true CN116392253A (zh) 2023-07-07

Family

ID=87010346

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310419921.0A Pending CN116392253A (zh) 2023-04-19 2023-04-19 一种应用于手术机器人的主动摆位方法及系统

Country Status (1)

Country Link
CN (1) CN116392253A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116922399A (zh) * 2023-09-15 2023-10-24 季华实验室 机械臂重力补偿方法、装置、电子设备及存储介质

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116922399A (zh) * 2023-09-15 2023-10-24 季华实验室 机械臂重力补偿方法、装置、电子设备及存储介质
CN116922399B (zh) * 2023-09-15 2023-11-24 季华实验室 机械臂重力补偿方法、装置、电子设备及存储介质

Similar Documents

Publication Publication Date Title
US11850014B2 (en) Control system, control method, and surgical arm system
US10765485B2 (en) Medical support arm device and method of controlling medical support arm device
JP2022169722A (ja) 機械システムの制御方法，機械システム用コントローラ，ロボットマニピュレータ，および非一時的なコンピュータ可読記憶媒体
Li et al. A constrained optimization approach to virtual fixtures
JP2001157975A (ja) ロボットの制御装置
KR20170016631A (ko) 여자유도 로봇 제어 시스템, 방법, 및 상기 방법을 실행시키기 위한 컴퓨터 판독 가능한 프로그램을 기록한 기록 매체
CN116392253A (zh) 一种应用于手术机器人的主动摆位方法及系统
Cortesao et al. Robot force control on a beating heart
CN116277035B (zh) 机器人的控制方法、装置、处理器及电子设备
Hanses et al. Hand-guiding robots along predefined geometric paths under hard joint constraints
CN113742992B (zh) 基于深度学习的主从控制方法及应用
Castillo-Cruces et al. Virtual fixtures with autonomous error compensation for human–robot cooperative tasks
CN116672031B (zh) 机器人的控制方法、装置、处理器及电子设备
Dehghani et al. Vision-based calibration of a Hexa parallel robot
Hähn et al. Hybrid compliance compensation for path accuracy enhancement in robot machining
Sheng et al. Hybrid vision-force robot force control for tasks on soft tissues
Rossell et al. An effective strategy of real-time vision-based control for a Stewart platform
Wang et al. A discrete-jointed robot model based control strategy for spatial continuum manipulators
Zuo et al. Stereo vision guided control of a Stewart platform
Pulloquinga et al. Trajectory planner for type II singularities avoidance based on output twist screws
Marayong et al. Control methods for guidance virtual fixtures in compliant human-machine interfaces
Duan et al. Design of Robot Control System for Knee Joint Replacement Surgery Based on STM32
Tao et al. An RBF-PD Control Method for Robot Grasping of Moving Object
Lei et al. Vision-based position/impedance control for robotic assembly task
Xu et al. Assembly precision design for parallel robotic mechanism based on uncertain hybrid tolerance allocation

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