自动驾驶中的CBF与MPC结合：原理剖析与工程实践

1次阅读

没有评论

共计 2081 个字符，预计需要花费 6 分钟才能阅读完成。

传统模型预测控制 (MPC) 在自动驾驶中面临动态环境下的两大挑战：

安全边界模糊：常规约束（如速度限制）无法应对突发障碍物
计算延迟风险：复杂场景下求解耗时增加，可能错过最佳控制时机

例如当行人突然闯入车道时，仅考虑运动学约束的 MPC 可能生成穿过行人轨迹的 ” 最优解 ”。这引出了 安全关键控制 的核心需求：如何保证所有预测轨迹绝对安全？

常见的环境约束处理方法包括：

人工势场法
优点：计算简单，物理直观
缺点：易陷入局部极小值，参数敏感
安全集方法
优点：数学严谨，稳定性可证明
缺点：保守性强，可能限制可行解空间
控制屏障函数(CBF)
优势：
1. 形式化安全保证（通过 Lyapunov-like 函数）
2. 可与优化控制自然融合
3. 适应非线性系统

定义系统状态 $x$ 和控制输入 $u$，安全集 $S = {x | h(x) \geq 0}$。CBF 要求存在扩展类 $\mathcal{K}$ 函数 $\alpha$ 使得：

$$\sup_{u \in U} [L_f h(x) + L_g h(x)u] \geq -\alpha(h(x))$$

将其作为硬约束嵌入 MPC：

$$
\begin{aligned}
\min_{u} & \quad \sum_{k=0}^{N} (x_k^T Q x_k + u_k^T R u_k) \
\text{s.t.} & \quad x_{k+1} = f(x_k, u_k) \
& \quad L_f h(x_k) + L_g h(x_k)u_k \geq -\alpha(h(x_k))
\end{aligned}
$$

# 车辆模型定义（以自行车模型为例）def vehicle_model(x, u, dt):
    beta = np.arctan(0.5*np.tan(u[1]))
    dx = np.array([x[2]*np.cos(x[3]+beta),
        x[2]*np.sin(x[3]+beta),
        u[0],
        x[2]*np.sin(beta)/1.8  # L=1.8m 轴距
    ])
    return x + dx*dt

# CBF 约束构造
def build_cbf_constraint(x_pred, obs_pos, r_safe=3.0):
    """
    x_pred: 预测状态序列 [N,4]
    obs_pos: 障碍物坐标 [2,]
    r_safe: 安全半径
    """
    h = []
    for x in x_pred:
        # 相对位置距离作为 h 函数
        dist = np.linalg.norm(x[:2] - obs_pos) - r_safe
        # 计算 Lie 导数项
        grad_h = np.zeros(4)
        grad_h[:2] = (x[:2]-obs_pos)/max(1e-6, dist+r_safe)
        h_dot = grad_h @ vehicle_model(x, [0,0], 0)  # 零输入下的导数

        # 添加 CBF 约束
        h.append(h_dot + 0.5*dist)  # α(h)=0.5h
    return np.array(h)

# MPC 求解器配置
def solve_mpc(x0, obs_info):
    prob = cp.Problem(objective=cp.Minimize(cp.sum_squares(Q@x_pred) + cp.sum_squares(R@u)),
        constraints=[x_pred[1:] == vehicle_model(x_pred[:-1], u, dt),  # 动力学约束
            build_cbf_constraint(x_pred, obs_info) >= 0,      # CBF 约束
            u <= umax, u >= umin                              # 控制量限制
        ])
    prob.solve(solver=cp.OSQP, warm_start=True)
    return u[0].value

对比方案：纯 MPC vs CBF-MPC
环境配置：
CPU: Intel i7-11800H @2.3GHz
求解器: OSQP 0.6.2
预测时域: N=10