CN115097824A - 一种复杂环境下的车辆路径规划方法 - Google Patents

Abstract

本发明涉及一种复杂环境下的车辆路径规划方法，属于智能交通技术领域。本方法采用一种增量式加载与搜索路径规划算法，在增量式加载路径规划算法中重用之前已加载区域的规划结果，进一步提高路径规划的速度。在加载地图数据时采用增量式方法，避免一次性加载大量地图数据，大幅减少了地理信息处理时间。在加载区域范围内，使用打破路径对称性的增量式搜索路径规划算法，进一步提升了路径规划速度。同时，针对现有基于启发式搜索算法的对称重复搜索问题，引入直线轨迹偏离值，区别原本具有相同优先级的对称点打破路径的对称性，减少算法对冗余栅格的探索，提升了规划速度。

Description

一种复杂环境下的车辆路径规划方法

技术领域

本发明涉及一种复杂环境下的车辆路径规划方法，属于智能交通技术领域。

背景技术

在智能交通领域，随着无人驾驶、车路协同等技术快速发展应用，系统对于车辆路径规划能力提出了越来越高的要求。

车辆路径规划，是根据规划目标和车辆性能及其它约束条件，通过算法快速规划出一条或者多条最优路径供用户决策。随着当前导航技术的发展，车辆已经不仅局限于拓扑路网环境，在特殊情况下，处于野外的越野车辆在无路网拓扑或野外与路网并存的情况下，如何高效准确的进行路径规划是值得研究的问题。

除了传统的大规模路网的城市环境外，在地形复杂、存在无道路区域的野外环境也同样面临路径规划需求。无路网环境下对地理信息建模，需要综合考虑众多环境地形因素，例如：河流、道路、土质等。对地图进行栅格化建模，再应用Dijkstra、A*算法以及基于二者的双向寻路算法是在栅格路径规划中被广泛采用的经典算法。但是，经典路径规划算法如Dijkstra算法和A^*搜索算法在大规模网格环境下效率并不乐观。Dijkstra算法需要O(n²)的时间复杂度寻找最佳路径。而A^*算法在面对密集障碍物时同样需要大量时间进行规划才能到达终点，计算量会随搜索空间增大指数级增长。特别是野外路径规划应用场景通常地图面积较大，海量数据加之计算能力有限，使得规划时间过长，是导致大型地图中寻路性能瓶颈的重要原因。如何进一步提升大规模地图下的路径规划效率，是本领域研究的重点。

另外，在路径规划领域中，启发式算法通常存在重复搜索的问题，在相同的开始位置和终止位置上一般存在许多可能的代价相同的最优路径，这些路径是相互对称的，它们之间唯一的区别在于车辆移动的顺序。解决启发式路径规划算法的对称重复搜索问题，如何针对大规模地图中打破路径对称性，减少重复搜索，具有重要意义。

发明内容

本发明的目的是针对现有技术存在的不足和缺陷，为了有效解决在自动驾驶中的车辆路径规划耗时严重，以及现有算法存在的因路径对称性导致重复搜索、效率低下等技术问题，创造性地提出一种复杂环境下的车辆路径规划方法。

本方法的创新点在于：首次提出了一种增量式加载与搜索路径规划算法(Incremental Load and Search Path Planning Algorithm，ILSP)，旨在增量式加载路径规划算法中重用之前已加载区域的规划结果，进一步提高路径规划的速度。在加载地图数据时采用增量式方法，避免一次性加载大量地图数据，大幅减少了地理信息处理时间。在加载区域范围内，使用打破路径对称性的增量式搜索路径规划算法，进一步提升了路径规划速度。同时，针对现有基于启发式搜索算法的对称重复搜索问题，引入直线轨迹偏离值，区别原本具有相同优先级的对称点打破路径的对称性，减少算法对冗余栅格的探索，提升了规划速度。

本发明是采用以下技术方案实现的。

一种复杂环境下的车辆路径规划方法，包括以下步骤：

步骤1：选定路径起始点和终止点。以起止点为基准规划出一个矩形，以该矩形作为基准向外扩展，得到将基准矩形包裹在内的规划中心矩形。

然后，对规划中心矩形进行地理信息建模，具体可以采用以下方法：

步骤1.1：对规划区域进行地理信息建模。

本发明采用栅格化处理方式，将地图根据设置的栅格边长划分为若干小栅格。其中，单个栅格边长l_grid通过式1计算：

其中，l_grid表示单个栅格边长，S_i表示区域内部的多边形面积(如湖泊、居民地等)，n表示区域内的多边形个数。

地图的地理信息数据书由多个矢量图层构成，每个图层分别存储区域内的某类特征地理信息集合。对地理信息数据划分栅格，如图1所示，对栅格化后的规划区域通过遍历栅格，在遍历时将行列数转换成高斯坐标，借助地理系统引擎获得所需图层数据。

步骤1.2：土质图层信息处理。

对于土质图层的信息，在路径规划时参考车辆在不同土质上的平均行驶速度，以车辆在平沙地的行驶速度为基准，在建模阶段设置栅格u的土质速度比例u_ratio，由式2计算得出：

其中，v_soil为当前网格土质所对应的车辆行驶速度，v_Flatsand是土质为平沙地时的车辆行驶速度。

步骤1.3：拓扑路网建模。

本发明采用图论中的有向图结构存储路网进行建模，生成符合规划需求的有向图。

具体地，将地图道路矢量图层中的数据构建为拓扑模型，表示为如下式：

G＝(V_a,E_a,W_a) (3)

V_a＝{v_i|i∈1,2,3...,n} (4)

W_a＝{w_ij|i,j∈1,2,3,...,n} (5)

E_a＝{e_i|i∈1,2,3,...,n} (6)

其中，G表示整个道路拓扑图，V_a表示路网中的道路起止点以及不同道路连接点抽象成的顶点，v_i表示第i个顶点；W_a表示弧的代价值，w_ij表示第i和j个结点间的代价值；E_a是道路抽象成为的边集，e_i表示第i条边，集合中的每个元素是一条道路，由道路连接的路口和该道路的属性构成，表示为：

e_a＝(S_id,E_id,Q_a,P_v) (7)

其中，S_id是该道路的起始点id，E_id是该道路的终止点id；Q_a是该道路的属性信息，包括该道路的道路等级、道路的长度，P_v是道路的定位点集合。

将路网采取有向图建模后，将矢量路网数据以图的结构存储，以邻接表的形式存储路网。

步骤2：采用增量式加载路径规划算法，提高车辆路径规划效率。

在确定规划中心矩形，也就是整个规划区域后，将规划中心矩形划分为sp份，将加载区域标识定义为mark，每个区域内的已规划行列数分别为ld_rows_mark、ld_cols_mark，并分别初始化为0。将增量式加载地图在每次加载地图回合中增加的行、列数值分别定义为rows、cols。

其中，rows和cols的计算方式如下：

rows＝[ROWS/sp] (8)

cols＝[COLS/sp] (9)

其中，ROWS为规划中心矩形总行数，COLS为规划中心矩形的总列数，[]为向上取整符号。

在每次的增量式加载新回合中，每个区域的行、列数为ld_rows_mark、ld_cols_mark，mark指的是每次加载的地图在规划中心矩形的区域标识。mark取值0、1、2、3，mark＝0代表起点所在的规划中心矩形区域，mark＝1代表目的地所在的矩形区域，mark＝2代表与起点同左侧或右侧的矩形区域，mark＝3即为与终点同右侧或左侧的区域。

如果总行数减去已经加载的行数或列数，小于每次的基础增量值rows或cols，则本次的规划区域矩形行数/列数增加值等于总行数/列数减去已经加载的行数/列数的值，否则仍按照rows或cols增加。

当mark＝0和mark＝1时，l_rows_mark的计算公式如下：

l_cols_mark的计算公式如下:

当mark＝2和mark＝3时，l_rows_mark的计算公式如下：

l_cols_mark的计算公式如下:

步骤3：增量式加载路径规划。

针对没有路径的情况，根据mark标记值加载对应区域地图数据，使用启发式算法计算可行路径。具体地，方法如下：

首先按照步骤2对地图进行网格化建模，对路网进行建模，将初始mark赋值为0，再根据mark的值加载相应区域地图，并使用启发式算法进行计算。

步骤3.1：当mark＝0时，首先加载以起始点为中心，以ld_rows₀、ld_cols₀为边长的矩形范围内的地理数据。加载内容包括路径规划所需要考虑的所有地理信息(如常见的高程信息、水系信息、土质信息和道路信息等)。数据加载完成后，使用启发式路径规划算法(如A*、LPA*算法等)，计算从起始点所在栅格到终止点所在栅格的可行路径。

不同于一般的路径规划过程，在本发明中，超过当前已加载矩形范围的栅格会被认为不可通行，如果待更新栅格点超过了已加载矩形的范围将不会考虑该栅格。直到规划过程中存储的待扩展栅格点的优先队列为空时，本次搜索结束。在本发明中，从优先队列中取出栅格为扩展栅格点，扩展栅格和以扩展栅格为中心展开的八个方向的栅格统称为探索栅格点。通过步骤2中式(10)、(11)更新ld_rows₀、ld_cols₀值。如果搜索时与mark＝1区域相遇，则转步骤7。否则mark赋值为1。

步骤3.2：当mark＝1时，加载以终止点为中心，以ld_rows₁、ld_cols₁为边长的矩形范围内的地理数据。使用启发式搜索算法在矩形范围内从终止点向起始点进行路径规划。

同样不考虑超过当前已加载矩形范围的栅格。如果搜索时与mark＝0区域相遇，则转步骤7，否则mark赋值为0，以步骤2中式(10)、(11)更新ld_rows₁、ld_cols₁值，进行下一次搜索。

步骤3.3：当mark＝0、1区域有障碍物，全部搜索完毕未相遇时，令mark＝2，以ld_rows₂、ld_cols₂为边长，以mark＝0和mark＝1矩形区域相交行列处为边界，加载mark＝2区域地理信息，从开始栅格点继续向终止栅格点方向探索。如果搜索时与mark＝1区域相遇，则转步骤7。否则mark赋值为3，以步骤2中式(12)、(13)更新ld_rows₂、ld_cols₂值，进行下一次搜索。

步骤3.4：当mark＝3时，以mark＝0、mark＝1区域相交行列处为边界，以ld_rows₃、ld_cols₃为边长，与终止点在相交列同侧。从开始栅格点继续向终止栅格点方向搜索。如果搜索时与mark＝1区域相遇，则转步骤7。否则mark赋值为2，以步骤2中式(12)、(13)更新ld_rows₃、ld_cols₃值。进行下一次搜索。

若所有网格全部搜索完毕仍然没有相遇，则返回起止点之间没有可行路径。

由此可见，本发明通过在不同mark区域中根据加载后规划的情况进行区域切换，这样每次只加载一小块区域数据，避免了一次性加载大量数据导致机器空间不足或因为计算过程中加载大量数据导致模型计算速度过慢的问题。

步骤4：针对野外与路网并存的路径规划。

由于路网上行驶车辆的速度、安全性都较野外区域更高，因此，设车辆在搜索过程中遇到路网的情况选择上路行驶。

当没有遇到上路点时，采用步骤3中纯野外路径规划。当规划遇到路网时，当在mark＝0区域搜索中遇到上路点A，冻结该区域，在mark＝1区域搜索，如果同样遇到上路点B，则在路网拓扑上进行从A到B的路径规划。如果路网规划失败(A到B之间没有连通的路网)，则找到距离目标点最近的下路点，以下路点为基准重新开始野外路径规划。

步骤5：针对规划中路径对称性问题，在LPA*算法中引入“代价倾向值”，用于打破路径的对称性，减少冗余栅格的探索，提升规划效率。在加载区域使用增量式搜索算法Lifelong Planning A*(LPA*)，重用先前增量式加载过程中的信息，避免在新区域加载时需要重新从开始栅格点计算路径，提升规划效率。

同时，在LPA*算法中引入“代价倾向值”，用于打破路径的对称性，减少冗余栅格的探索，进一步提升规划效率，使用本发明代价倾向值能够减少对对称路径的搜索，打破搜索路径的对称性，提高效率。

具体地，包括以下步骤：

步骤5.1：用g^*(s)表示开始距离，即结点从s_start到s的实际最短路径。一旦知道所有点的开始距离，就能够通过简单的递减开始距离，追溯出一条从起始点到终止点的最短路径(类似于A*算法中从目标点到起始点通过迭代父节点追溯最短距离)。如果某些点的代价发生改变，只计算被该点改变代价受到影响的g^*(s)，没有被影响的点的开始距离不需要重新计算。

其中，

pred(s)表示顶点s在图中的所有前驱点集；c(s',s)表示从s'到s的代价值。

LPA*会计算A*算法引入的g值(搜索过程中s_start到s的预计最短距离，可看作对g^*(s)的估计值)，所有顶点的g值满足：

以外，LPA*还会计算第二种对开始距离g*(s)的估计：rhs(s)，它满足以下关系：

如果一个点的g值与它的rhs值相同，称为局部一致。如果所有的顶点的g值满足局部一致，则回溯到一条从起始点s_start到任意点u的最短路径。

步骤5.2：打破路径对称性，进一步减少冗余搜索。在车辆路径规划时启发函数h(s)通常取为结点与目标点的距离，在原有启发值h(s)的基础上加入了直线轨迹偏离评估值sh，sh的值受当前点s到起始点到终点s_end的直线距离的影响，在启发值h上加入sh，使得搜索过程中更倾向于选择起始点到终点的直线附近的点，减少对对称路径的搜索，从而提高搜索效率。

启发函数h(s)的计算方式如下：

cross＝|(s.x-s_end.x)*(s_start.y-s_end.y)-(s_start.x-s_end.x)*(s.y-s_end.y)|(17)

sh＝d*tri (21)

h＝h_old+sh (22)

其中，x、y分别表示点s的横坐标、纵坐标；cross是起始点到目标点的向量

当前点到目标向量

间的叉积，d表示当前点s到起始点与终止点所在直线的垂直距离，tri是当前点到起始点与终止点的距离之和减去起始点到终止点的距离，点s距离起始点到终点连接的直线越近则该值越小，h_old是根据结点与终止点距离得出启发值，sh由d与tri计算得出，为直线轨迹偏离评估值。|a|表示起始点与终止点的欧式距离。

在添加了直线轨迹偏离值sh后，能够使得在优先级队列中更加靠近起始点和终止点中间连线的点具有更高的优先级，从而实现打破对称路径的目标，减少对对称路径的冗余搜索，进一步缩小算法需要搜索的范围，提高了算法效率。

步骤6：针对野外与路网并存复杂地图下的路网断裂情况。在实际应用中由于地图数量较差、有禁止通行区域等情况经常会导致两个上路点间路网规划失败，本发明的解决方案如下。

步骤6.1：判断路网是否断裂。首先按步骤2至5分别由起止点进行野外路径规划，寻找上路点。令靠近起始点和终止点的上路点分别为a₁、b₁，之后在a₁、b₁之间进行路网规划。

在将路网数据转换成的邻接表结构上使用A*算法，因为使用欧几里得距离不适合用来估计路网规划阶段的启发值，因此，本发明在路网上顶点到终止点的距离估计采用SPFA-BR(Shortest Path Faster Algorithm-Broken Road)算法来计算，通过在经典的SPFA(Shortest Path Faster Algorithm)算法基础上加入判断路网断裂功能以及对最佳下路点搜索功能。点到点的距离采用地图中路网图层道路线段距离的真实值，当对a₁、b₁进行路网规划失败时意味着出现断路，转入断路处理。

步骤6.2：寻找下路点，终止点侧的下路点选择原则为路网规划中距离起始点最近的点b₂作为下路点。起始点侧的下路点选择原则为路网规划中距离点b₂最近的点a₂作为下路点。

步骤6.3：得出断裂路网规划结果。保留起始点到点a₂的路径规划结果作为path₁，点b₂到终止点的路径规划结果作为path₂，对a₂、b₂之间使用A*算法在野外区域规划出路径path₃。最终规划结果path＝path₁∪path₂∪path₃。

步骤7：将双向规划的路径点整理成最终的路径，并输出规划结果。

有益效果

本发明方法，对比现有技术，具有以下优点：

1.本发明在加载地图数据时采用增量式方法，避免了一次性将所有地图信息加载至内存空间，大幅减少了地理信息处理时间。在加载地理信息后的路径规划阶段采用启发式搜索算法进行路径的规划，有效缩短规划时间，显著提高了搜索效率。实验证明，本方法能够有效缩短规划时间。

2.本方法对由于地图数据缺失或任务规划需要造成的路网断裂等问题，提出了有效的解决方案，能够判断路网是否存在断裂，如果存在断裂，对于从起点开始的路网选择距离终点最近的点作为下路点，而对于从终点开始反向规划的路网则选择距离起点最近的点作为下路点。最终保证在路网存在断裂的情况下，也能够规划出可行的路径。

附图说明

图1为本发明方法的栅格化地理信息建模示意图；

图2为具体实施实例中本发明方法的流程图；

图3为具体实施实例中本发明方法首次加载mark＝0区域地图信息示意图；

图4为具体实施实例中本发明方法首次加载mark＝1区域后示意图；

图5为具体实施实例中地图增量式加载路径规划成功示意图；

图6为具体实施实例中mark＝2与mark＝3区域地图信息加载示意图；

图7为具体实施实例中增量式加载mark＝2与mark＝3区域后路径规划成功示意图；

图8为具体实施实例中遇到路网情况示意图。

具体实施方式

下面结合附图和实施例，对本发明方法做进一步详细说明。

实施例

本实施例叙述了本发明一种复杂环境下的车辆路径规划方法的具体实现过程，图2为本实施例的实现流程示意图。

从图2中可以看出，本发明及本实施例的具体实现步骤如下:

步骤1：在选定起始点和终止点后，以起止点为基准规确定一个矩形，并以该矩形作为基准向外扩展N米，得到将基准矩形包裹在内的规划中心矩形。对规划中心矩形进行地理信息建模。

步骤1.1：根据式(1)计算栅格化边长，将整片区域栅格化。

步骤1.2：读取并保存区域内所有路网信息。

步骤1.3：如图3所示，初始化mark＝0，读取区域内路网、水系、土质信息并保存。

步骤2：没有遇到路网情况下，增量式加载地形，进行启发式搜索。

步骤2.1：根据mark值加载对应区域的地形信息。当mark＝0(整个区域的左上部分)计算完成后，转到mark＝1(整个区域的右下部分)进行计算，如图4所示。

步骤2.2：以图5为例，起始点所在区域为mark＝0，终止点所在区域为mark＝1。对mark＝0、1区域进行对称加载搜索，使用LPA*算法(或A*算法等启发式路径规划算法)并使用式(16)(22)计算已加载点的启发式信息f＝g+h，根据启发式信息对加载区域进行规划。根据式(10)、(11)更新每次加载的网格区域，直到两个区域中出现了相遇点则路径规划成功。如图5所示。图中白色网格为未加载网格，浅灰色网格代表规划中探索过的栅格，黑色网格代表启发式搜索中未探索栅格，白色虚线连接代表规划出的路径。

步骤2.3：遇到障碍物情况，以图6为例，其中起始点所在区域为mark＝0，终止点所在区域为mark＝1，起始点同侧的区域为mark＝2(图6中为左下区域网格)，终止点同侧的区域为mark＝3(图6中为右上区域网格)。当mark＝0、1区域加载完成却没有相遇时(如图6中因为标识为不可通行的黑色网格区域阻挡)，对mark＝2、3区域进行对称加载搜索，根据式(12)、(13)更新每次加载的网格区域，直到两个区域中出现能将起止点区域连接起来的点则路径规划成功。如图7所示。

步骤3：遇到上路点时则转为路网规划。

步骤3.1：如图8所示。在mark＝0区域进行搜索时遇到上路点P1，则冻结mark＝0区域，不断在mark＝1矩形区域加载地理信息并进行搜索，直到遇到同样存在公路的栅格P3，停止mark＝1区域的搜索。使用A*算法在路网拓扑上进行在P1与P3之间的路径规划，最后在起始点到P1的路径、P1与P3之间的路网路径、P3到终止点的路径共同构成一条从起始点到终止点的可通行路径。

步骤3.2：当路网规划失败时，即步骤3.1中上路点P1与P3之间没有连通的路网时，则根据P1与P3分别寻找合适的下路点Q1和Q3，再以Q1和Q3分别作为起止点使用步骤2中野外路径规划算法进行纯野外路规划，最终起始点到点Q1的路径规划结果作为path₁，点Q3到终止点的路径规划结果作为path₂，对Q1、Q3之间在野外区域规划出路径path₃。最终规划结果path＝path₁∪path₂∪path₃。

按照步骤1到步骤3，即可完成复杂环境下增量式加载与搜索规划车辆路径，通过本发明中的方法，减少了需要加载地形的网格数，大幅缩短了规划时间，通过打破路径对称性的方法减少对称道路的搜索也进一步提升了规划效率。此外，使用本发明提出的断路解决方法，也能够成功解决路径规划中常见的断路问题。

Claims (3)

1.一种复杂环境下的车辆路径规划方法，其特征在于，包括以下步骤：

步骤1：选定路径起始点和终止点；以起止点为基准规划出一个矩形，以该矩形作为基准向外扩展，得到将基准矩形包裹在内的规划中心矩形；然后，对规划中心矩形进行地理信息建模；

步骤2：采用增量式加载路径规划算法，提高车辆路径规划效率；

在确定规划中心矩形，也就是整个规划区域后，将规划中心矩形划分为sp份，将加载区域标识定义为mark，每个区域内的已规划行列数分别为ld_rows_mark、ld_cols_mark，并分别初始化为0；将增量式加载地图在每次加载地图回合中增加的行、列数值分别定义为rows、cols；

其中，rows和cols的计算方式如下：

rows＝[ROWS/sp]

cols＝[COLS/sp]

其中，ROWS为规划中心矩形总行数，COLS为规划中心矩形的总列数，[]为向上取整符号；

在每次的增量式加载新回合中，每个区域的行、列数为ld_rows_mark、ld_cols_mark，mark指的是每次加载的地图在规划中心矩形的区域标识；mark取值0、1、2、3，mark＝0代表起点所在的规划中心矩形区域，mark＝1代表目的地所在的矩形区域，mark＝2代表与起点同左侧或右侧的矩形区域，mark＝3即为与终点同右侧或左侧的区域；

如果总行数减去已经加载的行数或列数，小于每次的基础增量值rows或cols，则本次的规划区域矩形行数/列数增加值等于总行数/列数减去已经加载的行数/列数的值，否则仍按照rows或cols增加；

当mark＝0和mark＝1时，l_rows_mark的计算公式如下：

l_cols_mark的计算公式如下:

当mark＝2和mark＝3时，l_rows_mark的计算公式如下：

l_cols_mark的计算公式如下:

步骤3：增量式加载路径规划；

针对没有路径的情况，根据mark标记值加载对应区域地图数据，使用启发式算法计算可行路径：

首先按照步骤2对地图进行网格化建模，对路网进行建模，将初始mark赋值为0，再根据mark的值加载相应区域地图，并使用启发式算法进行计算；

步骤3.1：当mark＝0时，首先加载以起始点为中心，以ld_rows₀、ld_cols₀为边长的矩形范围内的地理数据；加载内容包括路径规划所需要考虑的所有地理信息；数据加载完成后，使用启发式路径规划算法，计算从起始点所在栅格到终止点所在栅格的可行路径；

超过当前已加载矩形范围的栅格会被认为不可通行，如果待更新栅格点超过了已加载矩形的范围将不会考虑该栅格，直到规划过程中存储的待扩展栅格点的优先队列为空时，本次搜索结束；

从优先队列中取出栅格为扩展栅格点，扩展栅格和以扩展栅格为中心展开的八个方向的栅格统称为探索栅格点；通过步骤2方法更新ld_rows₀、ld_cols₀值；如果搜索时与mark＝1区域相遇，则转步骤7；否则mark赋值为1；

步骤3.2：当mark＝1时，加载以终止点为中心，以ld_rows₁、ld_cols₁为边长的矩形范围内的地理数据；使用启发式搜索算法在矩形范围内从终止点向起始点进行路径规划；

同样不考虑超过当前已加载矩形范围的栅格；如果搜索时与mark＝0区域相遇，则转步骤7，否则mark赋值为0，根据步骤2中方法更新ld_rows₁、ld_cols₁值，进行下一次搜索；

步骤3.3：当mark＝0、1区域有障碍物，全部搜索完毕未相遇时，令mark＝2，以ld_rows₂、ld_cols₂为边长，以mark＝0和mark＝1矩形区域相交行列处为边界，加载mark＝2区域地理信息，从开始栅格点继续向终止栅格点方向探索；如果搜索时与mark＝1区域相遇，则转步骤7；否则mark赋值为3，根据步骤2中方法更新ld_rows₂、ld_cols₂值，进行下一次搜索；

步骤3.4：当mark＝3时，以mark＝0、mark＝1区域相交行列处为边界，以ld_rows₃、ld_cols₃为边长，与终止点在相交列同侧；从开始栅格点继续向终止栅格点方向搜索；如果搜索时与mark＝1区域相遇，则转步骤7；否则mark赋值为2，根据步骤2中方法更新ld_rows₃、ld_cols₃值；进行下一次搜索；

若所有网格全部搜索完毕仍然没有相遇，则返回起止点之间没有可行路径；

步骤4：针对野外与路网并存的路径规划；

由于路网上行驶车辆的速度、安全性都较野外区域更高，因此，设车辆在搜索过程中遇到路网的情况选择上路行驶；

当没有遇到上路点时，采用步骤3中纯野外路径规划；当规划遇到路网时，当在mark＝0区域搜索中遇到上路点A，冻结该区域，在mark＝1区域搜索，如果同样遇到上路点B，则在路网拓扑上进行从A到B的路径规划；如果路网规划失败，即A到B之间没有连通的路网，则找到距离目标点最近的下路点，以下路点为基准重新开始野外路径规划；

步骤5：针对规划中路径对称性问题，在加载区域使用增量式搜索算法LPA*，在LPA*算法中引入“代价倾向值”，重用先前增量式加载过程中的信息，避免在新区域加载时需要重新从开始栅格点计算路径，提升规划效率；

步骤6：针对野外与路网并存复杂地图下的路网断裂情况：

步骤6.1：判断路网是否断裂；首先按步骤2至5分别由起止点进行野外路径规划，寻找上路点；令靠近起始点和终止点的上路点分别为a₁、b₁，之后在a₁、b₁之间进行路网规划；

在将路网数据转换成的邻接表结构上使用A*算法，在路网上顶点到终止点的距离估计采用SPFA-BR算法来计算，通过SPFA算法基础上加入判断路网断裂功能以及对最佳下路点搜索功能；点到点的距离采用地图中路网图层道路线段距离的真实值，当对a₁、b₁进行路网规划失败时意味着出现断路，转入断路处理；

步骤6.2：寻找下路点，终止点侧的下路点选择原则为路网规划中距离起始点最近的点b₂作为下路点；起始点侧的下路点选择原则为路网规划中距离点b₂最近的点a₂作为下路点；

步骤6.3：得出断裂路网规划结果；保留起始点到点a₂的路径规划结果作为path₁，点b₂到终止点的路径规划结果作为path₂，对a₂、b₂之间使用A*算法在野外区域规划出路径path₃；最终规划结果path＝path₁∪path₂∪path₃；

步骤7：将双向规划的路径点整理成最终的路径，并输出规划结果。

2.如权利要求1所述的一种复杂环境下的车辆路径规划方法，其特征在于，步骤1中，对规划中心矩形进行地理信息建模，包括以下步骤：

步骤1.1：对规划区域进行地理信息建模；

采用栅格化处理方式，将地图根据设置的栅格边长划分为若干小栅格；其中，单个栅格边长l_grid通过下式计算：

其中，l_grid表示单个栅格边长，S_i表示区域内部的多边形面积，n表示区域内的多边形个数；

地图的地理信息数据书由多个矢量图层构成，每个图层分别存储区域内的某类特征地理信息集合；对地理信息数据划分栅格，对栅格化后的规划区域通过遍历栅格，在遍历时将行列数转换成高斯坐标，借助地理系统引擎获得所需图层数据；

步骤1.2：土质图层信息处理；

对于土质图层的信息，在路径规划时参考车辆在不同土质上的平均行驶速度，以车辆在平沙地的行驶速度为基准，在建模阶段设置栅格u的土质速度比例u_ratio，由下式计算得出：

其中，v_soil为当前网格土质所对应的车辆行驶速度，v_Flatsand是土质为平沙地时的车辆行驶速度；

步骤1.3：拓扑路网建模；

采用图论中的有向图结构存储路网进行建模，生成符合规划需求的有向图；将地图道路矢量图层中的数据构建为拓扑模型，表示为如下式：

G＝(V_a,E_a,W_a)

V_a＝{v_i|i∈1,2,3...,n}

W_a＝{w_ij|i,j∈1,2,3,...,n}

E_a＝{e_i|i∈1,2,3,...,n}

其中，G表示整个道路拓扑图，V_a表示路网中的道路起止点以及不同道路连接点抽象成的顶点，v_i表示第i个顶点；W_a表示弧的代价值，w_ij表示第i和j个结点间的代价值；E_a是道路抽象成为的边集，e_i表示第i条边，集合中的每个元素是一条道路，由道路连接的路口和该道路的属性构成，表示为：

e_a＝(S_id,E_id,Q_a,P_v)

其中，S_id是该道路的起始点id，E_id是该道路的终止点id；Q_a是该道路的属性信息，包括该道路的道路等级、道路的长度，P_v是道路的定位点集合；

将路网采取有向图建模后，将矢量路网数据以图的结构存储，以邻接表的形式存储路网。

3.如权利要求1所述的一种复杂环境下的车辆路径规划方法，其特征在于，步骤5包括以下步骤：

步骤5.1：用g^*(s)表示开始距离，即结点从s_start到s的实际最短路径；一旦知道所有点的开始距离，就能够通过简单的递减开始距离，追溯出一条从起始点到终止点的最短路径；如果某些点的代价发生改变，只计算被该点改变代价受到影响的g^*(s)，没有被影响的点的开始距离不需要重新计算；

其中，

pred(s)表示顶点s在图中的所有前驱点集；c(s',s)表示从s'到s的代价值；

LPA*计算A*算法引入的g值，所有顶点的g值满足：

以外，LPA*还会计算第二种对开始距离g*(s)的估计：rhs(s)，它满足以下关系：

如果一个点的g值与它的rhs值相同，称为局部一致；如果所有的顶点的g值满足局部一致，则回溯到一条从起始点s_start到任意点u的最短路径；

步骤5.2：打破路径对称性，进一步减少冗余搜索；在车辆路径规划时，启发函数h(s)取为结点与目标点的距离，在原有启发值h(s)的基础上加入直线轨迹偏离评估值sh，sh的值受当前点s到起始点到终点s_end的直线距离的影响，在启发值h上加入sh，使得搜索过程中更倾向于选择起始点到终点的直线附近的点，减少对对称路径的搜索；

启发函数h(s)的计算方式如下：

cross＝|(s.x-s_end.x)*(s_start.y-s_end.y)-(s_start.x-s_end.x)*(s.y-s_end.y)|

sh＝d*tri

h＝h_old+sh

其中，x、y分别表示点s的横坐标、纵坐标；cross是起始点到目标点的向量

当前点到目标向量

间的叉积，d表示当前点s到起始点与终止点所在直线的垂直距离，tri是当前点到起始点与终止点的距离之和减去起始点到终止点的距离，点s距离起始点到终点连接的直线越近则该值越小，h_old是根据结点与终止点距离得出启发值，sh由d与tri计算得出，为直线轨迹偏离评估值；|a|表示起始点与终止点的欧式距离。

Images