To answer your first question: if you really want to find the true kinematic equations for differential drive, I wouldn't start approximating by assuming that each wheel has moved in a straight line. Instead, find the turning radius, calculate the center point of the arc, and then calculate the robot's next point. The turning radius would be infinite if the robot is moving straight, but in the straight case the math is simple. Here's
So, just imagine that over each time step, or each time you calculate the change in the incremental sensors, the robot travels from point A to point B on an arc like this:
Here's some sample code with the math simplified:
// leftDelta and rightDelta = distance that the left and right wheel have moved along
// the ground
if (fabs(leftDelta - rightDelta) < 1.0e-6) { // basically going straight
new_x = x + leftDelta * cos(heading);
new_y = y + rightDelta * sin(heading);
new_heading = heading;
} else {
float R = unitsAxisWidth * (leftDelta + rightDelta) / (2 * (rightDelta - leftDelta)),
wd = (rightDelta - leftDelta) / unitsAxisWidth;
new_x = x + R * sin(wd + heading) - R * sin(heading);
new_y = y - R * cos(wd + heading) + R * cos(heading);
new_heading = boundAngle(heading + wd);
}
I used similar math in a simulator to demonstrate different ways of steering: http://www.cs.utexas.edu/~rjnevels/RobotSimulator4/demos/SteeringDemo/
To answer your first question: if you really want to find the true kinematic equations for differential drive, I wouldn't start approximating by assuming that each wheel has moved in a straight line. Instead, find the turning radius, calculate the center point of the arc, and then calculate the robot's next point. The turning radius would be infinite if the robot is moving straight, but in the straight case the math is simple. Here's some sample code:
// leftDelta and rightDelta = distance that the left and right wheel have moved along
// the ground
if (fabs(leftDelta - rightDelta) < 1.0e-6) { // basically going straight
new_x = x + leftDelta * cos(heading);
new_y = y + rightDelta * sin(heading);
new_heading = heading;
} else {
float R = unitsAxisWidth * (leftDelta + rightDelta) / (2 * (rightDelta - leftDelta)),
wd = (rightDelta - leftDelta) / unitsAxisWidth;
new_x = x + R * sin(wd + heading) - R * sin(heading);
new_y = y - R * cos(wd + heading) + R * cos(heading);
new_heading = boundAngle(heading + wd);
}
I used similar math in a simulator to demonstrate different ways of steering: http://www.cs.utexas.edu/~rjnevels/RobotSimulator4/demos/SteeringDemo/
To answer your first question: if you really want to find the true kinematic equations for differential drive, I wouldn't start approximating by assuming that each wheel has moved in a straight line. Instead, find the turning radius, calculate the center point of the arc, and then calculate the robot's next point. The turning radius would be infinite if the robot is moving straight, but in the straight case the math is simple.
So, just imagine that over each time step, or each time you calculate the change in the incremental sensors, the robot travels from point A to point B on an arc like this:
Here's some sample code with the math simplified:
// leftDelta and rightDelta = distance that the left and right wheel have moved along
// the ground
if (fabs(leftDelta - rightDelta) < 1.0e-6) { // basically going straight
new_x = x + leftDelta * cos(heading);
new_y = y + rightDelta * sin(heading);
new_heading = heading;
} else {
float R = unitsAxisWidth * (leftDelta + rightDelta) / (2 * (rightDelta - leftDelta)),
wd = (rightDelta - leftDelta) / unitsAxisWidth;
new_x = x + R * sin(wd + heading) - R * sin(heading);
new_y = y - R * cos(wd + heading) + R * cos(heading);
new_heading = boundAngle(heading + wd);
}
I used similar math in a simulator to demonstrate different ways of steering: http://www.cs.utexas.edu/~rjnevels/RobotSimulator4/demos/SteeringDemo/
