Pure Pursuit: seguimiento de trayectorias con un único parámetro

El peldaño que nadie enseña

Un planificador te da una trayectoria. A* te da una lista de celdas de la rejilla; un relevamiento te da una lista de waypoints; una misión te da una línea en un mapa. Ninguno le dice a las ruedas qué hacer.

Ese hueco es donde vive el pursuit puro, y es un párrafo de geometría:

Encuentra el punto de la trayectoria a una distancia fija por delante de ti. Conduce el arco circular que pasa por él. Repite.

No hay señal de error, ni término integral, ni tabla de ganancias. Hay un número —la distancia de anticipación— y lo decide todo.

La geometría

Pon el punto objetivo en el marco propio del robot: x hacia delante, y a la izquierda, a distancia L_d. Un arco circular desde la posición actual del robot pasando por ese punto tiene curvatura

κ = 2y / L_d²

que merece leerse dos veces, porque todo se desprende de ella. El desplazamiento lateral y está en el numerador, así que el arco se dobla más cuanto más fuera de eje esté el objetivo. La anticipación está al cuadrado en el denominador, así que doblarla reduce la curvatura a la cuarta parte.

Expresado como radio, y como ángulo de dirección para un vehículo con entrejejes b:

R = L_d² / 2y            δ = atan(2 b sin α / L_d)

Para una tracción diferencial no hay ángulo de dirección — ordenas directamente una tasa de rotación, ω = v κ. La misma curvatura, otro actuador.

La única perilla, y por qué se pelea consigo misma

Anticipación corta. El punto objetivo está cerca, así que un pequeño desplazamiento lateral produce una curvatura grande. El robot sigue la trayectoria ceñido — y reacciona con violencia a cada bamboleo, incluidos los que acaba de causar su propia dirección.

Anticipación larga. El punto objetivo está lejos, la curvatura es suave, la dirección es fluida — y en una esquina el punto objetivo ya ha redondeado la curva mientras el robot no, así que el robot corta la esquina.

Esos dos tiran en direcciones opuestas y no hay nada en el algoritmo que los equilibre. Tú eliges un número.

La trampa en toda deducción de libro de texto

Escribe el pursuit puro en papel, con un robot que gira al instante, y más corto siempre es mejor. El error cae monótonamente a medida que la anticipación se reduce. No hay óptimo, y la forma de U de arriba no existe.

Los robots reales tienen retardo. La dirección tarda en alcanzar el ángulo ordenado; la pose sobre la que actúas tiene unas decenas de milisegundos de antigüedad cuando las ruedas responden. El simulador modela una constante de tiempo de dirección de 0,25 s y 0,15 s de retardo de detección y cómputo — 0,4 s de retardo total, que es perfectamente normal en un rover de hobby.

Ese retardo es la razón completa de que exista el óptimo. Una anticipación corta ordena una corrección, la corrección llega tarde, el robot ya ha pasado del error, y la siguiente orden corrige demasiado hacia el otro lado. Un controlador más rápido que su propio actuador oscila. Es el mismo hecho que rige la ganancia derivativa de un PID y el mismo que está detrás de la distancia de frenado: la latencia no es un detalle, es lo que fija el ajuste.

Entonces, ¿cómo lo eliges?

Los óptimos medidos, y lo que son en unidades que se transfieren:

Dos lecturas honestas de esa tabla.

La anticipación crece con la velocidad, pero menos que de forma proporcional. El consejo habitual «hazlo un número fijo de segundos de recorrido» se acerca pero no acierta — el tiempo cae de 0,90 s a 0,47 s en todo ese rango.

Se sitúa entre una y dos veces la distancia que recorres durante tu propia latencia de control. Esa es la regla útil, porque es medible: encuentra tu latencia, multiplícala por tu velocidad y empieza ahí. En este robot v × latencia va de 0,4 m a 1,2 m y los óptimos van de 0,9 m a 1,4 m.

En la práctica, entrégalo como una pequeña tabla lineal y recórtala:

float lookahead = constrain(K_LOOK * speed + L_MIN, L_MIN, L_MAX);

L_MIN importa más que K_LOOK. En parado el planificador quiere anticipación cero, que es una división por cero con disfraz.

Cómo implementarlo

// Pure pursuit over a waypoint list. Returns the curvature to command.
float pursue(float x, float y, float heading, float lookahead) {
  // 1. Where am I on the path? Search only near the arc length I last reached —
  //    a global search latches onto whichever segment happens to be nearest, and a
  //    path that doubles back will teleport your progress to a corner you passed
  //    twenty seconds ago.
  float s = projectOntoPath(x, y, lastS);
  lastS = max(lastS, s);

  // 2. The goal point, one lookahead further along.
  Point goal = pointAtArcLength(lastS + lookahead);

  // 3. Into the robot frame, then straight into the curvature formula.
  float dx = goal.x - x, dy = goal.y - y;
  float lateral = -sin(heading) * dx + cos(heading) * dy;
  float ld = max(0.3f, hypotf(dx, dy));
  return 2.0f * lateral / (ld * ld);
}

Tres cosas en doce líneas valen más que el resto:

• lastS es estado, y solo avanza. Sin él, cualquier trayectoria que se cruce consigo misma dejaría varado al controlador.
• Recorta ld. A medida que el punto objetivo se acerca, la curvatura tiende a infinito.
• El punto objetivo está en la trayectoria, no es el waypoint más cercano. Apuntar a los waypoints hace que el robot zigzaguee entre ellos; apuntar a un punto del segmento es lo que hace suave el movimiento.

Lo que hace y lo que no hace

No se queda quieto y gira para encararse a la trayectoria. El pursuit puro no tiene noción de estar quieto — la curvatura es una forma, y una forma necesita movimiento hacia delante. Arranca el robot aproximadamente alineado, o conduce antes un pequeño arco de apertura.

Tiene un radio de giro mínimo y la trayectoria puede no respetarlo. Si una esquina es más cerrada que b / tan(δ_max), el robot no puede seguirla por más que ajustes. Suaviza la trayectoria o frena para la esquina — que es justo para lo que sirve un perfil de movimiento.

Siempre corta las esquinas, por diseño. En la pasada medida a 2 m/s conducía un 0,8 % menos que la trayectoria ordenada. Si eso es inaceptable, quieres otro controlador — Stanley dirige sobre el error lateral y se abre en curva en vez de cortar.

No puede arreglar una mala estimación de posición. Todo lo de arriba asume que el robot sabe dónde está. Dale un fix de GPS a 1 Hz y el error RMS pasa de 0,09 m a 1,25 m — mira navegación por waypoints con GPS.

Cuando va mal

Conduce los dos controladores por el mismo recorrido en el simulador de seguimiento de trayectoria — arrastra la anticipación y toda la pasada se vuelve a representar. Después construye el rover con waypoints GPS, donde la estimación de posición pasa a ser la parte difícil.