Question

地图格式OpenDrive，提供（以及其他）道路的几何形状。道路的每个区段可以具有不同的几何形状（例如，线，弧，螺旋，多项式）。提供的道路几何形状信息＆＃34;螺旋＆＃34;，如下：

 - 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

我的目标是在螺旋上插入点，给出一个＆＃34;分辨率＆＃34;参数（例如，分辨率= 1，沿着螺旋线每个仪表插入一个点）。螺旋几何形状使得它引入曲率的恒定变化（1 /半径），从而创建从线到弧的平滑且稳定的过渡，使得车辆上的横向加速力小于从...的过渡。直接线到弧（线曲率= 0，弧曲率=常数）。

螺旋总是有一个曲率为0的终点（它连接到道路的线段），另一个作为常数（例如0.05，它连接到弧线）。根据连接顺序，curvStart可以等于0或常数，而curvEnd也可以等于0或常数。它们不能同时等于0或恒定。

下面的代码是一个函数，它将前面讨论的参数（由格式给出）和分辨率作为参数。

目前，我遇到以下问题：

插入相距1米的等距点（见图1）
获取积分的正确标题（查看图2）
查找最近2个案例的解决方案

根据我对如何完成任务的研究，我得到了一些有用的资源，但没有一个帮助我获得最终解决方案：

OpenDrive Specification
Open source road generation and editing software - 第40页（31）
Euler Spiral Wiki
Cephes library，scipy.special.fresnel函数从中生成
Klothoide - ＆＃34; Clothoid＆＃34;的德语版本Wiki页面有更多公式
Parameterized function for clothoid
SciPy: What are the arguments in scipy.special.fresnel(x\[, out1, out2\])? - 指出＆＃34;该功能的scipy实现按pi / 2＆＃34;

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，其中 s 是弧长 (Wikipedia)。
我删除了 length 的 spiral_interp_centre 参数，因为缩放（在下面的代码注释中解释）必须使用所需的弧长。

缩放误差不影响从spiral_interp_centre得到的弧长，但会影响得到曲率的精度。要进行验证，请将以下代码中的 scalar 从 math.pi 更改为 2（Oscar 的答案中使用的值）。弧长（打印如下）保持不变，但曲率发生变化（与所需值不同）。

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

Python - OpenDrive Map - 使用菲涅耳积分的螺旋/ Clothoid / Euler螺旋/ Curu螺旋插值

2 个答案: