4、避障算法选型：VFH+、DWA、TEB 深度解析
说到避障算法选型，我其实挺感慨的。
刚入行那会儿，我总觉得避障嘛，找个算法跑起来就行。结果呢？机器人要么原地转圈，要么撞墙。后来我才明白——选对算法，比调参更重要。
今天咱们就聊聊三种主流避障算法：VFH+、DWA 和 TEB。我会把它们的原理、优缺点、适用场景，以及我踩过的坑，一次性说清楚。
核心观点：没有最好的算法，只有最合适的场景。选型前，先搞清楚你的机器人是啥、跑在哪、要求多高。
4.1 VFH+ 算法原理
VFH+，全称 Vector Field Histogram+，说白了就是「向量场直方图」的升级版。
它的核心思路很简单：把周围障碍物投影到一个极坐标直方图上，然后找一条「干净」的方向走。
我记得第一次在 Gazebo 里跑 VFH+，用的是 Pioneer 3-DX 小车。环境是个布满柱子的仓库。VFH+ 的表现让我挺惊喜的——它不像某些算法那样犹豫不决，而是很果断地选了一个方向就冲过去。
VFH+ 的工作流程大致分三步：
- 构建直方图：用激光雷达数据，把障碍物映射到 360° 的极坐标网格里。每个扇区有一个「障碍物密度值」。
- 二值化处理：设定一个阈值，高于阈值的扇区标记为「不可通行」，低于的标记为「可通行」。
- 选择方向：在可通行区域里，选一个最接近目标方向的角度作为前进方向。
💡 我的小技巧：VFH+ 里有个参数叫 window_diameter，它控制直方图的平滑程度。我习惯设成机器人宽度的 1.5 倍左右。太小了容易抖动，太大了会丢失细节。
VFH+ 最大的优点是计算量小、响应快。在嵌入式平台上跑都没问题。但缺点也很明显——它不考虑机器人的运动学约束。说白了，它只管「往哪走」，不管「怎么走」。所以如果你的机器人是差速轮还好，要是阿克曼底盘，VFH+ 就容易出现「方向给了，但转不过去」的尴尬。
4.2 DWA 算法原理
DWA，Dynamic Window Approach，动态窗口法。这名字听着挺唬人，其实核心思想就一句话：在速度空间里采样，找一条最好的轨迹。
你想想看，机器人能跑多快、能转多快，是有物理限制的。DWA 就是在当前时刻，把所有可能的 (线速度, 角速度) 组合都试一遍，然后模拟出对应的轨迹，再打分选最优。
DWA 的评价函数通常包含三个维度：
- 朝向得分：轨迹终点朝向目标的程度
- 速度得分：鼓励高速行驶
- 障碍物距离得分：离障碍物越远越好
这三个维度加权求和，得分最高的轨迹就是最终的执行轨迹。
⚠️ 我曾经踩过的坑：DWA 的权重调参是个玄学。有一次我在 Gazebo 里调一个差速轮小车，障碍物距离权重设得太高，结果机器人离墙 2 米就开始减速，最后直接停在了走廊中间。后来我把权重从 0.5 降到 0.2，才恢复正常。
DWA 的优点是天然考虑运动学约束，生成的轨迹平滑、可执行。但它也有个硬伤——局部最优。说白了，它只看眼前几步，容易陷入死胡同。比如 U 形障碍物，DWA 大概率会钻进去出不来。
4.3 TEB 算法原理
TEB，Timed Elastic Band，时间弹性带。这个名字很有意思——「弹性带」指的是把轨迹想象成一根橡皮筋，而「时间」维度让这根橡皮筋有了速度信息。
我个人觉得，TEB 是这三种算法里最「聪明」的一个。它把避障问题建模成一个带约束的优化问题，目标函数里同时考虑了：
- 路径长度（越短越好）
- 时间消耗（越快越好）
- 障碍物距离（越远越好）
- 运动学约束（不能原地掉头）
- 动力学约束（加速度有限）
然后通过 g2o 或 Ceres 这样的优化库去求解。嗯，听着就很「学术」对吧？但实际用起来，效果确实好。
TEB 的典型配置参数：
# TEB 参数示例
TebLocalPlannerROS:
odom_topic: /odom
map_frame: /map
# 轨迹参数
teb_autosize: True
dt_ref: 0.3
dt_hysteresis: 0.1
max_samples: 500
# 约束参数
min_obstacle_dist: 0.5
inflation_dist: 0.6
include_costmap_obstacles: True
# 优化参数
no_inner_iterations: 5
no_outer_iterations: 2
optimization_activate: True📌 注意：TEB 的 dt_ref 参数控制轨迹上两个点之间的时间间隔。我习惯设 0.3 秒。设太小了轨迹点太多，计算量大；设太大了轨迹太粗糙，容易漏掉障碍物。
TEB 的优点是轨迹质量高、能处理复杂环境。但它也有代价——计算量大。在 Jetson Nano 上跑 TEB，帧率可能只有 10Hz 左右。而 VFH+ 能跑到 50Hz 以上。
4.4 三种算法对比与选型
好了，三种算法都讲完了。咱们来做个横向对比，这样你选型的时候心里就有数了。
| 对比维度 | VFH+ | DWA | TEB |
|---|---|---|---|
| 核心思想 | 极坐标直方图选方向 | 速度空间采样+评价 | 轨迹优化+时间弹性 |
| 计算量 | 低（适合嵌入式） | 中 | 高（需要优化库） |
| 运动学约束 | 不考虑 | 考虑 | 考虑（含动力学） |
| 轨迹质量 | 一般（折线多） | 较好（平滑） | 优秀（最优轨迹） |
| 动态障碍物 | 弱 | 中 | 强（可重规划） |
| 局部最优问题 | 有（U形障碍物） | 有（U形障碍物） | 较少（优化求解） |
| 典型场景 | 仓库、走廊、简单环境 | 室内、差速轮、服务机器人 | 复杂环境、动态场景、自动驾驶 |
我的选型建议：
- 如果你用的是低成本 MCU，算力有限：选 VFH+。它简单粗暴，够用就好。
- 如果是差速轮室内机器人，环境不太复杂：选 DWA。它平衡了性能和计算量，是 ROS 里最常用的局部规划器。
- 如果环境复杂、有动态障碍物、或者对轨迹质量要求高：选 TEB。虽然计算量大，但效果确实好。我去年做一个仓储机器人项目，就是靠 TEB 搞定了密集货架间的穿梭。
💡 一个小建议：在 Gazebo 里做仿真测试时，可以先用 DWA 快速验证整体逻辑，等需要精细化调优时再切换到 TEB。这样能节省大量调试时间。
嗯，关于避障算法选型，今天就聊到这儿。这三种算法各有千秋，关键还是看你的具体需求。记住我开头说的那句话——没有最好的算法，只有最合适的场景。