0
$\begingroup$

Rosanswers logo

I am using robot_localization package for the localization of a robot placed outdoors and equipped with RTK GPS, wheel odometry and IMU. When the robot is placed somewhere and is not moving, I can notice that there is a drift in the yaw output of the filtered odometry.
I am using one instance for odom->base_link transform, only with wheel odometry and IMU. The map-> base_link transform is using wheel odometry, IMU and the GPS. Why is this happening?

Also, when the robot starts moving we notice in Rviz that the robot performs a full rotation around itself before moving forward, while the raw IMU measurements do not show any such rotation.

Below is the configuration that I use with the robot_localization for odom->base link.

frequency: 30
sensor_timeout: 0.1
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.2
print_diagnostics: true
debug: false
publish_tf: true

map_frame: map
odom_frame: robot_odom
base_link_frame: robot_base_footprint
world_frame: robot_odom

# Wheel odometry:
odom0: wheel_odom
odom0_config: [false, false, false,
               false, false, false,
               true,  true,  false,
               false, false, false,
               false, false, false]
odom0_queue_size: 10
odom0_nodelay: false
odom0_differential: true
odom0_relative: false

# imu configure:
imu0: imu/data
imu0_config: [false, false, false,
          false,  false, true,
          false, false, false,
          false,  false,  true,
          false,  false,  false]
imu0_nodelay: true
imu0_differential: true
imu0_relative: false
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true
gravitational_acceleration: 9.7530

initial_estimate_covariance: [1.0,  0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                          0,    1.0,  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,    1.0,  0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                          0,    0,    0,    0,    1.0,  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,    1.0,  0,    0,    0,     0,     0,     0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,    1.0,  0,    0,     0,     0,     0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,    0,    1.0,  0,     0,     0,     0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,    0,    0,    1.0,   0,     0,     0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1.0,   0,     0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1.0,   0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1.0,  0,    0,
                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1.0,  0,
                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1.0]

For map->odom transformation, the configuration is below.

frequency: 20
sensor_timeout: 0.1
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.2
print_diagnostics: true
debug: false
publish_tf: true

map_frame: map
odom_frame: robot_odom
base_link_frame: robot_base_footprint
world_frame: map

# Wheel odometry:
odom0: wheel_odom
odom0_config: [false, false, false,
               false, false, false,
               true,  true,  false,
               true, true, false,
               false, false, false]
odom0_queue_size: 10
odom0_nodelay: false
odom0_differential: true
odom0_relative: false

# GPS odometry:
odom1: odometry_filtered/gps
odom1_config: [true,  true,  false,
               false, false, false,
               false, false, false,
               false, false, false,
               false, false, false]
odom1_queue_size: 1
odom1_nodelay: false
odom1_differential: false
odom1_relative: false

imu0: imu/data
imu0_config: [false, false, false,
          false,  false, true,
          false, false, false,
          false,  false,  true,
          false,  false,  false]
imu0_nodelay: true
imu0_differential: true
imu0_relative: false
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true
gravitational_acceleration: 9.7530

initial_estimate_covariance: [1.0,  0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                          0,    1.0,  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,    1.0,  0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                          0,    0,    0,    0,    1.0,  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,    1.0,  0,    0,    0,     0,     0,     0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,    1.0,  0,    0,     0,     0,     0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,    0,    1.0,  0,     0,     0,     0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,    0,    0,    1.0,   0,     0,     0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1.0,   0,     0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1.0,   0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1.0,  0,    0,
                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1.0,  0,
                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1.0]

Below I attach a sample message from each sensor input of the EKF

/gps/fix of type sensor_msgs/NavSatFix

 header:   seq: 32664   stamp:
     secs: 1661508366
     nsecs:  99655800   frame_id: "rbvogui_robot_gps_base_link" status: 
 status: 2   service: 3 latitude:
 40.5665821 longitude: 22.9988951 altitude: 121.998 position_covariance:
 [0.00019600000000000002, 0.0, 0.0,
 0.0, 0.00019600000000000002, 0.0, 0.0, 0.0, 0.0001] position_covariance_type: 2

/odometry_filtered/gps of type nav_msgs/Odometry

header:   seq: 1676   stamp:
    secs: 1661508365
    nsecs: 999655841   frame_id: "map" child_frame_id: '' pose:   pose:
    position:
      x: -0.354980841282
      y: -0.125639199847
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0   covariance: [0.000196, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.000196, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001, 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] twist:   twist:
    linear:
      x: 0.0
      y: 0.0
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: 0.0   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/data of type sensor_msgs/Imu

header:
  seq: 164366
  stamp:
    secs: 1661508366
    nsecs: 125832720
  frame_id: "rbvogui_robot_imu_link"
orientation:
  x: -0.00217130490235
  y: 0.0141727779251
  z: -0.182312654489
  w: 0.983136060682
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity:
  x: 0.00039325808757
  y: -0.000539525935892
  z: -0.000493917963468
angular_velocity_covariance: [1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07]
linear_acceleration:
  x: -0.24476377666
  y: 0.331257969141
  z: 9.89319229126
linear_acceleration_covariance: [8.999999999999999e-08, 0.0, 0.0, 0.0, 8.999999999999999e-08, 0.0, 0.0, 0.0, 8.999999999999999e-08]

/odom of type nav_msgs/Odometry

header:
  seq: 164160
  stamp:
    secs: 1661508366
    nsecs: 124339813
  frame_id: "rbvogui_robot_odom"
child_frame_id: "rbvogui_robot_base_footprint"
pose:
  pose:
    position:
      x: 4.39330503478
      y: -4.54358692208
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: -0.964641142203
      w: 0.263566816518
  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]
twist:
  twist:
    linear:
      x: -0.000237381657637
      y: 0.0
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: 0.000494545131131
  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]

Originally posted by kosmastsk on ROS Answers with karma: 210 on 2022-07-21

Post score: 2


Original comments

Comment by alexppdm on 2022-07-27:
Hi, Have you found any solution? I also have issues with a similar configuration. The issue I am facing is mainly the same (robot drifting when it is stopped) but there is also another major problem. When the node starts and the robot starts moving the odometry/filtered rotates and jumps around for a while until it moves for about 3 meters. Afterward, the localization works fine until it stops and the drift you mentioned is presented again.

Comment by Tom Moore on 2022-08-26:
Have you tried looking at the raw IMU data? Is it also drifting? The EKF won't be able to correct for erroneous data.

Comment by Tom Moore on 2022-08-26:
Please add a sample message from each sensor input.

Comment by kosmastsk on 2022-08-26:
I added a sample message from each sensor input above.

Comment by kosmastsk on 2022-08-26:
By looking the raw IMU data from the robot, we notice that there is a small drift in the measuments, so you're right this must be the cause for drifting while not moving. But when the robot starts moving, the IMU measurement has normal values, but the robot in Rviz rotates around itself before moving straight. Is this somehow related to the IMU?

Comment by Tom Moore on 2022-09-26:
I think @tomarRobin's answer is correct, but to confirm, you can just turn off inputs. First, make sure you enable yaw velocity from the wheel encoders (and leave that on; there's no reason to have it disabled). Next, try not fusing the IMU at all. If the EKF output looks OK, throw in yaw velocity only from the IMU. If it still looks OK, then you obviously have an issue with the magnetometer data (i.e., the yaw) from the IMU. But my guess is that tomarRobin's thoughts will be accurate, and your angular velocity from the IMU has nonzero values when stationary (this could be in addition to the mag data being bad). You can modify the IMU driver to do what was suggested in the answer below or you can write an interim node, as suggested.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

Hello folks, the drift you see in the output of robot localization is due to imu data. Most IMUs have this, depends on how advance/expansive the device is. The way around could be to write a simple imu correction node which takes in raw imu data and raw odometry from wheels, if the read linear velocity is very close to 0 you simply put yaw rate to 0. And publish a new imu message from this node with this correction and use that as a input to robot localization. So anytime robot stops we will stop pushing yaw drift to robot localization and the output will be drift free.

Also for the robot rotating when you start again is because the odom frame has already drifted due to yaw drift and now robot thinks it is facing some other direction and it tries to correct its heading before moving.

P.s. I am just telling you a way to make it work, surely there must be something better than this but it works.


Originally posted by tomarRobin with karma: 111 on 2022-09-01

This answer was ACCEPTED on the original site

Post score: 2

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.