¿Cómo calcular el "próximo" punto en la línea que está a 50 metros del punto actual en la tierra? (barco autónomo)

Question

Actualmente estoy construyendo un barco autónomo para el que defino una trayectoria a seguir. Este camino consiste en múltiples puntos de paso que están conectados por líneas rectas.

No es necesario que el barco esté exactamente en la línea entre los waypoints. La mayor parte del tiempo estará en aguas abiertas, así que no hay problema si se desvía un par de metros. Lo que quiero hacer es que el barco apunte a un punto que esté 50 metros por delante en la línea de rumbo. Así que en la imagen de arriba he añadido un círculo alrededor del barco con un radio de 50 metros. Eso significa que quiero que el barco apunte al punto Z. Tengo las siguientes coordenadas:

          Latitude        Longitude
------------------------------------------
Boat: 52.42373335663094, 5.075082778930664;
X:    52.42315721146138, 5.074074268341065
Y:    52.42389341072507, 5.0763434171676645
Z:    ???

Estoy un poco desconcertado sobre cómo podría encontrar las coordenadas de este punto Z. Primero asumí que la tierra era una trama plana (supuse que a esta escala el redondeo de la tierra no es tan importante) y traté de usar mi mejor aritmética de Pitágoras, pero no puedo resolverlo. Y mucho menos cuando también tengo que tener en cuenta el redondeo de la tierra.

Para complicarlo aún más, el círculo de 50 metros se cruza dos veces con el trazado del camino, así que ¿cómo puedo saber cuál de las dos intersecciones es la más lejana del camino? Primero pensé en medir simplemente la distancia hasta el punto X (el inicio del camino), pero si viaja en círculo hasta el inicio entonces el barco empezará a hacer atajos directamente desde el inicio hasta el final.

¿Alguien tiene una idea de cómo puedo manejar estos problemas? ¿Cómo obtengo las intersecciones con el eje de carreteras y cómo puedo averiguar cuál de las dos intersecciones es la más lejana en el eje de carreteras? ¡Todos los consejos son bienvenidos!

Answer 1

Algunas ideas.

Hay que transformar todo a coordenadas cartesianas. Suponiendo que el rayo de la tierra es de 6371000m, se puede establecer un sistema de referencia empezando por el punto X y calculando luego los puntos deseados. Sigue un script en Mathematica que muestra los principales pasos.

Boat = -{52.42373335663094, 5.075082778930664};
X = -{52.42315721146138, 5.074074268341065};
Y = -{52.42389341072507, 5.0763434171676645};
Origin = X;
frac = Pi/180;
Eradius = 6371000;
oC = Boat - Origin;
oY = Y - Origin;
CartoC = Eradius oC frac;
CartoY = Eradius oY frac;
equCir = (CartoC - {x, y}).(CartoC - {x, y}) - 50^2;
equLin = lambda CartoY;
sols = Solve[{equCir == 0, y/x == CartoY[[2]]/CartoY[[1]]}, {x, y}][[2]];
gr1 = ContourPlot[equCir == 0, {x, CartoC[[1]] - 100, CartoC[[1]] + 100}, {y, CartoC[[2]] - 100, CartoC[[2]] + 100}];
gr2 = ParametricPlot[equLin, {lambda, 0, 1}];
ptC = Graphics[{Red, Disk[CartoC, 2]}];
Z = {x, y} /. sols;
ptZ = Graphics[{Red, Disk[Z, 2]}];
Show[gr1, gr2, ptC, ptZ]

Answer 2

He proporcionado un algoritmo de cómo calcualte los coordiantes de punto $Z$ si supones que estás en el plano (no en la esfera) y las coordenadas de los otros puntos están todas en coordenadas cartesianas planas. Esto significa que tendrás que recalcular las coordenadas de geográficas a cartesianas planas.

Primero voy a escribir el algoritmo, y después, si lo deseas, puedes leer la derivación de las fórmulas.

Algoritmo. Entrada: Punto $X \, (x_1,x_2)$ , punto $Y\, (y_1, y_2)$ y la posición del barco $B \, (b_1, b_2)$ .

Paso 1. Calcular $$\begin{align} &|XY|^2 = (y_1-x_1)^2 + (y_2 - x_2)^2\\ &|XB|^2 = (b_1-x_1)^2 + (b_2 - x_2)^2\\ &(\vec{XY} \circ \vec{XB}) = (y_1-x_1)(b_1-x_1) + (y_2-x_2)(b_2-x_2) \end{align}$$ Paso 2. Si $(\vec{XY}\circ\vec{XB}) \leq |XY|^2$ set $$k = \frac{1}{|XY|^2}\left((\vec{XY}\circ\vec{XB}) + \sqrt{2500\,|XY|^2 - |XB|^2|XY|^2 + (\vec{XY}\circ\vec{XB})^2}\right)$$ Si no, si $(\vec{XY}\circ\vec{XB}) > |XY|^2$ set $$k = \frac{1}{|XY|^2}\left((\vec{XY}\circ\vec{XB}) - \sqrt{2500\,|XY|^2 - |XB|^2|XY|^2 + (\vec{XY}\circ\vec{XB})^2}\right)$$ Paso 3. Las coordenadas del punto $Z$ son $$Z \,\, \Big(\, x_1 + k\,(y_1-x_1),\,x_2 + k\,(y_2-x_2)\,\Big)$$

En la sintaxis de Matlab:

   X = [x1, x2];  %coordinates of X
   Y = [y1, y2];  %coordinates of Y 
   B = [b1, b2];  %coordinates of the boat
   Z = [0, 0];    %just initialization of the output Z

   distance_XY_2 = (Y(1) - X(1))^2 + (Y(2) - X(2))^2;
   distance_XB_2 = (B(1) - X(1))^2 + (B(2) - X(2))^2; 
   XY_dot_XB = (Y(1) - X(1))*(B(1) - X(1)) + (Y(2) - X(2))*(B(2) - X(2));

   if XY_dot_XB <= distance_XY_2
      k = ( XY_dot_XB + sqrt( 2500*distance_XY_2 - distance_BY_2*distance_XY_2 + XY_dot_XB^2) )/(distance_XY_2);
   elsif XY_dot_XB > distance_XY_2 
      k = ( XY_dot_XB - sqrt( 2500*distance_XY_2 - distance_BY_2*distance_XY_2 + XY_dot_XB^2) )/(distance_XY_2);
   end

   Z(1) = X(1) + k*(Y(1) - X(1));  %the first coordinate of Z
   Z(2) = X(2) + k*(Y(2) - X(2));  %the second coordinate of Z

---

Derivación de las fórmulas del algoritmo: Para justificar el algoritmo, se pueden utilizar vectores y sus longitudes para derivar las fórmulas. Sea $$\vec{XY} = Y - X = (y_1, y_2) - (x_1,x_2) = (y_1 - x_1, \, y_2 - x_2)$$ y $$|XY| = \sqrt{(y_1-x_1)^2+(y_2-x_2)^2}$$

1. Calcular el vector $\frac{\vec{XY}}{|XY|} = \frac{(y_1-x_1, \, y_2-x_2)}{\sqrt{(y_1-x_1)^2+(y_2-x_2)^2}} = \left(\frac{y_1-x_1}{\sqrt{(y_1-x_1)^2+(y_2-x_2)^2}}\, , \,\, \frac{y_1-x_1}{\sqrt{(y_2-x_2)^2+(y_2-x_2)^2}}\right)$
2. Calcular el vector $\vec{XB} = B - X = (b_1 - x_1, \, b_2 - x_2)$
3. Calcular el producto punto $$\left(\frac{\vec{XY}}{|XY|}\circ\vec{XB}\right) = \frac{(\vec{XY}\circ\vec{XB})}{|XY|} = \frac{(y_1-x_1)(b_1-x_1) + (y_2-x_2)(b_2-x_2)}{\sqrt{(y_2-x_2)^2+(y_2-x_2)^2}}$$
4. La cantidad $h_B = |BH_B| = \sqrt{|XB|^2 - \frac{(\vec{XY}\circ\vec{XB})^2}{|XY|^2}}$ es la distancia entre el punto $B$ y la proyección ortogonal de $B$ en la línea $XY$ , denotado por $H_B$ . Esto se basa en el teorema de Pitágoras del triángulo rectángulo $\Delta \, XBH_B$ .
5. Por el teorema de Pitágoras para el triángulo rectángulo $\Delta BZH_B$ para el que conocemos la hipotenusa $|BZ| = 50$ y el lado $|BH_B| = h_B$ podemos calcular el tercer lado $$\begin{align} |ZH_B| &= \sqrt{|BZ|^2 - h _B^2} = \sqrt{50^2 - h _B^2} \end{align}$$
6. Desde $Z$ está en la línea $XY$ el vector $\vec{XZ}$ se alinea con el vector $\vec{XY}$ y por lo tanto $\vec{XZ}$ se alinea con el vector unitario $\frac{\vec{XY}}{|XY|}$ . Además, la distancia $|XZ|$ es la suma de las distancias (orientadas) $|XH_B|$ y $|ZH_B|$ es decir $$|XZ| = \frac{(\vec{XY}\circ\vec{XB})}{|XY|} + |ZH_B| = \frac{(\vec{XY}\circ\vec{XB})}{|XY|} + \sqrt{50^2 - h _B^2}$$ si $\frac{(\vec{XY}\circ\vec{XB})}{|XY|} \leq |XY|$ y $$|XZ| = \frac{(\vec{XY}\circ\vec{XB})}{|XY|} - |ZH_B| = \frac{(\vec{XY}\circ\vec{XB})}{|XY|} - \sqrt{50^2 - h _B^2}$$ si $\frac{(\vec{XY}\circ\vec{XB})}{|XY|} > |XY|$

Por lo tanto, $$\vec{XZ} = |XZ| \, \frac{\vec{XY}}{|XY|}$$

7. En consecuencia, tenemos $$\begin{align}\vec{XZ} &= \left(\frac{(\vec{XY}\circ\vec{XB})}{|XY|} \pm \sqrt{50^2 - h_B^2} \right)\, \frac{\vec{XY}}{|XY|} \\ &= \left(\frac{(\vec{XY}\circ\vec{XB})}{|XY|} \pm \sqrt{2500 - |XB|^2 + \frac{(\vec{XY}\circ\vec{XB})^2}{|XY|^2}}\right)\, \frac{\vec{XY}}{|XY|}\\ &= \left(\frac{(\vec{XY}\circ\vec{XB})}{|XY|} \pm \sqrt{\frac{2500|XY|^2 - |XB|^2|XY|^2 + (\vec{XY}\circ\vec{XB})^2}{|XY|^2}}\right)\, \frac{\vec{XY}}{|XY|}\\ &= \frac{1}{|XY|^2}\left((\vec{XY}\circ\vec{XB}) \pm \sqrt{2500|XY|^2 - |XB|^2|XY|^2 + (\vec{XY}\circ\vec{XB})^2}\right)\, \vec{XY} \end{align}$$

8. Así, si $(\vec{XY}\circ\vec{XB}) \leq |XY|^2$ $$\vec{XZ} = \frac{1}{|XY|^2}\left((\vec{XY}\circ\vec{XB}) + \sqrt{2500|XY|^2 - |XB|^2|XY|^2 + (\vec{XY}\circ\vec{XB})^2}\right)\, \vec{XY}$$ y si $(\vec{XY}\circ\vec{XB}) > |XY|^2$ $$\vec{XZ} = \frac{1}{|XY|^2}\left((\vec{XY}\circ\vec{XB}) - \sqrt{2500|XY|^2 - |XB|^2|XY|^2 + (\vec{XY}\circ\vec{XB})^2}\right)\, \vec{XY}$$

9. Las coordenadas del punto $Z$ vienen dadas por $$Z = X + \vec{XZ}$$