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

2. 静态初始对准

文件 src/initalign.py 的功能是实现静态初始对准（Static Alignment）过程，通过采集一段时间内的 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)
        # 将当前 IMU 数据加入 EKF
        gyro_sum += imu_cur.angular_velocity  # 累加陀螺仪角速度
        acc_sum += imu_cur.acceleration       # 累加加速度
        k += 1
        imu_cur = next(imu_stream)            # 获取下一条 IMU 数据
    
    # 计算陀螺仪和加速度计的平均值
    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)  # 将当前传感器数据加入 EKF
        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）的核心算法

文件 src/ekf.py 实现了 Unitree Go2 机器人的扩展卡尔曼滤波（EKF）核心算法，用于融合 IMU 与机器人关节传感器数据，估计机器人的位姿（位置、速度、姿态）以及 IMU 偏差和缩放误差。它包括状态初始化、IMU 数据处理、足端位置与速度计算、EKF 状态预测与测量更新、反馈修正、协方差管理以及导航状态输出功能，实现了机器人在动态运动中对状态的实时估计和误差补偿。文件 src/ekf.py 的实现流程如下所示。

（1）EKF 常量定义与初始化

下面代码的功能是定义 EKF 所需的常量、状态索引、噪声索引、脚端状态函数，以及 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:
        """将偏航角限制在 [0,2π]"""
        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()  # 当前与前一时刻 IMU
        self.pvacur_ = PVA()                # 当前导航状态
        self.pvapre_ = PVA()                # 前一导航状态
        self.imuerror_ = ImuError()         # IMU 误差
        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.measurement_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)

（2）EKF 状态初始化

下面代码的功能是定义 EKF 状态初始化函数 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)  # 使用 SciPy Rotation 表示
    
    # IMU 误差初始化
    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）INS 传播与状态转移

下面代码的功能是定义函数 insPropagation，将 IMU 数据传播到下一个时刻，计算状态转移矩阵 F、过程噪声矩阵 Qd，并更新协方差。

def insPropagation(self, imupre: IMU, imucur: IMU):
    """基于前一时刻和当前 IMU 数据进行 INS 传播"""
    INSMech.imuCompensate(imucur, self.imuerror_)  # IMU 误差补偿
    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
    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)
    
    # 衰减 IMU 偏差
    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
    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)
    
    # EKF 协方差预测
    self.EKFPredict(Phi, Qd)

（4）EKF 测量更新

下面代码的功能是定义函数 measUpdate，这是一个 EKF 测量更新函数，能够根据脚端接触情况计算相对位置和速度的创新量 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
            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)
            
            # 调用 EKF 更新
            self.EKFUpdate(dz, H, R)
            self.measurement_updated_ = True

（5）状态反馈修正

下面代码的功能是定义函数 stateFeedback，将 EKF 更新后的状态误差反馈到实际状态中，修正位置、速度、姿态和 IMU 误差，同时清零误差向量。

def stateFeedback(self):
    """状态反馈，将 delta_x_ 应用到当前状态"""
    if not self.measurement_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)
    
    # 修正 IMU 偏差
    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.measurement_updated_ = False

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

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