机器人 / 系统课时
用 IMU 数据估计旋转变化
用 IMU 数据估计旋转变化
机器人原地转动时,可以对 IMU 陀螺仪的角速度按时间积分,得到相对旋转角度。但即使机器人完全静止,微小的陀螺仪零偏也会累积成明显的角度误差。下面使用 ROS 2 Jazzy 订阅 IMU 数据,实现静止零偏估计、偏航角积分,并通过实际转动检查坐标方向、单位和漂移。
学习目标
- 识别
sensor_msgs/msg/Imu中的角速度、时间戳和坐标系,正确理解旋转正方向与单位。 - 使用静止样本估计陀螺仪零偏,根据消息时间戳积分得到相对偏航角。
- 通过静止和正反向旋转试验,区分零偏漂移、坐标方向错误和单位转换问题。
确认 IMU 消息与坐标方向
ROS 2 Jazzy 使用 sensor_msgs/msg/Imu 表示惯性测量数据。陀螺仪提供的是角速度,不是角度;需要结合时间间隔才能计算旋转变化。标准消息中的角速度单位为 rad/s,线加速度单位为 m/s²。
| 消息字段 | 含义 | 本课用途 |
|---|---|---|
header.stamp |
测量时间戳 | 计算相邻采样间隔 |
header.frame_id |
数据所属坐标系 | 检查 IMU 安装方向 |
angular_velocity.z |
绕传感器 Z 轴的角速度,rad/s | 平面偏航角积分 |
orientation |
四元数姿态估计 | 本次不直接使用 |
orientation_covariance |
姿态估计协方差 | 首元素为 -1 时表示没有姿态估计 |
按照 ROS REP-103,常见机器人车体坐标系 base_link 采用 X 向前、Y 向左、Z 向上。绕 Z 轴的正旋转遵循右手定则:从上方俯视,逆时针为正,顺时针为负。
这里有一个前提:只有 IMU 的测量坐标轴与车体坐标轴对齐、机器人基本保持水平时,才能直接把 angular_velocity.z 近似当作车体偏航角速度。如果 imu_link 相对 base_link 存在安装旋转,应先利用静态 TF 或安装标定旋转将角速度向量变换到车体坐标系。
在 Ubuntu 24.04 和 ROS 2 Jazzy 环境中,先启动真实 IMU 驱动或带 IMU 的仿真,然后执行:
source /opt/ros/jazzy/setup.bash
ros2 topic info /imu/data
ros2 topic echo /imu/data --once预期结果:话题类型为 sensor_msgs/msg/Imu,回显消息包含 header、orientation、angular_velocity 和 linear_acceleration 等字段。消息数值、发布者数量及 frame_id 由实际设备决定。如果使用其他话题名称,后续订阅代码也要同步修改。
理解角速度积分与零偏漂移
对于近似水平运动的差速机器人,定义当前相对偏航角为 、绕 Z 轴角速度测量值为 、估计零偏为 ,则一阶离散积分为:
其中 是连续两帧测量时间戳之差,单位为秒。积分结果为弧度,显示时再乘以 转为角度。例如,校正后的角速度为 0.50 rad/s,持续 2 秒,旋转变化就是 1.00 rad,约为 57.3°。
陀螺仪实际测量可以近似写成 ,其中 是零偏, 是测量噪声。零偏即使很小,只要持续积分就会累积误差。假设机器人静止时 Z 轴角速度仍为 +0.01 rad/s,不补偿的情况下,30 秒后就会产生约 0.30 rad,也就是 17.2° 的虚假旋转。

最直接的补偿方法是在启动后保持机器人静止,连续采集一段角速度数据,用样本平均值估计 。例如 100 Hz 的 IMU 采集 100 帧约需 1 秒。如果标定期间机器人发生转动,真实角速度就会被错误地算进零偏,后续积分反而产生系统性误差。
在 ROS 2 中订阅并积分角速度
下面编写一个独立的 rclpy 节点,直接订阅 /imu/data。示例假设 IMU Z 轴与 base_link 的 Z 轴同向,机器人启动后保持静止至少 100 帧,随后才开始运动。代码使用 qos_profile_sensor_data 以兼容常见的 Best Effort 传感器发布者。
将以下完整程序保存为 imu_yaw_integrator.py:
import math
import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import Imu
class ImuYawNode(Node):
def __init__(self):
super().__init__("imu_yaw_integrator")
self.subscription = self.create_subscription(
Imu, "/imu/data", self.on_imu, qos_profile_sensor_data
)
self.static_samples = []
self.bias_z = None
self.last_time = None
self.yaw_rad = 0.0
self.integrated_count = 0
def on_imu(self, msg):
stamp = msg.header.stamp
time_s = stamp.sec + stamp.nanosec * 1e-9
if time_s <= 0:
return
omega_z = msg.angular_velocity.z
if not math.isfinite(omega_z):
return
if self.bias_z is None:
# 标定阶段必须静止,前100帧只估计零偏,不累计角度。
self.static_samples.append(omega_z)
if len(self.static_samples) == 100:
self.bias_z = sum(self.static_samples) / 100.0
self.last_time = time_s
print(f"bias_z={self.bias_z:.4f} rad/s", flush=True)
return
# 使用消息采样时间而不是回调到达时间。
dt = time_s - self.last_time
if dt <= 0:
return
self.last_time = time_s
if dt > 0.2:
return # 长时间中断不按正常连续采样进行积分。
self.yaw_rad += (omega_z - self.bias_z) * dt
self.integrated_count += 1
if self.integrated_count % 100 == 0:
yaw_deg = math.degrees(self.yaw_rad)
print(f"yaw={yaw_deg:.1f} deg", flush=True)
def main():
rclpy.init()
node = ImuYawNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()程序依次完成静止零偏估计、相邻时间戳求差、零偏补偿和角度累加。代码使用消息自身的时间戳,避免把网络或执行器调度延迟误认为真实采样时间。对不递增的时间戳直接丢弃;对于超过 0.2 秒的采样间隔,跳过该区间的积分,避免将未知运动过程当作连续有效数据。
在已经加载 ROS 2 Jazzy 环境、rclpy 和 sensor_msgs 可用且 IMU 正在发布数据的终端执行:
python3 imu_yaw_integrator.py预期输出格式如下,<BIAS> 和 <YAW> 是实际测量得到的动态数值,分别保留四位和一位小数:
bias_z=<BIAS> rad/s
yaw=<YAW> deg第一行在收到第 100 帧有效标定数据时打印;第二行在此后每处理 100 帧有效积分数据时打印一次。如果 IMU 以 100 Hz 发布,约每秒输出一次角度。按 Ctrl+C 可以结束节点。这里的 yaw 是未限制到 ±180° 的累计相对角度,允许连续转动多圈。
用静止与正反转验证结果
测试前确认底盘能够安全原地转动,并且没有其他控制节点同时发送运动命令。最小验收过程如下:
- 将机器人水平放置,启动 IMU 和积分节点,至少保持静止 100 帧,等待打印
bias_z。 - 标定完成后继续静止 30 秒,观察
yaw是否持续向同一方向增长。演示时可以把累计变化超过 5° 作为排查触发值,但它不是所有 IMU 通用的性能标准。 - 从机器人上方观察,手动使底盘逆时针旋转约 90°。在坐标轴对齐的前提下,累计角度应增加约 90°,即约 +1.57 rad。
- 再顺时针转回起始方向,累计相对角度应接近 0°。重复两三轮,观察是否出现越来越大的残余角度。
针对观察到的异常,可以按下表直接定位:
| 观察到的现象 | 优先检查 | 修正方法 |
|---|---|---|
| 静止时角度持续增加 | 静止零偏、温度变化 | 充分静止后重新标定,评估更长采样窗口 |
| 逆时针转动却得到负角度 | IMU 安装朝向、Z 轴正方向 | 核对 imu_link 到 base_link 的静态变换 |
| 角度相差约 57.3 倍 | rad/s 与 °/s 混用 | ROS 消息按 rad/s 积分,只在显示时转换单位 |
| 角度间歇跳变或漏计 | 时间戳乱序、暂停、丢帧 | 检查 header.stamp 和采样间隔;缺失区间不能凭空恢复 |
| 车体倾斜后角度明显失准 | 仅积分传感器 Z 轴的平面假设 | 使用完整三轴姿态估计和正确坐标变换 |
还要区分角速度零偏与坐标轴错误:零偏通常表现为静止时缓慢漂移,而坐标轴错误通常表现为运动方向相反或不同轴的旋转混入。不要通过直接取负号掩盖未经确认的 IMU 安装错误。
在车体发生明显俯仰或横滚时,简单积分 Z 轴角速度不再等价于世界坐标系中的偏航角变化。实际三维姿态估计需要对三轴角速度进行完整姿态积分,并结合重力、磁场或其他外部观测进行约束;重力可以辅助校正横滚和俯仰,但不能单独确定绝对偏航角。
本实验的验收标准是:静止时漂移相对较小,正反转符号符合车体坐标约定,积分量级与实际角度接近,同时能解释长期运行后仍存在的累计误差。