IMU 安全状态机——让巡检车在被搬起来的时候知道自己被搬起来了

巡检车在甘蔗田里跑着跑着，有人走过来把它搬走了——系统完全不知道，导航还在傻乎乎地算路径，电机还在空转。更离谱的：车从台阶上摔下去了，IMU 数据疯狂跳动，但代码只是在日志里打了几行 warning，该走的路还在走。

这些事真发生过。后来我写了一套基于 IMU 的安全状态机，四个独立的检测器，每个都走”阈值 + 时间窗口 + 状态机 + 滞回恢复”的模式。这篇把这几个检测器的逻辑拆开讲。

统一模式：阈值 + 时间窗口 + 滞回

所有检测器共享同一种设计：

信号超过阈值 → 开始计时
持续超过阈值达 N 秒 → 触发事件
信号回落到（阈值 - 滞回值）以下 → 开始恢复计时
持续低于恢复阈值达 M 秒 → 解除事件

滞回是关键。没有滞回的话，信号在阈值附近抖动，状态就会反复触发/解除，下游的导航系统跟着反复启停，比不检测还糟糕。

拿起检测：你的车被人抱走了

这是最常见的场景——工作人员搬车换位置，或者熊孩子把车提起来看。

判断依据两条路，满足任一即进入候选状态：

姿态路径：倾斜角 ≥ 50° 且 IMU 静止（加速度在 0.85g~1.15g，角速度 ≤ 18°/s）。持续 0.8 秒确认。

动态路径：加速度超出了”正常站着”的范围（< 0.7g 或 > 1.3g），或者角速度 ≥ 90°/s。持续 0.35 秒确认。比姿态路径快，因为车被猛地拎起来时加速度变化先于姿态变化。

pickup_pose_candidate = tilt >= 50.0 and stationary
pickup_dynamic_candidate = (
    accel_magnitude_g <= 0.70
    or accel_magnitude_g >= 1.30
    or gyro_dps >= 90.0
)
pickup_candidate = pickup_pose_candidate or pickup_dynamic_candidate
pickup_confirm_target = 0.8 if pickup_pose_candidate else 0.35

恢复条件：倾斜角降到 50° - 8° = 42° 以下，且静止 0.8 秒。8° 的滞回防止车刚放下还没放稳就解除刹车。

摔落检测：自由落体 + 冲击

这个是最戏剧性的：车从高处掉下来。

检测分两步——先检测自由落体，再等冲击：

# 第一步：自由落体
if accel_magnitude_g < 0.3:  # 加速度小于 0.3g = 几乎失重
    freefall_start_t = now
    in_freefall = True

# 失重结束，检查持续时间
freefall_duration = now - freefall_start_t
if freefall_duration >= 0.08:  # 至少 80ms
    waiting_impact = True
    impact_deadline = now + 0.50  # 500ms 内等待冲击

# 第二步：冲击
if waiting_impact:
    if accel_magnitude_g >= 2.8:  # 2.8g 冲击 = 着地了
        fall_triggered = True
    elif now > impact_deadline:
        waiting_impact = False  # 没等到冲击，可能是假警报

为什么不是”加速度大就算摔”？因为车正常行驶过减速带时加速度也能到 2g+。自由落体 → 冲击的时序组合才有意义：先失重再砸地，这是摔落的物理特征，过个坑不具备。

恢复条件比拿起更严格：倾斜 ≤ 15°、加速度正常、角速度低，三个条件同时满足 0.8 秒。摔了之后得确认车真的稳了。

侧翻检测：70° 倒在那儿

最简单的检测器：倾斜 ≥ 70° 且静止 1 秒。

为什么还要加”静止”？因为车高速转弯时瞬时倾斜可能很大，但在动态中 IMU 的倾斜计算精度下降。静止 + 大倾斜 = 真的翻了。

if tilt >= 70.0 and stationary:
    tipover_start_t = now
    if now - tipover_start_t >= 1.0:
        tipover_triggered = True

恢复：倾斜回到 70° - 8° = 62° 以下且静止。8° 滞回防止车被扶到 69° 又倒回去的抖动。

深度相机：障碍物和坑洞

IMU 检测的是”车出事了”，深度相机检测的是”前面有东西”。

从深度图中间裁一个 ROI，计算两个比例：

obstacle_ratio：距离小于 0.45m 的像素占比 ≥ 6% → 有障碍物
invalid_ratio：深度值无效（0 或 inf）的像素占比 ≥ 55% → 有坑洞

坑洞检测有个特别坑的问题：甘蔗叶子和反光表面也会让深度值无效。深度相机说”前面是坑”，但其实是甘蔗叶子挡住了。

解法：雷达交叉验证。激光雷达不受反光影响，如果雷达说前方 30° 扇区内最近障碍物 > 0.35m（也就是”前面没问题”），那深度相机的坑洞判定就要求更高的 invalid_ratio（从 55% 提高到 90%）才触发。

if (
    is_pit
    and lidar_cross_validation_enabled
    and (not lidar_confirms_obstacle())
    and invalid_ratio < 0.90
):
    is_pit = False  # 雷达说没事，深度大概率是误报

坡道检测：不是障碍物，是斜坡

深度图里障碍物和坡道看起来一样——都是”近处有东西”。区别在于：坡道的深度是从远到近单调递减的，障碍物是突然出现的。

把 ROI 从上到下分成 4 个条带，计算每个条带的中位深度。如果从上到下深度严格递减（远处深近处浅，符合透视），且梯度 > 0.25m，且 IMU 俯仰角 < 25°，那就是坡道而不是障碍物——不触发刹车。

for i in range(1, len(medians)):
    if medians[i] >= medians[i - 1]:
        return False  # 不单调递减，不是坡道

gradient = medians[0] - medians[-1]
if gradient < 0.25: return False
if abs(pitch_deg) > 25.0: return False
return True

IMU 俯仰角的二次验证很关键：真坡道上俯仰角会变化，如果深度看起来像坡道但 IMU 说车是平的，那可能是深度噪声。

帧数确认：别被一帧噪声骗了

所有深度检测都走帧数确认：连续 2 帧检测到障碍物才触发，连续 3 帧没有才解除。触发比解除快——安全第一，宁可误刹不可漏刹。

def _update_block_state(self, blocked_candidate, reason, ...):
    if blocked_candidate:
        self._block_candidate_streak += 1
        self._clear_candidate_streak = 0
        if self._block_candidate_streak >= self.block_confirm_frames:  # 2
            self._set_blocked(True, reason, ...)
    else:
        self._clear_candidate_streak += 1
        self._block_candidate_streak = 0
        if self._clear_candidate_streak >= self.clear_confirm_frames:  # 3
            self._set_blocked(False, '', ...)

几个调参经验

参数	默认值	怎么调
拿起倾斜角	50°	车体重心低可降到 45°，高可提到 55°
坠落失重阈值	0.3g	太高会被颠簸误触发
坠落冲击阈值	2.8g	过减速带大约 2g，留点余量
侧翻确认时间	1.0s	太短会被急转弯误判
恢复滞回	8°	核心参数，太小抖动太大
坑洞无效比	55%	有雷达交叉验证可降到 45%

这套东西跑了几个月，最深的感触是：田野里没有”干净”的传感器数据。深度相机被甘蔗叶子骗，IMU 被减速带骗，雷达被地面反射骗。单一传感器做安全判断是不靠谱的，必须交叉验证 + 时间窗口 + 滞回，三层过滤把误报压到可接受的水平。