3

I am using a differential wheel robot for my project. I need to know the current coordinates of the robot with respect to it's initial position taken as the origin. I m doing the computation on an Arduino UNO and the only sensory input that I get is from the two encoders. I have the function updateOdomenty() called in the loop() and this is it's corresponding code:

void updateOdometry()
{

  static int encoderRPosPrev = 0;
  static int encoderLPosPrev = 0;


  float SR = distancePerCount * (encoderRPos - encoderRPosPrev);
  float SL = distancePerCount * (encoderLPos - encoderLPosPrev);

  encoderRPosPrev = encoderRPos;
  encoderLPosPrev = encoderLPos;


  x += SR * cos(theta);           
  y += SL * sin(theta);
  theta += (SR - SL) / wheelDistance;

  if(theta > 6.28)
    theta -= 6.28;
  else if(theta < -6.28)
    theta += 6.28;
}

This is the code that me and my team mates made after reading this paper. I am wondering if this is the best possible way to solve this problem with an Arduino. If not, how is odometry done in differential wheeled systems?

daltonfury42
  • 277
  • 2
  • 11
  • Oh just a quick comment - instead of using "6.28" I think you can use "2PI" instead in Arduino language. If you don't do this you might* have a bug if the heading angle falls between these vaues. – Andy Dec 18 '15 at 10:10

1 Answers1

4

I would add a few lines after you check that theta is between +/- 2pi:

meanDistance = (SL + SR)/2;
posX = posX + meanDistance*cos (theta);
posY = posY + meanDistance*sin(theta);

This of course assumes theta is positive CCW starting from the +x-axis. This is similar but not the same as your code for X and Y, but your code appears to put the X origin on the right wheel and Y origin on the left, and I'm not sure why you would do that.

Additionally, your code for theta uses a small angle sine approximation, sin(theta) = theta = dy/dL. You could use a true asin for this instead.

Otherwise, taking my comments above into account, this is how I would do differential-drive dead reckoning. Your next, least-expensive upgrade would be a compass or magnetometer, but that would probably come with a decent IMU, which would be your second upgrade, with GPS being the last. But, as I said, if all you need is dead reckoning, you've basically got it.

Chuck
  • 16,061
  • 2
  • 18
  • 47
  • I am analyzing what you said. For one thing, the previous encoder values are initialized to zero only at the first function call as they are static. – daltonfury42 Dec 16 '15 at 20:13
  • @daltonfury42 - You are correct about the static function call and I am not a C programmer. Regarding my comment about your choice of origins, consider a zero point turn, such that SL = - SR. In this case, my code, (SL + SR)/2, gives a distance traveled of zero, which then updates X and Y with no change. Your code would actually move X a distance of SRcos(theta) and actually move Y a distance of SLsin(theta), as there are non-zero distances traveled by each wheel. – Chuck Dec 17 '15 at 01:23
  • You said "all you need is dead reckoning", my aim is to navigate to a certain coordinate from the current coordinate. So this dead reconing function would be a first step. Am I going in the right direction? – daltonfury42 Dec 17 '15 at 06:13
  • "Am I going in the right direction?" (literally!) - yes, dead reckoning is the general phrase for keeping track of position relative to the start, by accounting for known velocity and direction. "Odometry" is just one way of doing this. Adding in compass/IMU, etc. as Chuck says allows more refined dead reckoning. What you've got so far looks basically correct to me. – Andy Dec 17 '15 at 11:39
  • @daltonfury42 - Yes, you are doing fine. Dead reckoning, as Andy mentions, is trying to estimate your position by relative means - how much time elapsed at a particular speed, how many wheel encoder counts did you go, etc. The other alternative would be to navigate with absolute means, with a range finder, GPS, or some other device. With relative measurements, you're only as good as your sensors, so if you have wheel slip (most commonly), then you have no means to account for the slip. But, for your project, I'm sure it's good enough. – Chuck Dec 17 '15 at 13:41
  • I started with coded very similar to that of @daltonfury42. Where exactly does the asin function go? Would it be theta += asin((SR - SL) / wheelDistance);? – Marcel May 17 '16 at 23:44
  • what is wheelDistance value? – Benyamin Jafari Feb 24 '19 at 18:05
  • 1
    @BenyaminJafari - wheelDistance is the distance between the left and right wheels; it's called "track" on this Wikipedia page. – Chuck Feb 25 '19 at 05:58