12.05.2019 10:57
b__s__v 19 5100 Источник

Ультразвуковой GPS. Концептуальная модель


Перед тем как отправляться в столь долгое плавание стоит проверить, а так ли все реально сделать на коленке.

О чем эта статья: как быстро и недорого сделать простую ультразвуковую GPS.

Список необходимых устройств


  • HC-SR04 3 шт.
  • Arduino 1 шт.
  • Моток проводов.

Концепция


image
Рис. 1 – Общая идея устройства

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

Все это соединено по схеме:

image
Рис. 2 – Схема подключения устройств

И конечно же вы подключаете Arduino по USB к компьютеру.

Как это все работает:

  1. Измерить расстояние от приемника до излучателя 1
  2. Послать сигнал о начале измерения расстояния приемнику и излучателю 1 (дернуть им лапки Trig).
  3. Подождать пока приемник нам не выдаст длину.
  4. Повторить тоже самое для излучателя 2.
  5. Рассчитать координаты приемника.

Вспомним школьную геометрию


image
Рис. 3 – Геометрическое представление задачи

На основании данного рисунка составим формулы расчета координат:

image

A, F – высота излучателей относительно приемника;
С, Е – длины, полученные при измерении расстояния от излучателей до приемника;
G – расстояние между излучателями.

Практика


Установите два излучателя под потолок, на приличном расстоянии друг от друга (3 метра самое то) направьте их излучателями в одну точку, вокруг которой и будет сформирована ваша рабочая область.

Закрепите приемник на чем-нибудь плоском (например, кусок доски) и замотайте ему излучатель скотчем, чтобы нам не создавал лишнего ультразвукового зашумления в комнате.
Соедините все по схеме представленной выше.

Примерно вот так могут выглядеть крепежи для излучателей и подложка для приемника:

image
Рис. 4 – Вид модулей

Программа


Рассмотрим ключевые части кода подробнее.

Заставляем излучатель 1 и приемник начать измерение расстояния переведя вход Trig данных устройств из низкого состояния в высокое, на 10 микросекунд и обратно в низкое. 

  digitalWrite(trigPinRessiv, LOW);  digitalWrite(trigPinTransmit1, LOW);
  delayMicroseconds(5);
  digitalWrite(trigPinRessiv, HIGH);  digitalWrite(trigPinTransmit1, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPinRessiv, LOW);  digitalWrite(trigPinTransmit1, LOW);

Обычно данные устройства сами излучают УЗ сигнал и ждут пока он от чего то отразится, и придет обратно к ним. Но мы их обманываем, в нашем случае один посылает а другой получает, тот что получил думает что это его сигнал, хотя это сигнал другого устройства, и выдает нам расстояние до этого другого устройства.

Ждем пока датчик начнет нам сообщать длительность полета УЗ сигнала:

while (digitalRead(echoPinRessiv) == LOW);

Записываем время начала получения сигнала:

timeStartSignal = micros();

Ждем пока датчик не прекратит сообщать нам время пролета УЗ сигнала:

while (digitalRead(echoPinRessiv) == HIGH);

Записываем время окончания:

  timeEndSignal = micros();

По нехитрой формуле вычисляем расстояние от излучателя до приемника:

  lenC = ((timeEndSignal-timeStartSignal)/58.00)*10.00;

Ждем пока не затихнет в комнате УЗ шум:

delay(100);

Стоит отметить, что датчик сообщает нам расстояние при помощи опускания выхода Echo в Low на промежуток времени прямо пропорциональный измеренному расстоянию.

Тоже самое повторяем для второго излучателя.

При помощи правил о прямоугольном треугольнике проецируем полученное расстояние на плоскость пола (рис. 3).

Реализуем программно формулу перехода от трехмерных координат к плоскости, формула представлена выше: 

lenB = sqrt((lenC*2.00)*(lenC*2.00) - lenA*lenA);

К сожалению, у нас возникают погрешности и для их удаления я вывел вот такую опытную формулу, удалите её и посмотрите, что у вас получится.

  measurementError = 26.437 - 0.08*lenC/10;
  lenB = (lenB + measurementError*10)/10.00;

То же самое повторяем для датчика 2

Вычисляем координаты на плоскости

Находим угол Альфа:

  alfa = acos(((lenG*lenG + lenD*lenD - lenB*lenB)*1.00) / ((2*lenE*lenG)*1.00));

Находим сами координаты: 

  koord_X = lenE*cos(1.57-alfa);
  koord_Y = lenE*cos(alfa);

Если значение координат выходит за пределы возможных то заменяем его предыдущим значением:

  if((koord_X > 0) && (koord_X < 500) && (koord_Y > 0) && (koord_Y < 500))
  {
    oldKoord_X = koord_X;
    oldKoord_Y = koord_Y;
  }else{  
    koord_X = oldKoord_X;
    koord_Y = oldKoord_Y;
  }

Делаем буфер для 6 значений координат и постоянно его сдвигаем:

  koord_X5 = koord_X4;
  koord_X4 = koord_X3;
  koord_X3 = koord_X2;
  koord_X2 = koord_X1;
  koord_X1 = koord_X;
  
  koord_Y5 = koord_Y4;
  koord_Y4 = koord_Y3;
  koord_Y3 = koord_Y2;
  koord_Y2 = koord_Y1;
  koord_Y1 = koord_Y;

Получаем среднее значение по прошлым 6 измерениям:

  averageKoord_X = (koord_X + koord_X1 + koord_X2 + koord_X3 + koord_X4 + koord_X5)/6;
  averageKoord_Y = (koord_Y + koord_Y1 + koord_Y2 + koord_Y3 + koord_Y4 + koord_Y5)/6;

Отправляем координаты на ПК:

  Serial.println(averageKoord_X);
  Serial.println(averageKoord_Y);

Функции:

float asin(float c)
float acos(float c)
float atan(float c)

просто берем и используем =)

Весь код целиком:

int trigPinRessiv = 8;      
int echoPinRessiv = 9;      
int trigPinTransmit1 = 2;   
int trigPinTransmit2 = 3;  

int i;
long lenA = 2700;    //sensor height in mm
long lenG = 305;    // distance between sensors in cm

long koord_X,  koord_Y;
long koord_X1, koord_Y1;
long koord_X2, koord_Y2;
long koord_X3, koord_Y3;
long koord_X4, koord_Y4;
long koord_X5, koord_Y5;

long oldKoord_X = 0, oldKoord_Y = 0;
long averageKoord_X, averageKoord_Y;
long measurementError;
float alfa;
long timeStartSignal, timeEndSignal;
long lenC, lenE, lenB, lenD;

void setup() {
  Serial.begin (115200);
  pinMode(trigPinRessiv, OUTPUT);
  pinMode(echoPinRessiv, INPUT);
  pinMode(trigPinTransmit1, OUTPUT);
  pinMode(trigPinTransmit2, OUTPUT);
}
void loop()
{
  averageKoord_X = 0;
  averageKoord_Y = 0;
  
  digitalWrite(trigPinRessiv, LOW);  digitalWrite(trigPinTransmit1, LOW);
  delayMicroseconds(5);
  digitalWrite(trigPinRessiv, HIGH);  digitalWrite(trigPinTransmit1, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPinRessiv, LOW);  digitalWrite(trigPinTransmit1, LOW);
  
  while (digitalRead(echoPinRessiv) == LOW);   timeStartSignal = micros();
  while (digitalRead(echoPinRessiv) == HIGH);  timeEndSignal = micros();
  
  lenC = ((timeEndSignal-timeStartSignal)/58.00)*10.00;
  delay(100);

  digitalWrite(trigPinRessiv, LOW);  digitalWrite(trigPinTransmit2, LOW);
  delayMicroseconds(5);
  digitalWrite(trigPinRessiv, HIGH);  digitalWrite(trigPinTransmit2, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPinRessiv, LOW);  digitalWrite(trigPinTransmit2, LOW);
  
  while (digitalRead(echoPinRessiv) == LOW);  timeStartSignal = micros();
  while (digitalRead(echoPinRessiv) == HIGH);  timeEndSignal = micros();
  
  lenE = ((timeEndSignal-timeStartSignal)/58.00)*10.00;
  delay(100);


  lenB = sqrt((lenC*2.00)*(lenC*2.00) - lenA*lenA);
  measurementError = 26.437 - 0.08*lenC/10;
  lenB = (lenB + measurementError*10)/10.00;

  lenD = sqrt((lenE*2.00)*(lenE*2.00) - lenA*lenA);
  measurementError = 26.437 - 0.08*lenD/10;
  lenD = (lenD + measurementError*10)/10.00;
  
  alfa = acos(((lenG*lenG + lenD*lenD - lenB*lenB)*1.00)/((2*lenE*lenG)*1.00));
  
  koord_X = lenE*cos(1.57-alfa);
  koord_Y = lenE*cos(alfa);
  
  if((koord_X > 0) && (koord_X < 500) && (koord_Y > 0) && (koord_Y < 500))
  {
    oldKoord_X = koord_X;
    oldKoord_Y = koord_Y;
  }else{  
    koord_X = oldKoord_X;
    koord_Y = oldKoord_Y;
  }
  
  koord_X5 = koord_X4;
  koord_X4 = koord_X3;
  koord_X3 = koord_X2;
  koord_X2 = koord_X1;
  koord_X1 = koord_X;
  
  koord_Y5 = koord_Y4;
  koord_Y4 = koord_Y3;
  koord_Y3 = koord_Y2;
  koord_Y2 = koord_Y1;
  koord_Y1 = koord_Y;
  
  averageKoord_X = (koord_X + koord_X1 + koord_X2 + koord_X3 + koord_X4 + koord_X5)/6;
  averageKoord_Y = (koord_Y + koord_Y1 + koord_Y2 + koord_Y3 + koord_Y4 + koord_Y5)/6;
  
   
}

float asin(float c)
{
  float out;
  out = ((c+(pow(c,3))/6+(3*pow(c,5))/40+(5*pow(c,7))/112 +(35*pow(c,9))/1152 +(0.022*pow(c,11))+(0.0173*pow(c,13))+(0.0139*pow(c,15)) + (0.0115*pow(c,17))+(0.01*pow(c,19))));
  
  if(c >= .96 && c < .97)   {    out=1.287+(3.82*(c-.96));    }
  if(c>=.97 && c<.98)       {    out=(1.325+4.5*(c-.97));     }
  if(c>=.98 && c<.99)       {    out=(1.37+6*(c-.98));        }
  if(c>=.99 && c<=1)        {    out=(1.43+14*(c-.99));       }
  
  return out;
}



float acos(float c)
{
  float out;
  out=asin(sqrt(1-c*c));
  return out;
}

float atan(float c)
{
  float out;
  out=asin(c/(sqrt(1+c*c)));
  return out;
}

Вот мы и получили простейшую ультразвуковую систему GPS с областью действия метр на метр, на видео показано как это все работает.


Визуализация траектории сделана в Matlab, как сделать такую же визуализацию напишу в следующей статье.

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

Буду рад вашим мнениям и отзывам на данную тему, проект все ещё жив =)

Страничка проекта

Вдохновлялся следующими источниками:

Википедия
Пост на Хабре «Indoor «GPS» с точностью +-2см»

Комментарии (19)


  1. Andy_Big
    12.05.2019 15:35
    #20142052

    Вот бы еще излучатели отвязать от проводов :)
    Ну и приемник по-английски — receiver, а не ressiver :)


  1. Hilbert
    12.05.2019 18:43
    #20142610

    Зачем называть подобные системы «GPS»? В статье, на которую вы ссылаетесь в конце, хотя бы в кавычки взяли. Суть GPS в глобальности, лучше называть подобные системы просто системами позиционирования.


    1. Andy_Big
      12.05.2019 18:53
      #20142640
      +1

      LPS :)


    1. b__s__v Автор
      13.05.2019 15:14
      #20146450

      Я назвал GPS только для простоты восприятия, и потому что моя система как и GPS определяет координаты объекта, если бы я назвал статью "ультразвуковая система навигации для закрытых помещений" то её бы никто не нашел в поиске, да и если бы нашли то чтение закончили ещё на названии =)


  1. WFF
    12.05.2019 19:15
    #20142686

    Если нужна одна точка с координатами, то лучше сделать ее излучателем. Соответственно, будет два приемника. Это позволит, как минимум, в два раза повысить частоту измерения.


    1. b__s__v Автор
      13.05.2019 15:15
      #20146456

      В следующей статье развивающей данную систему так и будет.


  1. mikkab
    12.05.2019 21:57
    #20143250

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


    1. b__s__v Автор
      13.05.2019 15:21
      #20146488

      Здравствуйте, в более позднем виде системы я использую HC-SR04 как ультразвуковой микрофон и сам анализирую волну, это улучшило систему, но углы все равно не впечатляют.
      Я пробовал вот такой микрофон ADMP401 https://ru.aliexpress.com/item/ADMP401-ADMP404-MEMS-Breakout-Arduino-1-3/32966959737.html?spm=a2g0s.9042311.0.0.546933edslkyxS
      но тут очень слабая амплитуда, работать можно но, хочется лучше.
      Буду очень благодарен если вы мне посоветуете более хорошие излучатели и микрофоны, которые желательно можно было бы купить на Али Експрессе или в другом дешевом магазине.


      1. mikkab
        13.05.2019 15:45
        #20146630

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


        1. b__s__v Автор
          13.05.2019 16:21
          #20146908

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


  1. Andy_Big
    12.05.2019 22:14
    #20143304

    Интересно, а можно ли реализовать «местное» позиционирование на следующем принципе?:
    — на отслеживаемом объекте размещается излучатель, например в ИК-диапазоне, можно с модуляцией излучения
    — в углах помещения висят постоянно вращающиеся узконаправленные (по горизонтали) приемники, засекающие излучатель
    — когда приемник во время вращения засекает излучатель, он засекает угол, на котором это произошло
    — два приемника, висящие в разных концах помещения, дадут два угла направления на излучатель, дальше несложные вычисления дадут положение излучателя в пространстве (точнее, на плоскости)
    — увеличение числа приемников даст прибавку к точности определения координат излучателя

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


    1. Tangeman
      13.05.2019 01:05
      #20143740

      Решение такого типа реализовать можно (теоретически), но практически это будет очень дорого, энергоёмко и с сомнительной надежностью.

      Сам факт наличия механики делает систему менее надежной, менее точной и более энергопотребляющей, но самый интересный вопрос — как приёмник отличит отражение от источника (который излучает во всех направлениях), особенно если в помещении есть хорошо отражающие поверхности (или фрагменты поверхностей)?

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

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


      1. Andy_Big
        13.05.2019 07:12
        #20144020

        Сам факт наличия механики делает систему менее надежной, менее точной и более энергопотребляющей

        Это да, не спорю. Но я и не предлагаю срочно внедрять это, просто ради интереса сделать такую систему, как вот этот ультразвуковой вариант, который тоже не слишком подходит для практического применения :)
        как приёмник отличит отражение от источника (который излучает во всех направлениях), особенно если в помещении есть хорошо отражающие поверхности (или фрагменты поверхностей)?

        Думаю, что обработка полученных от разных приемников данных сможет отсеять отражения. Если первый приемник увидел излучатель в нескольких местах, а данные второго приемника коррелируются только с одним из показаний первого приемника, то остальные показания первого приемника можно считать ошибочными :)
        — гарантировать обнаружение источника более чем одним приёмником, одновременно или в пределах небольшого временного интервала (поскольку они узконаправленные);

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

        Теоретически это можно решить увеличением количества приемников, но опять же — это просто «for fun», а не рац. предложение для внедрения :)


    1. WFF
      13.05.2019 09:07
      #20144260

      Можно.
      Вы описали принцип работы HTC VIVE VRS. Точность позиционирования в миллиметрах, 10 раз в секунду. Очень низкая стоимость одного датчика. Всего два опорных «излучателя».


      1. Andy_Big
        13.05.2019 09:16
        #20144300

        Там именно вращающиеся датчики?


        1. python273
          13.05.2019 14:52
          #20146290

          Да, только там наоборот работает. Маяки (так и называются — lighthouse) делают вспышку и светят лазером сначала в одной плоскости, потом в другой. На контроллере и хедсете есть сенсоры, которые считают задержку. Трекинг работает даже с одним маяком.


          https://www.youtube.com/watch?v=J54dotTt7k0


          1. Andy_Big
            13.05.2019 15:15
            #20146452

            Сначала подумал, что измеряется время распространения света, потом понял. Очень интересный принцип, спасибо :)


  1. Andy_Big
    13.05.2019 07:11
    #20144018

    del


  1. FadeToBlack
    13.05.2019 07:29
    #20144050

    Спасибо за статью! Но настоятельно советую автору «подтянуть» программирование, чтобы расширить свои возможности.