解读 05 · fall.rs:摔倒检测与起身
解读对象:
duck-control/src/fall.rs(272 行)
需要的前置:《什么鸭公开课》第 04 课(61→14 契约)+ 解读 04
先说清题目边界:fall.rs 里没有起身。它只做一件事——看见摔倒正在开始,而不是确认一桩已经结束的摔倒。至于"摔了怎么办"(软倒、起身),是 robotd 的控制决策,起身本身是一个训练出来的技能。这个文件的价值全在一个窗口里:安全层的判定(投影重力 z 过 -0.5 持续 200 ms)触发时,机器人已经在地上或再过几十毫秒落地;而软倒的全部价值,都花在落地之前那段时间(fall.rs:3-8)。
1. 用物理恒等式免掉滤波器
预测靠一个导数。投影重力随躯干旋转,体坐标系下恰好满足 ġ = −ω × g,只取 z 分量就是一次乘减:
ġz = −(ωx·gy − ωy·gx)
这就是 FallPredictor::gravity_z_rate(imu) 的全部(fall.rs:110-112)。测试钉的是解析值:从 30° 前倾、以 1 rad/s 倾转,重力 z 应以每秒 0.5 上升,容差 1e-12(fall.rs:170-172)。
注释特意说明了不做什么:不对 SFLP 四元数做数值微分。陀螺仪是同一 12 字节块里的直接测量,而微分四元数会把滤波器自身的滞后加进这个"全靠早"的量里(fall.rs:25-27)。
2. 三个条件,缺一不可
observe(fall.rs:120)每 tick 检查,当前重力 z 加上 gravity_z_rate 乘以 lookahead(默认 300 ms,fall.rs:75)得到 predicted_z(fall.rs:115),三条件同时满足才算"正在摔":
- 已经倾斜:
gravity[2] > tilt_z(默认 -0.90,约离直立 26°,fall.rs:73)。直立机器人上的陀螺仪尖峰是落脚、是推搡、是被人拎起来——没有位置闸的预测器会把这三种都叫成摔。 - 还在倒下去:
rate > 0(重力 z 从 -1 往 0 爬)。从倾斜中恢复的负速率,再大也不算。 - 预测越线:lookahead 之后的 z 超过
predicted_z(默认 -0.5,与安全层同值,语义是"它将在 0.3 s 后成为安全层眼中的摔倒",fall.rs:63-65)。
条件 2 和测试 a_static_tilt_is_not_a_fall(fall.rs:220)共同划清了与安全层的分工:被手扶在半倾姿势的机器人,位置判据会报警,预测器不会。
3. 边沿触发,迟侧调参
通过三条件后还要去抖:debounce = 60 ms,50 Hz 下三个 tick(fall.rs:76、47-48)。触发是边沿而非电平——fired 标志保证每次摔倒只返回一次 true(fall.rs:136-138),轮询的调用方得到的是一个时刻,不是一路脉冲。复 位是双向的:只有机器人"不再像在摔"时才重新武装(fall.rs:128-131),免得第一跤还没落地又触发第二跤;调用方接管(开始软倒)时可调 reset()(fall.rs:145)把进行中的计数清零。
默认值刻意放在迟侧(fall.rs:44-49),因为误报的代价不对称:过早软倒,机器人会在本来能走过去的倾斜上自己趴下——每一例假阳性都是机器人自己造成的摔跤;过迟,硬着陆,模式白买。测试 a_slow_lean_waits(fall.rs:231)钉住了这条边界:26°、0.3 rad/s 的慢倾,预测 z 约 -0.86,够不着 -0.5,正确答案是等站立策略去救,而不是抢先躺下。
4. 起身去哪了
仓库里与"起身"对应的是训练侧的 standup 任务与对应的 ONNX 策略(经 policy.rs 的 Net::SitStand 一类会话调度加载);fall.rs 只负责递出"该行动了"的那一个边沿。检测与响应分层,正是 解读 04 那条"安全层只报告、上层做决策"设计的另一半。
你带走的收获
- 位置判据确认"已发生",速率判据预测"进行中"——两者阈值语义要对齐(预测终点 = 安全层阈值)。
- 有现成物理恒等式时,直接测量永远好过对滤波输出做数值微分:微分放大滞后与噪声。
- 多条件触发器要按"哪类假阳性更贵"排序设计闸门;误报会改变系统行为的检测器,默认值应放迟侧。
- 长动作的触发用边沿(one-shot)而非电平,调用方才不会重复响应同一事件。
- 去抖窗口用 tick 数表达(60 ms = 3 tick @ 50 Hz),与控制频率的耦合一目了然。