跳到主要内容腿式机器人 IMU 融合与状态估计实战（二） | 极客日志PythonAI算法
腿式机器人 IMU 融合与状态估计实战（二）
腿式机器人 IMU 融合与状态估计的实战实现。通过静态初始对准计算初始姿态与零偏，利用惯性导航机械模型进行 IMU 数据传播与补偿。核心采用扩展卡尔曼滤波（EKF），融合 IMU 与足端传感器数据，结合足端接触状态进行预测与更新，实现对机器人位姿、速度及 IMU 误差的实时估计与补偿，最终输出高精度导航状态。
未来可期1 浏览 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 数据传播位置、速度和姿态"""
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
temp1 = np.cross(imucur_dtheta, imucur_dvel) / 2
temp2 = np.cross(imupre_dtheta, imucur_dvel) / 12
temp3 = np.cross(imupre_dvel, imupre_dtheta) / 12
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)
微信扫一扫，关注极客日志
微信公众号「极客日志」，在微信中扫描左侧二维码关注。展示文案：极客日志 zeeklog
相关免费在线工具
- 加密/解密文本
使用加密算法（如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
0
0
2
12
@staticmethod
def
imuCompensate
imu: IMU, imuerror: ImuError
"""对 IMU 数据进行偏差和缩放误差补偿"""
3
3
1
1
文件 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_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
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.setInitFootPos(footpos_in_body)
ekf.setInitGyroBias(gyro_mean)
ekf.setInitAttitude(roll, pitch)
文件 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()
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.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))
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)
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_)
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)
（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 = 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.measurement_updated_ = True
（5）下面代码的功能是定义函数 stateFeedback，将 EKF 更新后的状态误差反馈到实际状态中，修正位置、速度、姿态和 MU 误差，同时清零误差向量。
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)
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 状态估计图
