Simulador de seguimiento de trayectorias: Pure Pursuit

Controles

Controlador permite alternar entre Pure Pursuit y Stanley. Distancia de mira es el parámetro central de Pure Pursuit (deshabilitado en Stanley, que no lo utiliza).

Velocidad y distancia de mira interactúan fuertemente: analiza la respuesta óptima a 1 m/s y compárala posteriormente a 3 m/s.

Fuente de posición sustituye la odometría ideal por una señal GPS de 1 Hz con ruido métrico retenida entre periodos. El error siempre se puntúa frente a la trayectoria física real.

Arrastra cualquier punto de paso (waypoint) o pulsa 1–8 para seleccionarlo y reubicarlo mediante las flechas del teclado.

Teoría

Ambos esquemas resuelven el cálculo del ángulo de dirección a partir de la pose actual y la trayectoria deseada, pero desde paradigmas distintos:

Pure Pursuit se basa en pura geometría. Determina un punto objetivo sobre el camino a una distancia de mira L_d por delante y calcula el arco circular tangente que conecta el eje trasero con dicho punto:

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

donde y representa la desviación lateral del punto objetivo en el marco del robot y b es la batalla entre ejes. Posee un único parámetro con dimensión de longitud, facilitando su extrapolación entre distintos vehículos.

Stanley se basa en teoría de control. Toma como referencia el eje delantero y suma dos contribuciones:

δ = θ_e + atan(k · e / v)

el error de orientación relativo a la tangente de la trayectoria más el arco tangente del error transversal escalado inversamente con la velocidad. Dividir por la velocidad permite conservar estabilidad en un amplio rango dinámico. La ganancia k es adimensional y específica de cada vehículo.

Algoritmo

En cada periodo de control:

1. Proyectar la posición del robot sobre el camino, restringiendo la búsqueda en una ventana próxima a la longitud de arco alcanzada en la iteración previa.
2. Resolver empates en esquinas asignando la proyección al segmento saliente, evitando errores ficticios nulos mientras el chasis continúa recto.
3. Calcular el ángulo de dirección según la ley seleccionada y acotarlo a los límites mecánicos del tren delantero.
4. Aplicar los retardos dinámicos e integrar el modelo cinemático de bicicleta.
5. Calcular el error respecto a la verdad fundamental física.

Código fuente

Implementación completa de Pure Pursuit lista para microcontrolador:

float lastS = 0;   // arc length last reached — state, and it only moves forward

float pursue(float x, float y, float heading, float speed) {
  // The lookahead is scheduled on speed, with a hard floor: at a standstill a
  // proportional schedule asks for zero, which is a division by zero in disguise.
  float lookahead = constrain(0.7f * speed + 0.4f, 0.4f, 3.0f);

  float s = projectOntoPath(x, y, lastS);      // windowed around lastS
  lastS = max(lastS, s);
  Point goal = pointAtArcLength(lastS + lookahead);

  float dx = goal.x - x, dy = goal.y - y;
  float lateral = -sinf(heading) * dx + cosf(heading) * dy;
  float ld = max(0.3f, hypotf(dx, dy));        // clamp, or curvature blows up

  float curvature = 2.0f * lateral / (ld * ld);
  return curvature;                            // omega = v * curvature
}

Aspectos esenciales: fijar una distancia de mira mínima no nula en reposo, garantizar la monotonicidad creciente de lastS y acotar la distancia efectiva ld para prevenir divergencias en el cálculo de curvatura.

Suposiciones y diferencias con el hardware

El entorno asume un mapa exacto, distancia entre ejes perfectamente conocida, adherencia ideal y retardos lineales de primer orden. En un robot físico, el deslizamiento de ruedas altera la estimación odometría, la holgura mecánica introduce bandas muertas en cada inversión de giro y en vehículos de tracción diferencial la velocidad angular de guiado sustituye al ángulo de dirección (ω = v · κ).

Diagrama de circuito

El controlador requiere una infraestructura sensorial precisa:

Posición. Encoders en cuadratura en ambas ruedas con odometría diferencial calculada en cada ciclo de lazo.

Orientación. Giróscopo para integración de corto plazo asistido por magnetómetro para referencias absolutas de rumbo.

Corrección absoluta exterior. Módulo GPS integrado mediante filtro de fusión en lugar de guiado directo sobre coordenadas crudas.

Para profundizar en los fundamentos matemáticos, consulta los tutoriales de persecución pura (pure pursuit) y controladores de error transversal; para el montaje físico, el rover guiado por waypoints GPS desarrolla la integración progresiva.