map 格式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或常数。下面的代码是一个函数，该函数将先前讨论的参数(由格式赋予)和分辨率作为参数。
目前，我遇到以下问题:
通过对如何完成任务的研究，我发现了一些有用的资源，但是没有一个能帮助我获得最终的解决方案:
scipy.special.fresnel(x\[, out1, out2\]) ?-指出“该函数的科学实现将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()
最佳答案
我不确定您当前的代码是否正确。我编写了一个简短的脚本，使用相似的参数来插值欧拉螺旋线，并且得出不同的结果:
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。它应该相关吗？如果您的示例代码显示了螺旋线段之前和之后的线段图，那么将更容易看出哪个是正确的，并且更容易知道每个参数的含义。
关于Python-OpenDrive映射-使用菲涅尔积分的螺旋/类波/欧拉螺旋/Cornu螺旋插值，我们在Stack Overflow上找到一个类似的问题：https://stackoverflow.com/questions/48884655/