用BEV车道线拟合介绍Kalman滤波

Kalman滤波

核心思想

车道线参数在时序上变化缓慢（除非急转弯），所以可以用运动学预测 + 观测更新的方式，让当前帧的噪声测量不直接影响输出，而是与历史估计融合。

状态定义

车道线用三次多项式表示（lateral offset为y，纵向距离为x）：

复制代码

y = c0 + c1*x + c2*x² + c3*x³

c0: 横向偏移（m）
c1: 航向角（heading，近似 tan(θ)）
c2: 曲率
c3: 曲率变化率

Kalman状态向量（单条车道线）：

复制代码

X = [c0, c1, c2, c3]ᵀ

预测步（Prediction）------ 用自车运动传播

每帧之间自车行驶了 ds 米、横向位移 dy、航向变化 dθ，车道线参数需要随之变换（坐标系跟随自车）。

近似线性化的状态转移：

python 复制代码

def predict_lane_params(c0, c1, c2, c3, ds, dy, dtheta):
    """
    ds: 自车纵向位移
    dy: 自车横向位移
    dtheta: 自车航向变化（rad）
    """
    c0_new = c0 - dy + c1*ds + c2*ds**2 + c3*ds**3
    c1_new = c1 - dtheta + 2*c2*ds + 3*c3*ds**2
    c2_new = c2 + 3*c3*ds
    c3_new = c3
    return np.array([c0_new, c1_new, c2_new, c3_new])

状态转移矩阵 F（线性化）：

复制代码

     c0  c1   c2    c3
c0 [ 1   ds   ds²   ds³ ]
c1 [ 0   1    2ds   3ds²]
c2 [ 0   0    1     3ds ]
c3 [ 0   0    0     1   ]

预测：

python 复制代码

X_pred = F @ X_prev
P_pred = F @ P_prev @ F.T + Q   # Q: 过程噪声

过程噪声Q反映车道线本身的变化快慢：

python 复制代码

# c3变化最慢，c0变化最快（横向偏移受里程计误差影响）
Q = diag([0.01, 0.001, 0.0001, 0.00001])

更新步（Update）------ 关键：自适应R

当前帧拟合得到观测值 Z = [c0_meas, c1_meas, c2_meas, c3_meas]

观测方程： H = I（直接观测状态）

python 复制代码

K = P_pred @ H.T @ inv(H @ P_pred @ H.T + R)   # Kalman增益
X_est = X_pred + K @ (Z - H @ X_pred)
P_est = (I - K @ H) @ P_pred

关键：R的自适应调节

python 复制代码

def compute_R(points, confidences, is_night, far_point_ratio):
    """
    R越大 → 越不信任当前观测 → 输出更接近预测（历史）
    R越小 → 越信任当前观测 → 输出更接近当前帧测量
    """
    base_R = diag([0.05, 0.01, 0.001, 0.0001])

    # 有效点数少 → 测量不可靠
    n_valid = (confidences > threshold).sum()
    point_factor = max(1.0, 20.0 / n_valid)

    # 夜晚 → 远端点不可信，c1测量噪声增大
    night_factor = 3.0 if is_night else 1.0

    # 远端点（>80m）占比高但置信度低 → c1受影响最大
    far_low_conf = (far_point_ratio > 0.3) and is_night
    c1_factor = 5.0 if far_low_conf else 1.0

    R_adaptive = base_R * point_factor * night_factor
    R_adaptive[1, 1] *= c1_factor   # 单独放大c1的观测噪声

    return R_adaptive

X_prev / P_prev 的含义

X_prev, P_prev 是上一帧卡尔曼滤波更新步之后的后验估计，不是网络原始输出。

变量	含义	来源
`X_pred`	预测先验，只用运动学	上帧`X_est` + 自车运动
`Z`	当前帧观测值	BEV网络 → 拟合
`X_est`	后验估计，最终输出	预测 + 观测融合
`X_prev`	下帧的输入	= 本帧`X_est`

夜晚远端点噪声大时，R增大 → K减小 → X_est更靠近X_pred（历史先验），网络噪声对c1的污染被自动抑制。

完整流程图

第N帧
初始化
下一帧
更新步 (Posterior)
K = P_pred·Hᵀ·(H·P_pred·Hᵀ + R)⁻¹
X_est = X_pred + K·(Z - X_pred)

P_est = (I - K·H)·P_pred
BEV网络检测
输出车道线点

(x_i, μ_i, σ_i)
加权多项式拟合

w_i = 1/σ_i²
当前帧观测值 Z = [c0,c1,c2,c3]_meas

观测噪声 R = f(σ_i, 夜晚标志)
预测步 (Prior)
X_pred = F·X_prev

P_pred = F·P_prev·Fᵀ + Q
X_prev = 初始车道线参数(历史预测)

P_prev = 初始协方差矩阵(历史预测)
自车运动

ds, dy, dθ

来自IMU/轮速计
输出稳定的车道线参数

c0, c1, c2, c3
存储供下帧使用

X_prev ← X_est

P_prev ← P_est