第4章避障技术

第四节：避障技术——碰撞检测模型、包围盒算法、人工势场法、动态窗口法、TEB算法

各位同学，大家好。今天我们聊一个非常实际的问题——避障。

做路径规划，最怕什么？最怕规划了半天，机器人一头撞上去。我早年调试一台AGV，明明路径算得漂漂亮亮，结果现场一个纸箱没识别到，直接怼上去，电机都憋停了。从那以后，我对避障技术就格外上心。

避障技术说白了就两件事：检测到障碍，然后躲开它。但这两件事，每一件都有好几种玩法。今天我把主流的几种方法串起来讲，大家看看哪种适合你的场景。

一、碰撞检测模型——先知道会不会撞

避障的第一步，是判断机器人和障碍物有没有接触风险。这个判断就叫碰撞检测。

我习惯把碰撞检测分成两类：

静态检测：环境不变，提前算好有没有碰撞。
动态检测：环境在变，边走边算。

不管哪一类，核心都是几何求交。把机器人和障碍物都抽象成几何体，然后算它们有没有重叠。

核心公式（点到线段距离）：

// 计算点P到线段AB的最短距离
float PointToSegmentDistance(Point P, Point A, Point B) {
    Vector AB = B - A;
    Vector AP = P - A;
    float t = dot(AP, AB) / dot(AB, AB);
    t = clamp(t, 0.0, 1.0);
    Point closest = A + t * AB;
    return distance(P, closest);
}

嗯，这里要注意：实际项目中，机器人不是质点。你得把机器人轮廓也考虑进去。我见过有人直接用质点模型做碰撞检测，结果机器人卡在门框里——因为忽略了机器人本身的宽度。

二、包围盒算法——让碰撞检测快起来

直接对复杂形状做碰撞检测，计算量太大了。你想想看，一个机械臂有6个关节，每个关节都是不规则形状，要是逐面片去算，CPU直接冒烟。

所以就有了包围盒。说白了，就是用简单的盒子把复杂物体包起来。

常用的包围盒有几种：

类型	特点	适用场景
AABB（轴对齐包围盒）	与坐标轴对齐，计算最快	静态环境、粗略检测
OBB（有向包围盒）	可旋转，贴合度更好	旋转物体、机械臂连杆
包围球	计算最简单，但包裹不紧	快速粗筛、移动机器人
凸包	最贴合，但计算最慢	高精度碰撞检测

我个人习惯用两级检测：先用包围球做粗筛，再用OBB做精检。这样又快又准。我在做仓储机器人项目时，就是靠这个策略把碰撞检测周期从50ms降到了5ms。

避坑指南：我曾经犯过一个错——AABB包围盒没考虑机器人旋转。结果机器人原地转了个弯，包围盒直接穿模到墙里去了。记住：只要机器人会旋转，就别用AABB，至少用OBB。

三、人工势场法——让障碍物“推开”机器人

这个方法很有意思。它把目标点想象成引力源，把障碍物想象成斥力源。机器人在引力场和斥力场的合力下运动。

公式很简单：

F_total = F_attraction + F_repulsion

F_attraction = k_att * (target - robot_pos)
F_repulsion = k_rep * (1/d - 1/d0) * (1/d²) * direction

其中d是到障碍物的距离，d0是斥力影响范围。

这个方法的好处是实时性好，每步只需要算几个向量加减。但有个致命问题——局部极小值。机器人可能卡在U形障碍物里出不来。

我遇到过这种情况：一台扫地机器人在沙发和墙角之间来回晃悠，就是出不来。后来我加了随机扰动，检测到机器人长时间不动，就给它一个随机方向的小推力，这才破局。

注意：人工势场法不适合狭窄通道。障碍物两边的斥力会把机器人“挤”在中间，导致震荡。如果你要过窄门，别用这个方法。

四、动态窗口法（DWA）——边走边看

DWA的全称是Dynamic Window Approach。它不提前规划全局路径，而是在当前时刻，评估所有可能的运动，选一个最好的。

具体怎么做？

根据机器人当前速度，生成一个“速度窗口”（能加速到多少、能减速到多少）。
在这个窗口里采样多组速度（线速度+角速度）。
对每组速度，模拟未来一小段时间的运动轨迹。
用评价函数给每条轨迹打分：越靠近目标越好、越远离障碍物越好、速度越快越好。
选分数最高的那组速度，执行。

评价函数长这样：

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

α、β、γ是权重，需要调参。我调参时有个经验：先调β（避障权重），再调α（目标权重）。因为安全第一，先保证不撞，再谈效率。

DWA的优点是反应快，适合动态障碍物。缺点是没有全局视野，容易走进死胡同。所以实际项目中，我通常把DWA和全局路径规划结合——全局规划给个大致方向，DWA做局部避障。

五、TEB算法——带时间的最优控制

TEB的全称是Timed Elastic Band。这个名字有点抽象，我换个说法：把路径想象成一根橡皮筋，两端固定（起点和终点），中间受各种力（障碍物斥力、路径平滑力、时间最优力）的拉扯，最终找到一条既安全又快速的路径。

TEB的核心思想是：同时优化路径和时间。它把路径离散成一系列位姿点，每个点还带一个时间戳。优化目标包括：

路径总时间最短
路径平滑
远离障碍物
满足机器人运动学约束（最大速度、最大加速度）

优化过程用图优化（Graph Optimization）来做。说白了，就是构建一个图，节点是位姿点，边是约束条件，然后用最小二乘法求解。

TEB的优化目标函数：

min ∑(时间差²) + ∑(障碍物距离惩罚) + ∑(路径平滑惩罚) + ∑(运动学约束惩罚)

TEB的优点是路径质量高，能生成非常平滑的轨迹。缺点是比较吃算力，不适合算力有限的嵌入式平台。

我去年做的一个移动抓取项目，用的就是TEB。机械臂在移动底盘上，底盘用TEB规划路径，机械臂同时做抓取规划。效果不错，但调试参数花了两周——TEB的权重太多了，每个惩罚项的系数都得细调。

知识体系总览

下面这张图，我把今天讲的五种技术串起来了。大家看看它们之间的关系：

从这张图能看出来，五种技术不是互斥的，而是可以组合使用的。比如：用包围盒加速碰撞检测，用DWA做局部避障，用TEB做全局路径优化。我现在的项目基本都是这种混合方案。

好了，避障技术就讲到这里。每种方法都有它的脾气，选型时一定要结合你的机器人类型、环境复杂度、算力资源来定。别盲目追求高大上，有时候一个简单的包围盒+DWA就能解决问题。

📚 课程目录

第四节：避障技术——碰撞检测模型、包围盒算法、人工势场法、动态窗口法、TEB算法

一、碰撞检测模型——先知道会不会撞

二、包围盒算法——让碰撞检测快起来

三、人工势场法——让障碍物“推开”机器人

四、动态窗口法（DWA）——边走边看

五、TEB算法——带时间的最优控制

知识体系总览