CN113985896A - 一种自动驾驶车辆避障路径规划方法、车辆及可读存储介质 - Google Patents

Abstract

本发明提供一种自动驾驶车辆避障路径规划方法、车辆及可读存储介质，方法涉及自动驾驶车辆路径规划领域，方法在提高算法收敛速度和改善规划路径质量方面效果显著。首先，利用车载感知系统获取的环境数据建立二维栅格化地图模型；其次，利用本发明构造的全局启发函数优化蚁群算法状态转移概率，在基于车速和贝塞耳光滑曲线的约束条件下，引导一定规模的蚂蚁依次向目标点移动；再次，利用本发明提出的一种基于动态调整的信息素更新策略，引导优化过程快速向全局可行最优解收敛；最后，达到规定的迭代次数，迭代终止，生成输出全局最优的自动驾驶车辆避障规划路径。

Description

一种自动驾驶车辆避障路径规划方法、车辆及可读存储介质

技术领域

本发明涉及自动驾驶车辆路径规划领域，尤其涉及一种自动驾驶车辆避障路径规划方法、车辆及可读存储介质。

背景技术

路径规划是自动驾驶车辆运动控制的关键技术，通常被描述为自动驾驶车辆在空间环境约束下寻找一条从起始点通往目标点的最优无碰撞路径。自动驾驶车辆又称为轮式移动机器人，其路径规划算法的研究源自机器人技术。现有技术中公开了诸多路径规划算法，比如蚁群算法、Dijastra算法、A*算法、粒子群算法、快速探索随机树算法(RRT)、人工势场法、禁忌搜索算法、神经网络法、动态窗口算法等。但大多数算法针对具体问题需要进行优化整合，自动驾驶车辆安全行驶要求延时必须控制在毫秒甚至微秒级别，对算法效率、规划路径转弯的平滑性提出了较高的要求。

而蚁群算法是由意大利学者M.Dorigo等人于1996年首先提出，它源于对蚂蚁群体觅食机制的研究。蚁群算法具有自组织、分布式、正反馈、鲁棒性强等优点，易于与其他优化算法相结合，已经广泛用于求解车辆路径规划、机器人路径规划、无人机飞行路径规划等问题。但是，蚁群算法理论模型缺乏科学的数学基础，关键参数的选择多依靠试验和经验来确定，易出现收敛速度慢、路径拐点多转弯角度大、局部收敛或搜索停滞等典型问题。

发明内容

为解决基本蚁群算法在路径规划中存在的收敛速度慢、路径弯角度大等技术问题，本发明提供了一种改进蚁群算法的自动驾驶车辆避障路径规划方法。

方法包括：

步骤1：利用车载感知系统获取环境数据，并构建基于自动驾驶车辆二维栅格化的行驶道路地图模型；

在行驶道路地图模型中定位起始点S和目标点E；获取空间所有节点信息，计算邻接矩阵D和计算启发式信息矩阵；

步骤2：初始化迭代次数N，蚂蚁规模M，信息启发式因子α，期望启发式因子β，信息素挥发系数ρ，ε、t,信息素浓度，当前路径列表RT以及禁忌表TS；将起始点S分别置于禁忌表RT和当前路径列表TS中；

步骤3：查询邻接矩阵D，获取当前节点i出发下一步可行节点的集合

然后利用全局启发函数η_ij(t)优化蚁群算法状态转移概率

计算第m(m＝1,2,3,…,M)只蚂蚁选择相邻节点的概率；

按轮盘法选择下一节点，将选定的节点作为新的当前节点；

更新当前路径列表RT和禁忌表TS；

步骤4：更新蚂蚁序号，若第m只蚂蚁的当前路径列表包含了目标点或无路径且m≥M，则转入步骤5，否则返回步骤3；

步骤5：更新信息素，计算当前迭代最优路径，并利用动态调整的信息素增量模型Δτ_ij更新信息素矩阵；

步骤6：若n≥N，则输出最优路径并停止迭代，否则返回步骤3。

进一步需要说明的是，步骤3中的蚁群算法状态转移概率

表达式为：

式中：

为t时刻第m只蚂蚁从当前位置节点i到相邻位置节点j的状态转移概率；

τ_ij(t)为路径(i,j)上的信息素浓度；

η_ij(t)为蚂蚁m在节点i处选择相邻节点j的启发函数，反映边(i,j)的能见度；

α为信息启发式因子，反映信息素对蚂蚁选择路径的影响力；

β为期望启发式因子，反应启发式信息在指导蚁群搜索过程中的相对重要程度；

U^m为蚂蚁尚未访问的下一节点的集合；

s为与当前位置节点i相邻的可选节点的集合；

τ_is(t)为当前位置节点i与各相邻节点之间的信息素浓度；

η_s(t)表示蚂蚁k在节点i与各相邻节点之间的启发函数。

进一步需要说明的是，步骤3中的全局启发函数η_ij(t)表达式为：

式中：d_jE为j到目标节点E之间的距离，即

x_E为目标节点的横坐标，y_E为目标节点的纵坐标；t为相邻节点j对影响路径选择的权重；1-t为目标节点E对影响路径选择的权重。

进一步需要说明的是，t的大小取决于车速、相邻节点之间的距离；

当车辆均速行驶时，t≈d_ij/(d_ij+d_jE)；

当d_jE远大于d_ij时，t→0，则全局启发函数η_ij(t)表达式可近似为：

全局启发函数η_ij(t)的理论基础为贝塞耳曲线函数；

贝塞耳曲线函数构造的全局启发函表达式为：

n+1个顶点的n次贝塞耳曲线表达式为：

式中：P_i(i＝0,1,2,…,n)为各顶点的位置向量，B_i,n(t)为伯恩斯坦基函数，其表达式为：

进一步需要说明的是，步骤5中动态调整的信息素增量模型Δτ_ij表达式为：

式中：n为第n次迭代；

L_n,m为当前路径距离，即第m只蚂蚁产生的路径的距离；

L_min为最优路径距离，即第n次迭代产生的最短路径距离；

L_max为最差路径距离，即第n次迭代产生的最长路径距离；

L_idv为理想路径距离，即起始点与目标点的直线距离；

δ为最优路径距离与最差路径的差值，即δ＝L_max-L_n,m；

ε为第n次迭代可接受路径误差，ε值为一常数。

进一步需要说明的是，在迭代过程中，新的信息素增量机制自适应动态调整信息素的强度，使优化过程加速向全局最优路径收敛；

当δ＞ε时，L_max与L_n,m差值越大，信息素强度越大，全局收敛时间缩短，算法求解效率得到提升；

当δ≤ε时，L_n,m与L_min越接近，信息素浓度蒸发越快；

L_idv在迭代过程中起到参照作用，作为最佳路径决策依据。

本发明还提供一种实现自动驾驶车辆避障路径规划方法的车辆，包括：存储器，用于存储计算机程序及自动驾驶车辆避障路径规划方法；处理器，用于执行所述计算机程序及自动驾驶车辆避障路径规划方法，以实现自动驾驶车辆避障路径规划方法的步骤。

本发明还提供一种具有自动驾驶车辆避障路径规划方法的可读存储介质，可读存储介质上存储有计算机程序，计算机程序被处理器执行以实现自动驾驶车辆避障路径规划方法的步骤。

从以上技术方案可以看出，本发明具有以下优点：

本发明提供的自动驾驶车辆避障路径规划方法是针对基本蚁群算法全局启发性不足、路径转弯角大的问题，构造了全局启发函数，该启发函数在改善优化过程局部收敛和规划路径质量方面效果显著。

本发明提供的自动驾驶车辆避障路径规划方法基于动态调整的信息素更新策略，充分利用迭代路径信息形成了信息素强度的自调节机制，使算法优化过程自适应调整信息素的强度，全局收敛速度得到明显提升。

附图说明

为了更清楚地说明本发明的技术方案，下面将对描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1自动驾驶车辆避障路径规划方法流程图；

图2二维栅格模型图；

图3移动决策图；

图4本发明的规划的路径结果图；

图5本发明的规划收敛曲线图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明提供的自动驾驶车辆避障路径规划方法中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

自动驾驶车辆避障路径规划方法的附图中所示的方框图仅仅是功能实体，不一定必须与物理上独立的实体相对应。即，可以采用软件形式来实现这些功能实体，或在一个或多个硬件模块或集成电路中实现这些功能实体，或在不同网络和/或处理器装置和/或微控制器装置中实现这些功能实体。

在本发明提供的自动驾驶车辆避障路径规划方法中，应该理解到，所揭露的系统、装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另外，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口、装置或单元的间接耦合或通信连接，也可以是电的，机械的或其它的形式连接。

本发明公开提供的一种改进蚁群算法的自动驾驶车辆避障路径规划方法的流程图如图1所示，具体实施步骤如下：

步骤1：利用车载感知系统获取的环境数据构建自动驾驶车辆二维栅格化行驶道路地图模型。仿真环境如图2所示的20*20二维栅格地图。图2中黑色栅格表示障碍物，白色栅格是自由栅格。设每个栅格的中心坐标为栅格的直角坐标，每个栅格编号与直角坐标一一对应，则栅格地图中的任意一点的坐标与栅格编号i的映射关系如下所示：

xi＝a·(mod(i,MM)-0.5) (7)

yi＝a·(MM+0.5-ceil(i/MM)) (8)

式中：a为每个栅格边长；MM为横坐标的最大栅格数值；mod(a,b)为(a/b)取余结果；ceil函数为朝正无穷大方向取整。

在栅格地图中，在当前节点i位置路径决策可选方向有“十”字型或“米”字型。根据自动驾驶车辆特点，本文采用“米”字型方向选择规则，如图3所示。

步骤2：首先定位自动驾驶车辆移动起始点S和目标点E，计算邻接矩阵D；然后初始化迭代次数N和蚂蚁规模M，信息素权值系数α，启发因子系数β，信息素蒸发系数ρ；最后将起始点将起始点S置于禁忌表RT和当前路径列表TS。

步骤3：对路径进行选择，具体的讲，首先查询邻接矩阵D，获取当前节点i出发下一步可行节点的集合U^m；然后利用构造的全局启发函数η_ij(t)优化蚁群算法状态转移概率

在基于车速和贝塞耳光滑曲线的约束条件下，引导蚂蚁按轮盘法选择下一节点。

步骤3中所述优化的状态转移概率函数表达式为：

式中：

为t时刻第m只蚂蚁从当前位置节点i到相邻位置节点j的状态转移概率；τ_ij(t)为路径(i,j)上的信息素浓度；η_ij(t)为蚂蚁m在节点i处选择相邻节点j的启发函数，反映边(i,j)的能见度；U^m为蚂蚁尚未访问的下一节点的集合；s为与当前位置节点i相邻的可选节点的集合；τ_is(t)为当前位置节点i与各相邻节点之间的信息素浓度；η_is(t)表示蚂蚁k在节点i与各相邻节点之间的启发函数。

进一步的讲，步骤3所述构造的全局启发函数与利用贝塞耳曲线平滑处理方法：

n+1个顶点的n次贝塞耳曲线表达式为：

式中：P_i(i＝0,1,2,…,n)为各顶点的位置向量，B_i,n(t)为伯恩斯坦基函数。

令η_ij(t)＝P(t),P₀＝1/d_ij和P₁＝1/d_jE，按贝塞耳曲线函数一阶展开，即取n＝1，则

全局启发函数η_ij(t)表达式为：

式中：d_jE为j到目标节点E之间的距离，即

x_E为目标节点的横坐标，y_E为目标节点的纵坐标；t为相邻节点j对影响路径选择的权重；1-t为目标节点E对影响路径选择的权重。

t的大小取决于车速、相邻节点之间的距离。当车辆均速行驶时，t≈d_ij/(d_ij+d_jE)。当d_jE远大于d_ij时，t→0，则全局启发函数η_ij(t)表达式可近似为：

全局启发函数η_ij(t)利用相邻节点和目标点的信息，在车速和贝塞耳光滑曲线的约束条件下，增强了蚂蚁对目标节点的感知，解决了基本蚁群算法全局启发性不足、盲目搜索和路径转弯角度大的问题。

步骤4：更新蚂蚁编号。首先将选定的下一节点作为新的当前节点，更新当前路径列表RT和禁忌表TS；然后判断若第m只蚂蚁的当前路径列表包含了目标点或无路径且m≥M，则转入步骤5，否则，m＝m+1，返回步骤3。

进一步的讲，动态调整的信息素增量模型Δτ_ij表达式为：

式中：n为第n次迭代；L_n,m为当前路径距离，即第m只蚂蚁产生的路径的距离；L_min为最优路径距离，即第n次迭代产生的最短路径距离；L_max为最差路径距离，即第n次迭代产生的最长路径距离；L_idv为理想路径距离，即起始点与目标点的直线距离；δ为最优路径距离与最差路径的差值，即δ＝L_max-L_n,m；ε为第n次迭代可接受路径误差，ε值为一常数。

在迭代过程中，新的信息素增量机制可以自适应动态调整信息素的强度，使优化过程加速向全局最优路径收敛。当δ＞ε时，L_max与L_n,m差值越大，信息素强度越大，全局收敛时间缩短，算法求解效率得到提升；当δ≤ε时，L_n,m与L_min越接近，信息素浓度蒸发越快，使算法避免出现过早收敛陷入局部最优。L_idv在迭代过程中起到参照作用，可作为最佳路径决策依据。

步骤5：更新信息素。首先计算当前迭代路径，然后按照本发明一种基于动态调整的信息素更新模型更新信息素函数τ_ij(t)。

步骤6：若n≥N，则输出最优路径并停止迭代，否则返回步骤3。

本发明中，步骤2所述算法参数和仿真实验结果如表1、2所示。

表1仿真实验参数设置

表2仿真实验结果

在具体实施例中，由图4可见，本发明的改进算法可以实现自动驾驶车辆在复杂的障碍环境中寻找到一条从起始节点出发到达目标点的无碰撞最优路径。

在具体实施例中，由图5和表2结果显示，本发明算法第迭代8次达到收敛，规划路径距离为31.799，明显优于基本算法。本发明算法全路径拐点有11个，拐角之和为495度，其中45度拐角18次、90度拐角1次；而基本算法的拐点数为17次，拐角之和为1440度，其中直角拐角有21次，45度拐角6次，显然本发明算法规划路径更平滑。

在具体实施例中，通过仿真对比分析，本发明公开的一种改进蚁群算法的自动驾驶车辆避障路径规划方法，在提高算法收敛速度和改善规划路径质量方面效果显著。

基于上述方法本发明还提供一种实现自动驾驶车辆避障路径规划方法的车辆，包括：

存储器，用于存储计算机程序及自动驾驶车辆避障路径规划方法；

处理器，用于执行所述计算机程序及自动驾驶车辆避障路径规划方法，以实现自动驾驶车辆避障路径规划方法的步骤。

本发明还提供一种具有自动驾驶车辆避障路径规划方法的可读存储介质，可读存储介质上存储有计算机程序，所述计算机程序被处理器执行以实现自动驾驶车辆避障路径规划方法的步骤。

车辆设有显示单元，显示单元可以包括液晶显示器(LCD，Liquid CrystalDisplay)、薄膜晶体管LCD(TFT-LCD，Thin Film Transistor-LCD)、有机发光二极管(OLED，Organic Light-Emitting Diode)显示器、柔性显示器、三维(3D)显示器等等中的至少一种。这些显示器中的一些可以被构造为透明状以允许用户从外部观看，实现自动驾驶车辆避障路径规划显示，自动驾驶车辆避障路径规划参数设置以及自动驾驶车辆避障路径规划处理等等。

具有自动驾驶车辆避障路径规划方法的可读存储介质是结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

实现自动驾驶车辆避障路径规划方法的存储介质中，存储有能够实现本说明书上述方法的程序产品。在一些可能的实施方式中，本公开的各个方面还可以实现为一种程序产品的形式，其包括程序代码，当所述程序产品在终端设备上运行时，所述程序代码用于使所述终端设备执行本说明书上述“示例性方法”部分中描述的根据本公开各种示例性实施方式的步骤。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (8)

1.一种自动驾驶车辆避障路径规划方法，其特征在于，方法包括：

步骤1：利用车载感知系统获取环境数据，并构建基于自动驾驶车辆二维栅格化的行驶道路地图模型；

在行驶道路地图模型中定位起始点S和目标点E；获取空间所有节点信息，计算邻接矩阵D和计算启发式信息矩阵；

步骤2：初始化迭代次数N，蚂蚁规模M，信息启发式因子α，期望启发式因子β，信息素挥发系数ρ，ε、t,信息素浓度，当前路径列表RT以及禁忌表TS；将起始点S分别置于禁忌表RT和当前路径列表TS中；

步骤3：查询邻接矩阵D，获取当前节点i出发下一步可行节点的集合

然后利用全局启发函数η_ij(t)优化蚁群算法状态转移概率

计算第m(m＝1,2,3,…,M)只蚂蚁选择相邻节点的概率；

按轮盘法选择下一节点，将选定的节点作为新的当前节点；

更新当前路径列表RT和禁忌表TS；

步骤4：更新蚂蚁序号，若第m只蚂蚁的当前路径列表包含了目标点或无路径且m≥M，则转入步骤5，否则返回步骤3；

步骤5：更新信息素，计算当前迭代最优路径，并利用动态调整的信息素增量模型Δτ_ij更新信息素矩阵；

步骤6：若n≥N，则输出最优路径并停止迭代，否则返回步骤3。

2.根据权利要求1所述的自动驾驶车辆避障路径规划方法，其特征在于，

步骤3中的蚁群算法状态转移概率

表达式为：

式中：

为t时刻第m只蚂蚁从当前位置节点i到相邻位置节点j的状态转移概率；

τ_ij(t)为路径(i,j)上的信息素浓度；

η_ij(t)为蚂蚁m在节点i处选择相邻节点j的启发函数，反映边(i,j)的能见度；

α为信息启发式因子，反映信息素对蚂蚁选择路径的影响力；

β为期望启发式因子，反应启发式信息在指导蚁群搜索过程中的相对重要程度；

U^m为蚂蚁尚未访问的下一节点的集合；

s为与当前位置节点i相邻的可选节点的集合；

τ_is(t)为当前位置节点i与各相邻节点之间的信息素浓度；

η_is(t)表示蚂蚁k在节点i与各相邻节点之间的启发函数。

3.根据权利要求1所述的自动驾驶车辆避障路径规划方法，其特征在于，

步骤3中的全局启发函数η_ij(t)表达式为：

式中：d_jE为j到目标节点E之间的距离，即

x_E为目标节点的横坐标，y_E为目标节点的纵坐标；t为相邻节点j对影响路径选择的权重；1-t为目标节点E对影响路径选择的权重。

4.根据权利要求3所述的自动驾驶车辆避障路径规划方法，其特征在于，

t的大小取决于车速、相邻节点之间的距离；

当车辆均速行驶时，t≈d_ij/(d_ij+d_jE)；

当d_jE远大于d_ij时，t→0，则全局启发函数η_ij(t)表达式可近似为：

全局启发函数η_ij(t)的理论基础为贝塞耳曲线函数；

贝塞耳曲线函数构造的全局启发函表达式为：

n+1个顶点的n次贝塞耳曲线表达式为：

式中：P_i(i＝0,1,2,…,n)为各顶点的位置向量，B_i,n(t)为伯恩斯坦基函数，其表达式为：

5.根据权利要求1或2所述的自动驾驶车辆避障路径规划方法，其特征在于，

步骤5中动态调整的信息素增量模型Δτ_ij表达式为：

式中：n为第n次迭代；

L_n,m为当前路径距离，即第m只蚂蚁产生的路径的距离；

L_min为最优路径距离，即第n次迭代产生的最短路径距离；

L_max为最差路径距离，即第n次迭代产生的最长路径距离；

L_idv为理想路径距离，即起始点与目标点的直线距离；

δ为最优路径距离与最差路径的差值，即δ＝L_max-L_n,m；

ε为第n次迭代可接受路径误差，ε值为一常数。

6.根据权利要求5所述的自动驾驶车辆避障路径规划方法，其特征在于，

在迭代过程中，新的信息素增量机制自适应动态调整信息素的强度，使优化过程加速向全局最优路径收敛；

当δ＞ε时，L_max与L_n,m差值越大，信息素强度越大，全局收敛时间缩短，算法求解效率得到提升；

当δ≤ε时，L_n,m与L_min越接近，信息素浓度蒸发越快；

L_idv在迭代过程中起到参照作用，作为最佳路径决策依据。

7.一种实现自动驾驶车辆避障路径规划方法的车辆，其特征在于，包括：

存储器，用于存储计算机程序及自动驾驶车辆避障路径规划方法；

处理器，用于执行所述计算机程序及自动驾驶车辆避障路径规划方法，以实现如权利要求1至6任意一项所述自动驾驶车辆避障路径规划方法的步骤。

8.一种具有自动驾驶车辆避障路径规划方法的可读存储介质，其特征在于，所述可读存储介质上存储有计算机程序，所述计算机程序被处理器执行以实现如权利要求1至6任意一项所述自动驾驶车辆避障路径规划方法的步骤。

Images