TEB
TEB（Timed Elastic Band）局部路径规划算法是一种用于移动机器人路径规划的实时算法，专注于优化路径的时间和空间特性，以实现平滑且高效的运动。TEB算法通过将全局路径转化为一条时间弹性带，并对其进行优化，使机器人能够在动态环境中避障并到达目标位置。以下是TEB算法的基本原理：
TEB算法原理
1. 路径表示
TEB将路径表示为一系列带有时间约束的位姿（poses），即路径上的每个点都包含位置和时间信息。这样，可以对路径进行空间和时间上的优化。
2. 代价函数的构建
TEB算法通过一个代价函数来评估路径的质量。代价函数通常由以下几个分量构成： - 障碍物距离代价：衡量路径是否避开障碍物，越远离障碍物代价值越低。 - 速度代价：衡量机器人在路径上移动的速度，鼓励较高的速度以提高运动效率，但也需要平滑过渡。 - 加速度代价：衡量路径上速度的变化，鼓励平滑的速度变化，减少加速度和减速度带来的不平稳。 - 时间代价：衡量路径完成所需的时间，鼓励尽快到达目标。
3. 优化过程
TEB算法采用优化方法（如梯度下降、二次规划等）对路径进行迭代优化，以最小化代价函数。优化过程主要包括以下步骤： - 初始化路径：从全局路径规划算法（如A*或Dijkstra）获取初始路径，并对其进行离散化处理，得到初始的位姿序列。 - 时间参数优化：调整路径上每个位姿的时间分配，确保路径在时间上是可行的。 - 空间参数优化：调整路径上每个位姿的空间位置，确保路径在空间上是可行的，并避开障碍物。 - 联合优化：同时优化时间和空间参数，以获得全局最优解。
4. 实时更新
TEB算法在机器人运动过程中，实时更新路径以适应动态环境变化。每个时间步，机器人根据当前状态和环境信息，对路径进行重新优化，确保其能够安全避障并朝向目标前进。
步骤总结
- 路径初始化：从全局路径规划算法获取初始路径并离散化。
- 代价函数构建：定义障碍物距离代价、速度代价、加速度代价和时间代价等。
- 路径优化：采用优化方法对路径进行迭代优化，最小化代价函数。
- 实时更新：在机器人运动过程中，实时更新路径以适应动态环境变化。
TEB算法通过对路径的时间和空间参数进行联合优化，实现了机器人在动态环境中的实时路径规划和避障。其优点在于能够生成平滑、高效的路径，并适应动态环境变化，但优化过程可能较为复杂，需要较高的计算资源。
# DWA
DWA（Dynamic Window
Approach）局部路径规划算法是一种用于移动机器人避障和路径规划的实时算法。其主要思想是通过在机器人当前速度空间中寻找最优速度，以确保机器人能够安全地避开障碍物并向目标前进。下面是DWA算法的基本原理：
DWA算法原理
1. 动态窗口的定义
动态窗口是一个以当前时间为基础的速度空间，包含了机器人在下一时间步内可以达到的速度。动态窗口受限于以下三个方面： - 运动学约束：机器人在当前时间步内的最大加速度和减速度限制了速度变化范围。 - 动态约束：机器人在当前时间步内能够安全停下所需的速度范围。 - 环境约束：考虑到环境中的障碍物，只有那些在下一时间步内不会导致碰撞的速度才是有效的。
2. 评价函数的构建
DWA算法通过一个评价函数来评估不同速度下的路径质量。评价函数通常由以下几个分量构成： - 目标函数：衡量速度是否朝向目标前进，通常是速度与目标方向的夹角或距离。 - 速度函数：衡量机器人速度的大小，鼓励较高的速度以提高运动效率。 - 障碍物距离函数：衡量路径是否避开障碍物，越远离障碍物得分越高。
3. 搜索最优速度
在动态窗口内，通过遍历所有可能的速度组合，计算每个组合的评价函数值，并选择评价函数值最高的速度作为机器人在下一时间步的速度。
4. 路径跟踪和更新
机器人按照选定的最优速度进行移动，并在每个时间步重复上述过程，实时更新动态窗口和评价函数，逐步朝向目标前进并避开障碍物。
步骤总结
- 生成动态窗口：根据当前速度和运动学约束生成可行的速度空间。
- 模拟轨迹：对动态窗口内的每个速度组合进行轨迹模拟。
- 评价轨迹：使用评价函数对模拟轨迹进行打分。
- 选择最优速度：选择评价得分最高的速度作为下一步的运动速度。
- 更新状态：机器人按照选定速度移动，更新当前位置和速度，并重复上述过程。
DWA算法通过在速度空间中寻找最优解，实现了机器人在动态环境中的实时路径规划和避障。其优点在于计算效率高，能够适应动态变化的环境，但也存在对复杂环境中的局部最优解问题敏感等缺点。
# MPC
MPC（Model Predictive
Control，模型预测控制）是一种高级控制策略，广泛应用于机器人路径规划和运动控制中。MPC通过预测未来一段时间内系统的动态行为，优化控制输入以达到期望目标。以下是MPC局部路径规划算法的基本原理：
MPC算法原理
1. 预测模型
MPC基于系统的数学模型进行预测。对于机器人路径规划，通常使用运动学或动力学模型来描述机器人在未来一段时间内的行为。这些模型可以包括机器人的位置、速度、加速度等状态变量。
2. 预测时域
MPC在每个控制周期内，考虑一个固定的预测时域（prediction horizon），即未来若干时间步内的系统行为。在这个时域内，算法预测系统状态的变化，并计算相应的控制输入。
3. 代价函数的构建
MPC通过一个代价函数来评估不同控制输入的优劣。代价函数通常包含以下几个部分： - 轨迹跟踪误差：衡量机器人实际路径与目标路径之间的偏差，偏差越小代价值越低。 - 控制输入代价：衡量控制输入的大小和变化，鼓励较小的控制输入以实现平滑控制。 - 障碍物距离代价：衡量路径是否避开障碍物，越远离障碍物代价值越低。
4. 优化问题的求解
在每个控制周期内，MPC通过求解一个优化问题来确定最优的控制输入序列。优化问题通常是一个带约束的非线性规划问题，求解过程包括： - 初始化预测：根据当前系统状态，使用预测模型预测未来时域内的系统状态。 - 构建优化问题：根据预测状态和代价函数，构建优化问题。 - 求解优化问题：使用数值优化算法（如QP、SQP等）求解优化问题，获得最优的控制输入序列。
5. 应用控制输入和滚动优化
MPC只应用优化得到的第一个控制输入到系统，然后进入下一个控制周期。这样做的好处是能够实时应对环境变化和模型不确定性。具体步骤如下： 1. 应用控制输入：将优化得到的第一个控制输入应用到机器人系统。 2. 状态更新：根据新的系统状态和传感器信息，更新系统状态。 3. 滚动优化：进入下一个控制周期，重复上述过程。
步骤总结
- 预测模型：建立描述机器人动态行为的数学模型。
- 预测时域：确定一个固定的预测时域。
- 代价函数构建：定义轨迹跟踪误差、控制输入代价和障碍物距离代价。
- 优化问题求解：在每个控制周期内，求解优化问题以获得最优控制输入序列。
- 应用控制输入和滚动优化：应用第一个控制输入，更新系统状态，进入下一个控制周期。
MPC算法通过在未来时域内预测系统行为和优化控制输入，实现了机器人在动态环境中的实时路径规划和避障。其优点在于能够处理多种约束和目标，具有很高的灵活性和鲁棒性，但计算复杂度较高，需要较强的实时计算能力。
对比及结论
以下是对DWA、TEB和MPC三种局部路径规划算法的对比，从计算资源、适应性和复杂度等方面进行分析，以便说明在计算资源有限的室内场景中哪种算法更适合使用。
| 特性 | DWA（Dynamic Window Approach） | TEB（Timed Elastic Band） | MPC（Model Predictive Control） |
|---|---|---|---|
| 计算复杂度 | 较低 | 中等 | 较高 |
| 实时性 | 高 | 中等 | 较低 |
| 路径平滑性 | 一般 | 高 | 高 |
| 避障能力 | 较好 | 好 | 很好 |
| 适应动态环境 | 较好 | 很好 | 很好 |
| 实现难度 | 较低 | 中等 | 高 |
| 计算资源需求 | 低 | 中等 | 高 |
| 鲁棒性 | 较高 | 较高 | 很高 |
适用性分析
- DWA：
- 计算复杂度低，能够在较短时间内计算出合适的速度，适合实时应用。
- 在避障和路径平滑性上表现一般，但足以应对大多数室内环境。
- 计算资源需求较低，适合计算资源有限的场景。
- TEB：
- 通过优化时间和空间参数，可以生成平滑、高效的路径。
- 计算复杂度和计算资源需求中等，在路径平滑性和避障能力上有良好表现。
- 对于动态环境适应性好，但在资源有限的情况下，可能会出现性能瓶颈。
- MPC：
- 通过预测未来的系统行为进行优化，生成的路径在避障和平滑性上表现最好。
- 计算复杂度高，优化问题求解需要较多的计算资源和时间。
- 虽然具有很高的灵活性和鲁棒性，但在计算资源有限的环境中不太适用。
结论
在计算资源有限的室内场景中，DWA（Dynamic Window Approach） 更适合使用。这是因为DWA算法计算复杂度低，实时性高，对计算资源的需求较少，能够在有限的计算能力下实现较好的路径规划和避障效果。虽然在路径平滑性和优化效果上不如TEB和MPC，但其综合表现和资源适配性使其成为计算资源受限情况下的最佳选择。