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

权利要求书

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中的所有路点即可完成寻路。