CN114347045A - 一种双机械臂协同运动控制方法及系统 - Google Patents

一种双机械臂协同运动控制方法及系统 Download PDF

Info

Publication number
CN114347045A
CN114347045A CN202210279442.9A CN202210279442A CN114347045A CN 114347045 A CN114347045 A CN 114347045A CN 202210279442 A CN202210279442 A CN 202210279442A CN 114347045 A CN114347045 A CN 114347045A
Authority
CN
China
Prior art keywords
coordinate system
mechanical arm
arm
slave
main
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
CN202210279442.9A
Other languages
English (en)
Other versions
CN114347045B (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.)
Foshan Guanwan Intelligent Technology Co ltd
Foshan Institute Of Intelligent Equipment Technology
Original Assignee
Foshan Guanwan Intelligent Technology Co ltd
Foshan Institute Of Intelligent Equipment Technology
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 Foshan Guanwan Intelligent Technology Co ltd, Foshan Institute Of Intelligent Equipment Technology filed Critical Foshan Guanwan Intelligent Technology Co ltd
Priority to CN202210279442.9A priority Critical patent/CN114347045B/zh
Publication of CN114347045A publication Critical patent/CN114347045A/zh
Application granted granted Critical
Publication of CN114347045B publication Critical patent/CN114347045B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Manipulator (AREA)

Abstract

本发明涉及机器人应用技术领域，具体涉及一种双机械臂协同运动控制方法及系统，双机械臂包括主机械臂和从机械臂，其方法包括：标定主机械臂基坐标系和从机械臂基坐标系；建立主机械臂和从机械臂间的位姿约束关系和速度约束关系；构建自适应阻抗控制器并约束其稳定性；生成主臂参考运动轨迹并利用自适应阻抗控制器进行跟踪误差修正，再转换为关节空间下的主臂运动轨迹；将修正后的主臂参考运动轨迹转换为从臂参考运动轨迹，再转换为关节空间下的从臂运动轨迹；控制主机械臂按照主臂运动轨迹移动以及控制从机械臂按照从臂运动轨迹移动，使得双机械臂夹取的物品到达目标位置。本发明通过引入自适应阻抗控制器提高对双机械臂协同运动的控制性能。

Description

一种双机械臂协同运动控制方法及系统
技术领域
本发明涉及机器人应用技术领域，具体是涉及一种双机械臂协同运动控制方法及系统。
背景技术
双机械臂相对于单机械臂而言具有搬运大负载、灵巧敏捷、适应复杂环境的优势，然而在双机械臂的协作过程中，需要采用两个独立的控制器来进行运动控制，彼此之间的协调性能变差，并且为了提高双机械臂的搬运效果，对双机械臂的协同控制将更加复杂、困难。目前已有学者提出对双机械臂实施主从控制思想，即在主从机械臂之间满足一定约束关系时，对主机械臂采用基于位置的控制方式，对从机械臂通过力传感器感知主机械臂的运动趋势来实现跟随主机械臂运动的目的，这也就要求从机械臂需要具备快速的力跟随响应能力以避免出现系统不稳定，然而在实际应用时，力传感器和位置控制器很难做到高速响应，因此当前的主从控制策略只适用于低速场合。
发明内容
本发明提供一种双机械臂协同运动控制方法及系统，以解决现有技术中所存在的一个或多个技术问题，至少提供一种有益的选择或创造条件。
本发明实施例提供一种双机械臂协同运动控制方法，所述双机械臂包括主机械臂和从机械臂，所述方法包括：
利用主机械臂自带的第一示教器对主机械臂基坐标系进行标定，再利用从机械臂自带的第二示教器对从机械臂基坐标系进行标定；
建立主机械臂与从机械臂之间的位姿约束关系和速度约束关系；
以减少位置误差和力跟踪误差为目标来构建自适应阻抗控制器，同时基于李雅普诺夫稳定性定理对所述自适应阻抗控制器的稳定性进行约束；
生成主机械臂将物品从初始位置抓取至目标位置的主臂参考运动轨迹，利用所述自适应阻抗控制器对所述主臂参考运动轨迹进行跟踪误差修正，再基于逆运动学将修正后的主臂参考运动轨迹转换为在关节空间下的主臂运动轨迹；
基于所述位姿约束关系对从机械臂进行位置控制，将所述修正后的主臂参考运动轨迹转换得到从臂参考运动轨迹，再基于逆运动学将所述从臂参考运动轨迹转换为在关节空间下的从臂运动轨迹；
基于所述速度约束关系控制主机械臂按照所述主臂运动轨迹进行移动以及控制从机械臂按照所述从臂运动轨迹进行移动，使得物品到达所述目标位置。
进一步地，所述自适应阻抗控制器的数学模型为：
Figure 287436DEST_PATH_IMAGE001
其中，
Figure 321251DEST_PATH_IMAGE002
为加速度，
Figure DEST_PATH_IMAGE003
为速度，
Figure 32855DEST_PATH_IMAGE004
为位置，
Figure DEST_PATH_IMAGE005
为期望位置，
Figure 105502DEST_PATH_IMAGE006
为加速度误差，
Figure DEST_PATH_IMAGE007
为速度 误差，
Figure 182042DEST_PATH_IMAGE008
为位置误差，
Figure DEST_PATH_IMAGE009
、
Figure 765470DEST_PATH_IMAGE010
和
Figure DEST_PATH_IMAGE011
均为所述自适应阻抗控制器的数学模型的影响系数，
Figure 202137DEST_PATH_IMAGE012
为阻尼参数，
Figure DEST_PATH_IMAGE013
为质量参数，
Figure 125093DEST_PATH_IMAGE014
为感知接触力变化的灵敏度，
Figure DEST_PATH_IMAGE015
为环境刚度，
Figure 434852DEST_PATH_IMAGE016
为刚度 系数，
Figure DEST_PATH_IMAGE017
为力跟踪误差，
Figure 692527DEST_PATH_IMAGE018
为期望力，
Figure DEST_PATH_IMAGE019
为环境位置，
Figure 745933DEST_PATH_IMAGE020
为辅助项。
进一步地，所述基于李雅普诺夫稳定性定理对所述自适应阻抗控制器的稳定性进行约束包括：
基于李雅普诺夫稳定性定理，确定所述自适应阻抗控制器的稳定性测量函数为：
Figure DEST_PATH_IMAGE021
计算对所述稳定性测量函数的微分结果为：
Figure 507085DEST_PATH_IMAGE022
其中：
Figure DEST_PATH_IMAGE023
根据所述自适应阻抗控制器保持稳定运行的条件为微分结果
Figure 987745DEST_PATH_IMAGE024
小于0，结 合影响系数
Figure DEST_PATH_IMAGE025
、
Figure 749027DEST_PATH_IMAGE026
和
Figure DEST_PATH_IMAGE027
的表达式确定所述自适应阻抗控制器的控制率为：
Figure 278229DEST_PATH_IMAGE028
式中，
Figure DEST_PATH_IMAGE029
为所述自适应阻抗控制器的稳定性系数，
Figure 972515DEST_PATH_IMAGE030
为所述自适应阻抗控 制器与理想自适应阻抗控制器之间的模型误差矩阵，
Figure 79536DEST_PATH_IMAGE032
为时间，
Figure DEST_PATH_IMAGE033
为转置符号，
Figure 859273DEST_PATH_IMAGE034
为非奇异正 定矩阵，
Figure DEST_PATH_IMAGE035
、
Figure 192166DEST_PATH_IMAGE036
和
Figure DEST_PATH_IMAGE037
均为特征矩阵
Figure 6538DEST_PATH_IMAGE038
的参数，
Figure DEST_PATH_IMAGE039
和
Figure 750372DEST_PATH_IMAGE040
均为理想自适应阻抗控制器的 数学模型的影响系数，
Figure DEST_PATH_IMAGE041
为影响系数
Figure 17405DEST_PATH_IMAGE025
的一阶导数，
Figure 419568DEST_PATH_IMAGE042
为影响系数
Figure DEST_PATH_IMAGE043
的一阶导数，
Figure 88446DEST_PATH_IMAGE044
为影响系数
Figure DEST_PATH_IMAGE045
的一阶导数，
Figure 268761DEST_PATH_IMAGE046
为正定对称矩阵，
Figure DEST_PATH_IMAGE047
为假设的变化参数，
Figure 944462DEST_PATH_IMAGE048
、
Figure DEST_PATH_IMAGE049
和
Figure 681473DEST_PATH_IMAGE050
均为 假设的变换矩阵
Figure DEST_PATH_IMAGE051
的参数，
Figure 408121DEST_PATH_IMAGE052
为理想位置误差，
Figure 837965DEST_PATH_IMAGE053
为理想速度误差，
Figure DEST_PATH_IMAGE054
为辅助项
Figure 17274DEST_PATH_IMAGE055
的初 始值，
Figure DEST_PATH_IMAGE056
为初始力跟踪误差，
Figure 292397DEST_PATH_IMAGE057
为初始灵敏度，
Figure DEST_PATH_IMAGE058
、
Figure 119889DEST_PATH_IMAGE059
和
Figure DEST_PATH_IMAGE060
均为常数。
进一步地，所述利用主机械臂自带的第一示教器对主机械臂基坐标系进行标定，再利用从机械臂自带的第二示教器对从机械臂基坐标系进行标定包括：
在主机械臂和从机械臂之间所形成的共同空间内固定放置一个标准工件，且在主机械臂的末端执行器处固定设置第一标定针和在从机械臂的末端执行器处固定设置第二标定针；
基于主机械臂自带的第一示教器已优先创建主机械臂基坐标系，当通过第一示教器控制主机械臂从初始位置开始移动直至第一标定针的顶端与标准工件的顶端接触时，获取标准工件相对于主机械臂基坐标系的第一位姿标定结果；
基于从机械臂自带的第二示教器已优先创建从机械臂基坐标系，当通过第二示教器控制从机械臂从初始位置开始移动直至第二标定针的顶端与标准工件的顶端接触时，获取标准工件相对于从机械臂基坐标系的第二位姿标定结果；
根据第一位姿标定结果和第二位姿标定结果，确定主机械臂基坐标系与从机械臂基坐标系之间的齐次变换矩阵。
进一步地，所述主机械臂与从机械臂之间的位姿约束关系为：
Figure 455056DEST_PATH_IMAGE061
其中，
Figure DEST_PATH_IMAGE062
为主机械臂基坐标系相对于工件坐标系的齐次变换矩阵，
Figure 121661DEST_PATH_IMAGE063
为从机械 臂基坐标系相对于工件坐标系的齐次变换矩阵，
Figure DEST_PATH_IMAGE064
为主机械臂末端执行器所在坐标系相 对于主机械臂基坐标系的齐次变换矩阵，
Figure 200475DEST_PATH_IMAGE065
为从机械臂末端执行器所在坐标系相对于从 机械臂基坐标系的齐次变换矩阵，
Figure DEST_PATH_IMAGE066
为物品所在坐标系相对于主机械臂末端执行器所在 坐标系的齐次变换矩阵，
Figure 619824DEST_PATH_IMAGE067
为物品所在坐标系相对于从机械臂末端执行器所在坐标系的齐 次变换矩阵，
Figure DEST_PATH_IMAGE068
为主机械臂基坐标系与从机械臂基坐标系之间的齐次变换矩阵。
进一步地，所述主机械臂与从机械臂之间的速度约束关系为：
Figure 594733DEST_PATH_IMAGE069
其中，
Figure DEST_PATH_IMAGE070
为单位矩阵，
Figure 545372DEST_PATH_IMAGE071
为零矩阵，
Figure DEST_PATH_IMAGE072
为物品所在坐标系相对于世界坐标系的旋 转矩阵，
Figure 614828DEST_PATH_IMAGE073
为主机械臂末端执行器所在坐标系相对于物品所在坐标系的位置矩阵，
Figure DEST_PATH_IMAGE074
为主 机械臂末端执行器所在坐标系相对于世界坐标系的速度矩阵，
Figure 967312DEST_PATH_IMAGE075
为主机械臂末端执行器 所在坐标系相对于世界坐标系的角速度矩阵，
Figure DEST_PATH_IMAGE076
为从机械臂末端执行器所在坐标系相对于 物品所在坐标系的位置矩阵，
Figure 831231DEST_PATH_IMAGE077
为从机械臂末端执行器所在坐标系相对于世界坐标系的 速度矩阵，
Figure DEST_PATH_IMAGE078
为从机械臂末端执行器所在坐标系相对于世界坐标系的角速度矩阵。
进一步地，所述生成主机械臂将物品从初始位置抓取至目标位置的主臂参考运动轨迹包括：
当双机械臂抓取到物品时，规划主机械臂将物品从初始位置抓取至目标位置的主臂初始运动轨迹；
对所述主臂初始运动轨迹进行位姿插补运算，得到主臂参考运动轨迹。
另外，本发明实施例还提供了一种双机械臂协同运动控制系统，所述系统包括：
一个或多个处理器；
存储器；
一个或多个应用程序，其存储在所述存储器中并被配置为由所述一个或多个处理器执行，所述一个或多个应用程序配置用于执行上述的双机械臂协同运动控制方法。
本发明至少具有以下有益效果：通过利用双机械臂自带的示教器执行单点标定，相对于传统的标定方法需要借助摄像机并引入最小二乘法来执行双机械臂之间的齐次变换矩阵的拟合求解而言，操作简单且精度更高，在一定程度上将减少双机械臂末端执行器的轨迹误差。本发明提出的自适应阻抗控制器是在传统阻抗控制器上作出的改进，可克服双机械臂运作时所存在的环境刚度不确定、对机械臂末端执行器的接触力变化不敏感的问题，对机械臂的控制性能更强，更能适应复杂的工作环境。本发明提出对主机械臂进行自适应阻抗控制，对从机械臂进行位置控制，通过使用单个力传感器起到两个力传感器协同搬运的效果，在一定程度上将节省成本。
附图说明
附图用来提供对本发明技术方案的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明的技术方案，并不构成对本发明技术方案的限制。
图1是本发明实施例中的一种双机械臂协同运动控制方法的流程示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。
需要说明的是，虽然在系统示意图中进行了功能模块划分，在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于系统中的模块划分，或流程图中的顺序执行所示出或描述的步骤。说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。
请参考图1，图1是本发明实施例提供的一种双机械臂协同运动控制方法的流程示意图，其中所述双机械臂包括主机械臂和从机械臂，所述方法包括如下步骤：
S101、利用主机械臂自带的第一示教器对主机械臂基坐标系进行标定，再利用从机械臂自带的第二示教器对从机械臂基坐标系进行标定。
本发明实施例在执行标定前的准备工作为：在主机械臂和从机械臂之间所形成的共同空间内固定放置一个标准工件，同时确保以标准工件的顶端为原点所创建的工件坐标系相对于世界坐标系的方向相同，且在主机械臂的末端执行器处固定设置第一标定针和在从机械臂的末端执行器处固定设置第二标定针。
基于主机械臂自带的第一示教器已优先创建主机械臂基坐标系以及从机械臂自带的第二示教器已优先创建从机械臂基坐标系，本发明实施例执行标定的具体实施过程如下：
（1）当通过第一示教器控制主机械臂从初始位置开始移动直至第一标定针的顶端 与标准工件的顶端接触时，获取标准工件相对于主机械臂基坐标系的第一位姿标定结果为
Figure 472428DEST_PATH_IMAGE079
，其中
Figure 893045DEST_PATH_IMAGE080
为标准工件在主机械臂基坐标系下的位置坐标 信息，
Figure 365615DEST_PATH_IMAGE081
为工件坐标系相对于主机械臂基坐标系的旋转角度信息，
Figure 479064DEST_PATH_IMAGE082
为工件坐标 系相对于主机械臂基坐标系的滚转角，
Figure 341978DEST_PATH_IMAGE083
为工件坐标系相对于主机械臂基坐标系的俯仰 角，
Figure 566286DEST_PATH_IMAGE084
为工件坐标系相对于主机械臂基坐标系的偏航角。
（2）当通过第二示教器控制从机械臂从初始位置开始移动直至第二标定针的顶端 与标准工件的顶端接触时，获取标准工件相对于从机械臂基坐标系的第二位姿标定结果为
Figure 627783DEST_PATH_IMAGE085
，其中
Figure 912134DEST_PATH_IMAGE086
为标准工件在从机械臂基坐标系下的坐标信 息，
Figure 514541DEST_PATH_IMAGE087
为工件坐标系相对于从机械臂基坐标系的旋转角度信息，
Figure 542540DEST_PATH_IMAGE088
为工件坐标系相 对于从机械臂基坐标系的滚转角，
Figure 724122DEST_PATH_IMAGE089
为工件坐标系相对于从机械臂基坐标系的俯仰角，
Figure 851478DEST_PATH_IMAGE090
为工件坐标系相对于从机械臂基坐标系的偏航角。
（3）根据第一位姿标定结果和第二位姿标定结果，确定主机械臂基坐标系与从机械臂基坐标系之间的齐次变换矩阵。
在步骤（3）中，首先根据第一位姿标定结果可确定主机械臂基坐标系相对于工件坐标系的齐次变换矩阵为：
Figure 751301DEST_PATH_IMAGE091
再根据第二位姿标定结果可确定从机械臂基坐标系相对于工件坐标系的齐次变换矩阵为：
Figure 317412DEST_PATH_IMAGE092
最后确定主机械臂基坐标系与从机械臂基坐标系之间的齐次变换矩阵为：
Figure 619080DEST_PATH_IMAGE093
其中，c为余弦函数cos的缩写，s为正弦函数sin的缩写。
S102、建立主机械臂与从机械臂之间的位姿约束关系和速度约束关系。
在本发明实施例中，由于齐次变换矩阵可用来表示机械臂的位置和姿态，进而建立主机械臂与从机械臂之间的位姿约束关系为：
Figure 901026DEST_PATH_IMAGE094
其中，
Figure 288145DEST_PATH_IMAGE062
为主机械臂基坐标系相对于工件坐标系的齐次变换矩阵，
Figure 657946DEST_PATH_IMAGE063
为从机械 臂基坐标系相对于工件坐标系的齐次变换矩阵，
Figure 751804DEST_PATH_IMAGE064
为主机械臂末端执行器所在坐标系相 对于主机械臂基坐标系的齐次变换矩阵，
Figure 548859DEST_PATH_IMAGE095
为从机械臂末端执行器所在坐标系相对于从 机械臂基坐标系的齐次变换矩阵，
Figure 157695DEST_PATH_IMAGE066
为物品所在坐标系相对于主机械臂末端执行器所在 坐标系的齐次变换矩阵，
Figure 518138DEST_PATH_IMAGE096
为物品所在坐标系相对于从机械臂末端执行器所在坐标系的齐 次变换矩阵，
Figure 528819DEST_PATH_IMAGE068
为主机械臂基坐标系与从机械臂基坐标系之间的齐次变换矩阵。
在本发明实施例中，主机械臂与从机械臂之间的速度约束关系的建立过程具体如下：
首先确定双机械臂之间所抓取的物品的运动速度与双机械臂末端执行器（包括主机械臂末端执行器和从机械臂末端执行器）的运行速度满足：
Figure 496775DEST_PATH_IMAGE097
由于物品与双机械臂形成闭链，三者之间无相对运动，再确定物品的角速度与双机械臂末端执行器的角速度满足：
Figure 592907DEST_PATH_IMAGE098
由于在搬运物品时双机械臂的速度和加速度均保持相对一致，最后确定主机械臂与从机械臂之间的速度约束关系为：
Figure 225883DEST_PATH_IMAGE099
其中，
Figure 91071DEST_PATH_IMAGE100
为物品所在坐标系相对于世界坐标系的速度矩阵，
Figure 229928DEST_PATH_IMAGE101
为物品所在坐标系 相对于世界坐标系的角速度矩阵，
Figure 16618DEST_PATH_IMAGE102
为3×3的单位矩阵，
Figure 266334DEST_PATH_IMAGE103
为3×3的零矩阵，
Figure 251608DEST_PATH_IMAGE104
为物品所 在坐标系相对于世界坐标系的旋转矩阵，
Figure 561366DEST_PATH_IMAGE073
为主机械臂末端执行器所在坐标系相对于物品 所在坐标系的位置矩阵，
Figure 569773DEST_PATH_IMAGE074
为主机械臂末端执行器所在坐标系相对于世界坐标系的速度 矩阵，
Figure 623180DEST_PATH_IMAGE105
为主机械臂末端执行器所在坐标系相对于世界坐标系的角速度矩阵，
Figure 462960DEST_PATH_IMAGE106
为从机械 臂末端执行器所在坐标系相对于物品所在坐标系的位置矩阵，
Figure 943620DEST_PATH_IMAGE107
为从机械臂末端执行器 所在坐标系相对于世界坐标系的速度矩阵，
Figure 420082DEST_PATH_IMAGE108
为从机械臂末端执行器所在坐标系相对于 世界坐标系的角速度矩阵。
S103、以减少位置误差和力跟踪误差为目标来构建自适应阻抗控制器，同时基于李雅普诺夫稳定性定理对所述自适应阻抗控制器的稳定性进行约束。
在本发明实施例中，由于传统阻抗控制器对机械臂位置跟踪的精度不高且对机械臂末端执行器接触力变化的感知不敏感，此时以减少位置误差和力跟踪误差为目标，构建出自适应阻抗控制器的数学模型为：
Figure 277180DEST_PATH_IMAGE109
结合
Figure 971466DEST_PATH_IMAGE110
，可将上述数学模型进一步转换为：
Figure 560711DEST_PATH_IMAGE111
其中，
Figure 606027DEST_PATH_IMAGE112
为加速度，
Figure 1236DEST_PATH_IMAGE003
为速度，
Figure 471401DEST_PATH_IMAGE004
为位置，
Figure 559442DEST_PATH_IMAGE005
为期望位置，
Figure 826476DEST_PATH_IMAGE006
为加速度误差，
Figure 228638DEST_PATH_IMAGE007
为速度 误差，
Figure 631938DEST_PATH_IMAGE008
为位置误差，
Figure 890881DEST_PATH_IMAGE113
、
Figure 832161DEST_PATH_IMAGE010
和
Figure 569173DEST_PATH_IMAGE011
均为所述自适应阻抗控制器的数学模型的影响系数，
Figure 92558DEST_PATH_IMAGE012
为阻尼参数，
Figure 522402DEST_PATH_IMAGE013
为质量参数，
Figure 419820DEST_PATH_IMAGE114
为感知接触力变化的灵敏度，
Figure 960523DEST_PATH_IMAGE015
为环境刚度，
Figure 603994DEST_PATH_IMAGE016
为刚度 系数，
Figure 142422DEST_PATH_IMAGE115
为力跟踪误差，
Figure 871344DEST_PATH_IMAGE018
为期望力，
Figure 950158DEST_PATH_IMAGE019
为环境位置，
Figure 575699DEST_PATH_IMAGE020
为辅助项。
在此基础上，基于李雅普诺夫稳定性定理对所述自适应阻抗控制器的稳定性进行约束，具体实施过程如下：
首先确定所述自适应阻抗控制器的稳定性测量函数为：
Figure 81767DEST_PATH_IMAGE116
再计算对所述稳定性测量函数的微分结果为：
Figure 297985DEST_PATH_IMAGE117
其中：
Figure 118173DEST_PATH_IMAGE118
接着，根据所述自适应阻抗控制器保持稳定运行的条件为微分结果
Figure 470657DEST_PATH_IMAGE119
小于 0，则令关于
Figure 334577DEST_PATH_IMAGE120
表达式的前三项为0，进而得到：
Figure 772511DEST_PATH_IMAGE121
最后结合上述关于影响系数
Figure 193128DEST_PATH_IMAGE025
、
Figure 868960DEST_PATH_IMAGE122
和
Figure 716831DEST_PATH_IMAGE123
的表达式，确定所述自适应阻抗控制 器的控制率为：
Figure 642061DEST_PATH_IMAGE124
式中，
Figure 53320DEST_PATH_IMAGE125
为所述自适应阻抗控制器的稳定性系数，
Figure 380396DEST_PATH_IMAGE126
为所述自适应阻抗控 制器与理想自适应阻抗控制器之间的模型误差矩阵，
Figure 664747DEST_PATH_IMAGE127
为时间，
Figure 998645DEST_PATH_IMAGE128
为转置符号，
Figure 761065DEST_PATH_IMAGE129
为非奇异正 定矩阵，
Figure 942647DEST_PATH_IMAGE130
、
Figure 397900DEST_PATH_IMAGE036
和
Figure 500985DEST_PATH_IMAGE131
均为特征矩阵
Figure 67095DEST_PATH_IMAGE132
的参数，
Figure 103184DEST_PATH_IMAGE133
和
Figure 729338DEST_PATH_IMAGE134
均为理想自适应阻抗控制器的数 学模型的影响系数，
Figure 54140DEST_PATH_IMAGE135
为影响系数
Figure 158362DEST_PATH_IMAGE136
的一阶导数，
Figure 314537DEST_PATH_IMAGE137
为影响系数
Figure 30033DEST_PATH_IMAGE138
的一阶导数，
Figure 638869DEST_PATH_IMAGE139
为影响系数
Figure 812362DEST_PATH_IMAGE123
的一阶导数，
Figure 26305DEST_PATH_IMAGE046
为正定对称矩阵，
Figure 994261DEST_PATH_IMAGE047
为假设的变化参数，
Figure 90393DEST_PATH_IMAGE140
、
Figure 536418DEST_PATH_IMAGE141
和
Figure 588557DEST_PATH_IMAGE142
均为假 设的变换矩阵
Figure 930676DEST_PATH_IMAGE143
的参数，
Figure 514104DEST_PATH_IMAGE144
为理想位置误差，
Figure 763820DEST_PATH_IMAGE145
为理想速度误差，
Figure 936044DEST_PATH_IMAGE054
为辅助项
Figure 245803DEST_PATH_IMAGE055
的初始 值，
Figure 316527DEST_PATH_IMAGE146
为初始力跟踪误差，
Figure 291305DEST_PATH_IMAGE147
为初始灵敏度，
Figure 131085DEST_PATH_IMAGE148
、
Figure 611745DEST_PATH_IMAGE059
和
Figure 904186DEST_PATH_IMAGE149
均为常数。
S104、生成主机械臂将物品从初始位置抓取至目标位置的主臂参考运动轨迹，利用所述自适应阻抗控制器对所述主臂参考运动轨迹进行跟踪误差修正，再基于逆运动学将修正后的主臂参考运动轨迹转换为在关节空间下的主臂运动轨迹。
本发明实施过程包括：当双机械臂抓取到物品时，首先规划主机械臂将物品从初始位置抓取至目标位置的主臂初始运动轨迹，利用现有的轨迹插补算法对所述主臂初始运动轨迹进行位姿插补运算以得到若干个离散的插补点，再将所述若干个插补点进行依次连接以得到主臂参考运动轨迹；其次利用所述自适应阻抗控制器对所述主臂参考运动轨迹进行跟踪误差修正，此步骤属于对双机械臂的常规控制操作，而本发明的目的主要是对传统阻抗控制器作出改进，在此不再进行赘述；最后对修正后的主臂参考运动轨迹所包含的各个主臂位姿数据点进行逆运动学计算以得到对应的各个主臂关节控制点，再将所述各个主臂关节控制点进行时间串联以得到主臂运动轨迹。
S105、基于所述位姿约束关系对从机械臂进行位置控制，将所述修正后的主臂参考运动轨迹转换得到从臂参考运动轨迹，再基于逆运动学将所述从臂参考运动轨迹转换为在关节空间下的从臂运动轨迹。
本发明实施过程包括：首先将所述修正后的主臂参考运动轨迹所包含的各个主臂位姿数据点按照上述步骤S102所提供的位姿约束关系式进行逐个转换以得到对应的各个从臂位姿数据点，再将所述各个从臂位姿数据点进行依次连接以得到从臂参考运动轨迹；最后将所述各个从臂位姿数据点进行逆运动学计算以得到对应的各个从臂关节控制点，再将所述各个从臂关节控制点进行时间串联以得到从臂运动轨迹。
S106、基于所述速度约束关系控制主机械臂按照所述主臂运动轨迹进行移动以及控制从机械臂按照所述从臂运动轨迹进行移动，使得物品到达所述目标位置。
在本发明实施例中，通过利用双机械臂自带的示教器执行单点标定，相对于传统的标定方法需要借助摄像机并引入最小二乘法来执行双机械臂之间的齐次变换矩阵的拟合求解而言，操作简单且精度更高，在一定程度上将减少双机械臂末端执行器的轨迹误差。本发明提出的自适应阻抗控制器是在传统阻抗控制器上作出的改进，可克服双机械臂运作时所存在的环境刚度不确定、对机械臂末端执行器的接触力变化不敏感的问题，对机械臂的控制性能更强，更能适应复杂的工作环境。本发明提出对主机械臂进行自适应阻抗控制，对从机械臂进行位置控制，通过使用单个力传感器起到两个力传感器协同搬运的效果，在一定程度上将节省成本。
另外，本发明实施例提供一种双机械臂协同运动控制系统，所述系统包括：
一个或多个处理器；
存储器；
一个或多个应用程序，其存储在所述存储器中并被配置为由所述一个或多个处理器执行，所述一个或多个应用程序配置用于执行上述实施例中的双机械臂协同运动控制方法。
上述方法实施例中的内容均适用于本系统实施例中，本系统实施例所实现的功能与上述方法实施例相同，并且所达到的有益效果与上述方法实施例相同。
所述处理器可以是中央处理单元(Central-Processing-Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital-Signal-Processor，DSP)、专用集成电路(Application-Specific-Integrated-Circuit，ASIC)、现场可编程门阵列(Field-Programmable-Gate-Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，所述处理器是双机械臂协同运动控制系统的控制中心，利用各种接口和线路连接整个双机械臂协同运动控制系统可运行装置的各个部分。
所述存储器可用于存储所述计算机程序和/或模块，所述处理器通过运行或执行存储在所述存储器内的计算机程序和/或模块，以及调用存储在存储器内的数据，实现双机械臂协同运动控制系统的各种功能。所述存储器可主要包括存 储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储数据区可存储根据手机的使用所创建的数据(比如音频数据、电话本等)等。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(Smart-Media-Card，SMC)，安全数字(Secure-Digital，SD)卡，闪存卡(Flash-Card)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。
尽管本申请的描述已经相当详尽且特别对几个实施例进行描述，但其并非旨在局限于任何这些细节或实施例或任何特殊实施例，而是应当将其视作是通过参考所附权利要求，考虑到现有技术为这些权利要求提供广义的可能性解释，从而有效地涵盖本申请的预定范围。此外，上文以发明人可预见的实施例对本申请进行描述，其目的是为了提供有用的描述，而那些目前尚未预见的对本申请的非实质性改动仍可代表本申请的等效改动。

Claims (8)

1.一种双机械臂协同运动控制方法，其特征在于，所述双机械臂包括主机械臂和从机械臂，所述方法包括：
利用主机械臂自带的第一示教器对主机械臂基坐标系进行标定，再利用从机械臂自带的第二示教器对从机械臂基坐标系进行标定；
建立主机械臂与从机械臂之间的位姿约束关系和速度约束关系；
以减少位置误差和力跟踪误差为目标来构建自适应阻抗控制器，同时基于李雅普诺夫稳定性定理对所述自适应阻抗控制器的稳定性进行约束；
生成主机械臂将物品从初始位置抓取至目标位置的主臂参考运动轨迹，利用所述自适应阻抗控制器对所述主臂参考运动轨迹进行跟踪误差修正，再基于逆运动学将修正后的主臂参考运动轨迹转换为在关节空间下的主臂运动轨迹；
基于所述位姿约束关系对从机械臂进行位置控制，将所述修正后的主臂参考运动轨迹转换得到从臂参考运动轨迹，再基于逆运动学将所述从臂参考运动轨迹转换为在关节空间下的从臂运动轨迹；
基于所述速度约束关系控制主机械臂按照所述主臂运动轨迹进行移动以及控制从机械臂按照所述从臂运动轨迹进行移动，使得物品到达所述目标位置。
2.根据权利要求1所述的双机械臂协同运动控制方法，其特征在于，所述自适应阻抗控制器的数学模型为：
Figure 31012DEST_PATH_IMAGE001
其中，
Figure 399545DEST_PATH_IMAGE002
为加速度，
Figure 854797DEST_PATH_IMAGE003
为速度，
Figure 754620DEST_PATH_IMAGE004
为位置，
Figure 320731DEST_PATH_IMAGE005
为期望位置，
Figure 294503DEST_PATH_IMAGE006
为加速度误差，
Figure 920656DEST_PATH_IMAGE007
为速度误差，
Figure 42196DEST_PATH_IMAGE008
为位置误差，
Figure 598948DEST_PATH_IMAGE009
、
Figure 755123DEST_PATH_IMAGE010
和
Figure 552178DEST_PATH_IMAGE011
均为所述自适应阻抗控制器的数学模型的影响系数，
Figure 98697DEST_PATH_IMAGE012
为阻 尼参数，
Figure 6610DEST_PATH_IMAGE013
为质量参数，
Figure 17291DEST_PATH_IMAGE014
为感知接触力变化的灵敏度，
Figure 175128DEST_PATH_IMAGE015
为环境刚度，
Figure 271260DEST_PATH_IMAGE016
为刚度系数，
Figure 982864DEST_PATH_IMAGE017
为力跟踪误差，
Figure 113631DEST_PATH_IMAGE018
为期望力，
Figure 190171DEST_PATH_IMAGE019
为环境位置，
Figure 508020DEST_PATH_IMAGE020
为辅助项。
3.根据权利要求2所述的双机械臂协同运动控制方法，其特征在于，所述基于李雅普诺夫稳定性定理对所述自适应阻抗控制器的稳定性进行约束包括：
基于李雅普诺夫稳定性定理，确定所述自适应阻抗控制器的稳定性测量函数为：
Figure 23315DEST_PATH_IMAGE021
计算对所述稳定性测量函数的微分结果为：
Figure 8588DEST_PATH_IMAGE022
其中：
Figure 505298DEST_PATH_IMAGE023
根据所述自适应阻抗控制器保持稳定运行的条件为微分结果
Figure 310443DEST_PATH_IMAGE024
小于0，结合影 响系数
Figure 363849DEST_PATH_IMAGE025
、
Figure 141313DEST_PATH_IMAGE026
和
Figure 621972DEST_PATH_IMAGE011
的表达式确定所述自适应阻抗控制器的控制率为：
Figure 179993DEST_PATH_IMAGE027
式中，
Figure 958462DEST_PATH_IMAGE028
为所述自适应阻抗控制器的稳定性系数，
Figure 918328DEST_PATH_IMAGE029
为所述自适应阻抗控制器与 理想自适应阻抗控制器之间的模型误差矩阵，
Figure 569889DEST_PATH_IMAGE031
为时间，
Figure 349626DEST_PATH_IMAGE032
为转置符号，
Figure 682518DEST_PATH_IMAGE033
为非奇异正定矩 阵，
Figure 496890DEST_PATH_IMAGE034
、
Figure 319353DEST_PATH_IMAGE035
和
Figure 773337DEST_PATH_IMAGE036
均为特征矩阵
Figure 972237DEST_PATH_IMAGE037
的参数，
Figure 641116DEST_PATH_IMAGE038
和
Figure 572163DEST_PATH_IMAGE039
均为理想自适应阻抗控制器的数学 模型的影响系数，
Figure 326492DEST_PATH_IMAGE040
为影响系数
Figure 329083DEST_PATH_IMAGE041
的一阶导数，
Figure 852468DEST_PATH_IMAGE042
为影响系数
Figure 224192DEST_PATH_IMAGE043
的一阶导数，
Figure 465818DEST_PATH_IMAGE044
为影响系数
Figure 6520DEST_PATH_IMAGE045
的一阶导数，
Figure 384412DEST_PATH_IMAGE046
为正定对称矩阵，
Figure 922841DEST_PATH_IMAGE047
为假设的变化参数，
Figure 386183DEST_PATH_IMAGE048
、
Figure 730577DEST_PATH_IMAGE049
和
Figure 149926DEST_PATH_IMAGE050
均为假设 的变换矩阵
Figure 921573DEST_PATH_IMAGE051
的参数，
Figure 872211DEST_PATH_IMAGE052
为理想位置误差，
Figure 754716DEST_PATH_IMAGE053
为理想速度误差，
Figure 44883DEST_PATH_IMAGE054
为辅助项
Figure 987432DEST_PATH_IMAGE055
的初始 值，
Figure 425366DEST_PATH_IMAGE056
为初始力跟踪误差，
Figure 298513DEST_PATH_IMAGE057
为初始灵敏度，
Figure 505504DEST_PATH_IMAGE058
、
Figure 618953DEST_PATH_IMAGE059
和
Figure 544184DEST_PATH_IMAGE060
均为常数。
4.根据权利要求1所述的双机械臂协同运动控制方法，其特征在于，所述利用主机械臂自带的第一示教器对主机械臂基坐标系进行标定，再利用从机械臂自带的第二示教器对从机械臂基坐标系进行标定包括：
在主机械臂和从机械臂之间所形成的共同空间内固定放置一个标准工件，且在主机械臂的末端执行器处固定设置第一标定针和在从机械臂的末端执行器处固定设置第二标定针；
基于主机械臂自带的第一示教器已优先创建主机械臂基坐标系，当通过第一示教器控制主机械臂从初始位置开始移动直至第一标定针的顶端与标准工件的顶端接触时，获取标准工件相对于主机械臂基坐标系的第一位姿标定结果；
基于从机械臂自带的第二示教器已优先创建从机械臂基坐标系，当通过第二示教器控制从机械臂从初始位置开始移动直至第二标定针的顶端与标准工件的顶端接触时，获取标准工件相对于从机械臂基坐标系的第二位姿标定结果；
根据第一位姿标定结果和第二位姿标定结果，确定主机械臂基坐标系与从机械臂基坐标系之间的齐次变换矩阵。
5.根据权利要求1所述的双机械臂协同运动控制方法，其特征在于，所述主机械臂与从机械臂之间的位姿约束关系为：
Figure 706175DEST_PATH_IMAGE061
其中，
Figure 33251DEST_PATH_IMAGE062
为主机械臂基坐标系相对于工件坐标系的齐次变换矩阵，
Figure 52023DEST_PATH_IMAGE063
为从机械臂基坐 标系相对于工件坐标系的齐次变换矩阵，
Figure 651500DEST_PATH_IMAGE064
为主机械臂末端执行器所在坐标系相对于主 机械臂基坐标系的齐次变换矩阵，
Figure 679499DEST_PATH_IMAGE065
为从机械臂末端执行器所在坐标系相对于从机械臂 基坐标系的齐次变换矩阵，
Figure 861082DEST_PATH_IMAGE066
为物品所在坐标系相对于主机械臂末端执行器所在坐标系 的齐次变换矩阵，
Figure 316334DEST_PATH_IMAGE067
为物品所在坐标系相对于从机械臂末端执行器所在坐标系的齐次变换 矩阵，
Figure 888260DEST_PATH_IMAGE068
为主机械臂基坐标系与从机械臂基坐标系之间的齐次变换矩阵。
6.根据权利要求1所述的双机械臂协同运动控制方法，其特征在于，所述主机械臂与从机械臂之间的速度约束关系为：
Figure 454371DEST_PATH_IMAGE069
其中，
Figure 756039DEST_PATH_IMAGE070
为单位矩阵，
Figure 572073DEST_PATH_IMAGE071
为零矩阵，
Figure 693613DEST_PATH_IMAGE072
为物品所在坐标系相对于世界坐标系的旋转矩 阵，
Figure 63414DEST_PATH_IMAGE073
为主机械臂末端执行器所在坐标系相对于物品所在坐标系的位置矩阵，
Figure 219589DEST_PATH_IMAGE074
为主机械 臂末端执行器所在坐标系相对于世界坐标系的速度矩阵，
Figure 954327DEST_PATH_IMAGE075
为主机械臂末端执行器所在 坐标系相对于世界坐标系的角速度矩阵，
Figure 563163DEST_PATH_IMAGE076
为从机械臂末端执行器所在坐标系相对于物品 所在坐标系的位置矩阵，
Figure 471076DEST_PATH_IMAGE077
为从机械臂末端执行器所在坐标系相对于世界坐标系的速度 矩阵，
Figure 668708DEST_PATH_IMAGE078
为从机械臂末端执行器所在坐标系相对于世界坐标系的角速度矩阵。
7.根据权利要求1所述的双机械臂协同运动控制方法，其特征在于，所述生成主机械臂将物品从初始位置抓取至目标位置的主臂参考运动轨迹包括：
当双机械臂抓取到物品时，规划主机械臂将物品从初始位置抓取至目标位置的主臂初始运动轨迹；
对所述主臂初始运动轨迹进行位姿插补运算，得到主臂参考运动轨迹。
8.一种双机械臂协同运动控制系统，其特征在于，所述系统包括：
一个或多个处理器；
存储器；
一个或多个应用程序，其存储在所述存储器中并被配置为由所述一个或多个处理器执行，所述一个或多个应用程序配置用于执行如权利要求1-7中任意一项所述的双机械臂协同运动控制方法。
CN202210279442.9A 2022-03-22 2022-03-22 一种双机械臂协同运动控制方法及系统 Active CN114347045B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210279442.9A CN114347045B (zh) 2022-03-22 2022-03-22 一种双机械臂协同运动控制方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210279442.9A CN114347045B (zh) 2022-03-22 2022-03-22 一种双机械臂协同运动控制方法及系统

Publications (2)

Publication Number Publication Date
CN114347045A true CN114347045A (zh) 2022-04-15
CN114347045B CN114347045B (zh) 2022-05-20

Family

ID=81095213

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210279442.9A Active CN114347045B (zh) 2022-03-22 2022-03-22 一种双机械臂协同运动控制方法及系统

Country Status (1)

Country Link
CN (1) CN114347045B (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115724199A (zh) * 2022-12-07 2023-03-03 赛那德科技有限公司 基于相机视觉的无序包裹坐标标定方法
CN115890653A (zh) * 2022-09-28 2023-04-04 华中科技大学 基于多通道的双臂机器人协同控制方法、装置及可读介质
CN115946118A (zh) * 2022-12-30 2023-04-11 成都卡诺普机器人技术股份有限公司 一种多机器人同时与一个外部工装协同方法、介质及系统
CN117442359A (zh) * 2023-12-22 2024-01-26 无锡蔚动智能科技有限公司 一种基于双臂协同控制的医用无影灯控制系统及方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2009018380A (ja) * 2007-07-12 2009-01-29 Toyota Motor Corp ロボット、ロボットの制御方法、及びロボットの制御システム
CN113636348A (zh) * 2021-08-05 2021-11-12 张莉 一种建筑安装使用的玻璃转运系统

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2009018380A (ja) * 2007-07-12 2009-01-29 Toyota Motor Corp ロボット、ロボットの制御方法、及びロボットの制御システム
CN113636348A (zh) * 2021-08-05 2021-11-12 张莉 一种建筑安装使用的玻璃转运系统

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
双臂机器人协调运动规划研究: "庄照波", 《万方》 *
王琪 等: "双臂机器人的协调控制算法综述", 《计算机工程与应用》 *

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115890653A (zh) * 2022-09-28 2023-04-04 华中科技大学 基于多通道的双臂机器人协同控制方法、装置及可读介质
CN115724199A (zh) * 2022-12-07 2023-03-03 赛那德科技有限公司 基于相机视觉的无序包裹坐标标定方法
CN115946118A (zh) * 2022-12-30 2023-04-11 成都卡诺普机器人技术股份有限公司 一种多机器人同时与一个外部工装协同方法、介质及系统
CN115946118B (zh) * 2022-12-30 2023-12-19 成都卡诺普机器人技术股份有限公司 一种多机器人同时与一个外部工装协同方法、介质及系统
CN117442359A (zh) * 2023-12-22 2024-01-26 无锡蔚动智能科技有限公司 一种基于双臂协同控制的医用无影灯控制系统及方法

Also Published As

Publication number Publication date
CN114347045B (zh) 2022-05-20

Similar Documents

Publication Publication Date Title
CN114347045B (zh) 一种双机械臂协同运动控制方法及系统
CN111360827B (zh) 一种视觉伺服切换控制方法及系统
CN106475999B (zh) 刚性条件下基于阻抗模型的双臂协调的加速度控制方法
CN102226677B (zh) 具有协作关系的多机器人系统的基坐标系标定方法
CN104965517B (zh) 一种机器人笛卡尔空间轨迹的规划方法
CN109159151A (zh) 一种机械臂空间轨迹跟踪动态补偿方法和系统
CN107443382A (zh) 工业机器人结构参数误差辨识与补偿方法
CN113715016B (zh) 一种基于3d视觉的机器人抓取方法、系统、装置及介质
CN105945946A (zh) 一种基于g代码编程的六轴机械手运动控制方法
CN114952868B (zh) 7自由度srs型机械臂控制方法及装置、弹琴机器人
CN111258267B (zh) 一种人机协作搬运系统及其混合视触控制方法
CN108406769A (zh) 串联机器人的关节旋转角度确定方法及装置
CN113379849A (zh) 基于深度相机的机器人自主识别智能抓取方法及系统
CN107253191A (zh) 一种双机械臂系统及其协调控制方法
Marinho et al. Manipulator control based on the dual quaternion framework for intuitive teleoperation using kinect
CN111216136A (zh) 多自由度机械手臂控制系统、方法、存储介质、计算机
CN104267665B (zh) 一种视觉智能数控系统的数据交换方法
CN111267090A (zh) 一种双臂机器人主从控制系统及方法
CN109048911B (zh) 一种基于矩形特征的机器人视觉控制方法
CN116276990A (zh) 基于神经网络训练的两自由度并联结构运动学正解方法
JPH01267706A (ja) ロボットの制御方法
CN113927603B (zh) 机械臂拖动控制方法、装置、计算机设备以及存储介质
CN107443369A (zh) 一种基于视觉测量模型逆辨识的机器臂无标定伺服控制方法
CN108247636A (zh) 并联机器人闭环反馈控制方法、系统及存储介质
Kong et al. A novel approach to deriving the unit-homogeneous Jacobian matrices of mechanisms

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