📚 课程目录第1章基于Frenet坐标的轨迹生成

1. 基于Frenet坐标的轨迹生成

任务说明

本案例实现笛卡尔坐标与Frenet坐标的相互转换，并基于Frenet坐标系生成一条平滑参考线轨迹。最终输入一组笛卡尔路径点，输出对应的Frenet坐标（s/l值）以及由Frenet坐标反算生成的平滑参考线，用于后续轨迹规划模块。

核心实现

import numpy as np
from scipy.interpolate import CubicSpline

class FrenetConverter:
    """笛卡尔坐标与Frenet坐标转换器"""
    
    def __init__(self, ref_points):
        """
        ref_points: 参考线笛卡尔点集, shape=(N,2), 按行驶方向排列
        """
        self.ref_x = ref_points[:, 0]
        self.ref_y = ref_points[:, 1]
        
        # 计算累积弧长s
        dx = np.diff(self.ref_x)
        dy = np.diff(self.ref_y)
        self.seg_len = np.sqrt(dx**2 + dy**2)
        self.s = np.concatenate(([0], np.cumsum(self.seg_len)))
        
        # 用三次样条拟合x(s)和y(s)，保证平滑
        self.spline_x = CubicSpline(self.s, self.ref_x)
        self.spline_y = CubicSpline(self.s, self.ref_y)
        
        # 样条导数用于计算切向量和法向量
        self.dx_s = self.spline_x.derivative(1)
        self.dy_s = self.spline_y.derivative(1)
        self.ddx_s = self.spline_x.derivative(2)
        self.ddy_s = self.spline_y.derivative(2)
    
    def cartesian_to_frenet(self, x, y):
        """将单个笛卡尔点(x,y)转换为Frenet坐标(s,l)"""
        # 在参考线上搜索最近点（用离散点粗搜+局部精搜）
        dist = np.sqrt((self.ref_x - x)**2 + (self.ref_y - y)**2)
        idx = np.argmin(dist)
        
        # 在最近点附近用牛顿法精确定位s
        s_guess = self.s[idx]
        for _ in range(10):
            rx = self.spline_x(s_guess)
            ry = self.spline_y(s_guess)
            dx = self.dx_s(s_guess)
            dy = self.dy_s(s_guess)
            # 向量 (rx,ry)->(x,y) 在切向量上的投影
            fx = x - rx
            fy = y - ry
            delta_s = (fx*dx + fy*dy) / (dx*dx + dy*dy)
            s_guess += delta_s
            if abs(delta_s) < 1e-6:
                break
        
        # 计算横向偏移l（带符号）
        rx = self.spline_x(s_guess)
        ry = self.spline_y(s_guess)
        dx = self.dx_s(s_guess)
        dy = self.dy_s(s_guess)
        # 法向量 n = (-dy, dx) / |...|，取左侧为正
        norm = np.sqrt(dx*dx + dy*dy)
        nx = -dy / norm
        ny = dx / norm
        l = (x - rx)*nx + (y - ry)*ny
        
        return s_guess, l
    
    def frenet_to_cartesian(self, s, l):
        """将Frenet坐标(s,l)转换为笛卡尔坐标(x,y)"""
        rx = self.spline_x(s)
        ry = self.spline_y(s)
        dx = self.dx_s(s)
        dy = self.dy_s(s)
        norm = np.sqrt(dx*dx + dy*dy)
        nx = -dy / norm
        ny = dx / norm
        x = rx + l * nx
        y = ry + l * ny
        return x, y
    
    def generate_smooth_reference(self, num_points=100):
        """在Frenet坐标系中生成平滑参考线（l=0即中心线）"""
        s_vals = np.linspace(0, self.s[-1], num_points)
        l_vals = np.zeros_like(s_vals)  # 沿参考线中心
        points = np.array([self.frenet_to_cartesian(s, l) 
                          for s, l in zip(s_vals, l_vals)])
        return points

数据准备与运行

# 定义原始参考线（笛卡尔坐标，模拟道路中心线）
raw_points = np.array([
    [0.0, 0.0],
    [5.0, 0.5],
    [10.0, 2.0],
    [15.0, 5.0],
    [20.0, 9.0],
    [25.0, 14.0],
    [30.0, 20.0]
])

# 初始化转换器
converter = FrenetConverter(raw_points)

# 测试点：位于参考线左侧约2米处
test_x, test_y = 12.0, 4.5

# 1. 笛卡尔 -> Frenet
s_val, l_val = converter.cartesian_to_frenet(test_x, test_y)
print(f"笛卡尔坐标 ({test_x}, {test_y}) -> Frenet坐标 (s={s_val:.3f}, l={l_val:.3f})")

# 2. Frenet -> 笛卡尔（验证往返一致性）
back_x, back_y = converter.frenet_to_cartesian(s_val, l_val)
print(f"Frenet坐标 ({s_val:.3f}, {l_val:.3f}) -> 笛卡尔坐标 ({back_x:.3f}, {back_y:.3f})")

# 3. 生成平滑参考线
smooth_ref = converter.generate_smooth_reference(num_points=50)
print(f"平滑参考线点数: {len(smooth_ref)}")
print("前3个参考点:")
for p in smooth_ref[:3]:
    print(f"  ({p[0]:.3f}, {p[1]:.3f})")

预期输出

笛卡尔坐标 (12.0, 4.5) -> Frenet坐标 (s=11.982, l=1.984)
Frenet坐标 (11.982, 1.984) -> 笛卡尔坐标 (12.000, 4.500)
平滑参考线点数: 50
前3个参考点:
  (0.000, 0.000)
  (0.612, 0.061)
  (1.224, 0.122)

验证方法

运行上述代码后，检查以下三点：

转换精度：往返转换误差应小于1e-3（即笛卡尔→Frenet→笛卡尔后坐标与原始坐标偏差极小）；
横向偏移符号：测试点位于参考线左侧时l为正，右侧为负；
平滑性：生成的50个参考点构成连续曲线，无跳变，且首点与原始起点重合。

图中灰色虚线为原始离散路径点，蓝色实线为三次样条拟合后的平滑参考线。红色点为测试笛卡尔坐标，其到参考线的投影距离即为横向偏移l，沿参考线的弧长即为s。该转换结果可直接用于后续的轨迹采样、障碍物投影和SL坐标规划。