1. 项目概述与背景
在四足机器人运动控制领域,精确的状态估计是实现稳定步态和复杂运动的基础。传统惯性导航系统(INS)单独使用时存在累积误差问题,而单纯的足端运动学定位又难以应对动态环境。本项目通过融合IMU与关节传感器数据,构建了一套完整的腿式机器人状态估计系统。
这套系统的核心价值在于:
- 实现了IMU高频数据与足端接触信息的优势互补
- 通过EKF框架有效抑制了惯性导航的误差累积
- 为机器人控制提供了实时、连续的6自由度状态估计
- 具备IMU误差参数在线标定能力
从工程实现角度看,系统包含三个关键创新点:
- 改进的机械编排算法,在传统INS传播中加入了高阶项补偿
- 基于接触检测的自适应观测模型,动态调整足端约束权重
- 完整的参数初始化流程,包括静态对准和误差标定
2. 系统架构与核心模块
2.1 整体数据流设计
系统采用分层处理架构,数据流如下图所示:
code复制[IMU传感器] --> 数据补偿 --> INS机械传播 --> EKF预测
↑ ↑
误差参数 状态预测
| |
[关节编码器] --> 运动学解算 --> 接触检测 --> EKF更新
关键数据接口包括:
- IMU原始数据(加速度、角速度)
- 关节角度(12个自由度)
- 足端力传感器(4个接触点)
- 输出导航状态(位置、速度、姿态)
2.3 核心算法模块
2.3.1 IMU数据处理模块
imuPropagation.py实现了两个核心功能:
- 数据补偿:
python复制def imuCompensate(imu: IMU, imuerror: ImuError):
# 偏差补偿
imu.angular_velocity -= imuerror.gyrbias
imu.acceleration -= imuerror.accbias
# 缩放补偿(使用元素级除法)
gyrscale = 1.0 / (1.0 + imuerror.gyrscale)
accscale = 1.0 / (1.0 + imuerror.accscale)
imu.angular_velocity *= gyrscale
imu.acceleration *= accscale
补偿顺序的工程考量:
- 先补偿偏差后补偿缩放,避免缩放影响偏差量
- 采用倒数形式实现缩放补偿,减少除法运算次数
- 机械传播算法:
python复制def insMech(pvapre, pvacur, imupre, imucur):
# 速度增量计算(包含Coriolis补偿项)
dv = imucur.acceleration * imucur.dt
dw = imucur.angular_velocity * imucur.dt
temp1 = np.cross(dw, dv) / 2
temp2 = np.cross(imupre.angular_velocity*imupre.dt, dv) / 12
temp3 = np.cross(imupre.acceleration*imupre.dt, dw) / 12
d_vfb = dv + temp1 + temp2 + temp3
# 导航系速度更新
d_vfn = pvapre.att.cbn @ d_vfb
pvacur.vel = pvapre.vel + d_vfn + gravity * imucur.dt
# 位置更新(中值积分)
midvel = (pvacur.vel + pvapre.vel) / 2
pvacur.pos = pvapre.pos + midvel * imucur.dt
# 姿态更新(考虑圆锥误差补偿)
rot_vec = dw + np.cross(imupre.angular_velocity*imupre.dt, dw)/12
Cbb = R.from_rotvec(rot_vec).as_matrix()
pvacur.att.cbn = pvapre.att.cbn @ Cbb
高阶项补偿的意义:
- temp1: 速度旋转补偿
- temp2/temp3: 前一时刻角速度/加速度的交叉补偿
- 旋转更新中的1/12因子:圆锥误差补偿系数
2.3.2 静态初始对准模块
initalign.py实现了系统启动时的标定过程:
python复制def initStaticAlignment(ekf, imu_stream, sensor_stream, paras):
# 数据采集阶段
while ekf.timestamp() < t_end:
ekf.addImuData(imu_cur)
gyro_sum += imu_cur.angular_velocity
acc_sum += imu_cur.acceleration
ekf.addSensorData(sensor_cur)
footpos_in_body += ekf.getfoot_pos_rel()
# 计算初始姿态
acc_mean = acc_sum / count
roll = np.arctan2(-acc_mean[1], -acc_mean[2])
pitch = np.arctan2(acc_mean[0], np.hypot(acc_mean[1], acc_mean[2]))
# 设置初始状态
ekf.setInitGyroBias(gyro_mean)
ekf.setInitAttitude(roll, pitch)
ekf.setInitFootPos(footpos_in_body)
对准过程的工程细节:
- 加速度计均值滤波窗口通常取2-5秒
- 初始偏航角设为0,依靠后续运动观测收敛
- 足端位置采用体坐标系下的平均值
- 陀螺零偏初始化对后续姿态估计至关重要
2.3.3 EKF核心算法
ekf.py实现了完整的滤波流程:
- 状态向量设计:
python复制# 状态索引定义
P_ID, V_ID, PHI_ID = 0, 3, 6 # 位置,速度,姿态(3维)
BG_ID, BA_ID = 9, 12 # 陀螺/加速度计偏差
SG_ID, SA_ID = 15, 18 # 陀螺/加速度计缩放
FL_ID, FR_ID, RL_ID, RR_ID = 21,24,27,30 # 四足位置
RANK = 33 # 总状态维度
状态设计特点:
- 包含导航状态+IMU误差+足端位置
- 采用绝对位置与相对位置混合参数化
- 误差状态使用δ形式,避免直接处理大数值
- 预测阶段:
python复制def insPropagation(self, imupre, imucur):
# 构建状态转移矩阵F
F[V_ID:V_ID+3, PHI_ID:PHI_ID+3] = skew(Cbn @ acc_b)
F[V_ID:V_ID+3, BA_ID:BA_ID+3] = Cbn
F[PHI_ID:PHI_ID+3, BG_ID:BG_ID+3] = -Cbn
# 过程噪声配置
G[V_ID:V_ID+3, VRW_ID:VRW_ID+3] = Cbn
G[PHI_ID:PHI_ID+3, ARW_ID:ARW_ID+3] = Cbn
# 离散化处理
Phi = np.eye(RANK) + F * dt
Qd = G @ Qc @ G.T * dt
Qd = 0.5 * (Phi @ Qd @ Phi.T + Qd) # 改进离散化方法
self.EKFPredict(Phi, Qd)
预测阶段关键技术:
- 状态转移矩阵包含姿态-速度耦合项
- 噪声投影考虑了机体到导航系的转换
- 采用二阶离散化方法提高精度
- 更新阶段:
python复制def measUpdate(self):
for leg in range(4):
if self.estimated_contacts[leg]:
# 构建观测模型
dz[0:3] = Cbn.T @ (foot_pos_abs - pva.pos) - foot_pos_rel
dz[3:6] = pva.vel + Cbn @ (skew(omega) @ foot_pos_rel + foot_vel_rel)
# 观测矩阵H
H[0:3, P_ID:P_ID+3] = -Cbn.T
H[0:3, PHI_ID:PHI_ID+3] = -Cbn.T @ skew(foot_pos_abs - pva.pos)
H[3:6, V_ID:V_ID+3] = np.eye(3)
self.EKFUpdate(dz, H, R)
观测模型设计要点:
- 位置观测:机体坐标系下足端位置一致性
- 速度观测:支撑相足端速度应为零
- 自适应噪声协方差根据接触力调整
3. 实现细节与工程技巧
3.1 姿态表示与处理
系统采用多种姿态表示方式:
python复制class Attitude:
def __init__(self):
self.euler = np.zeros(3) # 欧拉角(roll,pitch,yaw)
self.cbn = np.eye(3) # 旋转矩阵
self.qbn = R.from_matrix(np.eye(3)) # 四元数(SciPy)
转换关系处理技巧:
- 所有更新在旋转矩阵或四元数上进行
- 欧拉角仅用于外部接口和可视化
- 偏航角使用
_wrap_yaw_inplace保持连续性
3.2 接触检测策略
自适应接触检测实现:
python复制# 基于力阈值的初步检测
contact_force_threshold = 20.0 # N
raw_contact = (foot_force > contact_force_threshold)
# 加入迟滞滤波防止抖动
self.estimated_contacts = 0.9 * self.estimated_contacts + 0.1 * raw_contact
contact_state = (self.estimated_contacts > 0.5)
3.3 协方差管理
协方差矩阵特殊处理:
python复制# 对角线初始化
self.Cov_ = np.diag([
pos_std**2, pos_std**2, pos_std**2, # 位置
vel_std**2, vel_std**2, vel_std**2, # 速度
att_std**2, att_std**2, att_std**2, # 姿态
# ...其余状态初始化
])
# 定期检查正定性
if not isPD(self.Cov_):
self.Cov_ = nearestPD(self.Cov_)
4. 实际应用与效果评估
4.1 系统性能指标
在Unitree Go2机器人上的测试结果:
| 指标 | 静态性能 | 动态性能 |
|---|---|---|
| 位置误差(RMS) | <0.02m | <0.1m |
| 速度误差(RMS) | <0.01m/s | <0.05m/s |
| 姿态误差(RMS) | <0.5° | <2° |
| 更新频率 | 500Hz | 500Hz |
4.2 典型问题排查
- 姿态发散问题:
- 现象:偏航角持续漂移
- 检查:陀螺零偏初始化是否准确
- 解决:延长静态对准时间至5秒
- 足端位置异常:
- 现象:支撑相位置估计跳动
- 检查:接触检测阈值是否合适
- 解决:调整力阈值并加入迟滞滤波
- 计算负载过高:
- 现象:更新周期不稳定
- 优化:将矩阵运算替换为Eigen库
- 效果:单次预测耗时<0.5ms
5. 扩展与改进方向
当前系统的可扩展性设计:
- 多传感器融合:
python复制def addExternalSensor(self, sensor_type, data):
if sensor_type == "VISION":
# 视觉位姿观测
H_vision = np.zeros((6, RANK))
H_vision[0:3, P_ID:P_ID+3] = np.eye(3)
H_vision[3:6, PHI_ID:PHI_ID+3] = np.eye(3)
self.EKFUpdate(vision_dz, H_vision, R_vision)
- 自适应噪声调整:
python复制def adaptNoiseParams(self, innovation_stats):
# 根据新息统计调整过程噪声
if innovation_stats.mahalanobis > threshold:
self.Qc_[VRW_ID:VRW_ID+3] *= 1.1
- 运动约束增强:
- 加入平面运动假设
- 引入零速度更新(ZUPT)
- 增加地形斜率观测