这一篇继续以训练好的 TienKung 人形机器人为例，在 Isaac Lab 中加入一个 IMU 传感器，然后手动给理想测量叠加陀螺仪固定零偏、陀螺仪白噪声和加速度计白噪声，最后对比融合互补滤波、普通卡尔曼滤波和误差状态卡尔曼滤波三种姿态估计方法。

因为本来想验证一下卡尔曼滤波，但是实现过程中发现欧拉角三个方向的旋转有耦合，所以调研到了ESKF的方法，这里由于时间原因没有细纠，这部分还有待仔细推导，先埋个点[TODO]

当前实验使用三轴角速度和三轴加速度，没有磁力计或其他绝对航向观测。因此Roll和Pitch可以利用重力方向进行校正，Yaw主要依靠陀螺仪传播，长时间运行仍然可能出现漂移。

需要先说明，这里新增的三种滤波器只用于学习、绘图和对比，不会把滤波结果传给强化学习策略。策略使用的仍然是环境原来定义的 observation，因此这一部分不会改变训练好的控制策略。

Isaac Lab IMU 输出

首先在 play_kaleman.py 中临时继承原来的场景配置，然后给机器人 pelvis 部分加入一个 IMU。这样不会修改原来的训练场景配置。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
class PlayKalmanSceneCfg(base_scene_cfg):
    def __init__(self, *args, **kwargs):
        super().__init__(*args, **kwargs)
        self.pelvis_imu = ImuCfg(
            prim_path="{ENV_REGEX_NS}/Robot/pelvis",
            update_period=env_cfg.sim.dt,
            history_length=1,
            gravity_bias=(0.0, 0.0, GRAVITY_M_S2),
            offset=ImuCfg.OffsetCfg(
                pos=IMU_OFFSET_POS,
                rot=IMU_OFFSET_ROT_WXYZ,
            ),
            debug_vis=False,
        )

当前 IMU 相对于 pelvis 的位置偏移为0，旋转偏移为单位四元数，因此 IMU 坐标系与 pelvis 坐标系重合。

程序实际读取：

1
2
3
4
5
6
7
imu_data = env.scene["pelvis_imu"].data

latest_q_truth = quat_normalize(
    imu_data.quat_w[0].detach().cpu().to(torch.float64)
)
gyro_true = imu_data.ang_vel_b[0].detach().cpu().to(torch.float64)
accel_true = imu_data.lin_acc_b[0].detach().cpu().to(torch.float64)

quat_w 是 IMU 在世界坐标系中的姿态四元数，顺序为 WXYZ。它属于仿真提供的完整状态信息，真实六轴 IMU 不会直接提供这样的无误差姿态。

ang_vel_b 是当前 IMU/机体系下的三轴角速度，lin_acc_b 是当前 IMU/机体系下的三轴线加速度。Isaac Lab 2.1.0 中，世界系线加速度由速度数值差分并加入 gravity_bias 得到：

当前：

随后再把结果旋转到 IMU 坐标系。因此机器人静止时，加速度计输出接近机体系中的重力反方向；机器人运动时，输出还会包含 pelvis 的真实平动加速度，并不等于纯粹的重力投影。

传感器误差

当前 Isaac IMU 没有配置随机误差，所以读取理想输出后，再手动构造带误差的传感器测量：

其中 $b_g$ 是程序启动时随机生成的三轴固定陀螺仪零偏，$n_g$ 和 $n_a$ 分别是陀螺仪与加速度计白噪声。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
class SensorErrorModel:
    def measure(
        self,
        gyro_true: torch.Tensor,
        accel_true: torch.Tensor,
    ) -> tuple[torch.Tensor, torch.Tensor]:
        gyro_noise = (
            torch.randn(3, generator=self.generator, dtype=torch.float64)
            * self.gyro_noise_std
        )
        accel_noise = (
            torch.randn(3, generator=self.generator, dtype=torch.float64)
            * ACCEL_WHITE_NOISE_STD_M_S2
        )
        return (
            gyro_true + self.gyro_bias + gyro_noise,
            accel_true + accel_noise,
        )

本次实验中，三轴固定零偏分别独立采样于：

陀螺仪白噪声标准差为：

加速度计白噪声标准差为：

当前没有给加速度计加入固定零偏，也没有让真实注入的陀螺仪零偏随时间变化。

初始姿态

三个滤波器在开始更新前都需要一个初始姿态。当前主实验使用仿真 quat_w 在初始化时赋值一次：

1
2
3
4
5
q_initial = latest_q_truth.clone()

complementary.reset(q_initial)
eskf.reset(q_initial)
kalman.reset(q_initial)

这样做是为了先排除重力初始对准误差，只比较后续传播和校正效果。初始化完成后，quat_w 不再参与三个滤波器的预测和观测更新，只用于绘制 Truth 曲线和计算误差。

当前代码仍然保留了只依赖六轴 IMU 的重力对准选项。重力对准会利用一段时间内的陀螺仪相对旋转，把各帧加速度搬到同一个参考坐标系后求平均，再得到Roll和Pitch初值；Yaw初值只能人为规定。真实机器人上不能使用仿真姿态真值，应当采用这一类重力对准或其他外部姿态来源。

机器人控制频率为50 Hz，物理仿真与 IMU 更新频率为200 Hz。为了让三个滤波器按 IMU 频率更新，在环境的每个 physics 子步中加入一个默认关闭的回调：

1
self.physics_step_callback: Callable[[float], None] | None = None

回调放在 scene.update() 之后：

1
2
3
4
5
6
7
8
for _ in range(self.cfg.sim.decimation):
    self.robot.set_joint_position_target(processed_actions)
    self.scene.write_data_to_sim()
    self.sim.step(render=False)
    self.scene.update(dt=self.physics_dt)

    if self.physics_step_callback is not None:
        self.physics_step_callback(self.physics_dt)

因此三个滤波器实际使用：

而不是50 Hz控制周期对应的 $0.02s$。

进入滤波器前，加速度还会经过一阶低通：

当前时间常数为 $\tau=0.4s$。只有原始加速度和低通加速度的模长都接近 $9.81m/s^2$ 时，才允许把当前加速度方向用于姿态校正。

当前 physics 回调中的主要计算为：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
gyro_measured, accel_measured = sensor_error.measure(
    gyro_true, accel_true
)

low_pass_alpha = 1.0 - math.exp(
    -physics_dt / ACCEL_CORRECTION_LOW_PASS_TIME_CONSTANT_S
)
filtered_accel += low_pass_alpha * (
    accel_measured - filtered_accel
)
accel_valid = _accel_gate(accel_measured, filtered_accel)

complementary.update(
    gyro_measured, filtered_accel, physics_dt, accel_valid
)
eskf.update(
    gyro_measured, filtered_accel, physics_dt, accel_valid
)
kalman.update(
    gyro_measured, filtered_accel, physics_dt, accel_valid
)

普通卡尔曼滤波

普通卡尔曼滤波使用六维状态：

其中 $\phi$、$\theta$、$\psi$ 分别是Roll、Pitch、Yaw，$b_x$、$b_y$、$b_z$ 是三轴陀螺仪零偏估计。

输入是三轴陀螺仪：

加速度计只能给出Roll和Pitch观测：

状态空间方程

离散状态空间方程为：

其中：

展开后为：

这里的零偏预测模型确实是“当前零偏等于上一时刻零偏”。但是状态矩阵右上角的 $-\Delta t$ 会让零偏误差经过积分变成姿态误差，这一点后面会用于根据姿态残差修正零偏。

代码中对应：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
transition_matrix = torch.eye(6, dtype=self.dtype)
transition_matrix[0, 3] = -dt
transition_matrix[1, 4] = -dt
transition_matrix[2, 5] = -dt

control_matrix = torch.zeros((6, 3), dtype=self.dtype)
control_matrix[0, 0] = dt
control_matrix[1, 1] = dt
control_matrix[2, 2] = dt

predicted_state = (
    transition_matrix @ self.state
    + control_matrix @ gyro
)

观测方程

观测方程为：

其中：

后三列为0，表示加速度计没有直接测量陀螺仪零偏。但状态预测会在姿态误差和零偏误差之间建立交叉协方差，因此卡尔曼增益中对应 $b_x$、$b_y$ 的行仍然可以不为0。

可以把它理解为：如果 $b_y$ 估计不准确，那么扣除零偏后的Y轴角速度就不准确，经过积分后逐渐表现为Pitch误差；加速度计发现持续存在的Pitch残差后，再通过状态之间的相关性反向修正 $b_y$。

当前普通卡尔曼滤波中，Yaw与Roll/Pitch完全解耦，加速度计又不观测Yaw，所以 $b_z$ 没有校正通道，会保持初值0。

Q和R

过程噪声协方差为：

前三项表示陀螺仪白噪声经过一个采样周期积分后形成的角度误差，后三项表示零偏随机游走。

观测噪声协方差为：

当前普通卡尔曼滤波设置：

这里的 $R$ 不能只按照加速度计白噪声 $0.05m/s^2$ 直接换算，因为机器人运动时还存在没有建模的平动加速度和步态冲击。

滤波更新仍然沿用上一篇文章中的标准卡尔曼滤波公式，代码使用 Joseph Form 更新协方差：

1
2
3
4
5
joseph_left = self.identity - kalman_gain @ self.observation_matrix
updated_covariance = (
    joseph_left @ predicted_covariance @ joseph_left.T
    + kalman_gain @ self.measurement_noise @ kalman_gain.T
)

代码中的变量 innovation 对应中文文献里常说的“新息”：

而：

称为新息协方差矩阵。当前代码利用 $y_k^TS_k^{-1}y_k$ 做NIS门控，拒绝明显不符合当前预测的加速度观测。

普通卡尔曼滤波内部始终使用欧拉角状态。四元数只在初始化时转换成欧拉角，并在滤波结束后把欧拉角重新转换成四元数，用于统一绘图接口。

ESKF

普通卡尔曼滤波直接把欧拉角作为状态，并且把机体系陀螺仪三轴近似当成Roll、Pitch、Yaw的变化率。ESKF则用四元数保存完整姿态，再对当前四元数附近的小误差进行线性卡尔曼滤波。

ESKF是Error-State Kalman Filter，即误差状态卡尔曼滤波。当前实现的名义状态为：

误差状态为：

姿态误差采用右乘定义：

这里要求较小的是估计误差 $\delta\theta$，不是机器人的实际姿态角。机器人即使已经发生较大转动，只要当前名义四元数与真实姿态接近，误差状态仍然可以使用小角度线性化。

名义状态传播

首先从陀螺仪测量中扣除当前零偏估计：

然后传播四元数：

代码为：

1
2
3
4
5
6
7
corrected_gyro = gyro - self.gyro_bias
self.q = quat_normalize(
    quat_multiply(
        self.q,
        quat_exp(corrected_gyro * dt),
    )
)

误差状态方程

连续时间误差模型为：

一阶离散化后：

其中：

右上角的 $-I_3\Delta t$ 与普通卡尔曼滤波中的 $-\Delta t$ 含义相同：零偏误差会经过陀螺仪积分变成姿态误差，从而建立姿态与零偏之间的交叉协方差。

代码为：

1
2
3
4
5
6
7
8
transition = torch.eye(6, dtype=torch.float64)
transition[:3, :3] -= skew(corrected_gyro) * dt
transition[:3, 3:] = -torch.eye(3, dtype=torch.float64) * dt

self.p = (
    transition @ self.p @ transition.T
    + process_noise
)

观测方程

ESKF不先把加速度换成Roll和Pitch，而是直接使用归一化加速度方向：

当前四元数预测的机体系重力方向为：

非线性观测模型为：

在当前名义四元数附近线性化后：

代码中对应：

1
2
3
4
5
6
7
8
9
measurement = accel / accel_norm
predicted = quat_rotate_inverse(
    self.q,
    torch.tensor([0.0, 0.0, 1.0], dtype=torch.float64),
)
residual = measurement - predicted

observation = torch.zeros((3, 6), dtype=torch.float64)
observation[:, :3] = skew(predicted)

观测矩阵的零偏部分依然为0。零偏能够更新，仍然依靠预测阶段形成的姿态—零偏交叉协方差，而不是加速度计直接测出了零偏。

当前加速度方向噪声标准差设置为 $1^\circ$。ESKF同样计算新息协方差矩阵与NIS，当加速度方向明显不符合当前姿态预测时，跳过这一帧校正。

误差注入

得到误差状态估计后：

姿态误差通过右乘注入名义四元数：

零偏误差直接加到名义零偏：

1
2
3
4
5
6
7
delta_state = kalman_gain @ residual
delta_theta = delta_state[:3]

self.q = quat_normalize(
    quat_multiply(self.q, quat_exp(delta_theta))
)
self.gyro_bias = self.gyro_bias + delta_state[3:]

误差已经注入名义状态后，误差状态重新定义为0，并使用reset Jacobian把协方差转换到新的误差坐标系：

这就是当前实现与普通全状态EKF最明显的区别：ESKF维护名义四元数，但卡尔曼滤波处理的是局部姿态误差与零偏误差。

单个重力方向只能提供两个独立约束，绕重力方向旋转不会改变观测。因此仅靠当前六轴 IMU 无法获得绝对Yaw，Yaw以及对应的零偏估计仍然缺少充分约束。

融合互补滤波

融合互补滤波同样使用四元数传播姿态，但不维护协方差，也不估计陀螺仪零偏。

加速度给出的机体系上方向为：

当前四元数预测的机体系上方向为：

两个方向的叉积近似为姿态误差：

然后将比例校正加入角速度：

最后传播四元数：

当前 $k_p=2.0s^{-1}$。这个方法结构简单，短时间姿态跟踪效果也可以很好，但固定陀螺仪零偏仍然会造成长期漂移。

结果对比

image-20260718184539322

卡尔曼2. 00_00_00-00_00_30

在保存的一次12秒运动记录中，机器人前向速度命令为 $0.4m/s$，Yaw角速度命令为 $0.5rad/s$。ESKF的Roll/Pitch RMSE约为 $0.338^\circ/0.390^\circ$，互补滤波约为 $0.779^\circ/0.727^\circ$，普通卡尔曼滤波约为 $1.097^\circ/0.491^\circ$。

补充

加速度计

实际六轴 IMU 的加速度计测量的是比力。按照本文坐标约定：

机器人静止时 $a_W=0$，所以加速度计主要反映重力方向；机器人运动时 $a_W\neq0$，测量中同时存在平动加速度。

通常可以检查：

是否足够小，作为加速度观测是否相对可信的一个条件。但模长接近 $g$ 并不能证明方向一定等于重力方向，这也是动态机器人姿态估计比静止IMU更困难的地方。

四元数

四元数写成：

符号 $\otimes$ 表示四元数乘法，也称为 Hamilton product。它不是普通数字乘法、矩阵对应元素相乘或单独的向量点乘、叉乘，而是专门用于组合两个四元数的运算。

设：

那么：

如果把四元数写成标量部分和向量部分：

同一个运算也可以写成：

可以看到，Hamilton积内部同时包含标量乘法、向量点积和向量叉积，但整体并不等于其中任何一种运算。

四元数乘法的物理意义是组合旋转。按照本文使用的主动旋转约定：

因此旋转顺序不能随意交换。一般情况下：

当前代码中的四元数表示机体系到世界系的旋转，陀螺仪角速度又在机体系表达，所以采用右乘更新：

其中 $\delta q_k$ 是当前采样周期内的机体系增量旋转。

接下来具体看 $\delta q_k$ 如何由陀螺仪得到。陀螺仪给出机体系角速度：

在一个采样周期 $\Delta t$ 内，对应的旋转向量为：

旋转角度是旋转向量的模：

展开为：

这里得到的 $\Delta\theta$ 是一个标量，单位为弧度。

当 $\Delta\theta\neq0$ 时，旋转轴是旋转向量的单位方向：

展开为：

并且：

已知旋转角 $\Delta\theta$ 和单位旋转轴 $\mathbf u$ 后，增量四元数为：

也就是：

一个具体例子

为了把旋转向量、增量四元数和Hamilton积连起来，先假设机器人从单位姿态开始：

第一个旋转是在机体系Z轴旋转 $90^\circ$。对应旋转向量为：

所以旋转角和旋转轴分别为：

得到增量四元数：

更新姿态：

接着再绕当前机体系X轴旋转 $90^\circ$：

按照当前代码的右乘约定：

代入Hamilton积：

如果交换顺序：

两个结果不同，这就说明四元数乘法不能交换顺序。

最后再看向量旋转。假设当前机体系中指向机器人前方的向量为：

四元数 $q$ 表示机体系到世界系的旋转，那么该向量在世界系中的方向为：

这就是互补滤波和ESKF中陀螺仪传播四元数的核心计算。

欧拉角速度耦合

以常见的ZYX欧拉角为例：

其中 $\psi$、$\theta$、$\phi$ 分别为Yaw、Pitch、Roll。

注意，这三个旋转轴不是同一套坐标系里的三个固定轴。按照ZYX欧拉角的旋转顺序：

  • Yaw $\psi$：绕世界坐标系的 $z_W$ 轴旋转；
  • Pitch $\theta$：绕完成Yaw旋转后的中间坐标系 $y$ 轴旋转；
  • Roll $\phi$：绕完成Yaw和Pitch旋转后的最终机体系 $x_B$ 轴旋转。

也就是说，Yaw、Pitch和Roll分别对应三个不同阶段的坐标轴，这些轴会随着前面的旋转发生变化。陀螺仪输出的 $p,q,r$ 则是同一个瞬时角速度向量在当前机体 $x_B,y_B,z_B$ 三轴上的分量，而 $\dot\phi,\dot\theta,\dot\psi$ 是三个欧拉角参数的变化率。两组三维量的物理含义并不相同，这就是欧拉角速度产生耦合的根本原因。

两者满足：

展开为：

反过来：

因此普通卡尔曼滤波中：

只是Roll和Pitch较小时的近似。当前普通卡尔曼滤波仍然能够工作，是因为实验使用了准确初始姿态、200 Hz更新以及持续的加速度校正，并不表示这个欧拉角传播模型在任意三维姿态下都严格成立。