（6-4-02）IMU融合与机体状态估计：综合实战：腿式机器人的IMU关节融合与状态估计（2）
6.4.3 状态估计
“src”目录包含本项目状态估计的核心算法实现和工具模块，涵盖惯性导航与人形机器人运动状态估计的完整流程，包括EKF状态预测与更新、IMU数据补偿与积分、机器人足端运动学计算、静态初始对准、导航结果与误差输出、数据流生成及可视化工具，整体提供从原始传感器数据到导航状态估计和分析的全链路功能，实现机器人高精度运动导航和状态监控。
1. IMU数据的传播与补偿
文件src/imuPropagation.py的功能是提供IMU数据的传播与补偿机制，用于惯性导航系统（INS）中状态更新。INSMech 类实现了基于前一时刻和当前IMU测量的速度、位置和姿态传播，同时对IMU角速度和加速度进行偏差与缩放误差补偿。_wrap_yaw_inplace用于将偏航角限制在 -π,π
范围内。
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) 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类的基础初始化，包括协方差矩阵和噪声矩阵初始化。
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误差和协方差矩阵赋值，为滤波器运行做好准备。
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，并更新协方差。
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更新状态和协方差。
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误差，同时清零误差向量。
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状态估计图
