IMU 驱动与姿态获取

本页深入讲解 Roboto 平台 IMU 驱动库 roboto_imu 的架构设计、数据流与实时性机制，覆盖从底层 CAN/串口协议解析到上层 RobotInterface 姿态读取的完整链路。目标读者为已具备 C++ 多线程与 Linux SocketCAN 基础的中级开发者，需理解 IMU 数据如何被接入强化学习推理环路与 ROS 2 生态。

整体架构与模块层次

roboto_imu 采用四层隔离架构，将抽象接口、协议传输、设备驱动与协议解码严格解耦，使得新增 IMU 厂商时只需在驱动层与解码层扩展，而协议层与抽象层完全复用。下图展示了从物理传感器到上层应用的数据通路。

flowchart TB
    subgraph AppLayer["应用层"]
        A1[RobotInterface]
        A2[InferenceNode<br/>观测堆叠 / ROS 发布]
    end
    
    subgraph AbstractLayer["抽象接口层"]
        B1[IMUDriver<br/>工厂模式]
    end
    
    subgraph DriverLayer["设备驱动层"]
        C1[HipnucIMUDriver<br/>CAN J1939 / Serial HI91]
    end
    
    subgraph ProtocolLayer["协议传输层"]
        D1[IMUSocketCAN<br/>单例 + SCHED_FIFO]
        D2[IMUSerialPort<br/>非阻塞串口]
    end
    
    subgraph DecodeLayer["协议解码层 (C)"]
        E1[hipnuc_j1939_parser]
        E2[hipnuc_dec<br/>HI91/HI92/HI81/HI83]
    end
    
    subgraph Hardware["物理硬件"]
        F1[HiPNUC IMU]
    end
    
    F1 -->|"CAN / UART"| D1
    F1 -->|"UART"| D2
    D1 -->|"can_frame"| C1
    D2 -->|"uint8_t[]"| C1
    C1 --> E1
    C1 --> E2
    C1 --> B1
    B1 --> A1
    A1 --> A2

架构的顶层是应用层的 RobotInterface，它通过 IMUDriver 抽象基类统一获取姿态数据；底层则由 IMUSocketCAN 单例与 IMUSerialPort 分别管理 CAN 总线与串口资源，两者均以独立接收线程运行，并通过回调函数将原始帧递交给 HipnucIMUDriver 进行协议解析与单位换算。

Sources: imu_driver.hpp, hipnuc_imu_driver.hpp

抽象接口层：工厂模式与统一 API

IMUDriver 作为抽象基类定义了所有 IMU 必须实现的统一传感器访问接口，包括四元数、角速度、线加速度与温度。该类采用工厂模式，通过静态方法 create_imu() 根据 imu_type 参数动态实例化具体驱动，当前仅支持 "HIPNUC" 类型，但架构上已为扩展预留了条件分支。

基类内部使用 std::vector<float> 存储四元数（w, x, y, z）与三维向量（x, y, z），并内嵌 spdlog 日志器用于运行时诊断。所有 get 方法默认返回拷贝而非引用，具体子类可通过 override 重写以实现线程安全的访问控制。

方法	返回值维度	单位	说明
`get_quat()`	4 (w, x, y, z)	无	机体坐标系下的姿态四元数
`get_ang_vel()`	3 (x, y, z)	rad/s	机体坐标系下的角速度
`get_lin_acc()`	3 (x, y, z)	m/s²	线加速度
`get_temperature()`	1	°C	芯片温度

Sources: imu_driver.hpp, imu_driver.cpp

协议传输层：SocketCAN 单例与串口非阻塞 I/O

协议传输层负责屏蔽底层总线差异，向上提供统一的原始帧回调接口。该层是驱动实时性的关键所在，两条路径均使用 SCHED_FIFO 实时调度 与 select 多路复用，确保接收线程以微秒级延迟获取传感器数据。

CAN 路径：IMUSocketCAN 单例

IMUSocketCAN 采用单例模式（按接口名区分实例），允许多个 HipnucIMUDriver 实例共享同一 CAN 总线（例如 can0），而无需重复创建 socket。其设计要点如下：

非阻塞 socket：通过 fcntl 设置 O_NONBLOCK，配合 select 实现 1ms 超时轮询，避免线程在 read 上永久阻塞；
实时优先级：接收线程通过 pthread_setschedparam 设置为 SCHED_FIFO、优先级 80，确保在实时内核下抢占用户态任务；
回调分发：维护一个 CanCbkMap，支持通过 set_key_extractor 自定义 CAN ID 到回调键的映射规则。对于 HiPNUC J1939 协议，驱动层将提取器设为 frame.can_id & 0x7F，从而按 IMU 节点 ID 分流不同设备的数据。

sequenceDiagram
    participant H as HipnucIMUDriver
    participant S as IMUSocketCAN 单例
    participant K as Linux Kernel<br/>SocketCAN
    participant C as can_rx 线程<br/>SCHED_FIFO prio 80

    H->>S: add_can_callback(cb, imu_id)
    H->>S: set_key_extractor(lambda)
    S->>K: socket(PF_CAN, SOCK_RAW)
    S->>K: bind(can0)
    S->>C: spawn_thread()
    loop select + read loop
        C->>K: select(1ms timeout)
        K-->>C: can_frame available
        C->>C: key = extractor(frame)
        C->>C: lookup callback_list[key]
        C->>H: callback_to_run(frame)
    end

串口路径：IMUSerialPort

IMUSerialPort 为每个串口设备独立创建实例，同样以独立线程接收数据。其配置固定为 8N1 无流控，波特率支持 9600 至 921600。与 CAN 路径类似，串口线程也设置为 SCHED_FIFO 优先级 80，并通过 select 以 1ms 超时读取 BUF_SIZE（1024 字节）缓冲区，将接收到的字节流通过 SerialCbkFunc 回调递交给驱动层。

Sources: socket_can.hpp, socket_can.cpp, serial_port.hpp, serial_port.cpp

设备驱动层：HipnucIMUDriver 与双协议解析

HipnucIMUDriver 继承自 IMUDriver，是 HiPNUC（超核电子）系列 IMU 的具体实现。它同时支持 CAN J1939 与 串口私有协议（HI91） 两种数据通路，并在构造函数中根据 interface_type 选择初始化逻辑。

构造函数与生命周期

构造时，若为串口则打开对应 ttyUSB 并注册串口回调；若为 CAN 则获取 IMUSocketCAN 单例实例，注册 CAN 回调并配置键提取器。析构时按接口类型执行反向清理：关闭串口或从 CAN 单例的回调列表中移除自身。这一设计确保同一 CAN 总线上可挂载多个 IMU，每个实例通过 imu_id 独立接收属于自己的数据帧。

Sources: hipnuc_imu_driver.cpp

CAN J1939 解析与单位换算

CAN 回调 can_rx_cbk 将 can_frame 拷贝为 hipnuc_can_frame_t 后，调用 hipnuc_j1939_parse_frame 进行 PGN（Parameter Group Number）解析。该解析器通过 CAN ID 中的 PGN 字段识别帧类型，当前驱动仅处理加速度与陀螺仪两类报文，其余类型（如四元数、磁力计、欧拉角）虽在解析器中已实现，但在回调中未做进一步更新。

关键点在于单位换算发生在驱动层而非解析层：

加速度：J1939 原始单位为 mG/LSB（0.00048828 g），回调中乘以 GRA_ACC (9.8) 转换为 m/s²；
陀螺仪：J1939 原始单位为 °/s/LSB（0.061035 °/s），回调中乘以 DEG_TO_RAD (0.01745329) 转换为 rad/s。

这种分层策略使得 C 语言协议解析器保持纯数据提取职责，而单位适配与业务逻辑保留在 C++ 驱动层，便于不同应用场景裁剪。

Sources: hipnuc_imu_driver.cpp, hipnuc_j1939_parser.c

串口私有协议解析（HI91）

串口回调 serial_rx_cbk 采用字节流状态机解析 HiPNUC 私有协议。每收到一个字节便调用 hipnuc_input()，当检测到完整 HI91 数据包时返回 1，驱动随即从 raw_.hi91 结构中提取四元数、陀螺仪与加速度。与 CAN 路径相同，陀螺仪与加速度在此完成 °/s → rad/s 和 g → m/s² 的换算。

HI91 帧结构采用紧凑二进制布局，包含温度、气压、系统时间、加速度、陀螺仪、磁力计、欧拉角与四元数等字段，所有浮点字段均为单精度 float，通过 __attribute__((__packed__)) 确保跨平台对齐。

Sources: hipnuc_imu_driver.cpp, hipnuc_dec.h

线程安全与数据一致性

HipnucIMUDriver 使用 std::shared_mutex 保护 can_sensor_data_t sensor_data_，实现单写多读的并发模型：

写路径（接收线程）：can_rx_cbk 与 serial_rx_cbk 内部使用 std::unique_lock，在解析与更新原始数据时独占锁；
读路径（用户线程）：get_quat()、get_ang_vel()、get_lin_acc()、get_temperature() 内部使用 std::shared_lock，允许多个读取者并发访问。

该设计对于强化学习推理环路至关重要：控制线程与推理线程可能同时读取 IMU 数据，而底层 CAN/串口接收线程以更高优先级持续写入，读写锁在读取远多于写入的场景下比 std::mutex 具有更高的并发吞吐量。

Sources: hipnuc_imu_driver.hpp, hipnuc_imu_driver.cpp

坐标系变换：RobotInterface 中的外参校正

原始 IMU 数据通常以其自身芯片坐标系输出，而机器人控制与推理需要统一的机体坐标系（Body Frame）。RobotInterface 在 robot.yaml 中通过 extrinsic_R 字段配置 IMU 安装外参（3×3 旋转矩阵，行优先），并在读取时执行在线坐标变换。

四元数变换：get_quat() 将 IMU 原始四元数乘以 extrinsic_q_inv_（extrinsic_R 对应的四元数逆），再 normalize()，得到机体坐标系下的姿态；
角速度变换：get_ang_vel() 将 IMU 原始角速度向量左乘 extrinsic_R_mat_，完成从 IMU 坐标系到机体坐标系的旋转。

这种外参校正完全封装在 RobotInterface 内，对上层 InferenceNode 透明。若 IMU 安装姿态与机体完全一致，可将 extrinsic_R 设为单位矩阵，此时变换退化为恒等操作。

Sources: robot_interface.hpp, robot_interface.cpp

上层集成：观测堆叠与 ROS 发布

在 InferenceNode 中，IMU 数据被用于两个核心场景：

推理观测输入：get_ang_vel_obs() 读取机体角速度并乘以 obs_scales_ang_vel_ 进行归一化；get_gravity_b_obs() 读取机体四元数，计算世界坐标系重力向量在机体坐标系下的投影（gravity_b），用于表征机体倾斜状态。若 gravity_b.z() 超过安全阈值，节点将触发紧急停机，防止机器人倒地后继续输出力矩。
ROS 2 话题发布：publish_imu() 将四元数与角速度封装为 sensor_msgs/msg/Imu，发布到 /imu 话题，供外部可视化与诊断工具订阅。

此外，read_imu ROS 2 服务允许外部节点通过服务调用触发一次 IMU 数据读取与发布，便于调试阶段手动验证传感器连接。

Sources: obs_manager.cpp, ros_interface.cpp, ros_interface.cpp

Python SDK 与 pybind11 绑定

roboto_imu 通过 pybind11 暴露为 Python 模块 imu_py，使得算法工程师无需编写 C++ 即可快速验证传感器数据。绑定接口保持与 C++ 完全一致，返回 list[float] 而非 NumPy 数组，避免了额外的依赖。

from imu_py import IMUDriver

imu = IMUDriver.create_imu(
    imu_id=0x08,
    interface_type="can",
    interface="can0",
    imu_type="HIPNUC",
    baudrate=0
)

quat = imu.get_quat()        # [w, x, y, z]
gyro = imu.get_ang_vel()     # [x, y, z] rad/s
accel = imu.get_lin_acc()    # [x, y, z] m/s²

pybind_module.cpp 中使用了 std::shared_ptr<IMUDriver> 作为持有类型，确保 Python 对象生命周期与底层 C++ 智能指针一致，避免跨语言边界时出现悬空指针。

Sources: pybind_module.cpp, scripts/imu_py_example.py

配置与初始化

机器人配置文件（robot.yaml）

RobotInterface 通过 YAML 配置初始化 IMU，典型配置如下：

imu:
  imu_id: 8                # CAN 节点 ID 或串口设备号
  imu_type: "HIPNUC"
  imu_interface_type: "can"   # 或 "serial"
  imu_interface: "can0"       # 或 "/dev/ttyUSB0"
  baudrate: 0                 # 串口场景下设置为 115200 或 921600

IMU 初始化脚本（init_imu.sh）

当使用 CAN 接口时，若需修改 HiPNUC IMU 的报文输出或波特率，可通过 init_imu.sh 下发 J1939 配置指令。该脚本默认将设备波特率从 500K 修改为 1M，并开启四元数输出、关闭欧拉角输出。执行后需手动将主机 CAN 接口同步到新的波特率。

Sources: robot_interface.cpp, init_imu.sh

构建系统与依赖

roboto_imu 使用 CMake 构建，核心依赖包括 spdlog、fmt、pybind11 与 ament_cmake（ROS 2 场景）。构建产物包括：

目标	类型	说明
`imu`	静态库	抽象接口 + 工厂方法
`hipnuc_imu`	静态库	HiPNUC 驱动 + C 协议解码器
`imu_protocol`	静态库	SocketCAN + 串口协议层
`imu_py`	Python 模块	pybind11 共享库

在 ROS 2 工作空间中，通过 colcon build --packages-select roboto_imu 即可编译。

Sources: CMakeLists.txt, package.xml

下一步阅读建议

理解 IMU 驱动后，建议继续阅读以下关联页面，以建立从传感器到控制指令的完整认知：

若需理解 IMU 数据如何在控制环路中被消费，请参阅 RobotInterface 硬件抽象层；
若需了解底层 CAN/CANFD 的帧结构与电机总线共享机制，请参阅通信协议层：CAN/CANFD 与串口；
若需理解 IMU 观测如何与关节状态一起堆叠为策略输入，请参阅观测堆叠与多策略切换。