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.