Handling of odometry data

[SeaTechRC]

Hello,

Upon further inspection I suspect there might be an incorrect implementation of calculating a new prediction in the odometer from the linear and angular velocity given by wheels. (At least in the case of differential wheeled robots.)

To explain: Currently the new pose calculated from the linear and angular velocity is calculated such that: $$x{t + \Delta t} = x{t} + \cos(\phi{t}) \cdot \Delta t$$ $$y{t + \Delta t} = y{t} + \sin(\phi{t}) \cdot \Delta t$$ $$\phi_{t + \Delta t} = \phi + \omega \cdot \Delta t$$

When looking at the kinematics of a differential wheeled robot we find that this equates to the derivative of the robots x, y and yaw times the time elapsed since the last update. This is okay as an approximation, however, it diverges especially quick if updates are infrequent.

The correct method would be to calculate the integral of the derivative. For this, it must be taken into account that the yaw (which is $$\phi$$) changes during driving. If we substitute the yaw with its respective function, we get that the integral for the relative movement is (from $$0$$ to $$\Delta t$$) is: $\Delta x = \int\limits_0^{\Delta t} V \cos(\phi_0 + \omega \cdot t) dt$

$\Delta y = \int\limits_0^{\Delta t} V \sin(\phi_0 + \omega \cdot t) dt$

$\Delta \phi = \int\limits_0^{\Delta t} \omega$

$x{t + \Delta t} = x{t} + \Delta x$ $y{t + \Delta t} = y{t} + \Delta y$ $\phi_{t + \Delta t} = \phi + \Delta \phi$

Calculating the integrals we get:

$\Delta x = \frac{V \cdot \sin(\phi_0 + \omega \cdot \Delta t)}{\omega}$

$\Delta y = -\frac{V \cdot \cos(\phi_0 + \omega \cdot \Delta t)}{\omega}$

$\Delta \phi = \omega \cdot \Delta t$

A good way to visualize it is, that the old method, at any given point, would simply move the robot in a straight line for $\Delta t$ seconds. Which does not match the curve the robot is actually driving (a circular path).

[SeaTechRC]

Correction to the integral: I made an error and didn't include the bounds.

The definite integrals would be: $\Delta x = V \cdot \frac{\sin(\omega \cdot \Delta t + \phi_0) - \sin(\phi_0)}{\omega}$

$\Delta y = -V \cdot \frac{\cos(\omega \cdot \Delta t + \phi_0) - \cos(\phi_0)}{\omega}$

Do note, that there is a seperate case where $\omega$ is 0 and therefore we simply drive in a straight line.

Adding acceleration and deceleration would also change the form of the integral, as the $V$ would be replaced with a function $V(t)$. This might be less important, but not taking acceleration into account is another small source of error.

[falkoschindler]

Thanks for bringing this up, @SeaTechRC! The current implementation is indeed rather simplistic and assumes frequent odometry measurements and a relatively slow and smooth motion. To be honest, I never really thought about using more complex formulas and expected them to be even more complicated than the ones you're proposing.

Some thoughts:

1. Assuming a circular motion is also just an approximation. Ultimately we don't know the true motion and need to make assumptions. But assuming to drive in circles is probably better than computing piecewise linear paths.
2. We usually work with robots not faster than 1 m/s and odometry measurements at least 20 times per second, resulting in 5cm line segments (at full speed!). When turning, the robots are usually much slower. So I doubt it makes a significant difference whether to assume a straight 5cm segment or an arc.
3. If your measurement intervals are longer, it's still vague to assume the robot is driving in a perfect circle. It could do all kinds of crazy things while the odometer isn't looking.
4. And if you're concerned about maximum precision, I doubt that wheel odometry will ever be that accurate, especially when turning.
5. If I'm correct you're referring to the implementation in Pose.__add__ where a PoseStep is added to a Pose. This piece of code is rather unrelated to the application of odometers and differential wheeled robots. Changing it would introduce a nasty breaking change, which wouldn't make much sense within the rosys.geometry module. So we would better add a new implementation within Odometer.handle_velocities.
6. Regarding the case where $\omega$ is 0: Even though we can easily handle this case with an if condition, I'm a bit worried about numerical instabilities when $\omega$ is close to 0. Actually I'm wondering how $\omega$ in the denominator works at all: For tiny turn rates the coordinate increments become huge. 🤔

Long story short, I don't think changing the formulas is strictly necessary, but I would probably accept a pull request. Apart from that I'd love to find a second source for these formulas - just to be sure. It's rather easy to introduce a subtle bug here that's hard to notice but would make things even more incorrect.

[SeaTechRC]

To comment on some of your thoughts:

1. I agree
2. We are working with the FieldFriend. It updates roughly 50 times a second (according to serial debug logs). I calculated some paths simulating a 50hz update rate and the difference is indeed fairly small (maybe 5mm on after driving ~1.5m along a circle with 1 meter radius)
3. I mostly agree. In reality the robot (at least the FieldFriend) moves very little without control inputs to the wheels.
4. I agree. We have observed (although that is some time ago we did testing) a rather large difference between the turning radius of the robot using the odometry data and GNSS data, which lead us to believe that there might a computational error with the odometry. This could have other reasons, but the difference was so large that a computational error seemed the most likely.
5. I suppose the linear and angular velocity is applicable to all kinematics models, but circular motion would be aswell.
6. That is true. The upper term of the fraction also becomes fairly small as the difference ($$cos(\omega\cdot\Delta t - \phi_0) - cos(\phi_0)$$ and the same for the sine version) becomes rather small aswell, but some limit above 0 should be set.

Thank you for the reminder about the refresh rate. This clears it up for me 👍 I suppose this might be useful as a heads up, in case there are any special cases (low refresh rate, tight and fast turning) either in future robots developed by your company or other people using rosys for their robots. Feel free to close the issue if you think it's not applicable.