Использование лидара от робота-пылесоса для системы предотвращения столкновений в автономных роботах

Привет!
Меня зовут Степан Бурмистров, уже лет 9 я преподаю робототехнику школьникам, и наблюдаю за тем, как быстро меняются возможности для создания роботов!
Подготавливая материалы для курса по ROS2, о котором писал в прошлой статье, поработал с лидаром от робота пылесоса, который интересен в первую очередь тем, что стоит в пределах 2000 руб.

Лидар (LiDAR — Light Detection and Ranging) — это оптический датчик, который использует лазерное излучение для измерения расстояния до объектов. Принцип его работы заключается в том, что устройство испускает лазерный импульс и замеряет время, за которое отражённый сигнал вернётся обратно. По этому времени вычисляется дистанция до препятствия. В бытовых роботах-пылесосах лидары активно применяются для построения карт помещений и планирования маршрута, позволяя устройству эффективно ориентироваться в пространстве. При этом лидары из таких устройств, несмотря на доступную цену, способны передавать достаточно точные данные, что делает их отличным выбором для учебных и экспериментальных проектов.

Обычно, процесс обработки данных с лидара - достаточно сложная задача, связанная с построением карты помещений и локализации робота. В данной работе описывается способ прямого взаимодействия с лидаром от робота-пылесоса без использования высокоуровневых фреймворков, таких как, например, ROS2.
Основная цель, которую я преследую, дать возможность начинающим инженерам-робототехникам попробовать поработать с лидаром и получить потенциал для скачка вперед, а также снять страх перед “сложным” на вид модулем и протоколом передачи данных.
Мы рассмотрим пошагово структуру данных лидара и разберемся, как легко и понятно визуализировать данные при помощи кольца из 12 светодиодов.

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

Оборудование и общая схема проекта

1. Лидар от робота-пылесоса.

   • В этом примере рассмотрим один из вариантов дешевых лидаров, который легко ищется по запросу: Лидар для Dreame F9/W10/D9/D9Pro/D9Plus/D9max/L10Pro.

     

   • Как правило, имеет пин TX (UART) для обмена данными на скорости 115200 бод, а также линию питания 5V и землю. Разъем у данного устройства с шагом 2мм. Подходит разъем JST PH2.0 (3 pin).

     

2. Микроконтроллер на базе ESP32.

   • На ESP32 мы организуем чтение пакетов лидара (используя аппаратный Serial1). Вариант Arduino + SoftSerial не походит, т.к. не хватает скорости для обработки пакетов.

     

   • Здесь же формируется визуальная индикация на кольце из 12 светодиодов (WS2812B или аналогичные).

   • Дополнительно ESP32 транслирует упрощённую информацию о статусах секторов на выходной аппаратный UART2.

3. Arduino Uno (или другой микроконтроллер)

   • инимает данные от ESP32 по интерфейсу SoftwareSerial.

   • Обрабатывает информацию о том, какие «секторы» зелёные, жёлтые или красные.

   • В рамках демо просто выводит эти данные в Serial-монитор.

4. Кольцо из 12 светодиодов (NeoPixel/WS2812B).

   • Каждый светодиод управляется по одному цифровому пину ESP32 (через библиотеку Adafruit NeoPixel).

   • Секторы соответствуют углам 0…360°, что мы делим на 12 равных частей.

   • Цвета индикаторов зависят от полученной дистанции: зелёный – всё в порядке, жёлтый – предупреждение, красный – опасное сближение.

     

5. Питание.

   • При использовании ESP32, Arduino Uno и светодиодов необходимо обеспечить соответствующие уровни напряжений и общую «землю».

   • Также важно убедиться, что ваш лидар может быть стабильно запитан от выбранного источника (обычно 5 В).

Пример потоков данных:

[Лидар] 
   TX ---> RX(Serial1) ESP32
   +5V и GND

[ESP32] 
   Pin17 ---> NeoPixelDin (12 светодиодов)
   TX2(16) ---> RX(SoftwareSerial=6) на Arduino
   RX2(4)  <--- TX(SoftwareSerial=3) на Arduino (при необходимости)
   общая земля

[Arduino UNO]
   Pin6 (SoftwareSerial RX) <--- Pin16 (TX2) ESP32
   Pin3 (SoftwareSerial TX) ---> Pin4 (RX2) ESP32 (опционально)

[Компьютер / Serial Monitor] 
   - Подключён к Arduino по USB
   - Подключён к ESP32 по USB (для отладки)

Схема проекта:

Структура пакета лидара и расшифровка данных

Заголовок и тело пакета

Лидары от разных производителей могут иметь различные форматы данных, однако многие бюджетные модели используют следующую структуру (36 байт на один пакет):

• Первые 4 байта – заголовок. Для рассматриваемого лидара:

  0x55, 0xAA, 0x03, 0x08

  По этим байтам мы «выравниваемся» и понимаем, что начинается новый пакет.

• Следующие 2 байта – скорость вращения лидара (rotationSpeedTmp).
  Интерпретируем как uint16_t, а затем переводим в обороты в минуту (RPM), обычно деля на 64.

• Далее 2 байта – угол начала сканирования (startAngleTmp), 16-битное число.

  Если результат вышел за диапазон 0–360, делаем нормализацию (добавляем или вычитаем 360, пока не попадём в [0, 360)).

• 8 групп по 3 байта (итого 24 байта) – данные о 8 точках:

  1. 2 байта на расстояние (distance) в миллиметрах, тип uint16_t;

  2. 1 байт на интенсивность отражённого сигнала (intensity).

• Завершающие 2 байта – угол конца сканирования (endAngleTmp). Аналогично стартовому углу, декодируем в градусы.

Определения углов для 8 точек данных

Мы имеем «начальный угол» startAngleDeg и «конечный угол» endAngleDeg. Лидар фактически сканирует некоторый сектор во время выдачи пакета, и внутри этого сектора расположены 8 точек. В коде:

float angleRange = endAngleDeg - startAngleDeg;
float angleInc = angleRange / 8.0f;
for (int i = 0; i < 8; i++) {
  float angle = startAngleDeg + i * angleInc;
  // нормализация angle в [0..360)
  ...
}

Если endAngleDeg < startAngleDeg, добавляется +360, чтобы получить корректный угол. Итоговые восемь углов хранятся в массиве packetAngles[].

Определение сектора, а также светодиода в кольце (12 светодиодов)

В данном проекте кольцо разделено на 12 секторов по 30°. Чтобы «нулевой» сектор соответствовал примерно углам 345..15°, вводят небольшой сдвиг на 15°:

float shifted = angleDeg + 15.0f;
...
int sector = (int)(shifted / 30.0f) % 12;
sector = (sector + SECTOR_OFFSET) % 12; // SECTOR_OFFSET позволяет подстроить смещение

Таким образом, каждая из 8 точек, пришедших в пакете, относится к конкретному сектору [0..11]. В коде также задаётся SECTOR_OFFSET = 1, чтобы «сместить» сектора, в зависимости от физического расположения лидара, при необходимости.

Код на ESP32: чтение данных лидара и визуализация

Полный код на GitHub

#include <Arduino.h>
#include <Adafruit_NeoPixel.h>  // Библиотека для работы с адресной светодиодной лентой

/********************************************************
 *  КОНСТАНТЫ И ПАРАМЕТРЫ
 ********************************************************/

// Определение количества «секторов» (равномерно делим 360° на 12 частей).
#define NUM_SECTORS 12

// Настройки адресной светодиодной ленты
#define LED_PIN 17                       // Пин, к которому подключена лента
#define LED_COUNT NUM_SECTORS            // По одному светодиоду на каждый сектор
#define LED_TYPE (NEO_GRB + NEO_KHZ800)  // Тип светодиодов (формат данных и частота)

// Создаём объект для управления лентой
Adafruit_NeoPixel strip(LED_COUNT, LED_PIN, LED_TYPE);

// Параметры UART для лидара (ESP32-специфичный Serial1)
#define LIDAR_RX_PIN 18
#define LIDAR_TX_PIN 19
#define BAUDRATE 115200

// UART2 - передача данных в следующий микроконтроллер
HardwareSerial UART2(2);
// Инициализация UART
#define UART_TX_PIN 16
#define UART_RX_PIN 4  // Любой свободный, если Arduino не отправляет обратно данные обратно
const unsigned long STATUS_PRINT_INTERVAL = 100; // 100 мс = 10 раз в секунду

// Структура пакета лидара: 4 байта заголовка, затем 32 байта данных
static const uint8_t LIDAR_HEADER[] = { 0x55, 0xAA, 0x03, 0x08 };
static const uint8_t LIDAR_HEADER_LEN = 4;
static const uint8_t LIDAR_BODY_LEN = 32;

// Пороговые расстояния (в миллиметрах) и время залипания аварии
// ALARM_DIST  — красная зона, WARNING_DIST — жёлтая зона
#define ALARM_DIST 400     // Менее 400 мм -> сектор в красном цвете
#define WARNING_DIST 650   // Менее 650 мм (но >= 400 мм) -> жёлтый
#define ALARM_HOLD_MS 300  // Время (мс), которое сектор будет «залипать» в красном
#define SECTOR_OFFSET 1    // Cдвиг секторов (0..11)

/********************************************************
 *  ГЛОБАЛЬНЫЕ МАССИВЫ
 ********************************************************/
// Храним текущее измеренное расстояние по каждому из 12 секторов.
// При отсутствии данных в секторе значение будет NO_VALUE.
static float sectorDistances[NUM_SECTORS] = { 0.0f };

// Время последнего обновления данных сектора (в миллисекундах).
static uint32_t sectorUpdateTime[NUM_SECTORS] = { 0 };

// Время, до которого сектор должен находиться в состоянии «тревоги» (красный цвет).
static uint32_t sectorAlarmUntil[NUM_SECTORS] = { 0 };

// Константа, обозначающая «нет данных».
static const float NO_VALUE = 99999.0f;

/********************************************************
 *  ВСПОМОГАТЕЛЬНЫЕ ФУНКЦИИ
 ********************************************************/

/**
 * @brief Блокирующее чтение заданного количества байт из Serial
 *        с учётом таймаута (timeout_ms).
 * 
 * @param ser       Ссылка на объект Serial (например, Serial1)
 * @param buffer    Указатель на буфер, куда записывать считанные байты
 * @param length    Количество байт, которое необходимо прочитать
 * @param timeout_ms Максимальное время ожидания в миллисекундах (по умолчанию 500 мс)
 * @return true, если все байты успешно прочитаны, иначе false
 */
bool readBytesWithTimeout(HardwareSerial &ser, uint8_t *buffer, size_t length, uint32_t timeout_ms = 500) {
  uint32_t start = millis();
  size_t count = 0;

  while (count < length) {
    if (ser.available()) {
      buffer[count++] = ser.read();
    }
    if (millis() - start > timeout_ms) {
      return false;  // Истёк таймаут, данные не успели прийти
    }
  }
  return true;
}

/**
 * @brief Ожидание появления в потоке UART специфического заголовка лидара.
 *        Заголовок определён в массиве LIDAR_HEADER.
 * 
 * @param ser Ссылка на Serial (обычно Serial1)
 * @return true, если заголовок найден, иначе false (по таймауту)
 */
bool waitForHeader(HardwareSerial &ser) {
  uint8_t matchPos = 0;
  uint32_t start = millis();

  // Пытаемся «выровнять» поток байт на заголовок (4 байта)
  while (true) {
    if (ser.available()) {
      uint8_t b = ser.read();
      if (b == LIDAR_HEADER[matchPos]) {
        // Совпало очередное ожидаемое значение заголовка
        matchPos++;
        if (matchPos == LIDAR_HEADER_LEN) {
          // Все байты заголовка совпали
          return true;
        }
      } else {
        // Сброс, если последовательность прервалась
        matchPos = 0;
      }
    }
    // Если долго не приходит корректный заголовок, выходим с false
    if (millis() - start > 100) {
      return false;
    }
  }
}

/**
 * @brief Преобразует «rawAngle» из пакета (двухбайтовое значение)
 *        в угол в градусах [0..360).
 * 
 * Формула для данного конкретного лидара: angle = (rawAngle - 0xA000) / 64
 * 
 * @param rawAngle Сырой угол (число 16 бит из пакета лидара)
 * @return Угол в градусах, нормализованный в диапазон [0..360)
 */
float decodeAngle(uint16_t rawAngle) {
  float angleDeg = (float)(rawAngle - 0xA000) / 64.0f;
  while (angleDeg < 0) angleDeg += 360.0f;
  while (angleDeg >= 360) angleDeg -= 360.0f;
  return angleDeg;
}

/**
 * @brief Определяет, к какому сектору [0..11] относится данный угол.
 *        Один сектор = 30°, но для удобства «нулевой» сектор
 *        смещён так, что его диапазон ~ 345..15 градусов.
 * 
 * @param angleDeg Угол в градусах [0..360)
 * @return Индекс сектора [0..11]
 */
int angleToSector(float angleDeg) {
  // Сдвигаем угол на +15°, чтобы 0-й сектор приходился примерно на зону 345..15
  float shifted = angleDeg + 15.0f;
  while (shifted < 0) shifted += 360.0f;
  while (shifted >= 360) shifted -= 360.0f;

  // Делим на 30°, получаем индекс сектора
  int sector = (int)(shifted / 30.0f) % NUM_SECTORS;
  sector = (sector + SECTOR_OFFSET) % NUM_SECTORS;
  return sector;
}

/********************************************************
 *  ОСНОВНАЯ ФУНКЦИЯ ПАРСИНГА И ОБРАБОТКИ ДАННЫХ ЛИДАРА
 ********************************************************/

/**
 * @brief Считывает один пакет данных из лидара, обновляет глобальные
 *        массивы расстояний, а также управляет цветом светодиодов.
 * 
 * @return true, если пакет обработан успешно, false — при ошибке чтения
 */
bool parseAndProcessPacket() {
  // 1) Дожидаемся заголовка лидара
  if (!waitForHeader(Serial1)) {
    return false;  // Заголовок не найден, пропускаем
  }

  // 2) Считываем тело пакета из 32 байт
  uint8_t buffer[LIDAR_BODY_LEN];
  if (!readBytesWithTimeout(Serial1, buffer, LIDAR_BODY_LEN, 500)) {
    Serial.println("Failed to read 32 bytes");
    return false;
  }

  // 3) Извлекаем общие данные пакета
  //    (Например, скорость вращения, углы начала и конца)
  uint16_t rotationSpeedTmp = buffer[0] | (buffer[1] << 8);
  float rpm = (float)rotationSpeedTmp / 64.0f;

  uint16_t startAngleTmp = buffer[2] | (buffer[3] << 8);
  float startAngleDeg = decodeAngle(startAngleTmp);

  // Данные о расстояниях и интенсивностях занимают 8 групп по 3 байта
  uint8_t offset = 4;
  uint16_t distances[8];
  uint8_t intensities[8];
  for (int i = 0; i < 8; i++) {
    distances[i] = (buffer[offset] | (buffer[offset + 1] << 8));
    intensities[i] = buffer[offset + 2];
    offset += 3;
  }

  // В самом конце пакета — «конечный угол» (2 байта)
  uint16_t endAngleTmp = buffer[offset] | (buffer[offset + 1] << 8);
  float endAngleDeg = decodeAngle(endAngleTmp);

  // Если конечный угол «меньше» начального, то добавим 360
  if (endAngleDeg < startAngleDeg) {
    endAngleDeg += 360.0f;
  }

  // 4) Вычисляем углы для всех 8 точек внутри пакета
  float angleRange = endAngleDeg - startAngleDeg;
  float angleInc = angleRange / 8.0f;
  float packetAngles[8];
  for (int i = 0; i < 8; i++) {
    float angle = startAngleDeg + i * angleInc;
    // Нормализуем в [0..360)
    while (angle < 0) angle += 360.0f;
    while (angle >= 360) angle -= 360.0f;
    packetAngles[i] = angle;
  }

  // 5) Создаём в ременный массив, где соберём минимальные расстояния по секторам
  //    (для 8 точек, что пришли в пакете).
  float tempSectorMin[NUM_SECTORS];
  for (int s = 0; s < NUM_SECTORS; s++) {
    tempSectorMin[s] = NO_VALUE;  // Изначально никаких данных
  }

  // 6) Проходим по всем 8 точкам пакета, фильтруя по интенсивности
  for (int i = 0; i < 8; i++) {
    if (intensities[i] > 15) {
      int sectorIndex = angleToSector(packetAngles[i]);
      float dist = (float)distances[i];
      // Запоминаем минимальное расстояние на сектор
      if (dist < tempSectorMin[sectorIndex]) {
        tempSectorMin[sectorIndex] = dist;
      }
    }
  }

  // 7) Обновляем глобальный массив расстояний и время их обновления
  uint32_t now = millis();
  for (int s = 0; s < NUM_SECTORS; s++) {
    if (tempSectorMin[s] != NO_VALUE) {
      sectorDistances[s] = tempSectorMin[s];
      sectorUpdateTime[s] = now;
    }
  }

  // 8) Если сектор не обновлялся более 500 мс, считаем, что данных по нему нет
  for (int s = 0; s < NUM_SECTORS; s++) {
    if ((now - sectorUpdateTime[s]) > 500) {
      sectorDistances[s] = NO_VALUE;
    }
  }

  // 9) Обработка «залипания» красного: если новое расстояние < ALARM_DIST,
  //    продлеваем время «AlarmUntil».
  for (int s = 0; s < NUM_SECTORS; s++) {
    float dist = sectorDistances[s];
    if (dist != NO_VALUE && dist < ALARM_DIST) {
      sectorAlarmUntil[s] = now + ALARM_HOLD_MS;
    }
  }

  // 10) Раскраска светодиодов в зависимости от дистанции и/или «залипания»
  for (int s = 0; s < NUM_SECTORS; s++) {
    float dist = sectorDistances[s];
    uint32_t color;

    if (dist == NO_VALUE) {
      // Если нет данных, сделаем (для примера) зелёный.
      // Если нужно выключать, то заменить на strip.Color(0, 0, 0).
      color = strip.Color(0, 255, 0);
    } else {
      // Смотрим, не активен ли режим «залипания» красного
      if (millis() < sectorAlarmUntil[s]) {
        // Держим красный цвет
        color = strip.Color(255, 0, 0);
      } else {
        // Обычная логика: <WARNING_DIST = жёлтый, иначе зелёный
        if (dist < WARNING_DIST) {
          color = strip.Color(255, 255, 0);  // Жёлтый
        } else {
          color = strip.Color(0, 255, 0);  // Зелёный
        }
      }
    }
    strip.setPixelColor(s, color);
  }

  // 11) Отправляем данные на ленту
  strip.show();

  //12) (Опционально) выводим отладочную информацию через Serial
  // Serial.print("RPM=");
  // Serial.print(rpm);
  // Serial.print("  sectorDistances: ");
  // for (int s = 0; s < NUM_SECTORS; s++) {
  //   Serial.print(sectorDistances[s]);
  //   Serial.print(", ");
  // }
  // Serial.println();
  // 13) Передаем статусы секторов по UART в виде строки
  //     "SECTORS: 0 1 2 ..." (0=зелёный, 1=жёлтый, 2=красный)
  Serial.print("SECTORS: ");
  for (int s = 0; s < NUM_SECTORS; s++) {
    int sectorStatus;
    float dist = sectorDistances[s];

    if (dist == NO_VALUE) {
      // Нет данных – по логике кода светится зелёный, значит статус = 0
      sectorStatus = 0;
    } else if (millis() < sectorAlarmUntil[s]) {
      // «Залипание» в красном
      sectorStatus = 2;
    } else if (dist < WARNING_DIST) {
      // Жёлтый
      sectorStatus = 1;
    } else {
      // Зелёный
      sectorStatus = 0;
    }

    Serial.print(sectorStatus);
    if (s < (NUM_SECTORS - 1)) {
      Serial.print(" ");
    }
  }
  Serial.println();

  return true;  // Пакет успешно обработан
}

void setup() {
  // Инициализация Serial для вывода отладочных сообщений в консоль
  Serial.begin(BAUDRATE);

  // Инициализация Serial1 для чтения данных лидара (указать пины RX/TX)
  Serial1.begin(BAUDRATE, SERIAL_8N1, LIDAR_RX_PIN, LIDAR_TX_PIN);

  // Инициализация Serial2 для передачи
  UART2.begin(BAUDRATE, SERIAL_8N1, UART_RX_PIN, UART_TX_PIN);

  // Инициализация адресной ленты светодиодов
  strip.begin();
  strip.show();             // Сразу гасим/обновляем ленту
  strip.setBrightness(50);  // При необходимости настроить яркость [0..255]

  Serial.println("ESP32 Lidar parser.");
}

void loop() {
  // В основном цикле просто пытаемся считать и обработать пакет
  parseAndProcessPacket();

  static unsigned long lastSendMillis = 0;
  if (millis() - lastSendMillis >= STATUS_PRINT_INTERVAL) {
    lastSendMillis = millis();

    UART2.print("SECTORS: ");
    for (int s = 0; s < NUM_SECTORS; s++) {
      int sectorStatus;
      float dist = sectorDistances[s];

      if (dist == NO_VALUE) {
        sectorStatus = 0;
      } else if (millis() < sectorAlarmUntil[s]) {
        sectorStatus = 2;
      } else if (dist < WARNING_DIST) {
        sectorStatus = 1;
      } else {
        sectorStatus = 0;
      }

      UART2.print(sectorStatus);
      if (s < (NUM_SECTORS - 1)) UART2.print(",");
    }
    UART2.println();
  }
}

Далее приведено объяснение принципов работы данного кода, а также раскрыты некоторые нюансы, которые могут оказаться неочевидными на первый взгляд. Важно понимать логику всего процесса: от получения пакета с данными из лидара до вывода цветовой индикации на 12-светодиодное кольцо и передачи статусов секторов во внешнее устройство (Arduino или другое).

• Основная задача — непрерывно считывать пакеты от лидара по аппаратному порту Serial1, определять расстояние до препятствий в разных угловых секторах, визуализировать эти данные цветами на светодиодном кольце и передавать упрощённую информацию дальше по UART2.

• Функция loop():

  1. Вызывает parseAndProcessPacket() — пытается прочитать и обработать один пакет лидара.

  2. Периодически (каждые 100 мс) формирует строку статусов секторов и отправляет её через UART2.

Константы и параметры

#define NUM_SECTORS 12       // Разбиваем полный обзор (360°) на 12 секторов
#define LED_PIN 17           // Пин, к которому подключено кольцо NeoPixel
#define LED_COUNT NUM_SECTORS
#define LED_TYPE (NEO_GRB + NEO_KHZ800)

#define LIDAR_RX_PIN 18      // RX вход ESP32 для чтения лидара
#define LIDAR_TX_PIN 19      // TX выход ESP32 (обычно не используется для лидара)
#define BAUDRATE 115200

HardwareSerial UART2(2);     // Второй аппаратный порт для передачи статусов
#define UART_TX_PIN 16
#define UART_RX_PIN 4
...
#define ALARM_DIST 400       // Менее 400 мм — красная зона
#define WARNING_DIST 650     // Менее 650 мм, но >= 400 мм — жёлтая зона
#define ALARM_HOLD_MS 300    // Время «залипания» красного (мс)
#define SECTOR_OFFSET 1      // Сдвиг «нулевого» сектора

static const float NO_VALUE = 99999.0f;  // «Нет данных» для сектора

• NUM_SECTORS = 12: кольцо из 12 светодиодов соответствует углам от 0 до 360° с шагом 30°.

• ALARM_DIST, WARNING_DIST: пороги для подсветки (красная, жёлтая, зелёная зоны).

• ALARM_HOLD_MS: если сектор обнаружил дистанцию меньше ALARM_DIST, он некоторое время (300 мс) будет оставаться в красном, даже если данные при следующем цикле приходят уже безопасными. Это необходимо, чтобы подтверждать наличие препятствия перед лидаром

• SECTOR_OFFSET: если физически нулевой светодиод не совпадает с «нулевым» градусом, мы можем сместить индексы секторов.

Глобальные массивы

static float sectorDistances[NUM_SECTORS] = { 0.0f };
static uint32_t sectorUpdateTime[NUM_SECTORS] = { 0 };
static uint32_t sectorAlarmUntil[NUM_SECTORS] = { 0 };

1. sectorDistances: текущее измеренное расстояние в миллиметрах в каждом из 12 секторов. Если «нет данных», оно хранит NO_VALUE.

2. sectorUpdateTime: когда (по millis()) в последний раз мы обновляли данные сектора. Если прошло более 500 мс без обновления, предполагаем, что данных нет (сектор снова становится NO_VALUE).

3. sectorAlarmUntil: время (также в миллисекундах), до которого сектор должен оставаться в «тревожном» (красном) состоянии.

Подготовка портов в setup()

void setup() {
  Serial.begin(BAUDRATE);              // Отладочный порт
  Serial1.begin(BAUDRATE, SERIAL_8N1, LIDAR_RX_PIN, LIDAR_TX_PIN);  // Лидар
  UART2.begin(BAUDRATE, SERIAL_8N1, UART_RX_PIN, UART_TX_PIN);      // Передача во внешний МК

  strip.begin();
  strip.show();
  strip.setBrightness(50); // Установка яркости (0..255)

  Serial.println("ESP32 Lidar parser.");
}

• Serial (USB) используется только для отладочных сообщений в консоли (можно смотреть в Serial Monitor).

• Serial1 (RX на пине 18) получает данные лидара на скорости 115200 бод.

• UART2 (TX на пине 16) отправляет информацию о секторах. Это удобно, чтобы ESP32 мог сообщать Ардуино или другой плате, какие сектора «опасны».

Основной цикл loop()

void loop() {
  parseAndProcessPacket();

  static unsigned long lastSendMillis = 0;
  if (millis() - lastSendMillis >= STATUS_PRINT_INTERVAL) {
    lastSendMillis = millis();

    UART2.print("SECTORS: ");
    for (int s = 0; s < NUM_SECTORS; s++) {
      int sectorStatus;
      float dist = sectorDistances[s];

      if (dist == NO_VALUE) {
        sectorStatus = 0;
      } else if (millis() < sectorAlarmUntil[s]) {
        sectorStatus = 2;
      } else if (dist < WARNING_DIST) {
        sectorStatus = 1;
      } else {
        sectorStatus = 0;
      }

      UART2.print(sectorStatus);
      if (s < (NUM_SECTORS - 1)) UART2.print(",");
    }
    UART2.println();
  }
}

1. parseAndProcessPacket(): главная функция считывания и обработки данных лидара.

2. Каждые 100 мс формируется строка "SECTORS: 0,1,0,2...", где для каждого сектора указывается статус (0=зелёный, 1=жёлтый, 2=красный). Отправляется в UART2.

Функции чтения и синхронизации

waitForHeader()

bool waitForHeader(HardwareSerial &ser) {
  uint8_t matchPos = 0;
  uint32_t start = millis();

  while (true) {
    if (ser.available()) {
      uint8_t b = ser.read();
      if (b == LIDAR_HEADER[matchPos]) {
        matchPos++;
        if (matchPos == LIDAR_HEADER_LEN) {
          return true;
        }
      } else {
        matchPos = 0;
      }
    }
    if (millis() - start > 100) {
      return false;
    }
  }
}

• Пытаемся «выровнять» поток байт на 4-байтовый заголовок: 0x55, 0xAA, 0x03, 0x08.

• Как только находим последовательность, считаем, что пакет лидара начинается (возвращаем true).

• В противном случае, если за 100 мс поток не совпал с заголовком, выходим с false.

Нюанс: поскольку лидар периодически передаёт пакеты, могут приходить «обрывки» данных. Мы ждём именно 4 байта заголовка, чтобы правильно синхронизироваться.

Парсинг и обработка пакета (parseAndProcessPacket())

1. Ожидание заголовка:

   if (!waitForHeader(Serial1)) {
     return false;
   }
   

2. Чтение 32 байт (тело пакета):

   uint8_t buffer[LIDAR_BODY_LEN];
   if (!readBytesWithTimeout(Serial1, buffer, LIDAR_BODY_LEN, 500)) {
     Serial.println("Failed to read 32 bytes");
     return false;
   }
   

3. Извлечение метаданных (скорость вращения, начальный и конечный углы):

   uint16_t rotationSpeedTmp = buffer[0] | (buffer[1] << 8);
   float rpm = (float)rotationSpeedTmp / 64.0f;
   
   uint16_t startAngleTmp = buffer[2] | (buffer[3] << 8);
   float startAngleDeg = decodeAngle(startAngleTmp);
   ...
   uint16_t endAngleTmp = buffer[offset] | (buffer[offset+1] << 8);
   float endAngleDeg = decodeAngle(endAngleTmp);
   if (endAngleDeg < startAngleDeg) {
     endAngleDeg += 360.0f;
   }
   

   • decodeAngle(): по спецификации конкретного лидара, угол определяется как (rawAngle - 0xA000) / 64. Затем делаем нормализацию (если вышли за 0..360).

4. Извлечение данных о 8 точках:

   // У нас есть 8 групп по 3 байта: distance(2 байта) + intensity(1 байт)
   uint16_t distances[8];
   uint8_t intensities[8];
   ...
   for (int i = 0; i < 8; i++) {
     distances[i]   = buffer[offset] | (buffer[offset+1] << 8);
     intensities[i] = buffer[offset+2];
     offset += 3;
   }
   

   • Если интенсивность отражённого сигнала низкая (<= 15), данные считаются «ненадёжными» и в дальнейшем игнорируются.

5. Вычисление углов каждой из 8 точек:

   float angleRange = endAngleDeg - startAngleDeg;
   float angleInc = angleRange / 8.0f;
   float packetAngles[8];
   for (int i = 0; i < 8; i++) {
     float angle = startAngleDeg + i * angleInc;
     // нормализация 0..360
     packetAngles[i] = angle;
   }
   

6. Сопоставление точек с секторами:

   float tempSectorMin[NUM_SECTORS];
   for (int s = 0; s < NUM_SECTORS; s++) {
     tempSectorMin[s] = NO_VALUE;
   }
   
   for (int i = 0; i < 8; i++) {
     if (intensities[i] > 15) {
       int sectorIndex = angleToSector(packetAngles[i]);
       float dist = (float)distances[i];
       if (dist < tempSectorMin[sectorIndex]) {
         tempSectorMin[sectorIndex] = dist;
       }
     }
   }
   

   • Функция angleToSector(angleDeg) добавляет +15° для смещения, затем делит на 30°, чтобы получить индекс [0..11]. Включён SECTOR_OFFSET (по умолчанию 1), чтобы физически «нулевой» светодиод не обязательно совпадал с 0°.

   • Приходящие расстояния для одного сектора могут быть от разных точек сканирования — мы берём минимальное расстояние. Так мы не пропустим самое ближнее препятствие.

7. Обновление глобального массива:

   uint32_t now = millis();
   for (int s = 0; s < NUM_SECTORS; s++) {
     if (tempSectorMin[s] != NO_VALUE) {
       sectorDistances[s] = tempSectorMin[s];
       sectorUpdateTime[s] = now;
     }
   }
   

8. Проверка «устаревших» данных:

   for (int s = 0; s < NUM_SECTORS; s++) {
     if ((now - sectorUpdateTime[s]) > 500) {
       sectorDistances[s] = NO_VALUE;
     }
   }
   

   • Если за последние 500 мс данных по сектору не приходило, мы считаем, что их нет (NO_VALUE). Это важно, поскольку лидар непрерывно вращается, и некоторые секторы могут на время «выпадать» (нет отражений, или затенение).

9. Обработка «залипания» красного:

   for (int s = 0; s < NUM_SECTORS; s++) {
     float dist = sectorDistances[s];
     if (dist != NO_VALUE && dist < ALARM_DIST) {
       sectorAlarmUntil[s] = now + ALARM_HOLD_MS;
     }
   }
   

   • Если расстояние < 400 мм, мы фиксируем сектор в красном состоянии на ближайшие 300 мс.

10. Раскраска светодиодов:

for (int s = 0; s < NUM_SECTORS; s++) {
  float dist = sectorDistances[s];
  uint32_t color;

  if (dist == NO_VALUE) {
    // Нет данных => (по умолчанию) зелёный
    color = strip.Color(0, 255, 0);
  } else {
    if (millis() < sectorAlarmUntil[s]) {
      // Красный, если есть «залипание»
      color = strip.Color(255, 0, 0);
    } else {
      // Иначе определяем цвет по порогам
      if (dist < WARNING_DIST) {
        // Жёлтый
        color = strip.Color(255, 255, 0);
      } else {
        // Зелёный
        color = strip.Color(0, 255, 0);
      }
    }
  }
  strip.setPixelColor(s, color);
}
strip.show();

• Важно отметить, что если NO_VALUE, мы условно выбираем зелёный (безопасный). Можно переопределить и гасить светодиод (strip.Color(0,0,0)), если нужно явно показывать «нет данных».

11. Передача статусов в Serial:

Serial.print("SECTORS: ");
for (int s = 0; s < NUM_SECTORS; s++) {
  int sectorStatus;
  float dist = sectorDistances[s];

  if (dist == NO_VALUE) {
    sectorStatus = 0;          // Зелёный
  } else if (millis() < sectorAlarmUntil[s]) {
    sectorStatus = 2;          // Красный
  } else if (dist < WARNING_DIST) {
    sectorStatus = 1;          // Жёлтый
  } else {
    sectorStatus = 0;          // Зелёный
  }
  Serial.print(sectorStatus);
  if (s < (NUM_SECTORS - 1)) {
    Serial.print(" ");
  }
}
Serial.println();

• Эта строка "SECTORS: ..." позволяет наблюдать статусы в реальном времени в Serial Monitor (подключенной по USB платы ESP32).

8. Периодическая передача в UART2

Помимо вывода в Serial, код каждые 100 мс формирует похожую строку для UART2:

if (millis() - lastSendMillis >= STATUS_PRINT_INTERVAL) {
  lastSendMillis = millis();

  UART2.print("SECTORS: ");
  ...
  UART2.println();
}

• На другом микроконтроллере (например, Arduino Uno через SoftwareSerial) мы можем парсить эту строку и понимать, какие сектора красные, жёлтые или зелёные.

Важные нюансы и «подводные камни»

1. Аппаратный порт ESP32 для лидара:

   • Используется Serial1 на пинах 18 (RX) и 19 (TX). Важно не путать с Serial (USB) или Serial2.

   • Скорость 115200 бод для лидара. ESP32 поддерживают эту скорость без проблем.

2. Стабильное питание лидара:

   • Лидару обычно требуется 5 В, а не 3.3 В. Нужно убедиться, что питание и уровни UART совместимы. Часто сам лидар имеет встроенный преобразователь UART/3.3 В, но это зависит от конкретной модели.

3. Интенсивность > 15:

   • Код выбрасывает данные с интенсивностью ≤ 15 (считая, что это «шум» или некачественное измерение). Если ваш лидар даёт другие значения интенсивности, возможно, потребуется подбирать этот порог.

4. Смещение сектора SECTOR_OFFSET:

   • Важно для того, чтобы «0-й сектор» на кольце соответствовал тому углу, где физически располагается передняя часть робота. Если вы хотите, чтобы сектор 0 смотрел вперёд, вам нужно найти экспериментально, какой SECTOR_OFFSET даёт совпадение по фактическому углу.

5. «Залипание» красного (ALARM_HOLD_MS):

   • Делает систему более надёжной в плане индикации: если красная зона мгновенно пропала, мы ещё сохраняем её на 300 мс, чтобы оператор (или другой микроконтроллер) успел отреагировать.

   • При желании время можно увеличить/уменьшить.

Код на Arduino Uno: приём статусов секторов

Простейший код на Arduino Uno слушает порт SoftwareSerial на пинах 6 (RX) и 3 (TX) (TX в данном случае не обязательно использовать). При получении строки формата "SECTORS: 0,1,0,2..." вызывается функция parseSectors(), которая разбирает числа и складывает их в массив sectorStatusArray. Далее мы просто печатаем их в Serial.

Если потребуется какая-то логика реагирования (например, задействовать моторы для объезда препятствия), всё, что нужно – обратиться к sectorStatusArray и анализировать какие сектора красные (2) или жёлтые (1), чтобы соответствующим образом менять движение.

Код на GitHub

#include <SoftwareSerial.h>

// Пины SoftwareSerial (RX, TX)
#define ESP_RX_PIN 6  // RX Arduino (подключаем к TX GPIO16 ESP32)
#define ESP_TX_PIN 3  // TX Arduino (если нужен обратный канал)

SoftwareSerial espSerial(ESP_RX_PIN, ESP_TX_PIN);

#define NUM_SECTORS 12
int sectorStatusArray[NUM_SECTORS];

// Парсим строку вида "SECTORS: 0,1,2,..."
bool parseSectors(String input, int sectors[]) {
  input.trim();

  if (!input.startsWith("SECTORS: ")) return false;

  input = input.substring(9);  // Убираем префикс "SECTORS: "

  for (int i = 0; i < NUM_SECTORS; i++) {
    int delimiter = input.indexOf(',');

    String token;

    if (delimiter == -1 && i < NUM_SECTORS - 1) return false;  // ошибка формата

    if (delimiter != -1) {
      token = input.substring(0, delimiter);
      input = input.substring(delimiter + 1);
    } else {
      token = input;
    }

    token.trim();
    sectors[i] = token.toInt();
  }

  return true;
}

void setup() {
  Serial.begin(115200);  // монитор порта для отладки
  espSerial.begin(115200); // та же скорость, что и у ESP
  Serial.println("Arduino ready.");
}

void loop() {
  if (espSerial.available()) {
    String line = espSerial.readStringUntil('n');
    if (parseSectors(line, sectorStatusArray)) {
      Serial.print("Sectors received: ");
      for (int i = 0; i < NUM_SECTORS; i++) {
        Serial.print(sectorStatusArray[i]);
        Serial.print(" ");
      }
      Serial.println();
    } else {
      Serial.println("Invalid data format");
    }
  }
}

• Arduino здесь выступает в роли «приёмника» информации о секторах.

• ESP32 передаёт строку с текстом "SECTORS: ..." по линии UART (с аппаратного UART2).

• Arduino считывает эту строку через SoftwareSerial (поскольку на Uno ограниченное число аппаратных UART) и парсит её.

• Впоследствии разработчик может использовать расшифрованные данные sectorStatusArray для принятия решений (например, остановить мотор при красном секторе).

Подключения и основные константы

#include <SoftwareSerial.h>

// Пины SoftwareSerial (RX, TX)
#define ESP_RX_PIN 6
#define ESP_TX_PIN 3

SoftwareSerial espSerial(ESP_RX_PIN, ESP_TX_PIN);

#define NUM_SECTORS 12
int sectorStatusArray[NUM_SECTORS];

1. ESP_RX_PIN = 6: это RX для Arduino, сюда должен подаваться сигнал TX с ESP32 (GPIO16).

2. ESP_TX_PIN = 3: это TX для Arduino, идущий (при необходимости) на RX ESP32 (GPIO4), но в данном примере оно обычно не используется.

3. NUM_SECTORS = 12: общее количество «секторов», соответствующее кольцу из 12 светодиодов на ESP32.

4. sectorStatusArray: в этот массив Arduino складывает полученные статусы секторов (0, 1 или 2).

Настройка портов в setup()

void setup() {
  Serial.begin(115200);  // монитор порта для отладки
  espSerial.begin(115200); // та же скорость, что и у ESP
  Serial.println("Arduino ready.");
}

• Serial.begin(115200): вывод отладочных сообщений в Serial Monitor (через USB).

• espSerial.begin(115200): инициализация SoftwareSerial, чтобы слушать данные от ESP32 со скоростью 115200 бод.

  Важный нюанс: не на всех платах Arduino Uno SoftwareSerial стабильно работает на такой высокой скорости. Для теста может быть достаточно, но если возникают потери данных, использовать плату с аппаратным UART (например, Arduino Mega).

Основной цикл loop()

void loop() {
  if (espSerial.available()) {
    String line = espSerial.readStringUntil('n');
    if (parseSectors(line, sectorStatusArray)) {
      Serial.print("Sectors received: ");
      for (int i = 0; i < NUM_SECTORS; i++) {
        Serial.print(sectorStatusArray[i]);
        Serial.print(" ");
      }
      Serial.println();
    } else {
      Serial.println("Invalid data format");
    }
  }
}

1. espSerial.available(): проверяет, есть ли входящие байты в программном UART.

2. espSerial.readStringUntil('n'): считывает всю строку (до символа перевода строки n).

3. parseSectors(line, sectorStatusArray): вызываем функцию разбора строки.

   • Если функция вернула true, значит формат строки корректный, и массив sectorStatusArray заполнен числами.

   • Если false, значит строка пришла в неверном формате.

4. При успешном парсинге Arduino выводит в Serial (USB) строку:

   Sectors received: 0 1 2 0 0 0 ...
   

   Соответственно, можно наблюдать в Serial Monitor, какие статусы пришли.

Функция parseSectors()

bool parseSectors(String input, int sectors[]) {
  input.trim();

  if (!input.startsWith("SECTORS: ")) return false;

  input = input.substring(9);  // Убираем префикс "SECTORS: "

  for (int i = 0; i < NUM_SECTORS; i++) {
    int delimiter = input.indexOf(',');

    String token;

    if (delimiter == -1 && i < NUM_SECTORS - 1) return false;  // ошибка формата

    if (delimiter != -1) {
      token = input.substring(0, delimiter);
      input = input.substring(delimiter + 1);
    } else {
      token = input;
    }

    token.trim();
    sectors[i] = token.toInt();
  }

  return true;
}

1. input.trim(): удаляем пробелы и символы перевода строки в начале и конце.

2. if (!input.startsWith("SECTORS: ")) return false;: проверяем, что строка начинается с "SECTORS: ". Иначе формат некорректный.

3. input.substring(9): убираем первые 9 символов ("SECTORS: "). Остаётся что-то вроде "0,1,2,0,2,0,...".

4. Цикл по i от 0 до NUM_SECTORS - 1:

   • Ищем запятую , методом input.indexOf(',').

   • Если запятая не найдена, а мы ещё не дошли до последнего сектора, считаем, что формат неверен (возвращаем false).

   • Если запятая найдена, берём подстроку от начала строки до запятой (token) и потом «срезаем» из input эту часть.

   • token.trim() — убираем лишние пробелы.

   • sectors[i] = token.toInt(); — преобразуем строку в целое число (0, 1 или 2).

5. По окончании цикла, если всё прошло гладко, возвращаем true.

Что делать с sectorStatusArray

После успешного парсинга в loop() у нас есть массив sectorStatusArray, состоящий из 12 чисел (каждое 0, 1 или 2). По умолчанию код делает только вывод в Serial:

Serial.print("Sectors received: ");
for (int i = 0; i < NUM_SECTORS; i++) {
  Serial.print(sectorStatusArray[i]);
  Serial.print(" ");
}
Serial.println();

• 0 = «зелёный» сектор (безопасно),

• 1 = «жёлтый» сектор (предупреждение),

• 2 = «красный» сектор (опасно).

При разработке робота можете добавить любую логику:

• Если любой сектор красный → остановить мотор или поехать обратно.

• Если сектор жёлтый → снизить скорость робота.

• И т. д.

Расширение: продолжение работы на том же ESP или одноплатном компьютере

В статье показан самый наглядный пример, когда ESP32 только считывает лидар и передаёт статусы секторов дальше, а логику принятия решений мы делегируем другим устройствам (Arduino или любому другому). Однако возможно реализовать всю систему целиком на ESP32 (или на другом современном микроконтроллере/одноплатнике), лишь расширив прошивку кодом, управляющим моторами, датчиками и т.д. Подобная гибкость позволяет интегрировать лидар в любую среду — важно лишь корректно разобраться с протоколом и один раз написать парсер.

Практическая значимость и участие в соревнованиях

Соревнования Евробот уже много лет предоставляют площадку для испытаний автономных и управляемых роботов. Однако в России большая часть команд делает на юниорскую лигу с дистанционным управлением, тогда как полноценные автономные платформы часто вызывают опасения у начинающих команд. Одно из главных препятствий на пути к автономности – необходимость надёжной системы предотвращения столкновений (множество датчиков или сложность интеграции лидара). Традиционные подходы (ROS, дополнительные платы, сложный код) нередко отпугивают молодых участников.

Одна из задач, для которой был создан данный проект: снять страх перед использованием «сложных» систем и продемонстрировать, как при помощи недорогого лидара от робота-пылесоса и микроконтроллера ESP32 можно быстро решить задачу обнаружения препятствий.

Дополнительно, чтобы облегчить включение лидара в проект, я спроектировал для него корпус, соответствующий регламенту Евробота. Файлы для 3D-печати доступны в репозитории на GitHub: они помогут удобно закрепить лидар на роботе и сохранить компактность конструкции в рамках регламента соревнований.

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