机器人 / 系统课时
用 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,实际结果还会受姿态与噪声影响。

检查输入和测量噪声
适用环境为 Ubuntu 24.04、ROS 2 Jazzy,已安装 robot_localization,轮速与 IMU 节点已经发布数据。以下话题名是本例约定;实际工程要改成自己的话题。先在已加载 Jazzy 环境的终端检查类型:
ros2 topic type /wheel/odom
ros2 topic type /imu/data预期输出(两条命令依次执行):
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。
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 环境的终端运行:
ros2 run robot_localization ekf_node --ros-args --params-file "$HOME/robot_ws/config/ekf.yaml"预期结果:进程持续运行,收到有效传感器数据后发布 /odometry/filtered;启动日志随环境变化,没有固定的完整输出。若参数文件不存在,先核对目录、文件名及 YAML 缩进。
验证运动与姿态稳定性
另开终端执行:
ros2 topic type /odometry/filtered预期输出:
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 连续更新。频率、时间戳、位置和旋转数值会因设备而异。
按以下顺序在安全空旷地面验收,先低速操作;阈值仅为本例验收基线,并非所有传感器的统一指标:
- 静止 60 s:观察位姿变化,目标为平移小于 0.05 m、航向变化小于 0.05 rad;确认不会持续单向旋转。
- 直行约 1 m:测得的平面位移应接近 1 m,记录相对于卷尺测量的误差,例如控制在 0.10 m 内。
- 原地旋转约 90°:检查航向变化约 1.57 rad,允许误差先按 0.10 rad 评估,同时观察 TF 是否突跳。
如果静止时角度持续漂移,优先校准陀螺仪零偏并检查时间戳、安装方向和 angular_velocity_covariance[8];如果直行轨迹弯曲,检查左右轮半径与轮距以及 IMU 坐标轴。若消息正常却完全没有估计值,确认话题名、QoS、传感器时间和所需 TF;如果位置突然跳变,先排查重复发布的 odom 到 base_link 以及错误坐标变换,而不是直接增大噪声掩盖问题。
这里只融合相对速度,因此即使短时间姿态很平滑,积分位置和航向仍会长期漂移。需要地图中的全局校正时,应另外引入定位或 SLAM,并由相应系统维护 map 到 odom 的关系。