# Drive unicycle kinematics equations through the geometry

The unicycle dynamics in discrete-time is given by:

$$\begin{array}{l} {x_{k + 1}} = {x_k} + \Delta {s_{k + 1}} \times \cos ({\theta _{k + 1}})\\ {x_{k + 1}} = {x_k} + \Delta {s_{k + 1}} \times \sin ({\theta _{k + 1}})\\ {\theta _{k + 1}} = {\theta _{k }} + \Delta {\theta _{k + 1}} \end{array}$$

However, some researchers have introduced an additional term to the argument of the trigonometric functions above; for example see: Autonomous Land Vehicles: Steps towards Service Robots By Karsten Berns, and Ewald Puttkamer

$$\begin{array}{l} {x_{k + 1}} = {x_k} + \Delta {s_{k + 1}} \times \cos ({\theta _{k }} + \frac{{\Delta {\theta _{k + 1}}}}{2})\\ {x_{k + 1}} = {x_k} + \Delta {s_{k + 1}} \times \sin ({\theta _{k }} + \frac{{\Delta {\theta _{k + 1}}}}{2})\\ {\theta _{k + 1}} = {\theta _{k }} + \Delta {\theta _{k + 1}} \end{array}$$

I was wondering which one is geometrically correct? In other words, how we can show that $$\cos ({\theta _k} + \frac{{\Delta {\theta _{k + 1}}}}{2}) = \cos ({\theta _{k + 1}})$$?

According to the following figure, we have

$$\Delta {x_{k + 1}} = \Delta {s_{k + 1}} \times \sin (\frac{{\Delta {\theta _{k + 1}}}}{2})$$

This may seem to be a strange result, since we said that

$$\Delta {x_{k + 1}} = \Delta {s_{k + 1}} \times \cos ({\theta _{k + 1}} + \frac{{\Delta {\theta _{k + 1}}}}{2})$$

## Edit

The solution of unicycle's system of ODEs with MIDPOINT METHOD:

## $$\begin{array}{l} {t_{k + 1}} = {t_k} + \frac{T}{2}\\ {\theta _{mid}} = {\theta _k} + \omega \frac{T}{2}\\ \begin{array}{*{20}{l}} {{x_{k + 1}} = {x_k} + v \times \cos ({\theta _{mid}}) \times T = {x_k} + v \times \cos ({\theta _k} + \omega \frac{T}{2}) \times T}\\ {{y_{k + 1}} = {y_k} + v \times \sin ({\theta _k} + \omega \frac{T}{2}) \times T}\\ {{\theta _{k + 1}} = {\theta _k} + \omega T} \end{array} \end{array}$$

Your system really operates in continuous time, described by differential equations. It's a convenient, even necessary, approximation to use discrete time and difference equations. The dynamics solution would result from integrating the differential equations; the difference equations approximately solve the problem by doing numeric integration.

As it happens, our predecessors over the previous two centuries have left us a rich literature about numeric integration methods, and approaches to difference equations. I spent some time refreshing my undergrad training by browsing Wikipedia for "Numerical methods for ordinary differential equations", "Euler method", "Midpoint method", "Runge-Kutta methods", etc. Recall that in the 18th and 19th centuries, computation was vastly more expensive than now, so there was a big payoff for seeking every last bit of computational efficiency. There is no One Method to Rule Them All.

Some variants evaluate integrand terms in the middle of the discrete interval. I haven't bothered to actually crank through the math, but I expect the geometric formulation you show (with the angle bisector being perpendicular to the chord) winds up equivalent to the midpoint method -- giving rise to $$\Delta \theta/2$$. That's only one of several methods and variants that may (or may not) be the very best compromise between compute time and accuracy. When our predecessors fiddled the equations to improve convergence (or something), that is equivalent to fiddling the details of the geometry.

Nowadays, computation is cheap enough that we don't need to struggle to find the very best method -- any of them would probably work fine for us. We just need one that is adequate, and if you find you need more accuracy, burn some compute cycles by using a smaller discrete step size.

At the risk of seeming to be an overly pragmatic roboticist, I would suggest "don't worry about it".

• However, regardless of the methods we can use for the discretization of ODEs, it's obvious from the figure that $\Delta {x_{k + 1}} = \Delta {s_{k + 1}} \times \sin (\frac{{\Delta {\theta _{k + 1}}}}{2})$, which is a strange result!? Since we must have $\Delta {x_{k + 1}} = \Delta {s_{k + 1}} \times \cos (\frac{{\Delta {\theta _{k + 1}}}}{2})$