Ask Your Question
1

robot_localization transform from map to odom drifts

asked 2021-05-31 19:10:33 -0600

gmsanchez gravatar image

Hi, I am trying to set up robot_localization on an outdoor mobile robot that I am building from scratch. The vehicle is a four wheeled mobile robot and the onboard computer is a Raspberry Pi 3 with a Navio2 hat from Emlid. The Navio2 has two IMUs: a MPU9250 and a LSM9DS1 and a U-blox M8N GPS (among other sensors). I developed a navio2_ros package to access those sensors data as ROS nodes. In my setup I am currently using the odometry from the DC motors, the LSM9DS1 and the GPS.

I have properly configured the IMU following the documentation and calibrated the magnetometer using the robot_calibration package. The gyroscope and accelerometer bias are removed with the imu_complementary_filter package and then imu_filter_madgwick package provides the orientation in ENU using the magnetometer. When the vehicle faces East, the yaw angle is 0 and when the vehicle faces North, the yaw angle is 90 degrees.

Then I have tried to configure a dual EKF setup with two ekf_localization_node and a navsat_transform_node. My launch file goes like this

  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_odom" clear_params="true" output="screen">
    <remap from="odometry/filtered" to="odometry/filtered_odom"/>
  </node>

  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_map" clear_params="true" output="screen">
    <remap from="odometry/filtered" to="odometry/filtered_map"/>
  </node>

  <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" clear_params="true" output="screen">
    <remap from="odometry/filtered" to="odometry/filtered_map"/>
    <remap from="gps/fix" to="/gps/fix" />
  </node>

and the YAML configuration file has the following content

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

  map_frame: map
  odom_frame: odom
  base_link_frame: base_link
  world_frame: odom

  odom0: hibachi_velocity_controller/odom
  odom0_config: [false, false, false,
                 false, false, false,
                 true,  true,  true,
                 false, false, true,
                 false, false, false]
  odom0_queue_size: 10
  odom0_nodelay: true
  odom0_differential: false
  odom0_relative: false

  imu0: imu/data
  imu0_config: [false, false, false,    # x,   y,   z
                true,  true,  true,     # r,   p,   w (yaw)
                false, false, false,    # dx,  dy,  dz
                true,  true,  true,     # dr,  dp,  dw
                false, false, false]    # ddx, ddy, ddz
  imu0_nodelay: true
  imu0_differential: true
  imu0_queue_size: 10
  imu0_remove_gravitational_acceleration: true

  use_control: false

ekf_se_map:
  frequency: 20
  sensor_timeout: 0.1
  two_d_mode: false
  transform_time_offset: 0.0
  transform_timeout: 0.0
  print_diagnostics: true
  debug: false
  dynamic_process_noise_covariance: false

  map_frame: map
  odom_frame: odom
  base_link_frame: base_link
  world_frame: map

  odom0: hibachi_velocity_controller/odom
  odom0_config: [false, false, false,    # x,   y,   z
                 false, false, false,    # r,   p,   w (yaw)
                 true,  true,  true,     # dx,  dy,  dz
                 false, false, true,     # dr,  dp,  dw
                 false, false, false]    # ddx, ddy, ddz

  odom0_queue_size: 10
  odom0_nodelay: true
  odom0_differential: false
  odom0_relative: false

  odom1: odometry/gps
  odom1_config: [true,  true,  false, # x,   y,   z
                 false, false, false, # r,   p,   w (yaw)
                 false, false, false, # dx,  dy,  dz
                 false, false, false, # dr,  dp,  dw
                 false, false, false] # ddx, ddy, ddz

  odom1_queue_size: 10
  odom1_nodelay: true
  odom1_differential: false
  odom1_relative: false

  imu0: imu/data
  imu0_config: [false, false, false, # x,   y,   z
                true,  true,  true, # r,   p,   w (yaw)
                false, false, false,  # dx,  dy,  dz
                true,  true,  true, # dr,  dp,  dw
                false ...
(more)
edit retag flag offensive close merge delete

Comments

Can you try NOT using the IMU data for both the odom and map EKFs? IMU's have such a high update rate that they can rapidly introduce error into the estimate.

JackB gravatar image JackB  ( 2021-06-04 16:22:43 -0600 )edit

@JakB thanks for the response! I removed the IMU from the local EKF but the results are fairly the same. Since the dual EKF and navsat example from the robot_localization package uses IMU in both filters, could you explain me a little bit more about that rationale?

gmsanchez gravatar image gmsanchez  ( 2021-06-05 16:54:46 -0600 )edit

@gmsanchez can you remove it from both the local and map filters please and try it? The reason is that IMUs usually produce data at a very fast rate and also can perform poorly when they are not under going large accelerations which ground robots rarely do. For both of these reasons they can add a lot of error to the state estimate quickly. Also interference from surrounding metal and electrical motors can interfere with the magnetic measurements as well.

JackB gravatar image JackB  ( 2021-06-07 09:01:20 -0600 )edit

@JackB in that case I should set "use_odometry_yaw" in navsat_transform node to true, right?

gmsanchez gravatar image gmsanchez  ( 2021-06-07 09:47:48 -0600 )edit

@gmsanchez no. The navsat transform node uses the IMU separately from the filter nodes so you shouldnt need to change anything there.

JackB gravatar image JackB  ( 2021-06-07 09:58:49 -0600 )edit

@JackB thanks for the responses. I have tried to do what you suggested. The map <--> odom transform drifts. You can see what happens in the following screen captures of RViz

  1. https://youtu.be/ATbT-u-b5HE
  2. https://youtu.be/GwpNd5KOc0k

is that expected behavior?

Thanks in advance!

gmsanchez gravatar image gmsanchez  ( 2021-06-07 16:21:30 -0600 )edit

It looks like your GPS is so noisy that it will be hard to generate a reliable origin from that?

JackB gravatar image JackB  ( 2021-06-07 16:24:24 -0600 )edit

@JackB I am using a pair of Emlid Reach GPS and using reach_ros_node to provide the data to robot_localization. I have configured one of them as a base station and the other as a rover. The base station sends the latitude, longitude and altitude corrections to the rover and as far as the configuration web page tells, the rover has centimeter level accuracy.

gmsanchez gravatar image gmsanchez  ( 2021-06-07 16:36:37 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2021-07-13 07:42:10 -0600

Tom Moore gravatar image

updated 2021-07-13 07:43:55 -0600

However, the transform from map to odom drifts a lot

It should drift. How much is "a lot"? Your odom-frame state estimate will drift without bound. If that transform didn't drift, you'd have no need for a Tier 2 EKF. See REP-105.

I am trying to understand what is the map to odom transform doing, but I can't get the grasp of it

Your robot has two state estimates. One is in the odom frame, and one is in the map frame. The state estimates (at least the pose portions of them) are equivalent to the odom->base_link and map->base_link transforms. So after a few minutes, your pose in the odom frame might be at (10, 10) with a heading of pi / 4 radians. At the same time, your pose in the map frame might be (12, 11) with a heading of pi / 3 radians.

But ROS doesn't allow a frame to have two parents. You cannot have map->base_link and odom->base_link published in the same transform tree. To get around this, we compute the map->odom transform as

T_map_odom = T_map_base_link * T_odom_base_link ^ -1

This lets us have a single transform tree that has map->odom->base_link. This lets us still look up a transform from base_link to odom or from base_link to map.

Since your map and odom frame poses will not be the same, that transform will change over time.

I am currently starting the robot on the same marked spot on the ground and asking it to go to a fixed latitude and longitude, but the transforms to map coordinates changes every time

Does it change every time you give it a fixed lat/long within the same run? In other words, which of these are you doing?

  1. Start navsat_transform_node, wait for transforms to get established. Give it a fixed lat/long, get map pose. Give it the same lat/long, get a different pose.
  2. Start navsat_transform_node, wait for transforms. Give it a fixed lat/long, get map pose. Stop software, start it again, wait for transforms. Give it the same lat/long, get a different pose.

If it's (2), I would expect that, because GPS will drift, and your GPS coordinates when you start between any two runs will change. Your IMU can also drift, as magnetometers are notoriously terrible.

Is it OK to set the fixed frame to map for RViz visualization? Or should the fixed frame be odom?

That's up to you. Most people would visualize in the map frame if they have it, but it depends on what you want to see.

In what frame should I give move_base the points if I want to make a GPS waypoint follower?

In your case, you'd want to issue map-frame coordinates. The odom frame state estimate is subject to drift (again, see REP-105, as well as your config - you have only velocity data fused, which is correct, but integration ... (more)

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2021-05-31 19:10:33 -0600

Seen: 231 times

Last updated: Jul 13 '21