Ask Your Question
1

robot motion estimation

asked 2012-11-26 19:57:35 -0500

Astronaut gravatar image

Hello

I have some problems with my amcl , the motion estimation and the calculated linear and angular velocity. My robot is an electrical wheelchair with camera, laser range finder and IMU. For the pose estimation am using the amcl ( which use only as an input the laser scans). So my amcl jump lots and also have a very noise datas. So my linear and angular velocity profiles are very very noise. So how to improve that??Should I use extended kalman filter for the localisation?? Im my node that calculate the velocity Im not using IMU, so how to improve that??This is my speed node and my angular velocity profiles

void speedCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
{

    btScalar r,p,y;
    btQuaternion q(msg->pose.pose.orientation.x,msg->pose.pose.orientation.y,msg->pose.pose.orientation.z,msg->pose.pose.orientation.w);
    btMatrix3x3(q).getEulerYPR(y,p,r);
    std_msgs::Float64 yaw;
    yaw.data = y;
    //ROS_INFO("Yaw: %f", yaw.data*57.29);


    if (count2 == 0){
        time_prev = ros::Time::now().toSec();
        count2=1;
    }

    double time_now = ros::Time::now().toSec();

    //double curr_theta = msg->theta;
    //double curr_x = msg->x;
    //double curr_y = msg->y;

    double curr_theta = yaw.data;
    double curr_x = msg->pose.pose.position.x;
    double curr_y = msg->pose.pose.position.y;

    //ROS_INFO(" before curr_x = %f, curr_y = %f", curr_x, curr_y);

    if (curr_x < 0){
        curr_x = curr_x * -1;
    }
    if (curr_y < 0){
        curr_y = curr_y * -1;
    }
    //ROS_INFO("after curr_x = %f, curr_y = %f", curr_x, curr_y);
    double time_dif = time_now - time_prev;
    if (prev_x != 0 || prev_y != 0){
        if (time_dif != 0){
            double dist_theta = (curr_theta - prev_theta);
            //if (dist_theta < 0){
              //  dist_theta = dist_theta * -1;
            //}
            double dist_x = (curr_x - prev_x);
            if (dist_x < 0){
                dist_x = dist_x * -1;
            }
            double dist_y = (curr_y - prev_y);
            if (dist_y < 0){
                dist_y = dist_y * -1;
            }
            double dist_tot = sqrt((dist_x * dist_x) + (dist_y * dist_y));// * 10;
            double speed = (dist_tot / time_dif);// * 3.6;
            double speed_theta = dist_theta /time_dif;

            if (speed < 0.12){
                speed = 0;
                zero_count++;
            }
            if (speed_theta < 0.07){
                speed_theta = 0;
            }

        ROS_INFO("linear speed= %f, anglular speed=  %f", speed, speed_theta)
                speed_sum = speed_sum + speed;
                speed_sum_theta = speed_sum_theta + speed_theta;
            if (speed_count == 20){
                speed = speed_sum / speed_count;
                speed_theta = speed_sum_theta / speed_count;
                if (zero_count > 7){
                    speed = 0;
                }
                speed_count = 0;
                speed_sum = 0;
                speed_sum_theta = 0;
                zero_count = 0;


                count = count + 1;

            }

        }
    }

    prev_theta = curr_theta;
    prev_x = curr_x;
    prev_y = curr_y;

    time_prev = time_now;

}

Velocity profile after passing a filter, because they were very very noisy.

image description

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
3

answered 2012-11-26 21:31:50 -0500

Lorenz gravatar image

If AMCL gives bad results, I guess the reason is poor odometry. Odometry should be continuous, i.e. it should not jump and AMCL heavily relies on it. I would generating odometry from laser scans as you mentioned in your other question a bad idea for a few reasons:

  1. Odometry will jump which will cause all different kinds of problems.

  2. Maybe a low frequency, depending on the frequency of your lasers. For instance, some Hokuyo lasers publish only with 10 Hz.

  3. No velocity data in the Odom message. While AMCL doesn't need that, higher-level nodes such as move_base do.

Both, points 1 an 2 could cause your problems. To get a really reliable solution, I believe you will have to use real encode values for odometry.

edit flag offensive delete link more

Comments

1

A one imortant comment. I do not have any encoders and not using the odometry. In my case all is laser based, so only using laser scans. so what can I do??

Astronaut gravatar imageAstronaut ( 2012-11-26 23:09:05 -0500 )edit
1

how to use move_base ???

Astronaut gravatar imageAstronaut ( 2012-11-26 23:14:55 -0500 )edit

we have this problem too Astronaut , but no answer for that ... have you found anything yet?

Mohsen Hk gravatar imageMohsen Hk ( 2013-05-09 13:12:26 -0500 )edit

well what we done is just compare other lokalisation method like hector slam, ekf with virtual and real odometry.. and we got different speed profiles..

Astronaut gravatar imageAstronaut ( 2013-05-09 15:07:10 -0500 )edit

hmm... i didn't get your point exactly. how can i estimate velocity "without encoder"? i can get position by Hector_mapping and AMCL. but how can i get velocity? (without encoder)

Mohsen Hk gravatar imageMohsen Hk ( 2013-05-09 22:19:59 -0500 )edit
1

from the pose.. hector mapping publish the pose as a twist massage so u can calculate it... delta_t = ros::Time::now().toSec() - time_last; odom.twist.twist.linear.x = (msg->pose.pose.position.x-xlast)/delta_t; odom.twist.twist.linear.y = (msg->pose.pose.position.y-ylast)/delta_t;

Astronaut gravatar imageAstronaut ( 2013-05-10 00:38:54 -0500 )edit

curr_yaw = tf::getYaw(msg->pose.pose.orientation); odom.twist.twist.angular.z = normalize(curr_yaw-yaw_last)/delta_t; vel_linear = sqrt(odom.twist.twist.linear.xodom.twist.twist.linear.x + odom.twist.twist.linear.yodom.twist.twist.linear.y); vel_angular = fabs(odom.twist.twist.angular.z*57.29);

Astronaut gravatar imageAstronaut ( 2013-05-12 16:00:42 -0500 )edit

Before all this you need PoseCallback where odom.pose.pose.position.x = msg->pose.pose.position.x; odom.pose.pose.position.y = msg->pose.pose.position.y; odom.pose.pose.orientation = msg->pose.pose.orientation; odom.header.stamp = msg->header.stamp;

Astronaut gravatar imageAstronaut ( 2013-05-12 16:02:04 -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

3 followers

Stats

Asked: 2012-11-26 19:57:35 -0500

Seen: 975 times

Last updated: Nov 26 '12