MuJoCo 差速小车滑移仿真与数据集采集框架（编码器 + IMU）

MuJoCo Differential-Drive Slip Perception and Neural Trajectory Compensation Using Wheel Encoders and an IMU

模块概述 / Overview

该模块使用 MuJoCo 3.12.0 构建四轮差速小车，在 ROS 2 Humble 中提供统一的速度控制、仿真时钟、真实里程计与坐标变换接口。系统以编码器和惯性测量单元数据为主要观测量，后续可在多摩擦条件下建立轮滑感知与轨迹补偿流程。

The module uses MuJoCo 3.12.0 to model a four-wheel differential-drive robot and exposes velocity control, simulation time, ground-truth odometry, and transform interfaces in ROS 2 Humble. Wheel encoder and inertial measurement unit data form the primary observations for subsequent slip perception and trajectory compensation across multiple friction conditions.

仿真验证局限与分布偏移说明

本模块的轮滑感知模型与全部样例数据均在 MuJoCo 仿真环境中生成、训练和评估，尚未在真实机器人硬件上测试。仿真摩擦模型、接触动力学与传感器噪声和真实世界存在差异，迁移到实物时可能出现分布偏移（sim-to-real gap）；本文档报告的指标仅对应 MuJoCo 样例测试划分，不代表实物性能。真实硬件验证列为后续工作。

系统结构 / Architecture

/cmd_vel
   |
   v
差速运动学 / Differential kinematics
   |
   v
MuJoCo 四轮执行器 / Four wheel actuators
   |
   +----> /ground_truth/odom
   +----> /clock
   +----> /tf

功能包使用 ament_python，模型、配置与 launch 文件均安装到包共享目录。运行时通过 ROS 2 包索引定位资源，不依赖用户目录、固定工作空间路径或网络地址。

The package uses ament_python; its model, configuration, and launch files are installed into the package share directory. Runtime resource lookup uses the ROS 2 package index and does not depend on a user directory, a fixed workspace path, or a network address.

编码器与 IMU / Encoders and IMU

MJCF 在车体中心定义三轴加速度计和陀螺仪。节点同时读取四个轮关节的位置与角速度，并通过局部随机数生成器加入固定种子的高斯噪声。MuJoCo 内部传感器保持无噪真值，噪声只施加于发布消息，因此实验可重复且便于比较原始状态。

The MJCF defines a three-axis accelerometer and gyroscope at the chassis centre. The node also reads position and angular velocity from all four wheel joints and applies seeded Gaussian noise through a local random generator. MuJoCo sensor values remain noise-free internally; noise is added only to published messages for reproducibility and comparison with the original state.

环境与构建 / Environment and Build

目标环境为 Ubuntu 22.04、ROS 2 Humble、Python 3.10 和 MuJoCo 3.12.0。

The target environment is Ubuntu 22.04, ROS 2 Humble, Python 3.10, and MuJoCo 3.12.0.

python3 -m pip install -r src/ground/mujoco_diff_drive_slip_compensation/requirements.txt
source /opt/ros/humble/setup.bash
colcon build --packages-select mujoco_diff_drive_slip_compensation
source install/setup.bash

启动与检查 / Launch and Inspect

ros2 launch mujoco_diff_drive_slip_compensation main.launch.py
ros2 topic list
ros2 topic echo /ground_truth/odom

另一个终端可发送速度指令：

Send a velocity command from another terminal:

ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist \
  "{linear: {x: 0.4}, angular: {z: 0.25}}"

参数 / Parameters

安全边界 / Safety Boundaries

• 指令含非有限数值时拒绝更新。 / Reject commands containing non-finite values.
• 指令超时后轮速归零。 / Set wheel speeds to zero after command timeout.
• 四轮目标速度按执行器上限等比例缩放。 / Scale all wheel targets proportionally to actuator limits.
• 包资源路径经过边界检查。 / Validate package-resource paths against directory escape.

数据采集 / Data Collection

一条 launch 命令同时启动仿真、确定性激励指令和采集器：

One launch command starts the simulation, deterministic excitation commands, and collector:

ros2 launch mujoco_diff_drive_slip_compensation collect.launch.py \
  output_directory:=data/raw

场景切换时车辆复位，新的 episode_id 与摩擦系数一起发布。采集器在编码器消息到达时组合最近的 IMU、指令、真实里程计和场景信息。每个 episode 独立写入 CSV，并在关闭时写入 JSON 清单。

At each scenario transition, the vehicle is reset and a new episode_id is published with its friction coefficient. On each encoder message, the collector combines the latest IMU, command, ground-truth odometry, and scenario state. Each episode is written to its own CSV and receives a JSON manifest when closed.

数据字段 / Data Fields

• 时间戳、episode、数据集划分、摩擦系数。 / Timestamp, episode, dataset split, and friction coefficient.
• 线速度与角速度指令。 / Linear and angular velocity commands.
• 四轮位置与角速度。 / Four-wheel positions and angular velocities.
• 三轴加速度与角速度。 / Three-axis acceleration and angular velocity.
• 真实二维位姿与速度。 / Ground-truth planar pose and velocity.

数据集使用固定种子和 episode 标识的稳定哈希执行 70%/15%/15% 划分。划分单位是完整 episode，而不是单帧，因此相邻帧不会跨越训练、验证和测试集合。

Dataset assignment uses a stable hash of the fixed seed and episode identifier with 70%/15%/15% ratios. The assignment unit is a complete episode rather than an individual frame, so adjacent frames cannot cross train, validation, and test sets.

滑移感知训练 / Slip-Perception Training

训练器读取第一天生成的 CSV 与 JSON manifest，逐 episode 验证格式版本、行数、时间戳单调性和 split 一致性。窗口长度默认为 12 帧；窗口只从一个 episode 内滑动，不在 episode 或 train/validation/test 边界连接样本。归一化均值和标准差只由 train episode 计算。

The trainer reads the CSV and JSON manifests produced by the collection pipeline and validates schema version, row count, monotonic timestamps, and split consistency per episode. The default window contains 12 frames. Windows never cross episode or train/validation/test boundaries. Feature means and standard deviations are fitted only on train episodes.

当前实现采用紧凑的 temporal-window MLP：tanh 隐藏层连接可训练线性输出头，使用 Huber 损失抑制极少数大滑移比例对常规样本的影响。纵向滑移率标签由编码器轮速与 MuJoCo 真实车体速度计算：

The current model is a compact temporal-window MLP with a tanh hidden layer and a trainable linear skip head. Huber loss limits the influence of rare high-slip ratios. The longitudinal target is derived from encoder wheel speed and MuJoCo ground-truth chassis speed:

$$\bar{\omega}_t = \frac{\omega_{LF,t}+\omega_{LR,t}+\omega_{RF,t}+\omega_{RR,t}}{4},\qquad s_t = \operatorname{clip}\left(1-\frac{v^{truth}_t}{r\bar{\omega}_t},-0.5,0.5\right).$$

当 |r ω̄| < 0.15 m/s 时将标签置零，以避免低速除法放大噪声。训练按固定随机种子打乱 train 窗口，使用 validation MAE 选择模型；测试 episode 不参与归一化或参数选择。模型以轻量 JSON 参数文件保存，不提交权重文件。

Targets are set to zero when |r ω̄| < 0.15 m/s to avoid amplifying noise through division near standstill. Train windows are shuffled with a fixed seed, validation MAE selects the model, and test episodes are excluded from normalization and parameter selection. The compact parameters are saved as JSON and no trained weights are committed.

训练流程 / Training Workflow

episode CSV + manifest
        |
validate schema / split / timestamps
        |
episode-local windows + slip labels
        |
train-only normalization -> MLP fitting
        |
validation selection -> held-out test metrics
        |
slip_model.json + training_metrics.json

生成同格式 MuJoCo 样例数据并训练： / Generate matching MuJoCo sample episodes and train:

generate_slip_sample_data --output data/sample --episodes-per-split 4 --duration 12
train_slip_model --data data/sample --output data/models/slip-v1 --epochs 300

使用 ROS 2 收集数据训练： / Train from ROS 2 collected data:

train_slip_model --data data/raw --output data/models/slip-v1 --epochs 300
ros2 launch mujoco_diff_drive_slip_compensation train.launch.py \
  data_directory:=data/raw output_directory:=data/models/slip-v1

默认数据集与输出目录为相对路径，需在 ROS 2 工作空间根目录（或功能包目录）下执行 launch，或通过对应参数传入绝对路径。

本机样例配置为 12 个 episode、每个 split 4 个完整 episode、12 帧窗口、24 个隐藏单元和 300 轮上限。实际提前停止于验证集不再改善时。隔离测试集含 916 个窗口，测得 MAE=0.0272、RMSE=0.0440、F1=0.774、accuracy=0.969，CPU 单窗口推理中位数为 0.0103 ms。该表仅代表样例测试，不代表其他摩擦和实机性能。样例生成器的 train/validation/test 三个划分使用同一组摩擦系数 0.02 / 0.08 / 0.4 / 0.9，测试集摩擦条件训练时均已出现，样例指标属同分布内插，不代表未见过的摩擦条件。

The measured local run used 12 episodes (four complete episodes per split), a 12-frame window, 24 hidden units, and a 300-epoch maximum with early stopping on validation MAE. Its isolated test split contained 916 windows and measured MAE=0.0272, RMSE=0.0440, F1=0.774, accuracy=0.969, and 0.0103 ms median CPU inference per window. These measurements apply only to this sample split, not other friction settings or physical hardware.

验证命令 / Verification Commands

python3 -m pytest src/ground/mujoco_diff_drive_slip_compensation/test -q
python3 -m compileall src/ground/mujoco_diff_drive_slip_compensation
colcon test --packages-select mujoco_diff_drive_slip_compensation
colcon test-result --verbose

大模型使用声明

本功能包开发过程中使用豆包（Doubao）辅助生成代码草稿、文档文案与测试用例；全部代码、数据集划分与指标均经作者人工复核，训练权重与样例数据由本地 MuJoCo 流程生成，未由大模型直接产出。