0
$\begingroup$

Rosanswers logo

Hi all,

I have a mobile robot and I am using robot_localization to fuse data from wheel_encoders, IMU and hokuyo lidar (using AMCL). In the first instance of robot_localization, I have wheel_encoders and IMU in the odom_frame and in the second instance of robot_localization, I have wheel_encoders and output of AMCL (amcl_pose (geometry_msgs/PoseWithCovarianceStamped)) in the map_frame.

This is my launch file for the second instance of robot_localization :

<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization2" clear_params="true">

  <param name="frequency" value="10"/>

  <param name="sensor_timeout" value="0.5"/>

  <param name="two_d_mode" value="true"/>

  <param name="map_frame" value="map"/>
  <param name="odom_frame" value="odom_combined"/>
  <param name="base_link_frame" value="base_footprint"/>
  <param name="world_frame" value="map"/>

  <param name="transform_time_offset" value="0.0"/>

  <param name="odom0" value="odom"/>
  <param name="pose0" value="amcl_pose"/>
  
  <rosparam param="odom0_config">[false, false, false,
                                  false, false, false,
                                  true, true, false,
                                  false, false, true,
                                  false, false, false]</rosparam>

  <rosparam param="pose0_config">[true,  true,  false,
                                  false, false, true,
                                  false, false, false,
                                  false, false, false,
                                  false, false, false]</rosparam>

  <param name="odom0_differential" value="true"/>
  <param name="pose0_differential" value="false"/>
  
  <param name="print_diagnostics" value="true"/>

  <param name="debug"           value="false"/>
  <param name="debug_out_file"  value="debug_ekf_localization.txt"/>

  <rosparam param="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]</rosparam>

       <rosparam param="initial_estimate_covariance">[0.3, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0.3, 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,    0.1, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    0,    1e-3, 0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    0,    0,    1e-3, 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-3 ,  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]</rosparam>


  <remap from="odometry/filtered" to="odometry/filtered2"/>

</node>

The covariance for wheel_encoders is 1e-7, the covariance for IMU is 1e-7 and the initial covariance for AMCL is its default value which is 0.25 (for initial_covariance in X,Y,Z)

Now, I have following doubts:

  1. When the robot starts, it takes lot of time to converge and during that time, the robot weaves a lot.
  2. When the robot makes a turn, its localization gets messed up and then it again takes sometime to converge and correct the localization and again, during that time the robot weaves a lot. This happens only when the robot turns.

Also, if I just use one instance of robot_localization with wheel_encoders and IMU in the odom_frame and then use amcl to provide the map->odom transformation, then the robot performs better and weaving of the robot is less and it converges faster. Therefore, I suspect it has something to do with the second instance of robot_localization.

Any help regarding this will be appreciated. Let me know if you need more information from my side.

Update 1 :

Sample odometry (/odom) message :

header: 
  seq: 1373
  stamp: 
    secs: 1446740943
    nsecs: 414337267
  frame_id: odom_combined
child_frame_id: base_footprint
pose: 
  pose: 
    position: 
      x: 1.55659262525
      y: -0.219513382396
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.215424694594
      w: 0.976520455986
  covariance: [1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-05]
twist: 
  twist: 
    linear: 
      x: 0.25119998306
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: -0.279628895983
  covariance: [1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07]

Sample amcl (/amcl_pose) message :

header: 
  seq: 166
  stamp: 
    secs: 1446741053
    nsecs: 58572500
  frame_id: map
pose: 
  pose: 
    position: 
      x: 4.81614080312
      y: 2.14873797135
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: -0.109653845072
      w: 0.99396983569
  covariance: [0.017594585241578642, -0.000982540070030069, 0.0, 0.0, 0.0, 0.0, -0.000982540070030069, 0.008169362154903936, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.009693177766675111]

Sample imu (/imu_data) message :

header: 
  seq: 3312
  stamp: 
    secs: 1446741137
    nsecs: 281814968
  frame_id: imu_frame
orientation: 
  x: 0.00522551208583
  y: 0.013448674883
  z: -0.106760334929
  w: -0.992483813037
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: 0.00222162960486
  y: 0.000351509311352
  z: -0.131620340348
angular_velocity_covariance: [1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07]
linear_acceleration: 
  x: 0.317059213562
  y: -0.175599007511
  z: 9.73220711629
linear_acceleration_covariance: [8.66124974095918e-06, 0.0, 0.0, 0.0, 8.66124974095918e-06, 0.0, 0.0, 0.0, 8.66124974095918e-06]

Also for second instance of robot_localization, I am using x,y,yaw (from amcl) and vx,vy,vyaw (encoders) and I have set the covariances for them but left covariances as 1e+9 for variables which I am not measuring. What should be the covariance values for variables which I am not using, should I make it 0? In robot_pose_ekf, I just increased the covariance values to a very large number for variables which I am not measuring.

Update 2:
Thanks @Tom Moore for the answer. I have updated the launch file (see above) but the robot still behaves the same way as mentioned above, restating here:

  1. When the robot starts, it takes lot of time to converge and during that time, the robot weaves a lot.
  2. When the robot makes a turn, it overshoots the path and then tries to come back on the path and overshoots on the other side and so on and then it again takes sometime to converge and come to the path but although the weaving reduces, it is still there. This happens only when the robot turns. When the robot goes straight on its path, it still weaves but turning causes lot of weaving.

Some more information about the robot : Its dimension is 92 cm * 64 cm * 100 cm. Its center of rotation is 10 cm from back of the robot and that is where I have specified the base_link of the robot. I am using move_base for planning with sbpl_lattice_planner as a global planner and dwa_local_planner as a local_planner.

This is the link to the video. Green arrows represent the output of second instance of robot_localization, blue arrows represent the output of first instance of robot_localization and red arrow is the odometry from the wheel_encoders.

Update 3 and Update 4:

The three plots are shown below for robot_localization 2nd instance ( Input : wheel_encoders odometry and AMCL and Output : odometry/filtered2). Please note that I am using x,y,yaw from AMCL (differential = false) and vx,vy,vyaw from wheel_encoders for second instance of robot_localization (see launch file above) (world_frame = map). AMCL is in map frame and wheel_encoders odometry is in odom_combined frame.

Plot-1- (pose/pose/position/X) image description

Plot-2- (pose/pose/position/Y) image description

Plot-3- (pose/pose/orientation --> YAW) image description

Now, I have turned off the YAW velocity (vyaw) from encoders in the second instance of robot_localization and this is the video. The performance is really bad.
Note: I am using Ultrawide band sensor to set the initial pose and hence it is not perfect but robot localization is able to correct it

Here are the plots for above settings :

Plot-1- (pose/pose/position/X) image description

Plot-2- (pose/pose/position/Y) image description

Plot-3- (pose/pose/orientation --> YAW) image description

I will run the robot after increasing process_noise_covariance for yaw and also setting transform_time_offset to 0.1 and update the question with the output.

Let me know if you need more information from me.

Thanks in advance.
Naman Kumar


Originally posted by Naman on ROS Answers with karma: 1464 on 2015-11-11

Post score: 5


Original comments

Comment by Tom Moore on 2015-11-11:
Can you post sample messages? Also, your initial_estimate_covariance should not have values of 1e**+**9 for variables that you are not measuring. You want it to have large values for variables that you are measuring.

Comment by Naman on 2015-11-11:
I have updated the original question. Thanks!

Comment by Tom Moore on 2015-11-12:
Can you please plot each of your input and output pose variables independently (please convert the quaternion to Euler angles) against each other? I'd like to see the time delay between input and output.

Comment by Naman on 2015-11-12:
@Tom Moore, I have updated the original question and put the plots. Let me know if you need more information from me.. Thanks again!

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

For any given measurement, there are two covariance matrices at play:

  1. The state's covariance matrix
  2. The measurement's covariance matrix

The one that concerns me in your case is your state covariance. You can set the initial value for this matrix using the initial_estimate_covariance parameter. Here's the rule you should follow: if you are measuring a variable, make the diagonal value in initial_estimate_covariance larger than that measurement's covariance. So, for example, if your measurement's covariace value for the variable in question is 1e-6, make the initial_estimate_covariance diagonal value 1e-3 or something. This will speed up convergence. If you are not measuring a given variable (e.g., roll), make the diagonal value in initial_estimate_covariance small (but never zero).

EDIT 1

It's hard to tell with the time scale what the lag is between the amcl yaw and the robot_localization yaw. For your plots, you might try (a) zooming in a bit more, and (b) subtracting the first time stamp from one of the data sources from all the timestamps from all sources so that we can see the time in the X axis better.

Also, can you turn OFF the yaw velocity from the wheel encoders in your second r_l config? I want to see the delay when you only have one sensor input.

Also, try increasing the process_noise_covariance for yaw. It will make the error in the state estimate grow faster, which will in turn cause it to trust your measurements more.

Also also, you might want to try the transform_time_offset parameter. I believe amcl future-dates its map->odom transform (with a default value of 0.1 seconds), so it may be worth doing the same.

EDIT 2

I'm not convinced you don't have another problem with transforms. If you are just fusing the pose data from amcl, then the two plots should be much closer to one another. In what frame is the amcl data published? Is amcl also publishing the map->odom_combined transform?


Originally posted by Tom Moore with karma: 13689 on 2015-11-12

This answer was ACCEPTED on the original site

Post score: 6


Original comments

Comment by Naman on 2015-11-13:
I have updated the plots and the question as you mentioned. Let me know if there is a need to make further changes to the plots! Thanks a lot!

Comment by jxl on 2016-12-30:
@Tom Moore, when study r_l,i have a similar question want to ask for your help,if you have time ,please look at this question,i will be very very appreciated,Thanks a lot!

$\endgroup$

Your Answer

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