# How to debug odometry error?

I have a custom 4 wheel robot (only back wheels are powered) and a laser range finder, and I'd like to have it do SLAM and navigation. However, after following this navigation tuning guide, I found that my odometry error is way too large in both translation and rotation.

I think there are three measurements that are critical (please correct me if I miss anything): circumference of the back wheels (I also have encoder information), the distance between the two back wheels, and the distance from the laser range finder to the back wheels.

So, I measured these numbers, and created an URDF for the robot (the robot itself is about 0.5x0.5m). However, there are probably some measurement error, because even simple translation for 2m showed some error (the laser data of the room is inconsistent before and after the robot translation). When testing the rotation, the error is a lot higher... So, I wonder if anyone could provide their experience in debugging the odometry error, so that I can get it right! Thanks in advance!

edit retag close merge delete

Sort by » oldest newest most voted

As measuring the transform may be tricky sometimes (e.g., wide wheels, laser at locations hard to measure), the way I debug odometry in the end is to use hector_slam to get the laser pose, get the encoder counts from both wheels, and then minimize the base_link's odometry differences obtained from both sources (laser pose and encoder counts). In this way, I can optimize several parameters at once (e.g., left wheel radius, right wheel radius, base_link width, pose of the laser relative to the base_link), and use my rough measurements as the initial values.

I collect the laser pose and encoder counts by driving the robot in rectangular routes and turning both clockwise and counter-clockwise. Below is my matlab program for such calibration optimization.

function calibrate_odometry(filepath, filePrefix)
% calibrate_odometry(filepath, filePrefix)
%
% This program calibrates odometry and the pose of the laser relative to the
% base_link. It expects 2 csv files parsed from your bag file.  One csv files
% stores the encoder counts of both your wheels (see initialize_index() for
% which columns corresponds to which data).  This program will minimize the
% differences in odometry created by both sources.  Hence, the variables it
% pose of the laser relative to the base_link (see initialize_params() and put
%
% filepath: path to the csv files.
% filePrefix: prefix of the csv files.  One csv file should be named as
%             [prefix]_pose2D_laser.csv, and the other is
%             [prefix]_encoder_num.csv.

global index

% default value
same_wheel_circ = false;
initialize_index();
x0 = initialize_params();

if same_wheel_circ
index.wheel_circ_right = index.wheel_circ_left;
end

% make theta continuous for optimization
pose2D_laser(:,index.theta) =...
make_theta_continuous(pose2D_laser(:,index.theta));

% plot how it looks like first
traj_encoder =...
title('before calibration')

% calibrate it
[x, traj_encoder, traj_laser] = calibrate_full(pose2D_laser, encoder_num, x0);
title('after calibration')

% print optimized number
print_optimized(x)
print_optimized_for_urdf(x)
end

function print_optimized_for_urdf(x)
global index
param = {'wheel_base',...
'rplidar_pos_x',...
'rplidar_pos_y',...
'rplidar_yaw'};

v = [x(index.wheel_base)...
x(index.wheel_circ_left)/(2*pi)...
x(index.wheel_circ_right)/(2*pi)...
x(index.laser_dist)*cos(x(index.laser_shift_theta))...
x(index.laser_dist)*sin(x(index.laser_shift_theta))...
x(index.laser_yaw)];

value = num2cell(v);

[param' value']
end

function print_optimized(x)
global index
param = {'wheel_base',...
'wheel_circ_left',...
'wheel_circ_right',...
'laser_dist',...
'laser_shift_theta',...
'laser_yaw'};

v = [x(index.wheel_base)...
x(index.wheel_circ_left)...
x(index.wheel_circ_right)...
x(index.laser_dist)...
x(index.laser_shift_theta)...
x(index.laser_yaw)];

value = num2cell(v);

[param' value']
end

function initialize_index()
global index
index = struct(...
... % this row is the index for Pose2D
'time', 1, 'x', 2, 'y', 3, 'theta', 4, ...
... % this row is the index for encoder counts
'leftwheel', 2, 'rightwheel', 3,...
... % these two rows are the index for x, the parameters to be optimized
'wheel_base', 1, 'wheel_circ_left', 2, 'wheel_circ_right', 3, ...
'laser_dist', 4, 'laser_shift_theta', 5, 'laser_yaw', 6);
end

function x0 = initialize_params()
wheel_base ...
more

Open up rviz and set your laserscan visualization to high persistence, so that you see subsequent scans overlaid on each other.

Make sure that the transform between base_laser and base_link is as good as you can get it, as a poor transform will poison subsequent tuning.

Then, tune your wheel circumference measurement until translational (forward/backward) odometry is as good as it's going to get, by driving the robot back and forth and making sure that the scans line up.

Then, tune your wheel track (distance between the back wheels) by spinning the robot in place until the scans line up.

more

Thanks for the suggestions! I'll do that. Also, do you know any tests that can debug each of the 3 measurements independently? I wonder how accurate it needs to be (e.g., <1mm)? Thanks again.

( 2015-03-23 08:38:33 -0600 )edit