1
$\begingroup$

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.

enter image description here

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!

$\endgroup$
2
  • $\begingroup$ longtitude/latitude are measures of angle. You are correct that it is much easier if you assume the earth is flat. Firstly you would have to translate your long/lat into a grid reference so that the units are distance. This is not a simple calculation but there are websites that can help you. $\endgroup$
    – G Aker
    Commented Apr 13, 2019 at 15:39
  • $\begingroup$ nhc.noaa.gov/gccalc.shtml: Here's a site that does it but I cannnot vouch for it's accuracy and as you are dealing with very small (relatively) distances you will need a lot of accuracy. $\endgroup$
    – G Aker
    Commented Apr 13, 2019 at 15:41

2 Answers 2

1
$\begingroup$

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]

enter image description here

$\endgroup$
1
$\begingroup$

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|}$$

  1. 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}

  2. 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}$$

  3. The coordinates of the point $Z$ are then given by $$Z = X + \vec{XZ}$$

$\endgroup$
2
  • $\begingroup$ Wow. Just wow. This is looks like a piece of art to me. I thought I was ok with maths, but this goes way over my head. I thought that to calculate this point Z I would need a couple lines of Python code. But I don't even understand many of the things you wrote here. I certainly didn't realise it would be this difficult to calculate. I realise this is the math stackexchange website, not stackoverflow, but it would be really awesome if you could help me translate this into a piece of working code (in any language you prefer). If not I of course also understand.. :-) $\endgroup$
    – kramer65
    Commented Apr 15, 2019 at 6:41
  • $\begingroup$ @kramer65 Sorry, I did not mean to intimidate you with my post. In general, the idea of the problem is to write the equation of the circle of radius $50$ and centered at point $B$ (the boat) and the equation of the line $XY$, and then solve the system of two equations with two variables find the intersection points of the two by solving a system of two equations with two variables. What I did is just slightly bypass the system of equations and tried to find the point directly. The algorithm relevant to you is just steps 1, 2 and 3. The rest is an explanation why these formulas are used. $\endgroup$ Commented Apr 15, 2019 at 12:15

You must log in to answer this question.

Not the answer you're looking for? Browse other questions tagged .