第15章：自动驾驶控制系统

自动驾驶技术是现代控制理论最具挑战性和影响力的应用之一。本章将系统介绍自动驾驶车辆的建模、规划与控制方法，从基础的车辆动力学模型出发，逐步深入到路径规划、轨迹跟踪、多车协同等核心技术。我们将特别关注工程实践中的关键问题，如模型不确定性、执行器约束、实时性要求等，并通过Waymo的实际案例展示这些技术如何在真实世界中应用。

15.1 车辆动力学建模

准确的车辆动力学模型是设计高性能控制器的基础。根据应用场景和计算资源的不同，我们需要在模型精度和复杂度之间权衡。

15.1.1 二自由度自行车模型

自行车模型是自动驾驶中最常用的简化模型，它将四轮车辆简化为前后两个轮子，忽略车辆的俯仰和侧倾运动，只考虑纵向、横向和横摆三个自由度。

     前轮
      |
      |  δ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}$$ 其中：

• $(x, y)$：车辆质心位置
• $\psi$：航向角
• $v$：车速
• $\beta$：质心侧偏角
• $\delta_f$：前轮转角
• $L_f, L_r$：质心到前后轴的距离
• $L = L_f + L_r$：轴距

动力学模型（考虑轮胎力）： $$\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}$$

15.1.2 轮胎模型

轮胎是车辆与路面的唯一接触点，其力学特性对车辆动态响应有决定性影响。

线性轮胎模型（小滑移角假设）： $$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$通过实验数据拟合得到，典型值：

• $B$：刚度因子（10-12）
• $C$：形状因子（1.3-1.9）
• $D$：峰值因子（$\mu mg$，$\mu$为路面摩擦系数）
• $E$：曲率因子（-1到1）

15.1.3 执行器动力学

实际车辆的执行器（转向、制动、油门）都有动态响应特性和饱和限制。

转向系统一阶动力学： $$\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_{max} = 3-4 \, m/s^2$（乘用车）
• 最大减速度：$|a_{min}| = 8-10 \, m/s^2$（ABS制动）
• 最大转向角：$|\delta_{f,max}| = 30-35°$
• 最大转向角速度：$|\dot{\delta}_{f,max}| = 30-60°/s$

15.1.4 完整状态空间模型

将上述模型线性化后，得到用于控制器设计的状态空间模型： $$\dot{\mathbf{x}} = A\mathbf{x} + B\mathbf{u}$$ 状态向量：$\mathbf{x} = [e_y, \dot{e}_y, e_\psi, \dot{e}_\psi]^T$

• $e_y$：横向位置偏差
• $e_\psi$：航向角偏差

控制输入：$\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}$$

15.2 路径规划与轨迹生成

路径规划负责生成从起点到终点的可行路径，而轨迹生成则在此基础上加入时间维度，生成满足动力学约束的轨迹。

15.2.1 全局路径规划

全局路径规划在高精地图上寻找最优路径，通常使用图搜索或采样方法。

Hybrid A*算法

结合了A*的最优性和车辆运动学约束：

1. 状态空间：$(x, y, \theta)$，其中$\theta$为航向角
2. 动作空间：离散的转向角和速度组合
3. 代价函数：$f(n) = g(n) + h(n)$ - $g(n)$：从起点到节点$n$的实际代价 - $h(n)$：启发式函数（如Dubins/Reeds-Shepp曲线长度）

算法伪代码：

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最小的父节点
   - 重新连接可以改善的节点

15.2.2 局部轨迹规划

局部轨迹规划在全局路径基础上，生成满足舒适性和安全性要求的平滑轨迹。

Frenet坐标系规划

将规划问题分解为沿道路方向（s）和垂直道路方向（d）：

横向轨迹（五次多项式）： $$d(t) = a_0 + a_1t + a_2t^2 + a_3t^3 + a_4t^4 + a_5t^5$$ 边界条件：

• 初始：$d(0) = d_0, \dot{d}(0) = \dot{d}_0, \ddot{d}(0) = \ddot{d}_0$
• 终端：$d(T) = d_f, \dot{d}(T) = 0, \ddot{d}(T) = 0$

纵向轨迹（四次或五次多项式）： $$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}$$ 其中：

• $J_{jerk} = \int_0^T \dddot{d}^2(t) dt$（舒适性）
• $J_{time} = T$（效率）
• $J_{offset} = (d_f - d_{target})^2$（跟踪精度）
• $J_{speed} = \int_0^T (v(t) - v_{desired})^2 dt$（速度跟踪）

15.2.3 避障与安全走廊

安全走廊生成

使用凸优化方法生成无碰撞轨迹：

1. 将障碍物膨胀（考虑车辆尺寸）
2. 构建时空占用图
3. 生成凸安全走廊序列
4. 在走廊内优化轨迹

时空轨迹优化（二次规划）： $$\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

动态规划求解：

• 状态：$(s_i, t_j, v_k)$
• 代价：加速度变化、时间、速度偏差
• 约束：加速度限制、避障

15.3 横向与纵向控制

轨迹跟踪控制通常分为横向（转向）和纵向（速度）两个子问题。

15.3.1 横向控制方法

Pure Pursuit控制器

基于几何的方法，通过跟踪前视点实现路径跟踪： $$\delta_f = \arctan\left(\frac{2L \sin(\alpha)}{L_d}\right)$$ 其中：

• $L_d$：前视距离（通常$L_d = k_v \cdot v + L_{d,min}$）
• $\alpha$：前视点与车辆航向的夹角

优点：简单鲁棒，计算量小 缺点：稳态误差，高速性能受限

Stanley控制器

斯坦福大学在DARPA挑战赛中使用的方法： $$\delta_f = \psi_e + \arctan\left(\frac{k_e \cdot e_y}{v}\right)$$ 其中：

• $\psi_e$：航向误差
• $e_y$：横向误差
• $k_e$：增益参数（典型值2.5）

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}]$$ 典型权重：

• $q_{e_y} = 10$（横向位置误差）
• $q_{e_\psi} = 1$（航向误差）
• $r_{\delta} = 1$（控制代价）

反馈增益：$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$

15.3.2 纵向控制

PID速度控制 $$a_{cmd} = K_p e_v + K_i \int e_v dt + K_d \frac{de_v}{dt} + a_{ff}$$ 其中：

• $e_v = v_{ref} - v$：速度误差
• $a_{ff}$：前馈项（克服空气阻力和坡度）

增益调度（根据速度调整）： $$K_p(v) = K_{p,0} \cdot (1 + \alpha_p \cdot v)$$ 自适应巡航控制（ACC）

安全跟车距离： $$d_{safe} = d_0 + T_{hw} \cdot v$$ 其中：

• $d_0$：最小静止距离（3-5米）
• $T_{hw}$：时间间隔（1.5-2秒）

多目标优化： $$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:
    模式 = 巡航

15.3.3 横纵向耦合控制

高速过弯时，横纵向动力学存在强耦合：

轮胎摩擦圆约束： $$\sqrt{F_x^2 + F_y^2} \leq \mu F_z$$ 协调控制策略：

1. 根据曲率预测所需横向力
2. 分配剩余摩擦力用于纵向控制
3. 动态调整速度以保证横向稳定性

过弯速度限制： $$v_{max} = \sqrt{\mu g r}$$ 其中$r$为弯道半径。

15.4 多车协同控制

多车协同控制通过车车通信（V2V）和车路协同（V2I），实现更高效、更安全的交通系统。

15.4.1 车队控制（Platooning）

车队控制使多辆车保持紧密队形行驶，减少空气阻力和道路占用。

纵向动力学模型

第$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}$$ 间距策略：

• 固定间距（CTG）：$d_{i,des} = d_0$
• 固定时间间隔（CTH）：$d_{i,des} = h \cdot v_i + d_0$

分布式控制架构

前车跟随+前导车信息（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)$$

15.4.2 交叉口协同

无信号灯交叉口的协同通过优化车辆通过顺序和轨迹。

冲突检测

两车轨迹$(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}$$ 分布式协商

基于拍卖的机制：

1. 每车计算通过代价（时间、燃油）
2. 竞价获得时隙
3. 赢家广播轨迹
4. 其他车辆调整计划

15.4.3 合流与换道协调

高速公路合流需要协调主道和匝道车辆。

虚拟车辆法

为合流车在主道创建"虚拟占位"： $$x_{virtual}(t) = x_{merge} - v_{main} \cdot (t_{merge} - t)$$ 主道车辆跟踪虚拟车辆，为合流车预留空间。

协同换道

三阶段协议：

1. 请求阶段：广播换道意图和期望间隙
2. 协商阶段：目标车道车辆调整间距
3. 执行阶段：协同完成换道动作

间隙生成控制： $$a_{create} = k_g(d_{gap,des} - d_{gap}) - k_v(v_{front} - v_{rear})$$

15.4.4 V2X通信与安全

通信协议

DSRC/C-V2X标准：

• 频率：5.9 GHz
• 延迟：< 100ms
• 范围：300-1000m
• 消息类型：BSM（基本安全消息）、SPaT（信号相位和定时）

一致性协议

分布式平均共识： $$\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()

15.5 深度案例：Waymo自动驾驶运动规划与控制

Waymo作为自动驾驶领域的先驱，其技术栈代表了当前的最高水平。

15.5.1 系统架构

Waymo采用模块化架构，主要包括：

传感器输入 → 感知 → 预测 → 规划 → 控制 → 执行器
     ↑                            ↓
     └──────── 地图与定位 ←─────────┘

传感器配置

• 5个激光雷达（1个长距、4个近距）
• 29个摄像头（覆盖360°）
• 6个毫米波雷达
• IMU和GPS

计算平台

• 主计算单元：多个TPU/GPU
• 冗余系统：独立安全监控
• 实时性要求：100Hz控制频率

15.5.2 行为预测

Waymo使用深度学习预测其他交通参与者的行为。

VectorNet架构

将场景表示为向量化的高精地图和轨迹：

• 地图要素：车道线、人行道、交通标志
• 历史轨迹：过去2秒的观测
• 输出：未来8秒的多模态轨迹分布

交互建模

使用图神经网络捕捉车辆间交互： $$h_i^{(l+1)} = \sigma\left(W_{self}h_i^{(l)} + \sum_{j \in \mathcal{N}_i} W_{edge}h_j^{(l)}\right)$$

15.5.3 运动规划框架

分层规划

1. 行为规划：决策树/有限状态机 - 车道保持、换道、转弯、停车

2. 轨迹规划：优化方法 - 时空联合优化 - 考虑预测不确定性

优化问题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$$

15.5.4 控制器实现

级联控制结构

轨迹 → 路径跟踪控制器 → 转向角
    ↓
    → 速度跟踪控制器 → 油门/刹车

自适应MPC控制器

Waymo使用自适应MPC处理模型不确定性：

1. 在线参数估计 - 轮胎刚度：$\hat{C}_\alpha = C_{\alpha,nom} \cdot (1 + \Delta_\alpha)$ - 质量：$\hat{m} = m_{nom} + \Delta_m$

2. 鲁棒约束收紧 $$\mathcal{X}_{tight} = \mathcal{X} \ominus \mathcal{E}$$ 其中$\mathcal{E}$为不确定性边界。

3. 多模型切换 - 干燥路面模型 - 湿滑路面模型 - 紧急制动模型

执行器补偿

延迟补偿（预测控制）： $$u(t) = u_{MPC}(t + T_{delay})$$ 非线性补偿（逆模型）： $$\delta_{cmd} = f^{-1}(\delta_{desired}, v, a_y)$$

15.5.5 安全保障

形式化验证

使用可达性分析验证安全性： $$\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

降级策略

故障处理层级：

1. 主系统故障 → 备份系统接管
2. 传感器故障 → 降级感知模式
3. 通信故障 → 保守驾驶策略
4. 严重故障 → 最小风险机动（MRM）

15.5.6 性能指标

Waymo公开的性能数据（2023）：

• 平均接管间隔：>10,000英里
• 碰撞率：<0.5次/百万英里
• 舒适性指标：加速度<2m/s²（95%时间）
• 计算延迟：<50ms（感知到控制）

本章小结

本章系统介绍了自动驾驶控制系统的核心技术。从车辆动力学建模开始，我们学习了自行车模型、轮胎模型等基础知识，理解了车辆运动的物理约束。在路径规划部分，掌握了全局规划（Hybrid A、RRT）和局部轨迹生成（Frenet坐标系、多项式拟合）的方法。横纵向控制部分详细介绍了从简单的Pure Pursuit到复杂的MPC等多种控制算法，以及如何处理横纵向耦合。多车协同控制展示了V2X技术的应用前景，包括车队控制、交叉口协同等场景。最后通过Waymo的深度案例，了解了工业界如何整合这些技术构建完整的自动驾驶系统。

关键概念回顾：

• 车辆动力学：自行车模型、轮胎侧偏角、摩擦圆约束
• 轨迹规划：Frenet坐标系、时空优化、安全走廊
• 横向控制：Pure Pursuit、Stanley、LQR、MPC
• 纵向控制：PID、ACC、速度规划
• 多车协同：弦稳定性、一致性协议、V2X通信
• 安全保障：RSS、形式化验证、降级策略

核心公式汇总：

1. 自行车运动学模型： $$\dot{\psi} = \frac{v}{L} \tan(\delta_f)$$

2. 轮胎侧偏力（线性）： $$F_y = -C_\alpha \alpha$$

3. 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)$$

4. 安全跟车距离： $$d_{safe} = d_0 + T_{hw} \cdot v$$

5. 弦稳定性条件： $$\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: 考虑最坏情况：反应时间内自车保持当前速度，前车立即最大加速。

常见陷阱与错误 (Gotchas)

1. 模型简化过度 - 错误：在所有场景使用运动学自行车模型 - 正确：低速用运动学模型，高速用动力学模型

2. 忽略执行器约束 - 错误：MPC输出直接作为控制命令 - 正确：考虑转向角速度限制和执行器延迟

3. 前视距离固定 - 错误：Pure Pursuit使用固定前视距离 - 正确：前视距离随速度自适应调整

4. 坐标系混淆 - 错误：混用全局坐标系和Frenet坐标系 - 正确：明确每个变量所在的坐标系

5. 通信延迟忽略 - 错误：V2X信息直接使用 - 正确：时间戳校验和延迟补偿

6. 单一传感器依赖 - 错误：仅依靠视觉进行定位 - 正确：多传感器融合提高鲁棒性

7. 忽略轮胎饱和 - 错误：线性轮胎模型用于极限工况 - 正确：检测并处理轮胎力饱和

8. 优化问题无解 - 错误：MPC约束过严导致无可行解 - 正确：软约束和降级策略

最佳实践检查清单

系统设计阶段

• [ ] 明确运行设计域（ODD）：速度范围、道路类型、天气条件
• [ ] 选择合适的车辆模型复杂度
• [ ] 定义安全需求和性能指标
• [ ] 设计冗余和降级策略
• [ ] 规划传感器配置和计算资源

算法开发阶段

• [ ] 控制器增益随工况自适应
• [ ] 预测模型包含不确定性
• [ ] 轨迹规划考虑舒适性约束
• [ ] 实现多层次安全检查
• [ ] 处理传感器故障和通信中断

测试验证阶段

• [ ] 仿真覆盖极端工况
• [ ] 硬件在环（HIL）测试
• [ ] 封闭场地测试标准场景
• [ ] 记录并分析接管事件
• [ ] 持续监控关键性能指标

部署运营阶段

• [ ] 实时性能监控
• [ ] 异常检测和报警
• [ ] 数据记录用于事后分析
• [ ] OTA更新机制
• [ ] 与交通管理系统集成

历史人物

Ernst Dickmanns (1940-)

被誉为"自动驾驶之父"的Ernst Dickmanns是德国慕尼黑联邦国防军大学的教授。1987年，他领导的团队改装了一辆奔驰面包车VaMoRs，在高速公路上实现了时速90公里的自动驾驶，这是世界上第一辆真正意义上的自动驾驶汽车。

主要贡献：

• 开创性地将计算机视觉应用于车辆控制
• 提出"4D方法"：空间3D + 时间维度的动态视觉
• 1994年在巴黎高速公路上演示1000公里自动驾驶
• 启发了后续DARPA挑战赛和现代自动驾驶发展

技术创新：

• 使用卡尔曼滤波进行运动预测
• 基于模型的计算机视觉方法
• 分层控制架构设计
• 实时图像处理和决策系统

Dickmanns的工作证明了自动驾驶的可行性，为今天的Waymo、Tesla等公司奠定了基础。他强调理论与实践结合，认为"真正的智能必须在真实世界中证明自己"。

前沿专题：安全关键控制与形式化验证

控制障碍函数（CBF）

控制障碍函数提供了一种系统化的方法来保证安全性。对于安全集$\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$为不确定性相关项。

运行时监控

安全监督器架构：

高级控制器 → 安全过滤器 → 执行器
              ↑
          安全监督器

监督器实时检查控制命令，必要时进行干预或切换到安全模式。

工具链

• KeYmaera X：混成系统定理证明器
• CORA：可达性分析工具箱
• TuLiP：时序逻辑规划工具
• S-TaLiRo：时序逻辑鲁棒性测试
• ARCH-COMP：形式化验证基准测试集

这些方法正在从研究走向工业应用，为L4/L5级自动驾驶提供数学保证的安全性。