现在，假如有一个机器人，它已经存储好一个全局的地图（哪里可通行，哪里不可通行），并且知道自己在其中的位置。现在要从给定的起点走到终点，我们应该怎么做？
有轨导航和无轨导航
在某些应用场景中，例如工厂或仓库，环境相对固定且对路径的准确性要求较高。这种情况下，我们可以使用有轨导航系统，比如通过在地面上铺设磁条或粘贴二维码等方式进行人工标识。机器人通过读取这些标识来确定自己的位置和导航路线。这种方法的优点是导航精度高，重复性好，适用于需要高度结构化环境中的重复工作。然而，它的缺点也很明显，包括安装成本高、环境改变后需要重新布置标识、灵活性较差。
或者，另一种选择是使用半有轨导航，这种方式不依赖于物理导轨，但会保留路线上的标识，如地面上的条形码。机器人则通过发射激光或使用视觉系统读取这些标识。这种方式结合了有轨导航的精确性和某些无轨导航的灵活性。其优点包括在有限的环境改动下仍能保持较好的导航效果，而缺点是仍然依赖于环境中的某些固定标识，且在标识被遮挡或损坏时容易出现导航错误。
更加通用且灵活的方式是无轨导航，机器人完全依靠自身装备的传感器（如激光、视觉或超声波）进行环境的地图构建和自我定位。这种方法使得机器人能在没有任何预设标记的环境中自由移动，非常适用于不断变化的环境如家庭或办公场所。机器人通过连续地扫描周围环境，并与内部地图进行匹配，来识别自己的位置和导航路线。这种方法的优点是极高的灵活性和适应性，但需要机器人自己的本事，挑战在于需要复杂的算法来处理环境数据，计算成本高。所以需要我们为机器人设计算法，这也是本文的主要内容。
路径规划
现在，我们要根据起点，终点和地图信息，规划出来一条路径。
首先嘛简单起见，我们假设地图只分为可通行和不可通行，并且把机器人视作质点（也就是忽略运动学和几何的约束，若要考虑机器人半径，可以把障碍物向外扩充来进行等效，这不是本文的重点）。
行车图 （Roadmap）
行车图法是路径规划中常用的一种技术，旨在创建一个简化的图，这个图反映了从起点到终点的可能路径。
可视图（Visibility Graph）是一种典型的行车图。我们连接起点，终点和所有障碍物顶点形成的边（去掉会穿过障碍物的），实际上就得到了一张图，在此图上运行搜索或者最短路算法就可以得到路径。其简单且路径长度优，扩充障碍边缘可以避免过于靠近障碍物。但是，对于边界不规则或非多边形的障碍物，这种方法可能不够精确或效率不高。
沃罗诺伊图（Voronoi Diagram）则提供了一种完全不同的方法。在此方法中，地图中的每个点被分配给最近的障碍物。对于那些与多个障碍物距离相等的点，它们构成了沃罗诺伊图的边界，这些边界理论上表示了远离任何障碍物的“最安全”的路径。通过连接这些边界点，形成了一张反映机器人应该行进的路径图。
沃罗诺伊图的优点在于它倾向于避免靠近障碍物，这在某些应用中非常有用，比如在拥挤的环境中。缺点是，由于这些路径可能绕过障碍物较远，所以行进的总距离可能比可视图法得到的路径长。此外，沃罗诺伊图的计算成本也相对较高，尤其是在障碍物多或者空间大的环境中。
单元分解
单元分解是一种完全不同的方案。它将空间中的分为若干的小区域，每一个区域作为一个单元，以单元为顶点、以单元之间的相邻关系为边构成一张连通图。
如果环境较为简单，可以通过精确的方法如扫描线进行空间划分。这种方法将每个障碍物精确地映射到单元中，保证了划分的精确性。如左图所示，可以明显看到环境被划分为多个清晰的单元，每个单元都精确对应环境中的可通行或不可通行区域。
这种方法的优点是精度高、执行效率好，适合于障碍物较少且布局简单的环境。然而，它的主要缺点是难以适应复杂多变的环境。在障碍物数量多或形状复杂的场景中，精确的单元划分可能导致计算量大增，降低了算法的执行效率。
更通用的方法是使用栅格地图进行近似分解，如右图所示。在这种方法中，整个环境被划分为规则的格子，每个格子代表一个单元。栅格地图通过覆盖整个环境的网格来近似表示障碍物和可通行区域。
栅格化单元分解的优点包括易于实现和适用于多种类型的环境。这种方法简化了计算过程，使其更加适合动态或复杂的环境。然而，它的缺点也很明显：分辨率依赖于格子的大小，较大的格子可能导致路径规划不够精确，而较小的格子则可能使计算成本急剧增加。此外，近似表示可能导致路径规划不够最优，例如在细长障碍物旁边可能不会生成最短路径。
势场法
假设地图是一个力场，目标点对机器人产生吸引力，障碍物对机器人产生排斥力，那么最终力的方向不就是行走的方向吗？这一次，我们不构建图，而是构建一个指示机器人行走的势场。势场中目标点对机器人产生吸引力，障碍物对机器人产生排斥力，所有力的合成决定机器人的合力大小和方向。当然具体怎么构建有多种方案，这里讲述一种构建函数。
目标的吸引公式可以这样设定：
\[U_{\text{attr}}(r) = \begin{cases} -k_{\text{attr}} \cdot r^2 & \text{if } r < d_{\text{max}} \\ -k_{\text{attr}} \cdot (2 d_{\text{max}} \cdot r - r^2)& \text{otherwise} \end{cases} \]其中，\(r\) 是机器人到目标点的距离，\(k_{\text{attr}}\) 是自己设定的吸引力系数，\(d_{\text{max}}\) 是阈值距离。当距离小于阈值 \(d_{\text{max}}\) 时，吸引力与距离的平方成正比；当距离大于阈值时，吸引力与距离成线性关系。这样设置可以在机器人远离目标点时减小吸引力，避免过冲。
障碍物的排斥公式可以这样设定：
\[U_{\text{repel}}(r) = \begin{cases} k_{\text{repel}} \cdot \left(\frac{1}{r} - \frac{1}{d_{\text{repel}}}\right) & \text{if } r < d_{\text{repel}} \\ 0 & \text{otherwise} \end{cases} \]其中，\(r\) 是机器人到最近障碍物的距离，\(k_{\text{repel}}\) 是自己设定的排斥力系数，\(d_{\text{repel}}\) 是排斥力作用的最大距离。当机器人靠近障碍物到一定距离内时，排斥力显著增加；当超出这个距离时，排斥力消失。
对势场求梯度得到力，机器人在任何位置上的总力由所有吸引力和排斥力的向量合成得到。总力的方向指示了机器人的行进方向，力的大小也可以使用，用来调节机器人的速度或者加速度什么的。通过对势场求梯度，我们可以得到每个位置上的力向量。所以这不仅是一种离线路径规划方法，所构建的势场也构成了机器人的控制律，能够作为在线实时避障算法
这种方法的优点是操作直观且可根据环境动态调整；缺点则包括在某些复杂环境中可能会遇到局部最小值问题，导致机器人陷入无法达到目标的状态。为了解决这一问题，可以结合其他导航策略，如路径平滑或动态重规划。
概率行车图（Probabilistic Roadmap, PRM）
我们开可以基于采样，带一点随机来规划路径。概率行车图（PRM）是一种基于采样的路径规划算法，广泛用于处理高维自由位形空间中的导航问题。这种方法适用于复杂环境中的路径规划，尤其是在机器人和其他自动化系统中非常有效。以下是流程
- 随机采样：从自由位形空间中随机采样多个点。这些点代表机器人可能占据的位置。
- 碰撞检测：对每个采样点进行碰撞检测，以确保这些点不与环境中的障碍物重叠。
- 局部路径规划：对于每对采样点，如果它们之间可以通过不与障碍物相交的直线或曲线相连，则在这两点之间创建一条边。
- 构建图：使用步骤3中的边和步骤2中的有效采样点构建一个图。
- 图搜索：使用图搜索算法（如A*或Dijkstra）在图中找到从起点到终点的最短路径。
其优点是明显的简单好算，并且由于采样的随机性和独立性，PRM特别适用于高维度问题，能有效处理多自由度机器人的路径规划。不过其计算质量明显很依赖采样，采样点太少可能无法完全表达自由空间的连通性，而有些时候我们也无法确定是不是充分采样了。
快速随机探索树（Rapidly-exploring Random Tree, RRT）
快速随机探索树（RRT）是一种有效的路径规划算法，特别适用于处理具有复杂障碍物和高自由度机器人的空间。RRT不仅在机器人领域得到广泛应用，也适用于其他多维度空间的探索任务。这种算法通过快速并随机地增长搜索树来探索空间，从而找到从起点到终点的路径。
RRT算法的基本流程：
-
初始化：创建根节点，其位置为起点。
-
迭代增长：
- 随机采样：在自由空间内随机选择一个点作为目标点（称为随机采样点）。
- 选择最近节点：在当前树中找到距离随机采样点最近的节点。
- 扩展节点：从最近节点向随机采样点方向扩展一定步长，创建新节点。这一步涉及到移动距离的限制，以确保控制在安全范围内。
- 碰撞检测：检查新节点到最近节点之间的路径是否与障碍物冲突。如果无碰撞，则将新节点添加到树中。
-
检查终点：如果新节点与终点的距离小于预设的阈值，则认为找到了路径。
-
循环迭代：重复步骤2，直到达到终点或达到最大迭代次数。
特点和优势：
- 高效探索：RRT通过不断向未探索区域扩展来高效地覆盖空间。
- 适用于复杂环境：RRT能够有效处理复杂的障碍布局和高维度问题。
- 灵活性：算法可以容易地适应不同的环境和需求，例如通过修改步长或增长策略来优化性能。
RRT是一种非常强大的工具，尤其适合在未知或动态变化的环境中进行快速路径规划。不过它同样是一个需要脸的算法。算法的性能受随机采样的影响，有时可能需要较长时间才能找到解，特别是在目标区域较小或障碍物较多的情况下。双向搜索是一种提升效率的好办法，通过同时从起点和终点搜索，可以显著减少达到目标所需的总迭代次数。这是因为两个方向的搜索可以覆盖空间的不同区域，使得搜索树更快地接近对方。
RRT 算法显然只能找到可行路径，明显不是最优路径。RRT* 可以对此做出改进。具体来说：
- 在添加新采样点之前，我们未必要将其连接到原来的“最近节点”上，可以再一定半径内自行搜索，连接到一个最短的节点。
- 添加新点之后，可以遍历一定范围内的其他节点，若其他节点连接到自己更优，则更新树
