Ask Your Question
0

How to fuse encoder ticks + IMU for odometry?

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

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

1 Answer

Sort by ยป oldest newest most voted
4

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

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 imagegvdhoorn ( 2019-01-09 06:34:49 -0500 )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 imagestevemartin ( 2019-01-09 06:38:54 -0500 )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 imagestevemartin ( 2019-01-09 06:54:07 -0500 )edit

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

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2019-01-09 07:34:10 -0500 )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 imagePeteBlackerThe3rd ( 2019-01-09 07:35:57 -0500 )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

1 follower

Stats

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

Seen: 1,044 times

Last updated: Jan 09