（6-4-02）IMU融合与机体状态估计：综合实战：腿式机器人的IMU关节融合与状态估计（2）

6.4.3 状态估计

"src"目录包含本项目状态估计的核心算法实现和工具模块，涵盖惯性导航与人形机器人运动状态估计的完整流程，包括EKF状态预测与更新、IMU数据补偿与积分、机器人足端运动学计算、静态初始对准、导航结果与误差输出、数据流生成及可视化工具，整体提供从原始传感器数据到导航状态估计和分析的全链路功能，实现机器人高精度运动导航和状态监控。

IMU数据的传播与补偿

文件src/imuPropagation.py的功能是提供IMU数据的传播与补偿机制，用于惯性导航系统（INS）中状态更新。INSMech 类实现了基于前一时刻和当前IMU测量的速度、位置和姿态传播，同时对IMU角速度和加速度进行偏差与缩放误差补偿。_wrap_yaw_inplace用于将偏航角限制在 -π,π 范围内。

python 复制代码

import numpy as np
from scipy.spatial.transform import Rotation as R

def _wrap_yaw_inplace(euler_rpy):
    """将欧拉角中的偏航角限制在 [-π, π] 范围内"""
    euler_rpy[2] = (euler_rpy[2] + np.pi) % (2*np.pi) - np.pi

class INSMech:
    """惯性导航机械模型，用于状态传播和 IMU 数据补偿"""

    @staticmethod
    def insMech(pvapre: PVA, pvacur: PVA, imupre: IMU, imucur: IMU):
        """基于前一时刻和当前 IMU 数据传播位置、速度和姿态"""
        # IMU 线加速度和角速度积分得到增量
        imucur_dvel = imucur.acceleration * imucur.dt
        imucur_dtheta = imucur.angular_velocity * imucur.dt
        imupre_dvel = imupre.acceleration * imupre.dt
        imupre_dtheta = imupre.angular_velocity * imupre.dt

        # 计算交叉项以提高积分精度（Coriolis 校正）
        temp1 = np.cross(imucur_dtheta, imucur_dvel) / 2
        temp2 = np.cross(imupre_dtheta, imucur_dvel) / 12
        temp3 = np.cross(imupre_dvel, imucur_dtheta) / 12

        # 合成速度增量
        d_vfb = imucur_dvel + temp1 + temp2 + temp3

        # 转到导航坐标系
        d_vfn = pvapre.att.cbn @ d_vfb

        # 考虑重力加速度
        gl = np.array([0, 0, NormG], dtype=np.float64)
        d_vgn = gl * imucur.dt

        # 更新速度
        pvacur.vel = pvapre.vel + d_vfn + d_vgn

        # 更新位置（使用速度中值积分）
        midvel = (pvacur.vel + pvapre.vel) / 2
        pvacur.pos = pvapre.pos + midvel * imucur.dt

        # 姿态更新
        rot_bframe = imucur_dtheta + np.cross(imupre_dtheta, imucur_dtheta) / 12
        Cbb = R.from_rotvec(rot_bframe).as_matrix()
        pvacur.att.cbn = pvapre.att.cbn @ Cbb
        pvacur.att.qbn = R.from_matrix(pvacur.att.cbn)

        # 更新欧拉角表示
        pvacur.att.euler = matrix2euler(pvacur.att.cbn)

    @staticmethod
    def imuCompensate(imu: IMU, imuerror: ImuError):
        """对 IMU 数据进行偏差和缩放误差补偿"""
        # 去除陀螺仪和加速度计偏差
        imu.angular_velocity -= imuerror.gyrbias
        imu.acceleration -= imuerror.accbias

        # 缩放补偿
        gyrscale = np.ones(3) + imuerror.gyrscale
        accscale = np.ones(3) + imuerror.accscale

        imu.angular_velocity = imu.angular_velocity * (1 / gyrscale)
        imu.acceleration = imu.acceleration * (1 / accscale)

静态初始对准

文件src/initalign.py的功能是实现静态初始对准（Static Alignment）过程，通过采集一段时间内的IMU和传感器数据，计算初始姿态（滚转角、俯仰角）、陀螺仪零偏以及足端位置，用于EKF的状态初始化，从而保证惯性导航系统在启动时的状态估计准确。

python 复制代码

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)            # 设置初始姿态

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

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

（1）下面代码的功能是定义EKF所需的常量、状态索引、噪声索引、脚端状态函数，以及EKF类的基础初始化，包括协方差矩阵和噪声矩阵初始化。

python 复制代码

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.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)

（2）下面代码的功能是定义EKF状态初始化函数initialization，将初始导航状态、IMU误差和协方差矩阵赋值，为滤波器运行做好准备。

python 复制代码

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

python 复制代码

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）下面代码的功能是定义函数measUpdate，这是一个EKF测量更新函数，能够根据脚端接触情况计算相对位置和速度的创新量dz，并调用EKFUpdate更新状态和协方差。

python 复制代码

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.measument_updated_ = True

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

python 复制代码

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)

    # 修正 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.measument_updated_ = False

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

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