Inverse kinematics for differential robot knowing linear and angular velocities

I have a differential robot and I am using ROS to run it. When I need to move the robot, I can send commands to it, specifically linear X and angular Z velocities. Linear X is in m/s whereas angular Z is in rad/s.

Know I need to compute what velocities should I set to both wheels in order to meet the linear and angular velocities. I have found this formula:

velocity_left_cmd = (linear_velocity – angular_velocity * WHEEL_BASE / 2.0)/WHEEL_RADIUS;

velocity_right_cmd = (linear_velocity + angular_velocity * WHEEL_BASE / 2.0)/WHEEL_RADIUS;


But it does not seem to be correct. If I put the linear velocity to be 0.5 m/s and angular to be 0.6 rad/s, then having my WHEEL_BASE of 0.14 meters and WHEEL_RADIUS of 0.03 meters, I get very large values, like 15 m/s on one of the wheels. That does not seem to be correct.

What is the right way to get the individual velocities on each wheel?

Your linear velocity should be the average of both wheel values. Assuming there's some wheel radius of WHEEL_RADIUS, as you've stated, then you should get each wheel speed as:

left_velocity = left_rpm * (RPM_TO_RAD_PER_S * DIST_PER_RAD);
linear_velocity = 0.5f * (left_velocity + right_velocity);


Units matter and they're important! I define RPM_TO_RAD_PER_S here because you need to convert from motor RPM to radians per second; that conversion is:

$$\left(\frac{1 \mbox{ rev}}{1 \mbox{ minute}}\right) * \left( \frac{1 \mbox{ minute}}{60 \mbox{ seconds}} \right) * \left(\frac{2\pi \mbox{ radians}}{1 \mbox{ rev}}\right) \\ \left(\frac{1 \require{cancel}\bcancel{\mbox{ rev}}}{1 \require{cancel}\cancel{\mbox{ minute}}}\right) * \left( \frac{1 \require{cancel}\cancel{\mbox{ minute}}}{60 \boxed{\mbox{ seconds}}} \right) * \left(\frac{2\pi \boxed{\mbox{ radians}}}{1 \require{cancel}\bcancel{\mbox{ rev}}}\right) \\ \mbox{RPM_TO_RAD_PER_S} = \frac{2\pi}{60} \ = 0.1047$$

The DIST_PER_RAD is just the circumference, 2*pi*WHEEL_RADIUS, divided by the number of radians in a complete revolution, which is just 2*pi. This means that DIST_PER_RAD is the same as the WHEEL_RADIUS, but I think it's important to point out why the equation works out the way that it does. Convert RPM to rad/s, and then use that value with the wheel radius to get speed.

Important to note - left_rpm and right_rpm actually need to be in RPM, and DIST_PER_RAD and/or WHEEL_RADIUS needs to be in meters in order to get the output velocities in meters per second.

Moving on to the rotation rate -

Assume for a moment that rotation is positive CCW, viewed from above. If, hypothetically, the robot is rotating CCW, then it would mean that the right wheel is moving some distance further than the left wheel. The difference in distances traveled becomes the arc length scribed by the right wheel (again, relative to the left), and the wheel base, or distance between the wheels, becomes the hypotenuse or radius for the rotation.

Continuing with the assumption, positive is CCW, you would wind up with an equation like:

angular_rotation = (right_distance - left_distance)/WHEEL_BASE;


You can divide through by time to get:

angular_rate = (right_velocity - left_velocity)/WHEEL_BASE;


This is coupled with the linear velocity equation, and you wind up with:

angular_rate = (right_velocity - left_velocity)/WHEEL_BASE;
linear_velocity = 0.5f * (left_velocity + right_velocity);


You can solve for the right_velocity in the angular_rate equation:

angular_rate = (right_velocity - left_velocity)/WHEEL_BASE;
angular_rate*WHEEL_BASE + left_velocity = right_velocity;


And then plug that into the linear velocity equation:

linear_velocity = 0.5f * (left_velocity + angular_rate*WHEEL_BASE + left_velocity);


Then solve for the left_velocity:

linear_velocity = 0.5f*left_velocity + 0.5f*angular_rate*WHEEL_BASE + 0.5f*left_velocity;
linear_velocity = 0.5f*left_velocity + 0.5f*left_velocity + 0.5f*angular_rate*WHEEL_BASE;
linear_velocity = left_velocity + 0.5f*angular_rate*WHEEL_BASE;
linear_velocity - 0.5f*angular_rate*WHEEL_BASE = left_velocity;

left_velocity = linear_velocity - 0.5f*angular_rate*WHEEL_BASE;


And then plug this back into the expression for the right_velocity:

angular_rate*WHEEL_BASE + left_velocity = right_velocity;
right_velocity = angular_rate*WHEEL_BASE + left_velocity;
right_velocity = angular_rate*WHEEL_BASE + linear_velocity - 0.5f*angular_rate*WHEEL_BASE;
right_velocity = angular_rate*WHEEL_BASE - 0.5f*angular_rate*WHEEL_BASE + linear_velocity;

right_velocity = 0.5f*angular_rate*WHEEL_BASE + linear_velocity;


This then gives the equations for left_ and right_velocities in terms of linear velocity and angular rate:

left_velocity =  linear_velocity - 0.5f*angular_rate*WHEEL_BASE;
right_velocity = linear_velocity + 0.5f*angular_rate*WHEEL_BASE;


Now, recall the equations for the conversion from _rpm to _velocity:

left_velocity = left_rpm * (RPM_TO_RAD_PER_S * DIST_PER_RAD);


And you can see that:

left_rpm  = left_velocity/(RPM_TO_RAD_PER_S * DIST_PER_RAD);


Which, substituting in the _velocity terms, gives:

left_rpm  = (linear_velocity - 0.5f*angular_rate*WHEEL_BASE)/(RPM_TO_RAD_PER_S * DIST_PER_RAD);


This looks similar to what you state from the start, but there's a conversion here from RPM to rad/s. If you wanted this in percentage, then you should use the percentage formula:

percentage = 100.0f * (value/maximum);


I would define a value to be something like:

RPM_TO_PERCENT = 100.0f/TOP_SPEED_RPM;


and then you can do:

left_percent  = left_rpm * RPM_TO_PERCENT;
right_percent = right_rpm * RPM_TO_PERCENT;


You can define any other values you want, too, such as a value that would get you to a PWM output on a 0-255 scale, like:

RPM_TO_PWM = 255.0f/TOP_SPEED_RPM;


and then cast to int on the equation:

left_pwm =  <int>(left_rpm * RPM*PWM);
right_pwm = <int>(right_rpm * RPM*PWM);


You haven't posted any sample data, so I'm assuming this is the answer to your question, at least - that the source of your trouble is a units problem. Don't worry about defining too many variables! I don't think you would ever run into a problem where you're at a limit on how many variables you have defined. Defining the conversion constants like this makes things easier to read, debug, maintain/update, etc.

Your result is correct: I got encoder values by a measurement, which perfectly fit with your equations. However, I don't understand your thoughts.

Firstly, you wrote velocity = rounds per something * circumference. You calculated with rpm, which is rounds per minute (= 60 s). But what to do in case of rps? Then you cannot divide through 60, which is here a quite important step to do.

Secondly, you mentioned that the DIST_PER_RAD is the circumference (2 * pi * r) divided by 2 * pi for a complete revolution, which results just in the radius. In my opinion there must be calculated v = n * 2 * pi * r (with n = number of revolutions in 1/s or 1/min or whatever). Then you'll get the distance, which belongs to n. Example: 1 round per something (s, min, h) is a complete revolution of the wheel in one second, minute or hour. We'll get just the half distance in case of n = 1/2 rounds per something (s, min, h). I.e. it isn't necessary to divide the circumference by 2 * pi.

However, when I calculate the velocity as v = n * 2 * pi * r I don't get the correct results, i.e. in fact the factor 0.1047 is absolutely necessary.

Do you've any idea where my mistake is?