b__s__v May 12 2019 at 13:57

Ультразвуковой GPS

6 min

9.5K

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

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

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

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

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

Концепция

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

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

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

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

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

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

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

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

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

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

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

Практика

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

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

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

Рис. 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см»

Tags:

Hubs: