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

1次阅读

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

传统模型预测控制 (MPC) 在自动驾驶中面临两大核心问题：

安全约束的硬边界处理：当障碍物突然切入时，标准 MPC 的代价函数可能无法保证绝对避碰
实时性要求：复杂环境下的非线性约束会显著增加求解时间

以紧急制动场景为例，常规 MPC 可能因优化目标（如舒适性）与安全需求冲突而产生危险解。此时需要引入具有形式化安全保证的机制——这正是控制屏障函数 (CBF) 的用武之地。

方法	安全性证明	计算效率	约束灵活性
人工势场法	❌ 经验性	⭐⭐⭐	⭐⭐
鲁棒 MPC	⭐⭐	⭐	⭐⭐⭐
CBF	⭐⭐⭐	⭐⭐	⭐⭐
CBF-MPC 复合	⭐⭐⭐	⭐⭐	⭐⭐⭐

形式化安全保证：通过 $h(x)\geq 0$ 定义安全集，满足 $\dot{h} + \gamma h \geq 0$ 即保证前向不变性
计算轻量化：相比鲁棒 MPC 的 min-max 优化，CBF 仅添加线性约束
组合灵活性：可与任意控制器叠加使用

结合后的优化问题表述为：

$$
\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 \nabla h(x_k)^T f(x_k, u_k) + \gamma h(x_k) \geq 0 \
& \quad u_{min} \leq u_k \leq u_{max}
\end{aligned}
$$

其中 $\gamma>0$ 为 CBF 参数，控制安全约束的严格程度。

import casadi as cs

# 定义系统模型（以单车模型为例）def bicycle_model(x, u, dt):
    beta = cs.atan(0.5*cs.tan(u[1]))  # 轮胎滑移角
    dx = cs.vertcat(x[3]*cs.cos(x[2] + beta),  # x 方向速度
        x[3]*cs.sin(x[2] + beta),  # y 方向速度
        x[3]*cs.tan(u[1])/L,       # 横摆角变化
        u[0]                       # 加速度
    )
    return x + dt*dx

# CBF 约束构造（以圆形障碍物为例）def cbf_constraint(x, obs_pos, obs_radius, safety_margin=0.5):
    dist_sq = (x[0]-obs_pos[0])**2 + (x[1]-obs_pos[1])**2
    h = dist_sq - (obs_radius + safety_margin)**2

    # 计算梯度∇h^T f(x,u)
    grad_h = cs.vertcat(2*(x[0]-obs_pos[0]),
        2*(x[1]-obs_pos[1]),
        0,
        0
    )
    return cs.jtimes(h, x, bicycle_model(x, u, dt)) + gamma*h

# MPC 问题构建
opti = cs.Opti()
X = opti.variable(4, N+1)  # 状态序列
U = opti.variable(2, N)    # 控制序列

# 添加 CBF 约束
for k in range(N):
    opti.subject_to(cbf_constraint(X[:,k], obs, r) >= 0)

# 求解器配置
opts = {
    'ipopt.print_level': 0,
    'print_time': 0,
    'expand': True
}
opti.solver('ipopt', opts)