CN113961001B - 一种基于rvo策略和最优控制算法的混合型多智能体协同路径规划方法 - Google Patents

Abstract

一种基于RVO策略和最优控制算法的混合型多智能体协同路径规划方法，属于自动化与控制技术领域。首先，根据待规划的智能体个数来确定参与规划的智能体数量，初始化各个智能体对应的初始和终端状态。其次，初始化协同路径规划过程中的计算步长，RVO策略与最优控制算法切换的阈值参数d。最后，依次检测各智能体当前位置与对应终端位置之间的距离与阈值d之间的关系：若智能体当前位置与对应终端位置之间的距离大于阈值d，采用RVO策略对该智能体进行轨迹规划；若距离小于或等于阈值d，采用最优控制算法对该智能体进行路径规划。本发明能够高精度的进行多智能体在线协同路径规划，所得到的结果能够严格符合终端约束要求，实现计算效率和精度之间的理想平衡。

Description

一种基于RVO策略和最优控制算法的混合型多智能体协同路 径规划方法

技术领域

本发明属于多智能体协同路径规划的自动化与控制技术领域，涉及一种基于RVO(RVO：Reciprocal Velocity Obstacle)策略和最优控制算法的混合型多智能体协同路径规划方法。

背景技术

多智能体协同路径规划是在保持相应约束，并使各个智能体在不与障碍物碰撞的情况下，寻找一条从初始状态到终端状态的可行轨迹。较单个智能体的路径规划问题而言，多智能体的协同路径规划问题存在的难点在于：

(1)除了单个智能体路径规划过程中需要考虑的约束外，还需考虑不同智能体之间的避碰约束，且随着智能体数量的增多，额外增加的避碰约束数量将急剧增加，其求解空间的维度更高，从而使得求解难度更大、更复杂。

(2)难以同时满足各个智能体的终端约束，即难以使得各个智能体均以要求的角度精确到达各自的终端位置。

目前，对于多智能体的协同路径规划问题主要集中在无人机(UAV)和无人车(USV)等领域，其中应用最广泛的方法有人工势场法、数学规划、人工智能算法、RVO策略、最优控制方法、基于网格建模方法和基于优先级的协同策略等。上述方法可应用于多智能体的协同路径规划问题，但难以在计算效率和精度之间达到理想的平衡。如基于数学规划、最优控制方法等，虽然精度可以满足要求，但计算效率较低；人工势场法、RVO策略等，虽然计算效率较高，但精度较低，尤其是难以使智能体精确到达终端位置。而在某些场景中，对智能体的终端约束有严格的要求，即要求智能体能以预定的角度精确地到达终点，但除了最优控制方法外，上述方法均难以满足这样严苛的终端约束。

综上所述，目前，工程应用上迫切需要一种能兼顾计算精度和效率的、具有良好适用性的多智能体的协同路径规划方法。

发明内容

为了解决多智能体协同路径规划方法难以兼顾计算效率和精度的问题，本发明结合RVO策略和最优控制算法提出一种多智能体协同路径规划方法，该多智能体协同路径规划方法计算效率高，能够实现在线协同路径规划，且规划结果能够严格满足各个智能体终端约束多智能体。

为了达到上述目的，本发明采用的技术方案为：

一种基于RVO策略和最优控制算法的混合型多智能体协同路径规划方法，包括以下步骤：

步骤1：根据待规划的智能体个数来确定参与规划的智能体数量M(M∈N⁺)，初始化各个智能体对应的初始和终端状态，如初始和终端处的位置、角度、速度等。对于第i(i∈N⁺)个智能体，其初始状态记为终端状态记为/>

步骤2：初始化协同路径规划过程中的计算步长δT，以及RVO策略与最优控制算法切换的阈值参数d(d≥0)。

步骤3：依次检测各智能体当前位置与对应终端位置之间的距离与阈值d之间的关系，从而判断是采用RVO策略，还是采用最优控制算法对智能体进行路径规划。具体如下：

步骤3-1：初始化时间迭代次数k，令k＝1；初始化RVO策略和最优控制算法切换次数j，令j＝0。

步骤3-2：在[t₀+(k-1)δT,t₀+kδT]内，初始化智能体序号i，令i＝1，其中，t₀表示起始时间。

步骤3-3：检测智能体i当前位置与对应终端位置之间的距离。

对于智能体i，将智能体i采用圆心为其几何中心位置半径R的特征圆进行描述，计算智能体i在t₀+(k-1)δT时刻的位置与对应终端位置/>之间的距离Sⁱ(t₀+(k-1)δT)，具体计算方法为：

其中，分别表示描述智能体i特征圆圆心的横纵标和纵坐标；/>分别表示智能体i终端位置的横纵标和纵坐标。

步骤3-4：若智能体i当前位置与对应终端位置之间的距离Sⁱ(t₀+(k-1)δT)大于阈值d，则采用RVO策略对智能体i进行轨迹规划，并存储其轨迹、时间数据。若智能体i当前位置与对应终端位置之间的距离Sⁱ(t₀+(k-1)δT)小于或等于阈值d，则转入步骤3-5，采用最优控制算法对智能体进行路径规划。步骤3-5：将该智能体i当前时刻的状态(包括位置、角度、速度)作为初始边界条件Xⁱ(t₀+(k-1)δT)，确定各个智能体的运动学、控制、状态约束条件。

对于第i个智能体，令其状态空间为xⁱ、控制变量为uⁱ，建立该智能体的运动学方程：

其中，t为时间变量。

对于第i个智能体，将该智能体对状态变量和控制变量施加的约束表示为如下不等式形式：

C(Xⁱ,Uⁱ,t)≤0 (3)

步骤3-6：建立智能体i轨迹规划的最优控制模型

根据公式(1)、(2)，建立如下智能体i轨迹规划的最优控制模型：

其中，w为终端时间权重因子；t_f表示末端时间。

步骤3-7：求解最优控制模型

根据公式(4)，采用最优控制算法对其进行求解，优化得出最小末端时间及对应的路径数据，并在接下来的时间步长内将其视为按照由最优控制算法所得到路径运动的动态障碍物。同时，令j＝j+1。

步骤4：若j<M，且i<M-j，i＝i+1，返回步骤3-3；若j<M，且i≥M-j，令k＝k+1，转入步骤3-2；若j≥M，退出并输出每个智能体基于RVO策略和最优控制算法得到的轨迹。

进一步的，所述步骤3-4中所述的最优控制算法包括伪谱法、保辛伪谱算法等。

相对于现有技术，本发明有益效果为：

本发明能以更高的精度和效率在线的进行多智能体的协同路径规划，所得到的结果可严格符合终端约束要求，从而实现计算效率和精度之间的理想平衡。所得结论可直接应用于工程实际，具有更好的工程许用性，且具有很强的可操作性和可行性，便于实际应用。

附图说明

图1为本发明的整体思路图。

图2为本发明的4个智能体的轨迹曲线。

图3为本发明的4个智能体的速度曲线；图3(a)为第1个智能体的速度曲线，图3(b)为第2个智能体的速度曲线，图3(c)为第3个智能体的速度曲线，图3(d)为第4个智能体的速度曲线。

图4为本发明的4个智能体的方位角角度曲线；图4(a)为第1个智能体的方位角角度曲线，图4(b)为第2个智能体的方位角角度曲线，图4(c)为第3个智能体的方位角角度曲线，图4(d)为第4个智能体的方位角角度曲线。

具体实施方式

以下结合具体实施例对本发明做进一步说明。

该案例对4个无人车类的智能体从0s开始进行协同路径规划，且4个智能体均不具备倒车功能。智能体1以0m/s、方位角为45°从[-80m，80m]处以0m/s、方位角为0°运动至[80m，80m]处；智能体2以0m/s、方位角为135°从[80m，-80m]处以0m/s、方位角为0°运动至[-80m，80m]处；智能体3以0m/s、方位角为-90°从[80m，80m]处以0m/s、方位角为0°运动至[-80m，-80m]处；智能体4以0m/s、方位角为-90°从[-80m，80m]处以0m/s、方位角为0°运动至[80m，-80m]处。该实施例一种基于RVO策略和最优控制算法的混合型多智能体协同路径规划方法，包括以下步骤：

根据步骤1，确定参与规划的智能体数量M＝4，确定各个智能体对应的初始和终端状态分别为：

根据步骤2，确定协同路径规划过程中的计算步长为δT＝1.5s，以及RVO策略与最优控制算法切换的阈值参数为d＝10.0692。

根据步骤3，依次检测各智能体当前位置与对应终端位置之间的距离与阈值d之间的关系，判断是采用RVO策略还是最优控制算法对智能体进行路径规划。

根据步骤3-1，令k＝1，j＝0。

根据步骤3-2，在[1.5(k-1),1.5k]内，令i＝1。

根据步骤3-3，检测智能体i当前位置与对应终端位置之间的距离。

对于智能体i，将智能体i用圆心为其几何中心位置半径R＝10m的特征圆进行描述，计算智能体i在1.5(k-1)时刻的位置与对应终端位置之间的距离Sⁱ(1.5(k-1))，具体计算方法为：

根据步骤3-4，若智能体i当前位置与对应终端位置之间的距离Sⁱ(1.5(k-1))大于阈值d，则采用RVO策略对智能体i进行轨迹规划，并存储其轨迹、时间数据。若智能体i当前位置与对应终端位置之间的距离Sⁱ(1.5(k-1))小于或等于阈值d，则转入步骤3-5。

根据步骤3-5，将该智能体i当前时刻的状态(包括位置、角度、速度)作为初始边界条件Xⁱ(1.5(k-1))，确定各个智能体的运动学、控制、状态约束条件。

对于第i个智能体，令其状态空间为xⁱ，其由横坐标xⁱ、纵坐标yⁱ、方位角θⁱ、速度vⁱ构成，即xⁱ＝[xⁱ,yⁱ,θⁱ,vⁱ]^T；控制变量为uⁱ，其由前轮转向角φⁱ、加速度aⁱ构成，即uⁱ＝[φⁱ,aⁱ]^T。从而建立该智能体的运动学方程为：

其中，t为时间变量，。

对于第i个智能体，将该智能体对状态变量和控制变量施加的约束表示为如下不等式形式：

根据步骤3-6，建立智能体i轨迹规划的最优控制模型如下：

根据步骤3-7，采用最优控制算法中常用的伪谱法对其进行求解，优化出最小末端时间及对应的路径数据，并在接下来的时间步长内将其视为按照由最优控制算法所得到路径运动的动态障碍物。同时，令j＝j+1。

根据步骤4，若j<4，且i<4-j，i＝i+1，返回步骤3-3；若j<4，且i≥4-j，令k＝k+1，转入步骤3-2；若j≥4，退出并输出每个智能体使用RVO策略和伪谱法得到的轨迹如图2所示，速度曲线如图3所示，方位角角度曲线如图4所示，其中实线部分是由RVO策略规划得到的轨迹，虚线部分由最有控制算法得到的轨迹。

在使用RVO策略规划进行规划时，平均每步耗时28.5ms；使用伪谱法进行规划时，平均耗时0.6s，这说明采用本发明所提出的算法具有较高的计算效率。同时，4个智能体的终端状态均可得到严格满足，这说明本发明所提出的方法不仅具有较高的计算效率，还具备较高的计算精度。

以上所述实施例仅表达本发明的实施方式，但并不能因此而理解为对本发明专利的范围的限制，应当指出，对于本领域的技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些均属于本发明的保护范围。

Claims (3)

1.一种基于RVO策略和最优控制算法的混合型多智能体协同路径规划方法，其特征在于，包括以下步骤：

步骤1：根据待规划的智能体个数确定参与规划的智能体数量M，其中M∈N⁺，初始化各个智能体对应的初始状态和终端状态；对于第i个智能体，其初始状态记为终端状态记为其中i∈N⁺；

步骤2：初始化协同路径规划过程中的计算步长δT，以及RVO策略与最优控制算法切换的阈值参数d，其中d≥0；

步骤3：依次检测各智能体当前位置与对应终端位置之间的距离与阈值d之间的关系，判断是采用RVO策略，还是采用最优控制算法对智能体进行路径规划，具体如下：

步骤3-1：初始化时间迭代次数k，令k＝1；初始化RVO策略和最优控制算法切换次数j，令j＝0；

步骤3-2：在[t₀+(k-1)δT,t₀+kδT]内，初始化智能体序号i，令i＝1，其中，t₀表示起始时间；

步骤3-3：检测智能体i当前位置与对应终端位置之间的距离；

对于智能体i，将智能体i采用圆心为其几何中心位置半径R的特征圆进行描述，计算智能体i在t₀+(k-1)δT时刻的位置与对应终端位置/>之间的距离Sⁱ(t₀+(k-1)δT)，具体计算方法为：

其中，分别表示描述智能体i特征圆圆心的横纵标和纵坐标；/>分别表示智能体i终端位置的横纵标和纵坐标；

步骤3-4：若智能体i当前位置与对应终端位置之间的距离Sⁱ(t₀+(k-1)δT)大于阈值d，则采用RVO策略对智能体i进行轨迹规划，并存储其轨迹、时间数据；若智能体i当前位置与对应终端位置之间的距离Sⁱ(t₀+(k-1)δT)小于或等于阈值d，则转入步骤3-5，采用最优控制算法对智能体进行路径规划；

步骤3-5：将该智能体i当前时刻的状态作为初始边界条件Xⁱ(t₀+(k-1)δT)，确定各个智能体的运动学、控制、状态约束条件；

对于第i个智能体，令其状态空间为xⁱ、控制变量为uⁱ，建立该智能体的运动学方程：

其中，t为时间变量；

对于第i个智能体，将该智能体对状态变量和控制变量施加的约束表示为如下不等式形式：

C(Xⁱ,Uⁱ,t)≤0 (3)

步骤3-6：建立智能体i轨迹规划的最优控制模型

根据公式(1)(2)，建立如下智能体i轨迹规划的最优控制模型：

其中，w为终端时间权重因子；t_f表示末端时间；

步骤3-7：求解最优控制模型

根据公式(4)，采用最优控制算法对其进行求解，优化出最小末端时间及对应的路径数据，并在接下来的时间步长内将其视为按照由最优控制算法所得到路径运动的动态障碍物；同时，令j＝j+1；

步骤4：若j<M，且i＜M-j，i＝i+1，返回步骤3-3；若j<M，且i≥M-j，令k＝k+1，转入步骤3-2；若j≥M，退出并输出每个智能体使用RVO策略和最优控制算法得到的轨迹。

2.根据权利要求1所述的一种基于RVO策略和最优控制算法的混合型多智能体协同路径规划方法，其特征在于，所述步骤1中初始状态、终端状态均包括初始、终端处的位置、角度、速度。

3.根据权利要求1或2所述的一种基于RVO策略和最优控制算法的混合型多智能体协同路径规划方法，其特征在于，所述的最优控制算法包括伪谱法、保辛伪谱算法。