CN106842907B - 一种多冗余度机械臂***的协同控制方法与装置 - Google Patents
一种多冗余度机械臂***的协同控制方法与装置 Download PDFInfo
- Publication number
- CN106842907B CN106842907B CN201710084021.XA CN201710084021A CN106842907B CN 106842907 B CN106842907 B CN 106842907B CN 201710084021 A CN201710084021 A CN 201710084021A CN 106842907 B CN106842907 B CN 106842907B
- Authority
- CN
- China
- Prior art keywords
- redundant manipulator
- target
- target redundant
- reference point
- manipulator
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 41
- 239000011159 matrix material Substances 0.000 claims abstract description 80
- 238000004891 communication Methods 0.000 claims abstract description 75
- 238000005457 optimization Methods 0.000 claims abstract description 27
- 239000012636 effector Substances 0.000 claims description 11
- 238000010276 construction Methods 0.000 claims description 9
- 238000013461 design Methods 0.000 claims description 4
- 238000006243 chemical reaction Methods 0.000 claims description 2
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 238000004519 manufacturing process Methods 0.000 description 3
- 238000003860 storage Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/0205—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric not using a model or a simulator of the controlled system
- G05B13/024—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric not using a model or a simulator of the controlled system in which a parameter or coefficient is automatically adjusted to optimise the performance
Landscapes
- Engineering & Computer Science (AREA)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Computation (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Numerical Control (AREA)
- Manipulator (AREA)
Abstract
本发明涉及机械臂控制领域,提出一种多冗余度机械臂***的协同控制方法与装置。所述控制方法包括:确定多冗余度机械臂***中的目标冗余度机械臂;获取所述目标冗余度机械臂的相邻冗余度机械臂的位置信息;获取所述目标冗余度机械臂的参考点轨迹信息;根据所述位置信息和所述参考点轨迹信息按照预设的规则构建所述目标冗余度机械臂对应的雅可比矩阵等式;根据二次型优化和标准二次规划方法确定所述目标冗余度机械臂的控制信号;根据所述控制信号控制所述目标冗余度机械臂,使得所述目标冗余度机械臂的可操作度最大化。利用本发明实施例提出的协同控制方法,可在通信受限的情况下实现多机械臂***的可操作度最大化分布式协同控制。
Description
技术领域
本发明涉及机械臂控制领域,具体涉及一种多冗余度机械臂***的协同控制方法与装置。
背景技术
冗余度机械臂具有自由度大于任务空间所需最少自由度的特性,广泛应用于装备制造、产品加工、机器作业等国民经济生产活动中,相较于传统的人工作业能大幅提高生产效率。当冗余度机械臂执行任务时,如何使得机械臂的可操作性最优化,以最大限度地躲避奇异,这种问题叫做机械臂可操作度最大化问题,在冗余度机械臂的运动规划中应当尽量保证可操作度的最大化。
在医疗手术、汽车装配等应用领域中,通常需要使用由多个冗余度机械臂组成的多冗余度机械臂***,因此如何对多冗余度机械臂***进行协同控制,保证多冗余度机械臂***中各个冗余度机械臂可操作度的最大化,成为人们必须考虑的问题。
目前,在广阔的操作环境中,多冗余度机械臂***中各个冗余度机械臂因距离以及通信负载的限制,通常不具有全局通信的能力,此外,控制中心也难以接入所有机械臂进行直接通信。在大规模环境下,冗余度机械臂之间有可能因为数量较多导致通信负载过大而难以通信,进而导致信息不能随时、随地进行共享,严重阻碍多机械臂***的可操作度最大化分布式协同控制。
发明内容
本发明提出一种多冗余度机械臂***的协同控制方法与装置,旨在解决如何在通信受限的情况下实现多机械臂***的可操作度最大化分布式协同控制的问题。
本发明实施例第一方面提供了一种多冗余度机械臂***的协同控制方法,所述多冗余度机械臂***包括两个以上的冗余度机械臂,所述两个以上的冗余度机械臂在通信拓扑图中连通;
所述控制方法包括:
确定所述两个以上的冗余度机械臂中至少一个冗余度机械臂为目标冗余度机械臂;
获取所述目标冗余度机械臂的相邻冗余度机械臂的位置信息,所述相邻冗余度机械臂与所述目标冗余度机械臂在通信拓扑图中相邻连接;
获取所述目标冗余度机械臂的参考点轨迹信息,所述参考点轨迹信息由预设的目标参考点和所述目标参考点的期望轨迹确定;
根据所述位置信息和所述参考点轨迹信息按照预设的规则构建所述目标冗余度机械臂对应的雅可比矩阵等式;
在所述雅可比矩阵等式、障碍物躲避不等式、关节角度极限和关节速度极限的约束下,根据二次型优化和标准二次规划方法确定所述目标冗余度机械臂的控制信号;
根据所述控制信号控制所述目标冗余度机械臂,使得所述目标冗余度机械臂的可操作度最大化。
本发明实施例第二方面提供了一种多冗余度机械臂***的协同控制装置,所述多冗余度机械臂***包括两个以上的冗余度机械臂,所述两个以上的冗余度机械臂在通信拓扑图中连通;
所述控制装置包括:
目标机械臂确定模块,用于确定所述两个以上的冗余度机械臂中至少一个冗余度机械臂为目标冗余度机械臂;
位置信息获取模块,用于获取所述目标冗余度机械臂的相邻冗余度机械臂的位置信息,所述相邻冗余度机械臂与所述目标冗余度机械臂在通信拓扑图中相邻连接;
参考点轨迹信息获取模块,用于获取所述目标冗余度机械臂的参考点轨迹信息,所述参考点轨迹信息由预设的目标参考点和所述目标参考点的期望轨迹确定;
等式构建模块,用于根据所述位置信息和所述参考点轨迹信息按照预设的规则构建所述目标冗余度机械臂对应的雅可比矩阵等式;
控制信号确定模块,用于在所述雅可比矩阵等式、障碍物躲避不等式、关节角度极限和关节速度极限的约束下,根据二次型优化和标准二次规划方法确定所述目标冗余度机械臂的控制信号;
机械臂控制模块,用于根据所述控制信号控制所述目标冗余度机械臂,使得所述目标冗余度机械臂的可操作度最大化。
在本发明实施例中,确定两个以上的冗余度机械臂中至少一个冗余度机械臂为目标冗余度机械臂;获取所述目标冗余度机械臂的相邻冗余度机械臂的位置信息,所述相邻冗余度机械臂与所述目标冗余度机械臂在通信拓扑图中相邻连接;获取所述目标冗余度机械臂的参考点轨迹信息,所述参考点轨迹信息由预设的目标参考点和所述目标参考点的期望轨迹确定;根据所述位置信息和所述参考点轨迹信息按照预设的规则构建所述目标冗余度机械臂对应的雅可比矩阵等式;在所述雅可比矩阵等式、障碍物躲避不等式、关节角度极限和关节速度极限的约束下,根据二次型优化和标准二次规划方法确定所述目标冗余度机械臂的控制信号;根据所述控制信号控制所述目标冗余度机械臂,使得所述目标冗余度机械臂的可操作度最大化。利用本发明实施例提出的协同控制方法,由于目标冗余度机械臂只需与数量较少的相邻冗余度机械臂进行通信,大大减小了通信负载,从而可在通信受限的情况下实现多机械臂***的可操作度最大化分布式协同控制。
附图说明
图1为本发明实施例中一种多冗余度机械臂***的协同控制方法一个实施例的流程图;
图2为图1中步骤104的一个实施例的具体流程图;
图3为图2中步骤1042的一个实施例的具体流程图;
图4为图1中步骤105的一个实施例的具体流程图;
图5为本发明实施例中一种多冗余度机械臂***的协同控制装置一个实施例的结构图。
具体实施方式
本发明提出一种多冗余度机械臂***的协同控制方法与装置,旨在解决如何在通信受限的情况下实现多机械臂***的可操作度最大化分布式协同控制的问题。
为使得本发明的发明目的、特征、优点能够更加的明显和易懂,下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,下面所描述的实施例仅仅是本发明一部分实施例,而非全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例,都属于本发明保护的范围。
图1示出了本发明一种多冗余度机械臂***的协同控制方法的一个实施例的流程图,所述多冗余度机械臂***包括两个以上的冗余度机械臂,所述两个以上的冗余度机械臂在通信拓扑图中连通;
如图1所示,所述控制方法包括:
101、确定所述两个以上的冗余度机械臂中至少一个冗余度机械臂为目标冗余度机械臂;
所述两个以上的冗余度机械臂在工作空间中按需分布,确定其中的一个或多个冗余度机械臂为目标冗余度机械臂,对所述目标冗余度机械臂实现可操作度最大化控制。
102、获取所述目标冗余度机械臂的相邻冗余度机械臂的位置信息,所述相邻冗余度机械臂与所述目标冗余度机械臂在通信拓扑图中相邻连接;
在确定目标冗余度机械臂后,获取所述目标冗余度机械臂的相邻冗余度机械臂的位置信息。所述相邻冗余度机械臂指与所述目标冗余度机械臂在通信拓扑图中相邻连接的冗余度机械臂,和目标冗余度机械臂属于同一多冗余度机械臂***。由于所述多冗余度机械臂***在通信拓扑图中是连通的,因此对于某个目标冗余度机械臂来说,其相邻冗余度机械臂的个数为一个或以上。所述位置信息可以是冗余度机械臂的末端执行器的位置信息,也可以是冗余度机械臂的其它部位的位置信息,目标冗余度机械臂可以和其相邻冗余度机械臂互相交换所述位置信息。
103、获取所述目标冗余度机械臂的参考点轨迹信息,所述参考点轨迹信息由预设的目标参考点和所述目标参考点的期望轨迹确定;
所述目标参考点是预设的位于目标冗余度机械臂工作空间中的位置参考点,所述参考点轨迹信息由所述目标参考点的位置及其期望轨迹确定。可以在多冗余度机械臂***的工作空间中设置一个控制中心,由所述控制中心将所述参考点轨迹信息发送给各个目标冗余度机械臂,也可以利用相邻冗余度机械臂或其它方式将所述参考点轨迹信息发送给各个目标冗余度机械臂。
104、根据所述位置信息和所述参考点轨迹信息按照预设的规则构建所述目标冗余度机械臂对应的雅可比矩阵等式;
在获取所述目标冗余度机械臂的相邻冗余度机械臂的位置信息和所述目标冗余度机械臂的参考点轨迹信息之后,根据所述位置信息和所述参考点轨迹信息按照预设的规则构建所述目标冗余度机械臂对应的雅可比矩阵等式。
进一步的,如图2所示,步骤104具体可以包括:
1041、构建所述目标冗余度机械臂对应的权重矩阵,所述权重矩阵中的各个元素为所述目标冗余度机械臂与所述两个以上的冗余度机械臂中的各个机械臂之间分别对应的第一连通权值;
所述权重矩阵的元素个数由所述多冗余度机械臂***中的机械臂个数决定,所述第一连通权值代表所述目标冗余度机械臂与所述两个以上的冗余度机械臂中的各个机械臂之间的通信连接关系。比如定义权重矩阵其第ij个元素定义为通信拓扑图上第i个冗余度机械臂与第j个冗余度机械臂之间的第一连通权值。
1042、根据所述参考点轨迹信息的获取状态确定所述目标冗余度机械臂的第二连通权值;
所述第二连通权值代表所述目标冗余度机械臂对于所述参考点轨迹信息的获取状态,可以用ρi来表示第i个冗余度机械臂的第二连通权值。
1043、根据所述权重矩阵、所述第二连通权值、所述位置信息和所述参考点轨迹信息构建所述目标冗余度机械臂对应的雅可比矩阵等式。
在构建权重矩阵以及确定第二连通权值后,可以根据所述权重矩阵、所述第二连通权值、所述位置信息和所述参考点轨迹信息构建所述目标冗余度机械臂对应的雅可比矩阵等式。
更进一步的,所述第一连通权值可以通过以下步骤确定:
(1)将所述目标冗余度机械臂与所述目标冗余度机械臂自身之间的第一连通权值设置为1;
(2)将所述目标冗余度机械臂与相邻冗余度机械臂之间的第一连通权值设置为1;
(3)将所述目标冗余度机械臂与所述两个以上的冗余度机械臂中的非自身且非相邻冗余度机械臂之间的第一连通权值设置为0。
对于上述步骤(1),所述目标冗余度机械臂与其自身之间具备通信连接关系,因此将所述第一连通权值设置为1。对于上述步骤(2),所述目标冗余度机械臂与相邻冗余度机械臂之间可以直接交换信息,因此将所述第一连通权值设置为1。对于上述步骤(3),所述目标冗余度机械臂与非自身且非相邻冗余度机械臂之间无需直接交换信息,因此将所述第一连通权值设置为0。
如图3所示,步骤1042具体可以包括:
10421、判断所述目标冗余度机械臂是否获取到所述参考点轨迹信息;
10422、若所述目标冗余度机械臂获取到所述参考点轨迹信息,则将所述第二连通权值设置为1;
10423、若所述目标冗余度机械臂未获取到所述参考点轨迹信息,则将所述第二连通权值设置为0。
根据所述参考点轨迹信息的获取状态确定所述目标冗余度机械臂的第二连通权值,若所述目标冗余度机械臂获取到所述参考点轨迹信息,则将所述第二连通权值设置为1,否则将所述第二连通权值设置为0。
所述雅可比矩阵等式的通用表达式具体可以为:
其中,Ji表示目标冗余度机械臂i的雅可比矩阵;表示目标冗余度机械臂i的关节速度向量;表示与目标冗余度机械臂i之间的第一连通权值为1的机械臂的编号;Aij表示目标冗余度机械臂i的权重矩阵,所述权重矩阵中的各个元素为目标冗余度机械臂i与冗余度机械臂j之间的第一连通权值;ρi表示目标冗余度机械臂i的第二连通权值;δi=ri-rirp为所述位置信息,ri为目标冗余度机械臂i的末端执行器位置,rirp为预设的目标冗余度机械臂i的末端执行器相对于所述目标参考点的距离向量;rd为所述目标参考点的期望轨迹;c0>0为控制算法收敛速度的参数,其越大则代表收敛速度越快。
上述雅可比矩阵等式(1)主要负责实现多机械臂之间的分布式协同运动约束,通过获取其他冗余度机械臂末端执行器的位置信息,即可将第i个冗余度机械臂的期望速度约束设置为上述其它冗余度机械臂相关位置的加权平均(权重即为雅可比等式约束右边所述),从而实现协同运动。基于雅可比矩阵等式约束(1)构建的多机械臂协同运动***具有高鲁棒性,低通信成本的特点,具体而言,对于多机械臂***,当有一个机械臂加入或者退出都不影响该***的稳定。此外,每个机械臂只需要和数量较少的相邻机械臂通信即可,不必满足全局通信的要求。假如有100个机械臂,则在全局通信情况下,总共需要的通信链接数为100*99/2=4950;而利用本发明实施例提出的分布式方案最少只需要99个通信链接数,大大减小了通信负载。
105、在所述雅可比矩阵等式、障碍物躲避不等式、关节角度极限和关节速度极限的约束下,根据二次型优化和标准二次规划方法确定所述目标冗余度机械臂的控制信号;
在构建好所述目标冗余度机械臂对应的雅可比矩阵等式之后,在所述雅可比矩阵等式、障碍物躲避不等式、关节角度极限和关节速度极限的约束下,根据二次型优化和标准二次规划方法确定所述目标冗余度机械臂的控制信号。
进一步的,如图4所示,步骤105具体可以包括:
1051、设置性能指标为可操作度最大化,在所述雅可比矩阵等式、障碍物躲避不等式、关节角度极限和关节速度极限的约束下确定二次型优化结果;
本发明实施例需要实现多机械臂***的可操作度最大化分布式协同控制,因此在二次型优化过程中设置性能指标为可操作度最大化,然后在所述雅可比矩阵等式、障碍物躲避不等式、关节角度极限和关节速度极限的约束下确定二次型优化结果。
更进一步的,若所述雅可比矩阵等式如公式(1)所示,则所述二次型优化具体可以为:
在第一约束条件下使得第一式子最小化;
所述第一约束条件包括:
所述第一式子为:
其中,上标T表示矩阵和向量的转置;表示矩阵的行列式值;c1>0是用来控制可操作度最大化速率的设计参数;Bi和ei是障碍物躲避参数;表示目标冗余度机械臂i的关节角上下限;表示目标冗余度机械臂i的关节速度上下限。
1052、将所述二次型优化结果转换为标准二次规划;
将二次型优化结果转换为标准二次规划,以便进行求解。
进一步的,若所述二次型优化如公式(2)至(6)所示,考虑到上述优化问题是在速度层上求解,因此需将第i个冗余度机械臂的关节角度约束(4)、关节速度约束(5)合并,从而可以得到以下基于速度的双端不等式约束:
其中,和分别表示第i个冗余度机械臂的合成双端约束的上下极限,它们的第p个元素分别定义为和其中为裕度,ip表示第i个冗余度机械臂的关节序号,p=1,…,m,m为机械臂的自由度数目,κ1>0用来调节和保证关节速度足够大的可行域。用xi表示第i个冗余度机械臂的关节速度上述二次型优化方案便可描述为如下的标准二次规划方案:
约束条件:Cixi=di (7)
Bixi≤ei (8)
1053、对所述标准二次规划进行求解,得到求解结果;
可以利用标准二次规划求解器或者数值方法对所述标准二次规划问题进行求解,得到各个目标冗余度机械臂的可操作度最大化运动规划方法的最优解。
1054、根据所述求解结果确定所述目标冗余度机械臂的控制信号。
根据所述求解结果确定所述目标冗余度机械臂的控制信号,然后利用所述控制信号对目标冗余度机械臂进行控制。
106、根据所述控制信号控制所述目标冗余度机械臂,使得所述目标冗余度机械臂的可操作度最大化。
在确定所述目标冗余度机械臂的控制信号后,根据所述控制信号控制所述目标冗余度机械臂。最终使得在各个机械臂末端执行器保持与参考点相对距离不变的情况下,整个冗余度机械臂***实现分布式协同可操作度最大化运动。
在本发明实施例中,确定两个以上的冗余度机械臂中至少一个冗余度机械臂为目标冗余度机械臂;获取所述目标冗余度机械臂的相邻冗余度机械臂的位置信息,所述相邻冗余度机械臂与所述目标冗余度机械臂在通信拓扑图中相邻连接;获取所述目标冗余度机械臂的参考点轨迹信息,所述参考点轨迹信息由预设的目标参考点和所述目标参考点的期望轨迹确定;根据所述位置信息和所述参考点轨迹信息按照预设的规则构建所述目标冗余度机械臂对应的雅可比矩阵等式;在所述雅可比矩阵等式、障碍物躲避不等式、关节角度极限和关节速度极限的约束下,根据二次型优化和标准二次规划方法确定所述目标冗余度机械臂的控制信号;根据所述控制信号控制所述目标冗余度机械臂,使得所述目标冗余度机械臂的可操作度最大化。利用本发明实施例提出的协同控制方法,由于目标冗余度机械臂只需与数量较少的相邻冗余度机械臂进行通信,大大减小了通信负载,从而可在通信受限的情况下实现多机械臂***的可操作度最大化分布式协同控制。
上面主要描述了一种多冗余度机械臂***的协同控制方法,下面将对一种多冗余度机械臂***的协同控制装置进行详细描述。
请参阅图5,示出了本发明实施例中一种多冗余度机械臂***的协同控制装置,所述多冗余度机械臂***包括两个以上的冗余度机械臂,所述两个以上的冗余度机械臂在通信拓扑图中连通;
所述控制装置包括:
目标机械臂确定模块501,用于确定所述两个以上的冗余度机械臂中至少一个冗余度机械臂为目标冗余度机械臂;
位置信息获取模块502,用于获取所述目标冗余度机械臂的相邻冗余度机械臂的位置信息,所述相邻冗余度机械臂与所述目标冗余度机械臂在通信拓扑图中相邻连接;
参考点轨迹信息获取模块503,用于获取所述目标冗余度机械臂的参考点轨迹信息,所述参考点轨迹信息由预设的目标参考点和所述目标参考点的期望轨迹确定;
等式构建模块504,用于根据所述位置信息和所述参考点轨迹信息按照预设的规则构建所述目标冗余度机械臂对应的雅可比矩阵等式;
控制信号确定模块505,用于在所述雅可比矩阵等式、障碍物躲避不等式、关节角度极限和关节速度极限的约束下,根据二次型优化和标准二次规划方法确定所述目标冗余度机械臂的控制信号;
机械臂控制模块506,用于根据所述控制信号控制所述目标冗余度机械臂,使得所述目标冗余度机械臂的可操作度最大化。
进一步的,所述等式构建模块504具体可以包括:
权重矩阵构建单元,用于构建所述目标冗余度机械臂对应的权重矩阵,所述权重矩阵中的各个元素为所述目标冗余度机械臂与所述两个以上的冗余度机械臂中的各个机械臂之间分别对应的第一连通权值;
第一确定单元,用于根据所述参考点轨迹信息的获取状态确定所述目标冗余度机械臂的第二连通权值;
等式构建单元,用于根据所述权重矩阵、所述第二连通权值、所述位置信息和所述参考点轨迹信息构建所述目标冗余度机械臂对应的雅可比矩阵等式。
更进一步的,所述权重矩阵构建单元具体可以包括:
第一设置模块,用于将所述目标冗余度机械臂与所述目标冗余度机械臂自身之间的第一连通权值设置为1;
第二设置模块,用于将所述目标冗余度机械臂与相邻冗余度机械臂之间的第一连通权值设置为1;
第三设置模块,用于将所述目标冗余度机械臂与所述两个以上的冗余度机械臂中的非自身且非相邻冗余度机械臂之间的第一连通权值设置为0;
所述第一确定单元具体可以包括:
判断模块,用于判断所述目标冗余度机械臂是否获取到所述参考点轨迹信息;
第四设置模块,用于若所述目标冗余度机械臂获取到所述参考点轨迹信息,则将所述第二连通权值设置为1;
第五设置模块,用于若所述目标冗余度机械臂未获取到所述参考点轨迹信息,则将所述第二连通权值设置为0;
所述等式构建单元构建的雅可比矩阵等式的通用表达式具体可以为:
其中,Ji表示目标冗余度机械臂i的雅可比矩阵;表示目标冗余度机械臂i的关节速度向量;表示与目标冗余度机械臂i之间的第一连通权值为1的机械臂的编号;Aij表示目标冗余度机械臂i的权重矩阵,所述权重矩阵中的各个元素为目标冗余度机械臂i与冗余度机械臂j之间的第一连通权值;ρi表示目标冗余度机械臂i的第二连通权值;δi=ri-rirp为所述位置信息,ri为目标冗余度机械臂i的末端执行器位置,rirp为预设的目标冗余度机械臂i的末端执行器相对于所述目标参考点的距离向量;rd为所述目标参考点的期望轨迹;c0>0为控制算法收敛速度的参数,其越大则代表收敛速度越快。
进一步的,所述控制信号确定模块505具体可以包括:
第二确定单元,用于设置性能指标为可操作度最大化,在所述雅可比矩阵等式、障碍物躲避不等式、关节角度极限和关节速度极限的约束下确定二次型优化结果;
转换单元,用于将所述二次型优化结果转换为标准二次规划;
求解单元,用于对所述标准二次规划进行求解,得到求解结果;
控制信号确定单元,用于根据所述求解结果确定所述目标冗余度机械臂的控制信号。
进一步的,所述控制信号确定模块采用的二次型优化具体可以为:
在第一约束条件下使得第一式子最小化;
所述第一约束条件包括:
其中,上标T表示矩阵和向量的转置;表示矩阵的行列式值;c1>0是用来控制可操作度最大化速率的设计参数;Bi和ei是障碍物躲避参数;表示目标冗余度机械臂i的关节角上下限;表示目标冗余度机械臂i的关节速度上下限。
所属领域的技术人员可以清楚地了解到,为描述的方便和简洁,上述描述的***,装置和单元的具体工作过程,可以参考前述方法实施例中的对应过程,在此不再赘述。
在本申请所提供的几个实施例中,应该理解到,所揭露的***,装置和方法,可以通过其它的方式实现。例如,以上所描述的装置实施例仅仅是示意性的,例如,所述单元的划分,仅仅为一种逻辑功能划分,实际实现时可以有另外的划分方式,例如多个单元或组件可以结合或者可以集成到另一个***,或一些特征可以忽略,或不执行。另一点,所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口,装置或单元的间接耦合或通信连接,可以是电性,机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的,作为单元显示的部件可以是或者也可以不是物理单元,即可以位于一个地方,或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外,在本发明各个实施例中的各功能单元可以集成在一个处理单元中,也可以是各个单元单独物理存在,也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现,也可以采用软件功能单元的形式实现。
所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时,可以存储在一个计算机可读取存储介质中。基于这样的理解,本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来,该计算机软件产品存储在一个存储介质中,包括若干指令用以使得一台计算机设备(可以是个人计算机,服务器,或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括:U盘、移动硬盘、只读存储器(ROM,Read-OnlyMemory)、随机存取存储器(RAM,Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述,以上实施例仅用以说明本发明的技术方案,而非对其限制;尽管参照前述实施例对本发明进行了详细的说明,本领域的普通技术人员应当理解:其依然可以对前述各实施例所记载的技术方案进行修改,或者对其中部分技术特征进行等同替换;而这些修改或者替换,并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。
Claims (10)
1.一种多冗余度机械臂***的协同控制方法,其特征在于,所述多冗余度机械臂***包括两个以上的冗余度机械臂,所述两个以上的冗余度机械臂在通信拓扑图中连通;
所述控制方法包括:
确定所述两个以上的冗余度机械臂中至少一个冗余度机械臂为目标冗余度机械臂;
获取所述目标冗余度机械臂的相邻冗余度机械臂的位置信息,所述相邻冗余度机械臂与所述目标冗余度机械臂在通信拓扑图中相邻连接;
获取所述目标冗余度机械臂的参考点轨迹信息,所述参考点轨迹信息由预设的目标参考点和所述目标参考点的期望轨迹确定;
根据所述位置信息和所述参考点轨迹信息按照预设的规则构建所述目标冗余度机械臂对应的雅可比矩阵等式;
在所述雅可比矩阵等式、障碍物躲避不等式、关节角度极限和关节速度极限的约束下,根据二次型优化和标准二次规划方法确定所述目标冗余度机械臂的控制信号;
根据所述控制信号控制所述目标冗余度机械臂,使得所述目标冗余度机械臂的可操作度最大化。
2.根据权利要求1所述的协同控制方法,其特征在于,所述根据所述位置信息和所述参考点轨迹信息按照预设的规则构建所述目标冗余度机械臂对应的雅可比矩阵等式具体包括:
构建所述目标冗余度机械臂对应的权重矩阵,所述权重矩阵中的各个元素为所述目标冗余度机械臂与所述两个以上的冗余度机械臂中的各个机械臂之间分别对应的第一连通权值;
根据所述参考点轨迹信息的获取状态确定所述目标冗余度机械臂的第二连通权值;
根据所述权重矩阵、所述第二连通权值、所述位置信息和所述参考点轨迹信息构建所述目标冗余度机械臂对应的雅可比矩阵等式。
3.根据权利要求2所述的协同控制方法,其特征在于,所述第一连通权值通过以下步骤确定:
将所述目标冗余度机械臂与所述目标冗余度机械臂自身之间的第一连通权值设置为1;
将所述目标冗余度机械臂与相邻冗余度机械臂之间的第一连通权值设置为1;
将所述目标冗余度机械臂与所述两个以上的冗余度机械臂中的非自身且非相邻冗余度机械臂之间的第一连通权值设置为0;
所述根据所述参考点轨迹信息的获取状态确定所述目标冗余度机械臂的第二连通权值具体包括:
判断所述目标冗余度机械臂是否获取到所述参考点轨迹信息;
若所述目标冗余度机械臂获取到所述参考点轨迹信息,则将所述第二连通权值设置为1;
若所述目标冗余度机械臂未获取到所述参考点轨迹信息,则将所述第二连通权值设置为0;
所述雅可比矩阵等式的通用表达式为:
4.根据权利要求1至3中任一项所述的协同控制方法,其特征在于,所述在所述雅可比矩阵等式、障碍物躲避不等式、关节角度极限和关节速度极限的约束下,根据二次型优化和标准二次规划方法确定所述目标冗余度机械臂的控制信号具体包括:
设置性能指标为可操作度最大化,在所述雅可比矩阵等式、障碍物躲避不等式、关节角度极限和关节速度极限的约束下确定二次型优化结果;
将所述二次型优化结果转换为标准二次规划;
对所述标准二次规划进行求解,得到求解结果;
根据所述求解结果确定所述目标冗余度机械臂的控制信号。
6.一种多冗余度机械臂***的协同控制装置,其特征在于,所述多冗余度机械臂***包括两个以上的冗余度机械臂,所述两个以上的冗余度机械臂在通信拓扑图中连通;
所述控制装置包括:
目标机械臂确定模块,用于确定所述两个以上的冗余度机械臂中至少一个冗余度机械臂为目标冗余度机械臂;
位置信息获取模块,用于获取所述目标冗余度机械臂的相邻冗余度机械臂的位置信息,所述相邻冗余度机械臂与所述目标冗余度机械臂在通信拓扑图中相邻连接;
参考点轨迹信息获取模块,用于获取所述目标冗余度机械臂的参考点轨迹信息,所述参考点轨迹信息由预设的目标参考点和所述目标参考点的期望轨迹确定;
等式构建模块,用于根据所述位置信息和所述参考点轨迹信息按照预设的规则构建所述目标冗余度机械臂对应的雅可比矩阵等式;
控制信号确定模块,用于在所述雅可比矩阵等式、障碍物躲避不等式、关节角度极限和关节速度极限的约束下,根据二次型优化和标准二次规划方法确定所述目标冗余度机械臂的控制信号;
机械臂控制模块,用于根据所述控制信号控制所述目标冗余度机械臂,使得所述目标冗余度机械臂的可操作度最大化。
7.根据权利要求6所述的协同控制装置,其特征在于,所述等式构建模块具体包括:
权重矩阵构建单元,用于构建所述目标冗余度机械臂对应的权重矩阵,所述权重矩阵中的各个元素为所述目标冗余度机械臂与所述两个以上的冗余度机械臂中的各个机械臂之间分别对应的第一连通权值;
第一确定单元,用于根据所述参考点轨迹信息的获取状态确定所述目标冗余度机械臂的第二连通权值;
等式构建单元,用于根据所述权重矩阵、所述第二连通权值、所述位置信息和所述参考点轨迹信息构建所述目标冗余度机械臂对应的雅可比矩阵等式。
8.根据权利要求7所述的协同控制装置,其特征在于,所述权重矩阵构建单元具体包括:
第一设置模块,用于将所述目标冗余度机械臂与所述目标冗余度机械臂自身之间的第一连通权值设置为1;
第二设置模块,用于将所述目标冗余度机械臂与相邻冗余度机械臂之间的第一连通权值设置为1;
第三设置模块,用于将所述目标冗余度机械臂与所述两个以上的冗余度机械臂中的非自身且非相邻冗余度机械臂之间的第一连通权值设置为0;
所述第一确定单元具体包括:
判断模块,用于判断所述目标冗余度机械臂是否获取到所述参考点轨迹信息;
第四设置模块,用于若所述目标冗余度机械臂获取到所述参考点轨迹信息,则将所述第二连通权值设置为1;
第五设置模块,用于若所述目标冗余度机械臂未获取到所述参考点轨迹信息,则将所述第二连通权值设置为0;
所述等式构建单元构建的雅可比矩阵等式的通用表达式为:
9.根据权利要求6至8中任一项所述的协同控制装置,其特征在于,所述控制信号确定模块具体包括:
第二确定单元,用于设置性能指标为可操作度最大化,在所述雅可比矩阵等式、障碍物躲避不等式、关节角度极限和关节速度极限的约束下确定二次型优化结果;
转换单元,用于将所述二次型优化结果转换为标准二次规划;
求解单元,用于对所述标准二次规划进行求解,得到求解结果;
控制信号确定单元,用于根据所述求解结果确定所述目标冗余度机械臂的控制信号。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710084021.XA CN106842907B (zh) | 2017-02-16 | 2017-02-16 | 一种多冗余度机械臂***的协同控制方法与装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710084021.XA CN106842907B (zh) | 2017-02-16 | 2017-02-16 | 一种多冗余度机械臂***的协同控制方法与装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN106842907A CN106842907A (zh) | 2017-06-13 |
CN106842907B true CN106842907B (zh) | 2020-03-27 |
Family
ID=59127743
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201710084021.XA Active CN106842907B (zh) | 2017-02-16 | 2017-02-16 | 一种多冗余度机械臂***的协同控制方法与装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN106842907B (zh) |
Families Citing this family (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107160401B (zh) * | 2017-06-27 | 2020-07-28 | 华南理工大学 | 一种解决冗余度机械臂关节角偏移问题的方法 |
CN107255926B (zh) * | 2017-06-27 | 2019-08-20 | 华南理工大学 | 一种快速求解冗余度机械臂关节角偏移问题的方法 |
CN107966907B (zh) * | 2017-11-30 | 2020-09-22 | 华南理工大学 | 一种应用于冗余度机械臂的障碍物躲避解决方法 |
CN108519740B (zh) * | 2018-05-05 | 2020-11-17 | 曲阜师范大学 | 一种全状态约束机械臂轨迹跟踪的协同控制方法 |
CN111113417B (zh) * | 2019-12-25 | 2021-10-29 | 广东省智能制造研究所 | 一种分布式多机器人协同运动控制方法及*** |
CN114800488B (zh) * | 2022-03-18 | 2023-06-20 | 清华大学深圳国际研究生院 | 一种基于深度强化学习的冗余机械臂可操作度优化方法及装置 |
Family Cites Families (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102514008B (zh) * | 2011-11-21 | 2014-03-19 | 中山大学 | 一种冗余度机械臂的不同层性能指标同时优化方法 |
CN103231381B (zh) * | 2013-05-03 | 2015-10-21 | 中山大学 | 一种冗余度机械臂的新型加速度层重复运动规划方法 |
CN105183009B (zh) * | 2015-10-15 | 2017-11-21 | 哈尔滨工程大学 | 一种冗余机械臂轨迹控制方法 |
-
2017
- 2017-02-16 CN CN201710084021.XA patent/CN106842907B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN106842907A (zh) | 2017-06-13 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106842907B (zh) | 一种多冗余度机械臂***的协同控制方法与装置 | |
CN106926238B (zh) | 基于突加度的多冗余度机械臂***的协同控制方法与装置 | |
CN109591020B (zh) | 一种多冗余度机械臂自适应可操纵性最优化的分布式协同控制方法与装置 | |
CN106826828B (zh) | 一种多冗余度机械臂***的协同控制方法与装置 | |
CN108326844B (zh) | 冗余度机械臂的可操作度优化的运动规划方法和装置 | |
Zhang et al. | RNN for perturbed manipulability optimization of manipulators based on a distributed scheme: A game-theoretic perspective | |
Desai et al. | Control of changes in formation for a team of mobile robots | |
CN111531538B (zh) | 一种切换拓扑下的多机械臂***一致性控制方法及装置 | |
CN109986562B (zh) | 一种平面机械臂***及其分散式容错控制方法 | |
CN110162035B (zh) | 一种集群机器人在有障碍物场景中的协同运动方法 | |
CN114527661B (zh) | 一种用于集群智能***的协同编队方法 | |
CN109407520A (zh) | 基于滑模控制理论的二阶多智能体***的容错一致性控制算法 | |
CN106406097B (zh) | 多机械臂***的分布式自适应协调控制方法 | |
CN110286691B (zh) | 基于线性微分包含的多无人机编队控制方法 | |
CN112947086B (zh) | 一种无人机和无人车组成的异构多智能体***编队控制中执行器故障的自适应补偿方法 | |
CN111399537B (zh) | 一种基于航路点的分布式无人机动态编队切换方法 | |
Yang et al. | Concise discrete ZNN controllers for end-effector tracking and obstacle avoidance of redundant manipulators | |
CN114063652B (zh) | 基于应力矩阵优化的多智能体***仿射编队控制方法 | |
CN115157262A (zh) | 关节受限冗余机械臂的障碍物躲避与优化控制方法、***及机器人 | |
CN111221318B (zh) | 一种基于模型预测控制算法的多机器人状态估计方法 | |
CN116819975A (zh) | 一种基于纯角度观测的多目标几何中心估计方法 | |
CN111844026A (zh) | 一种事件触发的多机器人分布式协同控制方法 | |
CN109079779B (zh) | 基于末端估计和操作度调整的多移动机械臂最优协同方法 | |
Martinez et al. | On robust rendezvous for mobile autonomous agents | |
CN110825092A (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 |