MLC的实现

MLC 模块

1. 概述 (Overview)

MLC.cpp 是一个用于自动驾驶强制换道 场景的决策与轨迹规划模块。其核心入口函数是 RunLaneChangeSlotSelectionLattice (及其并行版本 RunLaneChangeSlotSelectionLattice_Parallel)。

该模块的核心思想是：基于 Lattice (栅格) 规划方法，通过对时间、速度、跟车目标、让行模式等多个维度进行参数采样，生成海量的候选轨迹。随后，每一条轨迹都会经过严格的约束检查 (ConstraintCheck) 和成本计算 (Cost)。最后，系统会从所有通过检查的轨迹（包括上一帧的历史轨迹）中，选出总成本最低的一条作为本帧的决策结果。

该模块具备高度的：

• 安全性：通过 RelativeStatusCheck 对TTC、Headway进行严格检查，并包含 MultimodalTraj (多模态风险) 评估。
• 平顺性：横向轨迹主要使用 QuinticPolynomialCurve1d (五次多项式) 生成，并对曲率 (HeadingAndLddCalc) 进行约束。
• 稳定性：引入 UpdateHistoryTrajectory (历史轨迹连接) 和历史轨迹复用机制，避免决策高频抖动。
• 鲁棒性：提供了 purepursuit_sampler (Pure Pursuit 采样器) 作为五次多项式方案失败时的备选方案。
• 可追溯性：拥有强大的日志 (SaveDecisionResultsToCSV) 和可视化 (saveTrajectoriesForVisualization) 功能。

2. 核心执行流程 (Core Execution Flow)

模块的执行流程可分解为以下八个关键步骤：

1. 初始化与环境分析 (Initialization and Environmental Analysis)

在每个计算周期开始时，模块首先需要“理解”当前的驾驶环境。这个步骤会调用一系列辅助函数来“预处理”感知数据，为后续的轨迹采样做准备：

• 重置变量: 重置上一帧的决策结果和临时变量。

• 分析障碍物车道 (objLaneDecider):

  • 功能 (Purpose): (来自 image 14) 为每一个感知到的目标物体，判定其当前车道 (currentLane) 和未来车道 (FutureLane) 的归属。
  • 原理 (Principle): (来自 image 14) 通过分析该物体（上一帧）轨迹的横向左右边界，与本地地图中存储的所有车道中心线进行比较，来判断它归属于哪条车道。
  • 用途 (Usage): 这是环境分析的基础。后续的“跟车目标选择”（步骤3）和“碰撞检查”（步骤5）都强依赖这个函数分配的车道信息。

• 确定路径最近前车 (CIPVDecider_simple):

  • 功能 (Purpose): (来自 image 14) 寻找“路径内最近前车 (Closest In-Path Vehicle, CIPV)”。
  • 原理 (Principle): (来自 image 14) 遍历所有未被忽略的、且objLaneDecider判定为在“自车主车道”或“未来车道”上的目标，从中选取纵向距离(s)最近且在自车前方的那个目标。
  • 用途 (Usage): 确定纵向控制（gap_controller）的主要跟车对象。

• 过滤无效目标 (IgnoreObjDecision):

  • 功能 (Purpose): (来自 image 14) 这是一个性能优化函数。它会遍历所有物体，给那些无需参与后续复杂计算的物体打上“忽略” (isIgnored) 标记。
  • 原理 (Principle): (来自 image 14) 根据一系列规则进行过滤，例如：该目标是否有效、纵向距离是否过远（例如 > 200米）、横向距离是否过远、是否是“无效或跑飞”的目标等。
  • 用途 (Usage): 大幅减少后续最耗时的RelativeStatusCheck（步骤5）需要检查的目标数量，提高算法运行效率。

• 计算车流密度 (TgtLaneTrafficFlowDensityCalc):

  • 功能 (Purpose): (来自 image 24) 估算“目标车道”的交通拥堵程度，即车流密度。
  • 原理 (Principle): (来自 image 24)
    1. 首先，检查目标车道上的车辆对象，如果少于4辆，则认为数据不足以计算密度，直接返回一个极大值（表示稀疏）。
    2. 如果车辆足够，则计算所有相邻车辆之间的纵向间距。
    3. 为了防止异常值（例如一辆坏车导致间距超大）干扰结果，会去掉一个最大间距值。
    4. 最后，计算剩余间距的平均值，作为车流密度的指标（均值越小，密度越大）。
  • 用途 (Usage): (来自 image 7, 21) 这个密度值会用于后续的成本计算，例如SafeInterp1（安全距离插值）和costCalcRelative（相对成本）。在密集的车流中，系统可能会倾向于选择更保守（如更大Headway、更低TTC代价）的轨迹。

2. 历史轨迹处理与评估 (History Trajectory Processing and Evaluation)

为了保证决策的连续性和稳定性（避免车辆在两个相似选项之间高频抖动），模块在采样新轨迹之前，会首先处理并评估上一帧选中的“历史轨迹”。这个过程分为两步：

• A. 连接与插值 (UpdateHistoryTrajectory):

  • 功能 (Purpose): (来自 image 23) 解决一个核心问题：上一帧的轨迹（例如，从 t=0.1s 到 t=5.0s）与“当前时刻”（t=0s）的车辆真实状态在时间和空间上的不连续问题。此函数的目标就是将这条旧轨迹“嫁接”到当前车辆的真实状态上。
  • 原理与步骤 (Principle): (来自 image 23)
    1. 时间戳计算 (计算对应的原始时间点): 首先，函数会遍历历史轨迹上的每个点，根据时间步长（如0.1s）计算出它们对应的“原始绝对时间点”。
    2. 线性插值 (插值和连接): 在历史轨迹中，找到“当前时间”所在的区间（例如，在 t=0.0s 和 t=0.1s 之间），通过线性插值计算出一个理论上的“当前启动点”。
    3. 横向动态修正 (横向位置推采用 purepursuit 和 forwardSimulationLateral): 这是最关键的一步。
       • [核心问题]: 简单插值无法反映车辆当前的真实横向动态。例如：上一帧规划了一条直线（横向速度为0），但在这0.1秒的延迟中，由于路面颠簸或转向刚生效，车辆真实的横向速度可能已有 0.2 m/s。如果强行“拼接”上旧轨迹，会要求横向速度瞬时从 0.2 变为 0，导致物理上的“急动”(Jerk)，乘客会感觉车辆被“拽”了一下。
       • [修正原理]: (来自 image 23, 26) 为了解决这个问题，系统不进行“硬拼接”，而是启动“横向动态修正”。它把“历史轨迹”当作一个**“目标路径”，然后使用 purepursuit 算法来平滑地“跟踪”**这条路径。
       • [算法详解] purepursuit (纯跟踪算法):
         • 作用: (来自 image 26) 模拟人类驾驶：眼睛“瞄准”前方的一个“预瞄点”。
         • 流程: 算法会查看车辆当前的真实状态（例如，0.2 m/s 的横向速度），然后在“历史轨迹”上找到一个“预瞄点”。它会计算出一个完美的圆弧（即一个新曲率），这个圆弧能让车辆从当前状态出发，平滑地“瞄准”那个预瞄点。
       • [算法详解] forwardSimulationLateral (横向推演):
         • 作用: (来自 image 26) 这是一个执行器。它接收 purepursuit 计算出的新曲率，并结合车辆当前的横向速度/加速度，推演出下一个时间点（例如0.01秒后）的横向位置和速度。
       • [修正流程]: UpdateHistoryTrajectory 函数会循环调用这两个算法：purepursuit “瞄准”历史轨迹并计算新曲率 -> forwardSimulationLateral 执行这个曲率并推演下一步。这个过程会**“重新绘制”出一条新轨迹，这条新轨迹的起点是车辆的真实动态**，终点是平滑地回归到历史轨迹上。
    4. 纵向对齐 (横向做坐标平移): (注：图片原文“横向”应为“纵向”之误) 将整条轨迹在纵向（S轴）上平移，使其新的起点S坐标与自车当前的S坐标（s_ego）完全对齐。
    5. 时间戳重置: 将这条“嫁接”好的新轨迹的时间戳重置为从0开始（例如 0s, 0.1s, 0.2s…）。
  • 结果 (Result): 生成一条平滑地**“连接”**到当前车辆状态上的、可用于本帧评估的“新”历史轨迹。

• B. 重新评估 (Re-evaluation):

  • (来自 image 6, 7, 8) 这条“嫁接”后的历史轨迹，将作为第一条候选轨迹，进入本帧的评估流程，与所有即将生成的新轨迹公平竞争。
  • 步骤 (Steps):
    1. 约束检查 (ConstraintCheck): (来自 image 6, 25) 与所有新轨迹一样，首先对它进行严格的安全约束检查（详见步骤5）。检查它在当前新环境下是否越界、是否碰撞、TTC/Headway是否满足要求。
    2. 失败处理 (FailedFlag 为真): (来自 image 7) 如果检查失败（例如，上一帧规划换道，但这一帧突然蹿出来一辆车导致碰撞），则调用 UpdateDecisionResult (来自 image 22) 记录失败原因（例如 egoForcedFailedTrajectory，历史轨迹强制失败）。同时，系统会给它施加一个高额的惩罚成本，确保它不会被选中。
    3. 成本重算 (FailedFlag 为假): (来自 image 7, 21) 如果检查通过（轨迹在当前环境下仍然安全），系统会重新计算它的成本：
       • costCalcEgoTrajectory (来自 image 3): 计算自身成本（如平顺性、舒适性）。
       • costCalcRelative (来自 image 3, 21): 计算相对成本（与他车的TTC、Headway、EACC误差等）。
    4. 施加额外惩罚 (Penalty): (来自 image 7, 8) 即使轨迹安全，系统也会根据当前新态势追加特定的惩罚项，以反映潜在风险：
       • penalty_cost2: 如果存在多模态碰撞风险（如他车可能急刹），增加惩罚。
       • SafeInterp1: 结合轨迹终点与前车的距离和当前车流密度（来自步骤1），增加安全距离惩罚。
       • ForceYieldAbandon2: 如果轨迹放弃了之前的“强制让行”意图，增加惩罚。
    5. 记录结果: (来自 image 22) 调用 UpdateDecisionCost 和 UpdateDecisionResult 记录这条历史轨迹的最新成本和评估结果。

3. 多维参数采样 (Multi-dimensional Parameter Sampling)

这是模块的计算核心。在处理完历史轨迹后，系统开始生成全新的候选轨迹。它通过嵌套的 for 循环，遍历（即“采样”）所有可能的驾驶意图组合。

这就像是在大脑中同时推演几百种“如果我这么开会怎样？”的可能性。每一个循环都代表一个“决策维度”。（来自 image 5, 9, 11）

• A. 前向时间 (for (auto forwardTime : ForwardTimerSample)):

  • 含义: 采样“我打算往前开多久？”。例如，同时考虑开3秒、4秒、5秒、6秒…后的情况。
  • 用途: 决定了纵向轨迹的规划长度和时间。

• B. 换道时间 (for (auto laneChangeTime : LaneChangeTimerSample)):

  • 含义: 采样“如果我要换道，我打算用几秒完成？”。例如，同时考虑4秒换道、5秒换道、6秒换道…
  • 用途: 决定了横向轨迹的形态和缓急程度。如果为0，则代表“保持车道”。

• C. 速度模式 (for (auto speedChangeTime : SpeedChangePattern)):

  • 含义: 采样“我是要加速、减速还是保持匀速？”。
  • 用途: 决定了纵向轨迹的速度变化。

• D. 跟随目标 (for (auto targetObj_behind : followableObject)):

  • 含义: 这是最关键的纵向采样之一：“我的跟车目标是谁？”。
  • 逻辑: (来自 image 9, 10)
    1. FollowableObject 列表是在步骤1中 FollowableObjDecider (目标可跟随决策器) 生成的。
    2. 系统会检查一个最大跟随目标数 (MAX_FOLLOWABLE_OBJECT_NUM)，例如，只考虑本车道和目标车道上最多5个车 (来自 image 9, if (i == MAX_FOLLOWABLE_OBJECT_NUM || i == followableObject.size()))。
    3. targetObj_behind == -1 是一个特殊标记 (来自 image 10)，代表**“超越所有车”**（即不跟车，以期望速度巡航）。
    4. 循环遍历所有可跟随的目标车，为每一个目标车都生成一条“以它为跟车目标”的轨迹。
  • 用途: 生成多种不同的纵向策略（跟车A、跟车B、巡航…）。

• E. 车头时距 (for (auto tgtHeadway : HeadwaySample)):

  • 含义: 采样“如果我跟车，我打算保持多大的车头时距？”。例如，同时考虑1.5秒和2.0秒的时距。
  • 用途: (来自 image 10) 结合(D)的跟车目标，精细化纵向跟车轨迹。

• F. 强制让行 (for (int forceYieldMode = 0; ...; forceYieldMode++)):

  • 含义: 这是一个特殊的横向采样：“我是否需要执行一次‘强制让行’？”（例如，换道时遇到侧后方快车，需要先往旁边让一点，等它过去再换）。
  • 逻辑 (来自 image 11):
    1. forceYieldMode = 0 代表正常模式（不强制让行）。
    2. forceYieldMode = 1 代表强制让行模式。
    3. 当 forceYieldMode = 1 时，会有一系列**“剪枝” (Pruning)** 判断，用于跳过不合理的组合，以节省计算资源：
       • if (forwardTime < 2 || forwardTime > 3) continue;: 强制让行是一种短时行为，只在特定的短时间内（如2-3秒）有意义，太长或太短的规划不考虑。
       • if (forwardTime + laneChangeTime + 1 > 8) continue;: 总规划时间不能太长。
       • if (laneChangeTime >= 4) continue;: 换道时间不能太长。
       • if (speedChangeTime != 0) continue;: 强制让行时必须保持匀速，不能同时加减速，这太复杂了。
       • if (targetObj_behind == -1) continue;: 强制让行时必须有跟车目标，不能是“超越所有车”模式（没有车可以让你）。
    4. 如果所有条件都满足，则会计算让行的时间 ForceYieldTime 和横向偏移量 ForceYieldDy。
  • 用途: 生成一种特殊的、用于主动避让的横向轨迹。

4. 候选轨迹生成 (Candidate Trajectory Generation)

在嵌套循环的最内层，系统会拿到一个来自第三部分的“配方”组合（即一个 ego_traj_temp 结构体）。

现在，系统将按照这个“配方”，真正地“做”出这条轨迹。这个过程由函数 GenerateLaneChangeSlotSelectionLattice (来自 image 754c57) 执行。

该函数将这个复杂的二维（S-L）“画线”问题，分解为两个独立的一维（1D）问题来解决：

A. 纵向轨迹生成 (Longitudinal Trajectory)

• 目标: (来自 image 754c57, 754f00, 754f1d) 计算出未来每个时间点(i)的纵向位置(Dx)、速度(Vx)和加速度(Ax)。
• [核心逻辑]: 这个过程被巧妙地分为两部分：
  • 1. “迭代器” (for 循环): (来自 image 754c57) 这是一个按时间步进的 for (int i = 0; ...) 循环。
  • 2. “计算器” (gap_controller): (来自 image 74ccb7 等) 一个复杂的函数，只负责计算“当前这0.1秒”应该使用多大的加速度 ax。
• [执行流程]: “迭代器” for 循环每一步(i)，都会调用一次“计算器” gap_controller 来获取ax，然后用这个ax推算出下一步的状态。

• 1. “迭代器” (for 循环) 的步进逻辑

  (来自 image 754c57, 754f00, 754f1d)

  这是纵向轨迹的“骨架”，一个按时间步长（例如0.1s）执行的 for 循环：

  • t = 0.1s (循环 i=1):
    • a. 获取“配方”: 从 ego_traj_temp 中读取 targetObj_behind, tgtHeadway 等。
    • b. 调用“计算器”: 调用 gap_controller (输入当前 i 时刻的所有状态)。
    • c. 获取 ax: gap_controller 返回一个平滑的加速度 a_0.1。
    • d. 存储 ax: ego_traj_temp.egoTrajectory.Ax[i] = a_0.1。
    • e. 推演下一步: 调用 ForwardSimulation (输入 dx[i], vx[i], ax[i])，计算出 dx[i+1] 和 vx[i+1]。
  • t = 0.2s (循环 i=2):
    • a. 获取新状态: 获取 dx[i+1] 和 vx[i+1]。
    • b. 再次调用“计算器”: 再次调用 gap_controller (输入 i=2 时刻的新状态)。
    • c. 获取 ax: gap_controller 返回一个新的 a_0.2。
    • …循环往复… 直到达到 minTrajectoryLength_。

• 2. “计算器” (gap_controller) 的单次计算 (超详细分析)

  (来自 image 74ccb7.jpg, 74cf64.jpg, 74cf82.png, 74cfa0.jpg)

  当“迭代器”在 t=0.1s 时调用它，它只执行一次以下逻辑来返回 ax_0.1：

  • a. 计算多个安全/期望距离: (来自 image_74ccb7)

    • 它首先会根据“配方”中的时距、车辆长度(objlVx)、最小间距(Headwaymin)等，计算出多个关键距离，如 des_min_front (最小前车距离), des_safe_front (安全前车距离), des_safe_back (安全后车距离)。

  • b. 计算多个期望速度: (来自 image_74ccb7, 74cf64)

    • 它会计算 desVx (基于前车速度的期望速度)、desVx_back (基于后车速度的期望速度)、desiredSpdAprch (一个基于sqrt开根号的复杂接近速度函数)等。
    • 最后，它会综合这些速度，得出一个最终期望速度 desVx_final。这个速度通常是多个计算值中的最小值，以确保安全。

  • c. 计算两个核心加速度指令: (来自 image_74cf64)

    • aFollow: “跟车加速度”。计算出一个加速度，使自车速度 egoVx 平滑地趋近于 desVx_final (最终期望速度)。
    • aGapControl: “车距加速度”。计算出一个加速度，用于调整与前车的 current_gap_speed (当前间隙速度)，即控制车距。

  • d. 选择保守指令: (来自 image_74cf82)

    • double HLCax = min(aFollow, aGapControl);
    • [关键]：系统会取这两个加速度中的较小值（即更保守、更安全的那个），作为基础期望加速度 HLCax。

  • e. 选择控制模式 (Mode): (来自 image_74cf82)

    • 系统会根据哪条指令生效了（是aFollow还是aGapControl胜出）以及其他速度条件（如是否受后车desVx_back影响），来动态选择一个 Mode (模式 0, 1, 2, 3)。

  • f. 应用Jerk（舒适度）限制: (来自 image_74cfa0)

    • [最精妙的一步]: 系统会根据上一步选择的 Mode，来加载不同的 JerkUplim (Jerk上限) 和 JerkBotlim (Jerk下限)。
    • 这意味着，如果是 Mode = 3 (Gap控制，可能需要紧急减速)，Jerk限制会更宽松（JerkUplim = 1.5）；如果是 Mode = 0 (巡航)，Jerk限制会更严格（JerkUplim = 1.0），以保证舒适性。

  • g. 输出平滑加速度: (来自 image_74cfa0)

    • 系统计算 DesiredJerk_raw (原始期望Jerk)，并将其限制在(f)中查到的Jerk范围内（DesiredJerk）。
    • 最终输出的加速度 ax 是 lastax + Jerk * sampleTime。
    • [关键]：它不是一个瞬变的加速度值，而是在上一帧的加速度 lastax 基础上，施加一个平滑的、受Jerk限制的增量。这保证了加减速的绝对平顺。

• 纵向结果: “迭代器” for 循环执行完毕后，生成了一个完整的纵向点序列 ego_traj_temp.egoTrajectory，包含 Dx (位置), Vx (速度), Ax (加速度) 三个数组。这个序列“定”下了轨迹的“节奏”。

B. 横向轨迹生成 (Lateral Trajectory)

• 目标: (来自 image 754f3c ~ 754fbc) 计算出每个纵向位置(S)对应的横向偏移(L)。

• [核心概念：时空解耦 (Time-Space Decoupling)]

  • 这是一个核心设计：横向轨迹（B部分）不关心时间(t)，它只关心空间(S)。
  • 横向轨迹（B部分）是一个**“S-L”函数**，它回答的问题是：“在纵向距离S处，横向偏移L应该是多少？”
  • 纵向轨迹（A部分）是“时间(t)”和“空间(S)”的主人，它定义了 (t, S) 的关系（“时刻表”）。
  • 横向轨迹（B部分）是“空间(L)”的仆人，它定义了 (S, L) 的关系（“地图”）。
  • 如何合并: 系统按时间(t)循环。在t=0.1s时，先查(A)得到S=0.8m，再把S=0.8m代入(B)得到L=0.05m。这就实现了时空合并。

• [横向生成步骤]:

  • 系统有两种方案来生成这条线：

• 方案 1: 五次多项式 (Quintic Polynomial) - (主方案)

  • [为什么用它？]: (来自 image 1, 15) 五次多项式（L = a0 + a1*s + a2*s² + a3*s³ + a4*s⁴ + a5*s⁵）是一条神奇的数学曲线。因为它有6个系数(a0~a5)，所以它能唯一且完美地连接一个起始状态（位置L, 速度L’, 加速度L’‘）和一个终止状态（位置L, 速度L’, 加速度L’'）。这能确保轨迹的绝对平滑。

  • [生成步骤]:

    1. 计算分段索引: (来自 image 754f3c, 754f5b)

       • 目的: 确定(A)中生成的纵向轨迹点，哪些属于“前向段”，哪些属于“让行段”，哪些属于“换道段”。
       • 原理: 根据“配方”中的时间参数（ForwardTime, ForceYieldTime, LaneChangeTime）和时间步长 sampleTime，计算出数组索引：forwardEndIndex_raw, ForceYieldEndIndex_raw, LaneChangeEndIndex_raw。
       • [代码细节]: (来自 image 754f3c) 这里还有一段复杂的逻辑，用于处理纵向S坐标 Dx 可能回退（s < ...）的异常情况，确保 forwardEndIndex 等索引值始终正确。

    2. 确定起点 (StablizeDyDecider): (来自 image 13, 754f63)

       • 目的: 吸收车辆当前的横向动态（例如轻微漂移），避免“急动”。
       • 原理: 调用 StablizeDyDecider (如V3增强版所分析)，根据自车当前的横向速度 (vy_ego)，反算出“如果现在开始刹停横向运动，会漂多远？”。这个距离 StablizeDy 将被用作横向轨迹的“虚拟”平滑起点。
       • [代码细节]: (来自 image 754f63) useStablizeDySameSlot 标志位用于判断是否复用上一帧的 StablizeDy，以增加决策稳定性。

    3. 计算分段系数 (QuinticPolynomialCurve1d_ComputeCoefficients): (来自 image 754f63, 754f9b)

       • 目的: 为每一段轨迹计算其“数学公式”（五次多项式系数）。
       • 逻辑:
         • if (ForceYieldMode == 0) (正常模式):
           • 为 ForwardDx (前向段) 调用一次 ComputeCoefficients，得到系数 a0_0…a5_0。
           • 为 LaneChangeDx (换道段) 调用一次 ComputeCoefficients，得到系数 a1_1…a5_1。
         • else (让行模式):
           • 为 ForwardDx (前向段) 调用一次，得到系数 a0_...。
           • 为 ForceYieldDx (让行段) 调用一次，得到系数 a3_...。
           • 为 LaneChangeDx (换道段) 调用一次，得到系数 a1_...。
         • [代码细节]: (来自 image 754f9b) ForceYieldFailedDx (强制让行失败) 也有自己的一组系数 a4_...。

    4. 采样点列 (QJS): (来自 image 754f9b)

       • 目的: 使用(3)中算出的“数学公式”，生成实际的点列。
       • 流程:
         • 为每一段（前向、让行、换道、让行失败）单独调用 QJS (秦九韶算法)。
         • 输入: 该段的系数（如 a_coef_3）和该段对应的纵向S坐标切片（path_section，即(A)中 ego_traj_temp.egoTrajectory.Dx 的一部分）。
         • 输出: 该段的横向L点列（ego_l_forward, ego_l_forceYield, ego_l_lanechange, ego_l_forceYieldFailed）。

    5. 拼接 (TrajectoryCombine): (来自 image 754fbc)

       • 目的: 将(4)中生成的多段点列“缝合”成一条完整的轨迹。
       • 流程: 调用 TrajectoryCombine(ego_l_forward, ego_l_forceYield, 0, temp_traj)，将各段点列按顺序合并到 temp_traj 中。
       • [代码细节]: 备用的“让行失败”轨迹也会被单独拼接：TrajectoryCombine(ego_l_forward, ego_l_forceYieldFailed, 0, backup_traj)。

    6. 长度补齐 (temp_traj.resize): (来自 image 754fbc)

       • 问题: 纵向轨迹(A)有 minTrajectoryLength_ (例如100个点)，但(B)中拼接的横向轨迹可能只有80个点。
       • 解决: if (temp_traj.size() < ...)，调用 temp_traj.resize(minTrajectoryLength_, last_value)，用最后一个L值（temp_traj.back()）填充 temp_traj 的末尾，使其与纵向轨迹等长。备用的 backup_traj 也会做同样处理。

• 方案 2: Pure Pursuit (PP) - (备选/Fallback方案)
  • (来自 image 25, 26) [注：这批新代码中未显示，但存在于您之前的截图中]
  • 触发: 如果五次多项式方案因故（例如数学上无解）失败。
  • 调用 purepursuit_sampler:
    • 1. purepursuit (PP核心算法): 计算出一个“瞄准”目标路径的新曲率。
    • 2. forwardSimulationLateral (横向推演): 使用这个曲率，推演下一个点的横向位置。
    • 3. 循环此过程，生成一条备选的横向轨迹。

最终结果: (来自 image 754fbc)
ego_traj_temp.egoTrajectory.L = temp_traj;
ego_traj_temp.egoForceFailedTrajectory.L = backup_traj;

系统将这条完整的横向轨迹 temp_traj 赋值给 ego_traj_temp 结构体。此时，ego_traj_temp 包含了一条完整的、时空对应的候选轨迹，准备被送入下一步（第五部分）进行评估。

5. 轨迹评估与约束检查 (ConstraintCheck)

在第4步中，for 循环每生成一条完整的轨迹 (一个 ego_traj_temp 结构体)，就会立即将其送入本步骤进行“安全审查”。

这个审查的主函数是 ConstraintCheck (来自 image 2, 4, 25)。

• 目标: 判断该轨迹是否安全和可行。这是一个 Pass/Fail (通过/失败) 的检查。
• 结果: 如果检查失败，该轨迹被立即丢弃，并记录一个失败原因码 (来自 image 21)。for 循环继续 continue，开始生成下一条轨迹。
• 如果检查通过，该轨迹才会被允许进入第六部分 (成本计算)。

A. 主检查函数 (ConstraintCheck 的内部逻辑)

(来自 image 25)

当 ConstraintCheck 收到一条轨迹时，它会按顺序执行以下检查：

1. 曲率检查 (CalculateDLFromTrajectory):

   • 目的: 检查轨迹是否“拐弯过急”。
   • 原理: (来自 image 16, 27) 调用 CalculateDLFromTrajectory (计算轨迹的DL)，这个函数会计算轨迹的一阶导数(dl/ds)和二阶导数(d²l/ds²)，后者即曲率。
   • 检查: 检查曲率是否超过了车辆的物理极限（例如，方向盘打死）。如果超过，则 Fail。

2. 越界检查 (crossLaneCheck):

   • 目的: 检查轨迹是否“开出了路面”或“压到了实线”。
   • 原理: (来自 image 16, 19, 25) 调用 crossLaneCheck (车道交叉检查)。这个函数会考虑车身宽度 (egoWidth)，检查轨迹的最左/最右点是否超出了车道边界 (bound_left, bound_right)。
   • 检查: 如果越界，则 Fail。

3. 目标车道检查:

   • 目的: 检查轨迹的终点是否在“配方”所期望的目标车道 (TargetLane) 内。
   • 检查: 如果不在（例如，规划换道但没换过去），则 Fail。

4. [核心] 碰撞与让行检查 (RelativeStatusCheck):

   • 目的: 这是最重要、最复杂的安全检查，用于判断“我会不会撞到别人？”或“我会不会别到别人？”。
   • 检查: 调用 RelativeStatusCheck (详见B部分)。如果这个函数返回 Fail，则整条轨迹 Fail。

• [失败原因码 (Fail Code)]: (来自 image 21)
  如果上述任何一步失败，系统会记录一个数字代码，以便工程师调试：
  • 20: 碰撞 (Collision) - 阈值内
  • 21: TTC (碰撞时间) 过小
  • 22: Headway (车头时距) 过小
  • 23: 轨迹越界 (Out of Boundary)
  • 24: 目标物体被超车 (有横向重叠)
  • 27: 自车忽略了前车
  • 其他: 曲率、速度等物理约束失败

B. [深度拆解] 核心安全检查 (RelativeStatusCheck)

(来自 image 18, 19, 20)

ConstraintCheck 会把轨迹交给 RelativeStatusCheck (相对状态检查) 函数。这个函数是真正的“安全专家”。

• [核心原理：时空碰撞检查 (Spatio-Temporal Check)]

  • (回应您的问题) 这个检查不是一个“静态”检查（例如，“我的车头 vs 行人现在的位置”）。
  • 它是一个**“动态”的、“基于预测”的检查。它会对比两条“时间线”**：
    1. “我”的轨迹 (规划): (来自 ego_traj_temp) 我们在第4步生成的规划轨迹。我们精确地知道在未来的每一毫秒(i)，我们的车计划在哪里（Dx[i], L[i]）。
    2. “它”的轨迹 (预测): (来自 objInfo[...].objTrajectory) 系统从“感知预测”模块获取的所有其他物体（行人、车辆）的预测轨迹。我们也知道在未来的每一毫秒(i)，它们预计会在哪里（objTrajectory.Dx[i], objTrajectory.Vx[i]）。

• [执行流程]:
  RelativeStatusCheck 的核心是一个**for 循环**，它从 i = 0（当前时刻）一直循环到 i = minTrajectoryLength_（例如 t=5.0s）。

  在循环的每一步（例如 i = 30，代表 t=3.0s）：

  1. 获取两条“时间线”上的点:

     • “我”在哪？: 查找 ego_traj_temp.Dx[30] 和 ego_traj_temp.L[30]。
     • “目标J”在哪？: 查找 objInfo[j].objTrajectory.Dx[30] 和 objInfo[j].objTrajectory.L[30]。

  2. 检查：在 t=3.0s 这个时刻，我们俩会撞吗？

     • (回应您的例子：如果此时“行人”的预测位置 Dx[30] 已经走远了，那么在 i=30 这一步的检查自然就是“安全通过”。)

  3. 目标分类与检查: (来自 image 20)

     • 系统会根据“目标J”在 i=30 时的相对位置，将其分为三类，并调用不同的“专家”函数来检查：

     • 1. 前方物体 (Front Objects):

       • TTC_calc: (来自 image 17) 检查在 t=3.0s 时的TTC (碰撞时间)，是否小于安全阈值？
       • Headway_calc: 检查在 t=3.0s 时的Headway (车头时距)，是否小于安全阈值？

     • 2. 后方物体 (Rear Objects):

       • TTC_calc / Headway_calc: 检查（例如我方急刹）是否会导致后车在 t=3.0s 时追尾？
       • 让行检查: 检查我方（例如换道）是否“别”了后车？

     • 3. 侧方物体 (Lateral Objects):

       • CollisionConditionCalc: (来自 image 17) [最核心] 检查在 t=3.0s 这个时刻，两个车（考虑车身宽度）的几何外形是否重叠。
       • MultimodalTraj: (来自 image 15, 20) 检查“鬼探头”或“前车急刹”风险。例如，如果侧前方车辆突然刹停（一个与预测不符的“多模态”轨迹），我的轨迹是否来得及躲避？
       • EACCErrorCalc: (来自 image 17) 检查与侧方车辆的横向距离是否始终保持在安全范围内。

• [最终裁决]:
  RelativeStatusCheck 在任何一个时间点(i)，对任何一辆车(j)，发现了任何一个违规（TTC过小、碰撞、距离过近…），它会立即返回 Fail。

  ConstraintCheck 收到 Fail 结果，立即丢弃这条轨迹。

• [幸存者]:
  只有完美通过上述所有检查的轨迹（即在所有时间点对所有车辆都安全），才会被 ConstraintCheck 标记为 Pass (通过)，并被荣幸地送入第六部分 (成本计算)。

6. 成本计算 (Cost Calculation)

(来自 image 76b0fb ~ 76b4bb)

在第五部分 ConstraintCheck (约束检查) 中幸存下来的每一条轨迹（trajGroupInfo），都会被立即送入本步骤。

• 目标: (来自 image 76b0fb) ProcessTrajectory (处理轨迹) 函数会调用 costCalcEgoTrajectory 和 costCalcRelative，为这条“安全”的轨迹打一个“分数”（即 Cost，成本，分数越低越好）。
• 原理: 第五部分是“硬门槛”（Pass/Fail），只管安全。本步骤是“软评分”，它关心驾驶品质。

这个“总成本” (Total Cost) 是由三个主要部分加权求和而来的：

A. 自身轨迹成本 (costCalcEgoTrajectory)

(来自 image 76b419.jpg, 76b422.jpg, 76b43f.jpg, 76b460.jpg)

• 目的: 评估这条轨迹的自身品质，不考虑其他车辆。

• 总成本: traj_cost_temp.TotalCost_ego = Cost_L_1 + Cost_J + Cost_A + Cost_difference_x + Cost_difference_y + Cost_vibrationY + Cost_Efficency + Cost_SteeringWheelRotSpd + Cost_ltAffect;

• [详细拆解]:

  • 1. Cost_L_1 (横向偏移成本): (来自 image 76b422.jpg)

    • 目的: 惩罚“不贴近车道中心”的行为。
    • 原理:
      1. 计算轨迹上每个点(L)到最近车道中心线的横向距离 dist_to_nearest_lane。
      2. 调用 SafeInterp1 (查表函数) 将这个距离转换为成本。
      3. sum_cost_l 累加所有点的成本。
    • 成本: 越贴近车道中心，成本越低。

  • 2. Cost_J (Jerk 成本): (来自 image 76b422.jpg)

    • 目的: 惩罚“加减速不平滑”的行为（舒适度）。
    • 原理:
      1. 计算纵向加加速度 j_ego (jerk) 和横向加加速度 jy_ego。
      2. 找出纵向 min_j_ego (最小jerk) 和横向 min_jy_ego (最小jerk)。
      3. 调用 SafeInterp1 (查表函数) 将这两个Jerk值转换为成本。
    • 成本: Jerk 越小（变化越平滑），成本越低。

  • 3. Cost_A (加速度 成本): (来自 image 76b419.jpg)

    • 目的: 惩罚“剧烈加减速”的行为（舒适度）。
    • 原理:
      1. 找出纵向加速度 a_ego 的最小值 min_a_ego。
      2. 调用 SafeInterp1 (查表函数) 将这个加速度值转换为成本。
    • 成本: 加速度越小（越平稳），成本越低。

  • 4. Cost_difference_x / _y (轨迹差异成本): (来自 image 76b43f.jpg)

    • 目的: 惩罚“与上一帧轨迹差异过大”的行为（稳定性）。
    • 原理:
      1. if (isLastTrajectory) (如果存在上一帧轨迹)。
      2. 计算当前轨迹的起点(L) 与 上一帧轨迹的起点(L) 之间的横向差异 jXDiff。
      3. 计算当前轨迹的起点(Jerk) 与 上一帧轨迹的起点(Jerk) 之间的横向Jerk差异 jYDiff。
      4. 调用 SafeInterp1 (查表函数) 将这两个“差异”转换为成本。
    • 成本: 与上一帧轨迹越接近，成本越低。这能有效抑制决策抖动。

  • 5. Cost_vibrationY (横向振动成本): (来自 image 76b43f.jpg)

    • 目的: 惩罚“高频左右晃动”的行为（舒适度）。
    • 原理:
      1. 遍历轨迹的横向Jerk trajectory.Jy。
      2. 在一个时间窗口内（VibrationPeriodIndexLength），查找最大Jerk值 currentMaxPastJ。
      3. vibration_y[i] = currentMaxPastJ * abs(currentJ)，计算出一个“振动值”。
      4. 找出最大的振动值 max_vibration。
      5. 调用 SafeInterp1 (查表函数) 将这个“振动值”转换为成本。
    • 成本: 轨迹越平滑（无高频晃动），成本越低。

  • 6. Cost_Efficency (效率成本): (来自 image 76b419.jpg)

    • 目的: 惩罚“龟速行驶”的行为（效率）。
    • 原理:
      1. 计算一个“速度比例” speed_ratio[i] = trajectory.Vx[i] / desiredSpdtemp (当前速度 / 期望速度)。
      2. sum_cost_efficiency: 累加所有点 1.0 - speed_ratio 的值。
      3. cost_vt_ratio: 计算轨迹终点速度 trajectory.Vx.back() 与期望速度 egoInfo_...setspeed 的差异成本。
    • 成本: 轨迹速度越接近期望速度，成本越低。

  • 7. Cost_SteeringWheelRotSpd (方向盘转速成本): (来自 image 76b419.jpg)

    • 目的: 惩罚“猛打方向盘”的行为（舒适度/安全性）。
    • 原理:
      1. 计算轨迹中所有的方向盘转速 steeringWheelRotSpd。
      2. 找出最大转速 max_steeringWheelRotSpd。
      3. 调用 SafeInterp1 (查表函数) 将这个“最大转速”转换为成本。
    • 成本: 方向盘转动越慢，成本越低。

  • 8. Cost_ltAffect (轨迹终点对车道的影响成本): (来自 image 76b43f.jpg, 76b460.jpg)

    • 目的: 惩罚“轨迹终点停在不好的位置”的行为。
    • 原理:
      1. dist_to_nearest_lane.back(): 计算轨迹终点到最近车道线的距离。
      2. dy_to_extinct: (来自 image 76b460) 计算轨迹终点到车道边界的横向距离。
      3. 将这两个距离值都通过 SafeInterp1 (查表函数) 转换为成本。
    • 成本: 轨迹终点越靠近车道中心、远离边界，成本越低。

  • [总和]: (来自 image 76b460.jpg)

    • traj_cost_temp.TotalCost_ego = Cost_L_1 + Cost_J + ...
    • 将上述所有8个成本项相加，得到自身轨迹的总成本。

B. 相对轨迹成本 (costCalcRelative)

(来自 image 76b49d.jpg)

• 目的: 评估这条轨迹在与其他车辆互动时的**“礼貌性”和“安全性”**。

• 总成本: traj_cost_temp.TotalCost_rel = traj_cost_temp.Cost_Headway + traj_cost_temp.Cost_TTC + traj_cost_temp.Cost_EACC;

• [详细拆解]:

  • 1. Cost_TTC (TTC 成本):

    • 目的: 惩罚“潜在碰撞风险”的行为。
    • 原理: 第五步 ConstraintCheck 检查了 TTC 是否大于一个“硬门槛”（例如 > 2.0s）。这里则进行“软评分”。
    • 逻辑: traj_cost_temp.Cost_TTC = SafeInterp2(ttc_cost_mps_bkp, ttc_cost_bkp, result.TTC, ttc_ego_vx);
    • [关键]: (来自 image 21) SafeInterp2 是一个二维查表函数。它意味着成本不仅仅取决于 result.TTC (计算出的TTC值)，还取决于 ttc_ego_vx (当前车速)。
    • 含义: 在高速时，一个 5s 的TTC可能是低成本的；但在低速（例如跟车蠕行）时，一个 5s 的TTC可能反而代表跟车太远，成本会高。

  • 2. Cost_Headway (车头时距成本):

    • 目的: 惩罚“跟车过近”（不礼貌）的行为。
    • 原理: 同样，第五步检查了“硬门槛”（例如 > 1.5s），这里进行“软评分”。
    • 逻辑: double minHeadway_cost = SafeInterp2(headway_cost_mps_bkp, headway_cost_bkp, std::min(result.Headway, 2.0), ...);
    • [关键]:
      1. SafeInterp2: 成本同样取决于时距(Headway)和车速(ego_vx)。
      2. std::min(result.Headway, 2.0): 对用于计算的 Headway 值设置了一个2.0秒的上限，防止时距过大导致成本异常。
      3. (来自 image 76b49d) lastHeadway_cost 也会被计算，它代表轨迹终点的 Headway 成本。
      4. traj_cost_temp.Cost_Headway = std::min(minHeadway_cost, lastHeadway_cost);
      5. 系统会取“平均时距成本”和“终点时距成本”中较小（较优）的那个作为最终成本。

  • 3. Cost_EACC (横向安全距离成本):

    • 目的: 惩罚“横向贴车太近”的行为（例如换道时）。
    • 原理: (来自 image 17, 21, 76b49d)
    • 逻辑: traj_cost_temp.Cost_EACC = SafeInterp1(EACCDiffCost_bkp, EACCDiffCost, result.EACCDiffMax);
    • [关键]: result.EACCDiffMax 这个值是在第五步 (ConstraintCheck) 中 RelativeStatusCheck 函数计算得出的，它代表了在整条轨迹中，自车与侧方车辆曾经达到的“最小横向距离”。
    • 含义: 这个距离越小（贴得越近），SafeInterp1 查表得到的成本就越高。

  • [总和]: (来自 image 76b49d)

    • traj_cost_temp.TotalCost_rel = ... + Cost_Headway + Cost_TTC + Cost_EACC;
    • 将上述所有3个成本项相加，得到相对轨迹的总成本。

  • [重要备注：备用轨迹]: (来自 image 76b3c3)

    • 系统也会为那条**“备用的强制让行失败轨迹”** (egoForceFailedTrajectory) 单独调用一次 costCalcEgoTrajectory 和 costCalcRelative，计算出它自己的全套成本 (traj_cost_temp2)。

C. 惩罚项 (Cost_penalty)

(来自 image 76b3a6.jpg, 76b3c3.jpg)

• 目的: 在 A (自身成本) 和 B (相对成本) 的基础上，对某些**“虽然安全，但不鼓励”的特殊行为施加额外罚分 (Penalty)**。

• 总成本: traj_cost_temp.Cost_penalty = ... (各项惩罚累加)

• [详细拆解]:

  • 1. ForwardCost (前向时间成本): (来自 image 76b3c3)

    • 目的: 惩罚“规划时间过短”的行为。
    • 原理: double ForwardCost = SafeInterp2(...)。
    • 逻辑: 规划时长 ForwardTime 越短，成本越高。这鼓励系统尽可能规划更长的轨迹。

  • 2. collidewithMultiModal_penalty (多模态碰撞风险成本): (来自 image 76b3a6, 76b3c3)

    • 目的: 惩罚“冒险”行为。
    • 原理: if (result.collidewithMultiModal)。
    • 逻辑: 在第五步 ConstraintCheck 中，系统发现这条轨迹与某个“多模态”轨迹（例如，前车急刹）有碰撞风险。虽然在“主预测”上是安全的，但这里会施加一个高额罚分，以惩罚这种“赌博式”的轨迹。

  • 3. SwitchSlotCost (切换车位成本): (来自 image 76b3a6)

    • 目的: 惩罚“决策抖动”的行为。
    • 原理: if (isHistoryTrajectory && lastDecisionInfo...SlotID != ...)。
    • 逻辑: 仅当评估“历史轨迹”时，此项生效。如果“历史轨迹”的跟车目标(SlotID) 与“上一帧的最终决策”(lastDecisionInfo) 不一致（例如，上一帧决定跟车A，但历史轨迹是跟车B的），说明这条历史轨迹已经“过时”了，施加罚分，降低它胜出的概率。

  • 4. ForceYieldAbandon_penalty (放弃让行成本): (来自 image 76b3c3)

    • 目的: 惩罚“意图不坚决”的行为。
    • 原理: if (result.ForceYieldAbandon)。
    • 逻辑: 在第五步 ConstraintCheck 中，系统发现这条轨迹放弃了“强制让行”的意图。施加罚分。

  • 5. FallbackTimerCost / ManualLaneChangeCost (特殊模式成本): (来自 image 76b3a6, 76b3c3)

    • 逻辑: 如果轨迹是 isFallback (兜底轨迹) 或 lane_change_reason == 2 (人工换道)，它们会使用一套专属的成本（例如 GetFutureLaneChangeCost），而不是常规的A+B项。

D. 总成本计算 与 结果记录

(来自 image 76b3e1.jpg)

• 目的: 将 A, B, C 三项成本合成为一个最终分数，并存储它。

• [核心逻辑：历史轨迹奖励 (Stability Bonus)]:

  • 原理: (来自 image 76b3e1) 为了防止决策在两条成本相近（例如 100 vs 101）的轨迹之间高频“抖动”，系统会给“历史轨迹”一个优先奖励（即成本折扣）。
  • 逻辑:
    1. double lastTrajectoryBonusRatioTemp;
    2. if (isHistoryTrajectory || ...):
       • lastTrajectoryBonusRatioTemp = lastTrajectoryBonusRatio; (例如，0.8，即打8折)
    3. else (如果是新轨迹):
       • lastTrajectoryBonusRatioTemp = 1.0; (不打折)

• [最终总成本 (Total_cost_temp)]: (来自 image 76b3e1)

  • 公式:
    Total_cost_temp = (traj_cost_temp.TotalCost_ego * lastTrajectoryBonusRatioTemp) + (traj_cost_temp.TotalCost_rel * lastTrajectoryBonusRatioTemp) + traj_cost_temp.Cost_penalty;
  • 拆解:
    • (自身成本 * 折扣) + (相对成本 * 折扣) + (惩罚项)
  • 含义: “历史轨迹”的 Ego 成本和 Rel 成本会被打折，使其更容易胜出。但惩罚项 (Cost_penalty) 不会打折，如果历史轨迹有“切换车位”等惩罚，它依然要付出全额代价。

• [结果记录 (UpdateDecisionResult)]: (来自 image 76b3e1, 76b49d)

  • 目的: 将这条轨迹的最终总分和决策“配方”存入一个总列表 (DecisionAllResults) 中，等待最后（第七部分）的“开奖”。
  • 逻辑:
    1. trajGroupInfo.decisionResult.cost = Total_cost_temp; (将总分存入)
    2. UpdateDecisionResult(Total_cost_temp, trajGroupInfo); (调用存储函数)
    3. (来自 image 76b49d) 这个函数会将 trajGroupInfo.decisionResult (包含“配方”) 和 trajGroupInfo.decisionResult_cost (包含所有成本分项) 存入一个总的结果列表中。

• [循环结束]:
  此时，for 循环的这一轮结束了。我们得到了一个既安全（通过V），又**有完整成本（完成VI）**的候选轨迹。

  系统将继续 for Loop，去生成和评估下一条轨迹…直到所有“配方”都尝试完毕。

7. 决策选优与输出 (Decision Selection & Output)

(来自 image 3, 5, 13, 76b3e1)

在所有 for 循环（第3-6部分）全部结束后，系统会执行决策的最后一步：

A. 选优 (Find Champion)

• 目的: 从所有“幸存”的轨迹（即DecisionAllResults 列表）中，选出唯一的“冠军”轨迹。

• 原理: 查找列表中的“最低成本”。

• 逻辑:

  1. 系统遍历 DecisionAllResults 列表中的每一条轨迹。
  2. 它比较它们的最终总成本 (Total_cost_temp)。
  3. 它会找到那条拥有绝对最低 Total_cost_temp 的轨迹，将其标记为“冠军”。

• [关键：内置的稳定性] (来自 image 76b3e1):

  • 您可能会问：如何防止系统在两条成本相近（例如 100 vs 101）的轨迹之间“抖动”？
  • 答案：在第六部分 (D) 中，这个问题已经被优雅地解决了。
  • 当时，“历史轨迹”在计算总成本时，获得了成本折扣（lastTrajectoryBonusRatioTemp，例如打8折）。
  • 因此，在这个“选优”步骤中，系统无需执行任何特殊逻辑。
  • 举例：
    • 一条新轨迹的成本是 95。
    • “历史轨迹”的原始成本是 100，但它的“打折后”成本是 100 * 0.8 = 80。
  • 结果：当系统查找“最低成本”时，80 < 95，“历史轨迹”自动胜出。
  • 只有当一条新轨迹极其优秀（例如成本 79，比历史轨迹好 20% 以上）时，它才能战胜这个“折扣”，赢得冠军。
  • 这就通过一个简单的“最低成本”查找，完美地保证了决策的稳定性。

B. 兜底逻辑 (Fallback Logic)

• 目的: (来自 image 13, 25) 处理一个极端情况：如果所有的采样轨迹（例如500条）都未能通过第五部分（ConstraintCheck 安全审查）怎么办？
• 逻辑:
  1. 此时，DecisionAllResults 列表将是空的。
  2. 系统会检测到这个情况（“若未决策都失败”）。
  3. 它会放弃从列表选优，转而启用“Fallback (兜底) 模式”。
  4. 它会选择一条在第四部分 (B) (来自 image 754fbc) 中单独生成并拼接的egoForceFailedTrajectory（强制让行失败的备用轨迹），或者一条最简单的“保持车道”轨迹。
• 结果: 保证系统永远会输出一条轨迹，永远不会“卡死”或“无解”，确保了绝对的安全冗余。

C. 最终输出 (Final Output)

• 目的: (来自 image 5, 13) 将“冠军”轨迹发送给车辆。
• 逻辑:
  1. 系统从(A)或(B)中选出的“冠军”轨迹（即 ego_traj_temp 结构体）。
  2. 将其完整的轨迹点序列（Dx, Vx, Ax, L）和决策信息，作为 RunLaneChangeSlotSelectionLattice 函数的最终结果返回。
  3. 车辆的**“控制 (Control)”模块会接收这条轨迹，并将其翻译为实际的**方向盘转角、油门和刹车指令，在下一帧执行。

8. 辅助功能：日志与可视化 (Auxiliary: Logging & Vis)

(来自 image 23, 28, 76b49d, 76b4bb)

在主流程之外，该模块还拥有强大的辅助功能，用于工程师的调试 (Debug) 和复盘 (Review)。

A. 决策与成本日志 (SaveDecisionResultsToCSV)

• 目的: (来自 image 23, 76b49d) 记录“系统为什么会做出这个选择？”
• 触发: 在 RunLaneChangeSlotSelectionLattice 函数的最末尾调用。
• 逻辑: (来自 image 76b4bb)
  1. 打开两个CSV文件：resultFile (结果文件) 和 costFile (成本文件)。
  2. 遍历总列表: 遍历（for 循环） 在第六部分生成的所有轨迹（DecisionAllResults），包括那些成本很高、最终落选的轨迹。
  3. 写入 resultFile: 写入**“配方”**信息，例如：
     • Index, FailedReason (失败原因码), ForwardTime (规划时间), LaneChangeTime (换道时间), SpeedPattern (速度模式), TgtHeadway (时距), SlotID (跟车目标) …
  4. 写入 costFile: 写入**“成本明细”**，例如：
     • Index, TotalCost (总成本), Cost_L_1 (横向偏移), Cost_J (Jerk), Cost_A (加速度), Cost_TTC, Cost_Headway, Cost_penalty (惩罚) …
• 用途: 工程师可以通过分析这两个表格，复盘每一条轨迹的“得分”，精确地知道“冠军”轨迹为什么赢了，而“亚军”轨迹为什么输了。

B. 可视化日志 (saveTrajectoriesForVisualization)

• 目的: (来自 image 28) “眼见为实”，以图形化方式复现当时的场景。
• 逻辑:
  1. 保存轨迹 (CSV): 将关键的轨迹点（如自车轨迹、目标车轨迹、forcefailed轨迹、上一帧轨迹）的S-L坐标保存到 CSV 文件。
  2. 保存场景 (.config): 将当时的“静态”环境（如自车位置、目标车位置、车道线信息）保存到 .config 文件。
• 用途: 工程师可以使用一个专用的可视化工具，加载这两个文件，**在电脑上“播放”**出一个“小动画”，亲眼看到当时的交通流、自车的规划轨迹，以及它是如何与其它车辆互动的。