一、概述

本记录适用于以下硬件和环境：

- 树莓派（运行 Ubuntu 22.04，ROS2 Humble）  
- 幻尔科技扩展板（B 款或 C 款），其上带有一颗六轴 IMU（三轴加速度 + 三轴陀螺仪）  
- Intel RealSense D435 相机（仅在本阶段后期少量涉及，主要用于确认相机内参可直接从话题读取）

目标是将扩展板上的私有协议 IMU 数据转换为 ROS2 标准的 sensor_msgs/Imu 消息，并对数据进行基本的单位转换和粗略尺度校正，最终录制一段长时间的静止 IMU 数据包，供后续 Allan 方差分析使用。

本记录覆盖从环境搭建、代码编写、编译、测试，到长时间录制的所有详细操作，不涉及 Allan 方差分析本身和联合标定步骤。

---

二、硬件与软件准备

（1）硬件确认

请确认你的硬件符合以下条件：

• 扩展板型号为 B 或 C，带有“6轴IMU传感器”（扩展板 A 没有 IMU，不可用）。
• 扩展板通过串口（通常是 /dev/ttyAMA0）与树莓派连接。
• D435 相机已通过 USB 连接，且 realsense-ros 驱动能正常工作。
• 电源稳定，建议使用电池供电以减少振动干扰。

（2）软件环境

系统已安装：

• Ubuntu 22.04
• ROS2 Humble （完整桌面版）
• Python 3
• python3-serial（用于 SDK 的串口通信）
• ros-humble-realsense2-* 包（或从源码编译的 realsense-ros）
• colcon 构建工具
• 幻尔科技提供的 Python SDK 文件 ros_robot_controller_sdk.py（可从官方 demo 中获取）

如果缺少必要依赖，可通过以下命令安装：

sudo apt update
sudo apt install -y python3-serial python3-colcon-common-extensions

---

三、IMU 原始数据协议与单位分析

（1）SDK 中的 IMU 读取函数

官方 SDK 中的 Board.get_imu() 方法通过串口协议向扩展板查询 IMU 数据，成功时返回一个包含六个浮点数的元组：

(ax, ay, az, gx, gy, gz)

根据厂家反馈和实际测试，这六个值的物理单位分别为：

• 加速度 ax, ay, az：单位 g（重力加速度，1 g = 9.80665 m/s²）
• 角速度 gx, gy, gz：单位 °/s（度/秒）

（2）典型原始数据特征

将设备水平静止放置，连续打印若干组原始数据（单位 g 和 °/s），其典型值为：

• 加速度：X ≈ 0.056 g，Y ≈ -0.014 g，Z ≈ 0.860 g
• 角速度：X ≈ -3.4 °/s，Y ≈ -15.6 °/s，Z ≈ 0.43 °/s

由这些数据可以得出初步判断：

1. 加速度 Z 轴并不等于理论上的 1 g，合加速度约为 0.864 g，表明存在明显的整体尺度误差。
2. 陀螺仪各轴均有可观的固定零偏，尤其 Y 轴偏置高达约 -15.6 °/s，这在后续积分中会造成严重漂移。
3. 数据更新速率约 200 Hz（由 SDK 内部轮询和串口协议决定），对于视觉‑惯性融合已足够。

ROS2 标准 IMU 消息要求的数据单位为：

• 线性加速度：m/s²
• 角速度：rad/s

因此，在发布标准话题之前，必须进行单位转换。同时，为了改善静止时重力矢量的大小，还需对加速度进行粗略的整体尺度补偿。

---

四、创建 ROS2 节点，将私有协议 IMU 数据发布为 /imu/data_raw

以下步骤将 SDK 封装为一个标准 ROS2 节点，使其能够被其他 ROS 工具（如 ros2 bag、allan_ros2）直接订阅。

（1）创建工作空间与软件包

打开终端，执行以下命令创建工作空间和 Python 包：

mkdir -p ~/imu_calib_ws/src
cd ~/imu_calib_ws/src
ros2 pkg create --build-type ament_python hiwonder_imu_bridge --dependencies rclpy sensor_msgs

（2）复制 SDK 文件

将幻尔科技提供的 ros_robot_controller_sdk.py 文件复制到包目录中，使其可以被导入。假设该文件当前位于用户主目录：

cp ~/ros_robot_controller_sdk.py ~/imu_calib_ws/src/hiwonder_imu_bridge/hiwonder_imu_bridge/

（3）编写 IMU 发布节点

在包目录下创建 imu_publisher.py：

cd ~/imu_calib_ws/src/hiwonder_imu_bridge/hiwonder_imu_bridge
nano imu_publisher.py

文件完整内容如下（包含单位转换和粗略尺度因子）：

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Imu
import sys
import os
import math

sys.path.append(os.path.dirname(__file__))
from ros_robot_controller_sdk import Board

class HiwonderImuPublisher(Node):
    def __init__(self):
        super().__init__('hiwonder_imu_publisher')
        # 发布频率约 200 Hz，使用定时器周期 0.005 秒
        timer_period = 0.005
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.publisher_ = self.create_publisher(Imu, '/imu/data_raw', 10)
        
        # 初始化扩展板 SDK，开启数据接收
        self.board = Board()
        self.board.enable_reception()
        self.get_logger().info('Hiwonder IMU Publisher started on /imu/data_raw')

        # 单位转换常量
        self.G_TO_MS2 = 9.80665          # g -> m/s^2
        self.DEG_TO_RAD = math.pi / 180.0 # °/s -> rad/s

        # 加速度整体尺度因子
        # 由静止数据估算得到，修正后使静止时合加速度接近 1 g
        self.ACC_SCALE = 1.16

    def timer_callback(self):
        # 读取原始数据
        res = self.board.get_imu()
        if res is None:
            return

        # 原始值：加速度 (g)，角速度 (°/s)
        ax_g, ay_g, az_g = res[0], res[1], res[2]
        gx_dps, gy_dps, gz_dps = res[3], res[4], res[5]

        # 单位转换到标准量纲
        ax = ax_g * self.G_TO_MS2
        ay = ay_g * self.G_TO_MS2
        az = az_g * self.G_TO_MS2
        gx = gx_dps * self.DEG_TO_RAD
        gy = gy_dps * self.DEG_TO_RAD
        gz = gz_dps * self.DEG_TO_RAD

        # 应用加速度整体尺度补偿
        ax *= self.ACC_SCALE
        ay *= self.ACC_SCALE
        az *= self.ACC_SCALE

        # 封装 Imu 消息
        msg = Imu()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = 'imu_link'

        msg.linear_acceleration.x = ax
        msg.linear_acceleration.y = ay
        msg.linear_acceleration.z = az

        msg.angular_velocity.x = gx
        msg.angular_velocity.y = gy
        msg.angular_velocity.z = gz

        # 因为没有协方差信息，统一置为 -1 表示未知
        msg.orientation_covariance[0] = -1
        msg.angular_velocity_covariance[0] = -1
        msg.linear_acceleration_covariance[0] = -1

        self.publisher_.publish(msg)

def main(args=None):
    rclpy.init(args=args)
    node = HiwonderImuPublisher()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

（4）配置入口点

编辑包目录下的 setup.py 文件，确保 entry_points 部分如下所示：

entry_points={
    'console_scripts': [
        'imu_publisher = hiwonder_imu_bridge.imu_publisher:main',
    ],
},

（5）编译并测试节点

返回工作空间根目录，编译并 source：

cd ~/imu_calib_ws
colcon build --packages-select hiwonder_imu_bridge
source install/setup.bash

运行节点：

ros2 run hiwonder_imu_bridge imu_publisher

在另一个终端中监听发布的话题，确认数据输出：

ros2 topic echo /imu/data_raw

此时应能看到连续的 IMU 消息流，linear_acceleration 单位为 m/s²，angular_velocity 单位为 rad/s。

---

五、初步验证与静止偏置计算

（1）验证尺度补偿效果

保持设备在水平面上静止，观察 ros2 topic echo 的输出，检查加速度 Z 轴的值是否接近 9.8 m/s²。由于使用了 1.16 的尺度因子，理想情况下 Z 轴应约在 9.8 附近波动。同时，使用话题频率测量工具确认发布频率：

ros2 topic hz /imu/data_raw

正常结果应显示约 200 Hz。

（2）计算静止偏置（仅供记录，不注入节点）

我们曾用一小段静止数据估算出原始传感器的偏置如下（单位已转换为 g 和 °/s）：

• 加速度计偏置：bias_acc = [0.0606, -0.0204, 0.8609] g
• 陀螺仪偏置：bias_gyro = [-3.38, -15.61, 0.53] °/s

这些偏置值表明了传感器的原始状态，但在目前的节点中我们有意不扣除这些偏置。原因是 Kalibr 在进行相机‑IMU 联合标定时，会在线估计陀螺仪和加速度计的偏置，对于陀螺仪尤其需要保留原始的低频变化特性。因此，我们的发布节点只做单位转换和整体尺度补偿，不动偏置。

---

六、录制长时间静止数据用于 Allan 方差分析

Allan 方差分析需要 IMU 在绝对静止、无振动的环境下连续采集数小时的数据。本节说明如何录制满足要求的 ROS2 bag。

（1）准备静止环境

• 将设备（树莓派 + 扩展板 + D435）放置在厚重且稳固的平台上，例如大理石桌面或减振垫。
• 确保录制期间无人触碰、无风扇吹动、无其他明显振动源。
• 若非必要，可暂时断开与运动无关的外围设备，减小电气干扰。

（2）启动 IMU 发布节点

# 终端 1
cd ~/imu_calib_ws
source install/setup.bash
ros2 run hiwonder_imu_bridge imu_publisher

（3）录制 bag 包

打开新终端，切换到用于存放标定数据的目录（例如 ~/calib_data），执行录制命令：

mkdir -p ~/calib_data
cd ~/calib_data
ros2 bag record -o imu_allan /imu/data_raw

-o imu_allan 指定输出 bag 的名称为 imu_allan，实际会在当前目录下生成一个名为 imu_allan 的文件夹，内含 .db3 数据库文件和元数据。

录制过程中终端会显示当前录制状态，按下 Ctrl+C 可安全停止录制。

（4）录制时长建议

经典的 Allan 方差分析要求数据长度至少覆盖目标随机游走的时间尺度。对于 MEMS IMU，建议录制 3 小时以上。少数情况若受存储限制，也至少保证 2 小时，否则低频噪声成分无法准确估计。

录制过程中应避免任何操作，保持系统绝对静止。

（5）录制完成后的检查

停止录制后，可使用以下命令快速检查 bag 的信息：

ros2 bag info imu_allan

输出会显示话题名称、消息数量、总时长等信息。根据总消息数和时长可粗略验证录制是否正常，例如 200 Hz 持续 3 小时应有约 216 万条消息。

至此，你已经成功获得了一份可用于 Allan 方差分析的长时间静止 IMU 数据 imu_allan。

---

七、后续准备说明

这份 imu_allan bag 录制完成后，下一步就是用 allan_ros2 工具对其进行分析，提取加速度计和陀螺仪的噪声密度与随机游走参数。这些参数将填入 imu.yaml 文件，作为 Kalibr 联合标定的输入。同时，相机内参可从 D435 的 /camera/infra1/camera_info 话题直接提取生成 cam_intrinsic.yaml，无需重新标定相机。在此之后，再录制一段动态的相机‑IMU 联合标定数据，即可完成整个传感器系统的精确标定。

本记录所覆盖的所有操作已经过实际验证，每一步都有明确的命令和完整的代码，可直接复现。如果在运行过程中遇到任何问题，可以随时根据错误信息进行排查。