CN113070881B - 机器人运动控制方法、装置和机器人 - Google Patents

机器人运动控制方法、装置和机器人 Download PDF

Info

Publication number
CN113070881B
CN113070881B CN202110360660.0A CN202110360660A CN113070881B CN 113070881 B CN113070881 B CN 113070881B CN 202110360660 A CN202110360660 A CN 202110360660A CN 113070881 B CN113070881 B CN 113070881B
Authority
CN
China
Prior art keywords
joint
angle
robot
avoidance
task
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
CN202110360660.0A
Other languages
English (en)
Other versions
CN113070881A (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.)
Shenzhen Ubtech Technology Co ltd
Original Assignee
Shenzhen Ubtech Technology Co ltd
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 Shenzhen Ubtech Technology Co ltd filed Critical Shenzhen Ubtech Technology Co ltd
Priority to CN202110360660.0A priority Critical patent/CN113070881B/zh
Publication of CN113070881A publication Critical patent/CN113070881A/zh
Priority to PCT/CN2021/131890 priority patent/WO2022205940A1/zh
Application granted granted Critical
Publication of CN113070881B publication Critical patent/CN113070881B/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/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
    • 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/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/1612—Programme controls characterised by the hand, wrist, grip control

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Automation & Control Theory (AREA)
  • Health & Medical Sciences (AREA)
  • General Health & Medical Sciences (AREA)
  • Orthopedic Medicine & Surgery (AREA)
  • Manipulator (AREA)

Abstract

本申请实施例提供一种机器人运动控制方法、装置和机器人，该方法包括：根据任务轨迹信息和机器人实时反馈的关节角相关信息计算对应控制周期的期望运动状态信息；利用各个关节角求解对应控制周期的虚拟斥力并根据虚拟斥力计算避限位角相关信息；基于对应控制周期的期望运动状态信息和避限位角相关信息对优化模型求解，得到满足关节平滑避限位且用于执行任务的最优关节角加速度或角速度以用于在对应控制周期控制该机器人运动。本申请可根据每个控制周期的机器人状态，平滑地产生远离关节限位的行为，无需预先规划避撞轨迹，并在自由度满足的情况下，仍能保证规划任务的顺利执行，使得机器人安全稳定地运行。

Description

机器人运动控制方法、装置和机器人
技术领域
本申请涉及机器人控制技术领域，尤其涉及一种机器人运动控制方法、装置和机器人。
背景技术
随着机器人技术的发展，机器人正逐渐从结构化的环境中走出来，走进动态多变的非结构化环境中。在机器人执行多种复杂任务(机械臂运动跟随、足式机器人质心动量控制等)以及受到各种外界干扰(躲避障碍物撞击、柔顺响应外力拖拽等)时，均有可能产生关节超限位的问题。为此，在机器人在执行任务过程中，如何保证当关节角度接近关节限位时，能够平稳地远离关节限位，是实现机器人在非结构化环境中安全稳定运行的关键。
发明内容
本申请实施例提供一种机器人运动控制方法、装置和机器人，可以实现平稳地远离关节限位，且在自由度足够的情况下，仍保证原有任务能够顺利执行，从而使机器人安全稳定地运行。
本申请的实施例提供一种机器人运动控制方法，包括：
根据规划的任务轨迹信息和机器人在对应控制周期反馈的关节角相关信息计算机器人各个关节在对应控制周期的用于执行任务的期望运动状态信息，所述关节角相关信息包括各个关节角；
基于反馈的各个关节角求解对应控制周期的各个关节远离限位的虚拟斥力，并根据所述虚拟斥力计算各个关节的避限位角相关信息；
基于对应控制周期的所述期望运动状态信息和所述避限位角相关信息对关节平滑避限位优化模型进行最优求解，以得到满足关节平滑避限位且用于执行任务的关节角加速度或关节角速度；
利用所述关节角加速度或关节角速度生成对应控制周期的关节控制指令，各个控制周期的所述关节控制指令用于控制该机器人运动以完成所述规划的任务。
在本申请的一些实施例中，根据规划的任务轨迹信息和机器人在对应控制周期反馈的关节角相关信息计算机器人各个关节在对应控制周期的用于执行任务的期望运动状态信息，包括：
根据规划的任务轨迹信息获得机器人在对应控制周期的任务空间的参考运动状态信息；
获取机器人在对应控制周期反馈的关节角相关信息，并根据所述关节角相关信息计算机器人各个关节在任务空间的实际运动状态信息；
根据在任务空间的所述参考运动状态信息和所述实际运动状态信息基于预设关节控制律计算机器人各个关节在对应控制周期的用于执行任务的期望运动状态信息。
在本申请的一些实施例中，所述虚拟斥力的表达式基于关节的关节角和关节限位角范围预先构建，包括：
根据关节限位角范围确定所述关节的角度安全变化的上边界和下边界，以得到所述关节的角度安全范围；
当实际反馈的关节角位于所述关节的角度安全范围内，则所述虚拟斥力为0；当实际反馈的关节角位于所述关节的角度安全范围外，则在所述关节角越接近所述关节限位角范围的上限或下限的情况下，所述虚拟斥力越大且方向远离关节限位方向。
在本申请的一些实施例中，所述避限位角相关信息包括关节的避限位角，所述关节的避限位角与虚拟斥力的关系式基于关节导纳控制构建，所述关系式如下：
Figure BDA0003005422570000031
其中，ΔqJAL表示关节的避限位角；τJAL表示虚拟斥力；Mq、D和K分别为基于关节导纳控制的虚拟惯量矩阵、虚拟阻尼矩阵和虚拟刚度矩阵；s表示微分算子。
在本申请的一些实施例中，所述关节平滑避限位优化模型经过预先构建，包括：
构建关节的关节角相关信息到期望运动状态信息的映射方程；
基于关节的避限位角相关信息构建关节角速度或关节角加速度变化的表达式；
基于所述映射方程和所述关节角速度或关节角加速度变化的表达式的联立方程构建平滑避限位优化模型，其中，所述平滑避限位优化模型的优化变量与关节角速度或关节角加速度相关，基于该机器人执行任务过程中需满足的等式约束和/或不等式约束构建所述平滑避限位优化模型的约束条件。
在本申请的一些实施例中，所述基于关节的避限位角相关信息构建关节角速度或关节角加速度变化的表达式，包括：
基于关节的所述角度安全范围构建与关节角相关的激活因子的函数表达式，所述函数表达式用于求解所述关节在对应控制周期的激活因子；
以所述激活因子为对角元构建用于反映关节角动态变化的投影矩阵；
根据所述避限位角相关信息和所述投影矩阵构建关节角速度或关节角加速度变化的表达式。
在本申请的一些实施例中，所述避限位角相关信息还包括关节的避限位角加速度，所述期望运动状态信息包括关节的期望加速度；
若该机器人的关节类型为力控型关节或力位混合型关节，则基于所述映射方程和所述关节角加速度变化的表达式的联立方程来构建基于与关节角加速度有关的优化变量的平滑避限位优化模型；
所述映射方程为：
Figure BDA0003005422570000041
其中，J为雅克比矩阵；
Figure BDA0003005422570000042
表示待求解的关节角加速度；
Figure BDA0003005422570000043
表示关节在任务空间的期望加速度；
Figure BDA0003005422570000044
为雅克比矩阵J的导数；
Figure BDA0003005422570000045
为实际反馈的关节角速度；
以及，所述关节角加速度变化的表达式为：
Figure BDA0003005422570000046
其中，
Figure BDA0003005422570000047
表示关节的避限位角加速度；I为单位矩阵；Λ为投影矩阵；ξ1表示关节角加速度的增量。
在本申请的一些实施例中，所述平滑避限位优化模型采用凸二次规划优化函数构建，所述凸二次规划优化函数的表达式为：
Figure BDA0003005422570000051
Figure BDA0003005422570000052
其中，C和d均为不等式约束的系数矩阵，根据该机器人执行任务过程中需满足的等式约束和/或不等式约束进行求解；v1表示所述等式约束和/或不等式约束的第一松弛变量。
在本申请的一些实施例中，所述避限位角相关信息还包括关节的避限位角速度，所述期望运动状态信息包括关节的期望速度；
若该机器人的关节类型为位控型关节，则基于所述映射方程和所述关节角速度变化的表达式的联立方程构建基于与关节角速度有关的优化变量的平滑避限位优化模型；
所述映射方程为：
Figure BDA0003005422570000053
其中，J为雅克比矩阵；
Figure BDA0003005422570000054
表示待求解的关节角速度；
Figure BDA0003005422570000055
表示关节在任务空间的期望速度；
以及，所述关节角速度变化的表达式为：
Figure BDA0003005422570000056
其中，
Figure BDA0003005422570000057
表示关节的避限位角速度；I为单位矩阵；Λ为投影矩阵；ξ2表示关节的角速度的增量。
在本申请的一些实施例中，所述平滑避限位优化模型采用凸二次规划优化函数构建，所述凸二次规划优化函数的表达式为：
Figure BDA0003005422570000061
其中，C和d均为不等式约束的系数矩阵，根据该机器人执行任务过程中需满足的等式约束和/或不等式约束进行求解；v2表示所述等式约束和/或不等式约束的第二松弛变量。
本申请实施例还提供一种机器人运动控制装置，包括：
期望值计算模块，用于根据规划的任务轨迹信息和机器人在对应控制周期反馈的关节角相关信息计算机器人各个关节在对应控制周期的用于执行任务的期望运动状态信息，所述关节角相关信息包括各个关节角；
避限位角信息计算模块，用于基于反馈的各个关节角求解对应控制周期的各个关节远离限位的虚拟斥力，并根据所述虚拟斥力计算各个关节的避限位角相关信息；
优化模型求解模块，用于基于对应控制周期的所述期望运动状态信息和所述避限位角相关信息对关节平滑避限位优化模型进行最优求解，以得到满足关节平滑避限位且用于执行任务的关节角加速度或关节角速度；
控制指令执行模块，用于利用所述关节角加速度或关节角速度生成对应控制周期的关节控制指令，各个控制周期的所述关节控制指令用于控制该机器人运动以完成所述规划的任务。
本申请的实施例还提供一种机器人，所述机器人包括处理器和存储器，所述存储器存储有计算机程序，所述处理器用于执行所述计算机程序以实施上述的机器人运动控制方法。
本申请的实施例还提供一种可读存储介质，其存储有计算机程序，所述计算机程序在处理器上运行时，实施上述的机器人运动控制方法。
本申请的实施例具有如下有益效果：
本申请的实施例的机器人运动控制方法通过构建关节在远离限位方向上产生的虚拟斥力并结合关节导纳控制来求解避限位角相关信息，还通过构建平滑避限位的优化问题，优化求解得到能关节平滑避限位的各个关节的控制指令来控制机器人执行规划任务，可以实现机器人平滑地产生远离关节限位的行为，并在自由度满足的情况下，仍能保证原规划任务的顺利执行，使得机器人安全稳定地运行。该方法无需规划避撞轨迹，便可使机器人实时产生反射式的避撞行为，使得从软件算法层面保证机器人安全稳定运行等。
附图说明
为了更清楚地说明本申请实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本申请的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。
图1示出了本申请实施例的机器人运动控制方法的流程示意图；
图2示出了本申请实施例的机器人运动控制方法的关节平滑避限位优化模型构建的流程示意图；
图3示出了本申请实施例的机器人运动控制方法的期望运动状态信息获取的流程示意图；
图4示出了本申请实施例的机器人运动控制方法的关节角速度或关节角加速度变化的表达式构建的流程示意图；
图5示出了本申请实施例的机器人运动控制装置的结构示意图。
具体实施方式
下面将结合本申请实施例中附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。
通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。
对于机器人如何远离关节限位的问题，现有的方法主要有两类：(1)通过运动规划的方法，预先规划出使机器人远离关节限位的运动轨迹，然后将所得各关节运动轨迹的指令发送给驱动器从而控制各关节电机实现远离关节限位的运动。(2)通过逆运动学，优化求解任务空间期望运动下的各关节运动，当关节空间的期望转角即将与关节限位发生碰撞时，对运动进行截断。
但现有方法存在以下不足：对于第一种运动规划的方法，当机器人自由度较多、外界干扰(例如未来一段时间内障碍物的撞击轨迹或外力拖拽等扰动的大小和方向)难以预测时，这种运动规划的方法将难以实时在线地产生新的避限位轨迹，此时难以采用此类方法进行避限位。而对于第二种运动截断的方法，虽然能够防止碰撞的发生，但会造成机器人各关节加速度、速度和位置指令的突变，影响机器人运行平稳性和任务执行效果，甚至会造成机器人剧烈振动或急停等现象。
对此，本申请实施例提出一种机器人运动控制方法，该方法无需预先规划避撞轨迹，而能够平滑地产生远离关节限位的行为，并在自由度满足的情况下，优化求解得到的各关节控制指令，仍能保证任务的顺利执行，使得机器人安全稳定地运行。下面结合具体的实施例来对该方法进行说明。
实施例1
请参照图1，本实施例提出一种机器人运动控制方法，可适用于具有位控型、力控型关节以及力位混合控制型等关节类型的机器人，该方法是一种通用的运动控制方法，适用于工业、服务和特种等多种不同领域的机械臂、仿人机器人、多足机器人等的运动控制。
如图1所示，下面对该机器人运动控制方法进行详细说明。
步骤S10，根据规划的任务轨迹信息和机器人在对应控制周期反馈的关节角相关信息计算该机器人各个关节在对应控制周期的用于执行任务的期望运动状态信息。
其中，上述的任务轨迹通常由预先规划得到，作为机器人执行期望任务时的在任务空间的运动参考轨迹，该任务轨迹可以指包含多个控制周期在内的一段时间的参考轨迹，也可以是对应控制周期的参考轨迹，在此并不作限定。例如，若获得的是一段时间的参考轨迹信息，则在计算每个控制周期的期望运动状态信息时，可从中取出对应控制周期的参考信息进行计算。示范性地，该任务轨迹信息可包括但不限于为机器人执行规划任务的参考位置、参考速度以及参考加速度等。
上述，关节角相关信息主要是指该机器人在每个控制周期内实时反馈在关节空间的实际关节角、关节角速度和/或关节角加速度等信息。例如，可通过如角度编码器等传感器来采集得到。当然，除了实时反馈的关节角相关信息外，机器人还可根据需求来获得其他的状态反馈信息，在此并不作限定。例如，可通过安装在机械臂末端处的力传感器等力传感器或力矩传感器等来测量机器人末端实际受到的外力等，又或者，可通过如惯性测量单元(IMU)等来测量得到机器人的躯干姿态等。
对于上述步骤S10，示范性地，利用机器人实时反馈的实际状态信息和规划的参考值，结合反馈控制技术可计算获得机器人各个关节在任务空间的期望值。值得注意的是，该实时反馈得到的实际状态信息为机器人在关节空间的相关信息，在与参考值作运算前，需要将其转换为在任务空间对应的信息。通过考虑任务空间中机器人的实际状态与规划参考值之间的偏差，可以保证机器人在任务执行时的精度。
步骤S20，基于反馈的各个关节角求解对应控制周期的各个关节远离限位的虚拟斥力，并根据所述虚拟斥力计算各个关节的避限位角相关信息。
其中，该虚拟斥力是实际的关节角度在靠近关节限位角时定义的一种虚拟的用于排斥或远离关节限位的力或力矩，其方向为远离关节限位的方向。可以理解，该虚拟斥力可以指虚拟斥力，也可以指虚拟斥力矩，例如，对于平动型关节，由于无法产生力矩，则构建的是虚拟斥力；而对于转动型关节，由于可产生转动力矩，此时构建的为虚拟斥力矩。
例如，可通过预先构建与关节角、关节限位角度范围有关的虚拟斥力。其中，所述的关节限位角度范围是指该关节能够达到的最大角度范围，由角度上限和角度下限构成该限位角度范围，其大小往往取决于机器人实际机械结构设计。另外，考虑到关节角在到达限位角度的上限或下限之前通常有一段安全的转角范围，此时通常不会产生虚拟斥力，故还设定了关节的角度安全范围，即在该角度安全范围内关节可以任意转角而不会出现障碍。
示范性地，在构建该虚拟斥力时，应当满足：当实际反馈的关节角位于关节的角度安全范围内，则该虚拟斥力即为0；当实际反馈的关节角位于关节的角度安全范围外，则在所述关节角越接近关节限位角范围的上限或下限的情况下，此时产生的虚拟斥力越大且方向为远离关节限位方向。
其中，上述的避限位角相关信息可包括关节的避限位角、避限位角速度和/或避限位角加速度等。所述的避限位角是指远离关节限位的关节角，同理，避限位角速度和避限位角加速度分别是指远离关节限位的关节角速度和远离关节限位的关节角加速度。
为求解出关节的避限位角相关信息，可预先构建出虚拟斥力与避限位角度之间的关系式，在一种实施方式中，可基于关节导纳控制技术等来构建该虚拟斥力与关节角、关节的避限位角度之间的关系式，当然还可以采用其他方式构建，在此并不限定。通常地，由于关节角越接近关节限位时，虚拟斥力越大，进而计算得到的避限位角速度和避限位角加速度也将越大。
对于上述步骤S20，示范性地，获取到对应控制周期实时反馈的各个关节角后，将其代入虚拟斥力的表达式即可计算得到该对应控制周期内各个关节的虚拟斥力，进而，利用虚拟斥力与避限位角度之间的关系式来计算出对应控制周期内各个关节的避限位角相关信息，如避限位角速度和避限位角加速度等。通过构建虚拟斥力来进一步求解各个关节在远离关节限位且满足任务需求的关节角速度和/或关节角加速度，从而实现运动过程中的平滑避限位功能。
步骤S30，基于对应控制周期的所述期望运动状态信息和所述避限位角相关信息对关节平滑避限位优化模型进行最优求解，以得到满足关节平滑避限位且用于执行任务的关节角加速度或关节角速度。
其中，对于上述的关节平滑避限位优化模型，本实施例将通过预先构建一个关节平滑避限位优化模型，即构建一个平滑避限位优化问题，该优化模型采用与关节角加速度或关节角速度有关的参量作为优化变量，例如，该关节角加速度或关节角速度的增量(也可称调整量或变化增量)等。
进而，通过对该优化模型进行最优求解，并基于最优解得到最优关节角速度或关节角加速度，基于该最优关节角速度或关节角加速度得到的关节控制指令，既满足平滑地产生远离关节限位，又在自由度满足的情况下，可以保证机器人顺利执行规划的任务。
示范性地，如图2所示，该平滑避限位优化模型的预先构建，包括：
步骤S310，构建关节的关节角相关信息到期望运动状态信息的映射方程。
示范性地，根据机器人关节与末端速度之间的雅克比矩阵，可构建出实际反馈的关节角相关信息和任务空间的期望运动状态信息的映射方程。例如，对于力控型或力位混合型等关节，可构建基于加速度层面的映射方程；而对于力控型关节，由于不需利用加速度计算关节力矩，可构建出基于速度层面的映射方程。
步骤S320，基于关节的避限位角相关信息构建关节角速度或关节角加速度变化的表达式。
示范性地，可预先构建关节角速度或关节角加速度与避限位角信息之间的关节式，进而方便求解各个控制周期的关节角速度或关节角加速度变化。而为更好地体现机器人在执行任务过程中的自由度变化情况，本实施例将通过构建一个投影矩阵来反映关节自由度的变化情况，而该投影矩阵将用于优化模型的构建。其中，对于该投影矩阵的构建，例如，可基于关节的角度安全范围构建与关节角度相关的激活因子，进而利用该激活因子作为对角元来构建该投影矩阵。上述的步骤S310和S320并不限定执行的顺序，两者可以是同时执行，也可以按照相应的先后顺序执行。
步骤S330，基于所述映射方程和所述关节角速度或关节角加速度变化的表达式的联立方程构建基于与关节角速度或关节角加速度有关的参量作为优化变量的关节平滑避限位优化模型。
示范性地，将映射方程和关节角速度或角加速度变化的表达式进行联立，于是可得到两者的联立方程。进而，根据联立方程可构建出一个关于实现关节平滑避限位的优化问题。例如，该平滑避限位优化模型将以该关节角速度或关节角加速度的增量作为优化变量，当然，也可以有其他的变量，在此并不限定。
此外，为使机器人能够顺利执行规划的任务，还基于该机器人执行任务过程中需满足的等式约束和/或不等式约束构建该关节平滑避限位的优化问题的约束条件。可知，通过对该优化模型进行最优求解决，进而可计算得到最优的关节角速度或关节角加速度。
在一种实施方式中，对于该关节平滑避限位的优化模型，例如，可采用如凸二次规划(quadratic programming，QP)优化函数等进行构建，当然还可以采用其他的优化函数或基于该QP优化函数作适应性调整而构建的优化函数等来构建，都应当在本申请的保护范围之内。
应当明白的是，可根据不同类型的关节来确定是构建基于加速度层面的优化模型，还是构建基于速度层面的优化模型。对于加速度层面，则可选取关节角加速度有关的参量作为优化变量，对于速度层面，则可选取关节角速度有关的参量作为优化变量。
可以理解，通过上述的平滑避限位优化模型可以求解出最优的关节的角速度的增量，进而将其反代回相应的关节角速度或关节角加速度变化的表达式中，即可计算出最优的关节角加速度或关节角速度。由于上述的优化模型包含了投影矩阵，即考虑了关节的自由度变化情况，可以得知，当某个自由度方向未被占用，所需执行的任务不会受到避限位运动的影响。而当该方向自由度逐渐被占用，后续的任务逐渐则会利用剩余的自由度(未超过关节限位)来尽可能执行，进而优先保证了平滑的关节避限位功能，同时在自由度足够的情况下，仍能有效保证所需执行任务依靠剩余处于安全范围内的关节来完成。
步骤S40，利用所述关节角加速度或关节角速度生成对应控制周期的关节控制指令，各个控制周期的关节控制指令用于控制该机器人运动以完成规划的任务。
示范性地，在计算出最优的关节角加速度时，可通过数值积分可得关节角速度指令和关节角度指令；再者，结合动力学模型即可计算得到关节力矩指令。若得到的为最优的关节角速度，则通过数值积分即可得关节的位置指令。进而，将计算出相应的关节控制指令发送到对应的关节，以控制该机器人执行当前任务。可以理解，若当前控制周期执行完指令后，将进入下一控制周期，每个控制周期都将重复上述的步骤S10～S40，直到机器人完成所需执行的任务。
可以理解，上述的机器人运动控制方法通过构建虚拟斥力并结合关节导纳控制，能够使机器人在执行原有任务过程中，即使难以预测未来一段时间内外界的干扰，仍可根据当前控制周期机器人的状态，平滑地产生远离关节限位的行为，即实现平滑避限。而通过构建平滑避限位的优化问题，使接近限位的关节自由度逐渐被避限位运动占用，而机器人所需执行的任务逐渐变为在剩余的未超限位关节所构成的自由度中求解，从而实现了在关节超限位时优先保证关节避限位，并利用剩余自由度尽可能保证原有任务的顺利执行，而关节未超限位时则利用全部自由度优先执行所需任务。
本实施例的机器人运动控制方法无需规划避撞轨迹，便可使机器人实时产生反射式的避撞行为，从软件算法层面保证机器人安全稳定运行。此外，该方法输出关节端力矩、速度和位置指令，适用于位控、力控关节以及力位混合控制关节的串/并联构型机器人，适用于工业、服务和特种等多种领域的机械臂，四足机器人，仿人机器人的运动控制，是一种通用的运动控制方法。
实施例2
请参照图3，基于上述实施例1，本实施例提出一种机器人运动控制方法，该方法同样包括上述步骤S10～S40。下面对这些步骤进行具体描述。
在一种实施方式中，对于上述步骤S10“根据规划的任务轨迹信息和机器人在对应控制周期反馈的关节角相关信息计算机器人各个关节在对应控制周期的用于执行任务的期望运动状态信息”，如图3所示，包括：
步骤S110，根据规划的任务轨迹信息获得机器人在对应控制周期的任务空间的参考运动状态信息。
针对需要执行的任务，可预先规划出机器人在任务空间的运动轨迹信息，例如，该运动轨迹可以是机械臂末端的运动轨迹、机器人质心的轨迹，也可以是仿人机器人上半身姿态的转动轨迹等，具体可根据实际执行的任务来确定。示范性地，该参考运动状态信息可包括机器人在对应控制周期的在任务空间的参考位置、参考速度以及参考加速度等中的一种或多种。
步骤S120，获取该机器人在对应控制周期反馈的关节角相关信息，并根据该关节角相关信息计算机器人各个关节在任务空间的实际运动状态信息。
本实施例中，实时反馈的关节角相关信息主要包括各个关节的角度(即关节角)、角速度(即关节角速度)及角加速度(即关节角速度)。例如，可通过安装在各关节处的角度编码器等采集对应关节的角度，进而对关节角度进行微分处理，可计算得到对应的关节角速度及关节角加速度等。
可以理解，通过传感器测量得到的是关节空间的角度相关信息，故可通过正向运动学计算出机器人在任务空间对应的实际运动状态信息，该实际运动状态信息主要包括实际位置、实际速度和实际加速度等信息。值得注意的是，上述的步骤S110和S120并不严格限定执行的顺序，两者可以是同时执行，也可以按照相应的先后顺序执行。
步骤S130，根据在任务空间的所述参考运动状态信息和所述实际运动状态信息基于预设关节控制律计算机器人各个关节在对应控制周期的用于执行任务的期望运动状态信息。
示范性地，上述预设的关节控制律也称反馈控制律，例如，可以是机械臂出厂时设置或预先生成的，也可以是由用户根据实际需求预先设置的，其预设形式并不作限定。在一种实施方式中，该预设的关节控制律满足：
Figure BDA0003005422570000171
其中，xact和
Figure BDA0003005422570000172
分别表示实际位置和实际速度；xref、
Figure BDA0003005422570000173
和
Figure BDA0003005422570000174
分别表示参考位置、参考速度和参考加速度；Kp和Kd分别为该关节控制律中的位置增益和速度增益；
Figure BDA0003005422570000175
表示期望加速度。进一步地，可通过对该期望加速度进行积分以得到期望速度
Figure BDA0003005422570000176
和期望位置
Figure BDA0003005422570000177
可以理解，该关节控制律仅仅是一种示例，在实际运用中也可以根据实际需求对该关节控制律进行相应调整。
于是，在获得对应控制周期内的实际运动状态信息和参考运动状态信息后，基于预设的关节控制律即可求解出对应控制周期的期望运动状态信息。例如，当前的期望运动状态信息可包括期望位置、期望速度和/或期望加速度等。
在本申请的一些其他的实施例中，对于上述步骤S20中的“基于反馈的各个关节角求解对应控制周期的各个关节远离限位的虚拟斥力”，其中，关于该关节的角度安全范围和虚拟斥力的求解表达式，在一种实施方式中，该关节的角度安全范围的上边界和下边界可根据关节限位角范围来进一步确定。
示范性地，关节的角度安全范围与关节限位角范围满足如下关系：
Figure BDA0003005422570000181
Figure BDA0003005422570000182
其中，
Figure BDA0003005422570000183
和
Figure BDA0003005422570000184
分别表示第i个关节的角度安全范围的上边界和下边界；
Figure BDA0003005422570000189
和
Figure BDA00030054225700001810
分别表示第i个关节的限位角范围的上限和下限；比例系数α可根据工程经验来相应设定。当然，也可以采用其他的方式来选取出角度安全范围，在此并不作限定。
进而，根据上述步骤S20中描述的虚拟斥力的构建原则，在一种实施方式中，基于上述的关节限位角度范围及角度安全范围的关系式，若该机器人所有关节的虚拟斥力记为τJAL，则有：
Figure BDA0003005422570000185
其中，
Figure BDA0003005422570000186
其中，
Figure BDA00030054225700001811
表示第i个关节的虚拟斥力矩；τmax为最大关节力矩。可以理解，这里的第i个关节仅是以转动型关节进行描述。若为平动型关节，将该最大关节力矩τmax替代为最大关节力fmax即可。
由上述表达式可知，当关节i的角度qi位于关节角度安全范围的上边界
Figure BDA0003005422570000187
和下边界
Figure BDA0003005422570000188
之间时，该关节的虚拟斥力为0，此时无需进行关节避限位。而当关节角度qi大于
Figure BDA0003005422570000191
或小于
Figure BDA0003005422570000192
时，会产生使关节角度远离限位方向的虚拟斥力，即关节角度越接近限位方向，则虚拟斥力越大。当关节角度qi完全达到限位
Figure BDA0003005422570000193
或
Figure BDA0003005422570000194
时，则虚拟斥力最大。
在本申请的一些其他实施例中，对于上述步骤S20中的“根据所述虚拟斥力计算各个关节的避限位角相关信息”，其中，关于虚拟斥力与关节角、关节的避限位角度之间的关系式的预先构建，例如，可基于关节导纳控制技术来构建该虚拟斥力与关节角、关节的避限位角度之间的关系式。
示范性地，该关系式如下：
Figure BDA0003005422570000195
其中，ΔqJAL表示各个关节的避限位角；τJAL表示各个关节的虚拟斥力；Mq、D和K分别为基于关节导纳控制的虚拟惯量矩阵、虚拟阻尼矩阵和虚拟刚度矩阵；s表示微分算子。可以理解，这里的关系式通过传递函数的形式描述。其中，上述的虚拟惯量矩阵Mq与机器人的关节角相关信息有关，具体可通过获取的关节角、关节角速度等计算得到。
由上述关系式可知，关节角越接近限位，则虚拟斥力越大，进而计算所得的避限位角速度
Figure BDA0003005422570000196
和避限位角加速度
Figure BDA0003005422570000197
也越大。
在本申请的一些其他实施例中，对于上述步骤S310“构建关节的关节角相关信息到期望运动状态信息的映射方程”，可根据实际需求来构建。
例如，对于力控型或力位混合型等关节，基于加速度层面，可构建出如下映射方程：
Figure BDA0003005422570000198
其中，J为雅克比矩阵；
Figure BDA0003005422570000199
表示待求解的关节角加速度；
Figure BDA00030054225700001910
表示关节在任务空间的期望加速度；
Figure BDA0003005422570000201
为雅克比矩阵J的导数；
Figure BDA0003005422570000202
为反馈的关节角速度。
又例如，对于力控型关节，由于不需利用加速度计算关节力矩，基于速度层面，可构建出如下映射方程：
Figure BDA0003005422570000203
其中，J为雅克比矩阵；
Figure BDA0003005422570000204
表示待求解的关节角速度；
Figure BDA0003005422570000205
表示关节在任务空间的期望速度。
进一步地，对于上述步骤S320“基于关节的避限位角相关信息构建关节角速度或关节角加速度变化的表达式”，如图4所示，包括：
子步骤S321，基于关节的角度安全范围构建与关节角度相关的激活因子的函数表达式，所述函数表达式用于求解对应时刻的激活因子。
示范性地，可根据关节的角度安全范围来构建激活因子的函数表达式，其中，该激活因子与对应关节的角度有关。示范性地，当关节角度在安全范围上边界和下边界之间时，激活因子为0，而当关节角度超过安全范围边界接近限位时，激活因子由0趋于1。
子步骤S322，以激活因子为对角元构建反映关节角度动态变化的投影矩阵。
示范性地，对于以各个关节对应的激活因子作为主对角线上的元素而构建得到的投影矩阵Λ，则有：当Λ的第i个对角元为零时，此时表示第i个关节无避限位运动，而当关节i角度超过安全范围的上边界或下边界并逐渐接近限位边界时，第i个关节避限位运动逐渐增大，且Λ的第i个对角元会逐渐变为1。
例如，在一种实施方式中，激活因子βi克可表示为如下形式：
Figure BDA0003005422570000211
其中，fβ(x)为以x为自变量的从0到1的平滑函数，x＝0时，fβ(x)＝0，
Figure BDA0003005422570000212
时，fβ(x)＝1，该平滑函数可通过现有多项式、样条曲线等方式拟合得到。
于是，将激活因子作为对角元来构建一投影矩阵Λ，其中，Λ∈Rn×n。可以理解，Λ的第i个对角元为零时，此时表示第i个关节无避限位运动，而当关节i角度超过安全范围的上边界或下边界并逐渐接近限位边界时，第i个关节避限位运动逐渐增大，且Λ的第i个对角元会逐渐变为1。
子步骤S323，根据关节的避限位角相关信息和所述投影矩阵构建关节角速度或角加速度变化的表达式。
示范性地，可通过定义一个与关节角加速度或关节角速度有关的增量，利用该增量、关节的避限位角加速度及实际反馈的关节角信息等来得到待求解的关节角速度或关节角加速度。可以理解，该增量能够反映出关节角在对应控制周期的变化程度。
在一实施方式中，示范性地，构建的关节角加速度变化的表达式可为：
Figure BDA0003005422570000213
其中，
Figure BDA0003005422570000214
为待求解的关节角加速度；
Figure BDA0003005422570000215
表示关节的避限位角加速度；I为单位矩阵；Λ为投影矩阵；ξ1表示关节角加速度的增量。
同样，可构建得到关节角速度变化的表达式，如下：
Figure BDA0003005422570000221
其中，
Figure BDA0003005422570000222
为待求解的关节角速度；
Figure BDA0003005422570000223
表示关节的避限位角速度；I为单位矩阵；Λ为投影矩阵；ξ2表示关节角速度的增量。
于是，将该关节角加速度或关节角速度变化的表达式，结合上述步骤S310所构建得到的映射方程进行联立，可得到如下的联立方程：
Figure BDA0003005422570000224
或者，
Figure BDA0003005422570000225
进一步可选地，对于上述步骤S330中构建的平滑避限位优化模型，若以QP优化函数来构建加速度层面的优化模型，示范性地，该凸二次规划优化函数的表达式可为：
Figure BDA0003005422570000226
Figure BDA0003005422570000227
其中，
Figure BDA0003005422570000228
表示机器人执行任务过程中需满足的不等式约束的通用形式，v1表示优化模型的等式约束和/或不等式约束的第一松弛变量；C和d均为不等式约束的系数矩阵，可根据该机器人执行任务过程中需满足的等式约束和/或不等式约束求解。
又或者，可以构建速度层面的优化模型，若仍以QP优化函数为例，此时的凸二次规划优化函数的表达式为：
Figure BDA0003005422570000229
其中，v2表示所述等式约束和/或不等式约束的第二松弛变量。这里的v1和v2仅是为了区分为不同优化函数表达式中的松弛变量，该松弛变量是为了对OP优化函数进行最优化求解而构建的一优化变量。
可以理解，对于不同关节类型的机器人，由于所需的关节控制指令的不同，可构建出不同的关节平滑避限位优化模型。例如，对于力控型关节或力位混合型关节，可构建基于加速度层面的关节平滑避限位的优化问题。而对于位控型关节，由于无需通过动力学模型计算关节力矩，则可构建基于速度层面的关节平滑避限位的优化问题。
应当明白的是，对于上述构建的平滑避限位优化模型，为保证其求解得到的结果能够保证机器人能执行任务或避限位，还需要为该优化模型设置一些约束条件。例如，该机器人执行任务过程中需满足的不等式约束可包括但不限于包括，关节力矩约束、足底接触的摩擦锥约束和重力地面投影约束等中一种或多种，具体的约束条件可根据实际需求来设定。
以关节力矩约束为例，当要求机器人满足关节力矩约束时，即驱动关节力矩不能超过关节的最大力矩，由机器人的逆动力学建模方法可得到机器人逆动力学方程，通常表示为如下形式：
Figure BDA0003005422570000231
其中，M表示机器人的惯量矩阵；τc表示机器人的科氏力产生的力矩，τg表示机器人重力产生的力矩，JTF表示机器人所受外力F产生的力矩项。于是，按照上述的约束条件的形式，可以表示为如下形式：
Figure BDA0003005422570000232
其中，可得到C表示为
Figure BDA0003005422570000233
而d表示为
Figure BDA0003005422570000234
可以理解，通过求解上述的优化模型，可以求解得到关节角速度或关节角加速度的增量，进而将该增量回代到上述的变化表达式中，即可计算出上述步骤S30中的满足需求的关节角速度
Figure BDA0003005422570000241
或关节角加速度
Figure BDA0003005422570000242
以加速度层面的优化模型的求解过程进行分析，可知，当关节i的角度处于角度安全范围时，
Figure BDA0003005422570000243
Λ的第i个对角元为零，此时第i个关节无避限位运动，I-Λ矩阵的第i个对角元为1，因此该自由度方向未被占用，所需执行的任务不会受到关节i避限位运动的影响。而当关节i的角度超过安全范围的边界并逐渐接近角度限位的上下限时，第i个关节的避限位角加速度
Figure BDA0003005422570000244
将逐渐增大，且Λ的第i个对角元会逐渐变为1，I-Λ矩阵的第i个对角元趋于0，该方向自由度逐渐被占用，使得i关节加速度求解结果趋近于避限位加速度
Figure BDA0003005422570000245
后续任务逐渐则利用剩余自由度尽可能满足任务执行精度，进而优先保证了平滑的关节避限位功能，同时在自由度足够的情况下，仍能有效保证所需执行任务依靠剩余处于安全范围内的关节来完成。而当关节i的角度远离限位时，Λ的第i个对角元会逐渐变为0，I-Λ矩阵第i个对角元趋于1，该方向自由度逐渐被释放，使得机器人逐渐获得更多自由度优先执行所需任务。
本实施例的机器人运动控制方法通过结合相应的实施例来说明，无需规划避撞轨迹，便可使机器人实时产生反射式的避撞行为，从软件算法层面保证机器人安全稳定运行。
实施例3
请参照图5，基于上述实施例1的方法，本实施例提出一种机器人运动控制装置100。示范性地，该机器人运动控制装置100包括：
期望值计算模块110用于根据规划的任务轨迹信息和机器人在对应控制周期反馈的关节角相关信息计算机器人各个关节在对应控制周期的用于执行任务的期望运动状态信息，所述关节角相关信息包括各个关节角。
避限位角信息计算模块120用于基于反馈的各个关节角求解对应控制周期的各个关节远离限位的虚拟斥力，并根据所述虚拟斥力计算各个关节的避限位角相关信息。
优化模型求解模块130用于基于对应控制周期的所述期望运动状态信息和所述避限位角相关信息对关节平滑避限位优化模型进行最优求解，以得到满足关节平滑避限位且用于执行任务的关节角加速度或关节角速度。
控制指令执行模块140用于利用所述关节角加速度或关节角速度生成对应控制周期的关节控制指令，各个控制周期的所述关节控制指令用于控制该机器人运动以完成所述规划的任务。
可以理解，本实施例的装置对应于上述实施例1的方法，上述实施例1中的可选项同样适用于本实施例，故在此不再重复描述。
本申请还提供了一种机器人，例如，该机器人可以是运用于不同领域的机械臂、仿人机器人、多足机器人等。示范性地，该机器人包括处理器和存储器，其中，存储器存储有计算机程序，处理器通过运行所述计算机程序，从而使机器人执行上述的机器人运动控制方法或者上述机器人运动控制装置中的各个模块的功能。
本申请还提供了一种可读存储介质，用于储存上述机器人中使用的所述计算机程序。
在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和结构图显示了根据本申请的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，结构图和/或流程图中的每个方框、以及结构图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
另外，在本申请各个实施例中的各功能模块或单元可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或更多个模块集成形成一个独立的部分。
所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是智能手机、个人计算机、服务器、或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述，仅为本申请的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。

Claims (12)

1.一种机器人运动控制方法，其特征在于，包括：
根据规划的任务轨迹信息和机器人在对应控制周期反馈的关节角相关信息计算机器人各个关节在对应控制周期的用于执行任务的期望运动状态信息，所述关节角相关信息包括实时反馈的在关节空间的各个关节角、关节角速度和/或关节角加速度；
基于反馈的各个关节角求解对应控制周期的各个关节远离限位的虚拟斥力，并根据所述虚拟斥力计算各个关节的避限位角相关信息；其中，所述虚拟斥力是一种实际反馈的关节角度在靠近关节限位角时定义的用于排斥或远离关节限位的虚拟的力或力矩，且其方向为远离关节限位的方向；所述避限位角相关信息包括关节的避限位角、避限位角速度和/或避限位角加速度，所述关节的避限位角与虚拟斥力的关系式基于关节导纳控制构建；
其中，所述虚拟斥力的表达式基于关节的关节角和关节限位角范围预先构建，具体包括：根据关节限位角范围确定所述关节的角度安全变化的上边界和下边界，以作为角度安全范围；当实际反馈的关节角位于所述关节的角度安全范围内，则所述虚拟斥力为0；当实际反馈的关节角位于所述关节的角度安全范围外，则在所述关节角越接近所述关节限位角范围的上限或下限的情况下，所述虚拟斥力越大且方向远离关节限位方向；
基于对应控制周期的所述期望运动状态信息和所述避限位角相关信息对关节平滑避限位优化模型进行最优求解，以得到满足关节平滑避限位且用于执行任务的关节角加速度或关节角速度；其中，所述关节平滑避限位优化模型为一个采用与关节角加速度或关节角速度有关的参量作为优化变量的平滑避限位优化问题；
利用所述关节角加速度或关节角速度生成对应控制周期的关节控制指令，各个控制周期的所述关节控制指令用于控制该机器人运动以完成所述规划的任务。
2.根据权利要求1所述的机器人运动控制方法，其特征在于，所述根据规划的任务轨迹信息和机器人在对应控制周期反馈的关节角相关信息计算机器人各个关节在对应控制周期的用于执行任务的期望运动状态信息，包括：
根据规划的任务轨迹信息获得机器人在对应控制周期的任务空间的参考运动状态信息；
获取机器人在对应控制周期反馈的关节角相关信息，并根据所述关节角相关信息计算机器人各个关节在任务空间的实际运动状态信息；
根据在任务空间的所述参考运动状态信息和所述实际运动状态信息基于预设关节控制律计算机器人各个关节在对应控制周期的用于执行任务的期望运动状态信息。
3.根据权利要求1或2所述的机器人运动控制方法，其特征在于，所述关节的避限位角与虚拟斥力的所述关系式如下：
Figure FDA0003865797840000021
其中，ΔqJAL表示关节的避限位角；τJAL表示虚拟斥力；Mq、D和K分别为基于关节导纳控制的虚拟惯量矩阵、虚拟阻尼矩阵和虚拟刚度矩阵；s表示微分算子。
4.根据权利要求1所述的机器人运动控制方法，其特征在于，所述关节平滑避限位优化模型经过预先构建，包括：
构建关节的关节角相关信息到期望运动状态信息的映射方程；
基于关节的避限位角相关信息构建关节角速度或关节角加速度变化的表达式；
基于所述映射方程和所述关节角速度或关节角加速度变化的表达式的联立方程构建平滑避限位优化模型，其中，基于该机器人执行任务过程中需满足的等式约束和/或不等式约束构建所述平滑避限位优化模型的约束条件。
5.根据权利要求4所述的机器人运动控制方法，其特征在于，所述基于关节的避限位角相关信息构建关节角速度或关节角加速度变化的表达式，包括：
基于关节的所述角度安全范围构建与关节角相关的激活因子的函数表达式，所述函数表达式用于求解所述关节在对应控制周期的激活因子；
以所述激活因子为对角元构建用于反映关节角动态变化的投影矩阵；
根据所述避限位角相关信息和所述投影矩阵构建关节角速度或关节角加速度变化的表达式。
6.根据权利要求5所述的机器人运动控制方法，其特征在于，所述避限位角相关信息还包括关节的避限位角加速度，所述期望运动状态信息包括关节的期望加速度；
若该机器人的关节类型为力控型关节或力位混合型关节，则基于所述映射方程和所述关节角加速度变化的表达式的联立方程来构建基于与关节角加速度有关的优化变量的平滑避限位优化模型；
所述映射方程为：
Figure FDA0003865797840000041
其中，J为雅克比矩阵；
Figure FDA0003865797840000042
表示待求解的关节角加速度；
Figure FDA0003865797840000043
表示关节在任务空间的期望加速度；
Figure FDA0003865797840000044
为雅克比矩阵J的导数；
Figure FDA0003865797840000045
为实际反馈的关节角速度；
以及，所述关节角加速度变化的表达式为：
Figure FDA0003865797840000046
其中，
Figure FDA0003865797840000047
表示关节的避限位角加速度；I为单位矩阵；Λ为投影矩阵；ξ1表示关节角加速度的增量。
7.根据权利要求6所述的机器人运动控制方法，其特征在于，所述平滑避限位优化模型采用凸二次规划优化函数构建，所述凸二次规划优化函数的表达式为：
Figure FDA0003865797840000048
Figure FDA0003865797840000049
其中，C和d均为不等式约束的系数矩阵，根据该机器人执行任务过程中需满足的等式约束和/或不等式约束进行求解；v1表示所述等式约束和/或不等式约束的第一松弛变量。
8.根据权利要求5所述的机器人运动控制方法，其特征在于，所述避限位角相关信息还包括关节的避限位角速度，所述期望运动状态信息包括关节的期望速度；
若该机器人的关节类型为位控型关节，则基于所述映射方程和所述关节角速度变化的表达式的联立方程构建基于与关节角速度有关的优化变量的平滑避限位优化模型；
所述映射方程为：
Figure FDA0003865797840000051
其中，J为雅克比矩阵；
Figure FDA0003865797840000052
表示待求解的关节角速度；
Figure FDA0003865797840000053
表示关节在任务空间的期望速度；
以及，所述关节角速度变化的表达式为：
Figure FDA0003865797840000054
其中，
Figure FDA0003865797840000055
表示关节的避限位角速度；I为单位矩阵；Λ为投影矩阵；ξ2表示关节的角速度的增量。
9.根据权利要求8所述的机器人运动控制方法，其特征在于，所述平滑避限位优化模型采用凸二次规划优化函数构建，所述凸二次规划优化函数的表达式为：
Figure FDA0003865797840000056
其中，C和d均为不等式约束的系数矩阵，根据该机器人执行任务过程中需满足的等式约束和/或不等式约束进行求解；v2表示所述等式约束和/或不等式约束的第二松弛变量。
10.一种机器人运动控制装置，其特征在于，包括：
期望值计算模块，用于根据规划的任务轨迹信息和机器人在对应控制周期反馈的关节角相关信息计算机器人各个关节在对应控制周期的用于执行任务的期望运动状态信息，所述关节角相关信息包括实时反馈的在关节空间的各个关节角、关节角速度和/或关节角加速度；
避限位角信息计算模块，用于基于反馈的各个关节角求解对应控制周期的各个关节远离限位的虚拟斥力，并根据所述虚拟斥力计算各个关节的避限位角相关信息；其中，所述虚拟斥力是一种实际反馈的关节角度在靠近关节限位角时定义的用于排斥或远离关节限位的虚拟的力或力矩，且其方向为远离关节限位的方向；所述避限位角相关信息包括关节的避限位角、避限位角速度和/或避限位角加速度，所述关节的避限位角与虚拟斥力的关系式基于关节导纳控制构建；
其中，所述虚拟斥力的表达式基于关节的关节角和关节限位角范围预先构建，包括：根据关节限位角范围确定所述关节的角度安全变化的上边界和下边界，以得到所述关节的角度安全范围；当实际反馈的关节角位于所述关节的角度安全范围内，则所述虚拟斥力为0；当实际反馈的关节角位于所述关节的角度安全范围外，则在所述关节角越接近所述关节限位角范围的上限或下限的情况下，所述虚拟斥力越大且方向远离关节限位方向；
优化模型求解模块，用于基于对应控制周期的所述期望运动状态信息和所述避限位角相关信息对关节平滑避限位优化模型进行最优求解，以得到满足关节平滑避限位且用于执行任务的关节角加速度或关节角速度；其中，所述关节平滑避限位优化模型为一个采用与关节角加速度或关节角速度有关的参量作为优化变量的平滑避限位优化问题；
控制指令执行模块，用于利用所述关节角加速度或关节角速度生成对应控制周期的关节控制指令，各个控制周期的所述关节控制指令用于控制该机器人运动以完成所述任务。
11.一种机器人，其特征在于，所述机器人包括处理器和存储器，所述存储器存储有计算机程序，所述处理器用于执行所述计算机程序以实施权利要求1-9中任一项所述的机器人运动控制方法。
12.一种可读存储介质，其特征在于，其存储有计算机程序，所述计算机程序在处理器上运行时，实施根据权利要求1-9中任一项所述的机器人运动控制方法。
CN202110360660.0A 2021-04-02 2021-04-02 机器人运动控制方法、装置和机器人 Active CN113070881B (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN202110360660.0A CN113070881B (zh) 2021-04-02 2021-04-02 机器人运动控制方法、装置和机器人
PCT/CN2021/131890 WO2022205940A1 (zh) 2021-04-02 2021-11-19 机器人运动控制方法、装置和机器人

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110360660.0A CN113070881B (zh) 2021-04-02 2021-04-02 机器人运动控制方法、装置和机器人

Publications (2)

Publication Number Publication Date
CN113070881A CN113070881A (zh) 2021-07-06
CN113070881B true CN113070881B (zh) 2022-11-11

Family

ID=76614915

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110360660.0A Active CN113070881B (zh) 2021-04-02 2021-04-02 机器人运动控制方法、装置和机器人

Country Status (2)

Country Link
CN (1) CN113070881B (zh)
WO (1) WO2022205940A1 (zh)

Families Citing this family (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113070881B (zh) * 2021-04-02 2022-11-11 深圳市优必选科技股份有限公司 机器人运动控制方法、装置和机器人
CN113618739B (zh) * 2021-08-24 2022-07-29 深圳市优必选科技股份有限公司 一种机器人动态避障方法、装置和机器人
CN113618741B (zh) * 2021-08-24 2022-07-29 深圳市优必选科技股份有限公司 机器人关节位姿优化方法、机器人控制方法和机器人
CN113618742B (zh) * 2021-08-24 2022-07-29 深圳市优必选科技股份有限公司 一种机器人避障方法、装置和机器人
CN114084295A (zh) * 2021-11-12 2022-02-25 上海新纪元机器人有限公司 主动减少晃动的方法、系统及座椅
CN113977585B (zh) * 2021-11-25 2024-03-15 中国北方车辆研究所 一种虚拟力伺服柔顺控制方法
CN114347020B (zh) * 2021-12-22 2024-02-13 深圳市优必选科技股份有限公司 一种运动控制方法、运动控制装置及机器人
CN114326769B (zh) * 2021-12-28 2024-03-29 深圳市优必选科技股份有限公司 机器人运动矫正方法及装置、机器人控制设备和存储介质
CN115229772B (zh) * 2022-08-23 2023-07-18 深圳市越疆科技股份有限公司 机器人及其控制方法、装置、设备、存储介质、机械臂
CN116700012B (zh) * 2023-07-19 2024-03-01 合肥工业大学 一种多智能体的避撞编队合围控制器的设计方法
CN118003325B (zh) * 2024-02-28 2024-07-23 北京中科慧灵机器人技术有限公司 目标轨迹数据信息确定方法、装置、设备及存储介质

Family Cites Families (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS6140612A (ja) * 1984-08-01 1986-02-26 Shinko Electric Co Ltd 産業用ロボツト
JP5966372B2 (ja) * 2012-01-17 2016-08-10 セイコーエプソン株式会社 ロボット制御装置、ロボットシステム、ロボット制御方法及びロボット
KR102030141B1 (ko) * 2014-06-11 2019-10-10 현대자동차(주) 로봇의 팔꿈치 제어시스템 및 그 제어방법
CN105058411B (zh) * 2015-08-04 2017-01-25 苏州阿福机器人有限公司 外力驱动机械关节中运动件的方法
CN105538327A (zh) * 2016-03-03 2016-05-04 吉首大学 一种基于突加度的冗余度机械臂重复运动规划方法
CN105922265B (zh) * 2016-06-20 2018-08-24 广州视源电子科技股份有限公司 一种机械臂的运动轨迹规划方法、装置及机器人
CN107186711B (zh) * 2017-05-12 2021-06-15 广州视源电子科技股份有限公司 机械臂的限位保护方法、装置及机器人
CN109108965B (zh) * 2018-07-27 2021-07-23 武汉精锋微控科技有限公司 一种应用于机械手臂的笛卡尔空间运动预测方法
CN111469129A (zh) * 2020-04-24 2020-07-31 苏州新传品智能科技有限公司 一种基于双机械臂的船体外板曲面成形无碰撞运动规划方法
CN111538949B (zh) * 2020-07-10 2020-10-16 深圳市优必选科技股份有限公司 冗余机器人逆运动学求解方法、装置和冗余机器人
CN113070881B (zh) * 2021-04-02 2022-11-11 深圳市优必选科技股份有限公司 机器人运动控制方法、装置和机器人

Also Published As

Publication number Publication date
CN113070881A (zh) 2021-07-06
WO2022205940A1 (zh) 2022-10-06

Similar Documents

Publication Publication Date Title
CN113070881B (zh) 机器人运动控制方法、装置和机器人
CN112775976B (zh) 任务执行控制方法、装置、控制设备及可读存储介质
CN111538949B (zh) 冗余机器人逆运动学求解方法、装置和冗余机器人
JP7199178B2 (ja) ロボット制御装置、ロボット装置、ロボット制御のパラメータ調整方法、およびプログラム
JP7324932B2 (ja) 動的計画コントローラ
US20200282558A1 (en) System and method for controlling a robot with torque-controllable actuators
CN108189036B (zh) 力矩控制方法、装置、机器人及存储介质
US20190118375A1 (en) Related to generating a robot control policy from demonstrations collected via kinesthetic teaching of a robot
KR20170131455A (ko) 로봇 매니퓰레이터의 개방 루프 제어 및 폐쇄 루프 제어를 수행하는 장치 및 방법
JP2018030210A (ja) シミュレーション装置、制御システム、ロボットシステム、シミュレーション方法、プログラム及び記録媒体
CN114952821A (zh) 机器人运动控制方法、机器人及系统
Khan et al. ROS-based control for a robot manipulator with a demonstration of the ball-on-plate task
CN112975965B (zh) 仿人机器人的解耦控制方法、装置和仿人机器人
CN115657511A (zh) 机器人遥控操作系统和方法
JP2020075329A (ja) シミュレーション装置、制御システム、ロボットシステム、シミュレーション方法、プログラム及び記録媒体
Safeena et al. Adaptive super twisting control of Stewart platform based on super twisting observer
Kastritsi et al. A pHRI Framework for Modifying a Robot's Kinematic Behaviour via Varying Stiffness and Dynamical System Synchronization
CN112632803A (zh) 跟踪控制方法和装置、电子设备及存储介质
Xu et al. Passive bilateral teleoperation of a car-like mobile robot
CN114367989B (zh) 缺省的机械臂阻抗控制方法、装置、机械臂及存储介质
US20240116178A1 (en) Predictive control method for torque-rate control and vibration suppression
JP4289501B2 (ja) ロボット制御方法及びプログラム
Becerra-Vargas et al. Robust joint space control of a 6 DOF parallel robot
Elias Review of Hardware Implementation for the Two-wheeled Self-balancing Robot
CN116587278A (zh) 一种基于模型不确定性估计的机械臂强化学习控制方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
CB03 Change of inventor or designer information
CB03 Change of inventor or designer information

Inventor after: Han Gang

Inventor after: Xiong Youjun

Inventor before: Han Gang

Inventor before: Zhao Mingguo

Inventor before: Xiong Youjun