基于Separating Axis Theorem的机器人碰撞检测路径搜索异常求助

数据老陈

2026-10-4

问题排查与修复方案

你的SAT碰撞检测核心流程逻辑是正确的，但几个常见的实现疏漏会导致误判，将无碰撞状态识别为碰撞，导致路径搜索时所有可行节点都被过滤：

多边形顶点合法性问题：SAT仅适用于凸多边形，且顶点必须按顺时针/逆时针顺序连续排列，不能有乱序、重复点、凹多边形的情况。如果你的机器人角点、障碍物顶点不符合要求，计算得到的边法向量会完全错误，直接导致碰撞检测结果不准。
浮点数精度误差：投影计算使用浮点数时，边界位置的极小误差可能导致本来重叠的投影被判断为分离，或者反过来。可以在分离判断条件里加微小容差：
```
# 原判断逻辑
# if max_1 < min_2 or max_2 < min_1:
# 修改为加1e-6容差
if max_1 + 1e-6 < min_2 or max_2 + 1e-6 < min_1:
```
角度单位错误：你调用get_robot_corners时传入了math.radians(neighbour_theta_cts)，如果neighbour_theta_cts本身已经是弧度单位，二次转换会导致角度完全错误，机器人角点计算偏移，出现大范围误判碰撞。可以打印输出几个姿态下的机器人角点坐标，核对是否和预期一致。
功能验证测试：建议先单独对has_collided做单元测试：输入两个已知分离的凸多边形看是否返回False，输入两个重叠的凸多边形看是否返回True，先确保碰撞检测本身的输出符合预期。

你当前的实现没有做任何前置过滤，每次节点扩展都要对所有障碍物做完整的SAT计算，路径搜索节点多、障碍物多的时候耗时会指数级上升，优化按优先级排序如下：

加AABB预检测过滤：这是性价比最高的优化，能筛掉90%以上不需要执行SAT的场景：
1. 预计算所有静态障碍物的轴对齐包围盒(obs_min_x, obs_max_x, obs_min_y, obs_max_y)，提前存起来不用每次计算
2. 每次碰撞检测前，先计算当前机器人的AABB，和障碍物AABB先做判断：如果机器人AABB和障碍物AABB完全不重叠，直接跳过该障碍物的SAT检测
  示例AABB检测代码：
```
def aabb_overlap(robot_aabb, obs_aabb):
    r_min_x, r_max_x, r_min_y, r_max_y = robot_aabb
    o_min_x, o_max_x, o_min_y, o_max_y = obs_aabb
    return not (r_max_x < o_min_x or r_min_x > o_max_x or r_max_y < o_min_y or r_min_y > o_max_y)
```
预计算静态障碍物数据：静态障碍物的边、法向量都是固定的，不需要每次检测时实时遍历顶点计算，提前存好可以减少大量冗余计算。
空间划分过滤障碍物：如果障碍物数量超过20个，可以用网格/四叉树做空间划分，每次只检测机器人所在网格及相邻网格内的障碍物，不需要遍历全量障碍物。
简化几何模型：如果你的机器人可以近似为圆形，直接用中心距离 < 机器人半径 + 障碍物外接圆半径的判断代替SAT，性能能提升至少一个数量级。

如果要部署到STM控制器上，可以提前做这两个优化降低嵌入式端算力开销：

内容的提问来源于stack exchange，提问作者awkwardsquid122