Ультразвуковые дальномеры(US) — Эхолокация для роботов

Ультразвуковые датчики (сонары) работают по принципу эхолокации, подобно летучим мышам или дельфинам. Это один из самых доступных и понятных способов измерения расстояния в робототехнике.

\[ d = \frac{v_{\text{sound}}(T) \cdot \Delta t}{2} \] где:

• \(d\) — расстояние до объекта (м)
• \(v_{\text{sound}}(T)\) — скорость звука при температуре \(T\) (м/с)
• \(\Delta t\) — время между отправкой импульса и приемом эха (с)

\[ v_{\text{sound}}(T) = 331.3 \cdot \sqrt{1 + \frac{T}{273.15}} \] где \(T\) — температура воздуха в °C.

Таблица скоростей:

УЗ датчики имеют конусообразную зону чувствительности:

• HC-SR04: ~15-30° (полный угол)
• Чем уже конус → точнее определение направления
• Чем шире конус → больше охват, но выше вероятность ложных срабатываний

\[ d_{\min} \approx \frac{v_{\text{sound}} \cdot t_{\text{ringdown}}}{2} \] где \(t_{\text{ringdown}}\) — время затухания пьезоэлемента (~200 мкс для HC-SR04).

Для HC-SR04: \(d_{\min} \approx 0.0343 \text{ м/мкс} \times 200 \text{ мкс} / 2 \approx 3.4 \text{ см}\)

\[ d_{\max} = \frac{v_{\text{sound}} \cdot t_{\text{timeout}}}{2} \] где \(t_{\text{timeout}}\) — время ожидания эха в датчике.

HC-SR04: \(t_{\text{timeout}} \approx 38 \text{ мс} \Rightarrow d_{\max} \approx 6.5 \text{ м}\)

HC-SR04 → Arduino
  VCC   → 5V
  Trig  → D9
  Echo  → D10 (через делитель 5V→3.3V при необходимости)
  GND   → GND

class UltrasonicSensor {
private:
    int trig_pin_;
    int echo_pin_;
    float temperature_C_;
    
    float speedOfSound(float temp_C) {
        return 331.3 * sqrt(1.0 + temp_C / 273.15);
    }
    
public:
    UltrasonicSensor(int trig, int echo, float temp = 20.0)
        : trig_pin_(trig), echo_pin_(echo), temperature_C_(temp) {
        pinMode(trig_pin_, OUTPUT);
        pinMode(echo_pin_, INPUT);
    }
    
    float readDistance() {
        // Генерация импульса 10 мкс
        digitalWrite(trig_pin_, LOW);
        delayMicroseconds(2);
        digitalWrite(trig_pin_, HIGH);
        delayMicroseconds(10);
        digitalWrite(trig_pin_, LOW);
        
        // Измерение длительности эха
        long duration = pulseIn(echo_pin_, HIGH, 30000); // timeout 30 ms
        
        if (duration == 0) return NAN; // Эхо не вернулось
        
        // Расчет расстояния с температурной компенсацией
        float v_sound = speedOfSound(temperature_C_);
        float distance = v_sound * duration * 1e-6 / 2.0;
        
        // Фильтрация выбросов
        if (distance < 0.02 || distance > 4.0) return NAN;
        
        return distance; // метры
    }
    
    void setTemperature(float temp_C) {
        temperature_C_ = temp_C;
    }
};

US-100 → ESP32
  VCC  → 3.3V
  GND  → GND
  TX   → GPIO16 (RX)
  RX   → GPIO17 (TX)
  Mode → GND (для UART режима)

import serial
import struct

class US100Sensor:
    def __init__(self, port='/dev/ttyUSB0', baudrate=9600):
        self.ser = serial.Serial(port, baudrate, timeout=1)
        
    def read_distance(self):
        # Отправка команды измерения (0x55)
        self.ser.write(b'\x55')
        
        # Чтение результата (2 байта, big-endian)
        data = self.ser.read(2)
        if len(data) != 2:
            return None
            
        # Преобразование байтов в расстояние (мм)
        distance_mm = struct.unpack('>H', data)[0]
        
        # US-100 возвращает температуру если distance > 10000
        if distance_mm > 10000:
            temperature = (distance_mm - 10000) / 10.0
            return {'temperature': temperature}
        else:
            return {'distance': distance_mm / 1000.0}  # метры

Симптом: Датчик видит препятствие там, где его нет (отражение от пола/стен).

Решение: Использовать несколько датчиков и голосование: \[ d_{\text{final}} = \text{median}(d_1, d_2, d_3) \]

Принцип: Датчики слышат эхо от соседних сенсоров.

Решение: Последовательный опрос с задержкой: \[ \Delta t_{\min} = \frac{2 \cdot D_{\max}}{v_s} \] где \(D_{\max}\) — максимальная рабочая дальность.

Пример для HC-SR04: \(\Delta t_{\min} = \frac{2 \cdot 4}{343} \approx 23 \text{ мс}\)

Калибровочная кривая для HC-SR04 (эмпирическая): \[ d_{\text{corrected}} = a \cdot d_{\text{raw}}^2 + b \cdot d_{\text{raw}} + c \] где коэффициенты \(a, b, c\) определяются экспериментально.

// Простая система предотвращения столкновений
bool shouldStop(float distance, float safe_distance = 0.3) {
    if (isnan(distance)) return false; // Нет данных
    return distance < safe_distance;
}

Особенность: Звук хорошо отражается от жидкостей. Формула уровня: \[ h = H - \frac{v_{\text{sound}} \cdot \Delta t}{2} \] где \(H\) — высота бака, \(h\) — уровень жидкости.

Использование 3+ датчиков для определения положения объекта: \[ \begin{aligned} d_1^2 &= (x - x_1)^2 + (y - y_1)^2 \\ d_2^2 &= (x - x_2)^2 + (y - y_2)^2 \\ d_3^2 &= (x - x_3)^2 + (y - y_3)^2 \end{aligned} \] Решение системы дает координаты \((x, y)\) объекта.

Принцип: Микроэлектромеханические системы для создания фазированных решеток.

Преимущество: Узкий луч, электронное сканирование.

Идея: Использование нескольких частот (40 кГц, 120 кГц, 200 кГц).

Применение: Определение материала объекта по частотной характеристике эха.

Концепция: Массивы из сотен УЗ излучателей/приемников.

Разрешение: Несколько миллиметров на расстоянии до 1 м.

Цель: Измерить реальную диаграмму направленности HC-SR04.

Оборудование: HC-SR04, сервопривод, Arduino, линейка.

Протокол:

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

2. Поворачивайте датчик с шагом 5°

3. На каждом шаге измеряйте расстояние до фиксированного объекта

4. Постройте график: угол → измеренное расстояние

Ожидаемый результат: Колоколообразная кривая с максимумом при прямом угле.

Ультразвук — отличная отправная точка для изучения дальнометрии:

1. Инфракрасные дальномеры — узкий луч, но зависимость от цвета
2. Оптические ToF датчики — высокая точность, но чувствительность к свету
3. Лидары — полное 3D сканирование окружения
4. Сенсорная fusion — как объединить УЗ с другими сенсорами

Вывод: Ультразвуковые датчики — это “рабочие лошадки” робототехники: дешевые, надежные и универсальные. Они не самые точные и быстрые, но идеальны для базового обнаружения препятствий и измерений там, где другие технологии могут отказать.