CN110057361B - 一种基于gps的最短路径规划方法 - Google Patents

Abstract

本发明提出了一种基于GPS的最短路径规划方法，用于解决现有路径规划方法计算量大、复杂度高或者问题规模较大时搜索时间较长的问题。本发明采用树图的思想对GPS网图中各点进行阶数定义，从起点开始，通过贪婪机制不断选择下一节点，直至终点，确定第一次评估的优解；此基础上进行迭代运算与二次评估，不断更新优解，直至迭代结束，得到最优解；通过贪婪机制快速收敛数据，以较低的复杂度进行二次评估寻找最优解。以一所大学的一个校区为例进行验证：本发明进行路径规划可快速得出可行的最短路径，且规划时间快，所得路径相对于其他路径最短，可运用于汽车导航、旅游导航、物流运输等领域，提高了任务执行效率，市场前景广阔。

Description

一种基于GPS的最短路径规划方法

技术领域

本发明涉及移动机器人或智能车的全局路径规划的技术领域，尤其涉及一种基于GPS的最短路径规划方法。

背景技术

随着科学技术的进步，在智能车控制软件方面，地图包的构建、离散数学的发展对智能车全局路径规划算法的研究带来了便捷，因此在地图已知的情况下发展出了各种路径规划算法。

目前的路径规划方法可分为全局路径规划方法和局部路径规划方法。全局路径规划方法依据已获取的全局环境信息，给机器人规划出一条从起点到终点的运动路径，通常该方法的计算量大，复杂度高。局部路径规划方法是基于传感器获取的信息，在机器人行走过程中不断地确定周围的当前环境情况，并由此规划出一条从当前点到某一子目标点的最优无碰撞路径。由于该方法仅依靠局部信息，有时会产生局部极值点或震荡，无法保证机器人顺利到达目标点。

在预先知道全局环境信息情况下，为保证搜索到一条最优或者近似最优路径，通常使用全局路径规划，目前的全局路径规划的算法主要包括Dijkstra算法和A*算法等。Dijkstra算法的实质是广度优先搜索，是一种发散式的搜索，空间复杂度和时间复杂度都比较高。A*算法则釆用启发式评价函数实现有导向性搜索，算法效率比Dijkstra算法提高很多，但是当问题规模较大时，搜索的时间和空间复杂度仍然较高。

发明内容

针对现有路径规划方法计算量大、复杂度高或者问题规模较大时搜索时间较长的技术问题，本发明提出一种基于GPS的最短路径规划方法，将高德卫星地图与GPS坐标相结合，通过贪婪机制，快速收敛数据，以较低的复杂度进行二次评估寻找最优解，从而在智能车导航系统中可快速得到最优路径。

为了达到上述目的，本发明的技术方案是这样实现的：一种基于GPS的最短路径规划方法，其步骤如下：

步骤一：导入地图包，构建基于GPS的网络拓扑图；将起始点初始化为一阶节点，终点初始化为目的节点，同时初始化一阶节点和目的节点之间的各个二阶节点、三阶节点、……；

步骤二：初始化标定路径，并计算标定路径的长度：选择一阶节点到二阶节点的距离与二阶节点到目的节点的直线距离之和最小的二阶节点作为二阶节点的参考点；选择二阶节点的参考点到三阶节点的距离与三阶节点到目的节点的直线距离之和最小的三阶节点作为三阶节点的参考点，依次类推直至确定目的节点前一阶的参考点，一阶节点、各阶节点的参考点与目的节点确定的路径初始化为标定路径，并计算标定路径距离；

步骤三：重新选择二阶节点的参考点：从二阶节点中去除满足：一阶节点到二阶节点的路径距离与二阶节点到目的节点的直线距离之和大于标定路径距离的二阶节点；再从剩余二阶节点中任选一个二阶节点作为二阶节点的参考点；

步骤四：选择三阶节点至目的节点前一阶的参考点，并更新标定路径和标定路径距离：从三阶节点中去除满足：一阶节点到三阶节点的路径距离与三阶节点到目的节点的直线距离之和大于标定路径距离的三阶节点；再从剩余的三阶节点中选择使一阶节点到三阶节点的路径距离与三阶节点到目的节点的直线距离之和最小的三阶节点作为三阶节点的参考点，依次类推直至目的节点前一阶的参考点，利用步骤三和步骤四选择的各个节点的参考点更新标定路径，根据更新后的标定路径计算标定路径距离，并转到步骤三；

步骤五：若在步骤三和步骤四中选择某阶节点的参考点处选择下一阶节点的参考点时去除了所有下一阶节点，则舍弃该阶节点的参考点并回退至上一阶节点重新选择上一阶节点的参考点；

步骤六：循环迭代步骤三-步骤五，不断更新标定路径及标定路径距离，直到回退至一阶节点，则此时的标定路径即为最优路径。

所述基于GPS的网络拓扑图为：起始点为一阶节点{s₁}，一阶节点是一阶节点的参考点，与一阶节点直接连通的节点为二阶节点{s₂₁，s₂₂，s₂₃，…，s_2j，…}，且j＝1，2，3…；依次类推，i阶节点为{s_i1，s_i2，s_i3，…，s_ij，…}，i＝1，2，3…v，v为节点总阶数，目的节点{s_v}的阶数为节点总阶数v且目的节点为目的节点的参考节点，且各阶节点均有一个参考点s_i；两相邻阶节点的参考点间距离记为u_r(r+1)，节点阶数r＝1，2，3…，v-1，h_r为r阶节点的参考点与目的节点之间的直线距离，一阶节点到i阶节点的路径距离k_ij是非直线的且路径中除最高阶的节点外其余点均为参考点，d_ij为i阶节点到目的节点的直线距离。

所述步骤二中初始化标定路径，并计算标定路径的长度的方法为：

S21：取使一阶节点到二阶节点的距离与二阶节点到目的节点的直线距离之和(u₁₂+d_2j)最小的二阶节点s_2j作为二阶节点的参考点s₂；

S22：取使二阶节点的参考点到三阶节点的距离与三阶节点到目的节点的直线距离之和(u₂₃+d_3j)最小的三阶节点s_3j作为三阶节点的参考点s₃；依次类推，取使(r-1)阶节点的参考点到r阶节点的距离与r阶节点到目的节点的直线距离之和(u_(r-1)r+d_rj)最小的r阶节点s_rj作为r阶节点的参考点s_r；

S23：直至，取使(v-2)阶节点的参考点到(v-1)阶节点的距离与(v-1)阶节点到目的节点的直线距离之和(u_(v-2)(v-1)+d_(v-1)j)最小的(v-1)阶节点s_(v-1)j作为(v-1)阶节点的参考点s_(v-1)；

S24：依次连接相邻各阶节点的参考点及目的节点的路径并初始化为标定路径ξ，计算标定路径距离

所述步骤三中重新选择二阶节点的参考点的方法为：计算一阶节点到二阶节点的路径距离与二阶节点到目的节点的直线距离之和(k_2j+d_2j)，排除原标定路径中满足

的二阶节点，而后从剩余的二阶节点中任选一个二阶节点作为新的二阶节点的参考点。

所述步骤四中选择三阶节点至目的节点前一阶的参考点，并更新标定路径和标定路径距离的方法为：计算一阶节点到三阶节点的路径距离与三阶节点到目的节点的直线距离之和(k_3j+d_3j)，排除原标定路径中满足

的三阶节点，再从剩余三阶节点中选择min(k_3j+d_3j)对应的三阶节点作为新的三阶节点的参考点；依次类推直至选择新的(v-1)阶节点的参考节点，连接一阶节点、新的二阶节点的参考点、新的三阶节点的参考点、……、新的(v-1)阶节点的参考节点和目的节点的路径作为新的标定路径并赋给标定路径ξ，计算新的标定路径的距离并赋给标定路径距离

然后，转到步骤三重新选择各阶节点的参考点。

本发明的有益效果：在PC端导入高德卫星地图，根据GPS坐标进行环境建模，构建GPS网图的拓扑图，采用树图的思想对GPS网图中各点进行阶数定义，从起点开始，通过贪婪机制不断选择下一节点，直至终点，如此进行第一次评估确定一个优解。之后在此基础上进行迭代运算与二次评估，不断更新优解，直至迭代结束，得到最优解；通过贪婪机制，快速收敛数据，以较低的复杂度进行二次评估寻找最优解。本发明以一所大学的一个校区为例进行验证：绘出校区内所有可畅通的路线，并标出各个路口作为网点，并对各网点之间的路段加权，权重为路段距离，选定起始点与目标点，而后采用本发明进行路径规划，即可快速得出可行的最短路径。本发明规划时间快，所得路径相对于其他路径最短，可运用于汽车导航、旅游导航、物流运输等领域，提高了任务执行效率，市场前景广阔。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明基于GPS的网络拓扑图的示例图。

图2为本发明的流程图。

图3为本发明具体使用时实施方法的流程图。

图4为图3中实施方法的基于GPS的网络拓扑图。

图5为利用图3实施方法规划后所得的最优路径。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图2所示，一种基于GPS的最短路径规划方法，步骤一：导入地图包，构建基于GPS的网络拓扑图；将起始点初始化为一阶节点，终点初始化为目的节点，同时初始化一阶节点和目的节点之间的各个二阶节点、三阶节点、……。

基于GPS的网络拓扑图采用树图的思想，基于各个节点的GPS坐标从起始点开始，经过v步到达某一节点，则此节点阶数为v。即GPS网图中每个节点的阶数由从起始点开始的连通性决定，例如图1中，B点既是二阶节点，又是三阶节点。基于GPS的网络拓扑图为：起始点为一阶节点{s₁}，一阶节点是一阶节点的参考点，与一阶节点直接连通的节点为二阶节点{s₂₁，s₂₂，s₂₃，…，s_2j，…}，且j＝1，2，3…；依次类推，i阶节点为{s_i1，s_i2，s_i3，…，s_ij，…}，i＝1，2，3…v，v为节点总阶数，目的节点{s_v}的阶数为节点总阶数v且目的节点为目的节点的参考节点，且各阶节点均有一个参考点s_i。因为一阶节点和目的节点只有一个节点，所以它们本身即是其参考节点。两相邻阶节点的参考点间距离记为u_r(r+1)，节点阶数r＝1，2，3…，v-1，h_r为r阶节点的参考点与目的节点之间的直线距离，一阶节点到i阶节点的路径距离k_ii是非直线的且路径中除最高阶的节点外其余点均为参考点，d_ij为i阶节点到目的节点的直线距离。

步骤二：初始化标定路径，并计算标定路径的长度：选择一阶节点到二阶节点的距离与二阶节点到目的节点的直线距离之和最小的二阶节点作为二阶节点的参考点；选择二阶节点的参考点到三阶节点的距离与三阶节点到目的节点的直线距离之和最小的三阶节点作为三阶节点的参考点，依次类推直至确定目的节点前一阶的参考点，一阶节点、各阶节点的参考点与目的节点确定的路径初始化为标定路径，并计算标定路径距离。

在初始化得到的标定路径中各阶节点的参考节点是该阶节点中与上一阶节点的参考点的距离与到目的节点的直接距离最近的节点为该阶节点的参考点。本发明首先选择二阶节点的参考点，利用贪婪机制不断的选择下一阶节点的参考点，直至终点，从而得到一个最优解作为初始的标定路径。所述步骤二中初始化标定路径，并计算标定路径的长度的方法为：

S21：取使一阶节点到二阶节点的距离与二阶节点到目的节点的直线距离之和(u₁₂+d_2j)最小的二阶节点s_2j作为二阶节点的参考点s₂；u₁₂是各个二阶节点到一阶节点的距离，h₂为各个二阶节点到目的节点的直线距离，使两个距离之和最小的二阶节点为该二阶节点的参考点s₂。

S22：取使二阶节点的参考点到三阶节点的距离与三阶节点到目的节点的直线距离之和(u₂₃+d_3j)最小的三阶节点s_3j作为三阶节点的参考点s₃；依次类推，取使(r-1)阶节点的参考点到r阶节点的距离与r阶节点到目的节点的直线距离之和(u_(r-1)r+d_rj)最小的r阶节点s_rj作为r阶节点的参考点s_r。前面的(r-1)阶节点的参考点已经确定，在求解r阶节点的参考点时，需要求取各个r阶节点与(r-1)阶节点的参考点之间的距离和各个r阶节点与目的节点之间的直线距离之和，选择使上述距离之和最小的r阶节点作为r阶节点的参考节点。

S23：直至，取使(v-2)阶节点的参考点到(v-1)阶节点的距离与(v-1)阶节点到目的节点的直线距离之和(u_(v-2)(v-1)+d_(v-1)j)最小的(v-1)阶节点s_(v-1)j作为(v-1)阶节点的参考点s_(v-1)；

S24：依次连接相邻各阶节点的参考点及目的节点的路径并初始化为标定路径ξ，计算标定路径距离

该步骤通过贪婪机制从起点开始不断选择下一阶节点的参考节点，直至终点，从而得到第一次评估确定的一个最优解。利用最优解来进行二次评估，防止算法进入死循环。

步骤三：重新选择二阶节点的参考点：从二阶节点中去除满足：一阶节点到二阶节点的路径距离与二阶节点到目的节点的直线距离之和大于标定路径距离的二阶节点；再从剩余二阶节点中任选一个二阶节点作为二阶节点的参考点。

所述步骤三中重新选择二阶节点的参考点的方法为：计算一阶节点到二阶节点的路径距离与二阶节点到目的节点的直线距离之和(k_2j+d_2j)，排除原标定路径中满足

的二阶节点，而后从剩余的二阶节点中任选一个二阶节点作为新的二阶节点的参考点。

步骤四：选择三阶节点至目的节点前一阶的参考点，并更新标定路径和标定路径距离：从三阶节点中去除满足：一阶节点到三阶节点的路径距离与三阶节点到目的节点的直线距离之和大于标定路径距离的三阶节点；再从剩余的三阶节点中选择使一阶节点到三阶节点的路径距离与三阶节点到目的节点的直线距离之和最小的三阶节点作为三阶节点的参考点，依次类推直至目的节点前一阶的参考点，利用步骤三和步骤四选择的各个节点的参考点更新标定路径，根据更新后的标定路径计算标定路径距离，并转到步骤三。

所述步骤四中选择三阶节点至目的节点前一阶的参考点，并更新标定路径和标定路径距离的方法为：计算一阶节点到三阶节点的路径距离与三阶节点到目的节点的直线距离之和(k_3j+d_3j)，排除原标定路径中满足

的三阶节点，再从剩余三阶节点中选择min(k_3j+d_3j)对应的三阶节点作为新的三阶节点的参考点；依次类推直至选择新的(v-1)阶节点的参考节点，连接一阶节点、新的二阶节点的参考点、新的三阶节点的参考点、……、新的(v-1)阶节点的参考节点和目的节点的路径作为新的标定路径并赋给标定路径ξ，计算新的标定路径的距离并赋给标定路径距离L；然后，转到步骤三重新选择各阶节点的参考点。

该过程利用步骤二求得一个最优解进行迭代运算进行二次评估，首先删除比最初最优解距离远的各阶节点，然后从剩余的各阶节点中选择路径距离加后续到终点的直线距离最小的节点作为该阶节点的参考点，从而选择不断更新优解，直至迭代结束，得到最后的最优解，因此，本发明在一个最优解的基础上进行二次评估避免了循环算法的死循环，通过与第一次评估得到的最优解相比较去除距离较远的各阶节点然后在剩余的节点中进行搜索，缩短了搜索时间和空间复杂度，减少了计算量和计算时间。

步骤五：若在步骤三和步骤四中选择某阶节点的参考点处选择下一阶节点的参考点时去除了所有下一阶节点，则舍弃该阶节点的参考点并回退至上一阶节点重新选择上一阶节点的参考点。

步骤六：循环迭代步骤三-步骤五，不断更新标定路径及标定路径距离，直到回退至一阶节点，则此时的标定路径即为最优路径。

以图1为例，进行最短路径规划，具体步骤、数据如下：

设各个节点之间的直线距离分别为：L_bp＝16,L_cp＝14,L_dp＝15,L_ep＝12,L_fp＝9.8,L_gp＝14.6，L_hp＝10.2,L_ip＝9.3，L_jp＝9,L_kp＝8.2,L_lp＝6,L_mp＝7.1。首先，采用贪婪机制求取初始标定路径，标定路径求解表如表1所示。

表1标定路径求解表

当前点	连通点	下一地点	判断条件
				A	B,C,E	B	min{L<sub>ab</sub>+L<sub>bp</sub>,L<sub>ac</sub>+L<sub>cp</sub>,L<sub>ae</sub>+L<sub>ep</sub>}＝L<sub>ab</sub>+L<sub>bp</sub>
B	D,E	E	min{L<sub>bd</sub>+L<sub>dp</sub>,L<sub>be</sub>+L<sub>ep</sub>}＝L<sub>be</sub>+L<sub>ep</sub>
				E	C,H,I	I	min{L<sub>ec</sub>+L<sub>cp</sub>,L<sub>eh</sub>+L<sub>hp</sub>,L<sub>ei</sub>+L<sub>ip</sub>}＝L<sub>ei</sub>+L<sub>ip</sub>
I	F,L,M	L	min{L<sub>if</sub>+L<sub>fp</sub>,L<sub>il</sub>+L<sub>lp</sub>,L<sub>im</sub>+L<sub>mp</sub>}＝L<sub>il</sub>+L<sub>lp</sub>
				L	N,O	N	min{L<sub>ln</sub>+L<sub>np</sub>,L<sub>lo</sub>+L<sub>op</sub>}＝L<sub>ln</sub>+L<sub>np</sub>
N	P	P	min{L<sub>np</sub>}＝L<sub>np</sub>

则初始的标定路径为A→B→E→I→L→N→P，从而得到第一次评估确定的一个最优解。

而后求解最优路径，标定路径长度用

来表示，则初始

路径距离用L...表示，例如路径A→B→E→I的路径距离为L_abei，最优路径求解表如表2所示。在第一次评估得到的初始的标定路径的基础上进行运算与二次评估，不断更新优解，直至迭代结束，得到最优解。

表2最优路径求解表

则最优路径为A→E→H→L→N→P。

而后将本发明方法应用在智能车导航系统的实施方法如图3所示，启动地面站软件及职能车，初始化系统，地面站软件与智能车进行无线通信，在地面站软件中导入高德卫星地图，在指定区域初始化基于GPS的拓扑化网图，地面站软件进行路径规划，即可找到最短的标定路径。以河南大学金明校区为例，进行路径规划，系统效果如图4和图5所示，图4为路径规划前的GPS网图，图5为通过本发明方法规划后所得最优路径。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种基于GPS的最短路径规划方法，其特征在于，其步骤如下：

步骤一：导入地图包，构建基于GPS的网络拓扑图；将起始点初始化为一阶节点，终点初始化为目的节点，同时初始化一阶节点和目的节点之间的各个二阶节点、三阶节点、……；

步骤二：初始化标定路径，并计算标定路径的长度：选择一阶节点到二阶节点的距离与二阶节点到目的节点的直线距离之和最小的二阶节点作为二阶节点的参考点；选择二阶节点的参考点到三阶节点的距离与三阶节点到目的节点的直线距离之和最小的三阶节点作为三阶节点的参考点，依次类推直至确定目的节点前一阶的参考点，一阶节点、各阶节点的参考点与目的节点确定的路径初始化为标定路径，并计算标定路径距离；

步骤三：重新选择二阶节点的参考点：从二阶节点中去除满足：一阶节点到二阶节点的路径距离与二阶节点到目的节点的直线距离之和大于标定路径距离的二阶节点；再从剩余二阶节点中任选一个二阶节点作为二阶节点的参考点；

步骤四：选择三阶节点至目的节点前一阶的参考点，并更新标定路径和标定路径距离：从三阶节点中去除满足：一阶节点到三阶节点的路径距离与三阶节点到目的节点的直线距离之和大于标定路径距离的三阶节点；再从剩余的三阶节点中选择使一阶节点到三阶节点的路径距离与三阶节点到目的节点的直线距离之和最小的三阶节点作为三阶节点的参考点，依次类推直至目的节点前一阶的参考点，利用步骤三和步骤四选择的各个节点的参考点更新标定路径，根据更新后的标定路径计算标定路径距离，并转到步骤三；

步骤五：若在步骤三和步骤四中选择某阶节点的参考点处选择下一阶节点的参考点时去除了所有下一阶节点，则舍弃该阶节点的参考点并回退至上一阶节点重新选择上一阶节点的参考点；

步骤六：循环迭代步骤三-步骤五，不断更新标定路径及标定路径距离，直到回退至一阶节点，则此时的标定路径即为最优路径。

2.根据权利要求1所述的基于GPS的最短路径规划方法，其特征在于，所述基于GPS的网络拓扑图为：起始点为一阶节点{s₁}，一阶节点是一阶节点的参考点，与一阶节点直接连通的节点为二阶节点{s₂₁,s₂₂,s₂₃,…,s_2j,…}，且j＝1,2,3…；依次类推，i阶节点为{s_i1,s_i2,s_i3,…,s_ij,…}，i＝1,2,3…v，v为节点总阶数，目的节点{s_v}的阶数为节点总阶数v且目的节点为目的节点的参考节点，且各阶节点均有一个参考点s_i；两相邻阶节点的参考点间距离记为u_r(r+1)，节点阶数r＝1,2,3…,v-1，h_r为r阶节点的参考点与目的节点之间的直线距离，一阶节点到i阶节点的路径距离k_ij是非直线的且路径中除最高阶的节点外其余点均为参考点，d_ij为i阶节点到目的节点的直线距离。

3.根据权利要求2所述的基于GPS的最短路径规划方法，其特征在于，所述步骤二中初始化标定路径，并计算标定路径的长度的方法为：

S21：取使一阶节点到二阶节点的距离与二阶节点到目的节点的直线距离之和(u₁₂+d_2j)最小的二阶节点s_2j作为二阶节点的参考点s₂；

S22：取使二阶节点的参考点到三阶节点的距离与三阶节点到目的节点的直线距离之和(u₂₃+d_3j)最小的三阶节点s_3j作为三阶节点的参考点s₃；依次类推，取使(r-1)阶节点的参考点到r阶节点的距离与r阶节点到目的节点的直线距离之和(u_(r-1)r+d_rj)最小的r阶节点s_rj作为r阶节点的参考点s_r；

S23：直至，取使(v-2)阶节点的参考点到(v-1)阶节点的距离与(v-1)阶节点到目的节点的直线距离之和(u_(v-2)(v-1)+d_(v-1)j)最小的(v-1)阶节点s_(v-1)j作为(v-1)阶节点的参考点s_(v-1)；

S24：依次连接相邻各阶节点的参考点及目的节点的路径并初始化为标定路径ξ，计算标定路径距离

4.根据权利要求2所述的基于GPS的最短路径规划方法，其特征在于，所述步骤三中重新选择二阶节点的参考点的方法为：计算一阶节点到二阶节点的路径距离与二阶节点到目的节点的直线距离之和(k_2j+d_2j)，排除原标定路径中满足

的二阶节点，而后从剩余的二阶节点中任选一个二阶节点作为新的二阶节点的参考点。

5.根据权利要求4所述的基于GPS的最短路径规划方法，其特征在于，所述步骤四中选择三阶节点至目的节点前一阶的参考点，并更新标定路径和标定路径距离的方法为：计算一阶节点到三阶节点的路径距离与三阶节点到目的节点的直线距离之和(k_3j+d_3j)，排除原标定路径中满足

的三阶节点，再从剩余三阶节点中选择min(k_3j+d_3j)对应的三阶节点作为新的三阶节点的参考点；依次类推直至选择新的(v-1)阶节点的参考节点，连接一阶节点、新的二阶节点的参考点、新的三阶节点的参考点、……、新的(v-1)阶节点的参考节点和目的节点的路径作为新的标定路径并赋给标定路径ξ，计算新的标定路径的距离并赋给标定路径距离

然后，转到步骤三重新选择各阶节点的参考点。

Images