第三章 车道线拟合与跟踪：滑动窗口法、二次曲线拟合、卡尔曼滤波跟踪、车道线置信度评估
车道线检测做到这一步，其实才刚走完一半。
你想想看，图像里提取出一堆像素点，它们只是散落的「点云」。怎么把这些点变成一条有意义的线？怎么让这条线在下一帧还能稳定跟上？这就是本章要啃的硬骨头。
我个人习惯把车道线处理拆成四个环节：搜索窗口 → 曲线拟合 → 状态跟踪 → 置信度打分。缺一个，系统就容易崩。
3.1 滑动窗口法：从像素到候选点
为什么不用全局搜索？因为太慢了。
实际道路图像里，车道线只占画面底部的一小条区域。滑动窗口法的思路很简单：从下往上，一个窗口一个窗口地找。
核心步骤：
- 取图像下半部分的直方图，找到左右两个峰值作为起始点
- 从底部向上滑动窗口（宽约80-120像素，高约40-60像素）
- 每个窗口内统计非零像素点，如果数量超过阈值（比如30个），就更新窗口中心
- 记录所有窗口内的像素坐标，作为后续拟合的输入
我在项目中遇到过一个问题：弯道曲率太大时，窗口会跟丢。后来我加了一个「记忆偏移」机制——上一帧的拟合曲线位置，作为当前帧窗口的初始偏移量。效果立竿见影。
def sliding_window(binary_warped, nwindows=9, margin=100, minpix=50):
"""
滑动窗口法提取车道线像素
binary_warped: 透视变换后的二值图
"""
# 直方图统计底部一半
histogram = np.sum(binary_warped[binary_warped.shape[0]//2:, :], axis=0)
# 找到左右峰值
midpoint = histogram.shape[0] // 2
leftx_base = np.argmax(histogram[:midpoint])
rightx_base = np.argmax(histogram[midpoint:]) + midpoint
# 窗口高度
window_height = binary_warped.shape[0] // nwindows
# 存储所有非零像素坐标
nonzero = binary_warped.nonzero()
nonzeroy = np.array(nonzero[0])
nonzerox = np.array(nonzero[1])
leftx_current = leftx_base
rightx_current = rightx_base
left_lane_inds = []
right_lane_inds = []
for window in range(nwindows):
# 窗口边界
win_y_low = binary_warped.shape[0] - (window+1)*window_height
win_y_high = binary_warped.shape[0] - window*window_height
win_xleft_low = leftx_current - margin
win_xleft_high = leftx_current + margin
win_xright_low = rightx_current - margin
win_xright_high = rightx_current + margin
# 找到窗口内的像素
good_left_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) &
(nonzerox >= win_xleft_low) & (nonzerox < win_xleft_high)).nonzero()[0]
good_right_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) &
(nonzerox >= win_xright_low) & (nonzerox < win_xright_high)).nonzero()[0]
left_lane_inds.append(good_left_inds)
right_lane_inds.append(good_right_inds)
# 如果像素足够多，更新窗口中心
if len(good_left_inds) > minpix:
leftx_current = np.int32(np.mean(nonzerox[good_left_inds]))
if len(good_right_inds) > minpix:
rightx_current = np.int32(np.mean(nonzerox[good_right_inds]))
# 合并所有索引
left_lane_inds = np.concatenate(left_lane_inds)
right_lane_inds = np.concatenate(right_lane_inds)
leftx = nonzerox[left_lane_inds]
lefty = nonzeroy[left_lane_inds]
rightx = nonzerox[right_lane_inds]
righty = nonzeroy[right_lane_inds]
return leftx, lefty, rightx, righty避坑指南：窗口的margin值不要设死。我曾经在高速场景用固定100像素，结果匝道弯道直接飞线。后来改成动态margin——根据上一帧曲率自适应调整，弯道时放大到150，直道缩回80。
3.2 二次曲线拟合：把点变成线
拿到像素点之后，下一步就是拟合。为什么用二次曲线？因为车道线在透视变换后的鸟瞰图里，基本符合抛物线形状。三次曲线过拟合，一次曲线又太刚。
说白了，二次曲线是「刚刚好」的选择。
def fit_poly(leftx, lefty, rightx, righty):
"""
二次曲线拟合
返回: 左右车道线的多项式系数
"""
# 拟合二次曲线: f(y) = A*y^2 + B*y + C
# 注意这里是以y为自变量，因为车道线是纵向的
left_fit = np.polyfit(lefty, leftx, 2)
right_fit = np.polyfit(righty, rightx, 2)
return left_fit, right_fit
# 生成拟合曲线点
def generate_points(ploty, left_fit, right_fit):
left_fitx = left_fit[0]*ploty**2 + left_fit[1]*ploty + left_fit[2]
right_fitx = right_fit[0]*ploty**2 + right_fit[1]*ploty + right_fit[2]
return left_fitx, right_fitx为什么用y做自变量？
常规思维是用x拟合y，但车道线是垂直走向的。一个x可能对应多个y值（比如弯道）。反过来，每个y值只对应一个车道线x位置。所以用y做自变量更合理。
嗯，这里要注意：拟合之前一定要做异常值剔除。我见过有人直接把所有点扔进去拟合，结果一个噪点把整条曲线带偏了。建议先用RANSAC或者简单的标准差过滤。
3.3 卡尔曼滤波跟踪：让曲线「稳」下来
单帧拟合的结果，抖动很大。为什么？因为图像噪声、光照变化、车辆颠簸，都会让像素点位置波动。
卡尔曼滤波就是干这个的——它把上一帧的状态和当前帧的观测融合起来，输出一个「平滑」的估计。
| 状态量 | 含义 | 维度 |
|---|---|---|
| A, B, C | 二次曲线系数 | 3 |
| ΔA, ΔB, ΔC | 系数变化率（速度） | 3 |
我建议用6维状态向量：3个曲线系数 + 3个变化率。这样模型能预测下一帧的曲线位置，即使这一帧检测失败，也能用预测值顶上去。
class LaneKalmanFilter:
def __init__(self):
# 状态向量: [A, B, C, dA, dB, dC]
self.x = np.zeros(6)
# 状态协方差矩阵
self.P = np.eye(6) * 1000
# 状态转移矩阵 (匀速模型)
self.F = np.array([
[1, 0, 0, 1, 0, 0],
[0, 1, 0, 0, 1, 0],
[0, 0, 1, 0, 0, 1],
[0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1]
])
# 观测矩阵 (只观测曲线系数)
self.H = np.array([
[1, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0]
])
# 过程噪声
self.Q = np.eye(6) * 0.01
# 观测噪声
self.R = np.eye(3) * 0.1
def predict(self):
self.x = self.F @ self.x
self.P = self.F @ self.P @ self.F.T + self.Q
return self.x[:3] # 返回预测的曲线系数
def update(self, z):
# z: 当前帧拟合得到的曲线系数 [A, B, C]
y = z - self.H @ self.x
S = self.H @ self.P @ self.H.T + self.R
K = self.P @ self.H.T @ np.linalg.inv(S)
self.x = self.x + K @ y
self.P = (np.eye(6) - K @ self.H) @ self.P
return self.x[:3]参数调优经验：过程噪声Q和观测噪声R的比值，决定了滤波器的「信任倾向」。Q/R越大，滤波器越相信观测值，响应快但抖动大；反之则越平滑但滞后。我一般先设Q=0.01, R=0.1，然后根据实车数据微调。
3.4 车道线置信度评估：别让烂数据害了你
卡尔曼滤波再牛，也架不住输入数据是垃圾。所以必须给每条拟合出来的车道线打个分。
我常用的评估维度有四个：
- 拟合残差：像素点到拟合曲线的平均距离。超过5个像素就扣分。
- 宽度一致性：左右车道线的间距是否在合理范围（3.5米左右）。偏差超过20%直接降权。
- 曲率连续性：当前帧曲率与上一帧曲率的差值。突变超过30%说明可能跟丢了。
- 像素密度：有效像素点数量。少于200个点，置信度打五折。
def compute_confidence(left_fit, right_fit, leftx, lefty, rightx, righty, prev_curvature):
score = 0.0
# 1. 拟合残差
left_residual = np.mean(np.abs(left_fit[0]*lefty**2 + left_fit[1]*lefty + left_fit[2] - leftx))
right_residual = np.mean(np.abs(right_fit[0]*righty**2 + right_fit[1]*righty + right_fit[2] - rightx))
if left_residual < 5 and right_residual < 5:
score += 0.3
# 2. 宽度一致性 (假设图像底部y=720)
y_eval = 720
left_x = left_fit[0]*y_eval**2 + left_fit[1]*y_eval + left_fit[2]
right_x = right_fit[0]*y_eval**2 + right_fit[1]*y_eval + right_fit[2]
lane_width = right_x - left_x
# 假设标准宽度为 3.7米，像素宽度约 600-800
if 600 < lane_width < 800:
score += 0.3
# 3. 曲率连续性
current_curvature = compute_curvature(left_fit, y_eval)
if prev_curvature is not None:
curvature_diff = abs(current_curvature - prev_curvature) / max(prev_curvature, 1)
if curvature_diff < 0.3:
score += 0.2
# 4. 像素密度
if len(leftx) > 200 and len(rightx) > 200:
score += 0.2
return min(score, 1.0)实战技巧：置信度低于0.5时，我选择完全信任卡尔曼滤波的预测值，不更新观测。等连续3帧置信度都恢复后，再重新融合观测。这个策略帮我扛过了很多隧道出口的强光干扰。
最后说一句：车道线跟踪不是一锤子买卖。它是一个闭环——当前帧的置信度会影响卡尔曼增益，卡尔曼输出又会影响下一帧的窗口位置。把这个循环跑顺了，你的车道保持才算真正「稳」了。