Чтение датчика HC-SR04: надёжное измерение расстояния

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

Как датчик сообщает расстояние

HC-SR04 использует простой двухконтактный интерфейс. Подайте на вывод Trigger уровень HIGH в течение 10 микросекунд; датчик сформирует восемь пачек импульсов с частотой 40 kHz и установит на выводе Echo высокий уровень до возвращения отражённого сигнала. Время, в течение которого Echo остаётся на уровне HIGH, и есть время распространения туда и обратно. Скорость звука составляет около 343 m/s, поэтому:

distance_cm = echo_time_us * 0.0343 / 2

Деление на два требуется потому, что звук прошёл путь туда и обратно. На странице компонента описаны подключение и физика; здесь важно корректно читать данные в коде.

Чтение с помощью pulseIn — всегда задавайте тайм-аут

Тайм-аут важен не только для корректности: pulseIn блокирует выполнение, поэтому потерянное эхо останавливает весь цикл на заданный интервал. Сколько это стоит движущемуся роботу, показано в руководстве о задержках без блокировки.

pulseIn измеряет время, в течение которого вывод остаётся на уровне HIGH. Ловушка заключается в следующем: если эхо не возвращается — в пределах досягаемости нет объекта или мягкая поверхность рассеивает звук, — pulseIn блокирует выполнение на полный интервал тайм-аута по умолчанию, останавливая контур регулирования. Всегда передавайте явный тайм-аут, рассчитанный на максимальный диапазон:

const int TRIG = 9, ECHO = 10;
const unsigned long TIMEOUT_US = 25000UL;  // ~4 m of round trip

long pingCm() {
  digitalWrite(TRIG, LOW);  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH); delayMicroseconds(10);
  digitalWrite(TRIG, LOW);

  unsigned long us = pulseIn(ECHO, HIGH, TIMEOUT_US);
  if (us == 0) return -1;             // timed out: nothing in range
  return (long)(us * 0.0343 / 2.0);
}

Возврат -1 при тайм-ауте — вместо 0 — важен. Значение 0 воспринимается как «препятствие касается датчика», что противоположно значению «путь свободен». Необходимо чётко различать «эха нет» и «объект очень близко».

Почему изменяются исходные показания

Если направить HC-SR04 на стену, показания всё равно будут колебаться, поскольку реальный мир неидеален:

• Мягкие или наклонные поверхности рассеивают эхо в сторону от приёмника, поэтому иногда возникает очень большое значение или потеря сигнала.
• Конус ~15° означает, что датчик сообщает о ближайшем объекте в конусе, поэтому боковая ножка стола может резко уменьшить показание.
• Слишком частый опрос (с интервалом менее ~60 ms) позволяет эху предыдущего импульса попасть в следующее измерение.

Если передавать такие скачки непосредственно в решение об остановке или повороте, робот начнёт дёргаться. Необходимо сглаживать поток, не добавляя запаздывания.

Медианный фильтр для 5 измерений

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

int cmpLong(const void* a, const void* b) {
  return (*(long*)a) - (*(long*)b);
}

long medianCm() {
  long s[5];
  for (int i = 0; i < 5; i++) {
    long v = pingCm();
    s[i] = (v < 0) ? 400 : v;   // treat timeout as "far", not "touching"
    delay(30);                  // let the previous echo die away
  }
  qsort(s, 5, sizeof(long), cmpLong);
  return s[2];                  // the middle value
}

Две детали делают такое решение надёжным: сопоставление тайм-аута с большим значением «далеко» (чтобы потерянное эхо никогда не воспринималось как препятствие) и небольшая delay между импульсами, предотвращающая взаимное влияние эха. Теоретическую основу фильтрации без избыточного сглаживания см. в руководстве шум датчиков, смещение и фильтрация.

Прекратите блокировать цикл

Всё описанное выше по-прежнему останавливает цикл. pulseIn ожидает, delay(30) ожидает, а пять измерений с интервалом 30 ms занимают 150 ms, в течение которых робот не читает другие датчики и не принимает решений. При 0.3 m/s это 4.5 cm слепого перемещения на каждое измерение.

Решение — реализовать управление датчиком в виде небольшого конечного автомата, который главный цикл поэтапно выполняет без ожидания:

enum PingState { IDLE, TRIGGERED, WAITING };

PingState state = IDLE;
unsigned long stateSince = 0;
long lastDistance = -1;

const unsigned long PING_PERIOD_MS = 60;   // let the previous echo die away
const unsigned long ECHO_TIMEOUT_MS = 30;  // beyond the sensor's range

void updatePing() {
  const unsigned long now = millis();

  switch (state) {
    case IDLE:
      if (now - stateSince < PING_PERIOD_MS) return;
      digitalWrite(TRIG, HIGH);
      stateSince = micros();          // note: microseconds for this short step
      state = TRIGGERED;
      break;

    case TRIGGERED:
      if (micros() - stateSince < 10) return;
      digitalWrite(TRIG, LOW);
      stateSince = now;
      state = WAITING;
      break;

    case WAITING:
      if (echoComplete) {             // set by the interrupt below
        lastDistance = echoMicros / 58.2;
        echoComplete = false;
        stateSince = now;
        state = IDLE;
      } else if (now - stateSince > ECHO_TIMEOUT_MS) {
        lastDistance = -1;            // no echo — far, or invisible
        stateSince = now;
        state = IDLE;
      }
      break;
  }
}

Теперь цикл вызывает updatePing() и переходит к другим задачам. lastDistance всегда содержит последнее завершённое измерение возрастом не более 60 ms, и больше ничто в роботе не ожидает его.

Измерение времени эха с помощью прерывания

Конечный автомат по-прежнему должен знать, когда вернулось эхо, а опрос digitalRead в главном цикле снижает разрешение. Прерывания по обоим фронтам вывода Echo не требуют дополнительных затрат:

volatile unsigned long echoStart = 0;
volatile unsigned long echoMicros = 0;
volatile bool echoComplete = false;

void echoISR() {
  if (digitalRead(ECHO) == HIGH) {
    echoStart = micros();
  } else if (echoStart != 0) {
    echoMicros = micros() - echoStart;
    echoStart = 0;
    echoComplete = true;
  }
}

void setup() {
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);
  attachInterrupt(digitalPinToInterrupt(ECHO), echoISR, CHANGE);
}

На Arduino Uno для этого необходимо использовать вывод 2 или 3 — два единственных вывода внешних прерываний, — но если в роботе есть колёсные энкодеры, они тоже нуждаются в обоих. Этот конфликт реален и часто становится причиной перехода проекта на прерывания по изменению выводов Nano, Mega или ESP32.

Обратите внимание на проверку echoStart != 0. Без неё задний фронт, пришедший без предшествующего переднего фронта — что происходит на первом прерывании после тайм-аута, — приведёт к вычислению бессмысленной длительности по устаревшей отметке времени.

Выбор фильтра

Медиана — подходящий вариант по умолчанию, но стоит понимать, как на самом деле работают альтернативы, поскольку они отказывают по-разному:

Шум ультразвукового датчика не является гауссовым: это отдельные сильные выбросы на фоне в остальном устойчивого сигнала, то есть именно тот случай, который среднее обрабатывает плохо, а медиана — безупречно. Одиночный выброс 400 cm среди пяти измерений по 30 cm изменяет среднее на 74 cm, а медиану — никак.

Температура, если нужна точность

Делитель 58.2 предполагает скорость звука 343 m/s при 20 °C. Эта скорость не постоянна:

speed (m/s) = 331.3 + 0.606 x temperature_celsius

При обходе препятствий полностью игнорируйте это — 7 cm на 2 m не изменят ни одного решения. Для робота, измеряющего зазор, или работающего в гараже зимой введите компенсацию:

float cmPerMicrosecond(float tempC) {
  return (331.3f + 0.606f * tempC) / 20000.0f;   // /1e6 for us, /2 for round trip
}

Если в роботе уже установлен MPU-6050 — или почти любой датчик по I²C, — то показание температуры доступно бесплатно.

Использование нескольких датчиков

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

const int SENSORS = 3;
int current = 0;
long distance[SENSORS];

void updateAll() {
  if (pingComplete(current)) {
    distance[current] = lastDistance;
    current = (current + 1) % SENSORS;    // round-robin
    startPing(current);
  }
}

Плата — частота обновления: при интервале 60 ms на каждый из трёх датчиков показание обновляется каждые 180 ms. Если этого недостаточно — а на движущемся роботе часто бывает именно так, — решение состоит либо в сокращении числа ультразвуковых датчиков, либо в добавлении VL53L0X, использующего свет и поэтому никак не взаимодействующего с гидролокатором.

Диагностика

Что делать дальше

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