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()）：- 检查轨迹是否在道路边界内。
- 验证加速度是否满足约束（如最大加速度）。
- 步骤1：风险评估 ：
-
generate_current_lane_trajectory()：
- 生成直线轨迹：
s_traj = s_start + v * dt * t，d_traj = d_start * ones。
- 生成直线轨迹：
-
validate_trajectory()：
- 边界检查 ：确保所有
d值在道路宽度的5%容差范围内。 - 动力学约束 ：检查加速度是否不超过
max_accel。
- 边界检查 ：确保所有
-
visualize()：
- 绘制道路 ：
- 绘制道路左右边界（黑色实线）。
- 绘制车道线（实线/虚线区分）。
- 绘制轨迹 ：
- 用蓝色曲线显示自车规划轨迹。
- 标注起点（绿色圆点）、终点（红色方块）。
- 绘制障碍物 ：
- 红色"×"标记障碍物，箭头显示其运动方向。
- 绘制道路 ：
3. 测试场景
功能：模拟真实道路环境并测试轨迹规划器。
代码逻辑：
-
定义道路路径：
- 使用三次样条曲线生成自然弯曲的道路（如正弦曲线）。
- 路点
waypoints由x = 40 * t、y = 50 * sin(0.3 * t)生成。
-
创建道路模型：
road = EnhancedRoadModel(waypoints, road_width=3.5, num_lanes=3)。
-
设置自车状态：
- 初始位置在道路s=40m处，速度15m/s。
-
定义障碍物：
- 包括同车道前车、左侧车道前车、右侧车道后车。
-
执行规划：
- 实例化
LaneChangePlanner，调用generate_lane_change_trajectory()生成轨迹。
- 实例化
-
输出结果：
- 打印轨迹起始点、终点、位移、换道时间等信息。
- 调用
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_width
self.num_lanes = num_lanes
self.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 = 0 else: 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/2 lane_index = min(self.num_lanes-1, max(0, int(relative_d // self.road_width))) return lane_index def 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 = dist min_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 - center d_val = np.dot(displacement, normal) return min_s, d_val def 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 * normal def 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_car
self.obs = obstacles
self.dt = dt
self.road = road_model
self.time_steps = int(T / dt)# 初始Frenet坐标 s0, d0 = self.road.cartesian_to_frenet(ego_car[:2]) self.init_s = s0 self.init_d = d0 self.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_time return elapsed > self.max_plan_time def 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_traj else: # 约束不满足时保持当前车道 return self.generate_current_lane_trajectory() def select_safest_lane(self): """选择最安全的车道（最少前方障碍物）""" lane_risks = [0] * self.road.num_lanes for 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_lane return safest_lane def calculate_target_d(self, target_lane): """计算目标d值（在目标车道的中心）""" # 车道中心计算：总宽度/2 是道路最左侧，每个车道中心是车道宽度/2 + n*车道宽度 lane_offset = (target_lane + 0.5) * self.road.road_width return lane_offset - self.road.total_width/2 def 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)) / 2 d_traj[i] = d return d_traj def 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_factor return s_traj def 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.dt accels = (speeds[1:] - speeds[:-1]) / self.dt if np.max(np.abs(accels)) > self.max_accel: return False return True def generate_current_lane_trajectory(self): """生成当前车道内的轨迹""" s_traj = self.init_s + np.arange(self.time_steps) * self.ego[2] * self.dt d_traj = np.ones(self.time_steps) * self.init_d return s_traj, d_traj def 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_width for lane_idx in range(self.road.num_lanes + 1): d_offset = -self.road.total_width/2 + lane_idx * lane_width lane_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 * normal lane_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 * t
y = 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 = 40 ego_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)