状态估计

“机器人不能直接感知自己的完整状态——它只能感知部分观测，然后从噪声数据中推断真实状态在哪里。”

具身智能体在物理流形 $\mathcal{M}_{phy}$ 上运动，但传感器返回的是带噪声的局部观测 $z_k$，而非完整状态 $x$。状态估计是 Spine 层的核心计算之一：在 1kHz 控制频率下，持续从噪声观测中重建系统在 $\mathcal{M}_{phy}$ 上的最优状态估计 $\hat{x}(t)$，为 CBF 安全监控、STL 约束验证和动力学控制提供统一的状态输入。

📐 理论定位

线性卡尔曼滤波（Kalman 1960）在高斯噪声与线性系统下给出最优无偏估计；EKF（Extended Kalman Filter） 通过对非线性函数的一阶线性化处理，将这一框架推广到机器人系统的非线性动力学——机械臂 DAE、接触动力学、SE(3) 位姿更新均属此类。EKF 自 1960 年代起在航空航天（阿波罗导航计算机）和机器人领域大量应用，是工业成熟的工程工具（△ 推广自 Kalman 1960）。

EICPS 的工程贡献在于：将 EKF 置于 1kHz Spine 层，以 ZOH（零阶保持） 填补 1Hz Brain 指令之间的频率鸿沟，并集成 PINN 物理先验 补偿动力学不确定性，形成可闭环运行的跨频状态估计架构。

状态估计问题

机器人系统的状态空间模型由两个方程描述：

其中 $w_k \sim \mathcal{N}(0, Q_k)$ 为过程噪声，$v_k \sim \mathcal{N}(0, R_k)$ 为观测噪声，$f$ 为非线性动力学（机械臂惯性方程、接触模型），$h$ 为非线性观测模型（正向运动学、IMU 模型）。

状态向量 $x$ 在 Prj167 中包含：关节角 $q \in \mathbb{R}^n$、关节速度 $\dot{q}$、末端位姿 $T \in SE(3)$、接触力估计 $\hat{f}_{ext}$。完整状态维度通常在 20–60 之间。

不可缺少的理由：Brain 层（1Hz）发出语义指令时，Spine 层（1kHz）无法”停下来等”——在两条 Brain 指令之间的约 1000 个控制周期内，系统持续运动，CBF 安全监控需要每毫秒知道机器人的精确状态。状态估计正是填补这一空白的计算引擎。

EKF 算法：预测–更新循环

EKF 将非线性函数 $f$、$h$ 在当前估计点处做一阶泰勒展开（Jacobian 线性化），然后在线性化模型上运行标准卡尔曼滤波。

预测步（Predict）——利用动力学模型向前传播：

其中 $F_k = \partial f/\partial x\big|_{\hat{x}_{k-1|k-1}}$ 为系统方程的 Jacobian，$P$ 为误差协方差矩阵。

更新步（Update）——利用传感器观测修正预测：

其中 $H_k = \partial h/\partial x\big|_{\hat{x}_{k|k-1}}$ 为观测方程的 Jacobian，$K_k$ 为卡尔曼增益——它自动在模型预测与传感器观测之间分配信任权重：$Q_k$ 大（模型不确定）则更信传感器，$R_k$ 大（传感器噪声高）则更信模型。

SE(3) 上的位姿更新：机械臂末端位姿 $T \in SE(3)$ 不是欧氏向量，不能直接做加减法。EKF 在李代数 $\mathfrak{se}(3)$ 上计算增量，再通过指数映射回 $SE(3)$：

强行在欧氏空间对旋转矩阵做加减会破坏正交性，产生”非物理位姿”——这是 $\mathcal{M}_{phy}$ 是真正黎曼流形而非 $\mathbb{R}^n$ 的工程后果。

跨频状态重建：ZOH 与异步融合

Brain 层（~1Hz）和 Spine 层（~1kHz）之间存在 1000:1 的频率鸿沟。Spine 层通过两种机制处理：

零阶保持（ZOH）：当 Brain 无新指令时，Spine 保持最后一次收到的参考轨迹继续执行，不等待。同时 EKF 以 1kHz 继续更新 $\hat{x}$，确保安全监控不断链。

异步传感器融合：各传感器的采样频率不同——编码器通常 1kHz，IMU 500Hz，力觉传感器 200Hz，视觉 30Hz。EKF 采用时间戳驱动更新：哪个传感器数据到达，就用哪个传感器的 $H_k$ 和 $R_k$ 执行更新步，其余时刻只做预测步。这保证每个传感器的时序信息都被充分利用，而不是对齐到最低采样率。

融合结果经接口 A 压缩上报 Brain：Brain 侧看到的是结构化状态摘要（位姿、速度、力矩裕量、安全余量），而非原始 1kHz 高频信号。

PINN 物理先验

EKF 的预测步依赖动力学模型 $f$。精确建模机器人动力学（质量矩阵、摩擦系数、接触刚度）需要大量标定工作，且参数会随磨损、温度、负载变化漂移。

**PINN（Physics-Informed Neural Network）**作为动力学先验：用神经网络 $f_\theta$ 近似系统动力学，但在损失函数中嵌入物理约束（能量守恒、牛顿定律、接触互补条件），确保网络输出在物理上一致。

优势：在训练数据稀少的操作边界（大负载、极端姿态）处，物理约束项提供正则化，防止网络在未见工况下外推失效——正是 Prj167 防振锤更换这类极端工况所需要的。

Prj167 中的状态估计

500kV 架空输电线路运检场景带来两类特殊挑战：

动态负载变化：防振锤更换过程中，机械臂末端负载在 0–15kg 之间大幅变化。质量矩阵 $M(q)$ 随之改变，导致 $f$ 的 Jacobian $F_k$ 在每个控制周期都不同。EKF 需要实时更新 $F_k$；PINN 先验通过在线微调（few-shot adaptation）补偿参数漂移，使估计误差保持在 5mm / 0.5° 以内。

强电磁干扰（EMI）：500kV 高压线产生的强电磁场影响编码器和 IMU 读数，表现为高频拖尾（噪声方差 $R_k$ 突增）。Spine 层通过 Weyl 定律检验（见下节）实时监控信噪比；一旦检出异常，自动调高受影响传感器的 $R_k$（降低卡尔曼增益），转而增大力觉传感器的贡献。这一自适应权重策略在强干扰区域仍可维持稳定的状态估计。

电气安全裕量估计：EKF 估计的末端位姿 $\hat{T} \in SE(3)$ 直接输入 CBF 计算 $h(x) = d(\hat{T}, \text{wire}) - d_{safe}$。位姿估计误差 $\sigma_{pos}$ 与安全裕量的关系：Spine 层在 CBF 约束中加入 $3\sigma_{pos}$ 保守裕量，确保即使估计有误差，实际距离也满足 $d_{safe}$。

传感器健康诊断：Weyl 定律检验

状态估计质量的上限由传感器信噪比决定。Spine 层通过Weyl 定律一致性检验持续监控传感器健康，无需额外传感器：

光滑流形上的谱系数服从幂律衰减（Weyl 定律）：

高频拖尾诊断：若高频段出现能量”平台”而非预期的指数衰减，即为加性白噪声信号——其功率谱密度在全频带均匀注入（$|\hat{\eta}_k|^2 \approx \sigma^2 = \text{const}$），使高频斜率 $\alpha$ 接近零。

传感器盲区诊断：Prj167 中导线可能遮挡摄像头视野，等效于在流形上”挖洞”（Dirichlet 零边界条件），导致 TDA 将盲区识别为”伪环路”（$\beta_1$ 虚假增加）。区分盲区与真实障碍的依据是持久性——盲区产生的拓扑特征随视角移动剧烈变化，真实障碍物的特征是稳定的。

诊断算法：

import numpy as np

def detect_sensor_noise(eigenvalues: np.ndarray,
                        spectral_coeff: np.ndarray,
                        alpha_thresh: float = 0.5) -> dict:
    """
    Weyl 定律失效检测：高频拖尾诊断
    eigenvalues:    LB 算子特征值序列（升序）
    spectral_coeff: 对应的谱能量 |f_hat_k|^2
    """
    k_start = len(eigenvalues) // 2        # 高频区从中点开始
    lam_high    = np.log(eigenvalues[k_start:] + 1e-12)
    energy_high = np.log(spectral_coeff[k_start:] + 1e-12)
    # 对数-对数线性回归，斜率即衰减指数 α
    coeffs   = np.polyfit(lam_high, energy_high, 1)
    alpha_fit = -coeffs[0]                 # 正值 = 快速衰减（健康）
    return {
        "alpha":       float(alpha_fit),
        "noisy":       alpha_fit < alpha_thresh,  # 斜率过小 → 噪声
        "noise_floor": float(np.exp(coeffs[1])),
    }

def adaptive_R_update(R_nominal: np.ndarray,
                      sensor_health: dict,
                      scale_noisy: float = 10.0) -> np.ndarray:
    """根据噪声诊断自适应调整观测噪声协方差 R"""
    if sensor_health["noisy"]:
        return R_nominal * scale_noisy   # 降低卡尔曼增益
    return R_nominal

🔢 完整实现：EKF 跨频调度器（Predict 1kHz / Update 按传感器触发）、PINN 在线微调、SE(3) 位姿更新（李代数增量）的 Python 实现见 系统部署架构。

→ 实时安全监控（CBF）（EKF 输出直接输入 CBF 的 $h(x)$ 计算）

→ 数学讲义·流形是什么 · 李群与 SE(3)

参考文献