笔者将展开一下内容如何判断里程计是否正确是否存在明显漂移如何进行ekf融合odom和imu一、 里程计的验证方式一打开Rviz,并配置好Odometry,在终端发送指令———小车将做圆周运动rostopic pub /cmd_vel geometry_msgs/Twist linear: x: 0.5 y: 0.0 z: 0.0 angular: x: 0.0 y: 0.0 z: 0.5 图片如下可以看到里程计非常正确。方式二# 查看里程计输出的变换rosrun tf tf_echo odom base_link小车不动判读是否有明显变化有明显变化说明传感器有问题了小车动查看odom到base_link的变换是否随时间平滑增长。里程计的消息#打印一条消息rostopicecho-n1/odom输出如下header: seq:9436stamp: secs:94nsecs:670000000frame_id:odomchild_frame_id:base_footprintpose: pose:#位姿 位置四元数协方差position: x: -2.1401414463866968e-05 y:0.0077910415807742744z: -3.4563704238987913e-08 orientation: x: -1.1606043130080346e-07 y: -1.0459215449422278e-08 z:0.00018421187923166863w:0.9999999830329849covariance:[1e-05,0.0,0.0,0.0,0.0,0.0,0.0, 1e-05,0.0,0.0,0.0,0.0,0.0,0.0,1000000000000.0,0.0,0.0,0.0,0.0,0.0,0.0,1000000000000.0,0.0,0.0,0.0,0.0,0.0,0.0,1000000000000.0,0.0,0.0,0.0,0.0,0.0,0.0,0.001]twist:#速度 线速度角速度 协方差twist: linear: x: -2.623882599008297e-07 y:6.450091786769381e-08 z:0.0angular: x:0.0y:0.0z:9.537386972954804e-08 covariance:[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]---IMU的消息rostopicecho-n1/imu终端输出header: seq:0stamp: secs:63nsecs:2000000frame_id:imu_linkorientation: x: -1.312914517275492e-07 y: -1.765048059772218e-08 z:0.00021882219936333975w:0.9999999760584136orientation_covariance:[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]angular_velocity: x:-0.00010329586138951379y:1.3167912071328429e-05 z: -4.082601877550837e-05 angular_velocity_covariance:[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]linear_acceleration: x:-0.0040040293128828395y:0.003198740830602146z:9.804194287400534linear_acceleration_covariance:[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]---二、基础知识矩阵[ X, Y, Z #坐标 roll, pitch, yaw #姿态翻滚 俯仰 偏航 X/dt, Y/dt, Z/dt #线速度 roll/dt, pitch/dt, yaw/dt #角速度 X/dt2, Y/dt2, Z/dt2 ] #机器人线性加速度上述矩阵的值要么是true要么是false三、对官方的参考打开下载的软件包其他位置-计算机-/opt/ros/noetic/share/robot_localization/读者可以自行打开自己下的软件包里的实例代码这里笔者对注释进行了中文翻译也小修改了一些参数# 滤波器输出位姿估计的频率Hz。滤波器收到任意一路输入消息后才开始运算# 启动后持续以该频率运行不受传感器消息到达频次影响。不设置默认30Hz。frequency:30silent_tf_failure:false# 传感器超时阈值秒。超过该时长未收到传感器数据滤波器仅执行预测步不做观测修正。# 可理解为滤波器持续输出的最低刷新频率。不指定默认等于1/frequency。sensor_timeout:0.1# ekf/ukf定位节点原生使用3D全向运动模型。开启后状态估计将忽略所有三维信息。# 适用于平面移动机器人消除IMU捕捉到地面微小起伏带来的干扰。不设置默认false。two_d_mode:true# 为滤波器发布的TF变换添加时间偏移用于时间预补偿适配部分第三方功能包。默认0.0。transform_time_offset:0.0# 设置TF监听器等待坐标变换可用的超时时间。不设置默认0.0。transform_timeout:0.0# 调试建议开启监听 /diagnostics_agg 话题查看节点上报的参数、数据异常信息。print_diagnostics:true# 调试模式。开启后输出海量矩阵日志严重占用CPU正常运行务必关闭。默认false。debug:false# 调试日志保存路径不指定默认 robot_localization_debug.txt需要填写完整路径。debug_out_file:/path/to/debug/file.txt# 是否通过 /tf 话题广播坐标变换。默认true。publish_tf:true# 是否输出加速度状态。默认false。publish_acceleration:false# 是否允许使用历史测量值重新发布更新后的状态。permit_corrected_publication:false# ROS REP-105定义四个核心坐标系base_link、odom、map、earth。# base_link 固连机器人本体odom、map 为世界固定坐标系。# 机器人在odom坐标系下长时间运行会产生漂移但短时精度高、轨迹连续适合局部运动规划。# map坐标系同样为世界固定坐标系全局位姿更准确但容易出现位姿跳变GPS、地图定位修正等场景。# earth坐标系用于关联多个map坐标系ekf、ukf节点不使用该坐标系。# 参数使用规则# 1. 根据系统配置修改 map_frame、odom_frame、base_link_frame 名称# 1a. 如果系统没有map坐标系删除该参数并将 world_frame 设置为 odom_frame# 2. 融合编码器里程计、视觉里程计、IMU这类连续数据时world_frame 设置为 odom_frame默认配置# 3. 如果融合GPS、路标定位等会产生跳变的全局绝对位置数据# 3a. world_frame 设置为 map_frame# 3b. 必须保证有其他节点发布 odom-base_link 变换可以是另一组robot_localization节点# 该节点**不要融合全局定位数据**map_frame:map# 不设置默认 mapodom_frame:odom# 不设置默认 odombase_link_frame:base_footprint# 不设置默认 base_linkworld_frame:odom# 不设置默认与 odom_frame 一致# 滤波器支持多类消息输入nav_msgs/Odometry、PoseWithCovarianceStamped、TwistWithCovarianceStamped、sensor_msgs/Imu# 可以添加多路传感器命名按序号递增 odom0、odom1、imu0、imu1参数值填写对应话题名必须手动配置。odom0:/odom# 控制传感器哪些观测量送入滤波器更新状态。# 顺序 x, y, z,# roll, pitch, yaw,# vx, vy, vz,# vroll, vpitch, vyaw,# ax, ay, az# 示例只使用里程计Z轴位置则仅第三个参数设true其余false。# 注意部分消息不具备全部状态量例如Twist消息不存在位姿信息前6项参数无效。# 每组传感器参数默认全部false必须手动配置。odom0_config:[true,true,false,false,false,true,true,true,false,false,false,true,false,false,false]# 高频传感器数据场景可增大订阅队列长度保证更多测量值参与融合。odom0_queue_size:2# 进阶参数ROS高频传输大数据会受Nagle算法影响产生异常。开启后订阅器启用tcpNoDelay关闭Nagle算法。odom0_nodelay:false# 进阶参数两个传感器同时观测同一个位姿量且协方差设置偏小滤波器会在两组测量之间来回震荡。# 开启微分模式后传感器绝对位姿数据会通过差分转为速度量参与融合。# 仅对带位姿信息的传感器生效twist速度类传感器开启无效。odom0_differential:false# 进阶参数节点启动后将第一条测量数据作为该传感器观测零点。# 和differential区别不会把位姿差分转为速度直接进行积分运算。仅需要观测从0开始时开启。odom0_relative:false# 进阶参数马氏距离异常剔除阈值过滤偏离当前机器人状态的野值测量。# 不设置默认无穷大无剔除。若无野值问题建议删除该参数。# 区分位姿、速度阈值同时携带位姿速度的消息会分别进行判断。odom0_pose_rejection_threshold:5odom0_twist_rejection_threshold:1# 多路传感器输入示例# odom1: example/another_odom# odom1_config: [false, false, true,# false, false, false,# false, false, false,# false, false, true,# false, false, false]# odom1_differential: false# odom1_relative: true# odom1_queue_size: 2# odom1_pose_rejection_threshold: 2# odom1_twist_rejection_threshold: 0.2# odom1_nodelay: false# pose0: example/pose# pose0_config: [true, true, false,# false, false, false,# false, false, false,# false, false, false,# false, false, false]# pose0_differential: true# pose0_relative: false# pose0_queue_size: 5# pose0_rejection_threshold: 2# pose0_nodelay: false# twist0: example/twist# twist0_config: [false, false, false,# false, false, false,# true, true, true,# false, false, false,# false, false, false]# twist0_queue_size: 3# twist0_rejection_threshold: 2# twist0_nodelay: falseimu0:/imuimu0_config:[false,false,false,false,false,false,false,false,false,false,false,true,false,false,false]imu0_nodelay:falseimu0_differential:falseimu0_relative:trueimu0_queue_size:5imu0_pose_rejection_threshold:0.8imu0_twist_rejection_threshold:0.8imu0_linear_acceleration_rejection_threshold:0.8# 进阶参数部分IMU内部已经去除重力加速度部分没有。# 如果你的IMU输出包含重力分量开启此项同时IMU数据必须遵循REP-103标准坐标系为ENU。imu0_remove_gravitational_acceleration:true# 进阶参数EKF/UKF执行标准预测-修正循环。无加速度观测时预测阶段默认保持上一时刻速度。# 修正阶段融合观测后得到新速度容易造成响应迟缓旋转时激光融合场景尤为明显。# 解决方案调大对应速度量的过程噪声或减小传感器测量噪声# 也可以利用机器人控制指令参与预测指令会换算为加速度参与预测步。# 注意如果已有传感器提供加速度观测则控制指令加速度项失效。# 是否在预测阶段使用控制指令。默认false。use_control:true# 控制指令消息类型false为Twisttrue为TwistStamped。默认false。stamped_control:false# 缓存控制指令有效时长超时后不再使用该指令做预测。默认0.2。control_timeout:0.2# 启用哪些速度控制量顺序vx, vy, vz, vroll, vpitch, vyawcontrol_config:[true,false,false,false,false,true]# 加速度上限需要和机器人实际运动特性匹配acceleration_limits:[1.3,0.0,0.0,0.0,0.0,3.4]# 减速度上限机器人加减速性能通常不一致deceleration_limits:[1.3,0.0,0.0,0.0,0.0,4.5]# 机器人无法瞬间达到最大加速度增益限制单次预测允许的加速度变化量acceleration_gains:[0.8,0.0,0.0,0.0,0.0,0.9]# 机器人无法瞬间达到最大减速度增益限制单次预测允许的减速度变化量deceleration_gains:[1.0,0.0,0.0,0.0,0.0,1.0]# 进阶参数过程噪声协方差矩阵调参难度较高和运动模型匹配度相关。# 数值越小越信任运动模型数值越大滤波器更相信传感器观测。# 状态顺序x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az# 某状态收敛慢可以适当增大对应对角线数值。不设置默认使用下方矩阵。process_noise_covariance:[0.05,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.05,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.06,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.03,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.03,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.06,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.025,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.025,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.04,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.02,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.015]# 进阶参数初始状态估计误差协方差矩阵。# 对应对角线数值设置越大滤波器启动后该状态收敛速度越快。# 注意没有传感器观测的状态不要设置过大数值。# 状态顺序x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az# 不设置默认使用下方矩阵。initial_estimate_covariance:[1e-9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1e-9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1e-9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1e-9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1e-9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1e-9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1e-9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1e-9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1e-9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1e-9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1e-9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1e-9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1e-9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1e-9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1e-9]四、操作流程编写launch文件启动ekf_node节点并加载.yaml参数配置文件参考官网.yaml实例文件结合自己情况配置.yaml文件需要关闭原始的的odom-base_footprint/base_link的坐标变换ekf_node会自己发布一个odom-base_footprint/base_link的坐标变换尽量用这个来实操1、启动仿真机器人发布根据车轮编码器的里程计消息以及IMU消息并且不要再发布odom与basef_footprint的坐标变换了这里还是建议使用 ros_control / ros2_control 框架控制器参数通过 YAML 配置经过笔者实践gazebo内置的插件貌似有点问题还是用ros_controller_config形式好一点2、调用robot_localization的ekf_node实现融合launch!-- 扩展卡尔曼滤波 — 融合 odom imu robot_localization ekf_localization_node 输出 /odometry/filtered 以及 odom - base_footprint TF --nodepkgrobot_localizationtypeekf_localization_nodenameekf_localizationoutputscreenclear_paramstruerosparamcommandloadfile$(find my_car)/config/ekf_localization.yaml//node/launch问题排查下图这种情况小车在gazebo做圆周运动但是rviz小车不走原因有odom话题但是odom没有消息内容,可以在终端输出以下指令自行排查rostopic list rostopic echo -n 1 /odom笔者能力有限可能思路比较乱还请谅解这里有实物的话建议观看b站赵虚左老师的ros2传感器的课程里面会有基础的解释当让防真的也可以看加深理解