CN108000512B - 一种移动机械臂系统及围捕控制方法 - Google Patents

Abstract

本发明提供了一种移动机械臂系统及围捕控制方法，所述方法包括：各所述移动机械臂实时获取各自所述末端执行器与被追捕物之间的距离并与编号相邻的所述移动机械臂交换信息，以通过迭代估计算法确定各自是否属于所述移动机械臂系统中设定数量的最接近被追捕物的移动机械臂之一，其中，所述设定数量大于或等于1且小于或等于所述移动机械臂的总数；所述设定数量的移动机械臂根据设定运行模式向所述被追捕物前进，以完成围捕任务。本发明中，各移动机械臂基于距离竞争的关系通过局部信息判断得出全局的信息，从而最终通过设定数量的移动机械臂根据设定运行模式向被追捕物前进，以完成围捕任务。

Description

一种移动机械臂系统及围捕控制方法

技术领域

本发明属于机器人技术领域，尤其涉及一种移动机械臂系统及围捕控制方法。

背景技术

移动机械臂由于具有可移动性，可以代替人类到各种环境中执行探险、探测和巡查等任务。经过多年的研究与发展，移动机械臂已经逐步走向实用化，在制造业、物流业、军事等多个领域，已经有了不少利用移动机械臂来提高生产效率或者替代人工作业的实例。在一些诸如国防等具有重大战略意义的项目中，移动机械臂的需求也日益明显。随着科技的进步，包括多个移动机械臂的移动机械臂系统在月球探测、深海开发、战争、反恐、地震救助、高危品处理等领域逐渐发挥出巨大的作用，并显现出广泛的应用前景。

然而，在广阔的环境中，移动机械臂系统中各个移动机械臂因距离以及随身可携带负载的限制，通常不具有全局通信的能力，也即，移动机械臂系统中每个移动机械臂只能与数量有限的移动机械臂通信。故，在大规模环境下进行围捕任务时，各移动机械臂之间可能由于通信受限的原因，导致信息不能随时随地进行共享，从而无法完成围捕任务。

发明内容

本发明的目的在于提供一种移动机械臂系统及围捕控制方法，以解决如何使移动机械臂系统在大规模环境下完成围捕任务的问题。

一种移动机械臂系统围捕控制方法，所述移动机械臂系统包括多个移动机械臂，且各所述移动机械臂均包括末端执行器；所述方法包括：

各所述移动机械臂实时获取各自所述末端执行器与被追捕物之间的距离并与编号相邻的所述移动机械臂交换信息，以通过迭代估计算法确定各自是否属于所述移动机械臂系统中设定数量的最接近被追捕物的移动机械臂之一；其中，所述设定数量大于或等于1且小于或等于所述移动机械臂的总数；

所述设定数量的移动机械臂根据设定运行模式向所述被追捕物前进，以完成围捕任务。

在其中一个实施例中，各所述移动机械臂实时获取各自所述末端执行器与被追捕物之间的距离并与编号相邻的所述移动机械臂交换信息，以通过迭代估计算法确定各自是否属于所述移动机械臂系统中设定数量的最接近被追捕物的移动机械臂之一的步骤包括：

获取所述末端执行器与被追捕物之间实时的距离；

与编号相邻的所述移动机械臂交换相应的状态信息；

根据所述距离、编号相邻的所述移动机械臂发送的状态信息及迭代估计算法进行迭代运算，并得出迭代结果；

根据所述迭代结果判断是否需要进入启动状态，若是，则切换至启动状态并按照所述设定运行模式向所述追捕物前进，否则，切换至静止状态；

每隔设定采样间隔，均重复执行上述步骤，依次循环，直至判断所述移动机械臂系统中设定数量的最接近被追捕物的移动机械臂均进入启动状态为止。

在其中一个实施例中，所述状态信息包括编号及与所述迭代估计算法相关的信息。

在其中一个实施例中，各所述移动机械臂均存有用于记录相邻编号的相邻编号列表；并且，与编号相邻的所述移动机械臂交换相应的状态信息的步骤包括：

向其他所述移动机械臂发送自己对应的状态信息；

接收其他移动机械臂发送的状态信息；

根据所述相邻编号列表从接收的所有所述状态信息中筛选出编号相邻的所述移动机械臂发送的状态信息。

在其中一个实施例中，所述迭代估计算法为：

其中，k为迭代次数；i为所述移动机械臂的编号；j∈N(i)为与第i个移动机械臂编号相邻的移动机械臂编号；η、z均为辅助变量，其初始值随机设定；A_ij为第i个移动机械臂与第j个移动机械臂之间的连接权值，且当两个移动机械臂的编号满足|i-j|≤1时，A_ij＝1，否则，A_ij＝0；c₀∈R为正的常数；τ∈R为所述设定采样间隔；γ为用于控制收敛速度的正参数；n为所述移动机械臂系统中所有移动机械臂的总数量；m为所述设定数量；s_i ^k+1为第k+1次迭代结果；P_Ω[u]为空间R到集合Ω＝{u|0≤u≤1}∈R的分段线性投影算子，且当u>1时，P_Ω[u]＝1，当u<0时，P_Ω[u]＝0，当u为其余数值时，P_Ω[u]＝u；d_i为每次进行迭代运算时获取的第i个移动机械臂的所述末端执行器与所述被追捕物之间的距离；b∈R为设计参数。

并且，根据所述迭代结果判断是否需要进入启动状态，若是，则切换至启动状态并按照所述设定运行模式向所述追捕物前进，否则，切换至静止状态的步骤为：

根据所述s_i ^k+1的值判断是否需要进入启动状态，若s_i ^k+1等于1，则切换至启动状态并按照所述设定运行模式向所述追捕物前进，若s_i ^k+1等于0，则切换至静止状态。

在其中一个实施例中，所述设定运行模式为沿距离范数的负梯度方向前进；其中，所述距离范数指空间上所述被追捕物的位置向量与所述移动机械臂中所述末端执行器的位置向量之间的直线距离。

在其中一个实施例中，各所述移动机械臂均包括移动平台和机械臂；所述机械臂的一端安装于所述移动平台上，所述机械臂的另一端设有所述末端执行器；所述移动平台底部设有驱动轮；并且，所述设定运行模式为：

其中，k为迭代次数；i为所述移动机械臂的编号；

表示第kτ时刻时，第i个移动机械臂的所述驱动轮和所述机械臂的联合角度向量；

表示第kτ时刻时，第i个移动机械臂的所述驱动轮和所述机械臂的联合速度向量；上标^T表示矩阵和向量的转置；s_i ^k为第k次迭代结果；c₂为速度反馈正系数；

表示第kτ时刻时，第i个移动机械臂的所述末端执行器速度向量和所述联合速度向量之间的连接矩阵；

表示第kτ时刻时，所述被追捕物的位置向量；

表示第kτ时刻时，第i个移动机械臂的所述末端执行器的位置向量。

在其中一个实施例中，所述驱动轮包括半径相同的左轮和右轮；并且，在所述设定运行模式中，所述

分别为：

其中，

在上述公式中，φ为所述移动平台的航向角；x_c、y_c分别为所述移动平台与所述机械臂的连接点所在坐标系中的横坐标、纵坐标，且所述坐标系所在平面与所述移动平台的表面平行；f(θ)为所述机械臂的前向运动学方程；θ为所述机械臂上所有关节的角度构成的联合向量；r表示所述左轮或所述右轮的半径；d表示所述左轮中心与所述右轮中心构成线段的中点到所述移动平台与所述机械臂的连接点的距离；b表示所述左轮或所述右轮与所述中点之间的距离；I为单位矩阵。

在其中一个实施例中，所述机械臂包括6个关节及6根连杆；各所述关节通过所述连杆连接在一起；并且，所述f(θ)的计算方式为：

其中，l_u表示第u根连杆的长度，且u＝1,2,3,4,5,6；c_v＝cos(θ_v)，s_v＝sin(θ_v)，s₃₂＝sin(θ₃+θ₂)；c₃₂＝cos(θ₃+θ₂)，且θ_v表示第v个关节的角度，v＝1,2,3,4,5,6；h为离所述移动平台最近的所述关节与所述移动平台底部之间的距离。

一种移动机械臂系统，且所述移动机械臂系统用于执行所述移动机械臂系统围捕控制方法。

在本发明提供的移动机械臂系统及围捕控制方法中，各所述移动机械臂实时获取各自所述末端执行器与被追捕物之间的距离并与编号相邻的所述移动机械臂交换信息，以通过迭代估计算法确定各自是否属于所述移动机械臂系统中设定数量的最接近被追捕物的移动机械臂之一，相当于各移动机械臂基于距离竞争的关系通过局部信息判断得出全局的信息，从而最终通过所述设定数量的移动机械臂根据设定运行模式向所述被追捕物前进，以完成围捕任务。因此，本发明提供的移动机械臂系统及围捕控制方法具有分布式处理的特点，对于通信受限的移动机械臂系统具有较好的控制效果，从而能够在大规模环境下完成围捕任务。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他实施例的附图。

图1是本发明一实施例提供的移动机械臂系统围捕控制方法的实现流程图；

图2是图1所示实施例的移动机械臂系统围捕控制方法中步骤S101的其中一种实现流程图；

图3是图2所示实施例的移动机械臂系统围捕控制方法中步骤S202的其中一种实现流程图；

图4是本发明一实施例提供的移动机械臂的结构示意图；

图5是图4所示实施例的移动机械臂中移动平台在坐标系下的结构示意图。

具体实施方式

为了便于理解本发明，下面将参照相关附图对本发明进行更全面的描述。附图中给出了本发明的较佳实施例。但是，本发明可以以许多不同的形式来实现，并不限于本文所描述的实施例。相反地，提供这些实施例的目的是使对本发明的公开内容的理解更加透彻全面。

除非另有定义，本文所使用的所有的技术和科学术语与属于发明的技术领域的技术人员通常理解的含义相同。本文中在发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在限制本发明。本文所使用的术语“和/或”包括一个或多个相关的所列项目的任意的和所有的组合。

图1示出了本发明一实施例提供的移动机械臂系统围捕控制方法。该移动机械臂系统围捕控制方法由移动机械臂系统执行，且移动机械臂系统包括多个移动机械臂。各移动机械臂均包括移动平台和机械臂，其既具有移动平台的运行性能又具有机械臂的执行性能。另外，各移动机械臂均包括末端执行器，且末端执行器是指用来执行各种操作和任务的器件。所述移动机械臂系统围捕控制方法的执行过程详述如下。

步骤S101、各所述移动机械臂实时获取各自所述末端执行器与被追捕物之间的距离并与编号相邻的所述移动机械臂交换信息，以通过迭代估计算法确定各自是否属于所述移动机械臂系统中设定数量的最接近被追捕物的移动机械臂之一。其中，所述设定数量大于或等于1且小于或等于所述移动机械臂的总数。

在本发明实施例中，各移动机械臂均对应一个唯一的编号，在执行步骤S101前，各移动机械臂在工作空间内随机分布。迭代估计算法属于迭代算法，即，各移动机械臂在执行步骤S101的过程中，均需经过多次迭代过程，从而准确判断出各自是否属于移动机械臂系统中设定数量的最接近被追捕物的移动机械臂之一。

实时获取各自所述末端执行器与被追捕物之间的距离并与编号相邻的所述移动机械臂交换信息，至少保证各移动机械臂在每执行一次迭代过程时，都能采用实时更新的所述距离及编号相邻的移动机械臂传来的信息。具体的，各移动机械臂可以利用雷达、红外传感器等测距传感器来获取各自末端执行器与被追捕物之间的距离。另外，各移动机械臂可以通过无线通信模块等与其他移动机械臂之间交换信息。关于编号相邻，示例的，假设移动机械臂系统共有n个移动机械臂，那么编号为1的移动机械臂只有一个编号相邻的移动机械臂(即编号为2的移动机械臂)；编号为n的移动机械臂同样只有一个编号相邻的移动机械臂(即编号为n-1的移动机械臂)；对于其他移动机械臂(假设编号为i)来说，与其编号相邻的移动机械臂均包括编号为i+1、i-1的两个移动机械臂。

因此，本发明实施例在整个移动机械臂系统中，各移动机械臂在运行迭代估计算法中只与编号相邻的移动机械臂之间交换信息，即只需要局部信息，最终即可使得所述移动机械臂系统筛选出设定数量的最接近被追捕物的移动机械臂，即基于距离竞争的关系得出全局信息。故，本发明实施例中的各移动机械臂基于距离竞争的关系通过局部信息从而得出全局信息。

步骤S102、所述设定数量的移动机械臂根据设定运行模式向所述被追捕物前进，以完成围捕任务。

在该步骤中，所述移动机械臂系统中除了所述设定数量之外的其他移动机械臂则保持静止状态，从而只需通过所述设定数量的离被追捕物最近的移动机械臂进行围捕操作。

因此，本发明实施例提供的上述移动机械臂系统围捕控制方法，各移动机械臂基于距离竞争的关系，通过局部信息判断得出全局的信息，从而最终利用所述设定数量的移动机械臂根据设定运行模式向所述被追捕物前进，以完成围捕任务。故，本发明提供的移动机械臂系统围捕控制方法具有分布式处理的特点，对于通信受限的移动机械臂系统具有较好的控制效果，从而能够在大规模环境下完成围捕任务。

在其中一个实施例中，步骤S101的实现流程如下，请参考图2。需要说明的是，图2中所有的步骤都是由同一个移动机械臂来执行的。

步骤S201、获取所述末端执行器与被追捕物之间实时的距离。

从步骤S201开始执行，则相当于进行一次迭代运算，故首先需要实时更新末端执行器与被追捕物之间的距离。

步骤S202、与编号相邻的所述移动机械臂交换相应的状态信息。

同理，每进行一次迭代运算，还需实时更新与编号相邻的移动机械臂交换的状态信息。可以理解的是，步骤S201与步骤S202之间的顺序不限于上述情况，例如还可以相反的顺序执行，或者在条件允许的前提下这两个步骤可以同时执行。

具体的，所述状态信息包括编号及与所述迭代估计算法相关的信息。其中，编号是指发送所述状态信息的移动机械臂的编号。因此，任一移动机械臂接收到其他移动机械臂发送的状态信息后，都能从该状态信息中知晓发送方的编号。

进一步的，各移动机械臂均存有用于记录相邻编号的相邻编号列表。那么，各移动机械臂将状态信息中的编号与所述相邻编号列表进行对照，即可得知所述状态信息的发送方是否是与自己编号相邻的移动机械臂。并且，步骤S202的其中一种实现流程如图3所示，包括以下内容。

步骤S301、向其他所述移动机械臂发送自己对应的状态信息。

步骤S302、接收其他移动机械臂发送的状态信息。

在该步骤中，接收的所有状态信息中也可能包括相邻编号之外的其他移动机械臂发送的状态信息。可以理解的是，步骤S301和步骤S302的执行顺序还可以为相反的顺序，或者同时执行。

步骤S303、根据所述相邻编号列表从接收的所有所述状态信息中筛选出编号相邻的所述移动机械臂发送的状态信息。

由于步骤S302中接收的所有状态信息中可能包括编号不相邻的移动机械臂发送的状态信息，因此，可以根据相邻编号列表来检验各状态信息包括的编号是否属于相邻编号，若是，则保留对应的状态信息；否则，剔除相应的状态信息，从而最终筛选出编号相邻的移动机械臂发送的状态信息。

可以理解的是，步骤S202的具体实现方式不限于上述情况，只要能够与编号相邻的所述移动机械臂交换相应的状态信息即可，例如：假设根据设定好的通讯方式能够确保各移动机械臂只能接收到编号相邻的移动机械臂发送的状态信息，这时状态信息的内容则无需包括编号，且步骤S303也可无需设置。

接下来请继续参考图2，步骤S101还包括以下内容。

步骤S203、根据所述距离、编号相邻的所述移动机械臂发送的状态信息及迭代估计算法进行迭代运算，并得出迭代结果。

在该步骤中，将所述距离、编号相邻的所述移动机械臂发送的状态信息代入迭代估计算法中，即可得出相应的迭代结果。其中，迭代估计算法能够使每个移动机械臂通过局部信息来自行判断从而得出全局信息，即判断自己是否属于设定数量的离被追捕物最近的移动机械臂之一。

在其中一个实施例中，所述迭代估计算法为：

其中，k为迭代次数。i为所述移动机械臂的编号。n为所述移动机械臂系统中所有移动机械臂的总数量。j∈N(i)为与第i个移动机械臂编号相邻的移动机械臂编号。A_ij为第i个移动机械臂与第j个移动机械臂之间的连接权值，且当两个移动机械臂的编号满足|i-j|≤1时，A_ij＝1，否则，A_ij＝0。本实施例中，定义一个矩阵A∈R^n×n，其第ij个元素定义为第i个移动机械臂与第j个移动机械臂之间的连接权值，即当两个移动机械臂的编号满足|i-j|≤1时，A_ij＝1，否则，A_ij＝0，那么，第i个移动机械臂与自己的连接权值为A_ii＝1，第1个移动机械臂、第n个移动机械臂则分别对应与第2个移动机械臂、第n-1个移动机械臂的连接权值为1。

另外，η、z均为辅助变量，其初始值随机设定，即η_i ⁰、z_i ⁰随机设定。其中，η_i为第i个移动机械臂对应的辅助变量值；η_j为与第i个移动机械臂编号相邻的第j个移动机械臂对应的辅助变量值。c₀∈R为正的常数。τ∈R为设定采样间隔，在实际应用中尽可能地小。γ为用于控制收敛速度的正参数，其越大则收敛速度越快。m为所述设定数量。s_i ^k+1为第i个移动机械臂得出第k+1次迭代结果，s_i ^k+1的初始值s_i ^k＝0＝s_i ⁰随机给定即可，值得指出的是，经过一定的迭代次数，s_i ^k+1的值为1或0。其中，P_Ω[u]为空间R到集合Ω＝{u|0≤u≤1}∈R的分段线性投影算子，且当u>1时，P_Ω[u]＝1，当u<0时，P_Ω[u]＝0，当u为其余数值时，P_Ω[u]＝u。d_i为第i个移动机械臂每次进行迭代运算时获取的自身的所述末端执行器与所述被追捕物之间的距离，即步骤S201中获取的距离。b∈R为设计参数，在实际应用中尽可能地小，如b＝0.01。

基于上述迭代估计算法，步骤S202中获取的编号相邻的移动机械臂的状态信息包括：η_j ^k、s_j ^k，即与迭代估计算法相关的信息。因此，上述迭代估计算法只需根据局部信息(即d_i ^k+1、η_j ^k、s_j ^k)即可得出全局信息(即s_i ^k+1)。

步骤S204、根据所述迭代结果判断是否需要进入启动状态，若是，则切换至启动状态并按照所述设定运行模式向所述追捕物前进，否则，切换至静止状态。

其中，第k次的迭代结果为s_j ^k。启动状态，是指开始向被追捕物前进。具体的，步骤S204为：根据所述s_i ^k+1的值判断是否需要进入启动状态，若s_i ^k+1等于1，则切换至启动状态并按照所述设定运行模式向所述追捕物前进，若s_i ^k+1等于0，则切换至静止状态。

步骤S205、每隔设定采样间隔，均重复执行上述步骤，依次循环，直至判断所述移动机械臂系统中设定数量的最接近被追捕物的移动机械臂均进入启动状态为止。

需要指出的是，每进行一次迭代过程，都会有相对应的移动机械臂进入启动状态并按照设定运行模式向所述追捕物前进，而在进行第一次迭代或者前几次迭代运算后，进入启动状态的各移动机械臂不一定属于设定数量的最接近被追捕物的移动机械臂。但是只要经过一定数量的迭代运算后，公式(2)中的z_i ^k+1就会收敛到一个合适的数值，使得距离被追捕物最近的设定数量(即m)个移动机械臂的距离d_i ^k+1代入公式(3)之后

的值大于1，而使得其余移动机械臂的距离d_i ^k+1代入公式(3)之后

的值小于0，即使得s_i ^k+1的值只有0或者1，同时也准确筛选出了设定数量的离被追捕物最近的移动机械臂，且使得这些移动机械臂均保持在启动状态并按照设定运行模式向被追捕物前进。

其中，判断所述移动机械臂系统中设定数量的最接近被追捕物的移动机械臂均是否均进入启动状态的判断条件可以为：设定合适的迭代总次数，且所述迭代总次数能够保证进行完所有的迭代运算后使得设定数量的离被追捕物最近的移动机械臂均处于启动状态。那么，各移动机械臂只要判断执行迭代的次数达到了所述迭代总次数后，则停止进行迭代运算。

在其中一个实施例中，所述设定运行模式为沿距离范数的负梯度方向前进。其中，所述距离范数指空间上所述被追捕物的位置向量与所述移动机械臂中所述末端执行器的位置向量之间的直线距离。

换言之，进入启动状态的移动机械臂沿距离范数的负梯度方向前进。距离范数相当于所述被追捕物的位置向量与所述末端执行器的位置向量之间的2-范数。并且，对于各进入启动状态的移动机械臂来说，各自对应的距离范数指空间上所述被追捕物的位置向量与各自末端执行器的位置向量之间的直线距离。另外，负梯度方向是指下降最快的方向。因此，移动机械臂沿所述距离范数的负梯度方向前进时，使得自己与被追捕物之间的距离缩短得最快，从而以最短的时间靠近被追捕物。

在其中一个实施例中，请参考图4，各所述移动机械臂均包括移动平台和机械臂。所述机械臂的一端安装于所述移动平台上，所述机械臂的另一端设有所述末端执行器。所述移动平台底部设有驱动轮。并且，所述设定运行模式为：

其中，k为迭代次数。i为所述移动机械臂的编号。

表示第kτ时刻时，第i个移动机械臂的所述驱动轮和所述机械臂的联合角度向量，换言之，

表示第kτ时刻时，第i个移动机械臂的所述驱动轮的角度和所述机械臂的角度构成的联合向量。

表示第kτ时刻时，第i个移动机械臂的所述驱动轮和所述机械臂的联合速度向量，且

为

的时间导数。上标^T表示矩阵和向量的转置。s_i ^k为第k次迭代结果，具体的，

表示第i个移动机械臂进行第k次迭代后得到的迭代结果。c₂为速度反馈正系数，且越大越好。

表示第kτ时刻时，第i个移动机械臂的所述末端执行器速度向量和所述联合速度向量之间的连接矩阵。

表示第kτ时刻时，所述被追捕物的位置向量。并且，

在每次迭代过程运算时实时测量。

表示第kτ时刻时，第i个移动机械臂的末端执行器的位置向量。

在本发明实施例中，公式(4)是基于所述距离范数并利用负梯度法得出的，从而使得处于启动状态的移动机械臂以所述距离范数的负梯度方向前进。其中，若迭代结果

等于0，将

代入公式(4)后，相当于第i个移动机械臂在进行完第k次迭代后切换至静止状态；若迭代结果

等于1，将

代入公式(4)后，相当于第i个移动机械臂在进行完第k次迭代后进入启动状态并向被追捕物以联合速度向量

前进。

具体的，请继续参考图4，并参考图5，所述驱动轮包括半径相同的左轮和右轮。并且，在所述设定运行模式中，所述

分别为：

其中，

在本发明实施例中，φ为所述移动平台的航向角。x_c、y_c分别为所述移动平台与所述机械臂的连接点所在坐标系中的横坐标、纵坐标，且所述坐标系所在平面与所述移动平台的表面平行。f(θ)为所述机械臂的前向运动学方程。θ为所述机械臂上所有关节的角度构成的联合向量。r表示所述左轮或所述右轮的半径其，例如为0.1025米。d表示所述左轮中心与所述右轮中心构成线段的中点到所述移动平台与所述机械臂的连接点的距离，例如为0.1米。b表示所述左轮或所述右轮与所述中点之间的距离，例如为0.32米。I为单位矩阵。

可以理解的是，

与

的计算方式不限于上述情况，例如若移动平台的结构发生变化时，则

与

发生相应的变化。

具体的，所述机械臂包括6个关节(即关节1至关节6)及6根连杆。各所述关节通过所述连杆连接在一起。并且公式(5)中的f(θ)的计算公式为：

其中，l_u表示第u根连杆的长度，且u＝1,2,3,4,5,6。本实施例中，各连杆的编号与关节的编号顺序相同。例如，所有连杆的长度为：l＝[0.065,0.555,0.19,0.13,0.082,0.018]^T。c_v＝cos(θ_v)，s_v＝sin(θ_v)，s₃₂＝sin(θ₃+θ₂)；c₃₂＝cos(θ₃+θ₂)，且θ_v表示第v个关节的角度，v＝1,2,3,4,5,6。本实施例中，各关节的角度以第(v-1)个关节为参考对象，且该机械臂的关节1至关节6的初始角度设置为[0,π/3,π/6,π,π/2,0]^T弧度。左轮和右轮的初始角度设置为[0,0]^T，那么整个移动机械臂的初始角度向量为：q(0)＝[0,0,0,π/3,π/6,π,π/2,0]^T。h为离所述移动平台最近的所述关节(即关节1)与所述移动平台底部之间的距离，例如：h＝0.698米。

可以理解的是，f(θ)的具体计算方式不限于上述情况，例如若机械臂的关节数量发生变化，则f(θ)相应发生变化。

接下来基于图4所示的移动机械臂的结构，将阐述关于上述公式(4)的推导原理。

首先，可以得出移动机械臂的运动学方程f(θ)：

其次，通过对移动平台的结构分析，可以得出二轮驱动的移动平台的运动学方程为：

和

并且，

其中，

和

分别表示移动平台的左轮、右轮的旋转速度，其初始值均可以设置为零。

表示移动平台航向角的速度(初始速度也可以设为零)。

表示移动平台和机械臂的连接点的速度，并且

分别表示所述连接点的横坐标的速度、纵坐标的速度。p_C＝[x_C,y_C]^T表示移动平台和机械臂的连接点的位置，其高度z_C设为零。r表示所述左轮或所述右轮的半径，例如为0.1025米。d表示所述左轮中心与所述右轮中心构成线段的中点到所述移动平台与所述机械臂的连接点的距离，例如为0.1米。b表示所述左轮或所述右轮与所述中点之间的距离，例如为0.32米。

接下来，根据上述移动机械臂的运动学方程和移动平台的运动学方程，可以得出整个移动机械臂的前向运动学方程：

其中，r_w表示末端执行器的位置。z(·)表示移动机械臂集成的前向运动学映射函数。

最后，定义

再利用负梯度法，即可得出所述设定运行模式：

即，

其中，

基于图4所示的移动机械臂的具体结构，所述移动机械臂系统围捕控制方法在对被追捕物进行围捕时，设定数量m可以为3，整个移动机械臂系统中移动机械臂的总体数量n为20，那么，在执行步骤S102时，每时刻共有3个移动机械臂处于启动状态并执行围捕操作。

在另一实施例中，还提供了一种移动机械臂系统，且该移动机械臂系统用于执行上述各实施例所述的移动机械臂系统围捕控制方法。

需要说明的是，图1至图3为本发明实施例的方法的流程示意图。应该理解的是，虽然图1至图3的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，其可以以其他的顺序执行。而且，图1至图3中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，其执行顺序也不必然是依次进行，而是可以与其他步骤或者其他步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。

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

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

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

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

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

Claims (9)

1.一种移动机械臂系统围捕控制方法，所述移动机械臂系统包括多个移动机械臂，且各所述移动机械臂均包括末端执行器；其特征在于，所述方法包括：

各所述移动机械臂实时获取各自所述末端执行器与被追捕物之间的距离并与编号相邻的所述移动机械臂交换信息，以通过迭代估计算法确定各自是否属于所述移动机械臂系统中设定数量的最接近被追捕物的移动机械臂之一；其中，所述设定数量大于或等于1且小于或等于所述移动机械臂的总数；

所述设定数量的移动机械臂根据设定运行模式向所述被追捕物前进，以完成围捕任务；

各所述移动机械臂实时获取各自所述末端执行器与被追捕物之间的距离并与编号相邻的所述移动机械臂交换信息，以通过迭代估计算法确定各自是否属于所述移动机械臂系统中设定数量的最接近被追捕物的移动机械臂之一的步骤包括：

获取所述末端执行器与被追捕物之间实时的距离；

与编号相邻的所述移动机械臂交换相应的状态信息；

根据所述距离、编号相邻的所述移动机械臂发送的状态信息及迭代估计算法进行迭代运算，并得出迭代结果；

根据所述迭代结果判断是否需要进入启动状态，若是，则切换至启动状态并按照所述设定运行模式向所述追捕物前进，否则，切换至静止状态；

每隔设定采样间隔，均重复执行上述步骤，依次循环，直至判断所述移动机械臂系统中设定数量的最接近被追捕物的移动机械臂均进入启动状态为止。

2.根据权利要求1所述的移动机械臂系统围捕控制方法，其特征在于，所述状态信息包括编号及与所述迭代估计算法相关的信息。

3.根据权利要求2所述的移动机械臂系统围捕控制方法，其特征在于，各所述移动机械臂均存有用于记录相邻编号的相邻编号列表；并且，与编号相邻的所述移动机械臂交换相应的状态信息的步骤包括：

向其他所述移动机械臂发送自己对应的状态信息；

接收其他移动机械臂发送的状态信息；

根据所述相邻编号列表从接收的所有所述状态信息中筛选出编号相邻的所述移动机械臂发送的状态信息。

4.根据权利要求1所述的移动机械臂系统围捕控制方法，其特征在于，所述迭代估计算法为：

z_i ^k+1＝-γ(nη_i ^k+1-m)τ+z_i ^k，

其中，k为迭代次数；i为所述移动机械臂的编号；j∈N(i)为与第i个移动机械臂编号相邻的移动机械臂编号；η、z均为辅助变量，其初始值随机设定；A_ij为第i个移动机械臂与第j个移动机械臂之间的连接权值，且当两个移动机械臂的编号满足|i-j|≤1时，A_ij＝1，否则，A_ij＝0；c₀∈R为正的常数；τ∈R为所述设定采样间隔；γ为用于控制收敛速度的正参数；n为所述移动机械臂系统中所有移动机械臂的总数量；m为所述设定数量；s_i ^k+1为第k+1次迭代结果；P_Ω[u]为空间R到集合Ω＝{u|0≤u≤1}∈R的分段线性投影算子，且当u>1时，P_Ω[u]＝1，当u<0时，P_Ω[u]＝0，当u为其余数值时，P_Ω[u]＝u；d_i为每次进行迭代运算时获取的第i个移动机械臂的所述末端执行器与所述被追捕物之间的距离；b∈R为设计参数；

并且，根据所述迭代结果判断是否需要进入启动状态，若是，则切换至启动状态并按照所述设定运行模式向所述追捕物前进，否则，切换至静止状态的步骤为：

根据所述s_i ^k+1的值判断是否需要进入启动状态，若s_i ^k+1等于1，则切换至启动状态并按照所述设定运行模式向所述追捕物前进，若s_i ^k+1等于0，则切换至静止状态。

5.根据权利要求1所述的移动机械臂系统围捕控制方法，其特征在于，所述设定运行模式为沿距离范数的负梯度方向前进；其中，所述距离范数指空间上所述被追捕物的位置向量与所述移动机械臂中所述末端执行器的位置向量之间的直线距离。

6.根据权利要求4所述的移动机械臂系统围捕控制方法，其特征在于，各所述移动机械臂均包括移动平台和机械臂；所述机械臂的一端安装于所述移动平台上，所述机械臂的另一端设有所述末端执行器；所述移动平台底部设有驱动轮；并且，所述设定运行模式为：

其中，k为迭代次数；i为所述移动机械臂的编号；

表示第kτ时刻时，第i个移动机械臂的所述驱动轮和所述机械臂的联合角度向量；

表示第kτ时刻时，第i个移动机械臂的所述驱动轮和所述机械臂的联合速度向量；上标^T表示矩阵和向量的转置；s_i ^k为第k次迭代结果；c₂为速度反馈正系数；

表示第kτ时刻时，第i个移动机械臂的所述末端执行器速度向量和所述联合速度向量之间的连接矩阵；

表示第kτ时刻时，所述被追捕物的位置向量；

表示第kτ时刻时，第i个移动机械臂的所述末端执行器的位置向量。

7.根据权利要求6所述的移动机械臂系统围捕控制方法，其特征在于，所述驱动轮包括半径相同的左轮和右轮；并且，在所述设定运行模式中，所述

分别为：

其中，

在上述公式中，φ为所述移动平台的航向角；x_c、y_c分别为所述移动平台与所述机械臂的连接点所在坐标系中的横坐标、纵坐标，且所述坐标系所在平面与所述移动平台的表面平行；f(θ)为所述机械臂的前向运动学方程；θ为所述机械臂上所有关节的角度构成的联合向量；r表示所述左轮或所述右轮的半径；d表示所述左轮中心与所述右轮中心构成线段的中点到所述移动平台与所述机械臂的连接点的距离；b表示所述左轮或所述右轮与所述中点之间的距离；I为单位矩阵。

8.根据权利要求7所述的移动机械臂系统围捕控制方法，其特征在于，所述机械臂包括6个关节及6根连杆；各所述关节通过所述连杆连接在一起；并且，所述f(θ)的计算方式为：

其中，l_u表示第u根连杆的长度，且u＝1,2,3,4,5,6；c_v＝cos(θ_v)，s_v＝sin(θ_v)，s₃₂＝sin(θ₃+θ₂)；c₃₂＝cos(θ₃+θ₂)，且θ_v表示第v个关节的角度，v＝1,2,3,4,5,6；h为离所述移动平台最近的所述关节与所述移动平台底部之间的距离。

9.一种移动机械臂系统，其特征在于，所述移动机械臂系统用于执行权利要求1至8中任一权利要求所述的移动机械臂系统围捕控制方法。

Images