Pure Pursuit: следование по пути с одним параметром

Ступень, которую никто не преподаёт

Планировщик даёт вам путь. A* даёт список ячеек сетки; обследование — список точек пути; задача миссии — линию на карте. Ничто из этого не говорит колёсам, что делать.

Именно в этом промежутке живёт Pure Pursuit, и он состоит из одного абзаца геометрии:

Найдите точку пути на фиксированном расстоянии впереди. Выполните круговую дугу, проходящую через неё. Повторите.

Здесь нет сигнала ошибки, интегрального члена или расписания коэффициентов. Есть одно число — упреждение — и оно определяет всё.

Геометрия

Поместите целевую точку в собственную систему координат робота: x вперёд, y влево, на расстоянии L_d. Круговая дуга из текущего положения робота через эту точку имеет кривизну

κ = 2y / L_d²

Эту формулу стоит прочитать дважды, потому что из неё следует всё остальное. Боковое смещение y стоит в числителе, поэтому дуга тем сильнее изгибается, чем дальше цель находится от оси. Упреждение стоит в знаменателе в квадрате, поэтому его удвоение уменьшает кривизну вчетверо.

Переведём это в радиус и угол поворота руля транспортного средства с колёсной базой b:

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

У дифференциального привода нет угла поворота руля — вы непосредственно задаёте частоту вращения, ω = v κ. Та же кривизна, другой исполнительный механизм.

Единственный параметр и его внутреннее противоречие

Короткое упреждение. Целевая точка близка, поэтому небольшое боковое смещение создаёт большую кривизну. Робот плотно отслеживает путь — и бурно реагирует на каждое колебание, включая те, которые вызвало его собственное рулевое управление.

Большое упреждение. Целевая точка далеко, кривизна плавная, руление спокойное — но на углу целевая точка уже обогнула поворот, а робот ещё нет, поэтому робот срезает угол.

Эти два варианта тянут в противоположные стороны, а в алгоритме нет ничего, что их уравновешивает. Вы выбираете число.

Ловушка в каждом учебном выводе

Запишите Pure Pursuit на бумаге для робота, который мгновенно рулит, и короткое упреждение всегда будет лучше. Ошибка монотонно уменьшается при сокращении упреждения. Оптимума нет, и U-образной кривой выше тоже нет.

У реальных роботов есть задержка. Рулевое управление требует времени, чтобы достичь заданного угла; к моменту, когда колёса отреагируют, оценка позы, на которой вы действуете, устарела на несколько десятков миллисекунд. Симулятор моделирует постоянную времени рулевого управления 0.25 s и задержку восприятия и вычислений 0.15 s — итого 0.4 s задержки, что совершенно обычно для любительского робота.

Именно эта задержка создаёт оптимум. Короткое упреждение формирует корректирующую команду, команда приходит поздно, робот уже прошёл ошибку, а следующая команда перекомпенсирует её в другую сторону. Регулятор, который быстрее собственного исполнительного механизма, осциллирует. Это тот же факт, который управляет дифференциальным коэффициентом ПИД-регулятора (PID) и стоит за расстоянием торможения: задержка — не деталь, а именно то, что задаёт настройку.

Как выбрать значение?

Ниже приведены измеренные оптимумы и их значения в единицах, которые можно перенести на другой робот:

У этой таблицы есть два честных вывода.

Упреждение растёт со скоростью, но не пропорционально. Распространённый совет «задайте фиксированное число секунд движения» близок, но неверен — время в этом диапазоне уменьшается с 0.90 s до 0.47 s.

Она находится на расстоянии от одного до двух расстояний, пройденных за время задержки управления. Это полезное правило, потому что его можно измерить: найдите задержку, умножьте её на скорость и начните с этого значения. У этого робота v × lag меняется от 0.4 m до 1.2 m, а оптимумы — от 0.9 m до 1.4 m.

На практике реализуйте это как небольшое линейное расписание с ограничением:

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

L_MIN важнее K_LOOK. При нулевой скорости расписание требует нулевого упреждения, а это деление на ноль под маской.

Реализация

// 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);
}

Три вещи в этих двенадцати строках важнее остального:

• lastS — это состояние, и оно движется только вперёд. Без него любой путь, пересекающий сам себя, заставит регулятор потеряться.
• Ограничивайте ld. По мере приближения целевой точки кривизна стремится к бесконечности.
• Целевая точка находится на пути, а не является ближайшей точкой пути. Наведение на точки заставляет робот зигзагообразно переходить между ними; наведение на точку сегмента делает движение плавным.

Что он умеет и чего не умеет

Он не умеет стоять на месте и разворачиваться лицом к пути. У Pure Pursuit нет понятия остановки — кривизна является формой, а форме нужно поступательное движение. Запустите робота примерно выровненным или сначала выполните короткую начальную дугу.

У него есть минимальный радиус поворота, и путь может ему не соответствовать. Если угол острее b / tan(δ_max), робот не сможет пройти его ни при какой настройке. Смягчите путь или замедлитесь перед углом — для этого и нужен профиль движения.

Он всегда срезает углы — это задумано. В измеренном прогоне при 2 m/s робот прошёл на 0.8% меньше заданного пути. Если это неприемлемо, нужен другой регулятор — Stanley рулит по поперечной ошибке и выбирает внешнюю траекторию вместо срезания.

Он не исправит плохую оценку позиции. Всё вышесказанное предполагает, что робот знает, где находится. Если передать ему GPS-поправку с частотой 1 Hz, среднеквадратичная (RMS) ошибка вырастет с 0.09 m до 1.25 m — см. навигацию по точкам с GPS.

Когда что-то идёт не так

Проведите оба регулятора по одному маршруту в симуляторе следования по пути — перетаскивайте упреждение, и весь прогон будет перестраиваться. Затем соберите ровер с точками GPS, где оценка позиции станет самой сложной частью.