基于距离及相对角度信息的多智能体系统刚性编队任务规划

黄捷 李俊杰

摘 要：针对仅在获取多智能体局部距离及相对角度信息条件下如何保持多智能体系统刚性编任务规划队问题， 本文提出了两种多智能体刚性编队任务规划方案。 首先提出一种基于距离和相对角度的刚性编队规划方案， 规划在未知相对位置情况下确保智能体刚性编队。 其次利用零空间投影的任务规划方法解决编队任务与避障任务的冲突， 确保编队移动过程中编队队形的稳定。 再次结合距离和相对角度提出一种整体刚性编队规划方案， 并在避障过程中将多智能体系统视为整体进行避障， 确保稳定的刚性编队和避障任务的完成。 经过仿真验证本文提出的两种方法在弱化相对位置信息的条件下能够较好的保持刚性编队并完成避障任务， 对比于依赖相对位置的零空间行为控制方法弱化了对于相对位置信息的依赖。

关键词：  刚性编队; 任务规划; 多智能体系统；  相对角度信息；  距离信息

中图分类号：  TJ760

文献标识码： A

文章编号：  1673-5048（2024）02-0079-12

DOI： 10.12132/ISSN.1673-5048.2024.0003

0 引  言

由于多智能体系统具备自主性、 分布性、 协同性等特性， 近二十年来， 多智能体系统的研究成为人工智能领域的热点之一。 多智能体系统通过各智能体间的通讯、 合作、 竞争及控制等来实现大规模自主无人系统群体智能特征的涌现。 多智能体系统协作不仅是单个智能体作用的简单相加， 其整体效能还包括个体之间相互作用的非线性增量。

多智能体是表征集群飞行器等无人系统的一种典型技术， 由于飞行器等运动体往往具有任务约束和编队的性能要求， 包括了集群任务规划与协同运动控制等关键技术。 在过去的研究中， Martin等［1］基于博弈理论提出一种合作博弈理论框架用以实现多机器人的集聚与编队生成； Flores-Resendiz等［2］提出一种由有界吸引分量和互补排斥分量组成的控制策略解决二阶多智能体系统的无碰撞编队控制问题； Lin等［3］基于李代数方法提出一种多四旋翼系统内外环分布式编队控制方案， 消除干扰和偏置扰动引起的不可靠角速度反馈； Kankashvar等［4］基于改进多智能体强化学习价值函数提出一种航天器编队飞行重构跟踪方法； Guo等［5］基于一致性理论和局部邻域信息提出一种包含避障， 避碰和编队重建控制项的分布式编队控制器。

刚性编队是多智能体编队领域的一种特殊编队方法， 其要求各智能体两两之间距离保持不变， 从而形成固定的刚性队形， 多智能体刚性编队包括了刚性任务规划与运动控制。 在一些具体的应用任务， 如大型物体的搬运［6］、 多无人机协同覆盖［7-8］和多航天器稳定编队［9-10］等研究中， 所有的智能体必须组成特定的队形并在执行任务期间保持整个队形不变， 保持严格的刚性编队。 当多智能体的坐标框架存在微小的错位时， 队形会发生扭曲， 并带来额外的平移运动， 这将影响整体框架稳定性。 为了增强多智能体坐标框架的鲁棒性， 可采用刚性理论［11］这一数学工具来保证多智能体系统的刚性编队保持。 Krick等［12］以及Aranda等［13］提出，  多智能体系统刚性编队要求在欧几里得空间中， 整个系统的队形逐渐收敛到一组由期望距离所定义的刚性队形。

图论在多智能体刚性编队问题中扮演关键角色， 尤其刚性图理论成为该领域的研究热点。 Aspnes等［14］的研究揭示了刚性概念与编队问题之间的关联，  并提出了一种基于刚性图设计刚性编队规划的方法。 Olfati-Saber等［15］采用质点模型， 提出了基于势函数的编队规划方法， 并利用刚性图论定义了编队队形， 实现了基本的刚性队形。 Zhou等［16］以及Arrichiello等［17］利用全局相对位置信息并基于零空间行为控制（Null-Space-based Behavioral Control， NSBC）的方法设计了刚性编队的控制律。 然而， 在实际应用场景下如物流智能体编队控制， 智能体室内编队探测过程中， 全局的相对位置信息及全部智能体的质心往往难以获得， 且获取相对位置信息一般需要依赖卫星定位。 刘国波［18］考虑弱化相对位置信息， 提出基于部分方位刚性的多智能体领航-跟随编队控制方法， 满足了刚性编队中智能体间的期望相对方位约束。

在弱化多智能体相对位置信息的刚性编队研究中， 部分研究者针对如何形成刚性编队的规划问题进行了研究。 肖志旋等［19］利用多智能體方位信息，  提出了一种基于方位信息的刚性编队方法， 使得系统能够实现无穷小方位刚性。 然而仅利用方位信息形成的编队仍存在缩放特性， 无法在距离上满足全局刚性的要求； Yang等［20-21］基于边缘应力结构提出了一个具有全局刚性的虚拟张拉整体框架， 以满足各智能体间的严格距离要求。 然而， 该方法对距离的限制要求过于严苛， 且构造张拉结构的过程较为复杂， 在特定场景下的鲁棒性较差； Chen等［22］利用初始三个智能体的相对位置信息并根据相对角度信息对其余智能体进行刚性编队， 保证队形的刚性要求。 然而该方法仍需获取初始三个智能体的相对位置信息， 在位置信息的要求上仍有提升空间。 Wang等［23］及Zou等［24］利用领航者测量相对于不确定目标的相对方位信息， 同时保证定位和绕航目标的实现。

在多智能体刚性编队问题中， 同时满足避障约束并保持编队稳定是刚性编队过程中的一大挑战。 目前， 研究人员采用了多种避障技术， 包括人工势场方法［25-26］， 基于行为的方法［27-28］和基于优化的方法［29-30］等。 这些方法在各种多智能体编队问题中得到了广泛应用， 但众所周知的是其都存在一些局限性， 例如容易陷入局部极小值问题而导致避障失败。 基于行为方法中的基于零空间的行为控制方法， 是一种通过零空间投影解决任务冲突来实现避障的方法。 该方法允许多智能体系统在执行主要任务的同时， 处理附加的次要任务或行为。 使得多智能体系统在执行主要任务时具备更高的灵活性和多功能性［31］。 例如， Zheng等［32］采用了零空间行为控制架构来实现多智能体系统的时变队形控制， 并避免与障碍物发生碰撞； Golluccio等［33］提出一种改进的学习算法对NSBC方法进行了扩充， 使得机器人能够动态选择适当的低优先级任务进行执行； Wang等［34］基于零空间行为控制方法提出了一种分层子任务融合策略， 用于解决多个子任务的无冲突融合调度； Zhang等［35］基于零空间的行为控制框架提出了一种自适应动态规划方法， 用于无人地面车辆-机械臂的协调控制。

由于飞行器编队方法大多依赖期望相对位置信息［36-37］， 然而快速运动的集群无人机等系统常常难以获得期望的相对位置信息， 为了降低各飞行器对位置信息依赖， 受到文献［16， 27］等研究成果的启发， 本文进一步研究如何在不使用相对位置信息情况下通过得到各智能体间的相对角度信息与距离信息来保证多智能体系统的刚性编队规划方案。 其中， 相对角度及距离分别表示在智能体i坐标系下与父节点j以及领航者k所形成的相对角度以及在智能体i坐标系下与父节点j之间的距离。

本文的创新点如下：

（1） 提出一种基于距离及相对角度限制约束的多智能体系统刚性编队任务规划方案。 相较于文献［16］，  该方案无需获取全局的编队位置信息， 消除了编队任务对于相对位置信息的依赖。 相较于文献［20-21］，  该方案考虑在编队移动过程中的避障问题， 解决编队任务与避障任务的冲突。

（2） 提出一种将多智能体编队在避障行为中视为整体的大范围避障方法， 该方法在保持编队刚性的要求下， 相较于文献［16］具有更小的位移及相对角度偏差， 在不需要进行经过较小障碍通道的情况下具有更小的相对角度和距离误差。

1 基本概念及问题描述

1.1 刚性图及刚性矩阵理论

多智能体系统刚性编队在数学上可以用如图1所示的杆-球链接结构来形象地描述。 其中球代表智能体， 木杆代表边。 球和球之间由一定数量的不可形变的木杆联系， 这种拥有杆-球链接的编队就是刚性编队。 这种编队结构可以通过图论方法进行数学表示。 在刚性编队中， 智能体对应图论中的顶点， 智能体之间的通讯对应图论中的边。 本节将介绍刚性概念的规范定义和一些重要的定理性质。

定义1： 在瘙綆2中， 各个智能体的位置信息可用pi={pix，  piy}来表示， 而各个智能体的位置合集为P={p1，  p2，  …， pn}， 各个智能体之间建立的通讯关系可用E来表示， E={e1，  e2，  …， em}。 整个队伍的队形用Fp=（{p1，  p2，  …， pn}，  E）来表示， 各个智能体之间的距离用dij=pi－pj2来表示， 式中P与E分别为数学中的顶点集与边集的概念。

定义2： 某一个队形Fp是由P，  E，  d所唯一决定的。

定义3： 在瘙綆2中， 如果刚性是一个队形Fp的唯一运动要求， 则Fp在执行任何任务的时候都能够保持E中的各条边的距离不变， 则称Fp为刚性编队队形。

定义4： 刚性矩阵R表征了刚性编队中各个顶点之间的链接关系及各条边的距离关系， 综合性反应了刚性编队的性能指标。 刚性矩阵R具有两种定义方式， 分别从不同的角度出发但最终得到的矩阵形式是相同的。

刚性矩阵在刚性编队中扮演着至关重要的角色。 多智能体刚性编队是指由多个智能体以固定的相对位置和姿态组成的编队。 刚性矩阵作为一种数学工具， 用于描述不同多智能体之间的相对位置关系。

具体而言， 刚性矩阵用于表示一个多智能体相对于另一个多智能体的旋转和平移关系。 这些关系可以通过测量多智能体之间的距离和相对角度来确定。 在刚性编队中， 每个多智能体都配备了传感器， 用于测量其与其他多智能体之间的距离和相对角度。 这些测量值可用于确定各智能体的期望位置。

刚性矩阵的计算和更新是刚性编队算法的核心。 通过不断计算和更新刚性矩阵， 多智能体可以实时调整其相对位置和姿态， 以保持编队的稳定和准确性。 刚性矩阵的主要求解方法包括定义刚性方程以获取雅可比矩阵， 并据此得到刚性矩阵。

由定义1能够给出刚性方程gG：瘙綆2n→瘙綆|E|的概念：

gG（p）=［‖pin1－pout1‖2，  …， ‖pinm－poutm‖2］T（1）

式中： pin1，  pout1，  …，  poutm∈1，  2，  3，  …， n， pini－pouti2是由E中的ei來决定的， 表征的是第i条边的情况， 而pini和pouti分别为第i条边的两个顶点。 而刚性方程是由各个智能体构成的图的边与顶点的分布情况来决定的， 所以， 求解刚性方程的雅可比矩阵， 便可以得到刚性矩阵：

JgG（p）=2（pin1－pout1）p1（pin1－pout1）p2…（pin1－pout1）pn

（pin2－pout2）p1…（pin2－pout2）pn

（pinm－poutm）p1……（pinm－poutm）pn（2）

除去系数2， 则得到的JgG（p）=R（p）， 由此可得到刚性矩阵R。 零空间行为控制方法求解得到的雅可比矩阵与得到刚性矩阵的思想相似［16］， 均是通过对编队任务进行求导以得到实时雅可比矩阵从而确定各个智能体的运动规则。

1.2 基于零空间的行为控制算法介绍

本文采用了基于零空间的行为控制方法［16］， 通过将多智能体的不同行为设计为不同的任务函数， 并根据实际经验对任务进行优先级设计。 此方法将低优先级的任务投影到高优先级任务的零空间向量上， 以实现在高优先级任务完整执行的同时， 执行低优先级任务与高优先级任务的不冲突部分， 解决任务的冲突问题， 保证编队稳定性。 根据实际经验， 本文将避碰和避障设置为多智能体的最高优先级任务， 编队形成、 编队切换等其他任务都作为次优先级任务。

基于上述任务优先级设计， 进一步对多智能体系统的基本任务进行设计。 假设通用任务函数为σ∈瘙綆m（m表示通用任务的维度）， 则任务函数为

σ=f（p）（3）

式中： p=［p1， p2， …， pn］T为多智能体位置的集合， n为智能体的数量;  f（·）为多智能体位置到任务函数的映射。 对式（3）求关于位置p的偏导可得：

σ·=f（p）pp·=J（p）p·（4）

式中： J（p）∈瘙綆m×n為雅可比矩阵， 表示任务函数σ到多智能体位置p的映射关系。

在任务的执行过程中， 期望的任务函数σd（t）存在对应的期望位置pd（t）， 期望轨迹可由期望速度的时间积分得到， 通过对雅可比矩阵进行伪逆运算， 求得任务函数速度输出的最小二乘解如下：

ν=J+σ·=JT（JJT）－1σ·（5）

式中： ν为任务的速度矢量输出指令； J+=JT（JJT）－1为雅可比矩阵的伪逆。

除参考速度外还需要参考轨迹， 由上述可知， 参考速度的时间积分可以得到参考轨迹， 但速度的离散时间积分过程会产生参考轨迹偏差， 该偏差通过闭环逆运动学方法进行偏差抵消， 则第l个任务的参考速度输出为

νl=J+lσ·d.l+Λlσ～l （6）

式中： νl为第l个任务的速度输出指令； J+l为雅可比矩阵Jl的伪逆； σ·d.l为期望任务函数的导数； Λl为正定增益矩阵； σ～l=σ·d.l－σl为任务偏差； σd.l为期望任务函数。

假设全局任务由多个基本任务组成， 将低优先级任务投影到高优先级任务的零空间上， 从而消除任务速度输出指令之间的冲突部分， 最终得到全局任务的速度输出为

νG=νl+（I－J+lJl）νl+1（7）

式中： νG为全局任务的速度输出指令； I－J+lJl为第l个任务的零空间矩阵。

1.3 问题描述

运动学模型： 考虑平面上有N个智能体， 每个智能体i由单积分器动力学表示［24］：

p·i=uii=1， …， N（8）

式中： pi∈瘙綆2为智能体i在全局坐标系中的位置； ui∈瘙綆2为控制输入。

规划目标： 智能体编队系统在满足自身状态、 输入约束的情况下， 能够按照期望的距离和相对角度跟踪领航者以形成刚性编队， 且能保持较高的编队刚性在编队整体移动到目标点的过程中， 具备与系统内部智能体、 系统外部预先未知障碍的避碰能力。 为满足规划目标， 现作出以下假设：

假设1： 智能体间通信拓扑满足如图2所示的拓扑结构， 任意智能体与其父节点和领航者节点之间存在通路。

其中通信拓扑结构可以用邻接矩阵形式表示如下：

C=01…00

001…0

0

1

0…000; D=0111。

式中： C为各跟随者之间的通信拓扑； D为领航者与各跟随者之间的通信拓扑。

假设2： 智能体具备一定的探测能力和通信能力。 在一个采样周期内， 智能体能够实时获取进入其探测范围内的障碍物的当前位置信息， 能够实时获取进入其通信范围的智能体所发送的距离信息及相对角度信息。

假设3： 不考虑外界干扰、 噪声、 空气阻力等对智能体动态的影响。

考虑到智能体计算资源有限， 且并不是所有障碍物都会给智能体带来威胁， 只有当障碍物与智能体的距离较近时， 才有发生碰撞的风险， 因此， 基于智能体能够实时探测障碍物的能力， 根据有无碰撞风险， 设计出跟踪、 避碰两种控制任务。

注： 本文主要考虑在行为控制框架下的运动学/任务层面的控制问题， 跟随者通过得到与其父节点及领航者节点的距离信息与相对角度信息以满足编队任务约束从而形成编队。 零空间行为控制的协作式任务为达到投影的目的， 会将其任务转化为个体的速度向量用于进行零空间投影， 但是任务变量设计中是包含团体信息的属于协作式行为， 任务函数中包含多体变量， 并非单一的单机控制。

2 基于NSBC算法的刚性编队规划设计

本节提出了基于零空间行为控制算法的带有距离及相对角度限制约束的刚性编队规划方案。 该方法采用领航者-跟随者策略， 并将规划方案设计分为编队环节和避障环节， 从而完成多智能体的刚性编队任务。

2.1 带相对角度与距离约束的智能体编队

多智能体刚性编队任务要求多智能体在不改变整体队形的情况下移动到期望位置， 同时保持整体编队的相对刚性。 假设p={p1， p2， …， pn}为各个智能体位置信息， 本节中定义的刚性编队任务函数为

σr=［dij， ≮jik］Ti∈1， 2， …， n（9）

式中： dij=pi－pj2为pj与pi期望的距离； ≮jik为控制pj－pi与pk－pi之间的夹角， 各智能体编队示意图以及≮jik的具体细节如图3所示。

本文弱化的相对位置信息指智能体在形成刚性编队过程中无需使用到期望的相对位置， 而仅通过期望距离和相对角度达到刚性编队的性能要求， 在实际编队控制中相对位置是难以使用传感器测量的。

本文使用顶点集V={1， 2， …， N}， 表示在平面上N个点的指标集合， 如图3所示， 为了描述射线j－i到射线j－k的有符号角， 需要使用有序三元组（i， j， k）， 显然， （k， i， j）和（j， i， k）对应的两个角是不同的， 其实际上被称为补角或共轭角。 其中每个三元组（j， i， k）的相对角度≮jik在［0， 2π］范围内逆时针测量， ΤV×V×V={（i， j， k）， i， j， k∈V， i≠j≠k}表示角集， 其每个元素都是有序三元组。

M表示角集合T的元素数T， 假设T中的三元组中没有一对互为补充的。 假设p中没有重叠点， 即对于每个i≠j有pi≠pj且i， j∈{1， 2， …， N}。 然后， 顶点集合V、 角度集合T和位置向量p的组合称为相对角度， 在本文中用≮jik表示。 若给定不重叠的位置pi、 pj和pk， 那么相对角度≮jik∈［0， 2π）可以通过下式唯一地计算：

≮jik=arccos（zTijzik）z⊥ij·zik≥0

2π－arccos（zTijzik）otherwise  （10）

式中： zij=pj－pi‖pj－pi‖； zik=pk－pi‖pk－pi‖； z⊥ij=Q0zij=0－110zij为zij逆时针旋转π2得到的矢量； ·表为点积。

定义任务误差为σ～r=σr－σd2×1， 其中σd表示多智能体的期望位置， 即要保持期望编队的期望距离与期望相对角度。

为求解任务雅可比矩阵， 需要对任务函数求偏导， 具体求解方式如下：

JgG（p）=σr1p1σr1p2…σr1pn

σr2p1σr2p2…σr2pn

σrnp1σrnp2…σrnpn（11）

式中： σripi=σrixiσriyi， 即对每一个任务在x， y方向上分别求偏导， 根据式（11）， 可以求解得到的雅可比矩阵为如下形式：

JgG（p）=［A， ±B］ （12）

雅可比矩阵JgG（p）式中每一项的具体形式如下：

Aij=－sgn（pi－pj）pi－pj2（pi－pj）22（13）

Bjik=1Cjikxj-2xi+xk（pj-pi2）（pk-pi2）+

Djik|xi-xj|sign（xi-xj）（pj-pi2）3（pk-pi2）+

|xi-xk|sign（xi-xk）（pj-pi2）（pk-pi2）3（14）

式中： Cjik=1-（（xi-xj）（xi-xk）+（yi-yj）（yi-yk））2（pj-pi2）2（pk-pi2）2；

Djik=（（xi-xj）（xi-xk）+（yi-yj）（yi-yk））。

该雅可比矩阵限制了两个智能体之间的距离与相对角度， 用于约束每个智能体与其邻居的相对位置， 从而实现分布式编队的刚性要求。

在求得编队的雅可比矩阵后， 结合零空间行为控制方法， 最终能得到编队任务函数的输出速度如下：

νr=J+gGΛrσ～r（15）

2.2 多智能体避障

假设障碍物po（i）为一球形障碍物， 且给定智能体一定的安全距离dd后可以视障碍物为一虚拟的球体， 当智能体距离障碍物质心位置小于安全距离时则认为其具有碰撞危险。 障碍物集合Or的定义如下：

Or=（pi∈瘙綆2， po（i）∈瘙綆2：‖pi－po（i）‖≤dd）（16）

式中： pi=［pix， piy］T为第i个智能体的位置即相对应的x， y坐标； po（i）为第i个智能体对应的障碍物位置坐标； dd为人为给定的安全距离， 设计第i个多智能体的任务函数为

σio=（max{pi－po（i）， dd}）（17）

由于是离散状态下， 所以避障任务的雅可比矩阵为Jo（i）=（pi－po（i））pi－po（i）即一个指向障碍物方向的梯度矢量。

最后得到的输出为避障速度为

νo（i）=J+o（i）λo（i）σ～io=J+oλo（i）（dd－‖pi－po（i）‖）（18）

式中： σ～io=（dd－‖pi－po（i）‖）为避障任务误差； λo（i）为设定的正定增益矩阵； J+o（i）为避障雅可比矩阵的伪逆。

当dd－‖pi－po（i）‖>0时， 表明智能体距离障碍物的距离小于给定的安全距离， 即进入虚拟球体， 需要进行避障任务， 得到的νo（i）=J+o（i）λo（i）σ～io； 反之， 当dd－‖pi－po（i）‖<0时， 表明智能体距离障碍物的距离大于给定的安全距离， 不需要进行避障任务， 得到的νo（i）=0。 避障任务的切换取决于任务误差， 并通过投影到零空间上实现避障速度的求解。

最终得到的合速度解为νG， 当多智能体系统未接近障碍物且未进入安全距离时合速度为

νG=νr+（I－J+gGJgG）νo（i）（19）

当多智能体系统接近障碍物且进入安全距离时合速度为

νG=νo（i）+（I－J+o（i）Jo（i））νr（20）

速度信息传递方式如结构图4所示。 各智能体能够根据其领航者以及其父节点的方位得到其期望的相对角度信息以及与父节点的距离信息以形成完整的编队任务约束。 编队任务与避障任务经过优先级调整后投影到零空间中得到智能体i的合速度， 图中所示避障任务为最高优先级。 最终由各智能体执行合速度并将实际的距离信息和相对角度信息等反馈信息， 传递至期望任务中以实现系统的完整闭环控制。

2.3 带相对角度与距离约束的智能体大范围编队避障

设计的编队任务函数为如下形式：

σr=［pi－pj2， ≮jik］T i∈1， 2， …， n（21）

具体编队方法如3.1节中相同， 而避障过程中采用将多智能體系统视作一个整体以进行大范围的避障。 将所有跟随者的任务仅设计为编队任务， 而避障任务依靠领航者确保较大的安全距离来实现。 具体的领航者避障任务设计与3.2节中相类似， 为

Or=（pi∈瘙綆2， po（i）∈瘙綆2：‖pi－po（i）‖≤dmax）（22）

式中： dmax为多智能体系统各智能体间的最大距离。

注1： 本文提出的编队方案以单积分器模型为例， 聚焦于编队规划模块。 由于行为控制与其他相关方法具有很强的融合性， 即使模型转换后也仍可适用于其他的控制器， 在文献［35， 38-40］中都展示了行为控制与不同物理模型以及不同控制器相结合的实例。

注2： 传统的多智能体基于父节点位置信息进行控制算法设计如下：

σf=［（p1－pb）T， …， （pn－pb）T］T

pb=1/n∑nk=1pk（23）

需要知道各智能体间的相对位置信息以得到质心位置信息形成刚性编队， 式（10）为本文自主设计引入的刚性编队约束， ≮jik为本文引入的相对角度信息用以弱化相对位置信息， 使得系统编队仅需得到智能体间距离信息及相对角度信息便可形成刚性编队， 能够降低对于相对位置信息的依赖。

2.4 带相对角度与距离约束的智能体编队任务稳定性证明

对本文提出的基于距离及相对角度信息的刚性编队规划设计方案的任务稳定性在数学上进行了证明。 其中， 任务稳定性指的是经过零空间投影后， 各任务仍能收敛到其各自稳定状态的性质。 从任务目的出发， 验证了不同优先级任务的稳定性。 假设K=2， 且下标更小的任务优先级更高； 选定李雅普诺夫函数为

Vη=12η～T1γη1η～1+…+12η～TKγηKη～K（24）

式中： η～K为优先级为K的任务设计， 在本文中为σr及σio， 其中由于K=2， 故有

Vη=12η～T1γη1η～1+12η～T2γη2η～2（25）

对式（25）进行求导：

V·η=σ～T2P2σ～2（26）

式中： σ～2=［η～T1， η～T2］T； P2=P2， 11P2， 12P2， 21P2， 22； P2， 11=

γη1Λη1； P2， 21=PT2， 12=12γη2Jη2J+η1Λη1； P2， 22=γη2Jη2（In－J+η1Jη1）J+η2Λη2。 式（26）满足以下不等式：

V·η≤－P^2， 11‖η～1‖2－P^2， 22‖η～2‖2+

2P^2， 21‖η～1‖‖η～2‖=－σ^T2P^2σ^2（27）

式中：

σ^2=［‖η～1‖， ‖η～2‖］T；  P^2=γ1Λ1 γ2Λ1-2γ2Λ1-2 γ2Λ2； Jη1=1； In－J+η1Jη1≤1。 其中γ1是满足条件γ1>γ2（Λ1）24Λ1Λ2的正常数， P^2是一个正定对称矩阵。 式（27）进一步可改写为

V·η≤－λ-P^2σ^T2σ^2≤0（28）

式中： λ-P^2为P^2的最小特征值。

所以可以得到在两个任务的情况下该编队规划方案具有稳定性， 而三个及以上的任务场景证明思路同上， 可参照文献［34， 41-42］进行扩展证明。

3 仿真验证

3.1 仿真例1

本节对所提出的算法进行仿真验证， 通过与基于相对位置的刚性编队方法［16］进行对比， 验证算法的有效性。

假设各智能体通信拓扑结构图如图5所示。

其通信拓扑可以通过邻接矩阵表示：

C=0100000100000100000100000; D=01111。

假设有5个智能体从初始位置p1=［0， 1］， p2=［－1， 2］， p3=［－1， 0］， p4=［1， 0］， p5=［－2， －1］运动到目标点p～1=［30， 30］， p～2=［29， 31］， p～3=［29， 29］， p～4=［31， 29］， p～5=［28， 28］。 在移动路径中有两个障碍物阻挡， 两个障碍物位置为po（1）=［4， 4］， po（2）=［15， 15.5］。 在仿真过程中智能体仅能得到其父节点和领航者的信息， 无法获取其他智能体的信息。 分别利用相对位置信息确定刚性队形的规划方案与带有相对角度与距离约束的规划方案以及基于整体的编队规划方案对智能体进行刚性编队。

首先， 采用基于相对位置信息多智能体刚性编队仿真如图6所示， 为保证刚性编队的要求， 每两个智能体之间需要保持恒定的距离， 从图中可以看出该方法能基本满足刚性编队的要求。 5个智能体各自之间的距离以及各自的相对角度如图7～8所示。

其次， 采用基于整体的多智能体刚性编队仿真如图9所示， 从图中可以看出该方法能很好的满足刚性编队的要求。 5个智能体各自之间的距离以及各自的相对角度如图10～11所示。

最后， 采用基于相对角度与距离信息的多智能体刚性编队仿真如图12所示， 从图中可以看出该方法能基本满足刚性编队的要求。 5个智能体各自之间的距离以及各自的相对角度如图13～14所示。

将上述基于相对位置， 整体编队以及基于距离及相对角度编队三种方法的仿真结果进行对比， 可以得到如图15～16所示的相对角度偏差对比结果图及如图17～18所示的距离偏差对比结果图。

图15～16中（A）为文献［16］所提方法的相对距离， （B）为本文提出的整体编队避障方法的相对角度， （C）为本文提出的基于距离及相对角度的相对角度。

图17～18中（A）为文献［16］所提方法的相对距离， （B）为本文提出的整体编队避障方法的相对距离， （C）为本文提出的基于距离及相对角度的相对距离。

从基于相对位置， 整体编队避障以及基于距离及相对角度编队三种方法所规划的刚性编队队形仿真结果可以得到以下结果：

（1） 基于整体的编队方法可以以维持刚性距离偏差不超过3.3%， 相对角度偏差不超过2.5%且非刚性時间不超过10%的性能很好的在完成整体避障的前提下完成前往目标点与编队任务， 但从仿真轨迹及理论研究表面， 基于整体的避障方法的避障范围较大， 使用该避障方法难以穿过小于dmax的狭窄空间；

（2） 基于相对角度与距离的编队方法可以以维持刚性距离偏差不超过38.3%， 相对角度偏差不超过42.5%且非刚性时间不超过20%的性能很好的在完成分布式避障的前提下完成前往目标点与编队任务， 与文献［16］直接使用相对位置的编队方法对比， 其在距离偏差性能上损失了约14%， 相对角度偏差性能提升了约23%， 非刚性时间性能提升了约2%， 能够在不损失较大编队刚性的前提下， 整个编队过程不需要得到全局的相对位置， 有效的放宽了编队的相对位置约束。

3.2 仿真例2

本节对所提出的算法进行仿真验证， 并进一步与文献［27］方法进行对比验证算法的有效性。

采用文献［27］方法的多智能体刚性编队仿真如图19所示， 从图中可以看出该方法能基本满足刚性编队的要求。 5个智能体各自之间的距离以及各自的相对角度如图20～21所示。

将基于相对位置， 基于距离及相对角度编队及文献［27］所用的3种方法的仿真结果进行对比， 可以得到如图22～23所示的相对角度偏差对比结果图及如图24～25所示的距离偏差对比结果图。

图22～23中（A）为文献［16］所提方法的相对角度， （C）为本文提出的基于距离及角度的相对角度， （D）为文献［27］提出方法的相对角度。

图24～25中（A）为文献［16］所提方法的距离， （C）为本文提出的基于距离及相对角度的距离， （D）为文献［27］提出方法的距离。

由仿真结果能够看出， 本文所提出的刚性编队方法与文献［27］使用方法对比， 其在距离偏差性能上提升了约4%， 相对角度偏差性能提升了约6%， 非刚性时间性能降低了约9%， 在不损失较大编队刚性的前提下， 整个编队过程不需要得到全局的相对位置， 有效的放宽了编队的相对位置约束。

3.3 仿真例3

对所提出的算法的拓扑结构及智能体数量进行修改并进行仿真验证。

假设各智能体通信拓撲结构图如图26所示。

其通信拓扑可以通过邻接矩阵表示：

C=0100100001001000010000000100000001000000010000000; D=0111111。

改变智能体数量及拓扑结构后采用基于相对角度与距离信息的多智能体刚性编队仿真如图27所示， 从图中可以看出该方法能基本满足刚性编队的要求。 5个智能体各自之间的距离及各自的相对角度如图28～29所示。

3.4 仿真例4

对所提出的算法增加了速度扰动并进行仿真验证。 增加速度扰动后采用基于相对角度与距离信息的多智能体刚性编队仿真如图30所示， 从图中可以看出该方法能基本满足刚性编队的要求。 5个智能体各自之间的距离以及各自的相对角度如图31～32所示。 在加入速度扰动后， 本文所提方案仍能够基本满足刚性编队的要求， 相对角度及距离偏差在较小范围内波动， 具备一定的鲁棒性。

4 总  结

针对多智能体刚性编队过程中不采用智能体相对位

置信息的前提下如何保持刚性队形的任务规划问题提出了两种规划方法， 其中基于整体的编队规划方案在刚性编队任务规划过程中采用距离信息及相对角度信息实现多个智能体的刚性编队过程， 在避障过程中将编队看作整体并进行大范围的避障行为， 能够在保证多智能体编队队形稳定前提下完成编队及避障任务； 基于相对角度与距离的编队规划方案在未知智能体相对位置情况下通过对其父节点的距离及相对角度信息进行跟踪， 解决多智能体的刚性编队问题， 能够在保证多智能体编队相对稳定并最终收敛到稳定编队的前提下进行编队避障。

本文提出的方法与直接使用相对位置的编队方法相比， 其损失了较小的距离偏差性能， 提升了相对角度偏差性能以及部分非刚性时间性能， 能够在不损失较大编队刚性的前提下， 有效的放宽编队的约束。 本文所提的多智能体系统刚性编队任务规划方案具有良好的推广性， 在引入智能体的精确模型后， 进一步融合复杂控制方法， 可以扩展应用场景。

参考文献：

［1］ Martin J G，  Muros F J，  Maestre J M，  et al. Multi-Robot Task Allocation Clustering Based on Game Theory［J］. Robotics and Autonomous Systems，  2023，  161： 104314.

［2］ Flores-Resendiz J F，  Avilés D，  Aranda-Bricaire E. Formation Control for Second-Order Multi-Agent Systems with Collision Avoidance［J］. Machines，  2023，  11（2）： 208.

［3］ Lin J，  Miao Z Q，  Wang Y N，  et al. Aggressive Formation Tracking for Multiple Quadrotors without Velocity Measurements over Directed Topologies［J］. IEEE Transactions on Aerospace and Electronic Systems，  2023，  59（5）： 5541-5553.

［4］ Kankashvar M，  Bolandi H，  Mozayani N. Multi-Agent Q-Learning Control of Spacecraft Formation Flying Reconfiguration Trajectories［J］. Advances in Space Research，  2023，  71（3）： 1627-1643.

［5］ Guo J J，  Qi J T，  Wang M M，  et al. Distributed Cooperative Obstacle Avoidance and Formation Reconfiguration for Multiple Quadrotors： Theory and Experiment［J］. Aerospace Science and Technology，  2023，  136： 108218.

［6］ Chen L M，  Cao M，  Li C J. Angle Rigidity and Its Usage to Stabilize Multiagent Formations in 2-D［J］. IEEE Transactions on Automatic Control，  2021，  66（8）： 3667-3681.

［7］ Sial M B，  Zhang Y W，  Wang S P，  et al. Bearing-Based Distributed Formation Control of Unmanned Aerial Vehicle Swarm by Quaternion-Based Attitude Synchronization in Three-Dimensional Space［J］. Drones，  2022，  6（9）： 227.

［8］ Li P，  Cao J，  Liang D C. UAV-BS Formation Control Method Based on Loose Coupling Structure［J］. IEEE Access，  2022，  10： 88330-88339.

［9］ Wang H，  Zhang J X，  Zhou Q R，  et al. Optimal Attitude Consensus Control for Rigid Spacecraft Formation Based on Control Lyapunov Functions［J］. IET Control Theory & Applications，  2023，  17（12）： 1720-1731.

［10］ Sun H J，  Wu Y Y，  Zhang J X. A Distributed Predefined-Time Attitude Coordination Control Scheme for Multiple Rigid Spacecraft［J］. Aerospace Science and Technology，  2023，  133： 108134.

［11］ Hendrickx J M，  Anderson B D O，  Delvenne J C，  et al. Directed Graphs for the Analysis of Rigidity and Persistence in Autonomous Agent Systems［J］. International Journal of Robust and Nonlinear Control，  2007，  17（10/11）： 960-981.

［12］ Krick L，  Broucke M E，  Francis B A. Stabilization of Infinitesimally Rigid Formations of Multi-Robot Networks［C］∥47th IEEE Conference on Decision and Control，  2008： 477-482.

［13］ Aranda M，  López-Nicolás G，  Sagüés C，  et al. Coordinate-Free Formation Stabilization Based on Relative Position Measurements［J］. Automatica，  2015，  57： 11-20.

［14］ Aspnes J，  Eren T，  Goldenberg D K，  et al. A Theory of Network Localization［J］. IEEE Transactions on Mobile Computing，  2006，  5（12）： 1663-1678.

［15］ Olfati-Saber R，  Murray R M. Graph Rigidity and Distributed Formation Stabilization of Multi-Vehicle Systems［C］∥41st IEEE Conference on Decision and Control，  2002： 2965-2971.

［16］ Zhou N，  Xia Y Q. Coordination Control Design for Formation Reconfiguration of Multiple Spacecraft［J］. IET Control Theory & Applications，  2015，  9（15）： 2222-2231.

［17］ Arrichiello F，  Chiaverini S，  Fossen T I. Formation Control of Underactuated Surface Vessels Using the Null-Space-Based Behavioral Control［C］∥IEEE/RSJ International Conference on Intelligent Robots and Systems，  2006： 5942-5947.

［18］ 劉国波. 基于方位刚性的多智能体主从编队控制研究［D］. 成都： 西南交通大学，  2019.

Liu Guobo. Research on Multi-Agent Leader-Following Formation Control Based on Bearing［D］.Chengdu： Southwest Jiaotong University，  2019. （in Chinese）

［19］ 肖志旋. 基于方位刚性理论的多智能体编队控制方法研究［D］. 长沙： 中南大学，  2022.

Xiao Zhixuan. Research on Multi-Agent Formation Control Methods Based on Bearing Rigidity Theory［D］.Changsha： Central South University，  2022. （in Chinese）

［20］ Yang Q K，  Cao M，  Fang H，  et al. Constructing Universally Rigid Tensegrity Frameworks with Application in Multiagent Formation Control［J］. IEEE Transactions on Automatic Control，  2019，  64（1）： 381-388.

［21］ Yang Q K，  Sun Z Y，  Cao M，  et al. Stress-Matrix-Based Formation Scaling Control［J］. Automatica，  2019，  101： 120-127.

［22］ Chen L M，  Yang Q K，  Shi M M，  et al. Stabilizing Angle Rigid Formations with Prescribed Orientation and Scale［J］. IEEE Transactions on Industrial Electronics，  2022，  69（11）： 11654-11664.

［23］ Wang L，  Zou Y，  Meng Z Y. Coordinated Localization and Circumnavigation of Multi-Agent Systems： A Distributed Observer-Based Approach［C］∥IEEE 15th International Conference on Control and Automation （ICCA），  2019： 1540-1544.

［24］ Zou Y，  Wang L，  Meng Z Y. Distributed Localization and Circumnavigation Algorithms for a Multiagent System with Persistent and Intermittent Bearing Measurements［J］. IEEE Transactions on Control Systems Technology，  2021，  29（5）： 2092-2101.

［25］ Guo Y H，  Zhou J，  Liu Y Y. Distributed RISE Control for Spacecraft Formation Reconfiguration with Collision Avoidance［J］. Journal of the Franklin Institute，  2019，  356（10）： 5332-5352.

［26］ Panagou D. A Distributed Feedback Motion Planning Protocol for Multiple Unicycle Agents of Different Classes［J］. IEEE Transactions on Automatic Control，  2017，  62（3）： 1178-1193.

［27］ Zhou N，  Cheng X D，  Sun Z Q，  et al. Fixed-Time Cooperative Behavioral Control for Networked Autonomous Agents with Second-Order Nonlinear Dynamics［J］. IEEE Transactions on Cyberne-tics，  2022，  52（9）： 9504-9518.

［28］ Xia Y Q，  Zhang J H，  Lu K F，  et al. Coordination Control Design for Formation Reconfiguration of Multiple Spacecraft［M］∥ Finite Time and Cooperative Control of Flight Vehicles. Singapore： Springer，  2019： 325-345.

［29］ 安凯，  郭振云，  黄伟，  等. 低/高速飞行器系统编队协同控制方法研究进展［J］. 航空兵器，  2022，  29（5）： 53-65.

An Kai，  Guo Zhenyun，  Huang Wei，  et al. Research Progress of Formation-Cooperative Control Methods for Low-Speed and High-Speed Vehicle Systems［J］. Aero Weaponry，  2022，  29（5）： 53-65.（in Chinese）

［30］ 赵超轮，  戴邵武，  何云风，  等. 基于DMPC的多无人机轨迹跟踪与避碰切换控制［J］. 航空兵器，  2023，  30（6）： 64-74.

Zhao Chaolun，  Dai Shaowu，  He Yunfeng，  et al. Trajectory Tracking and Collision Avoidance Switching Control for Multi-UAV Based on DMPC［J］. Aero Weaponry，  2023，  30（6）： 64-74.（in Chinese）

［31］ Huang J，  Zhou N，  Cao M. Adaptive Fuzzy Behavioral Control of Second-Order Autonomous Agents with Prioritized Missions： Theory and Experiments［J］. IEEE Transactions on Industrial Electronics，  2019，  66（12）： 9612-9622.

［32］ Zheng C B，  Pang Z H，  Wang J X，  et al. Null-Space-Based Time-Varying Formation Control of Uncertain Nonlinear Second-Order Multiagent Systems with Collision Avoidance［J］. IEEE Transactions on Industrial Electronics，  2023，  70（10）： 10476-10485.

［33］ Golluccio G，  Di Lillo P，  Marino A，  et al. When Local Optimization is Bad： Learning what to （not） Maximize in the Null-Space for Redundant Robot Control［C］∥9th International Conference on Control，  Decision and Information Technologies （CoDIT），  2023： 506-511.

［34］ Wang C H，  Wei Z Q，  Jiang W J，  et al. Cooperative Sensing Enhanced UAV Path-Following and Obstacle Avoidance with Variable Formation［J］. IEEE Transactions on Vehicular Technology，  2024（99）： 1-16.

［35］ Zhang Z Y，  Chen J F，  Mo Z B，  et al. A Behavior-Based Adaptive Dynamic Programming Method for Multiple Mobile Manipulators Coordination Control［J］. International Journal of Control，  Automation and Systems，  2023，  21（9）： 3022-3035.

［36］ 郭竹森. 基于北斗的飛行器落点定位及地面搜寻技术研究［D］. 太原： 中北大学，  2021.

Guo Zhusen. Based on BeiDous Aircraft Landing Point Positioning and Ground Search Technology Research［D］.Taiyuan： North University of China，  2021. （in Chinese）

［37］ 姚博誉，  路平，  杨森，  等. 四旋翼飞行器飞行控制技术综述［J］. 航空兵器，  2020，  27（1）： 9-16.

Yao Boyu，  Lu Ping，  Yang Sen，  et al. An Overview of Flight Control Technology for Quadrotor Aircrafts［J］. Aero Weaponry，  2020，  27（1）： 9-16.（in Chinese）

［38］ Chen J，  Gan M G，  Huang J，  et al. Formation Control of Multiple  Euler-Lagrange Systems via Null-Space-Based Behavioral Control［J］. Science China Information Sciences，  2016，  59（1）： 1-11.

［39］ Chen Y T，  Zhang Z Y，  Huang J. Dynamic Task Priority Planning for Null-Space Behavioral Control of Multi-Agent Systems［J］. IEEE Access，  2020，  8： 149643-149651.

［40］ Huang J，  Mo Z B，  Zhang Z Y，  et al. Behavioral Control Task Supervisor with Memory Based on Reinforcement Learning for Human-Multi-Robot Coordination Systems［J］. Frontiers of Information Technology & Electronic Engineering，  2022，  23（8）： 1174-1188.

［41］ Huang J，  Wu W H，  Zhang Z Y，  et al. A Human Decision-Making Behavior Model for Human-Robot Interaction in Multi-Robot Systems［J］. IEEE Access，  2020，  8： 197853-197862.

［42］ Antonelli G. Stability Analysis for Prioritized Closed-Loop Inverse Kinematic Algorithms for Redundant Robotic Systems［J］. IEEE Transactions on Robotics，  2009，  25（5）： 985-994.

Rigid Formation Task Planning for Multi-Agent Systems

Based on Distance and Relative Angle Information

Huang Jie1， 2*， Li Junjie1， 2

（1. Faculty of Electrical Engineering and Automation， Fuzhou University，  Fuzhou 350108， China；

2. 5G+Industrial Internet Research Institute， Fuzhou University， Fuzhou 350108， China）

Abstract： This paper proposes two rigid formation task planning schemes for multi-agent systems to address the problem of maintaining rigid formation task planning under the condition of only obtaining local distance and relative angle information of multi-agent systems. Firstly，  a rigid formation planning scheme based on distance and relative angle is proposed to ensure the rigid formation of intelligent agents in unknown relative positions. Secondly，  the task planning method of null space projection is used to solve the conflict between formation tasks and obstacle avoidance tasks，  ensuring the stability of formation during the movement process. An overall rigid formation planning scheme is proposed that combines distance and relative angle again，  and  the multi-agent system is considered as a whole for obstacle avoidance during the obstacle avoidance process，  ensuring stable rigid formation and the completion of obstacle avoidance tasks. After simulation verification，  the two methods proposed in this paper can maintain a rigid formation and complete obstacle avoidance tasks well under the condition of weakening relative position information. Compared with the null space behavior control method that relies on relative position，  it weakens the dependence on relative position information.

Key words： rigid formation; task planning; multi agent systems; relative angle information; distance information