0
$\begingroup$

I found formulas for the speeds of the robot in several directions Vx, Vy and w, when I try to find the coordinates of the robot from these speed.I want to define the global coordinates of the robot.

formulas i got from here https://github.com/michele-bertoni/omni3/blob/main/omni3.cpp#:~:text=this%2D%3E-,displacement%5BFORWARD%5D,-%3D%20T30R%20*

wR, wL, wB this angular speed wheel right, wheel left and wheel back.

public void forwardKinematics() {
    this.Vx = Math.tan(Math.toRadians(30)) * (this.wR - this.wL);
    this.Vy = Constants.WHEEL_RADIUS / 3 * (this.wR - 2 * this.wB + this.wL);
    this.w = (Constants.WHEEL_RADIUS / (3 * Constants.ROBOT_RADIUS)) * (this.wR + this.wB + this.wL);
}

my odometry code

wheelSpeed[0] = Vx

wheelSpeed[1.] = Vy

dt = 0.02

  x =  wheelSpeed[0] * 0.02,
  y =  wheelSpeed[1] * 0.02,
  w =  angle.minus(prevAngel).getRadians()

I have tried these equations:

enter image description here

But for some reason I got local coordinates. i want to get global coordinates to implement driving by coordinates.

$\endgroup$
4
  • $\begingroup$ You must define what do you mean by global coordinate system. With these equations your coordinate system has it's reference point on the initial pose of your robot $\endgroup$
    – nionios
    Sep 29, 2022 at 15:21
  • $\begingroup$ for me, the global coordinate system does not depend on which direction the robot is turning, that is, if the gyroscope initially shows 0 and when moving forward, only one coordinate will increase, and when moving sideways, the other will increase, then when turning 90 degrees, the robot will visually go straight as in the case of 0 degrees along the same coordinate. $\endgroup$
    – gleb
    Sep 29, 2022 at 15:33
  • $\begingroup$ If your vx , vy ,Xold , and Yold are measured w.r.t global frame, you should get Xnew and Ynew in global frame. $\endgroup$
    – McLovin
    Sep 29, 2022 at 16:26
  • $\begingroup$ I want to make sure that when the robot rotates, the coordinate axes of the global system do not rotate to the local one $\endgroup$
    – gleb
    Oct 1, 2022 at 9:04

1 Answer 1

1
$\begingroup$

It sounds like all you want is dead reckoning for a holonomic robot.

Here is our setup:

holonomic robot coordinates

Here, $X_W$, $Y_W$, and $\theta_W$ are our World frame. $X_L$ and $Y_L$ are our Local frame.

Where ever you "turn on" the robot, this will be the world origin. You will have to keep track of the world pose. And you will keep adding to it with incremental updates.

You are correct that you need to get the displacement since the last time step:

$\Delta X_L = V_X * \Delta t$

$\Delta Y_L = V_Y * \Delta t$

$\Delta \theta = \omega * \Delta t$

And one more that will be handy:

$H_L = \sqrt{{\Delta X_L}^2 + {\Delta Y_L}^2}$

Let's zoom in on the incremental displacement;

incremental displacement

Now you just have a little trigonometry to get the incremental displacements in the world frame:

$\Delta {X_W} = H_L \sin(\frac{\pi}{2}-\tan(\frac{\Delta Y_L}{\Delta X_L})-\theta_W)$

$\Delta {Y_W} = H_L \cos(\frac{\pi}{2}-\tan(\frac{\Delta Y_L}{\Delta X_L})-\theta_W)$

Then do the update:

$X_{W_{new}} = X_{W_{old}} + \Delta X_W$

$Y_{W_{new}} = Y_{W_{old}} + \Delta Y_W$

$\theta_{W_{new}} = \theta_{W_{old}} + \Delta \theta$

And you are done!

But some things to keep in mind regarding dead reckoning like this:

  1. it is going to diverge from reality probably very quickly. there are things like wheel slip, errors in your measured wheel diameter, uneven terrain, etc.
  2. you should measure actual wheel rotations with encoders. rarely will your motors rotate at exactly the prescribed velocities.
  3. dead reckoning is OK for short periods of time, and is a reasonable local assumption, but you usually want to supplement it with some "global" sensing.
$\endgroup$
1
  • $\begingroup$ Thank you, also in the main question I forgot to write that encoders are also on the wheels $\endgroup$
    – gleb
    Oct 17, 2022 at 2:23

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.