一种多目标航点规划方法及系统

技术领域

本发明涉及导航技术领域，特别涉及一种融合改进A*算法与SPFA算法的多目标航点规划方法及系统。

背景技术

随着现代科学技术的发展，自动导航技术无论是在自动驾驶还是移动导航机器人方面都起着至关重要的作用，而路径规划是实现移动机器人自动导航过程中最重要且不可或缺的一环。在复杂环境多目标航点任务规划的情况下，不同航点顺序的排序，需要人为在不同程度上给予干预，无法实现真正意义上的全自主路径规划导航。所以如何能够实现移动导航机器人在多航点任务下自主选取最优路径，便成为了当前的一大主流问题。

目前，常用于移动导航机器人全局路径规划的算法有A*算法，Dijkstra算法，RRT算法等。常用于移动导航机器人的局部路径规划有DWA算法，人工势场算法，AGV算法等。当前，A*算法主要被广泛的应用于全局路径规划中，凭借着其本身路径搜索的稳定性，成为了移动导航机器人路径规划应用最广泛的算法之一。如刘建娟等人在《融合改进A*与DWA算法的机器人动态路劲规划》中，公开了基于Floyd算法思想设计路径节点优化算法,删除冗余节点，减少转折，提高路径平滑度；基于全局最优设计DWA算法的动态窗口评价函数，用于区分已知障碍物和未知动态、静态障碍物，提取改进A*算法规划路径的关键点作为DWA算法的临时目标点，在全局最优的基础上实现了改进A*与DWA算法融合。

然而，路径中存在的大量冗余节点和曲折拐点，路径长度并非最短距离且运算时间较长，并且地图中的还会存在U型死角点与伪强迫邻居节点，无法有效地规划出合理的可行路径，另外在强迫邻居节点处b样条在规划路径拐角处平滑度不够，使得路径不能更加的贴近移动导航机器人在实际环境下的行进路线，因此，在复杂环境下的多目标航点任务中规划出合理且快捷的路径面临一定的挑战。

发明内容

基于此，本发明的目的是提出一种融合改进A*算法与SPFA算法的多目标航点规划方法及系统，目的在于实现在复杂环境下的多目标航点任务中规划出合理且快捷的可行路径。

根据本发明提出的一种融合改进A*算法与SPFA算法的多目标航点规划方法，所述方法包括：

对目标环境进行二维栅格化，以生成栅格化地图，在所述栅格化地图中标定导航器的起点和目标点集合；

对传统A*算法进行改进，以得到改进A*算法，再根据所述改进A*算法计算所述起点到各个目标点之间的代价值，将各个代价值记录于代价数组；

给所述代价数组添加权值，以得到具有高优先级的有向含负权图，将所述有向含负权图输入到SPFA算法中计算所述起点到所述目标点的单源最短路径，以高优先级作为阶段性终点，得到所述目标点集合的航点顺序，并将所述航点顺序记录于最短路径数组；

建立当前变量和目标变量，分别作为当前点索引和目标点索引，所述当前点索引指向所述起点，所述目标点索引指向所述最短路径数组队首，将所述当前变量和所述目标变量输入栅格化地图进行路径点标定；

通过所述改进A*算法在所述起点和所述目标点之间进行遍历，当确认所述目标变量指向于所述最短路径数组最后一位时，多航点规划任务完成。

本发明的一种融合改进A*算法与SPFA算法的多目标航点规划方法，通过搭建移动导航器将周围的环境栅格化，并标定导航器目标点集合和起点，以模拟任务场景，对于进行实验验证更方便；通过构建改进A*算法对起点至目标点的进行距离代价值计算，代价值可以反映两点间的远近，减少大量的路径点和拐点以及缩短了总路径长度，以此有效针对性地降低路径总代价；通过改进A*算法与SPFA算法融合，建立具有点优先级的有向且不含负环回的负边权地图，可以根据负边权地图中的点优先级得到最佳的航点顺序，导航器再依据航点顺序进行移动，以遍历任务目标点，以及实现全局路径规划算法，整体效率得以提升；并且在移动过程中，通过局部路径规划来躲避静态障碍物，能够很好的贴合移动导航器的可行轨迹；由此，与现有技术相比，本发明可以实现在复杂环境下的多目标航点任务中规划出合理且快捷的可行路径。

本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实施例了解到。

附图说明

图1为本发明第一实施例提出的多目标航点规划方法的流程图；

图2为本发明第一实施例的邻域的拓展与变化方法中的中心θ划分与13邻域拓展示意图，其中，(a)中心θ划分，(b)13邻域拓展；

图3为本发明第一实施例的存在U型死角点与存在伪强迫邻居的情况示意图，其中，(a)存在U型死角点情况，(b)存在伪强迫邻居情况；

图4为本发明第一实施例中强迫邻居与伪强迫邻居区分示意图；

图5为本发明第一实施例中未去除冗余节点与去除冗余节点对比示意图，其中，(a)未去除冗余节点，(b)去除冗余节点；

图6为本发明第一实施例中未平滑前与平滑后对比示意图，其中，(a)未平滑前，(b)平滑后；

图7为本发明第二实施例提出的多目标航点规划方法的流程图；

图8为本发明第三实施例中第一示例与第二示例的化简意图，其中，(a)第一示例，(b)第二示例；

图9为本发明第三实施例中对应图8的第一示例与第二示例的任务化简意图，其中，(a)第一示例1-1，(b)第一示例1-2，(c)示例第二示例2-1；

图10为本发明对比例中仿真实施的20*20实现效果示意图，其中，(a)传统A*算法生成的路径，(b)改进A*算法去除冗余节点后，(c)强迫邻居节点处添加b样条平滑后；

图11为本发明对比例中仿真实施的30*30实现效果示意图，其中，(a)传统A*算法生成的路径，(b)改进A*算法去除冗余节点后，(c)强迫邻居节点处添加b样条平滑后；

图12为本发明对比例中仿真实施的40*40实现效果示意图，其中，(a)传统A*算法生成的路径，(b)改进A*算法去除冗余节点后，(c)强迫邻居节点处添加b样条平滑后；

图13为本发明对比例中仿真实施的第一示例仿真实现效果示意图，其中，(a)传统A*算法生成的路径，(b)改进A*算法去除冗余节点后，(c)强迫邻居节点处添加b样条平滑后；

图14为本发明对比例中仿真实施的第二示例仿真实现效果示意图，其中，(a)传统A*算法生成的路径，(b)改进A*算法去除冗余节点后，(c)强迫邻居节点处添加b样条平滑后；

图15为本发明对比例中真实实施的第一示例实现效果示意图，其中，(a)实际建图场地，(b)实际建图二维栅格电子地图；

图16为本发明对比例中真实实施的第一示例路径规划效果示意图，其中，(a)传统A*算法结合SPFA，(b)改进A*算法结合SPFA；

图17为本发明第四实施例提出的多目标航点规划方法系统的结构示意图；

图18为本发明第四实施例提出的多目标航点规划方法系统中实物搭建硬件设计架构框图；

图19为本发明第四实施例提出的多目标航点规划方法系统中实实物搭建软件设计架构框图。

具体实施方式

为了便于理解本发明，下面将参照相关附图对本发明进行更全面的描述。附图中给出了本发明的若干个实施例。但是，本发明可以以许多不同的形式来实现，并不限于本文所描述的实施例。相反地，提供这些实施例的目的是使对本发明的公开内容更加透彻全面。

除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本发明。本文所使用的术语“及／或”包括一个或多个相关的所列项目的任意的和所有的组合。

请参阅图1，所示为本发明第一实施例中的多目标航点规划方法的流程图，该方法包括步骤S01至步骤S05，其中：

步骤S01：对目标环境进行二维栅格化，以生成栅格化地图，在所述栅格化地图中标定导航器的起点和目标点集合；

具体地，可以通过自主搭建的移动导航机器人，将周围的环境通过激光雷达与Karto-SLAM建图算法对环境地图二维栅格化，以使实际环境地图MAP栅格化，并标定导航机器的起点V

步骤S02：对传统A*算法进行改进，以得到改进A*算法，再根据所述改进A*算法计算所述起点到各个目标点之间的代价值，将各个代价值记录于代价数组；

具体地，可以将步骤S02再细分为三个步骤：

步骤S021：将导航器置于栅格化地图中心，选取栅格化地图中心周围的24邻域作为拓展邻域节点，通过导航器当前位置与目标点的欧式距离斜率得到邻域拓展方向的中心点后，以中心点θ为拓展角度中线向两端拓展5邻域，共形成13个邻域部分；

步骤S022：在传统A*算法的启发函数中引入角度概念并在邻域拓展角度代价的基础上加入局部直线代价，以及添加邻域内的障碍物影响函数，得到一种改进思路后的新启发函数，以对于存在的U型死角点与伪强迫邻居点进行识别并排除；

步骤S023：引入二次路径规划，以使路径中冗余拐点与强迫邻居节点处路径平滑，所述二次路径规划包括从起点出发去冗余节点以及对标记出的强迫邻居节点处进行b样条平滑处理。

进一步地，步骤S021中所述形成13个邻域部分的方式为:

以移动导航机器人当前的位置置于栅格中心，选取栅格周身的24邻域作为拓展邻域节点。通过引入角度概念，采用邻域拓展角度代价。引入角度概念，减少不必要的冗余节点与计算。若start为起点，end为目标节点，确定方向的最好方式就是欧式距离的斜率，斜率转角度计算公式表示为：

，

得到邻域拓展方向中心点后，以中心点θ为拓展角度中线向两端拓展5邻域。此方法中，邻域一共有13部分。

通常情况下，A*算法需要遍历以自身为中心的周边8个邻域节点，但是这8个节点中会存在3个无效代价节点。常见的做法是将原本的8邻域节点扩展至24邻域，这样可以加快A*算法的寻径速度。但是随着邻域的扩展，A*算法的计算量也会逐渐提升。同时参考邻域舍弃方式会发现，A*的邻域扩展算法会添加大量的冗余节点。为了解决传统A*算法24邻域拓展方式中存在的大量冗余节点问题，本发明提出一种邻域的拓展与变化方式。A*启发函数中可以通过引入角度概念加快启发函数的方向选择与定向。在邻域拓展中，同样可以引入角度概念，减少不必要的冗余节点与计算。通过斜率转角度计算公式并通过图2(a)所示，可以得到邻域拓展方向中心θ与角度范围内邻域的选取关系如表1所示：

表1 邻域拓展方向中心点θ表格

如图2(b)所示，从start点到end点的拓展中心角度θ可以求得为θ

进一步地，步骤S022中所述改进A*算法的启发函数思路为：

为了能够结合邻域的拓展与变化方式使得A*算法在路径选取上建立更加鲜明的方向特征，并加快A*算法下一步落脚点的快速预测，在传统A*算法的启发函数中进一步的引入了角度代价，并加入了局部直线代价。同时，针对存在的2种路径规划难点提出一种启发函数改进思路。传统A*算法中H(N)公式可以表示为:

，

改进后引入角度代价与局部直线代价启发函数H(N)公式可以表示为:

，

式中，H(N)

为解决存在的U型死角点与伪强迫邻居的情况，如图3(a)所示存在U型死角点，图3(b)中存在伪强迫邻居节点，为了解决存在U型死角点与存在伪强迫邻居节点的情况，通过添加邻域内的障碍物影响函数，将引入角度代价与局部直线代价后的启发函数公式再一次做出改进，首先需要明确伪强迫邻居与强迫邻居情况，两者情况如图4所示，强迫邻居在路径存在斜线时，只有一侧存在障碍物，而伪强迫邻居是两侧都存在障碍物，添加邻域内的障碍物影响函数后的启发函数H(N)公式修改如下：

，

此时，

进一步地，步骤S023中所述二次路径规划方式为:

二次路径规划分为两步，第一步是从起点出发去冗余节点，第二步是对标记出的强迫邻居节点处进行b样条平滑处理，使得路径更加符合机器人实际的可行路线。

第一步是从起点出发去冗余节点：

去除冗余节点中需要2个数组，分别为L1_point与L2_point数组，将A*得到的路径节点数组open_list赋值给L1_point，此处引用SPFA算法中出列与进列思想将L2_point数组作为中间缓存数组可以实现每个单点至单点间的最短路径选择；在进行去冗余节点前，需要先遍历数组open_list将强迫邻居节点标注出，去除冗余节点该过程步骤如下：

S0231:通过改进A*算法得到整条路径节点放置于open_list={(X

S0232:将当前点(X

S0233:使用L2_point记录当前点(X

S0234:如果步骤S0233中待检测点(X

S0235:直到NowPoint=n，此时NewPoint={(X

第二步是对标记出的强迫邻居节点处进行b样条平滑处理，使得路径更加符合机器人实际的可行路线：

通过采用强迫邻居的概念将集合S′中的强迫邻居标记出，在标记的路径节点处，对强迫邻居节点采用b样条的平滑方式进行路径的优化；

b样条曲线公式可以表示为：

，

其中，B

，

通过带入次数3(阶数3)可以推出b样条曲线的基函数如下所示：

，

由上式可以推出完整的b样条曲线方程为：

，

其中，P

步骤S03：给所述代价数组添加权值，以得到具有高优先级的有向含负权图，将所述有向含负权图输入到SPFA算法中计算所述起点到所述目标点的单源最短路径，以高优先级作为阶段性终点，得到所述目标点集合的航点顺序，并将所述航点顺序记录于最短路径数组；

具体还包括：通过改进A*算法与SPFA算法融合，以建立具有点优先级的有向且不含负环回的有向含负权图；采用SPFA算法结合合理规划算法，得到航点顺序，导航器将依据航点顺序以及根据改进A*算法在起点和目标点之间实现全局路径规划算法；导航器在移动过程中，根据DWA算法实现局部路径规划，以躲避动静态障碍物。

可以理解的，通过改进A*算法与SPFA算法融合，建立具有点优先级的有向且不含负环回的负边权地图，可以根据负边权地图中的点优先级得到最佳的航点顺序。

步骤S04：建立当前变量和目标变量，分别作为当前点索引和目标点索引，所述当前点索引指向所述起点，所述目标点索引指向所述最短路径数组队首，将所述当前变量和所述目标变量输入栅格化地图进行路径点标定；

步骤S05：通过所述改进A*算法在所述起点和所述目标点之间进行遍历，当确认所述目标变量指向于所述最短路径数组最后一位时，多航点规划任务完成。

具体还包括：在依次完成多个目标航点规划后，导航器将依据目标点索引状态判断是否到达最后一个航点，当完成多目标航点规划后，直接返回目标等待点，以等待下一次任务。

需要说明的是，导航器依据航点顺序进行移动，以遍历任务目标点，以及实现全局路径规划算法，整体效率得以提升；并且在移动过程中，通过局部路径规划来躲避静态障碍物，能够很好的贴合移动导航器的可行轨迹；

综上，根据上述的多目标航点规划方法，通过搭建移动导航器将周围的环境栅格化，并标定导航器目标点集合和起点，以模拟任务场景，对于进行实验验证更方便；通过构建改进A*算法对起点至目标点的进行距离代价值计算，代价值可以反映两点间的远近，减少大量的路径点和拐点以及缩短了总路径长度，以此有效针对性地降低路径总代价；通过改进A*算法与SPFA算法融合，建立具有点优先级的有向且不含负环回的负边权地图，可以根据负边权地图中的点优先级得到最佳的航点顺序，导航器再依据航点顺序进行移动，以遍历任务目标点，以及实现全局路径规划算法，整体效率得以提升；并且在移动过程中，通过局部路径规划来躲避静态障碍物，能够很好的贴合移动导航器的可行轨迹；由此，与现有技术相比，本发明可以实现在复杂环境下的多目标航点任务中规划出合理且快捷的可行路径。

请参阅图7，所示为本发明第二实施例提出的多目标航点规划方法的流程图，该方法包括步骤S101至步骤S108，其中：

步骤S101：通过移动导航机器人，将周围的环境通过激光雷达与Karto-SLAM建图算法对环境地图二维栅格化，实现实际环境地图MAP的栅格化；

步骤S102：在栅格化地图中模拟任务场景，并标定目标点集合V

步骤S103：标定移动导航机器人起点V

具体地，在步骤S103中改进的A*算法的具体步骤为:

步骤S1031:在二维栅格化的地图中，以移动导航机器人当前的位置置于栅格中心，选取栅格周身的24邻域作为拓展领域节点；通过引入角度概念，采用领域拓展角度代价；通过移动导航机器人当前位置与任务目标点的欧式距离斜率得到邻域拓展方向的中心点后，以中心点θ为拓展角度中线向两端拓展5邻域。此方法中，邻域一共有13部分，所以也叫13邻域拓展方式。

步骤S1032:在传统用A*的启发函数中进一步的引入了角度代价，并加入了局部直线代价，同时针对存在的2种路径规划难点，U型死角点与伪强迫邻居点提出一种启发函数改进思路。

步骤S1033:在通过步骤S1032得到的路径中，依然会存在大量的冗余拐点与强迫邻居节点处路径不平滑，移动导航机器人无法通过的情况；本发明通过引入二次路径规划用以解决路径中依然会存在大量的拐点与强迫邻居节点，强迫邻居节点处的路径规划未能充分考虑实际机器人的运行轨迹等问题。

步骤S104：从起点V

步骤S105：通过代价数组中记录的代价值，给有向含负权图中添加权值，具有高优先级的点权值为负数且路径离起点越近负数值则越大；

步骤S106：将有向含负权图输入到SPFA算法中计算出单源最短路径，以高优先级作为阶段性终点，得到多目标航点情况下的航点顺序，记录于最短路径数组Z={(X

需要说明的是，其中，本发明还以通过改变与SPFA算法结合的合理规划算法实现多复杂场景的实施，包括有病房药物送达，图书询盘取放，餐厅送餐，观光引导等等。

步骤S107：建立当前变量与目标变量分别作为当前点索引与目标点索引，当前点索引指向起点V

步骤S108：确认目标变量是否指向最短路径数组Z最后一位时，如果是，则多航点任务已完成，如果否，则将当前变量赋值给目标变量后，将目标变量指向最短路径数组Z下一位，重复步骤S107。

需要说明的是，在以上步骤S1031、步骤S1032和步骤S1033分别涉及到的13邻域拓展方式、启发函数改进思路以及二次路径规划方式可参考实施例一所述，但不限于以上提到的具体拓展范围，可以根据具体模拟场景设计领域拓展方式、根据具体路况设计领域障碍物影响函数以及根据路径复杂程度进行多次路径规划。

在本发明的第三实施例中，另外需要说明的是，SPFA算法结合合理规划算法，合理规划算法细节为：

其中，图8(a)第一示例中，Start点为起始点，End为结束点。A、B、C、D、E、F点皆是普通优先级点，vip1和vip2点是两个高优先级点。其中，因为地图路径局限性，通过箭头表示有向行驶，两点之间不能逆行，无线段连接两点间不可直达，线段中的权值来自于改进A*算法，为距离数值的映射。在任务规划中，会以高优先级点作为阶段性终点，然后将当前的阶段性终点作为下一阶段的起点，将多个阶段性路径规划进行合并，即为最终路径规划点顺序。

其中，图8(b)中第二示例，为单一补给点多配送情况，点优先级不可变。需要规划的只有配送点优先级。而在图8(b)中，存在为多补给点，多配送点。点将不具有固定优先级，依据满足条件进行优先级变化。图中start为起点，end为终点，1号，2号，3号，4号，5号，6号，7号为补给点，Ⅰ号，Ⅱ号为配送点。当不满足Ⅰ号，Ⅱ号配送需求时，两者优先级一致。当满足某一个配送需求，任务优先级将会提升，并在途径过程中进行停留。图8(b)中，实际路径规划不具有局限性，配送点Ⅰ号，Ⅱ号仅在算法中做桥接，不作为实际路径规划点中的重复经过点。

图9(a)为图8(a)的任务化简示意图，设定的航点顺序包括有A，E，B，VIP1。通过合理规划算法细节，将以高优先级点VIP1将作为第一个阶段性终点，通过SPFA算法计算出从Start点到达VIP1点的最短距离。通过SPFA算法，将队列的队首分别出列，对其他的连接点分别进行松弛操作，最后得出第一路径的结果为Start→A→VIP1。随后将VIP1点第二阶段起点，因为经过点A将不再考虑，通过SPFA算法计算VIP1与E,B,存在的最短路径，得出第二路径的结果为VIP1→E。因为最后只剩点B，任务点小于3个则不再进行路径规划。故得出的最佳路径点结果为:Start→A→VIP1→E→B。

图9(b)为图8(a)的任务化简示意图，设定的航点顺序包括有A，E，F，D，VIP1，VIP2。通过合理规划算法细节，将以高优先级点VIP1或者VIP2作为第一个阶段性终点，通过SPFA算法计算出从Start点分别到VIP1与VIP2的最短距离，通过对比选取代价最小的点作为第一阶段性的终点。通过对其他的连接点分别进行松弛操作，最后得出第一路径的结果为Start→A→VIP1。随后将VIP1点第二阶段起点，因为经过点A将不再考虑，将VIP2点第二阶段终点。通过SPFA算法对其他的连接点分别进行松弛操作，最后得出第二路径的结果为VIP1→E→F→VIP2。因为最后只剩点D，任务点小于3个则不再进行路径规划。故得出的最佳路径点结果为:Start→A→VIP1→E→F→VIP2→D。

图9(c)为图8(b)的任务化简示意图，设定的航点顺序包括有2号，3号，4号，5号，7号，Ⅰ号，Ⅱ号。其中通过实施例2中合理规划算法细节，Ⅰ号配送点需补给位置为2号，4号，7号，Ⅱ号号配送点需补给位置为2号，3号，5号。将所有点与权值信息输入SPFA算法中，通过SPFA算法能够得到第一阶段最佳路径为:Start→2号→3号→4号。因经过点2号，3号，4号将不再考虑，将4号作为第二阶段起点，将其余的点与边信息输入到SPFA算法中，通过SPFA算法能够得到在经过配送点Ⅰ号，Ⅱ号的情况下，7号的路径最短，因为不满足配送点Ⅰ号，Ⅱ号的配送需求，故不做停留，则第二阶段最佳路径为:4号→7号。因为到达7号点后，将满足配送点Ⅰ号的配送需求，则Ⅰ号具有高优先级，第三路径规划中，将配送点Ⅰ号作为终点，通过SPFA算法计算得出路径为:7号→Ⅱ号→Ⅰ号。因为不满足Ⅱ号的配送任务，故第三段最佳路径为:7号→Ⅰ号。因为最后只剩下一个补给点5号，与一个配送点Ⅱ号，因为当前配送点未满足配送需求，且补给点小于2个不再进行路径规划。故得出的最佳路径点结果为:Start→2号→3号→4号→7号→Ⅰ号→5号→Ⅱ号。

在本发明的对比例中，为了验证改进A*算法路径规划合理性，在建立的仿真实验环境中对比传统A*算法，改进A*算法(不带平滑)，与改进A*算法(带平滑)。

首先，分别建立20*20，30*30，与40*40的障碍物地图。然后分别在3种障碍物地图中运行传统A*算法，改进A*算法(不带平滑)，与改进A*算法(带平滑)。对输出结果分别进行比较，结果如下图10、11、12所示。

通过对比图10与图11中的3张图，图(a)是传统A*算法生成的路径，存在大量的冗余节点与曲折路径，并不适用于实际机器人的行进路线，通过改进A*算法去除冗余节点后如图(b)所示，路径变得平滑但是强迫邻居节点处依然没有考虑机器人本身的半径问题，与实际行进路线存在偏差，在强迫邻居节点处添加b样条平滑后如图(c)所示，会使得机器人在实际的行进过程中减少与障碍物边角碰撞的可能性。图12是40*40的复杂环境地图，其中隐藏着典型的U型死角点与伪强迫邻居节点的情况。在图12中，传统A*算法于坐标(31,13)处经历了一个伪强迫邻居节点，在传统A*的算法中会认为斜对角是一个可行进点，并没有考虑机器人的实际运行环境。通过添加障碍物影响函数对伪强迫邻居节点进行辨别，改进A*效果如图(b)所示，改进A*能够避开伪强迫邻居生成可行路径。通过对强迫邻居点进行b样条平滑后如图(c)所示。

为了比较改进A*算法与传统A*算法，将图10，11，12数据整理如下表2，3，4所示。由表2，3可知，在未经历伪强迫邻居的情况下，通过二次路径规划去除冗余节点使得改进A*算法的路径节点数量相比于传统A*算法减少了84.9%，减少了63.6%的拐点数量，整体代价减少了6.4%，缩减了运行时间。由表3所示，在经历了伪强迫邻居的情况，改进A*算法能够避开伪强迫邻居，并成功规划出安全路径。

表2 传统A*算法，改进A*算法(不带平滑)，与改进A*算法(带平滑)的数据(20*20)

表3 传统A*算法，改进A*算法(不带平滑)，与改进A*算法(带平滑)的数据(30*30)

表4 传统A*算法，改进A*算法(不带平滑)，与改进A*算法(带平滑)的数据(40*40)

本发明为了验证SPFA算法与改进A*算法融合能够在复杂环境下的多目标航点任务中成功规划出合理且快捷的路径。通过仿真实验环境搭建SPFA算法单体验证路径可行性的验证实验中的实验环境。并将SPFA运算出的路径结果同时运用于A*算法与改进A*算法，结果如图13所示。为了得到改进A*算法与传统A*算法分别结合SPFA算法后的直观效果，将图13数据整理如下表5所示。如图13中图(a)所示，为传统A*算法在SPFA实验环境下的路径规划，在坐标点(29,33)处存在一个U型死角点，传统A*算法的路径规划途径了该点。图13中图(b)为改进A*算法在SPFA实验环境下的路径规划，在图(a)中经过的U型死角点，在图(b)中通过添加障碍物影响函数成功避开U型死角点。并在图(b)的强迫节点处添加b样条平滑，实现效果如图13中图(c)所示。

表5 传统A*算法，改进A*算法(不带平滑)，与改进A*算法(带平滑)的数据(SPFA)

/>

发明为了能够验证该任务点的遍历顺序能够在改进A*算法下能够很好的实现在复杂环境下，实现具有多性质，多优先级任务点间的路径规划。通过在仿真实验环境中搭建的实际实验场地环境，并将运用SPFA求解规则下得到的任务点顺序，分别通过改进A*算法与传统A*算法进行实验。如图14所示。如图14中图(a)所示，为传统A*算法在实际实验场地仿真环境下的路径规划，通过改进A*算法去除冗余节点后，如图(b)所示，并在图(b)的强迫邻居节点出采用b样条平滑曲线，得到路径规划如图(c)所示。

为了进一步验证改进A*算法与SPFA算法相结合在实际运行场景中的可行性，在人为搭建的实验场地中，进行了实际场地运行实验。首先通过激光雷达使用Karto-Slam建图算法构建当前实验场景的二维栅格电子地图。如图15所示。图15中黑色线条为障碍物的轮廓，红色字母标定位置为设定好的路径节点。在该实验环境中对比传统A*算法结合SPFA与改进A*算法结合SPFA算法的实验效果如图16所示。为了得到改进A*算法与传统A*算法分别结合SPFA算法后的直观效果，将图16实际场地下的运行数据整理如下表6所示。

表 6 改进A*算法与传统A*算法分别结合SPFA算法实际场地运行数据

图16(a)为传统A*结合SPFA算法，在规划的路径中存在大量的曲折路径，而图16(b)改进A*结合SPFA算法中规划的路径相比于传统A*结合SPFA算法更加的平滑。在障碍物的拐角处，为了能够更加直观的观察到平滑，将障碍物膨胀系数缩减。能够直观的看到图16(b)中在障碍物拐角处的平滑路径。由表6可知改进A*融合SPFA算法相比于传统A*融合SPFA算法减少了18.6%的路径长度，整体运行时间减少了17.5%。同时，通过实际场地验证，A*融合SPFA算法在实际工程中能够很好的解决在具有点优先级的有向含负权图中的多目标航点的任务规划问题。

本发明提出了一种融合改进A*算法与SPFA算法的多目标航点规划方法，完成了提出算法的仿真实验与实际场地下的运行实验。通过仿真数据可以表明，改进A*算法相比于传统A*算法在SPFA实验环境中路径节点减少了84.9%，减少了66.6%的拐点数量，整体代价减少了10.8%，缩减了运行时间。改进A*算法在复杂环境中进行路径规划，能够避开U型死角点与伪强迫邻居节点。

请参阅图17，所示为本发明第三实施例提出的多目标航点规划系统的结构示意图，该系统包括：

模拟场景模块10，用于对目标环境进行二维栅格化，以生成栅格化地图，在所述栅格化地图中标定导航器的起点和目标点集合；

改进算法模块20，用于对传统A*算法进行改进，以得到改进A*算法，再根据所述改进A*算法计算所述起点到各个目标点之间的代价值，将各个代价值记录于代价数组；

融合算法模块30，用于给所述代价数组添加权值，以得到具有高优先级的有向含负权图，将所述有向含负权图输入到SPFA算法中计算所述起点到所述目标点的单源最短路径，以高优先级作为阶段性终点，得到所述目标点集合的航点顺序，并将所述航点顺序记录于最短路径数组；

标定路径模块40，用于建立当前变量和目标变量，分别作为当前点索引和目标点索引，所述当前点索引指向所述起点，所述目标点索引指向所述最短路径数组队首，将所述当前变量和所述目标变量输入栅格化地图进行路径点标定；

遍历检验模块50，用于通过所述改进A*算法在所述起点和所述目标点之间进行遍历，当确认所述目标变量指向于所述最短路径数组最后一位时，多航点规划任务完成。

本发明实施例通过激光雷达，霍尔元器件数据作为里程计输入，IMU数据作为imu_data输入数据，超声波数据作为局部代价地图中的额外参考层。通过Karto-SLAM建图算法将以上数据进行数据预处理，通过imu_filter将imu_data数据滤波，通过robot_pose_ekf算法将霍尔元器件数据作为里程计输入，imu_data,超声波数据，和激光雷达数据作为robot_pose_ekf算法输入量，做多传感器的数据融合得到移动导航机器人在当前位置中的odom坐标与map坐标，及两坐标之间的相对位置误差。并通过蒙特卡洛定位算法(amcl)通过读取激光雷达数据，对二维栅格地图中的大障碍物周边进行重定位匹配，移动导航机器人通过原地旋转，进行自身位置的重定位。通过键盘操控，发布速度话题，底层控制通过编写ros_lib系统在裸机开发中接收话题信息。得到控制信息后，操控驱动轮进行前后左右移动，并通过移动将超声波、imu、霍尔元器件数值再次打包成话题发送给上层控制器，用以数据融合并将周身的环境地图栅格化，实现环境地图的二维栅格化。

本发明实施例中实物搭建硬件设计架构框图如图18所示，本发明实物搭建软件设计架构框图如图19所示。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、 “示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明的保护范围应以所附权利要求为准。