腿式机器人 IMU 与关节融合及状态估计实战

2. 静态初始对准

initalign.py文件实现了静态初始对准过程。通过采集一段时间内的 IMU 和传感器数据，我们可以计算出初始姿态（滚转角、俯仰角）、陀螺仪零偏以及足端位置。这一步对于 EKF 的状态初始化至关重要，能确保惯性导航系统在启动时的状态估计准确。

import numpy as np
from typing import Iterable

def initStaticAlignment(
    ekf: EKF, 
    imu_stream: Iterable[IMU], 
    sensor_stream: Iterable[RobotSensor], 
    paras: Paras,
):
    """静态初始对准函数
    通过采集 IMU 和机器人传感器数据计算初始姿态、陀螺仪零偏和足端位置
    """
    # 跳过 IMU 数据直到达到起始时间
    imu_cur = next(imu_stream)
    while imu_cur.timestamp < paras.starttime:
        imu_cur = next(imu_stream)
    
    # 跳过传感器数据直到达到起始时间
    sensor_cur = next(sensor_stream)
    while sensor_cur.timestamp < paras.starttime:
        sensor_cur = next(sensor_stream)
    
    t_end = paras.starttime + paras.initAlignmentTime
    
    # 累加 IMU 数据用于计算平均值
    k = 0
    gyro_sum = np.zeros(3)
    acc_sum = np.zeros(3)
    
    while ekf.timestamp() < t_end:
        ekf.addImuData(imu_cur)
        gyro_sum += imu_cur.angular_velocity
        acc_sum += imu_cur.acceleration
        k += 1
        imu_cur = next(imu_stream)
    
    # 计算陀螺仪和加速度计的平均值
    gyro_mean = gyro_sum / max(k, 1)
    acc_mean = acc_sum / max(k, 1)
    
    # 通过加速度平均值计算初始滚转角和俯仰角
    roll = np.arctan2(-acc_mean[1], -acc_mean[2])
    pitch = np.arctan2(acc_mean[0], np.sqrt(acc_mean[1]**2 + acc_mean[2]**2))
    
    # 累加足端位置用于计算平均
    j = 0
    footpos_in_body = np.zeros((3, 4))
    
    while sensor_cur.timestamp < t_end:
        ekf.addSensorData(sensor_cur)
        for leg in range(4):
            ekf.computeRelFootPosVel(sensor_cur, leg)
            footpos_in_body += ekf.getfoot_pos_rel()
        j += 1
        sensor_cur = next(sensor_stream)
    
    # 计算平均足端位置
    footpos_in_body /= max(j, 1)
    
    # 将初始滚转角和俯仰角转为方向余弦矩阵
    Cbn0 = euler2cbn(np.array([roll, pitch, 0.0], dtype=np.float64))
    footpos_in_body = Cbn0 @ footpos_in_body
    
    # 转到导航坐标系
    # 将计算结果设置到 EKF
    ekf.setInitFootPos(footpos_in_body)
    ekf.setInitGyroBias(gyro_mean)
    ekf.setInitAttitude(roll, pitch)

3. 扩展卡尔曼滤波（EKF）的核心算法

ekf.py实现了 Unitree Go2 机器人的扩展卡尔曼滤波核心逻辑，用于融合 IMU 与关节传感器数据，估计机器人的位姿（位置、速度、姿态）以及 IMU 偏差和缩放误差。整个流程涵盖了状态初始化、IMU 数据处理、足端位置与速度计算、EKF 状态预测与测量更新、反馈修正以及协方差管理。

3.1 常量定义与初始化

首先定义 EKF 所需的常量、状态索引、噪声索引等基础结构，并初始化协方差矩阵和噪声矩阵。

def square(x: float | np.ndarray) -> float | np.ndarray:
    return x * x

# ------------ 状态索引 ------------
P_ID, V_ID, PHI_ID = 0, 3, 6
BG_ID, BA_ID, SG_ID, SA_ID = 9, 12, 15, 18
FL_ID, FR_ID, RL_ID, RR_ID = 21, 24, 27, 30
RANK = 33

# ------------ 噪声索引 ------------
ARW_ID, VRW_ID = 3, 0
BGSTD_ID, BASTD_ID = 6, 9
SGSTD_ID, SASTD_ID = 12, 15
FL_STD_ID, FR_STD_ID, RL_STD_ID, RR_STD_ID = 18, 21, 24, 27
NOISERANK = 30

foot_noise_block = [FL_STD_ID, FR_STD_ID, RL_STD_ID, RR_STD_ID]

def footstateid(leg: int) -> int:
    return 21 + leg*3

class EKF:
    @staticmethod
    def skew_symmetric(v: np.ndarray) -> np.ndarray:
        return np.array([
            [0, -v[2], v[1]],
            [v[2], 0, -v[0]],
            [-v[1], v[0], 0]
        ], dtype=float)

    @staticmethod
    def _wrap_yaw_0_2pi(yaw_val: float) -> float:
        y = float(yaw_val) % (2.0 * np.pi)
        return y if y >= 0.0 else y + 2.0 * np.pi

    def __init__(self, paras_: Paras, dataset: np.ndarray):
        self.paras_ = paras_
        self.dataset = dataset
        self.imucur_, self.imupre_ = IMU(), IMU()
        self.pvacur_ = PVA()
        self.pvapre_ = PVA()
        self.imuerror_ = ImuError()
        self.robotsensor_ = RobotSensor()
        
        self.foot_pos_abs_ = np.zeros((3, 4))
        self.foot_pos_rel_ = np.zeros((3,4))
        self.foot_vel_rel_ = np.zeros((3, 4))
        self.measument_updated_ = False
        self.estimated_contacts = np.zeros(4)
        
        self.ifinitAligned = False
        self.alignTimestamp = 0.0
        self.initAlignEpochs = 0
        self.init_gyro_mean = np.zeros(3)
        self.init_acc_mean = np.zeros(3)
        self.initfootposition_inbody = np.zeros((3, 4))
        
        self.Cov_ = np.zeros((RANK, RANK))
        self.Qc_ = np.zeros((NOISERANK, NOISERANK))
        self.delta_x_ = np.zeros((RANK, 1))
        
        # 根据 IMU 噪声初始化 Qc_
        imunoise = self.paras_.imunoise
        fac = 2.0 / imunoise.corr_time
        self.Qc_[ARW_ID:ARW_ID+3, ARW_ID:ARW_ID+3] = np.diag(square(imunoise.gyr_arw))
        self.Qc_[VRW_ID:VRW_ID+3, VRW_ID:VRW_ID+3] = np.diag(square(imunoise.acc_vrw))
        self.Qc_[BGSTD_ID:BGSTD_ID+3, BGSTD_ID:BGSTD_ID+3] = fac * np.diag(square(imunoise.gyrbias_std))
        self.Qc_[BASTD_ID:BASTD_ID+3, BASTD_ID:BASTD_ID+3] = fac * np.diag(square(imunoise.accbias_std))
        self.Qc_[SGSTD_ID:SGSTD_ID+3, SGSTD_ID:SGSTD_ID+3] = fac * np.diag(square(imunoise.gyrscale_std))
        self.Qc_[SASTD_ID:SASTD_ID+3, SASTD_ID:SASTD_ID+3] = fac * np.diag(square(imunoise.accscale_std))
        
        foot_pos_noise = 0.001
        for blk in foot_noise_block:
            self.Qc_[blk:blk + 3, blk:blk + 3] = np.eye(3) * square(foot_pos_noise)
            
        self.initialization(paras_.initstate, paras_.initstate_std)

3.2 状态初始化

initialization函数负责将初始导航状态、IMU 误差和协方差矩阵赋值，为滤波器运行做好准备。这里特别注意姿态矩阵和四元数的转换，以及对角线协方差的设置。

def initialization(self, initstate: NavState, initstate_std: NavState):
    """初始化 EKF 状态和协方差"""
    self.pvacur_.pos = initstate.pos.copy()
    self.pvacur_.vel = initstate.vel.copy()
    self.pvacur_.att.euler = initstate.euler.copy()
    
    cbn0 = euler2cbn(self.pvacur_.att.euler)
    self.pvacur_.att.cbn = cbn0
    self.pvacur_.att.qbn = R.from_matrix(cbn0)
    
    self.imuerror_.gyrbias = initstate.imuerror.gyrbias.copy()
    self.imuerror_.accbias = initstate.imuerror.accbias.copy()
    self.imuerror_.gyrscale = initstate.imuerror.gyrscale.copy()
    self.imuerror_.accscale = initstate.imuerror.accscale.copy()
    
    self.pvapre_ = copy.deepcopy(self.pvacur_)
    
    def put_diag(idx, std3):
        self.Cov_[idx:idx+3, idx:idx+3] = np.diag(np.square(std3))
    
    imu_std = initstate_std.imuerror
    put_diag(P_ID, initstate_std.pos)
    put_diag(V_ID, initstate_std.vel)
    put_diag(PHI_ID, initstate_std.euler)
    put_diag(BG_ID, imu_std.gyrbias)
    put_diag(BA_ID, imu_std.accbias)
    put_diag(SG_ID, imu_std.gyrscale)
    put_diag(SA_ID, imu_std.accscale)
    
    foot_pos_std = 0.01
    for idx in (FL_ID, FR_ID, RL_ID, RR_ID):
        self.Cov_[idx:idx+3, idx:idx+3] = np.eye(3) * (foot_pos_std ** 2)

3.3 INS 传播与协方差预测

insPropagation函数利用 IMU 数据传播到下一个时刻，计算状态转移矩阵 F、过程噪声矩阵 Qd，并更新协方差。这里的关键在于构建准确的线性化矩阵，并根据足端接触状态调整噪声输入。

def insPropagation(self, imupre: IMU, imucur: IMU):
    """基于前一时刻和当前 IMU 数据进行 INS 传播"""
    INSMech.imuCompensate(imucur, self.imuerror_)
    INSMech.insMech(self.pvapre_, self.pvacur_, imupre, imucur)
    
    Phi = np.zeros_like(self.Cov_)
    F = np.zeros_like(self.Cov_)
    Qd = np.zeros_like(self.Cov_)
    G = np.zeros((RANK, NOISERANK), dtype=np.float64)
    
    ff = self.robotsensor_.footforce.copy()
    th = getattr(self.paras_, "contact_force_threshold", 20.0)
    self.estimated_contacts = (ff > th).astype(float)
    
    F.fill(0.0)
    F[P_ID:P_ID+3, V_ID:V_ID+3] = np.eye(3)
    
    Cbn = self.pvapre_.att.cbn
    acc_b = imucur.acceleration
    F[V_ID:V_ID+3, PHI_ID:PHI_ID+3] = self.skew_symmetric(Cbn @ acc_b)
    F[V_ID:V_ID+3, BA_ID:BA_ID+3] = Cbn
    F[V_ID:V_ID+3, SA_ID:SA_ID+3] = Cbn @ np.diag(acc_b)
    
    F[PHI_ID:PHI_ID+3, BG_ID:BG_ID+3] = -Cbn
    omega_b = imucur.angular_velocity
    F[PHI_ID:PHI_ID+3, SG_ID:SG_ID+3] = -Cbn @ np.diag(omega_b)
    
    tau = self.paras_.imunoise.corr_time
    for blk in (BG_ID, BA_ID, SG_ID, SA_ID):
        F[blk:blk+3, blk:blk+3] = -np.eye(3) / tau
    
    G[V_ID:V_ID+3, VRW_ID:VRW_ID+3] = Cbn
    G[PHI_ID:PHI_ID+3, ARW_ID:ARW_ID+3] = Cbn
    G[BG_ID:BG_ID+3, BGSTD_ID:BGSTD_ID+3] = np.eye(3)
    G[BA_ID:BA_ID+3, BASTD_ID:BASTD_ID+3] = np.eye(3)
    G[SG_ID:SG_ID+3, SGSTD_ID:SGSTD_ID+3] = np.eye(3)
    G[SA_ID:SA_ID+3, SASTD_ID:SASTD_ID+3] = np.eye(3)
    
    big = 1e3
    for leg, blk_id in enumerate((FL_ID, FR_ID, RL_ID, RR_ID)):
        G[blk_id:blk_id+3, foot_noise_block[leg]:foot_noise_block[leg]+3] = \
            (1 + (1 - self.estimated_contacts[leg]) * big) * np.eye(3)
    
    dt = imucur.dt
    Phi = np.eye(RANK) + F * dt
    Qd = G @ self.Qc_ @ G.T * dt
    Qd = 0.5 * (Phi @ Qd @ Phi.T + Qd)
    
    self.EKFPredict(Phi, Qd)

3.4 测量更新

measUpdate函数根据脚端接触情况计算相对位置和速度的创新量 dz，并调用 EKFUpdate 更新状态和协方差。这里利用了足端静止约束来修正累积误差。

def measUpdate(self):
    """EKF 测量更新，使用脚端位置和速度约束"""
    for i in range(4):
        if self.estimated_contacts[i] != 0:
            self.computeRelFootPosVel(self.robotsensor_, i)
            dz = np.zeros((6, 1))
            dz[0:3, 0] = (
                self.pvacur_.att.cbn.T @ (self.foot_pos_abs_[:, i] - self.pvacur_.pos) - self.foot_pos_rel_[:, i]
            )
            dz[3:6, 0] = (
                self.pvacur_.vel + self.pvacur_.att.cbn @ (
                    self.skew_symmetric(self.imucur_.angular_velocity) @ self.foot_pos_rel_[:, i] + self.foot_vel_rel_[:, i]
                )
            )
            
            H = np.zeros((6, RANK))
            H[0:3, P_ID:P_ID+3] = -self.pvacur_.att.cbn.T
            H[0:3, PHI_ID:PHI_ID+3] = -self.pvacur_.att.cbn.T @ self.skew_symmetric(
                self.foot_pos_abs_[:, i] - self.pvacur_.pos
            )
            H[0:3, footstateid(i):footstateid(i)+3] = self.pvacur_.att.cbn.T
            H[3:6, V_ID:V_ID+3] = np.eye(3)
            H[3:6, PHI_ID:PHI_ID+3] = self.skew_symmetric(
                self.pvacur_.att.cbn.T @ (
                    self.skew_symmetric(self.imucur_.angular_velocity) @ self.foot_pos_rel_[:, i] + self.foot_vel_rel_[:, i]
                )
            )
            H[3:6, BG_ID:BG_ID+3] = -self.pvacur_.att.cbn.T @ self.skew_symmetric(self.foot_pos_rel_[:, i])
            H[3:6, SG_ID:SG_ID+3] = -self.pvacur_.att.cbn.T @ self.skew_symmetric(self.foot_vel_rel_[:, i]) @ np.diag(self.imucur_.angular_velocity)
            
            R = np.zeros((6, 6))
            R[0:3, 0:3] = np.eye(3) * (0.01 ** 2)
            R[3:6, 3:6] = np.eye(3) * (0.1 ** 2)
            
            self.EKFUpdate(dz, H, R)
            self.measument_updated_ = True

3.5 状态反馈

最后，stateFeedback函数将 EKF 更新后的状态误差反馈到实际状态中，修正位置、速度、姿态和 IMU 误差，同时清零误差向量，完成一次完整的滤波周期。

def stateFeedback(self):
    """状态反馈，将 delta_x_ 应用到当前状态"""
    if not self.measument_updated_:
        return
    
    self.pvacur_.pos -= self.delta_x_[P_ID:P_ID+3,0]
    self.pvacur_.vel -= self.delta_x_[V_ID:V_ID+3,0]
    
    delta_att = self.delta_x_[PHI_ID:PHI_ID+3,0]
    qbn = R.from_rotvec(delta_att)
    qn_rot = self.pvacur_.att.qbn
    q_new = qbn * qn_rot
    self.pvacur_.att.qbn = q_new
    self.pvacur_.att.cbn = q_new.as_matrix()
    self.pvacur_.att.euler = matrix2euler(self.pvacur_.att.cbn)
    
    self.imuerror_.gyrbias += self.delta_x_[BG_ID:BG_ID+3,0]
    self.imuerror_.accbias += self.delta_x_[BA_ID:BA_ID+3,0]
    self.imuerror_.gyrscale += self.delta_x_[SG_ID:SG_ID+3,0]
    self.imuerror_.accscale += self.delta_x_[SA_ID:SA_ID+3,0]
    
    for leg in range(4):
        blk = footstateid(leg)
        self.foot_pos_abs_[:,leg] -= self.delta_x_[blk:blk+3,0]
    
    self.delta_x_[:] = 0.0
    self.measument_updated_ = False

至此，核心功能介绍完毕。通过加载低状态数据将 IMU 和足端传感器信息输入 EKF 滤波器，结合初始静态对齐对姿态进行校准，并进行 IMU 传播和测量更新，最终得到机器人在三维空间中的位置、速度、姿态及 IMU 误差估计。可视化结果展示了 XY 平面轨迹的连续性和 EKF 状态估计的效果。

图 6-8 机器人运动路径的连续性和 EKF 状态估计图