自动驾驶新手入门：CBF与MPC结合的路径规划实战指南

1次阅读

没有评论

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

自动驾驶路径规划需要同时考虑安全性和实时性，传统 MPC（模型预测控制）虽然在优化控制性能方面表现出色，但在处理硬安全约束时存在明显不足。具体表现为：

MPC 的约束处理依赖于优化问题的可行性，当环境突然变化时可能无法保证安全
安全约束通常需要转化为代价函数中的软约束，这会降低安全性保障
计算复杂度随约束数量增加而显著上升，影响实时性

CBF（控制屏障函数）的引入正好弥补了这些缺陷。CBF 通过 Hamilton-Jacobi 不等式直接保证系统状态不会进入危险区域，无需依赖优化过程的收敛性。这种硬安全保证对于自动驾驶这样的安全关键系统至关重要。

让我们比较三种方案的特性：

纯 MPC 方案
优点：控制性能优化好，可实现平滑轨迹
缺点：安全性依赖约束处理，计算负担重
纯 CBF 方案
优点：安全保证严格，计算效率高
缺点：控制性能优化不足，轨迹可能不够平滑
CBF-MPC 混合方案
优点：兼具安全保证和优化性能
缺点：实现复杂度较高，需要调节两个模块的耦合

CBF 的核心是定义安全集 S = {x | h(x) ≥ 0}，其中 h(x) 是屏障函数。安全性要求满足：

$$
\dot{h}(x) + \gamma h(x) \geq 0
$$

其中 γ >0 是设计参数。这个不等式保证了系统状态永远不会离开安全集 S。

在 MPC 框架中，我们将 CBF 条件作为硬约束加入优化问题：

$$
\min_u J(x,u)
$$
$$
\text{s.t.} x_{k+1} = f(x_k,u_k)
$$
$$
\dot{h}(x_k) + \gamma h(x_k) \geq 0, \forall k
$$

以下是使用 cvxpy 的简化实现代码：

import cvxpy as cp
import numpy as np

# 系统参数
dt = 0.1  # 时间步长
N = 10    # 预测时域
gamma = 0.5  # CBF 参数

# 状态和控制变量
x = cp.Variable((4, N+1))  # 状态: [px, py, vx, vy]
u = cp.Variable((2, N))    # 控制: [ax, ay]

# 初始状态
x0 = np.array([0, 0, 1, 0])

# 障碍物参数
obs_pos = np.array([3, 1])
obs_radius = 1.0

# 代价函数
cost = 0
for t in range(N):
    cost += cp.sum_squares(u[:,t])  # 最小化加速度
    cost += cp.sum_squares(x[:2,t+1] - np.array([5,0]))  # 目标点跟踪

# 约束
constraints = [x[:,0] == x0]  # 初始条件

for t in range(N):
    # 系统动力学 (简化的双积分模型)
    constraints += [x[0,t+1] == x[0,t] + dt*x[2,t]]
    constraints += [x[1,t+1] == x[1,t] + dt*x[3,t]]
    constraints += [x[2,t+1] == x[2,t] + dt*u[0,t]]
    constraints += [x[3,t+1] == x[3,t] + dt*u[1,t]]

    # CBF 约束
    dist = cp.sum_squares(x[:2,t] - obs_pos) - obs_radius**2
    h_dot = 2*(x[0,t]-obs_pos[0])*x[2,t] + 2*(x[1,t]-obs_pos[1])*x[3,t]
    constraints += [h_dot + gamma*dist >= 0]

# 求解问题
prob = cp.Problem(cp.Minimize(cost), constraints)
prob.solve(solver=cp.ECOS)