一种基于轨迹平滑优化的无人车混合轨迹规划方法

技术领域

本发明涉及无人车导航和路径规划领域，具体涉及一种基于轨迹平滑优化的无人车混合轨迹规划方法。

背景技术

无人车作为一种智能化交通工具，具备自主驾驶和执行任务的能力，已经在诸多领域得到广泛应用。在工业生产领域，无人车能够进行自动化物料搬运、装配线的物料供应等任务，提高了生产效率和安全性。在生活服务领域，无人车能够进行货物配送、垃圾清运、移动广告等任务，方便了人们的生活，减轻了人力负担。

路径规划是无人车自动驾驶过程中的核心环节，对行进路径的选择直接影响到无人车的行进效率和工作效率。一个高效的路径规划算法能够使无人车选择更短的路径、避开拥堵路段，从而节省时间和能源，并且能够合理安排任务执行顺序，提高工作效率。

全局路径规划和局部路径规划是根据环境信息掌握程度进行划分的两种路径规划方法。全局路径规划是指在无人车出发前，基于对整个地图的环境信息进行规划，确定无人车需要行进的整体路径。而局部路径规划是指在行进过程中，根据实时感知到的环境信息，通过实时调整行进路径来避开障碍物。

全局路径规划算法能够在已知环境中规划出较优的路径，如Dijkstra算法、A*算法和RRT算法等。然而，这些算法无法考虑到未知的障碍物，因此在面对未知障碍物时，可能无法规避，导致发生碰撞事故。

局部路径规划算法能够根据实时感知到的环境信息，在已知环境中实时避开障碍物，如TEB算法、人工势场法和DWA算法等。然而，在障碍物较多的情况下，由于路径选择的限制，局部路径规划算法可能无法找到一条可行的路径，导致无法到达目标点。

目前，单一的全局路径规划算法或局部路径规划算法已无法满足复杂环境下无人车的行进要求。融合A*和DWA算法的路径规划算法提出，旨在弥补传统算法的不足之处。它综合利用全局路径规划和局部路径规划的优势，能够规划较优路径并实时避障。

发明内容

本发明目的在于针对上述现有技术的缺陷和问题，提出了一种基于轨迹平滑优化的无人车混合轨迹规划方法，该方法为实现无人车人既能以全局最优路径行驶，又能够实时避开障碍物，提出了一种基于轨迹平滑优化的A*算法和DWA算法的无人车混合轨迹规划方法。该算法将传统A*算法的搜索临域进行了减少，在启发函数中引入前点到目标点的估计成本与实际成本比，提高了搜索效率；删除轨迹中冗余节点、减少转折，实现路径平滑度的优化；提取改进A*算法规划路径的关键点作为DWA算法的中间目标点做全局引导，实现基于轨迹平滑优化的A*算法和DWA算法的融合。

本发明为解决上述技术问题采用以下技术方案：一种基于轨迹平滑优化的无人车混合轨迹规划方法，包括以下步骤：

步骤1：对A算法进行了改进，优化了搜索节点的选择策略。根据目桥节点相对于当前节点的位置，本发明排除了与目标无关的方向；

步骤2：对A算法进行了优化，改进了启发函数。本发明通过引入当前节点到目标节点的预估成本与实际成本的比值来改善启发函数，从而解决了传统A算法中由于启发式西数预估值不准确而导致的搜索效率低下的问题。

步骤3：针对路径平滑度进行优化。利用一种关键点选取策略，解决冗余的共线节点和非必要转折点过多的问题，最后保留必要的路径节点，得到只具有关键点的全局路径；

步骤4：将轨迹平滑优化A*算法与DWA算法相结合。DWA算法在每两个相邻节点之间进行局部轨迹规划，使局部路径规划遵循全局路径节点，从而使路径更加平滑，并实现实时避障。

作为优选，所述步骤1优化传统A*算法的搜索临域，传统的A*算法将当前节点的8邻域栅格展开进行轨迹规划，如图1所示。具体包括舍弃3个与目标点完全反向的不必要搜索方向，保留5个与目标点相关的搜索方向。

作为优选，所述步骤2优化传统A*算法的代价函数，传统的A*算法在搜索轨迹时可能会遍历很多不必要的节点，影响搜索效率。具体包括如下：在代价函数f(n)中随着无人车的当前位置变化而变化的当前点到目标点的估计成本与实际成本比；假设R为起点到目标点的距离，r为当前点到目标点的距离；从而达到对传统A*算法的优化，此时的代价函数如下：

作为优选，所述步骤3路径平滑的优化，传统A*算法路径规划是由连续栅格中心点连接组成，存在许多冗余的节点，路径转折次数多，路径不平滑。具体优化步骤如下：第一步：依次遍历路径中所有节点，如若当前节点与前后两个节点都在同一直线上，则剔除当前节点；第二步：剔除同一直线上的冗余点后，设路径中的节点为{P

作为优选，所述步骤4改进的A*算法与DWA算法的融合，步骤123实现了对A*算法的改进，得到了只包含起始点、关键点和日标点的导航路径，但无法避开环境中出现的末知障碍物。DNA算法具有良好的局部避障能力，但只有一个最终目标点作为指引，容易陷入局部最优。因此，本发明将两种算法相融合，以提取的改进A*算法规划的全局路径关键点作为DWA算法的中间目标点，优化后的评价两数使得局部路径规划遵循己规划的全局路径轮廓。通过融合导航算法，以实现移动机器人导航过程中的全局路径最优，同时兼具实时避障功能。

有益效果：

1、本发明在轨迹平滑优化A*算法中，改善搜索临域，优化评价函数，提高搜索效率；删除冗余节点,减少转折，提高路径平滑度。

2、本发明将优化后路径的关键点作为DWA算法的临时目标，实现局部规划，在全局最优的基础上实现随机避障，融合规划出一条全局最优的平滑路径。实验结果表用，融合算法既能实现全局最优的路径规划，又能顺利的躲避随机障碍物，完成复杂环境中的动态路径规划。

附图说明

图1为本发明A*算法轨迹规划图。

图2为本发明临域搜索方向示意图。

图3为本发明基于轨迹平滑优化的A*算法和DWA算法的混合轨迹规划方法流程图。

图4(a)本发明基于轨迹平滑优化的A*算法和DWA算法的混合轨迹规划路线图(将关键点设为当前目标点)。

图4(b)本发明基于轨迹平滑优化的A*算法和DWA算法的混合轨迹规划路线图(避开未知障碍物)。

具体实施方式

下面结合说明书附图和具体实施方式对本发明做进一步详细说明。

为本发明的技术方案能得到充分的理解，现对A*算法简要介绍如下：

传统的A*算法搜索原理是将起始点加入到open列表，将该点作为父节点加close列表，搜索其相邻的可到达节点加入open列表。依据代价函数计算open列表中节点的代价，选取代价最低的节点作为下一个父节点并放入close列表，再次搜索父节点可到达节点并计算其代价，依次循环，直到父节点为目标点的位置。

传统A*算法的代价函数为：

f(n)＝g(n)+h(n)式中，n代表当前节点，f(n)表示的是无人车在当前节点的代价函数，用来选取最优路径。g(n)代表无人车从起始点到当前点的实际代价值。h(n)为启发函数，代表从当前点n到目标点的估计代价值。一般情况下，h(n)小于从当前点n到目标点的实际代价值，当h(n)值为0时，此时只有g(n)起作用，算法即为Dijkstra算法。h(n)的估计值比实际代价值小时，算法搜索时间会因搜索空间的增大而增加。h(n)估计值比实际代价值大时，算法搜索速度会增加，但是算法可能无法搜索到最短路径。

不难看出h(n)对于搜索效率有很大的影响，h(n)有几种常见的表现形式：(1)曼哈顿距离；(2)切比雪夫距离；(3)欧几里得距离。本发明的一具体实施例采用的是欧几里得距离。

如图1所示，本发明为搜索区域，其中黑色格子为障碍物节点，白色格子为可走节点。灰色为为当前点选择下一点时的寻找的遍历节点，如果碰到障碍物，则不将障碍物作为考虑范围之内。直到最后的到达日标点停止，获得最短路径，如图1所示，折线连接的路径代表寻找到的最短路径。

本发明提出的一种基于轨迹平滑优化的A*算法和DWA算法的无人车混合轨迹规划方法，其核心在于对A*算法基于轨迹平滑优化的改进以及融合A*与DWA算法。以下实施例用于说明本发明，但不用来限制本发明的范围。

本实施例所述的基于轨迹平滑优化的A*算法和DWA算法的无人车混合轨迹规划方法的流程具体包括:

步骤1：优化传统A*算法的搜索临域，将目的地与起点的角度偏差纳入考虑范围，减少非必要的节点搜索；

步骤2：优化传统A*算法的代价函数，通过引入当前节点到目标节点的预估成本与实际成本的比值来改进启发函数，实现其自适应调整；

步骤3：路径平滑的优化，利用一种关键点选取策略，解决冗余的共线节点和转折点的问题，保留必要的路径节点,得到只具有关键点的全局路径；

步骤4：构造结合关键点信息的评价函数，然后应用改进DWA算法，使局部路径规划遵循全局路径轮廓,从而使路径更加平滑，并实现实时避障。

下面具体介绍步骤1中的优化搜索点选择策略：

A*算法将当前节点的8邻域栅格展开进行轨迹规划，如图1所示，蓝色栅格为当前点，n

由于最终目标点方向的限制，导致8邻域栅格中出现了不必要的网格，浪费了计算时间和存储空间。为了进一步提高搜索效率，本设计提出了一种根据目标点与当前点的相对位置选点策略，该策略舍弃3个不必要的搜索方向，保留5个与目标点相关的搜索方向。目标点与当前问题的直线与n

表1夹角α与三个丢弃方向对应关系表

下面具体介绍步骤2中的代价函数优化策略：

传统的A*算法在搜索轨迹时可能会遍历很多不必要的节点，影响搜索效率。造成这一问题的关键因素在A*算法的启发式函数的设计。如果启发式过程的估计值小于实际生成值，则搜索节点数量会增加，但仍有可能找到最优轨迹；如果启发式函数的估计值大于实际生成值，则搜索节点数量会减少，效率更高，但最优轨迹的寻找具有挑战性。如果启发式函数的估计值与实际生成值相等，则搜索效率最高。由于启发函数是欧式距离，启发函数的值始终不大于当前点到目标点的实际距离。当前点离目标点较远时，启发函数估计值远小于实际值，算法搜索节点多，运算效率低，此时应加大估计值的权重，提高运算效率；当前点逐渐靠近目标点时，估计值逐渐逼近实际值，为防止估计值过大而搜索不到最优路径，估计值权重应随之降低。综上，本文将代价函数改进为如式(1)所示。

式中，f(n)是综合代价值；h(n)是当前点到目标点的估计代价值；g(n)是起点到当前点的实际代价值。R为起点到目标点的距离，r为当前点到目标点的距离。

下面具体介绍步骤3中的冗余点删除策略：

冗余点删除策略经过上述改进虽然提高了搜索效率，但路径上仍然存在许多不必要的冗余点，不利于无人车路径跟随。本文提出冗余点删除策略，对冗余节点进行剔除，只保留必要的转折点。具体冗余点删除策略如下:

1)依次遍历路径中所有节点，如若当前节点与前后两个节点都在同一直线上，则剔除当前节点。

2)剔除同一直线上的冗余点后，设路径中的节点为{P

通过上述优化，可以减少搜索节点的数量，提高计算效率。

下面具体介绍步骤4中的算法融合优化：

无人车可以通过装载完整环境信息的仓库地图很来实现导航。然而，在复杂的设备工厂环境中，存在着大量的随机障碍物。在无人车规划出的原始导航轨迹上可能会出现未知障碍物，比如新增加的货物和其它正在移动的无人车。如果不采取相应措施，无人车很容易与障碍物发生碰撞。因此，为了实现无人车的实时避障，采用具有局部避障能力的DWA算法，规划局部轨迹避障，保证安全。DWA算法在速度可行范围内对多组无人车速度对

式中:

对上述评价函数的分析表明，只有一个最终目标点指导DWA算法。如果环境中有更多的障碍物，找到到达目标点的轨迹是困难的。考虑到可以设置一定数量的中间目标点作为DWA算法的导引点，可以在分段中实现局部轨迹规划，避免无法到达目标点。因此，我们利用关键问题信息构造一个评价函数来应用DWA算法，并将关键点作为中间目标点，保证在全局轨迹轮廓的指导下进行局部轨迹规划。评价函数记为

式中

本发明将轨迹平滑优化A*算法与DWA算法相结合，以满足全局最优轨迹和随机避障能力，从而实现了真对DWA算法的改进。设计采用轨迹平滑优化A*算法对全局方法进行规划，得到全局最优节点序列，然后利用DWA算法对每两个相邻节点之间的局部轨迹进行规划。算法的详细流程如图3所示。

首先，使用轨迹平滑优化A*算法规划全局轨迹，并使用其优化的成本函数F(n)对过程进行优化。得到了轨迹的临界点选择策略和必要的关键节点。DWA算法对无人车的速度进行采样，模拟每个速度对的轨迹，然后结合临界点信息的评价函数选择最优的轨迹规划，并以最优轨迹对应的速度控制无人车运动到目标点，从而实现了轨迹平滑优化A*算法与DWA算法的融合。最后，在融合导航算法下，无人车沿全局轨迹轮廓接近或到达中间目标点，直至到达最终目标点。

为了验证融合导航算法的轨迹规划和实时避障能力，我们选择了一个15×15的栅格地图进行实验。在由改进的A算法规划的全局轨迹上设置了三个未知的障碍物，无人车必须通过这些障碍物，这些障碍物由黑色网格表示。模拟实验结果如图4所示。虚线(见图4(a))显示了由轨返平滑优化A*算法规划的全局轨迹，标记表示作为无人车的中间目标的关键点。当它接近最终目的地时，下一个关键点接管作为新的中间目标。图4(b)显示了基于轨迹平滑优化A*算法和DWA的混合轨迹规划方法下的无人车轨迹，有效地避开了未知的障碍物。这条轨迹比由轨迹平滑优化A*算法规划的轨迹更加平滑。实验结果表明，通过构建包含关键点信息的评估函数，并使用每个关键点作为DWA算法在全局基础上规划局部轨迹的中间目标点，可以实现对随机障碍物的避障。在模拟环境中，基于轨迹平滑优化A算法和DWA的混合轨迹规划方法实现了无人车导航的全局轨迹优化和避障功能。

上述对实施例的描述是为便于本技术领域的普通技术人员能理解和应用本发明。所属领域的普通技术人员依然可以对本发明的具体实施方式进行修改或者等同替换，而末脱离本发明精神和范围的任何修改或者等同替换，其均应涵盖在本发明的权利要求范围当中。