[发明专利]基于已有道路数据的仿真车寻找最短路径的方法

说明书

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

技术领域

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

背景技术

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

发明内容

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

为了解决上述技术问题，本发明提出的技术方案是：一种基于已有道路数据的仿真车寻找最短路径的方法，所述道路数据为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；