CN113720342B - 导航路径规划方法和装置 - Google Patents
导航路径规划方法和装置 Download PDFInfo
- Publication number
- CN113720342B CN113720342B CN202110898323.7A CN202110898323A CN113720342B CN 113720342 B CN113720342 B CN 113720342B CN 202110898323 A CN202110898323 A CN 202110898323A CN 113720342 B CN113720342 B CN 113720342B
- Authority
- CN
- China
- Prior art keywords
- path
- point
- navigation
- target
- edge
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 67
- 238000010586 diagram Methods 0.000 claims description 23
- 230000008569 process Effects 0.000 claims description 13
- 230000000694 effects Effects 0.000 abstract description 3
- 238000005516 engineering process Methods 0.000 abstract description 3
- 239000013598 vector Substances 0.000 description 20
- 238000003780 insertion Methods 0.000 description 9
- 230000037431 insertion Effects 0.000 description 9
- 230000008901 benefit Effects 0.000 description 4
- 241000209094 Oryza Species 0.000 description 2
- 235000007164 Oryza sativa Nutrition 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 235000009566 rice Nutrition 0.000 description 2
- 230000004888 barrier function Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000018109 developmental process Effects 0.000 description 1
- 230000009191 jumping Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000011218 segmentation Effects 0.000 description 1
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
技术领域
本申请涉及导航技术领域,特别是涉及导航路径规划方法和装置。
背景技术
在相关技术中,在对三维场景进行导航路径规划时,可以选择导航网格地图作为导航地图,从而避免无向节点地图的信息丢失和栅格地图的内存限制,同时导航地图还具备栅格地图环境失真度低的优点。
图1是根据相关技术中的基于导航网格地图的路径规划方法的示意图,如图1所示,该方法将场景划分为多个呈凸多边形的可通行区域,并使用漏斗算法,根据凸多边形之间的公共顶点来生成导航路径,但这种方法生成的路径点均为可通行区域之间公共顶点,这些点在实际场景中为障碍物或者墙与可通行区域的边界,生成的路径并不在道路中间。
针对相关技术中,基于导航网格地图的路径规划方法生成的路径并不在道路中间的问题,尚未提出有效的解决方案。
发明内容
本申请实施例提供了一种导航路径规划方法和装置,以至少解决相关技术中基于导航网格地图的路径规划方法生成的路径并不在道路中间的问题。
第一方面,本申请实施例提供了一种导航路径规划方法,基于导航网格地图,所述方法包括:
获取导航起点和导航终点,以所述导航起点为起始点,使用漏斗算法得到第一路径点;
向可通行区域中间移动所述第一路径点,得到第二路径点,连接所述起始点和所述第二路径点,得到第一路径;
确定从所述起始点至所述第二路径点所经过的可通行区域,并在所述可通行区域大于一个的情况下,确定所述第一路径是否与各所述可通行区域间的公共边均相交;
若否,调整所述第一路径,得到与所述公共边均相交的导航子路径。
在其中一些实施例中,所述调整所述第一路径的过程包括:
确定与所述第一路径不相交的公共边,得到第一公共边,确定所述第一公共边的靠近所述起始点一侧的前一条公共边,得到第二公共边;
确定所述第一路径与所述第二公共边的交点,得到第三路径点;
在所述第一公共边上插入第一目标点,连接所述第三路径点和所述第一目标点,得到第二路径,以所述第三路径点为第二目标点的起点,沿所述第二路径长度方向,每间隔第一目标距离移动一次所述第二目标点,直到所述第二目标点与所述第二路径点的连线路径与所述第一公共边相交,并且交点与所述第一公共边的两端点的间距均大于预设的间距值;
依次连接所述起始点和所述第三路径点、所述第三路径点与所述第二目标点、所述第二目标点与所述第二路径点,得到所述导航子路径。
在其中一些实施例中,所述得到第二路径之后,所述方法包括:确定所述第二路径的长度是否小于所述第一目标距离,若是,取第一目标点位置作为所述第二目标点的位置。
在其中一些实施例中,所述得到导航子路径之后,所述方法包括:
更新所述起始点为所述第二路径点,以获取下一段的导航子路径;
在所述起始点与导航终点的连线路径处于可通行区域内的情况下,得到末段的导航子路径;
拼接所有导航子路径,得到导航路径。
在其中一些实施例中,移动所述第一路径点的过程包括:
确定所述第一路径点所在的各区域边中,是否存在至少一条区域边与所述第一路径点所在的公共边平行或近似平行;
若是,从所述第一路径点的位置沿所述第一路径点所在的各区域边间的角平分线的方向移动第二目标距离,得到所述第二路径点;
若否,从所述第一路径点的位置沿所述公共边的方向移动所述第二目标距离,得到所述第二路径点。
在其中一些实施例中,在所述区域边与所述公共边的夹角值大于0度且小于10度,或大于170度且小于180度的情况下,判定所述区域边与所述公共边平行或近似平行。
在其中一些实施例中,所述第二目标距离的确定过程包括:确定所述第一路径点所在的公共边的长度是否大于第一预设阈值,若是,所述第二目标距离为所述第一预设阈值的一半,若否,所述第二目标距离为所述公共边的长度的一半。
在其中一些实施例中,在所述第一公共边上插入所述第一目标点的过程包括:
确定所述第一公共边靠近所述第一路径的端点,得到第一端点;
以所述第一端点为起点,沿所述第一公共边的长度方向移动第三目标距离,插入所述目标点,其中,第三目标距离的确定过程包括:
确定所述第一公共边的长度是否大于第二预设阈值,若是,所述第三目标距离为所述第二预设阈值的一半,若否,所述第三目标距离为所述第一公共边的长度的一半。
在其中一些实施例中,所述使用漏斗算法得到第一路径点的过程包括:
确定所述导航网格地图中的每个可通行区域的中心点,以及确定有公共边的每对可通行区域,连接各对所述可通行区域的中心点,得到无向节点图;
根据所述导航起点、所述导航终点和所述无向节点图,确定从导航起点所在的可通行区域至导航终点所在的可通行区域所经过的可通行区域,得到目标可通行区域的集合;
确定各目标可通行区域间的公共边的集合,根据所述目标可通行区域的集合和所述公共边的集合,输入所述导航起点和所述导航起点所在可通行区域至漏斗算法中,得到所述第一路径点。
第二方面,本申请实施例提供了一种导航路径规划装置,基于导航网格地图,所述装置包括:
获取模块,用于获取导航起点和导航终点,以所述导航起点为起始点,使用漏斗算法得到第一路径点;
移动模块,用于向可通行区域中间移动所述第一路径点,得到第二路径点,连接所述起始点和所述第二路径点,得到第一路径;
确定模块,用于确定从所述起始点至所述第二路径点所经过的可通行区域,并在所述可通行区域大于一个的情况下,确定所述第一路径是否与各所述可通行区域间的公共边均相交;
调整模块,用于在所述第一路径与至少一条所述公共边不相交的情况下,调整所述第一路径,得到与所述公共边均相交的导航子路径。
相比于相关技术,本申请实施例提供的导航路径规划方法,基于导航网格地图,通过获取导航起点和导航终点,以导航起点为起始点,使用漏斗算法得到第一路径点;向可通行区域中间移动第一路径点,得到第二路径点,连接起始点和第二路径点,得到第一路径;确定从起始点至第二路径点所经过的可通行区域,并在可通行区域大于一个的情况下,确定第一路径是否与各可通行区域间的公共边均相交;若否,调整第一路径,得到与公共边均相交的导航子路径,解决了相关技术中基于导航网格地图的路径规划方法生成的路径并不在道路中间的问题,实现了便于用户跟着导航移动的效果。
附图说明
此处所说明的附图用来提供对本申请的进一步理解,构成本申请的一部分,本申请的示意性实施例及其说明用于解释本申请,并不构成对本申请的不当限定。在附图中:
图1是根据相关技术中的基于导航网格地图的路径规划方法的示意图;
图2是根据本申请实施例的导航路径规划方法的应用环境示意图;
图3是根据本申请第一实施例的导航路径规划方法的流程图;
图4是根据本申请第二实施例的无向节点图的创建过程的示意图;
图5是根据本申请第三实施例的导航路径规划方法的流程图;
图6是根据本申请第三实施例的第一路径点的获取过程示意图;
图7是根据本申请第三实施例的平行情况下获取新的路径点的示意图;
图8是根据本申请第四实施例的非平行情况下获取新的路径点的示意图;
图9是根据本申请第五实施例的路径超出可通行区域的示意图;
图10是根据本申请第三实施例的路径调整过程的示意图;
图11是根据本申请第六实施例的导航路径规划方法的示意图。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白,以下结合附图及实施例,对本申请进行描述和说明。应当理解,此处所描述的具体实施例仅仅用以解释本申请,并不用于限定本申请。基于本申请提供的实施例,本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例,都属于本申请保护的范围。
显而易见地,下面描述中的附图仅仅是本申请的一些示例或实施例,对于本领域的普通技术人员而言,在不付出创造性劳动的前提下,还可以根据这些附图将本申请应用于其他类似情景。此外,还可以理解的是,虽然这种开发过程中所作出的努力可能是复杂并且冗长的,然而对于与本申请公开的内容相关的本领域的普通技术人员而言,在本申请揭露的技术内容的基础上进行的一些设计,制造或者生产等变更只是常规的技术手段,不应当理解为本申请公开的内容不充分。
在本申请中提及“实施例”意味着,结合实施例描述的特定特征、结构或特性可以包含在本申请的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例,也不是与其它实施例互斥的独立的或备选的实施例。本领域普通技术人员显式地和隐式地理解的是,本申请所描述的实施例在不冲突的情况下,可以与其它实施例相结合。
除非另作定义,本申请所涉及的技术术语或者科学术语应当为本申请所属技术领域内具有一般技能的人士所理解的通常意义。本申请所涉及的“一”、“一个”、“一种”、“该”等类似词语并不表示数量限制,可表示单数或复数。本申请所涉及的术语“包括”、“包含”、“具有”以及它们任何变形,意图在于覆盖不排他的包含;例如包含了一系列步骤或模块(单元)的过程、方法、系统、产品或设备没有限定于已列出的步骤或单元,而是可以还包括没有列出的步骤或单元,或可以还包括对于这些过程、方法、产品或设备固有的其它步骤或单元。本申请所涉及的“连接”、“相连”、“耦接”等类似的词语并非限定于物理的或者机械的连接,而是可以包括电气的连接,不管是直接的还是间接的。本申请所涉及的“多个”是指两个或两个以上。“和/或”描述关联对象的关联关系,表示可以存在三种关系,例如,“A和/或B”可以表示:单独存在A,同时存在A和B,单独存在B这三种情况。字符“/”一般表示前后关联对象是一种“或”的关系。本申请所涉及的术语“第一”、“第二”、“第三”等仅仅是区别类似的对象,不代表针对对象的特定排序。
本申请提供的导航路径规划方法,可以应用于如图2所示的应用环境中,图2是根据本申请实施例的导航路径规划方法的应用环境示意图,如图2所示,服务器201获取导航起点和导航终点,以导航起点为起始点,使用漏斗算法得到第一路径点;向可通行区域中间移动第一路径点,得到第二路径点,连接起始点和第二路径点,得到第一路径;确定从起始点至第二路径点所经过的可通行区域,并在可通行区域大于一个的情况下,确定第一路径是否与各可通行区域间的公共边均相交;若否,调整第一路径,得到与公共边均相交的导航子路径,实现导航路径规划,其中,服务器201可以用独立的服务器或者是多个服务器组成的服务器集群来实现。
本实施例提供了一种导航路径规划方法,图3是根据本申请第一实施例的导航路径规划方法的流程图,如图3所示,该流程包括如下步骤:
步骤S301,获取导航起点和导航终点,以导航起点为起始点,使用漏斗算法得到第一路径点;
步骤S302,向可通行区域中间移动第一路径点,得到第二路径点,连接起始点和第二路径点,得到第一路径;
步骤S303,确定从起始点至第二路径点所经过的可通行区域,并在该可通行区域大于一个的情况下,确定第一路径是否与各可通行区域间的公共边均相交;
步骤S304,若否,调整第一路径,得到与公共边均相交的导航子路径。
通过步骤S301至S304,相对于相关技术中基于导航网格地图的路径规划方法生成的路径并不在道路中间的问题,本实施例基于导航网格地图,通过将漏斗算法搜索到的路径点移动到靠近可通行区域的中间位置,并考虑到移动路径点之后会导致路径穿出可通行区域的情况,对移动后的路径点与起始点所形成的新的路径进行调整,由于导航网格地图将场景划分为多个呈凸多边形的可通行区域,且由于凸多边形内任意两点组成的线段必定在该可通行区域内,所以只要路径线段与该线段两端点之间的可通行区域的所有公共边都相交,该路径线段中的任意子线段一定在可通行区域内,因此在调整路径时,使该路径与该路径两端点之间的可通行区域间的各公共边均相交,可以得到在可通行区域内的导航子路径,从而解决了相关技术中基于导航网格地图的路径规划方法生成的路径并不在道路中间的问题,实现了便于用户跟着导航移动的效果。
此外,本申请实施例还至少具有以下两个有益效果:
第一,由于本申请实施例充分利用了可通行区域凸多边形特性以及公共边的约束,使得路径生成不依赖于标注的场景地图,只要符合可通行区域是凸多边形,该方法生成的路径一定在可通行区域上且靠近道路中间,方法简单有效,可通用性强,计算复杂度低,具有较好的适用性;
第二,由于本申请实施例解决了相关技术中基于导航网格地图的路径规划方法生成的路径并不在道路中间的问题,当本导航路径规划方法应用于AR导航时,不容易发生AR物体穿模,使得AR效果更逼真。
在其中一些实施例中,图4是根据本申请第二实施例的无向节点图的创建过程的示意图,如图4所示,在路径规划之前,通过区域分割算法将实际环境转换为由多块凸多边形(可通行区域)组成的导航网格地图,计算导航网格地图中的每个可通行区域的中心点坐标,假设第i个可通行区域有m个顶点,那么该可通行区域的中心点坐标的计算公式为:
由于可通行区域均为凸多边形,所以得到的中心点一定在对应可通行区域的内部;
如果两个可通行区域之间有公共边,那么连接这两个可通行区域的中心点,例如Ri与Rj之间有公共边ViVj,连接这两个可通行区域的中心点Ci和Cj,用<Ci,Cj>和<Cj,Ci>表示,最后连接导航网格图中所有相互关联的可通行区域中心点,就可以得到表示可通行区域连接关系的无向节点图:
GRC=[<Ci,Cj>,<Cj,Ci>,...,<Cn,Cm>]。
在其中一些实施例中,图5是根据本申请第三实施例的导航路径规划方法的流程图,如图5所示,该流程包括如下步骤:
步骤S501,在输入起点为S和终点为G的情况下,根据位置信息可分别获取起点和终点所在的区域为Rs和Rg,然后将得到的可通行区域作为输入,通过A*算法在构建的无向节点图中搜索得到从Rs到Rg需经过区域Rp以及对应的公共边Ec;
步骤S502,图6是根据本申请第三实施例的第一路径点的获取过程示意图,如图6所示,以起点S及其所属可通行区域Rs=R13为初值,利用漏斗算法获取第一路径点Vf=V5,并获取第一条路径P1=SVf;
步骤S503,由于漏斗算法获取的路径点都位于可通行区域边界上,所以需要移动路径点使得生成的路径靠近可通行区域中间,移动的规则如下:
<1>取Rp中与路径点Vf关联的边Ej=[VfVi,VfVj,VfVk],对于区域集Rp中的每个可通行区域来说,每个顶点至多只能关联到三条边,分别为区域之间的公共边和区域边;
<2>以路径点Vf为起点,将公共边Vf Vi、区域边Vf Vj和区域边Vf Vk转换为向量和/>然后计算向量/>和/>以及向量/>和/>之间的夹角θ1和θ2([0°,180°]),利用向量之间的夹角来判断公共边与其他两条区域边是否平行,判断条件为:向量夹角小于10度或者大于170度,则公共边与对应区域边平行;
<3>如果两条区域边中的任意一条边与公共边平行,则计算两条区域边对应向量和/>之间夹角的角平分线向量:
图7是根据本申请第三实施例的平行情况下获取新的路径点的示意图,如图7所示,将路径点Vf沿着向量的反方向移动dmove米获得新的路径点Vf',其中dmove数值取值规则如下:如果公共边长度Lc大于预设阈值Lmax,dmove=Lmax/2,如果公共边长度Lc小于Lmax,那么dmove=Lc/2;
<4>如果两条区域边与公共边都不平行,图8是根据本申请第四实施例的非平行情况下获取新的路径点的示意图,如图8所示,将路径点Vf沿着向量方向移动dmove米获得新的路径点Vf';
步骤S504,图9是根据本申请第五实施例的路径超出可通行区域的示意图,如图9所示,移动路径点Vf得到新的路径点Vf'之后,得到P1=SVf,Pnew=SVf',移动路径点方法可对移动的路径点产生约束,但两个相邻路径点相连产生的线段无法受到可通行区域的约束,从而产生Pnew的部分路径不在可通行区域内的情况,为了能够使路径点移动后产生的路径依然在可通行区域内,可以利用以下性质:凸多边形内任意两点组成的线段必定在该可通行区域内,所以只要路径线段与起始点和终点之间可通行区域的所有公共边相交,那么路径线段中的任意子线段一定在可通行区域内;在步骤S503移动路径点之后,可获取从路径起点到终点需经过的区域Rp=[Ri,...,Rn]以及公共边集合Ecp=[ViVj,...,VnVm],判断移动路径点之后的路径Pnew=SVf'与公共边集合Ecp中的每一条公共边是否相交,如果路径Pnew与其中某一条公共边不相交则表示路径部分线段不在可通行区域之内,执行步骤S505,如果新路径与所有公共边相交或者路径起点和终点在同一个可通行区域内,则将新路径Pnew保存至路径集合中并执行步骤S506;
步骤S505,如果新路径与某一条公共边不相交,那么对新路径进行调整,图10是根据本申请第三实施例的路径调整过程的示意图,如图10所示,该过程包括如下步骤:
<1>假设新路径Pnew与由区域点Vi和区域点Vj组成的公共边Eci不相交,且Eci对应的区域为Ri和Ri+1,新路径与当前公共边Eci前一个公共边Eci-1的交点为IPs,可得路径Psi=SIPs(特别的,如图9所示,当公共边Eci没有前一个公共边Eci-1时(即公共边Eci为第一条公共边),IPs=S),由于路径Psi的两个端点都属于同一个区域R,所以路径Psi必满足在可通行区域内的条件,将路径Psi保存至路径集合中;
<2>公共边Eci中插入点I,插入规则如下:如果路径Pnew靠近公共边Eci端点Vi,那么以Vi为起点,朝着区域点Vj方向dinsert米插入点I,如果公共边Eci长度Lci大于预设阈值Lmax,那么取dinsert=Lmax/2,如果Lci小于Lmax,那么取dinsert=Lci/2;
<3>连接点IPs和点I构成向量并在向量/>中插入点IPi,插入规则如下:初始阶段设上一个路径插入点IPi-1=IPs,如果向量长度小于1m,直接取IPi=I,否则距离上一个路径插入点1m位置插入新路径点IPi,用向量表示为:
<4>以当前插入点IPi为起点,Vf'为路径终点构成路径Pinsert=IPiVf',判断路径Pinsert与公共边Eci是否相交,如果相交且交点与点Vi的距离大于设定值dset,dset可根据实际需求自主设置,例如,dset=0.5m,设置dset的目的是为了防止导航路径中经过的点为区域边界点,当然,在其他实施例中,也可以不设置交点与点Vi的距离大于设定值dset的限制条件;将路径Pmid=IPsIPi和路径Pinsert保存至路径集合中,执行步骤S506,否则跳至步骤S505的第<3>步;
步骤S506,将其终点Vf'作为新起点SP=Vf';
步骤S507,判断新起点SP与输入终点G是否可形成路径Pfinal=SPG,且路径Pfinal符合在可通行区域内的条件,若否,返回步骤S502继续执行;
步骤S508,若是,输出路径结果为[S,IPs,IPi,Vf',…,G]。
在其中一些实施例中,图11是根据本申请第六实施例的导航路径规划方法的示意图,如图11所示,场景对应的导航网格图为Pnav,Pnav=[R1,R2,R3,R4,R5,R6,R7,R8,R9,R10,R11,R12,R13],计算每个导航网格地图中的可通行区域中心点Cnav=[C1,...,C13],根据Pnav中的可通行区域之间的连接关系,可得:
GRC=[<C1,C5>,<C5,C1>,...,<C12,C13>],
如图11所示,该导航路径规划方法的执行过程包括如下步骤:
(1)输入起点S和终点G,由这两个点的位置信息可得对应的区域为R13和R1,通过A*算法可在GRC中搜索得到需要经过的区域为Rp=[R13,R12,R11,R9,R7,R6,R5,R1],对应的公共边Ec=[V1V2,V3V4,V5V6,V7V8,V9V8,V10V11,V12V13];
(2)以起点S及其所属可通行区域Rs为初值,利用漏斗算法可以获取第一个路径点为V5,对应的路径为P1=SV5;而顶点V5所对应的三条边为Ej=[V5V6,V5V3,V5V7],其中V5V6是区域R9和区域R11之间的公共边,V5V3为区域R11的边,V5V7为区域R9的边;
(3)以路径点V5为起点,可获得向量和/>向量/>和/>之间的夹角为90度,而向量/>和/>之间的夹角为180度,向量/>和/>之间的夹角大于170度,公共边V5V6与区域边V5V3平行,路径点沿着两条区域边向量夹角的角平分线向量反方向移动,生成点V5';
(4)连接点S和V5'形成路径Pnew=SV5',已知点S位于区域R13,点V5'位于区域R11,所以路径穿过区域R13、R12和R11,如果整条路径在可通行区域内需满足与公共边V1V2和V3V4都相交,而路径Pnew与公共边V3V4不相交,所以需要对路径Pnew调整使得路径Pnew受到公共边V3V4的约束;
(5)已知公共边V3V4的上一条公共边为V1V2,而路径Pnew与公共边V1V2的交点为IPs,可得路径Psi=SIPs,由于路径Psi的两个端点都属于区域R13,所以路径Psi必满足可通行区域内的条件,将其保存至路径集合中;
(6)在公共边V3V4中插入点I,通过几何关系可知路径Pnew位于点V4的上方,以点V4为起点,沿着向量方向dinsert米位置插入点I;
(7)连接点IPs和点I构成向量,以点IPs为起点,沿着向量方向1m位置插入点IP1;以点IP1为起点,V5'为终点构成路径Pinsert=IP1V5',判断路径Pinsert与公共边V3V4是否相交,结果为不相交,那么以IP1为起点并沿着向量/>方向1m位置插入点IP2,直到找到点IP3符合要求,然后以点IPs为起点,点IP3为终点构成路径Pmid=IPsIP3,将路径Pmid保存至路径集合中,同时将插入路径Pinsert=IP3V5'也保存至路径集合中;
(8)将路径V5'作为新的起点Sp=V5',返回步骤(2),直到新起点为V12'并与终点G形成路径Pfinal=V12'G,且该路径Pfinal符合均在可通行区域内的条件,最终可得路径为[S,IPs,IP3,V5',V8',V12',G]。
本领域的技术人员应该明白,以上实施例的各技术特征可以进行任意的组合,为使描述简洁,未对上述实施例中的各个技术特征所有可能的组合都进行描述,然而,只要这些技术特征的组合不存在矛盾,都应当认为是本说明书记载的范围。
以上实施例仅表达了本申请的几种实施方式,其描述较为具体和详细,但并不能因此而理解为对发明专利范围的限制。应当指出的是,对于本领域的普通技术人员来说,在不脱离本申请构思的前提下,还可以做出若干变形和改进,这些都属于本申请的保护范围。因此,本申请专利的保护范围应以所附权利要求为准。
Claims (9)
1.一种导航路径规划方法,基于导航网格地图,其特征在于,所述方法包括:
获取导航起点和导航终点,以所述导航起点为起始点,使用漏斗算法得到第一路径点;
向可通行区域中间移动所述第一路径点,得到第二路径点,连接所述起始点和所述第二路径点,得到第一路径;
确定从所述起始点至所述第二路径点所经过的可通行区域,并在所述可通行区域大于一个的情况下,确定所述第一路径是否与各所述可通行区域间的公共边均相交;
若否,调整所述第一路径,得到与所述公共边均相交的导航子路径,包括:确定与所述第一路径不相交的公共边,得到第一公共边,确定所述第一公共边的靠近所述起始点一侧的前一条公共边,得到第二公共边;
确定所述第一路径与所述第二公共边的交点,得到第三路径点;
在所述第一公共边上插入第一目标点,连接所述第三路径点和所述第一目标点,得到第二路径,以所述第三路径点为第二目标点的起点,沿所述第二路径长度方向,每间隔第一目标距离移动一次所述第二目标点,直到所述第二目标点与所述第二路径点的连线路径与所述第一公共边相交,并且交点与所述第一公共边的两端点的间距均大于预设的间距值;
依次连接所述起始点和所述第三路径点、所述第三路径点与所述第二目标点、所述第二目标点与所述第二路径点,得到所述导航子路径。
2.根据权利要求1所述的方法,其特征在于,所述得到第二路径之后,所述方法包括:确定所述第二路径的长度是否小于所述第一目标距离,若是,取第一目标点位置作为所述第二目标点的位置。
3.根据权利要求1所述的方法,其特征在于,所述得到导航子路径之后,所述方法包括:
更新所述起始点为所述第二路径点,以获取下一段的导航子路径;
在所述起始点与导航终点的连线路径处于可通行区域内的情况下,得到末段的导航子路径;
拼接所有导航子路径,得到导航路径。
4.根据权利要求1所述的方法,其特征在于,移动所述第一路径点的过程包括:
确定所述第一路径点所在的各区域边中,是否存在至少一条区域边与所述第一路径点所在的公共边平行或近似平行;
若是,从所述第一路径点的位置沿所述第一路径点所在的各区域边间的角平分线的方向移动第二目标距离,得到所述第二路径点;
若否,从所述第一路径点的位置沿所述公共边的方向移动所述第二目标距离,得到所述第二路径点。
5.根据权利要求4所述的方法,其特征在于,在所述区域边与所述公共边的夹角值大于0度且小于10度,或大于170度且小于180度的情况下,判定所述区域边与所述公共边平行或近似平行。
6.根据权利要求4所述的方法,其特征在于,所述第二目标距离的确定过程包括:确定所述第一路径点所在的公共边的长度是否大于第一预设阈值,若是,所述第二目标距离为所述第一预设阈值的一半,若否,所述第二目标距离为所述公共边的长度的一半。
7.根据权利要求1所述的方法,其特征在于,在所述第一公共边上插入所述第一目标点的过程包括:
确定所述第一公共边靠近所述第一路径的端点,得到第一端点;
以所述第一端点为起点,沿所述第一公共边的长度方向移动第三目标距离,插入所述目标点,其中,第三目标距离的确定过程包括:
确定所述第一公共边的长度是否大于第二预设阈值,若是,所述第三目标距离为所述第二预设阈值的一半,若否,所述第三目标距离为所述第一公共边的长度的一半。
8.根据权利要求1所述的方法,其特征在于,所述使用漏斗算法得到第一路径点的过程包括:
确定所述导航网格地图中的每个可通行区域的中心点,以及确定有公共边的每对可通行区域,连接各对所述可通行区域的中心点,得到无向节点图;
根据所述导航起点、所述导航终点和所述无向节点图,确定从导航起点所在的可通行区域至导航终点所在的可通行区域所经过的可通行区域,得到目标可通行区域的集合;
确定各目标可通行区域间的公共边的集合,根据所述目标可通行区域的集合和所述公共边的集合,输入所述导航起点和所述导航起点所在可通行区域至漏斗算法中,得到所述第一路径点。
9.一种导航路径规划装置,基于导航网格地图,其特征在于,所述装置包括:
获取模块,用于获取导航起点和导航终点,以所述导航起点为起始点,使用漏斗算法得到第一路径点;
移动模块,用于向可通行区域中间移动所述第一路径点,得到第二路径点,连接所述起始点和所述第二路径点,得到第一路径;
确定模块,用于确定从所述起始点至所述第二路径点所经过的可通行区域,并在所述可通行区域大于一个的情况下,确定所述第一路径是否与各所述可通行区域间的公共边均相交;
调整模块,用于在所述第一路径与至少一条所述公共边不相交的情况下,调整所述第一路径,得到与所述公共边均相交的导航子路径,包括:确定与所述第一路径不相交的公共边,得到第一公共边,确定所述第一公共边的靠近所述起始点一侧的前一条公共边,得到第二公共边;
确定所述第一路径与所述第二公共边的交点,得到第三路径点;
在所述第一公共边上插入第一目标点,连接所述第三路径点和所述第一目标点,得到第二路径,以所述第三路径点为第二目标点的起点,沿所述第二路径长度方向,每间隔第一目标距离移动一次所述第二目标点,直到所述第二目标点与所述第二路径点的连线路径与所述第一公共边相交,并且交点与所述第一公共边的两端点的间距均大于预设的间距值;
依次连接所述起始点和所述第三路径点、所述第三路径点与所述第二目标点、所述第二目标点与所述第二路径点,得到所述导航子路径。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110898323.7A CN113720342B (zh) | 2021-08-05 | 2021-08-05 | 导航路径规划方法和装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110898323.7A CN113720342B (zh) | 2021-08-05 | 2021-08-05 | 导航路径规划方法和装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113720342A CN113720342A (zh) | 2021-11-30 |
CN113720342B true CN113720342B (zh) | 2024-03-26 |
Family
ID=78674985
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110898323.7A Active CN113720342B (zh) | 2021-08-05 | 2021-08-05 | 导航路径规划方法和装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113720342B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115509240A (zh) * | 2022-11-21 | 2022-12-23 | 合肥井松智能科技股份有限公司 | 一种基于可通行区域的路径规划方法 |
Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2010223648A (ja) * | 2009-03-23 | 2010-10-07 | Denso It Laboratory Inc | ナビゲーションシステム、ナビゲーション装置、センター装置、ナビゲーション方法およびプログラム |
CN103837154A (zh) * | 2014-03-14 | 2014-06-04 | 北京工商大学 | 路径规划的方法及系统 |
CN104142156A (zh) * | 2014-08-26 | 2014-11-12 | 天津市普迅电力信息技术有限公司 | 一种路径导航方法 |
CN109154510A (zh) * | 2016-05-17 | 2019-01-04 | 微软技术许可有限责任公司 | 基于指定的中间停靠点来计算最佳路线 |
CN109668561A (zh) * | 2017-10-13 | 2019-04-23 | 中兴通讯股份有限公司 | 一种室内路径规划方法、终端及可读存储介质 |
CN111773717A (zh) * | 2020-06-30 | 2020-10-16 | 完美世界(北京)软件科技发展有限公司 | 对象控制方法和装置、存储介质和电子装置 |
CN111815778A (zh) * | 2020-06-22 | 2020-10-23 | 北京优锘科技有限公司 | 基于地面模型生成导航路径的方法 |
KR20210007406A (ko) * | 2019-07-11 | 2021-01-20 | 재단법인차세대융합기술연구원 | 최단 경로를 산출하는 방법, 컴퓨터로 판독 가능한 저장매체 및 시스템 |
CN112684789A (zh) * | 2019-10-17 | 2021-04-20 | 诺基亚通信公司 | 控制设备的移动 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US10901425B2 (en) * | 2018-11-30 | 2021-01-26 | Honda Motor Co., Ltd. | Systems and methods for navigational planning |
-
2021
- 2021-08-05 CN CN202110898323.7A patent/CN113720342B/zh active Active
Patent Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2010223648A (ja) * | 2009-03-23 | 2010-10-07 | Denso It Laboratory Inc | ナビゲーションシステム、ナビゲーション装置、センター装置、ナビゲーション方法およびプログラム |
CN103837154A (zh) * | 2014-03-14 | 2014-06-04 | 北京工商大学 | 路径规划的方法及系统 |
CN104142156A (zh) * | 2014-08-26 | 2014-11-12 | 天津市普迅电力信息技术有限公司 | 一种路径导航方法 |
CN109154510A (zh) * | 2016-05-17 | 2019-01-04 | 微软技术许可有限责任公司 | 基于指定的中间停靠点来计算最佳路线 |
CN109668561A (zh) * | 2017-10-13 | 2019-04-23 | 中兴通讯股份有限公司 | 一种室内路径规划方法、终端及可读存储介质 |
KR20210007406A (ko) * | 2019-07-11 | 2021-01-20 | 재단법인차세대융합기술연구원 | 최단 경로를 산출하는 방법, 컴퓨터로 판독 가능한 저장매체 및 시스템 |
CN112684789A (zh) * | 2019-10-17 | 2021-04-20 | 诺基亚通信公司 | 控制设备的移动 |
CN111815778A (zh) * | 2020-06-22 | 2020-10-23 | 北京优锘科技有限公司 | 基于地面模型生成导航路径的方法 |
CN111773717A (zh) * | 2020-06-30 | 2020-10-16 | 完美世界(北京)软件科技发展有限公司 | 对象控制方法和装置、存储介质和电子装置 |
Non-Patent Citations (4)
Title |
---|
三维场景中角色自动寻径算法研究;孟;刘邦权;;武汉理工大学学报(12);全文 * |
基于不规则障碍物环境下无人机的改进几何路径规划算法;凌富园;杜承烈;孙宝亮;顾青;;航空电子技术(04);全文 * |
基于船舶火灾蔓延态势的路径规划算法;孟顺;船舶工程;第43卷(第2期);第110-115页 * |
引入导航网格的室内路径规划算法;林巍凌;;测绘科学(02);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN113720342A (zh) | 2021-11-30 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN103745452B (zh) | 相机外参评估方法、装置、相机外参标定方法和装置 | |
CN111158365A (zh) | 一种路径规划方法、装置、机器人及存储介质 | |
CN105103195A (zh) | 图像处理 | |
CN113720342B (zh) | 导航路径规划方法和装置 | |
JP2019220099A (ja) | ステレオマッチング処理装置、ステレオマッチング処理方法、及びプログラム | |
WO2024056064A1 (zh) | 转弯路径规划方法、设备、车辆及存储介质 | |
CN105574875B (zh) | 一种基于极曲线几何的鱼眼图像稠密立体匹配方法 | |
CN113658342B (zh) | Ar导航路径生成的方法、系统、电子装置和存储介质 | |
JP3707770B2 (ja) | 地図情報修正装置および地図情報修正方法 | |
CN112629558A (zh) | 车辆惯导匹配矫正方法和装置、设备及存储介质 | |
JP2021002247A (ja) | 情報処理システム | |
US20150131914A1 (en) | Shape data generation method and apparatus | |
JP2802034B2 (ja) | 三次元物体の計測方法 | |
WO2023088127A1 (zh) | 室内导航方法、服务器、装置和终端 | |
JP2018132451A (ja) | 位置計測方法および装置 | |
JP6936673B2 (ja) | 地図データ更新システムおよび地図データ更新プログラム | |
CN104408710A (zh) | 一种全局视差估计方法和系统 | |
US11835347B2 (en) | Route search device, route search method, and route search program | |
CN103810700A (zh) | 基于深度图像利用遮挡信息确定下一最佳观测方位的方法 | |
CN112580744A (zh) | 众包数据道路片段中同一车道线测量数据的优化融合方法 | |
JP5323863B2 (ja) | 重複ポリトープを介した幾何学的要素による点の順序集合の近似 | |
US20110050694A1 (en) | System and method for rendering hair image | |
JP2755293B2 (ja) | ポリゴンメッシュ接続装置 | |
JP6469326B2 (ja) | 地図データ生成装置、端末装置、及び、地図データ生成方法 | |
CN116558542A (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 | ||
GR01 | Patent grant | ||
GR01 | Patent grant |