💯 欢迎光临清流君的博客小天地，这里是我分享技术与心得的温馨角落 💯

🔥 个人主页:【清流君】🔥 📚 系列专栏: 运动控制 | 决策规划 | 机器人数值优化 📚 🌟始终保持好奇心，探索未知可能性🌟

引言

  本篇博客介绍移动机器人运动规划的经典方法，分门别类地介绍各类规划算法。

  移动机器人的路径搜索算法，或称路径发现算法，旨在帮助机器人在复杂环境中找到一条几何上无碰撞、安全且可行的路径。路径搜索通常作为机器人运动规划中的前端环节，目标是尽可能降低计算开销和复杂度。因此，路径搜索算法一般将高维问题降维，先在低维空间中寻找一条粗略的可行路径。

  在完成路径搜索之后，机器人进入运动规划的后端环节——轨迹生成或轨迹优化。此阶段的目标是将前端得到的粗略低维路径，通过优化方法转化为高维的运动轨迹，以满足机器人的动力学约束。轨迹优化不仅要求路径的几何形状光滑、高阶连续，还需要保证路径的安全性和可行性，确保机器人能够在实际环境中顺利执行任务。因此，轨迹生成不仅仅是简单的路径规划，它涉及到更复杂的高维运动规划，最终形成符合机器人动力学特性的运动轨迹。

一、自主机器人定义

  首先简单介绍一下什么是自主机器人。

1.1 维基百科定义

  维基百科上是这样定义的：自主机器人就是 机器人可以在没有外界干扰的情况下，并且具备比较高级别的自动化程度去执行一系列的行为或者任务的机器人。

1.2 自主机器人组成

  自主机器人一般要具备基本的感知、规划、控制的整体链路来构成闭环。

• 感知
  首先具备感知的传感器，可以从外部环境中得到最基本的测量，基于这些外部获取的信息，做机器人的定位与建图，或者称之为感知。
• 规划
  基于定位和感知，移动机器人知道了自己在哪，以及周围的环境怎么样，就可以在此基础上做运动的规划。
• 控制
  最后生成的规划传给机器人的控制器，控制机器人完成一系列的行为或者任务。

1.3 典型自主机器人

下面是比较典型的自主机器人的应用的平台，包括空中机器人、地面机器人、水下机器人等。

  可以看到机器人具备各种各样不同的传感器，用于从外界去抓取环境信息，同时在机器人上具备一系列的处理单元计算模块，要做的就是去处理这些从外界中抓取的信息来做感知、定位、规划、控制等等。

1.4 运动规划算法分类

运动规划主要分成三个部分，首先要有前端路径搜索、路径发现，这部分包含三点：

• 基于 搜索 的路径规划
• 基于 随机采样 的路径规划
• 满足 动力学要求 的路径规划

二、基于搜索的算法

基于搜索的算法分为精确算法和近似算法。

(1) 精确算法

• 深度优先法、广度优先法
• 遍历获得所有路径后选择最优解
• 优化：优先级定义的广度优先法、 Dijstra 算法
• 存在问题：耗时，难以满足机器人在线快速规划要求

(2) 近似算法

• 启发式搜索算法
  $A$ *, $D$ *, Focused D*等
  贪心算法，具有很强的目的性，容易陷入局部最优
• 准启发式搜索算法
  退火、进化和蚁群优化等

2.1 基于深度优先和广度优先的搜索算法(DFS,BFS)

  深度优先搜索(DFS)和广度优先搜索(BFS)是图和树的两种基本搜索算法。 DFS 通过优先沿一条路径深入到尽可能深的位置，直至无路可走再回溯，适合用于遍历整个图或寻找所有可能的解；而 BFS 则逐层扩展节点，从起点开始依次访问距离最近的节点，适用于无权图中寻找最短路径。 DFS 通常使用递归或栈实现， BFS 则依赖队列结构，二者在空间和路径最优性上各有优劣。

  广度优先搜索( Breadth First Search , BFS ) 和 深度优先搜索( Depth First Search , DFS ) 的路径规划方式的区别如下图。

1. 搜索方式：

   • 广度优先搜索：从起点(红色星星)开始，依次访问与当前节点距离最近的所有节点，逐层向外扩展，按距离优先的方式逐步搜索到目标点(粉色叉号)。它保证在找到目标点时路径是最短的(最优解)。
   • 深度优先搜索：从起点出发，沿着一条路径深入到底(优先向下探索，直到无路可走或找到目标点)，然后回溯并尝试其他路径。它可能找到目标点但不一定是最短路径。

2. 路径特性：

   • BFS：搜索过程以“广度”为主，扩展范围较大，如图片左侧，呈现一个均匀扩散的紫色区域。
   • DFS：搜索过程以“深度”为主，可能形成弯曲、重复的路径，如图片右侧，搜索路径较长且有回溯。

3. 适用场景：

   • BFS 更适合在需要找到最短路径或解决最优解问题的场景，比如地图导航或迷宫求解。
   • DFS 常用于需要遍历整张图或寻找所有解的场景。

2.2 迪杰斯特拉算法(Dijkstra)

(1) Dijkstra算法核心思想

  迪杰斯特拉算法的核心思想是通过贪心策略(每次选择当前最短路径的节点)，逐步扩展最短路径，确保每个节点被访问时，它的路径长度已是最短，以找到从起点到图中所有其他顶点的最短路径。

• 采用优先级定义的广度优先搜索思想
• 在有向图中以起始点为中心按路径长度递增方式层层向外扩展，直到扩展到终止点

此算法适用于加权图，但要求所有边的权值为非负。

(2) Dijkstra算法主要过程

  设置两个集合 $S$ 和 $T$ ， $S$ 存放已经找到最短路径的顶点， $T$ 是尚未确定路径的顶点集合，同时也描述了起始点经过集合 $S$ 中顶点到该点的最短路径及长度。

  每次更新时，从 $T$ 中找出路径最短的点加入到集合 $S$ ， $T$ 中顶点最短路径及长度则根据加入点作为中间点后起始点到该点距离是否减小来决定是否更新。

• 优点：完备、最优
• 缺点：没有任何关于终点的信息，盲目地向各个方向均匀扩展，相当于穷举搜索

若代价都为 $1$ ，则
$$迪杰斯特拉算法=广度优先搜索算法$$

2.3 A* 搜索算法

(1) A* 算法核心思想

$$A\ast 算法=迪杰斯特拉算法+贪心算法$$

  前端路径搜索算法中的重点就是 $A$* 算法，它的原理和高效的工程实现。

  这里的图是二维的栅格网络地图，图里的节点跟周围的邻居是八连通的，允许上下左右的平移跟斜对角的移动，这样的每节点其实就是上个网络地图的节点，可以储存障碍物的信息，灰色区域就是有障碍物的不可以通行的节点。

  对于任何需要去做搜索的问题，一定会它会有与之相对应的搜索图。首先就要把搜索图给构建出来，就比如说这里所示的：

  要把问题抽象成它拓扑上互相连接起来的图，图里面连节点之间的连接，像这里所说的在图里面节点之间的连接，连接可以是有向的或者无向的。比如说从 $B$ 节点到 $A$ 节点是单向的，反向走不通，这些连接称之为边，图里可以看到，节点和节点之间是靠边连通的。

  接下来就可以用各种各样的图搜索的方法，在图上找一条最短的路径。这里是基于搜索的 $A$ * 算法的结果示意图：

  图中彩色的是搜索过程中，在图上所扩散过的节点，这一根线就是最终找到的最短路径。

(2) A* 算法主要过程

$A$ *算法流程图如下所示：

(3) Dijkstra 算法和 A* 算法的对比

搜索算法的核心是做简单的广度优先搜索，去遍历全图，还是带有很强目的性、指向性的起点到终点的最短路径的搜索？

这里引申出来的核心问题就是 Dijkstra 算法和 $A$ * 算法的对比：

• Dijkstra 的算法向各个方向扩展

• $A$ *主要向目标扩展，以确保最优

2.4 跳点算法(JPS)

(1) 跳点算法概述

  跳点算法是前几年才出来的比较新的搜索算法，叫做 Jump Point Search ( JPS ) ，实际上是一种 $A$ * 的系统性改良版本，是一种用于加速 $A$ * 算法的启发式搜索算法，常用于栅格地图中的路径规划问题。

• 传统的 $A$ * 算法在搜索路径时，会逐个遍历从起点到终点之间的所有邻居节点，逐步逼近目标。然而，这种方法在大的、稀疏的地图上可能效率较低，因为会处理大量的无效或冗余节点，导致搜索速度缓慢。
• JPS 通过减少路径中的冗余节点，跳过直线上的中间节点，仅在重要的转折点(跳点)进行搜索，极大地提高了搜索效率。

下面是 A* 算法与 JPS 跳点算法的对比，左图为A*算法，右图为JPS跳点算法：

(2) 跳点算法核心思想

  在某些特定的方向上，如果可以直接沿着当前方向无障碍地移动，则没有必要评估中间的每个节点，而是可以直接跳到更远的节点，直到遇到障碍、转折点或终点。这些被跳过的节点都属于同一条直线上的可行路径，无需额外检查。 JPS 通过这种“跳点”的策略，减少了搜索空间，同时保留了与 $A$ * 算法相同的最优性。

(3) 跳点算法具体实现

  在具体实现上， JPS 通过识别强迫邻居来决定跳点位置。强迫邻居是指，在某些方向上，如果当前节点的某个邻居节点没有被障碍物阻挡且无法继续前进，则需要从节点向不同方向扩展搜索。这些转折点成为跳点，确保路径能够覆盖地图中的所有重要位置。

  总的来说， JPS 在保持 $A$ * 算法完整性的前提下，通过减少对路径中无效节点的评估，显著提高了路径规划的效率，尤其适用于空旷或稀疏障碍的栅格地图。这使得 JPS 在实时路径规划、机器人导航以及游戏 AI 等应用中具有广泛的实际价值。

三、基于采样的算法

  基于随机采样的路径规划方法，这一类方法也是非常大的一类方法，包括 PRM ， RRT ， RRT * 以及启发式 RRT *

3.1 概率路图法(PRM)

(1) 核心思想

  概率路图是一种非常经典的，也比较早的随机采样的路径规划方法。基本思想是通过随机采样和碰撞检测找到自由位形空间中的路径点和无碰路径，构建连通图。

  比如说这里连接红色节点和红绿色节点的这样一条路径。与之结合的，比如说这里是也是图，当然图是用随机的方法构建的，这在后面基于采样的方法里，会再讲到，假设现在已经构建了这样图，之后同样用这样图搜索算法，在图上去搜连接起点到终点的最短的路径。

  可以看到对照上面抽象的拓扑图的话，这里每绿点就是这里的节点，这里每一条绿边就是这里的边，当然这里的边是无向的，就是说可以正向、反向都可以通信的。

PRM 需要考虑的几个问题：

• 随机位形选择
  通常采用均匀随机采样方式
• 寻找最近邻点
  可以采用 KD 树方法加速
• 生成局部路径
  在一定范围内直接连接图节点
• 检查路径无碰
  可以增量式取点或者二分法取点，判断点是否在障碍物区域内

(2) 非全联通情况

  当环境复杂，存在狭窄通道等困难场景时，由于 PRM 对自由位形空间的近似覆盖，存在图非全联通的情况。

解决方案：

• 对节点进行困难度表示，例如在该节点一定距离范围内的节点数的倒数、该节点到不与该节点连通的最近连通单元距离倒数、局部路径规划失败率等

• 以困难度作为该节点被选择为扩张节点的概率

• 选择后，从该节点出发随机选择方向运动，碰到障碍物后再随机选择方向运动，循环若干次或者成功建立与其他连通单元的连接

(3) PRM 算法优缺点

优点

• 简化了对环境的解析计算，可以快速构建得到行车图
• 适用于高维度自由位形空间中的规划
• 是一个近似完备的路径规划方法

缺点

• 对自由空间连通性表达的完整性依赖于采样次数
• 从算法通用性.上来讲难以评估需要多少时间做充分采样
• 不考虑机器人执行的可行性(相对 RRT 算法的缺点)

3.2 快速搜索随机树(RRT)

(1) RRT 算法核心思想

  随机扩展、快速采样的随机数算法，一般称为快速扩展随机树( RRT , Rapidly - exploring Random Tree )。

其核心思想如下：

• 连通图采用树的形式，以起始点作为树的根节点
    通过随机采样和增量式树状扩展快速探索高维搜索空间，从而找到一条从起点到目标的可行路径。算法以起点为根节点，逐步向随机采样点扩展树的分支。

• 采用在空间中随机采样、连接树中最近节点的方式拓展树
    在每次迭代中，随机在搜索空间采样一个点，找到树中距离该点最近的节点，然后沿着这个方向扩展一个步长，生成新的节点。

• 考虑机器人的运动执行能力
    这是与其他路径规划算法相比的特点，如果扩展路径在环境中无碰撞，则将新节点加入树中。

• 通过树结构可以直接回溯得到路径。
    通过不断重复这一过程，树会快速扩展并覆盖整个搜索空间，最终找到连接起点与目标点的可行路，不再需要最优路径搜索。

(2) 基本流程

规划结果如下：

(3) RRT 的特点

• 随机性强
• 扩展效率高
• 适用于高维复杂环境的路径规划问题

下面是 RRT 最简单的示意图：

  可以看到随着动图的进行，在环境中生成了很多很多随机的、不重复的采样点，去引导呢？这样复杂的树不断的去扩散，向未知的环境分得越来越密，会覆盖的范围越来越大。

  最后会希望随着采样的逐渐增多，会越来会在复杂的由采样组成的这样同样也是图里会得到一条越来质量越来越好的连接起点和终点的，期望是最短的这样一条路径，这就是随机采样的思想。希望用越来越多的采样去覆盖环境里没有探索过的区域去构成这样 graph 。

(4) 影响规划收敛速度的三个步骤

• 随机状态的采样
• 在搜索树中查找与随机状态距离最近的书点
• 新生成节点的防碰检测

3.3 改进的快速搜索随机树算法(RRT*)

(1) RRT* 算法核心思想

   RRT *( Rapidly - exploring Random Tree Star )算法是 RRT (快速扩展随机树)算法的改进版，其核心思想是通过随机采样、树的扩展和路径迭代优化，构建一棵渐进最优的路径树，逐渐改进路径规划的质量，最终找到一条代价最小的路径。它兼具随机性和优化性，适用于高维复杂环境的路径规划问题。

(2) 相比 RRT 算法的具体改进

  改进的快速扩展随机树算法( RRT *)相对于原始 RRT 算法的主要改进在于：RRT* 在构建树的过程中不仅扩展可行路径，还通过优化机制逐步调整树的结构，使路径代价逐渐趋近于全局最优。

具体改进包括：

• 引入“最优父节点”选择策略，在加入新节点时连接代价最小的父节点；
• 以及“局部重连”策略，通过调整新节点邻域内的连接关系优化周围路径。
• 相比 RRT 只生成一条可行路径， RRT * 能在迭代中不断改进路径质量，最终找到一条代价最优的路径。

四、基于运动学的路径搜索算法

4.1 状态栅格搜索(State Lattice Search)

  状态栅格搜索(State Lattice Search) 是一种用于机器人路径规划的运动规划算法，特别适合在连续空间中进行搜索。它基于将状态空间离散化，通过预定义的运动原语(motion primitives)在有限的状态集合中搜索可行路径。每个状态代表机器人在不同位置和朝向的组合，预定义的运动原语确保从状态到达另状态的轨迹是可行且平滑的。

  上面左图展示了机器人从状态 $x_0$ 开始向周围多个离散化状态扩展，每条路径都表示一种预定义的运动原语。右图同样展示了从状态 $x_0$ 开始的状态扩展，但更直观地体现了路径的不同方向和状态的稠密性。这些离散化的状态和路径形成了“栅格”或者“晶格”结构，称为状态栅格( state lattice )。

  通过这种离散化方式，状态栅格搜索算法可以有效减少连续空间中的搜索复杂度，同时确保生成的路径在动力学约束下是可行的。这种算法常用于自主驾驶和机器人导航中，能够生成平滑且可控的路径。

  状态栅格搜索算法中运动原语的生成方式和其在连续空间中的扩展特性可以看下面这两幅图：

  左图展示了从初始状态(如起点)向各个方向生成的多个运动原语。每条线代表一种可能的轨迹，均遵循预定义的运动模型。这些轨迹从位置出发，向各个不同的方向扩展，体现了算法如何离散化连续空间中的可行路径。通过这种方式，算法能够从状态延展到不同的后继状态，保持路径的可控性和光滑性。

  右图展示了状态栅格搜索算法的路径在更大空间中的应用。黑色的粗线表示搜索得到的最优路径，而背景中的灰色线条代表预先生成的运动原语网格。图形展示了机器人或车辆如何沿着这些预定义轨迹行进，以生成符合其运动学约束的路径。右上角的插图进一步细化展示了局部状态晶格如何从状态向多方向扩展。

4.2 混合A算法(Hybrid A)

  混合 $A$ * 算法结合了传统 $A$ * 算法的启发式搜索与连续空间中的正向模拟，适用于复杂路径规划场景。

• 它遵循 $A$ * 的基本框架，通过离散的网格进行全局搜索，但引入了车辆的运动学约束。
• 每个状态不是简单的网格点，而是通过正向模拟不同控制输入来生成的一系列可能状态，确保路径的可行性。

  混合 $A$ * 算法会在每个网格中只保留最优状态，如上图中的蓝圈所示，这有效减少了搜索空间，提升了算法效率，同时避免产生不必要的冗余轨迹。

  这种方法尤其适用于车辆导航等对路径连续性要求高的应用场景。

4.3 基于运动学的 RRT* 算法

  基于运动学的 RRT *( Kinodynamic RRT *)是一种改进的路径规划算法，专门用于解决具有运动学约束的系统，如自动驾驶汽车、无人机或机器人手臂等无法随意改变速度和方向的运动系统。 RRT * 是快速随机树( RRT )算法的优化版本，它不仅保留了 RRT 的高效探索特性，还通过逐渐优化生成的路径，保证其渐进最优性。基于运动学的 RRT * 在此基础上进一步引入了系统的运动学和动力学约束，使得生成的路径在满足环境约束的同时，也符合机器人自身的运动特性。

• 随机采样和树的构建
     RRT * 的基本思想是通过在工作空间中随机采样，并将这些采样点连接到树中逐步扩展搜索空间。在基于运动学的 RRT * 中，采样时不仅考虑位置，还考虑速度和加速度等状态变量，以确保生成的轨迹符合机器人或车辆的运动学约束。

• 最优路径更新
    与 RRT 不同， RRT * 会在每次新节点加入后，检查周围的节点，并尝试通过新节点更新路径，使得整棵树的路径成本更优。在基于运动学的 RRT * 中，路径优化需要考虑运动学限制，因此路径的更新不仅仅基于距离，还需要确保从节点到另节点的运动轨迹是物理可行的。

• 运动学约束的正向模拟
    传统的 RRT * 扩展树的方式是通过简单的直线连接，而基于运动学的 RRT * 使用正向模拟来生成新的节点。具体来说，机器人从当前节点出发，依据不同的控制输入进行正向模拟，得到一条符合运动学约束的可行轨迹，从而生成新的节点。这种方法确保生成的路径满足加速度、转弯半径等物理限制。

• 收敛性与渐进最优性
    RRT * 的优势在于它具有渐进最优性，随着采样次数的增加，生成的路径会逐渐逼近全局最优解。基于运动学的 RRT * 在此过程中，考虑了运动学约束，使得最终的路径不仅是最优的，也能够在真实环境中执行。

五、轨迹优化

5.1 基本最小间隙

  轨迹优化是生成一条从起点到终点的平滑、安全、高效的运动路径。通过轨迹优化，机器人不仅要避免碰撞，还需要满足运动学和动力学约束，如速度、加速度、关节角度限制等。这一过程通常涉及优化算法，如梯度下降、动态规划、 RRT 等，通过迭代调整路径，使其更符合目标要求。

  在实际应用中，轨迹优化广泛应用于工业机器人、自动驾驶、无人机等领域。例如，工业机器人在执行焊接或搬运任务时，需要通过优化其运动轨迹来减少能耗并提高效率。同时，轨迹优化还可以减少机械磨损，延长设备的使用寿命。

总之，轨迹优化通过有效提升路径的质量，使机器人能更智能地完成复杂任务。

5.2 最小间隙硬约束

   Minimum - snap 硬约束是一种在路径规划中常用的方法，用于确保机器人或车辆沿着预定的轨迹移动时尽可能减少速度和加速度的变化。这种方法的主要目标是实现平滑的运动，从而提高机器人的稳定性和安全性。在这两幅图中，可以看到一条由多个矩形组成的路径，每个矩形的边框都代表特定的方向。这些矩形通过箭头连接起来，表示机器人应该按照箭头的指示前进。这种布局方式有助于指导机器人沿着预定的路线移动，同时最小化速度和加速度的变化。

5.3 最小间隙软约束

  相比之下， Minimum - snap 软约束则更加灵活。它允许机器人在一定程度上偏离预定的轨迹，以适应环境中的障碍物或其他动态因素。在这两幅图中，绿色箭头可能代表了机器人可以接受的轻微偏移范围。当机器人遇到障碍物时，它可以稍微改变方向，绕过障碍物，然后继续沿原路前进。这种灵活性使得 Minimum - snap 软约束在实际应用中更为实用，因为它能够更好地应对复杂多变的环境。

  总的来说， Minimum - snap 硬约束和软约束都是路径规划中的重要概念。它们各自有其独特的优势和适用场景。在实际应用中，选择哪种约束取决于具体的需求和条件。例如，对于需要高精度和高稳定性的任务，如手术机器人或精密装配线上的机械臂， Minimum - snap 硬约束可能是更好的选择；而对于需要在复杂环境中导航的自主车辆或无人机， Minimum - snap 软约束则更能满足其需求。

六、总结

  本篇博客系统地介绍了移动机器人运动规划的经典方法，从自主机器人的定义和组成出发，深入探讨了基于搜索、基于采样和基于运动学的路径搜索算法，以及轨迹优化方法。

  文章涵盖了多种路径规划算法，包括基于深度优先和广度优先的搜索算法、 $A$ * 搜索算法、 JPS 跳点算法、 PRM 、 RRT 、 RRT * 以及基于运动学的路径搜索算法。

  此外，文章还介绍了轨迹优化中的基本最小间隙硬约束和软约束，旨在帮助读者全面理解移动机器人运动规划的基本原理和方法。

参考资料

  移动机器人运动规划