[Из песочницы] Ультразвуковой GPS12.05.2019 16:04

Ультразвуковой 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 см»

© Habrahabr.ru