自动驾驶轨迹离散化核心：Apollo DiscretizedTrajectory深度解析

1. 先看清楚DiscretizedTrajectory到底解决什么问题

做自动驾驶规划控制的朋友应该都有这个体会：规划模块输出的是一条连续的轨迹，但下游控制模块、仿真模块、决策模块真正消费数据时，拿到手的却是一串按时间排列的离散点。这条“连续轨迹离散化”的链路，在Apollo里就落在modules/planning/common/trajectory/DiscretizedTrajectory这个类上。它是planning模块里最常用的数据结构之一，几乎所有与轨迹相关的交互——给控制模块发轨迹点、在轨迹上做时间查询、按路径里程找最近点——都要经过它。

刚开始啃Apollo代码时，我花了很长时间才搞明白：为什么规划模块内部生成的是ADCTrajectory（protobuf消息），而各模块之间传的却是DiscretizedTrajectory对象？为什么查询轨迹点要用二分查找而不是直接遍历？为什么QueryNearestPoint找的是几何最近点，而不是时间轴上的最近点？这些疑问如果不彻底搞清楚，写下游代码时很容易踩坑。

这篇文章我会从类的整体设计思路讲起，再逐行拆解头文件与核心方法，结合我实际调试中遇到的坑，把DiscretizedTrajectory这200多行代码里涉及的每个关键设计决定都过一遍。适合正在学习Apollo planning模块的开发者、以及需要在规划和控制链路中做二次开发的朋友参考。

2. 整体设计思路：为什么轨迹必须是“离散的”

2.1 类在Apollo规划模块中的位置

先看调用链。Apollo planning的入口是OnLanePlanning::Run()，经过Planner::Plan()生成一条ADCTrajectory（protobuf消息），再通过ReferenceLineInfo::SetTrajectory()等逻辑传递给下游。而DiscretizedTrajectory的本质，是这条protobuf轨迹在C++内存里的“运行时表示”。

之所以不直接传递protobuf对象，是因为规划内部需要频繁对轨迹做时间、位置、航向的查询。protobuf的RepeatedPtrField遍历性能一般，且没有实现插值和二分查找这类算法。DiscretizedTrajectory把ADCTrajectory里的点拷贝到std::vector中，并对外提供一系列查询接口，这是典型的“消息数据 → 运行时结构”转换模式。

值得注意的是，DiscretizedTrajectory继承自Trajectory基类。Apollo的Trajectory基类非常“薄”，只定义了append_trajectory_point、set_trajectory_points、clear、empty、Evaluate等虚函数接口，真正的数据存储和算法实现全部落在DiscretizedTrajectory这个子类里。这种设计是为了将来如果有其他形式的轨迹（比如样条曲线轨迹、多项式轨迹），可以继承Trajectory并实现同样的接口，上层模块无需感知具体类型。

2.2 为什么采样间隔是0.1秒

DiscretizedTrajectory的默认采样间隔是0.1秒，对应规划周期10Hz。这个数值不是随意定的。

自动驾驶决策链条中，规划模块以10Hz的频率运行，而控制模块通常以50到100Hz的频率运行。如果规划模块直接丢给控制模块一条稀疏轨迹点，控制模块在两次规划之间拿不到新的参考点，就只能靠自身算法内插，控制效果会明显变差。于是规划模块在构建DiscretizedTrajectory时，就把轨迹按0.1秒均匀切片。下游控制模块拿到这些点时，仍需要以更高频率做线性插值，但由于原始点间隔已经比较密（0.1秒对应大约1到2米的间隔，取决于车速），插值误差被控制在可接受范围内。

如果你在二次开发中把采样间隔改成0.05秒甚至0.02秒，能获得更平滑的轨迹，但vector的存储和拷贝开销会成倍增加。我实测过，在轨迹点数量超过500个时，DiscretizedTrajectory的拷贝构造耗时会上一个台阶，这也是Apollo里用DISALLOW_COPY_AND_ASSIGN禁用拷贝的原因之一。

2.3 类内部的设计取舍

这个类的头文件非常精简，核心成员只有一个std::vector<TrajectoryPoint> trajectory_points_。所有查询方法都围绕这个vector展开。看似简单，但里面有三个关键设计决策值得琢磨：

第一个决策是“用时间索引优先”。Evaluate(relative_time)是最高频的查询方法，内部使用二分查找定位时间区间，再做线性插值。如果按路程索引，每次查询都需要把相对时间换算成累计里程，多了几步运算。而时间戳是轨迹点的固有属性，查询时天然有序，二分查找效率很高。

第二个决策是“查询路径点用path_s”。QueryPathPointWithPathS(path_s)这个方法支持按路径里程查询，它同样用二分查找。但注意，这里的“路径”指规划模块内部的参考线路径，不等同于轨迹。路径点列表可能比轨迹点列表长很多，所以它没有复用trajectory_points_，而是要求调用方传入一个path_points列表（std::vector<common::PathPoint>），在列表上做二分和线性插值。

第三个决策是“时间越界时夹紧而非报错”。Evaluate对传入的相对时间做了边界处理：小于0取第一个点，大于总时长取最后一个点。这个设计非常务实，因为规划和控制链路中经常出现时间不同步的小误差，如果每次越界都抛异常或返回错误码，下游代码会变得很啰嗦，而且大量额外的分支判断会拖慢控制周期。

3. 头文件与核心成员拆解

3.1 类声明与继承关系

直接看头文件的关键部分。为了便于说明，我结合常见版本的Apollo源码结构做了整理：

class DiscretizedTrajectory : public Trajectory { public: DiscretizedTrajectory() = default; explicit DiscretizedTrajectory(const std::vector<TrajectoryPoint>& points); explicit DiscretizedTrajectory(const ADCTrajectory& trajectory); virtual ~DiscretizedTrajectory() = default; DiscretizedTrajectory(const DiscretizedTrajectory&) = delete; DiscretizedTrajectory& operator=(const DiscretizedTrajectory&) = delete; void append_trajectory_point(const TrajectoryPoint& trajectory_point) override; void set_trajectory_points(const std::vector<TrajectoryPoint>& points) override; void clear() override; bool empty() const override; std::uint32_t NumOfPoints() const; const TrajectoryPoint& StartPoint() const; const TrajectoryPoint& LastPoint() const; const TrajectoryPoint& Evaluate(const double relative_time) const override; bool Evaluate(const double relative_time, TrajectoryPoint* const point) const override; int QueryLowerBoundPoint(const double relative_time, const double epsilon = 1.0e-5) const; int QueryNearestPoint(const Vec2d& position) const; int QueryNearestPoint(const Vec2d& position, const double head_heading, const double heading_buffer) const; int QueryPathPointWithPathS(const std::vector<common::PathPoint>& path_points, const double path_s) const; private: std::vector<TrajectoryPoint> trajectory_points_; };

代码里没有复杂模板、没有多重继承，就是一个“数据容器 + 查询算法的封装”。TrajectoryPoint是Apollo定义好的轨迹点结构，内部包含路径点信息（x、y、z、theta、kappa、s）、时间戳（relative_time）、纵向速度（v）、加速度（a）、加加速度（jerk）等。一个轨迹点就是车辆在某个时刻的完整运动状态。

3.2 构造函数与数据初始化流程

DiscretizedTrajectory有两个构造函数：从std::vector<TrajectoryPoint>构造，以及从ADCTrajectory构造。

从ADCTrajectory构造是规划主链路最常走的路径。构造函数内部遍历trajectory.trajectory_point()，把每个protobuf点转成TrajectoryPoint，塞进trajectory_points_。这里有一个比较隐蔽的细节：protobuf里的TrajectoryPoint带的是path_point+v+a+relative_time+steer等字段，而C++里的TrajectoryPoint结构体字段基本一一对应，转换成本不高，但要注意steer（前轮转角）在新版本中被逐步弱化，很多下游逻辑已经不再读取它。

从std::vector<TrajectoryPoint>构造的场景主要出现在测试代码和一些中间转换模块中。比如你在做LQR控制器的仿真验证时，可以自己造一串轨迹点塞进去，然后调用Evaluate验证控制器效果。

构造函数还有一个值得注意的点：Apollo在头文件里通过delete禁用了拷贝构造和赋值运算符。为什么？因为这条轨迹很可能在多个线程间共享，尤其是planning发布轨迹后，控制模块和仿真模块都要读它。如果允许拷贝，代码里很容易出现意外深拷贝，带来性能开销。实际开发中如果需要传递这条轨迹，用std::shared_ptr即可。

3.3 轨迹点的一致性与时间单调性

构造完成后，trajectory_points_里的点默认是严格按时间递增排列的。这个前提很关键，因为后面所有二分查找都依赖它。

但“默认”不等于“总是”。二次开发时如果绕过构造函数、手动调用append_trajectory_point()往尾部和中间插入点，就必须自己保证时间戳单调递增。我在这上面栽过一次：调试一个局部路径规划算法时，候选轨迹生成阶段用append_trajectory_point添加点，由于生成算法在回溯时按“先节点、后时刻”的顺序输出，导致点的时间戳出现跳变。结果QueryLowerBoundPoint返回的索引完全错位，Evaluate插值出的位置和速度出现明显毛刺。排查了很久才发现是构造时的时间顺序问题。

所以，凡是往DiscretizedTrajectory里写数据，请务必先确认时间单调性。如果从外部算法拿到的点无法保证，先排序再构造最稳妥。

4. 高频查询方法逐一解读

4.1 Evaluate：控制模块的“取点器”

Evaluate是DiscretizedTrajectory里使用频率最高的方法。控制模块每次控制周期（比如100Hz）都会调用它，传入当前时刻相对规划开始时刻的时间偏移，拿到对应的轨迹点作为参考。

方法内部逻辑可以拆成四步：

第一步，边界处理。如果相对时间小于等于0，直接返回第一个点；如果大于等于轨迹总时长，返回最后一个点。

第二步，定位时间区间。使用QueryLowerBoundPoint找到第一个时间戳大于等于目标时间的点的索引。这里用二分查找，时间复杂度O(log n)。

第三步，根据定位结果选择逻辑。如果index == 0，说明目标时间在第一个点之前，直接返回第一个点。如果index >= NumOfPoints() - 1，说明目标时间在后面，返回最后一个点。否则进入插值流程。

第四步，线性插值。取index - 1和index两个点，根据时间差做比例插值。插值的字段不仅仅是位置坐标，还包括速度、加速度、航向角等：

bool DiscretizedTrajectory::Evaluate(const double relative_time, TrajectoryPoint* const point) const { if (trajectory_points_.empty()) { return false; } if (relative_time <= 0.0) { *point = trajectory_points_.front(); return true; } ... std::uint32_t index = QueryLowerBoundPoint(relative_time); if (index == 0) { *point = trajectory_points_.front(); return true; } if (index >= NumOfPoints()) { *point = trajectory_points_.back(); return true; } const double time_diff = trajectory_points_[index].relative_time() - trajectory_points_[index - 1].relative_time(); const double ratio = (relative_time - trajectory_points_[index - 1].relative_time()) / time_diff; point->set_relative_time(relative_time); point->mutable_path_point()->set_s( common::util::lerp(trajectory_points_[index - 1].path_point().s(), trajectory_points_[index].path_point().s(), ratio)); point->set_v(common::util::lerp(trajectory_points_[index - 1].v(), trajectory_points_[index].v(), ratio)); ... }

这里用了lerp（linear interpolation）函数，对s、v、a、x、y、theta等字段逐项插值。注意航向角theta的插值用的是普通线性插值，没有做角度环绕处理。这在轨迹平滑连续的场景下问题不大，但如果两个相邻点的航向跨过了±π的边界（比如差角接近180度），插值结果会有明显跳变。

我在做停车场场景调试时就遇到过这种情况——车辆原地掉头时，相邻两个轨迹点的航向角一个是接近+180度，另一个接近-180度，线性插值出来中间值会跑到0度附近，导致控制指令瞬间反向。这个问题Apollo里部分场景通过角度归一化函数规避，但DiscretizedTrajectory::Evaluate本身没有做这种处理，二次开发时如果处理大转角场景，需要自己对theta做角度归一化后再插值。

4.2 QueryLowerBoundPoint：二分查找的时间下界

如果只看函数名，容易以为它只是给Evaluate做内部辅助。实际上它是DiscretizedTrajectory里另一个非常核心的查询接口，返回的是“第一个时间戳大于等于目标时间”的点的索引。

int DiscretizedTrajectory::QueryLowerBoundPoint(const double relative_time, const double epsilon) const { if (trajectory_points_.empty()) { return 0; } if (relative_time <= 0.0) { return 0; } const double target_time = relative_time - epsilon; auto it_lower = std::lower_bound( trajectory_points_.begin(), trajectory_points_.end(), target_time, [](const TrajectoryPoint& point, const double target_time) { return point.relative_time() < target_time; }); return static_cast<int>(it_lower - trajectory_points_.begin()); }

epsilon参数用于时间容差，默认是1e-5。这么小的值在大多数场景下只是防止浮点数精度问题，但如果你在仿真中遇到了“差一点点就命中但实际没命中”的问题，可以考虑把epsilon调大到1e-3。

二分查找的复杂度是O(log n)，对于几百个点的轨迹非常快。但要注意，Evaluate内部调用了QueryLowerBoundPoint，而QueryLowerBoundPoint没有做时间越界处理。当传入的relative_time大于轨迹总时长时，std::lower_bound会返回end()，对应索引可能等于NumOfPoints()。Evaluate里已经处理了这种情况，但如果你直接调用QueryLowerBoundPoint再自己取点，一定要判断索引是否越界。

我在一次二次开发中写过一个轨迹裁剪工具，直接调QueryLowerBoundPoint做时间裁剪，结果在裁剪点接近轨迹末尾时索引越界，程序直接崩溃。加了越界判断后问题消失。

4.3 QueryNearestPoint：几何最近点查找

这个方法解决的是“给定一个平面位置，找出轨迹上离它最近的轨迹点”。它内部直接线性遍历所有点，计算欧氏距离平方，取最小值。

int DiscretizedTrajectory::QueryNearestPoint(const Vec2d& position) const { if (trajectory_points_.empty()) { return 0; } double distance_sqr = std::numeric_limits<double>::max(); int index = 0; for (int i = 0; i < static_cast<int>(trajectory_points_.size()); ++i) { const double curr_distance_sqr = trajectory_points_[i].path_point().x() - position.x(); ... } return index; }

实现上它没有开方，直接用距离平方比较，减少计算量。这个函数在大轨迹（几百个点）上性能还能接受，但如果上游把轨迹扩展到上千个点，每次查询都遍历一遍就不太合适了。更优的做法是构建空间索引或使用KD-Tree，但Apollo目前在这条链路上没有做这种优化，因为规划轨迹点数通常不超过500个，遍历的成本约几十微秒，在10Hz规划周期里占比很小。

QueryNearestPoint还有一个带航向约束的重载版本：

int QueryNearestPoint(const Vec2d& position, const double head_heading, const double heading_buffer) const;

它会在遍历时额外检查轨迹点的航向角是否在[head_heading - heading_buffer, head_heading + heading_buffer]范围内，只有满足航向约束的点才参与最近距离比较。这个功能在匝道汇入、变道决策等需要“寻找前方同向轨迹点”的场景里很有用。但注意，如果你的车辆正在掉头或做横摆角很大的动作，heading_buffer设置得太小会导致找不到点，返回0号点，这会让决策逻辑误判为“前方没有可用轨迹点”。

4.4 QueryPathPointWithPathS：按里程查找路径点

QueryPathPointWithPathS解决的问题是：规划内部经常需要“在这条路径上走了多少米”来定位。它接收一个path_points列表和一个s值，返回路径上里程为s的点的索引。这个接口和轨迹本身关系不大，更像是一个独立的路径查询工具。

实现上跟QueryLowerBoundPoint类似，先用二分查找定位s所在的区间，再做线性插值。有一点要注意：它返回的是“插值后位于s处的路径点对应的索引”。这个索引和轨迹点的索引不是一回事，不能混用。

我见过有初学者在这里踩坑：用QueryPathPointWithPathS拿到索引后，直接用这个索引去trajectory_points_里取点。由于路径点列表和轨迹点列表长度不一致、顺序也未必一致，取出来的点完全不对。正确做法是：先用里程索引定位路径点，再通过路径点上的时间戳或其他关联字段，去轨迹点列表里查对应轨迹点。

4.5 AppendTrajectoryPoint与数据维护

append_trajectory_point在类里是尾插操作，但Apollo的实现在插入前做了一些处理：

void DiscretizedTrajectory::append_trajectory_point( const TrajectoryPoint& trajectory_point) { if (!trajectory_points_.empty()) { if (trajectory_points_.back().relative_time() < trajectory_point.relative_time()) { trajectory_points_.push_back(trajectory_point); } } else { trajectory_points_.push_back(trajectory_point); } }

如果待插入点的时间戳不大于末尾点的时间戳，就直接丢弃。这个保护逻辑帮调用方过滤掉了因浮点误差产生的时间倒退点，但同时也意味着：如果你真的需要“在轨迹中间插入点”，不能用这个方法，得自己构造新的vector然后调用set_trajectory_points。

set_trajectory_points是整体覆盖逻辑，适用于轨迹拼接、重采样等场景。clear方法清空所有点，empty判断是否为空。

5. 实战心得：我把这个类“用坏”的几次经历

5.1 QueryNearestPoint不等于时间轴最近点

第一次用QueryNearestPoint时，我想当然地以为它是“找距离当前时间最近的轨迹点”，结果发现它返回的是“距离某个平面坐标最近的轨迹点”。这两个语义完全不同。

时间轴最近点应该是用QueryLowerBoundPoint(relative_time)查找。而QueryNearestPoint是几何查询，用于“车辆当前位置距离轨迹上哪个点最近”，这对判断车辆是否偏离轨迹、是否应该重新规划非常关键。

如果你的逻辑里混用了这两个语义，会出现一个非常隐蔽的bug：车辆在一条闭环轨迹上行驶时，当前位置恰好处于几何上最接近某个轨迹点的位置，但那个点在时间上其实是上一周的轨迹点。如果你的决策逻辑依赖“最近点”来取速度限制、曲率限制，就可能取到过时的数据。

5.2 从0.01s步长改成0.05s步长后，控制系统开始“抽搐”

在做路径跟踪控制器仿真时，我用自定义轨迹生成器构造了一条步长为0.01秒的密集轨迹，直接塞进DiscretizedTrajectory。仿真效果很好。后来为了降低计算量，把步长改成0.05秒，结果控制器输出出现明显的抖动。

原因在于轨迹点的稀疏程度直接影响插值结果。步长越大，相邻轨迹点之间的速度、加速度变化越粗糙，线性插值出来的中间点与真实期望轨迹的偏差越大。尤其在中高速场景，0.05秒对应1.4米（车速100km/h时），如果轨迹曲率变化较快，插值点的航向误差会被放大。

这给我们的启示是：采样间隔需要根据场景动态调整。高速巡航可以适当增大间隔，但低速泊车、狭窄路况下需要更密的轨迹点。Apollo默认的0.1秒间隔在高速场景下表现尚可，如果你做的是园区低速车、自动泊车这类场景，建议把间隔缩小到0.02到0.05秒，控制效果会有明显改善。

5.3 浮点数比较的边界问题

QueryLowerBoundPoint里用了epsilon = 1.0e-5的时间容差。这个值在大多数情况下够用，但如果你拿到的轨迹点时间戳精度不高（比如从仿真工具导出时保留了小数点后3位），就可能在边界处出现“两个点的时间差恰好等于1e-5”的情况。

我遇到过一次：轨迹点A的时间戳是10.000，轨迹点B是10.001，目标时间是10.0005。二分查找时，由于A和B的距离只有0.001，目标时间减去epsilon后是10.0004，结果lower_bound返回了A的索引而不是B的索引。由于A和B之间的时间间隔很小，插值误差不大，但如果被查找的时间刚好是两个相邻点之间的“中点”，使用epsilon反而会让索引偏向较前的一个点。这个问题在Apollo原有逻辑里影响不大，因为轨迹点的相对时间本来就是0.1秒的整数倍，如果你做了重采样或时间偏移，就要留意epsilon的取值了。

5.4 空轨迹与首尾夹紧

Evaluate方法对空轨迹的处理是返回false，调用方必须处理这个返回值。实际操作中我发现，planning链路里很多模块拿到false后直接用了StartPoint()，而StartPoint()在空vector上会触发未定义行为，表现就是偶发崩溃或读到脏数据。

我的习惯是：在构造DiscretizedTrajectory之后，立刻检查empty()，如果为空就返回错误状态，不让它进入后续控制链路。在二次开发中，这个检查放在最前面，能省掉后面一大串if (point != nullptr)之类的防御代码。

6. 常见问题排查速查表

这张表几乎覆盖了我日常答疑中被问到最多的几类问题。如果你在二次开发中遇到了和表中不同的现象，建议先打印trajectory_points_的relative_time序列，检查单调性和间隔是否正常。大部分诡异问题最后都能追溯到数据源。

7. 写在最后的调试体会

这个类本身并不复杂，但它是理解Apollo规划模块数据流的一把钥匙。

我个人的体会是：读代码时不要满足于“知道每个函数在干什么”，要把自己代入到调用方。每次调用Evaluate时，控制模块的时钟和规划模块的时钟是有偏差的；每次调用QueryNearestPoint时，车辆当前位置可能已经在轨迹外侧好几米了。想清楚这些边界条件，才能真正理解Apollo为什么在接口设计上做了那些看似多余的判断。

另外，如果你要在自己的项目里复刻类似的轨迹数据结构，我建议直接借鉴DiscretizedTrajectory的思路：vector存储、二分查找定位、线性插值填充、边界夹紧处理、禁用拷贝构造。这套组合在绝大多数场景下是性能够用且逻辑清晰的。别过早引入空间索引或者样条插值这类复杂机制，先把这条“朴素”的离散轨迹链路跑通、调稳，再根据实际瓶颈做优化。