CN113494926A - 一种寻路方法、装置以及设备 - Google Patents

一种寻路方法、装置以及设备 Download PDF

Info

Publication number
CN113494926A
CN113494926A CN202111039750.6A CN202111039750A CN113494926A CN 113494926 A CN113494926 A CN 113494926A CN 202111039750 A CN202111039750 A CN 202111039750A CN 113494926 A CN113494926 A CN 113494926A
Authority
CN
China
Prior art keywords
node
current node
alternative
current
candidate
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202111039750.6A
Other languages
English (en)
Inventor
周锐
吴寅初
张晓东
李金广
曹东璞
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenzhen Huituo Infinite Technology Co ltd
Original Assignee
Shenzhen Huituo Infinite Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shenzhen Huituo Infinite Technology Co ltd filed Critical Shenzhen Huituo Infinite Technology Co ltd
Priority to CN202111039750.6A priority Critical patent/CN113494926A/zh
Publication of CN113494926A publication Critical patent/CN113494926A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

本申请提供一种寻路方法，包括：步骤一，将路径起点定义为当前节点；步骤二，判断所述当前节点是否是终点，若否，则遍历所述当前节点的新连接节点获得备选节点；步骤三，根据先验因子排除负节点后计算所述备选节点优先级，将所述优先级最大的所述备选节点的父指针指向当前节点，更新所述备选节点为新的当前节点；步骤四，重复步骤二和步骤三，直到判断所述当前节点为终点，将所述终点根据父指针的指向回溯到起点的路径作为最优路径。通过在进行优先级计算之前通过先验因子排除负节点，节省了计算时间，并减少寻路结果逆向而行的机会。本申请还提供一种寻路装置以及设备。

Description

一种寻路方法、装置以及设备
技术领域
本申请涉及一种路径优化技术，尤其涉及一种寻路方法。本申请还提供一种寻路装置和设备。
背景技术
寻路算法在实际生活中应用广泛，最为基础的应用就是在城市当中规划最优路径。目前寻路算法中通常采用迪杰斯特拉算法和A*算法。
所述迪杰斯特拉算法具体操作步骤是，设置已经求解出最短路径的S集和未求出最短路径的U集。获取一个标注精细的地图，并从所述地图中选取路径的起点。将起点放入S集合，其余所有节点都放入集合U。设S集中最后求出的节点为s，若U集中节点与s相连，则连边距离记为记录距离，不与s相连的节点距离记录为无穷大。
从U中选择距离s最短的节点记为x，将其加入S中，并从U中将x移除；
以x为当前节点，搜索直接连接点，计算连接权重，更新U中的节点距离；
重复步骤，直到遍历完所有的节点，此时从S中寻找目标节点的距离值，这个距离即为起点到达它的最短距离。
所述A*算法的操作步骤是，结合已知信息和初始状态对当前状态到目标状态进行代价估计。
首先设置一个OPEN表和一个CLOSE表，所述OPEN表中记录未经过遍历操作的节点，CLOSE表中记载已经完成遍历的节点。
初始化OPEN表和CLOSE表后，将路径的起始节点放入OPEN表中，此时OPEN表中只有一个初始节点。
从OPEN表中取出一个最小头节点，也就是最后加入的节点。开始时OPEN表中只有起始节点，因此将起始节点取出作为当前节点，将其父节点加入CLOSE节点，如果没有父节点则不加。
将当前节点从OPEN表中删除，判断所述当前节点是否是终点，若是则通过所述当前节点的父指针找到起点到终点的最优路径。
若不是，则寻找排除了CLOSE表中节点的当前节点连接点，判断所述连接点是否在OPEN表中，若不是则将所述连接点的父指针指向当前节点，并放入OPEN表中，若是，则计算所述连接点到其父节点的第一距离和所述连接点经过当前节点到达其父节点的第二距离。若所述第一距离大于第二距离，将所述连接点的父指针指向当前节点。
重复上述步骤直到找到终点，然后根据父指针，找到起点和终点之间的最优路径。
在上述两种最优路径的规划方法中，还存在以下问题：
1、所述迪杰斯特拉算法应用在一个路径节点少的地图中，可以准确的找到最优路劲，但是在一些路径节点多的地图中，则因为数据量庞大导致计算量暴涨，不能适应实时计算。
2、所述A*算法，在地图较为复杂时无法提供全部因数的估计，不能估计出最优路径。
发明内容
本申请公开一种寻路方法、装置以及设备，以解决寻路计算有延迟导致不能进行实时寻路以及寻路误差过大的问题。
本申请提供一种寻路方法，包括：
步骤一，将路径起点定义为当前节点；
步骤二，判断所述当前节点是否是终点，若否，则遍历所述当前节点的新连接节点获得备选节点；
步骤三，根据先验因子排除负节点后计算所述备选节点优先级，将所述优先级最大的所述备选节点的父指针指向当前节点，更新所述备选节点为新的当前节点；
步骤四，重复步骤二和步骤三，直到判断所述当前节点为终点，将所述终点根据父指针的指向回溯到起点的路径作为最优路径。
可选的，获取所述先验因子，包括：
根据所述当前节点和所述备选节点的位置，确定所述当前节点指向所述备选节点的第一连线；
计算所述第一连线和所述当前节点指向终点的第二连线之间的夹角，将所述夹角大于90度作为先验因子。
可选的，包括：
将经过遍历的所述当前节点添加到CLOSE表，以及判断所述备选节点是否在OPEN表中；
若否，将所述备选节点的父指针指向当前节点，加入所述OPEN表中；
若是，计算所述备选节点与所述备选节点的父节点的第一距离和所述备选节点经过当前节点到达所述备选节点的父节点的第二距离，若所述第一距离大于第二距离，将所述备选节点的父指针指向当前节点。
可选的，所述计算所述备选节点优先级，包括：
计算所述备选节点到终点的第一最优步数和所述备选节点到当前节点的第二最优步数；
将所述第一最优步数和第二最优步数的步数之和作为优先级参数。
可选的，所述优先级参数还包括红绿灯因子：
根据所述当前节点和备选节点之间的红绿灯数量、所述红绿灯的等待时间以及路径上的行进速度计算红绿灯因子，公式如下：
Figure 199400DEST_PATH_IMAGE001
Figure 501069DEST_PATH_IMAGE002
是红绿灯因子，所述
Figure 861643DEST_PATH_IMAGE003
是红绿灯的等待时间和行进速度的积，所述
Figure 983183DEST_PATH_IMAGE004
是当前节点到达所述备选节点之间的红绿灯数量。
可选的，所述起点的位置信息根据所述起点的节点ID获取，所述节点ID记录在交叉路口信息表和道路信息表中。
可选的，返回所述最优路径包括：返回道路ID以及车道ID。
可选的，返回所述最优路径还包括：寻路失败信息。
本申请还提供一种寻路装置，包括：
定义模块，用于将路径起点定义为当前节点；
遍历模块，判断所述当前节点是否是终点，若否，则遍历所述当前节点的新连接节点获得备选节点；
更新模块，用于根据先验因子排除负节点后计算所述备选节点优先级，将所述优先级最大的所述备选节点的父指针指向当前节点，更新所述备选节点为新的当前节点；
结果模块，用于重复遍历模块和更新模块的操作步骤，直到判断所述当前节点为终点，将所述终点根据父指针的指向回溯到起点的路径作为最优路径。
可选的，所述更新模块还包括：
先验因子单元，用于根据所述当前节点和所述备选节点的位置，确定所述当前节点指向所述备选节点的第一连线；
计算所述第一连线和所述当前节点指向终点的第二连线之间的夹角，将所述夹角大于90度作为先验因子。
可选的，所述更新模块还包括：
归置单元，用于将经过遍历的所述当前节点添加到CLOSE表，以及判断所述备选节点是否在OPEN表中；
若否，将所述备选节点的父指针指向当前节点，加入所述OPEN表中；
若是，计算所述备选节点与所述备选节点的父节点的第一距离和所述备选节点经过当前节点到达所述备选节点的父节点的第二距离，若所述第一距离大于第二距离，将所述备选节点的父指针指向当前节点。
可选的，所述更新模块还包括：
优先级计算单元，用于计算所述备选节点到终点的第一最优步数和所述备选节点到当前节点的第二最优步数；
将所述第一最优步数和第二最优步数的步数之和作为优先级参数。
可选的，所述更新模块还包括：
红绿灯因子单元，用于根据所述当前节点和备选节点之间的红绿灯数量、所述红绿灯的等待时间以及路径上的行进速度计算红绿灯因子，公式如下：
Figure 352984DEST_PATH_IMAGE005
Figure 243580DEST_PATH_IMAGE006
是红绿灯因子，所述
Figure 775055DEST_PATH_IMAGE007
是红绿灯的等待时间和行进速度的积，所述
Figure 616847DEST_PATH_IMAGE008
是当前节点到达所述备选节点之间的红绿灯数量。
本申请还提供一种寻路设备，包括：
显示器，用于显示最优路径；
存储器；用于存储寻路程序；
处理器，用于运行从所述存储器中调取寻路程序，包括：步骤一，将路径起点定义为当前节点；步骤二，判断所述当前节点是否是终点，若否，则遍历所述当前节点的新连接节点获得备选节点；步骤三，根据先验因子排除负节点后计算所述备选节点优先级，将所述优先级最大的所述备选节点的父指针指向当前节点，更新所述备选节点为新的当前节点；步骤四，重复步骤二和步骤三，直到判断所述当前节点为终点，将所述终点根据父指针的指向回溯到起点的路径作为最优路径。
本申请相对与现有技术的优点如下：
本申请提供一种寻路方法，包括：步骤一，将路径起点定义为当前节点；步骤二，判断所述当前节点是否是终点，若否，则遍历所述当前节点的新连接节点获得备选节点；步骤三，根据先验因子排除负节点后计算所述备选节点优先级，将所述优先级最大的所述备选节点的父指针指向当前节点，更新所述备选节点为新的当前节点；步骤四，重复步骤二和步骤三，直到判断所述当前节点为终点，将所述终点根据父指针的指向回溯到起点的路径作为最优路径。通过在进行优先级计算之前通过先验因子排除负节点，节省了计算时间，并减少寻路结果逆向而行的机会。
附图说明
图1是本申请中寻路方法流程图。
图2是本申请中备选节点筛选流程图。
图3是本申请红绿灯因子优化路径示意图。
图4是本申请中寻路装置示意图。
具体实施方式
在下面的描述中阐述了很多具体细节以便于充分理解本申请。但是本申请能够以很多不同于在此描述的其它方式来实施，本领域技术人员可以在不违背本申请内涵的情况下做类似推广，因此本申请不受下面公开的具体实施的限制。
本申请主要提供一种寻路方法，包括：步骤一，将路径起点定义为当前节点；步骤二，判断所述当前节点是否是终点，若否，则遍历所述当前节点的新连接节点获得备选节点；步骤三，根据先验因子排除负节点后计算所述备选节点优先级，将所述优先级最大的所述备选节点的父指针指向当前节点，更新所述备选节点为新的当前节点；步骤四，重复步骤二和步骤三，直到判断所述当前节点为终点，将所述终点根据父指针的指向回溯到起点的路径作为最优路径。通过先验因子排除负节点，减少了计算量，在保证结果最优的同时有效降低寻路响应延迟和寻路的误差。
为了清楚的对本申请的方案进行描述，下面结合附图进行详细说明。
图1是本申请中寻路方法流程图。
请参照图1所示，S101步骤一，将路径起点定义为当前节点；
本申请所述起点是从某一地点另外一地的起始点，所述起始点实际上是一种道路交通数据，是存储于OpenDrive文件中的道路和道路网数据。
从所述OpenDrive文件中读取OpenDrive数据，并将所述OpenDrive数据进行XML文件解析，然后将所述解析后的数据存储到数据库中。本申请所述OpenDrive数据中的道路交叉口数据和道路信息数据分别记载在Junction表和Road表中，所述道路交叉口就是道路的路口，所述道路信息包括道路上的信号灯、车道等信息。
本申请所述节点具有唯一的ID，根据所述节点ID，将起点和终点提取出来，所述ID记录有起点和终点的坐标。
获取到所述起点后，将所述起点定义为当前节点。
S102步骤二，判断所述当前节点是否是终点，若否，则遍历所述当前节点的新连接节点获得备选节点；
在获取到其点和终点的信息后，调用寻路算法执行起点到终点的寻路计算。
首先需要预先初始化OPEN表和CLOSE表，所述OPEN表中记载未经遍历的节点，所述CLOSE表中记载已经遍历完成的节点。
在完成初始化后，将起点添加到OPEN表中，此时OPEN表是刚刚初始化完成的，因此只有起点。
判断所述OPEN表是否为空，因为已经添加了起点，所以所述OPEN表不为空，然后将所述起点从OPEN表中取出装入CLOSE表中，即将当前节点从OPEN表中取出装入CLOSE表中。
在另一种情景下，所述OPEN表中没有节点，即所述OPEN表为空，这时则返回寻路失败信息，反之，若有多个节点，则需要提取出的节点是头节点，所述头节点即父节点可知，子节点未知的节点。
当当前节装入CLOSE表中时，即表明所述当前节点已经或者正要经历遍历过程，因此将所述当前节点从OPEN表中删除，然后进行后续处理。
所述当前节点存储CLOSE表中，接着对所述当前节点进行遍历，寻找所述当前节点连接的每个节点，并将遍历出的所有节点作为备选节点，其中所述备选节点不包含已经装入CLOSE表中的节点。
S103步骤三，根据先验因子排除负节点后计算所述备选节点优先级，将所述优先级最大的所述备选节点的父指针指向当前节点，更新所述备选节点为新的当前节点；
上述步骤已经选出了与所述当前节点连接备选节点，这些所述备选节点都可以作为所述当前节点的子节点，本申请首先需要将其中的一部分不符合要求的备选节点删除，然后进行下一步备选节点的处理。
本领域技术人员应当清楚的是，一条最优路径通常情况下是不可能相互以相反的方向连接，这个相反方向的备选节点就是负节点。因此本申请第一步是将所述备选节点和所述当前节点的第一连线与当前节点和终点连线的第二连线的夹角作为筛选条件进行所述备选节点的筛选。根据所述当前节点和所述备选节点的位置，确定所述当前节点指向所述备选节点的第一连线；计算所述第一连线和所述当前节点指向终点的第二连线之间的夹角，将所述夹角大于90度作为先验因子。
图2是本申请中备选节点筛选流程图。
请参照图2所示，所述当前节点2连接备选节点1，备选节点3和备选节点4，分别形成第一连线q、第一连线w和第一连线e。所述当前节点2和终点7的第二连线r。分别计算所述第二连线r和第一连线q、第一连线w、第一连线e的夹角，公式如下：
Figure 259181DEST_PATH_IMAGE009
Figure 269862DEST_PATH_IMAGE010
Figure 972239DEST_PATH_IMAGE011
其中，所述A是当前节点2，B是备选节点1、3或4，E是终点7，
Figure 68371DEST_PATH_IMAGE012
是两条连线的夹角，
Figure 514396DEST_PATH_IMAGE013
是两点连线与X轴的夹角。
在计算出当前节点2和所述备选节点1、3或者4的连线与当前节点2和终点7的连线的夹角后，根据所述夹角对备选节点1、3或4进行筛选。
其中相关与备选节点1的所述角度大于90度，根据所述先验因子的规则，将对应的备选节点1删除，其中相关与备选节点3、4的角度小于90度，所述备选节点保留，进入到下一步处理当中。
经过上述步骤，本申请已经将所述备选节点进行了初步筛选，并筛选出了可进行下一步处理的备选节点，极大的减少了备选节点的计算量。
计算所述备选节点的优先级，就是将所述当前节点到终点最优路径的节点挑选出来。
上述已经提到，所述当前节点已经被装入CLOSE表中，接下来判断所述备选节点是否在OPEN表中。具体的，若所当前节点是起点，则是每个所述备选节点的父节点都是当前节点，若所述当前节点是由备选节点转换而来的，则所述当前节点连接的备选节点有可能也连接前当前节点，若是，则所述备选节点已经被装入OPEN表中。
若所述备选节点不在OPEN表中，则可以判断所述备选节点是直接和所述当前节点连接的，则将所述备选节点的父指针指向当前节点，并将所述备选节点装入OPEN表中。
若所述备选节点在所述OPEN表中，则可以判断，所述备选节点和其父节点的连接有两种情况。其一是，所述备选节点通过当前节点和其父节点相连，将所述备选节点到当期节点的距离与当前节点到所述备选节点的父节点的距离和标记为第二距离。其二是备选节点直接连接父节点，将所述备选节点直接到其父节点的距离标记为第一距离。
判断所述第一距离和第二距离的关系，若所述第一距离大于第二距离，则将所述备选节点的父节点更换为当前节点。
通过上述设置，所述当前节点遍历后的备选节点的父节点全部设为当前节点，接着进行优先级判断，包括：计算所述备选节点到终点的第一最优步数和所述备选节点到当前节点的第二最优步数。
所述第一最优步数就是所述当前节点到达所述备选节点最短的距离或者最少步数，所述第二最优步是所述备选节点到达终点最短的距离或者最少步数。将所述第一最优步数和所述第二最优步数相加的出的结果是优先级参数，所述优先级参数最小的，为优先级最高的备选节点。
本申请计算所述最优距离或者步数，通过曼哈顿距离获得，其公式如下：
Figure 379584DEST_PATH_IMAGE014
其中，所述
Figure 987283DEST_PATH_IMAGE015
是优先级参数，所述
Figure 570711DEST_PATH_IMAGE016
和
Figure 820426DEST_PATH_IMAGE017
分布表示所述备选节点到达当前节点和终点的距离。
另一方面，本申请还需要对所述优先级进行优化，以所述优先级能够实际整合对路况信息的反映。
本申请中将红绿灯的等待时间整合到优先级判断上。在一条道路上可能有一个或者多个红绿灯，每个红绿灯可能需要一定的等待时间，因此红绿灯个数，以及等待时间实际上对出行具有重要的影响，而影响大小，需要根据行进的速度来判断。
图3是申请红绿灯因子优化路径示意图。
请参照图3所示。当前节点2到备选节点4的路径上具有红绿灯i，备选节点4到节点5有红绿灯o，节点6到终点7上有红绿灯p。按照红绿灯优化，则可以得到的路径是当前节点到备选节点3，再到节点5，再到终点7的路径。
所述红绿灯响应优先级的因素，本申请中称为红绿灯因子，所述红绿灯因子的计算方法如下：
Figure 540121DEST_PATH_IMAGE018
其中，
Figure 849879DEST_PATH_IMAGE019
是红绿灯因子，所述
Figure 920604DEST_PATH_IMAGE020
是红绿灯的等待时间和行进速度的积，所述
Figure 708431DEST_PATH_IMAGE021
是当前节点到达所述备选节点之间的红绿灯数量。
将所述红绿灯因子和曼哈顿距离直接相加，即得到如下公式：
Figure 548211DEST_PATH_IMAGE022
其中
Figure 28871DEST_PATH_IMAGE023
是优化后的优先级参数，所述
Figure 55733DEST_PATH_IMAGE024
包括公式
Figure 381672DEST_PATH_IMAGE025
。
通过上述公式，就可以得出优先级最高的所述备选节点，将选出的所述备选节点设置为当前节点。
S104步骤四，重复步骤二和步骤三，直到判断所述当前节点为终点，将所述终点根据父指针的指向回溯到起点的路径作为最优路径。
接下来，将所述当前节点冲OPEN表中取出，并判断所述当前节点是否是终点，以及重复上述S102~S103的步骤，直到判断出所述当前节点为终点，解释寻路过程。
将所述终点通过父指针巡回到起点，将所述终点根据父指针的指向回溯到起点的路径作为最优路径。所述最优路径的信息中包括道路ID以及车道ID。
对应于上述寻路方法，本申请还提供一种寻路装置。
图4是本申请中寻路装置示意图。
请参照图4所示，定义模块401，用于将路径起点定义为当前节点；
本申请所述起点是从某一地点另外一地的起始点，所述起始点实际上是一种道路交通数据，是存储于OpenDrive文件中的道路和道路网数据。
从所述OpenDrive文件中读取OpenDrive数据，并将所述OpenDrive数据进行XML文件解析，然后将所述解析后的数据存储到数据库中。本申请所述OpenDrive数据中的道路交叉口数据和道路信息数据分别记载在Junction表和Road表中，所述道路交叉口就是道路的路口，所述道路信息包括道路上的信号灯、车道等信息。
本申请所述节点具有唯一的ID，根据所述节点ID，将起点和终点提取出来，所述ID记录有起点和终点的坐标。
获取到所述起点后，将所述起点定义为当前节点。
遍历模块402，判断所述当前节点是否是终点，若否，则遍历所述当前节点的新连接节点获得备选节点；
在获取到其点和终点的信息后，调用寻路算法执行起点到终点的寻路计算。
所述遍历模块402包括：
首先需要预先初始化OPEN表和CLOSE表，所述OPEN表中记载未经遍历的节点，所述CLOSE表中记载已经遍历完成的节点。
在完成初始化后，将起点添加到OPEN表中，此时OPEN表是刚刚初始化完成的，因此只有起点。
判断所述OPEN表是否为空，因为已经添加了起点，所以所述OPEN表不为空，然后将所述起点从OPEN表中取出装入CLOSE表中，即将当前节点从OPEN表中取出装入CLOSE表中。
在另一种情景下，所述OPEN表中没有节点，即所述OPEN表为空，这时则返回寻路失败信息，反之，若有多个节点，则需要提取出的节点是头节点，所述头节点即父节点可知，子节点未知的节点。
当当前节点装入CLOSE表中时，即表明所述当前节点已经或者正要经历遍历过程，因此将所述当前节点从OPEN表中删除，然后进行后续处理。
所述当前节点存储CLOSE表中，接着对所述当前节点进行遍历，寻找所述当前节点连接的每个节点，并将遍历出的所有节点作为备选节点，其中所述备选节点不包含已经装入CLOSE表中的节点。
更新模块403，用于根据先验因子排除负节点后计算所述备选节点优先级，将所述优先级最大的所述备选节点的父指针指向当前节点，更新所述备选节点为新的当前节点；
上述步骤已经选出了与所述当前节点连接备选节点，这些所述备选节点都可以作为所述当前节点的子节点，本申请首先需要将其中的一部分不符合要求的备选节点删除，然后进行下一步备选节点的处理。
本领域技术人员应当清楚的是，一条最优路径通常情况下是不可能相互以相反的方向连接，这个相反方向的备选节点就是负节点。因此本申请所述更新模块还包括：先验因子单元，用于根据所述当前节点和所述备选节点的位置，确定所述当前节点指向所述备选节点的第一连线；计算所述第一连线和所述当前节点指向终点的第二连线之间的夹角，将所述夹角大于90度作为先验因子。
第一步是将所述备选节点和所述当前节点的第一连线与当前节点和终点连线的第二连线的夹角作为筛选条件进行所述备选节点的筛选。根据所述当前节点和所述备选节点的位置，确定所述当前节点指向所述备选节点的第一连线；计算所述第一连线和所述当前节点指向终点的第二连线之间的夹角，将所述夹角大于90度作为先验因子。
如图2所示，所述当前节点2连接备选节点1，备选节点3和备选节点4，分别形成第一连线q、第一连线w和第一连线e。所述当前节点2和终点7的第二连线r。分别计算所述第二连线r和第一连线q、第一连线w、第一连线e的夹角，公式如下：
Figure 810379DEST_PATH_IMAGE027
Figure 461940DEST_PATH_IMAGE029
Figure 241678DEST_PATH_IMAGE031
其中，所述A是当前节点2，B是备选节点1、3或4，E是终点7，
Figure 636887DEST_PATH_IMAGE032
是两条连线的夹角，
Figure 920101DEST_PATH_IMAGE033
是两点连线与X轴的夹角。
在计算出当前节点2和所述备选节点1、3或者4的连线与当前节点2和终点7的连线的夹角后，根据所述夹角对备选节点1、3或4进行筛选。
其中相关与备选节点1的所述角度大于90度，根据所述先验因子的规则，将对应的备选节点1删除，其中相关与备选节点3、4的角度小于90度，所述备选节点保留，进入到下一步处理当中。
经过上述步骤，本申请已经将所述备选节点进行了初步筛选，并筛选出了可进行下一步处理的备选节点，极大的减少了备选节点的计算量。
计算所述备选节点的优先级，就是将所述当前节点到终点最优路径的节点挑选出来。
上述已经提到，所述当前节点已经被装入CLOSE表中，所述更新模块403还包括：
归置单元，用于将所述经过遍历的当前节点添加到CLOSE表，以及判断所述备选节点是否在OPEN表中；
具体的，若所当前节点是起点，则是每个所述备选节点的父节点都是当前节点，若所述当前节点是由备选节点转换而来的，则所述当前节点连接的备选节点有可能也连接前当前节点，若是，则所述备选节点已经被装入OPEN表中。
若否，将所述备选节点的父指针指向当前节点，加入所述OPEN表中。若所述备选节点不在OPEN表中，则可以判断所述备选节点是直接和所述当前节点连接的，则将所述备选节点的父指针指向当前节点，并将所述备选节点装入OPEN表中。
若是，计算所述备选节点与所述备选节点的父节点的第一距离和所述备选节点经过当前节点到达所述备选节点的父节点的第二距离，若所述第一距离大于第二距离，将所述备选节点的父指针指向当前节点。若所述备选节点在所述OPEN表中，则可以判断，所述备选节点和其父节点的连接有两种情况。其一是，所述备选节点通过当前节点和其父节点相连，将所述备选节点到当期节点的距离与当前节点到所述备选节点的父节点的距离和标记为第二距离。其二是备选节点直接连接父节点，将所述备选节点直接到其父节点的距离标记为第一距离。
判断所述第一距离和第二距离的关系，若所述第一距离大于第二距离，则将所述备选节点的父节点更换为当前节点。
所述更新模块403还包括：
优先级计算单元，用于计算所述备选节点到终点的第一最优步数和所述备选节点到当前节点的第二最优步数；
所述当前节点遍历后的备选节点的父节点全部设为当前节点，接着进行优先级判断，包括：计算所述备选节点到终点的第一最优步数和所述备选节点到当前节点的第二最优步数。
所述第一最优步数就是所述当前节点到达所述备选节点最短的距离或者最少步数，所述第二最优步是所述备选节点到达终点最短的距离或者最少步数。将所述第一最优步数和所述第二最优步数相加的出的结果是优先级参数，所述优先级参数最小的，为优先级最高的备选节点。
将所述第一最优步数和第二最优步数的步数之和作为优先级参数。本申请计算所述最优距离或者步数，通过曼哈顿距离获得，其公式如下：
Figure 8142DEST_PATH_IMAGE034
其中，所述
Figure 242552DEST_PATH_IMAGE035
是优先级参数，所述
Figure 441453DEST_PATH_IMAGE036
和
Figure 844752DEST_PATH_IMAGE037
分布表示所述备选节点到达当前节点和终点的距离。
所述更新单元还包括：
红绿灯因子单元，用于根据所述当前节点和备选节点之间的红绿灯数量、所述红绿灯的等待时间以及路径上的行进速度计算红绿灯因子。
本申请中将红绿灯的等待时间整合到优先级判断上。在一条道路上可能有一个或者多个红绿灯，每个红绿灯可能需要一定的等待时间，因此红绿灯个数，以及等待时间实际上对出行具有重要的影响，而影响大小，需要根据行进的速度来判断。
所述红绿灯响应优先级的因素，本申请中称为红绿灯因子，所述红绿灯因子的计算方法如下：
Figure 838116DEST_PATH_IMAGE038
其中，
Figure 592445DEST_PATH_IMAGE039
是红绿灯因子，所述
Figure 63878DEST_PATH_IMAGE040
是红绿灯的等待时间和行进速度的积，所述
Figure 587263DEST_PATH_IMAGE041
是当前节点到达所述备选节点之间的红绿灯数量。
将所述红绿灯因子和曼哈顿距离直接相加，即得到如下公式：
Figure 485949DEST_PATH_IMAGE043
其中
Figure 196416DEST_PATH_IMAGE044
是优化后的优先级参数，所述
Figure 737119DEST_PATH_IMAGE045
包括公式
Figure 115010DEST_PATH_IMAGE047
。
通过上述公式，就可以得出优先级最高的所述备选节点，将选出的所述备选节点设置为当前节点。
结果模块404，用于重复遍历模块和更新模块的操作步骤，直到判断所述当前节点为终点，将所述终点根据父指针的指向回溯到起点的路径作为最优路径。
接下来，将所述当前节点冲OPEN表中取出，并判断所述当前节点是否是终点，以及重复上述402~403模块的处理，直到判断出所述当前节点为终点，解释寻路过程。
将所述终点通过父指针巡回到起点，将所述终点根据父指针的指向回溯到起点的路径作为最优路径。所述最优路径的信息中包括道路ID以及车道ID。
本申请还提供一种寻路设备，包括：
显示器，用于显示最优路径；
存储器；用于存储寻路程序；
处理器，用于运行从所述存储器中调取寻路程序，包括：步骤一，将路径起点定义为当前节点；步骤二，判断所述当前节点是否是终点，若否，则遍历所述当前节点的新连接节点获得备选节点；步骤三，根据先验因子排除负节点后计算所述备选节点优先级，将所述优先级最大的所述备选节点的父指针指向当前节点，更新所述备选节点为新的当前节点；步骤四，重复步骤二和步骤三，直到判断所述当前节点为终点，将所述终点根据父指针的指向回溯到起点的路径作为最优路径。

Claims (14)

1.一种寻路方法，其特征在于，包括：
步骤一，将路径起点定义为当前节点；
步骤二，判断所述当前节点是否是终点，若否，则遍历所述当前节点的新连接节点获得备选节点；
步骤三，根据先验因子排除负节点后计算所述备选节点优先级，将所述优先级最大的所述备选节点的父指针指向当前节点，更新所述备选节点为新的当前节点；
步骤四，重复步骤二和步骤三，直到判断所述当前节点为终点，将所述终点根据父指针的指向回溯到起点的路径作为最优路径。
2.根据权利要求1所述寻路方法，其特征在于，获取所述先验因子，包括：
根据所述当前节点和所述备选节点的位置，确定所述当前节点指向所述备选节点的第一连线；
计算所述第一连线和所述当前节点指向终点的第二连线之间的夹角，将所述夹角大于90度作为先验因子。
3.根据权利要求2所述寻路方法，其特征在于，包括：
将经过遍历的所述当前节点添加到CLOSE表，以及判断所述备选节点是否在OPEN表中；
若否，将所述备选节点的父指针指向当前节点，加入所述OPEN表中；
若是，计算所述备选节点与所述备选节点的父节点的第一距离和所述备选节点经过当前节点到达所述备选节点的父节点的第二距离，若所述第一距离大于第二距离，将所述备选节点的父指针指向当前节点。
4.根据权利要求2所述寻路方法，其特征在于，所述计算所述备选节点优先级，包括：
计算所述备选节点到终点的第一最优步数和所述备选节点到当前节点的第二最优步数；
将所述第一最优步数和第二最优步数的步数之和作为优先级参数。
5.根据权利要求4所述寻路方法，其特征在于，所述优先级参数还包括红绿灯因子：
根据所述当前节点和备选节点之间的红绿灯数量、所述红绿灯的等待时间以及路径上的行进速度计算红绿灯因子，公式如下：
Figure 384088DEST_PATH_IMAGE003
Figure 33507DEST_PATH_IMAGE004
是红绿灯因子，所述
Figure 743974DEST_PATH_IMAGE005
是红绿灯的等待时间和行进速度的积，所述
Figure 487939DEST_PATH_IMAGE006
是当前节点到达所述备选节点之间的红绿灯数量。
6.根据权利要求1所述寻路方法，其特征在于，所述起点的位置信息根据所述起点的节点ID获取，所述节点ID记录在交叉路口信息表和道路信息表中。
7.根据权利要求1~6所述寻路方法，其特征在于，返回所述最优路径包括：返回道路ID以及车道ID。
8.根据权利要求1~6所述寻路方法，其特征在于，返回所述最优路径还包括：寻路失败信息。
9.一种寻路装置，其特征在于，包括：
定义模块，用于将路径起点定义为当前节点；
遍历模块，判断所述当前节点是否是终点，若否，则遍历所述当前节点的新连接节点获得备选节点；
更新模块，用于根据先验因子排除负节点后计算所述备选节点优先级，将所述优先级最大的所述备选节点的父指针指向当前节点，更新所述备选节点为新的当前节点；
结果模块，用于重复遍历模块和更新模块的操作步骤，直到判断所述当前节点为终点，将所述终点根据父指针的指向回溯到起点的路径作为最优路径。
10.根据权利要求9所述寻路装置，其特征在于，所述更新模块还包括：
先验因子单元，用于根据所述当前节点和所述备选节点的位置，确定所述当前节点指向所述备选节点的第一连线；
计算所述第一连线和所述当前节点指向终点的第二连线之间的夹角，将所述夹角大于90度作为先验因子。
11.根据权利要求10所述寻路装置，其特征在于，所述更新模块还包括：
归置单元，用于将经过遍历的所述当前节点添加到CLOSE表，以及判断所述备选节点是否在OPEN表中；
若否，将所述备选节点的父指针指向当前节点，加入所述OPEN表中；
若是，计算所述备选节点与所述备选节点的父节点的第一距离和所述备选节点经过当前节点到达所述备选节点的父节点的第二距离，若所述第一距离大于第二距离，将所述备选节点的父指针指向当前节点。
12.根据权利要求10所述寻路装置，其特征在于，所述更新模块还包括：
优先级计算单元，用于计算所述备选节点到终点的第一最优步数和所述备选节点到当前节点的第二最优步数；
将所述第一最优步数和第二最优步数的步数之和作为优先级参数。
13.根据权利要求12所述寻路装置，其特征在于，所述更新模块还包括：
红绿灯因子单元，用于根据所述当前节点和备选节点之间的红绿灯数量、所述红绿灯的等待时间以及路径上的行进速度计算红绿灯因子，公式如下：
Figure DEST_PATH_IMAGE007
Figure DEST_PATH_IMAGE009
是红绿灯因子，所述
Figure 52781DEST_PATH_IMAGE005
是红绿灯的等待时间和行进速度的积，所述
Figure 122368DEST_PATH_IMAGE006
是当前节点到达所述备选节点之间的红绿灯数量。
14.一种寻路设备，其特征在于，包括：
显示器，用于显示最优路径；
存储器；用于存储寻路程序；
处理器，用于运行从所述存储器中调取寻路程序，包括：步骤一，将路径起点定义为当前节点；步骤二，判断所述当前节点是否是终点，若否，则遍历所述当前节点的新连接节点获得备选节点；步骤三，根据先验因子排除负节点后计算所述备选节点优先级，将所述优先级最大的所述备选节点的父指针指向当前节点，更新所述备选节点为新的当前节点；步骤四，重复步骤二和步骤三，直到判断所述当前节点为终点，将所述终点根据父指针的指向回溯到起点的路径作为最优路径。
CN202111039750.6A 2021-09-06 2021-09-06 一种寻路方法、装置以及设备 Pending CN113494926A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111039750.6A CN113494926A (zh) 2021-09-06 2021-09-06 一种寻路方法、装置以及设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111039750.6A CN113494926A (zh) 2021-09-06 2021-09-06 一种寻路方法、装置以及设备

Publications (1)

Publication Number Publication Date
CN113494926A true CN113494926A (zh) 2021-10-12

Family

ID=77997213

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111039750.6A Pending CN113494926A (zh) 2021-09-06 2021-09-06 一种寻路方法、装置以及设备

Country Status (1)

Country Link
CN (1) CN113494926A (zh)

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1480867A (zh) * 2002-10-16 2004-03-10 黄珏华 一种电子地图的制作方法和显示方法
CN102506849A (zh) * 2011-09-28 2012-06-20 浙江大学 寻找带约束的最短路径的方法
CN102927990A (zh) * 2012-10-29 2013-02-13 苏州两江科技有限公司 机车通过地理信息系统确定城市道路最优路径的方法
CN103226581A (zh) * 2013-04-02 2013-07-31 浙江大学 一种基于方向寻优的启发式最短路径搜索方法
CN104914862A (zh) * 2015-04-21 2015-09-16 电子科技大学 基于目标方向约束的路径规划算法
US20160035221A1 (en) * 2014-04-29 2016-02-04 Maxwell Consulting, LLC Systems and Methods for Traffic Guidance Nodes and Traffic Navigating Entities
CN106530779A (zh) * 2016-12-29 2017-03-22 长沙理工大学 一种基于城市交通控制信号灯的路径规划方法及系统
CN113110507A (zh) * 2021-05-13 2021-07-13 北京鼎翰科技有限公司 一种自主避障的路径规划方法

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1480867A (zh) * 2002-10-16 2004-03-10 黄珏华 一种电子地图的制作方法和显示方法
CN102506849A (zh) * 2011-09-28 2012-06-20 浙江大学 寻找带约束的最短路径的方法
CN102927990A (zh) * 2012-10-29 2013-02-13 苏州两江科技有限公司 机车通过地理信息系统确定城市道路最优路径的方法
CN103226581A (zh) * 2013-04-02 2013-07-31 浙江大学 一种基于方向寻优的启发式最短路径搜索方法
US20160035221A1 (en) * 2014-04-29 2016-02-04 Maxwell Consulting, LLC Systems and Methods for Traffic Guidance Nodes and Traffic Navigating Entities
CN104914862A (zh) * 2015-04-21 2015-09-16 电子科技大学 基于目标方向约束的路径规划算法
CN106530779A (zh) * 2016-12-29 2017-03-22 长沙理工大学 一种基于城市交通控制信号灯的路径规划方法及系统
CN113110507A (zh) * 2021-05-13 2021-07-13 北京鼎翰科技有限公司 一种自主避障的路径规划方法

Similar Documents

Publication Publication Date Title
US11182624B2 (en) Method, system and memory for constructing transverse topological relationship of lanes in high-definition map
US8681635B2 (en) Computer-implemented systems and methods for planning a route
CN113191550B (zh) 地图匹配方法及装置
US8532922B2 (en) Method for simplifying a description of a route of travel
US20220057231A1 (en) Methods and apparatuses for detecting map calibration errors
CN111488414B (zh) 道路任务匹配方法、装置与设备
CN111380540B (zh) 地图匹配方法及装置、介质、终端
WO2018058888A1 (zh) 一种街景图像的识别方法、装置、服务器及存储介质
EP3633551A1 (en) Obstacle distribution simulation method, device and terminal based on a probability graph
WO2023071029A1 (zh) 地图数据的处理方法、装置、电子设备及存储介质
CN114323037A (zh) 路段位置匹配、导航方法、装置以及存储介质
CN112307151B (zh) 导航数据处理方法及装置
CN113867356A (zh) 机器人路径规划方法、装置及机器人
CN111666359A (zh) Poi候选到达点挖掘方法、装置与设备
CN118031952A (zh) 一种地图场景验证方法、路径规划方法及相关装置
CN112269848A (zh) 一种众包轨迹数据融合方法及装置
CN113494926A (zh) 一种寻路方法、装置以及设备
CN117146797A (zh) 高精地图路口虚拟车道线的调整方法、装置、设备及介质
CN111858785A (zh) 地图离散型要素的匹配方法、装置、系统及存储介质
CN113008246B (zh) 地图匹配方法和装置
CN111506678B (zh) 到达点负样本生成方法、装置与设备
CN114036166A (zh) 高精地图数据更新方法、装置、电子设备以及存储介质
CN105488192A (zh) 点云数据k邻域搜索方法
CN106443732B (zh) 一种基于gps的路径图绘制方法和系统
CN112949358A (zh) 道路描述文件生成方法及设备

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
RJ01 Rejection of invention patent application after publication

Application publication date: 20211012

RJ01 Rejection of invention patent application after publication