CN112985437B - 灾区场景下路径导航方法及装置 - Google Patents

Abstract

本发明涉及一种灾区场景下路径导航方法及装置，基于原始电子地图和单兵发掘轨迹，建立应急增广电子地图；基于所述应急增广电子地图，建立避让路障策略和错时抢修策略；基于所述避让路障策略和错时抢修策略，建立抢修路径规划模型和离线最短路径模型；基于所述抢修路径规划模型，计算获得最优抢修路径；基于所述离线最短路径模型，计算获得最优运输路径。本发明能够获得最优抢修路径和最优运输路径，保证运输队伍快速可靠地到达抢修物资需求点。

Description

灾区场景下路径导航方法及装置

技术领域

本发明涉及一种灾区场景下路径导航方法及装置。

背景技术

为了尽可能地减轻自然灾害带来的损失，灾后工作的高效性必须得到保证。其中，供电恢复作为关键一环，却十分依赖于电力抢修物资的送达，但自然灾害对通讯线路同样造成了极大的破坏，导致常规的通信方式难以发挥作用，进一步导致灾区现场信息无法稳定传回后方，因而无法规划出一条快速可靠的运输路径。

现有单兵系统可以深入灾区现场进行数据挖掘及采集，获得大量现场数据，并通过北斗短报文等通信方式将数据传回后方。灾区场景中，单兵系统可以在采集过程中发掘一些区别于正常道路的潜在道路，从而为路径规划提供更多的备选方案。

发明内容

本发明的发明目的在于提供灾区场景下路径导航方法及装置，能够获得最优抢修路径和最优运输路径，保证运输队伍快速可靠地到达抢修物资需求点。

基于同一发明构思，本发明具有两个独立的技术方案：

1、一种灾区场景下路径导航方法，包括如下步骤：

步骤1：基于原始电子地图和单兵发掘轨迹，建立应急增广电子地图；

步骤2：基于所述应急增广电子地图，建立避让路障策略和错时抢修策略；

步骤3：基于所述避让路障策略和错时抢修策略，建立抢修路径规划模型和离线最短路径模型；

步骤4：基于所述抢修路径规划模型，计算获得最优抢修路径；基于所述离线最短路径模型，计算获得最优运输路径。

进一步地，步骤1中，所述应急增广电子地图通过如下方法建立，

步骤1.1：基于隐马尔可夫模型建立路段匹配模型；

步骤1.2：基于所述路段匹配模型，计算得到每个单兵轨迹点的最佳匹配路段；

步骤1.3：基于所述各单兵轨迹点的最佳匹配路段结果，对单兵轨迹点进行筛选，得到单兵发掘轨迹点；

步骤1.4：基于筛选后的结果，将单兵发掘轨迹与原始电子地图拟合，建立应急增广电子地图。

进一步地，步骤1.3中，对单兵发掘轨迹点进行筛选通过如下方法实现，

步骤1.3.1：计算各轨迹点与最佳匹配路段之间的匹配距离

步骤1.3.2：设置允许匹配距离误差值F_limit、最小发掘轨迹长度L_limit；

步骤1.3.3：判断每个轨迹点Y_i是否满足

大于F_limit，若满足则将该点设置为差异点；

步骤1.3.4：判断差异点的连续性，若连续的差异点数量大于或等于最小发掘轨迹长度L_limit，则该部分差异点均为单兵发掘轨迹上的点，若不满足，则该部分差异点仅是匹配错误点，不能成为单兵发掘轨迹上的点。

进一步地，步骤1.1中，基于隐马尔可夫模型建立路段匹配模型时，单兵发掘轨迹点坐标为可观测状态，路段为隐含状态，隐含状态转移至可观测状态的概率由单兵发掘轨迹点与路段间的匹配距离决定，所述匹配距离由轨迹点距路段的垂直距离、前进方向和路段间的夹角两部分构成。

进一步地，步骤2中，所述避让路障策略为，在前往物资需求点的路径上，当遇到路障时，结合路障后方的单兵发掘轨迹和地图上的相关路段，建立避开路障的路径。

进一步地，步骤2中，所述错时抢修策略为，根据抢修队先于运输队到达目标故障的时间差、目标故障的清理时间，计算运输队通过所述目标故障点需要的等待时间，通过如下公式计算，

T_i(t_i)＝max(T_1i+T_2i-t_i,0)

式中，T_i(t_i)为运输队通过目标故障i需要的等待时间,T_1i为抢修队从抢修出发点到目标路障i的时间，T_2i为目标路障i的清理时间，所述清理时间由单兵判定的路障等级直接得到，t_i为运输队到达目标路障i的时间。

进一步地，步骤3中，抢修路径规划模型通过方法建立，

以圆形为搜索区域形状，以目标路障i为搜索中心，构造出发点搜索区域，筛选出该区域内的出发点，设为备选出发点，即，与目标路障i间的欧式距离小于搜索区域半径的出发点，得到备选出发点集合A_i，求解备选出发点k到达目标路障i的时间

选出最快到达的出发点，则该出发点到达目标路障i的路径为最优抢修路径，

抢修路径规划模型为，

p,q＝1,2,...,n,p≠q

式中，

为备选出发点k到达目标路障i的最短时间，L_2k为备选出发点到目标路障i的某条抢修路径，R_2k为L_2k的集合，E为节点集合，包括路段交叉口和路障；T_pq为节点p、q之间的路段的通行时间，由路段长度l_pq和速度v_pq决定；x_pq为路段的标记变量，若路段pq位于当前考虑路径对象L_2k中，将其设为1，否则为0；T_p为节点p的通行时间，其中路段交叉点的值为0，路障节点的值为极大数；

为目标路障i要求抢修队的最晚到达时间。

进一步地，步骤3中，离线最短路径模型通过方法建立，

基于当前运输队的出发时刻t₀，运输队到达目标点用最少时间的路径为最优运输路径，

离线最短路径模型为，

i,j＝1,2,...,n,i≠j

t₀≠0

式中，P_opt(t₀)表示出发时刻为t₀时运输队的最优运输路径，L₁为运输队到达目标点的某条运输路径，R₁为L₁的集合，O、D分别为运输路径起始点，T_ij为节点i、j之间的路段的通行时间，T_max为目标点要求运输队的最晚到达时间。

进一步地，步骤4中，基于所述抢修路径规划模型，通过遗传算法计算获得最优抢修路径；基于所述离线最短路径模型，通过遗传算法计算获得最优运输路径。

2、一种灾区场景下路径导航装置，用于执行上述的方法。。

本发明具有的有益效果：

本发明基于原始电子地图和单兵发掘轨迹，建立应急增广电子地图；基于所述应急增广电子地图，建立避让路障策略和错时抢修策略；基于所述避让路障策略和错时抢修策略，建立抢修路径规划模型和离线最短路径模型；基于所述抢修路径规划模型，计算获得最优抢修路径；基于所述离线最短路径模型，计算获得最优运输路径。本发明通过建立应急增广电子地图为路径规划提供了更多备选路径，从而保证获得最优抢修路径和最优运输路径。本发明基于避让路障策略和错时抢修策略，建立抢修路径规划模型和离线最短路径模型，其中错时抢修策略充分利用抢修队和运输队到达路障的时间差，缩短运输队在路障节点的通行时间，保证获得最优运输路径。

本发明应急增广电子地图通过如下方法建立，基于隐马尔可夫模型建立路段匹配模型；基于所述路段匹配模型，计算得到每个单兵轨迹点的最佳匹配路段；基于所述各单兵轨迹点的最佳匹配路段结果，对单兵轨迹点进行筛选，得到单兵发掘轨迹点；基于筛选后的结果，将单兵发掘轨迹与原始电子地图拟合，建立应急增广电子地图。其中，对单兵发掘轨迹点进行筛选通过如下方法实现，计算各轨迹点与最佳匹配路段之间的匹配距离

设置允许匹配距离误差值F_limit、最小发掘轨迹长度L_limit；判断每个轨迹点Y_i是否满足

大于F_limit，若满足则将该点设置为差异点；判断差异点的连续性，若连续的差异点数量大于或等于最小发掘轨迹长度L_limit，则该部分差异点均为单兵发掘轨迹上的点，若不满足，则该部分差异点仅是匹配错误点，不能成为单兵发掘轨迹上的点。本发明通过上述方法实现单兵轨迹点筛选、建立应急增广电子地图，有效保证筛选的准确度和备选路径的充分性、可靠性。

本发明提出抢修路径规划模型，考虑图论约束和时间窗约束，该模型以抢修队伍到达目标路障点的时间最短为目标，计算结果为离线最短路径模型的计算提供基础；本发明提出离线最短路径模型，基于应急增广地图，避让路障策略和错时抢修策略，以应急物资运输队伍到达物资需求点的时间最短为目标，考虑图论约束和时间窗约束，保证获得最优运输路径的准确性。

附图说明

图1是本发明方法流程框图；

图2是路段匹配模型示意图；

图3是轨迹点与路段间的垂直距离和方向夹角示意图；

图4是避让路障策略示意图；

图5是传统遗传算法流程图。

具体实施方式

下面结合附图所示的实施方式对本发明进行详细说明，但应当说明的是，这些实施方式并非对本发明的限制，本领域普通技术人员根据这些实施方式所作的功能、方法、或者结构上的等效变换或替代，均属于本发明的保护范围之内。

实施例一：

灾区场景下路径导航方法

如图1所示，灾区场景下路径导航方法，包括如下步骤：

步骤1：基于原始电子地图和单兵发掘轨迹，建立应急增广电子地图。

本发明在现有电子地图的基础上附加单兵发掘轨迹生成应急增广电子地图，其中单兵发掘轨迹的来源包括两种情况，一种是类似于内部道路等现有电子地图中没有收录的道路，另一种是本身难以构成道路，但经处理可保证应急运输车辆通过的情况，比如围墙可打通的空地、可通过车辆的农田等。这两种情况的数据都可反映于单兵系统从灾区现场采集返回的轨迹点，但需经过筛选并拟合方可得到单兵发掘轨迹。

由于定位精度、传输误差等原因，单兵返回的轨迹点与其实际位置具有一定的偏差，直接筛选轨迹点具有一定的难度。本发明将隐马尔可夫模型(Hidden Markov Model,HMM)运用于路段匹配中，其中通过隐含状态转移概率计及网络拓扑结构，并将距离和方向因素都加以考虑，得到模型的观测概率，提高了路段匹配结果的准确度。

所述应急增广电子地图通过如下方法建立，

步骤1.1：基于隐马尔可夫模型建立路段匹配模型；

单兵发掘轨迹点坐标为可观测状态，路段为隐含状态，隐含状态转移至可观测状态的概率由单兵发掘轨迹点与路段间的匹配距离决定，所述匹配距离由轨迹点距路段的垂直距离、前进方向和路段间的夹角两部分构成。

本实施例中，如图2所示，基于隐马尔可夫模型(Hidden Markov Model,HMM)，建立轨迹点的路段匹配模型，

其中，向量组{X₁,X₂,…,X_i,…,X_n-1,X_n}表示隐含状态序列集合，向量X_i＝{S₁,S₂,…,S_j,…,S_m-1,S_m}表示第i个隐含状态序列，元素S_j表示第j个隐含状态，具体为每个轨迹点可能所属的路段；向量{Y₁,Y₂,…,Y_i,…,Y_n-1,Y_n}为可观测序列，元素Y_i表示第i个可观测状态，具体为接收到的轨迹点的坐标；

表示隐含状态转移概率，即隐含状态序列X_i中的某个隐含状态

转移到隐含状态序列X_i+1中的某个隐含状态

的概率，具体为路段之间的拓扑连接关系，若两个隐含状态是同一路段或者相连的两条路段，则概率为1，否则概率为0，如式1所示；

表示可观测状态的观测概率，即隐含状态序列中Xi的某个隐含状态

转移到可观测状态Y_i的概率，受轨迹点与路段间的匹配距离影响，其中匹配距离由轨迹点距路段的垂直距离、前进方向和路段间的夹角两部分构成，如式2-式4所示：

其中，

表示第i个轨迹点距离第j条可能所属的路段的垂直距离；

表示第i个轨迹点与第i+1个轨迹点之间的连线与第j条可能所属路段间的夹角，如图3所示；α、β分别表示垂直距离与方向夹角占匹配距离的权重；

为轨迹点i与路段j之间的匹配距离；m_i为第i个轨迹点可能所属的路段数量。

步骤1.2：基于所述路段匹配模型，计算得到每个单兵轨迹点的最佳匹配路段；

路段匹配模型中已知可观测序列和两种类型的概率，未知隐含状态序列，属于典型的预测问题，可基于动态规划思想，利用维特比(Viterbi)算法即可对其进行求解，得到每个轨迹点的最佳匹配路段。

步骤1.3：基于所述各单兵轨迹点的最佳匹配路段结果，对单兵轨迹点进行筛选，得到单兵发掘轨迹点；

步骤1.3.1：计算各轨迹点与最佳匹配路段之间的匹配距离

步骤1.3.2：设置允许匹配距离误差值F_limit、最小发掘轨迹长度L_limit；

步骤1.3.3：判断每个轨迹点Y_i是否满足

大于F_limit，若满足则将该点设置为差异点；

步骤1.3.4：判断差异点的连续性，若连续的差异点数量大于或等于最小发掘轨迹长度L_limit，则该部分差异点均为单兵发掘轨迹上的点，若不满足，则该部分差异点仅是匹配错误点，不能成为单兵发掘轨迹上的点。

步骤1.4：基于筛选后的结果，将单兵发掘轨迹与原始电子地图拟合，建立应急增广电子地图。对单兵发掘轨迹点进行筛选通过如下方法实现。

先将单兵发掘轨迹按时间顺序进行连接，其次将两端的连接点垂直投影到最佳匹配路段上，最后将发掘轨迹与连接点相连即可形成应急增广电子地图。

步骤2：基于所述应急增广电子地图，建立避让路障策略和错时抢修策略。

本发明提出了两种策略，使得规划得到的最优路径更加满足灾区场景要求。在此之前，需要确定路障的位置和等级信息并将其设置为节点，其中在轨迹点筛选时可得到其最佳匹配路段，通过垂直投影的方法可以获得路障位于最佳匹配路段中的位置，至于等级信息，路障点作为一种特殊的轨迹点，单兵系统在进行数据采集时会依据丰富的工作经验给出具体等级，并随其他信息一起传回后方。

所述避让路障策略为，在前往物资需求点的路径上，当遇到路障时，结合路障后方的单兵发掘轨迹和地图上的相关路段，建立避开路障的路径。

如图4所示，节点4为路障点，通过避让的策略生成了路径1-2-3-6-8-9。其中为尽快抵达物资需求点，发掘轨迹的通行速度一般设置为轨迹上允许的最高时速，考虑到发掘轨迹的来源多样，难以统一速度，所以速度由单兵系统依据工作经验给出。实现该策略仅需在形成备选路径集时考虑发掘轨迹即可，在进行最优路径选择时会通过比较原路径通行时间和避让路径通行时间来自动决定是否采用该策略，因为发掘轨迹的来源比较特殊，通行速度较正常通行道路慢，单兵系统仅会在路障前去发掘潜在道路，所以不影响正常道路上的行驶情况。

所述错时抢修策略为，根据抢修队先于运输队到达目标故障的时间差、目标故障的清理时间，计算运输队通过所述目标故障点需要的等待时间，通过如下公式计算，

T_i(t_i)＝max(T_1i+T_2i-t_i,0)

式中，T_i(t_i)为运输队通过目标故障i需要的等待时间,T_1i为抢修队从抢修出发点到目标路障i的时间，T_2i为目标路障i的清理时间，所述清理时间由单兵判定的路障等级直接得到，t_i为运输队到达目标路障i的时间。

本发明错时抢修策略给出了两个引入时间量的来源，一个是抢修队从抢修出发点到达目标路障i的时间T_1i，该时间量可通过抢修路径规划得到，另一个是目标路障i的清理时间T_2i，该时间量可由单兵系统判定的路障等级直接得到，因为路障等级判定过程是基于单兵系统在出发前已熟知路障等级判定标准，其中判定标准是基于路障的清理时间设置等级，所以清理时间可由等级直接换算得到。除此之外，本发明在抢修路径和运输路径规划中均有效利用单兵系统的发掘轨迹避让路障，能够尽可能缩短抢修队到达目标路障时间和运输路径通行时间。本发明考虑到运输队和抢修队于不同时刻出发，并且考虑到存在多个具备抢修能力的抢修出发节点，使得方法更贴合实际需求。

步骤3：基于所述避让路障策略和错时抢修策略，建立抢修路径规划模型和离线最短路径模型。

抢修路径规划不同于运输路径规划，该规划中仅考虑避让策略，不考虑错时抢修策略，即前往目标路障的过程中抢修队不会抢修其他路障，并通过设置路障节点的通行时间为极大数来避开路障。因为这样几乎不存在缩短抢修路径通行时间的可能，还会严重影响目标路障的抢修进度，不能满足灾区场景中快速响应命令的要求。考虑到具备抢修能力的节点不止一个，抢修路径规划模型通过方法建立，

以圆形为搜索区域形状，以目标路障i为搜索中心，构造出发点搜索区域，筛选出该区域内的出发点，设为备选出发点，即，与目标路障i间的欧式距离小于搜索区域半径的出发点，得到备选出发点集合A_i，求解备选出发点k到达目标路障i的时间

选出最快到达的出发点，则该出发点到达目标路障i的路径为最优抢修路径，

抢修路径规划模型为，

p,q＝1,2,...,n,p≠q (8)

式中，

为备选出发点k到达目标路障i的最短时间，L_2k为备选出发点到目标路障i的某条抢修路径，R_2k为L_2k的集合，E为节点集合，包括路段交叉口和路障；T_pq为节点p、q之间的路段的通行时间，由路段长度l_pq和速度v_pq决定；x_pq为路段的标记变量，若路段pq位于当前考虑路径对象L_2k中，将其设为1，否则为0；T_p为节点p的通行时间，其中路段交叉点的值为0，路障节点的值为极大数；

为目标路障i要求抢修队的最晚到达时间。

在抢修路径规划模型基础上，离线最短路径模型通过方法建立，

基于当前运输队的出发时刻t₀，运输队到达目标点用最少时间的路径为最优运输路径，

离线最短路径模型为，

i,j＝1,2,...,n,i≠j (13)

t₀≠0 (14)

式中，P_opt(t₀)表示出发时刻为t₀时运输队的最优运输路径，L₁为运输队到达目标点的某条运输路径，R₁为L₁的集合，O、D分别为运输路径起始点，T_ij为节点i、j之间的路段的通行时间，T_max为目标点要求运输队的最晚到达时间。

式(14)是考虑到运输队和抢修队出发时刻不同，导致t₀不为0。其他变量和约束的设置类同于上述抢修路径规划模型。

步骤4：基于所述抢修路径规划模型，计算获得最优抢修路径；基于所述离线最短路径模型，计算获得最优运输路径。

基于所述抢修路径规划模型，通过传统遗传算法计算获得最优抢修路径；基于所述离线最短路径模型，通过传统遗传算法计算获得最优运输路径。传统遗传算法流程如图5所示。其中染色体由表示路径的正整数序列构成，基因为网络节点编号，适应度函数均设置为路段通行时间和节点通行时间之和。

实施例二：一种灾区场景下路径导航装置

用于执行上述述的方法。

上文所列出的一系列的详细说明仅仅是针对本发明的可行性实施方式的具体说明，它们并非用以限制本发明的保护范围，凡未脱离本发明技艺精神所作的等效实施方式或变更均应包含在本发明的保护范围之内。

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。

Claims (3)

1.一种灾区场景下路径导航方法，其特征在于，包括如下步骤：

步骤1：基于原始电子地图和单兵发掘轨迹，建立应急增广电子地图；

步骤2：基于所述应急增广电子地图，建立避让路障策略和错时抢修策略；

步骤3：基于所述避让路障策略和错时抢修策略，建立抢修路径规划模型和离线最短路径模型；

步骤4：基于所述抢修路径规划模型，计算获得最优抢修路径；基于所述离线最短路径模型，计算获得最优运输路径；

步骤1中，所述应急增广电子地图通过如下方法建立，

步骤1.1：基于隐马尔可夫模型建立路段匹配模型；

步骤1.2：基于所述路段匹配模型，计算得到每个单兵轨迹点的最佳匹配路段；

步骤1.3：基于所述各单兵轨迹点的最佳匹配路段结果，对单兵轨迹点进行筛选，得到单兵发掘轨迹点；

步骤1.4：基于筛选后的结果，将单兵发掘轨迹与原始电子地图拟合，建立应急增广电子地图；

步骤1.3中，对单兵发掘轨迹点进行筛选通过如下方法实现，

步骤1.3.1：计算各轨迹点与最佳匹配路段之间的匹配距离

步骤1.3.2：设置允许匹配距离误差值F_limit、最小发掘轨迹长度L_limit；

步骤1.3.3：判断每个轨迹点Y_i是否满足

大于F_limit，若满足则将该点设置为差异点；

步骤1.3.4：判断差异点的连续性，若连续的差异点数量大于或等于最小发掘轨迹长度L_limit，则该部分差异点均为单兵发掘轨迹上的点，若不满足，则该部分差异点仅是匹配错误点，不能成为单兵发掘轨迹上的点；

步骤2中，所述避让路障策略为，在前往物资需求点的路径上，当遇到路障时，结合路障后方的单兵发掘轨迹和地图上的相关路段，建立避开路障的路径；

步骤2中，所述错时抢修策略为，根据抢修队先于运输队到达目标故障的时间差、目标故障的清理时间，计算运输队通过所述目标故障点需要的等待时间，通过如下公式计算，

T_i(t_i)＝max(T_1i+T_2i-t_i,0)

式中，T_i(t_i)为运输队通过目标故障i需要的等待时间，T_1i为抢修队从抢修出发点到目标路障i的时间，T_2i为目标路障i的清理时间，所述清理时间由单兵判定的路障等级直接得到，t_i为运输队到达目标路障i的时间；

步骤3中，抢修路径规划模型通过方法建立，

以圆形为搜索区域形状，以目标路障i为搜索中心，构造出发点搜索区域，筛选出该区域内的出发点，设为备选出发点，即，与目标路障i间的欧式距离小于搜索区域半径的出发点，得到备选出发点集合A_i，求解备选出发点k到达目标路障i的时间

选出最快到达的出发点，则该出发点到达目标路障i的路径为最优抢修路径，

抢修路径规划模型为，

p,q＝1,2,...,n,p≠q

式中，

为备选出发点k到达目标路障i的最短时间，L_2k为备选出发点到目标路障i的某条抢修路径，R_2k为L_2k的集合，E为节点集合，包括路段交叉口和路障；T_pq为节点p、q之间的路段的通行时间，由路段长度l_pq和速度v_pq决定；x_pq为路段的标记变量，若路段pq位于当前考虑路径对象L_2k中，将其设为1，否则为0；T_p为节点p的通行时间，其中路段交叉点的值为0，路障节点的值为极大数；

为目标路障i要求抢修队的最晚到达时间；

步骤3中，离线最短路径模型通过方法建立，

基于当前运输队的出发时刻t₀，运输队到达目标点用最少时间的路径为最优运输路径，

离线最短路径模型为，

i,j＝1,2,...,n,i≠j

t₀≠0

式中，P_opt(t₀)表示出发时刻为t₀时运输队的最优运输路径，L₁为运输队到达目标点的某条运输路径，R₁为L₁的集合，O、D分别为运输路径起始点，T_ij为节点i、j之间的路段的通行时间，T_max为目标点要求运输队的最晚到达时间；

步骤4中，基于所述抢修路径规划模型，通过遗传算法计算获得最优抢修路径；基于所述离线最短路径模型，通过遗传算法计算获得最优运输路径。

2.根据权利要求1所述的灾区场景下路径导航方法，其特征在于，步骤1.1中，基于隐马尔可夫模型建立路段匹配模型时，单兵发掘轨迹点坐标为可观测状态，路段为隐含状态，隐含状态转移至可观测状态的概率由单兵发掘轨迹点与路段间的匹配距离决定，所述匹配距离由轨迹点距路段的垂直距离、前进方向和路段间的夹角两部分构成。

3.一种灾区场景下路径导航装置，其特征在于，用于执行权利要求1或2所述的方法。

Images