GPS ultrasónico. Modelo conceptual
Antes de emprender un viaje tan largo, vale la pena verificar si todo se hace realmente en la rodilla.
De qué trata este artículo: cómo hacer un GPS ultrasónico simple de manera rápida y económica.
Lista de dispositivos requeridos
- HC-SR04 3 piezas
- Arduino 1 piezas
- Una bobina de cables.
Concepto
Fig. 1 - Idea general del dispositivoEn las esquinas superiores de la habitación están instalados HC-SR04, que desempeñan el papel de emisores, en el piso con el receptor hacia arriba, un tercero, desempeña el papel de un receptor.
Todo esto está conectado de acuerdo con el esquema:
Fig. 2 - Diagrama de conexión del dispositivoY, por supuesto, conecta Arduino a través de USB a una computadora.
Cómo funciona todo:
- Mida la distancia desde el receptor al emisor 1
- Envíe una señal sobre el comienzo de la medición de distancia al receptor y al emisor 1 (tire de las patas del Trig con él).
- Espere hasta que el receptor nos dé la longitud.
- Repita lo mismo para el emisor 2.
- Calcular coordenadas del receptor.
Recordar geometría escolar
Fig. 3 - Representación geométrica del problema.En base a esta figura, componimos las fórmulas para calcular las coordenadas:
A, F es la altura de los emisores en relación con el receptor;
C, E son las longitudes obtenidas midiendo la distancia desde los emisores hasta el receptor;
G es la distancia entre los emisores.
Practica
Instale dos radiadores debajo del techo, a una distancia decente entre sí (3 metros), diríjalos con radiadores en un punto, alrededor del cual se formará su área de trabajo.
Monte el receptor en algo plano (por ejemplo, un trozo de placa) y envuélvalo con cinta con una cinta para que no creemos ruido ultrasónico innecesario en la habitación.
Conecte todo de acuerdo con el esquema presentado anteriormente.
Así es como se verían los sujetadores para emisores y el sustrato para el receptor:
Fig. 4 - Tipo de módulosEl programa
Considere las partes clave del código con más detalle.
Obligamos al emisor 1 y al receptor a comenzar a medir la distancia moviendo la entrada Trig de estos dispositivos de menor a mayor, 10 microsegundos y nuevamente a baja.
digitalWrite(trigPinRessiv, LOW); digitalWrite(trigPinTransmit1, LOW); delayMicroseconds(5); digitalWrite(trigPinRessiv, HIGH); digitalWrite(trigPinTransmit1, HIGH); delayMicroseconds(10); digitalWrite(trigPinRessiv, LOW); digitalWrite(trigPinTransmit1, LOW);
Por lo general, estos dispositivos emiten una señal de ultrasonido y esperan hasta que se refleje en algo y vuelvan a ellos. Pero los estamos engañando, en nuestro caso, uno envía y el otro recibe, el que recibió cree que es su señal, aunque esta es una señal de otro dispositivo, y nos da la distancia a este otro dispositivo.
Esperamos hasta que el sensor comience a decirnos la duración del vuelo de la señal ultrasónica:
while (digitalRead(echoPinRessiv) == LOW);
Registre la hora de inicio de la señal:
timeStartSignal = micros();
Esperamos hasta que el sensor deje de decirnos el tiempo de vuelo de la señal ultrasónica:
while (digitalRead(echoPinRessiv) == HIGH);
Escribimos el tiempo final:
timeEndSignal = micros();
Usando una fórmula simple, calculamos la distancia desde el emisor hasta el receptor:
lenC = ((timeEndSignal-timeStartSignal)/58.00)*10.00;
Esperamos hasta que el ruido del ultrasonido disminuya en la habitación:
delay(100);
Vale la pena señalar que el sensor nos dice la distancia bajando la salida de eco a baja durante un período de tiempo directamente proporcional a la distancia medida.
Lo mismo se repite para el segundo emisor.
Usando las reglas en un triángulo rectángulo, proyecte la distancia resultante en el plano del piso (Fig. 3).
Implementamos el software para la transición de coordenadas tridimensionales al plano, la fórmula se presenta arriba:
lenB = sqrt((lenC*2.00)*(lenC*2.00) - lenA*lenA);
Desafortunadamente, tenemos errores, y para eliminarlos se me ocurrió una fórmula tan experimentada, elimínela y vea qué obtiene.
measurementError = 26.437 - 0.08*lenC/10; lenB = (lenB + measurementError*10)/10.00;
Repita lo mismo para el sensor 2.
Calculamos las coordenadas en el plano.
Encuentra el ángulo alfa:
alfa = acos(((lenG*lenG + lenD*lenD - lenB*lenB)*1.00) / ((2*lenE*lenG)*1.00));
Encuentra las coordenadas ellos mismos:
koord_X = lenE*cos(1.57-alfa); koord_Y = lenE*cos(alfa);
Si el valor de las coordenadas va más allá de lo posible, reemplácelo con el valor anterior:
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; }
Hacemos un búfer para 6 valores de coordenadas y lo cambiamos constantemente:
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;
Obtenemos el valor promedio de las últimas 6 mediciones:
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;
Enviar las coordenadas a la PC:
Serial.println(averageKoord_X); Serial.println(averageKoord_Y);
Funciones:
float asin(float c) float acos(float c) float atan(float c)
solo toma y usa =)
Código completo:
int trigPinRessiv = 8; int echoPinRessiv = 9; int trigPinTransmit1 = 2; int trigPinTransmit2 = 3; int i; long lenA = 2700;
Así que obtuvimos el sistema GPS ultrasónico más simple con un rango de metro por metro, el video muestra cómo funciona todo.
La visualización de la trayectoria se realiza en Matlab, escribiré cómo hacer la misma visualización en el próximo artículo.
En futuros artículos, consideraré con mayor profundidad las diversas partes de este sistema e intentaré mejorarlas.
Estaré encantado de escuchar sus opiniones y comentarios sobre este tema, el proyecto aún está vivo =)
Página del proyectoInspirado por las siguientes fuentes:
WikipediaUna publicación en el Habr "Indoor" GPS "con una precisión de + -2cm"