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

Posted 2021-04-26

地图格式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 Specification
• Open source road generation and editing software - 第40页（31）
• Euler Spiral Wiki
• qazxsw poi，从中派生出scipy.special.fresnel函数
• Cephes library - “Clothoid”维基页面的德语版本有更多的公式
• Klothoide
• Parameterized function for clothoid - 指出“功能的scipy实现按pi / 2扩展参数”

SciPy: What are the arguments in scipy.special.fresnel(x[, out1, out2])?

答案

我不确定您当前的代码是否正确。我写了一个简短的脚本来使用类似的参数插入欧拉螺旋，它给出了不同的结果：

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

有了这个，我得到了

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。它应该是相关的吗？

如果您的示例代码显示了螺旋段之前和之后的线段图，那么将更容易看出哪个是正确的并且知道每个参数的含义。