ros2_diff_drive

ROS2 差速底盘多传感器运动控制系统：STM32 固件 + 树莓派驱动 + 轮速/IMU EKF 融合里程计。

架构

 ros2_diff_drive/ ├── firmware/              STM32CubeMX + PlatformIO 固件 ├── src/ │   ├── diff_drive_msgs/   自定义消息/服务 │   ├── diff_drive_driver/ C++ 驱动节点（串口/odom/imu） │   └── diff_drive_bringup/ launch + config ├── config/                仓库级参数副本（bringup 包内为安装源） ├── launch/                便捷入口（指向 bringup） ├── scripts/               标定与验收脚本 └── docs/                  calibration / ekf_tuning / tuning_notes

包作用
diff_drive_msgsWheelStatus、DriverDiagnostics、SetParam/GetParam
diff_drive_driverbase_driver_node、xbox_teleop_node
diff_drive_bringupfull/base/ekf/teleop launch + yaml

话题：

话题类型说明
/cmd_velgeometry_msgs/Twist速度指令
/odom_rawnav_msgs/Odometry纯轮速里程计
/imu/datasensor_msgs/ImuMadgwick 姿态 + 角速度
/odometry/filterednav_msgs/OdometryEKF 融合（权威位姿）
/wheel_statusdiff_drive_msgs/WheelStatusticks/轮速/电池/标志
/driver_diagnosticsdiff_drive_msgs/DriverDiagnostics串口丢包/时效
/diagnosticsdiagnostic_msgs/DiagnosticArray标准诊断

服务：/enable /estop /reset_odom /set_param /get_paramTF：odom → base_link（EKF 发布）→ imu_link（static）

快速开始

1. 固件

 cd firmware pio run && pio run -t upload python docs/protocol_test.py /dev/ttyUSB0   # 冒烟：en / vel 100 100

2. ROS2 包

 cd ~/ros2_ws/src ln -s /path/to/ros2_diff_drive/src/diff_drive_msgs . ln -s /path/to/ros2_diff_drive/src/diff_drive_driver . ln -s /path/to/ros2_diff_drive/src/diff_drive_bringup . cd ~/ros2_ws rosdep install --from-paths src -y -i colcon build --packages-select diff_drive_msgs diff_drive_driver diff_drive_bringup source install/setup.bash

3. 起车

 # 一键：驱动 + EKF ros2 launch diff_drive_bringup full.launch.py serial_port:=/dev/ttyUSB0 ​ # 或分步 ros2 launch diff_drive_bringup base.launch.py serial_port:=/dev/ttyUSB0 ros2 launch diff_drive_bringup ekf.launch.py ​ # 遥控 ros2 launch diff_drive_bringup teleop.launch.py      # 键盘 ​ # Xbox 手柄（按住 LB 前进，RB 高速档） ros2 launch diff_drive_bringup xbox_teleop.launch.py ros2 topic echo /joy    # 先看轴/键映射，不对就改 config/xbox_teleop.yaml

验证：

 ros2 topic hz /odom_raw /imu/data /odometry/filtered ros2 run tf2_tools view_frames

标定顺序（重要）

1. IMU 零偏：静止采集 3s，均值写入 config/imu.yaml轮径：直行 5.00m，反推 wheel_radius轮距：原地多圈，反推 wheel_separationEKF 协方差：录 bag，对比 /odom_raw vs /odometry/filtered

 python3 scripts/calibrate_imu_bias.py --duration 5 --apply python3 scripts/calibrate_wheels.py radius --distance 5.0 --apply python3 scripts/calibrate_wheels.py separation --turns 5 --apply

完整流程、误差源与验收清单见 docs/calibration.md。
PID / 协方差 / 安全链总调参笔记见 docs/tuning_notes.md。

验收指标（目标）

指标目标
直线 5m 横向偏差< 10 cm
原地 360° 角度误差< 5°
融合后漂移 vs 纯轮速减少 40%+
心跳超时停车≤ 500 ms

对比脚本：

 ros2 bag record /odom_raw /imu/data /odometry/filtered /tf /cmd_vel python3 scripts/compare_odom_ekf.py --bag <bag> --truth-x 5.0

EKF 协方差怎么调、实验怎么记，见 docs/ekf_tuning.md 与 docs/ekf_tuning_log.md。

硬件接线

见 firmware/docs/wiring.md。注意：MPU6050 走 I2C2（PB10/PB11），不要占用 TIM4 编码器的 PB6/PB7。