Finding a smooth path between points on a 2d map with maximum curvature

Question

I have a set of points on a map (given by x,y coordinates) and I want to find a path between these points. The goal is to have a ship sail this path, so the path can't just be straight lines.

I believe I can calculate the tangent direction or the curve at the given points by looking at the previous and next point, as illustrated below. Since going from point A to B is 100m at an angle of 90° and going from point B to C is 200m at angle 0°, the tangent direction in point B would be $$\frac{100}{100+200} * 90° + \frac{200}{100+200} * 0° = 30°$$ The tangent of the very first point and the very last point would simply be chosen in the direction of the next and previous point respectively. So if there are no other points than A, B and C then the tangent at A would be 90° and the tangent at C would be 0°.

I'm stuck at what to do next. Preferably I would get a piecewise spline so that the curve between each pair of points can be calculated independently and the entire path doesn't have to be calculated up front, but that is not a requirement.

The special requirement for the curve is that the turns in the curve can't be sharper than a given parameter. The ship has a maximum rate of turn, it can't rotate faster than this maximum. So short U-turns can't exist in the curve.

Is it possible to construct such a curve, given just the points and the maximum curvature (and the speed of the ship if needed)?
If yes, could someone provide the necessary math or at least point me in the right direction?
If not, what other data would I have to provide in order to calculate such a path? If a spline with the maximum curvature restraint can't be calculated then at least I need a method to detect that the curve is too sharp.

Suggestions for solutions where the curve does not go through the points exactly are also welcome, but there would have to be a parameter that controls the maximum distance between the points and the curve (e.g. the curve can't be more than 20 meter away from any point in the input).

Answer 1

"Pure" Bezier curves are sometimes not sufficient. Splines maybe a solution. But there is a less known, rather simple, intermediate family of curves, called Rational Bezier (RB) curves (a step towards more general "Non Uniform Rational Bezier Splines" = NURBS) deserving to be known. Here is an explanation about them.

Let us recall that the (so-called "quadratic") Bezier curve passing through $A$ and $C$ and "driven" by intermediate point $B$ is defined by :

$$\tag{1}M=s^2 A + 2 st B + t^2 C \ \ \ \ \ \ \text{where} \ \ \ \ s:=1-t, \ \ 0 \leq t \leq 1.$$

(these curves are parabolas).

Remarks :

• The meaning of expression (1) is that either $M,A,B,C$ are considered as complex numbers or relationship (1) is projected onto $x$ and $y$ coordinates axes.

• (1) uses barycentric notation ; it makes sense because the sum of the weights $s^2+2st+t^2=(s+t)^2$ is equal to $1$.

Consider now the following equation :

$$\tag{2}M=\dfrac{s^2 A + 2 wst B + t^2 C}{s^2 + 2 wst + t^2}$$

where $w$ is a scalar weight taking any real value. Note that (1) is a particular case of (2) for $w=1$.

Let $C_w$ denote the curve with equation (2). It can be established that any $C_w$ curve is a conic section ; see Fig. 1 and its legend for a global understanding of the influence of parameter $w$ on the shape of curves $C_w$.

Please note that the denominator in (2) is such that the sum of the weights :

$$\dfrac{s^2}{s^2 + 2 wst + t^2}+\dfrac{2 wst}{s^2 + 2 wst + t^2}+\dfrac{t^2}{s^2 + 2 wst + t^2}$$

is still $1$.

Now, we reach the important point for you : the higher the value of $w$, the more $C_w$ is attracted by point $I$ with an ever increasing value of the maximum curvature.

Fig. 1 : RB curves $C_w$ (some values of $w$ are displayed). If $-1<w<1$, we have elliptic arcs, with the notable exception of $w=0$ (straight line). If $w=1$ (red line), we have the usual Bezier curve. If $w>1$, we get hyperbola branches.

Remarks :

• $C_w$, expressed in this way:

$$x=\dfrac{s^2 x_A + 2 wst x_B + t^2 x_C}{s^2 + 2 wst + t^2}, \ \ y=\dfrac{s^2 y_A + 2 wst y_B + t^2 y_C}{s^2 + 2 wst + t^2}$$

has a curvature that can be computed using classical formula :

$$k=\dfrac{y''x'-y'x''}{(x'^2+y'^2)^{3/2}}$$

as established for example in these lecture notes : (https://www.ima.umn.edu/~miller/1372curvature.pdf).

• Any conic curve can be represented by a parametric equation of the form (2).

• We have been working with RB curves having a single control point $B$ ; these are called quadratic RB curves. One can get more flexibility by using two control points, $B_1$ and $B_2$, with weights $w_1$ and $w_2$ (cubic RB curves) with the following definition :

$$M=\dfrac{s^3 A + 3 s^2t w_1 B_1 + 3 st^2 w_2 B_2 +t^3 C}{s^3 + 3 s^2t w_1 + 3 st^2 w_2 +t^3}.$$

• See (https://pages.mtu.edu/~shene/COURSES/cs3621/NOTES/spline/NURBS/RB-circles.html) or (https://people.eecs.berkeley.edu/~sequin/CS284/LEC2k/L27.htm) where it is shown in particular that one can make circles with quadratic RB curves.

Answer 2

I'm going to assume that your points are in a plane rather than on a sphere, i.e., that we're at a scale where the curvature of the earth is not relevant. And because you didn't mention obstructions, I'm going to ignore those. And now I can solve a simpler problem:

Given a sequence of points $P_0, \ldots, P_n$ in the plane, and initial and final tangent directions $v_0$ and $v_n$ and find a sequence of piecewise circular arcs (with a given upper bound on curvature) that passes through the points in sequence, with the given initial and final tangents. ("Circular" here includes straight-line segments, which are intuitively the arcs of circles of infinite radius.)

If we can solve this one, then we've solved your problem, although its possible that the solution will not be particularly appealing to you, as it may involve some very long arcs (i.e., a waste of fuel). Now the problem can be reduced to

Given an initial and final point $P$ and $Q$, and initial and final directions $v$ and $w$ (as unit vectors), find a circle-arc sequence passing through these in the given directions."

Why? Because we can randomly assign directions to each of the points $P_1, \ldots, P_{n-1}$ in the original problem to reduce it to $n$ cases of this simpler problem.

Lemma: If $v = w$, this is easy.

Case 1: If $Q$ lies on the line determined by $P$ and $v$, simply draw a line segment from $P$ to $Q$.

Case 2: If not, toss in two more points, as shown in the following figure, placing the lower red point at the intersection of the line through $Q$ orthogonal to $w$ and the line through $P$ in the direction $v$. The upper red point is placed high enough to make the radii of the blue and purple arcs both be large enough that they meet the curvature bounds (i.e., the radii are both chosen to be at least $1/k$, where $k$ is the maximum allowed curvature).

---

Now what about the non-parallel case?

Name the line through $P$ in direction $v$ by the name $\ell$. Name the line through $Q$ perpendicular to $w$ by the name $m$.

Case 1: $\ell$ and $m$ are parallel. I leave this case to you.

Case 2: Let $Q'$ be the intersection of $\ell$ and $m$. Draw a ray parallel to $w$ at $Q'$. Then we can find a circle-arc path from $Q$ to $Q'$ with initial and final tangents $w$ and $w$ using the blue and purple arcs from the previous lemma. Thus we have reduced to the case where $Q$ is on the line $\ell$ through $P$ in direction $v$, and $w$ and $v$ are not parallel. By moving $Q$ along this line (i.e., driving the ship along a straight line), we may further assume that $Q \ne P$, indeed, that $Q$ and $P$ are as far apart as needed.

We're now in the situation depicted in the following figure. The setup is shown in the box at the top; the solution is drawn below it:

Draw a line through $P$ perpendicular to $\ell$ (vertical in the figure) and a line through $Q$ perpendicular to $w$. These intersect at some point $C$; by moving $Q$ along $\ell$ if necessary we may assume that $Q$ and $P$ are both distance at least $1/k$ from $C$. Letting $r$ be the distance from $C$ to $P$, draw a circle (aqua in the figure) of radius $r$ through $C$; it meets the line $QC$ at a point $R$ whose tangent is $w$ (it also meets at another point, with tangent $-w$, but I've not drawn that). The circle arc from $P$ to $R$ is now something the ship can follow. But then we can again use the first solution (blue and purple arcs) to connect $(R, w)$ with $(Q, w)$ to complete a circle-arc path from $P$ to $Q$.

And that completes the construction.

Answer 3

It seems to me that what you're trying to do is very similar to the kind of motion planning that's done with robots and automated guided vehicles in the manufacturing industry.

The idea is to plan a path that visits certain points, avoids certain obstacles, and mimimizes the time of travel. The naive minimum distance solution is just a sequence of straight lines. But this requires that the moving thing change direction instantaneously, which is impossible unless it reduces its speed to zero. The minimum time solution is a smooth curve of some kind, but computing it is a pretty difficult optimal control problem.

This is an important problem in manufacturing, and there is a huge body of literature on the subject. There is one paper here, but you'll find hundreds of other ones if you search for terms like "trajectory planning".