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/s2)self.max_jerk = 5.0       # 最大加加速度(m/s3)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)