基于智能算法的路径规划代码python 算法 路径规划

第一章 路径规划算法概述

第一章 路径规划算法概述

---

文章目录

• 第一章 路径规划算法概述
• 前言
• 一、传统路径规划算法

• 1.1 Dijkstra算法
• 1.2 A*算法
• 1.3 D*算法
• 1.4 人工势场法

• 二、基于采样的路径规划算法

• 2.1 PRM算法
• 2.2 RRT算法

• 三、智能仿生路径规划算法

• 3.1 神经网络算法
• 3.2 蚁群算法
• 3.3遗传算法

• 总结

---

前言

随着半导体技术和人工智能的快速发展，无人驾驶越来越成为可能。无人驾驶系统的核心可以概述为四个部分：感知（Perception）、定位（Localization）、规划（Planning）和控制（Control）。

规划是指无人车为了到达某一目的地而做出决策和计划的过程。对于无人驾驶车辆而言，这个过程通常包括从起始地到达目的地，同时要避开障碍物，并且不断优化行车路线轨迹和行为，以保证乘车的安全舒适。

路径规划方法主要分为传统的路径规划算法、基于采样的路径规划算法、智能仿生算法。传统的路径规划算法主要有A算法、Dijkstra算法、D算法、人工势场法，基于采样的路径规划算法有PRM算法、RRT算法，智能仿生路径规划算法有神经网络算法、蚁群算法、遗传算法等。

---

一、传统路径规划算法

1.1 Dijkstra算法

Dijkstra算法是Edsger Wybe Dijkstra在1956年提出的一种用来寻找图形中结点之间最短路径的算法。主要特点是从起始点开始，采用贪心算法的策略，每次遍历到始点距离最近且未访问过的顶点的邻接节点，直到扩展到终点为止。Dijkstra算法在扩展的过程中，都是取出未访问结点中距离该点距离最小的结点，然后利用该结点去更新其他结点的距离值。
PS：此算法不能用于求负权图，要求所有边的权重都为非负值。 优点：
1） 如果最优路径存在，那么一定能找到最优路径
缺点：
1） 有权图中可能是负边
2） 扩展的结点很多，效率低

1.2 A*算法

A* 算法发表于1968年，A* 算法是将Dijkstra算法与广度优先搜索算法（BFS, Breath First Search）二者结合而成，通过借助启发式函数的作用，能够使该算法能够更快的找到最优路径。A*算法是静态路网中求解最短路径最有效的直接搜索方法。

• A* 算法是一个“搜索算法”，实质上是广度优先搜索算法（BFS）的优化。从起点开始，首先遍历起点周围邻近的点，然后再遍历已经遍历过的点邻近的点，逐步的向外扩散，直到找到终点。
• A* 算法的作用是“求解最短路径”，如在一张有障碍物的图上移动到目标点，以及八数码问题（从一个状态到另一个状态的最短途径）
• A* 算法的思路类似图的Dijkstra算法，采用贪心的策略，即“若A到C的最短路径经过B，则A到B的那一段必须取最短”，找出起点到每个可能到达的点的最短路径并记录。
• A* 算法与Dijkstra算法的不同之处在于，A* 算法是一个“启发式”算法，它已经有了一些我们告诉它的先验知识，如“朝着终点的方向走更可能走到”。它不仅关注已走过的路径，还会对未走过的点或状态进行预测。因此A* 算法相较与Dijkstra而言，调整了进行BFS的顺序，少搜索了哪些“不太可能经过的点”，更快地找到目标点的最短路径。另外一点，由于H选取的不同，A * 算法找到的路径可能并不是最短的，但是牺牲准确率带来的是效率的提升。

A*算法的启发式函数： $f(n)=g(n)+h(n)$
$g(n)$：从起点 A 移动到指定方格的移动代价，沿着到达该方格而生成的路径。
$h(n)$：从指定的方格移动到终点 B 的估算成本。这个通常被称为试探法，有点让人混淆。为什么这么叫呢，因为这是个猜测。直到我们找到了路径我们才会知道真正的距离，因为途中有各种各样的东西 ( 比如墙壁，水等 ) 。

$g(n)$来源于已知点信息，$h(n)$来源于对未知点信息的估计，$f(n)$为选择下一个将遍历节点的依据。

此外，$h(n)$还有一个特征：

• 在极端情况下，当启发函数$h(n)$始终为0，则将由$g(n)$决定节点的优先级，此时算法就退化成了Dijkstra算法。
• 如果$h(n)$始终小于等于节点$n$到终点的代价，则A*算法保证一定能够找到最短路径。但是当$h(n)$的值越小，算法将遍历越多的节点，也就导致算法越慢。
• 如果$h(n)$完全等于节点$n$到终点的代价，则A*算法将找到最佳路径，并且速度很快。可惜的是，并非所有场景下都能做到这一点。因为在没有达到终点之前，我们很难确切算出距离终点还有多远。
• 如果$h(n)$的值比节点n到终点的代价要大，则A* 算法不能保证找到最短路径，不过此时会很快。
• 在另外一个极端情况下，如果$h(n)$相较于$g(n)$大很多，则此时只有$h(n)$产生效果，这也就变成了广度优先搜索(BFS)。

优点：
1） 利用启发式函数，搜索范围小，提高了搜索效率
2） 如果最优路径存在，那么一定能找到最优路径
缺点：
1） A算法不适用于动态环境
2） A算法不太适合于高维空间，计算量大
3） 目标点不可达时会造成大量性能消耗

1.3 D*算法

D* 算法(Dynamic A Star)是卡耐基梅隆机器人中心的Stentz在1994年提出的主要用于机器人探路，并且美国火星探测器上就是应用的D* 路径规划算法。A* 算法适用于在静态路网中寻路，在环境变化后，往往需要replan，由于A* 不能有效利用上次计算的信息，故计算效率较低。D* 算法由于储存了空间中每个点到终点的最短路径信息，故在重规划时效率大大提升。A* 是正向搜索，而D* 特点是反向搜索，即从目标点开始搜索过程。在初次遍历时候，与Dijkstra算法一致，它将每个节点的信息都保存下来。

优点：
1）适用于动态环境的路径规划，搜索效率高
缺点：
1） 不适用于高维空间，计算量大
2） 不太适用于在距离较远的最短路径上发生变化的场景

1.4 人工势场法

人工势场法是由Khatib在1985年提出的一种基于虚拟力场的局部路径规划算法，该算法的基本思想是当机器人在周围环境中运动时，将环境设计成一种抽象的人造引力场，目标点对移动机器人产生“引力”，障碍物对移动机器人产生“斥力”，最后通过二者的合力来控制移动机器人的运动。应用人工势场法规划出来的路径一般是比较平滑并且安全的，但是存在着局部最优的问题。
利用势场函数$U$来建立人工势场，势场函数是一种可微函数，空间中某点处势场函数值的大小，代表了该点的势场强度。我们采用引力势场函数与斥力势场函数，用$U(q)$表示二者之和。
$U(q) = U_{att}(q) + U_{rep}(q)$
引力势场函数
$U_{att}(q) = \frac{1}{2}\xi\rho^{2}(q, q_{goal})$
其中，$\xi$表示引力增益，$\rho(q, q_{goal})$表示当前点到目标点的距离；
斥力势场函数：
$U_{rep}(q) = \begin{cases} 0,\,\,\rho(q, q_{obs})>\rho_{0}\\ \frac{1}{2}\eta(\frac{1}{\rho(q, q_{obs})} - \frac{1}{\rho_{0}})^2,\,\,\rho(q, q_{obs})\le\rho_{0}\\ \end{cases}$
$\eta$表示斥力增益，$\rho(q, q_{obs})$表示当前点到障碍物的距离，$\rho_{0}$表示障碍物作用距离阈值。

优点：
1） 规划出的路径一般是比较平滑且安全
2） 人工势场法是一种反馈控制策略，具有一定的鲁棒性
缺点：
1） 容易陷入局部最优的问题
2） 距离目标点较远时，引力特别大，斥力相对较小，可能会发生碰撞
3） 当目标点附近有障碍物时，斥力非常大，引力较小，很难到达目标点

二、基于采样的路径规划算法

2.1 PRM算法

随机路线图（PRM）算法是一种基于图搜索的算法，可以将连续状态空间转换成离散状态空间，在利用A＊等搜索算法在路线图上寻找路径，提高搜索效率。PRM算法是概率完备且不是最优的算法。
优点：
1） 适用于高维空间和复杂约束的路径规划问题
2） 搜索效率高，搜索速度快
缺点：
1）概率完备但不是最优

2.2 RRT算法

RRT算法是适用于高维空间，通过对状态空间中的采样点进行碰撞检测，避免了对空间的建模，较好的处理带有非完整约束的路径规划问题，有效的解决了高维空间和复杂约束的路径规划问题。该算法是概率完备但不是最优的算法。
RRT算法以初始点$q_{init}$作为根节点，通过随机采样增加叶子节点的方式，生成一个随机扩展树，当目标点位于随机扩展树上时，能够找到一天初始点到目标点的路径。首先，需要从状态空间中随机选择一个采样点$q_{rand}$，然后从随机树中选择一个距离$q_{rand}$最近的结点$q_{nearest}$，从$q_{nearest}$向$q_{rand}$扩展一个步长的距离，得到一个新的结点$q_{new}$，如果$q_{new}$与障碍物发生碰撞，则返回空；否则，将$q_{new}$加入到随机树中，重复上述步骤直到$q_{nearest}$和$q_{goal}$距离小于一个阈值。

优点：
1） 搜索效率比较高，搜索速度比较快
2） 适用于高维空间，不会产生维度灾难的问题
3） 只需对状态空间采样点进行碰撞检测，避免了对空间的建模
缺点：
1） 规划出的路径质量一般，可能存在棱角、不够光滑
2） RRT算法不太适用于存在狭长空间的环境
3） 规划出的路径可能不是最优路径
4） 不适用于动态环境的路径规划

三、智能仿生路径规划算法

3.1 神经网络算法

随着机器人自主性的不断提高，使其具有环境感知以及环境学习的能力，许多学者提出了深度强化学习算法来解决移动机器人处于动态复杂环境中路径规划的问题。深度强化学习算法充分利用了深度学习的感知能力与强化学习的决策能力，通过机器人与环境的交互过程不断试错，通过环境评价性的反馈，实现系统更加智能的决策控制，帮助移动机器人在某些复杂未知的环境中实现一定程度的自主化与智能化。
路径规划就是一个标准的MDP问题，强化学习可以通过值迭代等方法建立一个表格，用以存储状态s到动作a的映射。但是在复杂环境中会产生维度灾难的问题，因此神经网络可以解决维度灾难的问题。以DQN算法为例，来讲解神经网络算法在路径规划中的应用。DQN算法是Q-Learning算法与卷积神经网络（CNN）二者进行结合。DQN算法是由两个网络结构、初始参数相同的结构组成，一个是估计网络，用来输出估计值函数；另一个是目标网络，用来输出目标值函数。通过强化学习使机器人与环境进行交互得到样本（s,a,r,s’），将所有的样本集放入到经验回放池中。神经网络进行训练时，随机的从经验回放池中抽取batchsz数量的样本，将样本输入进神经网络，利用神经网络的非线性拟合能力，拟合出非线性函数来表达我们的Q值，利用e-greedy策略来进行选择智能体的动作。智能体执行完相应的动作之后，环境会反馈一个状态和奖励值，最后经过神经网络模型的训练和优化得到网络的训练参数，得到相对准确的动作输出。
优点：
1） 适合于动态复杂环境
2） 适用于高维空间，避免维度灾难的问题
缺点：
1） 对硬件的要求比较高
2） 参数调节比较困难

3.2 蚁群算法

蚁群算法（Ant Colony Optimization，ACO）是Dorigo在1992年提出的一种用来寻找优化路径的概率型算法，是由一群无智能或有轻微智能的个体通过相互写作而表现出智能行为，为求解复杂问题提供了一个新的可能性。该算法的主要思想是蚂蚁在寻找食物过程中发现路径的行为。该算法具有分布计算、信息正反馈和启发式搜索的特征，本质上是进化算法中的一种启发式全局优化算法。
蚁群算法的基本原理是利用蚂蚁在觅食过程中会释放信息素。
影响蚁群算法的因素：
1） 信息素如何撒播
2） 信息素如何挥发
3） 以何种方式让蚂蚁选择运动方向，减少盲目性和不必要性
4） 给予蚂蚁和环境一定的记忆能力能够帮助减少搜索空间
个体分布越均匀，种群多样性就越好，得到全局最优解的概率就越大，但是寻优时间就越长；个体分布越集中，种群多样性就越差，不利于发挥算法的探索能力。正反馈加快了蚁群算法的收敛速度，却使算法较早地集中于部分候选解，因此正反馈降低了种群的多样性，也不利于提高算法的全局寻优能力。
优点：
1） 蚁群算法的鲁棒性强
2） 采用正反馈机制，能够逼近最优解
3） 易与其他算法进行结合
缺点：
1） 盲目的随机搜索，搜索时间较长，搜索速度慢
2） 蚁群算法容易陷入局部最优
3） 蚁群算法参数的选择依赖于经验或试错

3.3遗传算法

遗传算法（Genetic Algorithm，GA）是由John Holland于20世纪70年代提出，该算法是根据大自然中生物体进化规律而设计提出的，来模拟达尔文生物进化论的自然选择和遗传学机理的生物进化过程的计算模型，是一种通过模拟自然进化过程搜索最优解的方法。其本质是一种高效、并行、全局搜索的方法，能在搜索过程中自动获取和积累有关搜索空间的知识，并自适应地控制搜索过程以求得最佳解。
优点：
1） 遗传算法具有广泛的应用领域
2） 遗传算法具有群体搜索的特性
3） 遗传算法基于概率规则，搜索更为灵活
4） 遗传算法直接以目标函数作为搜索信息，不涉及目标函数值求微分的过程
缺点：
1） 遗传算法效率比较低
2） 遗传算法容易过早收敛
3） 遗传算法在编码时容易出现不规范不准确的问题

---

总结

以上就是路径规划算法的基本介绍，本文只是浅显的讲解一下，鉴于我自己本身也是初学者，很多内容copy了别人的文章，写的也不够生动形象，以后本文会按照自己的理解持续优化，希望自己能填完这个坑。