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

详细阐述了腿式机器人中 IMU 与关节传感器融合的状态估计方案。主要涵盖 IMU 数据传播补偿、静态初始对准及扩展卡尔曼滤波（EKF）核心算法。通过 INS 机械模型实现状态预测，利用足端接触信息进行测量更新，并结合反馈修正机制输出高精度位姿与误差估计。文中提供了基于 Python 的 EKF 类实现代码，包括状态初始化、协方差管理、噪声矩阵构建及状态反馈逻辑，为机器人导航与控制提供技术参考。

墨染流年发布于 2026/4/5更新于 2026/4/130 浏览

文章配图

6.4.3 状态估计

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

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
        d_vfb = imucur_dvel + temp1 + temp2 + temp3
        
        d_vfn = pvapre.att.cbn @ d_vfb
        
        gl = np.array([, , NormG], dtype=np.float64)
        d_vgn = gl * imucur.dt
        
        pvacur.vel = pvapre.vel + d_vfn + d_vgn
        
        midvel = (pvacur.vel + pvapre.vel) / 
        pvacur.pos = pvapre.pos + midvel * imucur.dt
        
        rot_bframe = imucur_dtheta + np.cross(imupre_dtheta, imucur_dtheta) / 
        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)


     ():
        
        
        imu.angular_velocity -= imuerror.gyrbias
        imu.acceleration -= imuerror.accbias
        
        gyrscale = np.ones() + imuerror.gyrscale
        accscale = np.ones() + imuerror.accscale
        imu.angular_velocity = imu.angular_velocity * ( / gyrscale)
        imu.acceleration = imu.acceleration * ( / accscale)

相关免费在线工具

加密/解密文本

使用加密算法（如AES、TripleDES、Rabbit或RC4）加密和解密文本明文。在线工具，加密/解密文本在线工具，online

RSA密钥对生成器

生成新的随机RSA私钥和公钥pem证书。在线工具，RSA密钥对生成器在线工具，online

Mermaid 预览与可视化编辑

基于 Mermaid.js 实时预览流程图、时序图等图表，支持源码编辑与即时渲染。在线工具，Mermaid 预览与可视化编辑在线工具，online

curl 转代码

解析常见 curl 参数并生成 fetch、axios、PHP curl 或 Python requests 示例代码。在线工具，curl 转代码在线工具，online

Base64 字符串编码/解码

将字符串编码和解码为其 Base64 格式表示形式即可。在线工具，Base64 字符串编码/解码在线工具，online

Base64 文件转换器

将字符串、文件或图像转换为其 Base64 表示形式。在线工具，Base64 文件转换器在线工具，online

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

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)

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)

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

6.4.3 状态估计

微信扫一扫，关注极客日志

更多推荐文章

相关免费在线工具