CN111037560A - 一种协作机器人柔顺力控制方法及*** - Google Patents

一种协作机器人柔顺力控制方法及*** Download PDF

Info

Publication number
CN111037560A
CN111037560A CN201911358551.4A CN201911358551A CN111037560A CN 111037560 A CN111037560 A CN 111037560A CN 201911358551 A CN201911358551 A CN 201911358551A CN 111037560 A CN111037560 A CN 111037560A
Authority
CN
China
Prior art keywords
robot
representing
joint
force
control
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
CN201911358551.4A
Other languages
English (en)
Other versions
CN111037560B (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.)
Institute of Intelligent Manufacturing of Guangdong Academy of Sciences
Original Assignee
Guangdong Institute of Intelligent Manufacturing
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 Guangdong Institute of Intelligent Manufacturing filed Critical Guangdong Institute of Intelligent Manufacturing
Priority to CN201911358551.4A priority Critical patent/CN111037560B/zh
Publication of CN111037560A publication Critical patent/CN111037560A/zh
Application granted granted Critical
Publication of CN111037560B publication Critical patent/CN111037560B/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/1628—Programme controls characterised by the control loop
    • B25J9/1633—Programme controls characterised by the control loop compliant, force, torque control, e.g. combined with position control
    • 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/1605—Simulation of manipulator lay-out, design, modelling of manipulator
    • 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)
  • Manipulator (AREA)

Abstract

本发明公开了一种协作机器人柔顺力控制方法及***，其中，所述方法包括：获得机器人的状态变量并进行初始化；基于初始化状态变量获得当前旋转矩阵；基于当前旋转矩阵读取所述机器人的当前状态反馈信息；基于当前状态反馈信息构建实现柔顺力控制的等式约束以及机器人***内的关节角度、关节角速度与关节力矩的不等式约束；对关节力矩函数进行改写，并获得最终的约束优化模型；基于动态神经网络模型更新最终的约束优化模型中的状态变量和控制力矩；判断当前时间是否大于任务时间，若是，结束柔顺力控制，反之，返回获得当前旋转矩阵。在本发明实施例中，能够同时实现接触力方向的高精度力控制以及***方向上的运动控制。

Description

一种协作机器人柔顺力控制方法及***
技术领域
本发明涉及机器人的智能控制技术领域，尤其涉及一种协作机器人柔顺力控制方法及***。
背景技术
协作机器人是一种能够与人类在共同工作空间内协同工作的机器人。协作机器人能够直接和人类员工一起并肩工作而无需使用安全围栏进行隔离，且具有产线部署快速、任务切换简单、人机友好性好等特点，在医疗护理、轻工装配、电子信息以及家庭服务等领域有广阔的应用前景，被认为是实现“工业4.0”与“智能制造2025”的重要载体。
协作机器人进入实际应用最重要的前提是实现“人机共融”，其中提高***的柔顺性尤为重要。力控制可以提升***的柔顺性，增强人机交互，提供智能响应，使机器人在弱结构化、非结构化环境中自主操作。因此其应用领域更加广泛，例如柔性装配、双臂协调操作、人机交互、灵巧手抓取物体、足式机器人步态控制等。针对冗余机器人力控制的研究具有极强的应用价值与现实意义。
现有针对具有冗余自由度的协作机器人的力控制方法，主要是基于机器人雅克比矩阵的伪逆计算实现；但是这种方法普遍存在以下问题：1)对雅克比矩阵的伪逆计算导致计算成本过高；2)难以处理***的物理约束。
发明内容
本发明的目的在于克服现有技术的不足，本发明提供了一种协作机器人柔顺力控制方法及***，能够同时实现接触力方向的高精度力控制以及***方向上的运动控制，能够实现对关节力矩的在线优化，能够在柔顺力控制过程中保证机器人不超过其物理约束。
为了解决上述技术问题，本发明实施例提供了一种协作机器人柔顺力控制方法，所述方法包括：
获得机器人的状态变量并进行初始化；
基于初始化状态变量获得当前旋转矩阵；
基于所述当前旋转矩阵读取所述机器人的当前状态反馈信息；
基于所述当前状态反馈信息构建实现柔顺力控制的等式约束以及机器人***内的关节角度、关节角速度与关节力矩的不等式约束；
对关节力矩函数进行改写，并获得最终的约束优化模型；
基于动态神经网络模型更新最终的约束优化模型中的状态变量和控制力矩；
判断当前时间是否大于任务时间，若是，结束对所述机器人的柔顺力控制，反之，返回基于初始化状态变量获得当前旋转矩阵。
可选的，所述获得机器人的状态变量并进行初始化之前，还包括：
针对所述机器人运动与工件之间的接触力的正交特点，在工具坐标系和基座标系内进行分别建模，获得机器人运动建模***；
其中，基座标系的表示R0(x0,y0,z0)；工具坐标系的表示Rt(xt,yt,zt)。
可选的，所述机器人与工件之间的接触力与所述工具坐标系中的zt平行，同时xt与yt定义了所述机器人末端执行器的***；
在所述机器人操作过程中，所述机器人末端执行器实际位置x与期望轨迹xd存在一微小偏差，在所述基座标系R0(x0,y0,z0)和所述工具坐标系Rt(xt,yt,zt)下分别描述为所述机器人在基座标系下的与期望轨迹的偏差δX和所述机器人在工具坐标系下的与期望轨迹的偏差δXt。
可选的，所述在工具坐标系和基座标系内进行分别建模，获得机器人运动建模***的过程如下：
在所述工具坐标系Rt(xt,yt,zt)下由于忽略所述机器人与所述工件之间的摩擦，假设所述机器人与所述工件之间的接触为刚度接触，则所述接触力可以描述为：
Ft＝kf∑tδXt； (1)
其中，kf表示刚度系数；∑t＝diag(0,0,1)为一个对角矩阵，用于描述所述机器人在工具坐标系下的与期望轨迹的偏差δXt与其接触力之间的关系，其中，0表示在该方向上的位移不会产生接触力，反之取1；
定义
Figure BDA0002336595810000031
则在所述工具坐标系Rt(xt,yt,zt)下的位置跟踪误差et为：
Figure BDA0002336595810000032
在接触面已知的情况下，使用一个旋转矩阵St描述所述工具坐标系Rt(xt,yt,zt)和基座标系R0(x0,y0,z0)之间的旋转关系；
定义F和e0分别为所述基座标系R0(x0,y0,z0)下δXt与Ft的对应描述，则有：
Figure BDA0002336595810000033
et＝Ste0； (4)
δXt＝StδX； (5)
通过公式(1)-(5)的联立，则有：
Figure BDA0002336595810000034
Figure BDA0002336595810000035
其中，δX表示所述机器人在所述基座标系R0(x0,y0,z0)下的与期望轨迹的偏差；F表示在基座标系R0(x0,y0,z0)下的接触力；
在基座标系R0(x0,y0,z0)中位移δX可以描述为δX＝x-xd，其中，期望轨迹xd是R0中描述的期望位置信号，因此，对公式(6)-(7)重写为：
Figure BDA0002336595810000036
Figure BDA0002336595810000037
定义所述机器人的期望轨迹和指令力分别为xd和Fd；则根据公式(5)与公式(9)的描述，控制目标可以描述为设计面向冗余机器人的位置-来控制策略，使公式(8)描述的接触力F→Fd，同时使公式(9)描述的跟踪误差e0→0；∑t表示描述接触的参数矩阵；
Figure BDA0002336595810000038
表示描述运动的参数矩阵。
可选的，所述机器人运动建模***中为了简化描述，则定义
Figure BDA0002336595810000039
Figure BDA00023365958100000310
rd＝[Fd；0]，
Figure BDA00023365958100000311
则有公式(8)和公式(9)重写为：
A(f(θ)-xd)＝r； (10)
所述控制目标描述为通过设计关节，使r＝rd；∑t表示描述接触的参数矩阵；
Figure BDA00023365958100000312
表示描述运动的参数矩阵。
可选的，所述基于所述当前状态反馈信息构建实现柔顺力控制的等式约束，包括：
在机器人运动建模***下获得的当前状态反馈信息，对给定的期望轨迹xd与接触力Ft，实现位置-来控制的目标为：
Figure BDA0002336595810000041
则定义误差向量：
e＝r-rd＝[F-Fd；e0]； (16)
则等式可以在速度层重建为如下形式：
Figure BDA0002336595810000042
其中，k表示一正控制常数；
Figure BDA0002336595810000043
表示所述机器人的关节角速度；
Figure BDA0002336595810000044
表示误差向量的一阶导数；
Figure BDA0002336595810000045
表示期望轨迹的一阶导数；
Figure BDA0002336595810000046
表示rd的一阶导数，rd＝[Fd；0]，Fd表示机器人的力指令。
可选的，所述机器人***内的关节角度、关节角速度与关节力矩的不等式约束，包括：
将不等式约束归一化描述为速度层的不等式约束：
Figure BDA0002336595810000047
其中，
Figure BDA0002336595810000048
则关节力矩的不等式约束可以重写为：
Figure BDA0002336595810000049
其中，β>0，则对当末端执行器与工件的在基座标系R0(x0,y0,z0)下接触力为F时，其在各关节处施加的作用力矩的表达式求导可得：
Figure BDA00023365958100000410
联立公式(18)和(19)，即可得到关节力矩在角速度层的描述：
Figure BDA00023365958100000411
其中，
Figure BDA00023365958100000412
Figure BDA00023365958100000413
J表示Jacobian矩阵；
Figure BDA00023365958100000414
表示关节力矩约束的一阶导数；τ表示关节力矩约束；β表示正控制参数；θ表示机器人的关节角度；
Figure BDA00023365958100000415
表示机器人的关节角速度；H表示一个实数数组；
Figure BDA00023365958100000416
表示所述机器人的指令力的一阶导数；
Figure BDA00023365958100000417
表示实数。
可选的，所述对关节力矩函数进行改写，并获得最终的约束优化模型，包括：
将目标函数进行简化，使用所述机器人的指令力Fd代替在基座标系R0(x0,y0,z0)下的接触力F，则有：
Figure BDA0002336595810000051
若公式(13)描述的目标函数定义在关节角度层，由于最终的控制量为关节角速度
Figure BDA0002336595810000052
因此通过求取G2对θ的梯度，得到其在速度层上的替代描述：
Figure BDA0002336595810000053
对JT(θ)Fd求导得到：
Figure BDA0002336595810000054
其中，
Figure BDA0002336595810000055
是：
Figure BDA0002336595810000056
令H＝[H1,…,Hn]，则上式可描述数为：
Figure BDA0002336595810000057
由于公式(15)中的第二项与
Figure BDA0002336595810000058
不相关，则选择最终目标函数选取为
Figure BDA0002336595810000059
通过引入一个修正项
Figure BDA00023365958100000510
对目标函数中的
Figure BDA00023365958100000511
进行凸化处理，则最终的约束优化模型为：
Figure BDA00023365958100000512
Figure BDA00023365958100000513
Figure BDA00023365958100000514
Figure BDA00023365958100000515
其中，
Figure BDA00023365958100000516
表示所述机器人的指令力的转秩；J表示Jacobian矩阵；
Figure BDA00023365958100000517
表示机器人的关节角速度；rr表示参考指令；A表示简写的矩阵。
可选的，所述基于动态神经网络模型更新最终的约束优化模型中的状态变量和控制力矩，包括：
定义输入状态变量
Figure BDA0002336595810000061
为所述约束优化模型中的对偶状态变量，则拉格朗日函数选取为：
Figure BDA0002336595810000062
根据Karush-Kuhn-Tucker条件，优化所述约束优化模型中的最优解等价地表述为：
Figure BDA0002336595810000063
Figure BDA0002336595810000064
Figure BDA0002336595810000065
其中，PΩ(·)是一个限幅函数，定义为：
Figure BDA0002336595810000066
Figure BDA0002336595810000067
为一个投影函数，定义为：
Figure BDA0002336595810000068
在实时求解公式(24)时，基于动态神经网络模型的位置-力控制器设计设计为：
Figure BDA0002336595810000069
Figure BDA00023365958100000610
Figure BDA00023365958100000611
其中，
Figure BDA00023365958100000612
Figure BDA00023365958100000613
表示实数；
Figure BDA00023365958100000614
表示输入状态量λ2的一阶导数；
Figure BDA00023365958100000615
表示输入状态量λ1的一阶导数；
Figure BDA00023365958100000616
表示关节角速度；
Figure BDA00023365958100000617
表示关节角加速度；J表示Jacobian矩阵；rr表示参考指令；A表示简写矩阵；g1表示第一个元素；g2m表示第2m个元素；∈表示一个正常数。
另外，本发明实施例还提供了一种协作机器人柔顺力控制***，所述***包括：
初始化模块：用于获得机器人的状态变量并进行初始化；
获得模块：用于基于初始化状态变量获得当前旋转矩阵；
读取模块：用于基于所述当前旋转矩阵读取所述机器人的当前状态反馈信息；
构建模块：用于基于所述当前状态反馈信息构建实现柔顺力控制的等式约束以及机器人***内的关节角度、关节角速度与关节力矩的不等式约束；
改写模块：用于对关节力矩函数进行改写，并获得最终的约束优化模型；
更新模块：用于基于动态神经网络模型更新最终的约束优化模型中的状态变量和控制力矩；
判断模块：用于判断当前时间是否大于任务时间，若是，结束对所述机器人的柔顺力控制，反之，返回基于初始化状态变量获得当前旋转矩阵。
在本发明实施例中，通过实时本发明的方法，能够同时实现接触力方向的高精度力控制以及***方向上的运动控制；能够实现对关节力矩的在线优化；以及能够在柔顺力控制过程中保证机器人不超过其物理约束。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见的，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。
图1是本发明实施例中的协作机器人柔顺力控制方法的流程示意图；
图2是本发明实施例中的机器人位置-力控制示意图；
图3是本发明实施例中的协作机器人柔顺力控制***的结构组成示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
实施例
请参阅图1，图1是本发明实施例中的协作机器人柔顺力控制方法的流程示意图。
如图1所示，一种协作机器人柔顺力控制方法，所述方法包括：
S11：获得机器人的状态变量并进行初始化；
在本发明具体实施过程中，所述获得机器人的状态变量并进行初始化之前，还包括：针对所述机器人运动与工件之间的接触力的正交特点，在工具坐标系和基座标系内进行分别建模，获得机器人运动建模***；其中，基座标系的表示R0(x0,y0,z0)；工具坐标系的表示Rt(xt,yt,zt)。
进一步的，所述机器人与工件之间的接触力与所述工具坐标系中的zt平行，同时xt与yt定义了所述机器人末端执行器的***；
在所述机器人操作过程中，所述机器人末端执行器实际位置x与期望轨迹xd存在一微小偏差，在所述基座标系R0(x0,y0,z0)和所述工具坐标系Rt(xt,yt,zt)下分别描述为所述机器人在基座标系下的与期望轨迹的偏差δX和所述机器人在工具坐标系下的与期望轨迹的偏差δXt。
进一步的，所述在工具坐标系和基座标系内进行分别建模，获得机器人运动建模***的过程如下：
在所述工具坐标系Rt(xt,yt,zt)下由于忽略所述机器人与所述工件之间的摩擦，假设所述机器人与所述工件之间的接触为刚度接触，则所述接触力可以描述为：
Ft＝kf∑tδXt； (1)
其中，kf表示刚度系数；∑t＝diag(0,0,1)为一个对角矩阵，用于描述所述机器人在工具坐标系下的与期望轨迹的偏差δXt与其接触力之间的关系，其中，0表示在该方向上的位移不会产生接触力，反之取1；
定义
Figure BDA0002336595810000081
则在所述工具坐标系Rt(xt,yt,zt)下的位置跟踪误差et为：
Figure BDA0002336595810000091
在接触面已知的情况下，使用一个旋转矩阵St描述所述工具坐标系Rt(xt,yt,zt)和基座标系R0(x0,y0,z0)之间的旋转关系；
定义F和e0分别为所述基座标系R0(x0,y0,z0)下δXt与Ft的对应描述，则有：
Figure BDA0002336595810000092
et＝Ste0； (4)
δXt＝StδX； (5)
通过公式(1)-(5)的联立，则有：
Figure BDA0002336595810000093
Figure BDA0002336595810000094
其中，δX表示所述机器人在所述基座标系R0(x0,y0,z0)下的与期望轨迹的偏差；F表示在基座标系R0(x0,y0,z0)下的接触力；
在基座标系R0(x0,y0,z0)中位移δX可以描述为δX＝x-xd，其中，期望轨迹xd是R0中描述的期望位置信号，因此，对公式(6)-(7)重写为：
Figure BDA0002336595810000095
Figure BDA0002336595810000096
定义所述机器人的期望轨迹和指令力分别为xd和Fd；则根据公式(5)与公式(9)的描述，控制目标可以描述为设计面向冗余机器人的位置-来控制策略，使公式(8)描述的接触力F→Fd，同时使公式(9)描述的跟踪误差e0→0；∑t表示描述接触的参数矩阵；
Figure BDA0002336595810000097
表示描述运动的参数矩阵。
进一步的，所述机器人运动建模***中为了简化描述，则定义
Figure BDA0002336595810000098
Figure BDA0002336595810000099
rd＝[Fd；0]，
Figure BDA00023365958100000910
则有公式(8)和公式(9)重写为：
A(f(θ)-xd)＝r； (10)
所述控制目标描述为通过设计关节，使r＝rd；∑t表示描述接触的参数矩阵；
Figure BDA00023365958100000911
表示描述运动的参数矩阵。
具体的，首先根据针对机器人运动与接触力的正交特点，在工具坐标系和基座标系内进行分别建模；为了不失一般性，定义具坐标系Rt(xt,yt,zt)与基座标系R0(x0,y0,z0)，具体如图2所示，该机器人与工件之间的接触力与工具坐标系中的zt平行，同时xt与yt定义了所述机器人末端执行器的***；在操作过程中，机器人末端执行器实际位置x与期望轨迹xd存在一微小偏差，在基座标系R0(x0,y0,z0)和工具坐标系Rt(xt,yt,zt)下分别描述为机器人在基座标系下的与期望轨迹的偏差δX和机器人在工具坐标系下的与期望轨迹的偏差δXt。
在工具坐标系Rt(xt,yt,zt)下由于忽略所述机器人与所述工件之间的摩擦，假设机器人与所述工件之间的接触为刚度接触，则所述接触力可以描述为：
Ft＝kf∑tδXt； (1)
其中，kf表示刚度系数；∑t＝diag(0,0,1)为一个对角矩阵，用于描述所述机器人在工具坐标系下的与期望轨迹的偏差δXt与其接触力之间的关系，其中，0表示在该方向上的位移不会产生接触力，反之取1。
定义
Figure BDA0002336595810000101
则在工具坐标系Rt(xt,yt,zt)下的位置跟踪误差et为：
Figure BDA0002336595810000102
在接触面已知的情况下，使用一个旋转矩阵St描述工具坐标系Rt(xt,yt,zt)和基座标系R0(x0,y0,z0)之间的旋转关系。
定义F和e0分别为基座标系R0(x0,y0,z0)下δXt与Ft的对应描述，则有：
Figure BDA0002336595810000103
et＝Ste0； (4)
δXt＝StδX； (5)
通过公式(1)-(5)的联立，则有：
Figure BDA0002336595810000104
Figure BDA0002336595810000105
其中，δX表示机器人在所述基座标系R0(x0,y0,z0)下的与期望轨迹的偏差；F表示在基座标系R0(x0,y0,z0)下的接触力。
值得注意的是，在基座标系R0(x0,y0,z0)中位移δX可以描述为δX＝x-xd，其中，期望轨迹xd是R0中描述的期望位置信号，因此，对公式(6)-(7)重写为：
Figure BDA0002336595810000111
Figure BDA0002336595810000112
定义所述机器人的期望轨迹和指令力分别为xd和Fd；则根据公式(5)与公式(9)的描述，控制目标可以描述为设计面向冗余机器人的位置-来控制策略，使公式(8)描述的接触力F→Fd，同时使公式(9)描述的跟踪误差e0→0；∑t表示描述接触的参数矩阵；
Figure BDA0002336595810000113
表示描述运动的参数矩阵。
为了简化描述，定义
Figure BDA0002336595810000114
rd＝[Fd；0]，
Figure BDA0002336595810000115
则有公式(8)和公式(9)重写为：
A(f(θ)-xd)＝r； (10)
控制目标描述为通过设计关节，使r＝rd；∑t表示描述接触的参数矩阵；
Figure BDA0002336595810000116
表示描述运动的参数矩阵。
在构建机器人的***建模模型之后，然后获得机器人的状态变量，然后进行相应的初始化。
S12：基于初始化状态变量获得当前旋转矩阵；
在本发明具体实施过程中，在获得初始化状态变量之后，根据该初始化状态变量来获得当前旋转矩阵；即所建模的机器人***内的旋转矩阵St。
S13：基于所述当前旋转矩阵读取所述机器人的当前状态反馈信息；
在本发明具体实施过程中，通过该当前旋转矩阵获得机器人状态反馈的反馈信息，即通过机器人***获得的状态反馈信息包括在基座标系R0(x0,y0,z0)下的接触力、机器人的关节角速度和机器人的关节角度。
S14：基于所述当前状态反馈信息构建实现柔顺力控制的等式约束以及机器人***内的关节角度、关节角速度与关节力矩的不等式约束；
在本发明具体实施过程中，所述基于所述当前状态反馈信息构建实现柔顺力控制的等式约束，包括：在机器人运动建模***下获得的当前状态反馈信息，对给定的期望轨迹xd与接触力Ft，实现位置-来控制的目标为：
Figure BDA0002336595810000117
则定义误差向量：
e＝r-rd＝[F-Fd；e0]； (16)
则等式可以在速度层重建为如下形式：
Figure BDA0002336595810000121
其中，k表示一正控制常数；
Figure BDA0002336595810000122
表示所述机器人的关节角速度；
Figure BDA0002336595810000123
表示误差向量的一阶导数；
Figure BDA0002336595810000124
表示期望轨迹的一阶导数；
Figure BDA0002336595810000125
表示rd的一阶导数，rd＝[Fd；0]，Fd表示机器人的力指令。
进一步的，所述机器人***内的关节角度、关节角速度与关节力矩的不等式约束，包括：将不等式约束归一化描述为速度层的不等式约束：
Figure BDA0002336595810000126
Figure BDA0002336595810000127
其中，
Figure BDA0002336595810000128
则关节力矩的不等式约束可以重写为：
Figure BDA0002336595810000129
其中，β>0，则对当末端执行器与工件的在基座标系R0(x0,y0,z0)下接触力为F时，其在各关节处施加的作用力矩的表达式求导可得：
Figure BDA00023365958100001210
联立公式(18)和(19)，即可得到关节力矩在角速度层的描述：
Figure BDA00023365958100001211
其中，
Figure BDA00023365958100001212
Figure BDA00023365958100001213
J表示Jacobian矩阵；
Figure BDA00023365958100001214
表示关节力矩约束的一阶导数；τ表示关节力矩约束；β表示正控制参数；θ表示机器人的关节角度；
Figure BDA00023365958100001215
表示机器人的关节角速度；H表示一个实数数组；
Figure BDA00023365958100001216
表示所述机器人的指令力的一阶导数；
Figure BDA00023365958100001217
表示实数。
首先，进行基本QP问题描述；当末端执行器与工件的接触力为F时，其在各关节处施加的作用力矩为：
τ＝JT(θ)F； (11)
从节能的角度出发，选择目标函数为τTτ/2描述***的能量消耗；同时在接触力F较大时，为了避免在某一关节上产生过大力矩导致的安全风险，在关节角度约束、角速度约束的基础上，引入关节力矩约束τmin≤τ≤τmax；则冗余机器人的位置-力控制问题描述为如下的QP问题：
min G1＝FTJ(θ)JT(θ)F/2； (12a)
s.t.rd＝A(f(θ)-xd)； (12b)
θmin≤θ≤θmax； (12c)
Figure BDA0002336595810000131
τmin≤JT(θ)Fd≤τmax； (12e)
然后进行等式和不等式的约束重建；根据上述公式(10)与rd的定义，对给定的期望轨迹xd与指令力Fd，实现位置-力控制的目标为：
Figure BDA0002336595810000132
则定义误差向量：
e＝r-rd＝[F-Fd；e0]； (16)
则等式可以在速度层重建为如下形式：
Figure BDA0002336595810000133
其中，k表示一正控制常数；
Figure BDA0002336595810000134
表示所述机器人的关节角速度；
Figure BDA0002336595810000135
表示误差向量的一阶导数；
Figure BDA0002336595810000136
表示期望轨迹的一阶导数；
Figure BDA0002336595810000137
表示rd的一阶导数，rd＝[Fd；0]，Fd表示机器人的力指令。
对于上述不等式约束(12c)和(12d)，参考上述中的处理方法，将不等式约束归一化描述为速度层的不等式约束：
Figure BDA0002336595810000138
其中，
Figure BDA0002336595810000139
Figure BDA00023365958100001310
同理关节力矩的不等式约束(12e)可以重写为：
Figure BDA00023365958100001311
其中，β>0，则对当末端执行器与工件的在基座标系R0(x0,y0,z0)下接触力为F时，对公式(11)在各关节处施加的作用力矩的表达式求导可得：
Figure BDA00023365958100001312
联立公式(18)和(19)，即可得到关节力矩在角速度层的描述：
Figure BDA00023365958100001313
其中，
Figure BDA00023365958100001314
Figure BDA00023365958100001315
J表示Jacobian矩阵；
Figure BDA00023365958100001316
表示关节力矩约束的一阶导数；τ表示关节力矩约束；β表示正控制参数；θ表示机器人的关节角度；
Figure BDA00023365958100001317
表示机器人的关节角速度；H表示一个实数数组；
Figure BDA0002336595810000141
表示所述机器人的指令力的一阶导数；
Figure BDA0002336595810000142
表示实数。
综上，基于约束-优化思想的冗余机器人位置-力控制问题在角速度层上的描述如下：
Figure BDA0002336595810000143
Figure BDA0002336595810000144
Figure BDA0002336595810000145
其中，
Figure BDA0002336595810000147
S15：对关节力矩函数进行改写，并获得最终的约束优化模型；
在本发明具体实施过程中，所述对关节力矩函数进行改写，并获得最终的约束优化模型，包括：
将目标函数进行简化，使用所述机器人的指令力Fd代替在基座标系R0(x0,y0,z0)下的接触力F，则有：
Figure BDA0002336595810000148
若公式(13)描述的目标函数定义在关节角度层，由于最终的控制量为关节角速度
Figure BDA0002336595810000149
因此通过求取G2对θ的梯度，得到其在速度层上的替代描述：
Figure BDA00023365958100001410
对JT(θ)Fd求导得到：
Figure BDA00023365958100001411
其中，
Figure BDA00023365958100001412
是：
Figure BDA00023365958100001413
令H＝[H1,…,Hn]，则上式可描述数为：
Figure BDA00023365958100001414
由于公式(15)中的第二项与
Figure BDA00023365958100001415
不相关，则选择最终目标函数选取为
Figure BDA0002336595810000151
通过引入一个修正项
Figure BDA0002336595810000152
对目标函数中的
Figure BDA0002336595810000153
进行凸化处理，则最终的约束优化模型为：
Figure BDA0002336595810000154
Figure BDA0002336595810000155
Figure BDA0002336595810000156
Figure BDA0002336595810000157
其中，
Figure BDA0002336595810000158
表示所述机器人的指令力的转秩；J表示Jacobian矩阵；
Figure BDA0002336595810000159
表示机器人的关节角速度；rr表示参考指令；A表示简写的矩阵。
具体的，在公式(12)中包含了大量的非线性特征，包括Jacobian矩阵，以及实时接触力
Figure BDA00023365958100001510
这使后续控制器设计变得困难，因此将该目标函数进行简化：使用机器人的指令力Fd代替在基座标系R0(x0,y0,z0)下的接触力F，则有：
Figure BDA00023365958100001511
在Fd与θ不相关，使用Fd可以大大降低目标函数的非线性程度；另一方面，通过合理的控制器设计，接触力F将最终收敛到Fd，因此替换前后的目标函数最终是等价的，如公式(13)描述的目标函数定义在关节角度层，由于最终的控制量为关节角速度
Figure BDA00023365958100001512
因此通过求取G2对θ的梯度，得到其在速度层上的替代描述：
Figure BDA00023365958100001513
对JT(θ)Fd求导得到：
Figure BDA00023365958100001514
其中，
Figure BDA00023365958100001515
是：
Figure BDA00023365958100001516
令H＝[H1,…,Hn]，则上式可描述数为：
Figure BDA00023365958100001517
由于公式(15)中的第二项与
Figure BDA0002336595810000161
不相关，则选择最终目标函数选取为
Figure BDA0002336595810000162
由于公式(21a)描述的目标函数对
Figure BDA0002336595810000163
而言是非凸的，通过引入一个修正项
Figure BDA0002336595810000164
对目标函数中的
Figure BDA0002336595810000165
进行凸化处理，则最终的约束优化模型为：
Figure BDA0002336595810000166
Figure BDA0002336595810000167
Figure BDA0002336595810000168
Figure BDA0002336595810000169
其中，
Figure BDA00023365958100001610
表示所述机器人的指令力的转秩；J表示Jacobian矩阵；
Figure BDA00023365958100001611
表示机器人的关节角速度；rr表示参考指令；A表示简写的矩阵。
S16：基于动态神经网络模型更新最终的约束优化模型中的状态变量和控制力矩；
在本发明具体实施过程中，所述基于动态神经网络模型更新最终的约束优化模型中的状态变量和控制力矩，包括：定义输入状态变量
Figure BDA00023365958100001612
Figure BDA00023365958100001613
为所述约束优化模型中的对偶状态变量，则拉格朗日函数选取为：
Figure BDA00023365958100001614
根据Karush-Kuhn-Tucker条件，优化所述约束优化模型中的最优解等价地表述为：
Figure BDA00023365958100001615
Figure BDA00023365958100001616
Figure BDA00023365958100001617
其中，PΩ(·)是一个限幅函数，定义为：
Figure BDA00023365958100001618
Figure BDA00023365958100001619
为一个投影函数，定义为：
Figure BDA0002336595810000171
在实时求解公式(24)时，基于动态神经网络模型的位置-力控制器设计设计为：
Figure BDA0002336595810000172
Figure BDA0002336595810000173
Figure BDA0002336595810000174
其中，
Figure BDA0002336595810000175
Figure BDA0002336595810000176
表示实数；
Figure BDA0002336595810000177
表示输入状态量λ2的一阶导数；
Figure BDA0002336595810000178
表示输入状态量λ1的一阶导数；
Figure BDA0002336595810000179
表示关节角速度；
Figure BDA00023365958100001710
表示关节角加速度；J表示Jacobian矩阵；rr表示参考指令；A表示简写矩阵；g1表示第一个元素；g2m表示第2m个元素；∈表示一个正常数。
具体的，定义输入状态变量
Figure BDA00023365958100001711
为约束公式(22b)和(22c)的对偶状态变量，则拉格朗日函数选取为：
Figure BDA00023365958100001712
根据Karush-Kuhn-Tucker条件，优化所述约束优化模型中的最优解等价地表述为：
Figure BDA00023365958100001713
Figure BDA00023365958100001714
Figure BDA00023365958100001715
其中，PΩ(·)是一个限幅函数，定义为：
Figure BDA00023365958100001716
Figure BDA00023365958100001717
为一个投影函数，定义为：
Figure BDA00023365958100001718
在实时求解公式(24)时，基于动态神经网络模型的位置-力控制器设计设计为：
Figure BDA00023365958100001719
Figure BDA00023365958100001720
Figure BDA0002336595810000181
其中，
Figure BDA0002336595810000182
Figure BDA0002336595810000183
表示实数；
Figure BDA0002336595810000184
表示输入状态量λ2的一阶导数；
Figure BDA0002336595810000185
表示输入状态量λ1的一阶导数；
Figure BDA0002336595810000186
表示关节角速度；
Figure BDA0002336595810000187
表示关节角加速度；J表示Jacobian矩阵；rr表示参考指令；A表示简写矩阵；g1表示第一个元素；g2m表示第2m个元素；∈表示一个正常数。
S17：判断当前时间是否大于任务时间；
在本发明具体实施过程中，需要判断当前的时间是否大于任务时间，在大于的时候，执行S18，否则返回S12。
S18：若是，结束对所述机器人的柔顺力控制，反之，返回基于初始化状态变量获得当前旋转矩阵。
在本发明具体实施过程中，在判断当前的时间是大于任务时间时，结束对机器人的柔顺力控制。
在本发明实施例中，通过实时本发明的方法，能够同时实现接触力方向的高精度力控制以及***方向上的运动控制；能够实现对关节力矩的在线优化；以及能够在柔顺力控制过程中保证机器人不超过其物理约束。
实施例
请参阅图3，图3是本发明实施例中的协作机器人柔顺力控制***的结构组成示意图。
如图3所示，一种协作机器人柔顺力控制***，所述***包括：
初始化模块21：用于获得机器人的状态变量并进行初始化；
在本发明具体实施过程中，所述获得机器人的状态变量并进行初始化之前，还包括：针对所述机器人运动与工件之间的接触力的正交特点，在工具坐标系和基座标系内进行分别建模，获得机器人运动建模***；其中，基座标系的表示R0(x0,y0,z0)；工具坐标系的表示Rt(xt,yt,zt)。
进一步的，所述机器人与工件之间的接触力与所述工具坐标系中的zt平行，同时xt与yt定义了所述机器人末端执行器的***；
在所述机器人操作过程中，所述机器人末端执行器实际位置x与期望轨迹xd存在一微小偏差，在所述基座标系R0(x0,y0,z0)和所述工具坐标系Rt(xt,yt,zt)下分别描述为所述机器人在基座标系下的与期望轨迹的偏差δX和所述机器人在工具坐标系下的与期望轨迹的偏差δXt。
进一步的，所述在工具坐标系和基座标系内进行分别建模，获得机器人运动建模***的过程如下：
在所述工具坐标系Rt(xt,yt,zt)下由于忽略所述机器人与所述工件之间的摩擦，假设所述机器人与所述工件之间的接触为刚度接触，则所述接触力可以描述为：
Ft＝kf∑tδXt； (1)
其中，kf表示刚度系数；∑t＝diag(0,0,1)为一个对角矩阵，用于描述所述机器人在工具坐标系下的与期望轨迹的偏差δXt与其接触力之间的关系，其中，0表示在该方向上的位移不会产生接触力，反之取1；
定义
Figure BDA0002336595810000191
则在所述工具坐标系Rt(xt,yt,zt)下的位置跟踪误差et为：
Figure BDA0002336595810000192
在接触面已知的情况下，使用一个旋转矩阵St描述所述工具坐标系Rt(xt,yt,zt)和基座标系R0(x0,y0,z0)之间的旋转关系；
定义F和e0分别为所述基座标系R0(x0,y0,z0)下δXt与Ft的对应描述，则有：
Figure BDA0002336595810000193
et＝Ste0； (4)
δXt＝StδX； (5)
通过公式(1)-(5)的联立，则有：
Figure BDA0002336595810000194
Figure BDA0002336595810000195
其中，δX表示所述机器人在所述基座标系R0(x0,y0,z0)下的与期望轨迹的偏差；F表示在基座标系R0(x0,y0,z0)下的接触力；
在基座标系R0(x0,y0,z0)中位移δX可以描述为δX＝x-xd，其中，期望轨迹xd是R0中描述的期望位置信号，因此，对公式(6)-(7)重写为：
Figure BDA0002336595810000196
Figure BDA0002336595810000197
定义所述机器人的期望轨迹和指令力分别为xd和Fd；则根据公式(5)与公式(9)的描述，控制目标可以描述为设计面向冗余机器人的位置-来控制策略，使公式(8)描述的接触力F→Fd，同时使公式(9)描述的跟踪误差e0→0；∑t表示描述接触的参数矩阵；
Figure BDA0002336595810000201
表示描述运动的参数矩阵。
进一步的，所述机器人运动建模***中为了简化描述，则定义
Figure BDA0002336595810000202
Figure BDA0002336595810000203
rd＝[Fd；0]，
Figure BDA0002336595810000204
则有公式(8)和公式(9)重写为：
A(f(θ)-xd)＝r； (10)
所述控制目标描述为通过设计关节，使r＝rd；∑t表示描述接触的参数矩阵；
Figure BDA0002336595810000207
表示描述运动的参数矩阵。
具体的，首先根据针对机器人运动与接触力的正交特点，在工具坐标系和基座标系内进行分别建模；为了不失一般性，定义具坐标系Rt(xt,yt,zt)与基座标系R0(x0,y0,z0)，具体如图2所示，该机器人与工件之间的接触力与工具坐标系中的zt平行，同时xt与yt定义了所述机器人末端执行器的***；在操作过程中，机器人末端执行器实际位置x与期望轨迹xd存在一微小偏差，在基座标系R0(x0,y0,z0)和工具坐标系Rt(xt,yt,zt)下分别描述为机器人在基座标系下的与期望轨迹的偏差δX和机器人在工具坐标系下的与期望轨迹的偏差δXt。
在工具坐标系Rt(xt,yt,zt)下由于忽略所述机器人与所述工件之间的摩擦，假设机器人与所述工件之间的接触为刚度接触，则所述接触力可以描述为：
Ft＝kf∑tδXt； (1)
其中，kf表示刚度系数；∑t＝diag(0,0,1)为一个对角矩阵，用于描述所述机器人在工具坐标系下的与期望轨迹的偏差δXt与其接触力之间的关系，其中，0表示在该方向上的位移不会产生接触力，反之取1。
定义
Figure BDA0002336595810000205
则在工具坐标系Rt(xt,yt,zt)下的位置跟踪误差et为：
Figure BDA0002336595810000206
在接触面已知的情况下，使用一个旋转矩阵St描述工具坐标系Rt(xt,yt,zt)和基座标系R0(x0,y0,z0)之间的旋转关系。
定义F和e0分别为基座标系R0(x0,y0,x0)下δXt与Ft的对应描述，则有：
Figure BDA0002336595810000211
et＝Ste0； (4)
δXt＝StδX； (5)
通过公式(1)-(5)的联立，则有：
Figure BDA0002336595810000212
Figure BDA0002336595810000213
其中，δX表示机器人在所述基座标系R0(x0,y0,z0)下的与期望轨迹的偏差；F表示在基座标系R0(x0,y0,z0)下的接触力。
值得注意的是，在基座标系T0(x0,y0,z0)中位移δX可以描述为δX＝x-xd，其中，期望轨迹xd是R0中描述的期望位置信号，因此，对公式(6)-(7)重写为：
Figure BDA0002336595810000214
Figure BDA0002336595810000215
定义所述机器人的期望轨迹和指令力分别为xd和Fd；则根据公式(5)与公式(9)的描述，控制目标可以描述为设计面向冗余机器人的位置-来控制策略，使公式(8)描述的接触力F→Fd，同时使公式(9)描述的跟踪误差e0→0；∑t表示描述接触的参数矩阵；
Figure BDA0002336595810000216
表示描述运动的参数矩阵。
为了简化描述，定义
Figure BDA0002336595810000217
rd＝[Fd；0]，
Figure BDA0002336595810000218
则有公式(8)和公式(9)重写为：
A(f(θ)-xd)＝r； (10)
控制目标描述为通过设计关节，使r＝rd；∑t表示描述接触的参数矩阵；
Figure BDA0002336595810000219
表示描述运动的参数矩阵。
在构建机器人的***建模模型之后，然后获得机器人的状态变量，然后进行相应的初始化。
获得模块22：用于基于初始化状态变量获得当前旋转矩阵；
在本发明具体实施过程中，在获得初始化状态变量之后，根据该初始化状态变量来获得当前旋转矩阵；即所建模的机器人***内的旋转矩阵St。
读取模块23：用于基于所述当前旋转矩阵读取所述机器人的当前状态反馈信息；
在本发明具体实施过程中，通过该当前旋转矩阵获得机器人状态反馈的反馈信息，即通过机器人***获得的状态反馈信息包括在基座标系R0(x0,y0,z0)下的接触力、机器人的关节角速度和机器人的关节角度。
构建模块24：用于基于所述当前状态反馈信息构建实现柔顺力控制的等式约束以及机器人***内的关节角度、关节角速度与关节力矩的不等式约束；
在本发明具体实施过程中，所述基于所述当前状态反馈信息构建实现柔顺力控制的等式约束，包括：在机器人运动建模***下获得的当前状态反馈信息，对给定的期望轨迹xd与接触力Ft，实现位置-来控制的目标为：
Figure BDA0002336595810000221
则定义误差向量：
e＝r-rd＝[F-Fd；e0]； (16)
则等式可以在速度层重建为如下形式：
Figure BDA0002336595810000222
其中，k表示一正控制常数；
Figure BDA0002336595810000223
表示所述机器人的关节角速度；
Figure BDA0002336595810000224
表示误差向量的一阶导数；
Figure BDA0002336595810000225
表示期望轨迹的一阶导数；
Figure BDA0002336595810000226
表示rd的一阶导数，rd＝[Fd；0]，Fd表示机器人的力指令。
进一步的，所述机器人***内的关节角度、关节角速度与关节力矩的不等式约束，包括：将不等式约束归一化描述为速度层的不等式约束：
Figure BDA0002336595810000227
Figure BDA0002336595810000228
其中，
Figure BDA0002336595810000229
则关节力矩的不等式约束可以重写为：
Figure BDA00023365958100002210
其中，β>0，则对当末端执行器与工件的在基座标系R0(x0,y0,z0)下接触力为F时，其在各关节处施加的作用力矩的表达式求导可得：
Figure BDA00023365958100002211
联立公式(18)和(19)，即可得到关节力矩在角速度层的描述：
Figure BDA0002336595810000231
其中，
Figure BDA0002336595810000232
Figure BDA0002336595810000233
J表示Jacobian矩阵；
Figure BDA0002336595810000234
表示关节力矩约束的一阶导数；τ表示关节力矩约束；β表示正控制参数；θ表示机器人的关节角度；
Figure BDA0002336595810000235
表示机器人的关节角速度；H表示一个实数数组；
Figure BDA0002336595810000236
表示所述机器人的指令力的一阶导数；
Figure BDA0002336595810000237
表示实数。
首先，进行基本QP问题描述；当末端执行器与工件的接触力为F时，其在各关节处施加的作用力矩为：
τ＝JT(θ)F； (11)
从节能的角度出发，选择目标函数为τTτ/2描述***的能量消耗；同时在接触力F较大时，为了避免在某一关节上产生过大力矩导致的安全风险，在关节角度约束、角速度约束的基础上，引入关节力矩约束τmin≤τ≤τmax；则冗余机器人的位置-力控制问题描述为如下的QP问题：
minG1＝FTJ(θ)JT(θ)F/2； (12a)
s.t.rd＝A(f(θ)-xd)； (12b)
θmin≤θ≤θmax； (12c)
Figure BDA0002336595810000238
τmin≤JT(θ)Fd≤τmax； (12e)
然后进行等式和不等式的约束重建；根据上述公式(10)与rd的定义，对给定的期望轨迹xd与指令力Fd，实现位置-力控制的目标为：
Figure BDA0002336595810000239
则定义误差向量：
e＝r-rd＝[F-Fd；e0]； (16)
则等式可以在速度层重建为如下形式：
Figure BDA00023365958100002310
其中，k表示一正控制常数；
Figure BDA00023365958100002311
表示所述机器人的关节角速度；
Figure BDA00023365958100002312
表示误差向量的一阶导数；
Figure BDA00023365958100002313
表示期望轨迹的一阶导数；
Figure BDA00023365958100002314
表示rd的一阶导数，rd＝[Fd；0]，Fd表示机器人的力指令。
对于上述不等式约束(12c)和(12d)，参考上述中的处理方法，将不等式约束归一化描述为速度层的不等式约束：
Figure BDA0002336595810000241
其中，
Figure BDA0002336595810000242
Figure BDA0002336595810000243
同理关节力矩的不等式约束(12e)可以重写为：
Figure BDA0002336595810000244
其中，β>0，则对当末端执行器与工件的在基座标系R0(x0,y0,z0)下接触力为F时，对公式(11)在各关节处施加的作用力矩的表达式求导可得：
Figure BDA0002336595810000245
联立公式(18)和(19)，即可得到关节力矩在角速度层的描述：
Figure BDA0002336595810000246
其中，
Figure BDA0002336595810000247
Figure BDA0002336595810000248
J表示Jacobian矩阵；
Figure BDA0002336595810000249
表示关节力矩约束的一阶导数；τ表示关节力矩约束；β表示正控制参数；θ表示机器人的关节角度；
Figure BDA00023365958100002410
表示机器人的关节角速度；H表示一个实数数组；
Figure BDA00023365958100002411
表示所述机器人的指令力的一阶导数；
Figure BDA00023365958100002412
表示实数。
综上，基于约束-优化思想的冗余机器人位置-力控制问题在角速度层上的描述如下：
Figure BDA00023365958100002413
Figure BDA00023365958100002414
Figure BDA00023365958100002415
Figure BDA00023365958100002416
其中，
Figure BDA00023365958100002417
改写模块25：用于对关节力矩函数进行改写，并获得最终的约束优化模型；
在本发明具体实施过程中，所述对关节力矩函数进行改写，并获得最终的约束优化模型，包括：
将目标函数进行简化，使用所述机器人的指令力Fd代替在基座标系R0(x0,y0,z0)下的接触力F，则有：
Figure BDA0002336595810000251
若公式(13)描述的目标函数定义在关节角度层，由于最终的控制量为关节角速度
Figure BDA0002336595810000252
因此通过求取G2对θ的梯度，得到其在速度层上的替代描述：
Figure BDA0002336595810000253
对JT(θ)Fd求导得到：
Figure BDA0002336595810000254
其中，
Figure BDA0002336595810000255
是：
Figure BDA0002336595810000256
令H＝[H1,…,Hn]，则上式可描述数为：
Figure BDA0002336595810000257
由于公式(15)中的第二项与
Figure BDA0002336595810000258
不相关，则选择最终目标函数选取为
Figure BDA0002336595810000259
通过引入一个修正项
Figure BDA00023365958100002510
对目标函数中的
Figure BDA00023365958100002511
进行凸化处理，则最终的约束优化模型为：
Figure BDA00023365958100002512
Figure BDA00023365958100002513
Figure BDA00023365958100002514
Figure BDA00023365958100002515
其中，
Figure BDA00023365958100002516
表示所述机器人的指令力的转秩；J表示Jacobian矩阵；
Figure BDA00023365958100002517
表示机器人的关节角速度；rr表示参考指令；A表示简写的矩阵。
具体的，在公式(12)中包含了大量的非线性特征，包括Jacobian矩阵，以及实时接触力
Figure BDA00023365958100002518
这使后续控制器设计变得困难，因此将该目标函数进行简化：使用机器人的指令力Fd代替在基座标系R0(x0,y0,z0)下的接触力F，则有：
Figure BDA00023365958100002519
在Fd与θ不相关，使用Fd可以大大降低目标函数的非线性程度；另一方面，通过合理的控制器设计，接触力F将最终收敛到Fd，因此替换前后的目标函数最终是等价的，如公式(13)描述的目标函数定义在关节角度层，由于最终的控制量为关节角速度
Figure BDA0002336595810000261
因此通过求取G2对θ的梯度，得到其在速度层上的替代描述：
Figure BDA0002336595810000262
对JT(θ)Fd求导得到：
Figure BDA0002336595810000263
其中，
Figure BDA0002336595810000264
是：
Figure BDA0002336595810000265
令H＝[H1,…,Hn]，则上式可描述数为：
Figure BDA0002336595810000266
由于公式(15)中的第二项与
Figure BDA0002336595810000267
不相关，则选择最终目标函数选取为
Figure BDA0002336595810000268
由于公式(21a)描述的目标函数对
Figure BDA0002336595810000269
而言是非凸的，通过引入一个修正项
Figure BDA00023365958100002610
对目标函数中的
Figure BDA00023365958100002611
进行凸化处理，则最终的约束优化模型为：
Figure BDA00023365958100002612
Figure BDA00023365958100002613
Figure BDA00023365958100002614
Figure BDA00023365958100002615
其中，
Figure BDA00023365958100002616
表示所述机器人的指令力的转秩；J表示Jacobian矩阵；
Figure BDA00023365958100002617
表示机器人的关节角速度；rr表示参考指令；A表示简写的矩阵。
更新模块26：用于基于动态神经网络模型更新最终的约束优化模型中的状态变量和控制力矩；
在本发明具体实施过程中，定义输入状态变量
Figure BDA00023365958100002618
为约束公式(22b)和(22c)的对偶状态变量，则拉格朗日函数选取为：
Figure BDA0002336595810000271
根据Karush-Kuhn-Tucker条件，优化所述约束优化模型中的最优解等价地表述为：
Figure BDA0002336595810000272
Figure BDA0002336595810000273
Figure BDA0002336595810000274
其中，PΩ(·)是一个限幅函数，定义为：
Figure BDA0002336595810000275
Figure BDA0002336595810000276
为一个投影函数，定义为：
Figure BDA0002336595810000277
在实时求解公式(24)时，基于动态神经网络模型的位置-力控制器设计设计为：
Figure BDA0002336595810000278
Figure BDA0002336595810000279
Figure BDA00023365958100002710
其中，
Figure BDA00023365958100002711
Figure BDA00023365958100002712
表示实数；
Figure BDA00023365958100002713
表示输入状态量λ2的一阶导数；
Figure BDA00023365958100002714
表示输入状态量λ1的一阶导数；
Figure BDA00023365958100002715
表示关节角速度；
Figure BDA00023365958100002716
表示关节角加速度；J表示Jacobian矩阵；rr表示参考指令；A表示简写矩阵；g1表示第一个元素；g2m表示第2m个元素；∈表示一个正常数。
判断模块27：用于判断当前时间是否大于任务时间，若是，结束对所述机器人的柔顺力控制，反之，返回基于初始化状态变量获得当前旋转矩阵。
在本发明具体实施过程中，需要判断当前的时间是否大于任务时间，在大于的时候；在判断当前的时间是大于任务时间时，结束对机器人的柔顺力控制。
在本发明实施例中，通过实时本发明的方法，能够同时实现接触力方向的高精度力控制以及***方向上的运动控制；能够实现对关节力矩的在线优化；以及能够在柔顺力控制过程中保证机器人不超过其物理约束。
本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于一计算机可读存储介质中，存储介质可以包括：只读存储器(ROM，Read Only Memory)、随机存取存储器(RAM，RandomAccess Memory)、磁盘或光盘等。
另外，以上对本发明实施例所提供的一种协作机器人柔顺力控制方法及***进行了详细介绍，本文中应采用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (10)

1.一种协作机器人柔顺力控制方法，其特征在于，所述方法包括：
获得机器人的状态变量并进行初始化；
基于初始化状态变量获得当前旋转矩阵；
基于所述当前旋转矩阵读取所述机器人的当前状态反馈信息；
基于所述当前状态反馈信息构建实现柔顺力控制的等式约束以及机器人***内的关节角度、关节角速度与关节力矩的不等式约束；
对关节力矩函数进行改写，并获得最终的约束优化模型；
基于动态神经网络模型更新最终的约束优化模型中的状态变量和控制力矩；
判断当前时间是否大于任务时间，若是，结束对所述机器人的柔顺力控制，反之，返回基于初始化状态变量获得当前旋转矩阵。
2.根据权利要求1所述的协作机器人柔顺力控制方法，其特征在于，所述获得机器人的状态变量并进行初始化之前，还包括：
针对所述机器人运动与工件之间的接触力的正交特点，在工具坐标系和基座标系内进行分别建模，获得机器人运动建模***；
其中，基座标系的表示R0(x0，y0，z0)；工具坐标系的表示Rt(xt，yt，zt)。
3.根据权利要求2所述的协作机器人柔顺力控制方法，其特征在于，所述机器人与工件之间的接触力与所述工具坐标系中的zt平行，同时xt与yt定义了所述机器人末端执行器的***；
在所述机器人操作过程中，所述机器人末端执行器实际位置x与期望轨迹xd存在一微小偏差，在所述基座标系R0(x0，y0，z0)和所述工具坐标系Rt(xt，yt，zt)下分别描述为所述机器人在基座标系下的与期望轨迹的偏差δX和所述机器人在工具坐标系下的与期望轨迹的偏差δXt。
4.根据权利要求3所述的协作机器人柔顺力控制方法，其特征在于，所述在工具坐标系和基座标系内进行分别建模，获得机器人运动建模***的过程如下：
在所述工具坐标系Rt(xt，yt，zt)下由于忽略所述机器人与所述工件之间的摩擦，假设所述机器人与所述工件之间的接触为刚度接触，则所述接触力可以描述为：
Ft＝kf∑tδXt； (1)
其中，kf表示刚度系数；∑t＝diag(0，0，1)为一个对角矩阵，用于描述所述机器人在工具坐标系下的与期望轨迹的偏差δXt与其接触力之间的关系，其中，0表示在该方向上的位移不会产生接触力，反之取1；
定义
Figure FDA0002336595800000021
则在所述工具坐标系Rt(xt，yt，zt)下的位置跟踪误差et为：
Figure FDA0002336595800000022
在接触面已知的情况下，使用一个旋转矩阵St描述所述工具坐标系Rt(xt，yt，zt)和基座标系R0(x0，y0，z0)之间的旋转关系；
定义F和e0分别为所述基座标系R0(x0，y0，z0)下δXt与Ft的对应描述，则有：
Figure FDA0002336595800000023
et＝Ste0； (4)
δXt＝StδX； (5)
通过公式(1)-(5)的联立，则有：
Figure FDA0002336595800000024
Figure FDA0002336595800000025
其中，δX表示所述机器人在所述基座标系R0(x0，y0，z0)下的与期望轨迹的偏差；F表示在基座标系R0(x0，y0，z0)下的接触力；
在基座标系R0(x0，y0，z0)中位移δX可以描述为δX＝x-xd，其中，期望轨迹xd是R0中描述的期望位置信号，因此，对公式(6)-(7)重写为：
Figure FDA0002336595800000026
Figure FDA0002336595800000031
定义所述机器人的期望轨迹和指令力分别为xd和Fd；则根据公式(5)与公式(9)的描述，控制目标可以描述为设计面向冗余机器人的位置-来控制策略，使公式(8)描述的接触力F→Fd，同时使公式(9)描述的跟踪误差e0→0；∑t表示描述接触的参数矩阵；
Figure FDA0002336595800000032
表示描述运动的参数矩阵。
5.根据权利要求4所述的协作机器人柔顺力控制方法，其特征在于，所述机器人运动建模***中为了简化描述，则定义
Figure FDA0002336595800000033
rd＝[Fd；0]，
Figure FDA0002336595800000034
则有公式(8)和公式(9)重写为：
A(f(θ)-xd)＝r； (10)
所述控制目标描述为通过设计关节，使r＝rd；∑t表示描述接触的参数矩阵；
Figure FDA0002336595800000035
表示描述运动的参数矩阵。
6.根据权利要求1所述的协作机器人柔顺力控制方法，其特征在于，所述基于所述当前状态反馈信息构建实现柔顺力控制的等式约束，包括：
在机器人运动建模***下获得的当前状态反馈信息，对给定的期望轨迹xd与接触力Ft，实现位置-来控制的目标为：
Figure FDA0002336595800000036
则定义误差向量：
e＝r-rd＝[F-Fd；e0]； (16)
则等式可以在速度层重建为如下形式：
Figure FDA0002336595800000037
其中，k表示一正控制常数；
Figure FDA0002336595800000038
表示所述机器人的关节角速度；
Figure FDA0002336595800000039
表示误差向量的一阶导数；
Figure FDA00023365958000000310
表示期望轨迹的一阶导数；
Figure FDA00023365958000000311
表示rd的一阶导数，rd＝[Fd；0]，Fd表示机器人的力指令。
7.根据权利要求1所述的协作机器人柔顺力控制方法，其特征在于，所述机器人***内的关节角度、关节角速度与关节力矩的不等式约束，包括：
将不等式约束归一化描述为速度层的不等式约束：
Figure FDA0002336595800000041
其中，
Figure FDA0002336595800000042
则关节力矩的不等式约束可以重写为：
Figure FDA0002336595800000043
其中，β＞0，则对当末端执行器与工件的在基座标系R0(x0，y0，z0)下接触力为F时，其在各关节处施加的作用力矩的表达式求导可得：
Figure FDA0002336595800000044
联立公式(18)和(19)，即可得到关节力矩在角速度层的描述：
Figure FDA0002336595800000045
其中，
Figure FDA0002336595800000046
Figure FDA0002336595800000047
J表示Jacobian矩阵；
Figure FDA0002336595800000048
表示关节力矩约束的一阶导数；τ表示关节力矩约束；β表示正控制参数；θ表示机器人的关节角度；
Figure FDA0002336595800000049
表示机器人的关节角速度；H表示一个实数数组；
Figure FDA00023365958000000410
表示所述机器人的指令力的一阶导数；
Figure FDA00023365958000000411
表示实数。
8.根据权利要求1所述的协作机器人柔顺力控制方法，其特征在于，所述对关节力矩函数进行改写，并获得最终的约束优化模型，包括：
将目标函数进行简化，使用所述机器人的指令力Fd代替在基座标系R0(x0，y0，z0)下的接触力F，则有：
Figure FDA00023365958000000412
若公式(13)描述的目标函数定义在关节角度层，由于最终的控制量为关节角速度
Figure FDA00023365958000000413
因此通过求取G2对θ的梯度，得到其在速度层上的替代描述：
Figure FDA00023365958000000414
对JT(θ)Fd求导得到：
Figure FDA00023365958000000415
其中，
Figure FDA00023365958000000416
是：
Figure FDA0002336595800000051
令H＝[H1，…，Hn]，则上式可描述数为：
Figure FDA0002336595800000052
由于公式(15)中的第二项与
Figure FDA0002336595800000053
不相关，则选择最终目标函数选取为
Figure FDA0002336595800000054
通过引入一个修正项
Figure FDA0002336595800000055
对目标函数中的
Figure FDA0002336595800000056
进行凸化处理，则最终的约束优化模型为：
Figure FDA0002336595800000057
Figure FDA0002336595800000058
Figure FDA0002336595800000059
Figure FDA00023365958000000510
其中，
Figure FDA00023365958000000511
表示所述机器人的指令力的转秩；J表示Jacobian矩阵；
Figure FDA00023365958000000512
表示机器人的关节角速度；rr表示参考指令；A表示简写的矩阵。
9.根据权利要求1所述的协作机器人柔顺力控制方法，其特征在于，所述基于动态神经网络模型更新最终的约束优化模型中的状态变量和控制力矩，包括：
定义输入状态变量
Figure FDA00023365958000000513
为所述约束优化模型中的对偶状态变量，则拉格朗日函数选取为：
Figure FDA00023365958000000514
根据Karush-Kuhn-Tucker条件，优化所述约束优化模型中的最优解等价地表述为：
Figure FDA00023365958000000515
Figure FDA00023365958000000516
Figure FDA00023365958000000517
其中，PΩ(·)是一个限幅函数，定义为：
Figure FDA0002336595800000061
Figure FDA0002336595800000062
为一个投影函数，定义为：
Figure FDA0002336595800000063
在实时求解公式(24)时，基于动态神经网络模型的位置-力控制器设计设计为：
Figure FDA0002336595800000064
Figure FDA0002336595800000065
Figure FDA0002336595800000066
其中，
Figure FDA0002336595800000067
Figure FDA0002336595800000068
表示实数；
Figure FDA0002336595800000069
表示输入状态量λ2的一阶导数；
Figure FDA00023365958000000610
表示输入状态量λ1的一阶导数；
Figure FDA00023365958000000611
表示关节角速度；
Figure FDA00023365958000000612
表示关节角加速度；J表示Jacobian矩阵；rr表示参考指令；A表示简写矩阵；g1表示第一个元素；g2m表示第2m个元素；∈表示一个正常数。
10.一种协作机器人柔顺力控制***，其特征在于，所述***包括：
初始化模块：用于获得机器人的状态变量并进行初始化；
获得模块：用于基于初始化状态变量获得当前旋转矩阵；
读取模块：用于基于所述当前旋转矩阵读取所述机器人的当前状态反馈信息；
构建模块：用于基于所述当前状态反馈信息构建实现柔顺力控制的等式约束以及机器人***内的关节角度、关节角速度与关节力矩的不等式约束；
改写模块：用于对关节力矩函数进行改写，并获得最终的约束优化模型；
更新模块：用于基于动态神经网络模型更新最终的约束优化模型中的状态变量和控制力矩；
判断模块：用于判断当前时间是否大于任务时间，若是，结束对所述机器人的柔顺力控制，反之，返回基于初始化状态变量获得当前旋转矩阵。
CN201911358551.4A 2019-12-25 2019-12-25 一种协作机器人柔顺力控制方法及*** Active CN111037560B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911358551.4A CN111037560B (zh) 2019-12-25 2019-12-25 一种协作机器人柔顺力控制方法及***

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911358551.4A CN111037560B (zh) 2019-12-25 2019-12-25 一种协作机器人柔顺力控制方法及***

Publications (2)

Publication Number Publication Date
CN111037560A true CN111037560A (zh) 2020-04-21
CN111037560B CN111037560B (zh) 2021-06-25

Family

ID=70240336

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911358551.4A Active CN111037560B (zh) 2019-12-25 2019-12-25 一种协作机器人柔顺力控制方法及***

Country Status (1)

Country Link
CN (1) CN111037560B (zh)

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111941421A (zh) * 2020-06-22 2020-11-17 哈尔滨工业大学(深圳)(哈尔滨工业大学深圳科技创新研究院) 一种基于多机器人协同操作的自适应模糊力跟踪控制方法
CN112873208A (zh) * 2021-01-29 2021-06-01 佛山树客智能机器人科技有限公司 一种抗噪与动力学约束的机器人实时运动规划方法及装置
CN113084828A (zh) * 2021-04-02 2021-07-09 上海电气集团股份有限公司 一种运动控制方法、装置、设备和存储介质
CN113146615A (zh) * 2021-01-29 2021-07-23 佛山树客智能机器人科技有限公司 一种多机器人协同搬运控制方法及装置
CN113618741A (zh) * 2021-08-24 2021-11-09 深圳市优必选科技股份有限公司 机器人关节位姿优化方法、机器人控制方法和机器人
CN113703319A (zh) * 2021-08-26 2021-11-26 合肥工业大学 基于强化学习的关节模组不等式约束最优鲁棒控制方法
CN114260896A (zh) * 2021-12-20 2022-04-01 广东省科学院智能制造研究所 一种协作机器人的柔顺力控制方法及***
US20220305645A1 (en) * 2021-03-25 2022-09-29 Mitsubishi Electric Research Laboratories, Inc. System and Method for Robotic Assembly Based on Adaptive Compliance

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101804627A (zh) * 2010-04-02 2010-08-18 中山大学 一种冗余度机械臂运动规划方法
CN107378952A (zh) * 2017-08-16 2017-11-24 华南理工大学 一种冗余度机械臂末端执行器姿态保持的解决方法
CN107662211A (zh) * 2017-10-16 2018-02-06 西北工业大学 一种基于量子粒子群算法的空间机器人预测控制方法
US20180111270A1 (en) * 2016-10-24 2018-04-26 Seiko Epson Corporation Control apparatus, robot, and robot system
CN108714894A (zh) * 2018-05-03 2018-10-30 华南理工大学 一种求解双冗余机械臂互相碰撞的动力学方法
DE102018210864B3 (de) * 2018-07-02 2019-07-18 Kuka Deutschland Gmbh Verfahren und System zum Regeln eines Roboters
EP3546134A1 (en) * 2018-03-28 2019-10-02 BAE SYSTEMS plc Collaborative robot system

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101804627A (zh) * 2010-04-02 2010-08-18 中山大学 一种冗余度机械臂运动规划方法
US20180111270A1 (en) * 2016-10-24 2018-04-26 Seiko Epson Corporation Control apparatus, robot, and robot system
CN107378952A (zh) * 2017-08-16 2017-11-24 华南理工大学 一种冗余度机械臂末端执行器姿态保持的解决方法
CN107662211A (zh) * 2017-10-16 2018-02-06 西北工业大学 一种基于量子粒子群算法的空间机器人预测控制方法
EP3546134A1 (en) * 2018-03-28 2019-10-02 BAE SYSTEMS plc Collaborative robot system
CN108714894A (zh) * 2018-05-03 2018-10-30 华南理工大学 一种求解双冗余机械臂互相碰撞的动力学方法
DE102018210864B3 (de) * 2018-07-02 2019-07-18 Kuka Deutschland Gmbh Verfahren und System zum Regeln eines Roboters

Cited By (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111941421A (zh) * 2020-06-22 2020-11-17 哈尔滨工业大学(深圳)(哈尔滨工业大学深圳科技创新研究院) 一种基于多机器人协同操作的自适应模糊力跟踪控制方法
CN111941421B (zh) * 2020-06-22 2022-02-18 哈尔滨工业大学(深圳)(哈尔滨工业大学深圳科技创新研究院) 一种基于多机器人协同操作的自适应模糊力跟踪控制方法
CN113146615A (zh) * 2021-01-29 2021-07-23 佛山树客智能机器人科技有限公司 一种多机器人协同搬运控制方法及装置
CN112873208A (zh) * 2021-01-29 2021-06-01 佛山树客智能机器人科技有限公司 一种抗噪与动力学约束的机器人实时运动规划方法及装置
US20220305645A1 (en) * 2021-03-25 2022-09-29 Mitsubishi Electric Research Laboratories, Inc. System and Method for Robotic Assembly Based on Adaptive Compliance
US11673264B2 (en) * 2021-03-25 2023-06-13 Mitsubishi Electric Research Laboratories, Inc. System and method for robotic assembly based on adaptive compliance
CN113084828A (zh) * 2021-04-02 2021-07-09 上海电气集团股份有限公司 一种运动控制方法、装置、设备和存储介质
CN113618741A (zh) * 2021-08-24 2021-11-09 深圳市优必选科技股份有限公司 机器人关节位姿优化方法、机器人控制方法和机器人
CN113618741B (zh) * 2021-08-24 2022-07-29 深圳市优必选科技股份有限公司 机器人关节位姿优化方法、机器人控制方法和机器人
CN113703319A (zh) * 2021-08-26 2021-11-26 合肥工业大学 基于强化学习的关节模组不等式约束最优鲁棒控制方法
CN113703319B (zh) * 2021-08-26 2023-07-28 合肥工业大学 基于强化学习的关节模组不等式约束最优鲁棒控制方法
CN114260896A (zh) * 2021-12-20 2022-04-01 广东省科学院智能制造研究所 一种协作机器人的柔顺力控制方法及***
CN114260896B (zh) * 2021-12-20 2024-01-09 广东省科学院智能制造研究所 一种协作机器人的柔顺力控制方法及***

Also Published As

Publication number Publication date
CN111037560B (zh) 2021-06-25

Similar Documents

Publication Publication Date Title
CN111037560B (zh) 一种协作机器人柔顺力控制方法及***
CN110039542B (zh) 具有速度方向控制的视觉伺服跟踪控制方法及机器人***
Cheah et al. Adaptive Jacobian vision based control for robots with uncertain depth information
Xu et al. Dynamic neural networks based kinematic control for redundant manipulators with model uncertainties
Steinmetz et al. Simultaneous kinesthetic teaching of positional and force requirements for sequential in-contact tasks
WO2007127723A2 (en) Control of robots from human motion descriptors
Rambow et al. Autonomous manipulation of deformable objects based on teleoperated demonstrations
Sun et al. Single master bimanual teleoperation system with efficient regulation
CN111702767A (zh) 一种基于反演模糊自适应的机械手阻抗控制方法
Mazare et al. Adaptive variable impedance control for a modular soft robot manipulator in configuration space
Nath et al. Teleoperation with kinematically redundant robot manipulators with sub-task objectives
Markdahl et al. Distributed cooperative object attitude manipulation
Zhou et al. Collision-free compliance control for redundant manipulators: an optimization case
Huang et al. Toward generalizable robotic dual-arm flipping manipulation
Prats et al. Vision force control in task-oriented grasping and manipulation
Stulp et al. Reinforcement learning of impedance control in stochastic force fields
Yu et al. A haptic shared control algorithm for flexible human assistance to semi-autonomous robots
Hu et al. Neural learning of stable dynamical systems based on extreme learning machine
CN105467841B (zh) 一种类人机器人上肢运动的类神经控制方法
Sun et al. A Framework of Robot Manipulability Learning and Control, and Its Application in Telerobotics
Papageorgiou et al. Learning by demonstration for constrained tasks
Padula et al. A new pseudoinverse for manipulator collision avoidance
Jia et al. Tri-criteria optimization motion planning at acceleration-level of dual redundant manipulators
Liu et al. Contact Force/Motion Hybrid Control for a Hydraulic Legged Mobile Manipulator via a Force-Controlled Floating Base
Zhai et al. Effective learning and online modulation for robotic variable impedance skills

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
CP01 Change in the name or title of a patent holder

Address after: 510070 Building No. 15, compound 100, martyrs Road, Guangzhou, Guangdong

Patentee after: Institute of intelligent manufacturing, Guangdong Academy of Sciences

Address before: 510070 Building No. 15, compound 100, martyrs Road, Guangzhou, Guangdong

Patentee before: GUANGDONG INSTITUTE OF INTELLIGENT MANUFACTURING

CP01 Change in the name or title of a patent holder