Wednesday, 16 December 2009

Differential drive to forward/turnrate conversion

When controlling a differential drive robot like the epuck, I like to be able to control the speeds of the left and right wheel with my code. This allows you to easily program simple things like Braitenberg vehicles because you can directly interact with the wheel speeds.

HOWEVER. With Player/Stage if you're using a differential drive robot you can only interact with the motors using the robot's forward speed (metres/sec) and its turning rate (radians/sec). In order for your left and right wheel speed controller to properly interact with Player/Stage you need to convert between the two. This involves some pretty horrible maths and kinematics type stuff. For the lazy, here is the conversion between differential wheel speeds in metres/sec into forward speed and turn rates for Player/Stage. Instead of left and right speeds though we use inner and outer wheel speeds because the wheel on the inside must be slower than the outer one because it travels less far and this affects the maths.

newspeed = Inner_wheel_speed
newturnrate = (Outer_wheel_speed - Inner_wheel_speed)/distance_between_wheels
All measures are given in metres/sec.



Explanation

In the Player example "laserobstacleavoid.cc" they use the following code to convert from differential wheel speeds to forward speed / turn rate:
newspeed = (r+l)/1e3;
newturnrate = dtor(r-l);

Where does 1e3 come from? Where do these formulas come from?

Since I'm no mathematician, so I used Google and came across this forum thread which discusses how to turn forward speeds and turn rates into left and right wheel speeds. This is the reverse of what we want to do here. I won't copy and paste the entire post here but these are the relevant formulae:
  • inner wheel speed (radians/second) = forward speed (metres/sec) / wheel radius (metres)
  • outer wheel speed (radians/second) = ( forward speed (metres/sec) + turn rate (radians/sec) * wheel separation )/wheel radius (metres)

The inner wheel is the one on the inside of the turn, and the outer wheel is the one on the outside. I want to provide wheel speeds in metres per second and convert these into a forward speed in metres/sec and a turn speed in radians/sec.
We'll shorted some of these terms to make the formulae sane. Ir is the inner wheel speed in radians/second, Im is the inner wheel speed in metres/second. Similarly Or and Om are the outer wheel speeds in radians and metres respectively. Newspeed will be the foward speed that we want to give Player, and newturnrate is the turning rate to give Player.

Let's start by looking at the inner wheel speed. The formula from the website says:
Ir = newspeed/wheel_radius
so...
newspeed = Ir*wheel_radius [1]

Let's convert Ir to Im:
We have the wheel speed Im in m/s as our fuction input, we should find out how many turns of the wheel we need to make in order to cover that distance. The circumference of the wheel is 2pi*wheel_radius so that it how far each wheel turn takes us. To find how many wheel turns are required per second we do:
number of wheel turns per sec = Im/2pi*wheel_radius
A whole wheel revolution is 2pi radians so we multiply our number of wheel revolutions by 2pi to get the number of radians/second we need to turn.
Ir = (2pi*Im)/(2pi*wheel_radius)
Ir = Im/wheel_radius [2]
substitute [2] into [1]:
newspeed = (Im*wheel_radius)/(wheel_radius)
newspeed = Im

That's easy enough to code! Now for the outer wheel. From the website:
Or = ( newspeed + newturnrate * wheel_separation )/wheel_radius
Now we convert Or to Om in the same way we did for the inner wheel in [2]
Or = Om/wheel_radius
so...
Om/wheel_radius = ( newspeed + newturnrate * wheel_separation )/wheel_radius
wheel radius cancels out on both sides:
Om = newspeed + (newturnrate * wheel_separation)
rearranging...
newturnrate = (Om - newspeed)/wheel_separation
newturnrate = (Om - Im)/wheel_separation

Finally we have a conversion! We can get the wheel separation by measuring the robot, Im and Om are our inputs.
Here's what you've all been waiting for, it's my code for converting wheel speeds to player commands:

/**
converts differential drive wheel speeds into a forward speed and turn rate that Player understands.
@param left speed of left wheel in metres/second
@param right speed of right wheel in metres/second
*/
setMotors(double left, double right)
{
/* conversion of differential drive values to 
forward and turn speeds from 
http://www.physicsforums.com/showthread.php?t=263149 */


/* wheel separation of an epuck. 
* Replace with correct value
* for your robot */
const double separation = 52*0.001; //52 mm
double newspeed, newturnrate;


//which is the outer wheel and which is the inner?
if(left > right) //if left is outer wheel
{
//newspeed is inner wheel speed
newspeed = right;
//newturnrate = (outer(m) - newspeed)/separation
newturnrate = (left - newspeed)/separation;
}
else if(right > left) //right is outer wheel
{
//newspeed is inner wheel speed
newspeed = left;
//newturnrate = (outer(m) - newspeed)/separation
newturnrate = (right - newspeed)/separation;
}
else
{
newspeed = left; //could also be right
newturnrate = 0;
}


p2dProxy.SetSpeed(newspeed, newturnrate);
}

3 comments:

  1. Hi Jenny,

    Thanks for the code, it was very helpful! I did find a few inaccuracies when I used it in my project, so I made some improvements that I thought you might want to see.

    Picture the following situation: I turn my robot's left wheel on, and leave the right wheel off. The robot should now spin around its right wheel. The code you've presented above doesn't exactly do that: since the forward speed is set to the speed of the inner wheel (which is in this case standing still) the robot does not move, and rotates around its centre point instead of its inner wheel.

    Another problem arises when I turn my robot's wheels on in opposing directions. The robot should spin around its centre point; in the original code, it would do something else entirely.

    Fortunately, the situation is easily fixed. Think about the first one-wheel-blocked-case. The robot has speed there, but not the speed of the inner wheel (which is standing still). In fact, the correct speed is that of an average between the two wheels. If my left wheel is spinning at 2 m/s and the right one at 0 m/s, the correct speed is 1 m/s. A formula to compute this is:

    newspeed = inner_wheel_speed + (outer_wheel_speed - inner_wheel_speed)/2

    Now what would happen when the wheels run in opposing directions, say at 1 and -1 m/s? Using this updated code the speed is 0, and the robot turns on the spot, exactly as it should!

    Note that since 'newspeed' now no longer is equal to the inner wheel speed, the next line setting the 'newturnrate' must also be slightly changed.

    Here is the full updated code snippet:

    ---------------
    // which is the outer wheel and which is the inner?
    if (left > right) { // left is outer wheel
    // newspeed is inner wheel speed
    newspeed = right + (left - right)/2;
    // newturnrate = (outer(m) - newspeed)/separation
    newturnrate = (left - right) / separation;
    }
    else if (right > left) { //right is outer wheel
    //newspeed is inner wheel speed
    newspeed = left + (right - left)/2;
    //newturnrate = (outer(m) - newspeed)/separation
    newturnrate = (right - newspeed) / separation;
    }
    else {
    newspeed = left; //could also be right
    newturnrate = 0;
    }
    ---------------

    Cheers!

    ReplyDelete
  2. Woopsy! I made two big mistakes in the code I pasted above. The issues are with the lines setting the 'newturnrate'. In the first case, left > right, it should have been (right - left) instead of (left - right), and in the second case I should have replaced 'newspeed' with 'left'. Yes, in both cases the line is now the same.

    Here's the patched code, which should be correct this time:
    ----------------
    // which is the outer wheel and which is the inner?
    if (left > right) { // left is outer wheel
    // newspeed is average between inner and outer wheel speed
    newspeed = right + (left - right)/2;
    newturnrate = (right - left) / separation;
    }
    else if (right > left) { //right is outer wheel
    // newspeed is average between inner and outer wheel speed
    newspeed = left + (right - left)/2;
    newturnrate = (right - left) / separation;
    }
    else {
    newspeed = left; //could also be right
    newturnrate = 0;
    }
    ----------------

    Hope that helps!

    ReplyDelete