41-EFT.WP.Comms.Navigation v1.0 | 第7章 运动学与动力学：IMU/里程计/视觉

目录 ／ 文档-技术白皮书 ／ 41-EFT.WP.Comms.Navigation v1.0

第7章 运动学与动力学：IMU/里程计/视觉

I. 目标与适用域

• 本章给出载体连续/离散运动学—动力学的最小可用模型、误差状态线性化、IMU 噪声与标定、预积分（因子图）、轮速里程计与视觉观测（单目/双目/深度）的建模与融合接口。
• 公式/符号/定义统一英文并用反引号；若与到达时通道联合（如与 TOA/TDOA/FOA/CP 融合），须沿用既定 T_arr 口径并在数据卡显式记录 gamma(ell) 与 d ell。

II. 状态与过程模型（连续/离散）

• S70-1（状态向量）：x = [ p_WB, v_WB, q_WB, b_g, b_a ]，其中 p_WB ∈ ℝ^3、v_WB ∈ ℝ^3、q_WB ∈ SO(3)、b_g,b_a ∈ ℝ^3。必要时增广 x ← [x, s_vo, Δt_cam, Δt_wheel]（单目尺度与时间偏差）。
• S70-2（连续时间模型）：
  \dot p_WB = v_WB；
  \dot v_WB = R(q_WB) ( a_m - b_a - n_a ) + g_W；
  \dot q_WB = 0.5 · Ω( ω_m - b_g - n_g ) · q_WB；
  \dot b_g = n_{wg}，\dot b_a = n_{wa}。
  其中 a_m, ω_m 为 IMU 量测，n_* 为高斯噪声，Ω(·) 为角速度四元数算子。
• S70-3（离散化·一次保持）：步长 Δt 下
  x_{k+1} ≈ f(x_k,u_k,Δt) = [ p_k + v_k Δt + 0.5( R_k( a_m−b_a ) + g_W )Δt^2; v_k + ( R_k( a_m−b_a ) + g_W )Δt; q_k ⊗ Exp( (ω_m−b_g)Δt ); b_{g,k}; b_{a,k} ]。
  线性化引出转移 F_k 与噪声映射 G_k，过程协方差 Q_k = G_k Q_c G_k^T Δt。

III. 误差状态与线性化（Error-State EKF）

• S70-4（误差参数化）：δx = [ δp, δv, δθ, δb_g, δb_a ]，δθ ≈ 小角向量（SO(3) 误差用左乘）。
• S70-5（误差动力学）：δ\dot x = A · δx + L · n，其中
  A = [[0,I,0,0,0],[0,0,-R( a_m−b_a )×, -R, -I],[0,0,-(ω_m−b_g)×, -I, 0],[0,0,0,0,0],[0,0,0,0,0]]（示意），n = [n_a,n_g,n_{wa},n_{wg}]。
• S70-6（离散误差传播）：P_{k+1} = Φ_k P_k Φ_k^T + Q_k，Φ_k = expm(A_k Δt)。

IV. IMU 噪声、稳定度与标定

• S70-7（噪声模型）：角速度/加速度测量噪声密度 σ_g, σ_a 与偏置随机游走 σ_{wg}, σ_{wa}；通过 Allan 偏差 σ_y(τ) 估计 Q_c。
• S70-8（温度/非正交/尺度）：ω_true = M_g ω_m + o_g + T_g(T)，a_true = M_a a_m + o_a + T_a(T)，标定矩阵 M_*、零偏 o_* 与温度项 T_* 进入系统项。
• M7-1（IMU 标定流程）：静置/旋转台/多姿态数据 → 估计 M_g,M_a,o_g,o_a,σ_* → 生成校准与噪声卡。

V. 预积分（因子图/VIO）

• S70-9（预积分量）：在窗口 [k,k+1] 上定义 ΔR, Δv, Δp 及其协方差与对 b_g,b_a 的雅可比；偏置变化以一阶修正。
• S70-10（预积分残差）：
  r_ΔR = Log( (ΔR_meas)^{-1} · ( R_k^T R_{k+1} ) )，
  r_Δv = R_k^T ( v_{k+1} - v_k - g_W Δt ) - Δv_meas - J_{vg} δb_g - J_{va} δb_a，
  r_Δp = R_k^T ( p_{k+1} - p_k - v_k Δt - 0.5 g_W Δt^2 ) - Δp_meas - J_{pg} δb_g - J_{pa} δb_a。
• M7-2（预积分生成）：输入 IMU stream, calib, σ_* → 累积 ΔR,Δv,Δp,Σ_Δ 与雅可比，写入因子图。

VI. 轮速里程计（Wheel Odometry）

• S70-11（量测模型）：y_w = V_B + ε_w 或差速模型 y_w = [v_x, ω_z]^T + ε；与地面约束 v_y≈0、无侧滑假设绑定。
• S70-12（残差）：r_w = V_B(x) − y_w，V_B(x) = [ (R_WB^T v_WB)_x, (ω_B)_z ]^T。
• S70-13（系统项）：轮径/齿比/同步误差→尺度因子 s_w 与偏置 b_w；滑移用混合似然或 Huber/Tukey 再加权。

VII. 视觉观测（VO/VIO）

• S70-14（投影模型）：针孔 u = Π( K · ( R_BC ( R_WB^T ( P_W − p_WB ) ) + t_BC ) )，K 为内参，T_BC=[R_BC,t_BC]。
• S70-15（特征残差）：r_uv = u_meas − Π(·)；对 x, T_BC, K 求雅可比；滚动快门用时间行模型修正。
• S70-16（尺度与时间偏差）：单目引入 s_vo；相机—IMU 时间偏差 Δt_cam 进入重投影时间轴。
• M7-3（外参与时间标定）：手—眼/自标定/时序对齐 → 估计 T_BC, Δt_cam 与不确定度。

VIII. 融合框架与数值实现

• S70-17（滤波/平滑）：误差状态 EKF/UKF 与因子图（滑窗 MAP）；IMU 预积分因子 + 视觉/里程计/几何通道因子联合优化。
• S70-18（边缘化与窗口管理）：Schur 边缘化旧关键帧，保留先验；数值采用稀疏列最小化与鲁棒核。
• M7-4（融合管线）：ingest → time_sync → imu_propagate → factors_build → optimize → out_states；产生 x̂, P 或 trajectory + cov。

IX. 与第4章/第5章/第6章的耦合要点

• S70-19（通道联合）：TOA/TDOA/AOA/FOA/CP 残差与本章先验共同构成 H, Σ_y；FOA 对高度可观测性增益与第5章一致。
• S70-20（同步先验并入）：第6章输出的 cov([b_t,\dot b_t]) 并入状态或观测协方差。
• S70-21（几何设计）：基于第5章 GDOP/cond(F) 指标选择观测几何与关键帧策略。

X. 数据契约（本章必备/推荐字段）

unit_system: "SI"

imu:

sigmas: {sigma_g: "<rad/s/√Hz>", sigma_a: "<m/s^2/√Hz>", sigma_wg: "<rad/s^2/√Hz>", sigma_wa: "<m/s^3/√Hz>"}

calib: {Mg: "<3x3>", Ma: "<3x3>", og: "<3>", oa: "<3>", temp_model: "<optional>"}

gravity_W: [0, 0, -9.80665]

wheel:

model: "vx_omega|full_kinematic"

scale: "<s_w>", bias: "<b_w>", slip_flag: "<bool|score>"

vision:

camera: {K: "<3x3>", dist: "<coeffs>", model: "pinhole|fisheye"}

extrinsics: {T_BC: "<SE3>", cov: "<6x6>"}

timing: {dt_cam: "<s>", mode: "global|rolling"}

state0:

p_WB: "<m>", v_WB: "<m/s>", q_WB: "<w,x,y,z>", bg: "<rad/s>", ba: "<m/s^2>"

fusion:

window: {size: n, strategy: "keyframe|fixed-lag"}

robust: {loss: "Huber|Cauchy", params: {...}}

covariance:

Qc: "<process PSD>", P0: "<prior>", Σ_meas: "<block-diagonal or sparse>"

references:

- "EFT.WP.Comms.Navigation v1.0:Ch.2 S20-*"

- "EFT.WP.Comms.Navigation v1.0:Ch.4 S40-*"

- "EFT.WP.Comms.Navigation v1.0:Ch.5 S50-*"

- "EFT.WP.Comms.Navigation v1.0:Ch.6 S60-*"

XI. 实现绑定（接口原型）

• I7-1 imu_propagate(x, P, imu_stream, calib, dt) -> {x_pred, P_pred}
• I7-2 imu_preintegrate(imu_stream, calib) -> {ΔR, Δv, Δp, Σ_Δ, J_Δ}
• I7-3 build_wheel_factor(y_w, model, cov) -> {factor_w}
• I7-4 build_vision_factor(tracks, K, T_BC, timing) -> {factors_vo}
• I7-5 calibrate_extrinsics(sync_data) -> {T_BC, Δt_cam, cov}
• I7-6 synchronize_timestamps(streams, mode) -> {aligned_streams, Δt}
• I7-7 run_filter(init, factors, priors) -> {x̂, P}
• I7-8 run_smoother(window, factors, priors) -> {traj, cov}

XII. 质量门（本章适用）

• Q1 量纲/单位：check_dim 通过；rad, m, s, m/s, m/s^2 等 SI 列完整。
• Q2 噪声一致性：σ_g, σ_a, σ_{wg}, σ_{wa} 与 Allan 拟合一致；过程噪声 Q_c 与离散 Q_k 一致。
• Q3 外参/时间：T_BC, Δt_cam, Δt_wheel 的估计与不确定度在数据卡中显式记录并在因子图中更新。
• Q4 预积分正确性：ΔR,Δv,Δp 的协方差与雅可比在偏置变化时做一阶修正；数值稳定（避免奇异）。
• Q5 鲁棒性：轮滑/遮挡/滚动快门等异常须通过 nlos/slip 标志或稳健损失进入优化；边缘化先验的条件数受控。

XIII. 跨卷引用与本章锚点

1. 跨卷引用（固定写法）：见本卷第2章（术语/计量基线）、第4章（观测模型）、第5章（几何与 GDOP）、第6章（同步链）。
2. 本章锚点：
   • 最小方程：S70-1—S70-21
   • 流程：M7-1—M7-4
   • 接口：I7-1—I7-8

XIV. 小结
本章给出了从连续/离散动力学、误差状态、IMU 噪声与标定到预积分与里程计/视觉观测的端到端建模与实现绑定。通过数据契约、质量门与统一接口，可将运动学先验稳定注入融合框架，并与第4–6章的几何/同步/通道观测协同，支撑第8章与第10章的融合估计与实验设计。