"""
Clothoid Path Planner
Author: Daniel Ingram (daniel-s-ingram)
Atsushi Sakai (AtsushiSakai)
Reference paper: Fast and accurate G1 fitting of clothoid curves
https://www.researchgate.net/publication/237062806
"""
from collections import namedtuple
import matplotlib.pyplot as plt
import numpy as np
import scipy.integrate as integrate
from scipy.optimize import fsolve
from math import atan2, cos, hypot, pi, sin
from matplotlib import animation
Point = namedtuple("Point", ["x", "y"])
show_animation = True
def generate_clothoid_paths(start_point, start_yaw_list,
goal_point, goal_yaw_list,
n_path_points):
"""
Generate clothoid path list. This function generate multiple clothoid paths
from multiple orientations(yaw) at start points to multiple orientations
(yaw) at goal point.
:param start_point: Start point of the path
:param start_yaw_list: Orientation list at start point in radian
:param goal_point: Goal point of the path
:param goal_yaw_list: Orientation list at goal point in radian
:param n_path_points: number of path points
:return: clothoid path list
"""
clothoids = []
for start_yaw in start_yaw_list:
for goal_yaw in goal_yaw_list:
clothoid = generate_clothoid_path(start_point, start_yaw,
goal_point, goal_yaw,
n_path_points)
clothoids.append(clothoid)
return clothoids
def generate_clothoid_path(start_point, start_yaw,
goal_point, goal_yaw, n_path_points):
"""
Generate a clothoid path list.
:param start_point: Start point of the path
:param start_yaw: Orientation at start point in radian
:param goal_point: Goal point of the path
:param goal_yaw: Orientation at goal point in radian
:param n_path_points: number of path points
:return: a clothoid path
"""
dx = goal_point.x - start_point.x
dy = goal_point.y - start_point.y
r = hypot(dx, dy)
phi = atan2(dy, dx)
phi1 = normalize_angle(start_yaw - phi)
phi2 = normalize_angle(goal_yaw - phi)
delta = phi2 - phi1
try:
# Step1: Solve g function
A = solve_g_for_root(phi1, phi2, delta)
# Step2: Calculate path parameters
L = compute_path_length(r, phi1, delta, A)
curvature = compute_curvature(delta, A, L)
curvature_rate = compute_curvature_rate(A, L)
except Exception as e:
print(f"Failed to generate clothoid points: {e}")
return None
# Step3: Construct a path with Fresnel integral
points = []
for s in np.linspace(0, L, n_path_points):
try:
x = start_point.x + s * X(curvature_rate * s ** 2, curvature * s,
start_yaw)
y = start_point.y + s * Y(curvature_rate * s ** 2, curvature * s,
start_yaw)
points.append(Point(x, y))
except Exception as e:
print(f"Skipping failed clothoid point: {e}")
return points
def X(a, b, c):
return integrate.quad(lambda t: cos((a/2)*t**2 + b*t + c), 0, 1)[0]
def Y(a, b, c):
return integrate.quad(lambda t: sin((a/2)*t**2 + b*t + c), 0, 1)[0]
def solve_g_for_root(theta1, theta2, delta):
initial_guess = 3*(theta1 + theta2)
return fsolve(lambda A: Y(2*A, delta - A, theta1), [initial_guess])
def compute_path_length(r, theta1, delta, A):
return r / X(2*A, delta - A, theta1)
def compute_curvature(delta, A, L):
return (delta - A) / L
def compute_curvature_rate(A, L):
return 2 * A / (L**2)
def normalize_angle(angle_rad):
return (angle_rad + pi) % (2 * pi) - pi
def get_axes_limits(clothoids):
x_vals = [p.x for clothoid in clothoids for p in clothoid]
y_vals = [p.y for clothoid in clothoids for p in clothoid]
x_min = min(x_vals)
x_max = max(x_vals)
y_min = min(y_vals)
y_max = max(y_vals)
x_offset = 0.1*(x_max - x_min)
y_offset = 0.1*(y_max - y_min)
x_min = x_min - x_offset
x_max = x_max + x_offset
y_min = y_min - y_offset
y_max = y_max + y_offset
return x_min, x_max, y_min, y_max
def draw_clothoids(start, goal, num_steps, clothoidal_paths,
save_animation=False):
fig = plt.figure(figsize=(10, 10))
x_min, x_max, y_min, y_max = get_axes_limits(clothoidal_paths)
axes = plt.axes(xlim=(x_min, x_max), ylim=(y_min, y_max))
axes.plot(start.x, start.y, 'ro')
axes.plot(goal.x, goal.y, 'ro')
lines = [axes.plot([], [], 'b-')[0] for _ in range(len(clothoidal_paths))]
def animate(i):
for line, clothoid_path in zip(lines, clothoidal_paths):
x = [p.x for p in clothoid_path[:i]]
y = [p.y for p in clothoid_path[:i]]
line.set_data(x, y)
return lines
anim = animation.FuncAnimation(
fig,
animate,
frames=num_steps,
interval=25,
blit=True
)
if save_animation:
anim.save('clothoid.gif', fps=30, writer="imagemagick")
plt.show()
def main():
start_point = Point(0, 0)
start_orientation_list = [0.0]
goal_point = Point(10, 0)
goal_orientation_list = np.linspace(-pi, pi, 75)
num_path_points = 100
clothoid_paths = generate_clothoid_paths(
start_point, start_orientation_list,
goal_point, goal_orientation_list,
num_path_points)
if show_animation:
draw_clothoids(start_point, goal_point,
num_path_points, clothoid_paths,
save_animation=False)
if __name__ == "__main__":
main()
