CN113799116B - 直接力反馈控制方法、装置、控制器和机器人 - Google Patents

直接力反馈控制方法、装置、控制器和机器人 Download PDF

Info

Publication number
CN113799116B
CN113799116B CN202010538326.5A CN202010538326A CN113799116B CN 113799116 B CN113799116 B CN 113799116B CN 202010538326 A CN202010538326 A CN 202010538326A CN 113799116 B CN113799116 B CN 113799116B
Authority
CN
China
Prior art keywords
force
actual
mechanical arm
tail end
cartesian space
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
Application number
CN202010538326.5A
Other languages
English (en)
Other versions
CN113799116A (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.)
Beijing Youbixuan Intelligent Robot Co ltd
Original Assignee
Ubtech Robotics Corp
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 Ubtech Robotics Corp filed Critical Ubtech Robotics Corp
Priority to CN202010538326.5A priority Critical patent/CN113799116B/zh
Priority to US17/198,167 priority patent/US11654557B2/en
Publication of CN113799116A publication Critical patent/CN113799116A/zh
Application granted granted Critical
Publication of CN113799116B publication Critical patent/CN113799116B/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
    • 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
    • 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/085—Force or torque sensors
    • 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
    • B25J13/089—Determining the position of the robot with reference to its environment
    • 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/1602—Programme controls characterised by the control system, structure, architecture
    • 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/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/1607—Calculation of inertia, jacobian matrixes and inverses
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/37—Measurements
    • G05B2219/37091—Motion and force
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/39—Robotics, robotics to robotics hand
    • G05B2219/39346—Workspace impedance control
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/50—Machine tool, machine tool null till machine tool work handling
    • G05B2219/50391—Robot

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Human Computer Interaction (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Manipulator (AREA)

Abstract

本发明实施例提供一种直接力反馈控制方法、装置、控制器和机器人，该方法应用于机械臂，该方法包括：获取机械臂末端在笛卡尔空间的实际位置、实际速度和所受实际外力；根据获取的实际位置、实际速度、实际外力和机械臂末端的期望位置、期望速度和期望加速度计算末端在笛卡尔空间的阻抗控制分量；根据末端的期望交互力、实际外力和实际速度计算末端在笛卡尔空间的力控制分量；判断该实际外力是否大于预设阈值，若大于，则将该力控制分量与阻抗控制分量进行叠加，得到末端在笛卡尔空间的总力控制量。本发明的技术方案同时考虑了阻抗控制和直接力反馈闭环，在保证安全接触的情况下，能够实现更加精确的末端力控制，且接触过程中控制更加柔顺等。

Description

直接力反馈控制方法、装置、控制器和机器人
技术领域
本发明涉及机器人控制技术领域，尤其涉及一种直接力反馈控制方法、 装置、控制器和机器人。
背景技术
对机器人通常有两种控制方式，分别是直接力反馈控制方式和间接力 反馈控制方式，其中，直接力反馈控制方式是指将反馈的力或力矩作为力 控指令进行关节控制；而间接力控制方式通常是指将反馈的力或力矩转换 为相应的位置指令进行关节控制。利用直接力反馈控制可以实现一些对于 力控制有较高精度要求的场合，例如，在工业领域中对零件的打磨、擦洗 以及服务领域中进行按摩等。
然而，传统的直接力反馈控制器在实际应用过程中，当机器人从与环 境接触到非接触，或者从与环境非接触到接触的过程中，由于力反馈控制 器的快速响应往往会产生快速超过机器人关节限位，或者与环境接触时出 现力过大而导致对机器人或者环境造成破坏。
发明内容
有鉴于此，本发明的目的是为了克服现有技术中的不足，提供一种直 接力反馈控制方法、装置、控制器和机器人。
本发明的一实施例提供一种直接力反馈控制方法，应用于机械臂控制， 所述方法包括：
获取机械臂末端在笛卡尔空间的实际位置、实际速度和所受实际外力；
根据获取的所述实际位置、所述实际速度、所述实际外力和所述机械 臂末端的期望位置、期望速度和期望加速度计算所述机械臂末端在笛卡尔 空间的阻抗控制分量；
根据所述机械臂末端受到的期望交互力、所述实际外力和所述实际速 度计算所述机械臂末端在笛卡尔空间的力控制分量；
判断所述实际外力是否大于预设阈值，若大于，则将所述力控制分量 与所述阻抗控制分量进行叠加，得到所述机械臂末端在笛卡尔空间的总力 控制量。
在一些实施例中，上述的直接力反馈控制方法还包括：
若所述实际外力小于所述预设阈值，则将预设力控制分量与所述阻抗 控制分量进行叠加，得到所述机械臂末端在笛卡尔空间的总力控制量。
在一些实施例中，所述机械臂末端设有力传感器，所述获取机械臂末 端在笛卡尔空间的实际位置、实际速度和所受实际外力包括：
根据获取的所述机械臂在关节空间的关节角位移和关节角速度通过正 运动学函数计算所述机械臂末端在笛卡尔空间的实际位置和实际速度；
读取由所述力传感器测量得到所述机械臂末端在笛卡尔空间所受的实 际外力。
在一些实施例中，所述根据获取的所述实际位置、所述实际速度、所 述实际外力和所述机械臂末端的期望位置、期望速度和期望加速度计算所 述机械臂末端在笛卡尔空间的阻抗控制分量包括：
通过力选择矩阵对所述实际位置、所述实际速度、所述实际外力以及 所述机械臂末端在笛卡尔空间的期望位置、期望速度和期望加速度进行任 务子空间投影，以得到在所述任务子空间中各自对应的投影；
计算所述实际位置的投影与所述期望位置的投影之间的位置偏差，以 及所述实际速度的投影与所述期望速度的投影之间的速度偏差；
根据所述位置偏差、所述速度偏差和所述实际外力的投影基于弹簧-质 量-阻尼模型计算阻抗控制量，并根据所述阻抗控制量和所述期望加速度的 投影计算在所述任务子空间的加速度控制量；
将所述加速度控制量由所述任务子空间映射至所述笛卡尔空间后利用 雅克比矩阵计算对应的阻抗控制分量。
在一些实施例中，所述根据所述机械臂末端受到的期望交互力、所述 实际外力和所述实际速度计算所述机械臂末端在笛卡尔空间的力控制分量 包括：
通过力选择矩阵对所述实际外力、所述实际速度和所述机械臂末端在 笛卡尔空间的期望交互力进行任务子空间投影，以得到在所述任务子空间 中各自对应的投影；
计算所述实际外力的投影与所述期望交互力的投影之间的力偏差，并 根据所述力偏差经过比例-积分运算后叠加由所述实际速度的投影得到的速 度阻尼量，以得到在所述任务子空间的力控制量；
将所述力控制量由所述任务子空间映射至所述笛卡尔空间后利用雅克 比矩阵计算对应的力控制分量。
在一些实施例中，所述预设力控制分量为0。
本发明的一实施例还提供一种直接力反馈控制装置，应用于机械臂控 制，所述装置包括：
获取模块，用于获取机械臂末端在笛卡尔空间的实际位置、实际速度 和所受实际外力；
阻抗控制分量计算模块，用于根据获取的所述实际位置、所述实际速 度、所述实际外力和所述机械臂末端的期望位置、期望速度和期望加速度 计算所述机械臂末端在笛卡尔空间的阻抗控制分量；
力控制分量计算模块，用于根据所述机械臂末端受到的期望交互力、 所述实际外力和所述实际速度计算所述机械臂末端在笛卡尔空间的力控制 分量；
力控制总量输出模块，用于判断所述实际外力是否大于预设阈值，若 大于，则将所述力控制分量与所述阻抗控制分量进行叠加，得到并输出所 述机械臂末端在笛卡尔空间的总力控制量。
进一步地，在上述的直接力反馈控制装置中，还包括：
所述力控制总量输出模块还用于若所述实际外力小于所述预设阈值， 则将预设力控制分量与所述阻抗控制分量进行叠加，得到所述机械臂末端 在笛卡尔空间的总力控制量。
本发明的一实施例还提供一种控制器，应用于机械臂控制，所述控制 器包括处理器和存储器，所述存储器存储有计算机程序，所述处理器用于 执行所述计算机程序以实施上述的直接力反馈控制方法。
本发明的一实施例还提供一种机器人，采用上述的控制器进行机械臂 控制。
本发明的一实施例还提供一种计算机可读存储介质，其存储有计算机 程序，所述计算机程序被执行时，实施上述的直接力反馈控制方法。
本发明的实施例具有如下优点：
本发明实施例的直接力反馈控制方法通过计算两部分控制量，分别是 阻抗控制分量以及根据机械臂末端受到的外力进行直接力闭环反馈计算得 到的力控制分量，通过结合两部分控制量得到对机械臂末端的力控制输出， 本发明的方法同时考虑了阻抗和直接力反馈闭环，与现有的力控方法相比， 在保证安全接触的情况下，能够实现更加精确的末端力控制，且接触过程 中控制更加柔顺等。
附图说明
为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需 要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些 实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲， 在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。
图1示出了本发明实施例的直接力反馈控制方法的第一流程示意图；
图2示出了本发明实施例的直接力反馈控制方法的第二流程示意图；
图3示出了本发明实施例的直接力反馈控制方法的第三流程示意图；
图4示出了本发明实施例的直接力反馈控制方法的第四流程示意图；
图5示出了本发明实施例的一种机械臂控制器的应用示意图；
图6示出了本发明实施例的直接力反馈控制装置的结构示意图。
主要元件符号说明：
10-直接力反馈控制装置；110-获取模块；120-阻抗控制分量计算模块； 130-力控制分量计算模块；140-力控制总量输出模块。
具体实施方式
下面将结合本发明实施例中附图，对本发明实施例中的技术方案进行 清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例， 而不是全部的实施例。
通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同 的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详 细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选 定实施例。基于本发明的实施例，本领域技术人员在没有做出创造性劳动 的前提下所获得的所有其他实施例，都属于本发明保护的范围。
在下文中，可在本发明的各种实施例中使用的术语“包括”、“具有” 及其同源词仅意在表示特定特征、数字、步骤、操作、元件、组件或前述 项的组合，并且不应被理解为首先排除一个或更多个其它特征、数字、步 骤、操作、元件、组件或前述项的组合的存在或增加一个或更多个特征、 数字、步骤、操作、元件、组件或前述项的组合的可能性。
此外，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理 解为指示或暗示相对重要性。
除非另有限定，否则在这里使用的所有术语(包括技术术语和科学术 语)具有与本发明的各种实施例所属领域普通技术人员通常理解的含义相 同的含义。所述术语(诸如在一般使用的词典中限定的术语)将被解释为具 有与在相关技术领域中的语境含义相同的含义并且将不被解释为具有理 想化的含义或过于正式的含义，除非在本发明的各种实施例中被清楚地限 定。
实施例1
请参照图1，本实施例提出一种直接力反馈控制方法，可应用于对具有 冗余自由度的机械臂及包含该机械臂的机器人的控制等，该方法能够实现 对机械臂末端的精确力控制输出。下面对该直接力反馈控制方法进行详细 说明。
步骤S10，获取机械臂末端在笛卡尔空间的实际位置、实际速度和所受 实际外力。
示范性地，可通过机械臂关节处安装的角位移传感器或位置编码器等 采集各个关节的角位移(也称为轨迹信息)qact，进而对角位移qact进行差 分即可得到对应的关节角速度
Figure BDA0002537858340000071
当然，也可以采用角速度传感器直接测 量得到各关节角速度等。
由于上述获取得到的与关节相关的数据是基于关节空间而言的，为实 现对关节的直接力矩或力控制，需要根据在关节空间中的这些关节数据利 用正运动学计算出机械臂的末端在笛卡尔空间中的实际位置和实际速度。 其中，关于通过机器人正运动学如何计算机械臂关节与末端之间的映射关 系可参见已公开的相关文献，在此不作详述。可以理解，机械臂的末端在 笛卡尔空间中执行任务，故本实施例中的笛卡尔空间也称为任务空间。
示范性地，该机械臂末端处设有相应的力传感器，如六维力传感器等， 通过该力传感器可以测量得到该机械臂末端与外界接触面之间的交互力， 即末端实际受到的外力。
本实施例中，在计算对机械臂的力控制输出时，将考虑两部分控制量， 分别是基于阻抗控制的控制量，以及利用机械臂末端所受到的实际外力与 期望交互力之间的力偏差进行直接力反馈闭环而得到的控制量。通过引入 力反馈闭环及阻抗控制，将这两种控制量的叠加作为末端的总力控输出， 可以实现更加精确的末端力控制，使得末端表现出更加柔顺。
步骤S20，根据获取的实际位移、实际速度、实际外力和该机械臂末端 的期望位移、期望速度和期望加速度计算该机械臂末端在笛卡尔空间的阻 抗控制分量。
在一种实施例中，如图2所示，对于上述步骤S20的阻抗控制分量的 计算，主要包括如下步骤：
步骤S210，通过力选择矩阵对得到的实际位移、实际速度、实际外力 以及机械臂末端在笛卡尔空间的期望位移、期望速度和期望加速度进行任 务子空间投影，以得到在该任务子空间中各自对应的投影。
本实施例中，该阻抗控制主要是根据位置偏差、速度偏差以及环境外 力产生笛卡尔空间的广义控制力，然后转换为关节空间的力矩，以用于控 制机械臂的运动。通常地，选择矩阵主要用于将笛卡尔空间中的相关参数 投影到选取的控制方向的任务子空间，如上述的位置、速度等运动学参数 和/或该环境外力等动力学参数。示范性地，若选取位置控制方向，则任务 子空间为位置控制子空间；若选取力控制方向，则任务子空间为力控制子 空间。本实施例中，所述的力选择矩阵为力控制方向对应的选择矩阵，具 体可根据实际需求进行自定义，应满足该力选择矩阵的列向量相互正交。
示范性地，利用预先定义的力选择矩阵对上述的实际位移、实际速度、 实际外力以及期望位移、期望速度和期望加速度进行力控制子空间转换后， 将得到各自在该力控制子空间的对应投影。
例如，若力选择矩阵为SF，获取的机械臂末端在笛卡尔空间的实际位 移为xact，则其在当前任务子空间中的投影即为：
Figure RE-GDA0002590495700000091
或
Figure RE-GDA0002590495700000092
其中，
Figure RE-GDA0002590495700000093
和
Figure RE-GDA0002590495700000094
分别为该力选择矩阵的伪逆和转置。
类似地，对于其他的参数，则有实际速度
Figure RE-GDA0002590495700000095
的投影为
Figure RE-GDA0002590495700000096
实际 外力Fext的投影为
Figure RE-GDA0002590495700000097
期望位移xref的投影为
Figure RE-GDA0002590495700000098
期望速度
Figure RE-GDA0002590495700000099
的 投影为
Figure RE-GDA00025904957000000910
和期望加速度
Figure RE-GDA00025904957000000911
的投影为
Figure RE-GDA00025904957000000912
步骤S220，计算该实际位移的投影与期望位移的投影之间的位移偏差， 以及实际速度的投影与期望速度的投影之间的速度偏差。
示范性地，实际位移的投影
Figure RE-GDA00025904957000000913
与期望位移的投影
Figure RE-GDA00025904957000000914
之间的位移 偏差为
Figure RE-GDA00025904957000000915
即在当前任务子空间中的位移偏差。类似地，在当 前任务子空间中的速度偏差为
Figure RE-GDA00025904957000000916
步骤S230，根据所述位移偏差、所述速度偏差和所述实际外力的投影 基于弹簧-质量-阻尼模型计算阻抗控制量，并根据该阻抗控制量和该期望加 速度的投影计算在该任务子空间的加速度控制量。
为保证机械臂的柔顺性，可将机械臂等效为一个弹簧-质量-阻尼系统， 并基于弹簧-质量-阻尼模型计算对应的阻抗控制量。示范性地，若该阻抗控 制对应的刚度系数为Kd，质量系数为Md，阻尼系数为Dd，则阻抗控制量为：
Figure BDA00025378583400000917
然后，将该阻抗控制量与期望加速度的投影进行叠加，以得到该当前 任务子空间的加速度控制量。示范性地，若期望加速度的投影为
Figure BDA0002537858340000101
实际 外力的投影记为
Figure BDA0002537858340000102
该任务子空间中的加速度控制量
Figure BDA0002537858340000103
满足如下公式：
Figure BDA0002537858340000104
步骤S240，将该加速度控制量由该任务子空间映射至笛卡尔空间后利 用雅克比矩阵计算对应的阻抗控制分量。
由于计算得到的是当前任务子空间中的加速度控制量，利用该力选择 矩阵将其映射至笛卡尔空间；之后，根据笛卡尔空间的加速度控制量利用 雅克比矩阵计算关节空间对应的关节角加速度，最终计算得到对应的关节 力矩阻抗控制分量。
示范性地，若任务子空间中的加速度控制量为
Figure RE-GDA0002590495700000106
笛卡尔空间的加 速度控制量为
Figure RE-GDA0002590495700000107
雅克比矩阵为J，其广义逆为J#，则关节角加速度
Figure RE-GDA0002590495700000108
以及关节力矩阻抗控制分量τCarImp将满足：
Figure RE-GDA0002590495700000109
Figure RE-GDA00025904957000001010
步骤S30，根据该机械臂末端受到的期望交互力、实际外力和实际速度 计算该机械臂末端在笛卡尔空间的力控制分量。
在一种实施例中，如图3所示，对于上述步骤S30的力控制分量的计 算，主要包括如下步骤：
步骤S310，通过力选择矩阵对所述实际外力、所述实际速度和所述机 械臂末端在笛卡尔空间的期望交互力进行任务子空间投影，以得到在该任 务子空间中各自对应的投影。
对于该力控制分量，将选取的力控制方向的子空间作为当前的任务子 空间。可以理解，该力反馈控制部分的力选择矩阵与上述的阻抗控制部分 的力选择矩阵为同一矩阵。示范性地，若该力选择矩阵为SF，机械臂末端 在笛卡尔空间的期望交互力为Fref，经过子空间变换后，可得到期望交互 力的投影为
Figure RE-GDA0002590495700000111
或
Figure RE-GDA0002590495700000112
类似地，机械臂末端受到的 实际外力的投影为
Figure RE-GDA0002590495700000113
实际速度投影为
Figure RE-GDA0002590495700000114
步骤S320，计算实际外力的投影与期望交互力的投影之间的力偏差， 并根据所述力偏差经过比例-积分运算后叠加由实际速度的投影得到的速度 阻尼量，以得到在该任务子空间的力控制量。
示范性地，对于计算得到的力偏差，经过比例-积分(PI)运算后作为 关节力控制量的一部分。本实施例中，还添加一速度阻尼项作为力控制量 的另一部分，通过将这两部分叠加作为所需的力控制分量。可以理解，通 过计算在任务子空间中实际外力与期望交互力之间的力偏差来直接计算对 机械臂进行力控制的力控制分量，属于直接力反馈控制方式。
对于上述的速度阻尼项，可通过该机械臂末端在任务子空间中的实际 速度的投影
Figure RE-GDA0002590495700000115
及该力控制部分对应的阻尼系数DF计算得到，即为
Figure RE-GDA0002590495700000116
步骤S330，将该力控制量由该任务子空间映射至笛卡尔空间后利用雅 克比矩阵计算对应的力控制分量。
由于计算得到的是当前任务子空间中的力控制分量，利用力选择矩阵 将其映射至笛卡尔空间；之后，根据在笛卡尔空间的力控制量利用雅克比 矩阵计算关节空间对应的关节控制力矩，即关节力矩阻抗控制分量。
示范性地，若在任务子空间的力控制量记为
Figure RE-GDA0002590495700000121
力选择矩阵为SF， 笛卡尔空间的力控制量为Fcmd，雅克比矩阵为J，其转置为JT，则对应的 关节力控制分量τcmd满足：
Figure RE-GDA0002590495700000122
步骤S40，判断所述实际外力是否大于预设阈值，若大于，则将所述力 控制分量与所述阻抗控制分量进行叠加，得到机械臂末端在笛卡尔空间的 总力控制量。
本实施例中，根据机械臂末端所受的实际外力，对末端与目标环境之 间的接触完成和脱离接触这两个过程进行区分，从而实现更加精确的末端 力控制。示范性地，可将末端在笛卡尔空间中受到的实际外力通过力选择 矩阵投影至力控制方向所在的任务子空间中进行判断，例如，当在任务子 空间的实际外力投影大于一预设阈值时，则可判断机械臂末端与目标环境 接触完成；反之，若该实际外力投影小于等于该预设阈值，则判断机械臂末端还没有完成接触或脱离了接触。
例如，若该预设阈值设为1，则可判断该实际外力投影是否大小1。若 大于，则判断该机械臂与目标环境接触完成；若小于，则判断该机械臂还 没有完成接触或脱离了接触。可以理解，该预设阈值可以根据实际需求进 行设定，在此并不作限定。
而在判断出末端接触完成时，将根据受到的实际外力进行直接力反馈 闭环计算得到力控制分量，并与阻抗控制分量进行叠加作为关节的总力控 制量。可以理解，对于该接触完成的情况，末端会受到环境对其的作用力， 而利用闭环反馈的作用力直接计算对应的力控制量，可以对末端力控制实 现很好地跟踪，进而达到不断调整末端的力控制输出目的。
在一些实施例中，如图4所示，对于上述的机械臂末端还没有完成接 触或脱离了接触的情况时，该直接力反馈控制方法还包括：
步骤S50，若所述实际外力小于或等于所述预设阈值，则将预设力控制 分量与所述阻抗控制分量进行叠加，得到机械臂末端在笛卡尔空间的总力 控制量。
示范性地，若实际外力小于等于上述的预设阈值，则此时可将预设力 控制分量进行输出并与阻抗控制分量进行叠加，以作为机械臂末端的总力 控制量。在一些实施方式中，该预设力控制分量设为0。
可以理解，本实施例的用于输出到末端进行力控制的控制量由两部分 组成，一部分是由阻抗控制的控制量，另一部分则主要是根据机械臂末端 在测量得到的实际所受外力进行力反馈闭环得到的控制量。
图5示出了一种采用本实施例的阻抗方法的控制器内部结构示意图。 该控制器主要包括用于输出阻抗控制分量τCarImp的阻抗控制部分和用于输出 力控制分量τcmd的力反馈闭环控制部分。其中，对于该力反馈闭环控制部分， 除了对实际外力与期望交互力的力偏差进行PI运算外，还增加一速度阻尼 项，以及区分机械臂末端与目标环境的接触情况来输出对应的力控制分量， 这样可以在保证安全接触的同时，对于不同的接触阶段实现区别控制，从 而实现更加精确的末端力控制输出。
实施例2
请参照图6，基于上述实施例1的直接力反馈控制方法，本实施例提出 一种直接力反馈控制装置10，应用于机械臂控制，该装置包括：
获取模块110，用于获取机械臂末端在笛卡尔空间的实际位置、实际速 度和所受实际外力。
阻抗控制分量计算模块120，用于根据获取的所述实际位置、所述实际 速度、所述实际外力和机械臂末端的期望位置、期望速度和期望加速度计 算机械臂末端在笛卡尔空间的阻抗控制分量。
力控制分量计算模块130，用于根据机械臂末端受到的期望交互力、所 述实际外力和所述实际速度计算机械臂末端在笛卡尔空间的力控制分量。
力控制总量输出模块140，用于判断所述实际外力是否大于预设阈值， 若大于，则将所述力控制分量与所述阻抗控制分量进行叠加，得到并输出 机械臂末端在笛卡尔空间的总力控制量。
在一些实施例中，该直接力反馈控制装置10还包括：力控制总量输出 模块140还用于若所述实际外力小于所述预设阈值，则将预设力控制分量 与所述阻抗控制分量进行叠加，得到机械臂末端在笛卡尔空间的总力控制 量。
例如，对于上述的力控制分量和预设力控制分量，可通过一软开关来 实现不同情况下的切换输出。示范性地，该开关的控制端与内部存储的预 设阈值关联，力控制分量与预设力控制分量分别作为该开关的不同输入， 该开关的输出与阻抗控制分量进行叠加操作。
可以理解，上述的直接力反馈控制装置10对应于实施例1的直接力反 馈控制方法。实施例1中的任何可选项也适用于本实施例，这里不再详述。
实施例3
本发明还提供了一种控制器，应用于机械臂控制，该控制器包括处理 器和存储器，其中，存储器存储有计算机程序，处理器用于执行所述计算 机程序以实施上述实施例1的直接力反馈控制方法或上述实施例2的直接 力反馈控制装置10中各个模块的功能，进而对机械臂进行精确的末端力控 制。
本发明还提出一种机器人，该机器人包括上述的控制器，并通过该控 制器对机械臂进行柔顺控制。示范性地，该机器人可为运用在各领域内的 机器人，如切割机器人、焊接机器人、打磨机器人或按摩机器人等等。
本发明还提供了一种计算机可读存储介质，用于储存上述控制器中使 用的所述计算机程序。
在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法， 也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的， 例如，附图中的流程图和结构图显示了根据本发明的多个实施例的装置、 方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上， 流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分， 所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在作为替换的实现方式中，方框中所标注 的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框 实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所 涉及的功能而定。也要注意的是，结构图和/或流程图中的每个方框、以及 结构图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用 的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
另外，在本发明各个实施例中的各功能模块或单元可以集成在一起形 成一个独立的部分，也可以是各个模块单独存在，也可以两个或更多个模 块集成形成一个独立的部分。
所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使 用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发 明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的 部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储 介质中，包括若干指令用以使得一台计算机设备(可以是智能手机、个人 计算机、服务器、或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM， Read-OnlyMemory)、随机存取存储器(RAM，Random Access Memory)、 磁碟或者光盘等各种可以存储程序代码的介质。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局 限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可 轻易想到变化或替换，都应涵盖在本发明的保护范围之内。

Claims (8)

1.一种直接力反馈控制方法，其特征在于，应用于机械臂控制，所述方法包括：
获取机械臂末端在笛卡尔空间的实际位置、实际速度和所受实际外力；
根据获取的所述实际位置、所述实际速度、所述实际外力和所述机械臂末端的期望位置、期望速度和期望加速度计算所述机械臂末端在笛卡尔空间的阻抗控制分量；
根据所述机械臂末端受到的期望交互力、所述实际外力和所述实际速度计算所述机械臂末端在笛卡尔空间的力控制分量；
判断所述实际外力是否大于预设阈值，若大于，则将所述力控制分量与所述阻抗控制分量进行叠加，得到所述机械臂末端在笛卡尔空间的总力控制量；
若所述实际外力小于所述预设阈值，则将预设力控制分量与所述阻抗控制分量进行叠加，得到所述机械臂末端在笛卡尔空间的总力控制量。
2.根据权利要求1所述的直接力反馈控制方法，其特征在于，所述机械臂末端设有力传感器，所述获取机械臂末端在笛卡尔空间的实际位置、实际速度和所受实际外力包括：
根据获取的所述机械臂在关节空间的关节角位移和关节角速度通过正运动学函数计算所述机械臂末端在笛卡尔空间的实际位置和实际速度；
读取由所述力传感器测量得到所述机械臂末端在笛卡尔空间所受的实际外力。
3.根据权利要求1所述的直接力反馈控制方法，其特征在于，所述根据获取的所述实际位置、所述实际速度、所述实际外力和所述机械臂末端的期望位置、期望速度和期望加速度计算所述机械臂末端在笛卡尔空间的阻抗控制分量包括：
通过力选择矩阵对所述实际位置、所述实际速度、所述实际外力以及所述机械臂末端在笛卡尔空间的期望位置、期望速度和期望加速度进行任务子空间投影，得到在所述任务子空间中各自对应的投影；
计算所述实际位置的投影与所述期望位置的投影之间的位置偏差，以及所述实际速度的投影与所述期望速度的投影之间的速度偏差；
根据所述位置偏差、所述速度偏差和所述实际外力的投影基于弹簧-质量-阻尼模型计算阻抗控制量，并根据所述阻抗控制量和所述期望加速度的投影计算在所述任务子空间的加速度控制量；
将所述加速度控制量由所述任务子空间映射至所述笛卡尔空间后利用雅克比矩阵计算对应的阻抗控制分量。
4.根据权利要求1所述的直接力反馈控制方法，其特征在于，所述根据所述机械臂末端受到的期望交互力、所述实际外力和所述实际速度计算所述机械臂末端在笛卡尔空间的力控制分量包括：
通过力选择矩阵对所述实际外力、所述实际速度和所述机械臂末端在笛卡尔空间的期望交互力进行任务子空间投影，以得到在所述任务子空间中各自对应的投影；
计算所述实际外力的投影与所述期望交互力的投影之间的力偏差，并根据所述力偏差经过比例-积分运算后叠加由所述实际速度的投影得到的速度阻尼量，得到在所述任务子空间的力控制量；
将所述力控制量由所述任务子空间映射至所述笛卡尔空间后利用雅克比矩阵计算对应的力控制分量。
5.根据权利要求1所述的直接力反馈控制方法，其特征在于，所述预设力控制分量为0。
6.一种直接力反馈控制装置，其特征在于，应用于机械臂控制，所述装置包括：
获取模块，用于获取机械臂末端在笛卡尔空间的实际位置、实际速度和所受实际外力；
阻抗控制分量计算模块，用于根据获取的所述实际位置、所述实际速度、所述实际外力和所述机械臂末端的期望位置、期望速度和期望加速度计算所述机械臂末端在笛卡尔空间的阻抗控制分量；
力控制分量计算模块，用于根据所述机械臂末端受到的期望交互力、所述实际外力和所述实际速度计算所述机械臂末端在笛卡尔空间的力控制分量；
力控制总量输出模块，用于判断所述实际外力是否大于预设阈值，若大于，则将所述力控制分量与所述阻抗控制分量进行叠加，得到所述机械臂末端在笛卡尔空间的总力控制量；
所述力控制总量输出模块还用于若所述实际外力小于所述预设阈值，则将预设力控制分量与所述阻抗控制分量进行叠加，得到并输出所述机械臂末端在笛卡尔空间的总力控制量。
7.一种控制器，其特征在于，应用于机械臂控制，所述控制器包括处理器和存储器，所述存储器存储有计算机程序，所述处理器用于执行所述计算机程序以实施权利要求1-5中任一项所述的直接力反馈控制方法。
8.一种机器人，其特征在于，采用根据权利要求7所述的控制器进行机械臂控制。
CN202010538326.5A 2020-06-12 2020-06-12 直接力反馈控制方法、装置、控制器和机器人 Active CN113799116B (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN202010538326.5A CN113799116B (zh) 2020-06-12 2020-06-12 直接力反馈控制方法、装置、控制器和机器人
US17/198,167 US11654557B2 (en) 2020-06-12 2021-03-10 Direct force feedback control method, and controller and robot using the same

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010538326.5A CN113799116B (zh) 2020-06-12 2020-06-12 直接力反馈控制方法、装置、控制器和机器人

Publications (2)

Publication Number Publication Date
CN113799116A CN113799116A (zh) 2021-12-17
CN113799116B true CN113799116B (zh) 2022-07-29

Family

ID=78824385

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010538326.5A Active CN113799116B (zh) 2020-06-12 2020-06-12 直接力反馈控制方法、装置、控制器和机器人

Country Status (2)

Country Link
US (1) US11654557B2 (zh)
CN (1) CN113799116B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114193452B (zh) * 2021-12-29 2024-04-26 佛山智异科技开发有限公司 多关节布料臂架末端轨迹控制方法、系统及存储介质
CN114536335A (zh) * 2022-02-24 2022-05-27 上海电气集团股份有限公司 机器人机械结构的控制方法、系统、电子设备及存储介质
CN117481719B (zh) * 2024-01-03 2024-04-30 北京壹点灵动科技有限公司 撑开器的控制方法和装置、存储介质及电子设备

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105700465A (zh) * 2014-11-26 2016-06-22 中国科学院沈阳自动化研究所 基于EtherCAT总线的机器人柔顺控制系统和方法
CN108639313A (zh) * 2018-05-22 2018-10-12 南京航空航天大学 一种飞机主动侧杆系统的高精度杆力控制方法
CN110421547A (zh) * 2019-07-12 2019-11-08 中南大学 一种基于估计动力学模型的双臂机器人协同阻抗控制方法
WO2020001742A1 (en) * 2018-06-26 2020-01-02 Franka Emika Gmbh Device for controlling a robot manipulator
CN110962129A (zh) * 2019-12-20 2020-04-07 中国科学院宁波材料技术与工程研究所 一种机械臂阻抗控制方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
GB2549072B (en) * 2016-03-24 2020-07-29 Cmr Surgical Ltd Robot control
DE102016004840A1 (de) * 2016-04-24 2017-10-26 Kastanienbaum GmbH Mobiler Roboter
US10583557B2 (en) * 2017-02-10 2020-03-10 GM Global Technology Operations LLC Redundant underactuated robot with multi-mode control framework
US20200282558A1 (en) * 2019-03-07 2020-09-10 LinkDyn Robotics Inc. System and method for controlling a robot with torque-controllable actuators
EP4321461A3 (en) * 2019-03-27 2024-05-08 Boston Dynamics, Inc. Robot and method for palletizing boxes

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105700465A (zh) * 2014-11-26 2016-06-22 中国科学院沈阳自动化研究所 基于EtherCAT总线的机器人柔顺控制系统和方法
CN108639313A (zh) * 2018-05-22 2018-10-12 南京航空航天大学 一种飞机主动侧杆系统的高精度杆力控制方法
WO2020001742A1 (en) * 2018-06-26 2020-01-02 Franka Emika Gmbh Device for controlling a robot manipulator
CN110421547A (zh) * 2019-07-12 2019-11-08 中南大学 一种基于估计动力学模型的双臂机器人协同阻抗控制方法
CN110962129A (zh) * 2019-12-20 2020-04-07 中国科学院宁波材料技术与工程研究所 一种机械臂阻抗控制方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于指尖力传感器的HIT机器人灵巧手笛卡尔阻抗控制;杨磊等;《控制与决策》;20041130(第11期);全文 *

Also Published As

Publication number Publication date
US20210387334A1 (en) 2021-12-16
US11654557B2 (en) 2023-05-23
CN113799116A (zh) 2021-12-17

Similar Documents

Publication Publication Date Title
CN113799116B (zh) 直接力反馈控制方法、装置、控制器和机器人
CN111730599B (zh) 阻抗控制方法、装置、阻抗控制器和机器人
Li et al. Finite-time output feedback tracking control for autonomous underwater vehicles
Zhang et al. Robust observer-based fault diagnosis for nonlinear systems using MATLAB®
CN111538234B (zh) 任务分层控制方法、装置、机器人和可读存储介质
US9427870B2 (en) Robotics apparatus, robot control method, program, and recording medium
WO2020017092A1 (ja) パラメータ同定装置、方法、及びプログラム
US20180154519A1 (en) Robot teaching system and control method thereof
EP3351353A1 (en) Robot control apparatus, robot system, robot control method and assembly manufacturing method
US11669055B2 (en) Vibration suppression device, method and computer-readable medium using estimated vibration torque
Urrea et al. Design and validation of a dynamic parameter identification model for industrial manipulator robots
JP3726884B2 (ja) 慣性計測装置を用いた姿勢推定装置及び方法並びにプログラム
Korayem et al. Application of the state‐dependent Riccati equation for flexible‐joint arms: Controller and estimator design
CN115157249A (zh) 一种动态目标跟踪方法、装置和机器人
CN113442115A (zh) 机器人拖动示教轨迹复现方法及系统、计算机存储介质
Guo et al. Nonlinear vision-based observer for visual servo control of an aerial robot in global positioning system denied environments
JP2011104740A (ja) 力制御装置
Kaminer et al. Application of nonlinear filtering to navigation system design using passive sensors
CN112975965B (zh) 仿人机器人的解耦控制方法、装置和仿人机器人
CN113927603B (zh) 机械臂拖动控制方法、装置、计算机设备以及存储介质
CN113515142B (zh) 无人机轨迹跟踪控制方法、装置、无人机和存储介质
Valera et al. Design and implementation of Kalman filters applied to Lego NXT based robots
Ta et al. Iterative learning control for integrated system of robot and machine tool
Bellali et al. Parameter estimation for fault diagnosis in nonlinear systems by ANFIS
Indri et al. Development of a general friction identification framework for industrial manipulators

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
TR01 Transfer of patent right

Effective date of registration: 20231204

Address after: Room 601, 6th Floor, Building 13, No. 3 Jinghai Fifth Road, Beijing Economic and Technological Development Zone (Tongzhou), Daxing District, Beijing, 100176

Patentee after: Beijing Youbixuan Intelligent Robot Co.,Ltd.

Address before: 518000 16th and 22nd Floors, C1 Building, Nanshan Zhiyuan, 1001 Xueyuan Avenue, Nanshan District, Shenzhen City, Guangdong Province

Patentee before: Shenzhen Youbixuan Technology Co.,Ltd.