Python - OpenDrive 地图 - 使用菲涅尔积分的螺旋线/回旋线/欧拉螺旋线/Cornu 螺旋线插值
Python - OpenDrive Map - Spiral / Clothoid / Euler Spiral / Cornu Spiral Interpolation using Fresnel Integrals
地图格式 OpenDrive 提供（除其他外）道路的几何形状。道路的每一段都可以有不同的几何形状（例如线、弧、螺旋、多项式）。为道路几何形状 "spiral" 提供的信息如下：
- 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
我的目标是在给定 "resolution" 参数的情况下沿螺旋线插值点（例如分辨率 = 1，沿螺旋线每米插值一个点）。
螺旋几何形状会引入曲率（1/半径）的恒定变化，从而创建从直线到弧形的平滑稳定过渡，从而使车辆上的横向加速度力小于从直线的过渡直接到弧线（线曲率 = 0，弧曲率 = 常数）。
螺旋线的终点之一的曲率始终为 0（它连接到道路的线段），另一个终点为常数（例如，0.05，它连接到弧线）。根据连接顺序，curvStart 可以等于 0 或常数，curvEnd 也可以等于 0 或常数。它们不能同时等于 0 或常数。
下面的代码是一个函数，它将前面讨论的参数（由格式给出）和分辨率作为参数。
目前，我遇到以下问题：
- 内插相距 1 米的等距点（检查绘图 1）
- 获取点的正确航向（检查图 2）
- 找到最后 2 个案例的解决方案
从我对如何完成任务的研究中，我发现了一些有用的资源，但其中 none 帮助我获得了最终解决方案：
- OpenDrive Specification
- Open source road generation and editing software - 第 40(31) 页
- Euler Spiral Wiki
- Cephes library，scipy.special.fresnel函数从中导出
- Klothoide - "Clothoid" Wiki 页面的德文版有更多公式
- Parameterized function for clothoid
- SciPy: What are the arguments in
scipy.special.fresnel(x\[, out1, out2\])? - 指出 "scipy implementation of the function scales the argument by 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()
有了这个我得到了
那么哪个是正确的？
另外如果曲率在最后为零，在开始时为non-zero，hdg代表什么？它是曲线起点还是终点的角度？您的函数还带有一个未使用的参数 s 。它应该是相关的吗？
如果您的示例代码显示了螺旋线段前后的线段图，那么将更容易看出哪个是正确的，并了解每个参数的含义。
刚刚发布了对 Oscar 回答的更正。有两处错误：
- 比例因子应为
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)。
- 我删除了
spiral_interp_centre 的 length 参数，因为缩放（在下面的代码注释中解释）必须使用所需的弧长。
缩放误差不影响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()
地图格式 OpenDrive 提供（除其他外）道路的几何形状。道路的每一段都可以有不同的几何形状（例如线、弧、螺旋、多项式）。为道路几何形状 "spiral" 提供的信息如下：
- 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
我的目标是在给定 "resolution" 参数的情况下沿螺旋线插值点（例如分辨率 = 1，沿螺旋线每米插值一个点）。 螺旋几何形状会引入曲率（1/半径）的恒定变化，从而创建从直线到弧形的平滑稳定过渡，从而使车辆上的横向加速度力小于从直线的过渡直接到弧线（线曲率 = 0，弧曲率 = 常数）。
螺旋线的终点之一的曲率始终为 0（它连接到道路的线段），另一个终点为常数（例如，0.05，它连接到弧线）。根据连接顺序，curvStart 可以等于 0 或常数，curvEnd 也可以等于 0 或常数。它们不能同时等于 0 或常数。
下面的代码是一个函数，它将前面讨论的参数（由格式给出）和分辨率作为参数。
目前，我遇到以下问题：
- 内插相距 1 米的等距点（检查绘图 1）
- 获取点的正确航向（检查图 2）
- 找到最后 2 个案例的解决方案
从我对如何完成任务的研究中，我发现了一些有用的资源，但其中 none 帮助我获得了最终解决方案：
- OpenDrive Specification
- Open source road generation and editing software - 第 40(31) 页
- Euler Spiral Wiki
- Cephes library，scipy.special.fresnel函数从中导出
- Klothoide - "Clothoid" Wiki 页面的德文版有更多公式
- Parameterized function for clothoid
- SciPy: What are the arguments in
scipy.special.fresnel(x\[, out1, out2\])? - 指出 "scipy implementation of the function scales the argument by 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()
有了这个我得到了
那么哪个是正确的？
另外如果曲率在最后为零，在开始时为non-zero，hdg代表什么？它是曲线起点还是终点的角度？您的函数还带有一个未使用的参数 s 。它应该是相关的吗？
如果您的示例代码显示了螺旋线段前后的线段图，那么将更容易看出哪个是正确的，并了解每个参数的含义。
刚刚发布了对 Oscar 回答的更正。有两处错误：
- 比例因子应为
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)。 - 我删除了
spiral_interp_centre的length参数，因为缩放（在下面的代码注释中解释）必须使用所需的弧长。
缩放误差不影响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()