机器人 / 系统课时

用 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 的仿真,然后执行:

bash
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 由实际设备决定。如果使用其他话题名称,后续订阅代码也要同步修改。

理解角速度积分与零偏漂移

对于近似水平运动的差速机器人,定义当前相对偏航角为 ψk\psi_k、绕 Z 轴角速度测量值为 ωz,k\omega_{z,k}、估计零偏为 bzb_z,则一阶离散积分为:

ψk=ψk−1+(ωz,k−bz)Δtk\psi_k=\psi_{k-1}+(\omega_{z,k}-b_z)\Delta t_k

其中 Δtk\Delta t_k 是连续两帧测量时间戳之差,单位为秒。积分结果为弧度,显示时再乘以 180/π180/\pi 转为角度。例如,校正后的角速度为 0.50 rad/s,持续 2 秒,旋转变化就是 1.00 rad,约为 57.3°。

陀螺仪实际测量可以近似写成 ωm=ωtrue+b+n\omega_m=\omega_{true}+b+n,其中 bb 是零偏,nn 是测量噪声。零偏即使很小,只要持续积分就会累积误差。假设机器人静止时 Z 轴角速度仍为 +0.01 rad/s,不补偿的情况下,30 秒后就会产生约 0.30 rad,也就是 17.2° 的虚假旋转。

IMU 坐标方向、零偏补偿积分数据流与静止漂移曲线

最直接的补偿方法是在启动后保持机器人静止,连续采集一段角速度数据,用样本平均值估计 bzb_z。例如 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:

python
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 正在发布数据的终端执行:

bash
python3 imu_yaw_integrator.py

预期输出格式如下,<BIAS> 和 <YAW> 是实际测量得到的动态数值,分别保留四位和一位小数:

text
bias_z=<BIAS> rad/s
yaw=<YAW> deg

第一行在收到第 100 帧有效标定数据时打印;第二行在此后每处理 100 帧有效积分数据时打印一次。如果 IMU 以 100 Hz 发布,约每秒输出一次角度。按 Ctrl+C 可以结束节点。这里的 yaw 是未限制到 ±180° 的累计相对角度,允许连续转动多圈。

用静止与正反转验证结果

测试前确认底盘能够安全原地转动,并且没有其他控制节点同时发送运动命令。最小验收过程如下:

  1. 将机器人水平放置,启动 IMU 和积分节点,至少保持静止 100 帧,等待打印 bias_z。
  2. 标定完成后继续静止 30 秒,观察 yaw 是否持续向同一方向增长。演示时可以把累计变化超过 5° 作为排查触发值,但它不是所有 IMU 通用的性能标准。
  3. 从机器人上方观察,手动使底盘逆时针旋转约 90°。在坐标轴对齐的前提下,累计角度应增加约 90°,即约 +1.57 rad。
  4. 再顺时针转回起始方向,累计相对角度应接近 0°。重复两三轮,观察是否出现越来越大的残余角度。

针对观察到的异常,可以按下表直接定位:

观察到的现象 优先检查 修正方法
静止时角度持续增加 静止零偏、温度变化 充分静止后重新标定,评估更长采样窗口
逆时针转动却得到负角度 IMU 安装朝向、Z 轴正方向 核对 imu_link 到 base_link 的静态变换
角度相差约 57.3 倍 rad/s 与 °/s 混用 ROS 消息按 rad/s 积分,只在显示时转换单位
角度间歇跳变或漏计 时间戳乱序、暂停、丢帧 检查 header.stamp 和采样间隔;缺失区间不能凭空恢复
车体倾斜后角度明显失准 仅积分传感器 Z 轴的平面假设 使用完整三轴姿态估计和正确坐标变换

还要区分角速度零偏与坐标轴错误:零偏通常表现为静止时缓慢漂移,而坐标轴错误通常表现为运动方向相反或不同轴的旋转混入。不要通过直接取负号掩盖未经确认的 IMU 安装错误。

在车体发生明显俯仰或横滚时,简单积分 Z 轴角速度不再等价于世界坐标系中的偏航角变化。实际三维姿态估计需要对三轴角速度进行完整姿态积分,并结合重力、磁场或其他外部观测进行约束;重力可以辅助校正横滚和俯仰,但不能单独确定绝对偏航角。

本实验的验收标准是:静止时漂移相对较小,正反转符号符合车体坐标约定,积分量级与实际角度接近,同时能解释长期运行后仍存在的累计误差。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

用 IMU 数据估计旋转变化

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通