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在线循环通常包含以下四个核心步骤我们可以用一个简单的流程图来建立直观认识轨迹采样根据当前车辆状态位置、速度、朝向和预测模块输出的障碍物未来轨迹在Frenet坐标系以道路中心线为参考下采样一系列候选的终点状态s, l, s_dot, l_dot等。轨迹生成针对每一个采样到的终点状态从运动基元库中或通过在线数值计算如多项式拟合生成一条连接当前状态到终点状态的轨迹。轨迹评价为每一条生成的候选轨迹计算一个代价Cost。代价函数是算法的灵魂通常包括安全性代价与静态障碍物、动态障碍物的距离。舒适性代价加速度、加加速度Jerk、曲率的平方和。目标代价与期望速度、期望车道中心的偏差。效率代价轨迹的长度或预计行驶时间。轨迹选择从所有候选轨迹中选择总代价最小的一条作为最终输出的轨迹。下面我们用一个表格来对比一下Lattice Planner和其他常见规划器的特点方便你根据项目需求做选择特性Lattice PlannerA*/Dijkstra (路径)RRT* (路径)优化类规划器 (如QP)输出类型轨迹(带时间、速度)路径 (仅空间点)路径 (仅空间点)轨迹(带时间、速度)环境适应性高度结构化环境(道路)通用 (栅格地图)复杂、非结构化环境结构化环境实时性高(依赖预计算)高中等较低 (在线求解优化问题)轨迹质量平滑符合动力学不平滑折线可能不平滑非常平滑最优确定性高(结果稳定)高低 (随机性)高核心思想运动基元采样与评价图搜索随机采样与树扩展数值优化3. 核心细节解析与C实现要点3.1 坐标系的选择为什么一定是Frenet坐标系在Lattice Planner中我们几乎总是在Frenet坐标系下工作而不是笛卡尔坐标系x, y。这是由道路环境的结构化特性决定的。在笛卡尔坐标系中一条弯曲的道路对应的是复杂的曲线方程车辆相对于道路的横向偏移计算起来很麻烦。而Frenet坐标系将道路中心线抽象为一条参考线用两个坐标来描述车辆位置s (纵向位移)沿着参考线从起点到车辆投影点的弧长。l (横向位移)车辆位置垂直于参考线方向的偏移量向左为正或向右为正取决于约定。这样做的好处极大解耦将复杂的二维平面运动分解为沿道路方向的纵向运动和垂直于道路的横向运动。规划时可以分别考虑纵向的跟车、超车和横向的换道、避障。简化问题车道保持的目标就是使 l0换道就是使 l 从一个值平滑地变化到另一个值如3.5米。障碍物的投影也变得更简单。匹配感知车道线检测的输出天然就是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); // t0时的约束: 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; // 加加速度代价 // ... 其他代价 };各子代价的计算方法碰撞代价这是硬约束一票否决。遍历轨迹上的每一个点检查其轮廓通常用矩形或多边形近似是否与任何障碍物的轮廓重叠。如果发生重叠则赋予一个极大的代价如1e8或直接将该轨迹剔除。缓冲距离代价即使没有碰撞我们也希望离障碍物远一些。计算轨迹上每个点到最近障碍物的距离d代价可以是exp(-alpha * d)或1/(d epsilon)。距离越近代价越高。横向偏移代价鼓励车辆保持在车道中心。计算轨迹上各点的横向位移l的平方和或绝对值之和。纵向速度代价鼓励车辆以期望速度如道路限速行驶。计算轨迹终点速度或平均速度与期望速度的偏差平方。舒适性代价曲率代价计算轨迹上各点的曲率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::vectorObstacle obstacles) { Trajectory best_trajectory; double min_cost std::numeric_limitsdouble::max(); // Step 1: 状态转换到Frenet坐标系 FrenetState start_frenet frenet_converter_.CartesianToFrenet(ego_state, ref_line); // Step 2: 终点状态采样 std::vectorFrenetState 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上采样。对于车道保持l0对于换道l±车道宽度。也可以采样几个中间值以应对轻微避障。void LatticePlanner::SampleEndStates(const FrenetState start, const ReferenceLine ref_line, const std::vectorObstacle obstacles, std::vectorFrenetState 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 // 横向采样假设当前车道中心l0右车道中心l-3.5 std::vectordouble 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::vectorObstacle obstacles) { TrajectoryCost cost; double sum_curvature 0.0; double sum_accel_sq 0.0; double closest_dist_to_obs std::numeric_limitsdouble::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)的复杂度在障碍物多时可能成为瓶颈。优化方法包括空间划分使用KD-Tree或网格划分障碍物快速查询轨迹点附近的障碍物。粗略筛选先计算障碍物与轨迹的包围盒是否相交不相交则跳过精细检测。时间维度对于动态障碍物需要在其预测轨迹的每个时间步进行检测计算量更大通常需要更精细的优化。5. 常见问题、调试技巧与性能优化5.1 轨迹抖动与不平滑问题这是实现Lattice Planner时最常见的问题。现象是输出的轨迹曲率或加速度不连续车辆控制器跟踪时产生顿挫。可能原因及解决方案参考线不平滑这是根源。检查输入的车道中心线参考点。如果相邻点之间的方向角变化剧烈Frenet坐标计算和轨迹生成都会出问题。解决对原始参考线进行强制的平滑处理。推荐使用三次样条插值Cubic Spline或多项式平滑。确保平滑后的参考线至少是C2连续位置、一阶导、二阶导连续。多项式拟合的病态方程当规划时间T过小或起止状态过于接近时用于求解多项式系数的矩阵A可能接近奇异导致数值不稳定解出的系数巨大轨迹震荡。解决增加规划时间T或对采样终点状态进行合理性检查避免起止点太近。在代码中可以计算矩阵A的条件数如果过大则拒绝生成该轨迹。Frenet到Cartesian转换误差如前所述忽略参考线曲率的近似转换公式在弯道会引入误差导致计算出的车辆航向角与轨迹切线方向不一致。解决实现精确的转换公式。或者在生成轨迹后增加一个后处理平滑步骤例如对轨迹的笛卡尔坐标点序列应用一个滑动平均滤波器或进行二次规划微调。代价函数权重失衡如果舒适性代价曲率、加速度的权重过低算法可能会选择一条“捷径”即使这条捷径需要急转弯。解决系统性地调整权重。建议使用网格搜索或自动化化工具如贝叶斯优化在仿真环境中调参。记录不同权重下的舒适性指标如加速度均方根RMS。5.2 实时性不达标规划算法必须在几十毫秒内完成否则就会失去时效性。性能瓶颈分析与优化采样数量过多这是最直接的因素。采样10个终点和采样100个终点计算量差10倍。优化设计更智能的采样策略。例如根据驾驶场景高速、拥堵动态调整采样分辨率。使用非均匀采样在更可能的最优解区域如当前车道中心、前车后方安全距离进行密集采样在其他区域稀疏采样。碰撞检测耗时O(N*M)的暴力检测在复杂场景下不可行。优化使用**轴对齐包围盒AABB**进行快速粗筛。将静态障碍物存入空间索引结构如R-Tree, Grid Map查询轨迹点周围一定范围内的障碍物。对于动态障碍物可以将其预测轨迹简化为几个关键时间点的多边形而不是每个时间步都检测。运动基元在线生成如果每次规划都重新解算多项式系数会有大量线性方程求解。优化预计算运动基元库。离线生成海量的、覆盖各种起止状态的轨迹并存储为查找表。在线规划时只需根据当前状态和采样终点从库中查找最接近的几条轨迹进行微调或直接使用。这能极大提升速度。代码层面优化使用Eigen并确保向量化Eigen库在启用编译器优化如-O3,-marchnative后能利用SIMD指令进行加速。避免动态内存分配在热循环如遍历轨迹点中避免使用std::vector::push_back可以预先分配好内存。并行化候选轨迹的评价是相互独立的可以轻松并行。使用C标准库的execution策略或OpenMP来并行化for循环。#include execution std::vectordouble 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在简单结构化道路表现良好但在以下场景可能失败或规划出次优轨迹密集动态障碍物场景如十字路口无序穿行采样空间可能被障碍物完全堵死找不到任何无碰撞轨迹。应对引入时空联合搜索。不仅采样空间终点也采样不同的时间偏移。或者采用交互式规划在代价函数中建模他车可能对你的行为做出的反应选择“合作性”更好的轨迹。狭窄通道或非结构化区域预定义的运动基元可能无法精确匹配通过狭窄间隙所需的复杂机动。应对与局部优化器结合。让Lattice Planner生成一个粗略的、无碰撞的轨迹作为初始解然后使用数值优化方法如凸优化、非线性优化对这个初始解进行微调使其更精确地贴合障碍物轮廓。长距离规划规划视野越长不确定性越大采样空间呈指数增长。应对采用分层规划。上层使用轻量级的、粗糙的Lattice规划器进行全局路径和车道选择。下层使用更精细的、考虑更多细节的Lattice或优化器进行短时域如2-3秒的轨迹生成。调试与可视化建议可视化一切将参考线、采样终点、所有候选轨迹用不同颜色表示代价、最终选择轨迹、障碍物及其预测轨迹都画出来。这是调试算法最有效的手段。可以使用ROS的RViz或简单的Matplotlib/Python脚本。记录与回放将每次规划周期的输入车辆状态、障碍物、中间结果采样点、所有轨迹代价、输出轨迹都记录下来。当出现异常行为时可以离线回放分析定位是哪个环节出了问题。单元测试为FrenetConverter、PolynomialGenerator、CostCalculator等核心模块编写详尽的单元测试确保基础计算的正确性。