本文档介绍 Atom01 部署框架中 IMU（惯性测量单元）驱动的架构设计与集成方法。IMU 是强化学习 locomotion 控制回路中最关键的感知源之一，为策略网络提供机体角速度与重力方向向量。本文档面向已完成基础部署环境配置的开发者，聚焦 roboto_imu 包的内部机制、实时数据流路径以及与推理节点的集成方式。

Sources: imu_driver.hpp, robot_interface.hpp

整体架构与数据流

roboto_imu 采用分层抽象 + 工厂创建的设计模式，将协议细节与业务接口解耦。顶层 IMUDriver 定义统一传感器数据访问接口，底层通过 SocketCAN 或串口与物理 IMU 通信。推理节点 InferenceNode 通过 RobotInterface 持有 IMU 实例，在每个控制周期提取四元数和角速度，用于构建策略观测向量。

flowchart LR
    subgraph Physical["物理层"]
        IMU["HiPNUC IMU"]
    end
    subgraph Protocol["协议层"]
        CAN["SocketCAN<br/>(J1939/CANopen)"]
        SER["Serial Port<br/>(UART HI91)"]
    end
    subgraph Driver["驱动层"]
        HIP["HipnucIMUDriver"]
        ABS["IMUDriver<br/>(抽象基类)"]
    end
    subgraph App["应用层"]
        RI["RobotInterface"]
        INF["InferenceNode<br/>(obs: ang_vel / gravity_b)"]
    end

    IMU -->|CAN帧| CAN
    IMU -->|串口字节流| SER
    CAN -->|can_frame| HIP
    SER -->|"uint8_t[]"| HIP
    HIP -->|quat / ang_vel / acc| ABS
    ABS --> RI
    RI -->|get_quat()<br/>get_ang_vel()| INF

整个数据链路在 Linux 实时调度策略下运行：CAN 接收线程与串口接收线程均绑定 SCHED_FIFO 优先级 80，确保传感器数据在毫秒级抖动范围内到达驱动层缓冲区。推理线程以 70 优先级运行，通过 shared_mutex 的读锁并发访问最新传感器状态，避免阻塞数据接收。

Sources: socket_can.cpp, serial_port.cpp, hipnuc_imu_driver.cpp

驱动层设计：抽象基类与工厂模式

IMUDriver 作为抽象基类，仅暴露传感器读接口与静态工厂方法 create_imu()。这种设计允许在不修改上层代码的前提下，未来接入其他品牌 IMU。

create_imu() 根据 imu_type 字符串分派到具体实现类。当前唯一实现为 HipnucIMUDriver，它同时支持 CAN（J1939 协议）和串口（HiPNUC 私有协议 HI91）两种物理通道。上层代码只需持有 IMUDriver 指针即可透明访问传感器数据。

Sources: imu_driver.cpp, imu_driver.hpp

协议层：SocketCAN 与串口

SocketCAN 接口

IMUSocketCAN 以单例模式管理每个 CAN 网络接口（如 can0、can4）。同一进程中的多个驱动实例（例如电机驱动与 IMU 驱动共享同一 CAN 总线）可复用同一个 socket，避免重复创建内核资源。接收线程通过 select() 非阻塞读取帧，依据用户注册的 key_extractor 提取回调 ID，将帧路由到对应驱动实例的回调函数。

在 IMU 场景中，key_extractor 被配置为提取 frame.can_id & 0x7F，即 J1939 报文的源地址字段。这意味着同一 CAN 总线上可挂载多个 IMU，只要它们的节点 ID 不同，驱动层即可通过 ID 完成报文分拣。

Sources: socket_can.hpp, socket_can.cpp

串口接口

IMUSerialPort 负责 UART 通信，默认配置为 8N1（8 数据位、无校验、1 停止位），并支持常见波特率。接收线程同样以 SCHED_FIFO 优先级 80 运行，通过 select() 在 1ms 超时循环中读取数据，将字节流透传给驱动回调。串口模式适用于 HiPNUC 上位机默认出厂配置，无需修改 CAN 网络拓扑即可快速验证。

Sources: serial_port.hpp, serial_port.cpp

数据解码：J1939 与 HI91 协议

HipnucIMUDriver 根据 interface_type 选择不同的解码路径：

CAN 模式（J1939）：接收线程解析 CAN 扩展帧，通过 PGN（Parameter Group Number）识别报文类型。hipnuc_j1939_parse_frame() 将原始字节转换为物理量。当前驱动在回调中仅处理加速度（CAN_MSG_ACCEL）和陀螺仪（CAN_MSG_GYRO）两类报文，分别乘以 GRA_ACC（9.8）和 DEG_TO_RAD（0.01745329）完成单位转换。注意，在 CAN 模式下，当前代码版本未在回调中解析四元数——若需四元数，应确保 IMU 配置为输出 0xFF46 PGN，并扩展解析逻辑。

串口模式（HI91）：使用 hipnuc_input() 逐字节解析 HiPNUC 私有协议。当检测到完整 0x91 数据包时，一次性提取四元数、陀螺仪、加速度和温度。串口模式天然包含四元数，无需额外配置。

两种模式共享同一块 can_sensor_data_t 结构体作为内部状态缓存，并通过 std::shared_mutex 保护读写并发。unique_lock 用于回调写入，shared_lock 用于上层读取，实现高吞吐并发访问。

Sources: hipnuc_imu_driver.cpp, hipnuc_j1939_parser.c, hipnuc_dec.h

硬件初始化与配置

出厂默认的 HiPNUC IMU 通常运行在 500Kbps CAN 波特率，且可能同时输出欧拉角与四元数。为了与 Atom01 部署框架匹配（推理节点期望 1Mbps 总线速率和精简数据），需要使用 init_imu.sh 脚本完成一次性配置。该脚本通过 cansend 向 0CEF0808 发送 J1939 配置写命令：

1. 开启四元数输出，关闭欧拉角输出；
2. 将 CAN 波特率修改为 1M；
3. 保存配置并复位设备。

脚本执行完成后，必须手动将主机的 CAN 接口重新 up 为 1M 波特率。对于 Orange Pi 5 Plus，建议配合 udev 规则 99-auto-up-devs-orangepi.rules 自动完成接口命名和 up 操作；对于串口模式，udev 规则会将匹配的 CP210x 设备映射为 /dev/ttyUSB0 并赋予 0666 权限。

Sources: init_imu.sh, 99-auto-up-devs-orangepi.rules

与推理节点的集成

IMU 并非独立运行，而是嵌入在 RobotInterface 的生命周期内。RobotInterface 在构造阶段读取 robot.yaml 中的 imu 字段，通过 IMUDriver::create_imu() 创建实例。推理节点在每个 inference() 周期调用 robot_->get_ang_vel() 和 robot_->get_quat()，分别填入观测向量的 ang_vel（3 维）和 gravity_b（3 维）段。

RobotInterface 还提供了外参旋转矩阵 extrinsic_R 的配置能力。若 IMU 安装姿态与机体坐标系不一致，可通过 robot.yaml 中的 extrinsic_R 9 元素行主序矩阵进行坐标变换。get_quat() 内部将原始四元数与 extrinsic_q_inv_ 相乘并归一化；get_ang_vel() 则通过 extrinsic_R_mat_ 旋转向量。默认值为单位矩阵，表示 IMU 与机体完全对齐。

推理节点在 get_gravity_b_obs() 中会将世界坐标系的重力向量 (0, 0, -1) 通过机体四元数逆变换到机体坐标系，同时监测 gravity_b.z()：若该值大于 gravity_z_upper_（默认 -0.5），则判定机器人跌倒并立即触发安全停机。这是 IMU 数据在控制回路中承担的核心安全职责之一。

Sources: robot_interface.hpp, obs_manager.cpp, robot.yaml

Python SDK 与独立使用

roboto_imu 通过 pybind11 暴露 imu_py 模块，使 Python 脚本无需 ROS2 即可直接访问 IMU。这在硬件调试和传感器标定阶段尤为实用。

import imu_py

imu = imu_py.IMUDriver.create_imu(
    imu_id=8,
    interface_type="serial",
    interface="/dev/ttyUSB0",
    imu_type="HIPNUC",
    baudrate=921600
)

quat = imu.get_quat()        # [w, x, y, z]
gyro = imu.get_ang_vel()     # [x, y, z] rad/s
acc  = imu.get_lin_acc()     # [x, y, z] m/s²
temp = imu.get_temperature() # °C

在 C++ 中，接口完全一致：

auto imu = IMUDriver::create_imu(0x08, "can", "can0", "HIPNUC");
auto quat = imu->get_quat();
auto gyro = imu->get_ang_vel();

推理节点启动后，也可通过 ROS2 服务读取当前 IMU 状态：ros2 service call /read_imu std_srvs/srv/Trigger，该服务内部调用 publish_imu() 向 /imu 话题发布 sensor_msgs/Imu 消息。

Sources: pybind_module.cpp, imu_py_example.py, ros_interface.cpp

常见故障排查

Sources: hipnuc_imu_driver.cpp, obs_manager.cpp

延伸阅读与下一步

完成 IMU 驱动理解后，建议继续阅读 电机驱动与CAN总线通信，以理解 RobotInterface 如何将 IMU 与电机驱动整合为统一的机器人硬件抽象层。若需修改推理频率或观测向量布局，可参考 推理节点与ONNX策略部署 中关于 obs_layouts 和 decimation 的配置说明。