Robot positioning problem

Question

The problem i am facing is to try and calculate the x and y position of a robot with dead reckoning.

Reading from the encoders and getting proper rotations of the wheels of my robot works. The robot has 3 wheels, where 2 of the 3 can be controlled like in this picture:

Calculating the distance from these wheel rotations and from those distances the heading of the robot is tested and works properly. Here is the code for the heading:

void MotorHandler::UpdateHeading(Position& currentPosition, double leftMotorDistance, double rightMotorDistance)
{
   currentPosition.heading = (double)((int)(((rightMotorDistance - leftMotorDistance) / (2 * PI * AXLE_LENGTH)) * FULL_ROTATION_DEGREES) % 360);
}

Where axle_length is the length between both wheels and full_rotation_degree is 360.

But after this, when i try to calculate the x and y of the robot at any given moment i get weird results. Here is the code for calculating the x and y:

void MotorHandler::CalculateCurrentPosition(Position& currentPosition)
{
double leftMotorDistance = (motors[0]->GetRotation()/FULL_ROTATION_PULSES) * WHEEL_CIRCUMFERENCE;
double rightMotorDistance = (motors[1]->GetRotation()/FULL_ROTATION_PULSES) * WHEEL_CIRCUMFERENCE;

double headingToRadians = (currentPosition.heading * PI) / 180;

if (fabs(leftMotorDistance - rightMotorDistance) < 1.0e-6)
{
    currentPosition.X = leftMotorDistance * cos(headingToRadians);
    currentPosition.Y = rightMotorDistance * sin(headingToRadians);
}
else
{
    double wd = (rightMotorDistance - leftMotorDistance) / AXLE_LENGTH;
    double expr = (AXLE_LENGTH * (rightMotorDistance + leftMotorDistance)) / (2 * (rightMotorDistance - leftMotorDistance));

    currentPosition.X = expr * sin(wd + headingToRadians) - expr * sin(headingToRadians);
    currentPosition.Y = -expr * cos(wd + headingToRadians) + expr *  cos(headingToRadians);
    UpdateHeading(currentPosition, leftMotorDistance, rightMotorDistance);
}
}

Where the starting x, y and heading are 0. As said, calculating the heading of the robot works properly, which means that the distances of the wheels also are calculated correctly and works properly.

Links used:

https://www.cs.princeton.edu/courses/archive/fall11/cos495/COS495-Lecture5-Odometry.pdf http://rossum.sourceforge.net/papers/DiffSteer/
Calculate position of differential drive robot

Ty!

Answer 1

This is another aspect of a longer answer I wrote here. Briefly restated, your problem is that a wheeled robot like this is nonholonomic, which means you can just use absolute encoder counts and get a valid result for position and heading.

You have two axes of control: a left wheel and a right wheel; and you have three degrees of freedom: left/right translation, forward/backward translation, and rotation.

I'll restate the example given at the other answer. Consider:

1. Moving forward 10 meters and turning 180 degrees, =OR=
2. Turning 180 degrees and moving forward 10 meters.

How do these scenarios work in your code? It looks like your heading is probably calculated correctly, but your position is based solely on the current heading. In both cases above, the robot ends after having turned 180 degrees. In the second case, your code should give correct answers, because there is no translation before you rotate.

The first case, however, would see your code correctly track the movement as it goes forward 10 meters but then, as the robot rotates, your code uses the current heading to index the distance traveled, and so it would appear as if your robot were quickly scribing a circle of radius 10 meters.

Again, as stated in the other answer, the solution is to do as @50k4 mentioned and accumuluate current position as a differential quantity for each sample, and not just look at overall counts.