机器人 / 系统课时

用 EKF 融合轮速与 IMU 数据

用 EKF 融合轮速与 IMU 数据

本课用 ROS 2 Jazzy 的 robot_localization 将轮式里程计的前向速度与 IMU 的偏航角速度融合,输出连续的局部位姿。完成后,你能启动 EKF、检查协方差和 TF,并通过静止、直行与旋转实验判断估计是否可信。

学习目标

  • 识别 EKF 状态、轮速与 IMU 观测及其噪声来源。
  • 配置 ekf_node 的观测掩码、坐标系和过程噪声矩阵。
  • 验证 /odometry/filtered 的连续性、漂移及 TF 发布关系。

选取状态与观测

robot_localization 的状态向量按 x、y、z、roll、pitch、yaw、vx、vy、vz、vroll、vpitch、vyaw、ax、ay、az 排列,共 15 维。地面差速机器人设 two_d_mode: true,主要关注平面位置、航向、前向速度和偏航角速度。

这次只选两项独立观测:轮式 /wheel/odom 的 twist.twist.linear.x(vx),以及 /imu/data 的 angular_velocity.z(vyaw)。不再同时融合由同一组编码器积分出的轮式位置、航向和角速度,以免重复计算相关信息。

EKF 每个周期先按运动模型预测状态及其不确定性 P,再用观测更新。Q 表示运动模型的过程噪声;R 来自每条传感器消息的协方差。R 越大,滤波器越不信任该测量。举例:机器人朝前以 0.20 m/s 运动、偏航角速度为 0.10 rad/s,经过 0.1 s,理想情况下约前进 0.02 m、转动 0.01 rad,实际结果还会受姿态与噪声影响。

轮速与 IMU 经 EKF 预测和观测更新得到局部位姿的数据流

检查输入和测量噪声

适用环境为 Ubuntu 24.04、ROS 2 Jazzy,已安装 robot_localization,轮速与 IMU 节点已经发布数据。以下话题名是本例约定;实际工程要改成自己的话题。先在已加载 Jazzy 环境的终端检查类型:

bash
ros2 topic type /wheel/odom
ros2 topic type /imu/data

预期输出(两条命令依次执行):

text
nav_msgs/msg/Odometry
sensor_msgs/msg/Imu

确认轮速消息的 child_frame_id 对应车体坐标(如 base_link),IMU 的 header.frame_id 对应实际安装坐标(如 imu_link),而且 TF 能把 IMU 数据变换到 base_link。IMU 应符合 ROS 的 ENU 坐标约定;时间戳需与机器人其他节点使用同一时钟。

以标定所得标准差为例:轮式前向速度 0.03 m/s,消息中的 twist.covariance[0] 应填方差 0.0009;IMU 偏航角速度 0.02 rad/s,其 angular_velocity_covariance[8] 应为 0.0004。这只是数值示范,应由静止采样与运动对照实验重新估计。不要把标准差直接填入方差字段,也不要把未知误差写成零方差。

配置并启动 EKF

将下面的完整 ROS 2 参数文件保存为 $HOME/robot_ws/config/ekf.yaml,目录不存在时先创建。ekf_filter_node 必须与 ekf_node 的默认节点名一致。布尔数组的 15 个位置严格对应前述状态顺序,只有 odom0_config 的第 7 项和 imu0_config 的第 12 项为 true。

yaml
ekf_filter_node:
  ros__parameters:
    frequency: 30.0
    sensor_timeout: 0.2
    two_d_mode: true
    publish_tf: true
    map_frame: map
    odom_frame: odom
    base_link_frame: base_link
    world_frame: odom

    odom0: /wheel/odom
    # 只采纳编码器估计的前向速度 vx
    odom0_config: [false, false, false,
                   false, false, false,
                   true, false, false,
                   false, false, false,
                   false, false, false]

    imu0: /imu/data
    # 只采纳陀螺仪偏航角速度 vyaw
    imu0_config: [false, false, false,
                  false, false, false,
                  false, false, false,
                  false, false, true,
                  false, false, false]

    # Q 的 15 个对角元素;非对角项为 0
    process_noise_covariance: [0.05, 0.05, 0.06,
                               0.03, 0.03, 0.06,
                               0.025, 0.025, 0.04,
                               0.01, 0.01, 0.02,
                               0.01, 0.01, 0.015]

这组 Q 对角线以软件默认量级作为起点,而传感器测量噪声 R 直接取输入消息的协方差,不在此 YAML 中额外捏造。若旋转估计跟随过慢,可先检查 IMU 偏航角速度的 R 是否被高估,再小幅调整 Q 中 vyaw 对应的第 12 项(本例为 0.02),每次只修改一个量并重测。具体配置语义可核对 robot_localization 参数文档。

启动前须关闭轮式里程计节点对 odom 到 base_link 的重复 TF 发布,让本 EKF 独占这条变换;world_frame: odom 表示只估计连续局部里程计,不发布全局 map 到 odom 校正。在另一个已加载 Jazzy 环境的终端运行:

bash
ros2 run robot_localization ekf_node --ros-args --params-file "$HOME/robot_ws/config/ekf.yaml"

预期结果:进程持续运行,收到有效传感器数据后发布 /odometry/filtered;启动日志随环境变化,没有固定的完整输出。若参数文件不存在,先核对目录、文件名及 YAML 缩进。

验证运动与姿态稳定性

另开终端执行:

bash
ros2 topic type /odometry/filtered

预期输出:

text
nav_msgs/msg/Odometry

类型正确只证明话题存在;还需要通过 ros2 topic hz /odometry/filtered 检查持续发布(目标接近配置的 30 Hz),并用 ros2 topic echo /odometry/filtered --once 查看 header.frame_id: odom、child_frame_id: base_link、位置、姿态四元数及协方差。通过 ros2 run tf2_ros tf2_echo odom base_link 确认 TF 连续更新。频率、时间戳、位置和旋转数值会因设备而异。

按以下顺序在安全空旷地面验收,先低速操作;阈值仅为本例验收基线,并非所有传感器的统一指标:

  1. 静止 60 s:观察位姿变化,目标为平移小于 0.05 m、航向变化小于 0.05 rad;确认不会持续单向旋转。
  2. 直行约 1 m:测得的平面位移应接近 1 m,记录相对于卷尺测量的误差,例如控制在 0.10 m 内。
  3. 原地旋转约 90°:检查航向变化约 1.57 rad,允许误差先按 0.10 rad 评估,同时观察 TF 是否突跳。

如果静止时角度持续漂移,优先校准陀螺仪零偏并检查时间戳、安装方向和 angular_velocity_covariance[8];如果直行轨迹弯曲,检查左右轮半径与轮距以及 IMU 坐标轴。若消息正常却完全没有估计值,确认话题名、QoS、传感器时间和所需 TF;如果位置突然跳变,先排查重复发布的 odom 到 base_link 以及错误坐标变换,而不是直接增大噪声掩盖问题。

这里只融合相对速度,因此即使短时间姿态很平滑,积分位置和航向仍会长期漂移。需要地图中的全局校正时,应另外引入定位或 SLAM,并由相应系统维护 map 到 odom 的关系。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

用 EKF 融合轮速与 IMU 数据

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通