CN106826828B - 一种多冗余度机械臂系统的协同控制方法与装置 - Google Patents

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。

所述雅可比矩阵等式的通用表达式具体可以为：

其中，J_i表示目标冗余度机械臂i的雅可比矩阵，为J_i的时间导数；表示目标冗余度机械臂i的关节速度向量，为的时间导数；j∈N(i)表示与目标冗余度机械臂i之间的第一连通权值为1的机械臂的编号；A_ij表示目标冗余度机械臂i的权重矩阵，所述权重矩阵中的各个元素为目标冗余度机械臂i与冗余度机械臂j之间的第一连通权值；ρ_i表示目标冗余度机械臂i的第二连通权值；δ_i＝r_i-r_irp为所述位置信息，r_i为目标冗余度机械臂i的末端执行器位置，r_irp为预设的目标冗余度机械臂i的末端执行器相对于所述目标参考点的距离向量；为所述速度信息，为目标冗余度机械臂i的末端执行器速度，r_d为所述目标参考点的期望位置，为所述目标参考点的期望速度；c₀＞0为控制算法收敛速度的参数，其越大则代表收敛速度越快。

上述雅可比矩阵等式(1)主要负责实现多机械臂之间的分布式协同运动约束，通过获取其他冗余度机械臂末端执行器的位置和速度信息，即可将第i个冗余度机械臂的期望加速度约束设置为上述其它冗余度机械臂相关位置、速度的加权平均(权重即为雅可比等式约束右边所述)，从而实现协同运动。基于雅可比矩阵等式约束(1)构建的多机械臂协同运动系统具有高鲁棒性，低通信成本的特点，具体而言，对于多机械臂系统，当有一个机械臂加入或者退出都不影响该系统的稳定。此外，每个机械臂只需要和数量较少的相邻机械臂通信即可，不必满足全局通信的要求。假如有100个机械臂，则在全局通信情况下，总共需要的通信链接数为100*99/2＝4950；而利用本发明实施例提出的分布式方案最少只需要99个通信链接数，大大减小了通信负载。

105、在所述雅可比矩阵等式、关节角度极限、关节速度极限和关节加速度极限的约束下，根据二次型优化和标准二次规划方法确定所述目标冗余度机械臂的控制信号；

在构建好所述目标冗余度机械臂对应的雅可比矩阵等式之后，在所述雅可比矩阵等式、关节角度极限、关节速度极限和关节加速度极限的约束下，根据二次型优化和标准二次规划方法确定所述目标冗余度机械臂的控制信号。

进一步的，如图4所示，步骤105具体可以包括：

1051、设置最小化性能指标为重复运动，在所述雅可比矩阵等式、关节角度极限、关节速度极限和关节加速度极限的约束下确定二次型优化结果；

本发明实施例需要实现多机械臂系统的分布式协同重复运动，因此在二次型优化过程中设置最小化性能指标为重复运动，然后在所述雅可比矩阵等式、关节角度极限、关节速度极限和关节加速度极限的约束下确定二次型优化结果。

更进一步的，若所述雅可比矩阵等式如公式(1)所示，则所述二次型优化具体可以为：

在第一约束条件下使得第一式子最小化；

所述第一约束条件包括：

雅可比矩阵等式约束条件

关节角度极限约束条件

关节速度极限约束条件

关节加速度极限约束条件

所述第一式子为：

其中，上标T表示矩阵和向量的转置；c₁＞0和c₂＞0是用来控制关节位移幅值和收敛率的设计参数，θ_i(0)表示目标冗余度机械臂i的初始关节角；表示目标冗余度机械臂i的关节角上下限；表示目标冗余度机械臂i的关节速度上下限，表示目标冗余度机械臂i的关节加速度上下限。

1052、将所述二次型优化结果转换为标准二次规划；

将二次型优化结果转换为标准二次规划，以便进行求解。

进一步的，若所述二次型优化如公式(2)至(6)所示，考虑到上述优化问题是在加速度层上求解，因此需将第i个冗余度机械臂的关节角度约束(3)、关节速度约束(4)和关节加速度约束(5)合并，从而可以得到以下基于加速度的双端不等式约束：

其中，和分别表示第i个冗余度机械臂的合成双端约束的上下极限，它们的第p个元素分别定义为和其中为裕度，ip表示第i个冗余度机械臂的关节序号，p＝1，2，…，m，m为机械臂的自由度数目，κ₁＞0和κ₂＞0用来调节和保证关节加速度足够大的可行域。用x_i表示第i个冗余度机械臂的关节加速度上述二次型优化方案(2)至(6)便可描述为如下的标准二次规划方案：

约束条件：C_ixi＝d_i (7)

最小化：

其中，W为单位矩阵，C_i＝J_i，

1053、对所述标准二次规划进行求解，得到求解结果；

可以利用标准二次规划求解器或者数值方法对所述标准二次规划问题进行求解，得到各个目标冗余度机械臂的加速度重复运动规划方法的最优解。

1054、根据所述求解结果确定所述目标冗余度机械臂的控制信号。

根据所述求解结果确定所述目标冗余度机械臂的控制信号，然后利用所述控制信号对目标冗余度机械臂进行控制。

106、根据所述控制信号控制所述目标冗余度机械臂，使得所述目标冗余度机械臂实现重复运动。

在确定所述目标冗余度机械臂的控制信号后，根据所述控制信号控制所述目标冗余度机械臂，最终使得在各个机械臂末端执行器保持与参考点相对距离不变的情况下，整个冗余度机械臂系统实现分布式协同加速度层重复运动。所述重复运动指机械臂在执行完路径为封闭曲线的任务后，各个关节角都能够回到其初始位置，然后执行下一次相同的任务。如果冗余度机械臂不能实现重复运动，则其工作时可能危害自身或周围操作人员的安全。

在本发明实施例中，确定多冗余度机械臂系统中的目标冗余度机械臂；获取所述目标冗余度机械臂的相邻冗余度机械臂的位置和速度信息；获取所述目标冗余度机械臂的参考点轨迹信息；根据所述位置和速度信息，以及所述参考点轨迹信息按照预设的规则构建所述目标冗余度机械臂对应的雅可比矩阵等式；根据二次型优化和标准二次规划方法确定所述目标冗余度机械臂的控制信号；根据所述控制信号控制所述目标冗余度机械臂，使得所述目标冗余度机械臂实现重复运动。利用本发明实施例提出的协同控制方法，由于目标冗余度机械臂只需与数量较少的相邻冗余度机械臂进行通信，大大减小了通信负载，从而可在通信受限的情况下实现多机械臂系统的分布式协同重复运动。

上面主要描述了一种多冗余度机械臂系统的协同控制方法，下面将对一种多冗余度机械臂系统的协同控制装置进行详细描述。

请参阅图5，示出了本发明实施例中一种多冗余度机械臂系统的协同控制装置，所述多冗余度机械臂系统包括两个以上的冗余度机械臂，所述两个以上的冗余度机械臂在通信拓扑图中连通；

所述控制装置包括：

目标机械臂确定模块501，用于确定所述两个以上的冗余度机械臂中至少一个冗余度机械臂为目标冗余度机械臂；

信息获取模块502，用于获取所述目标冗余度机械臂的相邻冗余度机械臂的位置和速度信息，所述相邻冗余度机械臂与所述目标冗余度机械臂在通信拓扑图中相邻连接；

参考点轨迹信息获取模块503，用于获取所述目标冗余度机械臂的参考点轨迹信息，所述参考点轨迹信息由预设的目标参考点和所述目标参考点的期望轨迹确定；

等式构建模块504，用于根据所述位置和速度信息，以及所述参考点轨迹信息按照预设的规则构建所述目标冗余度机械臂对应的雅可比矩阵等式；

控制信号确定模块505，用于在所述雅可比矩阵等式、关节角度极限、关节速度极限和关节加速度极限的约束下，根据二次型优化和标准二次规划方法确定所述目标冗余度机械臂的控制信号；

机械臂控制模块506，用于根据所述控制信号控制所述目标冗余度机械臂，使得所述目标冗余度机械臂实现重复运动。

进一步的，所述等式构建模块504具体可以包括：

权重矩阵构建单元，用于构建所述目标冗余度机械臂对应的权重矩阵，所述权重矩阵中的各个元素为所述目标冗余度机械臂与所述两个以上的冗余度机械臂中的各个机械臂之间分别对应的第一连通权值；

第一确定单元，用于根据所述参考点轨迹信息的获取状态确定所述目标冗余度机械臂的第二连通权值；

等式构建单元，用于根据所述位置和速度信息、所述参考点轨迹信息、所述权重矩阵和所述第二连通权值构建所述目标冗余度机械臂对应的雅可比矩阵等式。

更进一步的，所述权重矩阵构建单元具体可以包括：

第一设置模块，用于将所述目标冗余度机械臂与所述目标冗余度机械臂自身之间的第一连通权值设置为1；

第二设置模块，用于将所述目标冗余度机械臂与相邻冗余度机械臂之间的第一连通权值设置为1；

第三设置模块，用于将所述目标冗余度机械臂与所述两个以上的冗余度机械臂中的非自身且非相邻冗余度机械臂之间的第一连通权值设置为0；

所述第一确定单元具体可以包括：

判断模块，用于判断所述目标冗余度机械臂是否获取到所述参考点轨迹信息；

第四设置模块，用于若所述目标冗余度机械臂获取到所述参考点轨迹信息，则将所述第二连通权值设置为1；

第五设置模块，用于若所述目标冗余度机械臂未获取到所述参考点轨迹信息，则将所述第二连通权值设置为0；

所述等式构建单元构建的雅可比矩阵等式的通用表达式具体可以为：

其中，J_i表示目标冗余度机械臂i的雅可比矩阵，为J_i的时间导数；表示目标冗余度机械臂i的关节速度向量，为的时间导数；j∈N(i)表示与目标冗余度机械臂i之间的第一连通权值为1的机械臂的编号；A_ij表示目标冗余度机械臂i的权重矩阵，所述权重矩阵中的各个元素为目标冗余度机械臂i与冗余度机械臂j之间的第一连通权值；ρ_i表示目标冗余度机械臂i的第二连通权值；δ_i＝r_i-r_irp为所述位置信息，r_i为目标冗余度机械臂i的末端执行器位置，r_irp为预设的目标冗余度机械臂i的末端执行器相对于所述目标参考点的距离向量；为所述速度信息，为目标冗余度机械臂i的末端执行器速度，r_d为所述目标参考点的期望位置，为所述目标参考点的期望速度；c₀＞0为控制算法收敛速度的参数，其越大则代表收敛速度越快。

进一步的，所述控制信号确定模块505具体可以包括：

第二确定单元，用于设置最小化性能指标为重复运动，在所述雅可比矩阵等式、关节角度极限、关节速度极限和关节加速度极限的约束下确定二次型优化结果；

转换单元，用于将所述二次型优化结果转换为标准二次规划；

求解单元，用于对所述标准二次规划进行求解，得到求解结果；

控制信号确定单元，用于根据所述求解结果确定所述目标冗余度机械臂的控制信号。

进一步的，所述控制信号确定模块采用的二次型优化具体可以为：

在第一约束条件下使得第一式子最小化；

所述第一约束条件包括：

雅可比矩阵等式约束条件

关节角度极限约束条件

关节速度极限约束条件

关节加速度极限约束条件

所述第一式子为

其中，上标^T表示矩阵和向量的转置；c₁＞0和c₂＞0是用来控制关节位移幅值和收敛率的设计参数，θ_i(0)表示目标冗余度机械臂i的初始关节角；表示目标冗余度机械臂i的关节角上下限；表示目标冗余度机械臂i的关节速度上下限，表示目标冗余度机械臂i的关节加速度上下限。

所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统，装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

在本申请所提供的几个实施例中，应该理解到，所揭露的系统，装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述，以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (10)

1.一种多冗余度机械臂系统的协同控制方法，其特征在于，所述多冗余度机械臂系统包括两个以上的冗余度机械臂，所述两个以上的冗余度机械臂在通信拓扑图中连通；

所述控制方法包括：

确定所述两个以上的冗余度机械臂中至少一个冗余度机械臂为目标冗余度机械臂；

获取所述目标冗余度机械臂的相邻冗余度机械臂的位置和速度信息，所述相邻冗余度机械臂与所述目标冗余度机械臂在通信拓扑图中相邻连接；

获取所述目标冗余度机械臂的参考点轨迹信息，所述参考点轨迹信息由预设的目标参考点和所述目标参考点的期望轨迹确定；

根据所述位置和速度信息，以及所述参考点轨迹信息按照预设的规则构建所述目标冗余度机械臂对应的雅可比矩阵等式；

在所述雅可比矩阵等式、关节角度极限、关节速度极限和关节加速度极限的约束下，根据二次型优化和标准二次规划方法确定所述目标冗余度机械臂的控制信号；

根据所述控制信号控制所述目标冗余度机械臂，使得所述目标冗余度机械臂实现重复运动。

2.根据权利要求1所述的协同控制方法，其特征在于，所述根据所述位置和速度信息，以及所述参考点轨迹信息按照预设的规则构建所述目标冗余度机械臂对应的雅可比矩阵等式具体包括：

构建所述目标冗余度机械臂对应的权重矩阵，所述权重矩阵中的各个元素为所述目标冗余度机械臂与所述两个以上的冗余度机械臂中的各个机械臂之间分别对应的第一连通权值；

根据所述参考点轨迹信息的获取状态确定所述目标冗余度机械臂的第二连通权值；

根据所述位置和速度信息、所述参考点轨迹信息、所述权重矩阵和所述第二连通权值构建所述目标冗余度机械臂对应的雅可比矩阵等式。

3.根据权利要求2所述的协同控制方法，其特征在于，所述第一连通权值通过以下步骤确定：

将所述目标冗余度机械臂与所述目标冗余度机械臂自身之间的第一连通权值设置为1；

将所述目标冗余度机械臂与相邻冗余度机械臂之间的第一连通权值设置为1；

将所述目标冗余度机械臂与所述两个以上的冗余度机械臂中的非自身且非相邻冗余度机械臂之间的第一连通权值设置为0；

所述根据所述参考点轨迹信息的获取状态确定所述目标冗余度机械臂的第二连通权值具体包括：

判断所述目标冗余度机械臂是否获取到所述参考点轨迹信息；

若所述目标冗余度机械臂获取到所述参考点轨迹信息，则将所述第二连通权值设置为1；

若所述目标冗余度机械臂未获取到所述参考点轨迹信息，则将所述第二连通权值设置为0；

所述雅可比矩阵等式的通用表达式为：

其中，J_i表示目标冗余度机械臂i的雅可比矩阵，为J_i的时间导数；表示目标冗余度机械臂i的关节速度向量，为的时间导数；表示与目标冗余度机械臂i之间的第一连通权值为1的机械臂的编号；A_ij表示目标冗余度机械臂i的权重矩阵，所述权重矩阵中的各个元素为目标冗余度机械臂i与冗余度机械臂j之间的第一连通权值；ρ_i表示目标冗余度机械臂i的第二连通权值；δ_i＝r_i-r_irp为所述位置信息，r_i为目标冗余度机械臂i的末端执行器位置，r_irp为预设的目标冗余度机械臂i的末端执行器相对于所述目标参考点的距离向量；为所述速度信息，为目标冗余度机械臂i的末端执行器速度，r_d为所述目标参考点的期望位置，为所述目标参考点的期望速度；c₀＞0为控制算法收敛速度的参数，其越大则代表收敛速度越快。

4.根据权利要求1至3中任一项所述的协同控制方法，其特征在于，所述在所述雅可比矩阵等式、关节角度极限、关节速度极限和关节加速度极限的约束下，根据二次型优化和标准二次规划方法确定所述目标冗余度机械臂的控制信号具体包括：

设置最小化性能指标为重复运动，在所述雅可比矩阵等式、关节角度极限、关节速度极限和关节加速度极限的约束下确定二次型优化结果；

将所述二次型优化结果转换为标准二次规划；

对所述标准二次规划进行求解，得到求解结果；

根据所述求解结果确定所述目标冗余度机械臂的控制信号。

5.根据权利要求3所述的协同控制方法，其特征在于，所述二次型优化为：

在第一约束条件下使得第一式子最小化；

所述第一约束条件包括：

雅可比矩阵等式约束条件

关节角度极限约束条件

关节速度极限约束条件

关节加速度极限约束条件

所述第一式子为

其中，上标T表示矩阵和向量的转置；c₁＞0和c₂＞0是用来控制关节位移幅值和收敛率的设计参数，θ_i(0)表示目标冗余度机械臂i的初始关节角；表示目标冗余度机械臂i的关节角上下限；表示目标冗余度机械臂i的关节速度上下限，表示目标冗余度机械臂i的关节加速度上下限。

6.一种多冗余度机械臂系统的协同控制装置，其特征在于，所述多冗余度机械臂系统包括两个以上的冗余度机械臂，所述两个以上的冗余度机械臂在通信拓扑图中连通；

所述控制装置包括：

目标机械臂确定模块，用于确定所述两个以上的冗余度机械臂中至少一个冗余度机械臂为目标冗余度机械臂；

信息获取模块，用于获取所述目标冗余度机械臂的相邻冗余度机械臂的位置和速度信息，所述相邻冗余度机械臂与所述目标冗余度机械臂在通信拓扑图中相邻连接；

参考点轨迹信息获取模块，用于获取所述目标冗余度机械臂的参考点轨迹信息，所述参考点轨迹信息由预设的目标参考点和所述目标参考点的期望轨迹确定；

等式构建模块，用于根据所述位置和速度信息，以及所述参考点轨迹信息按照预设的规则构建所述目标冗余度机械臂对应的雅可比矩阵等式；

控制信号确定模块，用于在所述雅可比矩阵等式、关节角度极限、关节速度极限和关节加速度极限的约束下，根据二次型优化和标准二次规划方法确定所述目标冗余度机械臂的控制信号；

机械臂控制模块，用于根据所述控制信号控制所述目标冗余度机械臂，使得所述目标冗余度机械臂实现重复运动。

7.根据权利要求6所述的协同控制装置，其特征在于，所述等式构建模块具体包括：

权重矩阵构建单元，用于构建所述目标冗余度机械臂对应的权重矩阵，所述权重矩阵中的各个元素为所述目标冗余度机械臂与所述两个以上的冗余度机械臂中的各个机械臂之间分别对应的第一连通权值；

第一确定单元，用于根据所述参考点轨迹信息的获取状态确定所述目标冗余度机械臂的第二连通权值；

等式构建单元，用于根据所述位置和速度信息、所述参考点轨迹信息、所述权重矩阵和所述第二连通权值构建所述目标冗余度机械臂对应的雅可比矩阵等式。

8.根据权利要求7所述的协同控制装置，其特征在于，所述权重矩阵构建单元具体包括：

第一设置模块，用于将所述目标冗余度机械臂与所述目标冗余度机械臂自身之间的第一连通权值设置为1；

第二设置模块，用于将所述目标冗余度机械臂与相邻冗余度机械臂之间的第一连通权值设置为1；

第三设置模块，用于将所述目标冗余度机械臂与所述两个以上的冗余度机械臂中的非自身且非相邻冗余度机械臂之间的第一连通权值设置为0；

所述第一确定单元具体包括：

判断模块，用于判断所述目标冗余度机械臂是否获取到所述参考点轨迹信息；

第四设置模块，用于若所述目标冗余度机械臂获取到所述参考点轨迹信息，则将所述第二连通权值设置为1；

第五设置模块，用于若所述目标冗余度机械臂未获取到所述参考点轨迹信息，则将所述第二连通权值设置为0；

所述等式构建单元构建的雅可比矩阵等式的通用表达式为：

其中，J_i表示目标冗余度机械臂i的雅可比矩阵，为J_i的时间导数；表示目标冗余度机械臂i的关节速度向量，为的时间导数；表示与目标冗余度机械臂i之间的第一连通权值为1的机械臂的编号；A_ij表示目标冗余度机械臂i的权重矩阵，所述权重矩阵中的各个元素为目标冗余度机械臂i与冗余度机械臂j之间的第一连通权值；ρ_i表示目标冗余度机械臂i的第二连通权值；δ_i＝r_i-r_irp为所述位置信息，r_i为目标冗余度机械臂i的末端执行器位置，r_irp为预设的目标冗余度机械臂i的末端执行器相对于所述目标参考点的距离向量；为所述速度信息，为目标冗余度机械臂i的末端执行器速度，r_d为所述目标参考点的期望位置，为所述目标参考点的期望速度；c₀＞0为控制算法收敛速度的参数，其越大则代表收敛速度越快。

9.根据权利要求6至8中任一项所述的协同控制装置，其特征在于，所述控制信号确定模块具体包括：

第二确定单元，用于设置最小化性能指标为重复运动，在所述雅可比矩阵等式、关节角度极限、关节速度极限和关节加速度极限的约束下确定二次型优化结果；

转换单元，用于将所述二次型优化结果转换为标准二次规划；

求解单元，用于对所述标准二次规划进行求解，得到求解结果；

控制信号确定单元，用于根据所述求解结果确定所述目标冗余度机械臂的控制信号。

10.根据权利要求8所述的协同控制装置，其特征在于，所述控制信号确定模块采用的二次型优化为：

在第一约束条件下使得第一式子最小化；

所述第一约束条件包括：

雅可比矩阵等式约束条件

关节角度极限约束条件

关节速度极限约束条件

关节加速度极限约束条件

所述第一式子为

其中，上标T表示矩阵和向量的转置；c₁＞0和c₂＞0是用来控制关节位移幅值和收敛率的设计参数，θ_i(0)表示目标冗余度机械臂i的初始关节角；表示目标冗余度机械臂i的关节角上下限；表示目标冗余度机械臂i的关节速度上下限，表示目标冗余度机械臂i的关节加速度上下限。