【自动驾驶黑科技】基于Frenet坐标系的车道变换轨迹规划系统实现（附完整代码）

1. 代码结构概览

该代码实现了一个车道变换轨迹规划系统，包含两个核心模块：

• 道路建模（EnhancedRoadModel）：基于样条曲线构建道路模型。
• 轨迹规划（LaneChangePlanner）：根据障碍物状态和道路模型生成安全轨迹，并可视化结果。

---

2. 关键模块详解

2.1 道路建模（EnhancedRoadModel）

功能：构建道路的几何模型，包括中心线、车道线和边界。

核心方法：

• 构造函数：

  • 接收waypoints（道路路径点）、road_width（车道宽度）、num_lanes（车道数）。
  • 使用scipy.interpolate.CubicSpline对路径点进行样条插值，生成平滑的道路中心线。

• get_road_bounds()：

  • 计算道路的左右边界：
    • 通过求样条导数得到每个点的切线方向。
    • 计算法线方向（垂直于切线），在中心线两侧偏移road_width/2得到边界。

• 其他方法：

  • get_center_line()：返回道路中心线的坐标。
  • frenet_to_cartesian(s, d)：将Frenet坐标（s：沿道路方向，d：横向偏移）转换为笛卡尔坐标。

---

2.2 轨迹规划（LaneChangePlanner）

功能：根据自车状态、障碍物信息，生成安全的轨迹（s和d序列）。

核心方法：

• 构造函数：

  • 接收自车状态ego_car（[x, y, v, heading]）、障碍物列表obstacles（[x, y, v, heading]）、道路模型road、时间步长dt和总时间T。

• generate_lane_change_trajectory()：

  • 步骤1：风险评估：
    • 计算障碍物与自车的相对距离和速度，判断是否需要换道。
    • 若当前车道有障碍物，优先选择换道。
  • 步骤2：生成轨迹：
    • 当前车道轨迹（generate_current_lane_trajectory()）：保持横向偏移d不变，沿道路方向移动。
    • 换道轨迹：根据障碍物位置调整d值，确保轨迹避开障碍物。
  • 步骤3：验证轨迹（validate_trajectory()）：
    • 检查轨迹是否在道路边界内。
    • 验证加速度是否满足约束（如最大加速度）。

• generate_current_lane_trajectory()：

  • 生成直线轨迹：s_traj = s_start + v * dt * t，d_traj = d_start * ones。

• validate_trajectory()：

  • 边界检查：确保所有d值在道路宽度的5%容差范围内。
  • 动力学约束：检查加速度是否不超过max_accel。

• visualize()：

  • 绘制道路：
    • 绘制道路左右边界（黑色实线）。
    • 绘制车道线（实线/虚线区分）。
  • 绘制轨迹：
    • 用蓝色曲线显示自车规划轨迹。
    • 标注起点（绿色圆点）、终点（红色方块）。
  • 绘制障碍物：
    • 红色“×”标记障碍物，箭头显示其运动方向。

---

3. 测试场景

功能：模拟真实道路环境并测试轨迹规划器。

代码逻辑：

1. 定义道路路径：

   • 使用三次样条曲线生成自然弯曲的道路（如正弦曲线）。
   • 路点waypoints由x = 40 * t、y = 50 * sin(0.3 * t)生成。

2. 创建道路模型：

   • road = EnhancedRoadModel(waypoints, road_width=3.5, num_lanes=3)。

3. 设置自车状态：

   • 初始位置在道路s=40m处，速度15m/s。

4. 定义障碍物：

   • 包括同车道前车、左侧车道前车、右侧车道后车。

5. 执行规划：

   • 实例化LaneChangePlanner，调用generate_lane_change_trajectory()生成轨迹。

6. 输出结果：

   • 打印轨迹起始点、终点、位移、换道时间等信息。
   • 调用visualize()显示道路、自车轨迹、障碍物等。

---

4. 关键技术点

• Frenet坐标系：使用s（沿道路方向）和d（横向偏移）描述轨迹，适合道路建模。
• 样条插值：生成平滑道路中心线，避免路径突变。
• 避障逻辑：通过比较障碍物与自车的相对位置和速度，动态调整轨迹。
• 轨迹验证：确保生成的轨迹符合物理约束（如加速度限制）。
• 可视化：用Matplotlib绘制道路、轨迹、障碍物，直观展示规划结果。

import numpy as np
from scipy.spatial.distance import cdist
from scipy.interpolate import CubicSpline
import matplotlib.pyplot as plt
import timeclass EnhancedRoadModel:"""精确道路建模工具，确保几何约束"""def __init__(self, waypoints, road_width=3.5, num_lanes=3):self.waypoints = np.array(waypoints)self.road_width = road_widthself.num_lanes = num_lanesself.total_width = road_width * num_lanes# 计算弧长参数化seg_lengths = np.sqrt(np.sum(np.diff(self.waypoints, axis=0)**2, axis=1))self.s_points = np.insert(np.cumsum(seg_lengths), 0, 0)# 使用三次样条避免数值问题self.center_spline_x = CubicSpline(self.s_points, self.waypoints[:, 0])self.center_spline_y = CubicSpline(self.s_points, self.waypoints[:, 1])# 预计算方向角self.headings = []for s in self.s_points:dx = self.center_spline_x(s, 1)  # 一阶导数dy = self.center_spline_y(s, 1)if dx == 0 and dy == 0:  # 处理零导数值heading = 0else:heading = np.arctan2(dy, dx)self.headings.append(heading)def get_center_point(self, s):"""获取道路中心点坐标"""s_clamped = max(self.s_points[0], min(s, self.s_points[-1]))return np.array([self.center_spline_x(s_clamped), self.center_spline_y(s_clamped)])def get_heading(self, s):"""获取道路方向角（简化版）"""# 找到最近采样点idx = np.argmin(np.abs(self.s_points - s))return self.headings[min(idx, len(self.headings)-1)]def get_current_lane(self, d):"""确定车辆当前所在车道"""# d在-总宽度/2到总宽度/2之间# 转换为车道索引relative_d = d + self.total_width/2lane_index = min(self.num_lanes-1, max(0, int(relative_d // self.road_width)))return lane_indexdef cartesian_to_frenet(self, point):"""笛卡尔坐标转Frenet坐标（精确版）"""# 在曲线上找到最近点min_dist = float('inf')min_s = self.s_points[0]# 在整个路点中搜索for s in self.s_points:center = self.get_center_point(s)dist = np.linalg.norm(point - center)if dist < min_dist:min_dist = distmin_s = s# 计算横向位移center = self.get_center_point(min_s)heading = self.get_heading(min_s)tangent = np.array([np.cos(heading), np.sin(heading)])normal = np.array([-tangent[1], tangent[0]])displacement = point - centerd_val = np.dot(displacement, normal)return min_s, d_valdef frenet_to_cartesian(self, s, d):"""Frenet坐标转笛卡尔坐标"""s_clamped = max(self.s_points[0], min(s, self.s_points[-1]))center = self.get_center_point(s_clamped)heading = self.get_heading(s_clamped)normal = np.array([-np.sin(heading), np.cos(heading)])return center + d * normaldef get_road_bounds(self):"""获取道路边界点"""left_bounds = []right_bounds = []for s in self.s_points:center = self.get_center_point(s)heading = self.get_heading(s)normal = np.array([-np.sin(heading), np.cos(heading)])left_bounds.append(center + (self.total_width/2) * normal)right_bounds.append(center - (self.total_width/2) * normal)return np.array(left_bounds), np.array(right_bounds)class LaneChangePlanner:"""车道变换规划器，确保轨迹合理"""def __init__(self, ego_car, obstacles, road_model, dt=0.2, T=8.0):self.ego = ego_carself.obs = obstaclesself.dt = dtself.road = road_modelself.time_steps = int(T / dt)# 初始Frenet坐标s0, d0 = self.road.cartesian_to_frenet(ego_car[:2])self.init_s = s0self.init_d = d0self.init_lane = self.road.get_current_lane(d0)# 约束参数self.safe_distance = 4.0  # 最小安全距离self.max_accel = 3.0      # 最大加速度(m/s²)self.max_jerk = 5.0       # 最大加加速度(m/s³)self.max_curv = 0.2       # 最大曲率(1/m)# 轨迹规划时间控制self.max_plan_time = 3.0  # 最大规划时间(秒)self.start_time = time.time()def _check_timeout(self):"""检查是否超时"""elapsed = time.time() - self.start_timereturn elapsed > self.max_plan_timedef generate_lane_change_trajectory(self):"""生成合理的车道变换轨迹"""# 确定目标车道（智能选择最安全的车道）best_lane = self.select_safest_lane()# 计算目标d值（目标车道中心线）target_d = self.calculate_target_d(best_lane)# 生成车道变换轨迹s_traj = self.generate_speed_profile()d_traj = self.generate_smooth_lateral_trajectory(target_d)# 约束检查if self.validate_trajectory(s_traj, d_traj):return s_traj, d_trajelse:# 约束不满足时保持当前车道return self.generate_current_lane_trajectory()def select_safest_lane(self):"""选择最安全的车道（最少前方障碍物）"""lane_risks = [0] * self.road.num_lanesfor ob in self.obs:# 计算障碍物的Frenet坐标s_ob, d_ob = self.road.cartesian_to_frenet(ob[:2])ob_lane = self.road.get_current_lane(d_ob)# 只考虑前方的障碍物if s_ob > self.init_s - 20:  # 包括略后方的障碍物# 计算风险（距离越近风险越高）risk = max(0, 1 - (s_ob - self.init_s)/50)lane_risks[ob_lane] += risk# 选择风险最低的车道（排除无效值）safest_lane = np.argmin(lane_risks)# 如果前方风险高，建议保持当前车道if lane_risks[safest_lane] > lane_risks[self.init_lane] * 0.8:return self.init_lanereturn safest_lanedef calculate_target_d(self, target_lane):"""计算目标d值（在目标车道的中心）"""# 车道中心计算：总宽度/2 是道路最左侧，每个车道中心是车道宽度/2 + n*车道宽度lane_offset = (target_lane + 0.5) * self.road.road_widthreturn lane_offset - self.road.total_width/2def generate_smooth_lateral_trajectory(self, target_d):"""生成平滑的横向轨迹（余弦函数过渡）"""# 计算过渡距离s_range = min(self.road.s_points[-1] - self.init_s, 100)# 使用余弦函数实现平滑过渡d_traj = np.zeros(self.time_steps)for i in range(self.time_steps):progress = min(1.0, i * self.dt * self.ego[2] / s_range)d = self.init_d + (target_d - self.init_d) * (1 - np.cos(np.pi * progress)) / 2d_traj[i] = dreturn d_trajdef generate_speed_profile(self):"""生成合理的纵向速度剖面"""# 基本恒速方案s_traj = self.init_s + np.arange(self.time_steps) * self.ego[2] * self.dt# 根据前方障碍物调整速度for ob in self.obs:s_ob, d_ob = self.road.cartesian_to_frenet(ob[:2])ego_lane = self.road.get_current_lane(self.init_d)ob_lane = self.road.get_current_lane(d_ob)# 如果是同车道前车if ob_lane == ego_lane and s_ob > self.init_s:if s_ob - self.init_s < 50:  # 50米内safe_speed = min(self.ego[2], ob[2] + 1.0)  # 比前车快1m/s# 在靠近障碍物时减速time_to_ob = max(1.0, (s_ob - self.init_s) / max(1.0, self.ego[2] - ob[2]))for i in range(self.time_steps):progress = min(1.0, (s_traj[i] - self.init_s) / (s_ob - self.init_s))if progress > 0.7:safe_factor = max(0.7, 1 - (progress - 0.7)/0.3)s_traj[i] = s_traj[max(0, i-1)] + safe_speed * self.dt * safe_factorreturn s_trajdef validate_trajectory(self, s_traj, d_traj):"""验证轨迹是否满足约束"""# 检查所有点是否在道路边界内for d in d_traj:if abs(d) > self.road.total_width/2 * 1.05:  # 允许5%的容差return False# 检查速度变化是否合理displacements = s_traj[1:] - s_traj[:-1]speeds = displacements / self.dtaccels = (speeds[1:] - speeds[:-1]) / self.dtif np.max(np.abs(accels)) > self.max_accel:return Falsereturn Truedef generate_current_lane_trajectory(self):"""生成当前车道内的轨迹"""s_traj = self.init_s + np.arange(self.time_steps) * self.ego[2] * self.dtd_traj = np.ones(self.time_steps) * self.init_dreturn s_traj, d_trajdef visualize(self, s_traj, d_traj):"""可视化道路环境与轨迹"""plt.figure(figsize=(12, 8))# 绘制道路边界left_bound, right_bound = self.road.get_road_bounds()plt.plot(left_bound[:, 0], left_bound[:, 1], 'k-', linewidth=2)plt.plot(right_bound[:, 0], right_bound[:, 1], 'k-', linewidth=2)# 绘制车道线lane_width = self.road.road_widthfor lane_idx in range(self.road.num_lanes + 1):d_offset = -self.road.total_width/2 + lane_idx * lane_widthlane_points = []for s in self.road.s_points:center = self.road.get_center_point(s)heading = self.road.get_heading(s)normal = np.array([-np.sin(heading), np.cos(heading)])point = center + d_offset * normallane_points.append(point)lane_points = np.array(lane_points)# 区分实线和虚线if lane_idx == 0 or lane_idx == self.road.num_lanes:plt.plot(lane_points[:, 0], lane_points[:, 1], 'k-', alpha=0.7)else:plt.plot(lane_points[:, 0], lane_points[:, 1], 'k--', alpha=0.5)# 绘制自车轨迹if s_traj is not None and d_traj is not None:cart_traj = []for i in range(len(s_traj)):s = s_traj[i]d = d_traj[i]cart_point = self.road.frenet_to_cartesian(s, d)cart_traj.append(cart_point)cart_traj = np.array(cart_traj)plt.plot(cart_traj[:, 0], cart_traj[:, 1], 'b-', linewidth=3, label='规划轨迹')# 起点和终点plt.scatter(cart_traj[0, 0], cart_traj[0, 1], s=80, c='g', marker='o', label='起点')plt.scatter(cart_traj[-1, 0], cart_traj[-1, 1], s=80, c='r', marker='s', label='终点')# 轨迹信息dist_info = f"纵向移动: {s_traj[-1]-s_traj[0]:.1f}m"time_info = f"时间: {self.dt * len(s_traj):.1f}s"lane_info = f"横向偏移: {d_traj[0]:.2f} → {d_traj[-1]:.2f}m"plt.annotate(dist_info, xy=(0.05, 0.95), xycoords='axes fraction', fontsize=10)plt.annotate(time_info, xy=(0.05, 0.90), xycoords='axes fraction', fontsize=10)plt.annotate(lane_info, xy=(0.05, 0.85), xycoords='axes fraction', fontsize=10)# 绘制障碍物for i, ob in enumerate(self.obs):plt.scatter(ob[0], ob[1], s=80, c='r', marker='x', label=f'障碍物{i+1}' if i == 0 else "")# 预测轨迹（显示箭头方向）plt.arrow(ob[0], ob[1], ob[2]*np.cos(ob[3]), ob[2]*np.sin(ob[3]),head_width=3, head_length=5, fc='r', ec='r', alpha=0.7)# 自车初始位置plt.scatter(self.ego[0], self.ego[1], s=120, c='y', marker='*', edgecolor='k', label='自车')plt.text(self.ego[0] - 10, self.ego[1] + 5, f"{self.ego[2]:.1f}m/s", fontsize=10, bbox=dict(facecolor='yellow', alpha=0.7))plt.title('车道变换轨迹规划', fontsize=14)plt.xlabel('X坐标 (m)')plt.ylabel('Y坐标 (m)')plt.legend(loc='best')plt.grid(True, alpha=0.3)plt.axis('equal')plt.tight_layout()plt.show()# ===== 测试场景 - 模拟真实道路曲线 =====
if __name__ == "__main__":# 定义自然弯曲的道路（三次样条曲线）t = np.linspace(0, 2*np.pi, 10)x = 40 * ty = 50 * np.sin(0.3 * t)waypoints = np.column_stack((x, y))# 创建道路模型road = EnhancedRoadModel(waypoints, road_width=3.5, num_lanes=3)# 自车初始状态 [x, y, v, heading]ego_init_s = 40ego_init_pos = road.frenet_to_cartesian(ego_init_s, 0)ego_car = [ego_init_pos[0], ego_init_pos[1], 15, 0]# 障碍物定义 [x, y, v, heading]obstacles = [# 前车（同车道）[*road.frenet_to_cartesian(100, 0), 13, 0],# 左侧车道前车[*road.frenet_to_cartesian(60, 3.5), 16, 0],# 右侧车道后车[*road.frenet_to_cartesian(20, -3.5), 14, 0],]# 创建规划器planner = LaneChangePlanner(ego_car, obstacles, road, dt=0.2, T=8.0)# 执行轨迹规划print("开始轨迹规划...")s_traj, d_traj = planner.generate_lane_change_trajectory()# 输出结果print("规划成功完成!")print(f"起点: s={s_traj[0]:.1f}m, d={d_traj[0]:.1f}m")print(f"终点: s={s_traj[-1]:.1f}m, d={d_traj[-1]:.1f}m")print(f"纵向位移: {s_traj[-1]-s_traj[0]:.1f}m")print(f"横向变化: {d_traj[-1]-d_traj[0]:.1f}m")print(f"换道时间: {len(s_traj)*planner.dt:.1f}s")# 可视化print("生成可视化...")planner.visualize(s_traj, d_traj)