pythonscipyspiralfresnel

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


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:

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:

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()

Solution

  • 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

    Plot from above code

    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.