CN114083537B - 机械臂夹持控制方法、装置、机器人和可读存储介质 - Google Patents

机械臂夹持控制方法、装置、机器人和可读存储介质 Download PDF

Info

Publication number
CN114083537B
CN114083537B CN202111443726.9A CN202111443726A CN114083537B CN 114083537 B CN114083537 B CN 114083537B CN 202111443726 A CN202111443726 A CN 202111443726A CN 114083537 B CN114083537 B CN 114083537B
Authority
CN
China
Prior art keywords
control
subtask
priority
robot
joint
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
CN202111443726.9A
Other languages
English (en)
Other versions
CN114083537A (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.)
Ubtech Robotics Corp
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 CN202111443726.9A priority Critical patent/CN114083537B/zh
Publication of CN114083537A publication Critical patent/CN114083537A/zh
Application granted granted Critical
Publication of CN114083537B publication Critical patent/CN114083537B/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/1679—Programme controls characterised by the tasks executed
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J15/00—Gripping heads and other end effectors
    • B25J15/08—Gripping heads and other end effectors having finger members
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J18/00—Arms
    • 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/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1661—Programme controls characterised by programming, planning systems for manipulators characterised by task planning, object-oriented languages
    • 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/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
    • Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
    • Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]

Abstract

本申请实施例提供一种机械臂夹持控制方法、装置、机器人和可读存储介质，该方法包括：获取机械臂执行夹持任务时的实际运动状态，所述实际运动状态包括各个优先级子任务的实际值；根据对应优先级子任务的期望值和实际值计算对应优先级子任务的任务控制量；按照优先级由高到低的顺序，依次根据对应优先级子任务的任务控制量进行逆运动学优化求解，得到所需的目标关节控制量；根据该目标关节控制量对物体进行夹持控制。该方法能够在机械臂执行夹持任务时，尤其是受到外力扰动等情况下，可以保证优先执行在夹持过程中更为重要的子任务，如稳定夹持物体等。

Description

机械臂夹持控制方法、装置、机器人和可读存储介质
技术领域
本申请涉及机器人控制技术领域，尤其涉及一种机械臂夹持控制方法、装置、机器人和可读存储介质。
背景技术
相较于常见的单机械臂平台，双臂机器人在灵活性、负载能力以及工作空间上存在明显的优势，更适用于执行更为复杂的操作任务。在实际应用场景中，比如双臂搬运箱子、端托盘等任务，常需要机器人双臂协同操作完成任务。在这个任务中，我们需要保证夹持物体的稳定不滑落，并同时控制夹持物体执行期望的运动轨迹。目前，常见的方法主要是基于抓取矩阵对物体受力进行内外力分解后，分别对二者进行控制以后，再映射到机械臂关节控制。然而，当机器人处于餐馆、商店、机场等复杂环境中，可能会受到来自周围的人或者环境的干扰或撞击，或是机械臂处于自身运动和力性能的边界上，在这种极端情况下，原先的稳定夹持物体和物体运动轨迹任务无法完全执行，此时基于抓取矩阵分解的控制器就失效了，且产生的碰撞力可能导致机械臂的损坏或对周围的人或物造成伤害。
发明内容
本申请实施例提供一种机械臂夹持控制方法、装置、机器人和可读存储介质，可以在机械臂执行夹持任务时，能够保证优先执行在夹持过程中更为重要的子任务，实现低优先级的子任务求解不影响高优先级的求解结果，从而保证了严格的分层执行效果等。
第一方面，本申请实施例提供一种机械臂夹持控制方法，包括：
获取机器人执行夹持任务时的实际运动状态，所述实际运动状态包括各个优先级子任务的实际值；
根据对应优先级子任务的期望值和所述实际值计算对应优先级子任务在任务空间的任务控制量；
按照优先级由高到低的顺序，依次根据对应优先级子任务的所述任务控制量进行逆运动学优化求解，得到所需的目标关节控制量；其中，在先得到的各个优先级子任务的关节控制量用于作为求解下一优先级子任务的关节控制量的约束条件，将最低优先级子任务的关节控制量作为所述目标关节控制量；
根据所述目标关节控制量对被夹物体进行夹持控制。
在一些实施例中，所述多个优先级子任务包括三个，分别是优先级由高到低的物体稳定夹持控制子任务、物体运动控制子任务以及所述机器人的关节运动控制子任务。
在一些实施例中，所述物体稳定夹持控制子任务的任务控制量的获取，包括：
根据所述机器人的机械臂产生的实际夹持内力与期望夹持力之间的偏差、物体在夹持内空间的实际位置与期望位置之间的偏差、以及在夹持内空间的期望速度，计算得到用于稳定夹持的速度控制量。
在一些实施例中，所述物体运动控制子任务的任务控制量的获取，包括：
根据物体受到的实际合外力与期望合外力之间的偏差、物体在世界坐标空间的实际位置与期望位置之间的偏差、以及在世界坐标空间的期望速度，计算得到用于控制物体运动的速度控制量。
在一些实施例中，所述机器人的关节运动控制子任务的任务控制量的获取，包括：
根据所述机器人各关节受到的外力矩、各关节在关节空间的实际位置与期望位置之间的偏差、以及在关节空间的期望速度，计算得到用于控制关节空间运动的速度控制量。
在一些实施例中，所述机器人的机械臂产生的实际夹持内力的计算，包括：
通过所述机器人的双臂末端各自设置的传感器分别获得第一臂和第二臂末端在各自坐标系下受到的作用力；
根据第一臂坐标系与第二臂坐标系的坐标变换矩阵、及所述第二臂坐标系下的所述作用力，计算得到转换到所述第一臂坐标系下的第一转换作用力；
根据所述第一臂坐标系下的所述作用力和所述第一转换作用力，计算得到机械臂产生的实际夹持内力。
在一些实施例中，所述物体受到的实际合外力的获取，包括：
根据所述第一臂坐标系与世界坐标系的坐标变换矩阵、及所述第一臂坐标系下的作用力，计算得到转换到世界坐标系下的第二作用力；
根据所述第二臂坐标系与世界坐标系的坐标变换矩阵、及所述第二臂坐标系下的作用力，计算得到转换到世界坐标系下的第三作用力；
根据所述第二作用力和所述第三作用力，计算得到所述物体受到的实际合外力。
基于机器人逆运动学，每个优先级子任务预先构建有对应的关节控制指令优化函数，以用于求解对应子任务的最优关节控制量；
其中，每个优先级子任务的所述关节控制指令优化函数均以关节速度为优化变量且约束条件包括所述机器人满足的关节物理约束。
在一些实施例中，所述多个优先级子任务包括优先级最高的物体稳定夹持控制子任务，所述物体稳定夹持控制子任务的关节控制指令优化函数如下：
Figure BDA0003384170520000041
Figure BDA0003384170520000042
其中，E＝-J1+J2；
其中，
Figure BDA0003384170520000043
表示所述物体稳定夹持控制子任务的最优关节控制量；argmin‖‖2表示在约束条件范围内寻找使目标的二范数最小的函数；bm和bM分别表示所述机器人满足的关节物理约束的上边界和下边界；/>
Figure BDA0003384170520000044
表示关节速度；/>
Figure BDA0003384170520000045
表示所述物体稳定夹持控制子任务的任务控制量；J1和J2分别表示所述机器人的双臂末端的速度雅克比矩阵。
在一些实施例中，所述多个优先级子任务包括优先级次高的物体运动控制子任务，所述物体运动控制子任务的关节控制指令优化函数如下：
Figure BDA0003384170520000046
Figure BDA0003384170520000047
Figure BDA0003384170520000051
其中，
Figure BDA0003384170520000052
其中，
Figure BDA0003384170520000053
和/>
Figure BDA0003384170520000054
分别表示所述物体运动控制子任务的最优关节控制量和任务控制量。
在一些实施例中，所述多个优先级子任务包括优先级最低的关节运动控制子任务，所述关节运动控制子任务的关节控制指令优化函数如下：
Figure BDA0003384170520000055
Figure BDA0003384170520000056
Figure BDA0003384170520000057
Figure BDA0003384170520000058
其中，
Figure BDA0003384170520000059
和/>
Figure BDA00033841705200000510
分别表示所述关节运动控制子任务的最优关节控制量和任务控制量。
第二方面，本申请实施例还提供一种机械臂夹持控制装置，包括：
状态获取模块，用于获取机器人执行夹持任务时的实际运动状态，所述实际运动状态包括各个优先级子任务相应的实际值；
计算模块，用于根据对应优先级子任务的期望值和所述实际值计算对应优先级子任务在任务空间的任务控制量；
优化求解模块，用于按照优先级由高到低的顺序，依次根据对应优先级子任务的所述任务控制量进行逆运动学优化求解，得到所需的目标关节控制量；其中，在先得到的各个优先级子任务的关节控制量用于作为求解下一优先级子任务的关节控制量的约束条件，将最低优先级子任务的关节控制量作为所述目标关节控制量；
关节控制模块，用于根据所述目标关节控制量对被夹物体进行夹持控制。
第三方面，本申请实施例还提供一种机器人，所述机器人包括至少两条机械臂、处理器和存储器，所述机械臂用于夹持物体，所述存储器存储有计算机程序，所述处理器用于执行所述计算机程序以实施上述的机械臂夹持控制方法。
第四方面，本申请实施例还提供一种可读存储介质，其存储有计算机程序，所述计算机程序在处理器上执行时，实施上述的机械臂夹持控制方法。
本申请的实施例具有如下有益效果：
本实施例的机械臂夹持控制方法通过将机器人执行夹持物体的任务进行拆解，并依照夹持过程中这多个子任务的重要程度设置子任务的优先级，且设置不同导纳控制器进行任务控制量计算；与此同时，采用多优先级分层优化的方法求解机器人的逆运动学来得到最终的关节控制指令，这种多优先级的迭代求解能够实现低优先级的任务求解不影响高优先级的求解结果，从而保证了严格的分层执行效果，且每一层的优化求解过程中都考虑了关节的位置、速度等物理约束，使得计算的控制量不会超关节的实际能够执行的范围等。尤其是在如出现碰撞等复杂情况下可以保证物体的稳定夹持及柔顺响应，以降低碰撞带来的伤害等。
附图说明
为了更清楚地说明本申请实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本申请的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。
图1示出了本申请实施例的机械臂夹持控制方法的第一流程图；
图2示出了本申请实施例的机械臂夹持控制方法的系统架构图；
图3示出了本申请实施例的机械臂夹持控制方法的第二流程图；
图4示出了本申请实施例的机械臂夹持控制方法的第三流程图；
图5示出了本申请实施例的机械臂夹持控制装置的结构示意图。
具体实施方式
下面将结合本申请实施例中附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。
通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。
在下文中，可在本申请的各种实施例中使用的术语“包括”、“具有”及其同源词仅意在表示特定特征、数字、步骤、操作、元件、组件或前述项的组合，并且不应被理解为首先排除一个或更多个其它特征、数字、步骤、操作、元件、组件或前述项的组合的存在或增加一个或更多个特征、数字、步骤、操作、元件、组件或前述项的组合的可能性。
此外，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
除非另有限定，否则在这里使用的所有术语(包括技术术语和科学术语)具有与本申请的各种实施例所属领域普通技术人员通常理解的含义相同的含义。所述术语(诸如在一般使用的词典中限定的术语)将被解释为具有与在相关技术领域中的语境含义相同的含义并且将不被解释为具有理想化的含义或过于正式的含义，除非在本申请的各种实施例中被清楚地限定。
在复杂且带有交互的实际场景中，对于机器人执行机械臂夹持物体这一任务，本申请将该任务分解为多个小的子任务，如被夹物体的稳定夹持、被夹物体的运动控制、机械臂的运动控制及受到外力扰动时的柔顺响应等，同时需要保证上述控制指令满足机器人自身的硬件约束条件，如因机械结构不可达而存在的关节限位、关节限速等。并且按照这些子任务在夹持物体任务中的重要程度进行优先级划分，从而得到不同优先级的多个子任务。例如，在满足机器人关节自身硬件约束的情况下，通过优先保证夹持任务中被夹持的物体被稳定夹持不滑落，而后再控制被夹持的物体执行期望运动轨迹，最后再利用剩余的冗余自由度执行关节位置控制等。对于这些子任务，本申请采用导纳控制器来分别实现每个子任务的控制，使得机器人在受到外扰动和碰撞等时能够对碰撞力进行柔顺响应以降低碰撞带来的伤害。
下面结合具体的实施例来说明该机械臂夹持控制方法。
实施例1
图1所示为本实施例提出的机械臂夹持控制方法的一种流程图。
示范性地，该机械臂夹持控制方法包括：
步骤S110，获取机器人执行夹持任务时的实际运动状态，所述实际运动状态包括各个优先级子任务的实际值。
其中，上述的实际运动状态可包括该机器人各个关节的实际位置、实际速度、驱动力矩及受到的外力矩等，以及每个机械臂末端操作器的位置、速度、及受到的实际外力等，具体可根据实际任务需求获得，这里不作限定。可以理解，上述的各种运动状态有的基于机器人身上设置的本体传感器等直接采集得到，有时基于采集的数据作进一步计算得到。
例如，上述关节的位置可通过机器人各关节处设置的位置编码器等获得；关节的速度可通过对关节位置的差分操作获得；而关节的驱动力矩可通过关节处安装的力矩传感器或基于关节电流力矩系数等方法获得，关节外力矩可通过动量观测器估计得到等。
例如，上述的每个机械臂末端受到的外力可通过在各自末端操作器处安装的六维力/力矩传感器获得，以双臂夹持情况为例，这里将第一臂和第二臂末端在各自坐标系下受到的作用外力分别记为W1，W2∈R6，其中，每个机械臂末端受到的外力为六维向量。而机械臂末端操作器的位置可基于采集得到的关节位置通过机器人正运动学计算得到，如第一臂末端的位置x1和第二臂末端的位置x2分别满足，x1＝f1(q),x2＝f2(q)∈R6，其中，q为机器人各个关节的位置，f1()和f2()分别为正运动学函数。以及，末端操作器的速度可基于各关节的速度及雅克比矩阵计算得到，如第一臂末端的速度
Figure BDA0003384170520000091
和第二臂末端的速度/>
Figure BDA0003384170520000092
满足：
Figure BDA0003384170520000093
其中，/>
Figure BDA0003384170520000094
为机器人各个关节的速度，J1和J2分别为第一臂和第二臂对应的雅克比矩阵。
本实施例中，将机械臂夹持物体的任务划分为三个具有不同优先级的子任务，如图2所示，在一种实施方式中，分别是优先级由高到低的物体稳定夹持控制子任务、物体运动控制子任务、以及机器人的关节运动控制子任务，即在满足机器人关节自身硬件约束的情况下，通过优先保证夹持任务中被夹持的物体被稳定夹持不滑落，其次控制被夹持的物体执行期望的运动轨迹，最后再利用余下的冗余自由度来执行关节位置控制任务等。应当明白的是，这里主要以上述分解的三种子任务来实现基于多优先级的机械臂夹持控制。若在一些场景下还有其他的需求，则可以划分为更多的子任务，如还可包括机械臂肘部的运动控制等，这里不作限定。
示范性地，当机器人在执行机械臂夹持任务时，各优先级子任务相应的运动状态会更新，故上述的实际运动状态将包括每个优先级子任务执行时实际的位置、速度、力等信息。
以优先级最高的物体稳定夹持控制子任务为例，其是通过控制机器人机械臂产生的对被夹持物体的夹持内力实现的稳定控制。在一种实施方式中，该物体稳定夹持控制子任务包括物体受到的实际夹持内力、以及物体在夹持内空间的实际位置、实际速度等。
其中，上述的物体受到的实际夹持内力可通过各个机械臂末端受到的外力计算得到，具体地，以两个机械臂夹持为例，如图3所示，包括：
子步骤S210，通过机器人的双臂末端各自设置的传感器分别获得第一臂和第二臂末端在各自坐标系下受到的作用力；子步骤S220，可根据第一臂的坐标系与第二臂的坐标系的坐标变换矩阵、以及第二臂坐标系下受到的作用力，计算得到转换到第一臂坐标系下的第一转换作用力；以及子步骤S230，根据第一臂坐标系下受到的作用力和该第一转换作用力，计算得到双臂产生的实际夹持内力。
若上述用表达式表示，即有：
Figure BDA0003384170520000111
其中，Wr为实际夹持内力，I为6×6的单位矩阵，/>
Figure BDA0003384170520000112
为6×6的坐标变换矩阵，用于将第二臂坐标系下的力转换到第一臂坐标系下。
而上述的物体在夹持内空间的实际位置xr可通过两个机械臂末端的位置计算得到，即满足：xr＝x2-x1∈R6；其中，x1和x2分别为第一臂末端和第二臂末端的位置。同理，上述的物体在夹持内空间的实际速度
Figure BDA0003384170520000113
可通过两个机械臂末端的速度计算得到，即满足：
Figure BDA0003384170520000114
其中，/>
Figure BDA0003384170520000115
和/>
Figure BDA0003384170520000116
分别为第一臂末端和第二臂末端的速度。
对于优先级次高的物体运动控制子任务，其用于控制物体所受到的双机械臂对它的合外力。在一种实施方式中，该物体运动控制子任务包括物体受到的实际合外力、以及在世界坐标系下的实际位置、实际速度等。
其中，上述的实际合外力可通过两个机械臂末端受到的外力计算得到，具体地，如图4所示，包括：子步骤S310，根据第一臂坐标系与世界坐标系的坐标变换矩阵、及第一臂坐标系下受到的作用力，计算得到转换到世界坐标系下的第二作用力；子步骤S320，根据第二臂坐标系与世界坐标系的坐标变换矩阵、及第二臂坐标系下受到的作用力，计算得到转换到世界坐标系下的第三作用力；子步骤S330，根据该第二作用力和第三作用力，计算得到物体受到的实际合外力。
若上述用表达式表示，即有：
Figure BDA0003384170520000117
其中，Wa为实际合外力，/>
Figure BDA0003384170520000118
分别为6×6的坐标变换矩阵，分别表示将第一臂坐标系、第一臂坐标系下的力转换到世界坐标系下。
而上述的物体在世界坐标系下的实际位置xa的计算公式为
Figure BDA0003384170520000121
Figure BDA0003384170520000122
以及，物体在世界坐标系下的实际速度/>
Figure BDA0003384170520000123
的计算公式为/>
Figure BDA0003384170520000124
Figure BDA0003384170520000125
本实施例中，每个优先级子任务设置有对应的导纳控制器，以用于分别计算对应优先级子任务在任务空间的控制量。示范性地，获得机器人的运动状态后，将根据对应优先级子任务的实际值及期望值利用各自的导纳控制器计算出对应子任务的控制量。可以理解，这里的期望值在计算控制量的过程中为已知量，可以是执行任务前根据规划预先设置或计算得到的等。
步骤S120，根据对应优先级子任务的期望值和实际值计算对应优先级子任务在任务空间的任务控制量。
对于上述的物体稳定夹持控制子任务，在一种实施方式中，其导纳控制器的表达式如下：
Figure BDA0003384170520000126
其中，
Figure BDA0003384170520000127
其中，
Figure BDA0003384170520000128
为用于稳定夹持的速度控制量，/>
Figure BDA0003384170520000129
为实际夹持内力Wr与期望夹持内力Wr,d之间的偏差，/>
Figure BDA00033841705200001210
为物体在夹持内空间的实际位置xr与期望位置xr,d之间的偏差，/>
Figure BDA00033841705200001211
为物体在夹持内空间的期望速度，Kr和Br分别为夹持内空间的刚度矩阵和阻尼矩阵。
可知，将得到的机器人机械臂产生的实际夹持内力Wr、给定的期望夹持力Wr,d、物体在夹持内空间的实际位置xr与期望位置xr,d、以及物体在夹持内空间的期望速度
Figure BDA00033841705200001212
代入上述的导纳控制器，即可计算出用于稳定夹持物体所需的控制量/>
Figure BDA00033841705200001213
对于上述的物体运动控制子任务，在一种实施方式中，其导纳控制器的表达式如下：
Figure BDA0003384170520000131
其中，
Figure BDA0003384170520000132
其中，
Figure BDA0003384170520000133
为用于控制物体运动的速度控制量，/>
Figure BDA0003384170520000134
为实际合外力Wa与期望合外力Wa,d之间的偏差，/>
Figure BDA0003384170520000135
为物体在世界坐标空间的实际位置xa与期望位置xa,d之间的偏差，/>
Figure BDA0003384170520000136
为物体在世界坐标空间的期望速度，Ka和Ba分别为物体的刚度矩阵和阻尼矩阵。
可知，将得到的物体受到的实际合外力Wa、期望合外力Wa,d、物体在世界坐标空间的实际位置xa、期望位置xa,d、以及在世界坐标空间的期望速度
Figure BDA0003384170520000137
代入上述的导纳控制器，即可计算出用于控制物体运动的控制量/>
Figure BDA0003384170520000138
对于上述的关节运动控制子任务，在一种实施方式中，其导纳控制器的表达式如下：
Figure BDA0003384170520000139
其中，
Figure BDA00033841705200001310
其中，
Figure BDA00033841705200001311
为用于控制关节空间运动的速度控制量，/>
Figure BDA00033841705200001312
为在关节空间内的实际关节位置qa和期望关节位置qa,d之间的偏差，/>
Figure BDA00033841705200001313
为期望的关节速度，KJ和BJ分别为关节空间的刚度矩阵和阻尼矩阵。
可知，将上述得到的机器人各关节受到的外力矩
Figure BDA00033841705200001314
各关节在关节空间的实际位置qa与期望位置qa,d之间的偏差、以及在关节空间的期望速度/>
Figure BDA00033841705200001315
计算得到用于控制关节空间运动的控制量/>
Figure BDA00033841705200001316
由于需要将上述各优先级子任务的控制量转为机器人所需的关节控制量，以便驱动机器人各个关节进行相应运动，从而执行上述各子任务。
步骤S130，按照优先级由高到低的顺序，依次根据对应优先级子任务的任务控制量进行逆运动学优化求解，得到所需的目标关节控制量。
本实施例中，将使用优先级分层的QP(Quadratic Programming)优化进行机器人的逆运动学求解，具体地，即按照优先级由高到低的顺序，依次将对应优先级子任务的任务控制量作为本层输入，并将关节的物理约束以及在先计算得到的各优先级的最优解(即求解到的关节控制量)，作为本层的约束条件进行优化，以得到本层的关节控制量。如此重复本步骤，直至计算出最后一层(即优先级最低的子任务)的优化结果，将其作为所需的目标关节控制指令，发送给机器人各个关节。基于分优先级的优化求解方法能够严格的保证高优先级任务的执行效果，尤其是在一些极端条件下，所有的子任务可能无法全都执行，此时的执行效果尤为凸显。
示范性地，基于机器人逆运动学，每个优先级子任务预先构建有对应的关节控制指令优化函数，以用于求解对应子任务的最优关节控制量。其中，每个优先级子任务的关节控制指令优化函数均以关节速度为优化变量且约束条件均包括机器人满足的关节物理约束。例如，这里将采用在约束条件范围内寻找使目标的二范数最小的函数来构建各关节控制指令优化函数。可以理解，上述的目标将根据不同的优先级子任务而不同。并且，上述的优化函数除了采用使目标的二范数最小之外，也可以构建其他的函数，如使目标的平方最小等，这里不作限定。
下面基于双臂夹持任务以上述的三个优先级子任务的关节控制量的计算过程进行说明。
对于物体稳定夹持控制子任务，基于夹持内空间下的机械臂末端的速度
Figure BDA0003384170520000151
与关节速度/>
Figure BDA0003384170520000152
之间的关系满足：/>
Figure BDA0003384170520000153
其中，E＝-J1+J2，J1和J2分别表示机器人的双臂末端的速度雅克比矩阵；于是，在一种实施方式中，其关节控制指令优化函数如下：
Figure BDA0003384170520000154
Figure BDA0003384170520000155
其中，
Figure BDA0003384170520000156
表示物体稳定夹持控制子任务的最优关节控制量；argmin‖‖2表示在约束条件范围内寻找使目标/>
Figure BDA0003384170520000157
的二范数最小的函数；/>
Figure BDA0003384170520000158
表示关节速度；/>
Figure BDA0003384170520000159
表示物体稳定夹持控制子任务的任务控制量；bm和bM分别表示机器人满足的关节物理约束的上边界和下边界。
进一步说明，bm和bM可包括根据关节限位及速度约束等计算到的约束边界，具体地，
Figure BDA00033841705200001510
Figure BDA00033841705200001511
为关节的速度下限，qmin为关节的位置下边界，dt为控制器的采样时间，/>
Figure BDA00033841705200001512
Figure BDA00033841705200001513
Figure BDA00033841705200001514
为关节的速度上限，qmax为关节的位置上边界。
进而，在求解出优先级最高的最优关节控制量后，再将其作为次高优先级子任务的关节控制指令优化函数的约束条件，使得优先级次之的计算结果不会影响上一层的稳定夹持任务的执行。
对于物体运动控制子任务，同样基于世界坐标空间下的机械臂末端的速度
Figure BDA00033841705200001515
与关节速度/>
Figure BDA00033841705200001516
之间的关系满足：/>
Figure BDA00033841705200001517
其中，/>
Figure BDA00033841705200001518
于是，在一种实施方式中，其关节控制指令优化函数如下：
Figure BDA00033841705200001519
Figure BDA00033841705200001520
Figure BDA0003384170520000161
其中，
Figure BDA0003384170520000162
和/>
Figure BDA0003384170520000163
分别表示物体运动控制子任务的最优关节控制量和任务控制量。
最后，在求解出次高优先级子任务的最优解后，将前两层求解到的最优解作为优先级最低的约束条件，以得到第三个子任务的关节控制量，使得优先级最低的计算结果不会影响上两层任务的执行。
对于关节运动控制子任务，在一种实施方式中，其关节控制指令优化函数如下：
Figure BDA0003384170520000164
Figure BDA0003384170520000165
Figure BDA0003384170520000166
Figure BDA0003384170520000167
其中，
Figure BDA0003384170520000168
和/>
Figure BDA0003384170520000169
分别表示关节运动控制子任务的最优关节控制量和任务控制量。
步骤S140，根据所述目标关节控制量进行夹持控制。
示范性地，将上述求解得到的优先级最低的子任务的关节控制量
Figure BDA00033841705200001610
作为机械臂最终所需的关节控制量/>
Figure BDA00033841705200001611
即令/>
Figure BDA00033841705200001612
而后通过数值积分可得到所需的关节位置指令以发送给相应机械臂的关节电机，使得机器人能够同时实现上述三个子任务。
上述的机械臂夹持控制方法可应用于各种夹持物体的复杂场景，如搬箱子、端盘子等，尤其是可能会受到来自周围环境或人的扰动的情况。以箱子为例，该方法能够在满足机器人自身性能的同时实现箱子的稳定夹持、箱子的运动控制以及机械臂关节空间的控制，并且能够柔顺响应来自环境或人的外扰动。进一步地，当遇到极端情况下，如有人碰到箱子等，该方法能够优先控制箱子的稳定夹持，保证箱子不滑落，并在稳定后再控制箱子执行期望的运动轨迹。因此，该方法能够大大提高机器人在实际环境中的可应用性。又例如，一个双臂机器人端着餐盘，要从左侧放到右侧桌子上，在移动餐盘过程中，当有人碰到机械臂时，机器人能够顺应碰撞力改变胳膊的姿态来减缓碰撞，此时餐盘依旧能够按照期望的轨迹移动，当有人拖动餐盘时，机器人会先夹紧餐盘防止餐盘掉落并顺着拖动方向运动，在松开餐盘后，双臂机器人将重新控制餐盘按照原先的期望轨迹运动。应当理解，本实施例的机械臂夹持控制方法不仅可以适用于双臂机器人，同样适用于具有三个及三个以上机械臂的机器人，例如，在计算物体受到的实际夹持内力及合外力时，可将其他各机械臂各自受到的作用力均转换到同一机械臂坐标系下来计算等，原理同双臂夹持控制的类似，这里不再展开描述。
本实施例的机械臂夹持控制方法通过将机器人执行夹持物体的任务进行拆解，如分成上述的三个子任务，即物体稳定夹持子任务、物体运动控制子任务和关节运动控制子任务，并依照夹持过程中这些子任务的重要程度设置子任务的优先级，且设置不同导纳控制器进行任务控制量计算；与此同时，采用多优先级分层优化的方法求解机器人的逆运动学来得到最终的关节控制指令，这种多优先级的迭代求解能够实现低优先级的任务求解不影响高优先级的求解结果，从而保证了严格的分层执行效果，且每一层的优化求解过程中都考虑了关节的位置、速度等物理约束，使得计算的控制量不会超关节的实际能够执行的范围等。
实施例2
请参照图5，基于上述实施例1的方法，本实施例提出一种机械臂夹持控制装置100，示范性地，该机械臂夹持控制装置100包括：
状态获取模块110，用于获取机器人执行夹持任务时的实际运动状态，所述实际运动状态包括各个优先级子任务相应的实际值；
计算模块120，用于根据对应优先级子任务的期望值和所述实际值计算对应优先级子任务在任务空间的任务控制量；
优化求解模块130，用于按照优先级由高到低的顺序，依次根据对应优先级子任务的所述任务控制量进行逆运动学优化求解，得到所需的目标关节控制量；其中，在先得到的各个优先级子任务的关节控制量用于作为求解下一优先级子任务的关节控制量的约束条件，将最低优先级子任务的关节控制量作为所述目标关节控制量；
关节控制模块140，用于根据所述目标关节控制量进行夹持控制。
可以理解，本实施例的装置对应于上述实施例1的方法，上述实施例1中的可选项同样适用于本实施例，故在此不再重复描述。
本申请还提供了一种机器人，例如，可以是双臂机器人，也可以是三机械臂的机器人等。示范性地，该机器人包括至少两条机械臂、处理器和存储器，其中，机械臂用于夹持物体，存储器存储有计算机程序，处理器通过运行所述计算机程序，从而使机器人执行上述的机械臂夹持控制方法或者上述机械臂夹持控制装置中的各个模块的功能。
本申请还提供了一种可读存储介质，用于储存上述机器人中使用的所述计算机程序。
在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和结构图显示了根据本申请的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，结构图和/或流程图中的每个方框、以及结构图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
另外，在本申请各个实施例中的各功能模块或单元可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或更多个模块集成形成一个独立的部分。
所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是智能手机、个人计算机、服务器、或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述，仅为本申请的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。

Claims (12)

1.一种机械臂夹持控制方法，其特征在于，包括：
获取机器人执行夹持任务时的实际运动状态，所述实际运动状态包括多个优先级子任务的实际值；所述多个优先级子任务包括三个，分别是优先级由高到低的物体稳定夹持控制子任务、物体运动控制子任务以及所述机器人的关节运动控制子任务；
根据对应优先级子任务的期望值和所述实际值计算对应优先级子任务在任务空间的任务控制量；
按照优先级由高到低的顺序，依次根据对应优先级子任务的所述任务控制量进行逆运动学优化求解，得到所需的目标关节控制量；其中，在先得到的各个优先级子任务的关节控制量用于作为求解下一优先级子任务的关节控制量的约束条件，将最低优先级子任务的关节控制量作为所述目标关节控制量；
根据所述目标关节控制量对被夹物体进行夹持控制；
其中，所述物体稳定夹持控制子任务的任务控制量的获取，包括：
根据所述机器人的机械臂产生的实际夹持内力与期望夹持力之间的偏差、物体在夹持内空间的实际位置与期望位置之间的偏差、以及在夹持内空间的期望速度，计算得到用于稳定夹持的速度控制量。
2.根据权利要求1所述的机械臂夹持控制方法，其特征在于，所述物体运动控制子任务的任务控制量的获取，包括：
根据物体受到的实际合外力与期望合外力之间的偏差、物体在世界坐标空间的实际位置与期望位置之间的偏差、以及在世界坐标空间的期望速度，计算得到用于控制物体运动的速度控制量。
3.根据权利要求1所述的机械臂夹持控制方法，其特征在于，所述机器人的关节运动控制子任务的任务控制量的获取，包括：
根据所述机器人各关节受到的外力矩、各关节在关节空间的实际位置与期望位置之间的偏差、以及在关节空间的期望速度，计算得到用于控制关节空间运动的速度控制量。
4.根据权利要求1所述的机械臂夹持控制方法，其特征在于，所述机器人的机械臂产生的实际夹持内力的计算，包括：
通过所述机器人的双臂末端各自设置的传感器分别获得第一臂和第二臂末端在各自坐标系下受到的作用力；
根据第一臂坐标系与第二臂坐标系的坐标变换矩阵、及所述第二臂坐标系下的所述作用力，计算得到转换到所述第一臂坐标系下的第一转换作用力；
根据所述第一臂坐标系下的所述作用力和所述第一转换作用力，计算得到双臂产生的实际夹持内力。
5.根据权利要求4所述的机械臂夹持控制方法，其特征在于，所述物体受到的实际合外力的获取，包括：
根据所述第一臂坐标系与世界坐标系的坐标变换矩阵、及所述第一臂坐标系下的作用力，计算得到转换到世界坐标系下的第二作用力；
根据所述第二臂坐标系与世界坐标系的坐标变换矩阵、及所述第二臂坐标系下的作用力，计算得到转换到世界坐标系下的第三作用力；
根据所述第二作用力和所述第三作用力，计算得到所述物体受到的实际合外力。
6.根据权利要求1所述的机械臂夹持控制方法，其特征在于，基于机器人逆运动学，每个优先级子任务预先构建有对应的关节控制指令优化函数，以用于求解对应子任务的最优关节控制量；
其中，每个优先级子任务的所述关节控制指令优化函数均以关节速度为优化变量且约束条件包括所述机器人满足的关节物理约束。
7.根据权利要求6所述的机械臂夹持控制方法，其特征在于，所述多个优先级子任务包括优先级最高的物体稳定夹持控制子任务，所述物体稳定夹持控制子任务的关节控制指令优化函数如下：
Figure FDA0004238267350000031
Figure FDA0004238267350000032
其中，E＝-J1+J2；
其中，
Figure FDA0004238267350000033
表示所述物体稳定夹持控制子任务的最优关节控制量；argmin‖ ‖2表示在约束条件范围内寻找使目标的二范数最小的函数；bm和bM分别表示所述机器人满足的关节物理约束的上边界和下边界；/>
Figure FDA0004238267350000034
表示关节速度；/>
Figure FDA0004238267350000035
表示所述物体稳定夹持控制子任务的任务控制量；J1和J2分别表示所述机器人的双臂末端的速度雅克比矩阵。
8.根据权利要求7所述的机械臂夹持控制方法，其特征在于，所述多个优先级子任务包括优先级次高的物体运动控制子任务，所述物体运动控制子任务的关节控制指令优化函数如下：
Figure FDA0004238267350000036
Figure FDA0004238267350000037
Figure FDA0004238267350000041
其中，
Figure FDA0004238267350000042
其中，
Figure FDA0004238267350000043
和/>
Figure FDA0004238267350000044
分别表示所述物体运动控制子任务的最优关节控制量和任务控制量。
9.根据权利要求8所述的机械臂夹持控制方法，其特征在于，所述多个优先级子任务包括优先级最低的关节运动控制子任务，所述关节运动控制子任务的关节控制指令优化函数如下：
Figure FDA0004238267350000045
Figure FDA0004238267350000046
Figure FDA0004238267350000047
Figure FDA0004238267350000048
其中，
Figure FDA0004238267350000049
和/>
Figure FDA00042382673500000410
分别表示所述关节运动控制子任务的最优关节控制量和任务控制量。
10.一种机械臂夹持控制装置，其特征在于，包括：
状态获取模块，用于获取机器人执行夹持任务时的实际运动状态，所述实际运动状态包括各个优先级子任务相应的实际值；所述多个优先级子任务包括三个，分别是优先级由高到低的物体稳定夹持控制子任务、物体运动控制子任务以及所述机器人的关节运动控制子任务；
计算模块，用于根据对应优先级子任务的期望值和所述实际值计算对应优先级子任务在任务空间的任务控制量；
优化求解模块，用于按照优先级由高到低的顺序，依次根据对应优先级子任务的所述任务控制量进行逆运动学优化求解，得到所需的目标关节控制量；其中，在先得到的各个优先级子任务的关节控制量用于作为求解下一优先级子任务的关节控制量的约束条件，将最低优先级子任务的关节控制量作为所述目标关节控制量；
关节控制模块，用于根据所述目标关节控制量对被夹物体进行夹持控制；
其中，所述物体稳定夹持控制子任务的任务控制量的获取，包括：
根据所述机器人的机械臂产生的实际夹持内力与期望夹持力之间的偏差、物体在夹持内空间的实际位置与期望位置之间的偏差、以及在夹持内空间的期望速度，计算得到用于稳定夹持的速度控制量。
11.一种机器人，其特征在于，所述机器人包括至少两条机械臂、处理器和存储器，所述机械臂用于夹持物体，所述存储器存储有计算机程序，所述处理器用于执行所述计算机程序以实施权利要求1-9中任一项所述的机械臂夹持控制方法。
12.一种可读存储介质，其特征在于，其存储有计算机程序，所述计算机程序在处理器上执行时，实施根据权利要求1-9中任一项所述的机械臂夹持控制方法。
CN202111443726.9A 2021-11-30 2021-11-30 机械臂夹持控制方法、装置、机器人和可读存储介质 Active CN114083537B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111443726.9A CN114083537B (zh) 2021-11-30 2021-11-30 机械臂夹持控制方法、装置、机器人和可读存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111443726.9A CN114083537B (zh) 2021-11-30 2021-11-30 机械臂夹持控制方法、装置、机器人和可读存储介质

Publications (2)

Publication Number Publication Date
CN114083537A CN114083537A (zh) 2022-02-25
CN114083537B true CN114083537B (zh) 2023-07-07

Family

ID=80305864

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111443726.9A Active CN114083537B (zh) 2021-11-30 2021-11-30 机械臂夹持控制方法、装置、机器人和可读存储介质

Country Status (1)

Country Link
CN (1) CN114083537B (zh)

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106826814B (zh) * 2016-12-16 2019-12-10 深圳市优必选科技有限公司 一种机器人的运动控制方法和运动控制系统
CN109079780B (zh) * 2018-08-08 2020-11-10 北京理工大学 基于广义坐标的分布式移动机械臂任务分层优化控制方法
CN110104216B (zh) * 2019-01-28 2022-09-06 西北工业大学深圳研究院 一种针对运动学冗余双臂空间机器人的协同路径规划方法
CN112975938B (zh) * 2019-12-12 2022-04-05 中国科学院沈阳自动化研究所 一种基于零空间的机械臂速度层轨迹规划方法
EP3871841A1 (en) * 2020-02-28 2021-09-01 KUKA Deutschland GmbH Robot control
CN112068576B (zh) * 2020-07-21 2022-07-12 华中科技大学 一种基于任务分级时序优化的水下无人艇-双机械臂协同控制方法
CN112698574B (zh) * 2020-12-29 2022-05-13 南京理工大学 一种基于混合任务优先级的双臂空间机器人协调控制方法

Also Published As

Publication number Publication date
CN114083537A (zh) 2022-02-25

Similar Documents

Publication Publication Date Title
CN111538949B (zh) 冗余机器人逆运动学求解方法、装置和冗余机器人
El-Ghazaly et al. Adaptive terminal sliding mode control of a redundantly-actuated cable-driven parallel manipulator: CoGiRo
WO2020056299A1 (en) Deep reinforcement learning-based techniques for end to end robot navigation
CN109591014B (zh) 一种双臂协作机器人的双臂协调搬运方法
US9981381B1 (en) Real time generation of phase synchronized trajectories
US20100280659A1 (en) Tension distribution in a tendon-driven robotic finger
US20060195228A1 (en) Robot locus control method and apparatus and program of robot locus control method
CN107351081B (zh) 具有速度优化特性的冗余度机械臂突加度层运动规划方法
EP3568268A2 (en) Improvements related to generating a robot control policy from demonstrations collected via kinesthetic teaching of a robot
Bohez et al. Sensor fusion for robot control through deep reinforcement learning
WO2020093253A1 (zh) 机器人的运动控制方法、控制系统和存储装置
JP2011183527A (ja) 冗長ロボットの関節目標値決定方法及び冗長ロボットの制御装置
CN114274147B (zh) 目标跟踪控制方法及装置、机械臂控制设备和存储介质
Garofalo et al. Task-space tracking control for underactuated aerial manipulators
Elshabasy et al. Power optimization of planar redundant manipulator moving along constrained-end trajectory using hybrid techniques
CN114083537B (zh) 机械臂夹持控制方法、装置、机器人和可读存储介质
CN109491381B (zh) 基于观测器的多移动机器人自适应编队跟踪控制方法
Aranda et al. Deformation-based shape control with a multirobot system
Flacco et al. Unilateral constraints in the reverse priority redundancy resolution method
Sandoval et al. Generalized framework for control of redundant manipulators in robot-assisted minimally invasive surgery
Hong et al. Regularized hierarchical quadratic program for real-time whole-body motion generation
CN114083538B (zh) 双机械臂稳定夹持实现方法、装置和机器人
Tufail et al. Impedance control schemes for bilateral teleoperation
Abro et al. Review of hybrid control designs for underactuated quadrotor with unmodelled dynamic factors
JP2019084665A (ja) ロボット制御装置、ロボット、ロボットシステム、及びプログラム

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