Hola a todos, estoy intentando programar un vehículo autonomo para que vaya de un punto a otro indicandole la coordenada GPS de destino.
La idea es hacerlo sacando de las propias coordenadas toda la información que necesito. Quiero evitar usar una brujula electrónica. Aplicando Pitágoras sacar la distancia (distancia = sqrt(x^2 + y^2)) y aplicando trigonometría básica del cole (E.S.O) sacar la dirección (dirección = arc tang(y/x)).
La distancia la puedo sacar directamente de las coordenadas en Latitud y Longitud, pero la dirección si lo hago a partir de estas coordenadas me las dan referidas a los puntos cardinales y no al propio vehículo, como he dicho antes no quiero usar brujula electrónica si es posible.
Para que la dirección sea referida al propio vehículo necesito hacerlo, o al menos no se me ocurre otra forma que, a partir de las coordenadas en formato UTM.
El problema: la conversión de las coordenadas.
Estoy siguiendo las Ecuaciones de Coticchia-Surace que se pueden ver en el PDF que adjunto. Creo que las he seguido al pie de la letra al programar mi Arduino Mega 2560 R2 (como módulo GPS uso el MTK-3329).
Lo que he programado es lo siguiente para hacer las pruebas de su funcionamiento y por lo visto hay algún error en el proceso porque no da como corresponde.
double latDest = 43.4884075, lonDest = 3.801873306; ///< Variables donde se van a guardar las coordenadas de destino.
double x = 0, y = 0;
void setup()
{
Serial.begin(9600);
}
void loop()
{
conversionUTM(latDest, lonDest);
delay(7000);
}
void conversionUTM (double lati, double longi)
{
/*!
* Transformación de las coordenadas geográficas a UTM
*/
/// Sobre la geometría del delipsoide WGS84
double a = 6378137.0;
double b = 6356752.3142;
// float e = sqrt((a*a) + (b*b))/a; ///< Excentricidad.
double e = sqrt((a*a) + (b*b))/b; ///< Segunda excentricidad.
double e2 = e * e; ///< al cuadrado. Usaremos esta directamente.
double c = a*a / b; ///< Radio Polar de Curvatura.
/// Sobre la longitud y latitud. Conversión de grados decimales a radianes.
/*!
* Cálculo del signo de la longitud:
* - Si la longitud está referida al Oeste del meridiano de Greenwich,
* entonces la longitud es negativa (-).
* - Si la longitud está referida al Este del meridiano de Greenwich,
* entonces la longitud es positiva 8+).
*/
double latRad = lati * PI / 180.0; ///< Latitud en Radianes.
double lonRad = longi * PI / 180.0; ///< Longitud en Radianes.
/// Sobre el huso.
int h = int((lati / 6.0) + 31.0); ///< Nos interesa quedarnos solo con la parte entera.
int landa0 = h * 6 - 183; ///< Cálculo del meridiano central del huso en radianes.
double Dlanda = lonRad - (landa0 * PI / 180.0); ///< Desplazamiento del punto a calcular con respecto al meridiano central del huso.
/*!
* Ecuaciones de Coticchia-Surace para el paso de Geográficas a UTM (Problema directo);
*/
/// Cálculo de Parámetros.
double coslatRad = cos (latRad);
double coslatRad2 = coslatRad * coslatRad;
double A = coslatRad * sin (Dlanda);
double xi = 0.5 * log ((1 + A) / (1 - A));
double n = atan(tan(latRad) / cos(Dlanda)) - latRad;
double v = (c / sqrt(1 + e2 * coslatRad2)) * 0.9996;
double z = (e2/ 2.0) * xi * xi * coslatRad2;
double A1 = sin (2 * latRad);
double A2 = A1 * coslatRad2;
double J2 = latRad + (A1 / 2.0);
double J4 = (3.0 * J2 + A2) / 4.0;
double J6 = (5.0 * J4 + A2 * coslatRad2) / 3.0;
double alf = 0.75 * e2;
double bet = (5.0 / 3.0) * alf * alf;
double gam = (35.0 / 27.0) * alf * alf * alf;
double Bfi = 0.9996 * c * (latRad - alf * J2 + bet * J4 - gam * J6);
/*!
* Cálculo final de coordenadas UTM
*/
Serial.println (" Las coordenadas GPS que se van a transformar son: ");
Serial.print (" Latitud: "); Serial.println (lati,5);
Serial.print (" Longitud: "); Serial.println (longi,5);
Serial.println (" Coordenadas UTM actuales: ");
x = xi * v * (1 + (z / 3.0)) + 500000; /*!< 500.000 es el retranqueo que se realiza en cada huso sobre el origen de
coordenadas en el eje X con el objeto de que no existan coordenadas negativas. */
Serial.print (" X = "); Serial.print (x,5); Serial.println (" (m)");
y = n * v * (1 + z) + Bfi; /*!< En el caso de latitudes al sur del ecuador, se sumará al valor de Y 10.000.000
para evitar coordenadas negativas. */
Serial.print (" Y = "); Serial.print (y,5); Serial.println (" (m)");
}
Estoy ya unas 18h dandole vueltas y no veo el fallo. ¿Me podeís ayudar a encontrar el fallo?
Para comprobar la conversión que me realiza el programa uso esta web: http://www.asturnatura.com/sinflac/calculadora-conversiones-coordenadas.php
Desde ya muchísimas gracias por vuestra ayuda.
Geograficas-UTM_Hoja_A1-1.pdf (1.46 MB)