第四章 环境约束：障碍物避碰、自碰撞与边界约束

各位同学，今天我们来聊聊轨迹规划里最让人头疼的部分——环境约束。说实话，我入行那会儿，觉得轨迹规划最难的是数学推导。后来做了几个实际项目才发现，真正让算法崩溃的，往往是这些「环境约束」没处理好。

环境约束说白了就三件事：别撞上障碍物、别让自己打结、别跑出工作空间。听起来简单吧？但每个坑我都踩过不止一次。

4.1 静态障碍物避碰

静态障碍物，就是那些固定不动的物体。比如工厂里的机床、仓库里的货架。处理它们，我习惯用两种思路：

4.1.1 基于构型空间的方法

把机器人缩小成一个点，把障碍物膨胀成「禁区」。这样碰撞检测就变成了点是否在多边形内的问题。我在做六轴机械臂项目时，就是这么干的。

具体实现时，我常用两种方式：

• 精确建模：用凸多边形近似机器人连杆，用分离轴定理检测碰撞。精度高，但计算量大。
• 栅格化：把C-Space离散成网格，标记每个格子是否被障碍物占据。简单粗暴，适合低维空间。

4.1.2 基于距离场的方法

另一种思路是计算机器人到障碍物的距离。我特别喜欢用有符号距离场（SDF）。

// 伪代码：计算点到障碍物的有符号距离
float signedDistance(Vector3 point, Obstacle obstacle) {
    float dist = obstacle.distanceToSurface(point);
    if (obstacle.contains(point)) {
        return -dist;  // 内部为负
    } else {
        return dist;   // 外部为正
    }
}

有了距离场，约束就变成了d(q) ≥ d_safe。这个形式特别适合优化类轨迹规划方法。

4.2 动态障碍物避碰

动态障碍物就麻烦多了。它们会动，而且运动不确定。我记得有一次做AGV调度项目，一个工人突然从货架后面走出来，差点撞上机器人。

处理动态障碍物，我总结了三板斧：

1. 速度障碍法（VO）：在速度空间里找安全方向。适合移动机器人。
2. 时间弹性带（TEB）：把时间维度加进来，优化轨迹的同时考虑障碍物预测位置。
3. 模型预测控制（MPC）：滚动优化，每个时间步重新规划。计算量大，但鲁棒性好。

4.3 自碰撞检测

自碰撞，就是机器人的不同部位撞到一起。这事儿在冗余自由度机器人上特别常见。我做过一个7轴协作机器人项目，调试时眼睁睁看着它自己的肘关节撞到了大臂。

自碰撞检测的核心是：检查所有非相邻连杆对之间的距离。

连杆对	是否可能碰撞	检测方法
相邻连杆	否（关节连接）	跳过
相隔1个关节	可能	胶囊体碰撞检测
相隔2个及以上	可能	GJK算法

实际工程中，我习惯用胶囊体近似每个连杆。就是把连杆包成一个圆柱加两个半球。这样碰撞检测就变成了两个胶囊体之间的最近距离计算。

// 两个胶囊体之间的最近距离
float capsuleDistance(Capsule a, Capsule b) {
    // 计算两条线段之间的最近距离
    float segDist = segmentDistance(a.line, b.line);
    // 减去两个半径
    return segDist - a.radius - b.radius;
}

4.4 工作空间边界约束

工作空间边界，就是机器人不能越过的物理边界。比如安全围栏、工作台边缘。这个约束看起来简单，但处理不好会出大问题。

我习惯把边界约束分为两类：

• 硬约束：绝对不能违反。比如关节限位、安全围栏。
• 软约束：尽量不违反，但允许轻微越界。比如靠近边界时加惩罚项。

对于硬约束，我通常用投影法：如果轨迹点越界，直接把它拉回到边界上。但要注意，投影可能会让轨迹不连续。

4.5 知识体系总览

下面这张图是我自己总结的环境约束处理框架，你们可以照着这个思路来设计自己的系统：

你看，所有环境约束最终都可以统一成g(q) ≤ 0的形式。这样不管是优化方法还是采样方法，都能用同一套框架来处理。

好了，环境约束这块就讲到这里。你们在实际项目中遇到什么问题，随时可以来找我聊。毕竟这些坑，我都替你们踩过了。