CN113733106A - 力控仿人机器人的全身控制方法、装置、设备及介质 - Google Patents

力控仿人机器人的全身控制方法、装置、设备及介质 Download PDF

Info

Publication number
CN113733106A
CN113733106A CN202111302908.4A CN202111302908A CN113733106A CN 113733106 A CN113733106 A CN 113733106A CN 202111302908 A CN202111302908 A CN 202111302908A CN 113733106 A CN113733106 A CN 113733106A
Authority
CN
China
Prior art keywords
task
priority
jacobian matrix
executed
target
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.)
Granted
Application number
CN202111302908.4A
Other languages
English (en)
Other versions
CN113733106B (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 CN202111302908.4A priority Critical patent/CN113733106B/zh
Publication of CN113733106A publication Critical patent/CN113733106A/zh
Application granted granted Critical
Publication of CN113733106B publication Critical patent/CN113733106B/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
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • 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

Landscapes

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

Abstract

本申请涉及一种力控仿人机器人的全身控制方法、装置、设备及介质，其中方法包括：获取多个待执行任务，每个待执行任务携带有优先级，各个待执行任务是力控仿人机器人同步执行的任务；基于受约束的机器人全动力学、第k层优先级的任务雅克比矩阵需要在第k‑1层优先级的任务雅可比矩阵的零空间内、第k层优先级的任务雅克比矩阵是与第k‑1层优先级的任务雅克比矩阵相关的函数，对每个待执行任务进行目标广义关节空间关节出力计算；将各个目标广义关节空间关节出力作为全身控制结果。从而对需要同步执行的各个待执行任务进行整体协调控制，提高力控仿人机器人的整体协调性，降低同步执行的任务之间的冲突，确保优先级高的任务优先保证执行。

Description

力控仿人机器人的全身控制方法、装置、设备及介质
技术领域
本申请涉及到仿人机器人的运动控制技术领域，特别是涉及到一种力控仿人机器人的全身控制方法、装置、设备及介质。
背景技术
为了能够在复杂条件下工作，力控仿人机器人需要同步执行多种不同的任务，比如，同时进行支撑腿的足底约束、摆动腿的运动、质心的平衡控制、手眼的跟踪及上身姿态的控制等多个任务。而且力控仿人机器人需要同步执行多种不同的任务之间具有优先级的要求，比如，出于控制考虑需要将足底约束排在所有任务中优先级最高的位置，出于安全考虑需要将质心的平衡控制的优先级设为仅次足底约束的优先级，而上身姿态的控制可能不关键，因此将上身姿态的控制的优先级设为最低。另外，如何保证优先级高的任务不被优先级低的任务影响，也是全身控制规划需要解决的一个问题。现有技术采用手臂、腿、躯干和头部的分开规划和分开控制的方法，因力控仿人机器人被要求同步执行多种不同的任务，这些任务之间可能存在冲突，有的任务可能需要优先保证，有的任务可能没有办法完成需要做出调整，导致采用手臂、腿、躯干和头部的分开规划和分开控制的方法难以达到预期的灵巧程度、协调性以及多任务的执行能力，从而降低了力控仿人机器人工作的准确性。
发明内容
本申请的主要目的为提供一种力控仿人机器人的全身控制方法、装置、设备及介质，旨在解决现有技术的采用手臂、腿、躯干和头部的分开规划和分开控制的方法同步执行多种不同的任务，难以达到预期的灵巧程度、协调性以及多任务的执行能力，降低了力控仿人机器人工作的准确性的技术问题。
为了实现上述发明目的，本申请提出一种力控仿人机器人的全身控制方法，所述方法包括：
获取多个待执行任务，其中，每个所述待执行任务携带有优先级，各个所述待执行任务是力控仿人机器人同步执行的任务；
基于受约束的机器人全动力学、第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数，对每个所述待执行任务进行目标广义关节空间关节出力计算；
将各个所述目标广义关节空间关节出力作为全身控制规划结果。
进一步的，所述基于受约束的机器人全动力学、第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数，对每个所述待执行任务进行目标广义关节空间关节出力计算的步骤，包括：
获取计数器，其中，所述计数器初始化为1；
获取所述优先级等于所述计数器的值的所述待执行任务作为待规划任务；
获取所述待规划任务的目标零空间投影矩阵及目标名义惯性矩阵，其中，所述目标零空间投影矩阵是被控对象的雅克比矩阵的零空间投影矩阵；
基于受约束的机器人全动力学、第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数，根据所述目标零空间投影矩阵和所述目标名义惯性矩阵，对所述待规划任务进行所述目标广义关节空间关节出力计算；
将所述计数器加1，重复执行所述获取所述优先级等于所述计数器的值的所述待执行任务作为待规划任务的步骤，直至所述计数器的值大于所述待执行任务的数量。
进一步的，所述基于受约束的机器人全动力学、第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数，根据所述目标零空间投影矩阵和所述目标名义惯性矩阵，对所述待规划任务进行所述目标广义关节空间关节出力计算的步骤，包括：
获取从第1层所述优先级至所述待规划任务对应的所述优先级对应的每个所述待规划任务的目标任务雅克比矩阵需求值；
对获取的各个所述目标任务雅克比矩阵需求值进行相加计算，得到任务雅克比矩阵需求总值；
获取与所述待规划任务对应的目标雅可比矩阵和目标位控任务点工作空间加速度；
获取与各个所述待执行任务对应的目标关节角速度；
基于受约束的机器人全动力学、第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数，根据所述任务雅克比矩阵需求总值、所述目标关节角速度、所述目标位控任务点工作空间加速度、所述目标雅可比矩阵、所述目标零空间投影矩阵和所述目标名义惯性矩阵，对所述待规划任务进行所述目标广义关节空间关节出力计算。
进一步的，基于受约束的机器人全动力学、第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数，确定第k层所述优先级对应的所述待规划任务对应的所述目标广义关节空间关节出力的计算公式
Figure 238003DEST_PATH_IMAGE001
为：
Figure 827247DEST_PATH_IMAGE002
其中，
Figure 279088DEST_PATH_IMAGE003
是所述目标名义惯性矩阵，
Figure 205456DEST_PATH_IMAGE004
是所述目标位控任务点工作空间加速度，
Figure 190467DEST_PATH_IMAGE005
是所述目标雅可比矩阵，
Figure 685034DEST_PATH_IMAGE006
是所述目标雅可比矩阵的导数，
Figure 624171DEST_PATH_IMAGE007
是所述目标关节角速度，
Figure 619809DEST_PATH_IMAGE008
是所述力控仿人机器人的惯性矩阵的逆计算，
Figure 960791DEST_PATH_IMAGE009
是受约束的机器人全动力学对应的科式力项、离心力项、重力项及动力学方程建立在广义关节空间时的非线性项，
Figure 859215DEST_PATH_IMAGE010
是所述目标零空间投影矩阵的转置计算，
Figure 410282DEST_PATH_IMAGE011
是所述任务雅克比矩阵需求总值。
进一步的，受约束的机器人全动力学的计算方程为：
Figure 819398DEST_PATH_IMAGE012
其中，M是所述力控仿人机器人的惯性矩阵，
Figure 14887DEST_PATH_IMAGE013
是与各个所述待执行任务对应的目标关节角加速度，
Figure 975890DEST_PATH_IMAGE014
是从第1层所述优先级至第m层所述优先级对应的各个所述待执行任务对应的所述任务雅克比矩阵需求值的总值，
Figure 889619DEST_PATH_IMAGE015
是第k层所述优先级对应的所述待执行任务对应的所述任务雅克比矩阵需求值，m是所述待执行任务的数量；
将受约束的机器人全动力学的计算方程的等号两端均与
Figure 335382DEST_PATH_IMAGE016
相乘，得到第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数的计算公式：
Figure 650956DEST_PATH_IMAGE017
基于第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内，确定
Figure 517281DEST_PATH_IMAGE018
，则得到计算公式：
Figure 918307DEST_PATH_IMAGE019
第k层所述优先级对应的所述待执行任务对应的所述任务雅克比矩阵需求值的计算公式
Figure 669225DEST_PATH_IMAGE020
表述为：
Figure 698361DEST_PATH_IMAGE021
其中，
Figure 640647DEST_PATH_IMAGE022
是所述目标雅可比矩阵的转置计算，
Figure 263389DEST_PATH_IMAGE023
是第k层所述优先级对应的所述待执行任务对应的所述目标广义关节空间关节出力；
根据上述公式确定计算公式：
Figure 942632DEST_PATH_IMAGE024
Figure 967220DEST_PATH_IMAGE025
根据第k层所述优先级对应的所述待执行任务的位置控制关系确定计算公式：
Figure 316293DEST_PATH_IMAGE026
根据上述公式，确定第k层所述优先级对应的所述待规划任务对应的所述目标广义关节空间关节出力的计算公式
Figure 550965DEST_PATH_IMAGE027
为：
Figure 142221DEST_PATH_IMAGE028
进一步的，所述将各个所述目标广义关节空间关节出力作为全身控制规划结果的步骤之后，包括：
基于约束控制模块、位置控制模块、力控制模块及力位混合控制模块，根据所述全身控制规划结果对所述力控仿人机器人进行全身运动控制；
其中，所述约束控制模块用于基于约束点对所述力控仿人机器人与外界环境的接触情况以及所述力控仿人机器人的机械结构设计状况进行约束控制；
所述位置控制模块，用于基于末端位置对所述力控仿人机器人进行跟随位置指令的运动控制；
所述力控制模块，用于基于输出力对所述力控仿人机器人进行运动控制；
所述力位混合控制模块，用于基于所述末端位置和所述输出力对所述力控仿人机器人进行跟随位置及力指令的混合运动控制。
进一步的，所述待执行任务的分类包括：约束类型和任务类型；
所述分类为所述约束类型的各个所述待执行任务对应的各个所述优先级均高于所述分类为所述任务类型的各个所述待执行任务对应的各个所述优先级；
所述分类为所述约束类型的各个所述待执行任务对应的各个所述优先级的优先级顺序不可以调整。
本申请还提出了一种力控仿人机器人的全身控制装置，所述装置包括：
待执行任务获取模块，用于获取多个待执行任务，其中，每个所述待执行任务携带有优先级，各个所述待执行任务是力控仿人机器人同步执行的任务；
目标广义关节空间关节出力计算模块，用于基于受约束的机器人全动力学、第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数，对每个所述待执行任务进行目标广义关节空间关节出力计算；
全身控制结果确定模块，用于将各个所述目标广义关节空间关节出力作为全身控制规划结果。
本申请还提出了一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现上述任一项所述方法的步骤。
本申请还提出了一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述任一项所述的方法的步骤。
本申请的力控仿人机器人的全身控制方法、装置、设备及介质，其中方法首先获取多个待执行任务，其中，每个所述待执行任务携带有优先级，各个所述待执行任务是力控仿人机器人同步执行的任务，然后基于受约束的机器人全动力学、第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数，对每个所述待执行任务进行目标广义关节空间关节出力计算，最后将各个所述目标广义关节空间关节出力作为全身控制规划结果，从而实现基于第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数对需要同步执行的各个所述待执行任务进行整体协调控制，提高力控仿人机器人的整体协调性，降低了同步执行的任务之间的冲突，并且可以确保优先级高的任务优先保证执行，提高了力控仿人机器人工作的准确性，低优先级任务尽量满足。
附图说明
图1为本申请一实施例的力控仿人机器人的全身控制方法的流程示意图；
图2 为本申请一实施例的力控仿人机器人的全身控制装置的结构示意框图；
图3 为本申请一实施例的计算机设备的结构示意框图。
本申请目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。
参照图1，本申请实施例中提供一种力控仿人机器人的全身控制方法，所述方法包括：
S1：获取多个待执行任务，其中，每个所述待执行任务携带有优先级，各个所述待执行任务是力控仿人机器人同步执行的任务；
S2：基于受约束的机器人全动力学、第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数，对每个所述待执行任务进行目标广义关节空间关节出力计算；
S3：将各个所述目标广义关节空间关节出力作为全身控制规划结果。
本实施例首先获取多个待执行任务，其中，每个所述待执行任务携带有优先级，各个所述待执行任务是力控仿人机器人同步执行的任务，然后基于受约束的机器人全动力学、第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数，对每个所述待执行任务进行目标广义关节空间关节出力计算，最后将各个所述目标广义关节空间关节出力作为全身控制规划结果，从而实现基于第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数对需要同步执行的各个所述待执行任务进行整体协调控制，提高力控仿人机器人的整体协调性，降低了同步执行的任务之间的冲突，并且可以确保优先级高的任务优先保证执行，提高了力控仿人机器人工作的准确性，低优先级任务尽量满足。
力控仿人机器人，是力驱动的仿人机器人。仿人机器人，是模仿人的形态和行为而设计制造的机器人。
仿人机器人整体加起来有至少28个自由度，仿人机器人的自由度是常见的冗余工业机械臂的自由度的4倍，从而增加了运动规划和控制的难度。
对于S1，可以从数据库中获取多个待执行任务，也可以获取用户输入的多个待执行任务，还可以从第三方应用系统中获取多个待执行任务。
待执行任务，是力控仿人机器人需要执行的任务。
获取的多个待执行任务，是力控仿人机器人需要同步执行的任务。
其中，第k-1层所述优先级的优先级顺序高于第k层所述优先级的优先级顺序。
比如，第1层所述优先级的待执行任务为：脚底固定（坐标为：支撑脚位姿，控制策略为：位置控制），第2层所述优先级的待执行任务为：平衡（坐标为：压力等效中心点平面控制，控制策略为：优化控制），第3层所述优先级的待执行任务为：关节限制（坐标为：关节空间，控制策略为：优化控制），第4层所述优先级的待执行任务为：摆动脚（坐标为：摆动脚位姿，控制策略为：位置控制），第5层所述优先级的待执行任务为：上身姿态（坐标为：上身位姿，控制策略为：位置控制），第6层所述优先级的待执行任务为：注视（坐标为：头部位姿，控制策略为：位置控制），第7层所述优先级的待执行任务为：手部操作（坐标为：手部位姿，控制策略为：力位混合控制），在此举例不做具体限定。
优化控制，是指满足任务优先级动力学方程的能量最优。
对于S2，基于受约束的机器人全动力学、第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数，确定工作空间关节出力计算公式。
采用确定的工作空间关节出力计算公式，每次计算一个所述待执行任务的工作空间关节出力，将计算得到的工作空间关节出力作为该所述待执行任务对应的目标广义关节空间关节出力，其中，从优先级顺序高的所述待执行任务往优先级顺序低的所述待执行任务进行目标广义关节空间关节出力计算。比如，先计算与第k-1层所述优先级对应的所述待执行任务的目标广义关节空间关节出力，然后再计算与第k层所述优先级对应的所述待执行任务的目标广义关节空间关节出力。
受约束的机器人全动力学，也就是受约束点约束的机器人全动力学。
约束点，是机器人工作空间的约束点。力控仿人机器人的身体某一位置固定不动，从而形成约束点。比如，力控仿人机器人的的足部约束在地面，在此举例不做具体限定。
机器人工作空间，是指机器人末端执行器运动描述参考点所能达到的空间点的集合，一般用水平面和垂直面的投影表示。机器人工作空间的形状和大小是十分重要的，机器人在执行某作业时可能会因为存在手部不能到达的作业死区(dead zone)而不能完成任务。
任务雅克比矩阵，是力控仿人机器人的各个关节对任务的关节转矩（也就是力矩）。
目标广义关节空间关节出力，也就是在机器人工作空间中，力控仿人机器人的各个关节对所述待执行任务的关节转矩。
零空间，是在线性映射（即矩阵）的背景下出现的，指：像为零的原像空间，即{x|Ax=0}。
第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数，也就是说，第k层所述优先级的所述待执行任务在进行目标广义关节空间关节出力计算时与第k-1层所述优先级的所述任务雅克比矩阵关联。
第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内，也就是说，第k-1层所述优先级的所述待执行任务在进行目标广义关节空间关节出力计算时，与第k层所述优先级的所述任务雅克比矩阵无关。
对于S3，将各个所述目标广义关节空间关节出力直接作为全身控制规划结果。
力控仿人机器人的各个关节各自对应的关节驱动器，根据全身控制规划结果，在机器人工作空间对各个所述待执行任务提供关节转矩，从而实现对力控仿人机器人的全身运动的控制。
在一个实施例中，上述基于受约束的机器人全动力学、第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数，对每个所述待执行任务进行目标广义关节空间关节出力计算的步骤，包括：
S21：获取计数器，其中，所述计数器初始化为1；
S22：获取所述优先级等于所述计数器的值的所述待执行任务作为待规划任务；
S23：获取所述待规划任务的目标零空间投影矩阵及目标名义惯性矩阵，其中，所述目标零空间投影矩阵是被控对象的雅克比矩阵的零空间投影矩阵；
S24：基于受约束的机器人全动力学、第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数，根据所述目标零空间投影矩阵和所述目标名义惯性矩阵，对所述待规划任务进行所述目标广义关节空间关节出力计算；
S25：将所述计数器加1，重复执行所述获取所述优先级等于所述计数器的值的所述待执行任务作为待规划任务的步骤，直至所述计数器的值大于所述待执行任务的数量。
本实施例实现了从优先级顺序高的所述待执行任务往优先级顺序低的所述待执行任务进行目标广义关节空间关节出力计算，从而可以严格保证优先级高的任务优先执行，提高力控仿人机器人的整体协调性，降低了同步执行的任务之间的冲突，并且可以确保优先级高的任务优先保证执行，提高了力控仿人机器人工作的准确性；每次只计算一个所述待执行任务的所述目标广义关节空间关节出力，从而将不同的任务封闭在自己的层级内进行工作空间控制，降低任务之间的冲突。
对于S21，可以从数据库中获取计数器，也可以将计数器写入实现本申请的程序中。
所述计数器初始化为1，从而可以从第1层优先级对应的所述待执行任务开始进行目标广义关节空间关节出力计算。
对于S22，获取所述优先级等于所述计数器的值的所述待执行任务，将获取的所述待执行任务作为待规划任务。也就是说，待规划任务的优先级的值与所述计数器的值相同。
对于S23，获取所述所述待规划任务的零空间投影矩阵作为目标零空间投影矩阵；获取所述所述待规划任务的名义惯性矩阵作为目标名义惯性矩阵。
零空间投影矩阵，也就是力控仿人机器人的约束点的零空间投射器，也是力控仿人机器人的浮动基到约束点的雅可比矩阵的零空间。
其中，将力控仿人机器人的躯体中心作为基座，在运动过程中力控仿人机器人的躯体中心是运动的，故该基座也称为浮动基。
名义惯性矩阵，是广义关节坐标系里惯性矩阵在机器人工作空间内的表述。
零空间投影矩阵及名义惯性矩阵的具体计算方法在此不做赘述。
被控对象，也就是力控仿人机器人身上的想要控制的部位。比如，被控对象包括但不限于：膝盖。
对于S24，基于受约束的机器人全动力学、第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数，确定工作空间关节出力计算公式。
根据根据所述目标零空间投影矩阵、所述目标名义惯性矩阵、已经完全确定的待规划任务的前一个优先级顺序的所述待执行任务的数据、所述待规划任务的已知数据，确定的工作空间关节出力计算公式的各个参数的具体值，将确定各个参数的具体值代入工作空间关节出力计算公式进行工作空间关节出力计算，将计算得到的工作空间关节出力作为所述待规划任务对应的所述目标广义关节空间关节出力。
对于S25，将所述计数器以用于计算下一个优先级顺序的所述待执行任务的工作空间关节出力，重复执行步骤S22至步骤S25，直至所述计数器的值大于所述待执行任务的数量。当所述计数器的值大于所述待执行任务的数量时，已经完成所有所述待执行任务的工作空间关节出力的计算。
在一个实施例中，上述基于受约束的机器人全动力学、第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数，根据所述目标零空间投影矩阵和所述目标名义惯性矩阵，对所述待规划任务进行所述目标广义关节空间关节出力计算的步骤，包括：
S241：获取从第1层所述优先级至所述待规划任务对应的所述优先级对应的每个所述待规划任务的目标任务雅克比矩阵需求值；
S242：对获取的各个所述目标任务雅克比矩阵需求值进行相加计算，得到任务雅克比矩阵需求总值；
S243：获取与所述待规划任务对应的目标雅可比矩阵和目标位控任务点工作空间加速度；
S244：获取与各个所述待执行任务对应的目标关节角速度；
S245：基于受约束的机器人全动力学、第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数，根据所述任务雅克比矩阵需求总值、所述目标关节角速度、所述目标位控任务点工作空间加速度、所述目标雅可比矩阵、所述目标零空间投影矩阵和所述目标名义惯性矩阵，对所述待规划任务进行所述目标广义关节空间关节出力计算。
本实施例实现了根据根据所述目标零空间投影矩阵、所述目标名义惯性矩阵、已经完全确定的待规划任务的前一个优先级顺序的所述待执行任务的数据、所述待规划任务的已知数据，确定的工作空间关节出力计算公式的各个参数的具体值，将确定各个参数的具体值代入工作空间关节出力计算公式进行工作空间关节出力计算，从而实现了分层任务处理，为对需要同步执行的各个所述待执行任务进行整体协调控制提供了基础；每次只计算一个所述待执行任务的所述目标广义关节空间关节出力，从而将不同的任务封闭在自己的层级内进行工作空间控制，降低任务之间的冲突。
对于S241，可以从数据库中获取从第1层所述优先级至所述待规划任务对应的所述优先级对应的每个所述待规划任务的目标任务雅克比矩阵需求值。
其中，所述待规划任务对应的所述优先级是第k层所述优先级，所述目标任务雅克比矩阵需求值的数量为k个。
对于S242，对获取的各个所述目标任务雅克比矩阵需求值进行相加计算，将相加得到的数据作为任务雅克比矩阵需求总值。
对于S243，因在向量微积分中，雅可比矩阵是一阶偏导数以一定方式排列成的矩阵，其行列式称为雅可比行列式。
可以从数据库中获取力控仿人机器人的浮动基到在机器人工作空间中的与所述待规划任务对应的约束点的雅可比矩阵。
对于S244，获取与各个所述待执行任务对应的关节速度作为目标关节角速度，获取与各个所述待执行任务对应的位控任务点工作空间加速度作为目标位控任务点工作空间加速度。
位控任务点工作空间加速度，是与各个所述待执行任务对应的位控任务点在机器人工作空间的加速度。
位控任务点，采用位控作为控制策略的所述待执行任务的位置坐标。比如，采用力控仿人机器人的手部移动到目标位置，目标位置的位置坐标就是手部操作（所述待执行任务）的位控任务点，在此举例不做具体限定。
可以理解的是，对各个所述待执行任务进行工作空间关节出力计算时，采用相同的目标关节角速度和目标位控任务点工作空间加速度。
与各个所述待执行任务对应的目标关节角速度，是力控仿人机器人的关节角速度的初始值（也就是力控仿人机器人开始执行各个所述待执行任务时的关节角速度）。
对于S245，基于受约束的机器人全动力学、第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数，确定工作空间关节出力计算公式。
根据所述任务雅克比矩阵需求总值、所述目标关节角速度、所述目标位控任务点工作空间加速度、所述目标雅可比矩阵、所述目标零空间投影矩阵和所述目标名义惯性矩阵，确定的工作空间关节出力计算公式的各个参数的具体值，将确定各个参数的具体值代入工作空间关节出力计算公式进行工作空间关节出力计算，将计算得到的工作空间关节出力作为所述待规划任务对应的所述目标广义关节空间关节出力。
在一个实施例中，上述基于受约束的机器人全动力学、第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数，确定第k层所述优先级对应的所述待规划任务对应的所述目标广义关节空间关节出力的计算公式
Figure 286895DEST_PATH_IMAGE029
为：
Figure 931503DEST_PATH_IMAGE030
其中，
Figure 528837DEST_PATH_IMAGE031
是所述目标名义惯性矩阵，
Figure 425249DEST_PATH_IMAGE032
是所述目标位控任务点工作空间加速度，
Figure 657385DEST_PATH_IMAGE033
是所述目标雅可比矩阵，
Figure 738474DEST_PATH_IMAGE034
是所述目标雅可比矩阵的导数，
Figure 557525DEST_PATH_IMAGE035
是所述目标关节角速度，
Figure 257628DEST_PATH_IMAGE036
是所述力控仿人机器人的惯性矩阵的逆计算，
Figure 235948DEST_PATH_IMAGE037
是受约束的机器人全动力学对应的科式力项、离心力项、重力项及动力学方程建立在广义关节空间时的非线性项，
Figure 832146DEST_PATH_IMAGE038
是所述目标零空间投影矩阵的转置计算，
Figure 902608DEST_PATH_IMAGE039
是所述任务雅克比矩阵需求总值。
本实施例基于受约束的机器人全动力学、第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数，确定第k层所述优先级对应的所述待执行任务的工作空间关节出力计算公式，从而实现了分层任务处理，为对需要同步执行的各个所述待执行任务进行整体协调控制提供了基础。
在一个实施例中，上述受约束的机器人全动力学的计算方程为：
Figure 265456DEST_PATH_IMAGE040
其中，M是所述力控仿人机器人的惯性矩阵，
Figure 239228DEST_PATH_IMAGE041
是与各个所述待执行任务对应的目标关节角加速度，
Figure 6327DEST_PATH_IMAGE042
是从第1层所述优先级至第m层所述优先级对应的各个所述待执行任务对应的所述任务雅克比矩阵需求值的总值，
Figure 190183DEST_PATH_IMAGE043
是第k层所述优先级对应的所述待执行任务对应的所述任务雅克比矩阵需求值，m是所述待执行任务的数量；
将受约束的机器人全动力学的计算方程的等号两端均与
Figure 232089DEST_PATH_IMAGE044
相乘，得到第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数的计算公式：
Figure 293323DEST_PATH_IMAGE045
基于第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内，确定
Figure 621537DEST_PATH_IMAGE046
，则得到计算公式：
Figure 902476DEST_PATH_IMAGE047
第k层所述优先级对应的所述待执行任务对应的所述任务雅克比矩阵需求值的计算公式
Figure 748073DEST_PATH_IMAGE048
表述为：
Figure 430858DEST_PATH_IMAGE049
其中，
Figure 929972DEST_PATH_IMAGE050
是所述目标雅可比矩阵的转置计算，
Figure 196743DEST_PATH_IMAGE051
是第k层所述优先级对应的所述待执行任务对应的所述目标广义关节空间关节出力；
根据上述公式确定计算公式：
Figure 580451DEST_PATH_IMAGE052
Figure 242377DEST_PATH_IMAGE053
根据第k层所述优先级对应的所述待执行任务的位置控制关系确定计算公式：
Figure 787759DEST_PATH_IMAGE054
根据上述公式，确定第k层所述优先级对应的所述待规划任务对应的所述目标广义关节空间关节出力的计算公式
Figure 43291DEST_PATH_IMAGE055
为：
Figure 89744DEST_PATH_IMAGE056
本实施例基于受约束的机器人全动力学、第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数推导出第k层所述优先级对应的所述待执行任务的工作空间关节出力计算公式，从而实现了分层任务处理，为对需要同步执行的各个所述待执行任务进行整体协调控制提供了基础。
其中，
Figure 980077DEST_PATH_IMAGE057
是正定满秩矩阵。
在一个实施例中，上述将各个所述目标广义关节空间关节出力作为全身控制规划结果的步骤之后，包括：
S4：基于约束控制模块、位置控制模块、力控制模块及力位混合控制模块，根据所述全身控制规划结果对所述力控仿人机器人进行全身运动控制；
其中，所述约束控制模块用于基于约束点对所述力控仿人机器人与外界环境的接触情况以及所述力控仿人机器人的机械结构设计状况进行约束控制；
所述位置控制模块，用于基于末端位置对所述力控仿人机器人进行跟随位置指令的运动控制；
所述力控制模块，用于基于输出力对所述力控仿人机器人进行运动控制；
所述力位混合控制模块，用于基于所述末端位置和所述输出力对所述力控仿人机器人进行跟随位置及力指令的混合运动控制。
本实施例基于约束控制模块、位置控制模块、力控制模块及力位混合控制模块，根据所述全身控制规划结果对所述力控仿人机器人进行全身运动控制，从而实现了进行运动控制，提高力控仿人机器人的整体运动的协调性，降低了同步执行的任务之间的冲突。
对于S4，对于约束控制模块的约束控制定义为：
Figure 961940DEST_PATH_IMAGE058
Figure 829402DEST_PATH_IMAGE059
其中，
Figure 554912DEST_PATH_IMAGE060
是力控仿人机器人的约束点在机器人工作空间的加速度，
Figure 801217DEST_PATH_IMAGE061
是力控仿人机器人的浮动基到约束点的雅可比矩阵，
Figure 78615DEST_PATH_IMAGE062
是力控仿人机器人的浮动基到约束点的雅可比矩阵的导数，
Figure 541695DEST_PATH_IMAGE063
是与各个所述待执行任务对应的目标关节角加速度，
Figure 70896DEST_PATH_IMAGE064
是与各个所述待执行任务对应的所述目标关节角速度，
Figure 437287DEST_PATH_IMAGE065
是所述目标零空间投影矩阵，
Figure 620006DEST_PATH_IMAGE066
是对
Figure 337426DEST_PATH_IMAGE061
的逆计算，I是方阵。
对于所述位置控制模块的位置控制可以确定：
Figure 903275DEST_PATH_IMAGE067
其中，
Figure 248805DEST_PATH_IMAGE068
是第k层所述优先级的所述待执行任务对应的位控任务点在机器人工作空间的加速度（也就是所述目标位控任务点工作空间加速度），
Figure 8951DEST_PATH_IMAGE069
是力控仿人机器人的浮动基到第k层所述优先级的所述待执行任务对应的位控任务点的雅可比矩阵，
Figure 682509DEST_PATH_IMAGE070
是力控仿人机器人的浮动基到第k层所述优先级的所述待执行任务对应的位控任务点的雅可比矩阵的导数，
Figure 678147DEST_PATH_IMAGE071
是第k层所述优先级的所述待执行任务对应的末端运动轨迹，
Figure 753550DEST_PATH_IMAGE072
是第k层所述优先级的所述待执行任务对应的位控任务点的位置，
Figure 206570DEST_PATH_IMAGE073
是第k层所述优先级的所述待执行任务对应的位控任务点的速度，
Figure 633003DEST_PATH_IMAGE074
是力控仿人机器人的控制系统的阻尼系数，
Figure 901173DEST_PATH_IMAGE075
是力控仿人机器人的刚度。
其中，
Figure 96662DEST_PATH_IMAGE074
和
Figure 198611DEST_PATH_IMAGE075
是正定对角矩阵。
末端运动轨迹，是力控仿人机器人的末端的运动轨迹，也可以称为位置指令。
对于所述力控制模块的力控制可以确定：
Figure 971395DEST_PATH_IMAGE076
其中，
Figure 682736DEST_PATH_IMAGE077
第k层所述优先级对应的所述待规划任务对应的所述目标广义关节空间关节出力，
Figure 732732DEST_PATH_IMAGE078
是力控仿人机器人对第k层所述优先级对应的所述待规划任务的输出力的轨迹。
输出力，是力控仿人机器人对所述待规划任务的输出力。比如，力控仿人机器人的末端的抓取力度。
对于所述力位混合控制模块的力位混合控制可以确定：
Figure 864636DEST_PATH_IMAGE079
其中，
Figure 265662DEST_PATH_IMAGE080
是名义惯性矩阵，
Figure 751001DEST_PATH_IMAGE081
和
Figure 45716DEST_PATH_IMAGE082
分别为对角矩阵；当控制策略为位控（也就是位置控制）时
Figure 722422DEST_PATH_IMAGE083
中的与第k层所述优先级的所述待执行任务对应的向量元素的值为1，否则为0；当控制策略为力控（也就是力控制）时
Figure 610744DEST_PATH_IMAGE082
中的与第k层所述优先级的所述待执行任务对应的向量元素的值为1，否则为0；当控制策略为力位混合控制时，需要通过公式
Figure 165353DEST_PATH_IMAGE084
确定力控仿人机器人的浮动基到第k层所述优先级的所述待执行任务对应的位控任务点的雅可比矩阵及该雅可比矩阵的导数。
Figure 48996DEST_PATH_IMAGE081
和
Figure 663648DEST_PATH_IMAGE082
是力位混合控制选择矩阵。
在一个实施例中，上述待执行任务的分类包括：约束类型和任务类型；
所述分类为所述约束类型的各个所述待执行任务对应的各个所述优先级均高于所述分类为所述任务类型的各个所述待执行任务对应的各个所述优先级；
所述分类为所述约束类型的各个所述待执行任务对应的各个所述优先级的优先级顺序不可以调整。
本实施例通过分类为所述约束类型的各个所述待执行任务对应的各个所述优先级均高于所述分类为所述任务类型的各个所述待执行任务对应的各个所述优先级，以及所述分类为所述约束类型的各个所述待执行任务对应的各个所述优先级的优先级顺序不可以调整，从而确保了力控仿人机器人的安全。
可选的，所述分类为所述任务类型的各个所述待执行任务对应的各个所述优先级的优先级顺序可以调整，从而降低了同步执行的任务之间的冲突，提高力控仿人机器人的整体协调性。
参照图2，本申请还提出了一种力控仿人机器人的全身控制装置，所述装置包括：
待执行任务获取模块100，用于获取多个待执行任务，其中，每个所述待执行任务携带有优先级，各个所述待执行任务是力控仿人机器人同步执行的任务；
目标广义关节空间关节出力计算模块200，用于基于受约束的机器人全动力学、第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数，对每个所述待执行任务进行目标广义关节空间关节出力计算；
全身控制结果确定模块300，用于将各个所述目标广义关节空间关节出力作为全身控制规划结果。
本实施例首先获取多个待执行任务，其中，每个所述待执行任务携带有优先级，各个所述待执行任务是力控仿人机器人同步执行的任务，然后基于受约束的机器人全动力学、第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数，对每个所述待执行任务进行目标广义关节空间关节出力计算，最后将各个所述目标广义关节空间关节出力作为全身控制规划结果，从而实现基于第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数对需要同步执行的各个所述待执行任务进行整体协调控制，提高力控仿人机器人的整体协调性，降低了同步执行的任务之间的冲突，并且可以确保优先级高的任务优先保证执行，提高了力控仿人机器人工作的准确性，低优先级任务尽量满足。
参照图3，本申请实施例中还提供一种计算机设备，该计算机设备可以是服务器，其内部结构可以如图3所示。该计算机设备包括通过系统总线连接的处理器、存储器、网络接口和数据库。其中，该计算机设计的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统、计算机程序和数据库。该内存器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的数据库用于储存力控仿人机器人的全身控制方法等数据。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种力控仿人机器人的全身控制方法。所述力控仿人机器人的全身控制方法，包括：获取多个待执行任务，其中，每个所述待执行任务携带有优先级，各个所述待执行任务是力控仿人机器人同步执行的任务；基于受约束的机器人全动力学、第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数，对每个所述待执行任务进行目标广义关节空间关节出力计算；将各个所述目标广义关节空间关节出力作为全身控制规划结果。
本实施例首先获取多个待执行任务，其中，每个所述待执行任务携带有优先级，各个所述待执行任务是力控仿人机器人同步执行的任务，然后基于受约束的机器人全动力学、第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数，对每个所述待执行任务进行目标广义关节空间关节出力计算，最后将各个所述目标广义关节空间关节出力作为全身控制规划结果，从而实现基于第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数对需要同步执行的各个所述待执行任务进行整体协调控制，提高力控仿人机器人的整体协调性，降低了同步执行的任务之间的冲突，并且可以确保优先级高的任务优先保证执行，提高了力控仿人机器人工作的准确性，低优先级任务尽量满足。
本申请一实施例还提供一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现一种力控仿人机器人的全身控制方法，包括步骤：获取多个待执行任务，其中，每个所述待执行任务携带有优先级，各个所述待执行任务是力控仿人机器人同步执行的任务；基于受约束的机器人全动力学、第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数，对每个所述待执行任务进行目标广义关节空间关节出力计算；将各个所述目标广义关节空间关节出力作为全身控制规划结果。
上述执行的力控仿人机器人的全身控制方法，首先获取多个待执行任务，其中，每个所述待执行任务携带有优先级，各个所述待执行任务是力控仿人机器人同步执行的任务，然后基于受约束的机器人全动力学、第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数，对每个所述待执行任务进行目标广义关节空间关节出力计算，最后将各个所述目标广义关节空间关节出力作为全身控制规划结果，从而实现基于第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数对需要同步执行的各个所述待执行任务进行整体协调控制，提高力控仿人机器人的整体协调性，降低了同步执行的任务之间的冲突，并且可以确保优先级高的任务优先保证执行，提高了力控仿人机器人工作的准确性，低优先级任务尽量满足。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的和实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可以包括只读存储器（ROM）、可编程ROM（PROM）、电可编程ROM（EPROM）、电可擦除可编程ROM（EEPROM）或闪存。易失性存储器可包括随机存取存储器（RAM）或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM（SRAM）、动态RAM（DRAM）、同步DRAM（SDRAM）、双速据率SDRAM（SSRSDRAM）、增强型SDRAM（ESDRAM）、同步链路（Synchlink）DRAM（SLDRAM）、存储器总线（Rambus）直接RAM（RDRAM）、直接存储器总线动态RAM（DRDRAM）、以及存储器总线动态RAM（RDRAM）等。
需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、装置、物品或者方法不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、装置、物品或者方法所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、装置、物品或者方法中还存在另外的相同要素。
以上所述仅为本申请的优选实施例，并非因此限制本申请的专利范围，凡是利用本申请说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本申请的专利保护范围内。

Claims (10)

1.一种力控仿人机器人的全身控制方法，其特征在于，所述方法包括：
获取多个待执行任务，其中，每个所述待执行任务携带有优先级，各个所述待执行任务是力控仿人机器人同步执行的任务；
基于受约束的机器人全动力学、第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数，对每个所述待执行任务进行目标广义关节空间关节出力计算；
将各个所述目标广义关节空间关节出力作为全身控制规划结果。
2.根据权利要求1所述的力控仿人机器人的全身控制方法，其特征在于，所述基于受约束的机器人全动力学、第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数，对每个所述待执行任务进行目标广义关节空间关节出力计算的步骤，包括：
获取计数器，其中，所述计数器初始化为1；
获取所述优先级等于所述计数器的值的所述待执行任务作为待规划任务；
获取所述待规划任务的目标零空间投影矩阵及目标名义惯性矩阵，其中，所述目标零空间投影矩阵是被控对象的雅克比矩阵的零空间投影矩阵；
基于受约束的机器人全动力学、第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数，根据所述目标零空间投影矩阵和所述目标名义惯性矩阵，对所述待规划任务进行所述目标广义关节空间关节出力计算；
将所述计数器加1，重复执行所述获取所述优先级等于所述计数器的值的所述待执行任务作为待规划任务的步骤，直至所述计数器的值大于所述待执行任务的数量。
3.根据权利要求2所述的力控仿人机器人的全身控制方法，其特征在于，所述基于受约束的机器人全动力学、第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数，根据所述目标零空间投影矩阵和所述目标名义惯性矩阵，对所述待规划任务进行所述目标广义关节空间关节出力计算的步骤，包括：
获取从第1层所述优先级至所述待规划任务对应的所述优先级对应的每个所述待规划任务的目标任务雅克比矩阵需求值；
对获取的各个所述目标任务雅克比矩阵需求值进行相加计算，得到任务雅克比矩阵需求总值；
获取与所述待规划任务对应的目标雅可比矩阵和目标位控任务点工作空间加速度；
获取与各个所述待执行任务对应的目标关节角速度；
基于受约束的机器人全动力学、第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数，根据所述任务雅克比矩阵需求总值、所述目标关节角速度、所述目标位控任务点工作空间加速度、所述目标雅可比矩阵、所述目标零空间投影矩阵和所述目标名义惯性矩阵，对所述待规划任务进行所述目标广义关节空间关节出力计算。
4.根据权利要求3所述的力控仿人机器人的全身控制方法，其特征在于，基于受约束的机器人全动力学、第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数，确定第k层所述优先级对应的所述待规划任务对应的所述目标广义关节空间关节出力的计算公式
Figure 914159DEST_PATH_IMAGE001
为：
Figure 6880DEST_PATH_IMAGE002
其中，
Figure 151553DEST_PATH_IMAGE003
是所述目标名义惯性矩阵，
Figure 530582DEST_PATH_IMAGE004
是所述目标位控任务点工作空间加速度，
Figure 626452DEST_PATH_IMAGE005
是所述目标雅可比矩阵，
Figure 522863DEST_PATH_IMAGE006
是所述目标雅可比矩阵的导数，
Figure 381098DEST_PATH_IMAGE007
是所述目标关节角速度，
Figure 337553DEST_PATH_IMAGE008
是所述力控仿人机器人的惯性矩阵的逆计算，
Figure 422183DEST_PATH_IMAGE009
是受约束的机器人全动力学对应的科式力项、离心力项、重力项及动力学方程建立在广义关节空间时的非线性项，
Figure 981341DEST_PATH_IMAGE010
是所述目标零空间投影矩阵的转置计算，
Figure 67983DEST_PATH_IMAGE011
是所述任务雅克比矩阵需求总值。
5.根据权利要求4所述的力控仿人机器人的全身控制方法，其特征在于，受约束的机器人全动力学的计算方程为：
Figure 195339DEST_PATH_IMAGE012
其中，M是所述力控仿人机器人的惯性矩阵，
Figure 891900DEST_PATH_IMAGE013
是与各个所述待执行任务对应的目标关节角加速度，
Figure 864535DEST_PATH_IMAGE014
是从第1层所述优先级至第m层所述优先级对应的各个所述待执行任务对应的所述任务雅克比矩阵需求值的总值，
Figure 572728DEST_PATH_IMAGE015
是第k层所述优先级对应的所述待执行任务对应的所述任务雅克比矩阵需求值，m是所述待执行任务的数量；
将受约束的机器人全动力学的计算方程的等号两端均与
Figure 995619DEST_PATH_IMAGE016
相乘，得到第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数的计算公式：
Figure 553377DEST_PATH_IMAGE017
基于第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内，确定
Figure 329703DEST_PATH_IMAGE018
，则得到计算公式：
Figure 157982DEST_PATH_IMAGE019
第k层所述优先级对应的所述待执行任务对应的所述任务雅克比矩阵需求值的计算公式
Figure 486195DEST_PATH_IMAGE020
表述为：
Figure 767135DEST_PATH_IMAGE021
其中，
Figure 134704DEST_PATH_IMAGE022
是所述目标雅可比矩阵的转置计算，
Figure 676543DEST_PATH_IMAGE023
是第k层所述优先级对应的所述待执行任务对应的所述目标广义关节空间关节出力；
根据上述公式确定计算公式：
Figure 316603DEST_PATH_IMAGE024
Figure 84839DEST_PATH_IMAGE025
根据第k层所述优先级对应的所述待执行任务的位置控制关系确定计算公式：
Figure 62022DEST_PATH_IMAGE026
根据上述公式，确定第k层所述优先级对应的所述待规划任务对应的所述目标广义关节空间关节出力的计算公式
Figure 864893DEST_PATH_IMAGE027
为：
Figure 174390DEST_PATH_IMAGE028
。
6.根据权利要求1所述的力控仿人机器人的全身控制方法，其特征在于，所述将各个所述目标广义关节空间关节出力作为全身控制规划结果的步骤之后，包括：
基于约束控制模块、位置控制模块、力控制模块及力位混合控制模块，根据所述全身控制规划结果对所述力控仿人机器人进行全身运动控制；
其中，所述约束控制模块用于基于约束点对所述力控仿人机器人与外界环境的接触情况以及所述力控仿人机器人的机械结构设计状况进行约束控制；
所述位置控制模块，用于基于末端位置对所述力控仿人机器人进行跟随位置指令的运动控制；
所述力控制模块，用于基于输出力对所述力控仿人机器人进行跟随力指令的运动控制；
所述力位混合控制模块，用于基于所述末端位置和所述输出力对所述力控仿人机器人进行跟随位置及力指令的混合运动控制。
7.根据权利要求1所述的力控仿人机器人的全身控制方法，其特征在于，所述待执行任务的分类包括：约束类型和任务类型；
所述分类为所述约束类型的各个所述待执行任务对应的各个所述优先级均高于所述分类为所述任务类型的各个所述待执行任务对应的各个所述优先级；
所述分类为所述约束类型的各个所述待执行任务对应的各个所述优先级的优先级顺序不可以调整。
8.一种力控仿人机器人的全身控制装置，其特征在于，所述装置包括：
待执行任务获取模块，用于获取多个待执行任务，其中，每个所述待执行任务携带有优先级，各个所述待执行任务是力控仿人机器人同步执行的任务；
目标广义关节空间关节出力计算模块，用于基于受约束的机器人全动力学、第k层所述优先级的任务雅克比矩阵需要在第k-1层所述优先级的所述任务雅克比矩阵的零空间内、第k层所述优先级的所述任务雅克比矩阵是与第k-1层所述优先级的所述任务雅克比矩阵相关的函数，对每个所述待执行任务进行目标广义关节空间关节出力计算；
全身控制结果确定模块，用于将各个所述目标广义关节空间关节出力作为全身控制规划结果。
9.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至7中任一项所述方法的步骤。
10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至7中任一项所述的方法的步骤。
CN202111302908.4A 2021-11-05 2021-11-05 力控仿人机器人的全身控制方法、装置、设备及介质 Active CN113733106B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111302908.4A CN113733106B (zh) 2021-11-05 2021-11-05 力控仿人机器人的全身控制方法、装置、设备及介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111302908.4A CN113733106B (zh) 2021-11-05 2021-11-05 力控仿人机器人的全身控制方法、装置、设备及介质

Publications (2)

Publication Number Publication Date
CN113733106A true CN113733106A (zh) 2021-12-03
CN113733106B CN113733106B (zh) 2022-04-15

Family

ID=78727421

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111302908.4A Active CN113733106B (zh) 2021-11-05 2021-11-05 力控仿人机器人的全身控制方法、装置、设备及介质

Country Status (1)

Country Link
CN (1) CN113733106B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114326769A (zh) * 2021-12-28 2022-04-12 深圳市优必选科技股份有限公司 机器人运动矫正方法及装置、机器人控制设备和存储介质
CN114469642A (zh) * 2022-01-20 2022-05-13 深圳华鹊景医疗科技有限公司 康复机器人控制方法、装置及康复机器人
CN116945124A (zh) * 2022-04-20 2023-10-27 深圳市普渡科技有限公司 一种机器人及其控制方法、设备及介质

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2003334785A (ja) * 2002-03-15 2003-11-25 Sony Corp ロボットの行動制御システム及び行動制御方法、並びにロボット装置
US20050165507A1 (en) * 2002-10-10 2005-07-28 Satoru Shimizu Robot device operation control device and operation control method
CN104407611A (zh) * 2014-09-30 2015-03-11 同济大学 一种仿人机器人稳定行走的控制方法
CN111037572A (zh) * 2019-12-31 2020-04-21 江苏海洋大学 一种机器人的迈步优先顺序控制方法
CN111538234A (zh) * 2020-07-08 2020-08-14 深圳市优必选科技股份有限公司 任务分层控制方法、装置、机器人和可读存储介质
CN112975938A (zh) * 2019-12-12 2021-06-18 中国科学院沈阳自动化研究所 一种基于零空间的机械臂速度层轨迹规划方法
CN113246120A (zh) * 2021-03-08 2021-08-13 山东大学 一种液压四足双臂机器人的遥操作控制方法
CN113290559A (zh) * 2021-05-26 2021-08-24 深圳市优必选科技股份有限公司 机器人平衡控制方法、装置、机器人控制设备及存储介质

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2003334785A (ja) * 2002-03-15 2003-11-25 Sony Corp ロボットの行動制御システム及び行動制御方法、並びにロボット装置
US20050165507A1 (en) * 2002-10-10 2005-07-28 Satoru Shimizu Robot device operation control device and operation control method
CN104407611A (zh) * 2014-09-30 2015-03-11 同济大学 一种仿人机器人稳定行走的控制方法
CN112975938A (zh) * 2019-12-12 2021-06-18 中国科学院沈阳自动化研究所 一种基于零空间的机械臂速度层轨迹规划方法
CN111037572A (zh) * 2019-12-31 2020-04-21 江苏海洋大学 一种机器人的迈步优先顺序控制方法
CN111538234A (zh) * 2020-07-08 2020-08-14 深圳市优必选科技股份有限公司 任务分层控制方法、装置、机器人和可读存储介质
CN113246120A (zh) * 2021-03-08 2021-08-13 山东大学 一种液压四足双臂机器人的遥操作控制方法
CN113290559A (zh) * 2021-05-26 2021-08-24 深圳市优必选科技股份有限公司 机器人平衡控制方法、装置、机器人控制设备及存储介质

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114326769A (zh) * 2021-12-28 2022-04-12 深圳市优必选科技股份有限公司 机器人运动矫正方法及装置、机器人控制设备和存储介质
CN114326769B (zh) * 2021-12-28 2024-03-29 深圳市优必选科技股份有限公司 机器人运动矫正方法及装置、机器人控制设备和存储介质
CN114469642A (zh) * 2022-01-20 2022-05-13 深圳华鹊景医疗科技有限公司 康复机器人控制方法、装置及康复机器人
CN114469642B (zh) * 2022-01-20 2024-05-10 深圳华鹊景医疗科技有限公司 康复机器人控制方法、装置及康复机器人
CN116945124A (zh) * 2022-04-20 2023-10-27 深圳市普渡科技有限公司 一种机器人及其控制方法、设备及介质

Also Published As

Publication number Publication date
CN113733106B (zh) 2022-04-15

Similar Documents

Publication Publication Date Title
CN113733106B (zh) 力控仿人机器人的全身控制方法、装置、设备及介质
Caccavale et al. Cooperative manipulation
US9764469B1 (en) Generating robotic trajectories with motion harmonics
De Lasa et al. Prioritized optimization for task-space control
Chen et al. Optimal, fault-tolerant mappings to achieve secondary goals without compromising primary performance
Rokbani et al. IK-PSO, PSO inverse kinematics solver with application to biped gait generation
Zarafshan et al. Manipulation control of a space robot with flexible solar panels
Dariush et al. Constrained closed loop inverse kinematics
CN114952821A (zh) 机器人运动控制方法、机器人及系统
Bajrami et al. Kinematics and dynamics modelling of the biped robot
Fratu et al. Using the redundant inverse kinematics system for collision avoidance
Eftekhari et al. Neuro-fuzzy adaptive control of a revolute stewart platform carrying payloads of unknown inertia
CN113119102B (zh) 基于浮动基飞轮倒立摆的仿人机器人建模方法和装置
CN116834016A (zh) 一种双臂可重构机器人协调控制方法、系统、设备及介质
Mishra et al. Dynamic modelling of an elephant trunk like flexible bionic manipulator
Gienger et al. Exploiting task intervals for whole body robot control
Best Position and stiffness control of inflatable robotic links using rotary pneumatic actuation
Noshadi et al. Performance analysis of the computed torque based active force control for a planar parallel manipulator
Jishnu et al. Design of neural network-based adaptive inverse dynamics controller for motion control of stewart platform
JP2023547974A (ja) ロボットを制御するための方法及びコンピュータプログラム
Pajak et al. Planning of a point to point collision-free trajectory for mobile manipulators
Jesus et al. Singularity-free inverse kinematics with joint prioritization for manipulators
Ataei et al. An adaptive impedance control algorithm; application in exoskeleton robot
Galicki Collision‐free control of robotic manipulators in the task space
Li et al. Iterative calculation method for constraint motion by extended newton-euler method and application for forward dynamics

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
CB03 Change of inventor or designer information

Inventor after: Ju Xiaozhu

Inventor after: Xiong Youjun

Inventor before: Ju Xiaozhu

Inventor before: Zhao Mingguo

Inventor before: Xiong Youjun

CB03 Change of inventor or designer information