CN108214483B - 多固定基座冗余度机械臂的任务动态追踪方法和装置 - Google Patents

Abstract

一种多固定基座冗余度机械臂的任务动态追踪方法包括：获取多固定基座冗余度机械臂系统中的固定基座冗余度机械臂与目标物的距离，并根据固定基座冗余度机械臂的拓扑连接图的连接关系，相邻的两个固定基座冗余度机械臂之间交换信息；根据所述固定基座冗余度机械臂与目标物的距离，以及相邻的两个固定基座冗余度机械臂之间所交换的信息，确定与目标物最近的固定基座冗余度机械臂；启动所述与目标物的距离最近的固定基座冗余度机械臂的末端执行器，对目标物进行追踪。本方法能够有效的适应大规模环境下的固定基座冗余度机械臂之间的通信，并完成多固定基座冗余度机械臂系统的协作。

Description

多固定基座冗余度机械臂的任务动态追踪方法和装置

技术领域

本发明属于智能控制领域，尤其涉及多固定基座冗余度机械臂的任务动态追踪方法和装置。

背景技术

冗余度机械臂具有自由度大于任务空间所需最少自由度的特性，广泛应用于装备制造、产品加工、机器作业等国民经济生产活动中。冗余度机械臂经过多年的研究与发展，已经逐步走向实用化。随着科技的进步，多固定基座冗余度机械臂系统在医疗手术、汽车装配等领域逐渐发挥出巨大的作用，并显现出广泛的应用前景。例如，著名的达芬奇外科手术机器人即可被视为是一种多固定基座冗余度机械臂系统，其逐渐成为高水平医疗不可或缺的助手。

在广阔的环境中，多固定基座冗余度机械臂系统中各个固定基座冗余度机械臂因距离以及通信负载的限制，通常不具有全局通信的能力，也即，多固定基座冗余度机械臂系统中，每个固定基座冗余度机械臂只能与数量有限的固定基座冗余度机械臂通信。在大规模环境下，固定基座冗余度机械臂之间有可能因为数量较多导致通信负载过大而难以通信，导致信息不能随时、随地进行共享，不能有效的完成多固定基座冗余度机械臂系统协作。

发明内容

本发明的目的在于提供一种多固定基座冗余度机械臂的任务动态追踪方法，以解决现有技术固定基座冗余度机械臂之间有可能因为数量较多导致通信负载过大而难以通信，导致信息不能随时、随地进行共享，不能有效的完成多固定基座冗余度机械臂系统协作的问题。

第一方面，本发明实施例提供了一种多固定基座冗余度机械臂的任务动态追踪方法，所述方法包括：

获取多固定基座冗余度机械臂系统中的固定基座冗余度机械臂与目标物的距离，并根据固定基座冗余度机械臂的拓扑连接图的连接关系，相邻的两个固定基座冗余度机械臂之间交换信息；

根据所述固定基座冗余度机械臂与目标物的距离，以及相邻的两个固定基座冗余度机械臂之间所交换的信息，确定与目标物最近的固定基座冗余度机械臂；

启动所述与目标物的距离最近的固定基座冗余度机械臂的末端执行器，对目标物进行追踪。

结合第一方面，在第一方面的第一种可能实现方式中，所述根据所述固定基座冗余度机械臂与目标物的距离，以及相邻的两个固定基座冗余度机械臂之间所交换的信息，确定与目标物最近的固定基座冗余度机械臂步骤包括：

通过公式：

以及

确定固定基座冗余度机械臂是否为与目标物最近的固定基座冗余度机械臂，其中：矩阵A∈R^n×n，其第ij个元素定义为第i个固定基座冗余度机械臂与第j个固定基座冗余度机械臂之间的连接权值，即当第i个与第j个固定基座冗余度机械臂在拓扑图上连通时，A_ij＝1，否则，A_ij＝0，且第i个固定基座冗余度机械臂与自己的连接权值为A_ii＝1，s_i对应于第i个固定基座冗余度机械臂的启动状态，当s_i为0时，第i个固定基座冗余度机械臂不启动，否则启动，s_i的初始值

随机给定，η_i为辅助变量，其初始值随机给定，u_i＝z-d_i，z为正数，d_i为第i个固定基座冗余度机械臂实时获得的其末端执行器与移动目标物之间的距离，j∈N(i)表示与第i个固定基座冗余度机械臂连接权值为1的固定基座冗余度机械臂编号，γ与λ为控制算法收敛速度的正参数，τ∈R为采样间隔，c₀∈R与c₂∈R为正常数，上标k表示第k次迭代。

结合第一方面的第一种可能实现方式，在第一方面的第二种可能实现方式中，当第i个固定基座冗余度机械臂中，除第一个关节角外的其余关节角度的绝对值的和小于预先设定数值σ时，

其中

为正数。

结合第一方面的第一种可能实现方式，或者第一方面的第二种可能实现方式，在第一方面的第三种可能实现方式中，所述启动所述与目标物的距离最近的固定基座冗余度机械臂的末端执行器，对目标物进行追踪的步骤包括：

根据迭代后的固定基座冗余度机械臂的启动状态，结合公式：

确定第i个固定基座冗余度机械臂所获得的启动状态，其中：

表示在第kτ时刻时，第i个固定基座冗余度机械臂移动平台和机械臂的联合角度向量，

表示在第kτ时刻时，第i个固定基座冗余度机械臂移动平台和机械臂的联合速度向量，上标^T表示矩阵和向量的转置，

表示在第kτ时刻时，第i个固定基座冗余度机械臂雅可比矩阵的转置，

表示在第kτ时刻时，第i个固定基座冗余度机械臂末端执行器的位置向量，

表示在第kτ时刻时，移动目标物的位置向量，α代表速度反馈正系数。

结合第一方面，在第一方面的第四种可能实现方式中，所述固定基座冗余度机械臂为连杆机械臂，所述根据固定基座冗余度机械臂的拓扑连接图的连接关系，相邻的两个固定基座冗余度机械臂之间交换信息步骤具体为：

按序对所述连杆机械臂的固定基座冗余度机械臂进行编号，在编号相连的两个固定基座冗余度机械臂之间交换信息。

第二方面，本发明实施例提供了一种多固定基座冗余度机械臂的任务动态追踪装置，所述装置包括：

信息获取单元，用于获取多固定基座冗余度机械臂系统中的固定基座冗余度机械臂与目标物的距离，并根据固定基座冗余度机械臂的拓扑连接图的连接关系，相邻的两个固定基座冗余度机械臂之间交换信息；

确定单元，用于根据所述固定基座冗余度机械臂与目标物的距离，以及相邻的两个固定基座冗余度机械臂之间所交换的信息，确定与目标物最近的固定基座冗余度机械臂；

追踪单元，用于启动所述与目标物的距离最近的固定基座冗余度机械臂的末端执行器，对目标物进行追踪。

结合第二方面，在第二方面的第一种可能实现方式中，所述确定单元具体用于：

通过公式：

以及

确定固定基座冗余度机械臂是否为与目标物最近的固定基座冗余度机械臂，其中：矩阵A∈R^n×n，其第ij个元素定义为第i个固定基座冗余度机械臂与第j个固定基座冗余度机械臂之间的连接权值，即当第i个与第j个固定基座冗余度机械臂在拓扑图上连通时，A_ij＝1，否则，A_ij＝0，且第i个固定基座冗余度机械臂与自己的连接权值为A_ii＝1，s_i对应于第i个固定基座冗余度机械臂的启动状态，当s_i为0时，第i个固定基座冗余度机械臂不启动，否则启动，s_i的初始值

随机给定，η_i为辅助变量，其初始值随机给定，u_i＝z-d_i，z为正数，d_i为第i个固定基座冗余度机械臂实时获得的其末端执行器与移动目标物之间的距离，j∈N(i)表示与第i个固定基座冗余度机械臂连接权值为1的固定基座冗余度机械臂编号，γ与λ为控制算法收敛速度的正参数，τ∈R为采样间隔，c₀∈R与c₂∈R为正常数，上标k表示第k次迭代。

结合第二方面的第一种可能实现方式，在第二方面的第二种可能实现方式中，当第i个固定基座冗余度机械臂中，除第一个关节角外的其余关节角度的绝对值的和小于预先设定数值σ时，

其中

为正数。

结合第二方面的第一种可能实现方式，或第二方面的第二种可能实现方式，在第二方面的第三种可能实现方式中，所述追踪单元具体用于：

根据迭代后的固定基座冗余度机械臂的启动状态，结合公式：

确定第i个固定基座冗余度机械臂所获得的启动状态，其中：

表示在第kτ时刻时，第i个固定基座冗余度机械臂移动平台和机械臂的联合角度向量，

表示在第kτ时刻时，第i个固定基座冗余度机械臂移动平台和机械臂的联合速度向量，上标^T表示矩阵和向量的转置，

表示在第kτ时刻时，第i个固定基座冗余度机械臂雅可比矩阵的转置，

表示在第kτ时刻时，第i个固定基座冗余度机械臂末端执行器的位置向量，

表示在第kτ时刻时，移动目标物的位置向量，α代表速度反馈正系数。

结合第二方面，在第二方面的第四种可能实现方式中，所述固定基座冗余度机械臂为连杆机械臂，所述确定单元具体用于：

按序对所述连杆机械臂的固定基座冗余度机械臂进行编号，在编号相连的两个固定基座冗余度机械臂之间交换信息。

在本发明中，通过获取多固定基座冗余度机械臂系统中的固定基座冗余度机械臂与目标物的距离，并在固定基座冗余度机械臂拓扑连接图中的两个相邻的固定基座冗余度机械臂之间交换信息，根据获取的距离以及交换的信息，确定距离目标物最近的固定基座冗余度机械臂，并启动所述距离目标物最近的固定基座冗余度机械臂，对目标物进行追踪。本发明能够有效的适应大规模环境下的固定基座冗余度机械臂之间的通信，并完成多固定基座冗余度机械臂系统的协作。

附图说明

图1是本发明实施例提供的多固定基座冗余度机械臂的任务动态追踪方法的实现流程图；

图2是本发明实施例提供的多固定基座冗余度机械臂的结构示意图；

图3为本发明实施例提供的多固定基座冗余度机械臂的任务动态追踪装置的结构示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

本发明实施例的目的在于提供一种多固定基座冗余度机械臂的任务动态追踪方法和装置，以解决现有技术中多固定基座冗余度机械臂系统中各个固定基座冗余度机械臂因距离以及通信负载的限制，通常不具有全局通信的能力，也即，多固定基座冗余度机械臂系统中，每个固定基座冗余度机械臂只能与数量有限的固定基座冗余度机械臂通信。在大规模环境下，固定基座冗余度机械臂之间有可能因为数量较多导致通信负载过大而难以通信，导致信息不能随时、随地进行共享，不能有效的完成多固定基座冗余度机械臂系统协作的问题。下面结合附图具体说明。

图1示出了本发明第一实施例提供的多固定基座冗余度机械臂的任务动态追踪方法的实现流程，详述如下：

在步骤S101中，获取多固定基座冗余度机械臂系统中的固定基座冗余度机械臂与目标物的距离，并根据固定基座冗余度机械臂的拓扑连接图的连接关系，相邻的两个固定基座冗余度机械臂之间交换信息。

具体的，多固定基座冗余度机械臂系统中的每个固定基座冗余度机械臂(共有n个)可以在工作空间中按需分布，在每个固定基座冗余度机械臂上设置有末端执行器。所述末端执行器各自实时地获得自身的末端执行器与移动的、动态的目标物的距离，且在固定基座冗余度机械臂的拓扑图中，相邻的固定基座冗余度机械臂之间相互交换信息。即第i个固定基座冗余度机械臂能够实时地获得其末端执行器与移动目标物之间的距离d_i，此外，第i个固定基座冗余度机械臂与在固定基座冗余度机械臂的拓扑图中，与其联通的其它固定基座冗余度机械臂之间通信，互相交换信息。

在步骤S102中，根据所述固定基座冗余度机械臂与目标物的距离，以及相邻的两个固定基座冗余度机械臂之间所交换的信息，确定与目标物最近的固定基座冗余度机械臂。

具体的，所述根据所述固定基座冗余度机械臂与目标物的距离，以及相邻的两个固定基座冗余度机械臂之间所交换的信息，确定与目标物最近的固定基座冗余度机械臂步骤包括：

通过公式：

以及

确定固定基座冗余度机械臂是否为与目标物最近的固定基座冗余度机械臂，其中：矩阵A∈R^n×n，其第ij个元素定义为第i个固定基座冗余度机械臂与第j个固定基座冗余度机械臂之间的连接权值，即当第i个与第j个固定基座冗余度机械臂在拓扑图上连通时，A_ij＝1，否则，A_ij＝0，且第i个固定基座冗余度机械臂与自己的连接权值为A_ii＝1，s_i对应于第i个固定基座冗余度机械臂的启动状态，当s_i为0时，第i个固定基座冗余度机械臂不启动，否则启动，s_i的初始值

随机给定，η_i为辅助变量，其初始值随机给定，u_i＝z-d_i，z为正数，d_i为第i个固定基座冗余度机械臂实时获得的其末端执行器与移动目标物之间的距离，j∈N(i)表示与第i个固定基座冗余度机械臂连接权值为1的固定基座冗余度机械臂编号，γ与λ为控制算法收敛速度的正参数，τ∈R为采样间隔，c₀∈R与c₂∈R为正常数，上标k表示第k次迭代。经过一定的迭代次数，s_i的状态为一正数或0。

另外，作为本发明一种优选的实施方式，为了躲避机械臂奇异，当第i个固定基座冗余度机械臂中，除第一个关节角外的其余关节角度的绝对值的和小于预先设定数值σ时，

其中

为正数。

在步骤S103中，启动所述与目标物的距离最近的固定基座冗余度机械臂的末端执行器，对目标物进行追踪。

在获取到距离目标物最近的固定基座冗余度机械臂后，所述启动所述与目标物的距离最近的固定基座冗余度机械臂的末端执行器，对目标物进行追踪的步骤可以包括：

根据迭代后的固定基座冗余度机械臂的启动状态，结合公式：

确定第i个固定基座冗余度机械臂所获得的启动状态，其中：

表示在第kτ时刻时，第i个固定基座冗余度机械臂移动平台和机械臂的联合角度向量，

表示在第kτ时刻时，第i个固定基座冗余度机械臂移动平台和机械臂的联合速度向量，上标^T表示矩阵和向量的转置，

表示在第kτ时刻时，第i个固定基座冗余度机械臂雅可比矩阵的转置，

表示在第kτ时刻时，第i个固定基座冗余度机械臂末端执行器的位置向量，

表示在第kτ时刻时，移动目标物的位置向量，α代表速度反馈正系数。

容易看出：当s^k _i为正数时，则第i个固定基座冗余度机械臂末端执行器保持在启动状态朝移动目标物移动追踪；当s^k _i＝0时，则第i个固定基座冗余度机械臂保持在静止状态。

图2为本发明实施例提供的固定基座冗余度机械臂的控制示意图，在图2中，该机械由六个连杆所组成，通过关节9、关节10、关节11、关节12、关节13和关节14组成。在本发明专利示例中，该机械臂关节9-14的初始角度设置为[π/12,π/12,π/12,π/12,π/12,π/12]^T弧度。

图2所示：拓扑图中的通信结构采用邻对邻通信方式，即定义一个矩阵A∈R^n×n，其第ij个元素定义为第i个固定基座冗余度机械臂与第j个固定基座冗余度机械臂之间的连接权值，当两个固定基座冗余度机械臂的编号满足|i-j|≤1时，A_ij＝1，否则，A_ij＝0，第i个固定基座冗余度机械臂与自己的连接权值为A_ii＝1，第1个固定基座冗余度机械臂只与第2个固定基座冗余度机械臂交换信息，第n个固定基座冗余度机械臂只第n-1个固定基座冗余度机械臂交换信息。通过如下迭代，第i个固定基座冗余度机械臂自行判定是否需要启动：

其中，s_i对应于第i个固定基座冗余度机械臂的启动状态，即当其为0时，第i个固定基座冗余度机械臂不启动，否则启动，s_i的初始值

随机给定；η_i为辅助变量，其初始值随机给定；u_i＝z-d_i，z＝10000，d_i为第i个固定基座冗余度机械臂实时获得的其末端执行器与移动目标物之间的距离；j∈N(i)表示与第i个固定基座冗余度机械臂连接权值为1的固定基座冗余度机械臂编号；γ与λ为控制算法收敛速度的正参数，其越大则代表收敛速度越快；τ＝0.001秒为采样间隔；c₀＝c₂＝10；上标k表示第k次迭代。当某个固定基座冗余度机械臂除第一个关节角外的其余关节角度的绝对值的和小于设定数值σ＝0.01时，为躲避机械臂奇异，令其对应的

其中

考虑到是每个固定基座冗余度机械臂需要接入是否启动的信号，所以对于第i个固定基座冗余度机械臂所获得的s_i在每次迭代后应用到其的启动中：

其中，α＝1000。当s^k _i为正数时，则第i个固定基座冗余度机械臂的末端执行器保持在启动状态朝移动目标物移动追踪；当s^k _i＝0时，则第i个固定基座冗余度机械臂保持在静止状态。

图3为本发明实施例提供的多固定基座冗余度机械臂的任务动态追踪装置的结构示意图，包括：

信息获取单元301，用于获取多固定基座冗余度机械臂系统中的固定基座冗余度机械臂与目标物的距离，并根据固定基座冗余度机械臂的拓扑连接图的连接关系，相邻的两个固定基座冗余度机械臂之间交换信息；

确定单元302，用于根据所述固定基座冗余度机械臂与目标物的距离，以及相邻的两个固定基座冗余度机械臂之间所交换的信息，确定与目标物最近的固定基座冗余度机械臂；

追踪单元303，用于启动所述与目标物的距离最近的固定基座冗余度机械臂的末端执行器，对目标物进行追踪。

优选的，所述确定单元具体用于：

通过公式：

以及

确定固定基座冗余度机械臂是否为与目标物最近的固定基座冗余度机械臂，其中：矩阵A∈R^n×n，其第ij个元素定义为第i个固定基座冗余度机械臂与第j个固定基座冗余度机械臂之间的连接权值，即当第i个与第j个固定基座冗余度机械臂在拓扑图上连通时，A_ij＝1，否则，A_ij＝0，且第i个固定基座冗余度机械臂与自己的连接权值为A_ii＝1，s_i对应于第i个固定基座冗余度机械臂的启动状态，当s_i为0时，第i个固定基座冗余度机械臂不启动，否则启动，s_i的初始值s_i ^k＝0＝s_i ⁰随机给定，η_i为辅助变量，其初始值随机给定，u_i＝z-d_i，z为正数，d_i为第i个固定基座冗余度机械臂实时获得的其末端执行器与移动目标物之间的距离，j∈N(i)表示与第i个固定基座冗余度机械臂连接权值为1的固定基座冗余度机械臂编号，γ与λ为控制算法收敛速度的正参数，τ∈R为采样间隔，c₀∈R与c₂∈R为正常数，上标k表示第k次迭代。

优选的，当第i个固定基座冗余度机械臂中，除第一个关节角外的其余关节角度的绝对值的和小于预先设定数值σ时，

其中

为正数。

优选的，所述追踪单元具体用于：

根据迭代后的固定基座冗余度机械臂的启动状态，结合公式：

确定第i个固定基座冗余度机械臂所获得的启动状态，其中：

表示在第kτ时刻时，第i个固定基座冗余度机械臂移动平台和机械臂的联合角度向量，

表示在第kτ时刻时，第i个固定基座冗余度机械臂移动平台和机械臂的联合速度向量，上标^T表示矩阵和向量的转置，

表示在第kτ时刻时，第i个固定基座冗余度机械臂雅可比矩阵的转置，

表示在第kτ时刻时，第i个固定基座冗余度机械臂末端执行器的位置向量，

表示在第kτ时刻时，移动目标物的位置向量，α代表速度反馈正系数。

优选的，所述固定基座冗余度机械臂为连杆机械臂，所述确定单元具体用于：

按序对所述连杆机械臂的固定基座冗余度机械臂进行编号，在编号相连的两个固定基座冗余度机械臂之间交换信息。

图3所述多固定基座冗余度机械臂的任务动态追踪装置，与图1所述多固定基座冗余度机械臂的任务动态追踪方法对应。

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

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

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

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

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种多固定基座冗余度机械臂的任务动态追踪方法，其特征在于，所述方法包括：

获取多固定基座冗余度机械臂系统中的固定基座冗余度机械臂与目标物的距离，并根据固定基座冗余度机械臂的拓扑连接图的连接关系，相邻的两个固定基座冗余度机械臂之间交换信息；

根据所述固定基座冗余度机械臂与目标物的距离，以及相邻的两个固定基座冗余度机械臂之间所交换的信息，确定与目标物最近的固定基座冗余度机械臂；

启动所述与目标物的距离最近的固定基座冗余度机械臂的末端执行器，对目标物进行追踪；

所述根据所述固定基座冗余度机械臂与目标物的距离，以及相邻的两个固定基座冗余度机械臂之间所交换的信息，确定与目标物最近的固定基座冗余度机械臂步骤包括：

通过公式：

以及

确定固定基座冗余度机械臂是否为与目标物最近的固定基座冗余度机械臂，其中：矩阵A∈R^n×n，其第ij个元素定义为第i个固定基座冗余度机械臂与第j个固定基座冗余度机械臂之间的连接权值，即当第i个与第j个固定基座冗余度机械臂在拓扑图上连通时，A_ij＝1，否则，A_ij＝0，且第i个固定基座冗余度机械臂与自己的连接权值为A_ii＝1，s_i对应于第i个固定基座冗余度机械臂的启动状态，当s_i为0时，第i个固定基座冗余度机械臂不启动，否则启动，s_i的初始值s_i ^k＝0＝s_i ⁰随机给定，η_i为辅助变量，其初始值随机给定，u_i＝z-d_i，z为正数，d_i为第i个固定基座冗余度机械臂实时获得的其末端执行器与移动目标物之间的距离，j∈N(i)表示与第i个固定基座冗余度机械臂连接权值为1的固定基座冗余度机械臂编号，γ与λ为控制算法收敛速度的正参数，τ∈R为采样间隔，c₀∈R与c₂∈R为正常数，上标k表示第k次迭代。

2.根据权利要求1所述方法，其特征在于，当第i个固定基座冗余度机械臂中，除第一个关节角外的其余关节角度的绝对值的和小于预先设定数值σ时，

其中

为正数。

3.根据权利要求1或2所述方法，其特征在于，所述启动所述与目标物的距离最近的固定基座冗余度机械臂的末端执行器，对目标物进行追踪的步骤包括：

根据迭代后的固定基座冗余度机械臂的启动状态，结合公式：

确定第i个固定基座冗余度机械臂所获得的启动状态，其中：

表示在第kτ时刻时，第i个固定基座冗余度机械臂移动平台和机械臂的联合角度向量，

表示在第kτ时刻时，第i个固定基座冗余度机械臂移动平台和机械臂的联合速度向量，上标^T表示矩阵和向量的转置，

表示在第kτ时刻时，第i个固定基座冗余度机械臂雅可比矩阵的转置，

表示在第kτ时刻时，第i个固定基座冗余度机械臂末端执行器的位置向量，

表示在第kτ时刻时，移动目标物的位置向量，α代表速度反馈正系数。

4.根据权利要求1所述方法，其特征在于，所述固定基座冗余度机械臂为连杆机械臂，所述根据固定基座冗余度机械臂的拓扑连接图的连接关系，相邻的两个固定基座冗余度机械臂之间交换信息步骤具体为：

按序对所述连杆机械臂的固定基座冗余度机械臂进行编号，在编号相连的两个固定基座冗余度机械臂之间交换信息。

5.一种多固定基座冗余度机械臂的任务动态追踪装置，其特征在于，所述装置包括：

信息获取单元，用于获取多固定基座冗余度机械臂系统中的固定基座冗余度机械臂与目标物的距离，并根据固定基座冗余度机械臂的拓扑连接图的连接关系，相邻的两个固定基座冗余度机械臂之间交换信息；

确定单元，用于根据所述固定基座冗余度机械臂与目标物的距离，以及相邻的两个固定基座冗余度机械臂之间所交换的信息，确定与目标物最近的固定基座冗余度机械臂；

追踪单元，用于启动所述与目标物的距离最近的固定基座冗余度机械臂的末端执行器，对目标物进行追踪；

所述确定单元具体用于：

通过公式：

以及

确定固定基座冗余度机械臂是否为与目标物最近的固定基座冗余度机械臂，其中：矩阵A∈R^n×n，其第ij个元素定义为第i个固定基座冗余度机械臂与第j个固定基座冗余度机械臂之间的连接权值，即当第i个与第j个固定基座冗余度机械臂在拓扑图上连通时，A_ij＝1，否则，A_ij＝0，且第i个固定基座冗余度机械臂与自己的连接权值为A_ii＝1，s_i对应于第i个固定基座冗余度机械臂的启动状态，当s_i为0时，第i个固定基座冗余度机械臂不启动，否则启动，s_i的初始值s_i ^k＝0＝s_i ⁰随机给定，η_i为辅助变量，其初始值随机给定，u_i＝z-d_i，z为正数，d_i为第i个固定基座冗余度机械臂实时获得的其末端执行器与移动目标物之间的距离，j∈N(i)表示与第i个固定基座冗余度机械臂连接权值为1的固定基座冗余度机械臂编号，γ与λ为控制算法收敛速度的正参数，τ∈R为采样间隔，c₀∈R与c₂∈R为正常数，上标k表示第k次迭代。

6.根据权利要求5所述装置，其特征在于，当第i个固定基座冗余度机械臂中，除第一个关节角外的其余关节角度的绝对值的和小于预先设定数值σ时，

其中

为正数。

7.根据权利要求5或6所述装置，其特征在于，所述追踪单元具体用于：

根据迭代后的固定基座冗余度机械臂的启动状态，结合公式：

确定第i个固定基座冗余度机械臂所获得的启动状态，其中：

表示在第kτ时刻时，第i个固定基座冗余度机械臂移动平台和机械臂的联合角度向量，

表示在第kτ时刻时，第i个固定基座冗余度机械臂移动平台和机械臂的联合速度向量，上标^T表示矩阵和向量的转置，

表示在第kτ时刻时，第i个固定基座冗余度机械臂雅可比矩阵的转置，

表示在第kτ时刻时，第i个固定基座冗余度机械臂末端执行器的位置向量，

表示在第kτ时刻时，移动目标物的位置向量，α代表速度反馈正系数。

8.根据权利要求5所述装置，其特征在于，所述固定基座冗余度机械臂为连杆机械臂，所述确定单元具体用于：

按序对所述连杆机械臂的固定基座冗余度机械臂进行编号，在编号相连的两个固定基座冗余度机械臂之间交换信息。

Images