Python - Карта OpenDrive - Спиральная / Клотооидная / Спираль Эйлера / Спиральная интерполяция Куру с использованием интегралов Френеля

Question

Формат карты OpenDrive предоставляет (среди прочего) геометрию дороги. Каждый отрезок дороги может иметь различную геометрию (например, линия, дуга, спираль, полином). Предоставленная информация для геометрии дороги "спираль", является следующей:

 - 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

Моя цель - интерполировать точки вдоль спирали, учитывая параметр "разрешение" (например, разрешение = 1, интерполирует точку вдоль спирали каждый метр). Спиральная геометрия такова, что она вносит постоянное изменение кривизны (1/ радиус), так что она создает плавный и устойчивый переход от линии к дуге, так что силы поперечного ускорения на транспортном средстве меньше, чем переход от прямая линия к дуге (кривизна линии = 0, кривизна дуги = постоянная).

Спираль всегда имеет одну из своих конечных точек с кривизной 0 (где она соединяется с отрезком линии дороги), а другую - как постоянную (например, 0,05, где она соединяется с дугой). В зависимости от последовательности соединения curvStart может быть равно 0 или константе, а curvEnd также может быть равно 0 или константе. Они не могут быть равны 0 или постоянны одновременно.

Приведенный ниже код является функцией, которая принимает в качестве аргументов ранее обсужденные параметры (заданные форматом) и разрешение.

В настоящее время у меня возникают проблемы со следующим:

• Интерполировать равноудаленные точки на расстоянии 1 метра (проверьте график 1)
• Получить правильный заголовок точек (проверьте график 2)
• Найти решение для последних 2 случаев

Из моих исследований о том, как выполнить задачу, я нашел несколько полезных ресурсов, но ни один из них не помог мне получить окончательное решение:

• OpenDrive Спецификация
• Программное обеспечение для создания и редактирования дорог с открытым исходным кодом - стр. 40(31)
• Эйлер Спираль Вики
• Библиотека Cephes, из которой получена функция scipy.special.fresnel
• Klothoide - немецкая версия вики-страницы "Clothoid" имеет больше формул
• Параметризованная функция для клотоиды
• SciPy: Каковы аргументы в scipy.special.fresnel(x\[, out1, out2\]) ? - где указано, что "реализация функции scipy масштабирует аргумент до 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

Я не уверен, что ваш текущий код правильный. Я написал короткий скрипт для интерполяции спиралей Эйлера с использованием одинаковых параметров, и он дает разные результаты:

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

С этим я получаю

Так что правильно?

Также в случае, если кривизна равна нулю в конце и не равна нулю в начале, что делает hdg представлять? Это угол начала или конца кривой? Ваша функция также принимает аргумент s который не используется. Это должно быть актуально?

Если в вашем примере кода показан график отрезков линии до и после спиральных отрезков, то было бы легче увидеть, какой из них правильный, и узнать значение каждого из параметров.

Answer 2

Просто отправляю исправление к ответу Оскара. Есть две ошибочные части:

• Коэффициент масштабирования должен быть a = 1/sqrt(math.pi * arcLength * Radius) потому что scipy.special.fresnel использует cos(pi*t*t/2) и sin(pi*t*t/2). Итак, кривизна становится pi*s а не где sдлина дуги ( Википедия ).
• Я удалил length параметр, потому что масштабирование (объясненное ниже в комментариях к коду) должно использовать требуемую длину дуги.

Ошибка масштабирования не влияет на длину дуги, полученную из spiral_interp_centreно это влияет на точность полученной кривизны. Чтобы проверить, измените scalar в приведенном ниже коде из math.piдо 2 (значение, использованное в ответе Оскара). Длины дуги (указаны ниже) остаются неизменными, но кривизна изменяется (становится отличной от требуемого значения).

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