自动驾驶技术是现代控制理论最具挑战性和影响力的应用之一。本章将系统介绍自动驾驶车辆的建模、规划与控制方法，从基础的车辆动力学模型出发，逐步深入到路径规划、轨迹跟踪、多车协同等核心技术。我们将特别关注工程实践中的关键问题，如模型不确定性、执行器约束、实时性要求等，并通过Waymo的实际案例展示这些技术如何在真实世界中应用。
准确的车辆动力学模型是设计高性能控制器的基础。根据应用场景和计算资源的不同，我们需要在模型精度和复杂度之间权衡。
自行车模型是自动驾驶中最常用的简化模型，它将四轮车辆简化为前后两个轮子，忽略车辆的俯仰和侧倾运动，只考虑纵向、横向和横摆三个自由度。
前轮
|
| δf (前轮转角)
|/
====== Lf ====== CG ====== Lr ======
|
| v (速度)
|
后轮
运动学模型（低速，忽略轮胎侧滑）： \(\begin{aligned} \dot{x} &= v \cos(\psi + \beta) \\ \dot{y} &= v \sin(\psi + \beta) \\ \dot{\psi} &= \frac{v}{L_r} \sin(\beta) \\ \beta &= \arctan\left(\frac{L_r}{L_f + L_r} \tan(\delta_f)\right) \end{aligned}\)
其中：
动力学模型（考虑轮胎力）： \(\begin{aligned} m\dot{v}_x &= m v_y \dot{\psi} + F_{xf} + F_{xr} \\ m\dot{v}_y &= -m v_x \dot{\psi} + F_{yf} + F_{yr} \\ I_z\ddot{\psi} &= L_f F_{yf} - L_r F_{yr} \end{aligned}\)
轮胎是车辆与路面的唯一接触点，其力学特性对车辆动态响应有决定性影响。
线性轮胎模型（小滑移角假设）： \(F_y = -C_\alpha \alpha\)
其中$C_\alpha$是轮胎侧偏刚度，$\alpha$是轮胎侧偏角： \(\begin{aligned} \alpha_f &= \delta_f - \arctan\left(\frac{v_y + L_f \dot{\psi}}{v_x}\right) \\ \alpha_r &= -\arctan\left(\frac{v_y - L_r \dot{\psi}}{v_x}\right) \end{aligned}\)
Pacejka魔术公式（适用于大滑移角）： \(F_y = D \sin(C \arctan(B\alpha - E(B\alpha - \arctan(B\alpha))))\)
参数$B, C, D, E$通过实验数据拟合得到，典型值：
实际车辆的执行器（转向、制动、油门）都有动态响应特性和饱和限制。
转向系统一阶动力学： \(\tau_s \dot{\delta}_f + \delta_f = \delta_{f,cmd}\)
其中$\tau_s \approx 0.1-0.2$秒为转向系统时间常数。
制动系统动力学（考虑液压延迟）： \(\tau_b \dot{F}_b + F_b = K_b P_{cmd}\)
纵向加速度限制： \(a_{min} \leq a \leq a_{max}\)
典型值：
| 最大减速度：$ | a_{min} | = 8-10 \, m/s^2$（ABS制动） |
| 最大转向角：$ | \delta_{f,max} | = 30-35°$ |
| 最大转向角速度：$ | \dot{\delta}_{f,max} | = 30-60°/s$ |
将上述模型线性化后，得到用于控制器设计的状态空间模型：
\[\dot{\mathbf{x}} = A\mathbf{x} + B\mathbf{u}\]状态向量：$\mathbf{x} = [e_y, \dot{e}y, e\psi, \dot{e}_\psi]^T$
控制输入：$\mathbf{u} = [\delta_f, a_x]^T$
在参考速度$v_{ref}$附近线性化得到：
\[A = \begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & -\frac{2(C_f + C_r)}{mv_{ref}} & \frac{2(C_f + C_r)}{m} & -\frac{2(L_fC_f - L_rC_r)}{mv_{ref}} \\ 0 & 0 & 0 & 1 \\ 0 & -\frac{2(L_fC_f - L_rC_r)}{I_zv_{ref}} & \frac{2(L_fC_f - L_rC_r)}{I_z} & -\frac{2(L_f^2C_f + L_r^2C_r)}{I_zv_{ref}} \end{bmatrix}\] \[B = \begin{bmatrix} 0 & 0 \\ \frac{2C_f}{m} & 0 \\ 0 & 0 \\ \frac{2L_fC_f}{I_z} & 0 \end{bmatrix}\]路径规划负责生成从起点到终点的可行路径，而轨迹生成则在此基础上加入时间维度，生成满足动力学约束的轨迹。
全局路径规划在高精地图上寻找最优路径，通常使用图搜索或采样方法。
Hybrid A*算法
结合了A*的最优性和车辆运动学约束：
算法伪代码：
1. 初始化：open_list = {start}, closed_list = {}
2. while open_list不空：
3. current = open_list中f值最小的节点
4. if current接近goal：
5. 返回路径
6. 将current移至closed_list
7. for each action in 动作空间：
8. next = 应用运动学模型
9. if next可行且不在closed_list：
10. 计算f(next)
11. 加入或更新open_list
RRT*算法
快速探索随机树的渐进最优版本：
算法核心步骤：
1. 随机采样：x_rand = RandomSample()
2. 最近邻：x_near = Nearest(tree, x_rand)
3. 扩展：x_new = Steer(x_near, x_rand, step_size)
4. 重连接：
- 找到半径r内的所有节点
- 选择使cost最小的父节点
- 重新连接可以改善的节点
局部轨迹规划在全局路径基础上，生成满足舒适性和安全性要求的平滑轨迹。
Frenet坐标系规划
将规划问题分解为沿道路方向（s）和垂直道路方向（d）：
横向轨迹（五次多项式）： \(d(t) = a_0 + a_1t + a_2t^2 + a_3t^3 + a_4t^4 + a_5t^5\)
边界条件：
纵向轨迹（四次或五次多项式）： \(s(t) = b_0 + b_1t + b_2t^2 + b_3t^3 + b_4t^4\)
轨迹优化问题
最小化代价函数： \(J = w_1 J_{jerk} + w_2 J_{time} + w_3 J_{offset} + w_4 J_{speed}\)
其中：
安全走廊生成
使用凸优化方法生成无碰撞轨迹：
时空轨迹优化（二次规划）： \(\begin{aligned} \min_{\mathbf{x}} \quad & \mathbf{x}^T H \mathbf{x} + \mathbf{f}^T \mathbf{x} \\ \text{s.t.} \quad & A_{eq} \mathbf{x} = \mathbf{b}_{eq} \quad \text{(动力学约束)} \\ & A_{ineq} \mathbf{x} \leq \mathbf{b}_{ineq} \quad \text{(安全走廊约束)} \end{aligned}\)
速度规划
S-T图（距离-时间图）规划：
T
↑
| ╱╱╱ 障碍物1
| ╱╱╱╱
|╱╱╱╱ 可行轨迹
| ╲
| ╲ ╱╱╱ 障碍物2
| ╲╱╱╱
└────────────→ S
动态规划求解：
轨迹跟踪控制通常分为横向（转向）和纵向（速度）两个子问题。
Pure Pursuit控制器
基于几何的方法，通过跟踪前视点实现路径跟踪：
\[\delta_f = \arctan\left(\frac{2L \sin(\alpha)}{L_d}\right)\]其中：
优点：简单鲁棒，计算量小 缺点：稳态误差，高速性能受限
Stanley控制器
斯坦福大学在DARPA挑战赛中使用的方法：
\[\delta_f = \psi_e + \arctan\left(\frac{k_e \cdot e_y}{v}\right)\]其中：
LQR横向控制
基于线性二次调节器的最优控制：
代价函数： \(J = \int_0^{\infty} (\mathbf{x}^T Q \mathbf{x} + \mathbf{u}^T R \mathbf{u}) dt\)
权重矩阵选择： \(Q = \begin{bmatrix} q_{e_y} & 0 & 0 & 0 \\ 0 & q_{\dot{e}_y} & 0 & 0 \\ 0 & 0 & q_{e_\psi} & 0 \\ 0 & 0 & 0 & q_{\dot{e}_\psi} \end{bmatrix}, \quad R = [r_{\delta}]\)
典型权重：
反馈增益：$K = R^{-1}B^T P$，其中$P$满足代数Riccati方程。
模型预测控制（MPC）
考虑约束的滚动时域优化：
\[\begin{aligned} \min_{\mathbf{U}} \quad & \sum_{k=0}^{N-1} (\mathbf{x}_k^T Q \mathbf{x}_k + \mathbf{u}_k^T R \mathbf{u}_k) + \mathbf{x}_N^T Q_f \mathbf{x}_N \\ \text{s.t.} \quad & \mathbf{x}_{k+1} = A_d \mathbf{x}_k + B_d \mathbf{u}_k \\ & |\delta_{f,k}| \leq \delta_{f,max} \\ & |\Delta\delta_{f,k}| \leq \Delta\delta_{f,max} \end{aligned}\]预测时域：$N = 10-20$（对应1-2秒） 控制时域：$M = 3-5$
PID速度控制
\[a_{cmd} = K_p e_v + K_i \int e_v dt + K_d \frac{de_v}{dt} + a_{ff}\]其中：
增益调度（根据速度调整）： \(K_p(v) = K_{p,0} \cdot (1 + \alpha_p \cdot v)\)
自适应巡航控制（ACC）
安全跟车距离： \(d_{safe} = d_0 + T_{hw} \cdot v\)
其中：
多目标优化： \(J = w_1(v - v_{des})^2 + w_2(d - d_{safe})^2 + w_3 a^2 + w_4 j^2\)
切换逻辑：
if d < d_safe - margin:
模式 = 紧急制动
elif d < d_safe:
模式 = 跟车
else:
模式 = 巡航
高速过弯时，横纵向动力学存在强耦合：
轮胎摩擦圆约束： \(\sqrt{F_x^2 + F_y^2} \leq \mu F_z\)
协调控制策略：
过弯速度限制： \(v_{max} = \sqrt{\mu g r}\)
其中$r$为弯道半径。
多车协同控制通过车车通信（V2V）和车路协同（V2I），实现更高效、更安全的交通系统。
车队控制使多辆车保持紧密队形行驶，减少空气阻力和道路占用。
纵向动力学模型
第$i$辆车的动力学： \(\begin{aligned} \dot{x}_i &= v_i \\ \dot{v}_i &= a_i \\ \tau_i \dot{a}_i + a_i &= a_{i,des} \end{aligned}\)
间距策略：
分布式控制架构
前车跟随+前导车信息（PF+LV）： \(a_{i,des} = k_1(x_{i-1} - x_i - d_{i,des}) + k_2(v_{i-1} - v_i) + k_3(a_{i-1} - a_i) + k_4(a_0 - a_i)\)
其中$a_0$为领导车加速度。
稳定性分析
弦稳定性条件（避免扰动放大）： \(\left|\frac{X_i(s)}{X_{i-1}(s)}\right| \leq 1, \quad \forall \omega\)
对于CTH策略，稳定性要求： \(h > \tau_i + \frac{1}{2k_2}\)
通信延迟处理
Smith预估器补偿延迟$T_d$： \(\hat{x}_{i-1}(t) = x_{i-1}(t-T_d) + T_d \cdot v_{i-1}(t-T_d) + \frac{T_d^2}{2} a_{i-1}(t-T_d)\)
无信号灯交叉口的协同通过优化车辆通过顺序和轨迹。
冲突检测
两车轨迹$(s_i(t), s_j(t))$的冲突条件： \(\exists t: |p_i(s_i(t)) - p_j(s_j(t))| < d_{safe}\)
其中$p(\cdot)$将路径坐标映射到笛卡尔坐标。
集中式优化
最小化总通过时间： \(\begin{aligned} \min \quad & \sum_{i=1}^n t_{i,exit} \\ \text{s.t.} \quad & t_{j,enter} - t_{i,exit} \geq \Delta t_{safe} \quad \text{(先后顺序)} \\ & v_{min} \leq v_i(t) \leq v_{max} \\ & a_{min} \leq a_i(t) \leq a_{max} \end{aligned}\)
分布式协商
基于拍卖的机制：
高速公路合流需要协调主道和匝道车辆。
虚拟车辆法
为合流车在主道创建”虚拟占位”： \(x_{virtual}(t) = x_{merge} - v_{main} \cdot (t_{merge} - t)\)
主道车辆跟踪虚拟车辆，为合流车预留空间。
协同换道
三阶段协议：
间隙生成控制： \(a_{create} = k_g(d_{gap,des} - d_{gap}) - k_v(v_{front} - v_{rear})\)
通信协议
DSRC/C-V2X标准：
一致性协议
分布式平均共识： \(\dot{x}_i = \sum_{j \in \mathcal{N}_i} a_{ij}(x_j - x_i)\)
其中$\mathcal{N}_i$为节点$i$的邻居集合。
收敛条件：通信图强连通且平衡。
容错机制
Byzantine故障处理：
def byzantine_filter(measurements, threshold=2.0):
median = np.median(measurements)
mad = np.median(np.abs(measurements - median))
valid = np.abs(measurements - median) < threshold * mad
return measurements[valid].mean()
Waymo作为自动驾驶领域的先驱，其技术栈代表了当前的最高水平。
Waymo采用模块化架构，主要包括：
传感器输入 → 感知 → 预测 → 规划 → 控制 → 执行器
↑ ↓
└──────── 地图与定位 ←─────────┘
传感器配置
计算平台
Waymo使用深度学习预测其他交通参与者的行为。
VectorNet架构
将场景表示为向量化的高精地图和轨迹：
交互建模
使用图神经网络捕捉车辆间交互： \(h_i^{(l+1)} = \sigma\left(W_{self}h_i^{(l)} + \sum_{j \in \mathcal{N}_i} W_{edge}h_j^{(l)}\right)\)
分层规划
优化问题formulation
\[\begin{aligned} \min_{\xi} \quad & J_{smooth}(\xi) + J_{obs}(\xi) + J_{ref}(\xi) \\ \text{s.t.} \quad & g_{dyn}(\xi) = 0 \quad \text{(动力学约束)} \\ & h_{bound}(\xi) \leq 0 \quad \text{(道路边界)} \end{aligned}\]其中$\xi = [(x_0, y_0), …, (x_N, y_N)]$为轨迹点序列。
代价函数设计
平滑性代价： \(J_{smooth} = \sum_{i=1}^{N-1} \|\xi_{i+1} - 2\xi_i + \xi_{i-1}\|^2\)
障碍物代价（使用势场）： \(J_{obs} = \sum_{i,j} \exp\left(-\frac{d_{ij}^2}{2\sigma^2}\right)\)
参考线跟踪： \(J_{ref} = \sum_i \|\xi_i - \xi_{ref,i}\|^2\)
级联控制结构
轨迹 → 路径跟踪控制器 → 转向角
↓
→ 速度跟踪控制器 → 油门/刹车
自适应MPC控制器
Waymo使用自适应MPC处理模型不确定性：
鲁棒约束收紧 \(\mathcal{X}_{tight} = \mathcal{X} \ominus \mathcal{E}\) 其中$\mathcal{E}$为不确定性边界。
执行器补偿
延迟补偿（预测控制）： \(u(t) = u_{MPC}(t + T_{delay})\)
非线性补偿（逆模型）： \(\delta_{cmd} = f^{-1}(\delta_{desired}, v, a_y)\)
形式化验证
使用可达性分析验证安全性： \(\mathcal{R}_{t+\Delta t} = \text{Post}(\mathcal{R}_t, \mathcal{U}, \Delta t)\)
其中$\mathcal{R}_t$为$t$时刻的可达集。
责任敏感安全（RSS）
定义安全距离： \(d_{safe} = v_{ego} \cdot t_{response} + \frac{v_{ego}^2}{2a_{min,ego}} - \frac{v_{front}^2}{2a_{max,front}}\)
安全检查：
def is_safe_action(action, state):
future_state = predict(state, action)
for obstacle in obstacles:
if distance(future_state, obstacle) < d_safe:
return False
return True
降级策略
故障处理层级：
Waymo公开的性能数据（2023）：
本章系统介绍了自动驾驶控制系统的核心技术。从车辆动力学建模开始，我们学习了自行车模型、轮胎模型等基础知识，理解了车辆运动的物理约束。在路径规划部分，掌握了全局规划（Hybrid A、RRT）和局部轨迹生成（Frenet坐标系、多项式拟合）的方法。横纵向控制部分详细介绍了从简单的Pure Pursuit到复杂的MPC等多种控制算法，以及如何处理横纵向耦合。多车协同控制展示了V2X技术的应用前景，包括车队控制、交叉口协同等场景。最后通过Waymo的深度案例，了解了工业界如何整合这些技术构建完整的自动驾驶系统。
关键概念回顾：
核心公式汇总：
自行车运动学模型： \(\dot{\psi} = \frac{v}{L} \tan(\delta_f)\)
轮胎侧偏力（线性）： \(F_y = -C_\alpha \alpha\)
MPC优化问题： \(\min_{\mathbf{U}} \sum_{k=0}^{N-1} (\mathbf{x}_k^T Q \mathbf{x}_k + \mathbf{u}_k^T R \mathbf{u}_k)\)
安全跟车距离： \(d_{safe} = d_0 + T_{hw} \cdot v\)
弦稳定性条件： \(\left|\frac{X_i(s)}{X_{i-1}(s)}\right| \leq 1\)
练习15.1 推导自行车模型的状态空间方程 给定车速$v = 20 m/s$，质量$m = 1500 kg$，转动惯量$I_z = 3000 kg·m^2$，$L_f = 1.2 m$，$L_r = 1.5 m$，前后轮侧偏刚度$C_f = C_r = 100000 N/rad$。推导在该工作点的线性化状态空间模型，并分析系统的稳定性。
Hint: 先写出非线性动力学方程，然后在平衡点$(v_y = 0, \dot{\psi} = 0)$处线性化。
练习15.2 Pure Pursuit控制器设计 车辆以$v = 10 m/s$行驶，轴距$L = 2.7 m$。设计Pure Pursuit控制器跟踪半径$R = 50 m$的圆形路径。如果前视距离$L_d = 15 m$，计算稳态转向角。
Hint: 对于圆形路径，几何关系简化为$\sin(\alpha) = L_d/(2R)$。
练习15.3 轮胎力计算 车辆以$v_x = 30 m/s$直线行驶，突然施加$\delta_f = 5°$的转向。如果$C_f = 120000 N/rad$，$L_f = 1.1 m$，计算初始时刻的前轮横向力。假设初始$v_y = 0$，$\dot{\psi} = 0$。
Hint: 计算前轮侧偏角$\alpha_f = \delta_f - \arctan((v_y + L_f\dot{\psi})/v_x)$。
练习15.4 ACC安全距离计算 前车速度$v_{front} = 25 m/s$，自车速度$v_{ego} = 30 m/s$。如果时间间隔$T_{hw} = 1.8 s$，最小静止距离$d_0 = 4 m$，计算所需的安全跟车距离。如果实际距离为$50 m$，应该加速还是减速？
Hint: 使用公式$d_{safe} = d_0 + T_{hw} \cdot v_{ego}$。
练习15.5 MPC控制器设计与实现 设计一个MPC控制器用于车道保持。系统参数同练习15.1，预测时域$N = 10$，采样时间$T_s = 0.1 s$。如果当前横向偏差$e_y = 0.5 m$，航向偏差$e_\psi = 0.1 rad$，使用二次规划求解最优控制序列。
Hint: 构建预测模型$\mathbf{X} = \Phi \mathbf{x}_0 + \Gamma \mathbf{U}$，然后最小化$J = \mathbf{X}^T\bar{Q}\mathbf{X} + \mathbf{U}^T\bar{R}\mathbf{U}$。
练习15.6 车队稳定性分析 三车编队，车间时距$h = 0.8 s$，控制增益$k_1 = 0.5, k_2 = 1.0$，执行器延迟$\tau = 0.2 s$。分析该配置是否满足弦稳定性要求，如不满足，如何调整参数？
| Hint: 计算传递函数$G(s) = X_i(s)/X_{i-1}(s)$，检查$ | G(j\omega) | \leq 1$。 |
练习15.7 轨迹优化与避障 在Frenet坐标系下，设计一条从$(s_0 = 0, d_0 = 0)$到$(s_f = 100, d_f = 3.5)$的换道轨迹，时间$T = 5s$。障碍物位于$(s_{obs} = 50, d_{obs} = 0)$，尺寸为$5m \times 2m$。使用五次多项式生成横向轨迹，确保与障碍物的最小距离$> 1m$。
Hint: 参数化横向轨迹$d(t)$，在$t = T/2$时检查避障约束。
练习15.8 形式化安全验证 使用RSS模型验证紧急制动场景的安全性。自车速度$v_{ego} = 30 m/s$，前车速度$v_{front} = 15 m/s$，反应时间$t_{response} = 1 s$，自车最大减速度$a_{min,ego} = -8 m/s^2$，前车最大加速度$a_{max,front} = 3 m/s^2$。计算最小安全距离，并分析如果当前距离为$60m$，是否需要紧急制动？
Hint: 考虑最坏情况：反应时间内自车保持当前速度，前车立即最大加速。
Ernst Dickmanns (1940-)
被誉为”自动驾驶之父”的Ernst Dickmanns是德国慕尼黑联邦国防军大学的教授。1987年，他领导的团队改装了一辆奔驰面包车VaMoRs，在高速公路上实现了时速90公里的自动驾驶，这是世界上第一辆真正意义上的自动驾驶汽车。
主要贡献：
技术创新：
Dickmanns的工作证明了自动驾驶的可行性，为今天的Waymo、Tesla等公司奠定了基础。他强调理论与实践结合，认为”真正的智能必须在真实世界中证明自己”。
控制障碍函数提供了一种系统化的方法来保证安全性。对于安全集$\mathcal{C} = {x: h(x) \geq 0}$，如果存在扩展类$\mathcal{K}_\infty$函数$\alpha$使得：
\[\sup_{u \in U} [\mathcal{L}_f h(x) + \mathcal{L}_g h(x) u] \geq -\alpha(h(x))\]则系统前向不变，即从安全集出发的轨迹永远保持在安全集内。
CBF-QP控制器： \(\begin{aligned} u^* = \arg\min_{u} \quad & \|u - u_{nom}\|^2 \\ \text{s.t.} \quad & \mathcal{L}_f h(x) + \mathcal{L}_g h(x) u \geq -\alpha(h(x)) \end{aligned}\)
计算系统在所有可能的输入和扰动下的可达集：
Hamilton-Jacobi可达性： \(\frac{\partial V}{\partial t} + \min_{u} \max_{d} \left[ \frac{\partial V}{\partial x} \cdot f(x,u,d) \right] = 0\)
其中$V(x,t)$为值函数，其零水平集定义了向后可达集。
使用线性时序逻辑（LTL）或信号时序逻辑（STL）描述复杂的安全和性能需求：
STL示例： \(\varphi = \square_{[0,T]} (d_{ego,obs} > d_{safe}) \wedge \Diamond_{[0,T]} (|v - v_{ref}| < \epsilon)\)
含义：始终保持安全距离，且最终达到期望速度。
考虑不确定性的安全验证：
概率可达性： \(P(x(t) \in \mathcal{X}_{unsafe}) \leq \epsilon\)
随机CBF： \(\mathbb{E}[\mathcal{L} h(x,w)] \geq -\alpha(h(x)) + \beta \sigma_h(x)\)
其中$w$为随机扰动，$\sigma_h$为不确定性相关项。
安全监督器架构：
高级控制器 → 安全过滤器 → 执行器
↑
安全监督器
监督器实时检查控制命令，必要时进行干预或切换到安全模式。
这些方法正在从研究走向工业应用，为L4/L5级自动驾驶提供数学保证的安全性。