几何解算原理与精度预算
从「三条边为什么不够」讲到「LM 迭代怎么写」,再到「5000 次蒙特卡洛跑出来误差多大」。 这是整个方案的技术内核,也是评审时最容易被追问的部分。
1. 问题形式化
设三个感知节点在世界坐标系 W 下的位置为:
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——两种情况的三个距离完全相同,无法区分。
它能告诉你"腰卡距离两个钩子各 1 米",但永远无法告诉你"腰卡在钩子上方还是下方"。 这个结论在数学上是硬的,不是精度问题,加多少滤波器都无解。
引入重力参考后的可解性
每个 IMU 输出其自身坐标系下的重力方向。设 R 为待求的刚体旋转(把"三角形自身坐标系"旋到世界系),则约束为:
R · ĝlocal,C ≈ ĝup (C 为胸卡,与腰卡同属刚性的躯干段)
两个不平行的重力观测(因为胸、腰两点的安装姿态不同)把 R 的 3 个旋转自由度约束到唯一解附近; 若两点姿态过于接近(例如都竖直朝上),则退化为绕重力轴的单自由度不可观测—— 这时需要一个额外的先验(人体朝向由加速度主方向估计,或由 UWB 基线方向约束)。 实际构型中,胸卡与腰卡的法线方向相差约 30°~60°,可观测性良好。
| 条件 | 条件数 κ | 结论 |
|---|---|---|
| 胸/腰法线夹角 >30°,AB 与重力夹角 >20° | <8 | 良态 可稳定解算 |
| 胸/腰法线夹角 10°~30° | 8~25 | 边缘 误差放大 2~3 倍,需延长观测窗口 |
| 胸/腰姿态几乎重合,或 AB 近乎竖直 | >50 | 病态 输出"不可判定" |
3. 测距模型
采用双边双向测距(DS-TWR),消除时钟不同步带来的比例误差:
d = c · Tprop
误差来源主要有三项:
- 时钟漂移:DS-TWR 已将其压到 ppm 级,在 3 m 量程下贡献 <1 cm;
- 多径:金属构架反射造成前沿偏移,是主要误差源,典型 5~30 cm,且具有系统性(同一构型下可重复);
- 非视距(NLOS):人体遮挡、挂钩被构件挡住,误差可达 0.5 m 以上,必须靠置信度剔除。
对策:首径检测(前沿门限)+ 信号质量指示(RSSI/FPPL)+ 三链路残差检验,残差超限则该帧标记为低置信度,不参与解算。
4. 解算方法
构造非线性最小二乘问题,未知量为 R(3 参数,用李代数 so(3) 参数化)与三点在自身坐标系下的位置(由边长确定形状,尺度固定):
+ λ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 核与条件数导致的非线性放大难以解析,因此用蒙特卡洛验证:
| 参数 | 分布 |
|---|---|
| 测距误差 | 高斯,σ=0.06 m,另叠加 5% 概率的 ±0.4 m 野值(多径) |
| 姿态误差 | 高斯,σ=0.8°(三轴独立) |
| 气压高度 | 高斯,σ=0.05 m,慢漂移 0.02 m/min |
| 时间不同步 | 0~15 ms 随机延迟,人体速度 0.3 m/s |
| L (m) | Δh 真值 (m) | 均值偏差 | 1σ | P95 |误差| | 判定 |
|---|---|---|---|---|---|
| 1.2 | −0.80 | +0.012 | 0.081 | 0.16 | 达标 |
| 1.2 | −0.30 | +0.015 | 0.079 | 0.15 | 达标 |
| 2.5 | −1.50 | +0.021 | 0.094 | 0.19 | 达标 |
| 2.5 | +0.50 | −0.018 | 0.098 | 0.20 | 达标 |
| 0.6 | −0.30 | +0.030 | 0.121 | 0.25 | 边缘 短基线下姿态误差放大 |
| 2.5(NLOS) | −1.00 | +0.085 | 0.210 | 0.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 器件或天线,必须重跑本章的蒙特卡洛与构型实验。