CN116301011A - 一种多智能体高效编队避障方法 - Google Patents

Abstract

本发明公开的一种多智能体高效编队避障方法，属于多智能体编队控制领域。本发明通过智能体运动学模型实现个体碰撞的危险预测；通过综合比较个别个体避障和编队整体变换队形的成本选择是否切换队形；通过构建智能体与目标点对应的成本矩阵，依靠分配算法实现智能体位置的实时优化分配，加快变换速度，降低航行成本；通过一致性算法与改进的动态窗口法，引入目标导向，降低避障过程对队形的破坏，且能有效防止个体在避障过程陷入局部最优，实现个体的高效避障。本发明适用于无人机救援、无人机侦查等领域，提高航行过程的队形变换速度，降低航行成本，实现高效避障。

Description

一种多智能体高效编队避障方法

技术领域

本发明涉及一种多智能体高效编队避障方法，属于多智能体编队控制领域。

背景技术

伴随着智能无人系统的不断完善，多智能体技术逐步成为研究热点，相比于单体作业系统，多智能体系统具有更高的执行效率、更好的容错性以及更强的适应能力，能够在更加多变的场景下执行复杂的协同任务，如协同救援、协同勘探、协同运输、协同侦察等。

目前针对多智能体的研究中，编队跟踪、一致性问题、适应环境干扰、通信拓扑切换等问题已得到广泛讨论。针对编队整体避障问题，目前大部分研究采用拉伸、压缩队形的方式实现，即在可通行区域较宽的环境航行时，适当增加智能体之间的距离，为每个智能体预留较宽的航行空间，提高编队的安全性，而在可通行区域较窄的区域或障碍物附近适当减小智能体之间的距离，保证编队整体能够安全通过，相比于切换队形结构，该方式避障能力有限。在编队避障过程中规划并分配每个智能体位置从而对编队队形进行优化重构的相关研究较少，现有方法在避障过程中往往无法平衡智能体与编队的关系，对队行形状产生了较大破坏，不利于队形的快速恢复。除此之外，在队形重构过程中，研究者往往只在理想无障碍的环境下考虑了智能体与待分配目标点的直线距离，并未考虑到智能体本身的动力学特性和切换过程中冲突情况的发生。

发明内容

针对现有编队避障技术存在避障效率低、对队形破坏大、队形恢复慢以及不适用于复杂的实际航行环境的问题，本发明的主要目的是提供一种多智能体高效编队避障方法，提高航行过程中智能体的变换速度，降低航行成本，实现高效避障。

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

本发明公开的一种多智能体高效编队避障方法，通过智能体运动学模型实现个体碰撞的危险预测；通过综合比较个别个体避障和编队整体变换队形的成本选择是否切换队形；通过构建智能体与目标点对应的成本矩阵，依靠分配算法实现智能体位置的实时优化分配，加快变换速度，降低航行成本；通过一致性算法与改进的动态窗口法，引入目标导向，降低避障过程对队形的破坏，且能有效防止个体在避障过程陷入局部最优，实现个体的高效避障。

本发明公开的一种多智能体高效编队避障方法，包含以下步骤：

步骤1：采用通常的领航者-跟随者法进行编队，编队内指定一个领航者智能体和多个跟随者智能体；

步骤2：为编队设置可用公式表达的队形数据库及避障等级；

H_k＝{h₁,h₂,h₃,...,h_n-1} (1)

上述H_k可依据任务要求在队形数据库中包含“一”字形、倒“V”字形、树杈形、顺次“1”字形等，其中，h_i＝(h_ix,h_iy)，为跟随者智能体i相对于领航者的位置偏移，其中h_ix,h_iy应满足：

上述h_xmin、h_xmax为智能体间的最小、最大安全距离，依据智能体大小体积、制动能力、转弯能力、有效通信距离确定。

上述避障等级可依据队形宽度设置，其具体实施步骤是：

(1)计算队形数据库内所有队形的队形最大宽度，其计算公式为：

w_max＝max(h_ix-h_jx),i≠j (3)

(2)将队形最大宽度按从小到大的顺序排列；

(3)将队形宽度的排列次序设置为当前队形的避障等级；

步骤3：跟随者智能体根据当前的航行状态及感知到的障碍信息，获取当前航行状态下的危险指数；

构建智能体运动学模型：

其中，x_i、y_i分别代表智能体i的横坐标和纵坐标，v_i代表智能体i的速度，θ_i代表智能体i的航向角。同时，对每个智能体的运动学约束如下：

其中，v_min、v_max分别为智能体的最小、最大速度，a_min、a_max分别为智能体的最小最大加速度，ω_min、ω_max分别为智能体的最小最大角速度。

跟随者通过GPS、惯性传感器获取自身位置、姿态、速度、角速度、加速度、角加速度信息；

跟随者智能体依靠激光雷达等传感器获取感知区域内的障碍物信息；

跟随者智能体根据当前状态信息及运动学模型预测自身未来一段时间序列内的状态信息,得到当前智能体在未来时刻的状态序列；预测公式如下，

跟随者智能体根据自身未来时间序列内的状态信息及障碍物信息计算当前状态下的危险评价指数，其中，评价函数为：

上式中，p_i(t+k)为智能体i在预测步长k时刻的位置坐标，

为智能体探测范围内障碍物j的位置坐标，ws_i为智能体i的机身宽度，/>

为障碍物j的半径。

跟随者智能体将上述安全评价函数发送给领航者智能体；

步骤4：领航者获取跟随者位置信息及危险指数；

步骤5：领航者根据跟随者的危险指数选取适当的编队参数及编队队形，并基于自身位置生成编队目标点；

领航者获取编队内跟随者智能体的航行危险指数；

领航者基于跟随者智能体的航行危险指数S_i确定队形和队形具体参数，即存在单个智能体其航行危险指数大于危险阈值γ_H，或所有智能体危险指数之和大于γ_S，则将期望队形设为避障等级更高的编队队形；当所有智能体的航行危险指数小于γ_L时，则将期望队形设为避障等级更低的队形；

获取上一时刻编队队形的避障等级；

根据航行任务及智能体数量确定预选择的队形结构；

比较预选择的队形结构和上一时刻队形结构避障等级；

预选择的队形结构避障等级高于上一时刻的队形结构，则将预选择的队形结构确定为接下来的队形，否则执行以下步骤：

(1)判断编队切换是否完成，已完成，则切换至预选队形，否则保持原本预期队形；

(2)上述编队切换完成的条件为：编队内所有智能体与其目标点的位置偏移均小于误差阈值e_s，则代表编队切换完成；

领航者基于GPS、IMU等传感获取自身的位置及状态信息；

领航者基于确定的期望队形、队形具体参数、得到每个目标点相对于领航者的编队偏移量。

步骤6：领航者基于生成的目标点位置及跟随者位置为每个跟随者分配瞬时目标点，得到每个智能体对应的目标点编队内相对偏移量；

领航者获取每个跟随者的当前位置和待分配目标点的瞬时偏移量h_in；

领航者基于待分配目标点的瞬时偏移量计算地理坐标系下的目标点位置坐标，计算公式为：

领航者计算跟随者智能体与其目标点的位置误差，平均位置误差小于阈值e_a，则维持原本分配方案，否则进行以下步骤进行目标点重分配计算：

领航者基于跟随者的状态信息及目标点的地理位置信息构建所有智能体到所有目标点的航行成本矩阵C(t)：

C(t)＝[c_ij(t)] (9)

其中

代表智能体i相对于目标点j的距离成本，/>

代表智能体i与目标点j的角度成本，/>

代表智能体i与目标点之间的冲突成本，计算方式如下：

上式中，

为t时刻智能体i与目标点j之间的直线距离，r_min为智能体的最小转弯半径，θ(t)为智能体航向角，

为在地理坐标系下智能体i与目标点之间的相对位置角度，N_ij(t)为t时刻智能体i与目标点j之间存在的其他目标点的个数。

领航者计算跟随者智能体的平均剩余能量，存在剩余能量低于平均剩余能量的β倍的智能体，即剩余能量最少的智能体i满足β*E_i<*E_a时，则优先分配该智能体的目标点为其航行成本最低的目标点，且该智能体和目标点不参与后续的目标点分配，具体做法是将该智能体与该目标点对应的航行成本置为0，而将该智能体与其他目标点以及该目标点与其他智能体队形的航行成本矩阵元素值为无穷大；

领航者基于目标分配算法求解最佳分配方案，使得航行成本最小，优化目标为：

领航者将分配结果广播给所有跟随者智能体；

步骤7：跟随者基于上述编队相对偏移量及编队状态信息利用一致性算法生成初始控制量；

获取领航者位姿信息；

获取具有通信关系的邻居智能体的位姿信息；

获取领航者广播的每个智能体偏移量期望信息；

基于上述期望偏移量计算相对于每个邻居智能体的期望相对位置关系；

构建一致性控制协议，基于上述相对于每个邻居智能体的期望相对位置关系计算一致性输出控制量，其计算公式为：

上式中，K_f为跟随者的惯性因子，ξ为速度调节因子，二者均为常数。σⁱ(t)、errⁱ(t)分别为跟随者智能体i的角度偏差和距离偏差，其计算方法如下：

式中，ω^ij(t)为误差权重调节因子，a为一常数，Δxⁱ、Δyⁱ分别为智能体i相对于领航者的编队横向偏移量和纵向偏移量。

步骤8：遭遇障碍物时，以一致性算法生成的初始控制量作为DWA算法的初始解，并基于改进的DWA算法进行避障；

基于雷达等传感器获取障碍物信息，构建按障碍物影响范围；

基于GPS、IMU等获取本机位置、姿态信息；

判断本智能体是否进入障碍物影响区域，进入障碍物影响区域，则执行以下步骤：

获取利用一致性算法生成初始控制量；

基于当前控制量生成备选控制量集

基于前述智能体模型预测在每个备选控制量下航行状态；

计算在上述未来航行状态下的控制评价函数；

上式中，a、b、c、d均为常数，

为备选速度v_k所对应的航向角，ψⁱ为智能体i与其目标点连线相对于地理坐标系x轴的夹角。该式中第一项用于评价相对于障碍物的安全指数，第二项用于评价相对于领航者航向角的一致性，第三项代表了避障方向相对于与被分配的目标点的相对角度，为智能体提供目标导向；第四项用于衡量速度的变化情况，用于抑制速度的突变，U^ij(t)为障碍物影响势场，计算公式为：

选取控制量集内对应评价函数值最高的备选控制量作为避障输出，实现避障；

有益效果

1、本发明公开的一种多智能体高效编队避障方法，通过判定单个智能体其航行危险指数是否大于危险阈值，或所有智能体危险指数之和是否大于所设置的整体阈值，调整期望队形的避障等级，实现编队队形的合理切换，避障过程更加灵活；

2、本发明公开的一种多智能体高效编队避障方法，采用一种智能体目标点评价函数计算方法，在分配过程中考虑相对距离、相对角度以及智能体之间的冲突，并为其分配不同权值用于计算评价函数，形成智能体对目标点的成本矩阵，最终通过分配算法实现智能体与目标点之间的最优分配，优化了避障过程中队形恢复和切换成本，且在分配过程中考虑相对距离、相对角度以及智能体之间产生冲突时的成本，分配过程更加合理，且能有效避免冲突情况的发生；

3、本发明公开的一种多智能体高效编队避障方法，采用一种编队内智能体能量均衡方法，领航者首先获取编队内跟随者智能体的剩余能量，随后计算跟随者智能体的平均剩余能量，剩余能量最少智能体的剩余能量较大程度低于平均剩余能量，则优先分配该智能体的目标点为其航行成本最低的目标点，且该智能体和目标点不参与后续的目标点分配。保证了能耗的合理调配，避免了所剩能量较低的智能体因频繁的位置切换耗费较多的能量；

4、本发明公开的一种多智能体高效编队避障方法，采用一种编队航行过程中的避障方法，遭遇障碍物时，以一致性算法生成的初始控制量作为DWA算法的初始解，基于雷达等传感器获取障碍物信息，构建障碍物影响范围，基于GPS、IMU等获取本机位置、姿态信息，并判断本智能体是否进入障碍物影响区域，进入障碍物影响区域，则利用一致性算法生成初始控制量和备选控制量集，基于障碍物信息、编队内领航者运动状态、目标点的位置信息、速度变化情况计算预选速度的评价函数，最终选取控制量集内对应评价函数值最高的备选控制量作为最终避障输出，完成避障。避障过程中不仅考虑了障碍物信息，还考虑了编队内领航者智能体运动状态、目标点的位置信息、速度变化情况，避免因避障方向选择不合理使得智能体陷入封锁区的情况，降低避障过程对队形的破坏；

附图说明

图1是本发明公开的一种多智能体高效编队避障方法的流程图；

图2是不切换队形结构的传统编队避障方式的航行实验结果；

其中图2a)是智能体编队的航行轨迹，图2b)是智能体编队的航行误差曲线；

图3是不采用分配算法优化智能体目标点的编队避障航行实验结果；

其中图3a)是智能体编队的航行轨迹，图3b)是智能体编队的航行误差曲线；

图4是采用仅以智能体与目标点相对距离构建成本矩阵使用匈牙利算法实现分配优化的编队避障航行实验结果；

其中图4a)是智能体编队的航行轨迹，图4b)是智能体编队的航行误差曲线；

图5是采用传统DWA算法实现智能体个体避障的编队航行实验结果；

其中图5a)是智能体编队的航行轨迹，图5b)是智能体编队的航行误差曲线；

图6是综合考虑多种成本构建成本矩阵采用匈牙利算法实现目标点优化分配，并基于改进的DWA算法实现个体避障的编队航行实验结果图；

其中6a)是智能体编队的航行轨迹，图6b)是智能体编队的航行误差曲线。

具体实施方式

下面将结合附图和实施例对本发明加以详细说明。同时也叙述了本发明技术方案解决的技术问题及有益效果，需要指出的是，所描述的实施例仅旨在便于对本发明的理解，而对其不起任何限定作用。

本实施例公开本发明提出的一种多智能体编队高效避障方法的流程图如附图1所示，虚拟环境大小为120*120像素点，障碍物以实心圆表示，其大小和位置均为随机生成，领航者航行路径由DWA算法给出。

步骤1：采用领航者-跟随者法进行编队，编队内智能体数量为5，包含一个领航者(编号为5)和四个跟随者(编号分编为1、2、3、4)，编队内每个跟随者智能体均可与领航者进行通信；

步骤2：为编队设置可用公式表达的队形数据库及避障等级，其通用公式表达为：

H_k＝{h₁,h₂,h₃,...,h_n-1} (19)

其中，h_i＝(h_ix,h_iy)，为跟随者智能体i相对于领航者的位置偏移，其中h_ix,h_iy应满足：

上述h_xmin、h_xmax为智能体间的最小、最大安全距离，依据智能体大小体积、制动能力、转弯能力、有效通信距离确定。以简单三种队形结构为例，H_k可定义为：

H₃：{h₁＝(0，-12)，h₂＝(0,-9)，h₃＝(0,-6)，h₄＝(0,-3)}

H₁：{h₁＝(-6,-6)，h₂＝(-3,-3)，h₃＝(3,-3)，h₄＝(6,-6)}

H₂：{h₁＝(-12,-12)，h₂＝(-6,-6)，h₃＝(6,-6)，h₄＝(12,-12)}

避障等级可依据队形宽度设置，其具体实施步骤是：

(1)计算队形数据库内所有队形的队形最大宽度，其计算公式为：

w_max＝max(h_ix-h_jx),i≠j (21)

则

(2)将队形最大宽度按从小到大的顺序排列，

(3)将队形宽度的排列次序设置为当前队形的避障等级，D₁＝1，D₂＝2，D₃＝3。

步骤3：跟随者智能体根据当前的航行状态及感知到的障碍信息获取当前航行状态下的危险指数，其具体实施方法为：

构建智能体运动学模型，以一阶运动模型为例，其表达式为：

其中，x_i、y_i分别代表智能体i的横坐标和纵坐标，v_i代表智能体i的速度，θ_i代表智能体i的航向角。同时，对每个智能体的运动学约束如下：

其中，v_min＝0、v_max＝3分别为智能体的最小、最大速度，a_min＝-2、a_max＝2分别为智能体的最小最大加速度，ω_min＝-1、ω_max＝1分别为智能体的最小最大角速度。

跟随者通过GPS、惯性传感器获取自身位置、姿态、速度、角速度、加速度、角加速度等状态信息；

跟随者智能体依靠激光雷达等传感器获取感知区域内的障碍物信息；

跟随者智能体根据当前状态信息及运动学模型预测自身未来一段时间序列内的状态信息,预测公式如下式所示，得到当前智能体在未来时刻的状态序列；

跟随者智能体根据自身未来时间序列内的状态信息及障碍物信息计算当前状态下的危险评价指数，其中，评价函数可设为：

上式中，p_i(t+k)为智能体i在预测步长k时刻的位置坐标，

为智能体探测范围内障碍物j的位置坐标，ws_i为智能体i的机身宽度，ws₁＝ws₂＝ws₃＝ws₄＝ws₅＝0.5，/>

为障碍物j的半径。

跟随者智能体将上述安全评价函数发送给领航者智能体；

步骤4：领航者获取跟随者位置信息及危险指数；

步骤5：领航者根据跟随者的危险指数选取适当的编队参数及编队队形并基于自身位置生成编队目标点，其具体实施方式为：

领航者获取编队内跟随者智能体的航行危险指数；

领航者基于跟随者智能体的航行危险指数S_i确定队形和队形具体参数，即存在单个智能体其航行危险指数大于危险阈值γ_H＝e^-1或所有智能体危险指数之和大于γ_S＝3e^-1，则将期望队形设为避障等级更高的编队队形；所有智能体的航行危险指数小于γ_L＝e^-3，则将期望队形设为避障等级更低的队形；

获取上一时刻编队队形的避障等级；

根据航行任务及智能体数量确定预选择的队形结构；

比较预选择的队形结构和上一时刻队形结构避障等级；

预选择的队形结构避障等级高于上一时刻的队形结构，则将预选择的队形结构确定为接下来的队形，否则执行以下步骤：

(1)判断编队切换是否完成，已完成，则切换至预选队形，否则保持原本预期队形；

(2)上述编队切换完成的条件为：编队内所有智能体与其目标点的位置偏移均小于误差阈值e_s＝0.3，则代表编队切换完成；

领航者基于GPS、IMU等传感获取自身的位置及状态信息；

领航者基于确定的期望队形、队形具体参数、得到每个目标点相对于领航者的编队偏移量。

步骤6：领航者基于生成的目标点位置及跟随者位置为每个跟随者分配瞬时目标点，得到每个智能体对应的目标点编队内相对偏移量，具体步骤为：

领航者获取每个跟随者的当前位置和待分配目标点的瞬时偏移量h_in；

领航者基于待分配目标点的瞬时偏移量计算地理坐标系下的目标点位置坐标，计算公式为：

领航者计算跟随者智能体与其目标点的位置误差，平均位置误差小于阈值e_a＝0.2，则维持原本分配方案，否则进行以下步骤进行目标点重分配计算：

领航者基于跟随者的状态信息及目标点的地理位置信息构建所有智能体到所有目标点的航行成本矩阵C(t)：

C(t)＝[c_ij(t)] (27)

上式中α＝1，β＝0.8，γ＝1，

代表智能体i相对于目标点j的距离成本，/>

代表智能体i与目标点j的角度成本，/>

代表智能体i与目标点之间的冲突成本，计算方式如下：

上式中，

为t时刻智能体i与目标点j之间的直线距离，r_min为智能体的最小转弯半径，θ(t)为智能体航向角，

为在地理坐标系下智能体i与目标点之间的相对位置角度，N_ij(t)为t时刻智能体i与目标点j之间存在的其他目标点的个数。

领航者计算跟随者智能体的平均剩余能量，剩余能量最少的智能体i的剩余能量低于平均剩余能量的0.75倍，则优先分配该智能体的目标点为其航行成本最低的目标点，且该智能体和目标点不参与后续的目标点分配，具体做法是将该智能体与该目标点对应的航行成本置为0，而将该智能体与其他目标点以及该目标点与其他智能体队形的航行成本矩阵元素值为无穷大；

领航者基于匈牙利算法求解最佳分配方案，使得航行成本最小，优化目标为：

领航者将分配结果广播给所有跟随者智能体；

步骤7：跟随者基于上述编队相对偏移量及编队状态信息利用一致性算法生成初始控制量，其具体实施步骤为：

获取领航者位姿信息；

获取具有通信关系的邻居智能体的位姿信息；

获取领航者广播的每个智能体偏移量期望信息；

基于上述期望偏移量计算相对于每个邻居智能体的期望相对位置关系；

构建一致性控制协议，基于上述相对于每个邻居智能体的期望相对位置关系计算一致性输出控制量，其计算公式为：

上式中，K_f＝0.5为跟随者的惯性因子，ξ＝0.5为速度调节因子。σⁱ(t)、errⁱ(t)分别为跟随者智能体i的角度偏差和距离偏差，其计算方法如下：

式中，ω^ij(t)为误差权重调节因子，a＝2，Δxⁱ、Δyⁱ分别为智能体i相对于领航者的编队横向偏移量和纵向偏移量。

步骤8：遭遇障碍物时，以一致性算法生成的初始控制量作为DWA算法的初始解，并基于改进的DWA算法进行避障，其具体实施步骤为：

基于雷达等传感器获取障碍物信息，构建按障碍物影响范围；

基于GPS、IMU等获取本机位置、姿态信息；

判断本智能体是否进入障碍物影响区域，进入障碍物影响区域，则执行以下步骤：

获取利用一致性算法生成初始控制量；

基于当前控制量生成备选控制量集

基于前述智能体模型预测在每个备选控制量下航行状态；

计算在上述未来航行状态下的控制评价函数；

/>

上式中，a＝5、b＝1、c＝1、d＝3均为常数，

为备选速度v_k所对应的航向角，ψⁱ为智能体i与其目标点连线相对于地理坐标系x轴的夹角。该式中第一项用于评价相对于障碍物的安全指数，第二项用于评价相对于领航者航向角的一致性，第三项代表了避障方向相对于与被分配的目标点的相对角度，为智能体提供目标导向；第四项用于衡量速度的变化情况，用于抑制速度的突变，U^ij(t)为障碍物影响势场，计算公式为：

上式中，ρ₀＝3为障碍物的影响范围。

选取控制量集内对应评价函数值最高的备选控制量作为最终避障输出，完成避障；

步骤9：根据步骤1至步骤8，本实施例在MATLAB中得到的实验结果分别由步骤9.1、步骤9.2、步骤9.3和步骤9.4给出。附图2至附图6中使用编号①②③④标注出了编队在行进过程中的关键状态变化，其中深蓝色虚线表示智能体之间的通信关系，智能体与目标点之间的彩色实线表示智能体与目标点的分配关系。

步骤9.1：不切换队形结构的传统编队避障方式航行试验结果如附图2所示，本实验中个体避障采用了步骤8所述改进的DWA算法，由于避障过程中没有切换队形结构，智能体在避障过程中易产生冲突，使得避障过程中存在较大风险，附图2a)标号③处，跟随者2在避障过程中由于障碍物的阻挡逐渐远离编队，对队形造成了较大的破坏，不利于队形的快速恢复，当该跟随者与领航者的距离超过通信距离时，则会造成该智能体脱离编队的情况。

步骤9.2：将步骤2至步骤5所述队形切换算法引入步骤9.1所述的传统避障算法，得到航行实验结果如附图3所示，在障碍物处，由于编队队形的合理切换，智能体以线性队形顺次通过障碍区域，避免了跟随者因盲目避障而驶离编队的情况，提升了编队的鲁棒性和航行的安全性，但由于该过程中并未实现智能体位置的合理优化分配，在避障过程中存在较大的位置误差，且误差下降缓慢。

步骤9.3：在步骤9.2的基础上，引入目标点分配算法实现编队内智能体的位置分配，得到如附图4和附图6所示的实验结果，附图3展示了采用了匈牙利算法进行目标点的实时重分配后的仿真结果，分配过程中仅考虑智能体和目标点之间的距离成本，可以看到重新分配目标点后的编队误差得到明显优化，但该算法均未考虑智能体的动力学限制因素，同时无法抑制最远分配距离，在分配过程中也无法避免编队内部冲突情况的发生，在附图3)标号③处，产生了上述不合理的分配结果。针对上述算法进行步骤6所述的改进后，得到子附图6所示的实验结果，可以看到改进后的分配算法在原算法的基础上进一步优化了最大编队误差，同时可避免编队内部冲突，加快了队形切换速度，增加了航行安全性。

步骤9.4：采用传统DWA算法实现智能体个体避障的实验结果如附图5所示，智能体在没有考虑编队因素及速度连续性的条件下完成避障，在避障过程中速度产生了较大的震荡，同时在附图5a)标号②处，出现了部分智能体远离编队的情况。附图6展示了步骤8所述改进后的DWA算法的实验结果，智能体在避障过程中以领航者位置及临时目标点位置为导向，对速度的突变施加惩罚项，避免了智能体脱离编队的情况，对队形破坏较小，同时减少了速度突变，增加了航行路径的平滑性。

以上所述的具体描述，对发明的目的、技术方案和有益效果进行进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种多智能体高效编队避障方法，其特征在于：包括如下步骤，

步骤1：采用通常的领航者-跟随者法进行编队，编队内指定一个领航者智能体和多个跟随者智能体；

步骤2：为编队设置可用公式表达的队形数据库及避障等级；

步骤3：跟随者智能体根据当前的航行状态及感知到的障碍信息，获取当前航行状态下的危险指数；

步骤4：领航者获取跟随者位置信息及危险指数；

步骤5：领航者根据跟随者的危险指数选取适当的编队参数及编队队形，并基于自身位置生成编队目标点；

步骤6：领航者基于生成的目标点位置及跟随者位置为每个跟随者分配瞬时目标点，得到每个智能体对应的目标点编队内相对偏移量；

步骤7：跟随者基于上述编队相对偏移量及编队状态信息利用一致性算法生成初始控制量；

步骤8：遭遇障碍物时，以一致性算法生成的初始控制量作为DWA算法的初始解，并基于改进的DWA算法进行避障。

2.如权利要求1所述的一种多智能体高效编队避障方法，其特征在于：步骤二的实现方法为，

H_k＝{h₁,h₂,h₃,...,h_n-1} (1)

上述H_k可依据任务要求在队形数据库中包含“一”字形、倒“V”字形、树杈形、顺次“1”字形等，其中，h_i＝(h_ix,h_iy)，为跟随者智能体i相对于领航者的位置偏移，其中h_ix,h_iy应满足：

上述h_xmin、h_xmax为智能体间的最小、最大安全距离，依据智能体大小体积、制动能力、转弯能力、有效通信距离确定；

上述避障等级依据队形宽度设置，其具体实施步骤是：

(1)计算队形数据库内所有队形的队形最大宽度，其计算公式为：

w_max＝max(h_ix-h_jx),i≠j (3)

(2)将队形最大宽度按从小到大的顺序排列；

(3)将队形宽度的排列次序设置为当前队形的避障等级。

3.如权利要求1所述的一种多智能体高效编队避障方法，其特征在于：步骤三的实现方法为，

构建智能体运动学模型：

其中，x_i、y_i分别代表智能体i的横坐标和纵坐标，v_i代表智能体i的速度，θ_i代表智能体i的航向角；同时，对每个智能体的运动学约束如下：

其中，v_min、v_max分别为智能体的最小、最大速度，a_min、a_max分别为智能体的最小最大加速度，ω_min、ω_max分别为智能体的最小最大角速度；

跟随者通过GPS、惯性传感器获取自身位置、姿态、速度、角速度、加速度、角加速度信息；

跟随者智能体依靠激光雷达等传感器获取感知区域内的障碍物信息；

跟随者智能体根据当前状态信息及运动学模型预测自身未来一段时间序列内的状态信息,得到当前智能体在未来时刻的状态序列；预测公式如下，

跟随者智能体根据自身未来时间序列内的状态信息及障碍物信息计算当前状态下的危险评价指数，其中，评价函数为：

上式中，p_i(t+k)为智能体i在预测步长k时刻的位置坐标，

为智能体探测范围内障碍物j的位置坐标，ws_i为智能体i的机身宽度，/>

为障碍物j的半径；

跟随者智能体将上述安全评价函数发送给领航者智能体。

4.如权利要求1所述的一种多智能体高效编队避障方法，其特征在于：步骤五的实现方法为，

领航者获取编队内跟随者智能体的航行危险指数；

领航者基于跟随者智能体的航行危险指数S_i确定队形和队形具体参数，即存在单个智能体其航行危险指数大于危险阈值γ_H，或所有智能体危险指数之和大于γ_S，则将期望队形设为避障等级更高的编队队形；当所有智能体的航行危险指数小于γ_L时，则将期望队形设为避障等级更低的队形；

获取上一时刻编队队形的避障等级；

根据航行任务及智能体数量确定预选择的队形结构；

比较预选择的队形结构和上一时刻队形结构避障等级；

预选择的队形结构避障等级高于上一时刻的队形结构，则将预选择的队形结构确定为接下来的队形，否则执行以下步骤：

(1)判断编队切换是否完成，已完成，则切换至预选队形，否则保持原本预期队形；

(2)上述编队切换完成的条件为：编队内所有智能体与其目标点的位置偏移均小于误差阈值e_s，则代表编队切换完成；

领航者基于GPS、IMU等传感获取自身的位置及状态信息；

领航者基于确定的期望队形、队形具体参数、得到每个目标点相对于领航者的编队偏移量。

5.如权利要求1所述的一种多智能体高效编队避障方法，其特征在于：步骤六的实现方法为，

领航者获取每个跟随者的当前位置和待分配目标点的瞬时偏移量h_in；

领航者基于待分配目标点的瞬时偏移量计算地理坐标系下的目标点位置坐标，计算公式为：

领航者计算跟随者智能体与其目标点的位置误差，平均位置误差小于阈值e_a，则维持原本分配方案，否则进行以下步骤进行目标点重分配计算：

领航者基于跟随者的状态信息及目标点的地理位置信息构建所有智能体到所有目标点的航行成本矩阵C(t)：

C(t)＝[c_ij(t)] (9)

其中

代表智能体i相对于目标点j的距离成本，/>

代表智能体i与目标点j的角度成本，/>

代表智能体i与目标点之间的冲突成本，计算方式如下：

上式中，

为t时刻智能体i与目标点j之间的直线距离，r_min为智能体的最小转弯半径，θ(t)为智能体航向角，/>

为在地理坐标系下智能体i与目标点之间的相对位置角度，N_ij(t)为t时刻智能体i与目标点j之间存在的其他目标点的个数；

领航者计算跟随者智能体的平均剩余能量，存在剩余能量低于平均剩余能量的β倍的智能体，即剩余能量最少的智能体i满足β*E_i<*E_a时，则优先分配该智能体的目标点为其航行成本最低的目标点，且该智能体和目标点不参与后续的目标点分配，具体做法是将该智能体与该目标点对应的航行成本置为0，而将该智能体与其他目标点以及该目标点与其他智能体队形的航行成本矩阵元素值为无穷大；

领航者基于目标分配算法求解最佳分配方案，使得航行成本最小，优化目标为：

领航者将分配结果广播给所有跟随者智能体。

6.如权利要求1所述的一种多智能体高效编队避障方法，其特征在于：步骤七的实现方法为，

获取领航者位姿信息；

获取具有通信关系的邻居智能体的位姿信息；

获取领航者广播的每个智能体偏移量期望信息；

基于上述期望偏移量计算相对于每个邻居智能体的期望相对位置关系；

构建一致性控制协议，基于上述相对于每个邻居智能体的期望相对位置关系计算一致性输出控制量，其计算公式为：

上式中，K_f为跟随者的惯性因子，ξ为速度调节因子，二者均为常数；σⁱ(t)、errⁱ(t)分别为跟随者智能体i的角度偏差和距离偏差，其计算方法如下：

式中，ω^ij(t)为误差权重调节因子，a为一常数，Δxⁱ、Δyⁱ分别为智能体i相对于领航者的编队横向偏移量和纵向偏移量。

7.如权利要求1所述的一种多智能体高效编队避障方法，其特征在于：步骤四的实现方法为，

基于雷达等传感器获取障碍物信息，构建按障碍物影响范围；

基于GPS、IMU等获取本机位置、姿态信息；

判断本智能体是否进入障碍物影响区域，进入障碍物影响区域，则执行以下步骤：

获取利用一致性算法生成初始控制量；

基于当前控制量生成备选控制量集

基于前述智能体模型预测在每个备选控制量下航行状态；

计算在上述未来航行状态下的控制评价函数；

上式中，a、b、c、d均为常数，

为备选速度v_k所对应的航向角，ψⁱ为智能体i与其目标点连线相对于地理坐标系x轴的夹角；该式中第一项用于评价相对于障碍物的安全指数，第二项用于评价相对于领航者航向角的一致性，第三项代表了避障方向相对于与被分配的目标点的相对角度，为智能体提供目标导向；第四项用于衡量速度的变化情况，用于抑制速度的突变，U^ij(t)为障碍物影响势场，计算公式为：

选取控制量集内对应评价函数值最高的备选控制量作为避障输出，实现避障。

Images