基于末端估计和操作度调整的多移动机械臂最优协同方法与流程

本发明涉及多智能体控制技术领域，具体涉及一种基于末端估计和操作度调整的多移动机械臂最优协同方法。

背景技术

分布式协同是多机器人控制领域的一个基础问题，其目标为在不同的机器人之间通过局部信息交换达成一致性协议，以控制机器人完成一个共同任务，从而实现工业中对大范围复杂任务的迫切需要。而在这些机器人系统中，移动机械臂能够利用移动基座更大的活动范围以及机械臂灵活的操作能力适应复杂多变的任务，通过对末端的控制实现期望的协同任务。因此，移动机械臂分布式协同控制受到各领域的广泛关注。

然而，对于移动机械臂，其任务空间存在某些末端的操作在关节空间是不可行的，会导致系统状态量非奇异。在接近奇异的区域中，机械臂想要到达一个期望的位置或方向，其关节角速度或角加速度将会无限大，从而损坏机械臂。操作度是当前状态机械臂能够到达任意位置或任意方向的能力一种度量，其能够表示当前状态机械臂远离奇异位形的程度。因此，为了避免奇异位形，需要在实现分布式协同的过程中优化操作度函数。

为了实现这类带有优化问题的分布式协同控制，近年来人们开始在分布式协同中加入优化技术，提出最优协同问题，以实现多机器人协同的同时解决优化问题。在传统控制方法中，移动机械臂只能实现关节空间状态量的协同控制，但由于关节空间到任务空间的映射是一个非线性映射，末端位置为关节角和移动基座位置的一个非线性耦合量，无法直接使用传统方法进行控制。

因此对于多移动机械臂分布式最优协同问题，如何在合理优化操作度代价函数的同时，处理末端位置这一非线性耦合量的值以实现最优协同，是亟待解决的问题。

技术实现要素：

有鉴于此，本发明提供了基于末端估计和操作度调整的多移动机械臂最优协同方法，能够在合理优化操作度函数的同时，处理末端位置这一非线性耦合量的值以实现最优协同。

本发明的具体实施方式如下：

基于末端估计和操作度调整的多移动机械臂最优协同方法，包括以下步骤：

步骤一，以移动基座速度和关节角速度为控制量建立单个移动机械臂模型；

步骤二，设计优化问题的代价函数、约束条件以及可行集；

所述设计优化问题的代价函数同时考虑操作度优化和末端估计，通过引入一个线性末端估计量替代末端位置，使得优化问题中的约束不包含非线性项，再设计一个可变权值，使得操作度优化能根据机械臂实时的关节角自适应调整；

并以多移动机械臂末端位置和相应的移动基座能够形成给定队形以及代价函数最优解在可行集内作为约束条件；所述给定队形包括自身和邻居机械臂的末端位置和移动基座位置；

步骤三，以所述代价函数在可行集内为凸函数为目标选取可行集；

步骤四，以分布式的方法求解优化问题，使所述移动机械臂模型满足最优解，即操作度值达到设定的期望阈值以上、所述线性末端估计量等于末端位置以及满足约束条件。

进一步地，所述代价函数由末端估计误差的二次型函数与变权重操作度二次型函数加权和构成，所述末端估计误差的二次型函数形如所述变权重操作度二次型函数形如i为第i个移动机械臂，xi为末端位置，为线性末端估计量，pi为移动基座位置，θi为关节角，ρi为机械臂关节空间到任务空间的非线性映射，μi为操作度的值，可变权值γi(θi)定义为

μi^s为操作度期望阈值，0＜μi^s≤μi^m，μi^m为操作度最大值。

进一步地，所述可行集满足

进一步地，所述分布式的方法通过构建分布式梯度投影的控制方法实现；

所述分布式梯度投影的控制方法的获得方式如下：

基于拉格朗日函数，应用原始-对偶方法，利用所述可行集通过投影方法构建原始问题方程式，在对偶问题方程式中增加由移动基座和末端的速度形成的阻尼项。

有益效果：

本发明能在分布式协同控制的同时解决移动机械臂的操作度优化问题，能够避免关节奇异性以适应复杂多变的任务。每个移动机械臂仅需知道自己和邻居节点的状态，不需要进行全局信息交互，减少了系统通讯负担；每个移动机械臂自身进行运算控制，无需中心节点进行整体规划，降低了系统的计算负担，相对集中式结构具有更强的抗毁性与鲁棒性；

其次，代价函数同时考虑操作度优化和末端估计，通过引入一个线性变量以估计末端位置，使得优化问题中的约束不包含非线性项；同时，设计一个可变权值，使其能够根据实时机械臂姿态合理的调整关节角度，当操作度值达到了期望的阈值以上，则操作度优化权值变得非常小，以保证末端估计的效果；当操作度值远小于期望阈值时，则操作度优化权值变得非常大，以控制关节角度使得机械臂远离奇异位置。

附图说明

图1为本发明实施例中多移动机械臂之间通信拓扑示意图；

图2为本发明实施例中基于末端估计和操作度自适应的多移动机械臂最优协同方法的流程图；

图3为本发明实施例中多移动机械臂整体运动过程示意图；

图4为本发明实施例中各移动机械臂操作度变化图；

图5为本发明实施例中移动机械臂移动基座位置协同误差图；

图6为本发明实施例中移动机械臂末端位置协同误差图；

图7为本发明实施例中移动机械臂末端位置的估计误差图。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

移动机械臂由机械臂固定在移动基座上形成，每个移动机械臂可以获得自身的末端位置以及关节角度，且有一定计算能力以及和邻居通信的能力，本实例中的分布式最优协同任务为在优化操作度的同时实现末端编队以及移动基座编队。

本发明提供了一种基于末端估计和操作度调整的多移动机械臂最优协同方法，首先，设计了一个优化问题，其代价函数同时考虑操作度优化和末端估计，约束条件为末端编队以及移动基座编队。优化问题中，通过引入一个线性变量以估计末端位置，使得优化问题中的约束条件不包含非线性项；同时，对操作度设计一个可变权值，使得其能够根据实时机械臂姿态合理的调整关节角度，当操作度值达到了期望的阈值以上，则操作度优化权值变得非常小，以保证末端估计的效果；当操作度值远小于期望阈值时，则操作度优化权值变得非常大，以控制关节角度使得机械臂远离奇异位置。之后，应用一种分布式梯度投影的控制方法使得多移动操作臂系统以操作度优化权值自适应的方式完成分布式协同控制，最终实现操作度值达到期望的阈值以上且末端估计误差收敛到0。该方法流程图如图2所示。

本实例中实现本发明所述控制方法的步骤为：

步骤一：建立单个移动机械臂模型。

设协同搬运系统中移动机械臂的个数为n。则对于第i个移动机械臂，其模型为

应满足运动学约束

xi＝ρi(θi)+pi,i＝1,…,n(2)

其中，是末端位置，是移动基座位置，是关节角，ρi(·):为机械臂关节空间到任务空间的非线性映射，m、k分别为任务空间和关节空间的维度。

步骤二：设计优化问题的代价函数、约束条件以及可行集。

对于第i个移动机械臂，定义操作度最大值为可采用如下二次型函数保证操作度优化：

其中操作度μi满足ji(θi)为第i个移动机械臂的雅可比矩阵。为了设计变权重的操作度优化函数，可定义权值

其中，μi^s≤μi^m为操作度期望的阈值。改进式(3)得到变权重的操作度优化函数

再定义末端估计量由于实际末端位置为xi＝ρi(θi)+pi，末端估计误差为可采用如下末端估计误差函数

分布式优化问题的代价函数应同时考虑操作度优化和末端估计，故设计代价函数为式(5)、(6)之和，如下所示：

约束条件为末端编队以及移动基座编队，末端编队约束采用估计量以避免约束条件中出现非线性项，由此构建分布式优化问题：

其中，p＝[p1^t,…,pn^t]^t，θ＝[θ1^t,…,θn^t]^t，θi、ξi分别为pi、θi、的可行集，是移动基座编队约束，是末端编队约束，分别为移动基座编队和末端编队的队形变量，使得当队形形成时，满足

j为移动机械臂i的邻居。

步骤三、通过合理选取可行集，使得优化问题的代价函数在可行集内为凸函数。

设计可行集θi使得ρi(θi)、μi(θi)在θi上是凹的，同时设计ξi存在相对内点满足slater条件。

对于形如形式的二次型函数，其中g(z)＝[g1(z),…,gr(z)]^t，若gi(z)≤0，同时gi(z)是凹函数，i＝1,…,r，则其为凸函数；对于形如的变权重二次型函数，实际相当于三次型函数，若gi(z)≤0，同时gi(z)是凹函数，i＝1,…,r，其也为凸函数。

由于是二次型函数，可以选取移动基座期望队形向量满足不等式条件使得在可行集内为凸函数，同时μi(θi)-μi^m≤0恒成立，故在可行集内也为凸函数。综上所述，当时，代价函数为

是凸函数；当时，代价函数为

也是凸函数。

步骤四、构建分布式梯度投影的控制方法，以分布式的方式求解优化问题。

对于优化问题式(8)，其拉格朗日方程为

其中，λ＝[λ1^t,…,λn^t]^t，ν＝[ν1^t,…,νn^t]^t，l是移动机械臂之间通信拓扑的拉普拉斯矩阵，定义如下

式中，l为移动机械臂邻居编号，为第i个移动机械臂的邻居集，当第i,j个移动机械臂之间可以通信时，aij＝1，否则，aij＝0，

定义η＝[λ^t,ν^t]^t，则式(9)可以表示为

其中，i2为二维单位向量。则可以由如下投影方法

构建分布式梯度投影的控制方法：

其中中分别增加了和作为阻尼项，以保证控制方法(11)的稳定性，p代表投影。

通过分布式梯度投影的控制方法(11)可以保证

从而满足分布式协同控制需求，且即满足末端估计误差收敛到0，同时即操作度的值不小于期望阈值。

本实例组建了包含三个移动机械臂的协同系统，即n＝3，通信拓扑示意图如图1所示，下面给出本实例的软件仿真结果以论证本发明的有效性。

如图4-6所示，这三幅图展示的是三个移动机械臂分布式最优协同结果，控制方法为本发明提供的基于末端估计和操作度自适应的多移动机械臂最优协同方法。图3显示了多移动机械臂整体运动过程，图4为所有移动机械臂的操作度变化图。图5、图6分别详细显示了末端位置和移动基座位置的协同误差，图7显示了末端位置估计误差。从图中可以发现，对于这三个移动机械臂组成的协同系统，能够保证末端位置和移动基座位置的协同误差收敛到0，以及末端估计误差收敛到0，同时所有移动机械臂的操作度均不小于期望阈值，这证明了本发明的有效性。

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