IMU（惯性测量单元）为足式机器人提供姿态与角速度反馈，是策略网络观测值的核心来源之一。本文档聚焦于推理节点中 IMU 的硬件接入流程、IMU 坐标系到机器人本体坐标系的变换原理，以及变换后的数据如何被组装为策略输入并对外发布。理解这一链路有助于正确配置外参、排查姿态漂移问题，并为后续接入不同型号 IMU 或修改安装方式提供依据。

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

IMU 硬件接入与初始化

系统通过 RobotInterface 统一封装底层 IMU 驱动。在节点启动时，RobotInterface 读取 config/robot.yaml 中的 imu 字段，解析出设备标识、通信接口类型、设备节点路径、IMU 型号及波特率，随后调用工厂函数 IMUDriver::create_imu() 完成驱动实例化。当前默认配置面向串口型 HIPNUC IMU，挂载于 /dev/ttyUSB0，波特率 921600。RobotInterface 析构时会自动释放 IMU 驱动资源，确保硬件句柄安全关闭。

若 robot.yaml 中缺少 imu 节点，则 imu_ 指针保持为空，后续所有涉及姿态或角速度的调用都会抛出异常。该设计保证了配置即契约：只要 IMU 被配置，系统就假设其在线，并在推理线程中直接消费其数据。

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

坐标系定义与外参配置

机器人控制与策略训练通常以**本体坐标系（Body Frame）为基准，而 IMU 的测量值则在其传感器本地坐标系（IMU Frame）**中表达。当 IMU 的安装姿态与 Body 系不完全重合时，必须引入外参旋转矩阵进行坐标变换。

系统在 robot.yaml 的 robot.extrinsic_R 中配置该外参。它是一个 3×3 旋转矩阵，按行优先展开为 9 个浮点数。在 RobotInterface 构造函数中，这 9 个数被填入 Eigen::Matrix3f extrinsic_R_mat_，并同时被转换为四元数逆 extrinsic_q_inv_，供后续姿态变换使用。默认配置为单位矩阵，意味着当前硬件部署中 IMU 坐标系与 Body 坐标系对齐；若更换安装支架或更换 IMU 型号，开发者只需修改这 9 个数即可，无需改动源码。

robot:
    extrinsic_R: 
        [1.0, 0.0, 0.0, 
         0.0, 1.0, 0.0, 
         0.0, 0.0, 1.0]

以下 Mermaid 图展示了两个坐标系及外参的语义关系。图中 R 即 extrinsic_R，描述从 Body 系到 IMU 系的旋转；其逆则用于将 IMU 测量值映射回 Body 系。

flowchart LR
    subgraph World["世界坐标系 World"]
        direction TB
    end
    subgraph Body["本体坐标系 Body"]
        direction TB
    end
    subgraph IMU["IMU 坐标系 IMU"]
        direction TB
    end

    World -->|"q_body"| Body
    World -->|"q_imu"| IMU
    Body -->|"R (extrinsic_R)"| IMU
    IMU -->|"R⁻¹ / q_R⁻¹"| Body

Sources: robot_interface.cpp, robot.yaml

坐标变换的数学原理与实现

RobotInterface 暴露两个无状态访问接口：get_quat() 与 get_ang_vel()。二者均在读取原始数据后立即执行坐标变换，对外始终返回 Body 系下的结果。

姿态四元数变换在 get_quat() 中完成。原始四元数 q_imu（w, x, y, z 顺序）来自底层驱动，系统通过四元数乘法将其转换到 Body 系：

q_body = q_imu ⊗ q_R⁻¹

其中 q_R 是由 extrinsic_R_mat_ 构造的四元数，q_R⁻¹ 即 extrinsic_q_inv_。乘法完成后执行 normalize() 以消除数值误差，随后将 w, x, y, z 写入缓存并返回引用。

角速度向量变换在 get_ang_vel() 中完成。原始角速度 ω_imu 是三维向量，系统通过矩阵乘法将其映射到 Body 系：

ω_body = extrinsic_R_mat_ × ω_imu

这里直接使用了 Eigen::Map 进行零拷贝映射，将 std::vector<float> 与 Eigen::Vector3f 共享内存，变换结果就地写回 ang_vel_buf_。

两种变换在设计上有明确分工：四元数变换解决姿态对齐问题，向量变换解决角速度对齐问题。由于 get_quat() 与 get_ang_vel() 各自维护独立缓存且返回 const 引用，调用者可以在同一次控制周期内安全地多次访问，而不用担心重复计算引入额外开销。

Sources: robot_interface.hpp

观测值集成：角速度与重力向量

变换后的 IMU 数据通过观测值管理器融入策略网络输入。在 obs_manager.cpp 中，系统注册了两种与 IMU 直接相关的观测源：ang_vel 与 gravity_b。

ang_vel 观测通过 get_ang_vel_obs() 获取。该函数调用 robot_->get_ang_vel() 得到 Body 系角速度，再乘以归一化系数 obs_scales_ang_vel_ 后填入观测段。角速度直接反映机器人躯干的旋转动态，是策略维持平衡的关键反馈。

gravity_b 观测通过 get_gravity_b_obs() 获取。该函数调用 robot_->get_quat() 得到 Body 系姿态，随后将世界系下的单位重力向量 (0, 0, -1) 通过逆四元数 q_w2b = q_b2w.inverse() 旋转到 Body 系，得到 gravity_b。这一步骤等价于计算重力在机器人躯干上的投影，策略网络可据此判断躯干倾斜程度。值得注意的是，该函数同时承担安全监控职责：若 gravity_b.z() > gravity_z_upper_，系统判定机器人已摔倒，立即触发 RCLCPP_FATAL 并调用 rclcpp::shutdown() 终止进程。gravity_z_upper_ 的默认值为 -0.5，但不同策略可在各自 YAML 配置中覆盖，例如 inference_beyondmimic.yaml 将其放宽至 1.0 以适配非直立动作。

flowchart TB
    A["IMU 硬件"] -->|"原始数据"| B["IMUDriver"]
    B -->|"q_imu, ω_imu"| C["RobotInterface"]
    C -->|"坐标变换"| D["Body 系 q_body, ω_body"]
    D -->|"get_ang_vel()"| E["ang_vel 观测段<br/>× obs_scales_ang_vel_"]
    D -->|"get_quat()"| F["gravity_b 观测段<br/>q_w2b × [0,0,-1] × obs_scales_gravity_b"]
    E --> G["策略网络输入"]
    F --> G
    F -->|"z > threshold"| H["摔倒检测<br/>shutdown"]

Sources: obs_manager.cpp, ros_interface.cpp, inference.yaml, inference_beyondmimic.yaml

ROS 2 调试接口

为了便于外部工具（如 RViz、rqt、ros2 topic echo）实时监控 IMU 状态，推理节点在 ROS 2 层提供了话题发布与服务触发两种接口。

话题发布：InferenceNode 构造函数中创建了 /imu 话题发布者，类型为 sensor_msgs::msg::Imu，QoS 采用 KeepLast(1) + best_effort 策略。在 inference() 主循环的每次迭代中，系统调用 publish_imu()，将 robot_->get_quat() 与 robot_->get_ang_vel() 的结果分别填入 orientation 与 angular_velocity 字段并发布。由于发布动作与观测值更新同周期执行，开发者可通过订阅 /imu 获得与策略输入完全一致的数据视图。

服务触发：节点同时注册了 read_imu 服务（std_srvs::srv::Trigger），类型为同步触发。当发送服务请求时，read_imu_srv() 回调会调用 publish_imu() 完成一次即时发布，便于在推理暂停或调试阶段手动拉取 IMU 读数。若 IMU 未初始化，服务将返回 success = false 并提示 "IMU is not initialized, cannot read IMU."。

Sources: inference_node.hpp, inference_node.hpp, ros_interface.cpp, ros_interface.cpp, inference_node.cpp

配置、验证与故障排查

在首次部署或更换 IMU 硬件时，建议按以下顺序验证坐标变换的正确性：

1. 确认外参符号：将机器人静止平放于地面，观察 ros2 topic echo /imu 中的 orientation。若 extrinsic_R 为单位矩阵，则 w 应接近 1.0，x, y, z 接近 0.0；若 IMU 安装姿态与 Body 系存在旋转，需根据实际安装角度修改 extrinsic_R 的 9 个元素。
2. 验证角速度方向：手动绕机器人 Body 系的 Roll、Pitch、Yaw 轴缓慢旋转躯干，检查 /imu/angular_velocity 的符号是否与右手定则一致。若方向相反，说明 extrinsic_R 对应列存在符号错误。
3. 调整摔倒阈值：若机器人在执行大倾角动作（如趴下、翻滚）时被误判为摔倒，可适度放宽对应策略 YAML 中的 gravity_z_upper；反之，若直立时频繁触发保护，则应收紧该值。

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

与系统其他模块的关联

IMU 接入并非孤立存在，它与电机状态、观测值布局、闭环解耦等模块紧密协作。RobotInterface 同时管理电机与 IMU，为上层提供统一的硬件抽象；观测值管理器将 IMU 数据与关节数据按策略 YAML 中声明的 obs_layout 顺序拼接；若需修改观测维度或引入新的 IMU 特征（如加速度计），则应参考 观测布局配置与动态解析 与 新增观测源与模型适配指南。对于 IMU 与 Body 系之间的运动学映射细节，亦可结合 闭环解耦与运动学映射 中的坐标系约定进行交叉验证。