CN113070880B - 运动控制方法、装置、机器人控制设备及可读存储介质 - Google Patents

运动控制方法、装置、机器人控制设备及可读存储介质 Download PDF

Info

Publication number
CN113070880B
CN113070880B CN202110349082.0A CN202110349082A CN113070880B CN 113070880 B CN113070880 B CN 113070880B CN 202110349082 A CN202110349082 A CN 202110349082A CN 113070880 B CN113070880 B CN 113070880B
Authority
CN
China
Prior art keywords
matrix
vector
generalized coordinate
humanoid robot
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
CN202110349082.0A
Other languages
English (en)
Other versions
CN113070880A (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 CN202110349082.0A priority Critical patent/CN113070880B/zh
Publication of CN113070880A publication Critical patent/CN113070880A/zh
Priority to PCT/CN2021/131893 priority patent/WO2022205941A1/zh
Priority to US17/702,819 priority patent/US11938635B2/en
Application granted granted Critical
Publication of CN113070880B publication Critical patent/CN113070880B/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/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
    • 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
    • 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
    • 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
    • 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/40—Robotics, robotics mapping to robotics vision
    • G05B2219/40264—Human like, type robot arm

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Artificial Intelligence (AREA)
  • Evolutionary Computation (AREA)
  • Fuzzy Systems (AREA)
  • Software Systems (AREA)
  • Manipulator (AREA)

Abstract

本申请提供一种运动控制方法、装置、机器人控制设备及可读存储介质，涉及机器人控制技术领域。本申请通过计算仿人机器人在当前时刻的全身广义坐标向量相对于实际任务空间向量的逆向雅可比矩阵，并根据与全身广义坐标向量对应的预设位置限制条件，将实际任务空间向量及当前时刻的待执行任务空间向量结合到逆向雅可比矩阵的零空间中，计算与该待执行任务空间向量对应的目标广义坐标向量，接着按照目标广义坐标向量对仿人机器人的运动状态进行控制，以从零空间的角度对仿人机器人运动进行整体优化，达到控制仿人机器人避开关节运动极限的目的，从而在维持机器人腿部平衡的基础上扩增手臂操作空间，提升仿人机器人的操作性能。

Description

运动控制方法、装置、机器人控制设备及可读存储介质
技术领域
本申请涉及机器人控制技术领域，具体而言，涉及一种运动控制方法、装置、机器人控制设备及可读存储介质。
背景技术
随着科学技术的不断发展，机器人技术因具有极大的研究价值及应用价值受到了各行各业的广泛重视，其中仿人机器人控制便是机器人控制技术领域中的一项重要研究方向。而在仿人机器人的实际控制过程中，若仅考虑机器人手臂运动的控制操作，将无法确保机器人手臂能够具有足够的关节操作空间，同时也极易在手臂大范围运动时出现腿部无法保持平衡支撑的现象，从而导致仿人机器人的整体操作性能较差。
发明内容
有鉴于此，本申请的目的在于提供一种运动控制方法、装置、机器人控制设备及可读存储介质，能够从整体上对仿人机器人的运动进行优化以避开关节运动极限，在维持机器人腿部平衡的基础上扩增手臂操作空间，从而进一步提升仿人机器人的操作性能。
为了实现上述目的，本申请实施例采用的技术方案如下：
第一方面，本申请提供一种运动控制方法，所述方法包括：
获取仿人机器人在当前时刻的全身广义坐标向量、实际任务空间向量以及待执行任务空间向量，其中所述全身广义坐标向量由所述仿人机器人的腰部姿态矩阵及机械臂姿态矩阵组成；
根据所述腰部姿态矩阵及所述机械臂姿态矩阵，计算所述全身广义坐标向量相对于所述实际任务空间向量的逆向雅可比矩阵；
根据与所述全身广义坐标向量对应的预设位置限制条件，将所述实际任务空间向量及所述待执行任务空间向量结合到所述逆向雅可比矩阵的零空间中，计算与所述待执行任务空间向量对应的目标广义坐标向量；
按照所述目标广义坐标向量对所述仿人机器人的运动状态进行控制。
在可选的实施方式中，所述根据所述腰部姿态矩阵及所述机械臂姿态矩阵，计算所述全身广义坐标向量相对于所述实际任务空间向量的逆向雅可比矩阵的步骤，包括：
根据所述腰部姿态矩阵计算所述仿人机器人在当前时刻的腰部相对于足部的第一运动速度矩阵，其中所述第一运动速度矩阵包括对应腰部相对于足部的线速度及角速度；
根据所述机械臂姿态矩阵确定所述仿人机器人在当前时刻的机械臂末端相对于机械臂基座的位姿表达矩阵；
获取所述仿人机器人在当前时刻的机械臂基座相对于足部的旋转矩阵；
根据所述旋转矩阵以及所述位姿表达矩阵计算所述仿人机器人在当前时刻的机械臂末端相对于足部的第二运动速度矩阵，其中所述第二运动速度矩阵包括对应机械臂末端相对于足部的线速度及角速度；
将所述第一运动速度矩阵与所述第二运动速度矩阵进行速度叠加处理，得到对应的待处理速度矩阵；
根据所述待处理速度矩阵及所述全身广义坐标向量计算所述全身广义坐标向量相对于所述实际任务空间向量的正向雅可比矩阵；
对计算出的所述正向雅可比矩阵进行奇异值分解，得到对应的逆向雅可比矩阵。
在可选的实施方式中，所述待处理速度矩阵、所述全身广义坐标向量与所述正向雅可比矩阵之间的关联关系采用如下式子表达为：
Figure BDA0003001835880000021
其中，A用于表示所述待处理速度矩阵，ζ用于表示所述全身广义坐标向量，
Figure BDA0003001835880000022
用于表示所述全身广义坐标向量关于时间的一阶导数，J(ζ)用于表示所述正向雅可比矩阵。
在可选的实施方式中，所述根据与所述全身广义坐标向量对应的预设位置限制条件，将所述实际任务空间向量及所述待执行任务空间向量结合到所述逆向雅可比矩阵的零空间中，计算与所述待执行任务空间向量对应的目标广义坐标向量的步骤，包括：
计算所述待执行任务空间向量与所述实际任务空间向量之间的任务向量差异矩阵；
计算所述逆向雅可比矩阵在零空间中对应的零空间矩阵；
将所述全身广义坐标向量中的每项向量元素与所述预设位置限制条件中对应向量元素的预设数值上限及预设数值下限进行比较，并根据得到的所有比较结果针对所述全身广义坐标向量生成匹配的限位矫正矩阵；
根据所述逆向雅可比矩阵、所述零空间矩阵、所述任务向量差异矩阵以及所述限位矫正矩阵，计算所述仿人机器人针对所述待执行任务空间向量的广义向量补偿矩阵；
按照所述广义向量补偿矩阵对所述全身广义坐标向量进行向量补偿，得到所述目标广义坐标向量。
在可选的实施方式中，所述广义向量补偿矩阵、所述逆向雅可比矩阵、所述零空间矩阵、所述任务向量差异矩阵以及所述限位矫正矩阵之间的关联关系采用如下式子表达为：
Figure BDA0003001835880000031
其中，Δζ用于表示所述广义向量补偿矩阵，
Figure BDA0003001835880000032
用于表示所述逆向雅可比矩阵，Z(ζ)用于表示所述零空间矩阵，Δσ用于表示所述任务向量差异矩阵，ΔS用于表示所述限位矫正矩阵。
第二方面，本申请提供一种运动控制装置，所述装置包括：
运动信息获取模块，用于获取仿人机器人在当前时刻的全身广义坐标向量、实际任务空间向量以及待执行任务空间向量，其中所述全身广义坐标向量由所述仿人机器人的腰部姿态矩阵及机械臂姿态矩阵组成；
逆向矩阵计算模块，用于根据所述腰部姿态矩阵及所述机械臂姿态矩阵，计算所述全身广义坐标向量相对于所述实际任务空间向量的逆向雅可比矩阵；
广义向量计算模块，用于根据与所述全身广义坐标向量对应的预设位置限制条件，将所述实际任务空间向量及所述待执行任务空间向量结合到所述逆向雅可比矩阵的零空间中，计算与所述待执行任务空间向量对应的目标广义坐标向量；
运动状态控制模块，用于按照所述目标广义坐标向量对所述仿人机器人的运动状态进行控制。
在可选的实施方式中，所述逆向矩阵计算模块包括：
速度矩阵计算子模块，用于根据所述腰部姿态矩阵计算所述仿人机器人在当前时刻的腰部相对于足部的第一运动速度矩阵，其中所述第一运动速度矩阵包括对应腰部相对于足部的线速度及角速度；
位姿矩阵确定子模块，用于根据所述机械臂姿态矩阵确定所述仿人机器人在当前时刻的机械臂末端相对于机械臂基座的位姿表达矩阵；
旋转矩阵获取子模块，用于获取所述仿人机器人在当前时刻的机械臂基座相对于足部的旋转矩阵；
所述速度矩阵计算子模块，还用于根据所述旋转矩阵以及所述位姿表达矩阵计算所述仿人机器人在当前时刻的机械臂末端相对于足部的第二运动速度矩阵，其中所述第二运动速度矩阵包括对应机械臂末端相对于足部的线速度及角速度；
所述速度矩阵计算子模块，还用于将所述第一运动速度矩阵与所述第二运动速度矩阵进行速度叠加处理，得到对应的待处理速度矩阵；
正向矩阵计算子模块，用于根据所述待处理速度矩阵及所述全身广义坐标向量计算所述全身广义坐标向量相对于所述实际任务空间向量的正向雅可比矩阵；
逆向矩阵求解子模块，用于对计算出的所述正向雅可比矩阵进行奇异值分解，得到对应的逆向雅可比矩阵。
在可选的实施方式中，所述待处理速度矩阵、所述全身广义坐标向量与所述正向雅可比矩阵之间的关联关系采用如下式子表达为：
Figure BDA0003001835880000051
其中，A用于表示所述待处理速度矩阵，ζ用于表示所述全身广义坐标向量，
Figure BDA0003001835880000052
用于表示所述全身广义坐标向量关于时间的一阶导数，J(ζ)用于表示所述正向雅可比矩阵。
在可选的实施方式中，所述广义向量计算模块包括：
任务差异计算子模块，用于计算所述待执行任务空间向量与所述实际任务空间向量之间的任务向量差异矩阵；
空间矩阵计算子模块，用于计算所述逆向雅可比矩阵在零空间中对应的零空间矩阵；
限位矫正确定子模块，用于将所述全身广义坐标向量中的每项向量元素与所述预设位置限制条件中对应向量元素的预设数值上限及预设数值下限进行比较，并根据得到的所有比较结果针对所述全身广义坐标向量生成匹配的限位矫正矩阵；
向量补偿计算子模块，用于根据所述逆向雅可比矩阵、所述零空间矩阵、所述任务向量差异矩阵以及所述限位矫正矩阵，计算所述仿人机器人针对所述待执行任务空间向量的广义向量补偿矩阵；
广义向量补偿子模块，用于按照所述广义向量补偿矩阵对所述全身广义坐标向量进行向量补偿，得到所述目标广义坐标向量。
在可选的实施方式中，所述广义向量补偿矩阵、所述逆向雅可比矩阵、所述零空间矩阵、所述任务向量差异矩阵以及所述限位矫正矩阵之间的关联关系采用如下式子表达为：
Figure BDA0003001835880000053
其中，Δζ用于表示所述广义向量补偿矩阵，
Figure BDA0003001835880000061
用于表示所述逆向雅可比矩阵，Z(ζ)用于表示所述零空间矩阵，Δσ用于表示所述任务向量差异矩阵，ΔS用于表示所述限位矫正矩阵。
第三方面，本申请提供一种机器人控制设备，包括处理器和存储器，所述存储器存储有能够被所述处理器执行的计算机程序，所述处理器可执行所述计算机程序，以实现前述实施方式中任意一项所述的运动控制方法。
第四方面，本申请提供一种可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时，实现前述实施方式中任意一项所述的运动控制方法。
本申请实施例的有益效果包括以下内容：
本申请在获取到仿人机器人在当前时刻的全身广义坐标向量、实际任务空间向量以及待执行任务空间向量后，会相应地根据全身广义坐标向量中的腰部姿态矩阵及机械臂姿态矩阵，计算全身广义坐标向量相对于实际任务空间向量的逆向雅可比矩阵，并根据与全身广义坐标向量对应的预设位置限制条件，将实际任务空间向量及待执行任务空间向量结合到逆向雅可比矩阵的零空间中，计算与该待执行任务空间向量对应的目标广义坐标向量，接着按照目标广义坐标向量对仿人机器人的运动状态进行控制，以从零空间的角度采用预设关节运动极限条件对仿人机器人运动进行整体优化，达到控制仿人机器人避开关节运动极限的目的，从而在维持机器人腿部平衡的基础上扩增手臂操作空间，提升仿人机器人的操作性能。
为使本申请的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。
附图说明
为了更清楚地说明本申请实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本申请的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。
图1为本申请实施例提供的机器人控制设备的组成示意图；
图2为本申请实施例提供的运动控制方法的流程示意图；
图3为图2中的步骤S220包括的子步骤的流程示意图；
图4为图2中的步骤S230包括的子步骤的流程示意图；
图5为本申请实施例提供的运动控制装置的组成示意图；
图6为图5中的逆向矩阵计算模块的组成示意图；
图7为图5中的广义向量计算模块的组成示意图。
图标：10-机器人控制设备；11-存储器；12-处理器；13-通信单元；100-运动控制装置；110-运动信息获取模块；120-逆向矩阵计算模块；130-广义向量计算模块；140-运动状态控制模块；121-速度矩阵计算子模块；122-位姿矩阵确定子模块；123-旋转矩阵获取子模块；124-正向矩阵计算子模块；125-逆向矩阵求解子模块；131-任务差异计算子模块；132-空间矩阵计算子模块；133-限位矫正确定子模块；134-向量补偿计算子模块；135-广义向量补偿子模块。
具体实施方式
为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。
因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。
在本申请的描述中，需要理解的是，术语“中心”、“上”、“下”、“左”、“右”、“竖直”、“水平”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，或者是该申请产品使用时惯常摆放的方位或位置关系，或者是本领域技术人员惯常理解的方位或位置关系，仅是为了便于描述本申请和简化描述，而不是指示或暗示所指的设备或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本申请的限制。
同时，术语“第一”和“第二”等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本申请中的具体含义。
下面结合附图，对本申请的一些实施方式作详细说明。在不冲突的情况下，下述的实施例及实施例中的特征可以相互结合。
请参照图1，图1是本申请实施例提供的机器人控制设备10的组成示意图。在本申请实施例中，所述机器人控制设备10用于对仿人机器人的运动状况进行调控，所述机器人控制设备10可以与仿人机器人远程通信连接，也可以与所述仿人机器人集成在一起，以在控制仿人机器人运动时，确保该仿人机器人能够避开自身关节运动极限，并在维持机器人腿部平衡的基础上扩增手臂操作空间，提升仿人机器人的整体操作性能。
在本实施例中，所述机器人控制设备10可以包括存储器11、处理器12、通信单元13及运动控制装置100。其中，所述存储器11、所述处理器12及所述通信单元13各个元件相互之间直接或间接地电性连接，以实现数据的传输或交互。例如，所述存储器11、所述处理器12及所述通信单元13这些元件相互之间可通过一条或多条通讯总线或信号线实现电性连接。
在本实施例中，所述存储器11可以是，但不限于，随机存取存储器(Random AccessMemory，RAM)，只读存储器(Read Only Memory，ROM)，可编程只读存储器(ProgrammableRead-Only Memory，PROM)，可擦除只读存储器(Erasable Programmable Read-OnlyMemory，EPROM)，电可擦除只读存储器(Electric Erasable Programmable Read-OnlyMemory，EEPROM)等。其中，所述存储器11用于存储计算机程序，所述处理器12在接收到执行指令后，可相应地执行所述计算机程序。
其中，所述存储器11还用于存储针对所述仿人机器人的预设位置限制条件，所述预设位置限制条件用于表示所述仿人机器人各关节在运动过程中的极限运动范围，所述极限运动范围包括对应关节在具体运动尺度方面(例如，关节转动角度、关节分布位置等)的预设数值上限及预设数值下限。所述机器人控制设备10可利用所述预设位置限制条件对仿人机器人的关节运动极限进行具体限定。
在本实施例中，所述处理器12可以是一种具有信号的处理能力的集成电路芯片。所述处理器12可以是通用处理器，包括中央处理器(Central Processing Unit，CPU)、图形处理器(Graphics Processing Unit，GPU)及网络处理器(Network Processor，NP)、数字信号处理器(DSP)、专用集成电路(ASIC)、现成可编程门阵列(FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件中的至少一种。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，可以实现或者执行本申请实施例中的公开的各方法、步骤及逻辑框图。
在本实施例中，所述通信单元13用于通过网络建立所述机器人控制设备10与其他电子设备之间的通信连接，并通过所述网络收发数据，其中所述网络包括有线通信网络及无线通信网络。例如，所述机器人控制设备10可以通过所述通信单元13从作业规划设备处获取针对所述仿人机器人的与待执行任务对应的期望运动轨迹信息，并通过所述通信单元13向所述仿人机器人发送运动控制指令，使所述仿人机器人按照所述运动控制指令进行运动以对应完成所述待执行任务。
在本实施例中，所述运动控制装置100包括至少一个能够以软件或固件的形式存储于所述存储器11中或者在所述机器人控制设备10的操作系统中的软件功能模块。所述处理器12可用于执行所述存储器11存储的可执行模块，例如所述运动控制装置100所包括的软件功能模块及计算机程序等。所述机器人控制设备10可通过所述运动控制装置100从整体上对仿人机器人的运动进行优化以避开关节运动极限，并在维持机器人腿部平衡的基础上扩增手臂操作空间，从而提升仿人机器人的操作性能。
可以理解的是，图1所示的框图仅为所述机器人控制设备10的一种组成示意图，所述机器人控制设备10还可包括比图1中所示更多或者更少的组件，或者具有与图1所示不同的配置。图1中所示的各组件可以采用硬件、软件或其组合实现。
在本申请中，为确保所述机器人控制设备10能够从整体上对仿人机器人的运动进行优化以避开关节运动极限，并在维持机器人腿部平衡的基础上扩增手臂操作空间，提升仿人机器人的操作性能，本申请实施例提供一种针对仿人机器人的运动控制方法实现前述目的。下面对本申请提供的运动控制方法进行详细描述。
请参照图2，图2是本申请实施例提供的运动控制方法的流程示意图。在本申请实施例中，所述运动控制方法可以包括步骤S210～步骤S240。
步骤S210，获取仿人机器人在当前时刻的全身广义坐标向量、实际任务空间向量以及待执行任务空间向量。
在本实施例中，所述仿人机器人包括机械臂、机械足部、机械腿以及腰部结构，可将仿人机器人的与腰部结构相连接的两条机械腿视为一个浮动基座，在仿人机器人的作为支撑足的那个机械足部处构建一个固定坐标系。接着，可在该固定坐标系为基础上确定出该仿人机器人腰部在该固定坐标系下的腰部姿态表达式，即腰部姿态矩阵η＝[x y z rx ryrz]，其中x、y及z分别为仿人机器人腰部在固定坐标系下的三维坐标轴(X轴、Y轴及Z轴)上各自对应的坐标值，rx、ry及rz分别为仿人机器人腰部相对于固定坐标系下的三维坐标轴(X轴、Y轴及Z轴)的偏转角度值。同时，也可在该固定坐标系下确定出该仿人机器人的机械臂姿态表达式，即机械臂姿态矩阵q＝[q1,q2,q3,q4,…,qi]，其中qi用于表示该仿人机器人的机械臂的第i个关节在该固定坐标系下的关节角度值。在本实施例的一种实施方式中，所述机械臂可以是六自由度机械臂，即该机械臂包括六个关节(i＝6)；所述机械臂也可以是七自由度机械臂，即该机械臂包括七个关节(i＝7)。
由此，当获取到仿人机器人在当前时刻相对于固定参考系的腰部姿态矩阵及机械臂姿态矩阵后，可直接将所述腰部姿态矩阵及所述机械臂姿态矩阵组合形成对应的全身广义坐标向量ζ＝[η q]T，以通过所述全身广义坐标向量将仿人机器人的足部作为参照，将机器人腰部相对于足部的运动状态以及机械臂相对于足部的运动姿态统筹在一起，对仿人机器人在当前时刻的整体运动状况进行有效阐述。
此外，所述实际任务空间向量用于表示所述仿人机器人的机械臂末端在当前时刻下于上述固定坐标系下实际表现出的与任务空间相关的位姿状态信息。所述待执行任务空间向量用于表示在当前时刻的待执行任务被所述仿人机器人的机械臂末端完成时，所述仿人机器人的机械臂末端于上述固定坐标系下期望表现出的与任务空间相关的位姿状态信息。
步骤S220，根据腰部姿态矩阵及机械臂姿态矩阵，计算全身广义坐标向量相对于实际任务空间向量的逆向雅可比矩阵。
在本实施例中，当所述机器人控制设备10获取到所述仿人机器人在当前时刻对应的全身广义坐标向量后，可通过对该全身广义坐标向量进行解析，并根据广义正运动学揭露出的广义坐标向量与任务空间向量之间的联系σ＝f(ζ)(其中，σ用于表示任务空间向量，ζ用于表示广义坐标向量)，计算出所述仿人机器人在当前时刻的全身广义坐标向量与实际任务空间向量之间的正向雅可比矩阵，而后基于计算出的正向雅可比矩阵利用广义逆运动学原理进行奇异值(Singular Value Decomposition，SVD)分解，求解出满足广义逆运动学原理的逆向雅可比矩阵，以便于后续通过所述逆向雅可比矩阵确定出达成所述待执行任务空间向量的广义坐标向量数值。
可选地，请参照图3，图3是图2中的步骤S220包括的子步骤的流程示意图。在本实施例中，所述步骤S220可以包括子步骤S221～子步骤S227，以确保所述机器人控制设备10能够正常计算出仿人机器人在当前时刻下的广义坐标向量与任务空间向量之间的逆向雅可比矩阵。
子步骤S221，根据腰部姿态矩阵计算仿人机器人在当前时刻的腰部相对于足部的第一运动速度矩阵。
在本实施例中，所述第一运动速度矩阵包括对应腰部相对于足部的线速度及角速度，所述第一运动速度矩阵可由所述机器人控制设备10在根据所述腰部姿态矩阵确定出的腰部相对于足部的位姿基础上进行求导计算得到。此时，所述仿人机器人的腰部相对于足部的位姿可表达为：
Figure BDA0003001835880000121
其中，
Figure BDA0003001835880000122
用于表示仿人机器人的腰部相对于足部的位姿，Rotx(rx)用于表示对腰部姿态矩阵中rx分量的欧拉角，Roty(ry)用于表示对腰部姿态矩阵中ry分量的欧拉角，Rotz(rz)用于表示对腰部姿态矩阵中rz分量的欧拉角。
由此，所述第一运动速度矩阵即可采用如下式子计算得到：
Figure BDA0003001835880000123
其中，
Figure BDA0003001835880000124
用于表示对应腰部相对于足部的线速度，
Figure BDA0003001835880000125
用于表示对应腰部相对于足部的角速度。
子步骤S222，根据机械臂姿态矩阵确定仿人机器人在当前时刻的机械臂末端相对于机械臂基座的位姿表达矩阵。
在本实施例中，所述仿人机器人包括的机械臂通过机械臂基座与腰部所在躯干相连接，由此可通过对所述机械臂姿态矩阵进行分析求解出机械臂末端相对于机械臂基座的位姿表达矩阵。
子步骤S223，获取仿人机器人在当前时刻的机械臂基座相对于足部的旋转矩阵。
子步骤S224，根据旋转矩阵以及位姿表达矩阵计算仿人机器人在当前时刻的机械臂末端相对于足部的第二运动速度矩阵。
在本实施例中，所述第二运动速度矩阵包括对应机械臂末端相对于足部的线速度及角速度，所述第二运动速度矩阵可由所述机器人控制设备10在对所述位姿表达矩阵进行求导后与所述旋转矩阵进行矩阵聚合得到，此时所述第二运动速度矩阵可采用如下式子进行表达：
Figure BDA0003001835880000131
其中，
Figure BDA0003001835880000132
用于表示对应机械臂末端相对于足部的线速度，
Figure BDA0003001835880000133
用于表示对应机械臂末端相对于足部的角速度，
Figure BDA0003001835880000134
用于表示机械臂基座相对于足部的旋转矩阵，
Figure BDA0003001835880000135
由机械臂末端相对于机械臂基座的位姿表达矩阵进行求导得到。
子步骤S225，将第一运动速度矩阵与第二运动速度矩阵进行速度叠加处理，得到对应的待处理速度矩阵。
在本实施例中，所述待处理速度矩阵通过将所述第一运动速度矩阵及所述第二运动速度矩阵中速度类型相同的元素进行加法运算得到，此时所述待处理速度矩阵与所述第一运动速度矩阵及所述第二运动速度矩阵之间的叠加关系可采用如下式子进行表达：
Figure BDA0003001835880000141
其中，A用于表示所述待处理速度矩阵。
子步骤S226，根据待处理速度矩阵及全身广义坐标向量计算全身广义坐标向量相对于实际任务空间向量的正向雅可比矩阵。
在本实施例中，当求解出所述待处理速度矩阵后，可通过对上述广义正运动学揭露出的广义坐标向量与任务空间向量之间的联系σ＝f(ζ)进行求导变形，而后将该待处理速度矩阵代入到变形出的等式中，确定出所述待处理速度矩阵、所述全身广义坐标向量与所述正向雅可比矩阵之间的关联关系，进而基于该关联关系计算出所述全身广义坐标向量相对于所述实际任务空间向量的正向雅可比矩阵。此时，所述待处理速度矩阵、所述全身广义坐标向量与所述正向雅可比矩阵之间的关联关系采用如下式子表达为：
Figure BDA0003001835880000142
其中，A用于表示所述待处理速度矩阵，ζ用于表示所述全身广义坐标向量，
Figure BDA0003001835880000143
用于表示所述全身广义坐标向量关于时间的一阶导数，J(ζ)用于表示所述全身广义坐标向量相对于所述实际任务空间向量的正向雅可比矩阵。
子步骤S227，对计算出的正向雅可比矩阵进行奇异值分解，得到对应的逆向雅可比矩阵。
在本实施例中，所述机器人控制设备10可通过对计算出的正向雅可比矩阵J(ζ)进行SVD分解，得到与该正向雅可比矩阵J(ζ)相对应的逆向雅可比矩阵
Figure BDA0003001835880000144
由此，本申请可通过执行上述子步骤S221～子步骤S227，使所述机器人控制设备10正常计算出仿人机器人在当前时刻下的广义坐标向量与任务空间向量之间的逆向雅可比矩阵，以便于后续通过所述逆向雅可比矩阵确定出达成所述待执行任务空间向量的广义坐标向量数值。
请再次参照图2，步骤S230，根据与全身广义坐标向量对应的预设位置限制条件，将实际任务空间向量及待执行任务空间向量结合到逆向雅可比矩阵的零空间中，计算与待执行任务空间向量对应的目标广义坐标向量。
在本实施例中，所述预设位置限制条件可由多个子条件组成，每个子条件对应所述全身广义坐标向量中的一项向量元素，即每个子条件所包括的预设数值上限及预设数值下限用于对所述全身广义坐标向量中对应向量分量的极限运动范围进行限制，使所述仿人机器人在所有子条件对应的极限运动范围内运动时，能够保证所述仿人机器人是避开关节运动极限运动的，并可在持续维持机器人腿部平衡的基础上扩增手臂操作空间，进而提升仿人机器人的操作性能。
为此，本申请通过将所述实际任务空间向量及所述待执行任务空间向量结合到所述逆向雅可比矩阵的零空间中用以求解与该待执行任务空间向量匹配的广义坐标向量，并在广义坐标向量求解过程中使用所述预设位置限制条件对广义坐标向量进行数值限定，从而确保最终求得的目标广义坐标向量在被使用时，既能保证仿人机器人避开关节运动极限运动，又能保证仿人机器人在机器人腿部平衡的状态下有效完成待执行任务，提升仿人机器人的操作性能。
可选地，请参照图4，图4是图2中的步骤S230包括的子步骤的流程示意图。在本实施例中，所述步骤S230可以包括子步骤S231～子步骤S235，以确保所述机器人控制设备10计算出既能保证仿人机器人避开关节运动极限运动，又能保证仿人机器人在机器人腿部平衡的状态下有效完成待执行任务的目标广义坐标向量。
子步骤S231，计算待执行任务空间向量与实际任务空间向量之间的任务向量差异矩阵。
子步骤S232，计算逆向雅可比矩阵在零空间中对应的零空间矩阵。
在本实施例中，所述零空间矩阵与所述逆向雅可比矩阵之间的关联关系可采用如下式子进行表达：
Figure BDA0003001835880000161
其中，Z(ζ)用于表示所述零空间矩阵，
Figure BDA0003001835880000162
用于表示所述逆向雅可比矩阵，J(ζ)用于表示所述正向雅可比矩阵。
子步骤S233，将全身广义坐标向量中的每项向量元素与预设位置限制条件中对应向量元素的预设数值上限及预设数值下限进行比较，并根据得到的所有比较结果针对全身广义坐标向量生成匹配的限位矫正矩阵。
在本实施例中，所述机器人控制设备10存储有多个用于构建所述限位矫正矩阵的矫正构建策略，每个所述矫正构建策略单独地与所述全身广义坐标向量中的一项向量元素对应。当所述机器人控制设备10将所述全身广义坐标向量中的某项向量元素与所述预设位置限制条件中对应向量元素的预设数值上限及预设数值下限进行比较后，会调用与该向量元素对应的矫正构建策略根据比较结果针对该向量元素生成所述限位矫正矩阵中的一项矩阵元素，以通过构建出的所有矩阵元素在计算目标广义坐标向量时，对广义坐标向量进行数值范围限制，避免出现关节运动超限现象。
在本实施例的一种实施方式中，所述机器人控制设备10存在的矫正构建策略可采用如下式子进行表达：
Figure BDA0003001835880000163
其中，ΔSi用于表示限位矫正矩阵ΔS的第i个矩阵元素，ζi用于表示所述全身广义坐标向量中第i个向量元素，ζimin用于表示所述预设位置限制条件中与第i个向量元素对应的预设数值下限，ζimax用于表示所述预设位置限制条件中与第i个向量元素对应的预设数值上限，k用于表示所述机器人控制设备10针对关节运动极限的矫正程度。
子步骤S234，根据逆向雅可比矩阵、零空间矩阵、任务向量差异矩阵以及限位矫正矩阵，计算仿人机器人针对待执行任务空间向量的广义向量补偿矩阵。
在本实施例中，当所述机器人控制设备10得到所述逆向雅可比矩阵、所述零空间矩阵、所述任务向量差异矩阵以及所述限位矫正矩阵后，可将利用广义逆运动学原理在逆向雅可比矩阵的零空间中将所述任务向量差异矩阵以及所述限位矫正矩阵结合在一起，以求得所述仿人机器人要实现所述待执行任务空间向量所对应的待执行任务时，需要对当前全身广义坐标向量进行参数补偿的广义向量补偿矩阵，以保证所述全身广义坐标向量在经所述广义向量补偿矩阵处理后得到的目标广义坐标向量，能够保证仿人机器人对应运行时的运动状态能够保持在避开关节运动极限的状态，并且能够在维持机器人腿部平衡的基础上有效地执行完所述待执行任务，从而从零空间的角度采用预设关节运动极限条件对仿人机器人运动进行整体优化，提升了仿人机器人的操作性能。
此时，所述广义向量补偿矩阵、所述逆向雅可比矩阵、所述零空间矩阵、所述任务向量差异矩阵以及所述限位矫正矩阵之间的关联关系采用如下式子表达为：
Figure BDA0003001835880000171
其中，Δζ用于表示所述广义向量补偿矩阵，
Figure BDA0003001835880000172
用于表示所述逆向雅可比矩阵，Z(ζ)用于表示所述零空间矩阵，Δσ用于表示所述任务向量差异矩阵，ΔS用于表示所述限位矫正矩阵。
子步骤S235，按照广义向量补偿矩阵对全身广义坐标向量进行向量补偿，得到目标广义坐标向量。
在本实施例中，可通过将所述广义向量补偿矩阵与所述全身广义坐标向量中相同元素位置的元素内容进行矩阵加法运算，得到既能保证仿人机器人避开关节运动极限运动，又能保证仿人机器人在机器人腿部平衡的状态下有效完成待执行任务的目标广义坐标向量。
由此，本申请可通过执行上述子步骤S231～子步骤S235，使所述机器人控制设备10能够计算出既能保证仿人机器人避开关节运动极限运动，又能保证仿人机器人在机器人腿部平衡的状态下有效完成待执行任务的目标广义坐标向量。
请再次参照图2，步骤S240，按照目标广义坐标向量对仿人机器人的运动状态进行控制。
在本实施例中，当所述机器人控制设备10计算出与所述待执行任务空间向量对应的目标广义坐标向量后，可按照所述目标广义坐标向量对所述仿人机器人在当前时刻的全身广义坐标向量所对应的各项机器人器件(包括仿人机器人的机械臂各关节电机及腰部结构各关节电机)进行运动参数调整，使所述仿人机器人参照所述目标广义坐标向量进行运动表达，以保证所述仿人机器人能够在机器人腿部平衡的状态下有效完成待执行任务，并保证该仿人机器人避开关节运动极限运动，提升了仿人机器人的操作性能。
由此，本申请可通过执行上述步骤S210～步骤S240，从整体上对仿人机器人的运动进行优化以避开关节运动极限，并在维持机器人腿部平衡的基础上扩增手臂操作空间，从而提升仿人机器人的操作性能。
在本申请中，为确保所述机器人控制设备10能够通过所述运动控制装置100执行上述运动控制方法，本申请通过对所述运动控制装置100进行功能模块划分的方式实现前述功能。下面对本申请提供的运动控制装置100的具体组成进行相应描述。
请参照图5，图5是本申请实施例提供的运动控制装置100的组成示意图。在本申请实施例中，所述运动控制装置100可以包括运动信息获取模块110、逆向矩阵计算模块120、广义向量计算模块130及运动状态控制模块140。
运动信息获取模块110，用于获取仿人机器人在当前时刻的全身广义坐标向量、实际任务空间向量以及待执行任务空间向量，其中所述全身广义坐标向量由所述仿人机器人的腰部姿态矩阵及机械臂姿态矩阵组成。
逆向矩阵计算模块120，用于根据所述腰部姿态矩阵及所述机械臂姿态矩阵，计算所述全身广义坐标向量相对于所述实际任务空间向量的逆向雅可比矩阵。
广义向量计算模块130，用于根据与所述全身广义坐标向量对应的预设位置限制条件，将所述实际任务空间向量及所述待执行任务空间向量结合到所述逆向雅可比矩阵的零空间中，计算与所述待执行任务空间向量对应的目标广义坐标向量。
运动状态控制模块140，用于按照所述目标广义坐标向量对所述仿人机器人的运动状态进行控制。
可选地，请参照图6，图6是图5中的逆向矩阵计算模块120的组成示意图。在本实施例中，所述逆向矩阵计算模块120可以包括速度矩阵计算子模块121、位姿矩阵确定子模块122、旋转矩阵获取子模块123、正向矩阵计算子模块124及逆向矩阵求解子模块125。
速度矩阵计算子模块121，用于根据所述腰部姿态矩阵计算所述仿人机器人在当前时刻的腰部相对于足部的第一运动速度矩阵，其中所述第一运动速度矩阵包括对应腰部相对于足部的线速度及角速度。
位姿矩阵确定子模块122，用于根据所述机械臂姿态矩阵确定所述仿人机器人在当前时刻的机械臂末端相对于机械臂基座的位姿表达矩阵。
旋转矩阵获取子模块123，用于获取所述仿人机器人在当前时刻的机械臂基座相对于足部的旋转矩阵。
所述速度矩阵计算子模块121，还用于根据所述旋转矩阵以及所述位姿表达矩阵计算所述仿人机器人在当前时刻的机械臂末端相对于足部的第二运动速度矩阵，其中所述第二运动速度矩阵包括对应机械臂末端相对于足部的线速度及角速度。
所述速度矩阵计算子模块121，还用于将所述第一运动速度矩阵与所述第二运动速度矩阵进行速度叠加处理，得到对应的待处理速度矩阵。
正向矩阵计算子模块124，用于根据所述待处理速度矩阵及所述全身广义坐标向量计算所述全身广义坐标向量相对于所述实际任务空间向量的正向雅可比矩阵。
逆向矩阵求解子模块125，用于对计算出的所述正向雅可比矩阵进行奇异值分解，得到对应的逆向雅可比矩阵。
在此过程中，所述待处理速度矩阵、所述全身广义坐标向量与所述正向雅可比矩阵之间的关联关系采用如下式子表达为：
Figure BDA0003001835880000201
其中，A用于表示所述待处理速度矩阵，ζ用于表示所述全身广义坐标向量，
Figure BDA0003001835880000202
用于表示所述全身广义坐标向量关于时间的一阶导数，J(ζ)用于表示所述正向雅可比矩阵。
可选地，请参照图7，图7是图5中的广义向量计算模块130的组成示意图。在本实施例中，所述广义向量计算模块130可以包括任务差异计算子模块131、空间矩阵计算子模块132、限位矫正确定子模块133、向量补偿计算子模块134及广义向量补偿子模块135。
任务差异计算子模块131，用于计算所述待执行任务空间向量与所述实际任务空间向量之间的任务向量差异矩阵。
空间矩阵计算子模块132，用于计算所述逆向雅可比矩阵在零空间中对应的零空间矩阵。
限位矫正确定子模块133，用于将所述全身广义坐标向量中的每项向量元素与所述预设位置限制条件中对应向量元素的预设数值上限及预设数值下限进行比较，并根据得到的所有比较结果针对所述全身广义坐标向量生成匹配的限位矫正矩阵。
向量补偿计算子模块134，用于根据所述逆向雅可比矩阵、所述零空间矩阵、所述任务向量差异矩阵以及所述限位矫正矩阵，计算所述仿人机器人针对所述待执行任务空间向量的广义向量补偿矩阵。
广义向量补偿子模块135，用于按照所述广义向量补偿矩阵对所述全身广义坐标向量进行向量补偿，得到所述目标广义坐标向量。
在此过程中，所述广义向量补偿矩阵、所述逆向雅可比矩阵、所述零空间矩阵、所述任务向量差异矩阵以及所述限位矫正矩阵之间的关联关系采用如下式子表达为：
Figure BDA0003001835880000211
其中，Δζ用于表示所述广义向量补偿矩阵，
Figure BDA0003001835880000212
用于表示所述逆向雅可比矩阵，Z(ζ)用于表示所述零空间矩阵，Δσ用于表示所述任务向量差异矩阵，ΔS用于表示所述限位矫正矩阵。
需要说明的是，本申请实施例所提供的运动控制装置100，其基本原理及产生的技术效果与前述的运动控制方法相同。为简要描述，本实施例部分未提及之处，可参考上述的针对运动控制方法的描述内容。
在本申请所提供的实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和框图显示了根据本申请的实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
另外，在本申请各个实施例中的各功能模块可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或两个以上模块集成形成一个独立的部分。所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个可读存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个可读存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的可读存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
综上所述，在本申请提供的运动控制方法、装置、机器人控制设备及可读存储介质中，本申请在获取到仿人机器人在当前时刻的全身广义坐标向量、实际任务空间向量以及待执行任务空间向量后，会相应地根据全身广义坐标向量中的腰部姿态矩阵及机械臂姿态矩阵，计算全身广义坐标向量相对于实际任务空间向量的逆向雅可比矩阵，并根据与全身广义坐标向量对应的预设位置限制条件，将实际任务空间向量及待执行任务空间向量结合到逆向雅可比矩阵的零空间中，计算与该待执行任务空间向量对应的目标广义坐标向量，接着按照目标广义坐标向量对仿人机器人的运动状态进行控制，以从零空间的角度采用预设关节运动极限条件对仿人机器人运动进行整体优化，达到控制仿人机器人避开关节运动极限的目的，从而在维持机器人腿部平衡的基础上扩增手臂操作空间，提升仿人机器人的操作性能。
以上所述，仅为本申请的各种实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应当以权利要求的保护范围为准。

Claims (10)

1.一种运动控制方法，其特征在于，所述方法包括：
获取仿人机器人在当前时刻的全身广义坐标向量、实际任务空间向量以及待执行任务空间向量，其中所述全身广义坐标向量由所述仿人机器人的腰部姿态矩阵及机械臂姿态矩阵组成；
根据所述腰部姿态矩阵及所述机械臂姿态矩阵，计算所述全身广义坐标向量相对于所述实际任务空间向量的逆向雅可比矩阵；
根据与所述全身广义坐标向量对应的预设位置限制条件，将所述实际任务空间向量及所述待执行任务空间向量结合到所述逆向雅可比矩阵的零空间中，计算与所述待执行任务空间向量对应的目标广义坐标向量，其中所述预设位置限制条件用于描述所述仿人机器人各关节在运动过程中的极限运动范围；
按照所述目标广义坐标向量对所述仿人机器人的运动状态进行控制。
2.根据权利要求1所述的方法，其特征在于，所述根据所述腰部姿态矩阵及所述机械臂姿态矩阵，计算所述全身广义坐标向量相对于所述实际任务空间向量的逆向雅可比矩阵的步骤，包括：
根据所述腰部姿态矩阵计算所述仿人机器人在当前时刻的腰部相对于足部的第一运动速度矩阵，其中所述第一运动速度矩阵包括对应腰部相对于足部的线速度及角速度；
根据所述机械臂姿态矩阵确定所述仿人机器人在当前时刻的机械臂末端相对于机械臂基座的位姿表达矩阵；
获取所述仿人机器人在当前时刻的机械臂基座相对于足部的旋转矩阵；
根据所述旋转矩阵以及所述位姿表达矩阵计算所述仿人机器人在当前时刻的机械臂末端相对于足部的第二运动速度矩阵，其中所述第二运动速度矩阵包括对应机械臂末端相对于足部的线速度及角速度；
将所述第一运动速度矩阵与所述第二运动速度矩阵进行速度叠加处理，得到对应的待处理速度矩阵；
根据所述待处理速度矩阵及所述全身广义坐标向量计算所述全身广义坐标向量相对于所述实际任务空间向量的正向雅可比矩阵；
对计算出的所述正向雅可比矩阵进行奇异值分解，得到对应的逆向雅可比矩阵。
3.根据权利要求2所述的方法，其特征在于，所述待处理速度矩阵、所述全身广义坐标向量与所述正向雅可比矩阵之间的关联关系采用如下式子表达为：
Figure FDA0003755668870000021
其中，A用于表示所述待处理速度矩阵，ζ用于表示所述全身广义坐标向量，
Figure FDA0003755668870000022
用于表示所述全身广义坐标向量关于时间的一阶导数，J(ζ)用于表示所述正向雅可比矩阵。
4.根据权利要求1-3中任意一项所述的方法，其特征在于，所述根据与所述全身广义坐标向量对应的预设位置限制条件，将所述实际任务空间向量及所述待执行任务空间向量结合到所述逆向雅可比矩阵的零空间中，计算与所述待执行任务空间向量对应的目标广义坐标向量的步骤，包括：
计算所述待执行任务空间向量与所述实际任务空间向量之间的任务向量差异矩阵；
计算所述逆向雅可比矩阵在零空间中对应的零空间矩阵；
将所述全身广义坐标向量中的每项向量元素与所述预设位置限制条件中对应向量元素的预设数值上限及预设数值下限进行比较，并根据得到的所有比较结果针对所述全身广义坐标向量生成匹配的限位矫正矩阵；
根据所述逆向雅可比矩阵、所述零空间矩阵、所述任务向量差异矩阵以及所述限位矫正矩阵，计算所述仿人机器人针对所述待执行任务空间向量的广义向量补偿矩阵；
按照所述广义向量补偿矩阵对所述全身广义坐标向量进行向量补偿，得到所述目标广义坐标向量。
5.根据权利要求4所述的方法，其特征在于，所述广义向量补偿矩阵、所述逆向雅可比矩阵、所述零空间矩阵、所述任务向量差异矩阵以及所述限位矫正矩阵之间的关联关系采用如下式子表达为：
Figure FDA0003755668870000031
其中，Δζ用于表示所述广义向量补偿矩阵，
Figure FDA0003755668870000032
用于表示所述逆向雅可比矩阵，Z(ζ)用于表示所述零空间矩阵，Δσ用于表示所述任务向量差异矩阵，ΔS用于表示所述限位矫正矩阵。
6.一种运动控制装置，其特征在于，所述装置包括：
运动信息获取模块，用于获取仿人机器人在当前时刻的全身广义坐标向量、实际任务空间向量以及待执行任务空间向量，其中所述全身广义坐标向量由所述仿人机器人的腰部姿态矩阵及机械臂姿态矩阵组成；
逆向矩阵计算模块，用于根据所述腰部姿态矩阵及所述机械臂姿态矩阵，计算所述全身广义坐标向量相对于所述实际任务空间向量的逆向雅可比矩阵；
广义向量计算模块，用于根据与所述全身广义坐标向量对应的预设位置限制条件，将所述实际任务空间向量及所述待执行任务空间向量结合到所述逆向雅可比矩阵的零空间中，计算与所述待执行任务空间向量对应的目标广义坐标向量，其中所述预设位置限制条件用于描述所述仿人机器人各关节在运动过程中的极限运动范围；
运动状态控制模块，用于按照所述目标广义坐标向量对所述仿人机器人的运动状态进行控制。
7.根据权利要求6所述的装置，其特征在于，所述逆向矩阵计算模块包括：
速度矩阵计算子模块，用于根据所述腰部姿态矩阵计算所述仿人机器人在当前时刻的腰部相对于足部的第一运动速度矩阵，其中所述第一运动速度矩阵包括对应腰部相对于足部的线速度及角速度；
位姿矩阵确定子模块，用于根据所述机械臂姿态矩阵确定所述仿人机器人在当前时刻的机械臂末端相对于机械臂基座的位姿表达矩阵；
旋转矩阵获取子模块，用于获取所述仿人机器人在当前时刻的机械臂基座相对于足部的旋转矩阵；
所述速度矩阵计算子模块，还用于根据所述旋转矩阵以及所述位姿表达矩阵计算所述仿人机器人在当前时刻的机械臂末端相对于足部的第二运动速度矩阵，其中所述第二运动速度矩阵包括对应机械臂末端相对于足部的线速度及角速度；
所述速度矩阵计算子模块，还用于将所述第一运动速度矩阵与所述第二运动速度矩阵进行速度叠加处理，得到对应的待处理速度矩阵；
正向矩阵计算子模块，用于根据所述待处理速度矩阵及所述全身广义坐标向量计算所述全身广义坐标向量相对于所述实际任务空间向量的正向雅可比矩阵；
逆向矩阵求解子模块，用于对计算出的所述正向雅可比矩阵进行奇异值分解，得到对应的逆向雅可比矩阵。
8.根据权利要求6或7所述的装置，其特征在于，所述广义向量计算模块包括：
任务差异计算子模块，用于计算所述待执行任务空间向量与所述实际任务空间向量之间的任务向量差异矩阵；
空间矩阵计算子模块，用于计算所述逆向雅可比矩阵在零空间中对应的零空间矩阵；
限位矫正确定子模块，用于将所述全身广义坐标向量中的每项向量元素与所述预设位置限制条件中对应向量元素的预设数值上限及预设数值下限进行比较，并根据得到的所有比较结果针对所述全身广义坐标向量生成匹配的限位矫正矩阵；
向量补偿计算子模块，用于根据所述逆向雅可比矩阵、所述零空间矩阵、所述任务向量差异矩阵以及所述限位矫正矩阵，计算所述仿人机器人针对所述待执行任务空间向量的广义向量补偿矩阵；
广义向量补偿子模块，用于按照所述广义向量补偿矩阵对所述全身广义坐标向量进行向量补偿，得到所述目标广义坐标向量。
9.一种机器人控制设备，其特征在于，包括处理器和存储器，所述存储器存储有能够被所述处理器执行的计算机程序，所述处理器可执行所述计算机程序，以实现权利要求1-5中任意一项所述的运动控制方法。
10.一种可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时，实现权利要求1-5中任意一项所述的运动控制方法。
CN202110349082.0A 2021-03-31 2021-03-31 运动控制方法、装置、机器人控制设备及可读存储介质 Active CN113070880B (zh)

Priority Applications (3)

Application Number Priority Date Filing Date Title
CN202110349082.0A CN113070880B (zh) 2021-03-31 2021-03-31 运动控制方法、装置、机器人控制设备及可读存储介质
PCT/CN2021/131893 WO2022205941A1 (zh) 2021-03-31 2021-11-19 运动控制方法、装置、机器人控制设备及可读存储介质
US17/702,819 US11938635B2 (en) 2021-03-31 2022-03-24 Motion control method, robot controller and computer readable storage medium

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110349082.0A CN113070880B (zh) 2021-03-31 2021-03-31 运动控制方法、装置、机器人控制设备及可读存储介质

Publications (2)

Publication Number Publication Date
CN113070880A CN113070880A (zh) 2021-07-06
CN113070880B true CN113070880B (zh) 2022-09-09

Family

ID=76614203

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110349082.0A Active CN113070880B (zh) 2021-03-31 2021-03-31 运动控制方法、装置、机器人控制设备及可读存储介质

Country Status (3)

Country Link
US (1) US11938635B2 (zh)
CN (1) CN113070880B (zh)
WO (1) WO2022205941A1 (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113070880B (zh) * 2021-03-31 2022-09-09 深圳市优必选科技股份有限公司 运动控制方法、装置、机器人控制设备及可读存储介质
CN114326769B (zh) * 2021-12-28 2024-03-29 深圳市优必选科技股份有限公司 机器人运动矫正方法及装置、机器人控制设备和存储介质

Family Cites Families (22)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP3603279B2 (ja) * 2001-09-26 2004-12-22 独立行政法人科学技術振興機構 二脚歩行式人型ロボット
JP2005028468A (ja) * 2003-07-08 2005-02-03 National Institute Of Advanced Industrial & Technology ロボットの視覚座標系位置姿勢同定方法、座標変換方法および装置
JP2005351836A (ja) * 2004-06-14 2005-12-22 Furukawa Co Ltd 移動体位置測定装置
JP4682791B2 (ja) * 2005-10-12 2011-05-11 ソニー株式会社 操作空間物理量算出装置及び操作空間物理量算出方法、並びにコンピュータ・プログラム
EP1972415B1 (en) * 2007-03-23 2019-01-02 Honda Research Institute Europe GmbH Robots with collision avoidance functionality
KR20110016521A (ko) * 2009-08-12 2011-02-18 삼성전자주식회사 인간형 로봇의 전신 동작 제어 장치 및 그 방법
US8924015B2 (en) * 2009-09-14 2014-12-30 Honda Motor Co., Ltd. Whole-body humanoid control from upper-body task specifications
KR101667031B1 (ko) * 2009-11-02 2016-10-17 삼성전자 주식회사 로봇의 경로 계획 장치 및 그 방법
JP2012081568A (ja) * 2010-10-14 2012-04-26 Sony Corp ロボットの制御装置及び制御方法、並びにコンピューター・プログラム
US8874262B2 (en) * 2011-09-27 2014-10-28 Disney Enterprises, Inc. Operational space control of rigid-body dynamical systems including humanoid robots
JP5661023B2 (ja) * 2011-12-02 2015-01-28 本田技研工業株式会社 脚式移動ロボットの歩容生成装置及びロボットの動作目標生成装置
CN104097206B (zh) * 2013-04-07 2016-08-17 同济大学 基于动量控制的人型机器人全身运动自平衡控制方法
CN104407611B (zh) * 2014-09-30 2017-02-01 同济大学 一种仿人机器人稳定行走的控制方法
US11402402B2 (en) * 2016-01-12 2022-08-02 Bigmotion Technologies Inc. Systems and methods for human body motion capture
CN108241339B (zh) * 2017-12-27 2020-09-04 北京航空航天大学 仿人机械臂的运动求解和构型控制方法
CN108897220B (zh) * 2018-07-12 2020-09-29 上海硅族智能科技有限公司 一种自适应稳定平衡控制方法和系统以及双足仿人机器人
CN110900598B (zh) * 2019-10-15 2022-09-23 合肥工业大学 机器人三维运动空间动作模仿学习方法和系统
CN110722562B (zh) * 2019-10-28 2021-03-09 华中科技大学 一种用于机器人参数辨识的空间雅克比矩阵构造方法
US11579575B2 (en) * 2019-12-03 2023-02-14 Baidu Usa Llc Inverse reinforcement learning with model predictive control
CN111538949B (zh) * 2020-07-10 2020-10-16 深圳市优必选科技股份有限公司 冗余机器人逆运动学求解方法、装置和冗余机器人
CN111872941B (zh) * 2020-08-06 2021-09-07 深圳市优必选科技股份有限公司 平衡控制方法、装置、仿人机器人及可读存储介质
CN113070880B (zh) * 2021-03-31 2022-09-09 深圳市优必选科技股份有限公司 运动控制方法、装置、机器人控制设备及可读存储介质

Also Published As

Publication number Publication date
US11938635B2 (en) 2024-03-26
WO2022205941A1 (zh) 2022-10-06
US20220324106A1 (en) 2022-10-13
CN113070880A (zh) 2021-07-06

Similar Documents

Publication Publication Date Title
CN111872941B (zh) 平衡控制方法、装置、仿人机器人及可读存储介质
CN111538949B (zh) 冗余机器人逆运动学求解方法、装置和冗余机器人
CN113070880B (zh) 运动控制方法、装置、机器人控制设备及可读存储介质
Lee et al. Planning and control for collision-free cooperative aerial transportation
US11579591B2 (en) Feedforward control method for floating base dynamics, computer-readable storage medium and robot
US8140189B2 (en) Apparatus and method for computing operational-space physical quantity
JP2020108915A (ja) ロボット重心位置調整方法、装置、ロボット及びデータ記憶媒体
JP6321905B2 (ja) 関節システムの制御方法、記憶媒体、制御システム
US20220040851A1 (en) Robot posture control method and robot and computer readable storage medium using the same
CN109434831B (zh) 机器人运行方法、装置、机器人、电子设备及可读介质
CN113618742B (zh) 一种机器人避障方法、装置和机器人
Villarreal-Cervantes et al. Stabilization of a (3, 0) mobile robot by means of an event-triggered control
Zube Cartesian nonlinear model predictive control of redundant manipulators considering obstacles
CN109491381B (zh) 基于观测器的多移动机器人自适应编队跟踪控制方法
CN112597612B (zh) 机器人优化方法、装置、终端设备及计算机可读存储介质
Grandia et al. Contact invariant model learning for legged robot locomotion
US10386252B2 (en) Position control apparatus for identifying low-frequency disturbance
US20220379470A1 (en) Robot balance control method, robot controller and computer readable storage medium
Park et al. Cooperative-dual-task-space-based whole-body motion balancing for humanoid robots
Hiraoka et al. Online generation and control of quasi-static multi-contact motion by PWT Jacobian matrix with contact wrench estimation and joint load reduction
CN113021347A (zh) 机械臂控制方法、装置、作业控制设备及可读存储介质
CN112975965B (zh) 仿人机器人的解耦控制方法、装置和仿人机器人
CN114326769B (zh) 机器人运动矫正方法及装置、机器人控制设备和存储介质
CN113021345A (zh) 机械臂逆运动学控制方法、装置、设备及可读存储介质
Umar et al. Nonlinear Model Predictive Path-Following for Mecanum-Wheeled Omnidirectional Mobile Robot

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