自动驾驶轨迹规划：Lattice Planner原理与C++实现详解

1. 项目概述：从“格子”到“轨迹”的规划艺术

如果你正在自动驾驶、机器人导航或者游戏AI的领域里摸爬滚打，那么“规划”这个词对你来说一定不陌生。规划算法的核心任务，就是告诉你的智能体“下一步该往哪走”。今天要聊的Lattice Planner（格子规划器），就是解决这个问题的经典方案之一。我第一次接触它，是在一个机器人路径规划的项目里，当时被各种复杂的动态障碍物和狭窄通道搞得焦头烂额，直到尝试了Lattice的思路，才算是找到了一个既能在结构化道路上稳定输出，又能兼顾实时性和平滑性的解法。

简单来说，Lattice Planner是一种基于采样的轨迹规划算法。它的核心思想听起来很直观：不是去求解一个复杂的最优控制问题，而是在一个由时间和状态构成的“格子”（Lattice）里，预先定义好一簇可能的轨迹模板（也称为“运动基元”），然后根据当前的环境感知（比如障碍物、车道线）和目标状态，从这些模板里快速筛选出一条最优的、无碰撞的轨迹。这就像你要从A点走到B点，面前有草地、石板路、水泥路等多种预设好的走法，你只需要根据天气（环境）和你的鞋子（车辆动力学）选一条最合适的路走就行。

这个算法特别适合结构化环境，比如高速公路、城市道路，因为道路的拓扑结构相对固定，我们可以提前生成贴合车道中心线的轨迹簇。它用C++实现是再自然不过的选择，毕竟这个领域对计算性能的要求是刻在骨子里的，从感知、预测到规划、控制，整个链路都需要在毫秒级完成。接下来，我会结合代码，把Lattice Planner从理论到实现的每一个环节掰开揉碎，让你不仅能看懂，更能自己动手实现一个可用的版本。

2. Lattice Planner的核心原理与设计思路拆解

2.1 为什么是“格子”？—— 采样空间的离散化哲学

规划问题的本质是在一个高维连续空间（包含位置、速度、加速度、时间等）中搜索一条路径。直接在这个连续空间里搜索，计算量是指数爆炸的，这就是所谓的“维度灾难”。Lattice Planner的聪明之处在于，它主动放弃了对整个连续空间的穷举，转而采用一种“结构化采样”的策略。

这个“格子”，就是我们对状态空间的一种离散化。通常，我们会选取几个关键的状态维度进行离散。在车辆规划中，最常用的是纵向（沿着道路方向）和横向（垂直于道路方向）两个维度。我们在这两个维度上，按照一定的分辨率（比如纵向每米一个点，横向每0.1米一个点）进行采样。每一个采样点，就代表了车辆在某个时刻的一个可能状态（s, l），其中s是纵向位移，l是横向位移。

但是，光有离散的点还不够，我们需要的是连接这些点的、符合车辆运动学的轨迹。这就是“运动基元”登场的时候。运动基元是一段预先计算好的、从某个起始状态到某个终止状态的短轨迹。Lattice Planner会离线生成一个庞大的运动基元库，覆盖从各种可能的起始状态（速度、加速度）到各种可能的终止状态（目标s, l, 速度等）的轨迹。在线规划时，算法只需要根据当前车辆状态和预测的障碍物信息，从这个库里快速匹配和拼接基元，就能组合成一条完整的轨迹。

注意：运动基元的生成是离线的，这是保证在线实时性的关键。生成时需要充分考虑车辆的运动学约束（如最大曲率、最大加速度），确保每一条基元都是车辆物理上可执行的。

2.2 与A*、RRT等算法的本质区别

你可能会问，路径规划不是有A*、Dijkstra这些老牌算法吗？或者RRT、PRM这类基于随机采样的算法？它们和Lattice Planner区别在哪？

A/Dijkstra* 通常用于在二维或三维的栅格地图（Grid Map）上搜索最短路径。它们搜索的是“位置点”的序列，不包含时间、速度、加速度等信息，因此生成的是“路径”（Path），而不是“轨迹”（Trajectory）。路径没有时间信息，无法直接用于控制。而Lattice Planner生成的是包含时间戳、速度、曲率等信息的轨迹，可以直接下发给控制器去跟踪。

RRT/PRM这类算法是在高维构型空间（C-Space）中进行随机采样，构建一棵快速探索的树。它们的优势在于能处理非常复杂的、非结构化的障碍物环境，比如机械臂在杂乱仓库中的运动。但它们的缺点也很明显：生成的轨迹通常不够平滑，随机性导致结果不稳定，且不天然包含时间维度。Lattice Planner由于使用了预定义的运动基元，其生成的轨迹天生就是平滑且符合动力学的，在高度结构化的道路环境中，其稳定性和效率通常优于随机采样方法。

简而言之，Lattice Planner是介于“路径搜索”和“轨迹优化”之间的一种折中方案。它比纯路径搜索多了动力学约束和时间维度，又比完整的轨迹优化（如用二次规划求解）计算量小、实时性高。

2.3 算法流程总览：四步生成一条轨迹

一个完整的Lattice Planner在线循环通常包含以下四个核心步骤，我们可以用一个简单的流程图来建立直观认识：

1. 轨迹采样：根据当前车辆状态（位置、速度、朝向）和预测模块输出的障碍物未来轨迹，在Frenet坐标系（以道路中心线为参考）下，采样一系列候选的终点状态（s, l, s_dot, l_dot等）。
2. 轨迹生成：针对每一个采样到的终点状态，从运动基元库中，或通过在线数值计算（如多项式拟合），生成一条连接当前状态到终点状态的轨迹。
3. 轨迹评价：为每一条生成的候选轨迹计算一个代价（Cost）。代价函数是算法的灵魂，通常包括：
   • 安全性代价：与静态障碍物、动态障碍物的距离。
   • 舒适性代价：加速度、加加速度（Jerk）、曲率的平方和。
   • 目标代价：与期望速度、期望车道中心的偏差。
   • 效率代价：轨迹的长度或预计行驶时间。
4. 轨迹选择：从所有候选轨迹中，选择总代价最小的一条，作为最终输出的轨迹。

下面，我们用一个表格来对比一下Lattice Planner和其他常见规划器的特点，方便你根据项目需求做选择：

3. 核心细节解析与C++实现要点

3.1 坐标系的选择：为什么一定是Frenet坐标系？

在Lattice Planner中，我们几乎总是在Frenet坐标系下工作，而不是笛卡尔坐标系（x, y）。这是由道路环境的结构化特性决定的。

在笛卡尔坐标系中，一条弯曲的道路对应的是复杂的曲线方程，车辆相对于道路的横向偏移计算起来很麻烦。而Frenet坐标系将道路中心线抽象为一条参考线，用两个坐标来描述车辆位置：

• s (纵向位移)：沿着参考线从起点到车辆投影点的弧长。
• l (横向位移)：车辆位置垂直于参考线方向的偏移量，向左为正或向右为正取决于约定。

这样做的好处极大：

1. 解耦：将复杂的二维平面运动，分解为沿道路方向的纵向运动和垂直于道路的横向运动。规划时可以分别考虑纵向的跟车、超车和横向的换道、避障。
2. 简化问题：车道保持的目标就是使 l=0，换道就是使 l 从一个值平滑地变化到另一个值（如3.5米）。障碍物的投影也变得更简单。
3. 匹配感知：车道线检测的输出天然就是Frenet坐标。

在C++实现中，我们需要一个FrenetConverter类，负责笛卡尔坐标(x, y, theta)和Frenet坐标(s, l, s_dot, l_dot)之间的相互转换。这里面的核心难点是求取(x, y)在参考线上的投影点，通常需要用到数值方法，如牛顿迭代法。

// 伪代码示例：笛卡尔坐标转Frenet坐标的核心思路 FrenetPoint CartesianToFrenet(const CartesianPoint &cartesian, const ReferenceLine &ref_line) { FrenetPoint frenet; // 1. 找到参考线上距离cartesian最近的点作为投影点 double min_dist = INFINITY; size_t proj_index = 0; for (size_t i = 0; i < ref_line.points.size(); ++i) { double dist = Distance(cartesian, ref_line.points[i]); if (dist < min_dist) { min_dist = dist; proj_index = i; } } // 2. 计算纵向位移s (即投影点在参考线上的累积弧长) frenet.s = ref_line.points[proj_index].s; // 3. 计算横向位移l (带符号的距离) // 需要根据投影点的切向方向，计算垂直距离 auto &proj_pt = ref_line.points[proj_index]; Eigen::Vector2d vec_to_car(cartesian.x - proj_pt.x, cartesian.y - proj_pt.y); Eigen::Vector2d ref_tangent(cos(proj_pt.theta), sin(proj_pt.theta)); frenet.l = vec_to_car.dot(Eigen::Vector2d(-ref_tangent.y(), ref_tangent.x())); // 法向量点乘 // 4. 计算纵向和横向速度（需要用到车辆航向角与参考线切向角的差值） // ... 此处省略详细推导 return frenet; }

实操心得：参考线的平滑性至关重要。如果参考线（车道中心线）本身曲率跳动大，那么计算出的Frenet坐标和生成的轨迹都会出现抖动。在实际项目中，我们通常会对感知或地图提供的原始参考点进行平滑处理，例如使用样条插值（如Cubic Spline）或优化方法，生成一条二阶连续（C2）的平滑参考线。

3.2 运动基元生成：多项式拟合的魔法

如何生成一条从状态A到状态B的平滑轨迹？最常用的方法是使用多项式进行拟合。为什么是多项式？因为多项式函数无限可微，方便我们约束起始点和终止点的位置、速度、加速度甚至加加速度。

对于纵向和横向运动，我们通常分别进行独立规划。假设我们规划一个时长T的轨迹。

• 纵向轨迹 (s-t关系)：常用四次多项式或五次多项式。
  • 四次多项式：s(t) = a0 + a1*t + a2*t^2 + a3*t^3 + a4*t^4
  • 可以约束起点的s, s_dot, s_ddot，终点的s, s_dot。共5个约束条件，刚好解出5个系数。
  • 如果需要约束终点的纵向加速度s_ddot，则需要用到五次多项式。
• 横向轨迹 (l-s关系)：常用五次多项式。
  • 为什么是l-s而不是l-t？这是为了与纵向解耦，并确保轨迹的几何形状。我们规划横向偏移l如何随纵向进展s变化。
  • 五次多项式：l(s) = b0 + b1*s + b2*s^2 + b3*s^3 + b4*s^4 + b5*s^5
  • 可以约束起点的l, l_prime (dl/ds), l_prime_prime (d²l/ds²)，终点的l, l_prime, l_prime_prime。共6个约束，解6个系数。

在C++中，我们需要实现一个PolynomialTrajectoryGenerator类。它的核心函数是GenerateTrajectory，输入起止状态约束，输出多项式系数。求解系数本质是解一个线性方程组Ax = b，可以用Eigen库高效完成。

// 伪代码示例：纵向四次多项式轨迹生成 class LongitudinalQuarticPolynomial { public: LongitudinalQuarticPolynomial(double start_s, double start_v, double start_a, double end_s, double end_v, double T) { // 构建矩阵A和向量b Eigen::MatrixXd A(5, 5); Eigen::VectorXd b(5); // t=0时的约束: s(0), s_dot(0), s_ddot(0) A << 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 2, 0, 0, 1, T, T*T, T*T*T, T*T*T*T, 0, 1, 2*T, 3*T*T, 4*T*T*T; b << start_s, start_v, start_a, end_s, end_v; // 求解系数 [a0, a1, a2, a3, a4].T coefficients_ = A.colPivHouseholderQr().solve(b); } double EvaluateS(double t) const { return coefficients_[0] + coefficients_[1]*t + coefficients_[2]*t*t + coefficients_[3]*t*t*t + coefficients_[4]*t*t*t*t; } // 类似地实现EvaluateV, EvaluateA函数 private: Eigen::VectorXd coefficients_; };

3.3 代价函数设计：规划器的“价值观”

代价函数是Lattice Planner的大脑，它决定了算法“喜欢”什么样的轨迹。一个设计良好的代价函数需要在安全性、舒适性、效率等多个目标之间取得平衡。通常，总代价是多个子代价的加权和。

struct TrajectoryCost { double total_cost = 0.0; double collision_cost = 0.0; // 碰撞代价 double buffer_cost = 0.0; // 与障碍物缓冲距离代价 double lateral_offset_cost = 0.0; // 横向偏移代价 double longitudinal_speed_cost = 0.0; // 纵向速度偏差代价 double curvature_cost = 0.0; // 曲率代价 (舒适性) double acceleration_cost = 0.0; // 加速度代价 double jerk_cost = 0.0; // 加加速度代价 // ... 其他代价 };

各子代价的计算方法：

1. 碰撞代价：这是硬约束，一票否决。遍历轨迹上的每一个点，检查其轮廓（通常用矩形或多边形近似）是否与任何障碍物的轮廓重叠。如果发生重叠，则赋予一个极大的代价（如1e8），或直接将该轨迹剔除。
2. 缓冲距离代价：即使没有碰撞，我们也希望离障碍物远一些。计算轨迹上每个点到最近障碍物的距离d，代价可以是exp(-alpha * d)或1/(d + epsilon)。距离越近，代价越高。
3. 横向偏移代价：鼓励车辆保持在车道中心。计算轨迹上各点的横向位移l的平方和或绝对值之和。
4. 纵向速度代价：鼓励车辆以期望速度（如道路限速）行驶。计算轨迹终点速度或平均速度与期望速度的偏差平方。
5. 舒适性代价：
   • 曲率代价：计算轨迹上各点的曲率kappa的平方和。曲率大意味着转弯急，乘客不舒服。
   • 加速度/加加速度代价：计算纵向和横向加速度/加加速度的平方和。急加速、急刹车、突然的横向摆动都会影响舒适度。

权重的调参：这是算法工程师的“玄学”部分。通常，安全相关的代价（碰撞、缓冲）权重最高。舒适性代价次之。效率代价（速度）权重相对较低。权重需要大量实车测试和仿真来调整，没有银弹。一个常见的技巧是使用“自适应权重”，例如在高速场景下提高舒适性权重，在拥堵场景下提高跟车距离的权重。

4. 完整C++实现流程与代码解析

4.1 项目结构与依赖

一个典型的Lattice Planner C++项目会包含以下模块，我们可以用CMake来管理：

lattice_planner/ ├── CMakeLists.txt ├── include/ │ ├── lattice_planner/ │ │ ├── config.h // 参数配置结构体 │ │ ├── frenet_converter.h │ │ ├── polynomial_generator.h │ │ ├── trajectory.h // 轨迹数据结构 │ │ ├── cost_calculator.h │ │ └── lattice_planner.h // 主规划器类 ├── src/ │ ├── frenet_converter.cpp │ ├── polynomial_generator.cpp │ ├── cost_calculator.cpp │ └── lattice_planner.cpp └── test/ // 单元测试

核心依赖库：

• Eigen3：用于矩阵运算和多项式系数求解。必不可少，头文件库，集成简单。
• ROS (可选)：如果用于机器人或自动驾驶系统，消息传递、坐标变换、可视化等常用ROS。但算法核心本身不依赖ROS。
• Google Test (可选)：用于编写单元测试，保证代码质量。

一个简单的CMakeLists.txt骨架如下：

cmake_minimum_required(VERSION 3.10) project(LatticePlanner) set(CMAKE_CXX_STANDARD 14) # 查找Eigen3，假设它安装在系统路径 find_package(Eigen3 REQUIRED) # 设置头文件路径 include_directories( ${EIGEN3_INCLUDE_DIRS} ${CMAKE_CURRENT_SOURCE_DIR}/include ) # 添加可执行文件，例如一个测试demo add_executable(lattice_demo src/main.cpp src/lattice_planner.cpp ...) target_link_libraries(lattice_demo ${EIGEN3_LIBRARIES}) # 添加静态库，方便其他项目链接 add_library(lattice_planner_lib STATIC src/frenet_converter.cpp ...) target_include_directories(lattice_planner_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)

4.2 主规划循环代码逐行解析

让我们深入到LatticePlanner::Plan()这个核心函数中。

Trajectory LatticePlanner::Plan(const VehicleState &ego_state, const ReferenceLine &ref_line, const std::vector<Obstacle> &obstacles) { Trajectory best_trajectory; double min_cost = std::numeric_limits<double>::max(); // Step 1: 状态转换到Frenet坐标系 FrenetState start_frenet = frenet_converter_.CartesianToFrenet(ego_state, ref_line); // Step 2: 终点状态采样 std::vector<FrenetState> end_state_samples; SampleEndStates(start_frenet, ref_line, obstacles, end_state_samples); // Step 3: 为每个采样终点生成轨迹并评价 for (const auto &end_state : end_state_samples) { // 3.1 生成一条候选轨迹 Trajectory candidate_traj; if (!GenerateCandidateTrajectory(start_frenet, end_state, ref_line, candidate_traj)) { continue; // 生成失败（如无解），跳过 } // 3.2 计算轨迹代价 TrajectoryCost cost = cost_calculator_.Calculate(candidate_traj, ref_line, obstacles); // 3.3 碰撞检查（硬约束） if (cost.collision_cost > COLLISION_THRESHOLD) { continue; // 发生碰撞，直接丢弃 } // 3.4 选择代价最小的轨迹 if (cost.total_cost < min_cost) { min_cost = cost.total_cost; best_trajectory = candidate_traj; } } // Step 4: 后处理与返回 if (best_trajectory.points.empty()) { // 没有找到无碰撞轨迹！启用应急规划或返回上一周期轨迹 return GenerateEmergencyTrajectory(ego_state); } return PostProcess(best_trajectory); // 可能进行平滑或速度曲线修正 }

关键子函数详解：

SampleEndStates函数：这是决定规划多样性的关键。采样策略通常分纵向和横向。

• 纵向采样：在目标时间T（如3秒后）的纵向位置s上采样。可以按固定间隔采样，也可以根据前车速度动态调整。例如，采样“跟车”、“定速巡航”、“轻微加速”、“轻微减速”等几种模式。
• 横向采样：在目标横向位置l上采样。对于车道保持，l=0；对于换道，l=±车道宽度。也可以采样几个中间值以应对轻微避障。

void LatticePlanner::SampleEndStates(const FrenetState &start, const ReferenceLine &ref_line, const std::vector<Obstacle> &obstacles, std::vector<FrenetState> &samples) { samples.clear(); double planning_time = config_.planning_horizon; // e.g., 3.0 seconds // 纵向采样：基于当前速度和前车状态 double current_speed = start.s_dot; double target_s = start.s + current_speed * planning_time; // 基础：匀速运动 // 示例：采样5个纵向目标，从减速到加速 for (int i = -2; i <= 2; ++i) { double delta_s = i * config_.longitudinal_sample_step; // e.g., 2.0 meters FrenetState sample; sample.s = target_s + delta_s; sample.s_dot = current_speed + (delta_s / planning_time); // 粗略估算终点速度 sample.s_ddot = 0.0; // 假设终点加速度为0 // 横向采样：假设当前车道中心l=0，右车道中心l=-3.5 std::vector<double> lateral_targets = {0.0, -3.5}; for (double l : lateral_targets) { sample.l = l; sample.l_dot = 0.0; // 假设终点横向速度为0 sample.l_ddot = 0.0; samples.push_back(sample); } } // 更复杂的采样会考虑前车距离，决定是跟车、超车还是换道。 }

GenerateCandidateTrajectory函数：调用多项式生成器，分别生成纵向(s-t)和横向(l-s)轨迹，然后合并。

bool GenerateCandidateTrajectory(const FrenetState &start, const FrenetState &end, const ReferenceLine &ref_line, Trajectory &traj) { // 1. 生成纵向轨迹 (s关于时间t的函数) LongitudinalQuarticPoly lon_poly(start.s, start.s_dot, start.s_ddot, end.s, end.s_dot, config_.planning_time); // 2. 生成横向轨迹 (l关于纵向位移s的函数) // 注意：这里end.l, end.l_prime是相对于s的导数 QuinticPolynomial lat_poly(start.l, start.l_prime, start.l_prime_prime, end.l, end.l_prime, end.l_prime_prime, end.s - start.s); // 横向多项式是l(s)，参数是纵向位移差 // 3. 离散化时间，合成轨迹点 traj.points.clear(); for (double t = 0; t <= config_.planning_time; t += config_.traj_point_interval) { double s = lon_poly.EvaluateS(t); double s_dot = lon_poly.EvaluateV(t); double s_ddot = lon_poly.EvaluateA(t); // 将s代入横向多项式，得到l double l = lat_poly.Calculate(s - start.s); // 注意自变量是delta_s double l_prime = lat_poly.CalculateFirstDerivative(s - start.s); double l_prime_prime = lat_poly.CalculateSecondDerivative(s - start.s); // 将Frenet点(s, l)转换回笛卡尔坐标系(x, y)，并计算航向角、曲率等 CartesianPoint cartesian; if (!frenet_converter_.FrenetToCartesian(s, l, ref_line, cartesian)) { return false; // 转换失败 } // 计算速度、加速度在笛卡尔坐标系下的分量（需要链式求导） // ... 此处涉及Frenet到Cartesian速度/加速度的复杂转换 traj.points.push_back({t, cartesian.x, cartesian.y, cartesian.theta, ...}); } return true; }

踩坑记录：Frenet到Cartesian的速度、加速度转换是新手最容易出错的地方。因为v_x = s_dot * cos(theta_r) - l_dot * sin(theta_r)这个公式只在横向速度l_dot很小、且参考线曲率不大时近似成立。精确转换需要考虑参考线曲率kappa_r的影响，公式更为复杂。如果忽略这一点，在弯道生成的轨迹速度方向会严重偏离实际航向，导致控制跟踪失败。建议仔细推导或查阅权威资料中的完整公式。

4.3 轨迹评价与选择的实现细节

CostCalculator::Calculate函数是代价计算的核心。它需要遍历轨迹的每一个点，与所有障碍物进行交互计算。

TrajectoryCost CostCalculator::Calculate(const Trajectory &traj, const ReferenceLine &ref_line, const std::vector<Obstacle> &obstacles) { TrajectoryCost cost; double sum_curvature = 0.0; double sum_accel_sq = 0.0; double closest_dist_to_obs = std::numeric_limits<double>::max(); for (size_t i = 0; i < traj.points.size(); ++i) { const auto &pt = traj.points[i]; // 1. 计算舒适性代价（曲率、加速度） sum_curvature += pt.kappa * pt.kappa; sum_accel_sq += (pt.a_x * pt.a_x + pt.a_y * pt.a_y); // 2. 计算与障碍物的最近距离 for (const auto &obs : obstacles) { double dist = DistancePointToPolygon(pt, obs.polygon); if (dist < closest_dist_to_obs) { closest_dist_to_obs = dist; } // 3. 精确碰撞检测（使用车辆和障碍物的多边形） if (CheckCollision(ego_polygon_at_t, obs.polygon_at_t)) { cost.collision_cost = COLLISION_PENALTY; return cost; // 一旦碰撞，立即返回极大代价 } } // 4. 计算横向偏移代价 cost.lateral_offset_cost += std::fabs(pt.l); } // 归一化并加权求和 size_t n = traj.points.size(); cost.curvature_cost = config_.weight_curvature * sum_curvature / n; cost.acceleration_cost = config_.weight_accel * sum_accel_sq / n; cost.buffer_cost = config_.weight_buffer * std::exp(-config_.buffer_gain * closest_dist_to_obs); cost.lateral_offset_cost = config_.weight_lateral * cost.lateral_offset_cost / n; // ... 计算其他代价 cost.total_cost = cost.collision_cost + cost.buffer_cost + cost.lateral_offset_cost + cost.curvature_cost + cost.acceleration_cost + ...; return cost; }

碰撞检测优化：遍历所有轨迹点和所有障碍物是O(N*M)的复杂度，在障碍物多时可能成为瓶颈。优化方法包括：

1. 空间划分：使用KD-Tree或网格划分障碍物，快速查询轨迹点附近的障碍物。
2. 粗略筛选：先计算障碍物与轨迹的包围盒是否相交，不相交则跳过精细检测。
3. 时间维度：对于动态障碍物，需要在其预测轨迹的每个时间步进行检测，计算量更大，通常需要更精细的优化。

5. 常见问题、调试技巧与性能优化

5.1 轨迹抖动与不平滑问题

这是实现Lattice Planner时最常见的问题。现象是输出的轨迹曲率或加速度不连续，车辆控制器跟踪时产生顿挫。

可能原因及解决方案：

1. 参考线不平滑：这是根源。检查输入的车道中心线参考点。如果相邻点之间的方向角变化剧烈，Frenet坐标计算和轨迹生成都会出问题。

   • 解决：对原始参考线进行强制的平滑处理。推荐使用三次样条插值（Cubic Spline）或多项式平滑。确保平滑后的参考线至少是C2连续（位置、一阶导、二阶导连续）。

2. 多项式拟合的病态方程：当规划时间T过小或起止状态过于接近时，用于求解多项式系数的矩阵A可能接近奇异，导致数值不稳定，解出的系数巨大，轨迹震荡。

   • 解决：增加规划时间T，或对采样终点状态进行合理性检查，避免起止点太近。在代码中，可以计算矩阵A的条件数，如果过大则拒绝生成该轨迹。

3. Frenet到Cartesian转换误差：如前所述，忽略参考线曲率的近似转换公式在弯道会引入误差，导致计算出的车辆航向角与轨迹切线方向不一致。

   • 解决：实现精确的转换公式。或者，在生成轨迹后，增加一个后处理平滑步骤，例如对轨迹的笛卡尔坐标点序列应用一个滑动平均滤波器或进行二次规划微调。

4. 代价函数权重失衡：如果舒适性代价（曲率、加速度）的权重过低，算法可能会选择一条“捷径”，即使这条捷径需要急转弯。

   • 解决：系统性地调整权重。建议使用网格搜索或自动化化工具（如贝叶斯优化）在仿真环境中调参。记录不同权重下的舒适性指标（如加速度均方根RMS）。

5.2 实时性不达标

规划算法必须在几十毫秒内完成，否则就会失去时效性。

性能瓶颈分析与优化：

1. 采样数量过多：这是最直接的因素。采样10个终点和采样100个终点，计算量差10倍。

   • 优化：设计更智能的采样策略。例如，根据驾驶场景（高速、拥堵）动态调整采样分辨率。使用非均匀采样，在更可能的最优解区域（如当前车道中心、前车后方安全距离）进行密集采样，在其他区域稀疏采样。

2. 碰撞检测耗时：O(N*M)的暴力检测在复杂场景下不可行。

   • 优化：
     • 使用**轴对齐包围盒（AABB）**进行快速粗筛。
     • 将静态障碍物存入空间索引结构（如R-Tree, Grid Map），查询轨迹点周围一定范围内的障碍物。
     • 对于动态障碍物，可以将其预测轨迹简化为几个关键时间点的多边形，而不是每个时间步都检测。

3. 运动基元在线生成：如果每次规划都重新解算多项式系数，会有大量线性方程求解。

   • 优化：预计算运动基元库。离线生成海量的、覆盖各种起止状态的轨迹，并存储为查找表。在线规划时，只需根据当前状态和采样终点，从库中查找最接近的几条轨迹进行微调或直接使用。这能极大提升速度。

4. 代码层面优化：

   • 使用Eigen并确保向量化：Eigen库在启用编译器优化（如-O3,-march=native）后，能利用SIMD指令进行加速。
   • 避免动态内存分配：在热循环（如遍历轨迹点）中，避免使用std::vector::push_back，可以预先分配好内存。
   • 并行化：候选轨迹的评价是相互独立的，可以轻松并行。使用C++标准库的<execution>策略或OpenMP来并行化for循环。

   #include <execution> std::vector<double> costs(candidate_trajs.size()); std::transform(std::execution::par, candidate_trajs.begin(), candidate_trajs.end(), costs.begin(), [&](const Trajectory& traj) { return cost_calculator_.Calculate(traj, ref_line, obstacles).total_cost; });

5.3 在复杂场景下的失败与应对策略

Lattice Planner在简单结构化道路表现良好，但在以下场景可能失败或规划出次优轨迹：

1. 密集动态障碍物场景（如十字路口无序穿行）：采样空间可能被障碍物完全堵死，找不到任何无碰撞轨迹。

   • 应对：引入时空联合搜索。不仅采样空间终点，也采样不同的时间偏移。或者，采用交互式规划，在代价函数中建模他车可能对你的行为做出的反应，选择“合作性”更好的轨迹。

2. 狭窄通道或非结构化区域：预定义的运动基元可能无法精确匹配通过狭窄间隙所需的复杂机动。

   • 应对：与局部优化器结合。让Lattice Planner生成一个粗略的、无碰撞的轨迹作为初始解，然后使用数值优化方法（如凸优化、非线性优化）对这个初始解进行微调，使其更精确地贴合障碍物轮廓。

3. 长距离规划：规划视野越长，不确定性越大，采样空间呈指数增长。

   • 应对：采用分层规划。上层使用轻量级的、粗糙的Lattice规划器进行全局路径和车道选择。下层使用更精细的、考虑更多细节的Lattice或优化器进行短时域（如2-3秒）的轨迹生成。

调试与可视化建议：

• 可视化一切：将参考线、采样终点、所有候选轨迹（用不同颜色表示代价）、最终选择轨迹、障碍物及其预测轨迹都画出来。这是调试算法最有效的手段。可以使用ROS的RViz或简单的Matplotlib/Python脚本。
• 记录与回放：将每次规划周期的输入（车辆状态、障碍物）、中间结果（采样点、所有轨迹代价）、输出轨迹都记录下来。当出现异常行为时，可以离线回放分析，定位是哪个环节出了问题。
• 单元测试：为FrenetConverter、PolynomialGenerator、CostCalculator等核心模块编写详尽的单元测试，确保基础计算的正确性。