CN113741454A - 一种基于搜索的多智能体路径规划方法及系统 - Google Patents

Abstract

一种基于搜索的多智能体路径规划方法及系统，方法如下：步骤一、设置每个智能体的初始位姿和目标位姿、2D地图，将智能体和障碍物形状描述为几何图形，进行初始位姿和目标位姿的碰撞检测；步骤二、基于时空混合A*算法，对每个智能体进行低层次的路径规划；在混合A*基础上，搜索中加入时间属性，碰撞检测时考虑时间因素；每个智能体搜索到的路径的环境无碰撞，智能体之间可能有碰撞；步骤三、在高层次下对每个离散的时间点检测智能体的碰撞情况；如存在碰撞则为两个智能体分别构建约束，进行重新搜索，直至在所有时间点都通过碰撞检测，得到各智能体的无碰撞路径；步骤四、通过带前馈和反馈的轨迹算法进行阿克曼底盘的循迹导引，完成路径跟踪。

Description

一种基于搜索的多智能体路径规划方法及系统

技术领域

本发明属于多智能体协同的技术领域，具体涉及一种基于搜索的多智能体路径规划方法及系统。

背景技术

近年来，由于机器人和传感器的高速发展，多智能体协同系统由于其自主性高、协作能力强等特点，已经受到了人们的广泛关注。尤其在配备了激光雷达、惯性测量单元、深度相机等高性能传感器后，系统中各个智能体能够独立地完成高精度定位、与环境的交互，并可以高效地完成各类复杂的任务，例如仓库分拣、自动驾驶等。但在多智能体系统的应用中，需要考虑智能体的体积，以避免智能体与环境障碍物的碰撞，以及智能体之间的相互碰撞。因此，多智能体的无碰撞路径规划是多智能体协同系统中不可缺少的一环。

多智能体路径规划，是指在给定的障碍物环境下，每个智能体从各自的起始位姿移动到指定的目标位姿，并避免相互碰撞。多智能体路径规划是一个NP难问题，解决这一问题的方法有基于简并的方法、基于搜索的方法，以及蚁群算法、粒子群算法以及强化学习等方法等。然而，目前的方法研究大多都基于完全机器人的假设，即智能体被建模为圆形并可以随意旋转；或者采用离散网格作为搜索空间，得出的解决方案比较粗糙。但实际机器人大多设计为类似车辆的模型，具有最小转弯半径限制，因此这些不平滑的路径较难被实际机器人跟踪。另外，智能体的碰撞检测也是一个重要的问题。在实际情况中，智能体和障碍物通常具有各种形状，而在现有研究中，对于智能体往往不考虑碰撞或者设置一个安全半径，且关于实现无碰撞的策略比较匮乏。因此，研究出一种高效的多智能体无碰撞路径规划策略是十分必要的。

发明内容

本发明的目的是针对现有技术的缺陷，提供了一种基于搜索的多智能体路径规划方法及系统，以解决现有对于多智能体规划路径难以执行、碰撞检测不够精确的问题，同时其简单稳定的轨迹规划方法，能控制阿克曼底盘智能体跟踪所得路径。

为了实现以上目的，本发明采用以下技术方案：一种基于搜索的多智能体路径规划方法，包括以下步骤：

步骤一、设置每个智能体的初始位姿和目标位姿，设置2D地图，将智能体和障碍物形状描述为几何图形，进行初始位姿和目标位姿的碰撞检测，并确保智能体的初始状态和目标状态在2D地图范围内；

步骤二、基于时空混合A*算法，对每个智能体分别进行低层次的路径规划。时空混合A*在混合A*基础上，在搜索中加入时间属性，并在碰撞检测时考虑时间因素。每个智能体搜索到的路径的环境无碰，但智能体之间可能有碰；

步骤三、在高层次下，对每个离散的时间点检测智能体的碰撞情况；如存在碰撞则为两个智能体分别构建约束，进行重新搜索，直至在所有时间点都通过碰撞检测为止，得到各智能体的无碰撞路径；

步骤四、通过带前馈和反馈的轨迹算法进行阿克曼底盘的循迹导引，完成路径的跟踪。

进一步的，所述步骤一具体为：

S11、将路径规划问题抽象为一个2D平面，建立笛卡尔坐标系，将各个智能体的初始位姿和目标位姿表示为横坐标、纵坐标和朝向角的三元组(x,y,ψ)；

S12、将障碍物和智能体的形状描述为几何图形；

S13、对任务的可行性进行检验，单个智能体的初始状态、障碍物及其他智能体的初始状态之间是否有碰撞；单个智能体的目标状态、障碍物及其他智能体的目标状态之间是否有碰撞。

更进一步的，所述步骤S12将将障碍物和智能体的形状描述为几何图形具体为：将带有阿克曼底盘的智能体用一个矩形表示，将墙壁类障碍物用直线或矩形表示，其他障碍物用外接多边形表示。

进一步的，所述步骤二具体为：

S21、时空混合A*维护一个所有路径节点的优先序列，将路径节点的启发式罚函数作为优先序列的键值；在每次搜索中，选取优先序列中键值最小的路径节点进行扩展，并将无碰撞的子路径节点加入优先序列并进行简并。扩展即根据七种决策从当前时间节点生成下一个时间节点的子路径节点的过程，并为转弯、后退和改变方向附加更高的代价；

S22、当搜索到的路径节点距离目标点较近(较近指落入预设范围，例如两倍转弯半径)时，则使用ReedsShepp曲线将当前路径节点的位姿和目标位姿连接，并进行离散化和碰撞检测；如通过碰撞检测代表已经搜索到单个智能体的无碰可行路径，否则继续进行搜索。

更进一步的，所述七种决策包括停止、直行、前左转、前右转、后退、后左转、后右转。

进一步的，所述步骤三具体为：

S31、维护一个优先队列，队列中的元素为高层次搜索节点，将高层次搜索节点的代价作为优先队列的键值；

S32、在所有智能体完成路径搜索后，沿着时间线在每个时间节点中对所有智能体两两进行碰撞检测；如果未通过碰撞检测，则分别将碰撞的两个智能体作为关键智能体，生成高层次节点，将碰撞信息抽象为约束，创建两个高层次节点加入到优先队列中；

S33、每次从优先队列中取出代价最小的一个节点，使用步骤二的时空混合A*算法重新搜索关键智能体的路径；如果仍然不能通过碰撞检测，则重新按步骤S32创建节点加入到优先队列中，将此次碰撞约束和原节点约束一并加入到新节点中，直到搜索到所有智能体的无碰撞路径为止。

更进一步的，所述步骤S32中，每个高层次搜索节点包含约束信息和一个关键智能体，以及除该关键智能体以外所有智能体的路径；代价为这些路径的代价之和。

进一步的，所述步骤(1)到步骤(3)中，当智能体数量大于一个设定数量时将所有智能体分批次执行；当前批次将之前所有批次的智能体作为动态障碍物，作为影响当前批次的碰撞检测和重搜索。

进一步的，所述步骤四具体为：

S41、将路径的线位移和角位移作为两个通道，分别使用时间的分段五次多项式表示，将位移、速度、加速度在路径节点上连续作为等式约束，将速度、加速度上限值作为不等式约束，利用最小加速度方法构建二次规划问题：

min∑a(t)²＝minP^TQP

其中P为系数矩阵，Q为与时间间隔相关的常量系数矩阵。根据分段五次多项式的系数矩阵，即可计算出任意时刻的前馈线速度v_f和角速度ω_f；

S42、根据在线的定位信息，利用反馈方法计算实时的速度控制率；通过前馈线速度、角速度生成速度的基准值，再根据当前位姿、目标位姿计算速度的修正值，最终得到智能体的速度控制率。

本发明还提供一种基于搜索的多智能体路径规划系统，包括控制单元和存储有控制指令的存储介质，所述控制单元读取所述控制指令时，执行如权利要求上述的一种基于搜索的多智能体路径规划方法。

采用本发明技术方案，本发明的有益效果为：本发明考虑到智能体的非完整性运动学约束，尤其适用于阿卡曼底盘的智能体，对于阿卡曼底盘的机器人有着很好的循迹导引效果，在多智能体协同环境下生成多智能体的无碰路径规划；本方法具有判断清晰、运行快速、碰撞检测精确的优点，且生成的智能体路径连续光滑。

附图说明

图1为一种多智能体无碰撞路径规划方法的流程框图；

图2为阿克曼底盘智能体的决策空间；

图3为高层次碰撞检测和重规划示意图。

具体实施方式

结合附图对本发明具体方案具体实施例作进一步的阐述，使得本技术方案更加清楚、明白。

本实施例涉及一种基于搜索的多智能体路径规划方法，包括以下步骤：

步骤一、初始化环境信息，检测任务可行性；具体是设置每个智能体的初始位姿和目标位姿，设置2D地图，将智能体和障碍物形状描述为几何图形，进行初始位姿和目标位姿的碰撞检测，并确保智能体的初始状态和目标状态在2D地图范围内；

步骤二、在低层次为每个智能体分别搜索一条可行无碰撞路径；具体是基于时空混合A*算法，对每个智能体分别进行低层次的路径规划，搜索得到各自的环境无碰撞路径，但智能体之间可能存在碰撞；

步骤三、在高层次检测智能体之间的碰撞情况，为碰撞的智能体重新搜索路径；具体是在高层次下，对每个离散的时间点检测智能体的碰撞情况；如存在碰撞则为两个智能体分别构建约束，进行重新搜索，直至在所有时间点都通过碰撞检测为止，得到各智能体的无碰撞路径；

步骤四、智能体对各自路径进行跟踪，根据最小加速度的二次规划问题计算前馈加速度，并使用定位信息作为反馈计算实时的速度控制率；通过带前馈和反馈的轨迹算法进行阿克曼底盘的循迹导引，完成跟踪轨迹。

一种具体的实施方式中，所述步骤一按如下过程执行：

(1.1)将智能体、障碍物描述为其在2D平面下的投影，将其表示为位姿(x，y，ψ)和形状的集合，其中形状表示为在笛卡尔坐标系下外接多边形的顶点坐标；给定两个2D平面下的物体坐标和形状，通过坐标变换可以容易判断多边形是否相交，即智能体是否相互碰撞；

(1.2)为智能体设置一个安全系数s，在智能体坐标系下将每个顶点的横纵坐标扩大s倍作为智能体的碰撞检测范围，在离散时间的碰撞检测下保证连续时间下的无碰撞；

(1.3)如果要找到任务(指单个智能体进行无障碍运行的路径)的一个解，必须保证智能体的初始状态之间、智能体的初始状态与障碍物之间、智能体的目标状态之间、目标状态与障碍物不能有碰撞；以及智能体的初始状态和目标状态在地图范围内。

一种具体的实施方式中，所述步骤二按以下过程执行：

假设智能体在t时刻的位姿为(x，y，ψ)，假设智能体的转弯半径为r，搜索步长为l，令θ＝l/r，则由图2所示，根据智能体的七种决策，智能体在(t+1)时刻的位姿空间为：

停止：(x，y，ψ)

直行：(x+lcosψ，y+lsinψ，ψ)

前左转：(x+r[sin(ψ+θ)-sinψ]，y+r[(cosψ-cos(ψ-θ))]，ψ+θ)

前右转：(x+r[sin(θ-ψ)+sinψ]，y+r[(cos(ψ+θ))-cosψ]，ψ-θ)

后退：(x-lcosψ，y-lsinψ，ψ)

后左转：(x+r[sin(ψ-θ)-sinψ]，y+r[(cosψ-cos(ψ+θ))]，ψ-θ)

后右转：(x+r[-sin(θ+ψ)+sinψ]，y+r[(cos(ψ-θ))-cosψ]，ψ+θ)

为转弯和后退附加更高的代价，以减少路径的曲折，使智能体更趋向于直行。例如为每次停止和直行决策附加代价1，为前左转、前右转附加代价w₁＞1；为后退附加代价w₂＞1，为后左转、后右转附加代价w₁w₂。另外，如果相邻的两次决策具有相反的运动方向，则继续附加代价w₃＞1。

对于每个智能体，从低层次的优先序列加入起始位姿开始，如下进行：

(2.1)从优先序列中取出一个总代价最小的节点，其中节点总代价为累积代价和预估代价之和。其中预估代价取非完整无障碍损失和完整有障碍损失较大者，其中非完整无障碍损失为ReedsShepp曲线的长度，完整有障碍损失是预计算好的离散地图各网格到目标点的对角距离。检测该节点是否已经接近目标位姿；通常可以取两倍转弯半径作为阈值，如果距离小于阈值，则进入(2.3)检验后续路径的可行性；

(2.2)扩展取出的节点，生成下一个时间点的候选路径节点。进行碰撞检测，删除有碰幢的候选路径节点；对候选节点与之前节点进行简并，避免接近的节点被重复搜索；更新节点的累积代价和预估代价，将剩余节点加入优先序列中，重新进行步骤(2.1)；

(2.3)用ReedsShepp曲线连接当前节点和目标位姿，并根据智能体的搜索步长离散化曲线生成一系列路径节点。如果ReedsShepp曲线某段长度不是步长的倍数，可以允许非整数时间的路径节点。如果所有路径节点通过检测，则表明已经找到一个智能体的无碰可行路径；否则回到步骤(2.1)重新搜索。

一种具体的实施方式中，所述步骤三如下过程执行：

(3.1)为了保证智能体彼此无碰，沿着时间线在每个时间节点中对所有智能体两两进行碰撞检测；如果路径中有非整数的时间，则按照最近的两个节点进行线性插值得到中间节点。如果路径之间彼此无碰则搜索结束；否则维护一个优先队列，队列中的元素为高层次搜索节点，将高层次搜索节点的代价作为优先队列的键值，将碰撞信息抽象为约束，创建两个高层次搜索节点加入到优先队列中。每个高层次搜索节点包含约束信息和一个关键智能体，以及除该智能体以外所有智能体的路径；其代价为这些路径的代价之和。

(3.2)每次从优先队列中取出代价最小的一个节点，使用时空混合A*重新搜索关键智能体的路径。其中节点中的约束信息将阻止智能体在指定时间通过指定区域，智能体将根据环境选择原地等待其他智能体通过或绕道而行。然后继续进行高层次的碰撞检测，如果仍然不能通过碰撞检测，则继续依照(3.1)的方法创建节点加入到优先队列中，将此次碰撞约束和原节点约束一并加入到新节点中；直到搜索到所有智能体的无碰撞路径为止。

在沿时间线碰撞检测时保存检测结果，在下一次碰撞检测时直接读取结果，有益的结果是避免未碰撞的智能体进行重复的碰撞检测；创建节点时，将碰撞智能体在碰撞时间附近一系列时间都抽象为约束加入到节点中，有益的结果是避免由于相近的碰撞搜索多次，减少搜索次数。

当智能体数量较大时，可以分批次地进行高层次的碰撞检测，以减小碰撞检测的复杂度：

优选的，第一个批次按照步骤(3)，完成正常的碰撞检测和重搜索；

优选的，之后的批次将之前所有批次的智能体作为动态障碍物，加入到本批次的碰撞检测和重搜索中。构造动态障碍物和步骤(3.1)中构建约束的方法相同，阻止本批次的智能体在指定时间通过指定区域。

批次方法的一个有益结果是，加快大量智能体路径规划的搜索速度；但也会带来不利后果，即不能搜索到全局最优的结果；另外如果之前批次的智能体的目标位姿在当前批次智能体路径的必经之路上，且前批次智能体更早到达目标点时，将引起搜索的失败。

一种具体的实施方式中，所述步骤四如下过程执行：

(4.1)离线求解前馈系数矩阵：将路径的线位移和角位移作为两个通道，分别使用时间的分段五次多项式表示，将位移、速度、加速度在路径节点上连续作为等式约束，将速度、加速度上限值作为不等式约束，最小加速度优化构建二次规划问题

min∑a(t)²＝minP^TQP

(4.2)在线求得速度控制率。假设目标位姿为s_des＝(x_des，y_des，ψ_des)，当前位姿s＝(x，y，ψ)，位姿差值Δs＝S_des-s＝(Δx，Δy，Δψ)，令(Δx，Δy)在ψ角方向的投影p＝Δx_cosΔψ+Δy_sinΔψ，朝向差值α＝Δψ，当前坐标与目标位姿坐标连线方向和朝向差值β＝atan2(Δx，Δy)-ψ，(Δx，Δy)的模

则控制律可以描述为

其中k_ρ，k_α，k_β为控制器参数，前馈线速度v_f和角速度ω_f由当前时间t和当前段五次多项式系数矩阵P_i计算得到。

该反馈方法的有益结果是：前馈环规划出智能体的平滑的速度和角速度，符合物理空间的可执行特性；对比纯粹的反馈环大大减小跟踪误差；修正线速度与位置差在朝向上的投影长度成正比，避免横向的位置差增加导致修正速度增大，引起系统与目标点的偏离，增加跟踪系统的稳定性；修正角速度当前坐标与目标位姿坐标连线方向、目标朝向与当前朝向有关；与修正角速度第二项附加一个距离值作为权值，使得在距离目标点较远时趋向于朝向目标点，距离较近时趋向于朝向目标，在当前位姿和目标点接近时，避免β突变引起转向的不稳定。该速度控制律可以使系统快速地跟随目标位姿，具有简单稳定的特点。

本优选实施例所用算法参数如下表所示：

参数名	参数符号	推荐值
			转弯惩罚项	w<sub>1</sub>	1.5
后退惩罚项	w<sub>2</sub>	2.0
			换向惩罚项	w<sub>3</sub>	2.0
安全系数	s	1.2
			控制器参数	k<sub>ρ</sub>	0.5
控制器参数	k<sub>α</sub>	0.25
			控制器参数	k<sub>β</sub>	0.3

本实施例还提供一种基于搜索的多智能体路径规划系统，包括控制单元和存储有控制指令的存储介质，所述控制单元读取所述控制指令时，执行上述的一种基于搜索的多智能体路径规划方法。控制单元可以采用计算机等设备。

注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解，本发明不限于这里所述的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，而本发明的范围由所附的权利要求范围决定。

Claims (10)

1.一种基于搜索的多智能体路径规划方法，其特征在于，包括以下步骤：

步骤一、设置每个智能体的初始位姿和目标位姿，设置2D地图，将智能体和障碍物形状描述为几何图形，进行初始位姿和目标位姿的碰撞检测，并确保智能体的初始状态和目标状态在2D地图范围内；

步骤二、基于时空混合A*算法，对每个智能体分别进行低层次的路径规划；时空混合A*在混合A*基础上，在搜索中加入时间属性，并在碰撞检测时考虑时间因素；每个智能体搜索到的路径的环境无碰撞，但智能体之间可能有碰撞；

步骤三、在高层次下，对每个离散的时间点检测智能体的碰撞情况；如存在碰撞则为两个智能体分别构建约束，进行重新搜索，直至在所有时间点都通过碰撞检测为止，得到各智能体的无碰撞路径；

步骤四、通过带前馈和反馈的轨迹算法进行阿克曼底盘的循迹导引，完成路径的跟踪。

2.根据权利要求1所述的一种基于搜索的多智能体路径规划方法，其特征在于，所述步骤一具体为：

S11、将路径规划问题抽象为一个2D平面，建立笛卡尔坐标系，将各个智能体的初始位姿和目标位姿表示为横坐标、纵坐标和朝向角的三元组(x,y,ψ)；

S12、将障碍物和智能体的形状描述为几何图形；

S13、对任务的可行性进行检验，单个智能体的初始状态、障碍物及其他智能体的初始状态之间是否有碰撞；单个智能体的目标状态、障碍物及其他智能体的目标状态之间是否有碰撞。

3.根据权利要求2所述的一种基于搜索的多智能体路径规划方法，其特征在于，所述步骤S12将将障碍物和智能体的形状描述为几何图形具体为：将带有阿克曼底盘的智能体用一个矩形表示，将墙壁类障碍物用直线或矩形表示，其他障碍物用外接多边形表示。

4.根据权利要求1所述的一种基于搜索的多智能体路径规划方法，其特征在于，所述步骤二具体为：

S21、时空混合A*维护一个所有路径节点的优先序列，将路径节点的启发式罚函数作为优先序列的键值；在每次搜索中，选取优先序列中键值最小的路径节点进行扩展，并将无碰撞的子路径节点加入优先序列并进行简并。扩展即根据七种决策从当前时间节点生成下一个时间节点的子路径节点的过程，并为转弯、后退和改变方向附加更高的代价；

S22、当搜索到的路径节点距离目标点较近时，则使用ReedsShepp曲线将当前路径节点的位姿和目标位姿连接，并进行离散化和碰撞检测；如通过碰撞检测代表已经搜索到单个智能体的无碰可行路径，否则继续进行搜索。

5.根据权利要求4所述的一种基于搜索的多智能体路径规划方法，其特征在于，所述七种决策包括停止、直行、前左转、前右转、后退、后左转、后右转。

6.根据权利要求1所述的一种基于搜索的多智能体路径规划方法，其特征在于，所述步骤三具体为：

S31、维护一个优先队列，队列中的元素为高层次搜索节点，将节点的代价作为优先队列的键值；

S32、在所有智能体完成路径搜索后，沿着时间线在每个时间节点中对所有智能体两两进行碰撞检测；如果未通过碰撞检测，则分别将碰撞的两个智能体作为关键智能体，生成高层次节点，将碰撞信息抽象为约束，创建两个高层次节点加入到优先队列中；

S33、每次从优先队列中取出代价最小的一个节点，使用步骤二的时空混合A*算法重新搜索关键智能体的路径；如果仍然不能通过碰撞检测，则重新按步骤S32创建节点加入到优先队列中，将此次碰撞约束和原节点约束一并加入到新节点中，直到搜索到所有智能体的无碰撞路径为止。

7.根据权利要求6所述的一种基于搜索的多智能体路径规划方法，其特征在于，所述步骤S32中，每个高层次搜索节点包含约束信息和一个关键智能体，以及除该关键智能体以外所有智能体的路径；代价为这些路径的代价之和。

8.根据权利要求1所述的一种基于搜索的多智能体路径规划方法，其特征在于，所述步骤(1)到步骤(3)中，当智能体数量大于一个设定数量时将所有智能体分批次执行；当前批次将之前所有批次的智能体作为动态障碍物，作为影响当前批次的碰撞检测和重搜索。

9.根据权利要求1所述的一种基于搜索的多智能体路径规划方法，其特征在于，所述步骤四具体为：

S41、将路径的线位移和角位移作为两个通道，分别使用时间的分段五次多项式表示，将位移、速度、加速度在路径节点上连续作为等式约束，将速度、加速度上限值作为不等式约束，利用最小加速度方法构建二次规划问题：

min∑a(t)²＝minP^TQP

其中P为系数矩阵，Q为与时间间隔相关的常量系数矩阵。

根据分段五次多项式的系数矩阵，可计算出前馈线速度v_f和角速度ω_f；

S42、根据在线的定位信息，利用反馈方法计算实时的速度控制率；通过前馈线速度、角速度生成速度的基准值，再根据当前位姿、目标位姿计算速度的修正值。

10.一种基于搜索的多智能体路径规划系统，其特征在于，包括控制单元和存储有控制指令的存储介质，所述控制单元读取所述控制指令时，执行如权利要求1-8任一所述的一种基于搜索的多智能体路径规划方法。

Images