В этой статье объясняется, как определить местоположение объекта, используя Arduino, два ультразвуковых датчика и формулу Герона для треугольников. Измерение положения производится без механического вращения дальномеров.
Формула Герона позволяет рассчитать площадь треугольника, для которого известны длины всех сторон. Зная площадь треугольника, можно рассчитать положение отдельного объекта (относительно известной базовой линии), используя тригонометрию и теорему Пифагора.
Большие области обнаружения и хорошая точность возможны при использовании общедоступных ультразвуковых датчиков на подобии HC-SR04 или HY-SRF05.
В собранном виде это выглядит так:
Для эксперимента на полу с помощью верёвки и клейкой ленты делаем квадрат со сторонами 100см. От квадрата на расстоянии 50см размещаем дальномеры. К крышке от пластиковой ёмкости клейкой лентой прикрепляем нитку, за которую будет удобно тягать крышку.
Визуализация положения объекта сделана на Processing, объект отображается красным мигающим кругом:
Вот видео, как это всё работает:
На дальномере "B" нужно заклеить излучатель. Это можно сделать с помощью изоленты или скотча, подложив под него бумагу или наклеив его в несколько слоёв, чтобы обеспечить блокировку излучения ультразвука. В противном случае излучаемый ультразвук с этого дальномера будет мешать.
Основные компоненты - это одна Arduino Uno и два ультразвуковых дальномера на подобии HC-SR04 (в данном случае используется HY-SRF05) и немного проводов. Что бы не паять провода на прямую, используются штыревые соединения pls и pbs.
На этом изображении показаны диаграммы перекрытия лучей для дальномеров. Дальномер A будет получать отражённое эхо от любого объекта в розовом треугольнике. Датчик B будет получать только отраженное эхо от излучателя дальномера A. Определять положение объекта можно если объект находится примерно в зоне пересечения обоих треугольников. Что бы обеспечить большую рабочую зону, нужно расположить оба датчика достаточно далеко.
Площадь любого треугольника можно рассчитать по формуле:
площадь = основание * высота / 2Сделав перестановку, получаем:
высота = площадь * 2 / основаниеПока все хорошо... но как рассчитать площадь?
Ответ заключается в размещении двух ультразвуковых преобразователей на известном расстоянии друг от друга (базовая линия) и измерении расстояния, на котором каждый датчик находится от объекта, с помощью ультразвука.
Датчик А посылает импульс, который отражается от объекта во всех направлениях и попадает на приёмники обоих дальномеров. У дальномера B заклеен излучатель и его импульс блокируется.
Обратный путь излучённого сигнала к дальномеру A показан красным. При делении на два пройденного звуковой волной расстояния учете скорости звука (в микросекундах) мы можем вычислить расстояние (в сантиметрах) d1 по следующей формуле:
d1 = время / 59Путь к приёмнику B показан синим цветом. Если вычесть расстояние d1 из этой длины пути, мы получим расстояние d2. Формула для расчета:
d2 = время / 29,5 - d1Теперь у нас есть длина всех трех сторон треугольника.
Значение 59 для константы получается следующим образом. Скорость звука составляет приблизительно 340 м/с. Переведя в сантиметры/микросекунды получаем 0,034 см/мкс. 0,034 см/мкс это 29,412 мкс/см. Умножив 29,412 на 2, получаем 58,824 или округлив 59. Для получения большей точности это значение нужно немного скорректировать, учитывая параметры среды (температура воздуха, влажности и т.д.).
При вычислении d2 используется число 29,5, т.к. нужно учитывать не полный путь от дальномера и обратно, а только от объекта до дальномера.
В формуле Герона используется полупериметр, который вычисляется по формуле:
s = (a + b + c) / 2Теперь площадь можно рассчитать по следующей формуле:
площадь = sqrt(s*(s-a)*(s-b)*(s-c))Зная площадь, можно вычислить высоту (координата Y).
Координата X может быть рассчитана, если провести перпендикуляр из вершины треугольника к базовой линии, чтобы получить прямоугольный треугольник. В таком случае координата X может быть рассчитана с использованием следующей формулы:
c1 = sqrt (b2 - h2)Для загрузки прошивки в Arduino понадобиться Arduino IDE, которую можно скачать с официального сайта по следующей ссылке. Загрузите и установите Arduino IDE, если она еще не установлена.
Для визуализации используется приложение Processing, скачать его можно по следующей ссылке. Запустив среду Processing можно переключить языка интерфейса на русский. Для этого в меню нажмите "File" и затем " Preferences…"
Откроется окно, в котором можно сменить язык интерфейса:
В выпадающем списке выбираете язык и затем внизу окна нажимаете "Ok". Подключите Arduino Uno к компьютеру. Скачайте скетч dual_sensor _echo_locator.ino, откройте его в Arduino Ide и загрузите в плату Arduino Uno. После прошивки среда Ardino IDE больше не нужна, можете закрыть её. Только оставьте плату Arduino Uno подключенной к компьютеру.
Скачайте скетч dual_sensor_echo_locator.pde, откройте его в Processing и нажав в верху кнопку "Выполнить", запустите.
Если всё работает правильно, в консоли будут отображаться данные, поступающие от платы Arduino.
Если в коде установлен неправильный номер ком-порта, при запуске скетча в консоли будет выведен список всех доступных на данный момент ком-портов:
В квадратных скобках указывается индекс ком-порта, далее в кавычках идёт название. В скетче есть строка:
myPort = new Serial(this, Serial.list()[0], Baud_rate);В квадратных скобках нужно вписать индекс порта, к которому подключена Arduino. К примеру подключена Arduino не к "COM1", а к "COM8". В данном случае это индекс 1, соответственно 1 и вписываем:
myPort = new Serial(this, Serial.list()[1], Baud_rate);Размещаем датчики на расстоянии 100 см друг от друга и на расстоянии 50см от квадрата (метр на метр) из верёвки.
Датчики ориентируются не параллельно стороне квадрата, а под углом - поверните оба датчика примерно к диагонально противоположным углам у квадрата. При вращении датчиков Вы обнаружите положение, в котором на графическом дисплее появляется мигающая красная точка.