Unreliable localization using amcl algorithm ros2 humble

[stefanitro]

Hello,

I followed the ros2 navigation course and succeeded to build the map and localize the robot accurately, but with my real robot could not made the localization part. I made the exactly same steps and tried to modify the amcl parameters but it was almost the same.

First I initialize the position of the robot and it looks great:

After a few moves it doesn’t know exactly where the robot is:

And after a turn it is completely lost, even the laser is rotated:

The output from rqt_graph is the following:

And my tf_tree:

I am not sure that the amcl uses the /odom topic and I think that it should, as it is set in the parameters.

I use ros1_brigde for the lidar and /odom_matcher to compute the odometry from the laser data. I tested it and is very accurate.

Please tell me what did I miss and how to correct it.

Thank you,
Stefan

[duckfrost2]

It seems that the matching of the patterns of the localization is not working properly because, you have a huge dispersion. I would check:

1. Check that the localization is indeed subscribed to your odometry topic, that the names are correct and that you set the /odom and not odom only.
2. Have you tried in simulation first? Because I see that the map matching also has some laser points that are not present in the map? Maybe the environment changed a bit? Try it in simulation to check if it’s related to real robot systems or its a structural issue that happens also in simulation

[stefanitro]

Hi @duckfrost2,

Thank you for your answer.

I tried to make a smaller and easier map, thinking that the small differences were the problem. Unfortunately, I still have the same problem like in the pictures below, so I still believe that the issue is at the localization packages.

Initialization:

After some moves:

About the odometry subscription, I tried both with and without slash for odom_frame, I also tried to set odom_topic to /odom, but it has the same result. I will attach the yaml code from amcl just in case there is something else that I missed. With the launch file I launch the map_server node, amcl node and lifecycle_manager node for ['map_server','amcl']. I think that everything should work as it is supposed to.

I tried the exact same code and configurations in the simulation and it’s working perfect. This is why it is so weird.

Please let me know if you can guide me further because I have tried everything I could think of. Looking forward for your answer.

Thank you,
Stefan

amcl_config.yaml:

amcl:
  ros__parameters:
    use_sim_time: True

    # Overall Filter Parameters
    min_particles: 200
    max_particles: 8000
    pf_err: 0.05
    pf_z: 0.99
    update_min_d: 0.1
    update_min_a: 0.2
    resample_interval: 1
    transform_tolerance: 1.0
    recovery_alpha_slow: 0.0
    recovery_alpha_fast: 0.0
    save_pose_rate: 0.5

    # Laser Model Parameters
    laser_min_range: -1.0
    laser_max_range: 100.0
    max_beams: 60
    z_hit: 0.5 # 0.95
    z_short: 0.05
    z_max: 0.05
    z_rand: 0.5 # 0.05
    sigma_hit: 0.2
    lambda_short: 0.1
    laser_likelihood_max_dist: 2.0
    laser_model_type: "likelihood_field"
    
    # Odometry Model Parameters
    robot_model_type: "nav2_amcl::DifferentialMotionModel"
    alpha1: 0.2
    alpha2: 0.2
    alpha3: 0.2
    alpha4: 0.2
    alpha5: 0.2
    odom_frame_id: "odom"
    base_frame_id: "base_footprint"
    global_frame_id: "map"
    tf_broadcast: true
    scan_topic: /scan
    map_topic: /map

[duckfrost2]

Ok so it seems its the real robot setup then , not the amcl perse.

One thing I see, is the fact that in the real robot, the use_sim_time has to be False.
Other things that could be affecting are:

1. Check the odometry of the real robot when moving it 1 meters to the front, one meter to the side it moves exactly that in the odometry.
2. Maybe you have a rate issue, or comunication issue being very slow?
3. Increase the number of beams in the real robot, it may help the colcalization

[stefanitro]

Hi @duckfrost2,

Thank you for your help.

Edit 1: Straight line movements work great indeed for localization, unlike rotations which cause the particles to spread out very quickly and then completely lose calibration. Should I modify the alpha parameters?

The use_sim_time was set to False, it was only the last try and I forgot to change it when I copied it here. So that it’s not a problem.

The odometry data is accurate, just made a last check, so I would say that this is not an issue also.

About the rate issue, I think that everything is fine because the laser data is shown correctly in rviz and that is what everything is based on (the odometry and mapping or localization process). If you have a suggestion for testing it, please let me know.

Also, I increased the number of beams to 180, I think that it is a small improvement, maybe it is just my hope, but it seems a little better.

What I found in the last testing session was that on straight line, the localization is working, so the particles around the robot are getting much closer, but when it turns, it’s getting noisy very fast. In the picture below it was just a slow turn, if it is a faster one, it will be completely lost. I don’t really know what could be the problem.

After the turn was made:

And after straight line:

In the testing session I also noticed something at the mapping process that could be connected with this issue. The map is made correctly, very accurate, but after 4-5 turns through it, the map outline is not perfectly overlaid with the laser scan anymore. Even after this, made a few more rounds and the map remained the same, it did not made extra walls that did not exist or something.

Mapping:

Please let me know what is your opinion about this and how could I improve it.

Edit 2: Should the nodes be synchronized in time? Could this be a problem?

Thank you,
Stefan

[stefanitro]

I adjusted the alpha parameters and localization worked great after that.

The only problem I still found is the /odom tf drift, but the robot is localized pretty good. Also, the laser scan overlay in the mapping process is still unsolved, maybe the /odom drift is the cause for this also. I will try another algorithm for LiDAR odometry matcher.