CN115564140A - 矿区非结构化道路全局及局部路径分层规划方法及装置 - Google Patents

Abstract

本发明提供了一种矿区非结构化道路全局及局部路径分层规划方法及装置，其方法包括：获取矿区非结构化道路的测绘地图建立全局节点‑弧模型，根据全局节点‑弧模型规划车辆通行的初始全局路径；根据初始节点进行路段拆分，对拆分得到的全路段初始局部路径进行优化融合，得到全路段优化局部路径和优化全局路径；确定车辆行驶的起始和目标位置，并确定优化全局路径中的最优全局路径，根据最优全局路径确定车辆跟踪行驶的多段最优局部路径；跟踪行驶过程中若感知到出现障碍时，基于预设的车辆运动学及人工势场法重新进行临时局部路径调整。本发明提供的矿区非结构化道路全局及局部路径分层规划方法能够兼顾路径规划效率、效果以及规划路径的安全性。

Description

矿区非结构化道路全局及局部路径分层规划方法及装置

技术领域

本发明涉及智能交通下的路径规划技术领域，具体涉及一种矿区非结构化道路全局及局部路径分层规划方法及装置。

背景技术

近年来，随着自动驾驶发展和应用的不断成熟，智能车辆的路径规划越来越受到人们的关注，自动驾驶的路径规划对智能车的自主移动和线路的优化具有重要意义，根据实际道路的地图信息数据，智能车辆需要找一条从初始位置到达目标位置的距离最短或时间最短的一条安全路径已成为发展的主要趋势。如何选择一个合理安全有效的路径是实现车辆自动驾驶的关键。

现有的全局路径规划方法大多是通过将车辆进行椭圆化处理后利用A*算法、快速搜索树算法、D*算法以及启发式算法等算法进行路径搜索，然后对搜索出的路径进行平滑处理以达到可供车辆行驶的路径。而对于矿区这种边角特殊的非结构化道路，一方面，每次进行全局A*类似的搜索算法进行路径搜索时，计算量大、搜索的点数多、搜索范围大、效率低，并不能保证为最优路径。另一方面，现有的实时规划局部路径，也具有算法局限性，规划出的路径不一定为最优路径；另外，现有的路径搜索方法在一定程度会造成车辆距离两侧墙壁的冗余空间压缩，导致车辆规避风险的能力较差，因此如何在保障路径可行的条件下形成尽可能安全的行车路径是当前无人矿卡急需解决的问题。

综上，现有技术中对于非结构化道路的路径规划存在无法兼顾规划效率、路径规划效果以及规划路径的安全性的技术问题。

发明内容

有鉴于此，有必要提供一种矿区非结构化道路全局及局部路径分层规划方法及装置，解决现有技术中对于非结构化道路的路径规划存在无法兼顾规划效率、路径规划效果以及规划路径的安全性的技术问题。

为了解决上述技术问题，一方面，本发明提供了一种矿区非结构化道路全局及局部路径分层规划方法，包括：

获取矿区非结构化道路的测绘地图，确定所述非结构化道路的道路边界并在关键区域设置初始节点建立全局节点-弧模型，根据所述全局节点-弧模型规划车辆通行的初始全局路径；

根据所述初始节点对所述初始全局路径进行路段拆分，得到全路段初始局部路径，基于车辆运动学和车辆两侧冗余空间对所述全路段初始局部路径进行优化融合，得到全路段优化局部路径和优化全局路径；

确定车辆行驶的起始位置和目标位置，根据所述起始位置和目标位置确定所述优化全局路径中的最优全局路径，根据所述最优全局路径确定车辆跟踪行驶的多段最优局部路径；

当车辆根据所述最优全局路径和多段最优局部路径进行跟踪行驶，且所述最优局部路径出现障碍时，基于预设的车辆运动学及人工势场法重新进行临时局部路径规划，控制车辆根据规划的临时局部路径跟踪行驶，在绕过障碍后返回所述最优全局路径及多段最优局部路径继续跟踪行驶。

在一些可能实现的方式中，所述获取矿区非结构化道路的测绘地图，确定所述非结构化道路的道路边界并在关键区域设置初始节点建立全局节点-弧模型，根据所述全局节点-弧模型规划车辆通行的初始全局路径，包括：

通过对矿区的测绘，得到矿区非结构化道路的测绘地图；

根据所述测绘地图确定所有道路的道路边界以及关键区域和弯道区域，并在所述关键区域和弯道区域设置初始节点；

以所述测绘地图包含的所有道路为连接弧，所述初始节点为连接节点，构建节点-弧模型；

基于所述节点-弧模型获取可供车辆通行的初始全局路径。

在一些可能实现的方式中，所述根据所述初始虚拟节点对所述初始全局路径进行路段拆分，得到全路段初始局部路径，基于车辆运动学和车辆两侧冗余空间对所述全路段初始局部路径进行优化融合，得到全路段优化局部路径和优化全局路径，包括：

根据所述初始节点将所述初始全局路径的全路段拆分为直线路段和弯道路段，得到所述全路段初始局部路径；

以车辆运动学模型为基础条件，以车辆距离两侧墙壁的冗余空间相同作为优化目标，通过保证车辆距离两侧墙壁具有相同的冗余空间对所述全路段初始局部路径中直线路段进行优化，得到直线路段优化局部路径；

基于车辆在弯道路段行驶时车辆的姿态与距离道路内外侧墙壁的距离的关系确定车辆转弯过程中的最优状态，以所述车辆转弯过程中的最优状态为基础条件，以车辆外侧冗余空间的最大值为优化条件，通过保证车辆内外侧冗余空间相同对所述全路段初始局部路径中弯道路段进行优化，得到弯道路段优化局部路径；

基于不同路段的轨迹的交接处车辆状态和规划路径的差异，以相邻路段中车辆状态作为基础条件，车辆运动学模型作为约束条件，规划路径差异为零作为约束目标，对路径轨迹的交接处的车辆姿态进行调整，对相邻路段的交接处进行优化得到交接融合优化局部路径；

根据所述直线路段优化局部路径、弯道路段优化局部路径和交接融合优化局部路径得到所述全路段优化局部路径和优化全局路径。

在一些可能实现的方式中，所述基于车辆在弯道路段行驶时车辆的姿态与距离道路内外侧墙壁的距离的关系确定车辆转弯过程中的最优状态，以所述车辆转弯过程中的最优状态为基础条件，以车辆外侧冗余空间的最大值为优化条件，通过保证车辆内外侧冗余空间相同对所述全路段初始局部路径中弯道路段进行优化，得到弯道路段优化局部路径，包括:

基于所述测绘地图获取弯道路段平均曲率和车辆参数；

以所述弯道路段平均曲率、车辆参数以及车辆在弯道路段行驶时车辆的姿态与距离道路内外侧墙壁的距离的关系为基础条件，固定车辆外侧前端距离外侧墙壁的距离L_外，形成不同的车辆行驶轨迹，求解不同L_外下车辆内侧距离内侧墙壁最近的距离L_内，当所述L_外与L_内相等时确定当前车辆状态为转弯过程中的车辆最优状态，并以所述车辆最优状态作为车辆转弯过程中最优路径的中间位置；

以车辆运动学模型作为约束，以所述车辆最优状态作为基础条件，以前轮转角的增量作为动力学约束，以车辆外侧冗余空间最大为优化条件，车辆内外侧冗余空间一致时作为最优解，确定弯道路段的中间位置前半段最优路径与后半段最优路径，得到弯道路段优化局部路径；

在优化后的车辆轨迹上重新确定每个弯道路段的初始节点，得到每个弯道路段的起始节点和终止节点。

在一些可能实现的方式中，所述基于不同路段的轨迹的交接处车辆状态和规划路径的差异，以相邻路段中车辆状态作为基础条件，车辆运动学模型作为约束条件，规划路径差异为零作为约束目标，对路径轨迹的交接处的车辆姿态和规划轨迹进行调整，对相邻路段的交接处进行优化得到交接融合优化局部路径，包括：

获取相邻的直线路段和弯道路段在弯道路段的起始节点和终止节点处的不同车辆状态及规划轨迹；

以所述不同车辆状态为基础条件，车辆运动学模型作为约束条件，起始节点和终止节点处规划路径差异为零作为约束目标，对直线路段中的车辆姿态及规划轨迹进行调整，使弯道路段的起始节点和终止节点处的轨迹与直线路段规划轨迹形成平滑的过度曲线，得到交接融合优化局部路径。

在一些可能实现的方式中，所述确定车辆行驶的起始位置和目标位置，根据所述起始位置和目标位置确定所述优化全局路径中的最优全局路径，根据所述最优全局路径确定车辆跟踪行驶的多段最优局部路径，包括：

获取车辆行驶的起始位置和目标位置；

根据预设的全局路径搜索算法确定所述节点-弧模型中从起始位置到目标位置的节点-弧模型最优全局路径；

根据所述节点-弧模型最优全局路径确定最优全局路径，根据所述最优全局路径确定多段局部最优路径；

车辆根据所述多段局部最优路径和最优全局路径进行跟踪行驶。

在一些可能实现的方式中，所述车辆根据所述最优全局路径和多段最优局部路径跟踪行驶，若感知到所述最优局部路径出现障碍时，基于预设的车辆运动学及人工势场法重新进行临时局部路径规划，车辆根据规划的临时局部路径跟踪行驶，直到绕过障碍后返回所述最优全局路径及多段最优局部路径继续跟踪行驶，包括：

车辆根据预设的所述最优全局路径和多段最优局部路径由所述起始位置向终点位置跟踪行驶；

当车辆传感器感知到所述最优局部路径出现障碍时，根据道路情况判断车辆是否可以安全通过所述障碍；

当所述道路情况允许车辆安全通过所述障碍时，根据预设的车辆运动学及人工势场法重新进行临时局部路径规划，车辆根据规划的临时局部路径跟踪行驶，绕过障碍后返回所述最优全局路径及多段最优局部路径继续跟踪行驶；

当所述道路情况不允许车辆安全通过所述障碍时，车辆返回当前路径的上一节点处，以所述上一节点处为起点，采用Dijkstra算法重新进行路径规划，车辆根据得到的临时最优全局路径和临时多段最优局部路径继续跟踪行驶。

在一些可能实现的方式中，所述根据预设的车辆运动学及人工势场法重新进行临时局部路径规划，包括：

采用所述人工势场法进行临时局部路径规划过程中，车辆通过所述障碍时存在下一节点对车辆的吸引力，障碍物对车辆的斥力以及车辆边界斥力，当所述吸引力、斥力以及边界斥力的合力为零时，车辆陷入了局部最小值点无法移动；

基于人工势场法为车辆添加出逃力，使车辆行驶出局部最小值点，所述出逃力方向为与下一节点吸引力垂直的方向，且指向道路内侧方向；

所述出逃力从零逐步增大，直到带动车辆摆脱所述最小值点后，所述出逃力消失，车辆按照当前势场继续行驶。

在一些可能实现的方式中，确定所述车辆边界斥力，包括：

通过建立道路边界斥力势场防止车辆在避障的过程与车道边界发生碰撞，建立所述车道边界斥力势场的具体公式为：

其中，F_rep.bo为道路边界斥力，η_rep.bo为正比例增益系数，e为自然常数，s为车辆质心到全局最优路径和局部最优路径的最短距离，D为车辆运行时的当前车道宽度。

另一方面，本发明还提供了一种矿区非结构化道路全局及局部路径分层规划装置，包括：

地图模型建立模块，用于获取矿区非结构化道路的测绘地图，确定所述非结构化道路的道路边界并在关键区域设置初始节点建立全局节点-弧模型，根据所述全局节点-弧模型规划车辆通行的初始全局路径；

路径优化融合模块，用于根据所述初始节点对所述初始全局路径进行路段拆分，得到全路段初始局部路径，基于车辆运动学和车辆两侧冗余空间对所述全路段初始局部路径进行优化融合，得到全路段优化局部路径和优化全局路径；

路径规划跟踪模块，用于确定车辆行驶的起始位置和目标位置，根据所述起点和终点位置确定所述优化全局路径中的最优全局路径，根据所述最优全局路径确定车辆跟踪行驶的多段最优局部路径；

临时规划跟踪模块，用于当车辆根据所述最优全局路径和多段最优局部路径进行跟踪行驶，且所述最优局部路径出现障碍时，基于预设的车辆运动学及人工势场法重新进行临时局部路径规划，控制车辆根据规划的临时局部路径跟踪行驶，在绕过障碍后返回所述最优全局路径及多段最优局部路径继续跟踪行驶。

采用上述实施例的有益效果是：本发明提供的矿区非结构化道路全局及局部路径分层规划方法，一方面，通过建立节点-弧模型建立全局路径，并通过对节点之间的路径采用冗余空间优化的方法确定初始全局路径，通过建立节点-弧模型使得进行路径搜索规划时计算量更小、效率更高、路径规划效果更好；另一方面，采用节点-弧模型的方式进行路径优化过程中结合车辆运动学模型，使得车辆路径需要平滑处理的节点较少，车辆行驶平稳性增强，增加规划路径的安全性；另一方面，通过设定车辆距离两侧墙壁的冗余空间的距离作为优化条件，使车辆与两侧墙壁之间能够保持最大的安全距离，进一步保证了车辆行驶的安全性；最后，通过设定改进的人工势场法，使得车辆具有局部避障路径规划的能力，并采用Dijkstra算法重新进行路径规划，提高了车辆的应急避险能力，在增加通行安全性的同时也保证了重新规划的路径到达目标地点的效率。

附图说明

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

图1为本发明提供的矿区非结构化道路全局及局部路径分层规划方法的一个实施例流程示意图；

图2为本发明提供的图1中步骤S101一实施例的流程示意图；

图3为本发明提供的图1中步骤S102一实施例的流程示意图；

图4为本发明提供的弯道路径优化一实施例的示意图；

图5为本发明提供的路径融合一实施例的示意图；

图6为本发明提供的矿区道路规划路径一实施例的示意图；

图7为本发明提供的图1中步骤S103一实施例的流程示意图；

图8为本发明提供的图1中步骤S104一实施例的流程示意图；

图9为本发明提供的避障路径规划一实施例的示意图；

图10为本发明提供的矿区非结构化道路全局及局部路径分层规划装置一实施例的结构示意图。

具体实施方式

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

应当理解，示意性的附图并未按实物比例绘制。本发明中使用的流程图示出了根据本发明的一些实施例实现的操作。应当理解，流程图的操作可以不按顺序实现，没有逻辑的上下文关系的步骤可以反转顺序或者同时实施。此外，本领域技术人员在本发明内容的指引下，可以向流程图添加一个或多个其他操作，也可以从流程图中移除一个或多个操作。

附图中所示的一些方框图是功能实体，不一定必须与物理或逻辑上独立的实体相对应。可以采用软件形式来实现这些功能实体，或在一个或多个硬件模块或集成电路中实现这些功能实体，或在不同网络和/或处理器系统和/或微控制器系统中实现这些功能实体。

在本文中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本发明的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是，本文所描述的实施例可以与其它实施例相结合。

在对具体的实施例进行描述之前，对本发明中的名词进行相关说明：

人工势场法：人工势场法是一种经典的路径规划算法。该算法将目标和障碍物分别看做对机器人有引力和斥力的物体，机器人沿引力与斥力的合力来进行运动。

A*算法：A*(A-Star)算法是一种静态路网中求解最短路径最有效的直接搜索方法，也是解决许多搜索问题的有效算法。算法中的距离估算值与实际值越接近，最终搜索速度越快。

D*算法:D*算法是动态的A*算法，由静态A*算法发展而来，是一种启发式的路径搜索算法，适合面对周围环境未知或者周围环境存在动态变化的场景。与A相反，A从起点到目标点进行搜索，而D是从目标点向起点进行搜索反向传播，反向搜索。

Dijkstra算法：迪杰斯特拉算法(Dijkstra)是由荷兰计算机科学家狄克斯特拉于1959年提出的，因此又叫狄克斯特拉算法。是从一个顶点到其余各顶点的最短路径算法，解决的是有权图中最短路径问题。迪杰斯特拉算法主要特点是从起始点开始，采用贪心算法的策略，每次遍历到始点距离最近且未访问过的顶点的邻接节点，直到扩展到终点为止。

基于上述名词描述，现有技术中采用搜索算法对非结构化道路进行路径规划时往往因为路径搜索时计算量大、搜索的点数多、搜索范围大、效率低，导致无法保证路径规划的效率和最优路径，且对于非结构化道路传统的路径规划方法进行路径规划时未考虑道路结构对行车安全性的影响，导致规划的路径无法保证车辆的行驶安全性，因此本方发明旨在提出一种能够兼顾规划效率、路径规划效果以及规划路径安全性的矿区非结构化道路全局及局部路径分层规划方法。

本发明实施例提供了一种矿区非结构化道路全局及局部路径分层规划方法及装置，以下分别进行说明。

如图1所示，图1为本发明提供的矿区非结构化道路全局及局部路径分层规划方法的一个实施例流程示意图，矿区非结构化道路全局及局部路径分层规划方法包括：

S101、获取矿区非结构化道路的测绘地图，确定所述非结构化道路的道路边界并在关键区域设置初始节点建立全局节点-弧模型，根据所述全局节点-弧模型规划车辆通行的初始全局路径；

S102、根据所述初始节点对所述初始全局路径进行路段拆分，得到全路段初始局部路径，基于车辆运动学和车辆两侧冗余空间对所述全路段初始局部路径进行优化融合，得到全路段优化局部路径和优化全局路径；

S103、确定车辆行驶的起始位置和目标位置，根据所述起始位置和目标位置确定所述优化全局路径中的最优全局路径，根据所述最优全局路径确定车辆跟踪行驶的多段最优局部路径；

S104、当车辆根据所述最优全局路径和多段最优局部路径进行跟踪行驶，且所述最优局部路径出现障碍时，基于预设的车辆运动学及人工势场法重新进行临时局部路径规划，控制车辆根据规划的临时局部路径跟踪行驶，在绕过障碍后返回所述最优全局路径及多段最优局部路径继续跟踪行驶。

与现有技术相比，本发明实施例提供的矿区非结构化道路全局及局部路径分层规划方法，一方面，通过建立节点-弧模型建立全局路径，并通过对节点之间的路径采用冗余空间优化的方法确定优化全局路径，通过建立节点-弧模型使得进行路径搜索规划时计算量更小、效率更高、路径规划效果更好；另一方面，采用节点-弧模型的方式进行路径优化过程中结合车辆运动学模型，使得车辆路径需要平滑处理的节点较少，车辆行驶平稳性增强，增加规划路径的安全性；另一方面，通过设定车辆距离两侧墙壁的冗余空间的距离作为优化条件，使车辆与两侧墙壁之间能够保持最大的安全距离，进一步保证了车辆行驶的安全性；最后，通过设定改进的人工势场法，使得车辆具有局部避障路径规划的能力，提高了车辆的应急避险能力。

在本发明的一些实施例中，如图2所示，图2为本发明提供的图1中步骤S101一实施例的流程示意图，步骤S101，包括：

S201、通过对矿区的测绘，得到矿区非结构化道路的测绘地图；

S202、根据所述测绘地图确定所有道路的道路边界以及关键区域和弯道区域，并在所述关键区域和弯道区域设置初始节点；

S203、以所述测绘地图包含的所有道路为连接弧，所述初始节点为连接节点，构建节点-弧模型；

S204、基于所述节点-弧模型获取可供车辆通行的初始全局路径。

在本发明具体的实施例中，通过对矿区的测绘，得到矿区边界的测绘地图，在矿区道路的关键区域(主要包括弯道和其他非道路区域)设置虚拟节点，基于这些虚拟节点以道路为连接弧，所述虚拟节点为连接节点建立节点-弧模型的初始全局路径，所有虚拟节点均为初始节点，通过路径节点建立测绘地图的初始全局路径。

本发明实施例通过建立节点-弧模型建立全局路径，将矿区非结构化道路中的关键区域和道路简化为虚拟节点和连接弧，使得进行路径搜索规划时计算量更小、效率更高、路径规划效果更好。

进一步地，在本发明的一些实施例中，如图3所示，图3为本发明提供的图1中步骤S102一实施例的流程示意图，步骤S102，包括：

S301、根据初始节点将所述初始全局路径的全路段拆分为直线路段和弯道路段，得到所述全路段初始局部路径；

S302、以车辆运动学模型为基础条件，以车辆距离两侧墙壁的冗余空间相同作为优化目标，通过保证车辆距离两侧墙壁具有相同的冗余空间对所述全路段初始局部路径中直线路段进行优化，得到直线路段优化局部路径；

S303、基于车辆在弯道路段行驶时车辆的姿态与距离道路内外侧墙壁的距离的关系确定车辆转弯过程中的最优状态，以所述车辆转弯过程中的最优状态为基础条件，以车辆外侧冗余空间的最大值为优化条件，通过保证车辆内外侧冗余空间相同对所述全路段初始局部路径中弯道路段进行优化，得到弯道路段优化局部路径；

S304、基于不同路段的轨迹的交接处车辆状态和规划路径的差异，以相邻路段中车辆状态作为基础条件，车辆运动学模型作为约束条件，规划路径差异为零作为约束目标，对路径轨迹的交接处的车辆姿态进行调整，对相邻路段的交接处进行优化得到交接融合优化局部路径；

S305、根据所述直线路段优化局部路径、弯道路段优化局部路径和交接融合优化局部路径得到所述全路段优化局部路径和优化全局路径。

在本发明的具体的实施例中，步骤S301中，由于在直线路段和弯道路段中车辆的姿态有所不同，弯道路段中车身各部分距离道路两侧墙壁距离并不相同，常规的路径规划容易使车辆与道路两侧产生碰撞，因此需要对规划的初始路径进行拆分优化，根据初始节点所在位置将初始全局路径的全路段拆分为直线路段和弯道路段，得到全路段初始局部路径。

进一步地，步骤S302中，对直线路段的路径进行优化，以车辆运动学模型为基础条件，以车辆距离道路两侧墙壁的冗余空间相同作为优化目标，通过保证车辆距离两侧墙壁具有相同的冗余空间对全路段初始局部路径中直线路段进行优化，得到直线路段优化局部路径，车辆的运动学模型如下：

其中，

为下一刻车辆的质心x方向位置；

为下一刻车辆的质心y方向位置；

为下一刻车辆速度；

为下一刻车辆方向的横摆角；β为质心侧偏角；矿区车速较慢，β值较小可忽略；δ_f为前轮转角；δ_r为后轮转角；l_f为前轴与质心的距离；l_r为后轴与质心的距离。

具体的，基于节点-弧模型的全局路径，进行直线路段的路径优化，以车辆运动学模型为约束，以车辆距离两侧墙壁的冗余空间作为优化目标，通过保证车辆距离两侧墙壁的距离一致即保证相同的冗余空间作为约束条件形成各节点之间的最优局部路径。本实施例中节点间路段轨迹优化中车辆与两侧墙壁之间的位置关系满足：

L_左＝L_右

其中L_左表示车辆左侧距离墙壁的最近距离；L_右表示车辆右侧距离墙壁的最近距离，当车辆在直线路段上行驶的任一时刻满足以上约束条件时，车辆在局部路段中行驶时始终能保持左右两侧均有足够的冗余空间进行应急操作，以此形成车辆在直线路段的优化局部路径。

进一步地，步骤S303中，对弯道路段的路径进行优化，包括：

基于所述测绘地图获取弯道路段平均曲率和车辆参数；

以所述弯道路段平均曲率、车辆参数以及车辆在弯道路段行驶时车辆的姿态与距离道路内外侧墙壁的距离的关系为基础条件，固定车辆外侧前端距离外侧墙壁的距离L_外，形成不同的车辆行驶轨迹，求解不同L_外下车辆内侧距离内侧墙壁最近的距离L_内，当所述L_外与L_内相等时确定当前车辆状态为转弯过程中的车辆最优状态，并以所述车辆最优状态作为车辆转弯过程中最优路径的中间位置；

以车辆运动学模型作为约束，以所述车辆最优状态作为基础条件，以前轮转角的增量作为动力学约束，以车辆外侧冗余空间最大为优化条件，车辆内外侧冗余空间一致时作为最优解，确定弯道路段的中间位置前半段最优路径与后半段最优路径，得到弯道路段优化局部路径；

在优化后的车辆轨迹上重新确定每个弯道路段的初始节点，得到每个弯道路段的起始节点和终止节点。

具体的，对于弯道这种曲率较大的路段而言，局部路段的优化方法如图4所示，图4为本发明提供的弯道路径优化一实施例的示意图，L_内表示车辆距离内侧墙壁的最近距离；L_外表示车辆距离外侧墙壁的最近距离。由于车辆在转弯过程中存在内外轮差，相较于L_内的值L_外的值更加便于测量，通过固定L_外值，利用车辆运动学模型确定出一条完整的车辆转弯轨迹，并计算车辆在不同位置处的L_外值，基于前轮的转角范围作为运动学约束，可以得到下一刻车辆可能移动的任何位置，基于下一科车辆位置，可得到车辆外侧距离墙壁的距离L_外，车辆内侧距离墙壁的最近距离L_内，最优解的评价指标为：θ

θ值最接近0.5的位置为下一刻的最优解位置，图4中黑色实体矩形代表最优车辆状态。

通过调节L_外值的大小，可以形成多条车辆转弯轨迹，随着L_外值的增大，minL_内逐渐减小，当满足条件：

max(L_外)＝min(L_内)

此时，车辆的内外冗余空间都处于整体最大的状态，以此时的车辆状态作为车辆在转弯过程中的最优车辆状态。

基于前轮转角的最大连续增量，以车辆最优状态最为基础条件，考虑包含速度、车轮转角、车辆横摆角等车辆姿态因素，计算车辆上一时刻或下一时刻的所有可能的状态，并计算每一种车辆状态下的最优车辆内外两侧的冗余空间，选取最优的车辆状态最为下一时刻车辆状态计算的基础条件，最终得出车辆在整个弯道局部路径中的最优行驶轨迹，得到每个弯道路段的起始节点和终止节点代替原有的初始节点。

进一步地，步骤S304中，完成直线路段和弯道路段的路径优化后得到的整体规划路径并不连续，车辆状态也不相同，因此需要对直线路段和弯道路段连接处的部分路径进行融合，使得得到的整体规划路径连续。通过获取相邻的直线路段和弯道路段在弯道路段的起始节点和终止节点处的不同车辆状态及规划轨迹；以不同车辆状态为基础条件，车辆运动学模型作为约束条件，起始节点和终止节点处规划路径差异为零作为约束目标，对直线路段中的车辆姿态及规划轨迹进行调整，使弯道路段的起始节点和终止节点处的轨迹与直线路段规划轨迹形成平滑的过度曲线，得到交接融合优化局部路径。

具体的，在完成所有节点间的局部路径优化后，局部路径轨迹的交接处车辆状态会存在差异，因此需要对相邻路段中的最优路径进行平滑处理，使之构成相对平滑的过渡路径，如图5图所示，图5为本发明提供的路径融合一实施例的示意图，其中，虚线框为融合的车辆姿态调整，融合点均为弯道优化路径后的新节点，使路段之间的最优路径能够融合形成融合路径。

进一步的，步骤S305中，完成路径融合之后，根据直线路段优化局部路径、弯道路段优化局部路径和交接融合优化局部路径得到全路段优化局部路径和优化全局路径。

具体的，在完成所有节点之间的局部最优路径的平滑处理后，基于测绘地图的矿区无人卡车路径规划生成离线的矿区无人车辆全路段优化局部路径和优化全局路径。其路径实例图请参阅图6，图6为本发明提供的矿区道路规划路径一实施例的示意图，车辆由上料区的起始位置行驶至卸料区的目标位置，矿车的起始位置和目标位置均已标出，路段上的黑色圆点均为道路节点，节点之间的黑色细实线路径均为局部最优路径，对最终确定的所有节点之间的局部路径进行离线处理；采用Dijkstra算法在全局节点-弧模型中搜索从起始位置到达目标位置的全局最优路径，相邻节点之间的实际路径长度为代价估计，以总代价最小(车辆行驶距离最短)为优化目的，在全局路径中找到全局最优路径，图中由黑色粗实线路径表示，调取该路径上预设的局部最优路径，车辆按照全局最优路径+预设的局部最优路径向目标位置行驶。

本发明实施例采用节点-弧模型的方式进行路径优化过程中结合车辆运动学模型，使得车辆路径需要平滑处理的节点较少，车辆行驶平稳性增强，增加规划路径的安全性，通过设定车辆距离两侧墙壁的冗余空间的距离作为优化条件，使车辆与两侧墙壁之间能够保持最大的安全距离，进一步保证了车辆行驶的安全性。

进一步的，在本发明的一些实施例中，如图7所示，图7为本发明提供的图1中步骤S103一实施例的流程示意图，步骤S103包括：

S701、获取车辆行驶的起始位置和目标位置；

S702、根据预设的全局路径搜索算法确定所述节点-弧模型中从起始位置到目标位置的节点-弧模型最优全局路径；

S703、根据所述节点-弧模型最优全局路径确定最优全局路径，根据所述最优全局路径确定多段局部最优路径；

S704、车辆根据所述多段局部最优路径和最优全局路径进行跟踪行驶。

在本发明具体的实施例中，通过路径优化得到的全路段优化局部路径和优化全局路径离线储存与运输车辆上，应用对象不局限于现有的作业车辆，更换不同车辆后，路径更新简单，适应性较强。通过获取车辆行驶的起始位置和目标位置采用全局路径搜索算法调用离线储存节点-弧模型中的最优全局路径，并根据最优全局路径获取车辆跟踪行驶的局部最优路径。

进一步地，在本发明的一些实施例中，如图8所示，图8为本发明提供的图1中步骤S104一实施例的流程示意图，步骤S104包括：

S801、车辆根据预设的所述最优全局路径和多段最优局部路径由所述起始位置向终点位置跟踪行驶；

S802、当车辆传感器感知到所述最优局部路径出现障碍时，根据道路情况判断车辆是否可以安全通过所述障碍；

S803、当所述道路情况允许车辆安全通过所述障碍时，根据预设的车辆运动学及人工势场法重新进行临时局部路径规划，车辆根据规划的临时局部路径跟踪行驶，绕过障碍后返回所述最优全局路径及多段最优局部路径继续跟踪行驶；

S804、当所述道路情况不允许车辆安全通过所述障碍时，车辆返回当前路径的上一节点处，以所述上一节点处为起点，采用Dijkstra算法重新进行路径规划，车辆根据得到的临时最优全局路径和临时多段最优局部路径继续跟踪行驶。

在本发明的具体的实施例中，车辆进行作业时，根据预设的最优全局路径和多段最优局部路径从起始位置向终点位置跟踪行驶，车辆在行驶的过程中，车载雷达等传感器实时感知周围环境，当检测到当前路径中的道路环境与预设的局部路径环境不同或出现障碍时，根据道路情况判断车辆是否可以安全通过所述障碍，当所述道路情况允许车辆安全通过所述障碍时，则采用临时局部路径规划(临时局部路径规划用势场法，同样遵循车辆运动学与转弯半径的约束)进行局部避障，如图9所示，图9为本发明提供的避障路径规划一实施例的示意图，避障完成后返回预设的最优全局路径和多段最优局部路径，继续前进，当所述道路情况不允许车辆安全通过所述障碍时，车辆返回当前路径的上一节点处，以上一节点处为起点，采用Dijkstra算法重新进行路径规划，车辆根据得到的临时最优全局路径和临时多段最优局部路径继续跟踪行驶。

具体的，采用改进的人工势场法进行局部避障，目标点为该路段的下一节点，针对矿车在避障的过程中可能与车道边界发生碰撞，通过建立道路边界斥力势场防止车辆在避障的过程与车道边界发生碰撞，建立所述车道边界斥力势场的具体公式为：

其中，F_rep.bo为道路边界斥力，η_rep.bo为正比例增益系数，e为自然常数，s为车辆质心到全局最优路径和局部最优路径的最短距离，D为车辆运行时的当前车道宽度。将其与传统的人工势场法的斥力与引力相结合，从而产生新的合力，引导矿卡进行临时局部路径规划。

在局部避障的过程中，可能会存在下一节点对车辆的吸引力，障碍物对车辆的斥力，车辆边界力合力为零的情况，当合力为零时，车辆陷入了局部最小值点，无法移动，此时需要增加一个外力来干预，为出逃力F_esc，迫使车辆行驶出局部最小值点，出逃力方向为于下一节点吸引力垂直的方向，且指向道路内侧方向，设置出逃力从零值逐步增大，增大到合适值停止，直到带动车辆摆脱最小值点后，出逃离消失，车辆按照当前势场继续行驶。

为了使车辆避开障碍物后，尽快回到最优全局路径和多段最优局部路径上，加入预设局部最优路径的引力势场，当车辆避开障碍物后，预设局部最优路径的引力势场发挥作用，使车辆快速回到预设局部最优路径上去，当车辆返回预设局部最优路径上时，临时局部路径规划结束，继续按照最优全局路径和多段最优局部路径进行跟踪行驶。

本发明实施例通过设定改进的人工势场法，使得车辆具有局部避障路径规划的能力，并采用Dijkstra算法重新进行路径规划，提高了车辆的应急避险能力，在增加通行安全性的同时也保证了重新规划的路径到达目标地点的效率。

为了更好实施本发明实施例中的矿区非结构化道路全局及局部路径分层规划方法，在矿区非结构化道路全局及局部路径分层规划方法的基础之上，对应的，本发明实施例还提供了矿区非结构化道路全局及局部路径分层规划装置，如图10所示，矿区非结构化道路全局及局部路径分层规划装置1000包括：

地图模型建立模块1001，用于获取矿区非结构化道路的测绘地图，确定所述非结构化道路的道路边界并在关键区域设置初始节点建立全局节点-弧模型，根据所述全局节点-弧模型规划车辆通行的初始全局路径；

路径优化融合模块1002，用于根据所述初始节点对所述初始全局路径进行路段拆分，得到全路段初始局部路径，基于车辆运动学和车辆两侧冗余空间对所述全路段初始局部路径进行优化融合，得到全路段优化局部路径和优化全局路径；

路径规划跟踪模块1003，用于确定车辆行驶的起始位置和目标位置，根据所述起点和终点位置确定所述优化全局路径中的最优全局路径，根据所述最优全局路径确定车辆跟踪行驶的多段最优局部路径；

临时规划跟踪模块1004，用于当车辆根据所述最优全局路径和多段最优局部路径进行跟踪行驶，且所述最优局部路径出现障碍时，基于预设的车辆运动学及人工势场法重新进行临时局部路径规划，控制车辆根据规划的临时局部路径跟踪行驶，在绕过障碍后返回所述最优全局路径及多段最优局部路径继续跟踪行驶。

上述实施例提供的矿区非结构化道路全局及局部路径分层规划装置1000可实现上述矿区非结构化道路全局及局部路径分层规划方法实施例中描述的技术方案，上述各模块或单元具体实现的原理可参见上述矿区非结构化道路全局及局部路径分层规划方法实施例中的相应内容，此处不再赘述。

本领域技术人员可以理解，实现上述实施例方法的全部或部分流程，可以通过计算机程序来指令相关的硬件(如处理器，控制器等)来完成，计算机程序可存储于计算机可读存储介质中。其中，计算机可读存储介质为磁盘、光盘、只读存储记忆体或随机存储记忆体等。

以上对本发明所提供的矿区非结构化道路全局及局部路径分层规划方法及装置进行了详细介绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (10)

1.一种矿区非结构化道路全局及局部路径分层规划方法，其特征在于，包括：

获取矿区非结构化道路的测绘地图，确定所述非结构化道路的道路边界并在关键区域设置初始节点建立全局节点-弧模型，根据所述全局节点-弧模型规划车辆通行的初始全局路径；

根据所述初始节点对所述初始全局路径进行路段拆分，得到全路段初始局部路径，基于车辆运动学和车辆两侧冗余空间对所述全路段初始局部路径进行优化融合，得到全路段优化局部路径和优化全局路径；

确定车辆行驶的起始位置和目标位置，根据所述起始位置和目标位置确定所述优化全局路径中的最优全局路径，根据所述最优全局路径确定车辆跟踪行驶的多段最优局部路径；

当车辆根据所述最优全局路径和多段最优局部路径进行跟踪行驶，且所述最优局部路径出现障碍时，基于预设的车辆运动学及人工势场法重新进行临时局部路径规划，控制车辆根据规划的临时局部路径跟踪行驶，在绕过障碍后返回所述最优全局路径及多段最优局部路径继续跟踪行驶。

2.根据权利要求1所述的矿区非结构化道路全局及局部路径分层规划方法，其特征在于，所述获取矿区非结构化道路的测绘地图，确定所述非结构化道路的道路边界并在关键区域设置初始节点建立全局节点-弧模型，根据所述全局节点-弧模型规划车辆通行的初始全局路径，包括：

通过对矿区的测绘，得到矿区非结构化道路的测绘地图；

根据所述测绘地图确定所有道路的道路边界以及关键区域和弯道区域，并在所述关键区域和弯道区域设置初始节点；

以所述测绘地图包含的所有道路为连接弧，所述初始节点为连接节点，构建节点-弧模型；

基于所述节点-弧模型获取可供车辆通行的初始全局路径。

3.根据权利要求1所述的矿区非结构化道路全局及局部路径分层规划方法，其特征在于，所述根据所述初始节点对所述初始全局路径进行路段拆分，得到全路段初始局部路径，基于车辆运动学和车辆两侧冗余空间对所述全路段初始局部路径进行优化融合，得到全路段优化局部路径和优化全局路径，包括：

根据所述初始节点将所述初始全局路径的全路段拆分为直线路段和弯道路段，得到所述全路段初始局部路径；

以车辆运动学模型为基础条件，以车辆距离两侧墙壁的冗余空间相同作为优化目标，通过保证车辆距离两侧墙壁具有相同的冗余空间对所述全路段初始局部路径中直线路段进行优化，得到直线路段优化局部路径；

基于车辆在弯道路段行驶时车辆的姿态与距离道路内外侧墙壁的距离的关系确定车辆转弯过程中的最优状态，以所述车辆转弯过程中的最优状态为基础条件，以车辆外侧冗余空间的最大值为优化条件，通过保证车辆内外侧冗余空间相同对所述全路段初始局部路径中弯道路段进行优化，得到弯道路段优化局部路径；

基于不同路段的轨迹的交接处车辆状态和规划路径的差异，以相邻路段中车辆状态作为基础条件，车辆运动学模型作为约束条件，规划路径差异为零作为约束目标，对路径轨迹的交接处的车辆姿态进行调整，对相邻路段的交接处进行优化得到交接融合优化局部路径；

根据所述直线路段优化局部路径、弯道路段优化局部路径和交接融合优化局部路径得到所述全路段优化局部路径和优化全局路径。

4.根据权利要求3所述的矿区非结构化道路全局及局部路径分层规划方法，其特征在于，所述基于车辆在弯道路段行驶时车辆的姿态与距离道路内外侧墙壁的距离的关系确定车辆转弯过程中的最优状态，以所述车辆转弯过程中的最优状态为基础条件，以车辆外侧冗余空间的最大值为优化条件，通过保证车辆内外侧冗余空间相同对所述全路段初始局部路径中弯道路段进行优化，得到弯道路段优化局部路径，包括:

基于所述测绘地图获取弯道路段平均曲率和车辆参数；

以所述弯道路段平均曲率、车辆参数以及车辆在弯道路段行驶时车辆的姿态与距离道路内外侧墙壁的距离的关系为基础条件，固定车辆外侧前端距离外侧墙壁的距离L_外，形成不同的车辆行驶轨迹，求解不同L_外下车辆内侧距离内侧墙壁最近的距离L_内，当所述L_外与L_内相等时确定当前车辆状态为转弯过程中的车辆最优状态，并以所述车辆最优状态作为车辆转弯过程中最优路径的中间位置；

以车辆运动学模型作为约束，以所述车辆最优状态作为基础条件，以前轮转角的增量作为动力学约束，以车辆外侧冗余空间最大为优化条件，车辆内外侧冗余空间一致时作为最优解，确定弯道路段的中间位置前半段最优路径与后半段最优路径，得到弯道路段优化局部路径；

在优化后的车辆轨迹上重新确定每个弯道路段的初始节点，得到每个弯道路段的起始节点和终止节点。

5.根据权利要求4所述的矿区非结构化道路全局及局部路径分层规划方法，其特征在于，所述基于不同路段的轨迹的交接处车辆状态和规划路径的差异，以相邻路段中车辆状态作为基础条件，车辆运动学模型作为约束条件，规划路径差异为零作为约束目标，对路径轨迹的交接处的车辆姿态和规划轨迹进行调整，对相邻路段的交接处进行优化得到交接融合优化局部路径，包括：

获取相邻的直线路段和弯道路段在弯道路段的起始节点和终止节点处的不同车辆状态及规划轨迹；

以所述不同车辆状态为基础条件，车辆运动学模型作为约束条件，起始节点和终止节点处规划路径差异为零作为约束目标，对直线路段中的车辆姿态及规划轨迹进行调整，使弯道路段的起始节点和终止节点处的轨迹与直线路段规划轨迹形成平滑的过度曲线，得到交接融合优化局部路径。

6.根据权利要求1所述的矿区非结构化道路全局及局部路径分层规划方法，其特征在于，所述确定车辆行驶的起始位置和目标位置，根据所述起始位置和目标位置确定所述优化全局路径中的最优全局路径，根据所述最优全局路径确定车辆跟踪行驶的多段最优局部路径，包括：

获取车辆行驶的起始位置和目标位置；

根据预设的全局路径搜索算法确定所述节点-弧模型中从起始位置到目标位置的节点-弧模型最优全局路径；

根据所述节点-弧模型最优全局路径确定最优全局路径，根据所述最优全局路径确定多段局部最优路径；

车辆根据所述多段局部最优路径和最优全局路径进行跟踪行驶。

7.根据权利要求1所述的矿区非结构化道路全局及局部路径分层规划方法，其特征在于，所述当车辆根据所述最优全局路径和多段最优局部路径进行跟踪行驶，且所述最优局部路径出现障碍时，基于预设的车辆运动学及人工势场法重新进行临时局部路径规划，控制车辆根据规划的临时局部路径跟踪行驶，在绕过障碍后返回所述最优全局路径及多段最优局部路径继续跟踪行驶，包括：

车辆根据预设的所述最优全局路径和多段最优局部路径由所述起始位置向终点位置跟踪行驶；

当车辆传感器感知到所述最优局部路径出现障碍时，根据道路情况判断车辆是否可以安全通过所述障碍；

当所述道路情况允许车辆安全通过所述障碍时，根据预设的车辆运动学及人工势场法重新进行临时局部路径规划，车辆根据规划的临时局部路径跟踪行驶，绕过障碍后返回所述最优全局路径及多段最优局部路径继续跟踪行驶；

当所述道路情况不允许车辆安全通过所述障碍时，车辆返回当前路径的上一节点处，以所述上一节点处为起点，采用Dijkstra算法重新进行路径规划，车辆根据得到的临时最优全局路径和临时多段最优局部路径继续跟踪行驶。

8.根据权利要求7所述的矿区非结构化道路全局及局部路径分层规划方法，其特征在于，所述根据预设的车辆运动学及人工势场法重新进行临时局部路径规划，包括：

采用所述人工势场法进行临时局部路径规划过程中，车辆通过所述障碍时存在下一节点对车辆的吸引力，障碍物对车辆的斥力以及车辆边界斥力，当所述吸引力、斥力以及边界斥力的合力为零时，车辆陷入了局部最小值点无法移动；

基于人工势场法为车辆添加出逃力，使车辆行驶出局部最小值点，所述出逃力方向为与下一节点吸引力垂直的方向，且指向道路内侧方向；

所述出逃力从零逐步增大，直到带动车辆摆脱所述最小值点后，所述出逃力消失，车辆按照当前势场继续行驶。

9.根据权利要求8所述的矿区非结构化道路全局及局部路径分层规划方法，其特征在于，确定所述车辆边界斥力，包括：

通过建立道路边界斥力势场防止车辆在避障的过程与车道边界发生碰撞，建立所述车道边界斥力势场的具体公式为：

其中，F_rep.bo为道路边界斥力，η_rep.bo为正比例增益系数，e为自然常数，s为车辆质心到全局最优路径和局部最优路径的最短距离，D为车辆运行时的当前车道宽度。

10.一种矿区非结构化道路全局及局部路径分层规划装置，其特征在于，包括：

地图模型建立模块，用于获取矿区非结构化道路的测绘地图，确定所述非结构化道路的道路边界并在关键区域设置初始节点建立全局节点-弧模型，根据所述全局节点-弧模型规划车辆通行的初始全局路径；

路径优化融合模块，用于根据所述初始节点对所述初始全局路径进行路段拆分，得到全路段初始局部路径，基于车辆运动学和车辆两侧冗余空间对所述全路段初始局部路径进行优化融合，得到全路段优化局部路径和优化全局路径；

路径规划跟踪模块，用于确定车辆行驶的起始位置和目标位置，根据所述起点和终点位置确定所述优化全局路径中的最优全局路径，根据所述最优全局路径确定车辆跟踪行驶的多段最优局部路径；

临时规划跟踪模块，用于当车辆根据所述最优全局路径和多段最优局部路径进行跟踪行驶，且所述最优局部路径出现障碍时，基于预设的车辆运动学及人工势场法重新进行临时局部路径规划，控制车辆根据规划的临时局部路径跟踪行驶，在绕过障碍后返回所述最优全局路径及多段最优局部路径继续跟踪行驶。

Images