基于广义坐标的分布式移动机械臂任务分层优化控制方法与流程

本发明涉及多智能体控制技术领域，具体涉及一种基于广义坐标的分布式移动机械臂任务分层优化控制方法。

背景技术

近年来随着工业中对大范围复杂任务的迫切需要，多机械臂协同受到各领域的广泛关注。由于传统的固定机械臂工作空间有限，越来越多的人采用将固定机械臂安装在移动基座上的移动机械臂来实现复杂的协同任务。移动机械臂能够利用移动基座更大的活动范围以及机械臂灵活的操作能力适应复杂多变的任务，但是，其高冗余度和运动学非线性约束使得多移动机械臂的协同控制具有很大难度。

对移动机械臂来说，由于基座和机械臂的自由度之和一般都不会小于任务空间维度，因此移动机械臂具有冗余性。在协同控制主任务的情况下，各个移动机械臂还有冗余度完成其他一些次任务，或者优化目标。因此需要引入任务优先级概念，使得在主次任务同时存在时，移动机械臂能够按照任务等级进行运动控制，保证低优先级任务不影响高优先级任务，以解决可能存在的冲突问题。

操作度是当前状态机械臂能够到达任意位置或任意方向的能力一种度量，其能够表示当前状态机械臂远离奇异位形的程度。当机械臂在接近奇异的区域中，机械臂想要到达一个期望的位置或方向，其关节角速度或角加速度将会无限大，从而损坏机械臂。同时，由于多移动机械臂系统共享同一工作空间、搬运同一物体时都会产生资源冲突，因此还需要考虑避障避碰问题。实际工程中，通常会要求移动机械臂在实现协同控制主任务的过程中，完成操作度最大化以及避障避碰等次任务。

在传统的控制方法中，移动机械臂协同控制通常以集中式的方式实现以保证精度和鲁棒性，但是集中式将会给中心节点带来巨大的计算负担，很容易因为一个节点的故障导致整个系统崩溃，难以做到同时控制多个移动机械臂，同时也无法应对工业中灵活度要求很高的协同搬运、制造和组装的问题。而分布化的系统具有通讯负担小、计算复杂度小、鲁棒性强等特点，常常在实际工程中使用。

目前，对于分布式的多移动机械臂协同控制，大部分的研究都采用集中式规划与分布式控制相结合的方式进行，并非完全分布式的控制方法。少数完全分布式的控制方法则需要设计观测器，以获得所有移动机械臂的信息，这对于分布式系统来说是一个很大的计算负担。而对于任务优先级控制，主流的做法都是将末端执行器协同控制作为主任务设计在任务空间，而将操作度最大化以及避障避碰等次任务设计在雅可比矩阵的零空间里，这样做的好处是主次任务冲突时优先级能够很好的保证，但缺点是主任务占用冗余度过多时会完全牺牲掉次任务。

因此对于多移动机械臂协同中的任务优先级控制问题，如何使用完全分布式的控制方法实现，并且在保证任务优先级的情况下合理的分配冗余度，是亟待解决的问题。

技术实现要素：

有鉴于此，本发明提供了基于广义坐标的分布式移动机械臂任务分层优化控制方法，能够使用完全分布式的控制方法实现分布式的多移动机械臂协同控制。

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

基于广义坐标的分布式移动机械臂任务分层优化控制方法，主任务为多移动机械臂通过末端执行器协作搬运目标物体使目标物体中心跟踪一条给定参考轨迹，次任务为以所有机械臂的最大化操作度之和控制关节角以及控制移动基座编队避障，包括以下步骤：

步骤一，将末端执行器位置x与关节角θ整合为一个向量，定义为广义坐标q＝[x^t,θ^t]^t，并依据广义坐标建立单个移动机械臂模型；

步骤二，引入相对偏差ζ，将任务优先级控制中的主任务和次任务分层处理，分为：控制层的一致性协同跟踪使得q-ζ→qd以及优化层的带有约束条件的非凸优化问题使得ζ→ζ^*；任务优先级协同控制实现时，广义坐标与相对偏差之间的关系满足q-ζ^*＝qd，ζ^*为最优相对偏差，qd为给定参考轨迹向量；

步骤三，设计优化层的约束条件，包括基于自身和邻居机械臂的中心跟踪等式约束和目标物体形状等式约束，使得优化层的任务不影响控制层任务的实现；

步骤四，定义相对偏差的范围约束集合，使得所述非凸优化问题的代价函数在所述范围约束集合内为凸函数；

步骤五，将所述中心跟踪等式约束分解成n个局部约束等式形式，n为机械臂个数；定义非光滑罚项，在拉格朗日函数的基础上增加所述非光滑罚项获得改进的拉格朗日函数；利用所述范围约束集合，通过投影方法获得优化层的投影原始-对偶动力学更新律，以分布式的方式求解带有约束条件的非凸优化问题，使得t为时间；

步骤六，应用步骤五中得到的最优相对偏差ζ^*，设计控制层一致性协同跟踪控制律，使得实现分布式任务优先级协同控制。

进一步地，所述单个移动机械臂模型形如

u为控制量，j(θ)为该移动机械臂的雅可比矩阵，im、ik分别为m、k维单位向量。

进一步地，所述步骤四中使代价函数在所述范围约束集合内为凸函数的方法为：

代价函数由操作度最大化二次型函数和移动基座编队二次型函数加权之和构成，所述二次型函数形如

选取移动基座期望队形向量以保证所述代价函数在所述范围约束集合内是凸函数。

有益效果：

1、本发明通过分布式分层优化控制解决多移动机械臂协同搬运中的任务优先级问题，在优化层定义相对偏差来实现冗余度的合理分配，使得操作度最大化以及移动基座编队避障等次任务得到解决，同时不与搬运目标中心跟踪的主任务发生冲突；其次，将末端执行器位置与关节角整合为一个向量定义为广义坐标，通过扩展坐标维度，使得移动机械臂的冗余度可以依托于广义坐标进行合理分配；再者，通过在优化层设计投影原始-对偶动力学更新律以及在控制层设计一致性协同跟踪控制律使得整个分层优化控制框架为分布式结构，每个移动机械臂仅需知道自己和邻居节点的状态，不需要进行全局信息交互，减少了系统通讯负担，每个移动机械臂自身进行运算控制，无需中心节点进行整体规划，降低了系统的计算负担，相对集中式结构具有更强的抗毁性与鲁棒性。

2、本发明建立新的移动机械臂模型，将实际的工程问题转化为一个基于广义坐标的分层优化控制问题，能够将搬运目标中心跟踪、机械臂姿态调整以及移动基座运动控制问题在同一个框架下解决，保证控制量关节角的唯一解，解决了冗余度问题。

3、本发明采用带有非光滑罚函数的改进拉格朗日函数设计投影原始-对偶动力学更新律，使得带有约束条件的非凸优化问题中的中心跟踪耦合等式约束可以作为多个局部等式约束，以完全分布式的方法求解原优化问题。

附图说明

图1为本发明实施例中多移动机械臂协同搬运系统组成示意图；

图2为本发明实施例中多移动机械臂分布式分层优化控制的流程图；

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

图4为本发明实施例中多移动机械臂整体运动过程及障碍位置示意图；

图5为本发明实施例中多移动机械臂末端执行器位置运动过程示意图；

图6为本发明实施例中多移动机械臂移动基座位置运动过程及障碍位置示意图；

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

图8为本发明实施例中搬运物体的中心跟踪误差图；

图9(a)为本发明实施例中移动机械臂末端执行器位置的x轴误差图；

图9(b)为本发明实施例中移动机械臂末端执行器位置的y轴误差图。

具体实施方式

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

本实例中移动机械臂由机械臂固定在移动基座上形成，搬运目标为给定形状的物体，协同控制任务根据任务优先级可分为主、次任务，主任务为多移动机械臂通过末端执行器协作搬运目标物体使目标物体中心跟踪一条给定参考轨迹，次任务为控制关节姿态以及移动基座使所有机械臂的操作度之和保持较大值并且移动基座切换队形以适应复杂障碍环境。

本发明提供了一种基于广义坐标的分布式移动机械臂任务分层优化控制方法，将末端执行器位置和关节角整合在一起定义为广义坐标，将分层控制方法设计两层，上层为优化层，通过分布式优化广义坐标对应的相对偏差来实现冗余度的合理分配，实现操作度之和保持较大的值并且移动基座完成队形切换；下层为控制层，将优化层获得的相对偏差应用于广义坐标的一致性编队跟踪控制中，以实现被搬运物体中心跟踪给定参考轨迹的目的。流程图如图2所示，步骤如下：

步骤一：定义广义坐标，建立单个移动机械臂模型。

设协同搬运系统中移动机械臂的个数为n。则对于第i个移动机械臂，应满足运动学约束

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

其中，是末端执行器位置，是移动基座位置，是关节角。为机械臂关节空间到任务空间的非线性映射，m、k分别为任务空间和关节空间的维度。每个移动机械臂可以获得自身的末端执行器位置以及关节角度，且有一定计算能力以及和邻居通信的能力，同时至少有一个移动机械臂可以获得给定参考轨迹的位置σd和速度移动机械臂之间的通信拓扑为无向连通图，移动机械臂与给定参考轨迹之间为有向拓扑，示意图如图3所示。

设第i个移动机械臂的移动基座速度为关节角速度为定义广义坐标qi＝[xi^t,θi^t]^t，以及控制量ui＝[vi^t,ωi^t]^t。对(1)的两边求导，得到第i个移动机械臂的模型

其中ji(θi)为第i个移动机械臂的雅可比矩阵，im、ik分别为m、k维单位向量。

步骤二：定义广义坐标qi的相对偏差ζi，使得任务优先级控制任务完成时，qi与最优的相对偏差ζi^*满足如下关系

qi-ζi^*＝qd(3)

其中qd＝[σd^t,0k×m^t]^t，为给定参考轨迹向量。则任务优先级控制任务可以分解为优化层中相对偏差的分布式优化问题和控制层中广义坐标的跟踪问题。

针对优化层，第i个移动机械臂最优的相对偏差ζi^*应由分布式优化问题求解得到。为了便于表示xi、θi与qi的关系，定义矩阵和使得下式成立

定义ξi为第i个移动机械臂对移动基座队形中心的估计量。上述优化层的优化问题的优化目标由操作度最大化函数以及移动基座编队函数两部分组成，以实现次任务要求，如下所示：

其中为移动基座编队函数，δi^ξ表示移动基座期望队形向量，为操作度最大化函数，操作度μi满足ji(θi)为第i个移动机械臂的雅可比矩阵，μi^m为第i个移动机械臂操作度最大值。

为了合理分配冗余度以及不与主任务要求产生冲突，需设计相应约束条件，由此构建分布式优化问题：

其中，ζ＝[ζ1^t,…,ζn^t]^t，ξ＝[ξ1^t,…,ξn^t]^t，ωi、ψi为ζi、ξi的范围约束集合，j为移动机械臂i的邻居，和是主任务中的中心跟踪约束和目标物体形状约束，ξi-ξj＝0是移动基座编队中心估计量的一致性约束。

步骤三、通过合理选取移动基座期望队形向量，使得非凸优化问题的代价函数在范围约束集合内为凸函数。

设计相对偏差的范围约束集合ωi、ψi，使得在ωi上是凹的，同时存在相对内点ζ∈rint(ω)、ξ∈rint(ψ)满足slater条件。

代价函数由操作度最大化二次型函数和移动基座编队二次型函数加权之和构成，其中二次型函数形如但由于g(z)非线性，因此该函数是非凸的。对于形如形式的二次型函数，其中g(z)＝[g1(z),…,gr(z)]^t，若其为凸函数，则如下条件满足其一：

(1)gi(z)≤0，同时gi(z)是凹函数，i＝1,…,r；

(2)gi(z)≥0，同时gi(z)是凸函数，i＝1,…,r。

由于和都是二次型函数，可以选取移动基座期望队形向量满足不等式条件i＝1,…,n，使得在范围约束集合ωi、ψi内为凸函数，同时恒成立，故在范围约束集合ωi内也为凸函数。

综上所述，代价函数i＝1,…,n均为凸函数。

步骤四、设计非光滑罚项以构建改进拉格朗日方程，构建优化层的投影原始-对偶动力学更新律，以分布式的方式求解带有约束条件的非凸优化问题。

由于中心跟踪约束是一个聚合项，需要知道所有移动机械臂的信息，难以用分布式的方式处理，因此，将带有约束条件的非凸优化问题中的中心跟踪耦合等式约束写成n个局部约束的形式，并针对第i个移动机械臂定义局部拉格朗日乘子λi使得λ＝[λ1^t,…,λn^t]^t；并定义一致集由此可设计非光滑罚项

使得φ(λ)能够作为λi一致性程度的度量函数，当时，φ(λ)＝0。

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

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

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

利用非光滑罚项，可以设计改进拉格朗日函数

通过选择常量λ最终收敛于一致集故有即在一致集上改进拉格朗日函数(9)与原拉格朗日函数(8)等价。据此可以由如下投影方法

获得投影原始-对偶动力学更新律：

式中，p代表投影，通过投影原始-对偶动力学更新律式(10)可以保证从而得到满足次任务需求且不与主任务冲突的最优相对偏差ζi^*。

步骤五、应用最优相对偏差，设计控制层的一致性跟踪控制律，实现分布式任务优先级协同控制。

针对控制层，需要实现qi与最优的相对偏差ζi^*满足式(3)，即qi-ζi^*＝qd，其中qd＝[σd^t,0k×m^t]^t。故设计一致性跟踪控制律如下

其中，β>0为常数，当第i个移动机械臂可以获得给定参考轨迹的位置σd和速度时，bi＝1，否则，bi＝0。

由于移动机械臂之间为无向连通图，且至少有一个移动机械臂可以获得给定参考轨迹信息，因此整体通信拓扑中一定存在一个由给定参考轨迹到任意移动机械臂的有向路径，故整体通信拓扑存在有向生成树，从而又由上一步骤可知，因此最终qi与最优的相对偏差ζi^*必定满足qi-ζi^*＝qd。从而实现了通过末端执行器协作搬运目标物体使其中心跟踪给定参考轨迹的主任务，以及控制关节操作度以及移动基座编队的次任务。

本实例组建了包含三个移动机械臂的协同搬运系统，即n＝3，系统组成如图1所示，下面给出本实例的软件仿真结果以论证本方法的有效性。

如图4-6所示，这三幅图展示的是三个移动机械臂分布式任务优先级分层优化控制结果，其中第20s和第45s控制移动基座进行队形切换以避开障碍物。图4显示了多移动机械臂整体运动过程及障碍位置，图5、图6分别详细显示了末端执行器位置和移动基座位置。图7为所有移动机械臂的操作度变化图。从图中可以发现，对于这三个移动机械臂组成的搬运系统，能够保证协同搬运目标物体使其中心跟踪给定参考轨迹，同时也能优化各个移动机械臂操作度的值远大于0，以及控制移动基座切换编队达到避障目的。

图8为本发明实施例中搬运物体的中心跟踪误差图，图9(a)和图9(b)分别为本发明实施例中移动机械臂末端执行器位置的x轴误差图和y轴误差图，可以看出即使第20s和第45s移动基座发生了队形切换，上述误差仍然很快收敛到0，这证明了本方法的有效性。

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