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

Одиночный датчик расстояния, направленный вперёд, может сообщить роботу, что впереди есть объект, но не покажет, в какую сторону можно повернуться. Робот поворачивает вслепую и часто направляется прямо в другое препятствие или тупик. Установка HC-SR04 на сервопривод SG90 решает эту проблему: робот останавливается, смотрит влево и вправо и поворачивает в наиболее свободном направлении — так же, как человек на повороте с ограниченной видимостью.

Почему один неподвижный датчик запирает робота

Представьте робота, который видит только прямо перед собой. Он встречает стену и понимает, что нужно повернуть, но не знает, что лучше: влево или вправо. При неверном выборе он въезжает в угол, из которого только что выехал. Датчик на сервоприводе превращает одно число (расстояние впереди) в небольшую карту пространства перед роботом, поэтому поворот становится обоснованным решением, а не случайным выбором.

Установка датчика на сервопривод

Закрепите HC-SR04 на рычаге сервопривода и установите сервопривод в передней по центру части шасси так, чтобы рычаг перемещал датчик слева направо, а не сверху вниз. В центральном положении сервопривода датчик должен смотреть строго вперёд. Получится чистое сканирование от одной стороны через направление прямо вперёд к другой стороне.

Сканирование и измерение

Задайте сервоприводу несколько углов и снимите отфильтрованное показание на каждом. Используйте проверенную функцию измерения расстояния с фильтрацией из руководства о чтении HC-SR04 — нефильтрованное показание во время движения сервопривода слишком зашумлено для управления.

#include <Servo.h>
Servo scanner;

const int ANGLES[] = {30, 60, 90, 120, 150};
const int N = 5;

int bestAngle() {
  long best = -1;
  int bestA = 90;
  for (int i = 0; i < N; i++) {
    scanner.write(ANGLES[i]);
    delay(180);                 // let the servo REACH the angle first
    long d = medianCm();        // filtered read from the previous tutorial
    if (d > best) { best = d; bestA = ANGLES[i]; }
  }
  scanner.write(90);            // re-centre for driving
  return bestA;                 // > 90 = open to the left, < 90 = right
}

Не считывайте показание во время движения

Самая распространённая ошибка здесь — считывание до прибытия сервопривода в заданный угол. Вызов servo.write() завершается сразу, но SG90 требуется время на поворот. Если отправить импульс слишком рано, измеренное расстояние относится к тому месту, где датчик был, а не куда его направили. Короткая delay после каждого write() — достаточная для стабилизации сервопривода — сделает каждое показание достоверным.

Преобразование сканирования в решение

Наибольшее расстояние соответствует самому свободному направлению. Несколько уточнений делают решение осмысленным:

• При равенстве предпочитайте направление прямо вперёд. Если два направления имеют близкие значения, смещайте выбор к центру, чтобы робот не совершал ненужных рывков.
• Все направления закрыты? Это тупик — отъедьте назад и повторите сканирование, вместо того чтобы продолжать движение вперёд.

Скорость против охвата

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

Сколько времени на самом деле требуется сервоприводу?

«Короткая задержка» — самая расплывчатая инструкция в приведённом выше коде, поэтому её стоит заменить измеренным числом. Для SG90 при 4.8 V без нагрузки указано примерно 0.1 s на 60°, следовательно, время стабилизации зависит от длины перемещения:

Два фактора делают фактическое значение хуже спецификации. Нагрузка: HC-SR04 и его кабель на рычаге создают небольшую, но ненулевую нагрузку, замедляющую сервопривод и увеличивающую перерегулирование. Напряжение: при 4.0 V от просевшего блока тот же сервопривод заметно медленнее, чем при 6 V.

Измерьте свой экземпляр, не полагаясь на таблицу. Задайте шаг 60° и измерьте время до прекращения изменения показания расстояния:

void measureSettle() {
  scanner.write(60);  delay(500);            // known start
  scanner.write(120);                        // command the step
  const unsigned long t0 = millis();
  long prev = -1;
  for (int i = 0; i < 40; i++) {             // sample every 10 ms
    const long d = pingCm();
    Serial.print(millis() - t0); Serial.print(' '); Serial.println(d);
    delay(10);
  }
}

Показание стабилизируется в определённый момент. Используйте это число и небольшой запас — так вы устраните главный источник бессмысленных значений во всём поведении.

Сканирование в одном направлении быстрее хаотичного перемещения

Приведённый выше код переходит по углам 30 → 60 → 90 → 120 → 150, то есть уже выполняет одно направленное сканирование, и это сделано намеренно. При чтении углов в разрозненном порядке каждый шаг становится большим, а затраты на стабилизацию накапливаются на каждом шаге:

Полное сканирование — момент слепоты робота: он остановлен и не продвигается, поэтому сокращение длительности вдвое уменьшает время, в течение которого робот не реагирует на обстановку. Сканируйте по порядку, а при повторных сканированиях чередуйте направление, чтобы не платить за обратный ход.

Сканирование без блокировки

delay(180) на каждом шаге означает, что сканирование пяти углов блокирует выполнение почти на секунду. Для робота, который всё равно остановлен, это допустимо, но при этом не считываются концевые выключатели и замораживается любое другое поведение.

Здесь применяется тот же конечный автомат, что и при формировании импульса:

enum ScanState { SCAN_IDLE, SCAN_MOVING, SCAN_READING };

ScanState scanState = SCAN_IDLE;
uint8_t   scanIndex = 0;
unsigned long scanSince = 0;
long      scanResult[N];
bool      scanComplete = false;

void updateScan() {
  switch (scanState) {
    case SCAN_IDLE:
      break;

    case SCAN_MOVING:
      if (millis() - scanSince < SETTLE_MS) return;      // measured, above
      scanState = SCAN_READING;
      break;

    case SCAN_READING:
      scanResult[scanIndex] = medianCm();
      if (++scanIndex >= N) { scanComplete = true; scanState = SCAN_IDLE; scanner.write(90); }
      else { scanner.write(ANGLES[scanIndex]); scanSince = millis(); scanState = SCAN_MOVING; }
      break;
  }
}

Главный цикл вызывает updateScan() и продолжает работу. Концевые выключатели продолжают считываться, конечный автомат работает, а сканирование завершается, когда все углы обработаны.

Интерполяция между углами

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

// Weight each angle by how open it is, ignoring anything below the stop threshold.
float openBearing() {
  float weighted = 0, total = 0;
  for (int i = 0; i < N; i++) {
    if (scanResult[i] < STOP_CM) continue;             // blocked, contributes nothing
    const float w = scanResult[i] - STOP_CM;           // how much clearance beyond the threshold
    weighted += w * ANGLES[i];
    total    += w;
  }
  return (total > 0) ? (weighted / total) : 90.0f;     // 90 = straight ahead
}

Это реальное улучшение по сравнению с выбором максимума. Выбор максимума при расположении дверного проёма немного не по центру направит робота к краю проёма, а центроид — к его середине. Он также корректно работает при частично открытых соседних направлениях: если открыты два соседних угла, результат окажется между ними, а не произвольно на одном из них.

Конус ограничивает возможности сканирования

Полуугол луча HC-SR04 составляет примерно 15°, поэтому на 1 m ширина конуса составляет около 53 cm. При дискретизации с интервалом 30° зоны сильно перекрываются — соседние показания в основном наблюдают одно и то же пространство.

Важное следствие: сканирование не обеспечивает угловое разрешение лучше ширины луча. Показание 68 cm под 120° означает «что-то находится на расстоянии 68 cm в некотором месте примерно в пределах 15° от 120°» — этого достаточно для выбора направления поворота, но бесполезно для измерения ширины проёма. Если требуется второе, установите на тот же сервопривод VL53L0X с намеренно узким лучом и получите сканирование с различимым пространственным разрешением.

Диагностика