第三章传统避障算法：VFH+、DWA与TEB实战解析

说到传统避障算法，我得先坦白一件事。刚入行那会儿，我总觉得深度学习才是未来，传统算法都是老古董。直到有一次在工厂做AGV项目，客户要求必须实时响应，延迟不能超过50ms。神经网络还在加载模型呢，人家VFH+已经跑完三轮规划了。嗯，从那天起我学会了——合适的才是最好的。

核心观点：传统避障算法不是过时，而是经过工业验证的可靠方案。它们计算快、可解释性强、调试方便。在资源受限的嵌入式平台上，这些算法依然是主力。

3.1 VFH+算法：从传感器到控制指令的直通车

VFH+，全称Vector Field Histogram Plus。说白了，就是把激光雷达的数据变成一张“方向评分表”。

算法核心三步走：

构建极坐标直方图：把360度分成若干扇区，每个扇区统计障碍物密度
二值化处理：设定阈值，把连续密度变成“可通过/不可通过”的二元判断
选择最优方向：在可通行的扇区中，选一个最接近目标方向的

我个人习惯把VFH+叫做“急性子算法”。为什么？因为它不考虑速度，只考虑方向。每次决策都是独立的，不会回头看上一帧选了啥。这在某些场景下反而是优点——反应快。

实战经验：我在做扫地机器人时用过VFH+。当时遇到一个坑：地毯边缘的激光反射不稳定，导致直方图频繁跳变。解决方案是加了一个“历史投票机制”——当前帧的决策参考前3帧的结果，加权平均。效果立竿见影。

代码骨架（ROS实现）：

// VFH+核心函数伪代码
void VFHPlanner::updateHistogram(const sensor_msgs::LaserScan& scan) {
    // 1. 清空直方图
    histogram_.clear();
    
    // 2. 遍历激光点，填充扇区
    for (size_t i = 0; i < scan.ranges.size(); ++i) {
        float angle = scan.angle_min + i * scan.angle_increment;
        float range = scan.ranges[i];
        if (range < max_range_) {
            int sector = angleToSector(angle);
            histogram_[sector] += 1.0 / (range * range); // 距离越近权重越大
        }
    }
    
    // 3. 二值化
    for (auto& h : histogram_) {
        h = (h > threshold_) ? OBSTACLE : FREE;
    }
    
    // 4. 选方向
    int best_sector = findBestSector(target_direction_);
    publishCmdVel(sectorToAngle(best_sector), speed_);
}

避坑指南：我曾经在狭窄走廊里吃过亏。VFH+会把两侧墙壁都识别为障碍，中间留一条窄缝。如果机器人宽度刚好等于走廊宽度，算法会认为“无路可走”。解决办法是：在直方图里加入机器人半径的膨胀层。

3.2 DWA算法详解：速度空间里的博弈

DWA（Dynamic Window Approach）和VFH+完全不同。它不直接选方向，而是在速度空间里采样。

你想想看，机器人能跑多快、能转多急，是有物理限制的。DWA就是把这些限制画成一个“动态窗口”，然后在窗口里找最优的速度组合(v, ω)。

DWA的评估函数长这样：

cost = α * heading_cost + β * clearance_cost + γ * velocity_cost

heading_cost：朝向目标的程度，越正对目标分越低
clearance_cost：离最近障碍的距离，越远分越低
velocity_cost：速度大小，越快分越低（鼓励快速移动）

这三个权重α、β、γ怎么调？我踩过坑。有一次在仓库项目里，我把clearance_cost权重设得太高，结果机器人离障碍老远就开始绕路，效率极低。后来改成动态权重——靠近目标时降低clearance权重，提高heading权重。

关键区别：VFH+是“方向优先”，DWA是“速度优先”。DWA天然考虑了运动学约束，所以生成的轨迹更平滑。但代价是计算量大一些——每次要采样几百组速度。

DWA采样过程示意：

// DWA核心采样循环
for (float v = v_min; v <= v_max; v += v_step) {
    for (float w = w_min; w <= w_max; w += w_step) {
        // 检查是否在动态窗口内
        if (!inDynamicWindow(v, w, current_v, current_w, dt)) continue;
        
        // 模拟轨迹
        Trajectory traj = simulateTrajectory(v, w, dt);
        
        // 评估
        float cost = evaluateTrajectory(traj, goal, obstacles);
        
        // 更新最优
        if (cost < best_cost) {
            best_cost = cost;
            best_v = v;
            best_w = w;
        }
    }
}

调参心得：α、β、γ的初始值可以设为1:1:1，然后根据实际表现微调。我习惯先调β（避障权重），让机器人能安全通过最窄的通道，再调α（目标权重）保证不迷路，最后调γ（速度权重）优化效率。

3.3 TEB算法应用：时间最优的轨迹优化

TEB（Timed Elastic Band）这个名字挺形象——把轨迹想象成一条有弹性的带子，两端固定（起点和终点），中间受各种力（障碍物排斥力、平滑力、时间最优力）的作用变形。

说白了，TEB是一个在线优化问题。它把路径离散成一系列位姿点，每个点都带时间戳。优化的目标就是让这些点满足约束的同时，总时间最短。

TEB的约束条件：

运动学约束（最大速度、加速度、转弯半径）
障碍物避碰（点到障碍物的最小距离）
动力学约束（电机力矩限制）
时间最优（相邻点的时间间隔尽可能小）

我记得第一次用TEB是在一个动态障碍物频繁出现的场景。DWA老是卡在局部最优，VFH+又太抖。TEB的弹性特性让它能“挤”过不断变化的缝隙，效果惊艳。

注意：TEB不是万能的。它的优化过程需要迭代，如果环境变化太快（比如人突然冲到面前），优化可能来不及收敛。我一般会在TEB上层加一个安全层——当障碍物距离小于紧急阈值时，直接刹车。

3.4 算法对比与选型：没有银弹

做项目这么多年，我最大的体会就是：没有最好的算法，只有最合适的算法。下面这张表是我个人的选型经验总结：

特性	VFH+	DWA	TEB
计算复杂度	低（O(n)）	中（O(n*m)）	高（迭代优化）
轨迹平滑度	一般（方向突变）	好（速度连续）	优秀（全局优化）
动态环境适应	快（每帧独立）	中（需重采样）	慢（需重优化）
狭窄通道通过	差（易震荡）	中（依赖参数）	好（弹性变形）
嵌入式部署	轻松	可行	困难（需浮点运算）
调参难度	低（2-3个参数）	中（5-8个参数）	高（10+个参数）

我的选型建议：

预算有限、算力吃紧：选VFH+。比如玩具机器人、低成本扫地机
需要平滑运动、中等算力：选DWA。比如服务机器人、送餐机器人
复杂动态环境、算力充足：选TEB。比如自动驾驶小车、仓储AGV
混合方案：我最近的项目里，用VFH+做全局方向引导，DWA做局部速度控制，TEB做轨迹优化。三层叠加，效果1+1+1>3

一句话总结：VFH+是快刀斩乱麻，DWA是稳扎稳打，TEB是精雕细琢。选哪个，取决于你的机器人能跑多快、脑子有多好使、环境有多乱。

好了，三种算法都讲完了。说实话，每个算法都有它的脾气。VFH+像个直性子，DWA像个稳重的老司机，TEB像个精打细算的规划师。你在项目中用哪个，取决于你的机器人要面对什么样的世界。

可可精品资料库

📚 课程目录

第三章 传统避障算法：VFH+、DWA与TEB实战解析

3.1 VFH+算法：从传感器到控制指令的直通车

3.2 DWA算法详解：速度空间里的博弈

3.3 TEB算法应用：时间最优的轨迹优化

3.4 算法对比与选型：没有银弹

第三章传统避障算法：VFH+、DWA与TEB实战解析