基于已有道路数据的仿真车寻找最短路径的方法

摘要

本发明涉及一种基于已有道路数据的仿真车寻找最短路径的方法，包括以下步骤：计算与目标点最近的路点，并将该路点所在路径设置为目标路径；查看仿真车所在路径是否与目标路径相同，如是则计算目标点在当前路径的前方还是后方，如果是前方则得到最短路径为当前路径，否则使用递归计算最短路径的集合和最短距离；如仿真车所在路径是与目标路径不相同，则也使用递归计算最短路径的集合和最短距离。本发明根据路点数据自动寻路，不占用unity中的navigation组件，可以在仿真车行驶时避免打扰其他物体寻路，而且操作简单，反应速度快。

权利要求

1.一种基于已有道路数据的仿真车寻找最短路径的方法，所述道路数据为RoadsData，包括若干条路径Lane，每条路径上间隔预设距离设有路点LanePoint，路径Lane以list形式储存为路径集合LanesData，路点LanePoint以list形式储存为路点集合ListPoint，路点LanePoint在路点集合ListPoint中均按相对应的路径方向顺序排列；每条路径Lane均设有索引LaneID、起始路点PosStart、终止路点PosEnd、路径长度LaneLength以及可变道路径的索引集合List_sameLanesID，其中某路径的可变道路径包括该路径以及与该路径相邻的左、右两侧的路径；所述方法包括以下步骤：步骤一、确定距离目标点TargetPos最近的路点作为目标路点TargetPoint，将目标路点TargetPoint所在的路径作为目标路径TargetLane；

步骤二、然后判断目标路径TargetLane与仿真车所在的当前路径CurrentLane是否为同一路径，分为两种情况：

1）如果目标路径TargetLane与当前路径CurrentLane是同一路径，再判断目标路点TargetPoint与仿真车当前路点CurrentPoint的前后位置，如目标路点TargetPoint与仿真车当前路点CurrentPoint前后位置一致，则代表仿真车已经到达目标点；

如目标路点TargetPoint位于仿真车当前路点CurrentPoint的前方，则最短路径为当前路径CurrentLane；

如目标路点TargetPoint位于仿真车当前路点CurrentPoint的后方，则使用SearchWay递归方法寻找最短路径；

2）如果目标路径TargetLane与当前路径CurrentLane不是同一路径，则使用SearchWay递归方法寻找最短路径；

其中，SeatchWay递归方法需要传入两个参数，分别是路径集合ListLanes以及路径集合ListLanes中的道路长度LanesLength，即路径集合ListLanes中的各路径长度之和；

把目标路径TargetLane加入到路径集合ListLanes中作为路径集合ListLanes初始值，并设置其道路长度LanesLength的初始值为0；

设置一个路径集合ListLane2Target存储最短路径集合，其初始值为空；另外设置一个变量MinDis2Target存储路径集合ListLane2Target中各路径的长度之和并记作最短路径长度，其初始值为无限大；

SeatchWay递归方法的具体执行步骤如下：步骤S1、从路径集合ListLanes中取出最后一个路径LastLane加入最短路径集合ListLane2Target，并遍历路径集合LanesData中的所有路径，找到与路径LastLane相连的路径A；

如果路径集合ListLanes的道路长度LanesLength大于Mindis2Target，则返回重新执行步骤S1；

如果路径A已经包含在路径集合ListLanes中并且不和当前路径CurrentLane为同一条路径则跳过，返回重新执行步骤S1；

如果路径A的List_sameLanesID中包括当前路径CurrentLane的LaneID，则将路径A添加到路径集合ListLanes，然后将MinDis2Target设为路径集合ListLanes的长度LanesLength，并将路径A加入到最短路径集合ListLane2Target中，然后返回重新执行步骤S1；否则执行步骤S2；

步骤S2、向ListLanes中添加此路径A，并将路径集合ListLanes的道路长度LanesLength增加路径A的长度；然后返回执行步骤S1；

上述递归计算完成后，执行步骤S3；

步骤S3、查看变量MinDis2Target，如果为无限大则代表没有找到路径，否则最短路径集合ListLane2Target即为所求路径集合，变量MinDis2Target即为所求最短路径的长度；

步骤S4、仿真车遍历最短路径集合ListLane2Target中的所有路点即可完成寻路。

2.根据权利要求1所述的基于已有道路数据的仿真车寻找最短路径的方法，其特征在于：步骤一中，寻找目标路点TargetPoint的方法为：通过遍历所有LanesData中的所有List_point中的所有LanePoint，计算每个LanePoint与TargetPos的距离，得到距离最小的那个路点作为目标路点TargetPoint。

3.根据权利要求1所述的基于已有道路数据的仿真车寻找最短路径的方法，其特征在于：步骤二中，如果目标路径TargetLane与当前路径CurrentLane是同一路径，再判断目标路点TargetPoint与仿真车当前路点CurrentPoint的前后位置的方法如下：每条路径按顺序对路点设置索引值作为下标，当前路点CurrentPoint在当前路径CurrentLane的下标为currentIndex，目标路点TargetPoint在目标路径TargetLane的下标为targetIndex；如果currentIndex>targetIndex，则代表目标路点TargetPoint在仿真车当前路点CurrentPoint的后方，如果currentIndex=targetIndex，则代表仿真车已经到达目标路点，如果currentIndex

4.根据权利要求1所述的基于已有道路数据的仿真车寻找最短路径的方法，其特征在于：步骤二中，SeatchWay递归方法寻找最短路径时，设置一个阈值限制递归计算次数，即当递归计算次数超出阈值时，停止计算。

说明书

基于已有道路数据的仿真车寻找最短路径的方法

技术领域

[0001] 本发明涉及一种基于已有道路数据的仿真车寻找最短路径的方法，属于自动驾驶技术领域。

背景技术

[0002] 目前使用unity制作的仿真实现自动寻路方式有两种：一种是A星寻路，它需要将寻路地图网格化，过程较为麻烦，并且它是耗时的算法，不宜寻路频繁的场合；另一种是使
用Unity自带组件navigation，navigation组件虽然使用起来方便简单，但是它实现寻路的
时候自身无法正常仿真，会打扰其他物体寻路，影响正常使用，而且navigation组件需要解
决遇到坡度不能倾斜和车轮不转动等问题。

发明内容

[0003] 本发明要解决的技术问题是：提供一种耗时少、计算简单的最短路径寻路方法。

[0004] 为了解决上述技术问题，本发明提出的技术方案是：一种基于已有道路数据的仿真车寻找最短路径的方法，所述道路数据为RoadsData，包括若干条路径Lane，每条路径上
间隔预设距离设有路点LanePoint，路径Lane以list形式储存为路径集合LanesData，路点
LanePoint以list形式储存为路点集合ListPoint，路点LanePoint在路点集合ListPoint中
均按相对应的路径方向顺序排列；每条路径Lane均设有索引LaneID、起始路点PosStart、终
止路点PosEnd、路径长度LaneLength以及可变道路径的索引集合List_sameLanesID，其中
某路径的可变道路径包括该路径以及与该路径相邻的左、右两侧的路径；所述方法包括以
下步骤：

[0005] 步骤一、确定距离目标点TargetPos最近的路点作为目标路点TargetPoint，将目标路点TargetPoint所在的路径作为目标路径TargetLane；

[0006] 步骤二、然后判断目标路径TargetLane与仿真车所在的当前路径CurrentLane是否为同一路径，分为两种情况：

[0007] 1）如果目标路径TargetLane与当前路径CurrentLane是同一路径，再判断目标路点TargetPoint与仿真车当前路点CurrentPoint的前后位置，如目标路点TargetPoint与仿
真车当前路点CurrentPoint前后位置一致，则代表仿真车已经到达目标点；

[0008] 如目标路点TargetPoint位于仿真车当前路点CurrentPoint的前方，则最短路径为当前路径CurrentLane；

[0009] 如目标路点TargetPoint位于仿真车当前路点CurrentPoint的后方，则使用SearchWay递归方法寻找最短路径；

[0010] 2）如果目标路径TargetLane与当前路径CurrentLane不是同一路径，则使用SearchWay递归方法寻找最短路径；

[0011] 其中，SeatchWay递归方法需要传入两个参数，分别是路径集合ListLanes以及路径集合ListLanes中的道路长度LanesLength，即路径集合ListLanes中的各路径长度之和；

[0012] 把目标路径TargetLane加入到路径集合ListLanes中作为路径集合ListLanes初始值，并设置其道路长度LanesLength的初始值为0；

[0013] 设置一个路径集合ListLane2Target存储最短路径集合，其初始值为空；另外设置一个变量MinDis2Target存储路径集合ListLane2Target中各路径的长度之和并记作最短
路径长度，其初始值为无限大；

[0014] SeatchWay递归方法的具体执行步骤如下：

[0015] 步骤S1、从路径集合ListLanes中取出最后一个路径LastLane加入最短路径集合ListLane2Target，并遍历路径集合LanesData中的所有路径，找到与路径LastLane相连的
路径A；

[0016] 如果路径集合ListLanes的道路长度LanesLength大于Mindis2Target，则返回重新执行步骤S1；

[0017] 如果路径A已经包含在路径集合ListLanes中并且不和当前路径CurrentLane为同一条路径则跳过，返回重新执行步骤S1；

[0018] 如果路径A的List_sameLanesID中包括当前路径CurrentLane的LaneID，则将路径A添加到路径集合ListLanes，然后将MinDis2Target设为路径集合ListLanes的长度
LanesLength，并将路径A加入到最短路径集合ListLane2Target中，然后返回重新执行步骤
S1；否则执行步骤S2；

[0019] 步骤S2、向ListLanes中添加此路径A，并将路径集合ListLanes的道路长度LanesLength增加路径A的长度；然后返回执行步骤S1；

[0020] 上述递归计算完成后，执行步骤S3；

[0021] 步骤S3、查看变量MinDis2Target，如果为无限大则代表没有找到路径，否则最短路径集合ListLane2Target即为所求路径集合，变量MinDis2Target即为所求最短路径的长
度；

[0022] 步骤S4、仿真车遍历最短路径集合ListLane2Target中的所有路点即可完成寻路。

[0023] 本发明在自动驾驶仿真中根据已有道路的路径中设置路点（可以在仿真中根据已制作好的路点数据），根据路点数据自动寻路，从而很方便地得到到达目标点的最短路径集
合和道路长度。本发明不占用unity中的navigation组件，可以在仿真车行驶时避免打扰其
他物体寻路，而且本发明方法给出的是最短路径集合，仿真车仅需要跟随车道行驶即可完
成寻路，之后如遇到障碍、交通灯等执行对应方法即可，不会发生冲突或耦合。本发明使用
的路点寻路方法，操作简单，容易理解，计算量小，反应速度快，而且路点跟坡路、车轮转动
等均不相关，因此不受坡度倾斜和车轮不转动问题的影响。

具体实施方式

实施例

[0024] 本实施例中采用的道路数据为RoadsData，包括若干条路径Lane（路径Lane也可以理解为车道），每条路径上间隔预设距离（本实施例中预设距离为1米）设有路点LanePoint，
路径Lane以list形式储存为路径集合LanesData，路点LanePoint以list形式储存为路点集
合ListPoint，路点LanePoint在路点集合ListPoint中均按相对应的路径方向顺序排列。每
条路径Lane均设有索引LaneID、起始路点PosStart、终止路点PosEnd、路径长度LaneLength
以及可变道路径的索引集合List_sameLanesID，其中某路径的可变道路径包括该路径本身
以及与该路径相邻的左、右两侧的路径。

[0025] 本实施例的基于已有道路数据的仿真车寻找最短路径的方法，包括以下步骤：

[0026] 步骤一、确定距离目标点TargetPos最近的路点作为目标路点TargetPoint，将目标路点TargetPoint所在的路径作为目标路径TargetLane。

[0027] 本实施例中寻找目标路点TargetPoint的方法优选为：通过遍历所有LanesData中的所有List_point中的所有LanePoint，计算每个LanePoint与TargetPos的距离，得到距离
最小的那个路点作为目标路点TargetPoint。

[0028] 步骤二、然后判断目标路径TargetLane与仿真车所在的当前路径CurrentLane是否为同一路径，分为两种情况：

[0029] 1）如果目标路径TargetLane与当前路径CurrentLane是同一路径，再判断目标路点TargetPoint与仿真车当前路点CurrentPoint的前后位置，如目标路点TargetPoint与仿
真车当前路点CurrentPoint前后位置一致，则代表仿真车已经到达目标点。如目标路点
TargetPoint位于仿真车当前路点CurrentPoint的前方，则最短路径为当前路径
CurrentLane；

[0030] 如目标路点TargetPoint位于仿真车当前路点CurrentPoint的后方，则使用SearchWay递归方法寻找最短路径。

[0031] 2）如果目标路径TargetLane与当前路径CurrentLane不是同一路径，则使用SearchWay递归方法寻找最短路径。

[0032] 其中，SeatchWay递归方法需要传入两个参数，分别是路径集合ListLanes以及路径集合ListLanes中的道路长度LanesLength，即路径集合ListLanes中的各路径长度之和；

[0033] 把目标路径TargetLane加入到路径集合ListLanes中作为路径集合ListLanes初始值，并设置其道路长度LanesLength的初始值为0；

[0034] 设置一个路径集合ListLane2Target存储最短路径集合，其初始值为空；另外设置一个变量MinDis2Target存储路径集合ListLane2Target中各路径的长度之和并记作最短
路径长度，其初始值为无限大。

[0035] SeatchWay递归方法的具体执行步骤如下：

[0036] 步骤S1、从路径集合ListLanes中取出最后一个路径LastLane加入最短路径集合ListLane2Target，并遍历路径集合LanesData中的所有路径，找到与路径LastLane相连的
路径A；

[0037] 如果路径集合ListLanes的道路长度LanesLength大于Mindis2Target，代表这条路径比当前找到的最短路径长度要长，则返回重新执行步骤S1；

[0038] 如果路径A已经包含在路径集合ListLanes中并且不和当前路径CurrentLane为同一条路径则跳过，代表路径集合ListLanes中已经包含了该路径，如果选择它可能会绕圈不
可能为最短路径，而去掉该路径是为了防止终点被剔除掉，此时返回重新执行步骤S1；

[0039] 如果路径A的List_sameLanesID中包括当前路径CurrentLane的LaneID，则将路径A添加到路径集合ListLanes，然后将MinDis2Target设为路径集合ListLanes的长度
LanesLength，并将路径A加入到最短路径集合ListLane2Target中，然后返回重新执行步骤
S1，代表找到了当前查找到的最短的可以到达目标路径的集合；否则执行步骤S2；

[0040] 步骤S2、向ListLanes中添加此路径A，并将路径集合ListLanes的道路长度LanesLength增加路径A的长度；然后返回执行步骤S1。

[0041] 上述递归计算完成后，执行步骤S3。

[0042] 步骤S3、查看变量MinDis2Target，如果为无限大则代表没有找到路径，可能是最大路径集合数或最大路径总长度设置过小，也可能是目标点所在路径与仿真车当前路径无
法连接；否则最短路径集合ListLane2Target即为所求路径集合，变量MinDis2Target即为
所求最短路径的长度。

[0043] 步骤S4、仿真车遍历最短路径集合ListLane2Target中的所有路点即可完成寻路。

[0044] 步骤二中，在判断目标路点TargetPoint与仿真车当前路点CurrentPoint的前后位置时，本实施例采用的方法优选如下：每条路径按顺序对路点设置索引值作为下标，当前
路点CurrentPoint在当前路径CurrentLane的下标为currentIndex，目标路点TargetPoint
在目标路径TargetLane的下标为targetIndex；如果currentIndex>targetIndex，则代表目
标路点TargetPoint在仿真车当前路点CurrentPoint的后方，如果currentIndex=
targetIndex，则代表仿真车已经到达目标路点，如果currentIndex目标路点TargetPoint在仿真车当前路点CurrentPoint的前方。

[0045] 本实施例还可以作以下改进：步骤二中，SeatchWay递归方法寻找最短路径时，设置一个阈值限制递归计算次数，即当递归计算次数超出阈值时，停止计算，认为找不到最路
径，否则在找不到路径的时候会死循环导致卡死。

[0046] 本实施例方法没有使用unity常用的navagation组件，而是通过寻找路点的方法，使用递归的算法遍历了所有可能得出最短路径，达成自动寻路，计算方法类似A星寻路，但
是通过车道数据降低了很多计算量，并且能跟随车道方向行驶。本实施例可以跨平台运行，
尤其适合AI车辆的仿真。

[0047] 本发明不局限于上述实施例所述的具体技术方案，除上述实施例外，本发明还可以有其他实施方式。对于本领域的技术人员来说，凡在本发明的精神和原则之内，所作的任
何修改、等同替换、改进等形成的技术方案，均应包含在本发明的保护范围之内。