深度专题 D1 · 算法

几何解算原理与精度预算

从「三条边为什么不够」讲到「LM 迭代怎么写」,再到「5000 次蒙特卡洛跑出来误差多大」。 这是整个方案的技术内核,也是评审时最容易被追问的部分。

1. 问题形式化

设三个感知节点在世界坐标系 W 下的位置为:

pA, pB —— 两个挂钩
pW —— 腰腹卡扣(主控盒 IMU 所在位置)

需要求解的三个判定量:

符号表达式几何含义
L‖pA − pB‖双钩间距
Δh(pW − (pA+pB)/2) · ĝup腰卡相对两钩连线中点的垂直高差(向上为正)
d‖(pW − m) − [(pW − m)·û]û‖,m 为中点,û 为 A→B 单位向量腰卡在连线法平面内的水平偏移

其中 ĝ_up 是重力方向的单位向量(向上)。整个问题的难点就在这个 ĝup——它不是测出来的距离,而是姿态信息。

2. 自由度分析:三边为什么不够

三点在三维空间中有 9 个坐标。三个距离约束消耗 3 个自由度,剩余 6 个自由度正好等于刚体的 6 个运动自由度(3 平移 + 3 旋转)。

这意味着:三条边长唯一确定的是三角形的"形状",而不是它在重力场中的"朝向"。

具体到本问题:假设测出 AW = BW = 1.0 m、AB = 1.2 m,这是一个等腰三角形。它可以让 W 在连线下方 0.8 m,也可以整体翻转,让 W 在连线上方 0.8 m——两种情况的三个距离完全相同,无法区分。

这就是为什么"只用 UWB 测距"的产品做不出低挂高用判定

它能告诉你"腰卡距离两个钩子各 1 米",但永远无法告诉你"腰卡在钩子上方还是下方"。 这个结论在数学上是硬的,不是精度问题,加多少滤波器都无解。

引入重力参考后的可解性

每个 IMU 输出其自身坐标系下的重力方向。设 R 为待求的刚体旋转(把"三角形自身坐标系"旋到世界系),则约束为:

R · ĝlocal,W ≈ ĝup
R · ĝlocal,C ≈ ĝup   (C 为胸卡,与腰卡同属刚性的躯干段)

两个不平行的重力观测(因为胸、腰两点的安装姿态不同)把 R 的 3 个旋转自由度约束到唯一解附近; 若两点姿态过于接近(例如都竖直朝上),则退化为绕重力轴的单自由度不可观测—— 这时需要一个额外的先验(人体朝向由加速度主方向估计,或由 UWB 基线方向约束)。 实际构型中,胸卡与腰卡的法线方向相差约 30°~60°,可观测性良好。

表 D1-1 可观测性判据
条件条件数 κ结论
胸/腰法线夹角 >30°,AB 与重力夹角 >20°<8良态 可稳定解算
胸/腰法线夹角 10°~30°8~25边缘 误差放大 2~3 倍,需延长观测窗口
胸/腰姿态几乎重合,或 AB 近乎竖直>50病态 输出"不可判定"

3. 测距模型

采用双边双向测距(DS-TWR),消除时钟不同步带来的比例误差:

Tprop = ( Tround1·Tround2 − Treply1·Treply2 ) / ( Tround1 + Tround2 + Treply1 + Treply2 )
d = c · Tprop

误差来源主要有三项:

  • 时钟漂移:DS-TWR 已将其压到 ppm 级,在 3 m 量程下贡献 <1 cm;
  • 多径:金属构架反射造成前沿偏移,是主要误差源,典型 5~30 cm,且具有系统性(同一构型下可重复);
  • 非视距(NLOS):人体遮挡、挂钩被构件挡住,误差可达 0.5 m 以上,必须靠置信度剔除。

对策:首径检测(前沿门限)+ 信号质量指示(RSSI/FPPL)+ 三链路残差检验,残差超限则该帧标记为低置信度,不参与解算。

4. 解算方法

构造非线性最小二乘问题,未知量为 R(3 参数,用李代数 so(3) 参数化)与三点在自身坐标系下的位置(由边长确定形状,尺度固定):

minR Σi∈{AB,AW,BW} ρ( ‖dimeas − ‖pimodel(R)‖ ‖ / σi )
  + λ1 Σk∈{W,C} ‖R·ĝk − ĝup‖²
  + λ2 ‖h(pW) − hbaro‖²

其中 ρ(·) 为 Huber 鲁棒核(阈值 δ=1.5σ),用于抑制多径野值;λ 为权重,由各传感器的方差倒数确定。求解用 LM 迭代,初值取自上一帧结果(时间连续性),冷启动时用"腰在钩下"的物理先验。

迭代伪码

solve_geometry(dist[3], g_w, g_c, baro_h):
    R = last_R or R_init_from_gravity(g_w)      # 初值
    for it in 0..4:
        r = residual(R, dist, g_w, g_c, baro_h) # 6~7 维残差
        if norm(r) < EPS: break
        J = jacobian_numeric(R, ...)            # 3x7,数值差分
        W = diag(1/sigma_i^2)
        H = J^T W J + mu * I                    # LM 阻尼
        dR = solve(H, -J^T W r)                 # 3 维增量
        R = R * exp_so3(dR)
        if norm(r_new) > norm(r): mu *= 4 else mu /= 2
    if condition_number(H) > K_MAX: return UNKNOWN
    if norm(r) > R_MAX:              return UNKNOWN   # 残差过大 → 不判定
    return L, delta_h, d, confidence
工程要点:宁可"不可判定",不可"判错"

残差超限、条件数过大、置信度低时,系统输出 UNKNOWN 而不是猜测。 对安全设备而言,一次误判为合规的代价,远大于十次"请人工确认"。

5. 误差传播与蒙特卡洛

解析误差传播可给出量级,但 Huber 核与条件数导致的非线性放大难以解析,因此用蒙特卡洛验证:

表 D1-2 蒙特卡洛设置(5000 次/构型)
参数分布
测距误差高斯,σ=0.06 m,另叠加 5% 概率的 ±0.4 m 野值(多径)
姿态误差高斯,σ=0.8°(三轴独立)
气压高度高斯,σ=0.05 m,慢漂移 0.02 m/min
时间不同步0~15 ms 随机延迟,人体速度 0.3 m/s
表 D1-3 典型构型下的 Δh 误差(5000 次统计)
L (m)Δh 真值 (m)均值偏差1σP95 |误差|判定
1.2−0.80+0.0120.0810.16达标
1.2−0.30+0.0150.0790.15达标
2.5−1.50+0.0210.0940.19达标
2.5+0.50−0.0180.0980.20达标
0.6−0.30+0.0300.1210.25边缘 短基线下姿态误差放大
2.5(NLOS)−1.00+0.0850.2100.48超限 触发 UNKNOWN,不输出判定

结论:在 LOS 且构型良态时,P95 误差 ≤0.20 m,满足指标;短基线(L=0.6 m)时误差上升到 0.25 m, 因此对 L<0.8 m 的构型自动降低置信度并提示"双钩过近";NLOS 时误差爆炸,靠残差门控直接拒绝输出。

6. 阈值标定实验设计

算法精度只是输入之一,阈值本身也需要实验支撑。设计如下三因素实验:

因素水平观测指标
构型 Δh−1.5 / −0.8 / −0.3 / 0 / +0.5 m判定一致率(与真值比对)
人员动作静止 / 缓慢移动 / 快速换钩误报次数
环境实验室 / 变电站钢结构 / 户外铁塔有效判定率(非 UNKNOWN 占比)

每组 3 名受试者 × 3 次重复,共 135 次试验;据此确定 T_hold、T_rel 与置信度门限的最优组合。

7. 失效与边界

情形算法行为
只有单钩测距有效无法构成三角形,输出 UNKNOWN,进入 S1 转移态
两钩距离超出量程返回 L>L_max,标注"超出范围",不参与 Δh 计算
IMU 饱和(剧烈冲击)姿态失效,冻结上一帧结果 2 s,同时触发冲击检测通道
气压突变(进出室内)气压权重降为 0,仅靠 UWB + IMU,误差上升但在门限内
三链路残差同时偏大判为强多径环境,输出 UNKNOWN 并记录"环境异常"
回到主报告

本章的结论支撑了 第 4 章的精度预算表(0.093 m / 1σ,k=2 得 ±0.19 m) 与阈值设定(−0.30 m 门限,留 0.11 m 裕度)。若后续更换 UWB 器件或天线,必须重跑本章的蒙特卡洛与构型实验。