Forward Kinematics of A Differential Drive Robot

alfi411 · October 25, 2022, 5:05pm

Hello guys,
I've been braking my head over this for the past few days and can't make it work.. So, I have a small robot with a rotary encoder on each wheel and i want to track its position (in x and y) on a flat plane. I know it's called forward kinematics, but i can't seem to get it working. I found a lot of great public sheets, but i think, out of all of them, this is the best one It had the equations of how to calculate the X and Y on page 74:

Vicc=L * (Din / (Dout–Din) + ½) (page 73)
 //Din is the inner wheel of the turn traveled distance, Dout is the outer wheel

> Xtδ=Xt+Vicc*(sin(θt+θΔ) – sinθt)
> Ytδ=Yt+Vicc*(cosθt - cos(θt+θΔ))
> θtδ=θt+θΔ //angle measurement

Where Xtδ and Ytδ are the position of the robot (x, y); Xt/Yt/θt is the position of the robot when the counting of the position has been started; θΔ is the angle of the robot now;

Here's The code that I tried to write using the equations.

#include <Encoder.h>

Encoder Left(3, 2);
Encoder Right(11, 10);

float R = 385; //mm
float r = 12.5;
float L = 8;

double LeftDist = 0;
long LOldPos  = -999;
float LNewPos;
int Correction;

double RightDist = 0;
long ROldPos  = -999;
float RNewPos;

int fi = 0;
unsigned long t;

int Xt = 0;
int Xtb = 0;
int Yt = 0;
int Ytb = 0;
int fit = 0;
int fitb = 0;
int Vicc;

void setup() {
  Serial.begin(9600);
  t = millis();
}

void loop() {
  Encoders();
  if ((millis() - t) >= 100) {
    Xt = Xtb;
    Yt = Ytb;
    fit = fitb;

    Vicc = (L * (LeftDist / (RightDist / LeftDist))) + 0.5;
    Xtb = Xt + (Vicc * (sin(fit + fi) - sin (fit)));
    Ytb = Yt + (Vicc * (cos(fit) - cos(fit + fi)));
    fitb = fit + fi;

    t = millis();
  }
  Serial.print(Vicc);
  Serial.print("        ");
  Serial.print(Xtb);
  Serial.print("        ");
  Serial.println(Ytb);
}

//0.9mm/1 deg
void Encoders() {
  LNewPos = Left.read();
  if (LNewPos != LOldPos) {
    LOldPos = LNewPos;
    LeftDist = ((LNewPos / 840) * (2 * r * 3.14)) / 10; //MM / Rev
  }
  //-----------------------------------------------------
  RNewPos = Right.read();
  if (RNewPos != ROldPos) {
    ROldPos = RNewPos;
    RightDist = ((RNewPos / 840) * (2 * r * 3.14)) / 10; //MM / Rev
  }
  fi = (abs(RightDist - LeftDist) * 0.9) - Correction;
  if (fi >= 360) Correction += 360;
  else if (fi <= -360) Correction -= 360;
}

I expected it to write my coordinates in x and y, but when i moved the wheels, it sent me big, random numbers, which are fluctuating up and down, which looks like a sine wave. What am I doing wrong? Could you guide me to the right path? My end result, that I want to reach is controlling 2 both motor speeds and knowing the robots position. Thanks in advance!

jremington · October 25, 2022, 5:14pm

Hard to follow your implementation of the math. Are you using radians for the trigonometric function arguments, as required?

What is this?

  if (fi >= 360) Correction += 360;
  else if (fi <= -360) Correction -= 360;

gcjr · October 25, 2022, 7:53pm

testing math on a target is difficult.

what about trying to simulate it by modeling a robot moving in a circle

here's the initial output using your routines with the simulated encoder values -- corrected

    800 TicPerRot
      2 WheelDia
  127.3 TicPerIn
     24 Rad
  163.4 CircOuter
  138.2 CircInner
 a   0, L     0, R     0 - 0        0        0
 a  15, L   866, R   733 - 0        0        0
 a  30, L  1733, R  1466 - 0        0        0
 a  45, L  2600, R  2199 - 0        0        0
 a  60, L  3466, R  2933 - 0        0        0
 a  75, L  4333, R  3666 - 0        0        0
 a  90, L  5200, R  4399 - 0        0        0
 a 105, L  6066, R  5133 - 0        0        0
 a 120, L  6933, R  5866 - 0        0        0
 a 135, L  7800, R  6599 - 689        -688        722
 a 150, L  8666, R  7333 - 689        -688        722
 a 165, L  9533, R  8066 - 689        -688        722
 a 180, L 10400, R  8799 - 689        -688        722
 a 195, L 11266, R  9533 - 689        -688        722
 a 210, L 12133, R 10266 - 689        -688        722
 a 225, L 13000, R 10999 - 689        -688        722
 a 240, L 13866, R 11733 - 689        -688        722
 a 255, L 14733, R 12466 - 689        -688        722
 a 270, L 15600, R 13199 - 689        -688        722
 a 285, L 16466, R 13933 - 1455        1982        -145
 a 300, L 17333, R 14666 - 1455        1982        -145
 a 315, L 18200, R 15399 - 1455        1982        -145
 a 330, L 19066, R 16133 - 1455        1982        -145
 a 345, L 19933, R 16866 - 1455        1982        -145
 a 360, L 20800, R 17599 - 1455        1982        -145

#include <stdio.h>
#include <math.h>

#define abs(a)   (0 < a ? a : -a)

struct Encoder {
    float _tics;
    Encoder (int a, int b)  { }
    void set  (float tics)  { _tics = tics; }
    int  read (void)        { return  _tics; }
};

struct Serial {
    void begin (int )       { }

    void print (const char *s)  { printf ("%s", s); }
    void print (int i)          { printf ("%d", i); }
    void println (int i)        { printf ("%d\n", i); }
} Serial;

unsigned long millis (void)  {
    static unsigned long msec;

    return msec += 10;
}

// -----------------------------------------------------------------------------
Encoder Left(3, 2);
Encoder Right(11, 10);

float R = 385; //mm
float r = 12.5;
float L = 8;
double LeftDist = 0;
long LOldPos  = -999;
float LNewPos;
int Correction;
double RightDist = 0;
long ROldPos  = -999;
float RNewPos;
int fi = 0;
unsigned long t;
int Xt = 0;
int Xtb = 0;
int Yt = 0;
int Ytb = 0;
int fit = 0;
int fitb = 0;
int Vicc;

// ---------------------------------------------------------
void setup() {
    Serial.begin(9600);
    t = millis();
}

// ---------------------------------------------------------
//0.9mm/1 deg
void Encoders() {
    LNewPos = Left.read();
    if (LNewPos != LOldPos) {
        LOldPos  = LNewPos;
        LeftDist = ((LNewPos / 840) * (2 * r * 3.14)) / 10; //MM / Rev
    }
    //-----------------------------------------------------
    RNewPos = Right.read();
    if (RNewPos  != ROldPos) {
        ROldPos   = RNewPos;
        RightDist = ((RNewPos / 840) * (2 * r * 3.14)) / 10; //MM / Rev
    }

    fi = (abs(RightDist - LeftDist) * 0.9) - Correction;

    if (fi >= 360)
        Correction += 360;
    else if (fi <= -360)
        Correction -= 360;
}

// ---------------------------------------------------------
void loop() {
    Encoders();
    if ((millis() - t) >= 100) {
        Xt   = Xtb;
        Yt   = Ytb;
        fit  = fitb;
        Vicc = (L * (LeftDist / (RightDist / LeftDist))) + 0.5;
        Xtb  = Xt + (Vicc * (sin(fit + fi) - sin (fit)));
        Ytb  = Yt + (Vicc * (cos(fit) - cos(fit + fi)));
        fitb = fit + fi;
        t = millis();
    }
    Serial.print(Vicc);
    Serial.print("        ");
    Serial.print(Xtb);
    Serial.print("        ");
    Serial.println(Ytb);
}

// -----------------------------------------------------------------------------
/*
 dL = circL * k = (rad + 2) * k
 dR = circR * k = (rad - 2) * k

 dL - dR =  k * ((rad + 2) - (rad - 2))
 dL - dR =  k * 4

 */

// -----------------------------------------------------------------------------
const int   TicPerRot = 800;
const int   WheelDia  = 2;                    // inches
const float TicPerIn  = TicPerRot / (M_PI * WheelDia);

const int   Rad       = 24;                   // inches
const float CircOuter = M_TWOPI * (Rad + 2);  // in.
const float CircInner = M_TWOPI * (Rad - 2);  // in.

int
main ()
{
    printf (" %6d TicPerRot\n", TicPerRot);
    printf (" %6d WheelDia\n",  WheelDia);
    printf (" %6.1f TicPerIn\n",  TicPerIn);
    printf (" %6d Rad\n",       Rad);
    printf (" %6.1f CircOuter\n", CircOuter);
    printf (" %6.1f CircInner\n", CircInner);

    int dA  = 15;
    for (int a = 0; a <= 360; a += dA)  {
        Left.set  (TicPerIn * CircOuter * a / 360);
        Right.set (TicPerIn * CircInner * a / 360);

        printf (" a %3d", a);
        printf (", L %5d", Left.read ());
        printf (", R %5d - ", Right.read ());
        loop ();
    }

    return 0;
}

gcjr · October 25, 2022, 9:15pm

i replaced a call to Encoder() with a call to a function, analyze() (see comments), that assumes the path is a constant radius and attempts to determine the radius and # of degrees traveled

while this may fairly accurate estimate the position and angle of the 'bot after each iteration, i'm doubt it's accurate enough to accurately determine the final position

    800 TicPerRot
      2 WheelDia
  127.3 TicPerIn
     24 Rad
  163.4 CircOuter
  138.2 CircInner
 a   0, L     0, R     0 -  0.0000 fracCirc    0.0 dAngle   -nan rad
 a  15, L   866, R   733 -  0.0416 fracCirc   15.0 dAngle   24.0 rad
 a  30, L  1733, R  1466 -  0.0419 fracCirc   15.1 dAngle   23.9 rad
 a  45, L  2600, R  2199 -  0.0419 fracCirc   15.1 dAngle   23.9 rad
 a  60, L  3466, R  2933 -  0.0413 fracCirc   14.9 dAngle   24.2 rad
 a  75, L  4333, R  3666 -  0.0419 fracCirc   15.1 dAngle   23.9 rad
 a  90, L  5200, R  4399 -  0.0419 fracCirc   15.1 dAngle   23.9 rad
 a 105, L  6066, R  5133 -  0.0413 fracCirc   14.9 dAngle   24.2 rad
 a 120, L  6933, R  5866 -  0.0419 fracCirc   15.1 dAngle   23.9 rad
 a 135, L  7800, R  6599 -  0.0419 fracCirc   15.1 dAngle   23.9 rad
 a 150, L  8666, R  7333 -  0.0413 fracCirc   14.9 dAngle   24.2 rad
 a 165, L  9533, R  8066 -  0.0419 fracCirc   15.1 dAngle   23.9 rad
 a 180, L 10400, R  8799 -  0.0419 fracCirc   15.1 dAngle   23.9 rad
 a 195, L 11266, R  9533 -  0.0413 fracCirc   14.9 dAngle   24.2 rad
 a 210, L 12133, R 10266 -  0.0419 fracCirc   15.1 dAngle   23.9 rad
 a 225, L 13000, R 10999 -  0.0419 fracCirc   15.1 dAngle   23.9 rad
 a 240, L 13866, R 11733 -  0.0413 fracCirc   14.9 dAngle   24.2 rad
 a 255, L 14733, R 12466 -  0.0419 fracCirc   15.1 dAngle   23.9 rad
 a 270, L 15600, R 13199 -  0.0419 fracCirc   15.1 dAngle   23.9 rad
 a 285, L 16466, R 13933 -  0.0413 fracCirc   14.9 dAngle   24.2 rad
 a 300, L 17333, R 14666 -  0.0419 fracCirc   15.1 dAngle   23.9 rad
 a 315, L 18200, R 15399 -  0.0419 fracCirc   15.1 dAngle   23.9 rad
 a 330, L 19066, R 16133 -  0.0413 fracCirc   14.9 dAngle   24.2 rad
 a 345, L 19933, R 16866 -  0.0419 fracCirc   15.1 dAngle   23.9 rad
 a 360, L 20800, R 17599 -  0.0419 fracCirc   15.1 dAngle   23.9 rad

#include <stdio.h>
#include <math.h>

#define abs(a)   (0 < a ? a : -a)

struct Encoder {
    float _tics;
    Encoder (int a, int b)  { }
    void set  (float tics)  { _tics = tics; }
    int  read (void)        { return  _tics; }
};

struct Serial {
    void begin (int )       { }

    void print (const char *s)  { printf ("%s", s); }
    void print (int i)          { printf ("%d", i); }
    void println (int i)        { printf ("%d\n", i); }
} Serial;

unsigned long millis (void)  {
    static unsigned long msec;

    return msec += 10;
}

// -----------------------------------------------------------------------------
Encoder Left(3, 2);
Encoder Right(11, 10);

float R = 385; //mm
float r = 12.5;
float L = 8;
double LeftDist = 0;
long LOldPos  = -999;
float LNewPos;
int Correction;
double RightDist = 0;
long ROldPos  = -999;
float RNewPos;
int fi = 0;
unsigned long t;
int Xt = 0;
int Xtb = 0;
int Yt = 0;
int Ytb = 0;
int fit = 0;
int fitb = 0;
int Vicc;

// ---------------------------------------------------------
void setup() {
    Serial.begin(9600);
    t = millis();
}

// ---------------------------------------------------------
//0.9mm/1 deg
void Encoders() {
    LNewPos = Left.read();
    if (LNewPos != LOldPos) {
        LOldPos  = LNewPos;
        LeftDist = ((LNewPos / 840) * (2 * r * 3.14)) / 10; //MM / Rev
    }
    //-----------------------------------------------------
    RNewPos = Right.read();
    if (RNewPos  != ROldPos) {
        ROldPos   = RNewPos;
        RightDist = ((RNewPos / 840) * (2 * r * 3.14)) / 10; //MM / Rev
    }

    fi = (abs(RightDist - LeftDist) * 0.9) - Correction;

    if (fi >= 360)
        Correction += 360;
    else if (fi <= -360)
        Correction -= 360;
}

// ---------------------------------------------------------
void loop() {
    Encoders();
    if ((millis() - t) >= 100) {
        Xt   = Xtb;
        Yt   = Ytb;
        fit  = fitb;
        Vicc = (L * (LeftDist / (RightDist / LeftDist))) + 0.5;
        Xtb  = Xt + (Vicc * (sin(fit + fi) - sin (fit)));
        Ytb  = Yt + (Vicc * (cos(fit) - cos(fit + fi)));
        fitb = fit + fi;
        t = millis();
    }
    Serial.print(Vicc);
    Serial.print("        ");
    Serial.print(Xtb);
    Serial.print("        ");
    Serial.println(Ytb);
}

// -----------------------------------------------------------------------------
const int   TicPerRot = 800;
const int   WheelDia  = 2;                    // inches
const float TicPerIn  = TicPerRot / (M_PI * WheelDia);

const int   Rad       = 24;                   // inches
const float CircOuter = M_TWOPI * (Rad + 2);  // in.
const float CircInner = M_TWOPI * (Rad - 2);  // in.

/*
 dL = circL * k = TwoPi * TicPerIn * fracCirc * (rad + 2)
 dR = circR * k = TwoPi * TicPerIn * fracCirc * (rad - 2)

 dL - dR = TwoPi * TicPerIn * fracCirc * 4

 fracCirc = (dL - dR) / (TwoPi * TicPerIn * 4)

 dL + dR = TwoPi * TicPerIn * fracCirc * ((rad + 2) - (rad - 2))
 dL + dR = TwoPi * TicPerIn * fracCirc * 2 * rad

 rad  = (dL + dR) / (TwoPi * TicPerIn * fracCirc * 2)

 dist    = (dL + dR) / 2

 */

int posL;
int posR;

void
analyze (void)
{
    int pos = Left.read ();
    int dL  = pos - posL;
    posL = pos;

    pos     = Right.read ();
    int dR  = pos - posR;
    posR = pos;

    float fracCirc  = (dL - dR) / (M_TWOPI * TicPerIn * 4);
    float rad       = (dL + dR) / (M_TWOPI * TicPerIn * fracCirc * 2);

    printf (" %6.4f fracCirc", fracCirc);
    printf (" %6.1f dAngle", fracCirc * 360);
    printf (" %6.1f rad", rad);
    printf ("\n");
}

// -------------------------------------
int
main ()
{
    printf (" %6d TicPerRot\n", TicPerRot);
    printf (" %6d WheelDia\n",  WheelDia);
    printf (" %6.1f TicPerIn\n",  TicPerIn);
    printf (" %6d Rad\n",       Rad);
    printf (" %6.1f CircOuter\n", CircOuter);
    printf (" %6.1f CircInner\n", CircInner);

    int dA  = 15;
    for (int a = 0; a <= 360; a += dA)  {
        Left.set  (TicPerIn * CircOuter * a / 360);
        Right.set (TicPerIn * CircInner * a / 360);

        printf (" a %3d", a);
        printf (", L %5d", Left.read ());
        printf (", R %5d - ", Right.read ());
#if 0
        loop ();
#else
        analyze ();
#endif
    }

    return 0;
}

alfi411 · October 26, 2022, 3:30am

sorry it's a test code I made, I shouldn't have put it in

alfi411 · October 26, 2022, 3:32am

Thank you very much @gcjr , i will try to implement this code to my robot

system · April 24, 2023, 3:32am

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.

Topic		Replies	Views
I want to find x, y, of moving robot using encoder Motors, Mechanics, Power and CNC	9	1488	May 6, 2021
Inverse Kinematics of Differential Drive Robots and Odometry Programming	58	5529	December 31, 2022
Confirm Rotary Encoder Equations Robotics	3	2689	May 6, 2021
convert Programming	12	1160	May 5, 2021
Count wheel encoders Programming	2	2978	May 5, 2021

Forward Kinematics of A Differential Drive Robot

Related topics