CN116360457A - 一种基于自适应栅格和改进a*-dwa融合算法的路径规划方法 - Google Patents

Abstract

本发明涉及一种基于自适应栅格和改进A*‑DWA融合算法的路径规划方法，属于机器人领域，包括：构建基于自适应栅格尺寸的环境模型，基于障碍物的大小和数量确定自适应栅格边长，并计算栅格尺寸及最优栅格边长；根据已知障碍物信息改进A*算法，完成全局路径规划，得到一条节点少、平滑度高的路径；结合环境模型中的未知障碍物信息和改进的A*算法得到的全局路径规划节点，选择起点之后的节点依次作为改进的DWA算法中的临时目标点，改进的DWA算法从起点开始不断向临时目标点行进，到达临时目标点之后更新临时目标点，直到到达终点。本发明既能实现全局最优的路径规划，又能顺利的躲避随机障碍物，完成复杂环境中的动态路径规划。

Description

一种基于自适应栅格和改进A*-DWA融合算法的路径规划方法

技术领域

本发明涉及一种基于自适应栅格和改进A*-DWA融合算法的路径规划方法，属于机器人技术领域。

背景技术

移动机器人具备的自动执行任务的特性使得它被广泛的应用于工业生产、生活服务等众多领域当中。移动机器人的行进路径效率直接影响其工作效率，如何规划出一条安全、高效的路径成为了极具研究意义和应用价值的问题。良好的路径规划算法能够实现机器人在行进过程中对已知障碍物和未知障碍物实现避障，能够规划出一条路线较优、能安全到达目标点的路径。

根据对环境信息的掌握程度，路径规划可以分为全局路径规划和局部路径规划。应用于全局路径规划的算法有：Dijkstra算法、A*算法、RRT算法等。此类全局路径规划算法在已知环境下能够规划出一条从起点到终点的较优路径，但无法规避未知障碍物，容易在行进过程中发生碰撞，使得机器人无法顺利到达目标点。应用于局部路径规划的算法有：TEB算法、人工势场法、DWA算法等。此类局部路径规划算法能够在未知环境中实时避障保证机器人的安全行进，但在障碍物较多时容易出现机器人为了躲避障碍物陷入局部循环，无法到达目标点的情况。

显然，在复杂的环境下单一的全局路径规划算法或局部路径规划算法已经无法满足机器人的行进要求，为此有的学者提出了一种融合A*和DWA算法的路径规划算法。A*算法是是目前应用广泛的全局路径规划算法，具有路径较短、计算速度较快等优点。传统A*算法在实际应用中存在拐点多，路径不平滑等问题，影响机器人的运行。DWA(动态窗口法)是一类经典的局部路径规划算法，根据机器人的当前状态和运动模型推断机器人的轨迹并进行评价已确定机器人的行进路线。但是现有的融合A*和DWA算法的路径规划算法普遍存在着路径规划效率低、环境模型的构建过程复杂、不够合理等问题。

发明内容

针对现有技术的不足，本发明提供一种基于自适应栅格和改进A*-DWA融合算法的路径规划方法，主要创新点如下：(1)引入了自适应栅格尺寸，提高了算法的执行效率；(2)改进了A*算法，优化了算法的评价函数与最终路径节点的选取，减少了算法的搜索节点与最终路径节点，提高了算法的运算速度以及路径的平滑度；(3)改进了DWA算法，将障碍物细分为未知障碍物与已知障碍物，将其引入算法的评价函数中，增强避障能力，使之能够与A*算法相融合。本发明的方法实现了机器人在未知环境中的路径规划，并在算法的执行效率和路径选择上面取得了较好的效果。本发明主要应用在环境地图主体部分已知而未知障碍物较少的情况下，如电子游戏角色的移动路径规划。

本发明采用如下技术方案：

一种基于自适应栅格和改进A*-DWA融合算法的路径规划方法，包括如下步骤：

(1)构建基于自适应栅格尺寸的环境模型，基于障碍物的大小和数量确定自适应栅格边长，并计算自适应栅格边长下的栅格尺寸及最优栅格边长；

(2)根据环境模型中已知障碍物信息改进A*算法，完成全局路径规划，得到一条节点少、平滑度高的路径；

(3)结合环境模型中的未知障碍物信息和改进的A*算法得到的全局路径规划节点，选择起点之后的节点依次作为改进的DWA算法中的临时目标点，改进的DWA算法从起点开始不断向临时目标点行进，到达临时目标点之后更新临时目标点，直到到达终点。

优选的，步骤(1)中，设地图最小栅格边长为s_min，最大栅格边长为s_max，地图边长为m，则最大栅格尺寸可表示为如下：

已知障碍物的总面积可以表示如下：

其中，Size_{bar i}代表第i个障碍物的大小，n代表障碍物数量；

自适应栅格边长可表示如下：

自适应栅格边长下的栅格尺寸则如下：

最优栅格边长如下：

优选的，改进A*算法包括路径整合和优化启发函数。

优选的，路径整合的具体过程包括：

(2.1)从起点S开始判断当前方向与下一节点方向是否相同，若相同则删除下一节点，不相同则保留，遍历所有节点直至终点G；

(2.2)从起点S开始判断当前节点与不相邻的节点能否直接连接为一条路径，由整合后的路径到障碍物的距离确定，若该距离大于设置的安全距离，则进行整合，小于设置的安全距离，则保持原路径；

(2.3)将最后得到的路径输出。

优选的，优化启发函数的过程如下：

设机器人移动起点为(x_i,y_i)，终点为(x_j,y_j)，N为从起点到终点的矩形栅格内的障碍物数量，引入环境信息K，K表示在从起点到终点的矩形栅格内障碍物所占比率：

将环境信息K引入评价函数F(n)，根据栅格中障碍物的多少来调整启发函数G(n)的权重，改进后的评价函数可表示为：

F(n)＝G(n)+exp(-K+1)*H(n)。

可以看出，当环境中障碍物少时，H(n)系数大，能够减小搜索范围，提高搜索速度；当环境中障碍物多时，H(n)系数小，增大搜索范围，保证路径全局最优。

优选的，步骤(3)中，改进后的DWA将已知障碍物和未知障碍物进行区分，共同引入评价函数，改进后的评价函数为：

G(v,ω)＝αHead(v,ω)+βVel(v,ω)+γDist_s(v,ω)+μDist_d(v,ω)

其中，Head(v,ω)为当前模拟轨迹的终点方向与目标方向角偏差，Vel(v,ω)为当前所模拟的速度值大小的评价函数，Dist_s(v,ω)为已知障碍物与当前模拟轨迹的距离，Dist_d(v,ω)为未知障碍物与当前模拟轨迹的距离，α、β、γ、μ为各个项的加权系数。

本发明未详尽之处，均可参见现有技术。

本发明的有益效果为：

本发明构建了基于自适应栅格地图的环境模型，提高算法的搜索效率与精准度；在改进A*算法中，优化评价函数，提高搜索效率；删除冗余节点，减少转折，提高路径平滑度。将优化后路径的关键点作为DWA算法的临时目标，实现局部规划，在全局最优的基础上实现随机避障，融合规划出一条全局最优的平滑路径。实验结果表用，融合算法既能实现全局最优的路径规划，又能顺利的躲避随机障碍物，完成复杂环境中的动态路径规划。

附图说明

构成本申请的一部分的说明书附图用来提供对本申请的进一步理解，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。

图1为本发明的基于自适应栅格和改进A*-DWA融合算法的路径规划方法流程示意图；

图2为栅格法坐标示意图；

图3为四向拓展示意图；

图4为八向拓展示意图；

图5为优化前路径的示意图；

图6为优化后路径的示意图；

图7为融合算法流程图。

具体实施方式：

为了使本技术领域的人员更好的理解本说明书中的技术方案，下面结合本说明书实施中的附图，对本发明书实施例中的技术方案进行清楚、完整的描述，但不仅限于此，本发明未详尽说明的，均按本领域常规技术。

传统的路径规划方法绝大多数基于栅格地图进行构建，而栅格尺寸都是固定的，无法根据障碍物分布进行动态调整，影响了路径规划效率。A*与DWA融合算法是求解最短路径的混合路径方法之一，传统的A*算法但无法处理已知信息外的未知障碍物，另外，DWA算法在运行过程中能够实现对障碍物的实时避障，但容易陷入局部最优，导致机器人无法到达目标点。针对上述问题，我们提出了Pagad方法。如图1所示，本发明的基于自适应栅格和改进A*-DWA融合算法的路径规划方法，包括如下步骤：

(1)构建基于自适应栅格尺寸的环境模型，基于障碍物的大小和数量确定自适应栅格边长，并计算自适应栅格边长下的栅格尺寸及最优栅格边长；本步骤利用障碍物面积构建自适应栅格尺寸，提升了算法执行效率；

(2)根据环境模型中已知障碍物信息改进A*算法，完成全局路径规划，得到一条节点少、平滑度高的路径；

(3)结合环境模型中的未知障碍物信息和改进的A*算法得到的全局路径规划节点，选择起点之后的节点依次作为改进的DWA算法中的临时目标点，改进的DWA算法从起点开始不断向临时目标点行进，到达临时目标点之后更新临时目标点，直到到达终点。本发明优化了A*算法的启发函数，整合了搜索路径，将已知障碍物和未知障碍物引入到DWA算法评价函数，并实现了A*算法与DWA算法的融合，减少了搜索节点，提高路径平滑度，实现了机器人对未知障碍物的实时避障与全局路径规划。

基于自适应栅格尺寸的环境模型构建：

环境模型将机器人工作的现实环境转换为能够供自身识别的环境信息，对路径规划系统而言十分重要。本发明考虑实验环境以及模型构建的难易度选择栅格法构建环境模型，并提出了一种基于自适应栅格尺寸的环境模型构建方法。

传统的栅格法，通常会预先设置栅格尺寸，然后利用坐标法对栅格地图进行编号，即将二维栅格地图置于直角坐标系中，地图中的任意位置都可以用坐标(x,y)表示。如图2所示，将实际环境构建为大小为Size＝6×6的二维栅格地图，障碍物用黑色方格表示，起点用方格A表示，终点用方格B表示。那么，起点坐标为[1,6]，终点坐标为[6,1]，障碍物坐标为[(2,1),(3,4),(4,4),(5,5)]，其余白色方格为机器人活动范围，机器人能在除障碍物区域的其他区域中活动。

利用栅格法构建地图时，需要考虑以下几点：(1)环境地图尺寸和栅格尺寸；(2)环境模型中障碍物等位置与实验场景中一一对应；(3)计算机运算能力。其中栅格尺寸的选择至关重要，栅格尺寸太大会阻碍最优路径的发现，栅格尺寸设置太小会影响执行效率。为此，本发明综合考虑了现实环境尺寸、障碍物数量、障碍物大小三个因素，提出了一种基于自适应栅格尺寸的环境模型构建方法。

结合实际情况，本发明栅格边长与障碍物的数量、分布有关。障碍物数量越多、分布越复杂，理论上格边长应该越小。经过分析，障碍物的总面积占比可以一定程度上反应障碍物的数量和分布情况。为此构建基于障碍物的总面积占比的自适应最小栅格边长构建方法。

设地图最小栅格边长为s_min，最大栅格边长为s_max，地图边长为m，则最大栅格尺寸可表示为如下：

已知障碍物的总面积可以表示如下：

其中，Size_{bar_i}代表第i个障碍物的大小，n代表障碍物数量；

自适应栅格边长可表示如下：

自适应栅格边长下的栅格尺寸则如下：

最优栅格边长如下：

传统的A*算法：

传统的A*算法是一种能够实现全局路径规划的启发式算法，多应用于二维栅格地图中。A*算法是结合Dijkstra搜索算法与最佳优先搜索(BFS)算法的优点提出的。Dijkstra算法以拓展点与起点距离为代价进行路径搜索，但没有考虑与目标点之间的距离关系，能够花费较长时间搜索出最优路径。而BFS算法考虑了拓展点与目标点的关系，但得到的路径并不一定是最优的。A*算法综合Dijkstra算法中起点的代价与BFS算法中目标点的代价，建立新的评价函数。

A*算法构建openlist和closelist两个列表，从起点开始，将当前节点设置为父节点，向周围的相邻节点进行拓展，将拓展到的相邻节点加入openlist中计算评价函数数值，选择openlist中评价函数值最小的节点从openlist中移除并加入closelist中，将此节点继续设置为父节点，重复搜索openlist和closelist直到到达终点。

传统A*算法的评价函数如下述公式所示：

F(n)＝G(n)+H(n)

F(n)为算法的当前节点n的总代价；G(n)为从起点到节点n的实际代价；H(n)为当前节点到终点的估计代价，也被称为启发函数。对启发函数的选取影响评价函数的计算，进而影响整个算法的计算和路径选取。传统A*算法常用的启发函数有：曼哈顿距离、切比雪夫距离和欧几里得距离。本文选取欧几里得距离作为传统A*算法的启发函数。

A*算法在当前节点向邻域节点拓展时，通常采用四向、八向拓展，如图3、4所示，四向拓展的方向为上、下、左、右，八向拓展的的方向在前者基础上增加了左上、左下、右上、右下四个方向。四向拓展多用于仓储结构化的搜索方式，本发明采用八向拓展。

传统A*算法的评价函数设计不够合理，当前节点到起点的代价与到终点的代价在评价函数中所占的权重相当，使算法搜索节点多，搜索效率低，无法灵活的根据不同的环境进行调整。传统A*算法规划的路径节点多，常出现多个节点位于同一条直线上的情况，此时，机器人进行简单的运动却需要追踪较多的节点，同时，多段路径间可能存在没有障碍物阻挡但仍然存在转折点的情况，使得路径的转折点多且不平滑，不利于机器人的行进。A*算法在已知环境中进行全局路径规划，无法实现针对未知障碍物的实时避障。

改进的A*算法：

针对传统A*算法在二维栅格地图中规划的路径节点多、转折点多、路径不平滑以及算法搜索效率低的问题，本发明从路径整合和优化启发函数两方面对A*算法进行了改进：

A、路径整合

将多段路径整合为一段路径，剔除冗余节点，提高路径平滑度。如图5所示，初始路径由节点S、G以及1-8组成，可以看出路径转折多且节点冗余。

(2.1)从起点S开始判断当前方向与下一节点方向是否相同，若相同则删除下一节点，不相同则保留，遍历所有节点直至终点G；路径中删除节点1、3、5、7、8，经过此步骤后路径为S-2-4-6-G；

(2.2)从起点S开始判断当前节点与不相邻的节点能否直接连接为一条路径，由整合后的路径到障碍物的距离确定，若该距离大于设置的安全距离，则进行整合，小于设置的安全距离(安全距离可以根据实际运行环境进行设置，例如可设置为代表障碍物的方格的边心距)，则保持原路径；路段S-4与障碍物的距离大于设置的安全距离，将节点2舍弃，路段2-6与障碍物的距离小于安全距离，保留节点4，如图6所示，最终得到的路径为S-4-6-G；

(2.3)将最后得到的路径输出。

B、优化启发函数

传统A*算法根据评价函数对路径进行选择，当前节点到起点的实际代价H(n)与启发函数G(n)在评价函数中所占的权重影响算法的搜索效率，可以通过调整启发函数来优化算法的运算速度。

在复杂环境中，障碍物的存在往往使得算法需要扩大搜索范围来保证到达终点，这将导致算法搜索变慢、冗余节点变多。当障碍物较少时，可以通过提高启发函数H(n)的权重，减小搜索范围、提高搜索速度；当障碍物较多时，可以减少启发函数H(n)的权重，扩大搜索范围，避免陷入局部最优。为此引入环境信息K，K表示在从起点到终点的矩形栅格内障碍物所占比率。

设机器人移动起点为(x_i,y_i)，终点为(x_j,y_j)，N为从起点到终点的矩形栅格内的障碍物数量：

将环境信息K引入评价函数F(n)，根据栅格中障碍物的多少来调整启发函数G(n)的权重，改进后的评价函数可表示为：

F(n)＝G(n)+exp(-K+1)*H(n)。

可以看出，当环境中障碍物少时，H(n)系数大，能够减小搜索范围，提高搜索速度；当环境中障碍物多时，H(n)系数小，增大搜索范围，保证路径全局最优。

传统DWA算法：

DWA算法是一类经典的机器人局部路径规划算法，该算法在速度空间内采样线速度和角速度，并根据机器人的运动学模型预测其下一时间间隔的轨迹，对待评价轨迹进行评分，从而获得更加安全、平滑的最优局部路径。DWA算法的应用场景多为：机器人和目的地的坐标已知、但障碍物坐标未知；有传感器可以检测到障碍物，并且可以得到机器人与障碍物的距离和角度，能够弥补A*算法无法进行实时避障的不足。

动态窗口法对机器人的运动轨迹进行模拟，对模拟后的路径进行评分，选择得分最高的轨迹作为移动路径。设v(t)为机器人t时刻的线速度，ω(t)为机器人t时刻的角速度，运动学模型可以建立为：

上述公式将机器人构建为二维空间直角坐标系中的模型，Δt表示单位时间，x(t)表示机器人t时刻的横坐标，y(t)表示机器人t时刻的纵坐标，θ(t)表示机器人t时刻的角度。

动态窗口法对机器人速度空间进行采样，根据实际情况对采样范围进行约束。通过自身约束与环境约束，实现避障。

机器人速度约束：机器人的移动速度和旋转角速度应保持在一定范围之内；

v_m＝{(v,ω)v∈[v_min,v_max],ω∈[ω_min,ω_max]}

v_m是机器人的线速度和角速度的集合；

机器人加减速度约束：考虑到实际使用的电机性能，使机器人的最大加速度和最大减速度保持在合理范围之内；

式中，v_c表示当前线速度，ω_c表示当前角速度，

表示机器人最大线性加速度，/>

表示机器人最大角加速度，/>

表示机器人最小线性加速度，/>

表示机器人最小角加速度。

机器人安全距离约束：机器人在移动过程中，为避免与障碍物发生碰撞，在加减速度约束下应保持与障碍物的安全距离，使其在与障碍物相撞前将速度降为零；

上式中dist(v,ω)表示轨迹与障碍物的最近距离。

最终的速度空间为上述三个集合的交集，动态窗口V_r定义如下：

V_r＝V_m∩V_d∩V_s

改进的DWA算法：

传统动态窗口法在运行过程中能够成功避开障碍物，但在缺乏全局路径规划的引导下，容易陷入局部最优的循环，导致无法到达目标点。在改进后的融合算法中，利用全局路径规划对动态窗口法进行指引，障碍物将划分为已知障碍物和未知障碍物，需要对传统动态窗口法的评价函数进行改进。

传统动态窗口法的评价函数包含偏向角、线速度及模拟轨迹末端与障碍物距离三个因素，公式为：

G(v,ω)＝αHead(v,ω)+βVel(v,ω)+γDist(v,ω)

其中，Head(v,ω)为当前模拟轨迹的终点方向与目标方向角偏差，Vel(v,ω)为当前所模拟的速度值大小的评价函数，Dist(v,ω)为障碍物与当前模拟轨迹的距离，α、β、γ为各个项的加权系数。

由评价函数可知传统DWA算法只能评价单一障碍物，当同时存在未知障碍物和已知障碍物时，评价函数无法同时对两种障碍物进行评价，导致最终得到的路径与障碍物发生重叠，使机器人与障碍物发生碰撞。

改进后的DWA将已知障碍物和未知障碍物进行区分，共同引入评价函数，改进后的评价函数为：

G(v,ω)＝αHead(v,ω)+βVel(v,ω)+γDist_s(v,ω)+μDist_d(v,ω)

其中，Dist_s(v,ω)为已知障碍物与当前模拟轨迹的距离，Dist_d(v,ω)为未知障碍物与当前模拟轨迹的距离，α、β、γ、μ为各个项的加权系数。

各个项的加权系数数值的选取可由使用者的需求确定，例如，若想增强机器人到达目标点的能力，可提高α，若提高机器人对未知障碍物的避障能力，则可提高μ，通过实际应用中对加权系数的调试可得出最适合于使用者的参数。

无论是传统DWA算法还是改进后的DWA算法，机器人都会沿评价函数值最小的路径前进，在DWA算法应用过程中，容易出现评价函数陷入局部最小导致机器人无法规避障碍物到达目标点的情况。

算法融合：

A*算法能够利用已知的环境信息实现全局路径规划，但无法对未知障碍物进行实时避障，在存在未知障碍物的环境中，算法无法规划出一条完整的路径。DWA算法能够对障碍物实时避障完成局部路径规划，但容易陷入局部最优的情况导致无法顺利到达目标点，无法顺利规划出一条完整的路径。针对两种算法各自的优缺点，将优化后的A*算法和DWA算法进行融合，利用A*算法对DWA算法进行全局路径指引，解决对已知与未知障碍物的避障，还能在保证全局路径最优的情况下提高路径的平滑度，实现机器人能够实时避障的全局路径规划。

图7给出了融合算法设计框架，融合优化后的A*算法和DWA算法两种算法进行路径规划，主要步骤如下：

步骤1：利用已知信息构建栅格地图，地图中的障碍物分类为已知障碍物，优化后的A*算法在栅格地图中完成全局路径规划，得到一条节点少、平滑度高的路径。

步骤2：将在地图中出现的已知障碍物外的障碍物分类为未知障碍物。提取A*算法得到的节点，起点之后的节点依次作为优化后DWA算法中的临时目标点，优化后的DWA算法从起点开始不断向临时目标点行进，到达临时目标点之后则更新临时目标点，直到到达终点，实现机器人行进过程中的实时避障。

以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明所述原理的前提下，还可以作出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (6)

1.一种基于自适应栅格和改进A*-DWA融合算法的路径规划方法，其特征在于，包括如下步骤：

(1)构建基于自适应栅格尺寸的环境模型，基于障碍物的大小和数量确定自适应栅格边长，并计算自适应栅格边长下的栅格尺寸及最优栅格边长；

(2)根据环境模型中已知障碍物信息改进A*算法，完成全局路径规划，得到一条节点少、平滑度高的路径；

(3)结合环境模型中的未知障碍物信息和改进的A*算法得到的全局路径规划节点，选择起点之后的节点依次作为改进的DWA算法中的临时目标点，改进的DWA算法从起点开始不断向临时目标点行进，到达临时目标点之后更新临时目标点，直到到达终点。

2.根据权利要求1所述的基于自适应栅格和改进A*-DWA融合算法的路径规划方法，其特征在于，步骤(1)中，设地图最小栅格边长为s_min，最大栅格边长为s_max，地图边长为m，则最大栅格尺寸表示为如下：

已知障碍物的总面积表示如下：

其中，Size_{bar_i}代表第i个障碍物的大小，n代表障碍物数量；

自适应栅格边长表示如下：

自适应栅格边长下的栅格尺寸则如下：

最优栅格边长如下：

3.根据权利要求2所述的基于自适应栅格和改进A*-DWA融合算法的路径规划方法，其特征在于，改进A*算法包括路径整合和优化启发函数。

4.根据权利要求3所述的基于自适应栅格和改进A*-DWA融合算法的路径规划方法，其特征在于，路径整合的具体过程包括：

(2.1)从起点S开始判断当前方向与下一节点方向是否相同，若相同则删除下一节点，不相同则保留，遍历所有节点直至终点G；

(2.2)从起点S开始判断当前节点与不相邻的节点能否直接连接为一条路径，由整合后的路径到障碍物的距离确定，若该距离大于设置的安全距离，则进行整合，小于设置的安全距离，则保持原路径；

(2.3)将最后得到的路径输出。

5.根据权利要求4所述的基于自适应栅格和改进A*-DWA融合算法的路径规划方法，其特征在于，优化启发函数的过程如下：

设机器人移动起点为(x_i,y_i)，终点为(x_j,y_j)，N为从起点到终点的矩形栅格内的障碍物数量，引入环境信息K，K表示在从起点到终点的矩形栅格内障碍物所占比率：

将环境信息K引入评价函数F(n)，根据栅格中障碍物的多少来调整启发函数G(n)的权重，改进后的评价函数表示为：

F(n)＝G(n)+exp(-K+1)*H(n)。

6.根据权利要求5所述的基于自适应栅格和改进A*-DWA融合算法的路径规划方法，其特征在于，步骤(3)中，改进后的DWA将已知障碍物和未知障碍物进行区分，共同引入评价函数，改进后的评价函数为：

G(v,ω)＝αHead(v,ω)+βVel(v,ω)+γDist_s(v,ω)+μDist_d(v,ω)

其中，Head(v,ω)为当前模拟轨迹的终点方向与目标方向角偏差，Vel(v,ω)为当前所模拟的速度值大小的评价函数，Dist_s(v,ω)为已知障碍物与当前模拟轨迹的距离，Dist_d(v,ω)为未知障碍物与当前模拟轨迹的距离，α、β、γ、μ为各个项的加权系数。

Images