CN112605992A - 一种双臂机器人的循环运动控制方法及装置 - Google Patents

一种双臂机器人的循环运动控制方法及装置 Download PDF

Info

Publication number
CN112605992A
CN112605992A CN202011430600.3A CN202011430600A CN112605992A CN 112605992 A CN112605992 A CN 112605992A CN 202011430600 A CN202011430600 A CN 202011430600A CN 112605992 A CN112605992 A CN 112605992A
Authority
CN
China
Prior art keywords
arm robot
joint
robot
limit
right arm
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.)
Pending
Application number
CN202011430600.3A
Other languages
English (en)
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.)
Sun Yat Sen University
Original Assignee
Sun Yat Sen University
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 Sun Yat Sen University filed Critical Sun Yat Sen University
Priority to CN202011430600.3A priority Critical patent/CN112605992A/zh
Publication of CN112605992A publication Critical patent/CN112605992A/zh
Pending legal-status Critical Current

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/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/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/1679—Programme controls characterised by the tasks executed
    • B25J9/1682—Dual arm manipulator; Coordination of several manipulators

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

本发明公开了一种双臂机器人的循环运动控制方法及装置，适用于具有左右两个机械臂的机器人，所述方法包括：设定左臂机器人的性能指标与右臂机器人的性能指标，并同时最小化两个性能指标得到运动性能指标，基于所述运动性能指标生成突加度层循环运动的控制方案；生成所述左臂机器人的左约束不等式和所述右臂机器人的右约束不等式；基于所述左约束不等式和所述右约束不等式，将所述突加度层循环运动的控制方案转换生成一个标准的二次型规划问题，并调用预设的二次型规划求解器进行求解；获取所述预设的二次型规划求解器生成的求解结果，并根据所述求解结果驱动所述机器人的左右两个机械臂循环运动。本申请可以实现双臂机器人的循环运动控制。

Description

一种双臂机器人的循环运动控制方法及装置
技术领域
本发明涉及机器人运动规划及控制技术领域，尤其涉及一种双臂机器人的循环运动控制方法及装置。
背景技术
双臂机器人像人类一样拥有两只手臂，并且双臂在一定范围内可进行协调工作。相对于单臂机器人它可以大大增强机器人对复杂装配任务的适应性，同时可以提高工作空间的利用效率。
当机器人的末端任务为一个封闭曲线时，其各个关节可能回不到初始位置，这种现象叫做关节漂移现象或称非循环运动问题。
若机器人的各个关节变量无法满足给定的物理极限时，会导致机器人在运动过程中不能实现一个封闭曲线的循环运动或发生关节偏移现象，若，使得机器人在运动过程中会产生不可预料的情况，例如机器不可控或机件损毁，不但增加实验人员的实验风险，也增加了实验成本。
发明内容
本发明提出一种双臂机器人的循环运动控制方法及装置，以解决因机器人不能完成封闭曲线的循环运动或发生关节偏移现象而导致实验成本高，实验风险大的技术问题。
本发明一实施例提供了一种双臂机器人的循环运动控制方法，适用于具有左右两个机械臂的机器人，所述方法包括：
采用零化动力学方式分别设定左臂机器人的性能指标与右臂机器人的性能指标，并同时最小化两个性能指标得到运动性能指标，基于所述运动性能指标生成突加度层循环运动的控制方案；
基于零化动力学方式将所述左臂机器人的关节角度极限、左臂机器人的关节速度极限、左臂机器人的关节加速度极限和左臂机器人的关节突加度极限的不等式约束进行等效，生成所述左臂机器人的左约束不等式，基于零化动力学方式将所述右臂机器人的关节角度极限、右臂机器人的关节速度极限、右臂机器人的关节加速度极限和右臂机器人的关节突加度极限的不等式约束进行等效，生成所述右臂机器人的右约束不等式；
基于所述左约束不等式和所述右约束不等式，将所述突加度层循环运动的控制方案转换生成一个标准的二次型规划问题，并调用预设的二次型规划求解器进行求解；
获取所述预设的二次型规划求解器生成的求解结果，并根据所述求解结果驱动所述机器人的左右两个机械臂循环运动。
进一步的，所述运动性能指标受约束于基于突加度的左臂机器人的雅可比矩阵Jl(θl)、右臂机器人的雅可比矩阵Jr(θr)、左臂机器人的关节角度极限
Figure BDA0002826527830000021
左臂机器人的关节速度极限
Figure BDA0002826527830000022
左臂机器人的关节加速度极限
Figure BDA0002826527830000023
左臂机器人的关节突加度极限
Figure BDA0002826527830000024
右臂机器人的关节角度极限
Figure BDA0002826527830000025
右臂机器人的关节速度极限
Figure BDA0002826527830000026
右臂机器人的关节加速度极限
Figure BDA0002826527830000027
右臂机器人的关节突加度极限
Figure BDA0002826527830000028
其中，所述左臂机器人的性能指标为：
Figure BDA0002826527830000029
所述右臂机器人的性能指标为：
Figure BDA00028265278300000210
所述运动性能指标为：
Figure BDA00028265278300000211
上式中，In为单位矩阵，
Figure BDA00028265278300000212
所述左臂机器人的雅可比矩阵Jl(θl)满足
Figure BDA00028265278300000213
Figure BDA00028265278300000214
所述右臂机器人的雅可比矩阵Jr(θr)满足
Figure BDA00028265278300000215
Figure BDA00028265278300000216
η和μ为零化动力学设计参数，取值大于零，rdl和rdr是预先设定的机器人左右臂的期望路径，fl(θl)和fr(θr)是机器人左右臂的前向运动学映射函数，t为时间变量。
进一步的，所述基于零化动力学方式将所述左臂机器人的关节角度极限、左臂机器人的关节速度极限、左臂机器人的关节加速度极限和左臂机器人的关节突加度极限的不等式约束进行等效，生成所述左臂机器人的左约束不等式，包括：
获取所述左臂机器人的关节角度极限、左臂机器人的关节速度极限、左臂机器人的关节加速度极限和左臂机器人的关节突加度极限的不等式中的最大求导阶数为左臂机器人最大求导阶数；
基于零化动力学方式将所述左臂机器人的关节角度极限、左臂机器人的关节速度极限、左臂机器人的关节加速度极限和左臂机器人的关节突加度极限的不等式按照所述左臂机器人最大求导阶数求导，并约束生成左臂机器人的左约束不等式：
Figure BDA0002826527830000031
其中
Figure BDA0002826527830000032
Figure BDA0002826527830000033
进一步的，所述基于零化动力学方式将所述右臂机器人的关节角度极限、右臂机器人的关节速度极限、右臂机器人的关节加速度极限和右臂机器人的关节突加度极限的不等式约束进行等效，生成所述右臂机器人的右约束不等式，包括：
获取所述右臂机器人的关节角度极限、右臂机器人的关节速度极限、右臂机器人的关节加速度极限和右臂机器人的关节突加度极限的不等式中的最大求导阶数为右臂机器人最大求导阶数；
基于零化动力学方式将所述右臂机器人的关节角度极限、右臂机器人的关节速度极限、右臂机器人的关节加速度极限和右臂机器人的关节突加度极限的不等式按照所述右臂机器人最大求导阶数求导，并约束生成右臂机器人的右约束不等式：
Figure BDA0002826527830000034
其中
Figure BDA0002826527830000035
Figure BDA0002826527830000036
进一步的，所述二次型规划问题为：
Figure BDA0002826527830000041
并受约束于Auyu＝du和
Figure BDA0002826527830000042
其中
Figure BDA0002826527830000043
Wu＝[In,0；0,In]，pu＝[pl；pr]，Au＝[Jl(θl),0；0,Jr(θr)]，
Figure BDA0002826527830000044
进一步的，根据所述求解结果驱动所述机器人的左右两个机械臂循环运动，包括：
将所述求解结果转化为驱动所述机器人的控制信号；
将所述控制信号发送至所述机器人的预设电机中，以使所述预设电机驱动左右两个机械臂循环运动。
相应的，本发明一实施例还提供了一种双臂机器人的循环运动控制装置，适用于具有左右两个机械臂的机器人，所述装置包括：
设定模块，用于采用零化动力学方式分别设定左臂机器人的性能指标与右臂机器人的性能指标，并同时最小化两个性能指标得到运动性能指标，基于所述运动性能指标生成突加度层循环运动的控制方案；
机器人约束模块，用于基于零化动力学方式将所述左臂机器人的关节角度极限、左臂机器人的关节速度极限、左臂机器人的关节加速度极限和左臂机器人的关节突加度极限的不等式约束进行等效，生成所述左臂机器人的左约束不等式，基于零化动力学方式将所述右臂机器人的关节角度极限、右臂机器人的关节速度极限、右臂机器人的关节加速度极限和右臂机器人的关节突加度极限的不等式约束进行等效，生成所述右臂机器人的右约束不等式；
求解模块，用于基于所述左约束不等式和所述右约束不等式，将所述突加度层循环运动的控制方案转换生成一个标准的二次型规划问题，并调用预设的二次型规划求解器进行求解；
驱动模块，用于获取所述预设的二次型规划求解器生成的求解结果，并根据所述求解结果驱动所述机器人的左右两个机械臂循环运动。
进一步的，所述运动性能指标受约束于基于突加度的左臂机器人的雅可比矩阵Jl(θl)、右臂机器人的雅可比矩阵Jr(θr)、左臂机器人的关节角度极限
Figure BDA0002826527830000051
左臂机器人的关节速度极限
Figure BDA0002826527830000052
左臂机器人的关节加速度极限
Figure BDA0002826527830000053
左臂机器人的关节突加度极限
Figure BDA0002826527830000054
右臂机器人的关节角度极限
Figure BDA0002826527830000055
右臂机器人的关节速度极限
Figure BDA0002826527830000056
右臂机器人的关节加速度极限
Figure BDA0002826527830000057
右臂机器人的关节突加度极限
Figure BDA0002826527830000058
其中，所述左臂机器人的性能指标为：
Figure BDA0002826527830000059
所述右臂机器人的性能指标为：
Figure BDA00028265278300000510
所述运动性能指标为：
Figure BDA00028265278300000511
上式中，In为单位矩阵，
Figure BDA00028265278300000512
所述左臂机器人的雅可比矩阵Jl(θl)满足
Figure BDA00028265278300000513
Figure BDA00028265278300000514
所述右臂机器人的雅可比矩阵Jr(θr)满足
Figure BDA00028265278300000515
Figure BDA00028265278300000516
η和μ为零化动力学设计参数，取值大于零，rdl和rdr是预先设定的机器人左右臂的期望路径，fl(θl)和fr(θr)是机器人左右臂的前向运动学映射函数，t为时间变量。
进一步的，所述机器人约束模块还用于：
获取所述左臂机器人的关节角度极限、左臂机器人的关节速度极限、左臂机器人的关节加速度极限和左臂机器人的关节突加度极限的不等式中的最大求导阶数为左臂机器人最大求导阶数；
基于零化动力学方式将所述左臂机器人的关节角度极限、左臂机器人的关节速度极限、左臂机器人的关节加速度极限和左臂机器人的关节突加度极限的不等式按照所述左臂机器人最大求导阶数求导，并约束生成左臂机器人的左约束不等式：
Figure BDA00028265278300000517
其中
Figure BDA00028265278300000518
Figure BDA00028265278300000519
进一步的，所述机器人约束模块还用于：
获取所述右臂机器人的关节角度极限、右臂机器人的关节速度极限、右臂机器人的关节加速度极限和右臂机器人的关节突加度极限的不等式中的最大求导阶数为右臂机器人最大求导阶数；
基于零化动力学方式将所述右臂机器人的关节角度极限、右臂机器人的关节速度极限、右臂机器人的关节加速度极限和右臂机器人的关节突加度极限的不等式按照所述右臂机器人最大求导阶数求导，并约束生成右臂机器人的右约束不等式：
Figure BDA0002826527830000061
其中
Figure BDA0002826527830000062
Figure BDA0002826527830000063
进一步的，所述二次型规划问题为：
Figure BDA0002826527830000064
并受约束于Auyu＝du和
Figure BDA0002826527830000065
其中
Figure BDA0002826527830000066
Wu＝[In,0；0,In]，pu＝[pl；pr]，Au＝[Jl(θl),0；0,Jr(θr)]，
Figure BDA0002826527830000067
进一步的，所述驱动模块还用于：
将所述求解结果转化为驱动所述机器人的控制信号；
将所述控制信号发送至所述机器人的预设电机中，以使所述预设电机驱动左右两个机械臂循环运动。
相比于现有技术，本发明实施例提供的双臂机器人的循环运动控制方法及装置，其有益效果在于：本发明可以分别设定两个机器人的性能指标，再根据两个机器人的性能指标确定整个机器人的运动性能指标和控制方案，并设定其约束条件，使机器人的运动可以满足物理极限，而且本发明可以简化整个控制方案，将控制方案转换成一个二次型规划问题，再根据求解结果驱动机器人运动，从而可以简化整个步骤流程，提高控制精度，并实现双臂机器人的循环运动控制。
附图说明
图1是本发明一实施例提供的一种双臂机器人的循环运动控制方法的流程示意图；
图2为本发明的双臂机器人简化模型的示意图；
图3是本发明一实施例提供的一种双臂机器人的循环运动控制方法的操作流程图；
图4为本发明的双臂机器人实际运动轨迹的示意图；
图5为本发明的左臂机器人和右臂机器人末端执行器的实际轨迹ral、rar与期望路径rdl、rdr的示意图；
图6为本发明计算的左臂机器人和右臂机器人在X轴方向和Y轴方向跟踪误差eXl、eYl、eXr和eYr的示意图；
图7为本发明计算的左臂机器人的关节角度θl和右臂机器人的关节角度θr的示意图；
图8为本发明计算的左臂机器人的关节速度
Figure BDA0002826527830000071
和右臂机器人的关节速度
Figure BDA0002826527830000072
的示意图；
图9为本发明计算的左臂机器人的关节加速度
Figure BDA0002826527830000073
和右臂机器人的关节加速度
Figure BDA0002826527830000075
的示意图；
图10为本发明计算的左臂机器人的关节突加度
Figure BDA0002826527830000074
和右臂机器人的关节突加度
Figure BDA0002826527830000076
的示意图；
图11为本发明计算的左臂机器人和右臂机器人在三个周期内的关节角度θl和θr的示意图；
图12是本发明一实施例提供的一种双臂机器人的循环运动控制装置的结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
当机器人的各个关节变量无法满足给定的物理极限时，会导致机器人在运动过程中不能实现一个封闭曲线的循环运动或发生关节偏移现象，若，使得机器人在运动过程中会产生不可预料的情况，例如机器不可控或机件损毁，不但增加实验人员的实验风险，也增加了实验成本。
为了解决上述问题，下面将通过以下具体的实施例对本申请实施例提供的一种双臂机器人的循环运动控制方法进行详细介绍和说明。
参照图1，示出了本发明一实施例提供的一种双臂机器人的循环运动控制方法的流程示意图。
参照图2，示出了本发明的双臂机器人简化模型的示意图。在本实施例中，所述双臂机器人的循环运动控制方法可以应用于具有双机械臂的机器人，双机械臂分别是设置在机器人的左右两个机械臂。
其中，作为示例的，所述双臂机器人的循环运动控制方法，可以包括：
S11、采用零化动力学方式分别设定左臂机器人的性能指标与右臂机器人的性能指标，并同时最小化两个性能指标得到运动性能指标，基于所述运动性能指标生成突加度层循环运动的控制方案。
在本实施例中，所述运动性能指标受约束于基于突加度的左臂机器人的雅可比矩阵Jl(θl)、右臂机器人的雅可比矩阵Jr(θr)、左臂机器人的关节角度极限
Figure BDA0002826527830000081
左臂机器人的关节速度极限
Figure BDA0002826527830000082
左臂机器人的关节加速度极限
Figure BDA0002826527830000083
左臂机器人的关节突加度极限
Figure BDA0002826527830000084
右臂机器人的关节角度极限
Figure BDA0002826527830000085
右臂机器人的关节速度极限
Figure BDA0002826527830000086
右臂机器人的关节加速度极限
Figure BDA0002826527830000087
右臂机器人的关节突加度极限
Figure BDA0002826527830000088
其中，所述左臂机器人的性能指标为：
Figure BDA0002826527830000089
所述右臂机器人的性能指标为：
Figure BDA00028265278300000810
所述运动性能指标为：
Figure BDA00028265278300000811
上式中，In为单位矩阵，
Figure BDA00028265278300000812
所述左臂机器人的雅可比矩阵Jl(θl)满足
Figure BDA0002826527830000091
Figure BDA0002826527830000092
所述右臂机器人的雅可比矩阵Jr(θr)满足
Figure BDA0002826527830000093
Figure BDA0002826527830000094
η和μ为零化动力学设计参数，取值大于零，rdl和rdr是预先设定的机器人左右臂的期望路径，fl(θl)和fr(θr)是机器人左右臂的前向运动学映射函数，t为时间变量。
S12、基于零化动力学方式将所述左臂机器人的关节角度极限、左臂机器人的关节速度极限、左臂机器人的关节加速度极限和左臂机器人的关节突加度极限的不等式约束进行等效，生成所述左臂机器人的左约束不等式，基于零化动力学方式将所述右臂机器人的关节角度极限、右臂机器人的关节速度极限、右臂机器人的关节加速度极限和右臂机器人的关节突加度极限的不等式约束进行等效，生成所述右臂机器人的右约束不等式。
在一实施例中，步骤S12可以包括以下子步骤：
子步骤S121、获取所述左臂机器人的关节角度极限、左臂机器人的关节速度极限、左臂机器人的关节加速度极限和左臂机器人的关节突加度极限的不等式中的最大求导阶数为左臂机器人最大求导阶数。
子步骤S122、基于零化动力学方式将所述左臂机器人的关节角度极限、左臂机器人的关节速度极限、左臂机器人的关节加速度极限和左臂机器人的关节突加度极限的不等式按照所述左臂机器人最大求导阶数求导，并约束生成左臂机器人的左约束不等式：
Figure BDA0002826527830000095
其中
Figure BDA0002826527830000096
Figure BDA0002826527830000097
在另一实施例中，步骤S12还可以包括以下子步骤：
子步骤S123、获取所述右臂机器人的关节角度极限、右臂机器人的关节速度极限、右臂机器人的关节加速度极限和右臂机器人的关节突加度极限的不等式中的最大求导阶数为右臂机器人最大求导阶数。
子步骤S124、基于零化动力学方式将所述右臂机器人的关节角度极限、右臂机器人的关节速度极限、右臂机器人的关节加速度极限和右臂机器人的关节突加度极限的不等式按照所述右臂机器人最大求导阶数求导，并约束生成右臂机器人的右约束不等式：
Figure BDA0002826527830000101
其中
Figure BDA0002826527830000102
Figure BDA0002826527830000103
S13、基于所述左约束不等式和所述右约束不等式，将所述突加度层循环运动的控制方案转换生成一个标准的二次型规划问题，并调用预设的二次型规划求解器进行求解。
具体地，所述二次型规划问题为：
Figure BDA0002826527830000104
并受约束于Auyu＝du和
Figure BDA0002826527830000105
其中
Figure BDA0002826527830000106
Wu＝[In,0；0,In]，pu＝[pl；pr]，Au＝[Jl(θl),0；0,Jr(θr)]，
Figure BDA0002826527830000107
S14、获取所述预设的二次型规划求解器生成的求解结果，并根据所述求解结果驱动所述机器人的左右两个机械臂循环运动。
在实际操作中，由于机器人的机械臂是通过电机控制，为了方便控制，作为示例的，步骤S14可以包括以下子步骤：
子步骤S141、将所述求解结果转化为驱动所述机器人的控制信号。
子步骤S142、将所述控制信号发送至所述机器人的预设电机中，以使所述预设电机驱动左右两个机械臂循环运动。
参照图3，本发明一实施例提供的一种双臂机器人的循环运动控制方法的操作流程图，下面将结合实际操作说明。
在进行仿真实验时，采用的相关设定如下：设定左臂机器人的期望路径为一个类似沙漏形状的路径，右臂机器人的期望路径为一个类似肺形状的路径，其表达式分别如下：
Figure BDA0002826527830000111
其中，T是期望路径的周期；σ1、σ2、σ3、σ4是与期望路径初始位置有关的参数；设定任务执行时间为tf＝T＝30s；零化动力学设计参数为λ＝μ＝2和η＝3；设定左臂机器人的初始关节角度为[14π/15,-π/3,π/12,-π/4,-π/5]T rad，右臂机器人的初始关节角度为[π/15,π/3,-π/12,π/4,π/6]T rad；设定左臂机器人的关节角度上下限为[π,-0.9,0.3,-0.7,-0.4]T rad和[π/2,-1.1,0.1,-π/2,-0.7]T rad，右臂机器人的关节角度上下限为[π/2,1.1,-0.1,0.9,0.7]Trad和[0,0.9,-0.3,0.7,0.4]T rad；设定左臂机器人的关节速度上下限为[0.5,0.5,0.5,0.5,0.5]Trad/s和[-0.5,-0.5,-0.5,-0.5,-0.5]T rad/s，右臂机器人的关节速度上下限为[0.5,0.5,0.5,0.5,0.5]Trad/s和[-0.5,-0.5,-0.5,-0.5,-0.5]Trad/s；设定左臂机器人的关节加速度上下限为[0.3,0.3,0.3,0.3,0.3]Trad/s2和[-0.3,-0.3,-0.3,-0.3,-0.3]Trad/s2，右臂机器人的关节加速度上下限为[0.3,0.3,0.3,0.3,0.3]Trad/s2和[-0.3,-0.3,-0.3,-0.3,-0.3]T rad/s2；设定左臂机器人的关节突加度上下限为[0.2,0.2,0.2,0.2,0.2]Trad/s3和[-0.2,-0.2,-0.2,-0.2,-0.2]Trad/s3，右臂机器人的关节突加度上下限为[0.2,0.2,0.2,0.2,0.2]T rad/s3和[-0.2,-0.2,-0.2,-0.2,-0.2]Trad/s3。
参照图4，示出了双臂机器人的实际运动轨迹，其中左臂机器人完成的任务是跟踪一个类似沙漏形状的路径，右臂机器人完成的任务是跟踪一个类似肺形状的路径。
参照图5，示出了左臂机器人和右臂机器人末端执行器的实际轨迹ral、rar与期望路径rdl、rdr，其中实线为双臂机器人的实际跟踪轨迹，而虚线为期望路径。参照图4可知，实际轨迹跟期望路径两者几乎重合。
参照图6，示出了左臂机器人和右臂机器人在X轴方向和Y轴方向的跟踪误差eXl、eYl、eXr和eYr，其中左臂机器人两个方向的最大稳态跟踪误差都在10-7m的数量级，右臂机器人两个方向的最大稳态跟踪误差也都在10-7m的数量级，整体而言，跟踪精度较高。
参照图7，示出了左臂机器人的关节角度θl和右臂机器人的关节角度θr的变化情况，它们都在关节角度上下限范围内。
参照图8，示出了左臂机器人的关节速度
Figure BDA0002826527830000121
和右臂机器人的关节速度
Figure BDA0002826527830000122
的变化情况，它们都在关节速度上下限范围内。
参照图9，示出了左臂机器人的关节加速度
Figure BDA0002826527830000123
和右臂机器人的关节加速度
Figure BDA0002826527830000124
的变化情况，它们都在关节加速度上下限范围内。
参照图10，示出了左臂机器人的关节突加度
Figure BDA0002826527830000125
和右臂机器人的关节突加度
Figure BDA0002826527830000126
的变化情况，它们都在关节突加度上下限范围内。
参照图11，示出了在tf＝3T＝90s时，左臂机器人的关节角度θl和右臂机器人的关节角度θr在三个周期内的变化情况，在每个周期内，各个关节末态位置都与初始位置重合，实现了循环运动。
本发明可以分别设定两个机器人的性能指标，再根据两个机器人的性能指标确定整个机器人的运动性能指标和控制方案，并设定其约束条件，使机器人的运动可以满足物理极限，而且本发明可以简化整个控制方案，将控制方案转换成一个二次型规划问题，再根据求解结果驱动机器人运动，从而可以简化整个步骤流程，提高控制精度，并实现双臂机器人的循环运动控制。
本发明实施例还提供了一种双臂机器人的循环运动控制装置，参见图12，示出了本发明一实施例提供的一种双臂机器人的循环运动控制装置的结构示意图。
在本实施例中，所述双臂机器人的循环运动控制装置可以适用于具有左右两个机械臂的机器人。
其中，作为示例的，所述双臂机器人的循环运动控制装置可以包括：
设定模块1201，用于采用零化动力学方式分别设定左臂机器人的性能指标与右臂机器人的性能指标，并同时最小化两个性能指标得到运动性能指标，基于所述运动性能指标生成突加度层循环运动的控制方案；
机器人约束模块1202，用于基于零化动力学方式将所述左臂机器人的关节角度极限、左臂机器人的关节速度极限、左臂机器人的关节加速度极限和左臂机器人的关节突加度极限的不等式约束进行等效，生成所述左臂机器人的左约束不等式，基于零化动力学方式将所述右臂机器人的关节角度极限、右臂机器人的关节速度极限、右臂机器人的关节加速度极限和右臂机器人的关节突加度极限的不等式约束进行等效，生成所述右臂机器人的右约束不等式；
求解模块1203，用于基于所述左约束不等式和所述右约束不等式，将所述突加度层循环运动的控制方案转换生成一个标准的二次型规划问题，并调用预设的二次型规划求解器进行求解；
驱动模块1204，用于获取所述预设的二次型规划求解器生成的求解结果，并根据所述求解结果驱动所述机器人的左右两个机械臂循环运动。
进一步的，所述运动性能指标受约束于基于突加度的左臂机器人的雅可比矩阵Jl(θl)、右臂机器人的雅可比矩阵Jr(θr)、左臂机器人的关节角度极限
Figure BDA0002826527830000131
左臂机器人的关节速度极限
Figure BDA0002826527830000132
左臂机器人的关节加速度极限
Figure BDA0002826527830000133
左臂机器人的关节突加度极限
Figure BDA0002826527830000134
右臂机器人的关节角度极限
Figure BDA0002826527830000135
右臂机器人的关节速度极限
Figure BDA0002826527830000136
右臂机器人的关节加速度极限
Figure BDA0002826527830000137
右臂机器人的关节突加度极限
Figure BDA0002826527830000138
其中，所述左臂机器人的性能指标为：
Figure BDA0002826527830000139
所述右臂机器人的性能指标为：
Figure BDA00028265278300001310
所述运动性能指标为：
Figure BDA00028265278300001311
上式中，In为单位矩阵，
Figure BDA00028265278300001312
所述左臂机器人的雅可比矩阵Jl(θl)满足
Figure BDA00028265278300001313
Figure BDA00028265278300001314
所述右臂机器人的雅可比矩阵Jr(θr)满足
Figure BDA00028265278300001315
Figure BDA00028265278300001316
η和μ为零化动力学设计参数，取值大于零，rdl和rdr是预先设定的机器人左右臂的期望路径，fl(θl)和fr(θr)是机器人左右臂的前向运动学映射函数，t为时间变量。
进一步的，所述机器人约束模块还用于：
获取所述左臂机器人的关节角度极限、左臂机器人的关节速度极限、左臂机器人的关节加速度极限和左臂机器人的关节突加度极限的不等式中的最大求导阶数为左臂机器人最大求导阶数；
基于零化动力学方式将所述左臂机器人的关节角度极限、左臂机器人的关节速度极限、左臂机器人的关节加速度极限和左臂机器人的关节突加度极限的不等式按照所述左臂机器人最大求导阶数求导，并约束生成左臂机器人的左约束不等式：
Figure BDA0002826527830000141
其中
Figure BDA0002826527830000142
Figure BDA0002826527830000143
进一步的，所述机器人约束模块还用于：
获取所述右臂机器人的关节角度极限、右臂机器人的关节速度极限、右臂机器人的关节加速度极限和右臂机器人的关节突加度极限的不等式中的最大求导阶数为右臂机器人最大求导阶数；
基于零化动力学方式将所述右臂机器人的关节角度极限、右臂机器人的关节速度极限、右臂机器人的关节加速度极限和右臂机器人的关节突加度极限的不等式按照所述右臂机器人最大求导阶数求导，并约束生成右臂机器人的右约束不等式：
Figure BDA0002826527830000144
其中
Figure BDA0002826527830000145
Figure BDA0002826527830000146
进一步的，所述二次型规划问题为：
Figure BDA0002826527830000147
并受约束于Auyu＝du和
Figure BDA0002826527830000148
其中
Figure BDA0002826527830000149
Wu＝[In,0；0,In]，pu＝[pl；pr]，Au＝[Jl(θl),0；0,Jr(θr)]，
Figure BDA00028265278300001410
进一步的，所述驱动模块还用于：
将所述求解结果转化为驱动所述机器人的控制信号；
将所述控制信号发送至所述机器人的预设电机中，以使所述预设电机驱动左右两个机械臂循环运动。
进一步的，本申请实施例还提供了一种电子设备，包括：存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现如上述实施例所述的双臂机器人的循环运动控制方法。
进一步的，本申请实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机可执行指令，所述计算机可执行指令用于使计算机执行如上述实施例所述的双臂机器人的循环运动控制方法。
以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也视为本发明的保护范围。

Claims (10)

1.一种双臂机器人的循环运动控制方法，其特征在于，适用于具有左右两个机械臂的机器人，所述方法包括：
采用零化动力学方式分别设定左臂机器人的性能指标与右臂机器人的性能指标，并同时最小化两个性能指标得到运动性能指标，基于所述运动性能指标生成突加度层循环运动的控制方案；
基于零化动力学方式将所述左臂机器人的关节角度极限、左臂机器人的关节速度极限、左臂机器人的关节加速度极限和左臂机器人的关节突加度极限的不等式约束进行等效，生成所述左臂机器人的左约束不等式，基于零化动力学方式将所述右臂机器人的关节角度极限、右臂机器人的关节速度极限、右臂机器人的关节加速度极限和右臂机器人的关节突加度极限的不等式约束进行等效，生成所述右臂机器人的右约束不等式；
基于所述左约束不等式和所述右约束不等式，将所述突加度层循环运动的控制方案转换生成一个标准的二次型规划问题，并调用预设的二次型规划求解器进行求解；
获取所述预设的二次型规划求解器生成的求解结果，并根据所述求解结果驱动所述机器人的左右两个机械臂循环运动。
2.根据权利要求1所述的双臂机器人的循环运动控制方法，其特征在于，所述运动性能指标受约束于基于突加度的左臂机器人的雅可比矩阵Jl(θl)、右臂机器人的雅可比矩阵Jr(θr)、左臂机器人的关节角度极限
Figure FDA0002826527820000011
左臂机器人的关节速度极限
Figure FDA0002826527820000012
左臂机器人的关节加速度极限
Figure FDA0002826527820000013
左臂机器人的关节突加度极限
Figure FDA0002826527820000014
右臂机器人的关节角度极限
Figure FDA0002826527820000015
右臂机器人的关节速度极限
Figure FDA0002826527820000016
右臂机器人的关节加速度极限
Figure FDA0002826527820000017
右臂机器人的关节突加度极限
Figure FDA0002826527820000018
其中，所述左臂机器人的性能指标为：
Figure FDA0002826527820000019
所述右臂机器人的性能指标为：
Figure FDA0002826527820000021
所述运动性能指标为：
Figure FDA0002826527820000022
上式中，In为单位矩阵，
Figure FDA0002826527820000023
所述左臂机器人的雅可比矩阵Jl(θl)满足
Figure FDA0002826527820000024
Figure FDA0002826527820000025
所述右臂机器人的雅可比矩阵Jr(θr)满足
Figure FDA0002826527820000026
Figure FDA0002826527820000027
η和μ为零化动力学设计参数，取值大于零，rdl和rdr是预先设定的机器人左右臂的期望路径，fl(θl)和fr(θr)是机器人左右臂的前向运动学映射函数，t为时间变量。
3.根据权利要求1所述的双臂机器人的循环运动控制方法，其特征在于，所述基于零化动力学方式将所述左臂机器人的关节角度极限、左臂机器人的关节速度极限、左臂机器人的关节加速度极限和左臂机器人的关节突加度极限的不等式约束进行等效，生成所述左臂机器人的左约束不等式，包括：
获取所述左臂机器人的关节角度极限、左臂机器人的关节速度极限、左臂机器人的关节加速度极限和左臂机器人的关节突加度极限的不等式中的最大求导阶数为左臂机器人最大求导阶数；
基于零化动力学方式将所述左臂机器人的关节角度极限、左臂机器人的关节速度极限、左臂机器人的关节加速度极限和左臂机器人的关节突加度极限的不等式按照所述左臂机器人最大求导阶数求导，并约束生成左臂机器人的左约束不等式：
Figure FDA0002826527820000028
其中
Figure FDA0002826527820000029
Figure FDA00028265278200000210
4.根据权利要求1所述的双臂机器人的循环运动控制方法，其特征在于，所述基于零化动力学方式将所述右臂机器人的关节角度极限、右臂机器人的关节速度极限、右臂机器人的关节加速度极限和右臂机器人的关节突加度极限的不等式约束进行等效，生成所述右臂机器人的右约束不等式，包括：
获取所述右臂机器人的关节角度极限、右臂机器人的关节速度极限、右臂机器人的关节加速度极限和右臂机器人的关节突加度极限的不等式中的最大求导阶数为右臂机器人最大求导阶数；
基于零化动力学方式将所述右臂机器人的关节角度极限、右臂机器人的关节速度极限、右臂机器人的关节加速度极限和右臂机器人的关节突加度极限的不等式按照所述右臂机器人最大求导阶数求导，并约束生成右臂机器人的右约束不等式：
Figure FDA0002826527820000031
其中
Figure FDA0002826527820000032
Figure FDA0002826527820000033
5.根据权利要求1所述的双臂机器人的循环运动控制方法，其特征在于，所述二次型规划问题为：
Figure FDA0002826527820000034
并受约束于Auyu＝du和
Figure FDA0002826527820000035
其中
Figure FDA0002826527820000036
Wu＝[In,0；0,In]，pu＝[pl；pr]，Au＝[Jl(θl),0；0,Jr(θr)]，
Figure FDA0002826527820000037
Figure FDA0002826527820000038
6.根据权利要求1所述的双臂机器人的循环运动控制方法，其特征在于，根据所述求解结果驱动所述机器人的左右两个机械臂循环运动，包括：
将所述求解结果转化为驱动所述机器人的控制信号；
将所述控制信号发送至所述机器人的预设电机中，以使所述预设电机驱动左右两个机械臂循环运动。
7.一种双臂机器人的循环运动控制装置，其特征在于，适用于具有左右两个机械臂的机器人，所述装置包括：
设定模块，用于采用零化动力学方式分别设定左臂机器人的性能指标与右臂机器人的性能指标，并同时最小化两个性能指标得到运动性能指标，基于所述运动性能指标生成突加度层循环运动的控制方案；
机器人约束模块，用于基于零化动力学方式将所述左臂机器人的关节角度极限、左臂机器人的关节速度极限、左臂机器人的关节加速度极限和左臂机器人的关节突加度极限的不等式约束进行等效，生成所述左臂机器人的左约束不等式，基于零化动力学方式将所述右臂机器人的关节角度极限、右臂机器人的关节速度极限、右臂机器人的关节加速度极限和右臂机器人的关节突加度极限的不等式约束进行等效，生成所述右臂机器人的右约束不等式；
求解模块，用于基于所述左约束不等式和所述右约束不等式，将所述突加度层循环运动的控制方案转换生成一个标准的二次型规划问题，并调用预设的二次型规划求解器进行求解；
驱动模块，用于获取所述预设的二次型规划求解器生成的求解结果，并根据所述求解结果驱动所述机器人的左右两个机械臂循环运动。
8.根据权利要求7所述的双臂机器人的循环运动控制装置，其特征在于，所述运动性能指标受约束于基于突加度的左臂机器人的雅可比矩阵Jl(θl)、右臂机器人的雅可比矩阵Jr(θr)、左臂机器人的关节角度极限
Figure FDA0002826527820000041
左臂机器人的关节速度极限
Figure FDA0002826527820000042
左臂机器人的关节加速度极限
Figure FDA0002826527820000043
左臂机器人的关节突加度极限
Figure FDA0002826527820000044
右臂机器人的关节角度极限
Figure FDA0002826527820000045
右臂机器人的关节速度极限
Figure FDA0002826527820000046
右臂机器人的关节加速度极限
Figure FDA0002826527820000047
右臂机器人的关节突加度极限
Figure FDA0002826527820000048
其中，所述左臂机器人的性能指标为：
Figure FDA0002826527820000049
所述右臂机器人的性能指标为：
Figure FDA00028265278200000410
所述运动性能指标为：
Figure FDA00028265278200000411
上式中，In为单位矩阵，
Figure FDA00028265278200000412
所述左臂机器人的雅可比矩阵Jl(θl)满足
Figure FDA00028265278200000413
Figure FDA00028265278200000414
所述右臂机器人的雅可比矩阵Jr(θr)满足
Figure FDA00028265278200000415
Figure FDA0002826527820000051
η和μ为零化动力学设计参数，取值大于零，rdl和rdr是预先设定的机器人左右臂的期望路径，fl(θl)和fr(θr)是机器人左右臂的前向运动学映射函数，t为时间变量。
9.一种电子设备，包括：存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现如权利要求1-6任意一项所述的双臂机器人的循环运动控制方法。
10.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机可执行指令，所述计算机可执行指令用于使计算机执行如权利要求1-6任意一项所述的双臂机器人的循环运动控制方法。
CN202011430600.3A 2020-12-09 2020-12-09 一种双臂机器人的循环运动控制方法及装置 Pending CN112605992A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011430600.3A CN112605992A (zh) 2020-12-09 2020-12-09 一种双臂机器人的循环运动控制方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011430600.3A CN112605992A (zh) 2020-12-09 2020-12-09 一种双臂机器人的循环运动控制方法及装置

Publications (1)

Publication Number Publication Date
CN112605992A true CN112605992A (zh) 2021-04-06

Family

ID=75229636

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011430600.3A Pending CN112605992A (zh) 2020-12-09 2020-12-09 一种双臂机器人的循环运动控制方法及装置

Country Status (1)

Country Link
CN (1) CN112605992A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114147716A (zh) * 2021-12-09 2022-03-08 乐聚(深圳)机器人技术有限公司 机器人的控制方法、装置、控制器及存储介质
CN115890653A (zh) * 2022-09-28 2023-04-04 华中科技大学 基于多通道的双臂机器人协同控制方法、装置及可读介质

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104760041A (zh) * 2015-03-19 2015-07-08 中山大学 一种基于突加度的障碍物躲避运动规划方法
CN105538327A (zh) * 2016-03-03 2016-05-04 吉首大学 一种基于突加度的冗余度机械臂重复运动规划方法
CN105563490A (zh) * 2016-03-03 2016-05-11 吉首大学 一种移动机械臂障碍物躲避的容错运动规划方法
CN106926238A (zh) * 2017-02-16 2017-07-07 香港理工大学深圳研究院 基于突加度的多冗余度机械臂系统的协同控制方法与装置
CN107351081A (zh) * 2017-06-27 2017-11-17 华侨大学 具有速度优化特性的冗余度机械臂突加度层运动规划方法
JP2019177450A (ja) * 2018-03-30 2019-10-17 日本電産株式会社 キャリブレーション精度の評価方法及び評価装置
CN110561440A (zh) * 2019-09-27 2019-12-13 华侨大学 一种冗余度机械臂加速度层多目标规划方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104760041A (zh) * 2015-03-19 2015-07-08 中山大学 一种基于突加度的障碍物躲避运动规划方法
CN105538327A (zh) * 2016-03-03 2016-05-04 吉首大学 一种基于突加度的冗余度机械臂重复运动规划方法
CN105563490A (zh) * 2016-03-03 2016-05-11 吉首大学 一种移动机械臂障碍物躲避的容错运动规划方法
CN106926238A (zh) * 2017-02-16 2017-07-07 香港理工大学深圳研究院 基于突加度的多冗余度机械臂系统的协同控制方法与装置
CN107351081A (zh) * 2017-06-27 2017-11-17 华侨大学 具有速度优化特性的冗余度机械臂突加度层运动规划方法
JP2019177450A (ja) * 2018-03-30 2019-10-17 日本電産株式会社 キャリブレーション精度の評価方法及び評価装置
CN110561440A (zh) * 2019-09-27 2019-12-13 华侨大学 一种冗余度机械臂加速度层多目标规划方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
MIN YANG等: "Jerk-level cyclic motion planning and control for constrained redundant robot manipulators using Zhang dynamics: Theoretics", 《IEEE XPLORE》 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114147716A (zh) * 2021-12-09 2022-03-08 乐聚(深圳)机器人技术有限公司 机器人的控制方法、装置、控制器及存储介质
CN115890653A (zh) * 2022-09-28 2023-04-04 华中科技大学 基于多通道的双臂机器人协同控制方法、装置及可读介质

Similar Documents

Publication Publication Date Title
CN112757306B (zh) 一种机械臂逆解多解选择和时间最优轨迹规划算法
CN104723340B (zh) 基于连接和阻尼配置的柔性关节机械臂的阻抗控制方法
US20150045954A1 (en) Robot apparatus and robot controlling method
CN112605992A (zh) 一种双臂机器人的循环运动控制方法及装置
CN111702767A (zh) 一种基于反演模糊自适应的机械手阻抗控制方法
CN108181836B (zh) 一种针对柔性Timoshenko梁机械臂抗饱和的边界控制方法
US9676100B2 (en) Control apparatus of robot, robot, and program thereof
CN109129486A (zh) 一种抑制周期噪声的冗余度机械臂重复运动规划方法
CN115502966A (zh) 用于机器人的可变导纳控制方法
CN115890735B (zh) 机械臂系统、机械臂及其控制方法、控制器和存储介质
CN110744540B (zh) 一种工业机器人变阻抗控制器参数整定的方法
Dian et al. A Novel Disturbance-Rejection Control Framework for Cable-Driven Continuum Robots With Improved State Parameterizations
CN112668190B (zh) 一种三指灵巧手控制器构建方法、系统、设备及存储介质
Reis et al. Modeling and control of a multifingered robot hand for object grasping and manipulation tasks
CN112847373A (zh) 一种机器人轨迹同步控制方法、计算机可读存储介质
CN112180719A (zh) 基于人机协作系统的新型鲁棒有限时间轨迹控制方法
Yu et al. A haptic shared control algorithm for flexible human assistance to semi-autonomous robots
CN113848958B (zh) 基于四元数的全驱动抗退绕水下机器人有限时间容错轨迹跟踪控制方法
CN117290636A (zh) 一种scara机器人不等式约束模型构建方法及模型验证方法
Li et al. Indirect adaptive impedance control for dual-arm cooperative manipulation
Khurpade et al. A Virtual Model of 2D Planar Manipulator Dynamics
Botsali et al. Fuzzy logic trajectory control of flexible robot manipulator with rotating prismatic joint
Jiang et al. Compliant impedance control for a redundant manipulator during human robot interaction
Ha et al. Wireless-communicated computed-torque control of a SCARA robot and two-dimensional input shaping for a spherical pendulum
CN112828894B (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
RJ01 Rejection of invention patent application after publication

Application publication date: 20210406

RJ01 Rejection of invention patent application after publication