0
$\begingroup$

Rosanswers logo

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!


Originally posted by PT on ROS Answers with karma: 234 on 2015-03-22

Post score: 2

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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=0.52; % distance between the two back wheels
    wheel_circ=0.964; % circumference of the back wheels
    laser_dist=0.44; % distance from baselink to laser frame
    laser_shift_theta=-0.023; % angle between x-axis and the-direction-from-baselink-to-the-laser
    laser_yaw = -0.0175; % angle between x-axis and the-0-degree-of-the-laser (yaw)
    
    x0 = [wheel_base;...
    wheel_circ;...
    wheel_circ;...
    laser_dist;...
    laser_shift_theta;...
    laser_yaw];
end

function [x_b, y_b, theta_b] = tf_transform_laser_to_baselink...
    (x_l, y_l, theta_l, laser_dist, laser_shift_theta, laser_yaw)    

    theta_b = theta_l - laser_yaw;
    x_b = x_l - laser_dist * cos(theta_b + laser_shift_theta);
    y_b = y_l - laser_dist * sin(theta_b + laser_shift_theta);
end

function [x, reconst_baselink_encoder, reconst_baselink_laser] =...
    calibrate_full(pose2D_laser, encoder_num, x0)
    
    global index

    % optimization
    x = fminsearch(@calibrate_it, x0);

%     % use fmincon if you would like to have constraints on the variables        
%     A = [0 0 0 0 1 0];
%     b = 0;
%     x = fmincon(@calibrate_it, x0, A, b);
    
    % reconstruct the two traces
    reconst_baselink_laser =...
        create_baselink_traj_from_laser(pose2D_laser, x);
    reconst_baselink_encoder =...
        create_baselink_traj_from_encoder(encoder_num, reconst_baselink_laser(1,:), x);     
        
    function RMS = calibrate_it(x)        
        
        % reconstruct the trajectory of the baselink based on laser
        reconst_laser = create_baselink_traj_from_laser(pose2D_laser, x);
        
        % reconstruct path by the new x        
        reconst_encoder =...
            create_baselink_traj_from_encoder(encoder_num, reconst_laser(1,:), x); 
        
        % interp1 the trace
        reconst_encoder = interp1_odom_to_laser_time(reconst_encoder, pose2D_laser);

        % calculate distance
        dx = reconst_encoder(:,index.x)-reconst_laser(:,index.x);
        dy = reconst_encoder(:,index.y)-reconst_laser(:,index.y);
        RMS = nansum(sqrt(dx.^2 + dy.^2))/numel(dx);        
        fprintf('RMS = %f\n', RMS);
    end
end

function traj = create_baselink_traj_from_laser(laser, x)
    global index
    
    numSamples = size(laser,1);
    traj = [laser(:,1) nan(numSamples,3)];
    for i=1:numSamples
        [traj(i,index.x), traj(i,index.y), traj(i,index.theta)] =...
            tf_transform_laser_to_baselink...
            (laser(i,index.x), laser(i,index.y), laser(i,index.theta),...
            x(index.laser_dist), x(index.laser_shift_theta), x(index.laser_yaw));
    end
end

function newTheta = make_theta_continuous(theta)

    dth = diff(theta);
    ii = dth > pi;
    dth(ii) = dth(ii) - 2*pi;
    ii = dth < -pi;
    dth(ii) = dth(ii) + 2*pi;

    newTheta = [theta(1); dth];
    newTheta = cumsum(newTheta);
end

function nodom = interp1_odom_to_laser_time(odom, laser)
    global index;
    nodom = [laser(:,index.time)...
        interp1(odom(:,1), odom(:,2:end), laser(:,1))];
end

function pose = create_baselink_traj_from_encoder(encoder, init_pose, x)
    global index;

    encoder_count=30240; % encoder counts per revolution (7560 pulses)
    
    % initialize pose array
    pose = nan(size(encoder,1), size(encoder,2)+1);    
    pose(:,index.time) = encoder(:,index.time);
    
    curr_pose = init_pose;
    for i=1:size(encoder,1)
        % compute the movement
        left_wheel = (encoder(i,index.leftwheel)/encoder_count)*x(index.wheel_circ_left);
        right_wheel = (encoder(i,index.rightwheel)/encoder_count)*x(index.wheel_circ_right);
        distance = (left_wheel+right_wheel)/2.0;
       
        % movement in 3 axis
        dth = (right_wheel-left_wheel)/x(index.wheel_base);        
        dx = distance * cos(curr_pose(index.theta));
        dy = distance * sin(curr_pose(index.theta));
        
        % update pose
        curr_pose = curr_pose + [0 dx dy dth];        
        pose(i,index.x) = curr_pose(index.x);
        pose(i,index.y) = curr_pose(index.y);
        pose(i,index.theta) = curr_pose(index.theta);                
    end            
end

function plot_traj(traj1, traj2, trajLegend)
    global index;    
    trajColor = {'b', 'r'};
    
    figure; hold on;
    quiver(traj1(:,index.x), traj1(:,index.y), cos(traj1(:,index.theta)), sin(traj1(:,index.theta)));
    quiver(traj2(:,index.x), traj2(:,index.y), cos(traj2(:,index.theta)), sin(traj2(:,index.theta)));
    hold off;    
    legend(trajLegend)
end

function [pose2D_laser, encoder_num] =...
    load_data(filepath, prefix)
    
    suffix = {'pose2D_laser', 'encoder_num'};
    for i=1:numel(suffix)
        filename = sprintf('%s/%s_%s.csv', filepath, prefix, suffix{i});
        data = importdata(filename);
        eval(sprintf('%s = data;', suffix{i}));
    end    
end

Originally posted by PT with karma: 234 on 2015-04-11

This answer was ACCEPTED on the original site

Post score: 1

$\endgroup$

Your Answer

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