1. 从颠簸到丝滑：为什么自动驾驶车需要轨迹平滑？

想象一下，你正坐在一辆自动驾驶汽车里，它正沿着一条规划好的路线行驶。如果这条路线是由一系列离散的GPS点或地图点直接连接而成的，你的乘坐体验会怎样？大概率会像坐过山车一样，车子在每一个点上都可能发生生硬的转折，方向盘频繁地左右猛打，乘客不仅会感到头晕目眩，车辆的机械部件也会承受不必要的磨损。这显然不是我们想要的“老司机”体验。

在自动驾驶的路径规划模块中，规划器（比如Apollo的EM Planner或Lattice Planner）最初生成的路径，通常就是由这样一系列离散的路径点（Waypoints）构成的。这些点保证了车辆能从一个地方开到另一个地方，但“能开”和“开得好”是两码事。轨迹平滑（Trajectory Smoothing） 要解决的，正是这个“开得好”的问题。它的核心任务，是把这条由折线段组成的“锯齿状”原始路径，优化成一条连续、平滑、曲率变化柔和的曲线，让车辆能够平顺地跟踪。

为什么平滑如此重要？我总结下来主要有三点。第一是乘坐舒适性，平滑的轨迹意味着更小的横向加速度和更柔和的转向，乘客不会感到被甩来甩去。第二是车辆控制的可实现性，真实的车辆有物理极限，比如最大前轮转角、最大横摆角速度，一条曲率突变（想象一个急弯接一个急弯）的路径，控制模块根本无法执行，会导致车辆失控。第三是安全性，平滑的轨迹通常意味着更可预测的车辆行为，也给控制模块留出了更多的反应和调整空间。

在百度的Apollo开源框架中，这个任务由一个叫做 DiscretizedPointsSmoother 的模块来完成。它接收规划模块输出的离散路径点，通过一套精巧的数学建模和优化算法，输出一条优化后的平滑路径。今天，我就带你深入这个模块的“心脏”，看看它是如何将“平滑”这个感性的需求，转化为计算机可以理解和求解的数学问题，并最终实现效率与效果兼顾的。我们会聚焦于最核心的数学建模和约束处理，尤其是那个将非线性难题线性化的“神来之笔”。

2. 数学建模：把“感觉”变成“公式”

要把“平滑”量化，我们首先得定义清楚：什么是“好”的路径？在Apollo的平滑器看来，一条好的路径需要平衡三个目标：路径本身要平滑、路径总长度要合理、不能偏离原始参考线太远。这就像你要画一条修正线，既要线条流畅（平滑），又不能画得太长绕远路（长度），还得紧紧贴着原来的草稿线（偏移量）。接下来，我们就把这三个目标一一“翻译”成数学公式。

2.1 优化变量：我们动哪些“棋子”？

优化问题首先得明确变量。这里非常简单直接：原始参考线给了我们 n+1 个点 (x_i_ref, y_i_ref)。我们不打算天马行空地重新规划一条全新路径，而是在这些原始点附近进行“微调”。所以，我们的优化变量就是这 n+1 个点的调整后坐标 (x_i, y_i)。也就是说，算法的工作就是为每一个原始点，计算一个微小的偏移量 (dx_i, dy_i)，得到新点 (x_i, y_i)，所有这些新点连成的线，就是我们的平滑结果。

2.2 三大优化目标：平滑、长度与忠诚度

现在，我们来定义代价函数。总代价 cost 是三个子代价的加权和：cost = w_smooth * cost_smooth + w_length * cost_length + w_ref * cost_ref。权重系数 w 就像调节旋钮，让我们可以根据场景调整侧重点，比如在高速上更看重平滑，在狭窄通道里更看重紧贴参考线。

2.2.1 平滑度代价：让转角消失

平滑度代价是最关键的部分。Apollo主要采用了两种在数学上等价但形式不同的定义方法，理解它们对看懂代码很有帮助。

• 方法一：向量和模最小化（FEM，有限元风格） 这是Apollo中 fem_pos_deviation 系列求解器使用的核心思想。看连续三个点 P_{i-1}, P_i, P_{i+1}。我们构造两个向量：a = P_i - P_{i-1} 和 b = P_{i+1} - P_i。那么向量 (a + b) = P_{i+1} - P_{i-1}。 它的物理意义是什么？你可以把 P_i 想象成一根橡皮筋上的一个结点，a 和 b 是连接它的两段。当这三个点共线时，向量 a 和 b 方向完全一致，它们的和向量 (a+b) 的模达到最大值（等于两段长度之和），但此时“拐弯”的程度是最小的（没有拐弯）。然而，我们最小化的是 (a+b) 的模的平方吗？这里有个精妙之处。实际上，我们最小化的是 (P_{i+1} - P_i) - (P_i - P_{i-1}) = P_{i-1} + P_{i+1} - 2*P_i 的模的平方。这个量被称为“二阶差分”，它衡量的是中间点 P_i 的“加速度”。当路径是直线时，这个“加速度”为零；路径弯曲越剧烈，这个值越大。因此，平滑度代价 cost_smooth 通常定义为所有中间点的二阶差分平方和：

  cost_smooth = sum_over_i( || (x_{i-1} + x_{i+1} - 2*x_i) ||^2 + || (y_{i-1} + y_{i+1} - 2*y_i) ||^2 )
  

  这个形式是二次的，非常利于优化。

• 方法二：夹角余弦最大化（Cosθ方法） 这是 cos_theta_ipopt_interface 求解器使用的直观方法。还是看三个点 P_{i-1}, P_i, P_{i+1}。我们直接关注向量 a 和 b 之间的夹角 θ。夹角 θ 越小，三点越接近一条直线，路径越平滑。而 cosθ 在 θ=0 时取得最大值1。因此，我们可以将平滑度目标定义为 最大化所有连续三点夹角的余弦之和，等价于最小化 (1 - cosθ) 之和。 cosθ 可以通过向量点积公式计算：cosθ = (a·b) / (|a| * |b|)。然而，这个公式里包含了向量的模，会引入非线性（虽然也是可处理的）。这种方法在直觉上非常直接。

在实际应用中，FEM（二阶差分）形式因其纯粹的二次形式，计算效率更高，更常被用于需要实时求解的场景，配合后续要讲的QP（二次规划）求解器。

2.2.2 长度代价：别绕远路

长度代价很好理解：我们当然不希望平滑后的路径变得冗长。它被定义为相邻优化后点之间距离的平方和：

cost_length = sum_over_i( (x_{i+1} - x_i)^2 + (y_{i+1} - y_i)^2 )

最小化这个代价，会促使优化后的点尽可能靠近，从而避免路径被不必要的拉长。注意，这里用的是平方和，而不是直接的距离和，也是为了保持目标的二次性。

2.2.3 参考点偏移代价：忠于原路线

我们不能为了平滑而天马行空，必须尊重原始的参考线。原始参考线包含了地图车道信息、障碍物避让的初步结果等重要先验。偏移代价就是每个优化后的点与其对应的原始参考点之间距离的平方和：

cost_ref = sum_over_i( (x_i - x_i_ref)^2 + (y_i - y_i_ref)^2 )

这个代价项像一个“锚点”，把优化后的点拉向原始位置，防止平滑过程脱离实际可行的区域（比如开出车道）。

2.3 整合：一个标准的二次规划问题

把上面三项加起来，我们的总目标函数就是：

Minimize: w1 * Σ||P_{i-1}+P_{i+1}-2P_i||^2 + w2 * Σ||P_{i+1}-P_i||^2 + w3 * Σ||P_i - P_i_ref||^2

由于所有项都是优化变量 (x_i, y_i) 的二次项，整个目标函数是一个二次型（Quadratic Form）。如果我们把所有的 x_i, y_i 排列成一个长向量 X，那么这个目标函数就可以写成 1/2 * X^T * H * X + f^T * X 的标准形式，其中 H 是一个海森矩阵（Hessian Matrix），f 是线性项向量。这正好是二次规划（Quadratic Programming, QP） 问题的标准形式。QP问题是凸优化中一类求解速度非常快的问题，这为我们实现实时平滑打下了基础。

3. 给优化戴上“紧箍咒”：约束条件详解

只有目标函数，优化器可能会给出数学上最优但物理上荒谬的解。比如，为了极致平滑，它可能把所有点都优化到同一个点上（长度代价和偏移代价被权重压制时）。或者，它可能生成一条曲率极大的路径，车辆根本转不过去。因此，我们必须引入约束条件。

3.1 边界框约束：活动范围限制

这是最简单直接的约束。对于每个优化点 P_i，我们不能让它偏离原始点 P_i_ref 太远。通常是以原始点为中心，设置一个矩形的“信任区域”：

x_i_ref - bound <= x_i <= x_i_ref + bound
y_i_ref - bound <= y_i <= y_i_ref + bound

这个 bound 可以根据车道宽度、定位精度等因素动态设置。这是一个线性边界约束，QP求解器处理起来轻而易举。

3.2 曲率约束：车辆能转多大的弯？

这是整个平滑问题中最关键、也最具挑战性的约束。车辆不是质点，它有最小转弯半径 R_min 的限制。对应到路径上，就是路径上任何一点的曲率 κ 必须满足 |κ| <= κ_max = 1 / R_min。

那么，如何用离散的路径点 P_i 来表示曲率呢？ Apollo采用了几何上非常经典的方法：三点求曲率。对于连续三个点 P_{i-1}, P_i, P_{i+1}，假设它们在一个圆上，那么可以利用几何关系推导出该圆的半径（即曲率半径）。

推导过程涉及一些初中几何，核心思想是利用弦长和垂直平分线的性质。最终，可以得到一个用三点坐标表示的近似曲率公式。经过简化，对于点 P_i 处的近似曲率约束可以写为：

|(向量(P_{i-1}, P_i) 与 向量(P_i, P_{i+1})的某种叉积相关形式)| <= (某系数) * κ_max

具体来说，一个常见的近似是：

| (Δx_i * Δy_{i-1} - Δy_i * Δx_{i-1}) | / (ds^3) <= κ_max

其中 Δx_i = x_{i+1} - x_i, Δy_i = y_{i+1} - y_i, ds 是点之间的近似弧长。

麻烦来了！ 仔细观察这个不等式，它包含了优化变量 (x, y) 的乘法和除法，是一个非线性约束。如果我们把问题直接交给能处理非线性约束的求解器（如IPOPT），虽然可以求解，但计算开销大，很难满足自动驾驶毫秒级的实时性要求。

4. 化繁为简的魔法：曲率约束的线性化

能否将这个“拦路虎”般的非线性约束，变成QP喜欢的线性约束呢？Apollo的工程师们给出了漂亮的答案：一阶泰勒展开线性化。这是工程优化中一个非常强大的工具。

4.1 线性化的核心思想

我们有一个关于变量 X 的非线性约束函数 F(X) <= 0。假设我们有一个初始猜测点 X_ref（比如，我们的原始参考点）。在 X_ref 这个点附近，复杂的非线性函数 F(X) 可以近似为一个线性函数。这个近似就是一阶泰勒展开：

F(X) ≈ F(X_ref) + ∇F(X_ref)^T · (X - X_ref)

其中 ∇F(X_ref) 是 F 在 X_ref 处的梯度（导数）。现在，不等式 F(X) <= 0 就变成了：

F(X_ref) + ∇F(X_ref)^T · (X - X_ref) <= 0

看！不等式右边关于 X 是线性的（因为梯度是常数，X 是变量）。我们把一个非线性约束，在参考点附近近似成了一个线性约束。

4.2 应用到曲率约束

对于我们的曲率约束 |G(x_{i-1}, y_{i-1}, x_i, y_i, x_{i+1}, y_{i+1})| <= κ_max，我们取其等价形式 G(...)^2 <= κ_max^2 来处理绝对值（或直接处理两个不等式）。我们选择原始参考点序列 (x_ref, y_ref) 作为泰勒展开的基准点 X_ref。

1. 计算基准值：将参考点坐标代入非线性函数 G，计算出 G(X_ref)。
2. 计算梯度：这是最需要细心的一步。我们需要计算 G 函数对六个变量 (x_{i-1}, y_{i-1}, x_i, y_i, x_{i+1}, y_{i+1}) 的偏导数 ∂G/∂x_{i-1}, ∂G/∂y_{i-1}, ... 这在数学上是完全可行的，因为 G 是由多项式构成的。Apollo的代码中精确地实现了这些偏导数的计算。
3. 构造线性约束：对于第 i 个点，其线性化的曲率约束方程为：

   G(X_ref_i) + [∂G/∂x_{i-1}, ... ∂G/∂y_{i+1}]_ref · ([x_{i-1}, ... y_{i+1}] - [x_ref_{i-1}, ... y_ref_{i+1}])^T <= κ_max
   

   以及大于 -κ_max 的另一个约束（处理绝对值）。 这个方程可以整理成 A_i * X <= b_i 的形式，其中 A_i 是一个行向量，b_i 是一个标量。

对所有中间点 (i=1 到 n-1) 都执行这个过程，我们就得到了一系列线性不等式约束 A * X <= b。

4.3 松弛变量：给求解器一点弹性

严格曲率约束下，问题可能无解（比如原始参考线本身就有急弯）。为此，Apollo引入了松弛变量（Slack Variable） s_i。将约束改写为：

G_linearized(X) <= κ_max + s_i
s_i >= 0

同时，在目标函数中加入一个惩罚项 w_slack * Σ(s_i)^2。这样，如果某个弯道确实无法满足曲率限制，优化器可以通过增大 s_i（付出代价）来“违反”一点约束，从而保证问题总有解。这体现了工程上的务实思想。

5. 实战：从建模到求解的完整流程

现在，让我们把碎片拼起来，看看Apollo离散点平滑模块的完整工作流程。

5.1 问题组装

1. 输入：原始离散路径点 P_ref， 权重参数 (w_smooth, w_length, w_ref, w_slack)， 曲率上限 κ_max， 边界框大小 bound。
2. 构造二次目标函数：按照第2节，组装矩阵 H 和向量 f。这是一个稀疏矩阵，因为每个点只与前后点耦合。
3. 构造线性约束矩阵：
   • 边界约束：直接生成 lb <= X <= ub。
   • 线性化曲率约束：以 P_ref 为基准点，计算每一处的 G(X_ref) 和梯度 ∇G_ref，组装成 A_curv * X <= b_curv。
   • 松弛变量约束：将松弛变量 s 也作为优化变量加入变量向量 X' = [X; s]，并为其添加 s >= 0 的约束。曲率约束的右侧常数项 b_curv 需对应增加 s 的部分。
4. 完整QP问题：

   Minimize: 1/2 * [X; s]^T * H' * [X; s] + f'^T * [X; s]
   Subject to: A_eq * [X; s] = b_eq (如果有等式约束)
                A_ineq * [X; s] <= b_ineq (包含边界和曲率约束)
                lb' <= [X; s] <= ub'
   

5.2 求解器选择与调用

Apollo为此提供了多种后端求解器接口，主要分为两类：

• OSQP系列：用于求解纯二次规划问题。当我们使用上述线性化技巧后，问题就变成了QP，可以调用OSQP这类专用、高速的QP求解器。在代码中对应 FemPosDeviationOsqpInterface 等。
• IPOPT系列：用于求解更一般的非线性规划问题。如果我们不使用线性化，保留原始的非线性曲率约束，或者使用 cosθ 那种非线性目标，就需要IPOPT这类求解器。它功能强大但计算更慢，对应 CosThetaIpoptInterface 等。

在实际部署中，基于线性化+OSQP的方案是主流选择，因为它速度极快，能满足10Hz甚至更高的实时性要求。在我的实测中，对于一条上百个点的路径，优化在几毫秒内即可完成。

5.3 输出与迭代

求解器输出优化后的变量向量 X_opt，我们从中提取出平滑后的点坐标 (x_i_opt, y_i_opt)。这条新的路径点序列，就是最终送给控制模块的“丝滑”轨迹。

有时候，一次线性化可能不够精确，因为我们在 X_ref 处线性化，但最优解 X_opt 可能离 X_ref 有一定距离。为此，可以采用 序列二次规划（SQP） 的思想：将上一次优化得到的 X_opt 作为新的 X_ref，重新线性化曲率约束，再次求解QP。如此迭代几次，可以使线性化点越来越接近最优解，从而提高精度。Apollo中的 FemPosDeviationSqpOsqpInterface 就实现了这个策略。

6. 关键参数调优与实战经验

理论很完美，但调参才是让算法在真实场景中工作的关键。这里分享一些我的经验。

• 权重系数 (w_smooth, w_length, w_ref)：这是调节平滑器行为的“旋钮”。

  • w_smooth （平滑权重）：越大，路径越顺直。但过大可能导致路径过度“拉直”，偏离参考线甚至切弯。高速场景可适当调高。
  • w_ref （参考点权重）：越大，路径越忠实于原始参考线。在需要严格保持车道中心或紧贴障碍物生成的绕行路径时，应调高。但过高会削弱平滑效果。
  • w_length （长度权重）：通常设置一个较小的值，防止路径被异常拉长即可。它的影响相对前两者较小。
  • 经验：通常设置 w_smooth 和 w_ref 在同一数量级，通过调整它们的比例来取得平衡。例如 (10.0, 1.0, 100.0) 是一个常见的起点，表示非常重视贴近参考线，同时施加中等程度的平滑。

• 边界框 bound：它定义了每个点的最大活动范围。

  • 设置太小，优化器没有足够空间去平滑急弯，可能导致求解失败或效果差。
  • 设置太大，路径可能偏离到不可接受的区域（比如相邻车道）。
  • 动态设置：一个聪明的做法是根据车道宽度和曲率预判来动态设置 bound。在直道上可以放宽，在弯道处应收紧。

• 曲率上限 κ_max：这由车辆的物理极限决定，κ_max = 1 / R_min。必须设置得比车辆理论最小转弯半径更保守，要考虑到轮胎滑移、控制误差和舒适性余量。通常取车辆理论最小转弯半径的1.2到1.5倍作为 R_min 来计算 κ_max。

• 松弛变量权重 w_slack：这个权重需要设置得非常大（比如 100000.0）。目的是强烈惩罚任何使用松弛变量的行为，确保只有在绝对必要时（原始路径曲率严重超限）才会动用它，从而保证生成的路径尽可能满足曲率约束。

调试时，最好的方法是可视化。将原始路径、平滑后路径、曲率曲线以及车辆模型沿着路径的跟踪情况一起画出来。观察在哪些地方曲率接近极限，哪些地方偏离参考线过多，然后有针对性地调整参数。记住，没有一套参数能适应所有场景，城区窄路、高速大道、泊车场景可能需要不同的参数配置文件。在实际项目中，我们通常会为不同的驾驶场景（Scenario）配置不同的平滑器参数，这是工程落地不可或缺的一步。