第6章：决策、规划与控制

决策、规划与控制模块是自动驾驶系统的"大脑"，负责将感知和预测信息转化为具体的车辆控制指令。本章将深入探讨从高层行为决策到底层控制的完整技术链路，包括行为决策的架构设计、运动规划的优化方法、轨迹生成的约束处理，以及车辆控制的实现策略。我们将重点关注2023-2025年的最新进展，包括基于学习的规划方法、差分规划器、以及MPC在实时系统中的高效实现。

学习目标

完成本章学习后，您将能够：

设计和实现多层次的决策规划架构
掌握主流运动规划算法的原理和工程实现
理解轨迹优化中的成本函数设计和约束处理
实现横纵向解耦和联合的车辆控制器
评估规划结果的安全性、舒适性和效率
处理动态环境中的实时重规划问题

6.1 行为决策层

行为决策层负责生成高层驾驶策略，如车道保持、换道、超车、让行等。这一层需要理解交通规则、处理多智能体交互，并在安全性、效率和舒适性之间做出权衡。

6.1.1 有限状态机架构

有限状态机（FSM）是最经典的行为决策方法，通过明确的状态转换逻辑实现决策。尽管深度学习方法不断涌现，FSM因其可解释性和确定性在量产系统中仍占主导地位。

        [车道保持] ←─────────┐
           ↑ ↓                │
    [准备换道] ←→ [执行换道]   │
           ↓ ↑                │
        [紧急制动] ──────────→┘
           ↓
        [完全停止]

分层FSM设计：

顶层FSM（Mission Level）：
├── 正常驾驶模式
│   ├── 高速巡航
│   ├── 城市跟车
│   └── 路口通过
├── 特殊场景模式
│   ├── 施工区通过
│   ├── 紧急车辆避让
│   └── 恶劣天气模式
└── 异常处理模式
    ├── 最小风险机动（MRM）
    └── 安全停车

中层FSM（Behavior Level）：
├── 纵向行为
│   ├── 自由巡航
│   ├── 跟车
│   └── 停车
└── 横向行为
    ├── 车道保持
    ├── 换道
    └── 避障

优势：

逻辑清晰，易于理解和调试
符合功能安全要求（ISO 26262 ASIL-D）
状态转换条件可精确控制
便于形式化验证和测试覆盖

劣势：

难以处理复杂场景的组合爆炸（状态数指数增长）
状态转换边界可能导致振荡
缺乏对不确定性的建模能力
难以优雅处理部分可观测场景

工程实践要点：

分层架构设计： - Mission层：5-10个状态，处理高层任务 - Behavior层：每个Mission状态包含10-20个子状态 - Motion层：具体执行动作，与规划器接口
迟滞机制设计：

换道触发条件：

- 速度差 > 10 km/h 持续 3秒
- 目标车道评分优势 > 20% 持续 2秒
- 安全间距满足 TTC > 5秒

换道取消条件（更严格的阈值）：

- 目标车道TTC < 3秒（vs 触发的5秒）
- 原车道突然畅通（速度差 < 5 km/h）

状态转换条件参数化：

lane_change_params:
  trigger:
    speed_diff_threshold: 10.0  # km/h
    time_duration: 3.0           # seconds
    safety_ttc: 5.0              # seconds
  abort:
    critical_ttc: 3.0            # seconds
    lateral_progress: 0.5        # meters

异常处理机制： - 每个状态定义超时时间 - 异常状态自动降级到安全状态 - 保持状态历史用于故障诊断

状态一致性保证：

状态转换矩阵验证：

- 可达性检查：确保所有状态可达
- 死锁检测：避免状态循环锁死
- 优先级冲突：多条件触发时的仲裁

6.1.2 基于规则的决策树

决策树通过嵌套的if-then规则实现决策，常与FSM结合使用。现代系统通常采用可配置的规则引擎，支持在线更新和A/B测试。

分层决策架构：

决策树层次结构：
Level 1: 安全关键决策（Safety Critical）
├── 碰撞检测 (TTC < 1.5s)
│   ├── 紧急制动 (AEB)
│   ├── 紧急转向 (如果可行)
│   └── 组合避障 (制动+转向)
├── 规则违反检测
│   ├── 红灯闯入预防
│   ├── 逆行检测
│   └── 限速违反预防

Level 2: 交通规则遵守（Traffic Rules）
├── 信号灯决策
│   ├── 绿灯通行策略
│   ├── 黄灯决策（通过/停止）
│   └── 红灯等待位置
├── 标志牌响应
│   ├── 停车标志 (完全停止3秒)
│   ├── 让行标志 (减速观察)
│   └── 限速调整

Level 3: 驾驶策略（Driving Strategy）
├── 车道选择
│   ├── 最优车道评估
│   ├── 换道时机决策
│   └── 特殊车道处理（HOV、应急）
├── 速度管理
│   ├── 巡航速度设定
│   ├── 跟车距离调整
│   └── 弯道速度控制

Level 4: 舒适性优化（Comfort）
├── 加速度平滑
├── 路径曲率优化
└── 乘客偏好适应

规则引擎实现：

class RuleEngine:
    def __init__(self):
        self.rules = PriorityQueue()
        self.rule_history = deque(maxlen=100)

    def evaluate(self, context):
        """规则评估主循环"""
        decisions = []

        # 按优先级评估规则
        for priority, rule in self.rules:
            if rule.is_applicable(context):
                decision = rule.evaluate(context)

                # 冲突检测
                if not self.has_conflict(decision, decisions):
                    decisions.append(decision)

                    # 短路机制：安全规则触发后停止
                    if priority < SAFETY_THRESHOLD:
                        break

        return self.arbitrate(decisions)

规则优先级体系：

优先级值  类别              示例规则                    覆盖权限
------------------------------------------------------------------------
0-99     紧急安全          TTC < 1.5s → 紧急制动       可覆盖所有
100-199  法规遵守          红灯 → 停止                 可覆盖策略和舒适
200-299  安全缓冲          车距 < 2s → 减速            可覆盖舒适
300-399  驾驶策略          前车慢 → 考虑换道           可覆盖舒适
400-499  效率优化          多车道 → 选择最快车道       无覆盖权
500-599  舒适性            加速度 < 1.5 m/s²          无覆盖权

动态规则调整：

场景自适应：

scenario_rules:
  highway:
    min_following_distance: 2.0s
    max_acceleration: 2.5 m/s²
    lane_change_aggressiveness: 0.7

  urban:
    min_following_distance: 1.5s
    max_acceleration: 2.0 m/s²
    pedestrian_buffer: 3.0m

  parking:
    max_speed: 10 km/h
    min_obstacle_distance: 0.5m
    precision_mode: true

在线学习更新： - 基于驾驶员接管统计调整阈值 - A/B测试不同规则参数 - 根据区域特性微调（如不同城市的驾驶习惯）
规则冲突解决：

冲突仲裁策略：

1. 安全性 > 合法性 > 效率性 > 舒适性
2. 保守偏向：存在冲突时选择更保守的决策
3. 时序优先：紧急程度高的先执行
4. 上下文相关：根据场景调整权重

6.1.3 基于学习的决策方法

近年来，深度学习和强化学习在行为决策中得到广泛应用。2024-2025年的趋势是将大模型能力与传统方法结合，实现更智能和适应性更强的决策。

逆强化学习（IRL）：

从人类驾驶数据中学习奖励函数，然后用于决策。

Maximum Entropy IRL：

R(s,a) = θᵀφ(s,a)
P(τ|θ) ∝ exp(Σₜ R(sₜ,aₜ))

特征设计示例： python features = { 'safety': [ distance_to_vehicles, time_to_collision, lane_deviation ], 'efficiency': [ speed_difference, acceleration_magnitude, route_progress ], 'comfort': [ lateral_acceleration, jerk, steering_rate ], 'social': [ courtesy_score, # 让行行为 assertiveness, # 主动性 predictability # 可预测性 ] }

Neural IRL： - 使用深度网络直接学习奖励函数 - 处理高维状态空间（图像输入） - 端到端学习避免手工特征设计

模仿学习（IL）进展：

行为克隆（BC）改进：

class ImprovedBC:
    def __init__(self):
        self.policy = TransformerPolicy()
        self.safety_filter = SafetyNet()

    def predict(self, state, history):
        # 基础BC预测
        action = self.policy(state, history)

        # DAgger: 在线收集纠正数据
        if self.in_training:
            expert_action = self.get_expert_correction()
            self.buffer.add(state, expert_action)

        # 安全过滤
        safe_action = self.safety_filter(action, state)
        return safe_action

GAIL（生成对抗模仿学习）：

min_π max_D V(π,D) = E_π[log(1-D(s,a))] + E_expert[log D(s,a)]

判别器区分专家和学习策略
生成器（策略）欺骗判别器
不需要显式奖励函数

层次模仿学习：

High-level (意图): P(intent|scene)
Mid-level (行为): P(behavior|intent, scene)  
Low-level (轨迹): P(trajectory|behavior, scene)

强化学习决策：

Offline RL方法：

class ConservativeQL:
    """Conservative Q-Learning for offline training"""
    def update(self, batch):
        # 标准TD更新
        q_target = r + γ * min(Q₁(s',a'), Q₂(s',a'))

        # CQL惩罚项：降低OOD动作的Q值
        cql_loss = α * (Q(s,a_random) - Q(s,a_data))

        total_loss = td_loss + cql_loss

Model-based RL： - 学习环境模型进行规划 - World Models想象未来场景 - MBPO：模型和无模型结合

混合决策架构：

输入场景 → [Perception]
              ↓
    ┌─────────┴─────────┐
    ↓                   ↓
[Neural Planner]   [Rule Checker]
    ↓                   ↓
[Candidate Actions] [Constraints]
    ↓                   ↓
    └─────────┬─────────┘
              ↓
       [Safety Filter]
              ↓
       [Final Decision]

实现示例：

class HybridDecisionMaker:
    def __init__(self):
        self.neural_policy = load_pretrained_model()
        self.rule_engine = RuleEngine()
        self.safety_monitor = SafetyMonitor()

    def decide(self, observation):
        # 神经网络生成候选决策
        candidates = self.neural_policy.generate_options(
            observation, 
            num_samples=5
        )

        # 规则系统评分和过滤
        scored_candidates = []
        for action in candidates:
            if self.rule_engine.is_valid(action, observation):
                score = self.evaluate_action(action, observation)
                scored_candidates.append((score, action))

        # 选择最优且安全的动作
        best_action = max(scored_candidates, key=lambda x: x[0])[1]

        # 最终安全检查
        safe_action = self.safety_monitor.verify(best_action)

        return safe_action

Transformer在决策中的应用：

Decision Transformer (DT)：

输入序列：[s₁, a₁, r₁, s₂, a₂, r₂, ..., sₜ]
输出：aₜ₊₁

条件生成：以期望回报R为条件

Scene Transformer： - 多智能体联合预测 - 注意力机制建模交互 - 因果masking保证时序性
Trajectory Transformer：

class TrajectoryTransformer:
    def forward(self, states, returns_to_go):
        # 位置编码
        pos_emb = self.positional_encoding(seq_len)

        # 状态和回报嵌入
        state_emb = self.state_encoder(states)
        rtg_emb = self.return_encoder(returns_to_go)

        # 交叉注意力
        h = self.transformer(
            torch.cat([state_emb, rtg_emb], dim=1)
        )

        # 预测动作
        actions = self.action_head(h)
        return actions

6.1.4 多智能体交互建模

自动驾驶需要预测和响应其他交通参与者的行为。

博弈论方法：

Nash均衡求解多车交互
Level-k推理建模不同驾驶风格
社会价值导向（SVO）描述合作程度

意图识别：

基于轨迹模式的意图分类
注意力机制识别交互对象
概率图模型建模意图演化

6.2 运动规划

运动规划将行为决策转化为可行的轨迹，需要考虑动力学约束、障碍物避让和优化目标。

6.2.1 规划空间表示

笛卡尔空间 vs Frenet坐标系：

Frenet坐标系沿道路中心线定义，将规划问题解耦为横向（d）和纵向（s）：

s: 沿道路中心线的弧长
d: 到中心线的横向偏移
简化了车道保持和换道的表达

时空规划 vs 空间规划：

时空规划：轨迹表示为(x(t), y(t))，直接包含时间信息
空间规划：先规划路径，再进行速度规划
时空规划更适合动态障碍物，空间规划计算效率更高

6.2.2 基于图搜索的方法

A*及其变体：

f(n) = g(n) + h(n)

g(n): 起点到节点n的实际代价
h(n): 节点n到目标的启发式估计
保证最优性需要h(n)满足一致性

Hybrid A*：结合连续空间和离散搜索：

状态空间：(x, y, θ)
使用Reeds-Shepp曲线连接节点
考虑车辆运动学约束

工程优化：

多分辨率搜索：远处粗糙，近处精细
启发式函数设计：结合非完整约束的下界
节点扩展剪枝：基于可达性分析

6.2.3 基于采样的方法

RRT (Rapidly-exploring Random Tree)：

1. 随机采样配置空间点 x_rand
2. 找到树中最近节点 x_near
3. 扩展步长δ生成 x_new
4. 碰撞检测，加入树中

RRT*优化：

渐进最优性：随着采样增加收敛到最优解
重连接操作：新节点加入时优化局部路径
Informed RRT*：采样集中在椭圆区域提高效率

实时性改进：

Anytime RRT*：快速获得可行解，持续优化
Kinodynamic RRT*：直接在状态空间采样
并行化：GPU加速碰撞检测

6.2.4 基于优化的方法

将轨迹规划形式化为优化问题：

min J = ∫[L(x(t), u(t)) + λ·g(x(t))]dt
s.t. ẋ = f(x, u)  (动力学约束)
     h(x) ≤ 0     (障碍物约束)
     u_min ≤ u ≤ u_max (控制约束)

数值方法：

直接配点法：离散化轨迹，转化为NLP
打靶法：优化控制序列，前向积分状态
微分动态规划（DDP）：二阶方法，快速收敛

凸化技术：

顺序凸规划（SCP）：迭代线性化非凸约束
松弛方法：引入松弛变量处理非凸约束
信赖域方法：保证收敛性

6.3 轨迹优化

6.3.1 成本函数设计

轨迹优化的核心是设计合理的成本函数，平衡多个目标：

基础成本项：

J = w₁·J_safety + w₂·J_comfort + w₃·J_efficiency + w₄·J_feasibility

安全性成本：

障碍物距离：exp(-d/σ)，指数惩罚近距离
道路边界：二次惩罚越界
动态障碍物：考虑相对速度的TTC

舒适性成本：

加速度：∫a²dt
加加速度（jerk）：∫j²dt
横向加速度：∫(v²κ)²dt，其中κ是曲率
方向盘转角率：∫ω²dt

效率成本：

行程时间：T
与期望速度偏差：∫(v - v_ref)²dt
路径长度：∫√(ẋ² + ẏ²)dt

参数自适应：

场景相关权重：高速vs市区不同参数
在线学习：基于驾驶员反馈调整
多目标优化：Pareto前沿分析

6.3.2 约束处理

硬约束 vs 软约束：

硬约束：必须满足（碰撞避免、最大加速度）
软约束：可以违反但有惩罚（舒适性、效率）
障碍函数法：-log(h(x))将约束编码到目标函数

动力学约束：单车模型（适用于低速）：

ẋ = v·cos(θ)
ẏ = v·sin(θ)
θ̇ = v·tan(δ)/L

车辆动力学模型（高速需要）：

考虑轮胎滑移角、质心侧偏等

时间一致性约束：

速度连续性：|v(t+Δt) - v(t)| ≤ a_max·Δt
加速度连续性：|a(t+Δt) - a(t)| ≤ j_max·Δt
曲率连续性：G²连续的样条曲线

6.3.3 数值优化算法

IPOPT（内点法）：

适用于大规模非线性规划
二阶收敛速度
需要精确的梯度和Hessian

SQP（序列二次规划）：

局部二次近似
处理等式和不等式约束
适合中等规模问题

ADMM（交替方向乘子法）：

分布式优化
将问题分解为多个子问题
适合并行计算

实时性保证：

热启动：使用上一帧解作为初值
早停：达到可接受精度即可
分层优化：粗规划+局部精调
GPU加速：并行计算梯度

6.4 车辆控制

车辆控制层将规划的轨迹转换为具体的控制指令（方向盘转角、油门、刹车）。控制器设计需要考虑车辆动力学、执行器延迟和外部扰动。

6.4.1 横向控制

横向控制负责方向盘转角控制，实现路径跟踪。

Pure Pursuit（纯跟踪）：

基于几何的方法，通过前视点进行路径跟踪：

δ = arctan(2L·sin(α)/l_d)

其中：

L：轴距
α：前视点与车辆朝向夹角
l_d：前视距离（通常l_d = k·v，k为增益）

优点：简单稳定，低速性能好缺点：高速易振荡，稳态误差大

Stanley控制器：

Stanford大学DARPA挑战赛方案：

δ = θ_e + arctan(k·e/v)

其中：

θ_e：航向误差
e：横向误差
k：增益参数

特点：

同时考虑航向和位置误差
高速稳定性好
参数调节直观

LQR（线性二次调节器）：

基于线性化模型的最优控制：

状态空间模型：

ẋ = Ax + Bu
y = Cx

成本函数：

J = ∫(x^T Q x + u^T R u)dt

反馈增益通过Riccati方程求解：

K = R^(-1)B^T P

优势：

理论保证稳定性
多目标权衡
可处理多输入多输出

6.4.2 纵向控制

纵向控制管理速度，包括加速和制动。

PID控制器：

经典的反馈控制：

u(t) = K_p·e(t) + K_i·∫e(τ)dτ + K_d·de/dt

工程实践：

防积分饱和：限制积分项范围
微分滤波：避免噪声放大
增益调度：根据速度调整参数

前馈+反馈控制：

u = u_ff + u_fb

前馈项基于模型预测：

空气阻力：F_air = 0.5·ρ·C_d·A·v²
坡度阻力：F_slope = m·g·sin(θ)
滚动阻力：F_roll = C_r·m·g

反馈项补偿模型误差和扰动。

智能巡航控制（ACC）：

时距策略：

d_des = d_0 + τ·v

其中τ是期望时距（通常1.5-2秒）。

控制律设计考虑：

前车加速度估计
舒适性约束（|a| < 2m/s², |j| < 1m/s³）
燃油经济性

6.4.3 联合横纵向控制

Model Predictive Control (MPC)：

MPC通过在线求解优化问题实现最优控制：

min Σ[||x_k - x_ref||²_Q + ||u_k||²_R + ||Δu_k||²_S]
s.t. x_{k+1} = f(x_k, u_k)
     u_min ≤ u_k ≤ u_max
     |Δu_k| ≤ Δu_max
     g(x_k) ≤ 0  (约束)

实时MPC实现：

线性化：在参考轨迹附近线性化，形成线性时变MPC
快速求解器： - OSQP：二次规划求解器 - ACADO：自动微分和代码生成 - 显式MPC：离线计算，在线查表
预测模型： - 运动学模型（低速） - 单轨模型（中速） - 双轨模型（高速、极限工况）

鲁棒MPC：

考虑不确定性：

管道MPC：约束收紧保证鲁棒性
场景MPC：多场景优化
随机MPC：概率约束

6.4.4 执行器建模与补偿

执行器延迟：

转向系统延迟（50-100ms）：

δ_actual(t) = δ_cmd(t - τ_delay)

补偿策略：

Smith预估器
预测控制指令
延迟状态扩增

执行器动力学：

一阶系统近似：

τ·δ̇ + δ = δ_cmd

补偿方法：

逆动力学前馈
模型预测中包含执行器模型

制动系统特性：

液压延迟：100-200ms
制动力非线性
ABS介入影响

建模方法：

查表法
神经网络拟合
分段线性模型

6.5 安全性与舒适性设计

6.5.1 安全保障机制

分层安全架构：

Level 3: 主规划器（优化性能）
    ↓
Level 2: 安全监控器（验证轨迹）
    ↓  
Level 1: 紧急制动（最后防线）

形式化验证方法：

可达集分析：计算所有可能状态
障碍函数：保证前向不变性
控制障碍函数（CBF）：

ḣ(x) + α(h(x)) ≥ 0

保证安全集合前向不变

责任敏感安全（RSS）模型：

Intel/Mobileye提出的数学模型：

安全距离：d_safe = v·τ + v²/(2a_brake) - v_front²/(2a_front)
横向安全：最小横向距离
正确响应：定义安全动作空间

6.5.2 舒适性指标

ISO 2631标准：

振动舒适性评价：

加权加速度：考虑频率响应
暴露时间：长时间vs短时间
方向敏感性：垂直vs水平

驾驶舒适性指标：

加速度限制： - 纵向：|a_x| < 2.5 m/s²（正常） - 横向：|a_y| < 2.0 m/s²（正常） - 紧急情况可放宽到4-5 m/s²
加加速度限制： - 纵向：|j_x| < 2.0 m/s³ - 横向：|j_y| < 1.5 m/s³
频域分析： - 0.5-2 Hz：最敏感频段 - 低通滤波平滑轨迹

6.5.3 紧急避障策略

紧急制动 vs 紧急转向：

判断准则：

制动距离：d_brake = v²/(2·μ·g)
转向距离：d_swerve ≈ 1.2·d_brake（考虑横向动力学）

决策逻辑：

计算各选项的碰撞风险
评估次生事故风险
选择总风险最小方案

路径规划算法：

紧急情况下的快速规划：

多项式紧急变道
预计算紧急轨迹库
基于采样的快速评估

AEB（自动紧急制动）集成：

触发条件：

TTC < 阈值（通常0.6-1.2秒）
规划器无可行解
驾驶员未响应警告

分级制动：

预警：声光提示
部分制动：-4 m/s²
全力制动：-8 m/s²

本章小结

决策、规划与控制是自动驾驶系统的核心，本章深入介绍了从高层决策到底层控制的完整技术链路。

关键概念回顾

分层架构：

行为层：FSM、决策树、学习方法
运动规划：图搜索、采样、优化
轨迹优化：成本设计、约束处理
车辆控制：横向、纵向、MPC

核心算法：

Hybrid A*：结合连续和离散的路径搜索
RRT*：渐进最优的采样规划
MPC：模型预测控制的实时优化
CBF：控制障碍函数的安全保证

关键公式：

轨迹优化目标：

J = ∫[L(x,u) + λ·g(x)]dt

MPC优化问题：

min Σ||x_k - x_ref||²_Q + ||u_k||²_R
s.t. x_{k+1} = f(x_k, u_k)

安全距离模型（RSS）：

d_safe = v·τ + v²/(2a_brake)

工程实践要点

实时性保证： - 分层规划降低计算复杂度 - 热启动和增量优化 - GPU并行加速
鲁棒性设计： - 多层安全机制 - 模型不确定性处理 - 故障降级策略
性能调优： - 场景自适应参数 - 在线学习和适应 - 多目标平衡

练习题

基础题

练习6.1：有限状态机设计

设计一个简单的高速公路驾驶FSM，包含车道保持、准备换道、执行换道三个状态。定义状态转换条件和相应的触发事件。

提示：考虑速度差、安全间距、转向灯等因素

参考答案

状态定义：

LANE_KEEPING：车道保持
PREPARE_LC：准备换道
EXECUTE_LC：执行换道

转换条件：

LANE_KEEPING → PREPARE_LC：
前车速度 < 自车期望速度 - 5 km/h
跟车距离 < 2秒时距
目标车道可用
PREPARE_LC → EXECUTE_LC：
目标车道安全窗口 > 4秒
无碰撞风险
转向灯已开启 > 1秒
EXECUTE_LC → LANE_KEEPING：
车辆中心进入目标车道
横向偏差 < 0.2m
航向角偏差 < 5°
任意状态 → LANE_KEEPING（中止条件）：
检测到紧急情况
目标车道突然被占用
驾驶员接管

练习6.2：Pure Pursuit控制器参数

车辆以20 m/s速度行驶，轴距L=2.7m，前视距离l_d=15m，前视点相对车辆航向角α=10°。计算所需的方向盘转角。

提示：使用Pure Pursuit公式δ = arctan(2L·sin(α)/l_d)

参考答案

给定参数：

L = 2.7 m
l_d = 15 m
α = 10° = 0.1745 rad
v = 20 m/s

计算： δ = arctan(2L·sin(α)/l_d) = arctan(2 × 2.7 × sin(10°) / 15) = arctan(2 × 2.7 × 0.1736 / 15) = arctan(0.0625) = 0.0624 rad = 3.57°

验证：前视距离是否合适推荐：l_d = k·v，其中k=0.5-1.5 当前：l_d/v = 15/20 = 0.75 ✓ 合理范围内

结论：需要3.57°的方向盘转角

练习6.3：MPC预测时域选择

某城市自动驾驶车辆采用MPC控制，控制频率20Hz。如何选择预测时域N和控制时域M？分析不同选择的影响。

提示：考虑计算复杂度、预测精度、系统响应

参考答案

控制周期：Δt = 1/20 = 0.05s

典型选择：

短时域：N=20, M=5 (预测1秒，控制0.25秒) - 优点：计算快，适合动态环境 - 缺点：缺乏长远规划能力
中时域：N=40, M=10 (预测2秒，控制0.5秒) - 优点：平衡计算和性能 - 缺点：中等计算负载
长时域：N=60, M=15 (预测3秒，控制0.75秒) - 优点：更好的预见性 - 缺点：计算量大，模型误差累积

影响分析：

预测时域T_p = N×Δt：需覆盖主要动态（2-3秒）
控制时域T_c = M×Δt：通常取T_p的1/3到1/4
复杂度：O(N³)用于QP求解
稳定性：N太小可能不稳定，太大误差累积

推荐：城市场景N=30-40，高速场景N=40-60

挑战题

练习6.4：轨迹优化成本函数设计

设计一个用于城市十字路口左转的轨迹优化成本函数。需要考虑：对向来车、行人、舒适性、交通效率。写出成本函数的数学表达式并解释各项权重的选择理由。

提示：考虑安全性、舒适性、效率的权衡

参考答案

成本函数设计：

J = J_safety + J_comfort + J_efficiency + J_rules

详细展开：

安全成本（权重最高）：

J_safety = w₁·Σexp(-d_i/σ) + w₂·Σ(1/TTC_j)²

d_i：到静态障碍物距离
TTC_j：与动态物体的碰撞时间
w₁=100, w₂=50（安全优先级最高）
σ=2m（影响范围参数）

舒适性成本：

J_comfort = w₃·∫a²dt + w₄·∫j²dt + w₅·∫(v²κ)²dt

a：加速度，j：加加速度，κ：曲率
w₃=1, w₄=0.1, w₅=2
左转时横向加速度更重要

效率成本：

J_efficiency = w₆·T + w₇·∫(v-v_ref)²dt

T：总时间
v_ref：期望速度(考虑路口限速)
w₆=5, w₇=0.5

规则成本：

J_rules = w₈·P_lane + w₉·P_signal

P_lane：车道线违反惩罚
P_signal：信号灯违反惩罚（硬约束）
w₈=20, w₉=∞（不可违反）

权重选择理由：

安全 >> 规则 > 舒适 > 效率
左转特殊性：增大横向加速度权重
动态调整：根据交通密度调整效率权重

实施考虑：

对向来车：通过TTC和间隙接受模型
行人：增大安全距离（d_ped > 3m）
分阶段优化：先优化进入路口，再优化转弯

练习6.5：紧急避障决策

车辆以80 km/h行驶，突然发现前方40m处有静止障碍物。路面摩擦系数μ=0.7，车道宽度3.5m。请分析紧急制动vs紧急转向的决策，并给出最优避障策略。

提示：计算制动距离和转向所需横向位移

参考答案

初始条件：

v₀ = 80 km/h = 22.2 m/s
距离 = 40 m
μ = 0.7
车道宽度 = 3.5 m

紧急制动分析：

最大减速度：a_max = μ·g = 0.7 × 9.8 = 6.86 m/s²
制动距离：d_brake = v₀²/(2a_max) = 22.2²/(2×6.86) = 35.9 m

结论：制动距离 < 40m，纯制动可避免碰撞 ✓

紧急转向分析：

最大横向加速度：a_y_max = μ·g = 6.86 m/s²
转向半径：R = v₀²/a_y_max = 22.2²/6.86 = 71.9 m
横向位移3.5m所需纵向距离：
使用几何关系：x = √(2Ry) = √(2×71.9×3.5) = 22.4 m

结论：22.4m < 40m，转向也可避免碰撞 ✓

最优策略选择：

考虑因素：

制动成功率：95%（简单可靠）
转向成功率：80%（需要精确控制）
次生事故风险：制动<转向
路面不确定性影响：制动更鲁棒

推荐策略：

首选：紧急制动（更安全可靠）
如制动不足：制动+转向组合
具体实施： - 0-20m：最大制动 - 20-30m：评估效果 - 30-40m：必要时轻微转向辅助

安全裕度分析：

制动裕度：(40-35.9)/40 = 10.3%
考虑反应时间0.5s：实际裕度降低
建议：立即执行制动，持续评估

练习6.6：MPC实时性优化

某自动驾驶系统的MPC控制器在复杂场景下求解时间达到80ms，超过50ms的实时性要求。请提出至少三种优化方案，并分析各方案的优缺点。

提示：考虑算法、实现、硬件多个层面

参考答案

当前问题：求解时间80ms > 要求50ms

方案1：算法层优化

减少预测时域： - N=40→30, M=10→8 - 优点：计算量降低约40% - 缺点：预测能力下降
简化车辆模型： - 双轨→单轨→运动学模型 - 优点：状态维度减少 - 缺点：高速精度下降
热启动优化： - 使用上一时刻解移位作为初值 - 优点：迭代次数减少50% - 缺点：场景突变时效果差

方案2：实现层优化

代码生成： - 使用ACADO/CasADi生成C代码 - 优点：效率提升3-5倍 - 缺点：灵活性降低
稀疏矩阵优化： - 利用问题结构稀疏性 - 优点：内存和计算都优化 - 缺点：实现复杂
早停策略： - 次优解满足要求即停止 - 优点：最坏情况有保证 - 缺点：性能略有损失

方案3：系统层优化

分层MPC： - 路径层5Hz + 速度层20Hz - 优点：整体计算量降低 - 缺点：解耦可能次优
显式MPC： - 离线计算，在线查表 - 优点：在线计算<1ms - 缺点：离线计算量巨大，存储需求高
GPU并行化： - CUDA实现矩阵运算 - 优点：大幅加速 - 缺点：功耗增加，部署复杂

推荐组合方案：

短期：热启动 + 早停（可立即实施）
中期：代码生成 + 稀疏优化
长期：分层架构 + GPU加速

预期效果：

热启动：80ms → 60ms
代码生成：60ms → 40ms
组合优化：< 40ms ✓

验证方法：

硬件在环(HIL)测试
蒙特卡洛场景验证
最坏情况分析

练习6.7：多智能体交互决策

在无保护左转场景中，需要穿越对向车流。对向有3辆车以不同速度接近，如何建模这个决策问题？设计一个基于博弈论的解决方案。

提示：考虑不同驾驶员的驾驶风格和意图

参考答案

问题建模：

场景参数：

自车：准备左转
对向车1：距离50m，速度15m/s，激进型
对向车2：距离80m，速度12m/s，正常型
对向车3：距离120m，速度10m/s，保守型

博弈论框架：

玩家定义： - 自车：策略空间{等待, 缓慢通过, 快速通过} - 对向车i：策略空间{保持速度, 减速让行, 加速通过}
收益函数设计：

U_ego = -w₁·T_wait - w₂·Risk - w₃·Discomfort
U_other = -w₄·T_delay - w₅·Brake - w₆·Risk

驾驶风格建模（SVO角度）： - 激进型：θ=15°（利己） - 正常型：θ=30°（平衡） - 保守型：θ=45°（利他）

求解方法：

Level-k推理：

Level-0: 随机行为
Level-1: 假设他人Level-0
Level-2: 假设他人Level-1
自车采用Level-2推理

顺序博弈求解：

for each gap in gaps:
    p_yield = compute_yield_probability(gap, driver_type)
    expected_utility = p_yield * U_pass + (1-p_yield) * U_wait
    if expected_utility > threshold:
        select_gap(gap)

意图识别更新：

P(intent|observation) ∝ P(observation|intent) × P(intent)

决策算法：

1. 初始化驾驶风格先验
2. 观察对向车行为3秒
3. 更新驾驶风格后验概率
4. 计算每个间隙的通过概率：
   - Gap1(车1-车2): P=0.3 (太小)
   - Gap2(车2-车3): P=0.7 (可行)
   - Gap3(车3后): P=0.9 (安全)
5. 选择期望效用最大的间隙
6. 执行时持续监控和调整

实施细节：

安全约束：

最小间隙时间 > 4秒
最大加速度 < 2.5m/s²
碰撞概率 < 0.001

适应性调整：

根据对向车反应更新模型
保守初始化，逐步学习
紧急中止机制

结论：选择Gap2，缓慢通过，同时准备应对车2可能的加速

常见陷阱与错误（Gotchas）

1. 决策层设计陷阱

问题：FSM状态爆炸

错误：为每个场景创建新状态
正确：使用分层FSM和参数化条件

问题：决策振荡

错误：状态转换条件设置过于敏感
正确：引入迟滞和时间过滤

2. 规划算法陷阱

问题：局部最优

错误：过度依赖局部搜索
正确：结合全局规划和局部优化

问题：动态障碍物处理

错误：静态规划+简单避障
正确：时空联合规划或预测轨迹考虑

3. 轨迹优化陷阱

问题：数值不稳定

错误：约束过紧或初值太差
正确：约束松弛和多阶段优化

问题：实时性不足

错误：每帧完全重新求解
正确：增量优化和热启动

4. 控制器设计陷阱

问题：模型失配

错误：低速模型用于高速
正确：增益调度或切换控制

问题：延迟补偿不足

错误：忽略执行器和通信延迟
正确：预测补偿和鲁棒设计

5. 安全性陷阱

问题：安全验证不充分

错误：只测试正常场景
正确：极限工况和故障注入测试

问题：过度保守

错误：安全裕度设置过大
正确：动态调整和风险评估

调试技巧

可视化工具： - 实时显示规划轨迹 - 成本函数各项贡献 - 约束满足情况
日志分析： - 记录决策过程 - 优化迭代历史 - 控制误差统计
仿真验证： - 单元测试各模块 - 集成测试完整流程 - 蒙特卡洛随机测试
性能分析： - 计算时间分解 - 内存使用追踪 - 瓶颈识别优化

第6章：决策、规划与控制

学习目标

6.1 行为决策层

6.1.1 有限状态机架构

6.1.2 基于规则的决策树

6.1.3 基于学习的决策方法

6.1.4 多智能体交互建模

6.2 运动规划

6.2.1 规划空间表示

6.2.2 基于图搜索的方法

6.2.3 基于采样的方法

6.2.4 基于优化的方法

6.3 轨迹优化

6.3.1 成本函数设计

6.3.2 约束处理

6.3.3 数值优化算法

6.4 车辆控制

6.4.1 横向控制

6.4.2 纵向控制

6.4.3 联合横纵向控制

6.4.4 执行器建模与补偿

6.5 安全性与舒适性设计

6.5.1 安全保障机制

6.5.2 舒适性指标

6.5.3 紧急避障策略

本章小结

关键概念回顾

工程实践要点

最新技术趋势

练习题

基础题

挑战题

常见陷阱与错误（Gotchas）

1. 决策层设计陷阱

2. 规划算法陷阱

3. 轨迹优化陷阱

4. 控制器设计陷阱

5. 安全性陷阱

调试技巧