Calcular la posición del robot de accionamiento diferencial

¿Cómo calcula o actualiza la posición de un robot de accionamiento diferencial con sensores incrementales?

Hay un sensor incremental conectado a cada una de las dos ruedas diferenciales. Ambos sensores determinan la distancia resp. su rueda ha rodado durante un tiempo conocido .Δ r i g h t Δ $\Delta left$$\Delta right$$\Delta t$

Primero, supongamos que el centro entre ambas ruedas marca la posición del robot. En este caso, se podría calcular la posición como:

$$x = \frac{x_{left}+x_{right}}{2} \\ y = \frac{y_{left}+y_{right}}{2}$$

"Derivando" esas ecuaciones bajo el supuesto de que ambas ruedas ruedan en línea recta (lo que debería ser aproximadamente correcto para distancias pequeñas) obtengo:

$$\frac{\Delta x}{\Delta t} = \frac{1}{2}\left( \frac{\Delta left}{\Delta t} + \frac{\Delta right}{\Delta t}\right)cos(\theta) \\ \frac{\Delta y}{\Delta t} = \frac{1}{2}\left( \frac{\Delta left}{\Delta t} + \frac{\Delta right}{\Delta t}\right)sin(\theta)$$

Donde es el ángulo de orientación del robot. Para el cambio de este ángulo encontré la ecuación$\theta$

$$\frac{\Delta \theta}{\Delta t} = \frac{1}{w} \left( \frac{\Delta left}{\Delta t} - \frac{\Delta right}{\Delta t}\right)$$

Donde $w$ es la distancia entre ambas ruedas.

Debido a que $\Delta x$ y $\Delta y$ dependen de $\theta$ , me pregunto si primero debo calcular la nueva $\theta$ agregando $\Delta \theta$ o si prefiero usar la \ theta "antigua" $\theta$. ¿Hay alguna razón para usar uno sobre el otro?

Entonces, supongamos que el centro entre ambas ruedas no marca la posición del robot. En cambio, quiero usar un punto que marque el centro geométrico del cuadro delimitador del robot. Entonces $x$ e $y$ cambian a:

$$x = \frac{x_{left}+x_{right}}{2} + l\, cos(\theta)\\ y = \frac{y_{left}+y_{right}}{2} + l\, sin(\theta)$$

"Derivando" el primero da:

$$\frac{\Delta x}{\Delta t} = \frac{1}{2}\left( \frac{\Delta left}{\Delta t} + \frac{\Delta right}{\Delta t}\right)cos(\theta) - l\,sin(\theta)\,\frac{\Delta \theta}{\Delta t}$$

Ahora hay una dependencia en . ¿Es esta una razón para usar el "nuevo" ?$\Delta \theta$$\theta$

¿Existe algún método mejor para hacer una actualización simulada de posición y orientación? ¿Puede estar usando números complejos (el mismo enfoque que con los cuaterniones en 3D?) O coordenadas homogéneas?

Para responder a su primera pregunta: si realmente desea encontrar las verdaderas ecuaciones cinemáticas para la transmisión diferencial, no comenzaría a aproximarme asumiendo que cada rueda se ha movido en línea recta. En cambio, encuentre el radio de giro, calcule el punto central del arco y luego calcule el siguiente punto del robot. El radio de giro sería infinito si el robot se mueve en línea recta, pero en el caso recto la matemática es simple.

Entonces, imagine que en cada paso de tiempo, o cada vez que calcule el cambio en los sensores incrementales, el robot viaja del punto A al punto B en un arco como este: Aquí hay un código de muestra con las matemáticas simplificadas:

// leftDelta and rightDelta = distance that the left and right wheel have moved along
//  the ground

if (fabs(leftDelta - rightDelta) < 1.0e-6) { // basically going straight
    new_x = x + leftDelta * cos(heading);
    new_y = y + rightDelta * sin(heading);
    new_heading = heading;
} else {
    float R = unitsAxisWidth * (leftDelta + rightDelta) / (2 * (rightDelta - leftDelta)),
          wd = (rightDelta - leftDelta) / unitsAxisWidth;

    new_x = x + R * sin(wd + heading) - R * sin(heading);
    new_y = y - R * cos(wd + heading) + R * cos(heading);
    new_heading = boundAngle(heading + wd);
}

Utilicé matemáticas similares en un simulador para demostrar diferentes formas de dirección: http://www.cs.utexas.edu/~rjnevels/RobotSimulator4/demos/SteeringDemo/

$\Delta\theta$$\theta$$\Delta{x}, \Delta{y}$

$\Delta\theta$$\Delta{x}, \Delta{y}$$\theta$

$\Delta{t}$$0$