Ask Your Question
0

How to fuse encoder ticks + IMU for odometry?

asked 2019-01-09 05:12:45 -0600

stevemartin gravatar image

Hi,

I have an IMU which shows the orientation perfectly and also installed some optical encoders. When I move the robot straight, the encoders will show the movement more or less correctly. However, when I turn and then move, IMU first displays orientation then when I move after the turn suddenly my robot gets drifted back and then move. I guess that this is because encoders do their job separately but how can I fuse them together?

This is my encoder tick callback (one of them, second will be similar):

void Odometry::leftencoderCb(const std_msgs::Int64::ConstPtr& left_ticks)
{
    double enc = left_ticks->data;

    if((enc < encoder_low_wrap) && (prev_lencoder > encoder_high_wrap))
    {
        lmult = lmult + 1;
    }

    if((enc > encoder_high_wrap) && (prev_lencoder < encoder_low_wrap))
    {
        lmult = lmult + 1;
    }

    left = 1.0 * (enc + lmult * (encoder_max - encoder_min));
    prev_lencoder = enc;
}

Here is my IMU callback:

void Odometry::ImuCb(const sensor_msgs::Imu::ConstPtr& data)
{
    orient_x = data->orientation.x;
    orient_t = data->orientation.y;
    orient_z = data->orientation.z;
    orient_w = data->orientation.w;

}

And finally, here is my update code which updates odometry periodically:

void Odometry::update()
{

    ros::Time now = ros::Time::now();
    double elapsed;
    double d_left, d_right, d, th,x,y;

    elapsed = now.toSec() - then.toSec();

    if(enc_left == 0)
    {
        d_left = 0;
        d_right = 0;
    }
        else
        {
            d_left = (left - enc_left) / ( ticks_meter);
            d_right = (right - enc_right) / ( ticks_meter);
        }

        enc_left = left;
        enc_right = right;

        d = (d_left + d_right ) / 2.0;
        th = ( d_right - d_left ) / base_width;

        dx = d / elapsed;
        dr = th / elapsed;

        if (d != 0)
        {
            ROS_INFO("Distance has been changed!");
            x = cos(th) * d;
            y = -sin(th) * d;
            x_final += (cos(theta_final) * x - sin(theta_final) * y);
            y_final += (sin(theta_final) * x + cos(theta_final) * y);
        }


        theta_final += th;

        geometry_msgs::Quaternion odom_quat ;

        odom_quat.x = this->orient_x;
        odom_quat.y = this->orient_y;
        odom_quat.z = this->orient_z;
        odom_quat.w = this->orient_w;

        geometry_msgs::TransformStamped odom_trans;
        odom_trans.header.stamp = now;
        odom_trans.header.frame_id = "odom";
        odom_trans.child_frame_id = "base_footprint";

        odom_trans.transform.translation.x = x_final;
        odom_trans.transform.translation.y = y_final;
        odom_trans.transform.translation.z = 0.0;
        odom_trans.transform.rotation = odom_quat;

        odom_broadcaster.sendTransform(odom_trans);

            // Odometry values:
        nav_msgs::Odometry odom;
        odom.header.stamp = now;
        odom.header.frame_id = "odom";

        odom.pose.pose.position.x = x_final;
        odom.pose.pose.position.y = y_final;
        odom.pose.pose.position.z = 0.0;
        odom.pose.pose.orientation = odom_quat;

        odom.child_frame_id = "base_footprint";
        odom.twist.twist.linear.x = dx;
        odom.twist.twist.linear.y = 0.0; //dy;
        odom.twist.twist.angular.z = 0.0; //dr;

        odom_pub.publish(odom);

        then = now;

        ros::spinOnce();



    else ROS_INFO_STREAM("Not in a loop");
}

Can someone suggest me how can I write odometry code more correctly?

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
4

answered 2019-01-09 06:04:53 -0600

The short answer to this is to not write your own odometry fusion algorithm from scratch, unless you have to do this as part of an assignment or for research. The existing robot localization package is very powerful and is designed for doing exactly this.

You will need to modify your code so that your encoder information is published in a twist message so that it can be sent to the robot localization node, then you can follow the guide on their website to configure the sensor fusion. There is a bit of a learning curve when it comes to using this node, but it's worth the effort when it's setup properly.

edit flag offensive delete link more

Comments

1

You will need to modify your code so that your encoder information is published in a twist message

shouldn't that be nav_msgs/Odometry for a mobile base?

gvdhoorn gravatar image gvdhoorn  ( 2019-01-09 06:34:49 -0600 )edit

Okay I get it, so basically I need to remove the IMU callback and let IMU be a separate thing from odometry, am I right? As without IMU, the orientation is terrible in Odometry.

stevemartin gravatar image stevemartin  ( 2019-01-09 06:38:54 -0600 )edit

And then use odometry (which estimates the position based on encoder ticks) and IMU as two separate things and fuse them both to robot_localization package to get the filtered odometry?

stevemartin gravatar image stevemartin  ( 2019-01-09 06:54:07 -0600 )edit

@gvdhoorn yes that's correct. A twist message would not be able to represent the absolute orientation.

PeteBlackerThe3rd gravatar image PeteBlackerThe3rd  ( 2019-01-09 07:34:10 -0600 )edit

@stevemartin yes the robot localization package will take in the odometry and imu messages and fuse them for you into a new odometry estimate. When correctly setup it will use an EKF to take the best of each sensor.

PeteBlackerThe3rd gravatar image PeteBlackerThe3rd  ( 2019-01-09 07:35:57 -0600 )edit
0

answered 2020-02-17 21:10:00 -0600

sentry5588 gravatar image

updated 2020-02-17 21:11:43 -0600

If there are two statistically independent IMUs, say IMU A, and IMU B. Then one IMU can be used to create the odometry message.

For example, use IMU A and encoders to construct the odometry message. Then use EKF to fuse the odometry with IMU B, the final localization would perform better than only use encoders to construct the odometry message.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2019-01-09 05:12:45 -0600

Seen: 3,273 times

Last updated: Feb 17 '20