How to calculate "next" point on line which is 50 meters from current point on earth? (autonomous boat)

Question

I'm currently building an autonomous boat for which I define a path to follow. This path consists of multiple way points which are connected by straight lines.

The boat doesn't need to be exactly on the line between the waypoints. It will mostly be on open water so it's fine if it's a couple meters off. What I want to do is let the boat aim for a point which is 50 meters ahead on the wayline. So in the image above I added a circle around the boat with a radius of 50m. That means that I want the boat to aim for point Z. I've got the following coordinates:

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

I'm kinda puzzled on how I could find the coordinates of this point Z. I first assumed the earth was a flat raster (I figured that at this scale the rounding of the earth isn't that important) and tried using my best Pythagoras arithmetic, but I can't figure it out. Let alone when I also need to account for the rounding of the earth.

To complicate it even further, the 50m circle intersects twice with the wayline, so how do I know which of the two intersects is the furthest on the path? I first thought of simply measuring the distance to point X (the start of the path), but if it travels in a circle back to the start then the boat will start making shortcuts straight from start to finish.

Does anybody have an idea how I can handle these problems? How do I get the intersects with the wayline and how I can find out which of the two intersects is the furthest on the wayline? All tips are welcome!

Answer 1

Some ideas.

You have to transform everything to Cartesian coordinates. Assuming that the ray of the earth is 6371000m, a reference system can be established starting at point X and then calculating the desired points. Follow a script in Mathematica showing the main steps.

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

I have provided an algorithm of how to calcualte the coordiantes of point $Z$ if you assume that you are on the plane (not on the sphere) and the coordinates of the other points are all in planar cartesian coordinates. This means you will have to recalcualte the coordinates from geographic to planar cartesian.

First I am going to write the algorithm, and after that if you wish, you can read the derivation of the formulas.

Algorithm. Input: Point $X \, (x_1,x_2)$, point $Y\, (y_1, y_2)$ and the position of the boat $B \, (b_1, b_2)$.

Step 1. Calculate \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} Step 2. If $(\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)$$ else if $(\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)$$ Step 3. The coordinates of point $Z$ are $$Z \,\, \Big(\, x_1 + k\,(y_1-x_1),\,x_2 + k\,(y_2-x_2)\,\Big)$$

In Matlab syntax:

   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

---

Derivation of the formulas of the algorithm: To justify the algorithm, one can use vectors and their lengths to derive the formulas. Let $$\vec{XY} = Y - X = (y_1, y_2) - (x_1,x_2) = (y_1 - x_1, \, y_2 - x_2)$$ and $$|XY| = \sqrt{(y_1-x_1)^2+(y_2-x_2)^2}$$

1. Calculate the 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. Calculate the vector $\vec{XB} = B - X = (b_1 - x_1, \, b_2 - x_2)$
3. Calculate the dot product $$\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. The quantity $h_B = |BH_B| = \sqrt{|XB|^2 - \frac{(\vec{XY}\circ\vec{XB})^2}{|XY|^2}}$ is the distance between the point $B$ and the orthogonal projection of $B$ on the line $XY$, denoted by $H_B$. This is based on Pythagoras' theorem from the right triangle $\Delta \, XBH_B$.
5. By Pythagoras' theorem for the right triangle $\Delta BZH_B$, for which we know the hypotenuse $|BZ| = 50$ and the side $|BH_B| = h_B$, we can calculate the third side \begin{align} |ZH_B| &= \sqrt{|BZ|^2 - h _B^2} = \sqrt{50^2 - h _B^2} \end{align}
6. Since $Z$ is on the line $XY$, the vector $\vec{XZ}$ is aligned with the vector $\vec{XY}$, and thus $\vec{XZ}$ is aligned with the unit vector $\frac{\vec{XY}}{|XY|}$. Furthermore, the distance $|XZ|$ is the sum of the (oriented) distances $|XH_B|$ and $|ZH_B|$, i.e. $$|XZ| = \frac{(\vec{XY}\circ\vec{XB})}{|XY|} + |ZH_B| = \frac{(\vec{XY}\circ\vec{XB})}{|XY|} + \sqrt{50^2 - h _B^2} $$ if $\frac{(\vec{XY}\circ\vec{XB})}{|XY|} \leq |XY|$ and $$|XZ| = \frac{(\vec{XY}\circ\vec{XB})}{|XY|} - |ZH_B| = \frac{(\vec{XY}\circ\vec{XB})}{|XY|} - \sqrt{50^2 - h _B^2} $$ if $\frac{(\vec{XY}\circ\vec{XB})}{|XY|} > |XY|$

Therefore, $$\vec{XZ} = |XZ| \, \frac{\vec{XY}}{|XY|}$$

7. Consequently,we have \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. Thus, if $(\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}$$ and if $(\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. The coordinates of the point $Z$ are then given by $$Z = X + \vec{XZ}$$