Navegación punto a punto y giros

01 · Punto de partida

Un destino, no un movimiento

Hasta ahora ordenábamos movimientos: velocidades de ruedas (tema 26) o, en el brazo, ángulos. Un robot móvil útil recibe destinos: «andá a este punto», «girá hasta mirar hacia allá». Entre el destino y las velocidades de las ruedas hay un controlador de navegación que usa la pose estimada (tema 27) para decidir qué hacer en cada instante.

Las primitivas básicas son tres:

• Girar un ángulo en el lugar hasta apuntar a una orientación deseada.
• Avanzar una distancia en línea recta.
• Ir a una coordenada (x, y): combina las dos anteriores.

Todo es lazo cerrado sobre la pose estimada: el robot compara dónde cree estar con dónde quiere estar y corrige. Si la estimación es mala, la corrección es buena respecto de una pose equivocada.

02 · Las dos magnitudes

Distancia y rumbo al objetivo

Con la pose estimada (x, y, θ) y el objetivo (gx, gy), todo lo que el controlador necesita son dos números:

ρ es la distancia al objetivo. α es el ángulo entre hacia dónde mira el robot y hacia dónde está el objetivo, con signo: positivo si el objetivo queda a la izquierda, negativo si queda a la derecha. La normalización a (−π, π] (tema 20) es imprescindible: sin ella, un robot que mira a −175° con el objetivo a 175° vería un error de 350° y giraría casi una vuelta en el sentido largo, en vez de 10° en el corto.

Ejemplo: robot en (10, 20) con θ = 30° y objetivo en (40, 60). Resulta ρ = 50 cm; el rumbo al objetivo es atan2(40, 30) ≈ 53,1° y α ≈ 23,1°: debe girar a la izquierda unos 23°.

Todo el control de navegación consiste en reducir ρ a cero y, mientras tanto, mantener α cerca de cero. La estructura recuerda a la del brazo (cinemática inversa), pero con una diferencia: aquí no hay una fórmula directa de ángulos; hay que llegar mediante un proceso en el tiempo.

03 · Apuntar

Girar un ángulo

El giro en el lugar usa velocidades de rueda opuestas (tema 26): vL = −ω·b/2 y vR = +ω·b/2. La pregunta es qué ω ordenar en cada instante. Una regla proporcional, como en el tema 18, es:

Lejos del objetivo, el giro es rápido (hasta ωmáx); cerca, se desacelera de forma natural. Dos detalles importan:

• Velocidad mínima de giro. Una proporcional pura pediría velocidades tan bajas cerca del objetivo que el motor no arrancaría por la zona muerta (tema 13) y el robot se detendría antes de apuntar bien. Un ωmín (por ejemplo 0,4 rad/s) lo evita.
• Retardo del motor. El motor no frena al instante. Si la ganancia es alta, cuando la orden se apaga ya se pasó. El resultado es sobreimpulso, e incluso oscilación (laboratorio 2).

Con Kα = 3 1/s, ωmín = 0,4 rad/s y ωmáx = 2 rad/s, un giro de 90° con motor ideal tarda 1,2 s en entrar en ±2°. Con un retardo del motor de 100 ms, 1,1 s y 0,5° de sobreimpulso; con 300 ms, el sobreimpulso sube a 9°. Con ese retardo, un giro de 180° tarda unos 1,9 s.

Otra opción, de lazo abierto, es calcular las cuentas de encoder necesarias para el giro (Δs = θ·b/2 por rueda) y detenerse al alcanzarlas. Funciona sin controlador de ángulo, pero hereda directamente los errores del ancho de vía y no corrige sobreimpulsos.

04 · Sin desviarse

Avanzar una distancia

Avanzar d cm en línea recta parece trivial pero tiene dos partes: medir lo recorrido y mantener el rumbo.

Lo recorrido se obtiene de la odometría: la distancia entre la pose actual y la inicial del tramo (o su proyección sobre la dirección de avance). La velocidad se planifica con el perfil trapezoidal del tema 17: acelera, cruza y frena para llegar suavemente a d.

Para no desviarse, se corrige el rumbo con realimentación: se guarda el rumbo θref del inicio y se ordena ω = Kθ·normalizar(θref − θ). Si un motor es algo más lento, el rumbo cae un poco y la corrección lo compensa; es mucho mejor que dos PWM iguales y esperanza (tema 26). Esta corrección usa la orientación estimada, así que un giróscopo (tema 29) la mejora notablemente.

Ir a una coordenada puede verse como avanzar a un punto cambiante: en vez de mantener un rumbo fijo, se apunta continuamente al objetivo. De eso se ocupa la sección siguiente.

05 · Combinar giro y avance

Ir a una coordenada

Hay dos estrategias clásicas:

El algoritmo que usamos combina ambas con una máquina de estados de dos estados e histéresis:

• GIRAR: v = 0 y ω = Kα·α (con mínimo y máximo). Pasa a AVANZAR cuando |α| < 3°.
• AVANZAR: v = sat(Kρ·ρ, vmín, vmáx)·cos α (sin marcha atrás) y ω = Kα·α. Vuelve a GIRAR si |α| > 45°.

Los dos umbrales distintos (3° para salir de GIRAR y 45° para volver) son una histéresis: si fueran iguales, el robot oscilaría entre estados cada vez que α cruzara el umbral. El factor cos α hace que el avance se reduzca cuando el robot no apunta bien: con α = 60° avanza a la mitad, y con α ≥ 90° directamente no avanza.

Finalmente se aplican las limitaciones del tema 26: una rampa de aceleración sobre v y el escalado de v y ω si alguna rueda superaría su velocidad máxima, que conserva la curva. La velocidad mínima vmín evita el problema opuesto: que Kρ·ρ sea tan pequeño cerca del objetivo que el robot no llegue nunca (aproximación asintótica).

06 · Terminar bien

Cuándo se llegó

La condición de llegada es una tolerancia: ρ < ε. No se puede exigir ρ = 0: la pose es una estimación con ruido y resolución finita, y el robot tiene un paso mínimo por ciclo. Una tolerancia razonable es del orden de la incertidumbre de la propia estimación y de unas pocas veces la resolución de avance por ciclo.

Tres cuidados:

• Que no pase de largo. Con velocidad mínima vmín y ciclo Δt, el robot avanza vmín·Δt por ciclo; con 4 cm/s y 20 ms son 0,08 cm, muy por debajo de una tolerancia de 1 cm, así que no se salta la zona de llegada.
• El ángulo se indetermina cerca del objetivo. Cuando ρ es muy chico, α = atan2(dy, dx) − θ cambia de forma violenta con el ruido más mínimo. Por eso se declara la llegada antes de que eso ocurra y no se recalcula α dentro de la tolerancia.
• Orientación final. Llegar a un punto no fija la orientación. Si la tarea la exige, se añade un giro final en el lugar hacia el ángulo deseado (primitiva 1).

Con tolerancia de 1 cm, el robot de los ejemplos se detiene a 0,95 cm del objetivo según su propia estimación (llega desde un lado y para al entrar en la zona), no exactamente encima. Se puede bajar la tolerancia, pero entonces el límite lo fija la odometría, no el controlador.

07 · Una ruta

Secuencias de puntos sin bloquear

Una ruta es una lista de puntos. El navegador mantiene un índice y la máquina de estados, y en cada vuelta del programa avanza un paso (cada 20 ms). Al llegar a un punto, pasa al siguiente: vuelve a GIRAR, apunta al nuevo objetivo y avanza. No hay delay() ni bucles que esperen: entre paso y paso el programa lee sensores, comandos y señales de parada (tema 19).

Con un cuadrado de 60 cm, la secuencia es: girar 90° y avanzar 60 cm, cuatro veces. Cada tramo de los últimos tres dura unos 5,3 s con los parámetros de ejemplo (alrededor de 1,2 s de giro y 4 s de avance); el primero, sin giro inicial, 4,1 s.

Es útil separar tres piezas: la lista de destinos (datos), el navegador (el controlador de este tema) y la interfaz con el robot (pose estimada en, velocidades de rueda fuera). Con esa separación, el mismo navegador sirve con un robot simulado, como en nuestro sketch, o con el real.

Cuando haya obstáculos o líneas, otros comportamientos competirán con el navegador por el control de las ruedas. Cómo arbitrar esos conflictos es el tema de la arquitectura de subsunción (tema 33) y de las máquinas de estados jerárquicas (tema 38).

08 · La advertencia más importante

Lo que la odometría no sabe

El navegador lleva la pose estimada hasta el objetivo. Si la estimación tiene errores sistemáticos (tema 27), el robot real llega a otro lado, y lo hace «con total confianza». El controlador no tiene forma de enterarse: la realimentación se cierra sobre la estimación.

Con una diferencia de diámetros del 2 %, ir a un punto a 1 m en línea recta termina unos 6,7 cm al costado del objetivo real (ε·d²/(2b)), y esos errores se suman tramo a tramo si cada tramo parte de la pose estimada. En el cuadrado de 60 cm con 1 % de diferencia de diámetros y 2 % de error de ancho de vía, la distancia entre la estimación y la realidad llega a 1,5; 3,4; 5,1 y 5,7 cm en los cuatro puntos, aunque el robot informe siempre estar a menos de 1 cm.

Lo que puede mejorar la llegada real:

• Calibrar el modelo (tema 27): es lo más rentable y gratuito.
• Corregir el rumbo con un giróscopo o una brújula (tema 29), que ataca el error que más se amplifica.
• Usar referencias externas: ir hasta una pared o una marca y reiniciar la pose; los sensores de distancia (tema 6) o de línea (tema 30) sirven para el ajuste final.
• Acortar las rutas y reiniciar la pose con frecuencia.
• Aceptar la tolerancia real: si la tarea exige ±5 mm y la odometría da ±3 cm, el problema no es de control sino de sensores.

09 · Código para la placa

Navegación en Arduino

El sketch contiene el navegador completo y un robot simulado dentro del mismo programa, para probar la lógica sin hardware. Recorre un cuadrado de 60 cm y, al llegar a cada punto, informa la pose estimada, la real y el error real. Las constantes ERR_DIAMETROS y ERR_ANCHO permiten inyectar errores de modelo al robot simulado.

// Navegación punto a punto: el navegador se prueba contra un robot SIMULADO dentro del mismo sketch.
// Para usar un robot real, reemplazá leerPoseEstimada() por la odometría del tema 27 y
// ordenarRuedas() por las referencias de velocidad de los lazos del tema 18.

const float B = 15.0;                                  // distancia entre ruedas (cm)
const float V_RUEDA_MAX = 30.0;                        // cm/s por rueda
const float V_MAX = 20.0, V_MIN = 4.0;                 // cm/s de avance
const float W_MAX = 2.0, W_MIN = 0.4;                  // rad/s de giro
const float A_MAX = 40.0;                              // cm/s² de la orden de avance
const float K_RHO = 1.5, K_ALPHA = 3.0;                // ganancias (1/s)
const float TOL_POS = 1.0;                             // cm
const float ALPHA_GIRO = 45.0 * DEG_TO_RAD, ALPHA_OK = 3.0 * DEG_TO_RAD;
const float DT = 0.02;                                 // s

// Errores de modelo del robot simulado (0 = robot ideal).
const float ERR_DIAMETROS = 0.0, ERR_ANCHO = 0.0;

const float PUNTOS[][2] = {{60, 0}, {60, 60}, {0, 60}, {0, 0}};
const byte N_PUNTOS = sizeof(PUNTOS) / sizeof(PUNTOS[0]);

struct Pose { float x, y, th; };
enum Estado { GIRAR, AVANZAR, TERMINADO };

Pose real = {0, 0, 0}, estimada = {0, 0, 0};
Estado estado = GIRAR;
byte objetivo = 0;
float vPrevia = 0, tInicio = 0, tiempo = 0;

float normalizar(float a) {
  while (a > PI) a -= TWO_PI;
  while (a <= -PI) a += TWO_PI;
  return a;
}

Pose integrar(Pose p, float vl, float vr, float b) {
  const float ds = (vl + vr) / 2.0 * DT, dth = (vr - vl) / b * DT;
  if (fabs(dth) < 1e-9) return { p.x + ds * cos(p.th), p.y + ds * sin(p.th), p.th };
  const float r = ds / dth;
  return { p.x + r * (sin(p.th + dth) - sin(p.th)), p.y - r * (cos(p.th + dth) - cos(p.th)), p.th + dth };
}

// ---- Interfaz con el robot (reemplazar para usar hardware real) ----
Pose leerPoseEstimada() { return estimada; }

void ordenarRuedas(float vl, float vr) {
  estimada = integrar(estimada, vl, vr, B);                                   // lo que "cree" la odometría
  real = integrar(real, vl * (1 - ERR_DIAMETROS / 2), vr * (1 + ERR_DIAMETROS / 2), B * (1 + ERR_ANCHO));
}

// ---- Navegador ----
void paso() {
  const Pose p = leerPoseEstimada();
  const float gx = PUNTOS[objetivo][0], gy = PUNTOS[objetivo][1];
  const float dx = gx - p.x, dy = gy - p.y;
  const float rho = sqrt(dx * dx + dy * dy);
  const float alpha = normalizar(atan2(dy, dx) - p.th);

  if (rho < TOL_POS) {
    Serial.print("Punto "); Serial.print(objetivo + 1);
    Serial.print(": t="); Serial.print(tiempo - tInicio, 2);
    Serial.print(" s; estimada=("); Serial.print(p.x, 2); Serial.print(", "); Serial.print(p.y, 2);
    Serial.print("); real=("); Serial.print(real.x, 2); Serial.print(", "); Serial.print(real.y, 2);
    Serial.print("); error real="); Serial.print(sqrt((real.x - gx) * (real.x - gx) + (real.y - gy) * (real.y - gy)), 2);
    Serial.println(" cm");
    if (++objetivo >= N_PUNTOS) { estado = TERMINADO; ordenarRuedas(0, 0); return; }
    estado = GIRAR; tInicio = tiempo;
    return;
  }

  float vOrden = 0, w;
  if (estado == GIRAR) {
    w = copysign(constrain(K_ALPHA * fabs(alpha), W_MIN, W_MAX), alpha);
    if (fabs(alpha) < ALPHA_OK) estado = AVANZAR;
  } else {
    vOrden = constrain(K_RHO * rho, V_MIN, V_MAX) * max(0.0, cos(alpha));
    w = constrain(K_ALPHA * alpha, -W_MAX, W_MAX);
    if (fabs(alpha) > ALPHA_GIRO) estado = GIRAR;
  }
  float v = vPrevia + constrain(vOrden - vPrevia, -A_MAX * DT, A_MAX * DT);   // rampa de avance
  const float mayor = max(fabs(v - w * B / 2), fabs(v + w * B / 2));
  const float k = (mayor > V_RUEDA_MAX) ? V_RUEDA_MAX / mayor : 1.0;            // escalar si satura
  v *= k; w *= k;
  vPrevia = v;
  ordenarRuedas(v - w * B / 2, v + w * B / 2);
  tiempo += DT;
}

void setup() {
  Serial.begin(115200);
  Serial.println("Simulacion de navegacion por 4 puntos (cuadrado de 60 cm)");
  while (estado != TERMINADO) paso();
  Serial.println("Fin");
}

void loop() {}

Con los errores en cero, la salida es aproximadamente la de Python: el punto 1 en t=4.08 s con la estimada en (59.05, 0.00) y error real 0.95 cm; los puntos 2 a 4, t=5.26 s cada uno y errores de 0,94 a 0,95 cm. Con ERR_DIAMETROS = 0.01 y ERR_ANCHO = 0.02, la estimada no cambia y el error real crece: unos 1,5; 3,4; 5,1 y 5,7 cm.

Para pasar a un robot real, reemplazá dos funciones: leerPoseEstimada(), que devolvería la pose de la odometría del tema 27 (leída con las interrupciones), y ordenarRuedas(), que pasaría las dos velocidades a los lazos de control con encoder del tema 18. Probalo primero con las ruedas en el aire y con una velocidad máxima baja, y agregá una parada por comando o por sensor antes de soltar el robot (temas 4 y 19).

Como el sketch simulado resuelve todo en setup(), no es un modelo de estructura para el robot real, donde paso() se llamaría cada 20 ms desde loop(), sin bloquear.

10 · Exploración interactiva

Laboratorios: ir a un punto y girar

Con el objetivo (60; 40) y el robot mirando al +x, el modo híbrido tarda 5,4 s y recorre 71 cm; el continuo, 4,7 s. Con el objetivo a (0; 40), de costado: 4,3 s contra 3,3 s. Con rumbo inicial de 180° y objetivo (60; 40), el robot gira primero unos 146°. Poné diferencia de diámetros 1 % y ancho de vía 2 %: la estimación llega a 1 cm, pero el robot real queda a 1,9 cm. Con 2 % de diámetros y el objetivo a (100; 0), el error real es de 6,7 cm.

Con 90°, K_α = 3 y un retardo de 100 ms, entra en la tolerancia a 1,1 s con 0,5° de sobreimpulso. Con retardo cero, 1,2 s y sin sobreimpulso. Con τ = 200 ms y K_α = 8 el sobreimpulso sube a 8°; con τ = 300 ms y K_α = 3, a 9°. Con K_α = 1 es más lento (2,2 s) pero sin pasarse. Una ganancia alta y un motor lento son una combinación mala; en el tema 41 se verá por qué.

11 · Código ejecutable

Navegar en Python

El mismo navegador, en Python, recorre el cuadrado con un robot simulado, con y sin errores de modelo. Es el equivalente del sketch y sirve para ajustar las ganancias y las tolerancias antes de llevarlas al microcontrolador.

import math

B = 15.0                                   # distancia entre ruedas (cm)
V_RUEDA_MAX = 30.0                         # cm/s por rueda
V_MAX, V_MIN, W_MAX, W_MIN = 20.0, 4.0, 2.0, 0.4
A_MAX = 40.0                               # cm/s2
K_RHO, K_ALPHA = 1.5, 3.0                  # ganancias de distancia y de rumbo
TOL_POS, ALPHA_GIRO, ALPHA_OK = 1.0, math.radians(45), math.radians(3)
DT = 0.02

def normalizar(a):
    while a > math.pi: a -= 2 * math.pi
    while a <= -math.pi: a += 2 * math.pi
    return a

def avanzar(p, v, w, b, f_izq=1.0, f_der=1.0):
    """Integra un intervalo por arcos con velocidades de rueda (con factores de error)."""
    vl, vr = (v - w * b / 2) * f_izq, (v + w * b / 2) * f_der
    ds, dth = (vl + vr) / 2 * DT, (vr - vl) / b * DT
    x, y, th = p
    if abs(dth) < 1e-9:
        return (x + ds * math.cos(th), y + ds * math.sin(th), th)
    r = ds / dth
    return (x + r * (math.sin(th + dth) - math.sin(th)), y - r * (math.cos(th + dth) - math.cos(th)), th + dth)

def navegar(puntos, e_d=0.0, e_b=0.0, rumbo_inicial=0.0, verbose=True):
    real = est = (0.0, 0.0, rumbo_inicial)
    f_izq, f_der, b_real = 1 - e_d / 2, 1 + e_d / 2, B * (1 + e_b)
    v_prev, estado, t = 0.0, "GIRAR", 0.0
    for numero, (gx, gy) in enumerate(puntos, 1):
        estado = "GIRAR"
        inicio = t
        while True:
            dx, dy = gx - est[0], gy - est[1]
            rho = math.hypot(dx, dy)
            alpha = normalizar(math.atan2(dy, dx) - est[2])
            if rho < TOL_POS:
                break
            if estado == "GIRAR":
                v_cmd = 0.0
                w = math.copysign(min(W_MAX, max(W_MIN, K_ALPHA * abs(alpha))), alpha)
                if abs(alpha) < ALPHA_OK:
                    estado = "AVANZAR"
            else:
                v_cmd = max(V_MIN, min(V_MAX, K_RHO * rho)) * max(0.0, math.cos(alpha))
                w = max(-W_MAX, min(W_MAX, K_ALPHA * alpha))
                if abs(alpha) > ALPHA_GIRO:
                    estado = "GIRAR"
            v = v_prev + max(-A_MAX * DT, min(A_MAX * DT, v_cmd - v_prev))
            vl, vr = v - w * B / 2, v + w * B / 2
            k = min(1.0, V_RUEDA_MAX / max(abs(vl), abs(vr), 1e-9))
            v, w = v * k, w * k
            v_prev = v
            est = avanzar(est, v, w, B)
            real = avanzar(real, v, w, b_real, f_izq, f_der) if (e_d or e_b) else est
            t += DT
        if verbose:
            e_est = math.hypot(est[0] - gx, est[1] - gy)
            e_real = math.hypot(real[0] - gx, real[1] - gy)
            print(f"Punto {numero} ({gx:g}, {gy:g}): t={t - inicio:5.2f} s; estimada=({est[0]:6.2f}, {est[1]:6.2f}), "
                  f"orient={math.degrees(normalizar(est[2])):7.1f}; real=({real[0]:6.2f}, {real[1]:6.2f}); error real={e_real:5.2f} cm")
    return t, est, real

cuadrado = [(60, 0), (60, 60), (0, 60), (0, 0)]
print("Sin errores de modelo:")
navegar(cuadrado)
print("Con diferencia de diametros 1 % y ancho de via +2 %:")
navegar(cuadrado, e_d=0.01, e_b=0.02)

Sin errores de modelo, el robot llega al punto 1 en 4,08 s y a los demás en 5,26 s, a 0,94–0,95 cm del objetivo (la tolerancia de 1 cm lo detiene al entrar en la zona), con real y estimada idénticas. Con diámetros +1 % y ancho de vía +2 %, la estimación no cambia pero el error real crece a 1,49; 3,42; 5,10 y 5,70 cm: al final, el robot "cree" haber vuelto al origen y está a 5,7 cm de él.

Probá cambiar K_ALPHA a 8 y observá los tiempos; TOL_POS a 0,2 y comprobá que sigue llegando; V_MIN a 0,5 y mirá cuánto cuesta el último centímetro. Agregá un giro final hacia un ángulo deseado como cuarta primitiva.

12 · Método y diagnóstico

Pruebas y diagnóstico

1. Probá primero en simulación (Python o el sketch simulado): tiempos, estados y llegada con errores en cero.
2. Con el robot real y las ruedas en el aire, verificá que cada estado ordena las ruedas en el sentido correcto.
3. Probá un giro de 90° y de 180° en el lugar, y medí el ángulo real con una marca.
4. Probá un avance recto de 1 m y medí la distancia y el desvío lateral reales.
5. Probá ir a un punto a 60 cm de lado y medí el error de llegada real, no el informado.
6. Repetí el cuadrado y registrá el error en cada vértice; compará con la predicción de la simulación con tus errores de modelo.
7. Ajustá tolerancias y velocidades mínimas con el robot real, que tiene zona muerta y retardo.

13 · Práctica guiada

Predecí antes de comprobar

1. Distancia y ángulo

El robot está en (10; 20) con θ = 30°. ¿Cuánto valen ρ y α para el objetivo (40; 60)?

2. Giro corto

El robot mira a −175° y el objetivo está en la dirección 175°. ¿Qué α se obtiene y hacia dónde gira?

3. Velocidades de giro

Con ω = 1,5 rad/s y b = 15 cm, ¿qué velocidades tienen las ruedas en un giro en el lugar y cuánto tarda un giro de 90° a ritmo constante?

4. Control continuo

Con ρ = 30 cm, α = 60°, K_ρ = 1,5, v_máx = 20 cm/s, K_α = 3 y ω_máx = 2 rad/s, ¿cuánto valen v, ω y las velocidades de rueda?

5. Error de llegada real

Con 2 % de diferencia de diámetros y b = 15 cm, ¿cuánto se desvía lateralmente el robot real al ir a un punto a 1 m en línea recta?

6. Histéresis

¿Por qué se sale de GIRAR con |α| < 3° pero se vuelve con |α| > 45° y no con un único umbral?

7. Velocidad mínima

Con v_mín = 4 cm/s y ciclo de 20 ms, ¿cuánto avanza por ciclo? ¿Podría saltarse una zona de llegada de 1 cm?

14 · Síntesis

Ideas para recordar

¿Listo para el tema 29?

Antes de avanzar, ejecutá en simulación un cuadrado con tus errores de modelo estimados y anotá cuánto se aleja el robot real de cada vértice. Pensá qué sensor reduciría más ese error y por qué.

En el próximo tema estudiamos la orientación con IMU y brújula: corregir la dirección y reducir la deriva del giroscopio.