CN109591020B - 一种多冗余度机械臂自适应可操纵性最优化的分布式协同控制方法与装置 - Google Patents

Abstract

本发明提出一种多冗余度机械臂自适应可操纵性最优化的分布式协同控制方法与装置。整个系统采用模型未知的冗余度机械臂的自适应雅可比矩阵来协助计算。通过对雅可比矩阵进行自适应估计；确定需要接收期望信息的冗余度机械臂；获取系统内冗余度机械臂的相关信息；根据相关信息构建自适应雅可比矩阵等式；根据标准二次规划方法确定冗余度机械臂的控制信号；根据控制信号来控制冗余度机械臂。本方法一方面克服了因模型不确定性对生产工作带来影响的缺陷，增加了冗余度机械臂的工作精度。另一方面躲避了冗余度机械臂的各种物理约束，有效地扩大了冗余度机械臂的可操作度优化运动规划的应用空间。并利用机械臂间进行有限通信的分布式设计，在很大程度上减少了系统的计算负担。

Description

一种多冗余度机械臂自适应可操纵性最优化的分布式协同控 制方法与装置

技术领域

本发明涉及机械臂控制技术领域，具体涉及一种多冗余度机械臂自适应可操纵性最优化的分布式协同控制方法与装置。

背景技术

机械臂广泛地应用于工业制造、医疗手术、3D打印等国民经济生产的各领域中，它们为人们的生活和工作带来了极大便利，与此同时，在实际应用中，人们对机械臂的灵活性、精准性和可靠性也提出了更高的要求。

具体来说，冗余度机械臂拥有比任务空间所需最少自由度更多的自由度，该机械臂在无故障时具有良好的灵活性，因此可以比较轻松地完成复杂的子任务。然而如果当机械臂的可操作性非常小时，其在特定方向的运动将显著受限，从而会导致任务完成的失败。此外，机械臂在工作中也会由于关节磨损、负重压力变化和零件松弛等因素引起机械臂运动学参数的变化，从而导致机械臂的实际值与标称值出现差异进而影响工作精度。因此如何对受影响机械臂的雅可比矩阵进行重新估计以及如何提高冗余度机械臂的可操作性已成为提高当前工业生产精准性和稳定性的进阶课题。

在医疗手术、汽车装配和产品包装等应用领域中，通常需要对多个冗余度机械臂组成的多冗余度机械臂系统进行协同控制。此外，随着工业生产逐具规模化，在更广阔的操作环境中对机械臂进行协同控制必然成为一种趋势。然而对于多冗余度机械臂系统而言，由于系统中各个冗余度机械臂之间受间隔距离、通信负载以及计算负载的限制，通常不具有全局通信的能力。而在大规模工作环境中，倘若采用了全局通信的通信拓扑结构，该系统可能会因为冗余度机械臂数量较多造成通信负载过大从而致使通信系统崩溃，最终导致整个冗余度机械臂系统协同任务的失败。而采用分布式通信拓扑结构的冗余度机械臂只需与其临近冗余度机械臂进行通信，即使出现某些代理通信失败，整个多冗余度机械臂系统也能正常工作。

发明内容

本发明提出一种多冗余度机械臂自适应可操纵性最优化的分布式协同控制方法与装置，旨在解决在通信受限的情况下实现模型未知的多冗余度机械臂的分布式协同运动控制，并使系统内所有冗余度机械臂的可操作性均达到最优化。

本发明实施例第一方面提供了一种雅可比矩阵的自适应估计方法；

所述雅可比矩阵的自适应估计方法包括：

获取所述模型未知的多冗余度机械臂的速度信息、加速度信息、关节速度信息和关节加速度信息；

对雅可比矩阵进行自适应估计。

所述自适应雅可比矩阵迭代等式为：

机械臂末端执行器的位置；

为r(t)的时间导数；

为

的时间导数；

为冗余度机械臂的关节速度向量；

为

的时间导数；

为

的伪逆；ρ₁＞0和ρ₂＞0为控制算法收敛速度的设计参数。

本发明实施例第二方面提供了一种多冗余度机械臂自适应可操纵性最优化的分布式协同控制方法，所述多冗余度机械臂系统包括两个或两个以上的冗余度机械臂，所述两个或两个以上的冗余度机械臂在通信拓扑图中连通；

所述控制方法包括：

确定所述两个或两个以上的冗余度机械臂中任意一个或多个冗余度机械臂为接收期望信息的冗余度机械臂；

获取所述接收期望信息的冗余度机械臂的位置信息和关节速度信息；

获取与所述接收期望信息的冗余度机械臂临近的冗余度机械臂的位置信息和关节速度信息，所述临近冗余度机械臂与接收期望信息的冗余度机械臂在通信拓扑图中相邻连接；

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

根据所述位置信息、关节速度信息、参考点轨迹信息和机械臂的自适应雅可比矩阵，按照预设的规则构建所述接收期望信息的冗余度机械臂对应的自适应雅可比矩阵等式；

在所述自适应雅可比矩阵等式的约束下，根据二次型优化和标准二次规划方法确定所述接收期望信息的冗余度机械臂的控制信号；

根据所述控制信号控制所述接收期望信息的冗余度机械臂，实现多冗余度机械臂自适应可操纵性最优化的分布式协同控制。

本发明实施例第三方面提供了一种多冗余度机械臂自适应可操纵性最优化的分布式协同控制装置，所述多冗余度机械臂系统包括两个或两个以上的冗余度机械臂，所述多冗余度机械臂系统内的冗余度机械臂在通信拓扑图中连通；

所述控制装置包括：

冗余度机械臂信息获取模块，用于获取所述模型未知的冗余度机械臂的位置、速度、加速度信息和关节位置、关节速度、关节加速度信息；

自适应雅可比矩阵构建模块，用于根据所述速度信息、加速度信息、关节速度和关节加速度信息对雅可比矩阵进行自适应估计；

接收期望信息的冗余度机械臂确定模块，用于确定所述两个或两个以上的冗余度机械臂中任意一个或多个冗余度机械臂为接收期望信息的冗余度机械臂；

临近冗余度机械臂信息获取模块，用于获取与所述接收期望信息的冗余度机械臂临近的冗余度机械臂的位置信息和关节速度信息。所述临近冗余度机械臂与所述接收期望信息的冗余度机械臂在通信拓扑图中相邻连接；

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

等式构建模块，用于根据所述位置信息、关节速度信息和参考点轨迹信息，按照预设的规则构建所述接收期望信息的冗余度机械臂对应的自适应雅可比矩阵等式；

控制信号确定模块，在所述自适应雅可比矩阵等式的约束下，根据二次型优化和标准二次规划方法确定所述接收期望信息的冗余度机械臂的控制信号；

机械臂控制模块，用于根据所述控制信号控制所述接收期望信息的冗余度机械臂，实现多冗余度机械臂自适应可操纵性最优化的分布式协同控制。

利用本发明实施例提出的一种多冗余度机械臂自适应可操纵性最优化的分布式协同控制方法，接收期望信息的冗余度机械臂只需与较少数量的临近冗余度机械臂进行通信，因而大大减小了整个多冗余度机械臂系统的通信负载和计算负担，从而在通信受限的情况下实现模型未知的多冗余度机械臂分布式协同运动控制，并使系统内所有冗余度机械臂的可操作性均达到最优化。

附图说明

图1为本发明实施例中一种多冗余度机械臂自适应可操纵性最优化的分布式协同控制方法的一个实施例的流程图；

图2为图1中步骤105的一个实施例的具体流程图；

图3为图2中步骤1052的一个实施例的具体流程图；

图4为图1中步骤106的一个实施例的具体流程图；

图5为本发明实施例中一种多冗余度机械臂自适应可操纵性最优化的分布式协同控制装置一个实施例的结构图；

具体实施方式

本发明提出一种多冗余度机械臂自适应可操纵性最优化的分布式协同控制方法与装置，旨在解决在通信受限的情况下实现模型未知的多冗余度机械臂的分布式协同运动控制，并使系统内所有冗余度机械臂的可操作性均达到最优化。在整个多冗余度机械臂系统协同控制的过程中，我们均使用机械臂的自适应雅可比矩阵来协助完成多冗余度机械臂系统的协同控制任务。

所述雅可比矩阵的自适应估计方法包括：

获取所述模型未知的多冗余度机械臂的速度信息、加速度信息、关节速度信息和关节加速度信息；

对雅可比矩阵进行自适应估计。

所述自适应雅可比矩阵迭代等式为：

机械臂末端执行器的位置和方向向量；

为r(t)的时间导数；

为

的时间导数；

为冗余度机械臂的关节速度向量；

为

的时间导数；

为

的伪逆；ρ₁＞0和ρ₂＞0为控制算法收敛速度的设计参数。

根据上述对雅可比矩阵的自适应估计，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行进一步描述：

图1示出了本发明一种模型未知的多冗余度机械臂自适应可操纵性最优化的分布式协同控制方法的一个实施例的流程图，所述多冗余度机械臂系统包括两个或两个以上的冗余度机械臂，所述多冗余度机械臂系统内的冗余度机械臂在通信拓扑图中连通；

如图1所示，所述控制方法包括：

101、确定所述两个或两个以上的冗余度机械臂中任意一个或多个冗余度机械臂为接收期望信息的冗余度机械臂；

所述两个或两个以上的冗余度机械臂在工作空间中按需分布，确定其中的一个或多个冗余度机械臂为接收期望信息的冗余度机械臂。

102、获取所述接收期望信息的冗余度机械臂的位置信息和关节速度信息；

103、获取所述接收期望信息的冗余度机械臂的临近冗余度机械臂的位置信息和关节速度信息，所述临近冗余度机械臂与所述接收期望信息的冗余度机械臂在通信拓扑图中相邻连接；

在确定接收期望信息的冗余度机械臂后，获取所述接收期望信息的冗余度机械臂的临近冗余度机械臂的位置信息和速度信息。所述临近冗余度机械臂是指与所述接收期望信息的冗余度机械臂在通信拓扑图中相邻连接的冗余度机械臂，它和接收期望信息的冗余度机械臂属于同一多冗余度机械臂系统。所述多冗余度机械臂系统在通信拓扑图中是连通的，因此对于系统内的某个接收期望信息的冗余度机械臂来说，其临近冗余度机械臂的个数至少为一个或一个以上。

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

所述目标参考点是预设的位于接收期望信息的冗余度机械臂工作空间中的位置参考点，所述参考点轨迹信息由所述目标参考点的期望位置及其移动速度确定。可以在多冗余度机械臂系统的工作空间中设置一个控制中心，由所述控制中心将所述参考点轨迹信息发送给各个接收期望信息的冗余度机械臂，也可以利用临近冗余度机械臂或其它方式将所述参考点轨迹信息发送给各个接收期望信息的冗余度机械臂。

105、根据所述位置信息、关节速度信息和参考点轨迹信息，按照预设的规则构建所述接收期望信息的冗余度机械臂对应的自适应雅可比矩阵等式；

在获取与所述接收期望信息的冗余度机械臂临近的冗余度机械臂的位置信息和关节速度信息后，按照预设的规则构建所述接收期望信息的冗余度机械臂对应的自适应雅可比矩阵等式；

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

1051、构建所述接收期望信息的冗余度机械臂对应的权重矩阵，所述权重矩阵中的各个元素为所述接收期望信息的冗余度机械臂与所述多冗余度机械臂系统中的各个机械臂之间分别对应的第一连通权值；

所述权重矩阵的元素个数由所述多冗余度机械臂系统中的机械臂个数决定，所述第一连通权值代表所述接收期望信息的冗余度机械臂与所述多冗余度机械臂系统中的各个机械臂之间的通信连接关系。比如定义权重矩阵

其中D_ij表示权重矩阵D的第ij个元素，并定义其为通信拓扑图上第i个冗余度机械臂与第j个冗余度机械臂之间的第一连通权值。

1052、根据所述参考点轨迹信息的获取状态确定所述接收期望信息的冗余度机械臂的第二连通权值；

所述第二连通权值代表所述接收期望信息的冗余度机械臂对于所述参考点轨迹信息的获取状态，可以用κ_i来表示第i个冗余度机械臂的第二连通权值。

1053、根据所述位置信息、关节速度信息、所述参考点轨迹信息、所述权重矩阵和所述第二连通权值构建所述接收期望信息的冗余度机械臂对应的自适应雅可比矩阵等式。

在构建权重矩阵以及确定第二连通权值后，可以根据所述位置信息、关节速度信息、参考点轨迹信息、权重矩阵和第二连通权值构建所述接收期望信息的冗余度机械臂对应的自适应雅可比矩阵等式。

进一步来说，所述第一连通权值可以通过以下步骤确定：

(1)将所述接收期望信息的冗余度机械臂与所述接收期望信息的冗余度机械臂自身之间的第一连通权值设置为1；

(2)将所述接收期望信息的冗余度机械臂与所述临近冗余度机械臂之间的第一连通权值设置为1；

(3)将所述接收期望信息的冗余度机械臂与所述多冗余度机械臂系统中非自身且非相邻的冗余度机械臂之间的第一连通权值设置为0；

对于上述步骤(1)，所述接收期望信息的冗余度机械臂与其本身之间具备通信连接关系，因此将所述第一连通权值设置为1。对于上述步骤(2)，所述接收期望信息的冗余度机械臂与其临近冗余度机械臂之间可以直接交换信息，因此将所述第一连通权值设置为1。对于上述步骤(3)，所述接收期望信息的冗余度机械臂与所述多冗余度机械臂系统中非自身且非相邻的冗余度机械臂之间无需直接交换信息，因此将所述第一连通权值设置为0。

如图3所示，步骤1052具体可以包括：

10521、判断所述接收期望信息的冗余度机械臂是否获取到所述参考点轨迹信息；

10522、若所述接收期望信息的冗余度机械臂获取到所述参考点轨迹信息，则将所述第二连通权值设置为1；

10523、若所述接收期望信息的冗余度机械臂未获取到所述参考点轨迹信息，则将所述第二连通权值设置为0。

根据所述参考点轨迹信息的获取状态确定所述接收期望信息的冗余度机械臂的第二连通权值，若所述接收期望信息的冗余度机械臂获取到所述参考点轨迹信息，则将所述第二连通权值设置为1，否则将所述第二连通权值设置为0。

所述自适应雅可比矩阵的通用表达式为：

其中，

表示与接收期望信息的冗余度机械臂i之间的第一连通权值为1的冗余度机械臂的编号；D_ij表示接收期望信息的冗余度机械臂i的权重矩阵，所述权重矩阵中的各个元素为接收期望信息的冗余度机械臂i与冗余度机械臂j之间的第一连通权值；

为接收期望信息的冗余度机械臂i的自适应雅可比矩阵；

为接收期望信息的冗余度机械臂i的关节速度向量；κ_i表示接收期望信息的冗余度机械臂i的第二连通权值；χ_i＝r_i-r_cp为所述位置信息，r_i为接收期望信息的冗余度机械臂i的末端执行器位置，r_cp为预设的接收期望信息的冗余度机械臂i的末端执行器相对于所述目标参考点的距离向量；r_d为所述目标参考点的期望位置；

为所述目标参考点的期望速度；ρ＞0为控制算法收敛速度的设计参数，其值越大则代表收敛速度越快。

上述自适应雅可比矩阵等式主要负责实现多冗余度机械臂之间的分布式协同运动约束，通过获取系统中其它冗余度机械臂末端执行器的位置信息和速度信息，即可使整个多冗余机械臂系统内的冗余度机械臂实现协同运动。此外，基于该约束构建的多冗余度机械臂分布式协同运动系统具有高鲁棒性，低通信成本的特点。具体而言，当有少量机械臂加入或者退出时都不会影响多冗余度机械臂系统的稳定性，而且每个机械臂只需要和数量较少的临近机械臂通信，并不需要满足全局通信的要求。此举在现实应用中既增强了整个系统的稳定性，又大大减小了系统的通信负载。

106、在所述自适应雅可比矩阵等式、矩阵求逆等式的约束下，根据二次型优化和标准二次规划方法确定所述接收期望信息的冗余度机械臂的控制信号；

在构建好所述接收期望信息的冗余度机械臂对应的自适应雅可比矩阵等式之后，在所述自适应雅可比矩阵等式的约束下，根据二次型优化和标准二次规划方法确定所述接收期望信息的冗余度机械臂的控制信号。

进一步而言，如图4所示，步骤106具体可以包括：

1061、设置最优化性能指标为冗余度机械臂的可操作性，在所述自适应雅可比矩阵等式、矩阵求逆等式的约束下确定二次型优化结果；

本发明实施例需要在通信受限的情况下实现模型未知的多冗余度机械臂分布式协同运动控制，并使系统内所有冗余度机械臂的可操作性均达到最优化。因此在二次型优化过程中设置最优化性能指标为冗余度机械臂的可操作性，通过对雅可比矩阵进行自适应估计，然后在所述自适应雅可比矩阵等式、矩阵求逆等式的约束下确定二次型优化结果。所述二次型优化具体可以写为：

对雅可比矩阵进行自适应估计

所述第一约束条件包括：

自适应雅可比矩阵等式约束条件

矩阵求逆等式约束条件

所述第一式子为：

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

上述二次型优化方案可描述为如下的标准二次规划方案：

约束条件：

最小化：

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

可以利用标准二次规划求解器或者数值方法对所述标准二次规划问题进行求解。求解得到使未知模型的多冗余度机械臂达成可操作性最优化协作运动的最优解。

1064、根据所述求解结果确定所述接收期望信息的冗余度机械臂的控制信号；

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

107、根据所述控制信号控制所述接收期望信息的冗余度机械臂，使得所述接收期望信息的冗余度机械臂按照所述参考点的期望轨迹运动；

在通信受限的情况下，通过确定所述接收期望信息的冗余度机械臂的控制信号后，根据所述控制信号控制所述接收期望信息的冗余度机械臂，使得所述接收期望信息的冗余度机械臂按照所述参考点的期望轨迹运动，并在各约束等式的约束下使得系统内所有模型未知的多冗余度机械臂在实现分布式协同运动的同时均达成自身的可操作性最优化。所述可操作性最优化指冗余度机械臂在执行任务过程中不断对关节位型进行优化，以保证具有足够的灵活性，这在很大程度上避免了机械臂陷入关节奇异。如果冗余度机械臂陷入关节奇异，则其工作时不仅会导致任务执行的失败，还可能会对机械臂本身造成损坏。

在本发明实施例中，通过对雅可比矩阵的自适应估计操作有效地解决了冗余度机械臂参数变化的问题并由此提高了任务执行的精度；可操作性最优化的引入在极大程度上避免了机械臂可能出现的奇异性；而本发明实施例提出的协同控制方法，由于接收期望信息的冗余度机械臂只需与较少数量的临近冗余度机械臂进行通信，因而大大减小了通信负载和计算负载，使得任务可以高效稳定的完成。

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

请参阅图5，示出了本发明实施例中一种模型未知的多冗余度机械臂自适应可操纵性最优化的分布式协同控制装置，所述多冗余度机械臂系统包括两个或两个以上的冗余度机械臂，所述多冗余度机械臂系统内的冗余度机械臂在通信拓扑图中连通；

所述控制装置包括：

冗余度机械臂信息获取模块501，用于获取所述模型未知的多冗余度机械臂的位置、速度、加速度信息和关节位置、关节速度、关节加速度信息；

自适应雅可比矩阵构建模块502，用于根据所述速度信息、加速度信息、关节速度和关节加速度信息对雅可比矩阵进行自适应估计；

接收期望信息的冗余度机械臂确定模块503，用于确定所述两个或两个以上的冗余度机械臂中任意一个或多个冗余度机械臂为接收期望信息的冗余度机械臂；

临近冗余度机械臂信息获取模块504，用于获取与所述接收期望信息的冗余度机械臂临近的冗余度机械臂的位置信息和关节速度信息，所述临近冗余度机械臂与所述接收期望信息的冗余度机械臂在通信拓扑图中相邻连接；

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

等式构建模块506，用于根据所述位置信息、速度信息和参考点轨迹信息，按照预设的规则构建所述接收期望信息的冗余度机械臂对应的自适应雅可比矩阵等式；

控制信号确定模块507，用于在所述雅可比矩阵等式的约束下，根据二次型优化和标准二次规划方法确定所述接收期望信息的冗余度机械臂的控制信号；

机械臂控制模块508，用于根据所述控制信号控制所述接收期望信息的冗余度机械臂，使得所述接收期望信息的冗余度机械臂按照所述参考点的期望轨迹运动。

进一步来说，所述等式构建模块506具体可以包括：

权重矩阵构建单元，用于构建所述接收期望信息的冗余度机械臂对应的权重矩阵，所述权重矩阵中的各个元素为所述接收期望信息的冗余度机械臂与所述多冗余度机械臂系统中的各个机械臂之间分别对应的第一连通权值；

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

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

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

第一设置模块，用于将所述接收期望信息的冗余度机械臂与所述接收期望信息的冗余度机械臂自身之间的第一连通权值设置为1；

第二设置模块，用于将所述接收期望信息的冗余度机械臂与临近冗余度机械臂之间的第一连通权值设置为1；

第三设置模块，用于将所述接收期望信息的冗余度机械臂与所述多冗余度机械臂系统中非自身且非相邻的冗余度机械臂之间的第一连通权值设置为0；

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

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

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

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

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

其中，

表示与接收期望信息的冗余度机械臂i之间的第一连通权值为1的冗余度机械臂的编号；D_ij表示接收期望信息的冗余度机械臂i的权重矩阵，所述权重矩阵中的各个元素为接收期望信息的冗余度机械臂i与冗余度机械臂j之间的第一连通权值；

为接收期望信息的冗余度机械臂i的自适应雅可比矩阵；

为接收期望信息的冗余度机械臂i的关节速度向量；κ_i表示接收期望信息的冗余度机械臂i的第二连通权值；χ_i＝r_i-r_cp为所述位置信息，r_i为接收期望信息的冗余度机械臂i的末端执行器位置，r_cp为预设的接收期望信息的冗余度机械臂i的末端执行器相对于所述目标参考点的距离向量；r_d为所述目标参考点的期望位置；

为所述目标参考点的期望速度；ρ＞0为控制算法收敛速度的设计参数，其值越大则代表收敛速度越快。

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

第二确定单元，用于设置最优化性能指标为冗余度机械臂的可操作性，在所述自适应雅可比矩阵等式的约束下确定二次型优化结果；

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

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

控制信号确定单元，用于根据所述求解结果确定所述接收期望信息的冗余度机械臂的控制信号。

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

对雅可比矩阵进行自适应估计

所述第一约束条件包括：

自适应雅可比矩阵等式约束条件

矩阵求逆等式约束条件

所述第一式子为：

从以上实施例的描述可清楚的了解到上述系统、装置和单元的具体工作流程。对此，应该理解到，以上所描述的装置实施例仅为示意性的，我们仅是对上述单元做了一种逻辑上的划分以方便描述，而在实际实现时可以有另外的划分方式。

此外，本发明实施例中的各功能单元还可集成在一个处理单元中作为独立的产品使用或销售。对于该集成的单元来说，其既可以采用硬件单元的形式实现，也可以采用软件功能单元的形式实现。当采用软件功能单元的形式实现时，可将其存储在一个计算机可读取的存储介质中，例如：随机存取存储器(RAM)、闪存(Flash Memory)等等。

据上所述，本实施例仅用以说明本发明的技术方案，而并非对其限制。本领域的相关技术人员应当理解：对上述实施例中所涉及的技术方案仍可进行改进和优化，也可对其中部分技术特征进行等同替换。而这些改进或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的精神和范围。

Claims (8)

1.一种多冗余度机械臂自适应可操纵性最优化的分布式协同控制方法，其特征在于，多冗余度机械臂 系统中至少包括两个或两个以上的冗余度机械臂，通过利用其速度信息、加速度信息、关节速度信息和关节加速度信息可对冗余度机械臂的雅可比矩阵进行自适应估计，实现多冗余度机械臂自适应可操纵性最优化的分布式协同控制；

具体控制方法包括如下步骤：

确定所述两个或两个以上的冗余度机械臂中任意一个或多个冗余度机械臂为接收期望信息的冗余度机械臂；

获取所述接收期望信息的冗余度机械臂的位置信息和关节速度信息；

获取与所述接收期望信息的冗余度机械臂邻近的冗余度机械臂的位置信息和关节速度信息，该邻近冗余度机械臂与所述接收期望信息的冗余度机械臂在通信拓扑图中相邻连接；

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

根据所述接收期望信息的冗余度机械臂的位置信息、关节速度信息、参考点轨迹信息，按照预设的规则构建所述接收期望信息的冗余度机械臂对应的自适应雅可比矩阵等式；所述自适应雅可比矩阵等式具体设计为

机械臂末端执行器的位置；

为r(t)的时间导数；

为

的时间导数；

为冗余度机械臂的关节速度向量；

为

的时间导数；

为

的伪逆；ρ₁＞0和ρ₂＞0为控制算法收敛速度的设计参数；

在所述自适应雅可比矩阵等式的约束下，根据二次型优化和标准二次规划方法确定所述接收期望信息的冗余度机械臂的控制信号，优化目标为使各冗余度机械臂在第一约束条件下使得第一式子最小化；

所述第一约束条件包括：

接收期望信息的冗余度机械臂对应的自适应雅可比矩阵等式；

所述第一式子为

其中M表示冗余度机械臂的可操作度；

为M的时间导数，通过最大化

可使M沿其数值增加的方向快速地增大；

为机械臂的关节速度范数，用以调节系统运动能量的消耗；det(·)表示求一个方阵的行列式；上标T表示矩阵或向量的转置；q为冗余度机械臂的关节向量；q^k为冗余度机械臂的第k个关节；

为q^k的时间导数；vec(·)为矩阵向量化操作；D_k为

的简写；另外，为避免出现由于冗余度机械臂陷入奇异而导致系统不能正常运行，可借助估计等式

并将

整体作为一个估计值嵌入到求解过程中；在冗余度机械臂未陷入奇异时，其估计值即为直接求解

所得的理论值；在冗余度机械臂陷入奇异时，所述估计等式也会给出一个估计值来近似

可能存在的理论解；该矩阵求逆等式约束条件设置为

其中I为单位矩阵；

为克罗内克积；

根据所述控制信号控制接收期望信息的冗余度机械臂，实现多冗余度机械臂自适应可操纵性最优化的分布式协同控制。

2.根据权利要求1所述的一种多冗余度机械臂自适应可操纵性最优化的分布式协同控制方法，其特征在于，所述接收期望信息的冗余度机械臂对应的自适应雅可比矩阵等式构建步骤为：

构建所述接收期望信息的冗余度机械臂对应的权重矩阵；所述权重矩阵中的各个元素分别对应所述接收期望信息的冗余度机械臂与所述多冗余度机械臂系统中各个机械臂之间的第一连通权值；

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

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

3.根据权利要求2所述的一种多冗余度机械臂自适应可操纵性最优化的分布式协同控制方法，其特征在于，所述第一连通权值通过以下步骤确定：

将所述接收期望信息的冗余度机械臂与其自身之间的第一连通权值设置为1；

将所述接收期望信息的冗余度机械臂与所述邻近冗余度机械臂之间的第一连通权值设置为1；

将所述接收期望信息的冗余度机械臂与所述多冗余度机械臂系统中既非自身又非相邻的机械臂之间的第一连通权值设置为0；

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

判断所述接收期望信息的冗余度机械臂是否获取到所述参考点轨迹信息；

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

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

所述接收期望信息的冗余度机械臂对应的自适应雅可比矩阵等式为

其中，

表示与接收期望信息的冗余度机械臂i之间的第一连通权值为1的冗余度机械臂的编号；D_ij表示接收期望信息的冗余度机械臂i的权重矩阵，所述权重矩阵中的各个元素为接收期望信息的冗余度机械臂i与冗余度机械臂j之间的第一连通权值；

为接收期望信息的冗余度机械臂i的自适应雅可比矩阵；

为接收期望信息的冗余度机械臂i的关节速度向量；κ_i表示接收期望信息的冗余度机械臂i的第二连通权值；χ_i＝r_i-r_cp为所述位置信息，r_i为接收期望信息的冗余度机械臂i的末端执行器位置，r_cp为预设的接收期望信息的冗余度机械臂i的末端执行器相对于所述目标参考点的距离向量；r_d为所述目标参考点的期望位置；

为所述目标参考点的期望速度；ρ＞0为控制算法收敛速度的设计参数，其值越大则代表收敛速度越快。

4.根据权利要求1至3中任一项所述的一种多冗余度机械臂自适应可操纵性最优化的分布式协同控制方法，其特征在于，根据所述机械臂的自适应雅可比矩阵和自适应雅可比矩阵等式约束，利用二次型优化和标准二次规划方法确定所述接收期望信息的冗余度机械臂的控制信号，具体包括：

设置最优化性能指标为冗余度机械臂的可操作性，通过对雅可比矩阵进行自适应估计并根据所述自适应雅可比矩阵等式的约束确定二次型优化结果；

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

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

根据所述求解结果确定所述接收期望信息的冗余度机械臂的控制信号。

5.一种多冗余度机械臂自适应可操纵性最优化的分布式协同控制装置，其特征在于，多冗余度机械臂系统至少包括两个或两个以上的冗余度机械臂，其在通信拓扑图中连通；

所述控制装置包括：

冗余度机械臂信息获取模块，用于获取机械臂的位置、速度、加速度信息和关节位置、关节速度、关节加速度信息；

自适应雅可比矩阵构建模块，用于根据所述速度信息、加速度信息、关节速度和关节加速度信息对雅可比矩阵进行自适应估计；所述自适应雅可比矩阵构建模块的估计如下：

机械臂末端执行器的位置；

为r(t)的时间导数；

为

的时间导数；

为冗余度机械臂的关节速度向量；

为

的时间导数；

为

的伪逆；ρ₁＞0和ρ₂＞0为控制算法收敛速度的设计参数；

接收期望信息的冗余度机械臂确定模块，用于确定所述两个或两个以上的冗余度机械臂中任意一个或多个冗余度机械臂为接收期望信息的冗余度机械臂；

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

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

等式构建模块，用于根据所述位置信息、关节速度信息和参考点轨迹信息，按照预设的规则构建所述接收期望信息的冗余度机械臂对应的自适应雅可比矩阵等式；

控制信号确定模块，用于根据二次型优化和标准二次规划方法确定所述接收期望信息的冗余度机械臂的控制信号，控制目标为使得各冗余度机械臂在第一约束条件下使得第一式子最小化；

所述第一约束条件包括：

接收期望信息的冗余度机械臂对应的自适应雅可比矩阵等式；

所述第一式子为

其中M表示冗余度机械臂的可操作度；

为M的时间导数，通过最大化

可使M沿其数值增加的方向快速地增大；

为机械臂的关节速度范数，用以调节系统运动能量的消耗；det(·)表示求一个方阵的行列式；上标^T表示矩阵或向量的转置；q为冗余度机械臂的关节向量；q^k为冗余度机械臂的第k个关节；

为q^k的时间导数；vec(·)为矩阵向量化操作；D_k为

的简写；另外，为避免出现由于冗余度机械臂陷入奇异而导致系统不能正常运行，可借助估计等式

并将

整体作为一个估计值嵌入到求解过程中；在冗余度机械臂未陷入奇异时，其估计值即为直接求解

所得的理论值；在冗余度机械臂陷入奇异时，所述估计等式也会给出一个估计值来近似

可能存在的理论解；该矩阵求逆等式约束条件设置为

其中I为单位矩阵；

为克罗内克积；

机械臂控制模块，用于根据所述控制信号控制所述接收期望信息的冗余度机械臂，实现多冗余度机械臂自适应可操纵性最优化的分布式协同控制。

6.根据权利要求5所述一种多冗余度机械臂自适应可操纵性最优化的分布式协同控制装置，其特征在于，所述等式构建模块具体包括：

权重矩阵构建单元，用于构建所述接收期望信息的冗余度机械臂对应的权重矩阵，所述权重矩阵中的各个元素为所述接收期望信息的冗余度机械臂与所述多冗余度机械臂系统中的各个机械臂之间分别对应的第一连通权值；

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

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

7.根据权利要求6 所述的一种多冗余度机械臂自适应可操纵性最优化的分布式协同控制装置，其特征在于，所述权重矩阵构建单元具体包括：

第一设置模块，用于将所述接收期望信息的冗余度机械臂与所述接收期望信息的冗余度机械臂自身之间的第一连通权值设置为1；

第二设置模块，用于将所述接收期望信息的冗余度机械臂与邻近冗余度机械臂之间的第一连通权值设置为1；

第三设置模块，用于将所述接收期望信息的冗余度机械臂与所述多冗余度机械臂系统中非自身且非相邻的冗余度机械臂之间的第一连通权值设置为0；

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

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

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

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

所述接收期望信息的冗余度机械臂对应的自适应雅可比矩阵等式为

其中，

表示与接收期望信息的冗余度机械臂i之间的第一连通权值为1的冗余度机械臂的编号；D_ij表示接收期望信息的冗余度机械臂i的权重矩阵，所述权重矩阵中的各个元素为接收期望信息的冗余度机械臂i与冗余度机械臂j之间的第一连通权值；

为接收期望信息的冗余度机械臂i的自适应雅可比矩阵；

为接收期望信息的冗余度机械臂i的关节速度向量；κ_i表示接收期望信息的冗余度机械臂i的第二连通权值；χ_i＝r_i-r_cp为所述位置信息，r_i为接收期望信息的冗余度机械臂i的末端执行器位置，r_cp为预设的接收期望信息的冗余度机械臂i的末端执行器相对于所述目标参考点的距离向量；r_d为所述目标参考点的期望位置；

为所述目标参考点的期望速度；ρ＞0为控制算法收敛速度的设计参数，其值越大则代表收敛速度越快。

8.根据权利要求5至7中任一项所述的一种多冗余度机械臂自适应可操纵性最优化的分布式协同控制装置，其特征在于，所述控制信号确定模块具体包括：

第二确定单元，用于设置最优化性能指标为冗余度机械臂的可操作性，在所述自适应雅可比矩阵等式的约束下确定二次型优化结果；

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

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

控制信号确定单元，用于根据所述求解结果确定所述接收期望信息的冗余度机械臂的控制信号。

Images