CN114545971A - 一种通讯约束下多智能体分布式可飞路径规划方法、系统、计算机设备及介质 - Google Patents

Abstract

本发明属于无人机路径规划技术领域，涉及一种通讯约束下多智能体分布式可飞路径规划方法，包括以下步骤：各智能体通过自身传感器和自身通讯装置，分别得到周围局部障碍物参数和周围智能体参数信息；各智能体利用人工势场模型，选取最优局部目标点；根据最优局部目标点，各智能体进行局部Dubins路径规划；各智能体根据自身Dubins路径规划、局部障碍信息、通讯范围内其他智能体信息，利用改进RVO避障算法，进行指定时间的移动，同时进行无人智能体间和无人智能体与障碍物间避障；各智能体进行到达目标检测；若未到达目标，更新自身的当前位姿。解决突发未知场景下，目标和障碍物高动态，满足动力学约束可飞的无人集群路径规划问题。

Description

一种通讯约束下多智能体分布式可飞路径规划方法、系统、计 算机设备及介质

技术领域

本发明属于无人机路径规划技术领域，具体涉及一种通讯约束下多智能体分布式可飞路径规划方法、系统、计算机设备及介质。

背景技术

随着现代信息通讯和人工智能的发展，具有自主能力的无人智能体应用日趋广泛。无人集群解决了单一无人机/车处理复杂任务能力不足的问题，具有较高的鲁棒性和抗毁性，被广泛的应用于军用和民用领域中，而无人集群执行任务时，需要对自身运动路径进行规划，因此多无人机智能体协同路径规划问题成为当前的热点问题。传统的集中式路径规划方法存在严重依赖于先验地图信息，计算效率低，较少考虑无人装置动力学约束，对动态目标和动态障碍物的应对能力差，难以应对突发场景和未知环境等问题。近年来分布式协同路径规划方法，成为路径规划问题的研究热点。

现有的分布式多无人机智能体路径规划方法可分为两大类：一类基于深度强化学习，通过各智能体不断与环境直接进行试错迭代，获取反馈信息，从而优化自身策略，最终生成的策略用于对自身运动决策。然而这类方法未考虑智能体动力学约束，存在计算过程繁杂，训练时间过长，训练策略不稳定难以收敛，可拓展性差，实际应用困难等问题。另一类则是基于人工势场，通过将智能体的工作空间定义为势场，定义目标和障碍物分别为低势能点和高势能点，利用叠加的势场力来确定智能体下一步运动，最终完成智能体路径规划。这类方法依旧未考虑智能体动力学约束，且所得轨迹与势能函数的高度相关，参数调试复杂。因此现有方法很难解决现实突发场景中，目标与障碍物高动态，满足动力学约束可飞的无人集群路径规划问题。

发明内容

本发明的目的在于提供一种通讯约束下多智能体分布式可飞路径规划方法、系统、计算机设备及介质，解决突发场景下，目标和障碍物高动态，满足动力学约束可飞的无人集群路径规划问题。

本发明是通过以下技术方案来实现：

一种通讯约束下多智能体分布式可飞路径规划方法，包括以下步骤：

步骤一、各智能体通过自身传感器，获取周围的局部环境信息，得到周围局部障碍物参数；

各智能体通过自身通讯装置，与通讯范围内的其他智能体通讯，获取周围智能体参数信息；

步骤二、各智能体利用改进的人工势场模型，结合自身目标信息，从周围局部环境中选取最优局部目标点；

步骤三、根据步骤二中的最优局部目标点，各智能体结合自身的动力学约束、当前点和局部目标点的位姿，进行场景检测分解，完成局部Dubins路径规划；

步骤四、各智能体根据自身Dubins路径规划、局部障碍信息、通讯范围内其他智能体的参数信息，利用改进的RVO避障算法，进行指定时间的移动，同时进行无人智能体间和无人智能体与障碍物间避障；

步骤五、各智能体进行到达目标检测；若未到达目标，更新自身的当前位姿，返回步骤一，进行下一步的滚动时域路径规划；若已到达目标，结束。

进一步，步骤一中，各智能体通过自身传感器，获取周围的局部环境信息，得到周围局部障碍物参数，具体为：

步骤101、周围环境信息获取：

各无人机智能体a_i,i∈{1,…,N_u},半径为r_i,位姿为p_i＝(x_i,y_i,θ_i)，所携带传感器的探测半径为R_D ⁱ；θ_i为当前时刻智能体的方向角；N_u为无人机智能能体的个数；

则对于任意障碍物o_k＝(x_k,y_k,R_k,v_k),o_k∈{1,…,N_o}，N_o为障碍物个数；R_k表示障碍物的半径，v_k表示障碍物的速度；

当且仅当下式满足时，表示当前障碍物o_k能被智能体a_i感知到；

故智能体a_i能感知到的障碍物集合表示为：

进一步，步骤一中，各智能体通过自身通讯装置，与通讯范围内的其他智能体通讯，获取周围智能体参数信息，具体为：

各无人机智能体a_i，其位姿为p_i＝(x_i,y_i,θ_i),所携带通讯装置的通讯半径为R_C ⁱ，则对于任意其它无人机智能体a_j,当且仅当下式满足时，表示智能体a_i能获取智能体a_j的信息m_j；

m_j＝{p_j,v_j},v_j为当前时刻智能体a_j的速度；

智能体a_i能获得的其他智能体的信息集合表示为

进一步，步骤二具体包括以下步骤：

步骤201、各智能体a_i获取当前时刻自身目标的位置p^T _i＝(x^T _i,y^T _i,)，构建利用如下函数，构建势场模型：

对该智能体待选取点的吸引势定义为U_attract(x)，这些障碍物和邻接智能体对该智能体某待选取点的排斥势定义为U_repel(x)，各智能体周围待选取点x的势场能表示为：

U(x)＝U_attract(x)+U_repel(x)；

其中，k_attract表示引力势能系数，x表示当前智能体某待选取点位置，d(x,p^T _i)表示当前待选取点至目标点的欧式距离；

K_repel表示斥力势能系数，d(x,x′)表示当前待选取点至障碍物或其他智能体的欧式距离，d_min为当前智能体半径与对应障碍物或其他智能体半径的和；

当d(x,x′)≤d_min时，表示若智能体移动到x处，该智能体会与周围障碍物或其他智能体发生碰撞重合，因此斥力势能定义为正无穷；

步骤202、生成待选局部最优点集合：

各智能体获取当前设定运行速度大小v_set ⁱ，位姿为p_i＝(x_i,y_i,θ_i)，根据滚动时域，每次路径规划需要规划至少T_l时间长度路径；结合动力学约束，可获得智能体的采样点的方向改变角θ_△；

所设计的抽样方法具体为：以当前智能体位置为圆心，以v_set ⁱ×T_l长度为半径，以θ_i±θ_△为方向角限定，均匀在对应弧上进行总抽样点数为N_select的抽样，所得待选点集表示为Ω_i；

步骤203、选取最优局部目标点：

各智能体对步骤202所得到的待选点集Ω_i中的各点，分别计算其对应的势能值，对于无人机智能体a_i，其最优局部目标点位置为

若当前智能体与其目标距离小于探测半径R_D ⁱ时，认为可观测到目标，则此时最优局部目标点位置为最终目标点，p_local ⁱ＝p^T _i。

进一步，步骤三具体包括以下步骤：

步骤301、场景检测分解：

各智能体将局部最优目标点的方向设置为p_i指向p_local ⁱ的方向，当前位姿p_i与局部最优目标点p_local ⁱ连线形成的线段为

各智能体以障碍物集合O_i中各障碍物位置为圆心，以障碍物R_k+智能体r_i为半径做圆，线段

与这些圆相交场景分为以下三类：

1.未相交，利用步骤302进行普通Dubins规划；

2.存在单圆相割，利用步骤303进行避障Dubins规划，规避障碍物为O₃；

3.存在两圆及以上相割，在两割点中插入辅助点P_A1,P_A2，辅助点方向指向最优局部目标点，分别对p_i至P_A1、P_A1至P_A2、P_A2至p_local ⁱ利用步骤303进行多次避障Dubins规划，所得曲线组合即为所需路径；

步骤302、普通Dubins路径规划：

各智能体根据当前自身速度和自身动力学约束，得到最小曲率半径r_min，以此进行普通Dubins路径规划；

当所规划的路径与障碍物距离小于r_i时，在Dubins路径中的S直线部分返回步骤301，直线检测起始点为Dubins路径直线部分起始点，所得新路径替换原直线部分；

步骤303、避障Dubins路径规划：

采用最短避障Dubins路径规划方法，各智能体根据当前自身速度和自身动力学约束，得到最小曲率半径r_min；根据步骤301场景检测分解结果，得到避障Dubins路径规划的起始点P_s和规划终点P_e；

根据起始点Ps和规划终点Pe连线方位角确定绕飞方向；

然后计算起始圆、终止圆位置，其半径为r_min，所用公式如下：

其中，起始点位姿为P_s(x_s,y_s,θ_s),终止点位姿为P_e(x_e,y_e,θ_e)，起始圆圆心为P_cs (x_cs,y_cs)，终止圆圆心为P_cf (x_cf,y_cf)，当为向左绕飞时，等式中符号取“+，”当向右绕飞时，等式中符号取“-；”

之后，计算起始圆切出点P_l、障碍圆切入点P_n1、障碍圆切出点P_n2、终止圆切入点P_m；

其中，障碍圆O₁(x_o1,y_o1,r_o1),增广障碍圆O₁′(x_o1,y_o1,r_o1′)，障碍圆切入点P_n1(x_n1,y_n1)，起始圆切出点P_l(x_l,y_l)，增广障碍圆切点P_n1′(x_n1′,y_n1′)；障碍圆O₁半径r_o1为障碍物半径R₁与智能体半径r_i之和；增广障碍圆O₁′的半径r_o1′＝R₁+r_i+r_min；

同理计算出障碍圆切出点P_n2和终止圆切入点P_m；

其中，障碍圆O₁(x_o1,y_o1,r_o1),增广障碍圆O₁′(x_o1,y_o1,r_o1′)，障碍圆切出点P_n2(x_n2,y_n2)，终止圆切入点P_m(x_m,y_m)，增广障碍圆切点P_n2′(x_n2′,y_n2′)；障碍圆O₁半径r_o1为障碍物半径R₁与智能体半径r_i之和；增广障碍圆O₁′的半径r_o1′＝R₁+r_i+r_min；

所得的最短避障Dubins路径由弧段

直线段

弧段

直线段

弧段

组成；

其中弧段

和弧段

对应半径为r_min，弧段

对应的半径为r_o1，路径点均匀采样，某点位姿为指向下一路径点位姿；

步骤304、将所获得的路径首尾相接，得到各智能体当前位姿p_i至最优局部目标点p_local ⁱ的满足自身动力学约束的可飞Dubins路径。

进一步，步骤四具体包括以下步骤：

步骤401、各智能体根据Dubins路径和当前时刻位置得到优选速度

各智能体根据自身当前时刻位姿p_now ⁱ，从步骤四获得的路径中找到距当前位置最近的路径点p_du1 ⁱ，其下一路径点为p_du2 ⁱ，令p_now ⁱ指向p_du2 ⁱ的方向角为θ_path ⁱ,则对应的优选速度方向角

由下式确定：

θ_pref ⁱ＝λθ_du1 ⁱ+(1-λ)θ_path ⁱ；

其中，θ_du1 ⁱ为最近路径点p_du1 ⁱ对应的方向角，λ为调节系数；

根据当前该智能体设定速度大小v_set ⁱ与方向角

可计算出当前智能体的优选速度

步骤402、各智能体建立自身速度障碍空间：

首先，各智能体根据所获得的局部障碍信息O_i，利用下式计算其障碍物部分速度障碍空间：

其中p_A为智能体位置，v_A为当前智能体速度，v_B为障碍物速度；

为锥形区域；

然后，各智能体根据通讯范围内其他智能体的参数信息M_i，利用下式计算其智能体部分速度障碍空间：

其中

为对应的VO速度障碍空间，v_A为当前智能体速度，v_C为邻接智能体速度，v′_A为互惠速度障碍空间RVO内某速度；

各智能体将得到的所有速度障碍空间叠加，得到各智能体所对应的完整速度障碍空间Φ_i；

步骤403、各智能体计算最终速度v_final ⁱ：

各智能体根据当前位姿p_now ⁱ、运动速度v_now ⁱ和自身动力学约束，在自身最大最小速度[v_min,v_max]范围内，结合当前方位角θ_now ⁱ和最大转向角θ_△now ⁱ，在所围成的扇形区域内进行等间隔速度采样；

当采样所得速度集合至少存在一个未在速度障碍空间Φ_i内的速度时，选择其与

欧式距离最接近的速度作为当前最终速度v_final ⁱ；

步骤404、各智能体在最小执行间隔τ内，执行最终速度；

若距上一次Dubins路径规划已经过T_e时间，则进入步骤五；反之则返回步骤401。

进一步，若不存在任何不在速度障碍空间Φ_i内的速度时，假设共有4个待选速度{v_cand ¹，v_cand ²，v_cand ³，v_cand ⁴}，三个速度障碍空间Φ_i＝{φ₁,φ₂,φ₃}，φ₁～φ₃为VO或RVO，分别由对应的障碍物/智能体计算得出；

待选速度v_cand ¹，v_cand ²，v_cand ³，v_cand ⁴分别位于速度障碍空间(φ₁,φ₂)，(φ₂,φ₃)，(φ₁,φ₂)，(φ₂)内；

针对智能体某待选速度位于某速度障碍空间中，定义该速度障碍空间对当前智能体的重要性描述函数如下：

式中，p_m为当前速度障碍空间φ_m所对应的障碍物/智能体位置，‖p_m-p_i‖为当前智能体与对应障碍物/智能体间的欧式距离，R_m为障碍物/智能体半径，r_i为当前智能体半径；n为当前智能体指向对应障碍物/智能体位置的单位方向向量；

某待选速度

的运动特性保持函数描述如下：

对某智能体a_i，其待选速度

的联合计算函数如下：

式中，K_mot和K_imp分别是运动特性权重和重要性权重；

对于某智能体a_i，联合计算函数

最小值对应的待选速度

为所需的最终速度，

本发明还公开了一种通讯约束下多智能体分布式可飞路径规划系统，包括多个无人机智能体，在每个智能体上安装有传感器、通讯模块、规划器和执行器；

传感器和通讯模块均与规划器连接，规划器与执行器连接；

规划器包括障碍物参数获取模块、智能体参数获取模块、最优局部目标点计算模块、路径规划模块、避障模块和检测模块；

障碍物参数获取模块，用于各智能体通过自身传感器，获取周围的局部环境信息，得到周围局部障碍物参数；

智能体参数获取模块，用于各智能体通过自身通讯装置，与通讯范围内的其他智能体通讯，获取周围智能体参数信息；

最优局部目标点计算模块，用于各智能体利用改进的人工势场模型，结合自身目标信息，从周围局部环境中选取最优局部目标点；

路径规划模块，用于根据最优局部目标点，各智能体结合自身的动力学约束、当前点和局部目标点的位姿，进行场景检测分解，完成局部Dubins路径规划；

避障模块，用于各智能体根据自身Dubins路径规划、局部障碍信息、通讯范围内其他智能体的参数信息，利用改进的RVO避障算法，进行时间的移动，同时进行无人智能体间和无人智能体与障碍物间避障；

检测模块，用于各智能体进行到达目标检测；若未到达目标，更新自身的当前位姿，进行下一步的滚动时域路径规划；若已到达目标，结束。

本发明还公开了一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现所述方法的步骤。

本发明还公开了一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现所述方法的步骤。

与现有技术相比，本发明具有以下有益的技术效果：

本发明公开了一种通讯约束下多智能体分布式可飞路径规划方法，首先，各智能体仅依靠局部规划时刻自身局部感知和通讯的数据，无需依赖先验地图信息，适用于未知环境和突发场景；其次，计算路径时仅计算当前点至最优局部目标点部分，计算量显著减小；再次，在计算路径时考虑自身动力学约束，规划满足可飞条件的Dubins路径；之后，在计算局部最优目标点时，实时获取当前目标位置，在下一步路径规划时有效应对动态目标；最后，使用改进的RVO避障算法实现对动态障碍物和邻近智能体的避障，提高了算法的鲁棒性。与现有的分布式路径规划算法相比，本发明所提出的方法在规划Dubins路径时考虑了自身动力学约束，满足可飞条件。同时，本方法无需针对不同场景进行长时间的训练，在简单调试后即可使用，无人机智能体规模可随时自适应扩展。此外，最优局部目标点的选取显著减少了路径规划的计算复杂度。最后滚动时域和RVO避障的引入显著提高了本方法的鲁棒性，从而具有较好的工程应用价值。

附图说明

图1为本发明方法的架构框图；

图2为本发明方法的流程图；

图3为本发明步骤201智能体势场示意图；

图4为本发明步骤301场景检测分解示意图；图4a表示线段

与这些圆未相交；图4b表示线段

与这些圆存在单圆相割；图4c表示线段

与这些圆存在两圆及以上相割；

图5为本发明步骤303避障Dubins规划示意图；

图6为本发明步骤403联合最终速度选择方法示意图。

具体实施方式

下面结合附图及本发明的实施例对本发明的方法作进一步详细说明。

本发明以N_u架无人机智能能体N_o个障碍物场景进行通讯约束下分布式可飞路径规划为例，图1为本发明方法的系统架构框图。此处所描述的具体实施例仅用于解释本发明，并不用于限定发明。

如图2所示，本发明包括以下步骤：

步骤一，各智能体通过自身传感器，获取周围的局部环境信息，计算得到周围局部障碍物参数：

各无人机智能体a_i,i∈{1,…,N_u},半径为r_i,位姿为p_i＝(x_i,y_i,θ_i)，所携带传感器的探测半径为R_D ⁱ。θ_i为当前时刻智能体的方向角；

则对于任意障碍物o_k＝(x_k,y_k,R_k,v_k),o_k∈{1,…,N_o}。当且仅当下式满足时，表示当前障碍物o_k能被智能体a_i感知到。

故智能体a_i能感知到的障碍物集合表示为：

各智能体通过自身通讯装置，与通讯范围内的其他智能体通讯，获取周围智能体参数信息周围智能体信息获取：

各无人机智能体a_i，其位姿为p_i＝(x_i,y_i,θ_i),所携带通讯装置的通讯半径为

则对于任意无人机智能体a_i和a_j,当且仅当下式满足时，

表示智能体a_i能获取智能体a_j的信息m_j。m_j＝{p_j,v_j},v_j为当前时刻a_j的速度。故智能体a_i能获得的其他智能体的信息集合表示为

步骤二，各智能体利用改进的人工势场模型，结合自身目标信息，从周围局部环境中选取最优局部目标点p_local ⁱ；

步骤201、各智能体a_i获取当前时刻自身目标的位置p^T _i＝(x^T _i,y^T _i,)，构建利用如下函数，构建势场模型：

根据人工势场理论，势场中的目标点对当前智能体产生引力，环境中的障碍物和其他智能体产生斥力，引力和斥力的合力影响运动对象下一步的运动。因此在本发明中，当前智能体某待选取点位置用x表示，则其势场可以用U(x)表示。智能体目标位姿为p^T _i，对该智能体待选取点的吸引势为U_attract(x)。智能体能感知到的障碍物和获取到的智能体信息分别为O_i和M_i，这些障碍物和邻接智能体对该智能体某待选取点的排斥势定义为U_repel(x)。

因此在本发明中，各智能体周围待选取点x的势场能表示为

U(x)＝U_attract(x)+U_repel(x)

目标点对待选取点产生的势能大小与两者的距离相关，距离越大、势能越大，反之亦然。当两者的距离为0时，表示到达目标点，因此引力势能与距离成正比关系，在本发明中定义为

k_attract表示引力势能系数，根据具体场景设定。d(x,p^T _i)表示当前待选取点至目标点的欧式距离。

周围障碍物和周围智能体对待选取点产生斥力场。斥力场势能大小与当前待选取点距离周围障碍物和周围智能体的距离有关，距离越近，斥力势能越大，反之越小。在本发明中定义为：

K_repel表示斥力势能系数，根据具体场景设定。d(x,x′)表示当前待选取点至障碍物或其他智能体的欧式距离。d_min为当前智能体半径与对应障碍物或其他智能体半径的和。当d(x,x′)≤d_min时，表示若智能体移动到x处，该智能体会与周围障碍物或其他智能体发生碰撞重合，因此斥力势能定义为正无穷。

由本发明中上述公式，各智能体可对周围感知区域内的势场完成定义，所得势场如图3所示。

步骤202、生成待选局部最优点：在本发明中，为了减小每次规划的计算开销，采用了一种抽样方法选取候选局部目标点。各智能体获取当前设定运行速度大小v_set ⁱ，位姿为p_i＝(x_i,y_i,θ_i)，根据滚动时域，每次路径规划需要规划至少T_l时间长度路径。结合动力学约束，可获得智能体的采样点的方向改变角θ_△。所设计的抽样方法具体描述为：以当前智能体位置为圆心，以v_set ⁱ×T_l长度为半径，以θ_i±θ_△为方向角限定，均匀在对应弧上进行总抽样点数为N_select的抽样。所得待选点集表示为Ω_i。

步骤203、选取最优局部目标点：各智能体对步骤302所得到的待选点集Ω_i中的各点，分别计算其对应的势能值，对于无人机智能体a_i，其最优局部目标点位置为

若当前智能体与其目标距离小于探测半径R_D ⁱ时，认为可观测到目标，则此时最优局部目标点为最终目标点，即p_local ⁱ＝p^T _i。

步骤三，根据步骤203中的最优局部目标点，各智能体结合自身的动力学约束、当前点和局部目标点的位姿，进行场景检测分解，完成局部Dubins路径规划；

步骤301、场景检测分解：各智能体将局部最优目标点的方向设置为p_i指向p_local ⁱ的方向，当前位姿p_i与局部最优目标点p_local ⁱ连线形成的线段为

本发明通过如图4所示的场景检测分解的方法，对可能遇到的场景进行完备考虑，完成局部路径规划。

图4中，各智能体以障碍物集合O_i中各障碍物位置为圆心，以障碍物R_k+智能体r_i为半径做圆。线段

与这些圆相交场景分为以下三类：

1.未相交，如图4(a)所示，利用步骤302进行普通Dubins规划；

2.存在单圆相割，如图4(b)所示，利用步骤303进行避障Dubins规划，规避障碍物为O₃；

3.存在两圆及以上相割，如图4(c)所示，在两割点中插入辅助点P_A1,P_A2，辅助点方向指向最优局部目标点，分别对p_i至P_A1,P_A1至P_A2，P_A2至p_local ⁱ利用步骤303进行避障Dubins规划，所得曲线组合即为所需路径。

步骤302、普通Dubins路径规划：各智能体根据当前自身速度和自身动力学约束，得到最小曲率半径r_min，以此进行普通Dubins路径规划。各智能体根据起始点终止点位姿生成LSL、RSR、LSR、RSL共4种类型的Dubins路径(R为右转弧，L为左转弧，S为直线)，选取4种Dubins路径中最短的路径作为规划路径。当所规划的路径与障碍物距离小于r_i时，在Dubins路径中的S直线部分返回步骤301，直线检测起始点为Dubins路径直线部分起始点，所得新路径替换原直线部分。路径点均匀采样，某点位姿为指向下一路径点位姿。

步骤303、避障Dubins路径规划：为了得到满足避障需求的最短路径，本发明采用了如图5所示的最短避障Dubins路径规划方法。各智能体根据当前自身速度和自身动力学约束，得最小曲率半径r_min。根据步骤301场景检测分解结果，得到本次避障Dubins路径规划的起始点P_s和规划终点P_e。

首先确定绕飞方向，图5中P_s和P_e连线方位角在P_s与障碍物圆心P_o1连线方位角的右侧，因此智能体需向右绕飞，反之则需向左绕飞。然后计算起始圆、终止圆位置，其半径为r_min，所用公式如下：

其中，起始点位姿为P_s(x_s,y_s,θ_s),终止点位姿为P_e(x_e,y_e,θ_e)，起始圆圆心为P_cs (x_cs,y_cs)，终止圆圆心为P_cf (x_cf,y_cf)，当为向左绕飞时，等式中符号取“+，”当向右绕飞时，等式中符号取“-”。

之后，计算起始圆切出点P_l、障碍圆切入点P_n1、障碍圆切出点P_n2、终止圆切入点P_m。障碍圆O₁半径r_o1为障碍物半径R₁与智能体半径r_i之和。增广障碍圆O₁′的半径r_o1′＝R₁+r_i+r_min。过起始圆圆心P_cs得到与增广障碍圆相切的直线，切点为P_n1′。直线P_o1P_n1′与障碍圆O₁交于P_n1点。另过起始圆圆心P_cs得到与P_n1P_n1′平行的直线，交起始圆于P_l点。由于几何关系，可知四边形P_lP_n1P_n1′P_cs为矩形，所以P_l即为起始圆切出点，P_n1即为障碍圆切入点。

其中，障碍圆O₁(x_o1,y_o1,r_o1),增广障碍圆O₁′(x_o1,y_o1,r_o1′)，障碍圆切入点P_n1(x_n1,y_n1)，起始圆切出点P_l(x_l,y_l)，切点P_n1′(x_n1′,y_n1′)。

同理可计算出障碍圆切出点P_n2和终止圆切入点P_m。

其中，障碍圆O₁(x_o1,y_o1,r_o1),增广障碍圆O₁′(x_o1,y_o1,r_o1′)，障碍圆切出点P_n2(x_n2,y_n2)，终止圆切入点P_m(x_m,y_m)，增广障碍圆切点P_n2′(x_n2′,y_n2′)；障碍圆O₁半径r_o1为障碍物半径R₁与智能体半径r_i之和；增广障碍圆O₁′的半径r_o1′＝R₁+r_i+r_min。

因此所得的最短避障Dubins路径由5部分组成，弧段

直线段

弧段

直线段

弧段

其中弧段

和弧段

对应半径为r_min，弧段

对应的半径为r_o1。路径点均匀采样，某点位姿为指向下一路径点位姿。

步骤304、将所获得的路径首尾相接，各智能体可得到当前位姿p_i至最优局部目标点p_local ⁱ的满足自身动力学约束的可飞Dubins路径。

步骤四，各智能体根据自身Dubins路径、局部障碍信息O_i、通讯范围内其他智能体的参数信息M_i，利用改进的RVO避障算法，进行T_e时间的移动，同时进行无人智能体间和无人智能体与障碍物间避障；

步骤401、各智能体根据Dubins路径和当前时刻位置得到优选速度

各智能体根据自身当前时刻位姿p_now ⁱ，从步骤四获得的路径中找到距当前位置最近的路径点p_du1 ⁱ，其下一路径点为p_du2 ⁱ。令p_now ⁱ指向p_du2 ⁱ的方向角为θ_path ⁱ,则对应的优选速度方向角

可由下式确定：

θ_pref ⁱ＝λθ_du1 ⁱ+(1-λ)θ_path ⁱ

其中，θ_du1 ⁱ为最近路径点p_du1 ⁱ对应的方向角，λ为调节系数，一般可取0.95。

根据当前该智能体设定速度大小v_set ⁱ与方向角

可计算出当前智能体的优选速度。

步骤402、各智能体建立自身速度障碍空间：首先，各智能体根据所获得的局部障碍信息O_i进行障碍物部分速度障碍空间计算。根据速度障碍空间VO思想，智能体将与障碍物碰撞，当且仅当两者相对速度位于锥形区域，该锥形区域以智能体位置为顶点，两边与以障碍物为中心以障碍物与智能体半径和为半径的圆相切。针对智能体与障碍物的避碰场景，智能体需承担所有避撞责任，因此仅需计算VO即可。智能体遍历O_i中每一个障碍物，利用下式计算其障碍物部分速度障碍空间：

其中p_A为智能体位置，v_A为智能体速度，v_B为障碍物速度。

为所描述的锥形区域。

然后，各智能体根据通讯范围内其他智能体的参数信息M_i进行智能体部分速度障碍空间的计算。由于智能体间可以进行相互避障，故采用RVO计算。智能体遍历M_i中每一个周围智能体，利用下式计算其智能体部分速度障碍空间：

其中

为对应的VO速度障碍空间，v_A为当前智能体速度，v_C为邻接智能体速度，v′_A为互惠速度障碍空间RVO内某速度；

各智能体将得到的所有速度障碍空间叠加，便可得到各智能体所对应的完整速度障碍空间Φ_i。

步骤403、各智能体计算最终速度v_final ⁱ：各智能体根据当前位姿p_now ⁱ、运动速度v_now ⁱ和自身动力学约束，在自身最大最小速度[v_min,v_max]范围内，结合当前方位角θ_now ⁱ和最大转向角θ_△now ⁱ，在所围成的扇形区域内进行等间隔速度采样。

当采样所得速度集合至少存在一个未在速度障碍空间Φ_i内的速度时，选择其与v_pref ⁱ欧式距离最接近的速度作为当前最终速度v_final ⁱ。

若不存在任何不在速度障碍空间Φ_i内的速度时，本发明设计了一种综合考虑运动特性和重要性的联合最终速度选择方法。如图6所示，当前智能体优选速度

自身最大最小速度限制[v_min,v_max]，当前方位角θ_now ⁱ和最大转向角θ_△now ⁱ均已在图上标出。

本例中共有4个待选速度{v_cand ¹，v_cand ²，v_cand ³，v_cand ⁴}，三个速度障碍空间Φ_i＝{φ₁,φ₂,φ₃},φ₁～φ₃为上述描述的速度障碍空间VO或互惠速度障碍空间RVO，分别由对应的障碍物/智能体计算得出。由图可以看出待选速度v_cand ¹，v_cand ²，v_cand ³，v_cand ⁴分别位于速度障碍空间(φ₁,φ₂)，(φ₂,φ₃)，(φ₁,φ₂)，(φ₂)内。故针对智能体某待选速度位于某速度障碍空间中，本发明定义该速度障碍空间对当前智能体的重要性描述函数如下：

式中，p_m为当前速度障碍空间φ_m所对应的障碍物/智能体位置，‖p_m-p_i‖为当前智能体与对应障碍物/智能体间的欧式距离，R_m为障碍物/智能体半径，r_i为当前智能体半径。n为当前智能体指向对应障碍物/智能体位置的单位方向向量。

某待选速度

的运动特性保持函数描述如下：

因此对某智能体a_i，其待选速度

的联合计算函数如下：

式中，K_mot和K_imp分别是运动特性权重和重要性权重，默认取1，可根据实际情况修改。

因此，对于某智能体a_i，联合计算函数

最小值对应的待选速度

即为所需的最终速度，即

步骤404、各智能体在最小执行间隔τ内，执行最终速度。若距上一次Dubins路径规划已经过T_e时间，则进入步骤五；反之则返回步骤401。

步骤五，各智能体进行到达目标检测。若未到达目标，更新自身的当前位姿，返回步骤一，进行下一步的滚动时域路径规划；若已到达目标，算法结束。

本发明还公开了一种通讯约束下多智能体分布式可飞路径规划系统，如图1所示，包括多个智能体，在每个智能体上安装有传感器、通讯模块、规划器和执行器；

传感器和通讯模块均与规划器连接，规划器与执行器连接；

规划器包括障碍物参数获取模块、智能体参数获取模块、最优局部目标点计算模块、路径规划模块、避障模块和检测模块；

障碍物参数获取模块，用于各智能体通过自身传感器，获取周围的局部环境信息，得到周围局部障碍物参数；

智能体参数获取模块，用于各智能体通过自身通讯装置，与通讯范围内的其他智能体通讯，获取周围智能体参数信息；

最优局部目标点计算模块，用于各智能体利用改进的人工势场模型，结合自身目标信息，从周围局部环境中选取最优局部目标点；

路径规划模块，用于根据最优局部目标点，各智能体结合自身的动力学约束、当前点和局部目标点的位姿，进行场景检测分解，完成局部Dubins路径规划；

避障模块，用于各智能体根据自身Dubins路径规划、局部障碍信息、通讯范围内其他智能体的参数信息，利用改进的RVO避障算法，进行时间的移动，同时进行无人智能体间和无人智能体与障碍物间避障；

检测模块，用于各智能体进行到达目标检测；若未到达目标，更新自身的当前位姿，进行下一步的滚动时域路径规划；若已到达目标，结束。

本发明基于通讯约束下多智能体分布式可飞路径规划方法可采用完全硬件实施例、完全软件实施例或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本发明基于通讯约束下多智能体分布式可飞路径规划方法如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。计算机可读存储介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。其中，所述计算机存储介质可以是计算机能够存取的任何可用介质或数据存储设备，包括但不限于磁性存储器(例如软盘、硬盘、磁带、磁光盘(MO)等)、光学存储器(例如CD、DVD、BD、HVD等)、以及半导体存储器(例如ROM、EPROM、EEPROM、非易失性存储器(NANDFLASH)、固态硬盘(SSD))等。

在示例性实施例中，还提供计算机设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现所述基于通讯约束下多智能体分布式可飞路径规划方法的步骤。处理器可能是中央处理单元(CentralProcessingUnit，CPU)，还可以是其他通用处理器、数字信号处理器(DigitalSignalProcessor，DSP)、专用集成电路(ApplicationSpecificIntegratedCircuit，ASIC)、现成可编程门阵列(Field-ProgrammableGateArray，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。

综上所述，本发明设计了一种通讯约束下分布式多智能体可飞路径规划方法，方法于各智能体中分布式执行，无需先验环境信息，各智能体完全依靠自身局部感知和局部通讯获取周围障碍物和邻接智能体信息，采用滚动时域思想规划满足自身动力学约束的局部Dubins路径，在规划路径时通过设计的场景检测分解方法完备进行路径规划，在执行路径时利用改进的RVO进行对动态障碍物和周围智能体避障。同时无人机规模动态可变，计算需求小，参数易调节易部署，具有较好的工程应用价值。可有效应对大规模无人集群在突发未知场景中动态目标障碍物可飞路径规划问题。

本发明采用了分布式路径规划方法，各智能体完全依靠自身局部感知和局部通讯，采用滚动时域思想规划满足动力学约束的局部Dubins路径，在执行路径时利用改进的RVO进行动态避障，从而有效解决突发场景下，目标和障碍物高动态，满足动力学约束可飞的无人集群路径规划问题。

以上所述，仅是本发明的实施例，并非对本发明作任何限制，凡是根据本发明技术实质对以上实施例作的任何简单修改、变更以及等效结构变化，均仍属于本发明技术方案的保护范围之内。

Claims (10)

1.一种通讯约束下多智能体分布式可飞路径规划方法，其特征在于，包括以下步骤：

步骤一、各智能体通过自身传感器，获取周围的局部环境信息，得到周围局部障碍物参数；

各智能体通过自身通讯装置，与通讯范围内的其他智能体通讯，获取周围智能体参数信息；

步骤二、各智能体利用改进的人工势场模型，结合自身目标信息，从周围局部环境中选取最优局部目标点；

步骤三、根据步骤二中的最优局部目标点，各智能体结合自身的动力学约束、当前点和局部目标点的位姿，进行场景检测分解，完成局部Dubins路径规划；

步骤四、各智能体根据自身Dubins路径规划、局部障碍信息、通讯范围内其他智能体的参数信息，利用改进的RVO避障算法，进行指定时间的移动，同时进行无人智能体间和无人智能体与障碍物间避障；

步骤五、各智能体进行到达目标检测；若未到达目标，更新自身的当前位姿，返回步骤一，进行下一步的滚动时域路径规划；若已到达目标，结束。

2.根据权利要求1所述的一种通讯约束下多智能体分布式可飞路径规划方法，其特征在于，步骤一中，各智能体通过自身传感器，获取周围的局部环境信息，得到周围局部障碍物参数，具体为：

步骤101、周围环境信息获取：

各无人机智能体a_i,i∈{1,…,N_u},半径为r_i,位姿为p_i＝(x_i,y_i,θ_i)，所携带传感器的探测半径为R_D ⁱ；θ_i为当前时刻智能体的方向角；N_u为无人机智能能体的个数；

则对于任意障碍物o_k＝(x_k,y_k,R_k,v_k),o_k∈{1,…,N_o}，N_o为障碍物个数；R_k表示障碍物的半径，v_k表示障碍物的速度；

当且仅当下式满足时，表示当前障碍物o_k能被智能体a_i感知到；

故智能体a_i能感知到的障碍物集合表示为：

{i₁,…,i_m}∈{1,…,N_o}。

3.根据权利要求1所述的一种通讯约束下多智能体分布式可飞路径规划方法，其特征在于，步骤一中，各智能体通过自身通讯装置，与通讯范围内的其他智能体通讯，获取周围智能体参数信息，具体为：

各无人机智能体a_i，其位姿为p_i＝(x_i,y_i,θ_i),所携带通讯装置的通讯半径为R_C ⁱ，则对于任意其它无人机智能体a_j,当且仅当下式满足时，表示智能体a_i能获取智能体a_j的信息m_j；

m_j＝{p_j,v_j},v_j为当前时刻智能体a_j的速度；

智能体a_i能获得的其他智能体的信息集合表示为

{i₁,…,i_m}∈{1,…,N_u}。

4.根据权利要求1所述的一种通讯约束下多智能体分布式可飞路径规划方法，其特征在于，步骤二具体包括以下步骤：

步骤201、各智能体a_i获取当前时刻自身目标的位置p^T _i＝(x^T _i,y^T _i,)，构建利用如下函数，构建势场模型：

对该智能体待选取点的吸引势定义为U_attract(x)，这些障碍物和邻接智能体对该智能体某待选取点的排斥势定义为U_repel(x)，各智能体周围待选取点x的势场能表示为：

U(x)＝U_attract(x)+U_repel(x)；

其中，k_attract表示引力势能系数，x表示当前智能体某待选取点位置，d(x,p^T _i)表示当前待选取点至目标点的欧式距离；

K_repel表示斥力势能系数，d(x,x′)表示当前待选取点至障碍物或其他智能体的欧式距离，d_min为当前智能体半径与对应障碍物或其他智能体半径的和；

当d(x,x′)≤d_min时，表示若智能体移动到x处，该智能体会与周围障碍物或其他智能体发生碰撞重合，因此斥力势能定义为正无穷；

步骤202、生成待选局部最优点集合：

各智能体获取当前设定运行速度大小v_set ⁱ，位姿为p_i＝(x_i,y_i,θ_i)，根据滚动时域，每次路径规划需要规划至少T_l时间长度路径；结合动力学约束，可获得智能体的采样点的方向改变角θ_△；

所设计的抽样方法具体为：以当前智能体位置为圆心，以v_set ⁱ×T_l长度为半径，以θ_i±θ_△为方向角限定，均匀在对应弧上进行总抽样点数为N_select的抽样，所得待选点集表示为Ω_i；

步骤203、选取最优局部目标点：

各智能体对步骤202所得到的待选点集Ω_i中的各点，分别计算其对应的势能值，对于无人机智能体a_i，其最优局部目标点位置为

若当前智能体与其目标距离小于探测半径R_D ⁱ时，认为可观测到目标，则此时最优局部目标点位置为最终目标点，p_local ⁱ＝p^T _i。

5.根据权利要求1所述的一种通讯约束下多智能体分布式可飞路径规划方法，其特征在于，步骤三具体包括以下步骤：

步骤301、场景检测分解：

各智能体将局部最优目标点的方向设置为p_i指向p_local ⁱ的方向，当前位姿p_i与局部最优目标点p_local ⁱ连线形成的线段为

各智能体以障碍物集合O_i中各障碍物位置为圆心，以障碍物R_k+智能体r_i为半径做圆，线段

与这些圆相交场景分为以下三类：

1.未相交，利用步骤302进行普通Dubins规划；

2.存在单圆相割，利用步骤303进行避障Dubins规划，规避障碍物为O₃；

3.存在两圆及以上相割，在两割点中插入辅助点P_A1,P_A2，辅助点方向指向最优局部目标点，分别对p_i至P_A1、P_A1至P_A2、P_A2至p_local ⁱ利用步骤303进行多次避障Dubins规划，所得曲线组合即为所需路径；

步骤302、普通Dubins路径规划：

各智能体根据当前自身速度和自身动力学约束，得到最小曲率半径r_min，以此进行普通Dubins路径规划；

当所规划的路径与障碍物距离小于r_i时，在Dubins路径中的S直线部分返回步骤301，直线检测起始点为Dubins路径直线部分起始点，所得新路径替换原直线部分；

步骤303、避障Dubins路径规划：

采用最短避障Dubins路径规划方法，各智能体根据当前自身速度和自身动力学约束，得到最小曲率半径r_min；根据步骤301场景检测分解结果，得到避障Dubins路径规划的起始点P_s和规划终点P_e；

根据起始点Ps和规划终点Pe连线方位角确定绕飞方向；

然后计算起始圆、终止圆位置，其半径为r_min，所用公式如下：

其中，起始点位姿为P_s(x_s,y_s,θ_s),终止点位姿为P_e(x_e,y_e,θ_e)，起始圆圆心为P_cs (x_cs,y_cs)，终止圆圆心为P_cf (x_cf,y_cf)，当为向左绕飞时，等式中符号取“+，”当向右绕飞时，等式中符号取“-；”

之后，计算起始圆切出点P_l、障碍圆切入点P_n1、障碍圆切出点P_n2、终止圆切入点P_m；

其中，障碍圆O₁(x_o1,y_o1,r_o1),增广障碍圆O₁′(x_o1,y_o1,r_o1′)，障碍圆切入点P_n1(x_n1,y_n1)，起始圆切出点P_l(x_l,y_l)，增广障碍圆切点P_n1′(x_n1′,y_n1′)；障碍圆O₁半径r_o1为障碍物半径R₁与智能体半径r_i之和；增广障碍圆O₁′的半径r_o1′＝R₁+r_i+r_min；

同理计算出障碍圆切出点P_n2和终止圆切入点P_m；

其中，障碍圆O₁(x_o1,y_o1,r_o1),增广障碍圆O₁′(x_o1,y_o1,r_o1′)，障碍圆切出点P_n2(x_n2,y_n2)，终止圆切入点P_m(x_m,y_m)，增广障碍圆切点P_n2′(x_n2′,y_n2′)；障碍圆O₁半径r_o1为障碍物半径R₁与智能体半径r_i之和；增广障碍圆O₁′的半径r_o1′＝R₁+r_i+r_min；

所得的最短避障Dubins路径由弧段

直线段

弧段

直线段

弧段

组成；

其中弧段

和弧段

对应半径为r_min，弧段

对应的半径为r_o1，路径点均匀采样，某点位姿为指向下一路径点位姿；

步骤304、将所获得的路径首尾相接，得到各智能体当前位姿p_i至最优局部目标点p_local ⁱ的满足自身动力学约束的可飞Dubins路径。

6.根据权利要求1所述的一种通讯约束下多智能体分布式可飞路径规划方法，其特征在于，步骤四具体包括以下步骤：

步骤401、各智能体根据Dubins路径和当前时刻位置得到优选速度

各智能体根据自身当前时刻位姿p_now ⁱ，从步骤四获得的路径中找到距当前位置最近的路径点p_du1 ⁱ，其下一路径点为p_du2 ⁱ，令p_now ⁱ指向p_du2 ⁱ的方向角为θ_path ⁱ,则对应的优选速度方向角

由下式确定：

θ_pref ⁱ＝λθ_du1 ⁱ+(1-λ)θ_path ⁱ；

其中，θ_du1 ⁱ为最近路径点p_du1 ⁱ对应的方向角，λ为调节系数；

根据当前该智能体设定速度大小v_set ⁱ与方向角

可计算出当前智能体的优选速度

步骤402、各智能体建立自身速度障碍空间：

首先，各智能体根据所获得的局部障碍信息O_i，利用下式计算其障碍物部分速度障碍空间：

其中p_A为智能体位置，v_A为当前智能体速度，v_B为障碍物速度；

为锥形区域；

然后，各智能体根据通讯范围内其他智能体的参数信息M_i，利用下式计算其智能体部分速度障碍空间：

其中

为对应的VO速度障碍空间，v_A为当前智能体速度，v_C为邻接智能体速度，v′_A为互惠速度障碍空间RVO内某速度；

各智能体将得到的所有速度障碍空间叠加，得到各智能体所对应的完整速度障碍空间Φ_i；

步骤403、各智能体计算最终速度v_final ⁱ：

各智能体根据当前位姿p_now ⁱ、运动速度v_now ⁱ和自身动力学约束，在自身最大最小速度[v_min,v_max]范围内，结合当前方位角θ_now ⁱ和最大转向角θ_△now ⁱ，在所围成的扇形区域内进行等间隔速度采样；

当采样所得速度集合至少存在一个未在速度障碍空间Φ_i内的速度时，选择其与

欧式距离最接近的速度作为当前最终速度v_final ⁱ；

步骤404、各智能体在最小执行间隔τ内，执行最终速度；

若距上一次Dubins路径规划已经过T_e时间，则进入步骤五；反之则返回步骤401。

7.根据权利要求6所述的一种通讯约束下多智能体分布式可飞路径规划方法，其特征在于，若不存在任何不在速度障碍空间Φ_i内的速度时，假设共有4个待选速度{v_cand ¹，v_cand ²，v_cand ³，v_cand ⁴}，三个速度障碍空间Φ_i＝{φ₁,φ₂,φ₃}，φ₁～φ₃为VO或RVO，分别由对应的障碍物/智能体计算得出；

待选速度v_cand ¹，v_cand ²，v_cand ³，v_cand ⁴分别位于速度障碍空间(φ₁,φ₂)，(φ₂,φ₃)，(φ₁,φ₂)，(φ₂)内；

针对智能体某待选速度位于某速度障碍空间中，定义该速度障碍空间对当前智能体的重要性描述函数如下：

式中，p_m为当前速度障碍空间φ_m所对应的障碍物/智能体位置，‖p_m-p_i‖为当前智能体与对应障碍物/智能体间的欧式距离，R_m为障碍物/智能体半径，r_i为当前智能体半径；n为当前智能体指向对应障碍物/智能体位置的单位方向向量；

某待选速度

的运动特性保持函数描述如下：

对某智能体a_i，其待选速度

的联合计算函数如下：

式中，K_mot和K_imp分别是运动特性权重和重要性权重；

对于某智能体a_i，联合计算函数

最小值对应的待选速度

为所需的最终速度，

8.一种通讯约束下多智能体分布式可飞路径规划系统，其特征在于，包括多个无人机智能体，在每个智能体上安装有传感器、通讯模块、规划器和执行器；

传感器和通讯模块均与规划器连接，规划器与执行器连接；

规划器包括障碍物参数获取模块、智能体参数获取模块、最优局部目标点计算模块、路径规划模块、避障模块和检测模块；

障碍物参数获取模块，用于各智能体通过自身传感器，获取周围的局部环境信息，得到周围局部障碍物参数；

智能体参数获取模块，用于各智能体通过自身通讯装置，与通讯范围内的其他智能体通讯，获取周围智能体参数信息；

最优局部目标点计算模块，用于各智能体利用改进的人工势场模型，结合自身目标信息，从周围局部环境中选取最优局部目标点；

路径规划模块，用于根据最优局部目标点，各智能体结合自身的动力学约束、当前点和局部目标点的位姿，进行场景检测分解，完成局部Dubins路径规划；

避障模块，用于各智能体根据自身Dubins路径规划、局部障碍信息、通讯范围内其他智能体的参数信息，利用改进的RVO避障算法，进行时间的移动，同时进行无人智能体间和无人智能体与障碍物间避障；

检测模块，用于各智能体进行到达目标检测；若未到达目标，更新自身的当前位姿，进行下一步的滚动时域路径规划；若已到达目标，结束。

9.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至7任意一项所述方法的步骤。

10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至7中任意一项所述方法的步骤。

Images