Python - OpenDrive Map - Spiral / Clothoid / Euler Spiral / Cornu Spiral Interpolation using Fresnel Integrals

Question

The map format OpenDrive, provides (among others) the geometry of a road. Each segment of the road can have a different geometry (e.g. line, arc, spiral, polynomial). The provided information for a road geometry "spiral", is the following:

 - s      - relative position of the road segment in respect to the beginning
                                                of the road (not used in here)
 - x      - the "x" position of the starting point of the road segment
 - y      - the "y" position of the starting point of the road segment
 - hdg       - the heading of the starting point of the road segment
 - length      - the length of the road segment
 - curvStart   - the curvature at the start of the road segment
 - curvEnd     - the curvature at the end of the road segment

My goal is to interpolate points along the spiral, given a "resolution" parameter (e.g. resolution = 1, interpolates a point along the spiral each meter). The spiral geometry is such that it introduces a constant change in curvature (1/radius), so that it creates a smooth and stable transition from a line to an arc, so that lateral acceleration forces on a vehicle are smaller then a transition from a line to an arc directly (line curvature = 0, arc curvature = constant).

The spiral always has one of it's ending points with a curvature of 0 (where it connects to the line segment of the road) and the other as a constant (e.g. 0.05 where it connects to an arc). Depending on the connection sequence, curvStart can be equal to 0 or constant and curvEnd can be also equal to 0 or constant. They cannot be both equal to 0 or constant at the same time.

The code below is a function that takes as arguments the previously discussed parameters (given by the format) and the resolution.

Currently, I am experiencing troubles with the following:

• Interpolate equidistant points of 1 meter apart (check plot 1)
• Obtain the correct heading of the points (check plot 2)
• Find a solution for the last 2 cases

From my research on how to fulfill the task, I came upon few helpful resources, but none of them helped me obtain the final solution:

• OpenDrive Specification
• Open source road generation and editing software - page 40(31)
• Euler Spiral Wiki
• Cephes library, from which the scipy.special.fresnel function is derived
• Klothoide - the German version of the "Clothoid" Wiki page has more formulas
• Parameterized function for clothoid
• SciPy: What are the arguments in scipy.special.fresnel(x\[, out1, out2\])? - where it is pointed out that "scipy implementation of the function scales the argument by pi/2"

import numpy as np
from math import cos, sin, pi, radians
from scipy.special import fresnel
import matplotlib.pyplot as plt
%matplotlib inline

def spiralInterpolation(resolution, s, x, y, hdg, length, curvStart, curvEnd):
    points = np.zeros((int(length/resolution), 1))
    points = [i*resolution for i in range(len(points))]
    xx = np.zeros_like(points)
    yy = np.zeros_like(points)
    hh = np.zeros_like(points)
    if curvStart == 0 and curvEnd > 0:
        print("Case 1: curvStart == 0 and curvEnd > 0")
        radius = np.abs(1/curvEnd)
        A_sq = radius*length
        ss, cc = fresnel(np.square(points)/(2*A_sq*np.sqrt(np.pi/2)))
        xx = points*cc
        yy = points*ss
        hh = np.square(points)*2*radius*length
        xx, yy, hh = rotate(xx, yy, hh, hdg)
        xx, yy = translate(xx, yy, x, y)
        xx = np.insert(xx, 0, x, axis=0)
        yy = np.insert(yy, 0, y, axis=0)
        hh = np.insert(hh, 0, hdg, axis=0)

    elif curvStart == 0 and curvEnd < 0:
        print("Case 2: curvStart == 0 and curvEnd < 0")
        radius = np.abs(1/curvEnd)
        A_sq = radius*length
        ss, cc = fresnel(np.square(points)/(2*A_sq*np.sqrt(np.pi/2)))
        xx = points*cc
        yy = points*ss*-1
        hh = np.square(points)*2*radius*length
        xx, yy, hh = rotate(xx, yy, hh, hdg)
        xx, yy = translate(xx, yy, x, y)
        xx = np.insert(xx, 0, x, axis=0)
        yy = np.insert(yy, 0, y, axis=0)
        hh = np.insert(hh, 0, hdg, axis=0)

    elif curvEnd == 0 and curvStart > 0:
        print("Case 3: curvEnd == 0 and curvStart > 0")

    elif curvEnd == 0 and curvStart < 0:
        print("Case 4: curvEnd == 0 and curvStart < 0")

    else:
        print("The curvature parameters differ from the 4 predefined cases. "
              "Change curvStart and/or curvEnd")

    n_stations = int(length/resolution) + 1
    stations = np.zeros((n_stations, 3))
    for i in range(len(xx)):
        stations[i][0] = xx[i]
        stations[i][1] = yy[i]
        stations[i][2] = hh[i]

    return stations

def rotate(x, y, h, angle):
    # This function rotates the x and y vectors around zero
    xx = np.zeros_like(x)
    yy = np.zeros_like(y)
    hh = np.zeros_like(h)
    for i in range(len(x)):
        xx[i] = x[i]*cos(angle) - y[i]*sin(angle)
        yy[i] = x[i]*sin(angle) + y[i]*cos(angle)
        hh[i] = h[i] + angle
    return xx, yy, hh

def translate(x, y, x_delta, y_delta):
    # This function translates the x and y vectors with the delta values
    xx = np.zeros_like(x)
    yy = np.zeros_like(y)
    for i in range(len(x)):
        xx[i] = x[i] + x_delta
        yy[i] = y[i] + y_delta 
    return xx, yy

stations = spiralInterpolation(1, 77, 50, 100, radians(56), 40, 0, 1/20)

x = []
y = []
h = []

for station in stations:
    x.append(station[0])
    y.append(station[1])
    h.append(station[2])

plt.figure(figsize=(20,13))
plt.plot(x, y, '.')
plt.grid(True)
plt.axis('equal')
plt.show()

def get_heading_components(x, y, h, length=1):
    xa = np.zeros_like(x)
    ya = np.zeros_like(y)
    for i in range(len(x)):
        xa[i] = length*cos(h[i])
        ya[i] = length*sin(h[i])
    return xa, ya

xa, ya = get_heading_components(x, y, h)
plt.figure(figsize=(20,13))
plt.quiver(x, y, xa, ya, width=0.005)
plt.grid(True)
plt.axis('equal')
plt.show()

Answer 1

I am not sure if your current code is correct. I wrote a short script to interpolate Euler spirals using similar parameters and it gives different results:

import numpy as np
from math import cos, sin, pi, radians, sqrt
from scipy.special import fresnel
import matplotlib.pyplot as plt

def spiral_interp_centre(distance, x, y, hdg, length, curvEnd):
    '''Interpolate for a spiral centred on the origin'''
    # s doesn't seem to be needed...
    theta = hdg                    # Angle of the start of the curve
    Ltot = length                  # Length of curve
    Rend = 1 / curvEnd             # Radius of curvature at end of spiral

    # Rescale, compute and unscale
    a = 1 / sqrt(2 * Ltot * Rend)  # Scale factor
    distance_scaled = distance * a # Distance along normalised spiral
    deltay_scaled, deltax_scaled = fresnel(distance_scaled)
    deltax = deltax_scaled / a
    deltay = deltay_scaled / a

    # deltax and deltay give coordinates for theta=0
    deltax_rot = deltax * cos(theta) - deltay * sin(theta)
    deltay_rot = deltax * sin(theta) + deltay * cos(theta)

    # Spiral is relative to the starting coordinates
    xcoord = x + deltax_rot
    ycoord = y + deltay_rot

    return xcoord, ycoord

fig = plt.figure()
ax = fig.add_subplot(1, 1, 1)

# This version
xs = []
ys = []
for n in range(-100, 100+1):
    x, y = spiral_interp_centre(n, 50, 100, radians(56), 40, 1/20.)
    xs.append(x)
    ys.append(y)
ax.plot(xs, ys)

# Your version
from yourspiral import spiralInterpolation
stations = spiralInterpolation(1, 77, 50, 100, radians(56), 40, 0, 1/20.)
ax.plot(stations[:,0], stations[:,1])

ax.legend(['My spiral', 'Your spiral'])
fig.savefig('spiral.png')
plt.show()

With this I get

So which is correct?

Also in the case that the curvature is zero at the end and non-zero at the start what does hdg represent? Is it the angle of the start or end of the curve? Your function also takes an argument s which is unused. Is it supposed to be relevant?

If your example code showed a plot of the line segments before and after the spiral segments then it would be easier to see which is correct and to know the meaning of each of the parameters.

Answer 2

Just posting a correction to Oscar's answer. There are two erroneous parts:

• The scaling factor should be a = 1/sqrt(math.pi * arcLength * Radius) because scipy.special.fresnel uses cos(pi*t*t/2) and sin(pi*t*t/2). So, curvature becomes pi*s rather than s where s is arc length (Wikipedia).
• I removed the length parameter of spiral_interp_centre because scaling (explained below in the code comments) must use the required arc length.

The scaling error does not affect the arc length obtained from spiral_interp_centre but it affects the accuracy of the obtained curvature. To verify, change scalar in the code below from math.pi to 2 (the value used in Oscar's answer). The arc lengths (printed below) remain unchanged but the curvature changes (becomes different from the required value).

import math
import scipy.special
import matplotlib.pyplot as plt
import numpy as np

def arcLength(XY):
    return np.sum(np.hypot(np.diff(XY[:, 0]), np.diff(XY[:, 1])))

def getAreaOfTriangle(XY, i, j, k):
    xa, ya = XY[i, 0], XY[i, 1]
    xb, yb = XY[j, 0], XY[j, 1]
    xc, yc = XY[k, 0], XY[k, 1]
    return abs((xa * (yb - yc) + xb * (yc - ya) + xc * (ya - yb)) / 2)

def distance(XY, i, j):
    return np.linalg.norm(XY[i, :] - XY[j, :])

def getCurvatureUsingTriangle(XY, i, j, k):
    fAreaOfTriangle = getAreaOfTriangle(XY, i, j, k)
    AB = distance(XY, i, j)
    BC = distance(XY, j, k)
    CA = distance(XY, k, i)
    fKappa = 4 * fAreaOfTriangle / (AB * BC * CA)
    return fKappa

def spiral_interp_centre(arcLength, x_i, y_i, yaw_i, curvEnd, N=300):
    '''
    :param arcLength: Desired length of the spiral
    :param x_i: x-coordinate of initial point
    :param y_i: y-coordinate of initial point
    :param yaw_i: Initial yaw angle in radians
    :param curvEnd: Curvature at the end of the curve.
    :return:
    '''
    # Curvature along the Euler spiral is pi*t where t is the Fresnel integral limit.
    # curvEnd = 1/R
    # s = arcLength
    # t = Fresnel integral limit
    # Scalar a is used to find t such that (1/(a*R) = pi*t) and (a*s = t)
    # ====> 1/(pi*a*R) = a*s
    # ====> a^a*(pi*s*R)
    # ====> a = 1/sqrt(pi*s*R)
    # To achieve a specific curvature at a specific arc length, we must scale
    # the Fresnel integration limit
    scalar = math.pi
    distances = np.linspace(start=0.0, stop=arcLength, num=N)
    R = 1 / curvEnd  # Radius of curvature at end of spiral
    # Rescale, compute and unscale
    a = 1 / math.sqrt(scalar * arcLength * R) # Scale factor
    scaled_distances = a * distances # Distance along normalized spiral
    dy_scaled, dx_scaled = scipy.special.fresnel(scaled_distances)

    dx = dx_scaled / a
    dy = dy_scaled / a

    # Rotate the whole curve by yaw_i
    dx_rot = dx * math.cos(yaw_i) - dy * math.sin(yaw_i)
    dy_rot = dx * math.sin(yaw_i) + dy * math.cos(yaw_i)

    # Translate to (x_i, y_i)
    x = x_i + dx_rot
    y = y_i + dy_rot
    return np.concatenate((x[:, np.newaxis], y[:, np.newaxis]), axis=1)

fig = plt.figure()
ax = fig.add_subplot(1, 1, 1)
ax.set_aspect('equal')
R = 20.0
for d in range(400, 600, 20):
    XY = spiral_interp_centre(d, 50, 100, math.radians(56), 1/R, N=300)
    ax.plot(XY[:, 0], XY[:, 1])
    print('d={:.3f}, dd={:.3f}, R={:.3f}, RR={:.3f}'.format(d, arcLength(XY), R, 1/getCurvatureUsingTriangle(XY, 299, 298, 297)))
plt.show()