0
$\begingroup$

Rosanswers logo

hi, i'm trying to use the navigation stack but i think that i have trouble with amcl , i have a two wheel mobile robot like a big version of turtlebot with 0.5 m radius equiped with Hokuyo laser , and since my wheel encoder gives a bad odometry to correct it i bought an imu sensor " Sparkfun Razor IMU 9DOF " and i used ekf localization to combine the odometry from encoder and the imu data , i had also remap the odom topic of the amcl to the new odometry topic provided by ekf
my problem is that sometimes the laserscan don't match with map specially when the robot stop , here is an example shown on the video you can see that when i'm rotating the robot the scan matches with the map but when i stop it there is a delay between scans and the map , i'm losing my mind i tried every thing i found on ros but it's still not working can you tell what's wrong with my configuration ?

here is the video : video

here is my amcl configuration :

<launch>
  <arg name="use_map_topic"   default="false"/>
  <arg name="scan_topic"      default="scan"/>
  <arg name="initial_pose_x"  default="0.0"/>
  <arg name="initial_pose_y"  default="0.0"/>
  <arg name="initial_pose_a"  default="0.0"/>
  <arg name="odom_frame_id"   default="odom"/>
  <arg name="base_frame_id"   default="base_link"/>
  <arg name="global_frame_id" default="map"/>
  <node pkg="amcl" type="amcl" name="amcl">
    <param name="use_map_topic"             value="$(arg use_map_topic)"/>
    <remap from="/odom" to="/odometry/filtered"/>
    <!-- Publish scans from best pose at a max of 10 Hz -->
    <param name="odom_model_type"           value="diff"/>
    <param name="odom_alpha5"               value="0.1"/>
    <param name="gui_publish_rate"          value="10.0"/>
    <param name="laser_max_beams"             value="60"/>
    <param name="laser_max_range"           value="4.0"/>
    <param name="min_particles"             value="500"/>
    <param name="max_particles"             value="2000"/>
    <param name="kld_err"                   value="0.05"/>
    <param name="kld_z"                     value="0.99"/>
    <param name="odom_alpha1"               value="0.2"/>
    <param name="odom_alpha2"               value="0.2"/>
    <!-- translation std dev, m -->
    <param name="odom_alpha3"               value="0.2"/>
    <param name="odom_alpha4"               value="0.2"/>
    <param name="laser_z_hit"               value="0.5"/>
    <param name="laser_z_short"             value="0.05"/>
    <param name="laser_z_max"               value="0.05"/>
    <param name="laser_z_rand"              value="0.5"/>
    <param name="laser_sigma_hit"           value="0.2"/>
    <param name="laser_lambda_short"        value="0.1"/>
    <param name="laser_model_type"          value="likelihood_field"/>
    <!-- <param name="laser_model_type" value="beam"/> -->
    <param name="laser_likelihood_max_dist" value="2.0"/>
    <param name="update_min_d"              value="0.25"/>
    <param name="update_min_a"              value="0.2"/>
    <param name="odom_frame_id"             value="$(arg odom_frame_id)"/>
    <param name="base_frame_id"             value="$(arg base_frame_id)"/>
    <param name="global_frame_id"           value="$(arg global_frame_id)"/>
    <param name="resample_interval"         value="1"/>
    <!-- Increase tolerance because the computer can get quite busy -->
    <param name="transform_tolerance"       value="1.0"/>
    <param name="recovery_alpha_slow"       value="0.0"/>
    <param name="recovery_alpha_fast"       value="0.0"/>
    <param name="initial_pose_x"            value="$(arg initial_pose_x)"/>
    <param name="initial_pose_y"            value="$(arg initial_pose_y)"/>
    <param name="initial_pose_a"            value="$(arg initial_pose_a)"/>
    <remap from="scan"                      to="$(arg scan_topic)"/>
  </node>
</launch>

and here is the ekf yaml file i used :

frequency: 30
sensor_timeout: 0.1
two_d_mode: true
# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
publish_tf: true

# Whether to publish the acceleration state. Defaults to false if unspecified.
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

odom0: /odom

odom0_config: [true,  true,  false,
           false, false, false,
           false, false, false,
           false, false, true,
           false, false, false]
odom0_queue_size: 400
odom0_nodelay: false
odom0_differential: false
odom0_relative: false

imu0: /imu
imu0_config: [false, false, false,
          true,  true,  true,
          false, false, false,
          true,  true,  true,
          true,  true,  true]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 400

imu0_remove_gravitational_acceleration: true
imu1_remove_gravitational_acceleration: true
publish_tf: true


process_noise_covariance: [0.05, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                       0,    0.05, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                       0,    0,    0.06, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                       0,    0,    0,    0.03, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                       0,    0,    0,    0,    0.03, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                       0,    0,    0,    0,    0,    0.06, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                       0,    0,    0,    0,    0,    0,    0.025, 0,     0,    0,    0,    0,    0,    0,    0,
                       0,    0,    0,    0,    0,    0,    0,     0.025, 0,    0,    0,    0,    0,    0,    0,
                       0,    0,    0,    0,    0,    0,    0,     0,     0.04, 0,    0,    0,    0,    0,    0,
                       0,    0,    0,    0,    0,    0,    0,     0,     0,    0.01, 0,    0,    0,    0,    0,
                       0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.01, 0,    0,    0,    0,
                       0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.02, 0,    0,    0,
                       0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.01, 0,    0,
                       0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.01, 0,
                       0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.015]



initial_estimate_covariance: [1e-9, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                          0,    1e-9, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                          0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                          0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                          0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                          0,    0,    0,    0,    0,    1e-9, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                          0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,
                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,
                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]

and here is my tf three : image description


Originally posted by kesuke on ROS Answers with karma: 58 on 2018-07-23

Post score: 1


Original comments

Comment by billy on 2018-07-23:
What happens if you rotate the other direction? I suspect that the LIDAR isn't aligned correctly and you simply got lucky that it looks correct when rotating. I would expect the LIDAR to lag while rotating. Other direction probably much worse. Test and reply back.

Comment by kesuke on 2018-07-24:
thanks for answering , i tried what you said and it's the same result , i cheked that the LIDAR is aligned so i don't understand what's happennig it's like as if the amcl calculate the position late compared to the motion of the robot

Comment by Humpelstilzchen on 2018-07-24:
Does this also happen without amcl? (Use odom as fixed frame in rviz)

Comment by kesuke on 2018-07-24:
when i put odom as fixed frame it's the same result , i know that the odometry i'm getting from encoder is poor that's why i added an imu but i dont know it's the odom that causes trouble

Comment by Humpelstilzchen on 2018-07-24:
If I read your configuration file correctly you totally ignored the rotation from your wheels for odometry which is a good thing. So now we know the error comes from the IMU alone, correct? I suggest to calibrate the IMU and try again. Your problem might also come from magnetic interference.

Comment by Humpelstilzchen on 2018-07-24:
Btw personally since I replaced my Razor IMU with a Bosch BNO055 based one (Tinkerforge IMU2) my navigation experience improved a lot.

Comment by kesuke on 2018-07-24:
@Humpelstilzchen i forget to calibrate it , i don't know if it will solve the problem but i will try it now

Comment by aarontan on 2018-07-27:
@kesuke have you solved this problem? I am facing something similar

Comment by kesuke on 2018-08-02:
@aarontan not yet , i'm still searching

Comment by Humpelstilzchen on 2018-08-11:
Did the behavior change after calibration?

$\endgroup$

2 Answers 2

0
$\begingroup$

Rosanswers logo

I would only fuse velocities in your EKF, for starters. Stop fusing yaw from the IMU and X and Y position from your wheel odometry, and instead just fuse their velocities. BTW, you have roll and pitch enabled for your IMU, but you have two_d_mode on, so they'd be ignored anyway.

If I had to guess, your issue is one of covariances. You didn't include any sample wheel odometry or IMU messages (please do), but it looks to me that the EKF is trusting its own velocity estimate too much, so when the robot stops moving, it take the EKF a few cycles to converge to a 0 velocity. You can try increasing the process_noise_coviance for yaw velocity to see if that helps.


Originally posted by Tom Moore with karma: 13689 on 2018-08-13

This answer was ACCEPTED on the original site

Post score: 2

$\endgroup$
0
$\begingroup$

Rosanswers logo

To me it looks like amcl doesn't relocate because your rotation is < update_min_a ("Rotational movement required before performing a filter update"), so try reducing that parameter.

You should probably also increase the odom_alpha1/odom_alpha2 ("expected noise in odometry's rotation estimate") parameters if your IMU's orientation estimation is inaccurate.

Read through the parameter explanations in the wiki: http://wiki.ros.org/amcl


Originally posted by lalten with karma: 102 on 2018-08-11

This answer was NOT ACCEPTED on the original site

Post score: 1


Original comments

Comment by kesuke on 2018-08-13:
thank you i will try it

Comment by aarontan on 2018-09-10:
@kesuke did this solve the problem?

$\endgroup$

Your Answer

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