Ask Your Question
2

How to debug odometry error?

asked 2015-03-22 09:52:22 -0600

PT gravatar image

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 flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
1

answered 2015-04-11 01:41:45 -0600

PT gravatar image

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 pose of your laser (retrieved from hector_slam), and the other
% 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
% will optimize are left wheel radius, right wheel radius, base_link width,
% pose of the laser relative to the base_link (see initialize_params() and put
% your default values there).
%
% 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

    % load data
    [pose2D_laser, encoder_num] = load_data(filepath, filePrefix);

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

    % plot how it looks like first
    traj_laser = create_baselink_traj_from_laser(pose2D_laser, x0);        
    traj_encoder =...
        create_baselink_traj_from_encoder(encoder_num, traj_laser(1,:), x0);     
    plot_traj(traj_laser, traj_encoder, {'baselink\_from\_laser', 'reconstruct\_odom'});
    title('before calibration')

    % calibrate it    
    [x, traj_encoder, traj_laser] = calibrate_full(pose2D_laser, encoder_num, x0);  
    plot_traj(traj_laser, traj_encoder, {'baselink\_from\_laser', 'reconstruct\_odom'});
    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',...
        'wheel_radius_left',...
        'wheel_radius_right',...
        '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)
edit flag offensive delete link more
0

answered 2015-03-22 11:35:43 -0600

paulbovbel gravatar image

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.

edit flag offensive delete link more

Comments

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.

PT gravatar imagePT ( 2015-03-23 08:38:33 -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

2 followers

Stats

Asked: 2015-03-22 09:52:22 -0600

Seen: 605 times

Last updated: Apr 11 '15