基于改进蚁群优化算法的自动驾驶多车协同运动规划

宋佳艳，苏圣超

（上海工程技术大学电子电气工程学院，上海 201620）

0 概述

智能交通系统（Intelligent Transportation System，ITS）是利用数据通信传输、电子传感等技术对传统运输系统进行改进而形成的一种信息化、智能化、社会化的新型运输系统，对于缓解当前大中城市的交通压力起到了关键作用。自动驾驶技术的不断发展，能够有效减少交通事故，提高通行效率［1］。多车协同规划是自动驾驶中的重要部分，在很大程度上决定了整个交通系统的智能程度［2］。协同规划不同于单车运动轨迹的叠加［3］，主要是要求多车在行驶过程中考虑与其他车辆及环境之间的信息，通过规避障碍物且与其他车辆保持安全距离，最终到达各自的目的地［4-5］。自动驾驶的协同规划能够在提高安全性的同时，减少交通拥堵［6］。由于计算复杂度随着车辆数量增加，且需要确保车辆间的协同，因此如何在复杂交通环境中规划多车实时运动轨迹是亟需解决的问题［7-8］。

目前，国内外学者已对自动驾驶车辆协同运动规划展开研究。CHEN 等［9］根据优先级顺序对多车辆进行规划，将高优先级车辆视为低优先级车辆的时变障碍物，计算复杂度随着车辆数量线性缩放，由于该方法中的优先级算法一次只规划一辆车，因此当面对高度冲突的大规模规划情况时，整个系统的协同能力不足。LI等［10］提出基于顺序计算框架的增量约束动态优化方法，通过引入更多的避碰约束使每一个子问题比前一个子问题更难，将每一个子问题的最优解作为下一个子问题的初始解，直到得到最终解。由于该方法将原问题分散为多个部分逐步解决，因此无法确保在实际情况下能够进行在线规划。TAHIROVIC 等［11］提出一种基于双层区域覆盖的多车运动规划算法，上层为每辆车粗略规划行驶轨迹，下层对多车之间进行协调，为每辆车生成无碰撞轨迹。该算法由于协调过程不同时执行，因此导致了解决方案质量较差。HUANG 等［12］针对协同驾驶问题，提出一种基于人工势场（Artificial Potential Field，APF）和模型预测的协同驾驶方法，设置成本函数，从多目标多约束优化角度出发，很好地解决了自动驾驶运动规划问题，但该方法未考虑轨迹的实时优化。由上述研究可知，目前协同规划能有效保证运行车辆与障碍物及其他车辆之间避免发生碰撞并保持安全距离，但车辆之间的协同能力以及在线规划能力仍需进一步提升。

本文提出一种基于改进蚁群优化（Ant Colony Optimization，ACO）算法的自动驾驶多车协同运动规划方法。将蚁群优化算法应用于自动驾驶车辆运动规划中，并将多目标优化函数引入蚁群优化算法的信息素更新过程中，通过空间协同和轨迹优化的目标函数对信息素进行实时更新，同时对蚁群优化算法中的挥发因子进行自适应调整，以实现多车自动驾驶的在线协同运动规划。

1 自动驾驶车辆运动学模型

为保证自动驾驶车辆实际行驶轨迹平滑，在进行自动驾驶运动规划时需要满足车辆运动学约束条件，对车辆运动学模型进行分析。自动驾驶车辆运动学模型如图1 所示。

图1 自动驾驶车辆运动学模型Fig.1 Kinematics model of autonomous driving vehicle

自动驾驶车辆运动学约束方程［13］如式（1）所示：

其中：(x，y)表示自动驾驶车辆后轮中点位置；φ表示转角速度；θ表示前进方向；v表示车辆运行速度；L表示车辆前后轮轴距；u1、u2分别表示自动驾驶车辆加速度与前轮偏转角速度。

2 改进的蚁群优化算法

蚁群优化算法［14］是指自然界中蚂蚁可以在没有任何提示的情况下找到一条最优路径，这主要是由于蚂蚁在觅食路线上会分泌信息素［15］，随着迭代次数的增多，蚂蚁会在较短的路径上积累更多的信息素，这种信息素会在其他蚂蚁面临觅食路线选择时发出强烈信号进行准确的路线引导，从而大幅提高后续蚂蚁选择这条路线的概率，随着该过程不断重复，该算法构成一个正反馈回路［16-17］。蚁群优化算法主要包括概率选择和信息素更新2 个步骤。

2.1 概率选择

蚂蚁通过轮盘赌选择规则［18］进行下一个方向的选择，蚂蚁k从g走向下一个位置h的概率计算如下：

其中：Tgh为路径(g，h)上的信息素量；allowedk为禁忌表之外蚂蚁可以走的点；α为信息素因子；β为启发因子，表示下一个节点对当前节点的影响程度。

ηgh为启发函数［19］，计算公式如下：

其中：dgh为下一个可行节点到目标点的欧式距离。

两个节点之间的欧氏距离定义如下：

其中：Tl=(Xl，Yl)表示目标点的坐标；Rh=(whx，why)表示下一个可行节点的坐标。

2.2 信息素更新

在所有蚂蚁完成一次迭代后，路径上的信息素进行全局更新，信息素更新公式如下［20］：

其中：ρ为信息素挥发因子，ρ∈(0，1)，(1-ρ)为信息素残留因子；τgh(t)为当前时刻信息素浓度；Δτgh为信息素增量；为当前迭代周期蚂蚁k的信息素增量；Q为常数，表示信息素强度；Lk表示循环结束时蚂蚁k所走的路径长度；tr表示蚂蚁r已完成的任务集合。

将每一次迭代产生的最优解与历史最优解进行对比，产生新的最优解，在迭代次数达到所设定值时，输出最优路线［21］。

3 基于改进ACO 的自动驾驶多车协同规划

采用蚁群优化算法进行自动驾驶车辆的运动规划，根据自动驾驶车辆数量产生多个蚂蚁种群，各种群相互独立并规划相应的路线，在规划过程中通过多目标优化模型确保多辆汽车的协同性，对轨迹代价和空间协同约束进行数学建模，设立目标优化函数，并将目标函数应用于信息素更新规则，确保多辆自动驾驶汽车的协同安全性与轨迹平滑性。

3.1 多目标优化模型设计

3.1.1 空间协同代价与轨迹代价模型

车辆在进行运动规划时，为保证安全性，首先应考虑车辆与障碍物之间无碰撞。假设整个协同环境中存在Nn辆车和Nobs个障碍物，设置车辆与障碍物之间的安全距离dsafe1。理想的最优轨迹应适当与障碍物保持一定的距离，当车辆与障碍物之间的距离大于安全距离时，车辆与障碍物互不影响；当车辆与障碍物之间的距离小于安全距离时，应保持距离尽可能远。车辆与障碍物空间协同函数的计算公式如下：

其中：dik表示车辆i与障碍物k之间的距离；κ1为距离系数，表示期望车辆i远离障碍物k的程度。

车辆与障碍物之间的空间协同代价模型建立如下：

同样地，在考虑车辆与障碍物之间无碰撞的前提下，多车协同运动规划要求各车辆在行驶过程中应尽量保持距离。车辆与障碍物之间的安全距离为dsafe2，车辆间空间协同函数的计算公式如下：

其 中：dij为车辆i与车辆j之间的距离；κ2为距离系数，表示期望车辆i与车辆j之间互相远离的程度。

车辆间的空间协同代价模型建立如下：

轨迹平滑性与前轮偏转角的变化幅度有关，通过设置轨迹平滑程度的代价模型，表示车辆i在运动时域内因车轮转角变化而消耗的能量。轨迹代价函数的计算公式如下：

其中：φi(t)表示t时刻车辆i的前轮偏转角；tf表示车辆到达终点的时刻。

3.1.2 目标函数优化

基于上文描述，构建自动驾驶协同控制的轨迹优化数学模型，将空间协同代价模型与轨迹代价模型进行加权汇总，如式（13）所示，建立以空间协同代价模型与轨迹代价模型为目标函数的数学模型。

其中：f为综 合代价；vi表 示车辆i的运行速度；vmax表示安全情况下车辆的最大运行速度；Φmax表示最大转角速度；q1、q2、q3为各性能指标对应的权重系数，权重越大表示该部分在代价函数中越受重视，q1、q2、q3满足非负性和归一性

3.2 信息素更新规则改进

蚁群优化算法作为一种启发式智能算法，通过模拟自然界中蚂蚁寻找食物的过程来寻找问题的最优解［22］。蚂蚁在觅食过程中会在经过的道路上释放信息素，后续经过的蚂蚁会根据所留下的信息素浓度对下一个位置进行选择，因此信息素的分布对全局的规划起着至关重要的作用［23］。采用蚁群优化算法进行协同规划，信息素对各车辆起着引导作用，决定汽车的运动方向与路线，直接影响车辆间的协同能力，因此本文对信息素进行改进。

上文通过构造多目标优化函数，考虑了空间协同代价与轨迹代价，能够确保多车自动驾驶的自适应协同能力与安全性，同时又保证了乘车人的舒适度。因此，考虑将目标函数加入蚁群优化算法的信息素更新规则中，改进的信息素更新规则如式（14）所示：

将多目标优化所涉及空间协同代价和轨迹代价模型引入信息素更新过程中，通过多目标优化函数所得的实时数据对信息素进行相应的惩罚和奖励：若加入目标函数后的蚁群优化算法出现更优轨迹，则表示其对信息素具有正反馈，因此对信息素进行奖励，增加信息素的导向性，即可加快算法收敛速度；若加入目标函数后得到轨迹较差，则表示对信息素具有负反馈，在信息素更新时进行惩罚，弱化最优轨迹上所积累的信息素，给予次优或较优轨迹更大的被选择概率，提高轨迹选择的多样性，避免陷入局部最优。

3.3 挥发因子调整

蚁群优化算法中固定的挥发因子ρ实际上是按一种固定不变的模式确定每次的路径选择概率以及更新的信息量，不能很好地保证自动驾驶车辆方向选择的灵敏度以及轨迹的最优。面对复杂多变的环境，需要提高各种群对路径的全局搜索能力，提升算法实时性。

在蚁群优化算法中挥发因子ρ始终保持不变，表示信息素的蒸发程度，反映了蚂蚁个体之间的互相影响程度。若ρ设置不合理，蚂蚁可能会丧失全局搜索能力，也会影响收敛速度。当ρ过小时，信息素挥发慢，若蚂蚁一开始找到的是次优路径而非最优路径，受信息素挥发度的影响，次优路径的信息素逐渐积累，很难再找到最优路径，易陷入局部最优解，影响算法的随机性能；当ρ过大时，信息素挥发快，过多的无用搜索不利于蚂蚁选择下一个方向的路径，搜索能力降低，收敛速度减慢。为提高自动驾驶车辆在实际运动规划中的全局搜索能力以及收敛速度，对挥发因子的自适应更新调整如下：

其中：ρ∈(0，1)；s为当前迭代次数，表示s服从参数σ和μ的高斯函数，σ和μ为常数。

构造的新的信息素挥发因子遵循高斯分布，可以提高蚂蚁搜索的目的性以及搜索速度。高斯函数分布曲线如图2 所示，当搜索开始时，挥发因子较小，路径上信息素浓度对蚂蚁影响较大，使得蚁群向信息素多的路径靠拢。随着信息素不断更新，为避免陷入局部最优，挥发因子取得较大值，蚂蚁能够自由搜索更多路径。在搜索后期，为加快收敛速度，挥发因子取得一个较小值。经过多次的迭代，蚂蚁快速地集中于信息素浓度较高的路径。

图2 高斯函数分布曲线Fig.2 Gaussian function distribution curve

4 实验与结果分析

为验证本文方法的有效性，采用Matlab2019a 软件进行仿真实验，基于非结构化道路以及结构化道路场景进行运动规划，非结构化道路是指边缘比较规则，没有明显车道线的行车道路，例如园区、学校等场所内的道路，结构化道路是指具有清晰道路标志线的行车道路，例如高速公路、城市干道等道路。实验相关参数设置如表1 所示。

表1 实验相关参数设置Table 1 Setting of experiment correlation parameters

4.1 非结构化道路场景下的实验结果与分析

在非结构化道路场景下，为更好地描述动态障碍物在平面地图中的运动，将时间轴视为运动状态，使运动状态沿着各个方向维度离散化，从而将整个场景的变化统一记录在X-Y-T三维状态空间中。在规格为20 m×20 m 的地图中，静态障碍物所在的位置不随时间变化而移动，动态障碍物随着时间序列进行移动。设置自动驾驶车辆数量为Nn，障碍物数量为Nobs，为验证本文方法的有效性，在两种不同道路场景（Nn=2 和Nobs=3、Nn=4 和Nobs=4）下进行仿真实验。

设置自动驾驶车辆的起始点为A1，A2，…，An，终点为B1，B2，…，Bn。使用改进的蚁群优化算法进行自动驾驶运动规划，在两种场景下所规划的轨迹分别如图3、图4 所示。由图3、图4 可知，基于改进蚁群优化算法的自动驾驶协同运动规划能够有效规避障碍物，并为各自动驾驶车辆规划出无碰撞的平滑路线。各种群所代表的自动驾驶车辆在迭代过程中的综合代价变化曲线如图5、图6 所示。由图5、图6 可知，随着迭代次数的增加，综合代价快速下降，表明改进的蚁群优化算法在迭代初期能够快速收敛，在经过一定的迭代次数后，综合代价逐渐趋于稳定，最终收敛于特定值。

图3 Nn=2 和Nobs=3 条件下协同运动规划结果Fig.3 Results of collaborative motion planning under the conditions of Nn=2 and Nobs=3

图4 Nn=4 和Nobs=4 条件下协同运动规划结果Fig.4 Results of collaborative motion planning under the conditions of Nn=4 and Nobs=4

图5 Nn=2 和Nobs=3 条件下综合代价变化曲线Fig.5 Change curves of comprehensive cost under the condition of Nn=2 and Nobs=3

图6 Nn=4 和Nobs=4 条件下综合代价变化曲线Fig.6 Change curves of comprehensive cost under the condition of Nn=4 and Nobs=4

4.2 结构化道路场景下的实验结果与分析

构建简单结构化道路场景进行仿真实验，如图7（a）～图7（g）所示，3 辆自动驾驶车辆行驶在一条双车道的直线道路上，设置初始速度v1=v2=v3=20 m/s，各自动驾驶车辆速度可变并根据环境做出适当的控制，以响应动态环境的变化。采用本文方法进行自动驾驶协同运动规划，仿真结果如图7（h）所示。在图7 中，实线、虚线以及点划线分别表示车辆1、车辆2、车辆3 的规划路线，所生成的轨迹平滑，且各车辆之间保持协同控制并满足约束条件。

图7 结构化道路场景下的协同运动规划结果Fig.7 Result of collaborative motion planning under the scene of structured road

在模拟过程中，当T=0 s 时，各自动驾驶车辆分别位于0 m、40 m 以及80 m 处；当T=4 s 时，车辆2 继续保持v2=20 m/s 的速度向前行驶，此时车辆1 由于速度的增加，距离车辆2 越来越近，为避免发生事故，车辆1 采取变道措施；当T=8 s 时，车辆1 完成变道，避免了与车辆2 发生碰撞；当T=16 s 时，由于车辆3 以低于车辆1 的速度前进，阻碍了车辆1 的行驶，车辆1 在保持安全的前提下继续变道；当T=20 s时，由于车辆1 的变道，车辆2 若继续保持匀速行驶，距离车辆1 过近，存在安全隐患，为确保行车安全，采取减速措施；当T=24 s 时，车辆1 完成变道。

结构化道路场景中各自动驾驶车辆的综合代价曲线如图8 所示，在迭代初期，代价曲线的值快速下降，表明经过迭代次数的增加，各种群已能规划出较优轨迹。在后续迭代过程中，综合代价逐渐达到稳定值。在仿真过程中，各车辆速度及加速度的变化如图9（a）和图9（b）所示，各自动驾驶车辆的速度根据动态环境做出自适应调整。

图8 结构化道路场景下的综合代价变化曲线Fig.8 Change curves of comprehensive cost under the scene of structured road

图9 车辆参数变化Fig.9 Changes of vehicle parameters

4.3 性能对比

为验证本文方法性能，将其与APF 方法［12］进行对比。在相同参数设置条件下，通过代价函数对比协同收敛性能，分别记录第i次迭代得到的代价函数均值Ji与初次迭代得到的代价函数均值Jo，并用Ji/Jo表示算法收敛速度。两种方法在不同车辆数量情况下的收敛曲线如图10 和图11 所示，可以看出本文方法收敛速度明显优于APF 方法，且所得解的适应性更强，在复杂道路场景下，随着自动驾驶车辆数量增多，代价函数均值比减小，且收敛于更小的值，表明车辆间协同效果更好。

图10 Nn=3 时2 种方法的收敛曲线Fig.10 Convergence curves for two methods when Nn=3

图11 Nn=6 时2 种方法的收敛曲线Fig.11 Convergence curves for two methods when Nn=6

5 结束语

本文针对复杂道路场景下的自动驾驶多车运动规划问题，提出一种基于改进蚁群优化算法的协同控制方法。在蚁群优化算法的信息素更新过程中，引入以空间协同和轨迹代价为目标函数的多目标优化模型，使各自动驾驶车辆保持协同控制，同时对蚁群优化算法的挥发因子进行自适应调整，加快算法收敛速度。实验结果表明，该方法为多辆自动驾驶汽车的协同运动规划提供了一种有效的解决方案。后续将设计协同控制器，对多辆自动驾驶汽车的规划路线进行跟踪控制，并将本文方法应用于实际自动驾驶系统，进一步提升其稳定性。