robot_localization /map /odom drift

asked 2022-11-08 07:48:19 -0500

157 ●14 ●20 ●23 http://www.roboprotos.com

Hello everyone,

I use ROS2 Foxy along with robot_localization. I have a differential wheeled robot with odometry and 2 GPS rtk for position and heading.

The sensors work properly independently. I have a clean odometry track and the heading is working fine. The problem occurs when I merge them using robot_localization, with two ekf_localisation_node (local and global) and one navsat_transform_node. I observe a shift/drift between /map and /odom frames and I have my odometry track which is getting disturbed.

Under mapviz, using /map as fixed and target frame :

red and orange tracks are similar (/odom and /odometry/filtered/local)
yellow track is /odometry/gps
blue track is tf_frame with /odom frame (which has /map as a direct parent)
black track is /odometry/filtered/global

The blue track can become completely crazy : image description

Here are the parameters used in localization.yaml

navsat_transform:
  ros__parameters:
    magnetic_declination_radians: 0.0387 
    yaw_offset: 0.0 
    zero_altitude: true # True if operating in 2D
    publish_filtered_gps: true
    use_odometry_yaw: true
    wait_for_datum: false
    use_local_cartesian: false
    frequency: 30.0 # Hz
    delay: 0.0 # Seconds
    transform_timeout: 0.0
    broadcast_utm_transform: false
    broadcast_utm_transform_as_parent_frame: false

ekf_local:
  ros__parameters:
    frequency: 30.0 # Hz
    sensor_timeout: 0.1 # Seconds
    two_d_mode: true # No 3D information will be used (zero_altitude already set to true)
    publish_acceleration: false
    publish_tf: true
    reset_on_time_jump: false
    map_frame: map # Defaults to "map" if unspecified
    odom_frame: odom # Defaults to "odom" if unspecified
    base_link_frame: base_footprint # Defaults to "base_link" if unspecified
    world_frame: odom # Defaults to the value of odom_frame if unspecified

    odom0: odom # Wheel odometry
    odom0_config: [true, true, false, # [x, y, z]
                false, false, true, # [roll, pitch, yaw]
                false, false, false, # [x_vel, y_vel, z_vel]
                false, false, false, # [roll_vel, pitch_vel, yaw_vel]
                false, false, false] # [x_accel, y_accel, z_accel]
    odom0_nodelay: true
    odom0_differential: false
    odom0_relative: false
    odom0_queue_size: 30

ekf_global:
  ros__parameters:
    frequency: 30.0 # Hz
    sensor_timeout: 0.1 # Seconds
    two_d_mode: true # No 3D information will be used (zero_altitude already set to true)
    publish_acceleration: false
    publish_tf: true
    reset_on_time_jump: false
    map_frame: map # Defaults to "map" if unspecified
    odom_frame: odom # Defaults to "odom" if unspecified
    base_link_frame: base_footprint # Defaults to "base_link" if unspecified
    world_frame: map # Defaults to the value of odom_frame if unspecified

    odom0: odometry/gps # navsat_transform_node output
    odom0_config: [true, true, false, # [x, y, z]
                false, false, false, # [roll, pitch, yaw]
                false, false, false, # [x_vel, y_vel, z_vel]
                false, false, false, # [roll_vel, pitch_vel, yaw_vel]
                false, false, false] # [x_accel, y_accel, z_accel]
    odom0_nodelay: true
    odom0_differential: false
    odom0_relative: false
    odom0_queue_size: 30

    odom1: odom # Wheel odometry
    odom1_config: [true, true, false, # [x, y, z]
                false, false, false, # [roll, pitch, yaw]
                false, false, false, # [x_vel, y_vel, z_vel]
                false, false, false, # [roll_vel, pitch_vel, yaw_vel]
                false, false, false] # [x_accel, y_accel, z_accel]
    odom1_nodelay: true
    odom1_differential: false
    odom1_relative: false
    odom1_queue_size: 30

    imu0: gps_heading
    imu0_config: [false, false, false, # [x, y, z]
                false, false, true, # [roll, pitch, yaw]
                false, false, false, # [x_vel, y_vel, z_vel]
                false, false, false, # [roll_vel, pitch_vel, yaw_vel]
                false, false, false] # [x_accel, y_accel, z_accel]
    imu0_nodelay: false
    imu0_differential: false
    imu0_relative: false
    imu0_queue_size: 30

Do you have any idea what is causing this behavior ?

Many thanks

answered 2023-04-21 02:57:39 -0500

Tom Moore

flag of United Kingdom of Great Britain and Northern Ireland

13689 ●74 ●240 ●232 http://github.com/ayrton04

I observe a shift/drift between /map and /odom frames and I have my odometry track which is getting disturbed.

Any other issues notwithstanding, you absolutely will see the /map and /odom frames drift from one another. This is expected and correct. See REP-105 for details.

You should really include a sample message from each sensor input, but I can see some things in your config that I would address:

In your ekf_local instance, instead of fusing the robot's wheel encoder pose data, I'd recommend fusing the wheel encoder velocity data, if the sensor produces it. You only have one source here, so you can also leave this as-is if you like. However, I question the need for the first EKF instance then: you might as well see if your wheel encoder odometry node supports publishing the odom->base_link transform itself. If it does, you can drop ekf_local.
Your biggest issue in the ekf_global instance is that you are fusing two different sources of absolute X and Y position data, one from your wheel encoders and one from your GPS data. Those sources will not agree with one another at all over time, and will diverge. Your state estimate will start to jump back and forth as it receives each measurement. You need to instead fuse the velocity data from the wheel encoders if it produces it (get rid of the XY position data). If your wheel encoders don't produce that data, then fuse X, Y, and yaw, but turn on differential mode.
Does your gps_heading topic have the same coordinate frame as the XY position data? In other words, if your odometry/gps data moved the robot from (0, 0) to (1, 1), would the gps_heading topic read pi / 4? I am guessing not. Those have to be in agreement (or the relevant transform must exist to make sure they do). If they disagree, the prediction step will project the robot forward in the direction of the heading, but then get corrected to the odometry/gps data's XY coordinate, which will give the estimate a weird saw-tooth look.

I could comment more with sample sensor messages, but those are definitely issues that should be addressed.

edit flag offensive delete link

Comments

Hi Tom, thanks a lot for your comment. Your suggestions are exactly the kind of issues we have encounter. First, the gps_heading which was generated from 2 GPS had an inverted direction, so it worked already much better after putting a value of Pi for the navsat_transform yaw_offset. We also moved its TF above the middle of the wheels to avoid the saw-tooth effect. We recently tried without ekf_local and will check this idea of fusing the wheel velocity or differential position.

Cyril Jourdan ( 2023-04-26 16:33:48 -0500 )edit

add a comment

robot_localization /map /odom drift

1 Answer

Comments

Question Tools

Stats

Related questions

robot_localization /map /odom drift edit

1 Answer

Comments

Question Tools

Stats

Related questions

robot_localization /map /odom drift