robot_localization drifts when gps is missing (with RTK GPS, heading and IMU)

asked 2019-09-05 08:48:50 -0600

davidllp
21 ●2 ●4 ●4

updated 2023-06-18 09:50:18 -0600

lucasw

8729 ●137 ●239 ●256 https://github.com/lucasw

Hi everyone,

After searching on the forum, I haven't found any answer that solved this issue so I hope it is not a duplicate. The system we are using is: an outdoors robot that performs a high precision mapping of objects detected by a camera. To this objective the robot computes the localization using the following sensors:

GPS RTK:
- topic: /piksi_receiver/navsatfix_rtk_fix
- msg_type: sensor_msgs/NavSatFix
Heading RTK (in fact, there are 2 GPS antennas, and their drivers compute a high accurate heading using their position)
- topic: /heading (the driver sends a custom message in degrees and True north referenced, then, we transform it to ENU, so it complies with the required orientation specified by the robot_localization).
- msg_type: sensor_msgs/Imu
Imu
- topic: /imu0 (the imu has a magnetometer, but it's magnetic field is highly affected by the system, so we are using it as a VRU device and we are resetting the orientation.yaw with the known /heading, so it complies with the required ENU orientation).
- msg_type: sensor_msgs/Imu

Covariances of all sensors are set accordingly to their datasheets.

Problem explanation

While there is RTK quality of the gps everything works smoothly, however, when there's a loss in the quality of the gps, the position it provides has an important offset. Hence, we decided to only use the gps position when the signal has RTK quality. And we though that the ekf would fill in the missing data with the heading and the imu. However, as it can be seen in the images, while the heading (light blue arrow) keeps the correct orientation, the base_link has a drifting to the right over time. We tried different configurations of the robot_localization but all results were similar.

Has anyone an idea of what could be happening? Is there something wrong in the configuration of the robot localization? Are we missing something?

Thanks in advanced for your help!

Images

Images of the results commented hereafter (link to image as I don't have rights to upload them). Legend:

Bright green points: rtk gps points (in WSG84)
Grey points: bad gps quality (in WSG84)
Green, Red and Blue lines: are the frames tracking for Reference (gps1), Attitude (gps2 for heading computing) and Robot base_link (odometry of frames in /odom)
Light blue arrow: is the odometry of the robot with heading.

robot_localization configuration

All transforms are defined in a URDF except by the transform from map to odom that is specified as a static transform in the roslaunch of the localization node (see below).

ekf_imu_heading_localization.yaml

frequency: 60
sensor_timeout: 0.1
two_d_mode: false
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
debug_out_file: /path/to/debug/file.txt
publish_tf: true
publish_acceleration: false

map_frame: map              # Defaults to "map" if unspecified
odom_frame: odom            # Defaults to "odom" if unspecified
base_link_frame: base_link  # Defaults to "base_link" if unspecified
world_frame: odom           # Defaults to the value of odom_frame if unspecified

predict_to_current_time: true

odom0: /odometry/gps
odom0_config: [true,  true,  true,
               false, false, false,
               false, false, false,
               false, false, false,
               false ...

(more)

edit retag flag offensive close merge delete

Comments

IMUs accumulate a lot of position error over time. Using the imu feed as your only source of odometry is going to give you a lot of drift - because you are double integrating that noisy acceleration measurement.

Also if your imu orientation is incorrect, and your imu reports gravity in its accelerometer, your system will be interpreting gravity as some acceleration in all axes.

Hopefully Tom Moore comments on this, I am only a localizing beginner.

johnconn ( 2019-09-06 10:05:10 -0600 )edit

Actually, we did a test using the robot_localization only using the IMUs (configured in the same as the code provided) to observe the odometry it was producing. While the absolute positioning was obviously off, the odometry was a quite smooth straigh line. So, in this case, I'm not sure it's a IMU drifting issue. Additionally, the IMU orientation has been triple check because we also though it could be that! (However, it still remains a little bit unclear for me, how the orientation of each field must be expressed. Though, I'm pretty sure it is correct right now). With respect to the gravity, there is a parameter in the configuration of the robot_localization that eliminates the gravity if the IMU doesn't do it (imu0_remove_gravitational_acceleration). I'll try to provide a bag this afternoon to reproduce the system.

davidllp ( 2019-09-09 02:15:33 -0600 )edit

add a comment

answered 2019-11-04 04:14:41 -0600

Tom Moore

flag of United Kingdom of Great Britain and Northern Ireland

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

updated 2019-11-26 04:29:00 -0600

How large is that distance over which the RTK was not reporting? It seems like it's pretty far.

In general, as @johnconn said, the use of an IMU as an odometry source is not something I recommend. You appear to have a very nice IMU, as I'd have expected far worse performance over that kind of distance with just double-integration of IMU data. If I had to guess, there's some kind of bias in your accelerometer that is causing a slow lateral drift. You can try providing an image of the output (over that same distance, and preferably from the same location) when you just use the IMU and heading data. I'm assuming all of this came from a bag.

In any case, my recommendation would be to have some kind of odometry source, either from wheel encoders or through visual odometry. If the GPS quality is high and outages are not frequent, I think you can get away with a pretty noisy odometry source.

EDIT: Off the top of my head, and looking at your original drift image, if I had to guess, what you're seeing is this:

Let's ignore the IMU for a moment. Robot starts moving forward, receiving RTK inputs. At every RTK message, the filter is updated with a new pose. However, when it receives that new pose, the magic of matrix inversion means that if you don't have a velocity reference, the filter will actually produce a non-zero velocity, depending on the delta in your last two measurements. As long as you are receiving RTK inputs, this auto-generated velocity doesn't matter, because you are constant updating the pose.
You then cease to receive RTK measurements. The last velocity value that the filter produced, however, small, persists, because you have no velocity reference to correct it.
Now throw in the IMU. It won't help you, because it's just integrating linear acceleration data. So even if it reads that the vehicle is not accelerating laterally, it won't matter, because you already have a non-zero lateral (Y) velocity.

You need a velocity reference. If I were you, I'd take advantage of your vehicle's kinematics, and feed in a fake Y velocity measurement of 0 all the time to the filter. That is, unless you have to worry about things like sliding laterally in the mud.

edit flag offensive delete link

Comments

Thanks for answering. We considered adding visual odometry but for now the development has been centered in improving RTK coverage.

I've added the image robot_localization_imu_with_heading in which it can be seen the GPS data and the output of the localization node generated by only using the imu and the heading. As you'll see, the imu data is quite good. The trajectory is a straight line all along the zone of interest, the rotation offset only appears at the beginning and at the end, where angular velocities aren't good.

Could you please confirm us if the configuration files look correct to you? If you have the time, I've asked a related question about the IMU configuration of the same system since we are wondering that the issue may come from it.

EDIT:we'll test your yd=0 proposal asap once back on the topic. Thanks!

davidllp ( 2019-11-06 04:44:24 -0600 )edit

add a comment

robot_localization drifts when gps is missing (with RTK GPS, heading and IMU)

Comments

1 Answer

Comments

Question Tools

Stats

Related questions

robot_localization drifts when gps is missing (with RTK GPS, heading and IMU) edit

Comments

1 Answer

Comments

Question Tools

Stats

Related questions

robot_localization drifts when gps is missing (with RTK GPS, heading and IMU)