Хабрахабр

[Из песочницы] Ультразвуковой GPS

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

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

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

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

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

Концепция

image
Рис. 1 – Общая идея устройства
По верхним углам комнаты установлены HC-SR04, которые играют роль излучателей, на полу приемником вверх лежит третий, он у нас играет роль приемника.

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

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

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

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

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

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

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

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

image

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

Практика

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

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

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

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

Программа

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

Заставляем излучатель 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)) 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см»

Показать больше

Похожие публикации

Добавить комментарий

Ваш адрес email не будет опубликован. Обязательные поля помечены *

Кнопка «Наверх»