CN111053613B - 一种运动追踪方法、装置、机械臂及计算机可读存储介质 - Google Patents
一种运动追踪方法、装置、机械臂及计算机可读存储介质 Download PDFInfo
- Publication number
- CN111053613B CN111053613B CN201911262489.9A CN201911262489A CN111053613B CN 111053613 B CN111053613 B CN 111053613B CN 201911262489 A CN201911262489 A CN 201911262489A CN 111053613 B CN111053613 B CN 111053613B
- Authority
- CN
- China
- Prior art keywords
- joint
- mechanical arm
- moment
- tail end
- calculating
- 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
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/20—Surgical navigation systems; Devices for tracking or guiding surgical instruments, e.g. for frameless stereotaxis
-
- 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
- A61B34/75—Manipulators having means for prevention or compensation of hand tremors
-
- 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
- A61B34/77—Manipulators with motion or force scaling
-
- 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/20—Surgical navigation systems; Devices for tracking or guiding surgical instruments, e.g. for frameless stereotaxis
- A61B2034/2068—Surgical navigation systems; Devices for tracking or guiding surgical instruments, e.g. for frameless stereotaxis using pointers, e.g. pointers having reference marks for determining coordinates of body points
Abstract
本申请属于运动控制技术领域，尤其涉及一种运动追踪方法、装置、机械臂及计算机可读存储介质，通过计算机械臂末端的目标位置，并计算机械臂各个关节的参考关节角度以及机械臂末端的参考位置；然后，根据机械臂末端的目标位置、所述机械臂末端的参考位置以及参考关节角度计算得到机械臂各个关节的目标力矩增益；接着，将机械臂各个关节的重力补偿值与所述目标力矩增益的和作为致动器的力矩输入，使得机械臂能够在各个关节的致动器的驱动下，跟随追踪对象的移动而移动，完成对追踪对象的运动追踪。
Description
技术领域
本申请属于运动控制技术领域，尤其涉及一种运动追踪方法、装置、机械臂及计算机可读存储介质。
背景技术
在目前骨科器械辅助下的手术中，为了确保切骨或者钻孔打钉等操作的精准度，有两种方案可以消除病人骨头在操作过程中移动的不确定性对手术的影响。一种是将病人腿骨和骨科辅助器械固定，骨科辅助器械可以随着病人腿骨移动，通过调整器械的旋转和移动，给骨科动力系统提供轴向或者平面运动限制，然后利用协作型机械臂根据病人骨头位姿，对手术工具的运动进行实时跟踪和补偿，消除病人骨头在手术操作过程中移动的不确定性对手术的影响。另一种是通过将病人骨头进行柔性或者刚性固定在静止的器械上情况下，医生进行手术。但是，将病人腿骨固定在刚性的辅助器械上通常需要进行对病人腿骨进行钻孔打钉等操作，容易引起手术并发症，而术中利用绷带固定的柔性固定方案又容易导致手术操作偏差。
因此，将病人腿骨和骨科辅助器械固定，并使骨科辅助器械随着病人腿骨的移动而移动，然后利用协作型机械臂根据病人骨头位姿，对手术工具的运动进行实时跟踪和补偿的方式，可以更好的消除病人骨头在手术操作过程中移动的不确定性对手术的影响。
然而，目前的协作型机械臂还缺乏可以实现根据病人骨头位姿对手术工具的运动进行实时跟踪和补偿的方法。
发明内容
本申请实施例提供一种运动追踪方法、装置、机械臂及计算机可读存储介质，包括但不限于解决目前的协作型机械臂无法根据病人骨头位姿对手术工具的运动进行实时跟踪和补偿的技术问题。
本申请实施例第一方面提供一种运动追踪方法，应用于机械臂，所述机械臂的各个关节设置有致动器；所述致动器设置有编码器和力矩传感器；所述编码器用于输出与所述致动器对应的关节的关节角度，所述力矩传感器用于输出与所述致动器对应的关节的关节力矩；所述运动追踪方法包括：
获取机械臂末端与追踪对象之间的目标相对位姿数据，以及所述机械臂末端与追踪对象之间的实时相对位姿数据；
根据所述目标相对位姿数据与所述实时相对位姿数据确定机械臂末端的目标位置；
获取各个致动器的编码器输出的关节角度和力矩传感器输出的关节力矩，并根据所述各个致动器的编码器输出的关节角度和力矩传感器输出的关节力矩计算机械臂各个关节的参考关节角度；
根据所述机械臂各个关节的参考关节角度以及所述机械臂的DH表，计算得到所述机械臂末端的参考位置；
根据所述机械臂末端的目标位置、所述机械臂末端的参考位置以及所述机械臂各个关节的参考关节角度计算得到机械臂各个关节的目标力矩增益；
获取所述机械臂各个关节的重力补偿值，并分别将所述重力补偿值与关节对应的目标力矩增益的和作为关节对应的致动器的力矩输入，以使所述机械臂能够在各个关节致动器的驱动下跟随追踪对象的移动而移动。
本申请实施例第二方面提供一种运动追踪装置，配置于机械臂，所述机械臂的各个关节设置有致动器；所述致动器设置有编码器和力矩传感器；所述编码器用于输出与所述致动器对应的关节的关节角度，所述力矩传感器用于输出与所述致动器对应的关节的关节力矩；所述运动追踪装置包括：
获取单元，用于获取机械臂末端与追踪对象之间的目标相对位姿数据，以及所述机械臂末端与追踪对象之间的实时相对位姿数据；
确定单元，用于根据所述目标相对位姿数据与所述实时相对位姿数据确定机械臂末端的目标位置；
第一计算单元，用于获取各个致动器的编码器输出的关节角度和力矩传感器输出的关节力矩，并根据所述各个致动器的编码器输出的关节角度和力矩传感器输出的关节力矩计算机械臂各个关节的参考关节角度；
第二计算单元，用于根据所述机械臂各个关节的参考关节角度以及所述机械臂的DH表，计算得到所述机械臂末端的参考位置；
第三计算单元，用于根据所述机械臂末端的目标位置、所述机械臂末端的参考位置以及所述机械臂各个关节的参考关节角度计算得到机械臂各个关节的目标力矩增益；
追踪单元，用于获取所述机械臂各个关节的重力补偿值，并分别将所述重力补偿值与关节对应的目标力矩增益的和作为关节对应的致动器的力矩输入，以使所述机械臂能够在各个关节致动器的驱动下跟随追踪对象的移动而移动。
本申请实施例第三方面提供一种机械臂，所述机械臂的各个关节设置有致动器；所述致动器设置有编码器和力矩传感器；所述编码器用于输出与所述致动器对应的关节的关节角度，所述力矩传感器用于输出与所述致动器对应的关节的关节力矩；所述机械臂还包括：存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，处理器执行计算机程序时实现上述方法的步骤。
本申请实施例第四方面提供一种计算机可读存储介质，计算机可读存储介质存储有计算机程序，计算机程序被处理器执行时实现上述方法的步骤。
本申请实施例中，通过计算机械臂末端的目标位置，并计算机械臂各个关节的参考关节角度以及机械臂末端的参考位置；然后，根据机械臂末端的目标位置、所述机械臂末端的参考位置以及参考关节角度计算得到机械臂各个关节的目标力矩增益；接着，将机械臂各个关节的重力补偿值与所述目标力矩增益的和作为致动器的力矩输入，使得机械臂能够在各个关节的致动器的驱动下，跟随追踪对象的移动而移动，完成对所述追踪对象的运动追踪。因此，当追踪对象为病人骨头时，可以解决目前的协作型机械臂无法根据病人骨头位姿对手术工具的运动进行实时跟踪和补偿的技术问题。
附图说明
为了更清楚地说明本申请实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本申请的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。
图1是本申请实施例提供的一种运动追踪方法的实现流程示意图；
图2是本申请实施例提供的一种机械臂的结构示意图；
图3是本申请实施例提供的一种机械臂的关节的结构示意图；
图4是本申请实施例提供的一种运动追踪方法步骤105的具体实现流程示意图；
图5是本申请实施例提供的一种运动追踪方法步骤106的具体实现流程示意图；
图6是本申请实施例提供的运动追踪装置的结构示意图；
图7是本申请实施例提供的机械臂的另一结构示意图。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。
应当理解，当在本说明书和所附权利要求书中使用时，术语“包括”指示所描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其它特征、整体、步骤、操作、元素、组件和/或其集合的存在或添加。
为了说明本申请上述的技术方案，下面通过具体实施例来进行说明。
本申请实施例提供一种运动追踪方法、装置、机械臂及计算机可读存储介质，可以对需要进行运动追踪的对象进行运动追踪。
需要说明的是，本申请实施例中提供的运动追踪方法、装置、机械臂及计算机可读存储介质可以适用于对各种需要进行运动追踪的对象进行运动追踪的场景，为了描述的方便，下面以运动追踪对象为病人骨头为例进行说明。
如图1示出了本申请实施例提供的一种运动追踪方法的实现流程示意图，该方法可以应用于如图2所示的机械臂，并且，该机械臂的各个关节21、22、23设置有用于驱动机械臂的发生位姿变化的致动器。如图3所示，所述致动器31设置有编码器32和力矩传感器33；所述编码器32用于输出与所述致动器对应的关节的关节角度，所述力矩传感器33设置于致动器31的末端，用于输出与所述力矩传感器对应的关节的关节力矩。
基于上述机械臂，本申请提供的运动追踪方法可以包括步骤101至步骤106。
步骤101，获取机械臂末端与追踪对象之间的目标相对位姿数据，以及所述机械臂末端与追踪对象之间的实时相对位姿数据。
本申请实施例中，上述机械臂末端与追踪对象之间的目标相对位姿数据是指机械臂末端与追踪对象之间的相对位姿能够符合用户需求时的各自的位姿。即，机械臂末端与追踪对象之间最终需要保持的相对位姿。
例如，在目前骨科器械辅助下的手术中，为了确保切骨或者钻孔打钉等操作的精准度，需要消除病人骨头在操作过程中移动的不确定性对手术的影响，即，要使骨科辅助器械随着病人腿骨的移动而移动，以保持骨科辅助器械与病人骨头之间的相对位姿不变，而这种需要保持的相对位姿即为骨科辅助器械与病人骨头之间的目标相对位姿。
具体的，当上述机械臂末端与追踪对象之间的目标相对位姿数据为机械臂末端与病人骨头之间的目标相对位姿数据时，上述获取机械臂末端与追踪对象之间的目标相对位姿数据可以包括获取术前规划系统确定的机械臂末端与追踪对象之间的目标相对位姿数据。
其中，术前规划系统可以是基于医生的专业知识构建的术前规划系统。例如，该术前规划系统的构建的过程中可以利用大量的成功的手术案例的手术数据对人工智能模型进行训练，得到能够根据病人的骨头影像输出手术规划方案的术前规划系统。并且，该术前规划系统输出的手术规划方案包含机械臂末端于病人骨头之间的目标相对位姿数据。
在本申请的一些实施方式中，上述获取机械臂末端与追踪对象之间的实时相对位姿数据可以包括获取定位追踪系统采集的所述机械臂末端与追踪对象之间的实时相对位姿数据。
其中，上述定位追踪系统可以为相关技术中的红外光学定位追踪系统或电磁定位追踪系统，只要能够实现对机械臂末端以及追踪对象进行实时定位即可。
步骤102，根据所述目标相对位姿数据与所述实时相对位姿数据确定机械臂末端的目标位置。
在对追踪对象进行运动追踪时，追踪对象一般处于运动状态，并且该运动状态具有不确定性，因此，机械臂在对追踪对象进行动追踪时，通常是稍微落后于追踪对象的，所以在获取到机械臂末端与追踪对象之间的目标相对位姿数据，以及所述机械臂末端与追踪对象之间的实时相对位姿数据之后，可以通过比较目标相对位姿数据与所述实时相对位姿数据之间的差距，确定出机械臂末端在追踪对象的位姿发生变化之后需要到达的目标位置，以便保持机械臂末端与所述追踪对象之间的相对位姿能够调整为与上述目标相对位姿数据对应的相对位姿。
步骤103，获取各个致动器的编码器输出的关节角度和力矩传感器输出的关节力矩，并根据所述各个致动器的编码器输出的关节角度和力矩传感器输出的关节力矩计算机械臂各个关节的参考关节角度。
具体的，上述各个致动器的编码器输出的关节角度和力矩传感器输出的关节力矩可以利用机械臂的动力学方程根据致动器当前时刻的力矩输入计算得到。
本申请实施例中，上述参考关节角度为根据各个致动器的编码器输出的关节角度和各个致动器的力矩传感器输出的关节力矩计算得到的，与关节的真实关节角度不同。本申请的参考关节角度是经过关节力矩校正，已经消除机械臂摩擦力干扰得到的比编码器输出的关节角度更接近真实关节角度的关节角度。
在本申请的一些实施方式中，上述根据各个致动器的编码器输出的关节角度和力矩传感器输出的关节力矩计算机械臂各个关节的参考关节角度可以具体包括：计算各个关节的关节力矩除以关节对应的力矩传感器的刚度系数得到的商，并将所述关节力矩除以关节对应的力矩传感器的刚度系数得到的商与关节对应的编码器输出的关节角度相加得到关节对应的参考关节角度。
例如，在计算关节21的参考关节角度时，可以先计算关节21的关节力矩除以关节21对应的力矩传感器的刚度系数k得到的商H，将H与关节21对应的编码器输出的关节角度相加得到关节21的参考关节角度。
步骤104，根据所述机械臂各个关节的参考关节角度以及所述机械臂的DH表，计算得到所述机械臂末端的参考位置。
其中，机械臂的DH表记录有机械臂各个关节之间的参考关节坐标系的转换矩阵的参数。通过将各个关节的参考关节角度代入根据DH表得到的各个关节的转换矩阵，并将各个转换矩阵的相乘即可得到机械臂末端的参考位置。
步骤105，根据所述机械臂末端的目标位置、所述机械臂末端的参考位置以及所述机械臂各个关节的参考关节角度计算得到机械臂各个关节的目标力矩增益。
本申请实施例中，如图4所示，上述根据所述机械臂末端的目标位置、所述机械臂末端的参考位置以及所述机械臂各个关节的参考关节角度计算得到机械臂各个关节的目标力矩增益可以包括：步骤401至步骤404。
步骤401，获取所述机械臂的比例系数、所述机械臂的雅可比矩阵以及所述机械臂各个关节的阻尼系数。
其中，机械臂的比例系数可以在实际应用中根据机械臂的具体运动超调量和快速性等情况进行人为设定。机械臂各个关节的阻尼系数可以根据实践经验进行人为设定。机械臂的雅可比矩阵中的每个元素为对应的运动学方程的其中一个变量的导数。
步骤402，计算所述机械臂末端的目标位置与所述机械臂末端的参考位置的差值，并计算所述差值与所述机械臂的比例系数相乘得到的第一乘积。
步骤403，对所述机械臂各个关节的参考关节角度进行微分得到各个关节的关节速度，并计算所述各个关节的关节速度与所述机械臂的雅可比矩阵相乘得到的各个关节的参考末端速度与对应的关节的阻尼系数相乘得到的第二乘积。
步骤404，将所述第一乘积减去所述第二乘积之后乘以所述机械臂的雅可比矩阵的转置得到所述机械臂各个关节的目标力矩增益。
由于将所述第一乘积减去所述第二乘积之后得到的差值为机械臂在笛卡尔坐标系下的各个关节的目标力矩增益，因此，需要将所述第一乘积减去所述第二乘积之后得到的差值乘以所述机械臂的雅可比矩阵的转置，得到所述机械臂在关节空间下的各个关节的目标力矩增益。
步骤106，获取所述机械臂各个关节的重力补偿值，并将所述重力补偿值与所述目标力矩增益的和作为所述致动器的力矩输入，使得所述机械臂能够在所述致动器的驱动下跟随追踪对象的移动而移动。
例如，将所述重力补偿值与所述目标力矩增益的和作为所述致动器下一时刻的力矩输入，使得所述机械臂能够在所述致动器的驱动下跟随追踪对象的移动而移动，完成对所述追踪对象的运动追踪。
具体的，如图5所示，上述获取机械臂各个关节的重力补偿值可以包括：步骤501至步骤503。
步骤501，获取所述机械臂的基座、各个连杆以及各个关节的质量和质心分布。
步骤502，根据所述机械臂的基座、各个连杆以及各个关节的质量和质心分布计算所述机械臂各个基座、各个连杆以及各个关节的重力势能。
步骤503，计算所述机械臂各个基座、各个连杆以及各个关节的重力势能与对应的关节的参考关节角度的偏微分乘以对应的参考关节角度得到的所述机械臂各个关节的重力补偿值。
具体的，上述步骤501至步骤502的具体实现方式可以参看相关技术中的描述，此处不再赘述。
本申请实施例中，通过计算机械臂末端的目标位置，并计算机械臂各个关节的参考关节角度以及机械臂末端的参考位置；然后，根据机械臂末端的目标位置、所述机械臂末端的参考位置以及参考关节角度计算得到机械臂各个关节的目标力矩增益；接着，将机械臂各个关节的重力补偿值与所述目标力矩增益的和作为致动器的力矩输入，使得机械臂能够在各个关节的致动器的驱动下，跟随追踪对象的移动而移动，完成对所述追踪对象的运动追踪。因此，当追踪对象为病人骨头时，可以解决目前的协作型机械臂无法根据病人骨头位姿对手术工具的运动进行实时跟踪和补偿的技术问题。
在实际应用中，当上述追踪对象为病人骨头时，为了解决目前的协作型机械臂无法根据病人骨头位姿对手术工具的运动进行实时跟踪和补偿的技术问题，如图2所示，可以先获取协作型机械臂末端401与病人骨头之间的目标相对位姿数据，以及所述协作型机械臂末端与病人骨头之间的实时相对位姿数据；然后根据所述目标相对位姿数据与所述实时相对位姿数据确定协作型机械臂末端401的目标位置，并计算协作型机械臂各个关节的参考关节角度，得到所述协作型机械臂末端401的参考位置，接着，根据所述协作型机械臂末端401的目标位置、所述协作型机械臂末端401的参考位置以及所述协作型机械臂各个关节的参考关节角度计算得到协作型机械臂各个关节的目标力矩增益；接着，获取所述协作型机械臂各个关节的重力补偿值，并分别将所述重力补偿值与关节对应的目标力矩增益的和作为关节对应的致动器的力矩输入，以使所述协作型机械臂能够在各个关节致动器的驱动下跟随病人骨头的移动而移动，并进一步地实现对连接于协作型机械臂末端的手术工具402的运动进行实时跟踪和补偿，解决了目前的协作型机械臂无法根据病人骨头位姿对手术工具的运动进行实时跟踪和补偿的技术问题。
需要说明的是，对于前述的各方法实施例，为了简单描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本发明并不受所描述的动作顺序的限制，因为根据本发明，某些步骤可以采用其它顺序进行。
图6示出了本申请实施例提供的一种运动追踪装置600的结构示意图，该运动追踪装置配置于机械臂，所述机械臂的各个关节设置有致动器；所述致动器设置有编码器和力矩传感器；所述编码器用于输出与所述致动器对应的关节的关节角度，所述力矩传感器用于输出与所述致动器对应的关节的关节力矩；所述运动追踪装置包括：获取单元601、确定单元602、第一计算单元603、第二计算单元604、第三计算单元605和追踪单元606。
获取单元601，用于获取机械臂末端与追踪对象之间的目标相对位姿数据，以及所述机械臂末端与追踪对象之间的实时相对位姿数据；
确定单元602，用于根据所述目标相对位姿数据与所述实时相对位姿数据确定机械臂末端的目标位置；
第一计算单元603，用于获取各个致动器的编码器输出的关节角度和力矩传感器输出的关节力矩，并根据所述各个致动器的编码器输出的关节角度和力矩传感器输出的关节力矩计算机械臂各个关节的参考关节角度；
第二计算单元604，用于根据所述机械臂各个关节的参考关节角度以及所述机械臂的DH表，计算得到所述机械臂末端的参考位置；
第三计算单元605，用于根据所述机械臂末端的目标位置、所述机械臂末端的参考位置以及所述机械臂各个关节的参考关节角度计算得到机械臂各个关节的目标力矩增益；
追踪单元606，用于获取所述机械臂各个关节的重力补偿值，并分别将所述重力补偿值与关节对应的目标力矩增益的和作为关节对应的致动器的力矩输入，以使所述机械臂能够在各个关节致动器的驱动下跟随追踪对象的移动而移动。
在本申请的一些实施方式中，上述第一计算单元603还具体用于：计算各个关节的关节力矩除以关节对应的力矩传感器的刚度系数得到的商，并将所述关节力矩除以关节对应的力矩传感器的刚度系数得到的商与关节对应的编码器输出的关节角度相加得到关节对应的参考关节角度。
在本申请的一些实施方式中，上述第二计算单元604还具体用于：将各个关节的参考关节角度代入根据DH表得到的各个关节的转换矩阵，并将各个转换矩阵的相乘得到机械臂末端的参考位置。
在本申请的一些实施方式中，上述第三计算单元605还具体用于：
获取所述机械臂的比例系数、所述机械臂的雅可比矩阵以及所述机械臂各个关节的阻尼系数；
计算所述机械臂末端的目标位置与所述机械臂末端的参考位置的差值，并计算所述差值与所述机械臂的比例系数相乘得到的第一乘积；
对所述机械臂各个关节的参考关节角度进行微分得到各个关节的关节速度，并计算所述各个关节的关节速度与所述机械臂的雅可比矩阵相乘得到的各个关节的参考末端速度与对应的关节的阻尼系数相乘得到的第二乘积；
将所述第一乘积减去所述第二乘积之后乘以所述机械臂的雅可比矩阵的转置得到所述机械臂各个关节的目标力矩增益。
在本申请的一些实施方式中，上述追踪单元606还具体用于：
获取所述机械臂的基座、各个连杆以及各个关节的质量和质心分布；
根据所述机械臂的基座、各个连杆以及各个关节的质量和质心分布计算所述机械臂各个基座、各个连杆以及各个关节的重力势能；
计算所述机械臂各个基座、各个连杆以及各个关节的重力势能与对应的关节的参考关节角度的偏微分乘以对应的参考关节角度得到的所述机械臂各个关节的重力补偿值。
需要说明的是，为描述的方便和简洁，上述描述的运动追踪装置600的具体工作过程，可以参考上述图1至图5中描述的方法的对应过程，在此不再赘述。
如图7所示，本申请实施例还提供了一种机械臂的另一结构示意图。该机械臂可以包括：处理器70、存储器71以及存储在所述存储器71中并可在所述处理器70上运行的计算机程序72，例如，运动追踪程序；所述处理器70执行所述计算机程序72时实现上述各个运动追踪方法实施例中的步骤，例如图1所示的步骤101至106。或者，所述处理器70执行所述计算机程序72时实现上述各装置实施例中各模块/单元的功能，例如图6所示单元601至606的功能。
所述计算机程序可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储器71中，并由所述处理器70执行，以完成本申请。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序在所述机械臂中的执行过程。例如，所述计算机程序可以被分割成获取单元、确定单元、第一计算单元、第二计算单元、第三计算单元和追踪单元，各单元具体功能如下：
获取单元，用于获取机械臂末端与追踪对象之间的目标相对位姿数据，以及所述机械臂末端与追踪对象之间的实时相对位姿数据；
确定单元，用于根据所述目标相对位姿数据与所述实时相对位姿数据确定机械臂末端的目标位置；
第一计算单元，用于获取各个致动器的编码器输出的关节角度和力矩传感器输出的关节力矩，并根据所述各个致动器的编码器输出的关节角度和力矩传感器输出的关节力矩计算机械臂各个关节的参考关节角度；
第二计算单元，用于根据所述机械臂各个关节的参考关节角度以及所述机械臂的DH表，计算得到所述机械臂末端的参考位置；
第三计算单元，用于根据所述机械臂末端的目标位置、所述机械臂末端的参考位置以及所述机械臂各个关节的参考关节角度计算得到机械臂各个关节的目标力矩增益；
追踪单元，用于获取所述机械臂各个关节的重力补偿值，并分别将所述重力补偿值与关节对应的目标力矩增益的和作为关节对应的致动器的力矩输入，以使所述机械臂能够在各个关节致动器的驱动下跟随追踪对象的移动而移动。
所述机械臂可包括但不仅限于，处理器70、存储器71。本领域技术人员可以理解，图7仅仅是机械臂的示例，并不构成对机械臂的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述机械臂还可以包括输入输出设备、网络接入设备、总线等。
所称处理器70可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
所述存储器71可以是所述机械臂的内部存储单元，例如机械臂的硬盘或内存。所述存储器71也可以是所述机械臂的外部存储设备，例如所述机械臂上配备的插接式硬盘，智能存储卡(Smart Media Card，SMC)，安全数字(Secure Digital，SD)卡，闪存卡(FlashCard)等。进一步地，所述存储器71还可以既包括所述机械臂的内部存储单元也包括外部存储设备。所述存储器71用于存储所述计算机程序以及所述机械臂所需的其他程序和数据。所述存储器71还可以用于暂时地存储已经输出或者将要输出的数据。
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将所述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。上述系统中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。
在本申请所提供的实施例中，应该理解到，所揭露的装置/方法，可以通过其它的方式实现。例如，以上所描述的装置/方法实施例仅仅是示意性的，例如，所述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性，机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
所述集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(Read-Only Memory，ROM)、随机存取存储器(RandomAccess Memory，RAM)、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。
以上所述实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围，均应包含在本申请的保护范围之内。
Claims (10)
1.一种运动追踪方法，应用于机械臂，其特征在于，所述机械臂的各个关节设置有致动器；所述致动器设置有编码器和力矩传感器；所述编码器用于输出与所述致动器对应的关节的关节角度，所述力矩传感器用于输出与所述致动器对应的关节的关节力矩；所述运动追踪方法包括：
获取机械臂末端与追踪对象之间的目标相对位姿数据，以及所述机械臂末端与追踪对象之间的实时相对位姿数据；
通过比较所述目标相对位姿数据与所述实时相对位姿数据之间的差距确定机械臂末端在所述追踪对象的位姿发生变化后需要到达的目标位置；
获取各个致动器的编码器输出的关节角度和力矩传感器输出的关节力矩，并根据所述各个致动器的编码器输出的关节角度和力矩传感器输出的关节力矩计算机械臂各个关节的参考关节角度；
根据所述机械臂各个关节的参考关节角度以及所述机械臂的DH表，计算得到所述机械臂末端的参考位置；
根据所述机械臂末端的目标位置、所述机械臂末端的参考位置以及所述机械臂各个关节的参考关节角度计算得到机械臂各个关节的目标力矩增益；
获取所述机械臂各个关节的重力补偿值，并分别将所述重力补偿值与关节对应的目标力矩增益的和作为关节对应的致动器的力矩输入，以使所述机械臂能够在各个关节致动器的驱动下跟随追踪对象的移动而移动。
2.如权利要求1所述的运动追踪方法，其特征在于，所述根据所述各个致动器的编码器输出的关节角度和力矩传感器输出的关节力矩计算机械臂各个关节的参考关节角度，包括：计算各个关节的关节力矩除以关节对应的力矩传感器的刚度系数得到的商，并将所述关节力矩除以关节对应的力矩传感器的刚度系数得到的商与关节对应的编码器输出的关节角度相加得到关节对应的参考关节角度。
3.如权利要求1所述的运动追踪方法，其特征在于，所述根据所述各个关节的参考关节角度以及所述机械臂的DH表，计算得到所述机械臂末端的参考位置，包括：将各个关节的参考关节角度代入根据DH表得到的各个关节的转换矩阵，并将各个转换矩阵的相乘得到机械臂末端的参考位置。
4.如权利要求1所述的运动追踪方法，其特征在于，所述根据所述机械臂末端的目标位置、所述机械臂末端的参考位置以及所述机械臂各个关节的参考关节角度计算得到机械臂各个关节的目标力矩增益，包括：
获取所述机械臂的比例系数、所述机械臂的雅可比矩阵以及所述机械臂各个关节的阻尼系数；
计算所述机械臂末端的目标位置与所述机械臂末端的参考位置的差值，并计算所述差值与所述机械臂的比例系数相乘得到的第一乘积；
对所述机械臂各个关节的参考关节角度进行微分得到各个关节的关节速度，并计算所述各个关节的关节速度与所述机械臂的雅可比矩阵相乘得到的各个关节的参考末端速度与对应的关节的阻尼系数相乘得到的第二乘积；
将所述第一乘积减去所述第二乘积之后乘以所述机械臂的雅可比矩阵的转置得到所述机械臂各个关节的目标力矩增益。
5.如权利要求1所述的运动追踪方法，其特征在于，所述获取所述机械臂各个关节的重力补偿值包括：
获取所述机械臂的基座、各个连杆以及各个关节的质量和质心分布；
根据所述机械臂的基座、各个连杆以及各个关节的质量和质心分布计算所述机械臂各个基座、各个连杆以及各个关节的重力势能；
计算所述机械臂各个基座、各个连杆以及各个关节的重力势能与对应的关节的参考关节角度的偏微分乘以对应的参考关节角度得到的所述机械臂各个关节的重力补偿值。
6.一种运动追踪装置，配置于机械臂，其特征在于，所述机械臂的各个关节设置有致动器；所述致动器设置有编码器和力矩传感器；所述编码器用于输出与所述致动器对应的关节的关节角度，所述力矩传感器用于输出与所述致动器对应的关节的关节力矩；所述运动追踪装置包括：
获取单元，用于获取机械臂末端与追踪对象之间的目标相对位姿数据，以及所述机械臂末端与追踪对象之间的实时相对位姿数据；
确定单元，用于通过比较所述目标相对位姿数据与所述实时相对位姿数据之间的差距确定机械臂末端在所述追踪对象的位姿发生变化后需要到达的目标位置；
第一计算单元，用于获取各个致动器的编码器输出的关节角度和力矩传感器输出的关节力矩，并根据所述各个致动器的编码器输出的关节角度和力矩传感器输出的关节力矩计算机械臂各个关节的参考关节角度；
第二计算单元，用于根据所述机械臂各个关节的参考关节角度以及所述机械臂的DH表，计算得到所述机械臂末端的参考位置；
第三计算单元，用于根据所述机械臂末端的目标位置、所述机械臂末端的参考位置以及所述机械臂各个关节的参考关节角度计算得到机械臂各个关节的目标力矩增益；
追踪单元，用于获取所述机械臂各个关节的重力补偿值，并分别将所述重力补偿值与关节对应的目标力矩增益的和作为关节对应的致动器的力矩输入，以使所述机械臂能够在各个关节致动器的驱动下跟随追踪对象的移动而移动。
7.如权利要求6所述的运动追踪装置，其特征在于，所述第一计算单元还具体用于：
计算各个关节的关节力矩除以关节对应的力矩传感器的刚度系数得到的商，并将所述关节力矩除以关节对应的力矩传感器的刚度系数得到的商与关节对应的编码器输出的关节角度相加得到关节对应的参考关节角度。
8.如权利要求6所述的运动追踪装置，其特征在于，所述第二计算单元还具体用于：
将各个关节的参考关节角度代入根据DH表得到的各个关节的转换矩阵，并将各个转换矩阵的相乘得到机械臂末端的参考位置。
9.一种机械臂，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述机械臂的各个关节设置有致动器；所述致动器设置有编码器和力矩传感器；所述编码器用于输出与所述致动器对应的关节的关节角度，所述力矩传感器用于输出与所述致动器对应的关节的关节力矩；所述处理器执行所述计算机程序时实现如权利要求1-5任意一项所述方法的步骤。
10.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1-5任意一项所述方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911262489.9A CN111053613B (zh) | 2019-12-10 | 2019-12-10 | 一种运动追踪方法、装置、机械臂及计算机可读存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911262489.9A CN111053613B (zh) | 2019-12-10 | 2019-12-10 | 一种运动追踪方法、装置、机械臂及计算机可读存储介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111053613A CN111053613A (zh) | 2020-04-24 |
| CN111053613B true CN111053613B (zh) | 2020-10-13 |
Family
ID=70300548
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911262489.9A Active CN111053613B (zh) | 2019-12-10 | 2019-12-10 | 一种运动追踪方法、装置、机械臂及计算机可读存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111053613B (zh) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111618859B (zh) * | 2020-06-03 | 2021-04-13 | 杭州键嘉机器人有限公司 | 一种对于静止或低速工况下机械臂高精度力反馈的方法 |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105159096A (zh) * | 2015-10-10 | 2015-12-16 | 北京邮电大学 | 一种基于粒子群算法的冗余度空间机械臂关节力矩优化方法 |
| CN106361440A (zh) * | 2016-08-31 | 2017-02-01 | 北京术锐技术有限公司 | 一种柔性手术工具系统及其在运动约束下的控制方法 |
| CN108748147A (zh) * | 2018-06-01 | 2018-11-06 | 清华大学深圳研究生院 | 一种外骨骼机械臂的控制系统及方法 |
| CN109159151A (zh) * | 2018-10-23 | 2019-01-08 | 北京无线电测量研究所 | 一种机械臂空间轨迹跟踪动态补偿方法和系统 |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP6128767B2 (ja) * | 2012-07-05 | 2017-05-17 | キヤノン株式会社 | ロボット制御装置、及びロボット制御方法 |
| US9815202B2 (en) * | 2014-07-09 | 2017-11-14 | Canon Kabushiki Kaisha | Control method for robot apparatus, computer readable recording medium, and robot apparatus |
| US20160375580A1 (en) * | 2015-06-23 | 2016-12-29 | Kabushiki Kaisha Yaskawa Denki | Robot system and robot control method |
-
2019
- 2019-12-10 CN CN201911262489.9A patent/CN111053613B/zh active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105159096A (zh) * | 2015-10-10 | 2015-12-16 | 北京邮电大学 | 一种基于粒子群算法的冗余度空间机械臂关节力矩优化方法 |
| CN106361440A (zh) * | 2016-08-31 | 2017-02-01 | 北京术锐技术有限公司 | 一种柔性手术工具系统及其在运动约束下的控制方法 |
| CN108748147A (zh) * | 2018-06-01 | 2018-11-06 | 清华大学深圳研究生院 | 一种外骨骼机械臂的控制系统及方法 |
| CN109159151A (zh) * | 2018-10-23 | 2019-01-08 | 北京无线电测量研究所 | 一种机械臂空间轨迹跟踪动态补偿方法和系统 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111053613A (zh) | 2020-04-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110355751B (zh) | 控制装置和机器学习装置 | |
| EP3112096B1 (en) | Robot arm apparatus, calibration method, and program | |
| Tejado et al. | Fractional order human arm dynamics with variability analyses | |
| Wu et al. | A framework for calibration of electromagnetic surgical navigation system | |
| CN112720460B (zh) | 机器人控制方法、装置、计算机可读存储介质及机器人 | |
| CN113558771B (zh) | 机器人平面限位控制方法、装置和手术机器人 | |
| CN116277035B (zh) | 机器人的控制方法、装置、处理器及电子设备 | |
| CN111053613B (zh) | 一种运动追踪方法、装置、机械臂及计算机可读存储介质 | |
| Kobayashi et al. | Haptic feedback control in medical robots through fractional viscoelastic tissue model | |
| CN111113429B (zh) | 一种动作模仿方法、动作模仿装置及终端设备 | |
| CN114404047A (zh) | 定位方法、系统、装置、计算机设备和存储介质 | |
| CN113524201B (zh) | 机械臂位姿主动调节方法、装置、机械臂和可读存储介质 | |
| CN114089316A (zh) | 一种激光雷达-惯导的联合标定系统、方法及介质 | |
| CN116019564B (zh) | 膝关节手术机器人和控制方法 | |
| CN116392253A (zh) | 一种应用于手术机器人的主动摆位方法及系统 | |
| CN109773792A (zh) | 串联弹性驱动器的位置控制装置及方法、存储介质、设备 | |
| WO2017141573A1 (ja) | 演算装置、演算方法及び演算プログラム | |
| CN114654466A (zh) | 自动标定方法、装置、系统、电子设备及存储介质 | |
| CN109916352B (zh) | 一种获取机器人tcp坐标的方法和装置 | |
| CN116672031B (zh) | 机器人的控制方法、装置、处理器及电子设备 | |
| CN117116459B (zh) | 康复机器人主动康复训练控制方法及装置 | |
| CN117428791B (zh) | 一种用于肩部四轴康复机器人的逆运动学求解方法及系统 | |
| Meiringer et al. | Modelling and calibration of a five link elastic boom of a mobile concrete pump | |
| Zhu et al. | A Noninvasive Calibration-Free and Model-Free Surgical Robot for Automatic Fracture Reduction | |
| CN117414197A (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 | ||
| EE01 | Entry into force of recordation of patent licensing contract |
Application publication date: 20200424 Assignee: Bone shengyuanhua robot (Shenzhen) Co., Ltd Assignor: LianBo Intelligent Technology Co.,Ltd. Contract record no.: X2020990000472 Denomination of invention: A motion tracking method, device, manipulator and computer-readable storage medium License type: Common License Record date: 20200903 |
|
| EE01 | Entry into force of recordation of patent licensing contract | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20210311 Address after: No 802 Shenzhen Research Institute Chinese University of Hong Kong No 10 Yuexing 2nd Road Gaoxin community Yuehai street Nanshan District Shenzhen City Guangdong Province Patentee after: Yuanhua Intelligent Technology (Shenzhen) Co.,Ltd. Address before: Room 402, Jardine Plaza, 1 Connaught Plaza, central, Hong Kong, China Patentee before: LianBo Intelligent Technology Co.,Ltd. |
|
| TR01 | Transfer of patent right |