CN102645228A - 一种车载导航系统通过性寻路算法 - Google Patents

Abstract

一种车载导航系统通过性寻路算法，其特征在于，它采用通过性路网模型，将路径的起点和终点分设在两个路网中，两路网只在通过区域范围内才能相互连通，从而确保由起点到达终点的寻路过程中一定经过指定的通过区域；采用通过区域半径调节方案，在起点和终点距离较远时设置较大的通过区域半径，路网密度较小时设置较大的通过区域半径，从而确保规划路径有效经过控制点附近，但并不一定经过控制点；采用虚终点寻路通过设置虚终点并利用A*算法引导路径通过指定区域，采用变方向寻路通过更改寻路引导代价引导路径从指定区域向终点规划，从而确保结果的准确性和可靠性。本发明规划出的路线既能满足用户意图经过通过区域又能满足代价最低准则，可以用于各种嵌入式车载导航系统。

Description

一种车载导航系统通过性寻路算法

技术领域

本发明涉及一种最优路径规划方法，特别是关于一种基于用户意图通过某个指定区域的车载导航系统通过性寻路算法。

背景技术

最优路径规划可以在如代价最低、路段规避、通过特定区域等多种准则下进行。现有的路径规划算法大多是系统基于代价(距离、时间、花费等)最低准则进行从起点到终点的寻路，为用户选择一条代价最优路线。然而每个人都希望有参与选择的权利，车载导航系统的用户往往希望能够根据自己的需求选择一条合适的道路。例如一些用户希望经过某个特定区域，但又不确定是否要通过某一特定点。诸如此类的问题都属于用户自定义地理准则下的系统模糊最优化寻路问题，目前为止还没有一个成熟的算法能够有效地解决这类问题。

另一方面，目前由用户参与的最优路径规划算法大多以K最短路径算法为核心。这种K最短路径算法是系统在路网中搜索多条最优路径推荐给用户，由用户在其中选择最合适的路径。当K数值较大时，K最短路径算法的路径计算效率会降低很多，而且由于这种方法对机器的硬件性能要求较高，导致当前硬件配置较弱的嵌入式设备在实际执行K最短路径算法时会有一定的困难。

发明内容

针对上述问题，本发明的目的是提供一种能够基于用户意图通过某个指定区域的车载导航系统通过性寻路算法。该算法根据用户模糊指定的一个控制点，自动规划一条经过该控制点附近区域的最优路径，使规划出的路径既能满足用户意图通过指定区域又能满足代价最低准则，使用户以较快的方式到达目的地。

为实现上述目的，本发明采取以下技术方案：1、一种车载导航系统通过性寻路算法，它包括以下步骤：

1)设置一包括有一起点路网、一终点路网和一隔离面的通过性路网模型；所述起点路网和终点路网分别设置一规划路径的起点和终点，除所述起点和终点以及它们的邻接路段以外，所述起点路网和终点路网内的元素相同，均设置有若干节点和一半径为r的圆形通过区域，所述通过区域的圆心为用户指定的控制点；所述起点路网和终点路网通过所述隔离面隔离，仅通过所述通过区域内的节点相互连通；

2)在由步骤1)建立的通过性路网模型中分两步：虚终点寻路和变方向寻路进行通过性寻路：

①在起点路网中设置一虚终点，以起点路网中节点到虚终点的代价为引导代价，利用A*算法引导路径向通过区域规划拓展，在路径拓展区域完全覆盖通过区域后，停止虚终点寻路；

②以终点路网中通过区域内经过步骤①拓展到的节点到终点的代价为引导代价，利用A*算法引导路径向终点规划拓展，在到达终点后停止变方向寻路；

3)回溯步骤2)的寻路路径，并将回溯结果确定为经过通过区域的最优路径。

上述步骤2)的虚终点寻路和变方向寻路包括以下步骤：

2.1)在起点路网中起点与控制点的连线的延长线上，于靠近控制点的一端设置一虚终点，使控制点与虚终点之间的距离等于控制点与终点之间的距离；设置一优先队列Q，将其初始化仅含规划路径的起点；将优先队列Q外的所有节点初始化为未拓展节点，将所有未拓展节点到起点的已有代价U_g初始化为∞，将路径拓展区域完全覆盖通过区域的标志F_p初始化为0，将一计数器N_Q初始化为0；

2.2)从优先队列Q中取出当前总代价U_f＝U_g+U_h最小的节点V，U_h为节点到虚终点的启发代价；

2.3)判断节点V是否在通过区域内：

若在通过区域内，计数器N_Q减1，进入步骤2.9)；

若未在通过区域内，进入步骤2.4)；

2.4)判断节点V的相邻节点是否均已访问到：

若均已访问到，将节点V标记为已拓展节点，返回步骤2.2)；

若尚未全部访问到，进入步骤2.5)；

2.5)访问节点V的一尚未访问到的相邻节点U，判断节点U的拓展状态：

①若节点U为未拓展节点，计算节点U到起点的已有代价U_g和到虚终点的启发代价U_h，将节点U加入优先队列Q变为在拓展节点，进入步骤2.6)；

②若节点U为在拓展节点，计算节点U到起点的已有代价U_g′，进入步骤2.7)；

③若节点U为已拓展节点，返回步骤2.4)；

2.6)判断节点U是否在通过区域内：

若在通过区域内，计数器N_Q加1，将节点U加入复活队列R，进入步骤2.8)；

若未在通过区域内，返回步骤2.4)；

2.7)判断U_g′＜U_g是否成立：

若成立，更新优先队列Q中节点U到起点的已有代价U_g＝U_g′，返回步骤2.4)；

若未成立，直接返回步骤2.4)；

2.8)比较N_Q与预先设定的复活节点个数阈值N^t：

若N_Q≥N^t，F_p＝1，进入步骤2.9)；

若N_Q＜N^t，返回步骤2.4)；

2.9)判断N_Q＝0且F_p＝1是否成立：

若成立，结束虚终点寻路，进入步骤2.10)；

若未成立，返回步骤2.4)；

2.10)初始化优先队列Q＝R，将优先队列Q中所有节点的启发代价U_h更改为节点到终点的启发代价，并将优先队列Q外的所有节点初始化为未拓展节点；

2.11)从优先队列Q中取出当前总代价U_f＝U_g+U_h最小的节点V；

2.12)判断节点V是否为终点：

若节点V非终点，进入步骤2.13)；

若节点V为终点，进入步骤2.16)；

2.13)判断节点V的相邻节点是否均已访问到：

若均已拓展到，将节点V变为已拓展节点，返回步骤2.11)；

若未全部拓展到，进入步骤2.14)；

2.14)访问节点V的一尚未访问到的相邻节点U，判断节点U的拓展状态：

①若节点U为未拓展节点，计算节点U到起点的已有代价U_g和到终点的启发代价U_h，将节点U加入优先队列Q变为在拓展节点，返回步骤2.13)；

②若节点U为在拓展节点，计算节点U到起点的已有代价U_g′，进入步骤2.15)；

③若节点U为已拓展节点，返回步骤2.13)；

2.15)判断U_g′＜U_g是否成立：

若成立，更新优先队列Q中节点U到起点的已有代价U_g＝U_g′，返回步骤2.13)；

若未成立，直接返回步骤2.13)；

2.16)结束变方向寻路。

上述步骤2.8)中，复活节点个数阈值N^t设定为5。

上述步骤1)中，通过区域半径r为r＝min(r_l，r_d)，r_l为距离半径，r_d为密度半径；所述距离半径r_l根据起点或终点到控制点的距离确定，其计算表达式如下：

r_l＝min(α*l_ST，β*min(l_SM，l_MT))，

上式中，l_ST、l_SM、l_MT分别为起点到终点、起点到控制点、控制点到终点的距离；α和β分别为全路径距离影响因子和子路径距离影响因子；所述密度半径r_d根据路网密度确定，其计算表达式如下：

$r_d=\frac{N_B}{N_P}*r_B,$

上式中，N_B为路段基数，是半径为r_B的圆形区域内平均路段条数；r_B为路网区域的基半径，单位为米，通过全国路网数据统计获得；N_P是以控制点为中心、以r_B为半径的圆形目标区域内的路段条数，N_P通过一路段框选模型计算获得。

上述路段框选模型为：当一路段的外接矩形框与半径为r_B的圆形目标区域相交或内含时，认定该路段在目标区域内，否则认定该路段在目标区域外。

上述全路径距离影响因子α设定为1/35，子路径距离影响因子β设定为1/20，路段基数N_B设定为600。

本发明由于采取上述技术方案，具有以下优点：1、由于本发明采用通过性路网模型，将起点和终点分设在两个路网中，两路网只在通过区域范围内才能相互连通，因此能够确保由起点到达终点的寻路过程中一定经过指定的通过区域。2、由于本发明采用通过区域半径调节方案，在起点和终点距离较远时设置较大的通过区域半径，路网密度较小时设置较大的通过区域半径，因此能够确保规划路径有效经过控制点附近，但并不一定经过控制点。3、由于本发明采用虚终点寻路通过设置虚终点并利用A*算法引导路径通过指定区域，采用变方向寻路通过更改寻路引导代价引导路径从指定区域向终点规划，因此本发明是在A*算法的诱导特性的基础上对A*算法的一种改进，能够确保结果的准确性和可靠性，且系统的规划时间可以满足现有嵌入式设备的硬件要求。本发明可以用于各种嵌入式车载导航系统。

附图说明

图1是本发明的算法示意图

图2是本发明的通过性路网模型示意图

图3是本发明的路段框选模型示意图

图4是本发明的虚终点设置示意图

图5是本发明的虚终点寻路和变方向寻路示意图

图6是本发明的虚终点寻路工作流程示意图

图7是本发明的变方向寻路工作流程示意图

具体实施方式

下面结合附图和实施例对本发明进行详细的描述。

如图1所示，本发明的通过性寻路是指系统按照用户意图通过某个指定区域规划路径。图中S、T分别为规划路径的起点和终点；M为用户指定的控制点；通过区域P是以控制点M为圆心，以r为半径的圆形区域，其内的节点为复活节点(如图1浅色圆点所示)，通过性寻路是指路线要在从起点到终点的寻路过程中经过通过区域P内的复活节点；图中浅色实线所示为系统按照传统算法提供的最优路径，深色实线为系统按照本发明提供的通过性路径。与传统的最优路径相比，通过性路径能够按照用户意图在从起点到终点的过程中经过通过区域P。与S到M、M到T的两步规划相比，通过性寻路不要求路线必须经过控制点M，只要规划路线经过通过区域P且满足代价最低准则即可。

如图2所示，本发明需要建立一通过性路网模型，它包括两张路网和一个隔离面W＝{G，G′，X}，其中，平面X为隔离面，将起点路网G与终点路网G′隔离为两个互不连通的路网。起点路网G中通过区域P以外的节点不能到达终点路网G′，终点路网G′中通过区域P以外的节点也不能到达起点路网G，只有通过区域P内的节点才能连通起点路网G与终点路网G′。起点路网G中不包含终点T及其邻接路段，终点路网G′中不包含起点S和其邻接路段。除起点S和终点T以及它们的邻接路段以外，两张路网中的元素完全相同。

通过性路网模型中，通过区域P的大小可以按照以下控制策略进行调节：

①起点S与终点T的距离越大，半径r越大；

②起点S或终点T与控制点M的距离越大，半径r越大；

③控制点M附近的路网密度越大，半径r越小。

根据上述三点，通过区域P的半径r定义为r＝min(r_l，r_d)，其中r_l为距离半径，r_d为密度半径。

距离半径r_l是根据起点S或终点T到控制点M的距离确定的半径，其计算表达式如下：

r_l＝min(α*l_ST，β*min(l_SM，l_MT))，

上式中，l_ST、l_SM、l_MT分别为起点到终点、起点到控制点、控制点到终点的距离；α和β分别为全路径距离影响因子和子路径距离影响因子，根据经验α可以设定为1/35，β可以设定为1/20。

密度半径r_d是根据路网密度确定的半径，其计算表达式如下：

$r_d=\frac{N_B}{N_P}*r_B,$

上式中，N_B为路段基数，是半径为r_B的圆形区域内平均路段条数，N_B可以根据经验设定为600；r_B为路网区域的基半径，单位为米，通过全国路网数据统计获得；N_P是以控制点M为中心、以r_B为半径的圆形目标区域B内的路段条数，N_P通过一路段框选模型计算获得。

如图3所示，路段框选模型为：当一路段e的外接矩形框A与目标区域B相交(如图3(b)所示)，或内含时(如图3(c)和图3(d)所示)，认定路段e在目标区域B内，否则认定路段e在目标区域B外(如图3(a)所示)。

如图4所示，在A*算法中，终点引导路径的规划方向，为引导路径向通过区域P规划，需要在起点路网G中SM的延长线上设定一虚终点T′，且MT′＝MT。

如图5所示，通过性路径规划分为两步：虚终点寻路和变方向寻路。其中，第一步虚终点寻路在起点路网G中进行(如图5(a)所示)，以起点路网G中节点到虚终点T′的代价为引导代价，利用A*算法引导路径向通过区域P规划拓展，在路径拓展区域(图中虚线所示封闭图形)完全覆盖通过区域P后，停止虚终点寻路，通过区域P内拓展到的节点即为复活节点，复活节点的集合为复活队列R；第二步变方向寻路在终点路网G′中进行(如图5(b)所示)，以终点路网G′中复活节点到终点T的代价为引导代价，利用A*算法引导路径向终点T规划拓展，在到达终点T后停止变方向寻路。对虚终点寻路和变方向寻路的路径进行回溯，就获得了经过通过区域P的最优路径。

如图6、图7所示，虚终点寻路和变方向寻路的具体工作流程如下：

1)虚终点寻路：

1.1)在SM的延长线上设置一虚终点T′，且MT′＝MT；设置一优先队列Q，将其初始化仅含规划路径的起点Q＝{S}；将优先队列Q外的所有节点初始化为白色未拓展节点，将所有白色未拓展节点到起点S的已有代价(即节点与起点S之间的直线代价)U_g初始化为∞；将路径拓展区域完全覆盖通过区域P的标志F_p初始化为0；将一计数器N_Q初始化为0；

其中，为了能够更直观地了解路径拓展情况，本实施例采用了颜色标记节点的拓展状态，如将未访问到的节点设置为白色未拓展节点，将尚未从优点队列中取出拓展的节点设置为灰色在拓展节点，将已从优点队列中取出且其相邻节点均已访问到的节点设置为黑色已拓展节点，但实际应用中可以不限于此；

1.2)从优先队列Q中取出当前总代价U_f最小的节点V；

其中，总代价U_f＝U_g+U_h，U_h为节点V到虚终点T′的启发代价(即节点与虚终点T′之间的直线代价)；

1.3)判断节点V是否在通过区域P内：

若在通过区域P内，计数器N_Q减1，进入步骤1.9)；

若未在通过区域P内，进入步骤1.4)；

1.4)判断节点V的相邻节点是否均已访问到：

若均已访问到，将节点V标记为黑色已拓展节点，返回步骤1.2)；

若尚未全部访问到，进入步骤1.5)；

1.5)访问节点V的一尚未访问到的相邻节点U，判断节点U的拓展状态：

①若节点U为白色未拓展节点，计算节点U到起点S的已有代价U_g和到虚终点T′的启发代价U_h，将节点U加入优先队列Q变为灰色在拓展节点，进入步骤1.6)；

②若节点U为灰色在拓展节点，计算节点U到起点S的已有代价U_g′，进入步骤1.7)；

③若节点U为黑色已拓展节点，返回步骤1.4)；

1.6)判断节点U是否在通过区域P内：

若在通过区域P内，计数器N_Q加1，将节点U加入复活队列R，进入步骤1.8)；

若未在通过区域P内，返回步骤1.4)；

1.7)判断U_g′＜U_g是否成立：

若成立，更新优先队列Q中节点U到起点S的已有代价U_g＝U_g′，返回步骤1.4)；

若未成立，直接返回步骤1.4)；

1.8)比较N_Q与预先设定的复活节点个数阈值N^t：

若N_Q≥N^t，F_p＝1，进入步骤1.9)；

若N_Q＜N^t，返回步骤1.4)；

此处，复活节点个数阈值N^t可以按经验设定为5。

1.9)判断N_Q＝0且F_p＝1是否成立：

若成立，结束虚终点寻路，进入变方向寻路；

若未成立，返回步骤1.4)；

2)变方向寻路：

2.1)初始化优先队列Q＝R，将优先队列Q中所有节点的启发代价U_h更改为节点到终点T的启发代价；将优先队列Q外的所有节点初始化为白色未拓展节点；

2.2)从优先队列Q中取出当前总代价U_f＝U_g+U_h最小的节点V；

2.3)判断节点V是否为终点T：

若节点V非终点T，进入步骤2.4)；

若节点V为终点T，进入步骤2.7)；

2.4)判断节点V的相邻节点是否均已访问到：

若均已访问到，将节点V变为黑色已拓展节点，返回步骤2.2)，

若尚未全部访问到，进入步骤2.5)；

2.5)访问节点V的一尚未访问到的相邻节点U，判断节点U的拓展状态：

①若节点U为白色未拓展节点，计算节点U到起点S的已有代价U_g和到终点T的启发代价U_h，将节点U加入优先队列Q变为灰色在拓展节点，返回步骤2.4)；

②若节点U为灰色在拓展节点，计算节点U到起点S的已有代价U_g′，进入步骤2.6)；

③若节点U为黑色已拓展节点，返回步骤2.4)；

2.6)判断U_g′＜U_g是否成立：

若成立，更新优先队列Q中节点U到起点S的已有代价U_g＝U_g′，返回步骤2.4)；

若未成立，直接返回步骤2.4)；

2.7)结束变方向寻路。

上述实施例中，因变方向寻路步骤2.5)中需要计算节点U到起点S的已有代价U_g，为方便计算U_g，可以在变方向寻路步骤2.1)初始化时，在终点网路G′中添设规划路径的起点S，起点S在终点网路G′中孤立，没有邻接路段，也不与终点网路G′其它元素连通。

上述各实施例仅用于说明本发明，其中各部件的结构、连接方式等都是可以有所变化的，凡是在本发明技术方案的基础上进行的等同变换和改进，均不应排除在本发明的保护范围之外。

Claims (7)

1.一种车载导航系统通过性寻路算法，它包括以下步骤：

1)设置一包括有一起点路网、一终点路网和一隔离面的通过性路网模型；所述起点路网和终点路网分别设置一规划路径的起点和终点，除所述起点和终点以及它们的邻接路段以外，所述起点路网和终点路网内的元素相同，均设置有若干节点和一半径为r的圆形通过区域，所述通过区域的圆心为用户指定的控制点；所述起点路网和终点路网通过所述隔离面隔离，仅通过所述通过区域内的节点相互连通；

2)在由步骤1)建立的通过性路网模型中分两步：虚终点寻路和变方向寻路进行通过性寻路：

①在起点路网中设置一虚终点，以起点路网中节点到虚终点的代价为引导代价，利用A*算法引导路径向通过区域规划拓展，在路径拓展区域完全覆盖通过区域后，停止虚终点寻路；

②以终点路网中通过区域内经过步骤①拓展到的节点到终点的代价为引导代价，利用A*算法引导路径向终点规划拓展，在到达终点后停止变方向寻路；

3)回溯步骤2)的寻路路径，并将回溯结果确定为经过通过区域的最优路径。

2.如权利要求1所述的一种车载导航系统通过性寻路算法，其特征在于：所述步骤2)的虚终点寻路和变方向寻路包括以下步骤：

2.1)在起点路网中起点与控制点的连线的延长线上，于靠近控制点的一端设置一虚终点，使控制点与虚终点之间的距离等于控制点与终点之间的距离；设置一优先队列Q，将其初始化仅含规划路径的起点；将优先队列Q外的所有节点初始化为未拓展节点，将所有未拓展节点到起点的已有代价U_g初始化为∞，将路径拓展区域完全覆盖通过区域的标志F_p初始化为0，将一计数器N_Q初始化为0；

2.2)从优先队列Q中取出当前总代价U_f＝U_g+U_h最小的节点V，U_h为节点到虚终点的启发代价；

2.3)判断节点V是否在通过区域内：

若在通过区域内，计数器N_Q减1，进入步骤2.9)；

若未在通过区域内，进入步骤2.4)；

2.4)判断节点V的相邻节点是否均已访问到：

若均已访问到，将节点V标记为已拓展节点，返回步骤2.2)；

若尚未全部访问到，进入步骤2.5)；

2.5)访问节点V的一尚未访问到的相邻节点U，判断节点U的拓展状态：

①若节点U为未拓展节点，计算节点U到起点的已有代价U_g和到虚终点的启发代价U_h，将节点U加入优先队列Q变为在拓展节点，进入步骤2.6)；

②若节点U为在拓展节点，计算节点U到起点的已有代价U_g′，进入步骤2.7)；

③若节点U为已拓展节点，返回步骤2.4)；

2.6)判断节点U是否在通过区域内：

若在通过区域内，计数器N_Q加1，将节点U加入复活队列R，进入步骤2.8)；

若未在通过区域内，返回步骤2.4)；

2.7)判断U_g′＜U_g是否成立：

若成立，更新优先队列Q中节点U到起点的已有代价U_g＝U_g′，返回步骤2.4)；

若未成立，直接返回步骤2.4)；

2.8)比较N_Q与预先设定的复活节点个数阈值N^t：

若N_Q≥N^t，F_p＝1，进入步骤2.9)；

若N_Q＜N^t，返回步骤2.4)；

2.9)判断N_Q＝0且F_p＝1是否成立：

若成立，结束虚终点寻路，进入步骤2.10)；

若未成立，返回步骤2.4)；

2.10)初始化优先队列Q＝R，将优先队列Q中所有节点的启发代价U_h更改为节点到终点的启发代价，并将优先队列Q外的所有节点初始化为未拓展节点；

2.11)从优先队列Q中取出当前总代价U_f＝U_g+U_h最小的节点V；

2.12)判断节点V是否为终点：

若节点V非终点，进入步骤2.13)；

若节点V为终点，进入步骤2.16)；

2.13)判断节点V的相邻节点是否均已访问到：

若均已拓展到，将节点V变为已拓展节点，返回步骤2.11)；

若未全部拓展到，进入步骤2.14)；

2.14)访问节点V的一尚未访问到的相邻节点U，判断节点U的拓展状态：

①若节点U为未拓展节点，计算节点U到起点的已有代价U_g和到终点的启发代价U_h，将节点U加入优先队列Q变为在拓展节点，返回步骤2.13)；

②若节点U为在拓展节点，计算节点U到起点的已有代价U_g′，进入步骤2.15)；

③若节点U为已拓展节点，返回步骤2.13)；

2.15)判断U_g′＜U_g是否成立：

若成立，更新优先队列Q中节点U到起点的已有代价U_g＝U_g′，返回步骤2.13)；

若未成立，直接返回步骤2.13)；

2.16)结束变方向寻路。

3.如权利要求2所述的一种车载导航系统通过性寻路算法，其特征在于：所述步骤2.8)中，复活节点个数阈值N^t设定为5。

4.如权利要求1或2或3所述的一种车载导航系统通过性寻路算法，其特征在于：所述步骤1)中，通过区域半径r为r＝min(r_l，r_d)，r_l为距离半径，r_d为密度半径；所述距离半径r_l根据起点或终点到控制点的距离确定，其计算表达式如下：

r_l＝min(α*l_ST，β*min(l_SM，l_MT))，

上式中，l_ST、l_SM、l_MT分别为起点到终点、起点到控制点、控制点到终点的距离；α和β分别为全路径距离影响因子和子路径距离影响因子；所述密度半径r_d根据路网密度确定，其计算表达式如下：

$r_d=\frac{N_B}{N_P}*r_B,$

上式中，N_B为路段基数，是半径为r_B的圆形区域内平均路段条数；r_B为路网区域的基半径，单位为米，通过全国路网数据统计获得；N_P是以控制点为中心、以r_B为半径的圆形目标区域内的路段条数，N_P通过一路段框选模型计算获得。

5.如权利要求4所述的一种车载导航系统通过性寻路算法，其特征在于：所述路段框选模型为：当一路段的外接矩形框与半径为r_B的圆形目标区域相交或内含时，认定该路段在目标区域内，否则认定该路段在目标区域外。

6.如权利要求4所述的一种车载导航系统通过性寻路算法，其特征在于：所述全路径距离影响因子α设定为1/35，子路径距离影响因子β设定为1/20，路段基数N_B设定为600。

7.如权利要求5所述的一种车载导航系统通过性寻路算法，其特征在于：所述全路径距离影响因子α设定为1/35，子路径距离影响因子β设定为1/20，路段基数N_B设定为600。

Images