0
$\begingroup$

Rosanswers logo

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,  false,  false] # ddx, ddy, ddz
  imu0_nodelay: true
  imu0_differential: false
  imu0_relative: false
  imu0_queue_size: 10
  imu0_remove_gravitational_acceleration: true

  use_control: false

navsat_transform:
  frequency: 1
  delay: 3.0
  magnetic_declination_radians: -0.165092439  # For lat/long
  yaw_offset: 0.0
  zero_altitude: false
  # broadcast_utm_transform: true
  broadcast_cartesian_transform: true
  broadcast_cartesian_transform_as_parent_frame: false
  publish_filtered_gps: true
  use_odometry_yaw: false
  wait_for_datum: false

However, the transform from map to odom drifts a lot and I can't seem to get good results. I even tried to use a pair of Emlid Reach RTK GPS instead of the U-blox M8N, which stops some of the transform drift, but I can't get good results and I am quite lost in how to debug and search for what is working wrong in this setup.

Besides trying to make robot_localization work consistently, I have some extra questions:

  1. I am trying to understand what is the map to odom transform doing, but I can't get the grasp of it. Can someone help me with this?
  2. I am trying to make a GPS waypoint follower. I am currently using the waypoint_nav package. How is the Latitude Longitude to map supposed to work? 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: one time is (1.07402735, 7.05115267) and other time is (0.733598175 ,-2.47340248), for example. I don't get it. Should it be relatively the same if the origin latitude and longitude is fairly the same and I want to get the transformation to the same point every time?
  3. Is it OK to set the fixed frame to map for RViz visualization? Or should the fixed frame be odom?
  4. In what frame should I give move_base the points if I want to make a GPS waypoint follower? If I understand correctly, that should be map, but I am having some doubts since it appears that the transform from map to odom is to correct drift and when I visualize in RViz, the base_link to odom transform suits better what the vehicle did in the real world.

Any help will be really appreciated. If you need some extra data, please tell me and I will try to give more information.


Originally posted by gmsanchez on ROS Answers with karma: 13 on 2021-05-31

Post score: 1


Original comments

Comment by JackB on 2021-06-04:
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.

Comment by gmsanchez on 2021-06-05:
@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?

Comment by JackB on 2021-06-07:
@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.

Comment by gmsanchez on 2021-06-07:
@JackB in that case I should set "use_odometry_yaw" in navsat_transform node to true, right?

Comment by JackB on 2021-06-07:
@gmsanchez no. The navsat transform node uses the IMU separately from the filter nodes so you shouldnt need to change anything there.

Comment by gmsanchez on 2021-06-07:
@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!

Comment by JackB on 2021-06-07:
It looks like your GPS is so noisy that it will be hard to generate a reliable origin from that?

Comment by gmsanchez on 2021-06-07:
@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.

Comment by JackB on 2021-06-07:
Hmmm then what is that pink cone dashing around in the video?

Comment by gmsanchez on 2021-06-07:
The pink cone is the UTM to map transform. I did another try, this time I have waited for 10 minutes before moving the vehicle, giving the RTK GPS some time to settle. These are the videos

  1. https://youtu.be/eEhwgo
  2. https://youtu.be/rVK9_CP_28k

It seems that the drift is smaller, but I can't understand what happens to the transform as soon as I start moving the vehicle.

Comment by JackB on 2021-06-08:
@gmsanchez fyi you should have the map frame be the fixed frame. And it is actually expected that there will be some drift between map and odom. If there was no drift then they would represent almost the same thing and there would be no point of having them

Comment by gmsanchez on 2021-06-08:
@JackB I will change that and make the map frame the fixed frame in the RViz configuration. I am expecting drift for the map frame. But it puzzles me how that transform starts drifting and rotating away from the odom frame. I am trying to build a gps waypoint follower and I can't manage to make the vehicle to go to a single point consistently.

Let's suppose the vehicle is at (Lat_0, Long_0) and you want to move to (Lat_1, Long_1). Let's also suppose that (Lat_1, Long_1) is 4 meters to the East (which is aligned with the x-axis) and the vehicle starts with a yaw angle of 0 degrees. For move_base to reach that goal, (Lat_1, Long_1) is transformed to UTM coordinates and then to the ODOM frame. If map and odom frame are aligned, that point would be (4, 0), right? But sometimes that transformed point is something like (-2.8, 2.5), to give an example. To my understanding, since the transform from map to odom changes so much, I cannot get consistent results.

Does that make sense?

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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 of errors will lead to drift). So if you drive your robot to (10, 10) in the odom frame, and then drive it around a whole bunch, and then drive it back to (10, 10), the robot (in the real world) will not actually be in the same location, even though it thinks it is. However, that should generally not be true for the map frame. If you come back to the same position, the robot should actually be in the same position in the real world.


Originally posted by Tom Moore with karma: 13689 on 2021-07-13

This answer was ACCEPTED on the original site

Post score: 1

$\endgroup$

Your Answer

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