Симулятор движения по траектории: Pure Pursuit и Stanley

Управление

Регулятор выбирает один из двух вариантов: Pure Pursuit или Stanley. Упреждение — единственный регулируемый параметр Pure Pursuit, и для Stanley оно отключено, потому что Stanley его не использует — именно в этом и состоит суть.

Скорость и упреждение взаимодействуют, и с этим стоит поиграть: установите 1 m/s и найдите лучшее упреждение, затем установите 3 m/s и найдите его снова.

Источник положения заменяет идеальную одометрию обновлением 1 Hz с шумом масштаба метра, которое удерживается между обновлениями. Ошибка всегда оценивается относительно истины, поэтому штраф реальный, а не скрытый.

Перетащите любую контрольную точку или нажмите 1–8, чтобы выбрать её и перемещать стрелками. Весь прогон пересчитывается при перетаскивании.

Теория

Оба регулятора отвечают на один вопрос — какой угол руления выбрать, если известны моё положение и положение пути? — но отвечают на него совершенно по-разному.

Pure Pursuit основан на геометрии. Найдите точку пути на расстоянии упреждения L впереди и двигайтесь по дуге через неё:

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

Здесь y — боковое смещение целевой точки в системе координат робота, а b — колёсная база. Параметр один, причём это расстояние, а не коэффициент, поэтому его можно переносить между роботами.

Stanley — это управление. Привяжите систему координат к передней оси и добавьте два слагаемых:

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

Ошибка курса плюс арктангенс поперечной ошибки. Деление на скорость позволяет одному коэффициенту работать в диапазоне скоростей. k — настоящий коэффициент регулятора, и он не переносится.

Лаборатория добавляет то, что опускают учебные выводы: 0.4 s полной задержки как переходную характеристику рулевого привода первого порядка 0.25 s плюс 0.15 s задержки измерений и вычислений. Без неё Pure Pursuit монотонно улучшается по мере уменьшения упреждения, и весь вопрос настройки исчезает.

Алгоритм

На каждом шаге для обоих регуляторов:

1. Проецируйте на путь, выполняя поиск в окне вокруг последней достигнутой длины дуги, а не по всему пути. Глобальный поиск выбирает ближайший сегмент, поэтому трасса, которая возвращается назад, телепортирует прогресс робота в угол, пройденный минуту назад.
2. Разрешайте ничью в вершине в пользу исходящего сегмента. После поворота оба соседних сегмента проецируются в одну точку на одинаковом расстоянии; сохранение входящего сегмента передаёт регулятору ошибку курса и боковое смещение, почти равные нулю, пока он едет прямо с трассы, без предупреждения.
3. Вычислите угол руления по собственному правилу регулятора и ограничьте его механическим пределом.
4. Примените задержку, затем интегрируйте модель велосипеда.
5. Оценивайте по истине, а не по оценке положения.

Шаг 2 — не оптимизация. Именно он отличает работающий Stanley от Stanley, который уверенно съезжает с трассы на 78% каждого запуска.

Исходный код

Pure Pursuit в полном виде, в форме, которую он принимает на настоящем роботе:

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
}

Три важные строки — нижний предел расписания, монотонность lastS и ограничение ld. Всё остальное — формула.

Допущения и отличия от реального оборудования

Лаборатория даёт роботу идеальную карту пути, идеально известную колёсную базу, отсутствие проскальзывания и чистую модель задержки первого порядка. У настоящего робота нет ни одного из этих условий.

Проскальзывание колёс делает оценку положения ошибочной так, что ни один регулятор не может это обнаружить. Люфт редуктора добавляет мёртвый ход при каждом изменении направления руления, что действует как дополнительная задержка и увеличивает оптимальное упреждение. А привод на дифференциальном принципе вообще не имеет угла руления — у него есть заданная скорость вращения, поэтому ω = v κ заменяет atan, а колёсная база важна только для того, насколько плотно он может поворачивать.

После всего этого сохраняется только форма: внутренний оптимум, смещающийся вправо вместе со скоростью, и рост ошибки при этом оптимуме со скоростью.

Схема соединений

Регулятор — самая дешёвая часть. Дороже то, что он требует от остальных частей робота.

Положение. Квадратурные энкодеры на обоих колёсах и одометрия привода на дифференциальном принципе, которая их интегрирует с частотой контура. В помещении этого достаточно на несколько десятков метров.

Курс. Гироскоп для краткосрочного отсчёта, магнитометр для абсолютного ориентира или оба. Ошибку курса оба регулятора учитывают с наибольшим весом, и одометрия теряет её первой.

Абсолютная коррекция на улице. Модуль GPS, объединённый с оценкой, а не используемый напрямую, — разница составляет 0.09 m среднеквадратичной ошибки (RMS) против 1.25.

Полную теорию смотрите в материалах Pure Pursuit и поперечной ошибке; для сборки начните с робота-вездехода по контрольным точкам GPS, который сначала работает в помещении на энкодерах и выходит наружу только после работоспособности регулятора следования.