Точные повороты и следование вдоль стены в лабиринте

Введение

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

Поэтому алгоритм редко бывает причиной отказа. Отказывает перемещение: удержание по центру коридора и поворот ровно на девяносто градусов. Это руководство посвящено двум навыкам и причинам, почему очевидный подход к каждому из них неверен.

Почему одного счисления от пройденного пути недостаточно

Соблазнительный подход — управлять движением только по отсчётам энкодера: одна клетка — это N импульсов, четверть оборота — M импульсов, а лабиринт является сеткой, поэтому просто считайте.

Он не работает, потому что ошибки накапливаются и никогда не исправляются. Колёса проскальзывают при разгоне. Два двигателя не идентичны. Измеренная ширина колеи отличается на миллиметр, поэтому каждый поворот равен 88°, а не 90°. Каждая ошибка мала; ни одна не компенсирует другую. После двадцати клеток робот едет по диагонали в стены с полной уверенностью.

Исправление — использовать сам лабиринт как опорный ориентир. Стены прямые, параллельные и имеют известный интервал — это бесплатное измерение без дрейфа, доступное постоянно.

Следование вдоль стены как контур ПИД-регулятора (PID)

С датчиком расстояния с каждой стороны центрирование является задачей управления, которую вы уже решали в руководстве по роботу, следующему по линии: вычислить ошибку и привести её к нулю.

// Positive error = too close to the left wall = steer right.
float error = leftDistance - rightDistance;

float correction = Kp * error + Kd * (error - previousError);
previousError = error;

int base = 120;
setMotors(base + correction, base - correction);

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

Начните с нулевого Kd, увеличивайте Kp, пока робот не начнёт отслеживать коридор с небольшим покачиванием, затем добавляйте Kd, пока покачивание не уменьшится. Ki почти никогда не нужен: коридор лабиринта короткий, а интегральное действие в основном добавляет запаздывание и перерегулирование на каждой границе клетки.

Случай, который неверно обрабатывает наивный контур

В лабиринте есть открытые участки. Как только боковая стена заканчивается, датчик считывает расстояние до чего-то далёкого — противоположной стены открытой клетки или вообще ничего, — и leftDistance - rightDistance становится огромной ложной ошибкой. Робот резко уходит в проём, мимо которого он должен был проехать.

Проверяйте наличие стены, прежде чем доверять разности:

const int WALL_THRESHOLD = 100;   // mm — beyond this there is no wall

bool leftWall  = leftDistance  < WALL_THRESHOLD;
bool rightWall = rightDistance < WALL_THRESHOLD;

float error;
if (leftWall && rightWall) {
  error = leftDistance - rightDistance;        // centre between both
} else if (leftWall) {
  error = 2 * (leftDistance - IDEAL_SIDE);     // hold one wall at a set distance
} else if (rightWall) {
  error = 2 * (IDEAL_SIDE - rightDistance);
} else {
  error = 0;                                    // open cell — hold heading instead
}

Односторонние случаи удваивают коэффициент, потому что одна стена даёт половину информации: с двумя стенами смещение на 5 mm влево меняет разность на 10 mm, а с одной — на 5.

В случае отсутствия стен переход к error = 0 ведёт прямо насколько позволяет привод — а в реальности это не прямая. Здесь подключается гироскоп.

Точный поворот на 90 градусов

Подсчёт импульсов энкодера через поворот — естественная первая попытка, но она недостаточно точна. При развороте на месте проскальзывание больше, чем при прямом движении, а ошибка 2° на поворот даёт 20° после десяти поворотов: робот направлен по диагонали вдоль коридора.

Интегрируйте показания гироскопа. Показание датчика MPU6050 по оси Z — прямое измерение скорости вращения робота, и ему всё равно, проскальзывали ли колёса:

float turnDegrees(float target) {
  float heading = 0;
  unsigned long last = micros();

  while (fabs(heading) < fabs(target)) {
    unsigned long now = micros();
    float dt = (now - last) / 1000000.0;
    last = now;

    heading += (readGyroZ() - gyroBias) * dt;   // bias subtraction is essential

    int speed = (target > 0) ? TURN_SPEED : -TURN_SPEED;
    setMotors(speed, -speed);                   // pivot in place
  }
  setMotors(0, 0);
  return heading;                               // report the real angle achieved
}

Три вещи решают успех:

Вычитайте смещение гироскопа. Неоткалиброванный гироскоп при неподвижном роботе показывает один-два градуса в секунду, а вы интегрируете это значение. Как и в руководстве по наклону, измерьте среднее в покое и вычитайте его из каждого показания.

Останавливайтесь заранее и дайте накат. У робота есть момент вращения, поэтому отключение двигателей ровно при 90° приводит к перелёту. Остановитесь примерно при 85° и позвольте ему доехать — или лучше уменьшайте скорость поворота по мере уменьшения оставшегося угла.

Выравнивайтесь по передней стене. Когда после поворота стена оказывается перед роботом, она является идеальным ориентиром 90°. Медленно подъезжайте к ней и позвольте контуру следования вдоль стены выпрямить робот; это устраняет накопленную ошибку курса. Конструкторы Micromouse делают это при каждой возможности, превращая дрейфующий робот в самокорректирующийся.

Когда клетка пройдена

Подсчёт клеток связывает перемещение с алгоритмом лабиринта. Расстояние по энкодерам даёт приблизительную границу; стены сообщают её точно.

Наиболее надёжный триггер — переход показания бокового датчика от наличия стены к её отсутствию. Когда робот проходит границу клетки, боковой датчик, который показывал стену, внезапно показывает далёкое значение; этот переход происходит в известной точке клетки и является физическим ориентиром, а не накопленной оценкой.

Объедините два метода: используйте энкодеры для предсказания границы, а затем позвольте краю стены подтвердить или исправить её. Если оценки расходятся, доверяйте показанию датчика стены.

Проверьте перед запуском алгоритма

Четыре теста на настоящем лабиринте, в таком порядке:

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

Только после прохождения этих четырёх тестов имеет смысл запускать поверх них алгоритм решения лабиринта: планировщик с плохими данными о положении выдаёт уверенную бессмыслицу. Сначала посмотрите на слой планирования отдельно в симуляторе решения лабиринта, затем объедините оба слоя в проекте Micromouse.