1. 参考线:自动驾驶规划的地基
大家好,我是Zheng同学。在自动驾驶的规划模块里,如果让我选一个最基础、最核心的“地基”,那一定是参考线。很多刚接触Apollo或者自动驾驶规划的朋友,可能会被轨迹优化、速度规划这些“上层建筑”吸引,但忽略了参考线这个底层支撑。我刚开始做项目的时候也踩过这个坑,花了很多时间调轨迹,结果发现车辆开起来总是“画龙”,最后才发现是参考线不平滑导致的。
那么,参考线到底是什么呢?你可以把它想象成一条虚拟的“车道中心线”,是车辆在理想状态下应该行驶的路径。它由一系列带有位置、朝向、曲率等信息的点组成。规划模块后续所有的决策(比如绕行、停车)、轨迹生成和优化,都是在这条参考线的基础上进行的。如果这条线本身坑坑洼洼、曲率突变,那后续规划出来的轨迹质量可想而知,车辆控制起来就会一顿一顿,乘客体验极差。
为什么不能直接用高精地图提供的车道中心线呢?原因有几个。第一,高精地图的数据是人为采集的离散点,本身就不够平滑。第二,全局导航给出的路径可能很长,但规划模块只需要关注车辆前方一两百米的局部路径。第三,障碍物在一条不平滑的参考线上投影,位置可能不唯一,这会给后续的避障决策带来混乱。所以,我们必须基于原始的、粗糙的参考线,生成一条局部、长度适中且高度平滑的新参考线。这个过程,就是参考线平滑。它不仅是后续所有规划算法的前提,更能显著降低轨迹优化的计算复杂度,让整个系统跑得更稳、更快。
2. Apollo参考线的数据结构与处理流程
要理解平滑算法,我们得先看看Apollo里参考线是怎么被“装”起来的。这就像你要装修房子,得先了解房子的结构和管道布局。
2.1 核心数据结构:ReferenceLine与ReferencePoint
在Apollo的代码里(主要在 modules/planning/reference_line/ 目录下),参考线的核心是两个类:ReferenceLine 和 ReferencePoint。
ReferenceLine 你可以把它看作一个容器,它内部持有一个 std::vector<ReferencePoint>,也就是一系列有序的参考点。除此之外,它还包含了一些实用方法,比如把车辆坐标 (x, y) 转换到以参考线为基准的 Frenet 坐标系 (s, l),或者查询某一点处的道路宽度、限速等信息。这些功能对于判断车辆是否压线、是否超速至关重要。
ReferencePoint 则继承自 MapPathPoint,它描述了一个具体的路径点。除了从地图点继承来的二维坐标 (x, y) 和航向角 (heading) 外,它还增加了两个关键属性:曲率 (kappa) 和 曲率变化率 (dkappa)。这两个参数直接关系到车辆的方向盘转角和平顺性。一个曲率连续变化的参考线,才能让车辆平稳过弯。原始地图点只有坐标,航向和曲率都是后续计算出来的。计算航向很简单,用后一个点坐标减前一个点坐标得到向量,再求这个向量的角度(atan2函数)就行了。曲率的计算则更复杂一些,通常由平滑算法在优化过程中一并求得。
2.2 参考线的生成与平滑流程
整个参考线的处理流程可以清晰地分为两步,由 ReferenceLineProvider 这个“供应商”类来主导。
第一步:生成原始参考线。 这一步主要由 Routing(路由)模块的输出决定。Routing 模块进行的是车道级别的全局路径规划,它告诉车辆“从A车道换到B车道,再进入C车道”。ReferenceLineProvider 会结合车辆当前的状态(位置、速度)和地图信息,从 Routing 的结果中“截取”出一段局部的、连续的路径片段,这就是原始的 raw_reference_line。这个过程会考虑“前看距离”和“后看距离”,比如车辆速度快就多看远一点(250米),速度慢就少看一点(180米),确保有足够的规划空间。
第二步:平滑参考线。 拿到原始的“毛坯”参考线后,就进入核心的平滑环节。Apollo 提供了一个统一的平滑器接口 ReferenceLineSmoother,并根据配置选择具体的平滑算法实现。平滑器的输入是原始的 raw_reference_line,输出就是平滑后的、可以直接用于规划的理想 reference_line。整个平滑过程可以概括为三个子步骤:
- 设置锚点 (Anchor Points):在原始参考线上按固定间隔(默认0.25米)采样,得到一系列关键点。每个点会附带一个“框”状的约束范围(横向和纵向边界),平滑后的点不能超出这个框,以保证不会偏离原始路径太远。只有路径的起点和终点是“强约束”,必须严格通过。
- 调用平滑 (Smooth):这是算法的主入口,负责准备数据(比如点的坐标、约束边界)并调用底层求解器。
- 求解问题 (Solve):根据选择的算法(如二次规划QP、非线性规划NLP),构建数学优化问题并求解,得到平滑后的点坐标。
这个流程在代码里非常直观,我在调试的时候经常在这里加日志,看平滑前后的点坐标变化,非常有助于理解算法效果。
3. Apollo中的三大参考线平滑算法
Apollo 规划模块内置了三种参考线平滑器,采用经典的策略模式,通过配置文件可以灵活切换。这体现了框架设计上的优雅,让我们可以针对不同场景(比如高速巡航、低速泊车)选择最合适的算法。
离散点平滑 (Discrete Points Smoother):这是目前 Apollo 默认使用的算法。它的思想很直接:不就有一串原始的离散路径点嘛,我通过优化方法,微调这些点的位置,让它们连起来的线变得平滑,同时又不偏离原始点太远。它把问题转化成了一个有约束的优化问题来求解。我们后面会重点拆解它。
螺旋曲线平滑 (Spiral Reference Line Smoother):这种方法的数学味儿更浓一些。它用螺旋线(曲率随弧长线性变化的曲线)来拟合路径。螺旋线本身具有曲率连续变化的特性,所以能生成非常平滑、符合车辆运动学的路径。但它的计算相对复杂,求解难度较高。
多项式样条平滑 (QpSpline ReferenceLineSmoother):这种方法把整条路径分成多段,每一段用一个五次多项式曲线来表示。然后通过优化这些多项式的系数,使得拼接起来的整体曲线平滑,并且满足边界约束。它同样可以产生高阶连续(位置、速度、加速度)的路径。
在代码的初始化阶段,你会看到这样的判断逻辑,根据配置文件决定实例化哪个平滑器:
if (smoother_config_.has_qp_spline()) {
smoother_.reset(new QpSplineReferenceLineSmoother(smoother_config_));
} else if (smoother_config_.has_spiral()) {
smoother_.reset(new SpiralReferenceLineSmoother(smoother_config_));
} else if (smoother_config_.has_discrete_points()) {
smoother_.reset(new DiscretePointsReferenceLineSmoother(smoother_config_));
}
由于离散点平滑是默认且最常用的,我们接下来的深度解析就围绕它展开。理解了它,你就能掌握Apollo参考线平滑最核心的“内功心法”。
4. 离散点平滑算法深度剖析
离散点平滑,顾名思义,就是直接优化这一系列离散的路径点。它的目标很明确:让点连成的线更平滑、更紧凑,同时别跑偏。这听起来简单,但怎么用数学语言描述,并让计算机高效求解,就是学问了。
4.1 优化问题的构建:目标函数
Apollo 将平滑问题构建成了一个标准的二次规划(Quadratic Programming, QP)问题。二次规划的特点是目标函数是二次的,约束是线性的,有非常成熟高效的求解器(如OSQP)可用,速度很快。
优化变量就是我们所有待平滑的离散点的坐标 (x_k, y_k)。
目标函数由三部分组成,我们需要最小化它们的总和:
-
平滑度代价 (cost_smooth):这是最重要的部分,目的是让路径点之间的转向变化平缓。Apollo 提供了两种衡量方式:
- FemPosSmooth(默认):它最小化连续三个点构成的向量的模长平方。想象点 P1, P2, P3,它惩罚的是向量 (P3 - P1) 的长度。如果三点共线,这个值最小。它的函数形式是二次的,可以直接用QP求解,计算速度快,但平滑精度相对一般。
- CosThetaSmooth:它最大化连续两个线段夹角的余弦值(cosθ)。夹角θ越小,余弦值越大,路径就越直。但
cosθ本身不是关于点坐标的二次函数,这导致整个问题变成了非线性规划(NLP),需要用IPOPT这类求解器,计算量大,但平滑效果更好。
在实际项目中,我绝大部分时间都使用默认的
FemPosSmooth,因为它在性能和效果上取得了很好的平衡。除非是对平滑度有极致要求的特殊场景,否则没必要启用计算更重的CosThetaSmooth。 -
长度代价 (cost_length):惩罚路径的总长度。我们希望平滑后的路径不要比原始路径长太多,避免绕远路。这项也是二次形式。
-
偏离代价 (cost_deviation):惩罚平滑后的点与原始锚点之间的偏移距离。我们不能天马行空地平滑,必须让新路径大致贴合原始车道中心线。这项同样是二次形式。
把这三项加起来,就得到了一个标准的二次型目标函数:cost = cost_smooth + w1 * cost_length + w2 * cost_deviation。这里的 w1 和 w2 是权重参数,用来平衡三个目标的相对重要性。调参的时候,增大 w2 会让路径更贴近原始线,但可能牺牲一些平滑度。
4.2 优化问题的构建:约束条件
只有目标函数是不够的,我们还需要约束来保证结果的合理性。
-
位置约束(硬约束):虽然目标函数里有偏离代价,但那只是个“软约束”,是希望它小,但不能保证它一定不超过某个范围。因此,我们必须给每个点加上一个“硬约束框”,规定平滑后的点 (x_k, y_k) 必须在以原始锚点为中心、给定横向和纵向裕度构成的矩形框内。这个框的大小就是之前提到的
lateral_bound和longitudinal_bound。这是保证路径不脱离车道边界、安全可行的关键。 -
曲率约束(可选):一个真正舒适的路径,其曲率也应该是连续变化的,不能突变。曲率约束就是为了保证这一点。它要求路径上每一点的曲率不能超过车辆动力学允许的最大值(对应最小转弯半径)。但是,曲率是关于点坐标的非线性函数。一旦加上这个约束,整个优化问题就从二次规划变成了非线性规划,求解难度和耗时大大增加。
有趣的是,Apollo 默认配置是关闭曲率约束的。我猜主要是出于计算效率的考虑。在实际道路上,只要位置约束框设置合理,平滑出来的路径曲率通常也能满足要求。当然,你也可以在配置中打开它,系统会使用更复杂的非线性求解器(如IPOPT)或者采用序列二次规划(SQP)方法,将非线性约束线性化后再用OSQP迭代求解。
4.3 求解过程:二次规划与OSQP
对于默认不考虑曲率约束的情况,问题就是一个纯粹的二次规划。Apollo 使用 OSQP 求解器来高效求解。OSQP(Operator Splitting Quadratic Program)是一个专门用于求解凸二次规划问题的开源求解器,它的特点是速度快、数值稳定。
整个求解流程可以概括为:
- 将目标函数整理成
1/2 * x^T * P * x + q^T * x的标准QP形式,其中x是所有点的坐标拼接成的长向量。 - 将位置约束整理成
lb <= A * x <= ub的线性不等式约束形式。 - 调用OSQP求解器,输入矩阵
P,q,A,lb,ub。 - 求解器输出最优的
x*,即平滑后的所有点坐标。
这个过程在代码里被封装在 FemPosDeviationSmoother::QpWithOsqp 函数中。我曾经为了调试一个问题,手动构造过一个小规模的QP问题,对比OSQP的输出,来验证我们对问题建模是否正确,这是一个很好的学习方法。
4.4 算法变体:CosTheta与非线性求解
当你选择 CosThetaSmooth 方法,或者启用了曲率约束时,问题就变成了非线性规划。Apollo 为此集成了 IPOPT 求解器。IPOPT(Interior Point OPTimizer)是一个强大的大规模非线性优化求解器。
非线性规划的求解过程比QP复杂得多,也慢得多。它通常采用迭代法,比如从初始点出发,沿着函数下降的方向(考虑约束)一步步逼近最优解。调试这类问题时,需要特别注意初始值的好坏,糟糕的初始值可能导致求解失败或陷入局部最优。
还有一种折中的方法,就是 SQP(序列二次规划)。它的思想是:在非线性约束的当前点做泰勒展开,将其近似为线性约束;同时将目标函数也用二次函数近似。这样,在每一步迭代中,我们都在解一个近似的二次规划子问题(可以用OSQP快速解),然后用子问题的解更新当前点,如此反复,直到收敛。这在Apollo代码中对应 SqpWithOsqp 方法。
5. 实践中的关键参数与调试经验
理论懂了,最终还是要落到代码和调参上。在Apollo里,离散点平滑算法的配置主要在 modules/planning/conf/discrete_points_smoother_config.pb.txt 这个文件里。这里我结合自己的经验,说几个最关键、最常调的参数。
1. 采样间隔与锚点约束 (max_constraint_interval, boundary_bound)
max_constraint_interval:默认0.25米。这个值决定了我们在原始参考线上采样的密度。间隔越小,锚点越多,对原始路径的还原度越高,但优化变量也越多,计算量越大。在弯道急的地方,可以适当减小间隔以提高精度;在直道上,可以适当增大以提升速度。longitudinal_boundary_bound和lateral_boundary_bound:这是位置约束框的大小。纵向约束通常给得大一些(默认2.0米),因为沿着道路方向稍有前后偏移问题不大。横向约束是关键,它直接决定了车辆能偏离车道中心线多远。默认max_lateral_boundary_bound是0.5米,min_lateral_boundary_bound是0.1米。系统会根据车道边界类型(比如是否是路缘石curb)和自车宽度,在这个最大最小值之间动态调整每个点的横向裕度。如果你发现车辆太靠近车道一边,或者过于保守不敢靠近,调整这两个参数是首要选择。
2. 代价函数权重 (weight_fem_pos_deviation, weight_path_length, weight_ref_deviation)
这三个权重分别对应平滑度、路径长度和偏离度的代价。默认配置中,平滑度的权重 (weight_fem_pos_deviation) 通常是最大的,这保证了平滑性是首要目标。在实践中,我很少动平滑度权重,但会微调 weight_ref_deviation(偏离权重)。如果发现平滑后的路径“放飞自我”,偏离原始车道线太远,就适当增大这个权重。如果想给平滑度更多“自由发挥”的空间,就适当减小它。
3. 是否启用曲率约束 (apply_curvature_constraint)
如前面所说,默认是 false。除非你在做非常激烈的驾驶机动(比如极小半径掉头),或者对乘坐舒适性有极致要求,否则不建议打开。一旦打开,计算时间可能会增加一个数量级,对实时性是个挑战。
调试技巧:我常用的方法是把平滑前后的参考线在仿真环境或者离线工具里画出来对比。同时,打印出关键点的曲率和曲率变化率,看看有没有突变。有时候算法理论上没问题,但因为数值计算精度或者求解器配置,会导致结果有微小抖动,这时候可能需要检查OSQP的收敛精度等参数。
6. 从理论到代码:一个简化版的平滑实现
为了让大家更有体感,我写了一个极度简化的 FemPosSmooth 核心思想演示。这个例子不考虑约束,只演示如何构建和求解那个最小化“三点向量模长”的二次规划问题。真正的Apollo代码要考虑约束、边界、效率,要复杂得多。
假设我们有5个原始二维点,我们想平滑它们。
import numpy as np
import osqp
from scipy import sparse
# 1. 假设的原始点坐标 (x, y)
raw_points = np.array([[0., 0.], [1., 0.2], [2., 0.1], [3., 0.3], [4., 0.]])
n_points = len(raw_points)
# 2. 构建二次规划问题: min 0.5 * x^T * P * x + q^T * x
# 优化变量 x = [x0, y0, x1, y1, ..., x4, y4].T
n_vars = n_points * 2
# 2.1 构建平滑代价矩阵 P_smooth (FemPos形式)
# 代价: sum_i || (p_{i+2} - p_{i}) - 2*(p_{i+1} - p_i) ||^2 的一种简化近似,这里我们用相邻三点向量差
# 更准确地说,Apollo中cost_smooth = sum_i || (p_{i-1} + p_{i+1}) - 2*p_i ||^2
# 这等价于最小化二阶差分。我们构建对应的Hessian矩阵P。
# 对于一维序列,二阶差分矩阵D2是 (n-2) x n。对于二维,我们需要对x和y维度分别处理。
# 这里我们构建一个简化的P,体现“相邻点位置变化应平缓”的思想。
P = sparse.csc_matrix((n_vars, n_vars))
# 我们用一个非常简单的目标:最小化平滑点与原始点的偏移,同时加上一点相邻点差异的惩罚
# min sum_i ( (x_i - x_i_raw)^2 + (y_i - y_i_raw)^2 ) + w * sum_i ( (x_{i+1} - x_i)^2 + (y_{i+1} - y_i)^2 )
w_smooth = 0.5
w_deviation = 1.0
# 构建P矩阵 (是对角线或三对角形式)
data = []
row_ind = []
col_ind = []
for i in range(n_points):
# 偏差项 (x_i - x_i_raw)^2 的二次项系数为1 (对x和y)
idx_x = 2*i
idx_y = 2*i + 1
# 对角线元素
data.append(w_deviation) # x_i 的偏差代价
row_ind.append(idx_x); col_ind.append(idx_x)
data.append(w_deviation) # y_i 的偏差代价
row_ind.append(idx_y); col_ind.append(idx_y)
# 平滑项 (x_{i+1} - x_i)^2, 会贡献到 (i,i), (i,i+2), (i+2,i) 位置
if i < n_points - 1:
# 对 x_i: 系数为 w_smooth
data.append(w_smooth); row_ind.append(idx_x); col_ind.append(idx_x)
data.append(w_smooth); row_ind.append(idx_y); col_ind.append(idx_y)
# 对 x_{i+1}: 系数为 w_smooth
data.append(w_smooth); row_ind.append(idx_x+2); col_ind.append(idx_x+2)
data.append(w_smooth); row_ind.append(idx_y+2); col_ind.append(idx_y+2)
# 交叉项 -w_smooth * x_i * x_{i+1} (两次)
data.append(-w_smooth); row_ind.append(idx_x); col_ind.append(idx_x+2)
data.append(-w_smooth); row_ind.append(idx_x+2); col_ind.append(idx_x)
data.append(-w_smooth); row_ind.append(idx_y); col_ind.append(idx_y+2)
data.append(-w_smooth); row_ind.append(idx_y+2); col_ind.append(idx_y)
P = sparse.csc_matrix((data, (row_ind, col_ind)), shape=(n_vars, n_vars))
# 2.2 构建线性项 q
q = np.zeros(n_vars)
for i in range(n_points):
# min (x_i - x_i_raw)^2 展开后线性项是 -2 * x_i_raw * x_i,常数项忽略
q[2*i] = -2 * w_deviation * raw_points[i, 0]
q[2*i + 1] = -2 * w_deviation * raw_points[i, 1]
# 3. 约束 (这里我们简单加一个位置约束:第一个点和最后一个点位置固定)
# 即 x0 = raw_x0, y0 = raw_y0, x4 = raw_x4, y4 = raw_y4
A = sparse.eye(n_vars).tocsc() # 约束矩阵:单位阵表示每个变量自身约束
l = np.full(n_vars, -np.inf) # 下界
u = np.full(n_vars, np.inf) # 上界
# 固定首尾点
l[0] = u[0] = raw_points[0, 0] # x0
l[1] = u[1] = raw_points[0, 1] # y0
l[-2] = u[-2] = raw_points[-1, 0] # x_last
l[-1] = u[-1] = raw_points[-1, 1] # y_last
# 4. 用OSQP求解
prob = osqp.OSQP()
prob.setup(P, q, A, l, u, verbose=False)
res = prob.solve()
if res.info.status_val != 1:
print("OSQP求解失败")
else:
smoothed = res.x.reshape(-1, 2)
print("原始点:")
print(raw_points)
print("\n平滑后点:")
print(smoothed)
这段代码只是一个教学演示,帮你理解二次规划如何应用于点平滑。真正的工程实现要考虑稀疏矩阵的高效构建、各种边界约束的精确设置,以及数值稳定性。但它的核心逻辑是一致的:定义目标(要平滑),定义限制(不能乱跑),然后交给优化求解器算出最优解。
7. 总结与个人心得
聊了这么多,最后分享几点我踩过坑之后的体会。参考线平滑看似是规划里一个预处理步骤,但它对整套系统的影响是基础性的。一条好的参考线能让后面的轨迹优化事半功倍,一条差的参考线则会让后续所有模块的努力事倍功半。
首先,没有“最好”的算法,只有“最合适”的。 Apollo 提供了三种平滑器,默认的离散点二次规划(FemPos)在绝大多数场景下已经足够好用。它快、稳、可解释性强。不要盲目追求更复杂的螺旋线或样条曲线,除非你有明确的、现有的算法无法满足的需求(比如必须保证曲率连续可导)。
其次,参数调试要循序渐进,理解其物理意义。 不要一上来就胡乱改一堆参数。先从 lateral_boundary_bound 和 weight_ref_deviation 入手,这两个直接影响路径的“保守”程度。一边改,一边在仿真里看车辆实际跑的效果,感受参数变化带来的影响。
最后,一定要可视化,一定要可视化,一定要可视化! 重要的事情说三遍。把平滑前和平滑后的参考线画出来,把曲率图画出来,把约束框画出来。肉眼看到的问题,比看日志里的数字要直观一百倍。Apollo 自带的 Dreamview 工具就很好用,也可以自己写脚本用 matplotlib 画。
参考线平滑是自动驾驶规划里一个非常经典的优化问题应用。它把我们对“好路径”的感性认识(平滑、贴近车道、合理曲率),转化成了严谨的数学优化模型,再通过高效的数值求解器得到结果。理解了这个过程,你不仅掌握了Apollo的一个模块,更掌握了用数学工具解决工程问题的一种核心思路。希望这篇深度解析能帮你打通任督二脉,在实际项目中更好地驾驭这个“沉默的基石”。
302



被折叠的 条评论
为什么被折叠?



