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 time
class 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)