SLAM&Navigation 导航算法基础知识汇总
声明：
本篇是收集网上一些关于建图导航基础算法的简介汇总，主要用于个人学习。若有侵权，联系博主第一时间删除。
路径规划论文
https://github.com/zhm-real/PathPlanning
A*算法
Introduction to the A* Algorithm
算法动图演示：
https://www.redblobgames.com/pathfinding/a-star/introduction.html
算法介绍
A*（念做：A Star）算法是一种很常用的路径查找和图形遍历算法。它有较好的性能和准确度。
A*算法最初发表于1968年，由Stanford研究院的Peter Hart, Nils Nilsson以及Bertram Raphael发表。它可以被认为是Dijkstra算法的扩展。
广度优先搜索（BFS）
广度优先搜索(Breadth First)以广度做为优先级进行搜索。
从起点开始，首先遍历起点周围邻近的点，然后再遍历已经遍历过的点邻近的点，逐步的向外扩散，直到找到终点。这种算法就像洪水（Flood fill）一样向外扩张。在执行算法的过程中，每个点需要记录达到该点的前一个点的位置 – 可以称之为父节点。这样做之后，一旦到达终点，便可以从终点开始，反过来顺着父节点的顺序找到起点，由此就构成了一条路径。
Dijkstra算法
Dijkstra算法是由计算机科学家Edsger W. Dijkstra在1956年提出的。
Dijkstra算法用来寻找图形中节点之间的最短路径。在一些情况下，图形中相邻节点之间的移动代价并不相等。例如，游戏中的一幅图，既有平地也有山脉，那么游戏中的角色在平地和山脉中移动的速度通常是不相等的。
在Dijkstra算法中，需要计算每一个节点距离起点的总移动代价。同时，还需要一个优先队列结构。对于所有待遍历的节点，放入优先队列中会按照代价进行排序。在算法运行的过程中，每次都从优先队列中选出代价最小的作为下一个遍历的节点。直到到达终点为止。
下面对比了不考虑节点移动代价差异的广度优先搜索与考虑移动代价的Dijkstra算法的运算结果：
; 最佳优先搜索
在一些情况下，如果我们可以预先计算出每个节点到终点的距离，则我们可以利用这个信息更快的到达终点。
其原理也很简单。与Dijkstra算法类似，我们也使用一个优先队列，但此时以每个节点到达终点的距离作为优先级，每次始终选取到终点移动代价最小（离终点最近）的节点作为下一个遍历的节点。这种算法称之为最佳优先（Best First）算法。
这样做可以大大加快路径的搜索速度，如下图所示：
缺点：如果起点和终点之间存在障碍物，则最佳优先算法找到的很可能不是最短路径。
A*算法
A*算法实际上是综合上面这些算法的特点于一身的。
A*算法通过下面这个函数来计算每个节点的优先级。
其中：
- f(n)是节点n的综合优先级。当我们选择下一个要遍历的节点时，我们总会选取综合优先级最高（值最小）的节点。
- g(n) 是节点n距离起点的代价。
- h(n)是节点n距离终点的预计代价，这也就是A*算法的启发函数。
A*算法在运算过程中，每次从优先队列中选取f(n)值最小（优先级最高）的节点作为下一个待遍历的节点。
另外，A*算法使用两个集合来表示待遍历的节点，与已经遍历过的节点，这通常称之为 open_set和 close_set。
完整的A*算法描述如下：
* 初始化open_set和close_set；
* 将起点加入open_set中，并设置优先级为0（优先级最高）；
* 如果open_set不为空，则从open_set中选取优先级最高的节点n：
* 如果节点n为终点，则：
* 从终点开始逐步追踪parent节点，一直达到起点；
* 返回找到的结果路径，算法结束；
* 如果节点n不是终点，则：
* 将节点n从open_set中删除，并加入close_set中；
* 遍历节点n所有的邻近节点：
* 如果邻近节点m在close_set中，则：
* 跳过，选取下一个邻近节点
* 如果邻近节点m也不在open_set中，则：
* 设置节点m的parent为节点n
* 计算节点m的优先级
* 将节点m加入open_set中
启发函数
上面已经提到，启发函数会影响A*算法的行为。
- 在极端情况下，当启发函数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)产生效果，这也就变成了最佳优先搜索。
由上面这些信息我们可以知道，通过调节启发函数我们可以控制算法的速度和精确度。因为在一些情况，我们可能未必需要最短路径，而是希望能够尽快找到一个路径即可。这也是A*算法比较灵活的地方。
对于网格形式的图，有以下这些启发函数可以使用：
- 如果图形中只允许朝上下左右四个方向移动，则可以使用曼哈顿距离（Manhattan distance）。
- 如果图形中允许朝八个方向移动，则可以使用对角距离。
- 如果图形中允许朝任何方向移动，则可以使用欧几里得距离（Euclidean distance）。
关于距离
曼哈顿距离
如果图形中只允许朝上下左右四个方向移动，则启发函数可以使用曼哈顿距离，它的计算方法如下图所示：
计算曼哈顿距离的函数如下，这里的D是指两个相邻节点之间的移动代价，通常是一个固定的常数。
function heuristic(node) =
dx = abs(node.x - goal.x)
dy = abs(node.y - goal.y)
return D * (dx + dy)
对角距离
如果图形中允许斜着朝邻近的节点移动，则启发函数可以使用对角距离。它的计算方法如下：
计算对角距离的函数如下，这里的D2指的是两个斜着相邻节点之间的移动代价。如果所有节点都正方形，则其值就是
function heuristic(node) =
dx = abs(node.x - goal.x)
dy = abs(node.y - goal.y)
return D * (dx + dy) + (D2 - 2 * D) * min(dx, dy)
欧几里得距离
如果图形中允许朝任意方向移动，则可以使用欧几里得距离。
欧几里得距离是指两个节点之间的直线距离，因此其计算方法也是我们比较熟悉的：
其函数表示如下：
function heuristic(node) =
dx = abs(node.x - goal.x)
dy = abs(node.y - goal.y)
return D * sqrt(dx * dx + dy * dy)
附：几种寻路算法距离
https://zhuanlan.zhihu.com/p/110939583
算法变种
A*算法有不少的变种，这里我们介绍最主要的几个。
更多的内容请以访问维基百科：A* Variants。
ARA*
ARA 全称是Anytime Repairing A，也称为Anytime A*。
与其他Anytime算法一样，它具有灵活的时间成本，即使在它结束之前被中断，也可以返回路径查找或图形遍历问题的有效解决方案。方法是在逐步优化之前生成快速，非最优的结果。
在现实世界的规划问题中，问题的解决时间往往是有限的。与时间相关的规划者对这种情况都会比较熟悉：他们能够快速找到可行的解决方案，然后不断努力改进，直到时间用完为止。
启发式搜索ARA算法，它根据可用的搜索时间调整其性能边界。它首先使用松散边界快速找到次优解，然后在时间允许的情况下逐渐收紧边界。如果有足够的时间，它会找到可证明的最佳解决方方案。在改进其约束的同时，ARA重复使用以前的搜索工作，因此，比其他随时搜索方法更有效。
与A _算法不同，Anytime A_算法最重要的功能是，它们可以被停止，然后可以随时重启。该方法使用控制管理器类来处理时间限制以及停止和重新启动A*算法以找到初始的，可能是次优的解决方案，然后继续搜索改进的解决方案，直到达到可证明的最佳解决方案。
关于ARA*的更多内容可以阅读这篇论文：
D*
D _是Dynamic A_的简写，其算法和A*类似，不同的是，其代价的计算在算法运行过程中可能会发生变化。
D*包含了下面三种增量搜索算法：
- 原始的D*由Anthony Stentz发表。
- Focussed D _由Anthony Stentz发表，是一个增量启发式搜索算法，结合了A_和原始D*的思想。
- D Lite是由Sven Koenig和Maxim Likhachev基于LPA构建的算法。
所有三种搜索算法都解决了相同的基于假设的路径规划问题，包括使用自由空间假设进行规划。在这些环境中，机器人必须导航到未知地形中的给定目标坐标。它假设地形的未知部分（例如：它不包含障碍物），并在这些假设下找到从当前坐标到目标坐标的最短路径。
然后机器人沿着路径行进。当它观察到新的地图信息（例如以前未知的障碍物）时，它会将信息添加到其地图中，并在必要时将新的最短路径从其当前坐标重新添加到给定的目标坐标。它会重复该过程，直到达到目标坐标或确定无法达到目标坐标。在穿越未知地形时，可能经常发现新的障碍，因此重新计划需要很快。增量（启发式）搜索算法通过使用先前问题的经验来加速搜索当前问题，从而加速搜索类似搜索问题的序列。假设目标坐标没有改变，则所有三种搜索算法都比重复的A*搜索更有效。
D 及其变体已广泛用于移动机器人和自动车辆导航。当前系统通常基于D Lite而不是原始D 或Focussed D。
关于D*的更多内容可以阅读这两篇文章：
- Project “Fast Replanning （Incremental Heuristic Search）”
- Real-Time Replanning in Dynamic and Unknown Environments
Field D*
Field D _扩展了D_和D* Lite，是一种基于插值（ interpolation-based ）的规划算法，它使用线性插值来有效地生成低成本路径，从而消除不必要的转向。
在给定线性插值假设的情况下，路径是最优的，并且在实践中非常有效。该算法目前被各种现场机器人系统使用。
关于Field D*的详细内容可以看下面这篇论文：
Block A*
Block A 扩展自A，但它操作是一块（block）单元而不是单个单元。
其open_set中的每个条目都是已到达但尚未扩展的块，或者需要重新扩展的块。
open_set中块的优先级称为其堆值（heap value）。与A _类似，Block A_中的基本循环是删除具有最低堆值的条目并将其展开。在扩展期间使用LDDB来计算正在扩展的块中的边界单元的g值。
LDDB是一种新型数据库，它包含了本地邻域边界点之间的距离。
关于Block A*的更多内容可以看下面这篇论文：
DWA算法
算法参考
自动驾驶路径规划DWA算法原理解析
ROS导航-动态窗口方法（Dynamic Window Approach）
https://zhuanlan.zhihu.com/p/67335058
DWA算法总结
https://zhuanlan.zhihu.com/p/519958218
DWA算法概述
DWA算法（dynamic window approach），其原理主要是在速度空间（v,w）中 采样多组速度，并模拟出这些速度在一定时间内的 运动轨迹，并通过 评价函数对这些轨迹进行评价， 选取最优轨迹对应的（v,w）驱动机器人运动。
注：速度空间（v,w）：速度搜索空间，受到各种限制条件，后面会详细谈到。评价函数可以根据自己的需求进行更改，原始论文和ROS中的评价函数不一样。
优点：
- 计算复杂度低：考虑到速度和加速度的限制，只有安全的轨迹会被考虑，且每次采样的时间较短，因此轨迹空间较小
- 可以实现避障：可以实时避障，但是避障效果一般
- *适用于两轮差分和全向移动模型
缺点：
- 前瞻性不足：只模拟并评价了下一步，如在机器人前段遇见”C”字形障碍时，不能很好的避障
- 动态避障效果差： 模拟运动轨迹断，动态避障效果差
- 非全局最优路径： 每次都选择下一步的最佳路径，而非全局最优路径
- *不适用于阿克曼模型
机器人运动学模型
为什么要分析机器人运动学模型？应为要根据采样的速度（v,w）模拟机器人运动的轨迹，因此先要分析机器人运动学模型。下面以两轮移动机器人模型，分两个方面进行分析。
; 非全向移动机器人
机器人只能向前运动或者旋转；需要注意的是，上图中有两个坐标系，一个是机器人的坐标系，另外一个是世界坐标系（也就是我们的坐标轴），下式中v(t)指的是机器人坐标系中x方向的速度；t+ [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-DJbFS5uA-1665045181897)(https://www.zhihu.com/equation?tex=%5CDelta)] t时刻与 t时刻的位置、速度关系式如下：
全向移动机器人
对于全向移动机器人，在机器人坐标系中，不仅有x方向的速度，还有y方向的速度，另外还可以旋转。t+ [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-j1TaeYi1-1665045181898)(https://www.zhihu.com/equation?tex=%5CDelta)] t时刻与 t时刻的位置、速度关系式如下：
需要补充说明的是， 令 [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-jdzHKWPt-1665045181900)(https://www.zhihu.com/equation?tex=v_%7By%7D)] (t)=0，上式与非全向移动机器人中公式表达式完全相同，故ROS中采用2.2中的公式进行计算，所以DWA算法适用于两轮差速和全向移动机器人。
; 速度空间(v,w)
现在我们能够解决机器人运动轨迹的问题，但是速度空间（v,w）的问题还没有解决，我们能够进行采样的速度搜索空间是多少呢？机器人的速度受到各种因素限制，下面做一个简单的探讨。
移动机器人受自身最大速度最小速度的限制
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-V0bHDe62-1665045181900)(https://www.zhihu.com/equation?tex=V_%7Bs%7D)] 为机器人能够到达的所有矢量速度的集合；机器人受到最大最小线速度和角速度影响。
; 移动机器人受电机性能的影响
由于加速度有一个范围限制，所以最大加速度或最大减速度一定时间内能达到的速度 ，才会被保留，表达式如下：
移动机器人受障碍的影响
为了能在碰到障碍物前停下来，在最大减速度的条件下，速度满足以下条件：
其中dist(v,w)为（v,w）对应的轨迹上里障碍物最近的距离。
在上述三条约束条件的限制下，速度空间（v,w）会有一定的范围，另外会随着电机的线加速度、角加速度进行变换，速度空间会动态变化，我们将其称为 动态窗口。在满足约束条件的情况下，进行采样（v,w），可以得到相应的轨迹：
; 评价函数
在速度空间（v,w）中采样，根据运动学模型推测对应的轨迹，接下来引入评价函数，对轨迹进行打分，选取最优的轨迹。
一般来说，评价函数如下：
其中，heading(v,w)为方位角评价函数：评价机器人在当前的设定的速度下，轨迹末端朝向与目标点之间的角度差距；dist(v,w) 主要的意义为机器人处于预测轨迹末端点位置时与地图上最近障碍物的距离，对于靠近障碍物的采样点进行惩罚，确保机器人的避障能力，降低机器人与障碍物发生碰撞的概率；velocity(v,w)为当前机器人的线速度，为了促进机器人快速到达目标； α， β，γ ，σ为权重。当然，可以对评价函数进行优化，添加更多的评价函数指标。
K近邻算法(KNN)
k近邻算法的基本概念原理以及应用
k近邻算法(k-Nearest Neighbor，KNN)是一种 基本分类和回归方法。
K近邻算法，即是给定一个训练数据集，对新的输入实例，在训练数据集中找到与该实例 最邻近的K个实例， 这K个实例的多数属于某个类，就把该输入实例分类到这个类中。（ 这就类似于现实生活中少数服从多数的思想）根据这个说法，咱们来看下引自维基百科上的一幅图：
如上图所示，有 两类不同的样本数据，分别用蓝色的小正方形和红色的小三角形表示，而图正中间的那个绿色的圆所标示的数据则是 待分类的数据。这也就是我们的目的，来了一个新的数据点，我要得到它的类别是什么？好的，下面我们根据k近邻的思想来给绿色圆点进行分类。
- 如果K=3，绿色圆点的最邻近的3个点是2个红色小三角形和1个蓝色小正方形，少数从属于多数，基于统计的方法，判定绿色的这个待分类点属于红色的三角形一类。
- 如果K=5，绿色圆点的最邻近的5个邻居是2个红色三角形和3个蓝色的正方形，还是少数从属于多数，基于统计的方法，判定绿色的这个待分类点属于蓝色的正方形一类。
从上面例子我们可以看出，k近邻的算法思想非常的简单，也非常的容易理解，那么我们是不是就到此结束了， 该算法的原理我们也已经懂了，也知道怎么给新来的点如何进行归类，只要找到离它最近的k个实例，哪个类别最多即可。
; k近邻算法中k的选取以及特征归一化的重要性
选取k值以及它的影响
k近邻的k值我们应该怎么选取呢？
如果我们选取较小的k值，那么就会意味着我们的整体模型会变得复杂，容易发生过拟合！
假设我们选取k=1这个极端情况，怎么就使得模型变得复杂，又容易过拟合了呢？
假设我们有训练数据和待分类点如下图：
上图中有俩类，一个是 黑色的圆点，一个是 蓝色的长方形，现在我们的待分类点是 红色的五边形。
好，根据我们的k近邻算法步骤来决定待分类点应该归为哪一类。我们由图中可以得到， 很容易我们能够看出来五边形离黑色的圆点最近，k又等于1，那太好了，我们最终判定待分类点是黑色的圆点。
由这个处理过程我们很容易能够感觉出问题了，如果k太小了，比如等于1，那么模型就太复杂了， 我们很容易学习到噪声，也就非常容易判定为噪声类别，而在上图，如果，k大一点，k等于8， 把长方形都包括进来，我们很容易得到我们正确的分类应该是蓝色的长方形！如下图：
所谓的过拟合就是在训练集上准确率非常高，而在测试集上准确率低，经过上例，我们可以得到k太小会导致 过拟合， 很容易将一些噪声（如上图离五边形很近的黑色圆点）学习到模型中，而忽略了数据真实的分布！
如果我们选取较大的k值，就相当于用较大邻域中的训练数据进行预测，这时与输入实例较远的（不相似）训练实例也会对预测起作用，使预测发生错误，k值的增大意味着整体模型变得简单。
我们想，如果k=N（N为训练样本的个数）,那么无论输入实例是什么，都将简单地预测它属于在训练实例中最多的类。这时，模型是不是非常简单，这相当于你压根就没有训练模型呀！直接拿训练数据统计了一下各个数据的类别，找最大的而已！这好像下图所示：
我们统计了黑色圆形是8个，长方形个数是7个，那么哈哈，如果k=N，我就得出结论了，红色五边形是属于黑色圆形的（ 明显是错误的）
这个时候，模型过于简单，完全忽略训练数据实例中的大量有用信息，是不可取的。
k值既不能过大，也不能过小，在举的这个例子中，我们k值的选择，在下图红色圆边界之间这个范围是最好的，如下图：
（注：真实例子中不可能只有俩维特征，但是原理是一样的1，我们就是想找到较好的k值大小）
那么我们一般怎么选取呢？李航博士书上讲到，我们一般选取一个较小的数值，通常采取 交叉验证法来选取最优的k值。（ 也就是说，选取k值很重要的关键是实验调参，类似于神经网络选取多少层这种，通过调整超参数来得到一个较好的结果）
; 距离的度量
在上文中说到，k近邻算法是在训练数据集中找到与该实例 最邻近的K个实例，这K个实例的多数属于某个类，我们就说预测点属于哪个类。
定义中所说的最邻近是如何度量呢？我们怎么知道谁跟测试点最邻近。这里就会引出我们几种度量俩个点之间距离的标准。
我们可以有以下几种度量方式：
其中当p=2的时候，就是我们最常见的欧式距离，我们也一般都用欧式距离来衡量我们高维空间中俩点的距离。在实际应用中，距离函数的选择应该根据数据的特性和分析的需要而定，一般选取p=2欧式距离表示，这不是本文的重点。
特征归一化的必要性
首先举例如下，我用一个人身高(cm)与脚码（尺码）大小来作为特征值，类别为男性或者女性。我们现在如果有5个训练样本，分布如下：
A [(179,42),男] B [(178,43),男] C [(165,36)女] D [(177,42),男] E [(160,35),女]
通过上述训练样本，我们看出问题了吗？
很容易看到第一维身高特征是第二维脚码特征的4倍左右，那么在进行距离度量的时候，我们就会偏向于第一维特征。这样造成俩个特征并不是等价重要的，最终可能会导致距离计算错误，从而导致预测错误。口说无凭，举例如下：
现在我来了一个测试样本 F(167,43)，让我们来预测他是男性还是女性，我们采取k=3来预测。
下面我们用欧式距离分别算出F离训练样本的欧式距离，然后选取最近的3个，多数类别就是我们最终的结果，计算如下：
由计算可以得到，最近的前三个分别是C,D,E三个样本，那么由C,E为女性，D为男性，女性多于男性得到我们要预测的结果为 女性。
这样问题就来了，一个女性的脚43码的可能性，远远小于男性脚43码的可能性，那么为什么算法还是会预测F为女性呢？那是因为由于各个特征量纲的不同，在这里导致了身高的重要性已经远远大于脚码了，这是不客观的。所以我们应该让每个特征都是同等重要的！这也是我们要归一化的原因！归一化公式如下：
; 总结
1.我们提出了k近邻算法，算法的核心思想是，即是给定一个训练数据集，对新的输入实例，在训练数据集中找到与该实例最邻近的K个实例，这K个实例的多数属于某个类，就把该输入实例分类到这个类中。 更通俗说一遍算法的过程，来了一个新的输入实例，我们算出该实例与每一个训练点的距离（这里的复杂度为0(n)比较大，所以引出了下文的kd树等结构），然后找到前k个，这k个哪个类别数最多，我们就判断新的输入实例就是哪类！
2.与该实例最近邻的k个实例，这个最近邻的定义是通过不同 距离函数来定义，我们最常用的是欧式距离。
3.为了保证每个特征同等重要性，我们这里对每个特征进行 归一化。
4.k值的选取，既不能太大，也不能太小，何值为最好，需要实验调整参数确定！
Original: https://blog.csdn.net/qq_40695642/article/details/127184772
Author: 冷色调的夏天
Title: SLAM&Navigation 导航算法基础知识汇总
原创文章受到原创版权保护。转载请注明出处：https://www.johngo689.com/721056/
转载文章受原作者版权保护。转载请注明原作者出处！