Ask Your Question
5

Transient odometry rotation error (robot_localization)

asked 2015-10-19 19:08:01 -0600

Bradley Powers gravatar image

updated 2015-10-22 08:16:39 -0600

TLDR:

I'm working on a differential drive base with encoders for odometry and an IMU. I'm working on fusing these data sources using robot_localization in an effort to get a more accurate odometry estimate so that our localization will perform better. Outside of some reasonable noise, all of our data tracks desired yaw velocity pretty well:

rqt plot

However, we are experiencing a significant transient yaw error when starting or stopping a turn. There is not a corresponding error in linear motion.


The long version:

I'm working on a differential drive base with encoders for odometry and an IMU. I'm working on fusing these data sources using robot_localization in an effort to get a more accurate odometry estimate so that our localization will perform better. Outside of some reasonable noise, all of our data tracks desired yaw velocity pretty well:

rqt plot

However, we are experiencing a significant transient yaw error when starting or stopping a turn. There is not a corresponding error in linear motion.

Assuming that everything is functioning as it should be, we should be able to rotate the robot with laser scan persistence set to a couple of seconds in rviz, and see the environment remain static relative to the robot.

Ideally, when rotating, the lidar scan (with persistence) should look similar to this static lidar scan: static

When rotating and at steady state, the rotating lidar scan looks like this (which is pretty close to desired): steady state

When we start or stop rotation, however, it looks like this: transient

Which is significantly worse than desired. Ultimately, this leads to localization error when turning around corners and the like, which then ultimately messes up our navigation.

Here's a bagfile for your perusal:

Bag File

Topic names are namespaced by robot name (in this case, p1_001), so:

/p1_001/mobile_base_controller/cmd_vel is commanded velocity

/p1_001/scan is the lidar scan

/p1_001/mobile_base_controller/odom is the raw odometry

/p1_001/mobile_base_controller/imu is the raw imu data

/p1_001/odom is the fused output from robot_localization

I would appreciate it greatly to hear your thoughts on the matter!

EDIT:

Here are launch and config files for relevant nodes.

ROS control launchfile, launches hardware interface, Diff drive and IMU controller

ROS control config, mostly sensor covariances

Robot Localization Config file

edit retag flag offensive close merge delete

Comments

Any conclusion on this?

Akif gravatar imageAkif ( 2016-02-08 01:02:12 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
2

answered 2015-10-22 07:54:04 -0600

Tom Moore gravatar image

updated 2015-11-11 09:08:25 -0600

Can you post your launch file?

Looking at your plots, it looks as if your IMU registers the turn long before your wheel encoder odometry. Additionally, it looks as if the filtered output is tracking much closer to your wheel encoder odometry than your IMU. I'd have to look at their relative covariances (which I realize are available in the bag file) to determine why you're seeing a delay in the filter response, but my first checks would be to (a) make sure that your angular velocity in your controller is reported as soon as the robot starts turning (there's some lag between the two, if I'm reading your plot correctly), and (b) the relative covariance between your IMU angular velocity and odometry angular velocity are set correctly, i.e., I'd expect a much lower variance on yaw velocity for the IMU than for your wheel encoders. Finally, you might want to also try increasing your initial_estimate_covariance for yaw velocity. This will help it converge faster when your robot is first starting.

EDIT 1

Still looking at this. A few things:

  1. Did you intentionally turn the yaw off for your IMU0 config?

    imu0_config: [
        false, false, false,
        false, false, false,
        false, false, false,
        false, false, true,
        true, true, true
    ]
    

    Just want to make sure that was right.

  2. Have you had a chance to look into the delay in your wheel encoder odometry?

  3. Regardless, here's a sample odometry message:

    header: 
      seq: 1997
      stamp: 
        secs: 1445291702
        nsecs: 645665414
      frame_id: odom
    child_frame_id: p1_001/base_link
    pose: 
      pose: 
        position: 
          x: -0.00496491246256
          y: 2.86013970781e-05
          z: 0.0
        orientation: 
          x: 0.0
          y: 0.0
          z: -0.158302775866
          w: 0.987390617311
      covariance: [0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 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.1, 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.001]
    twist: 
      twist: 
        linear: 
          x: 0.00780897589398
          y: 0.0
          z: 0.0
        angular: 
          x: 0.0
          y: 0.0
          z: -1.10046405246
      covariance: [0.0001, 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.1, 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.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001]
    

    ...and here's a sample IMU message:

    header: 
      seq: 1965
      stamp: 
        secs: 1445291701
        nsecs: 25784241
      frame_id: p1_001/imu
    orientation: 
      x: -0.608557879925
      y: -0.377054542303
      z: 0.5998955369
      w: -0.357228845358
    orientation_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
    angular_velocity ...
(more)
edit flag offensive delete link more

Comments

Relevant launch/config files uploaded. Let me know if anything else would be relevant.

Bradley Powers gravatar imageBradley Powers ( 2015-10-22 08:17:09 -0600 )edit

I'll try to check this out tonight. Sorry for the delay.

Tom Moore gravatar imageTom Moore ( 2015-11-10 11:30:08 -0600 )edit

Much appreciated.

Bradley Powers gravatar imageBradley Powers ( 2015-11-10 13:53:36 -0600 )edit

I have a similar problem, which is coming from the fact that although my robot is a differential robot, it does not have simply two wheels but four tracks instead (like a tank). As a result depending on the floor material we experience different amount of error in the odometry.

mohsen1989m gravatar imagemohsen1989m ( 2015-11-18 07:12:16 -0600 )edit

I guess integrating IMU data is not a promising solution when you robot is slow, I tried to the exactly the same thing and it didn't worked. A better way could be trying to use laser scanner data and performing a laser match. there is a node called laser_scan_matcher but I couldn't install it...

mohsen1989m gravatar imagemohsen1989m ( 2015-11-18 07:15:07 -0600 )edit

In this case, though, you can see it the MATLAB plots that the EKF output is tracking much closer to the wheel encoder odometry than the IMU. I'd like to see what happens if that is addressed (along with the other ideas I suggested).

Tom Moore gravatar imageTom Moore ( 2015-11-18 07:30:57 -0600 )edit

You mean that you want to determine how much your EKF output should rely on the IMU data?

mohsen1989m gravatar imagemohsen1989m ( 2015-11-18 07:57:08 -0600 )edit
1

If you mean this laser_scan_matcher, you unfortunately need to build it from source. RE: IMU data: The above plots show the EKF tracks encoder data more tightly than IMU data, but the IMU data responds faster. Tom wants to see EKF data when this is swapped.

etappan gravatar imageetappan ( 2015-11-18 08:27:00 -0600 )edit

Your Answer

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

Add Answer

Question Tools

4 followers

Stats

Asked: 2015-10-19 19:08:01 -0600

Seen: 2,168 times

Last updated: Nov 11 '15