CN115903814B - 基于凸多边形树的多机器人最优编队路径规划方法 - Google Patents

Abstract

本发明公开了基于凸多边形树的多机器人最优编队路径规划方法，首先，通过激光扫描或SLAM算法采集障碍物的点云数据；然后基于扩展方向、障碍物以及现有凸多边形节点生成新凸多边形，并将旅行代价最小的凸多边形树最为初始编队路径；基于编队构型变换最少原则，将编队构型附加至初始编队路径中，然后基于直线最短原则对初始编队路径优化，以使机器人在连续编队构型情况下以直线通过凸多边形，从而找到机器人最优编队路径；通过不断对新生成的凸多边形检测，从而使机器人以最优编队路径到达目标点。本发明采用上述路径规划方法，提高了多机器人路径规划效率，有助于多机器人在复杂环境的作业。

Description

基于凸多边形树的多机器人最优编队路径规划方法

技术领域

本发明涉及机器人路径规划技术领域，尤其是涉及基于凸多边形树的多机器人最优编队路径规划方法。

背景技术

在大型、杂乱的二维环境下，可重构多机器人系统的许多应用均需考虑最优编队路径规划问题，如一群机器人在充满钟乳石的大洞穴中形成一个平台，以移动一个重物。在这些情况下，机器人在一个或多个编队中互连和机动，以平衡重型有效载荷，或维持水下有线通信，或协同拒绝过度的外部干扰。如果有必要和环境允许，多机器人系统可以在路径的某些位置重新配置为不同的编队构型，同时编队的机器人几何形状发生变化。这种情况下，现有系统和方法很难满足路径规划需求，致使多机器人运行效率低下、事故频发。因此，提出一种适用于可重构多机器人的路径规划方法十分必要。

发明内容

本发明的目的是提供一种适用于可重构多机器人的路径规划方法，在机器人行进过程中，以编队构型变化最少原则和路径最短原则对路径约束，从而找到最优编队路径，进而解决可重构多机器人在大型、复杂环境运行效率低的问题。

为实现上述目的，本发明提供了如下技术方案：

基于凸多边形树的多机器人最优编队路径规划方法，包括如下步骤：

S1、障碍物生成；通过激光扫描或SLAM算法采集障碍物的点云数据；

S2、凸多边形位置采样；基于扩展方向、障碍物点云数据以及现有凸多边形节点，在工作空间的非障碍物位置找到用于生成新凸多边形的位置坐标；

S3、凸多边形生成与连接；以新凸多边形的位置坐标为基点建立椭球体，通过最大化椭球体以及对椭球体与障碍物的分界更新，从而扩展出新凸多边形；对新凸多边形与现有凸多边形进行交集空间检测，并判断交集空间内是否存在允许机器人通行的编队构型；

S4、凸多边形树生成；将新凸多边形加入到凸多边形树中，然后重新对凸多边形树进行总旅行代价计算，并输出总旅行代价最小的凸多边形树，最终得到的凸多边形树内凸多边形的质点连接顺序即为机器人初始编队路径；

S5、凸多边形树优化；基于机器人编队构型变换次数最少原则，将编队构型附加至初始编队路径中，然后以直线最短原则对初始编队路径优化，以使机器人在连续编队构型情况下以直线通过凸多边形，从而找到机器人最优编队路径；

S6、对步骤S2-S5进行迭代，直至新凸多边形与目标点重合。

优选的，S2步骤基于障碍物感知采样器完成，障碍物感知采样器包括自动压缩编码器和混合密度网络；障碍物感知采样传感器将障碍物的点云数据压缩为一维向量空间的编码，将障碍物的编码、现有凸多边形节点以及扩展方向进行聚合并将其作为混合密度网络的输入向量，混合密度网络基于高斯分布混合模型对新凸多边形的位置坐标进行预测，直至收集到的新凸多边形的位置坐标或迭代次数达到阈值。

优选的，S3步骤中，计算交集空间与机器人编队构型空间的闵柯夫斯基差，以判断交集空间内是否存在允许机器人通行的编队构型以及编队构型的种类和数量。

优选的，S4步骤中，将新凸多边形加入到凸多边形树中，计算新凸多边形与其质心范围内的所有现有凸多边形的旅行代价，并将新凸多边形连接到旅行代价最小的现有凸多边形上；重新对新生成的凸多边形树的总旅行代价进行检测，以使凸多边形树整体的总旅行代价最小。

优选的，S5步骤中，统计每个凸多边形内机器人的编队构型，若两个不同编队构型之间的交集空间允许机器人保持编队构型通过，则将该编队构型附加到初始编队路径中；然后通过一条直线构造一对连续编队构型之间的路径，以使机器人以最短路径通过凸多边形。

本发明采用上述基于凸多边形树的多机器人最优编队路径规划方法，首先通过旅行代价找到最优凸多边形路径，然后基于直线最短原则和编队构型变换最少原则对最优凸多边形路径优化，从而找到最优编队路径。本方法实现了狭窄、复杂空间下的多机器人编队构型以及路径的寻优求解，提高了多机器人路径规划效率。

附图说明

图1为本发明方法的整体流程图；

图2为本发明实施例中CPT*的拓扑图；

图3为本发明实施例中障碍物感知采样器的结构；

图4为本发明实施例中MDN模型结构；

图5为本发明实施例中凸多边形交集空间示例；

图6为本发明实施例中闵可夫斯基差计算原理图；

图7为本发明实施例中CPT*生成过程的节点生成(a)和节点重连(b)；

图8为本发明实施例中旅行成本的有界估计误差示例图；

图9为本发明实施例中OAS的性能测试图；

图10为本发明实施例中OAS(a)与均匀采样器(b)的性能比较；

图11为本发明实施例中新凸多边形从采样的多边形位置展开；

图12为本发明实施例中最优凸多边形路径向最优编队路径转化图；

图13为本发明实施例中CPT*与COM之间计算时间的比较；

图14为本发明实施例中CPT*与COM之间的路径长度的比较；

图15为本发明实施例中狭窄的通道中进行规划的实例。

具体实施方式

以下结合附图和实施例对本发明的技术方案作进一步说明。

基于凸多边形树的多机器人最优编队路径规划方法整体流程如图1所示，本实例将从八个部分对最优编队路径规划进行阐述，包括问题定义、凸多边形生成器、凸多边形位置采样、凸多边形生成和连接、凸多边形树生成、凸多边形树优化、最优性退化有界、仿真。

一、问题定义

本方案研究了在布满障碍的二维工作空间中，多机器人编队从初始构型初始编队/>到目标构型集/>目标编队/>的最优编队路径规划问题。工作空间定义为/>其中L和D为工作空间尺寸。工作空间W由多个障碍物/>填充，其中为障碍物总数，/>为第i个障碍物在工作空间中的占用情况。假定障碍物的几何形状和位置是已知且无误差的，并以激光扫描或SLAM算法中的点云表示。

可重构多机器人编队在执行任务时必须相互连接并保持一定队形，例如，平衡重负载或协作拒绝过度干扰。在编队路径的某些位置，如果工作空间允许，机器人编队可以改变编队构型继续前进。设表示由n个预定义编队构型组成的索引集。第i组的几何形状由/>给出，例如正方形或直线组。值得指出的是，为了简化，机器人的编队构型几何是机器人几何形态联合的最小凸包。设编队构型定义为构型空间C中的/>然后用/>表示多机器人编队i在构型q下的工作空间占用情况。设编队状态s定义为编队构型q和所选编队I的指数的聚集，使/>其中/>然后，在状态s处的工作空间中的编队占用情况方便地表示为A(s)。

定义1(编队路径)。编队路径φ(τ)是从[0,1]到构型空间C和的联接空间的映射，其在C中的分量是连续的。

定义2(可行编队路径)。可行编队路径φ(τ)是对于所有τ∈[0，1]和的编队路径。可行编队路径也称为无碰撞编队路径。

定义3(强弱间隙)。如果沿该路径的编队到障碍物的最小距离为δ，则编队路径Φ具有强δ间隙。如果存在同伦变换π(α)，则称编队路径Φ具有弱δ间隙，使π(α＝0)＝φ，π(α＝1)具有强δ间隙，即π(α＝(0，1])具有强δα间隙。

定义4(同伦和同伦变换)。如果两个路径φ₁和φ₂有共同的端点，且φ₁可以通过同伦变换π(α)连续地转化为φ₂，那么路径φ₁和φ₂是同伦的。如此，目标点保持不变，路径沿α∈[0，1]的变化保持在构型空间中，则π(α＝O)＝φ₁，π(α＝1)＝φ₂。

针对多机器人编队最优编队路径规划问题，提出了一种的多机器人编队最优编队路径规划方法，累积的平移距离和变换次数都需要优化。

问题1。给定一个带有杂乱障碍的工作空间W，一个初始编队状态和一组目标编队状态找到一个无碰撞的编队路径φ(τ)，τ∈[0，1]，使φ(0)＝s₀，φ(1)∈g,旅行代价J(φ)最小化。假设问题的最优解Φ存在弱δ间隙。

定义5(可转换)。使得在凸多边形P内的编队A_i(p)和编队A_i(q)的并集的凸包H是P的子集，从而可以将P内的编队A_i(p)转化为P内的编队A_i(q)。

定义6(节点连接)。如果两个凸多边形节点P_i和P_j的交点允许至少一个多机器人编队，即存在的编队状态s，那么P_i和P_j就可以连接。

定义7(凸多边形路径)。凸多边形路径Φ是凸多边形的序列，相邻凸多边形可以连接，其中card(·)表示集合的基数，/>是Φ中凸多边形节点的索引集。

定理1(凸多边形路径中存在编队路径)。一个可行的编队路径φ存在于凸多边形路径Φ中，使得并且对于所有τ∈[0，1]，/>使得/>其中/>是Φ中凸多边形节点的索引集。

证明。根据定义7，在和/>的交点/>存在s_i，i+1。考虑了两种情况。①如果s_i，i+1和s_i+1，i+2具有相同的编队状态，则/>和/>的凸包/>之间保证有一条直线路径。由于凸包/>显然存在于/>中，在这种情况下，存在一条直线路径。②当s_i，i+1和s_i+1，i+2没有相同的编队状态。既然存在一个q使两种编队都存在于中，那么就存在q和s_i，i+1之间的路径以及q和s_i+1，i+2之间的路径。

定义8(两个凸多边形之间的旅行成本)。P_i和P_j之间的移动代价L(P_i，P_j)为P_i和P_j质心之间的距离。

定义9(路径成本)。与凸多边形路径Φ相关联的代价J_Φ被定义为Φ中连续凸多边形之间代价的累计值。

定义10(最优凸多边形路径)。最优凸多边形路径Φ^*为J_Φ最小的凸多边形路径。

为了简单起见，现在假设最优凸多边形路径由有限数量的凸多边形组成。这个假设是合理的，因为不鼓励转换。CPT*提供了一个拓扑图，如图2所示。每个凸多边形由一个不规则的五边形表示。注意，图2中图的每个连接都是双向的。一个凸多边形P与其他凸多边形相交，这些相交的凸多边形被称为P的“邻居”。

二、凸多边形生成器

与RRT*中的构型节点不同，CPT*中的节点是凸多边形P，每个凸多边形是自由工作空间的凸子集，它允许该凸多边形内存在一些编队，即/>使得/>和/>值得指出的是，与编队构型I和凸多边形P_i相关的可行编队构型集合/>可以通过计算闵柯夫斯基差值来生成。

因此，在阶段I中对工作空间进行采样，捕获工作空间的连通性，并最小化每个凸多边形的生成由3步组成：步骤(i)对凸多边形位置o∈ω_free进行采样；步骤(ii)从其采样位置开始增长和扩展凸多边形，通过在超平面内膨胀椭球体和更新将椭球体与障碍物分隔的超平面之间的迭代实现；超平面定义为椭球体(或凸多边形扩张过程中第一次迭代时的凸多边形位置)与其周围障碍边界之间的分隔平面；步骤(iii)确定采样的凸多边形是否可以连接到CPT*中的节点。为了扩大CPT*，被采样的凸多边形必须连接到CPT*，因此如果采用均匀采样策略，在有杂乱障碍物的工作空间中采样一个可连接凸多边形可能需要经过(i)到(iii)几十次迭代。

因此，有必要考虑一个有效的采样器，它可以采样具有更高概率的凸多边形位置，从而得到可连接的凸多边形。限制或避免采样低成功区域的凸多边形位置是必要的。然而，这样的要求最终陷入了一个两难的境地：成功率更好的区域需要通过迭代步骤(i)—(iii)对这些区域进行抽样。

三、凸多边形位置采样

在步骤(i)中随机生成一个凸多边形位置，作为步骤(ii)中椭球体的生长点。而生成概率较高的位置对于生成可行且可连接的凸多边形至关重要。这样的凸多边形很大程度上是由障碍物数量决定的。本方案提出的障碍物感知采样器(OAS)将感兴趣区域内的障碍物作为输入的一部分。这是因为凸多边形只会干涉它周围的障碍物，而且当工作空间相当大时，考虑所有障碍物是不切实际的。

OAS只考虑固定大小窗口(即感兴趣区域)内的障碍物，其位置由运行时CPT*的预期扩展面积决定。扩展方向(或目标位置)u和待连接的节点也是OAS的部分输入。固定大小窗口是工作空间的子集/>其中/>设O表示工作空间内障碍物的点云数据。

随着现代传感器和感知算法的发展，在线获得的障碍物表示通常缺乏封闭形式，例如来自立体摄像机或SLAM模块的点云。障碍物O是二维点云，但在高维空间中是无序的，因此不适合作为凸多边形位置采样的输入。因此，OAS是建立在两个神经网络模块上的，一个是自动压缩编码器(CAE)，一个是混合密度网络(MDN)，如图3所示。

CAE模块将障碍物O压缩为一维向量空间(潜在特征)中的编码e。e的大小在网络训练过程中也得到了收敛。CAE的编码器包括三个线性层和一个输出层，其中三个线性层分别包含512、256和128个隐藏神经元。每一线性层后面是一个PReLU激活层。解码器与编码器相反，仅在CAE训练过程中使用。

将障碍物编码e与(待连接节点)和u(扩展方向或目标位置)聚合，作为对MDN模块输入，如图3所示。MDN常用于分类问题，它从嵌入最后一层的高斯分布混合模型中输出样本。这里使用的MDN由三个线性层组成，每层后面有一个ReLu激活函数，每层神经元数量为256。在本方案中，MDN将局部障碍物和CPT*的生长意图(即/>和u)作为输入向量，以近似凸多边形位置分布。设/>表示待连接节点P的顶点位置，设表示P的向量化表示，设MDN的输入定义为MDN模块的结构如图4所示，其最后一层隐藏层实际上输出的是高斯分布混合模型的参数。

第i个高斯分量的参数包括均值μ_i(λ)和标准差σ_i(λ)，以及一个表示高斯分布概率的系数π_i。密度函数为

其中，X和Λ分别为输出和输入的随机变量，n_m为高斯分布的个数，第i个分布的密度记为系数的归一化由神经网络中的softmax函数实现，σ_i(λ)由指数函数确定，如下所示。

σ＝exp(H_n+1→3n) (4)

μ＝H_3n+1→4n (5)

其中H表示最后一个隐藏层的输出向量，Ha→b表示H中的第a项到第b项构成的向量。然后，MDN模块输出生成可连接凸多边形时成功率较高的凸多边形位置的样本，如

值得指出的是，ω(OAS感兴趣的局部区域)在W(工作空间)中的占用是由(待连接节点)和u(扩展方向或目标位置)共同决定的，ω与/>重叠，且沿u方向的长度最大，如图5所示。

离线训练:当给出一个新的编队路径规划问题时，在线使用OAS，分两步离线训练OAS。第一步，给定随机生成的障碍物的大量窗口空间ω数据集，联合训练CAE的编码器和解码器。第二步是基于训练好的CAE和成功样本的输出来训练MDN，后者是像文献“Multi-robot formation control and object transport in dynamic environments viaconstrained optimization Multi-robot formation control and object transportin dynamic environments via constrained optimization”那样通过试错获得的。

在第一步中，随机生成大量l*d尺寸的小工作空间具有不同的障碍物。设/>分别表示/>中的障碍点云。为了简化交叉验证的符号，以同样的方式对CAE和MDN的测试数据进行了额外抽样。然后给出训练CAE所使用的损失函数。

其中，θ^e和θ^d是编码器和解码器参数，是原始点云O_i重构的点云，ρ是正则化系数。

在第二步中，对每个采样的窗口空间，随机抽取n_p对(即和u)。在一个窗口空间中这样的一对称为一种情况。对于每一种情况，重复步骤(i)到(iii)，每次迭代都可以产生一个凸多边形位置坐标，该位置坐标可以生成一个凸多边形/>该凸多边形/>可以以定义6的方式连接到/>对每种情况继续迭代，直到收集到至少n_c个成功的凸多边形位置坐标/>或达到最大迭代次数。因此，训练数据/>其中n_x＝n_wn_pn_c为训练数据规模。

MDN以作为输入和输出进行训练，损失函数见公式(8)。

为了防止反向传播过程中的梯度消失或梯度爆炸，将训练CAE和MDN的学习率和剪辑阈值分别设置为1e-4和0.5。

四、凸多边形生成与连接

在生成CPT*的每个步骤中，使用训练过的OAS对一个凸多边形位置x_new进行采样，该位置进一步用于在工作空间中生成一个新凸多边形如图5所示。通过更新超平面和在超平面封闭区域内最大化一个椭球体的迭代，/>的形状和大小逐渐收敛。在文献“Computing large convex regions of obstaclefree space through semidefiniteprogramming”中详细介绍了在自由工作空间中展开凸多边形的方法。

需要连接到CPT*中的现有凸多边形节点，否则会浪费。通过聚合输入λ到MDN，提高了成功连接的机会。如果找不到联系，则重复(二)中步骤(i)到(iii)。在CPT*中，可以与多个凸多边形相交。设/>表示可连接到/>的节点集合。

如果和/>之间存在交集空间/>那它也是一个凸多边形，可以通过以下步骤方便地得到。第一步若/>的顶点存在于/>中，则将/>的顶点添加到列表l中。第二步将凸多边形边缘的所有交点添加到列表l中。第三步将l中的所有点逆时针排列并形成交集。计算复杂度O(n_l)，其中n_l是两个凸多边形顶点的总数。

由于假设编队的几何形状为凸形，和/>是否可连接在定义6中得到了正式定义，并由/>中是否存在编队状态决定。如果/>中存在至少一种编队状态，那么/>和/>可以连接。在不损失一般性的情况下，/>空间下的第j种机器人编队构型/>的存在可以由/>减去/>的Minkowski差(闵柯夫斯基差)来确定。

闵柯夫斯基差可以分三步得到。第一步得到编队几何形状的中心对称凸多边形第二步沿着/>的边界扫过/>从而得到图6中的绿色区域(中心区域)。在第三步中，/>中/>的闵柯夫斯基差是紫色区域(外环区域)，由/>减去绿色区域得到。如果闵柯夫斯基差不为空，则交集/>内存在几何形状/>的编队状态。

如果采样的凸多边形不能连接到CPT*，则舍弃，并按照(二)的步骤(i)—(iii)重新生成另一个凸多边形。

五、凸多边形树生成

是凸多边形节点/>连通性矩阵/>和估计从根节点到所有节点的最小移动距离的累积旅行代价向量/>的集合，其中/>是/>中凸多边形的索引集，/>是CPT*中凸多边形节点的数量。注意，n_T随着树的扩展而增长。

一开始，CPT*只包含作为根节点。与此根节点相关的累计旅行成本被设置为零。CPT*随着经典SBP的逼近而增长，迭代过程为：(a)随机选取/>中的一个节点作为/>(b)抽样一个新的凸多边形节点/>(c)将/>连接到/>(d)重新连接并在必要时更新/>(e)更新与每个节点相关的最小代价

在步骤(a)中，使用离散均匀分布对进行抽样，该节点将有很高的概率连接到新凸多边形节点/>可以探索更好的/>采样策略，以偏置CPT*的增长。向量u是由和/>决定的，这样窗口空间ω可以最大化地探索目标区域/>

在步骤(b)中，当输入可用时，可以使用凸多边形生成器对凸多边形节点/>进行采样。采样/>后，通过计算Minkowski差来检测/>到CPT*中/>质心范围内的所有凸多边形的连接关系，得到可连接的节点集合也称为/>的“邻居”。事实上，这个邻居集类似于经典RRT*方法中的节点邻居集。/>最终连接到步骤(c)中/>累积成本最低的/>上。/>可通过如下公式计算

其中为旅行成本，P定义为树中第k个凸多边形节点。

之后，在步骤(d)中，将中除/>以外的节点重新连线，类似于经典的RRT*算法，从根处获得这些节点更多的最优路径。若满足公式(10)，重新连接节点到新的父节点/>

经过多次采样，得到凸多边形树该/>连接初始节点和目标节点，如图1所示。CPT*示例的构造如图7所示，其中图7(a)显示了一个被采样的新凸多边形(用虚线框突出显示)，它连接到/>中的一个节点，该节点导致这个新凸多边形节点的移动成本最小。新添加的凸多边形节点之间的连接显示在虚线中。图7(b)显示了一个连接过程，其中虚线圈显示了/>的邻居凸多边形可能存在的区域。节点/>连接到/>并重新连接到/>从而获得更好的成本。/>和/>之间的联系如虚线所示，在/>中被剔除。/>和之间的连接显示在虚线中，并被添加到/>

如果中的节点数量达到阈值或凸多边形路径的成本改进收敛于零，则阶段I终止。阶段I输出最优凸多边形路径(即初始编队路径)，记为/>用于阶段II求解最优编队路径。

六、凸多边形树优化

最优凸多边形路径实际上是自由工作空间中相交凸子集的序列。阶段II构建一个优化问题，求解最优编队路径φ(τ)，τ∈[0，1]。

Φ^*中的凸多边形由重新编号，其中/>是Φ^*中的凸多边形的总数量。当确定每个凸多边形内的编队时，可生成自由构型空间。然而，两个不同形态的凸多边形之间横向的可行性必须通过凸多边形的交集来保证。设/>表示连续凸多边形/>和/>之间的交集，设/>表示交集/>中允许的编队的索引集，如图2所示。为了使路径上的变换数量最小化。沿着路径的编队构型变化需要最小化，这可以通过列举所有可能性轻松解决。

设表示沿Φ^*的最优形成选择，其变换次数最小化。然后，可以制定一个有约束的二次优化问题，并用于解决Φ^*。本优化问题中待优化变量的集合Q有两种类型，与Q相关的凸约束的集合/>也有两种类型。第一种类型是自由构型空间C_{free，i，i+1}中形成编队构型q_i，i+1，其建立在交集工作空间/>上。第二种类型是在构型空间/>中形成编队构型q_i,其建立在凸多边形/>上且/>请注意，如果/>q_i或/>不会出现在集合Q或集合/>中。

在Φ^*的第i个凸多边形中，编队构型的自由构型空间/>可以由Minkowski差得到，同样的，交点/>上的编队构型/>的自由构型空间也可由Minkowski差得到。自由构形空间/>和/>都是凸的。

重新索引Q和相关凸约束中的变量，让/>表示索引集。令/>表示Q中第i个变量q_i的约束条件，则优化可表述为

通过求解

得到关键的最优编队状态求解过程可由现有工具完成，如文献“Sdpt3—a matlab software package for semidefinite programming,version1.3”中的SDPT3或文献“Snopt:An sqp algorithm for large-scale constrainedoptimization”的SNOPT。

最优编队路径φ^*可以通过将分配给沿着初始编队路径(最优凸多边形路径)的节点(例如/>)，并通过填充这些关键最优编队状态之间的状态来获得。在编队空间中，通过一条直线构造一对连续关键编队状态之间的路径，例如φ(τ_i)和φ(τ_i+1)之间的向量段为

其中，τ∈[τ_i，τ_i+1]。

值得指出的是，当I_i≠I_i+1时，多机器人编队在τ_i处变换，其可行性由凸多边形的连通性来保证。如定理1所述，在凸约束的集合中保证连续编队状态之间的直线。

七、最优性退化有界

为实现编队路径规划的可扩展，需要对本方案的退化是否有界验证。首先分析了由于凸多边形之间的成本估计(见定义8)与真实成本(即公式(11)中的拉格朗日量)不匹配而导致的最优性退化，证明了最优性退化是有界的。之后，在给定固定的采样密度下，计算复杂度与整个搜索空间的勒贝格测度呈线性关系。

A.最优性退化

引理1(凸多边形间旅行成本的有限估计误差)。在不丧失一般性的前提下，假设P₁、P₂、P₃和P₄四个连通的凸多边形，并假设选择了编队。然后将它们的空闲编队空间表示为C₁、C₂、C₃和C₄。由式(12)得到的最优编队状态分别记为s₁、s₂、s₃和s₄。定义通过凸多边形P₃的旅行成本估算为最优成本为/>结果表明，与/>的差值为/>

证明。如图8所示，横移的最小代价是构型空间/>和/>之间最短的线。的值由/>的中心决定。这些中心实际上是凸多边形中最大椭球的质心。

由于凸多边形在l*d尺寸的小工作空间内采样，与/>中心之间的最大距离小于/>同理C3与C4之间的距离也小于/>因此，估计误差小于/>/>

B.计算复杂度

分析在线规划中关于工作空间大小的CPT*算法的计算复杂性，不包括离线训练的计算复杂度。回想一下，n_T表示阶段I的样本数量。阶段I的复杂性基于调用OAS网络的数量。每次调用的复杂度为其中z是网络层数，n_h是每层的最大神经元数。连接结点的复杂度为/>然后，由于凸多边形的最大边数和邻域内的凸多边形个数都是固定的，因此连接每个邻域内的节点的复杂度为O(1)；当窗口工作空间的重叠率固定时，小工作空间的数量与整个搜索空间C的Lebesgue测度v是线性的，阶段I的计算复杂度也是线性的。在阶段II中，寻找最优编队状态的复杂性是非常高效的，在文献“Aunifyingcomplexity certification framework for active-set methods for convexquadratic programming”中可以找到，这里不讨论。

八、仿真

A.OAS神经网络

OSA将大小为l*d的窗口区间ω中的障碍填充作为部分输入。l和d是根据障碍物的大小和编队几何形状来确定的，这样一个窗口空间平均包含约6个障碍物，并允许一个编队路径(如果存在)，其长度约是编队几何形状长度的10倍。仿真采用几个5m×5m的方块联合形成障碍物。多机器人团队中的每个机器人的尺寸约为1米×1米，窗口区间的大小为30m×30m。随机生成了大约2万个窗口区间来训练CAE。为每个窗口区间随机生成约20对扩展方向和待连接多边形节点。对于窗口区间中的每一对都生成了大约50个成功的多边形位置。这些成功的多边形位置都可以生成可连接的多边形。CAE和OAS的训练是在Intel XeonPlatinum 8375C 2.9GHz CPU和NVIDIA Geforce RTX 3090显卡上进行的。CAE的训练约为9小时。当训练达到大约300个迭代时，损失降至3.8。

如前所述，额外的测试数据被采样并用于验证训练的OAS神经网络。测试数据上的OAS损失可以达到3.9，图9描述了一个示例。红色点表示测试数据集中本示例中所需多边形位置的分布，绿色多边形表示OAS估计的高斯分布的混合，其条件是待连接多边形扩展方向u和窗口空间中障碍物ω的编码e。结果表明，经过训练的OAS可以连续生成高概率的凸多边形位置。

B.多边形的生成和连接

回想一下，生成可连接多边形节点包括三个步骤：(i)采样多边形位置；(ii)从采样位置生长和扩展多边形；(iii)检查采样多边形是否可以连接到CPT*中的节点。经过训练的OAS在100个窗口工作空间中进行了测试，随机产生障碍。OAS在每个窗口工作空间中采样了100个多边形位置。在步骤(ii)中使用每个位置来生成多边形，在步骤(iii)中进一步测试其连接性。可连接多边形的位置称为“成功样本”，否则称为“失败样本”。将OAS的性能与统一的抽样策略进行了比较。一个例子如图10所示，在100个工作区中，OAS的平均成功率为89％，而在测试案例中，统一随机抽样策略的平均成功率为10％。这些结果表明，OAS可以以生成可连接多边形的更高可能性对多边形位置进行采样。

生成多边形节点的示例如图11所示。CPT*中的多边形节点Pi是随机选取的，称为“待连接”多边形。给定扩展方向u，在工作空间上放置一个合适的窗口，该窗口中障碍物的编码e可以从CAE编码器获得。然后通过步骤(i)至(iii)的迭代生成新多边形。在图10中，红色点是OAS采样的多边形位置，具有实心边的多边形是新采样的多边形。/>和之间的交点/>区域由菱形网格突出显示。通过计算交集/>之间的Minkowski差并形成“十”字形来测试连通性，表明/>可以连接到/>

C.编队路径规划

CPT*的例子如图12所示。编队路径的优化问题是利用最优凸多边形路径给出的变量和约束条件创建的，分别如图12和图15所示。图12中的示例涉及尺寸为50m×50m的小工作空间。所发现的约束凸优化问题包括3个变量向量(q₂、q₃和q₄)，其约束条件C_2,3、C₃和C_3,4是从最优多边形路径Φ^*＝{P_2,3，P₃，P_3,4}。变量向量的凸约束或(可行区域)分别由具有虚线、虚线和虚线边界的多边形表示。蓝色实线所示的优化形成路径是通过求解约束凸优化问题获得的。

CPT*方法在20个尺寸为90m×90m的工作空间中进行了测试，其中障碍物是随机产生的。在每个工作空间中随机创建了大约5对可行的初始和目标队形状态，总共产生了100个编队路径规划问题。此外，初始点和目标点之间的距离要求大于40米。Alonso的COM方法也在这些问题中进行了测试，并将获得的结果与CPT*的结果进行了比较。记录了每个规划问题的最终路径长度和计算时间，分别如图13和图14所示。

通过分析用于获取路径的计算时间，将CPT*的效率与COM方法进行了比较。请注意，在解决某些规划问题时，COM方法可能会花费过多的时间来生成可连接多边形的成功样本，因此最大计算时间设置为1200秒。图13显示了解决这100个问题的平均计算时间及其方差。CPT*获得可行路径的平均时间约为67秒，而COM约为400秒。图13中的每个点表示一个规划问题，其x轴和y轴分别表示CPT*和COM解决该问题所需的计算时间。蓝色对角线上方的点是CPT*可以更有效地解决的问题。在对角线上方，100个问题中有90％的问题。可见，COM可以更有效地解决100个问题中的大约10个问题，其中均匀采样器是足够的，因此比建议的CPT*更有效。

通过分析路径长度，将CPT*给出的路径的最优性与COM方法进行了比较。编队路径的平均长度及其解决这100个问题的差异如图14所示。CPT*给出的平均路径长度约为67m，而COM输出路径的长度约为150m。图14中的每个点再次表示一个规划问题，其x轴和y轴值分别表示CPT*和COM解决的路径长度。蓝色对角线上方的点表示CPT*给出的路径较短的问题。在这100个问题中，几乎98％都在对角线之上。在路径优化方面，COM在100个问题中有2个问题优于CPT*。可见，CPT*的最优性。

图13和图14中的结果表明，CPT*不仅计算工作量更少，而且在大多数情况下提供了比COM更优的路径。此外，CPT*方法在拐角情况下进行了测试，如图15所示，编队必须通过狭窄通道，COM经常失效。而本方案首先给出了最优凸多边形路径(起始点与目标点之间的连续虚线)，然后优化出了最佳编队路径(起始点与目标点之间的连续实线)。

以上是本发明的具体实施方式，但本发明的保护范围不应局限于此。任何熟悉本领域的技术人员在本发明所揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内，因此本发明的保护范围应以权利要求书所限定的保护范围为准。

Claims (4)

1.基于凸多边形树的多机器人最优编队路径规划方法，其特征在于，包括：

S1、障碍物生成；通过激光扫描或SLAM算法采集障碍物的点云数据；

S2、凸多边形位置采样；基于扩展方向、障碍物点云数据以及现有凸多边形节点，在工作空间的非障碍物位置找到用于生成新凸多边形的位置坐标；基于障碍物感知采样器完成，障碍物感知采样器包括自动压缩编码器和混合密度网络；障碍物感知采样传感器将障碍物的点云数据压缩为一维向量空间的编码，将障碍物的编码、现有凸多边形节点以及扩展方向进行聚合并将其作为混合密度网络的输入向量，混合密度网络基于高斯分布混合模型对新凸多边形的位置坐标进行预测，直至收集到的新凸多边形的位置坐标或迭代次数达到阈值；

S3、凸多边形生成与连接；以新凸多边形的位置坐标为基点建立椭球体，通过最大化椭球体以及对椭球体与障碍物的分界更新，从而扩展出新凸多边形；对新凸多边形与现有凸多边形进行交集空间检测，并判断交集空间内是否存在允许机器人通行的编队构型；

S4、凸多边形树生成；将新凸多边形加入到凸多边形树中，然后重新对凸多边形树进行总旅行代价计算，并输出总旅行代价最小的凸多边形树，最终得到的凸多边形树内凸多边形的质点连接顺序即为机器人初始编队路径；

S5、凸多边形树优化；基于机器人编队构型变换次数最少原则，将编队构型附加至初始编队路径中，然后以直线最短原则对初始编队路径优化，以使机器人在连续编队构型情况下以直线通过凸多边形，从而找到机器人最优编队路径；

S6、对步骤S2-S5进行迭代，直至新凸多边形与目标点重合。

2.根据权利要求1所述的多机器人最优编队路径规划方法，其特征在于，S3步骤中，计算交集空间与机器人编队构型空间的闵柯夫斯基差，以判断交集空间内是否存在允许机器人通行的编队构型以及编队构型的种类和数量。

3.根据权利要求1所述的多机器人最优编队路径规划方法，其特征在于，S4步骤中，将新凸多边形加入到凸多边形树中，计算新凸多边形与其质心范围内的所有现有凸多边形的旅行代价，并将新凸多边形连接到旅行代价最小的现有凸多边形上；重新对新生成的凸多边形树的总旅行代价进行检测，以使凸多边形树整体的总旅行代价最小。

4.根据权利要求1所述的多机器人最优编队路径规划方法，其特征在于，统计每个凸多边形内机器人的编队构型，若两个不同编队构型之间的交集空间允许机器人保持编队构型通过，则将该编队构型附加到初始编队路径中；然后通过一条直线构造一对连续编队构型之间的路径，以使机器人以最短路径通过凸多边形。