P13 — Marco local, marco global y odometría con paso de tiempo

La pose de un robot diferencial en el plano es $\xi_G = (x, y, \theta)$. Sus velocidades en el marco local del robot son $\dot\xi_L = (\dot x_L, \dot y_L, \dot\theta_L)$, con $\dot y_L = 0$ porque las ruedas no se deslizan de lado. La relación entre los dos marcos es $\dot\xi_G = R(\theta)^{-1}\,\dot\xi_L$, con

$$R(\theta) = \begin{bmatrix} \cos\theta & \sin\theta & 0 \\ -\sin\theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{bmatrix}$$

La odometría con paso $\Delta t$ es

$$x_{k+1} = x_k + \dot x_L \cos\theta_k\,\Delta t \qquad y_{k+1} = y_k + \dot x_L \sin\theta_k\,\Delta t \qquad \theta_{k+1} = \theta_k + \dot\theta_L\,\Delta t$$

El robot parte de $(0,\ 0,\ 30°)$ con velocidades constantes $\dot x_L = 0.10$ m/s y $\dot\theta_L = 0.4$ rad/s.

1. Escriba $R(\theta)^{-1}$ y calcule $\dot\xi_G$ en el instante inicial. Interprete el resultado.
2. Con $\Delta t = 0.5$ s, calcule la pose después de 3 pasos (1.5 s). Dé $\theta$ en grados.
3. Con velocidades constantes el robot recorre un arco de circunferencia de radio $R = \dot x_L / \dot\theta_L$. La posición exacta después de un tiempo $T$ es $$x = R\,[\sin(\theta_0 + \dot\theta_L T) - \sin\theta_0] \qquad y = -R\,[\cos(\theta_0 + \dot\theta_L T) - \cos\theta_0]$$ Calcule $R$ y la posición exacta en $T = 1.5$ s. ¿De cuánto es el error de la odometría de la parte 2?
4. Repita el cálculo de la parte 2 usando $\theta_k + \dot\theta_L\,\Delta t/2$ en vez de $\theta_k$ dentro del seno y el coseno. ¿Cuánto cambia el error? Explique por qué.
5. Sin calcular, ¿qué pasaría con el error de la parte 2 si se usara $\Delta t = 0.05$ s?

Mostrar solución

1. $R(\theta)$ es una rotación, así que su inversa es su transpuesta:

   $$R(\theta)^{-1} = \begin{bmatrix} \cos\theta & -\sin\theta & 0 \\ \sin\theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{bmatrix}$$

   Con $\theta = 30°$: $\dot x = 0.10\cos 30° = 0.0866$ m/s, $\dot y = 0.10 \sin 30° = 0.050$ m/s, $\dot\theta = 0.4$ rad/s. El robot avanza a 0.10 m/s en la dirección en que mira, que es 30° respecto al eje $x$ global; el giro es igual en los dos marcos.

2. Cada paso avanza $0.10 \times 0.5 = 0.05$ m y gira $0.4 \times 0.5 = 0.2$ rad $= 11.46°$.

   Paso	$x$ (m)	$y$ (m)	$\theta$
   0	0	0	30.00°
   1	0.0433	0.0250	41.46°
   2	0.0808	0.0581	52.92°
   3	0.1109	0.0980	64.38°

3. $R = 0.10/0.4 = 0.25$ m. Ángulo final $\theta_0 + 0.6$ rad $= 64.38°$. $x = 0.25(\sin 64.38° - \sin 30°) = 0.1004$ m; $y = -0.25(\cos 64.38° - \cos 30°) = 0.1084$ m. Error: $\sqrt{(0.1109 - 0.1004)^2 + (0.0980 - 0.1084)^2} = 0.0148$ m, unos 1.5 cm en solo 15 cm de recorrido.

4. Con el ángulo medio, la posición después de 3 pasos es $(0.1006,\ 0.1086)$ m y el error baja a unos 0.25 mm, 60 veces menos. La fórmula de la parte 2 avanza todo el paso en la dirección inicial $\theta_k$, aunque durante el paso el robot va girando: siempre “se sale” hacia afuera de la curva. Usar el ángulo de la mitad del paso es usar la dirección promedio, que es casi exactamente la de la cuerda del arco.

5. El error disminuye mucho: con pasos 10 veces más cortos, el giro en cada paso es 10 veces menor y la aproximación de “avanzar en línea recta” se parece más al arco. Por eso la odometría se actualiza muchas veces por segundo.