Pure Pursuit Simulation¶

Verify pure pursuit algorithm in different scenarios.

In [1]:

import numpy as np
import matplotlib.pyplot as plt
import sys
sys.path.insert(0, '.')
from utils import *

import numpy as np import matplotlib.pyplot as plt import sys sys.path.insert(0, '.') from utils import *

1. Circle Path Tracking¶

In [2]:

# Generate half-circle path (radius 2m)
path_circle = circle_path(radius=2.0, num_points=200, start_angle=0, end_angle=np.pi)
print(f"path_circle: shape={path_circle.shape}")
print(f"  range_x: [{path_circle[:,0].min():.2f}, {path_circle[:,0].max():.2f}]")
print(f"  range_y: [{path_circle[:,1].min():.2f}, {path_circle[:,1].max():.2f}]")

# Initial pose: left side of circle, facing up
x0, y0 = path_circle[0]
theta0 = np.arctan2(path_circle[10, 1] - path_circle[0, 1], path_circle[10, 0] - path_circle[0, 0])
initial_pose = (x0, y0, theta0)

print(f"Initial pose: x={x0:.2f}, y={y0:.2f}, theta={np.degrees(theta0):.1f} deg")

# Generate half-circle path (radius 2m) path_circle = circle_path(radius=2.0, num_points=200, start_angle=0, end_angle=np.pi) print(f"path_circle: shape={path_circle.shape}") print(f" range_x: [{path_circle[:,0].min():.2f}, {path_circle[:,0].max():.2f}]") print(f" range_y: [{path_circle[:,1].min():.2f}, {path_circle[:,1].max():.2f}]") # Initial pose: left side of circle, facing up x0, y0 = path_circle[0] theta0 = np.arctan2(path_circle[10, 1] - path_circle[0, 1], path_circle[10, 0] - path_circle[0, 0]) initial_pose = (x0, y0, theta0) print(f"Initial pose: x={x0:.2f}, y={y0:.2f}, theta={np.degrees(theta0):.1f} deg")

path_circle: shape=(200, 2)
  range_x: [-2.00, 2.00]
  range_y: [0.00, 2.00]
Initial pose: x=2.00, y=0.00, theta=94.5 deg

In [3]:

# Run pure pursuit
v = 0.5  # m/s
L = 0.5  # lookahead distance

traj_circle = run_pure_pursuit(path_circle, v=v, L=L)

print(f"traj_circle: shape={traj_circle.shape}")
print(f"  range_x: [{traj_circle[:,0].min():.2f}, {traj_circle[:,0].max():.2f}]")
print(f"  range_y: [{traj_circle[:,1].min():.2f}, {traj_circle[:,1].max():.2f}]")
print(f"  range_theta: [{np.degrees(traj_circle[:,2].min()):.1f}, {np.degrees(traj_circle[:,2].max()):.1f}] deg")

# Run pure pursuit v = 0.5 # m/s L = 0.5 # lookahead distance traj_circle = run_pure_pursuit(path_circle, v=v, L=L) print(f"traj_circle: shape={traj_circle.shape}") print(f" range_x: [{traj_circle[:,0].min():.2f}, {traj_circle[:,0].max():.2f}]") print(f" range_y: [{traj_circle[:,1].min():.2f}, {traj_circle[:,1].max():.2f}]") print(f" range_theta: [{np.degrees(traj_circle[:,2].min()):.1f}, {np.degrees(traj_circle[:,2].max()):.1f}] deg")

traj_circle: shape=(12642, 3)
  range_x: [-2.01, 2.00]
  range_y: [-0.04, 2.00]
  range_theta: [90.5, 276.4] deg

In [4]:

# Plot
plt.figure(figsize=(8, 8))
plt.plot(path_circle[:, 0], path_circle[:, 1], 'b--', label='Reference', linewidth=2)
plt.plot(traj_circle[:, 0], traj_circle[:, 1], 'r-', label='Actual', linewidth=1.5)
plt.plot(traj_circle[0, 0], traj_circle[0, 1], 'go', markersize=10, label='Start')
plt.plot(traj_circle[-1, 0], traj_circle[-1, 1], 'ro', markersize=10, label='End')
plt.axis('equal')
plt.xlabel('X (m)')
plt.ylabel('Y (m)')
plt.title(f'Pure Pursuit - Circle (v={v}m/s, L={L}m)')
plt.legend()
plt.grid(True)
plt.show()

# Plot plt.figure(figsize=(8, 8)) plt.plot(path_circle[:, 0], path_circle[:, 1], 'b--', label='Reference', linewidth=2) plt.plot(traj_circle[:, 0], traj_circle[:, 1], 'r-', label='Actual', linewidth=1.5) plt.plot(traj_circle[0, 0], traj_circle[0, 1], 'go', markersize=10, label='Start') plt.plot(traj_circle[-1, 0], traj_circle[-1, 1], 'ro', markersize=10, label='End') plt.axis('equal') plt.xlabel('X (m)') plt.ylabel('Y (m)') plt.title(f'Pure Pursuit - Circle (v={v}m/s, L={L}m)') plt.legend() plt.grid(True) plt.show()

2. Figure-8 Path Tracking¶

In [5]:

# Generate figure-8 path
path_fig8 = figure8_path(num_points=300, scale=2.0)
print(f"path_fig8: shape={path_fig8.shape}")
print(f"  range_x: [{path_fig8[:,0].min():.2f}, {path_fig8[:,0].max():.2f}]")
print(f"  range_y: [{path_fig8[:,1].min():.2f}, {path_fig8[:,1].max():.2f}]")

# Generate figure-8 path path_fig8 = figure8_path(num_points=300, scale=2.0) print(f"path_fig8: shape={path_fig8.shape}") print(f" range_x: [{path_fig8[:,0].min():.2f}, {path_fig8[:,0].max():.2f}]") print(f" range_y: [{path_fig8[:,1].min():.2f}, {path_fig8[:,1].max():.2f}]")

path_fig8: shape=(300, 2)
  range_x: [-2.00, 2.00]
  range_y: [-1.00, 1.00]

In [6]:

# Figure-8 tracking - compare different lookahead distances
v = 0.5
L_values = [0.3, 0.5, 0.8]

# Store trajectories in dict for inspection
trajectories = {}

plt.figure(figsize=(10, 8))
plt.plot(path_fig8[:, 0], path_fig8[:, 1], 'k--', label='Reference', linewidth=2, alpha=0.5)

for L in L_values:
    traj = run_pure_pursuit(path_fig8, v=v, L=L)
    trajectories[f'L_{L}'] = traj  # Store for inspection
    plt.plot(traj[:, 0], traj[:, 1], label=f'L={L}m', linewidth=1.5)

plt.axis('equal')
plt.xlabel('X (m)')
plt.ylabel('Y (m)')
plt.title(f'Pure Pursuit - Figure-8 (v={v}m/s)')
plt.legend()
plt.grid(True)
plt.show()

# Inspect stored data
for key, traj in trajectories.items():
    print(f"{key}: shape={traj.shape}, range_x=[{traj[:,0].min():.2f}, {traj[:,0].max():.2f}], range_y=[{traj[:,1].min():.2f}, {traj[:,1].max():.2f}]")

# Figure-8 tracking - compare different lookahead distances v = 0.5 L_values = [0.3, 0.5, 0.8] # Store trajectories in dict for inspection trajectories = {} plt.figure(figsize=(10, 8)) plt.plot(path_fig8[:, 0], path_fig8[:, 1], 'k--', label='Reference', linewidth=2, alpha=0.5) for L in L_values: traj = run_pure_pursuit(path_fig8, v=v, L=L) trajectories[f'L_{L}'] = traj # Store for inspection plt.plot(traj[:, 0], traj[:, 1], label=f'L={L}m', linewidth=1.5) plt.axis('equal') plt.xlabel('X (m)') plt.ylabel('Y (m)') plt.title(f'Pure Pursuit - Figure-8 (v={v}m/s)') plt.legend() plt.grid(True) plt.show() # Inspect stored data for key, traj in trajectories.items(): print(f"{key}: shape={traj.shape}, range_x=[{traj[:,0].min():.2f}, {traj[:,0].max():.2f}], range_y=[{traj[:,1].min():.2f}, {traj[:,1].max():.2f}]")

L_0.3: shape=(24076, 3), range_x=[-2.00, 2.00], range_y=[-0.99, 0.98]
L_0.5: shape=(23838, 3), range_x=[-2.02, 2.02], range_y=[-0.97, 0.95]
L_0.8: shape=(23322, 3), range_x=[-2.06, 2.06], range_y=[-0.93, 0.87]

3. Parameter Sensitivity Analysis¶

In [7]:

# Compute tracking error for different L
def compute_tracking_error(traj, path):
    """Compute average minimum distance from trajectory to path"""
    errors = []
    for point in traj:
        dists = np.hypot(path[:, 0] - point[0], path[:, 1] - point[1])
        errors.append(np.min(dists))
    return np.mean(errors), np.max(errors)

L_range = np.linspace(0.2, 1.5, 14)
mean_errors = []
max_errors = []

for L in L_range:
    traj = run_pure_pursuit(path_fig8, v=0.5, L=L, max_steps=15000)
    mean_err, max_err = compute_tracking_error(traj, path_fig8)
    mean_errors.append(mean_err)
    max_errors.append(max_err)

# Store error data
error_data = np.column_stack([L_range, mean_errors, max_errors])
print(f"error_data: shape={error_data.shape}")
print("Columns: L, mean_error, max_error")
print(error_data[:5])  # Show first 5 rows

# Compute tracking error for different L def compute_tracking_error(traj, path): """Compute average minimum distance from trajectory to path""" errors = [] for point in traj: dists = np.hypot(path[:, 0] - point[0], path[:, 1] - point[1]) errors.append(np.min(dists)) return np.mean(errors), np.max(errors) L_range = np.linspace(0.2, 1.5, 14) mean_errors = [] max_errors = [] for L in L_range: traj = run_pure_pursuit(path_fig8, v=0.5, L=L, max_steps=15000) mean_err, max_err = compute_tracking_error(traj, path_fig8) mean_errors.append(mean_err) max_errors.append(max_err) # Store error data error_data = np.column_stack([L_range, mean_errors, max_errors]) print(f"error_data: shape={error_data.shape}") print("Columns: L, mean_error, max_error") print(error_data[:5]) # Show first 5 rows

error_data: shape=(14, 3)
Columns: L, mean_error, max_error
[[0.2        0.0118103  0.02968971]
 [0.3        0.01365421 0.02963404]
 [0.4        0.01763345 0.03742994]
 [0.5        0.02384418 0.05796176]
 [0.6        0.03211582 0.08421323]]

In [8]:

# Plot error curve
plt.figure(figsize=(10, 5))
plt.plot(L_range, mean_errors, 'b-o', label='Mean Error')
plt.plot(L_range, max_errors, 'r-s', label='Max Error')
plt.xlabel('Lookahead Distance L (m)')
plt.ylabel('Tracking Error (m)')
plt.title('Effect of Lookahead Distance on Tracking Error')
plt.legend()
plt.grid(True)
plt.show()

best_idx = np.argmin(mean_errors)
print(f"Optimal lookahead: L={L_range[best_idx]:.2f}m, mean error={mean_errors[best_idx]:.3f}m")

# Plot error curve plt.figure(figsize=(10, 5)) plt.plot(L_range, mean_errors, 'b-o', label='Mean Error') plt.plot(L_range, max_errors, 'r-s', label='Max Error') plt.xlabel('Lookahead Distance L (m)') plt.ylabel('Tracking Error (m)') plt.title('Effect of Lookahead Distance on Tracking Error') plt.legend() plt.grid(True) plt.show() best_idx = np.argmin(mean_errors) print(f"Optimal lookahead: L={L_range[best_idx]:.2f}m, mean error={mean_errors[best_idx]:.3f}m")

Optimal lookahead: L=0.20m, mean error=0.012m

4. Analysis and Conclusion¶

现象总结¶

1. 圆形轨迹：纯追踪算法跟踪圆形轨迹效果良好，稳态误差由前视距离$L$决定。
2. 8字形轨迹
   • 前视距离$L$较小时：响应速度快，但易出现轨迹抖动、路线偏移问题。
   • 前视距离$L$较大时：轨迹更平滑，但滞后现象加剧，弯道处误差增大。
3. 参数特性：前视距离$L$存在最优取值，取值过大或过小都会导致误差上升。

局限性¶

• 纯追踪属于几何算法，未考虑车辆动力学特性。
• 高速行驶时需增大前视距离，否则系统会失稳。
• 无法避让动态障碍物。
• 在曲率突变的轨迹上表现较差。