Autonomous Driving PnC Mini-Stack — Stage 17 Refined v5

ROS-independent Prediction–Decision–Planning–Control online closed loop + deterministic scenario regression + controlled benchmark + risk-aware robustness refinement

22 个确定性交互场景 · 16 组基准评测套件 · 178 项测试 · 21.3k 行 Python

S 弯慢车超车（curved_left_overtake）：ego 在曲率道路上完成决策 → 换道 → 超车 → 回正。轨迹由闭环运行日志直接渲染，不是示意动画。

左：Cartesian 轨迹（含目标车轨迹与换道触发时刻）；右：同一段运行的 Frenet s-l 轨迹 —— 换道被表达为 l 从 0 平滑升至 3.6，道路曲率在 Frenet 空间被自然展平。这张图也是本项目"横向-纵向解耦"设计的直接证据。

前车急刹（lead_hard_brake，前车 -6 m/s²）：速度、加速度/jerk、TTC/车距、安全监督干预时刻。t≈1.1 s 触发 hard-brake 意图，t=1.1~1.5 s 出现 6 次安全干预，TTC 最低 4.5 s，最低车距 1.70 m，全程零碰撞。

同场景的轨迹视图：ego 保持在 lane 1 内完成制动，邻道车辆轨迹（T1/T5）同步渲染，用于核对"未换道"这一期望契约。

上述数字均可由 artifacts/benchmark/ 与 git commit 追溯。本项目为研究级完整闭环原型，不是量产自动驾驶软件，也未做 ISO 26262 / SOTIF / NCAP 认证。

上面这些数字来自确定性单次运行。为回答「这些结论有多大的统计支撑」，仓库额外提供两项证据。

① 蒙特卡洛统计 — tools/mc_stats.py，每场景 30 次独立运行（s0 σ=1 m、v0 σ=0.3 m/s、噪声种子逐次重采样，基准种子固定可复现），realistic_lqr（含 RTK 级定位噪声 + 0.12 s 执行器滞后）：

stop_and_go 的安全干预为 3.20 ± 2.66，但样本极值达 39 —— 均值不足以描述尾部风险。 这正是需要统计而非单次采样的直接理由，也是本仓库把它显式列出来的原因。

② 外部公开基线 — external_baseline_ablation，用 IDM（Intelligent Driver Model）跟驰模型替换纵向规划器，横向规划、控制器、安全监督完全不变：

结论不是「谁更好」，而是可量化的取舍：DP + 凸 QP 用 2~5 倍的规划算力换取更高的通行效率； IDM 以极低算力达到同等安全水平，但在三个场景中通行量都更低（更保守）。两侧的 safety supervisor 与 risk guard 均保持开启，唯一变量是纵向规划器。

③ 二维敏感性网格 — tools/sensitivity_grid.py，定位噪声 σ(x/y) × ST 不确定性增益，4×4 全组合（lead_hard_brake，所有 32 次运行通过契约）：

这张网格限定了一条此前只在单一维度上成立的结论。干净感知下，不确定性增益 0.10 确实降低 jerk （2.74 → 2.52，-8%，与仓库既有结论一致）；但一旦加入定位噪声，噪声项主导了舒适性 （σ 从 0 → 0.20 m，jerk 增长约 6.5 倍），而增益在 σ ≥ 0.05 m 后几乎不再产生差异。

所以正确的表述不是「增益无效」，而是「增益的标定只在干净感知下有意义」—— 这正是需要二维网格 而不是单因素扫描的理由。原 P1-3 结论由此获得一个明确的适用边界。

当前版本：0.19.0（P4-1：反应式交通流——交通参与者可按 IDM 控制律对同车道真实前车（含 ego）做闭环跟驰反应，新增 3 个反应式场景与 reactive_traffic_ablation 套件，预测模型的价值排序在非剧本交通上真实分化：CA 1.566 < CV 1.601 < LSTM 2.227 ADE）。上一版本 0.18.0（P3：横向分段 jerk 路径 QP——把横向规划升级为与纵向同构的"采样定拓扑 → QP 走廊内平滑"两级结构，换道 jerk RMS -32%；动力学自行车 plant 接入在线闭环，六状态动力学 LTV-MPC 与运动学/几何控制器在真实轮胎横向动力学下同场消融）

这个工程已经从最初的车辆模型与单算法 Demo，发展为一个可以在 macOS 原生 Python 环境运行的自动驾驶 PnC Mini-Stack。Stage 17 建立 Scenario / Benchmark 平台；Refined v1 修复了 Stop-and-Go deterministic collision、缺少 Lane-change Abort、MPC 纵向过激等问题；Refined v2 继续把安全处理前移到 nominal planning，让 Safety Supervisor 从“经常救场”变成更接近真正的最后独立防线。

项目定位：

Research / Teaching / Engineering Prototype

不是量产自动驾驶软件，也不是 ISO 26262 / SOTIF / NCAP 安全认证平台。

01  Kinematic Bicycle + Simulator
02  Reference Line + Frenet
03  Prediction: CV / CA / LSTM
04  Behavior Decision: Gap / TTC / Cost + FSM
05  Frenet Lateral Planning
06  ST Graph
07  DP + QP Speed Planning + Trajectory
08  PID + Pure Pursuit + Stanley
09  Kinematic LQR
10  Kinematic LTV-MPC
11  Dynamic Bicycle + Vehicle Lateral Dynamics
12  Dynamic LQR
13  Dynamic LTV-MPC
14  Layered Architecture + frozen optional ROS2 adapter
15  Multi-rate Runtime + Freshness + Health + Fallback
16  Real Online Closed-loop Replanning + Stitching
17  Scenario Library + Benchmark / Ablation Framework
17R Robustness: Safety Supervisor + Abort + Emergency Planning
17R2 Risk-aware nominal planning + bounded/resumable benchmark runner
P3-1 Lateral Piecewise-Jerk Path QP (sampling topology -> corridor QP smoothing)
P3-2 Dynamic-Bicycle Plant Closed Loop + 6-state Dynamic LTV-MPC Tracking
P4-1 Reactive IDM Traffic (actors close the world loop around the ego)

可以概括成：

01~13 = Algorithms
14    = Architecture
15    = Runtime Infrastructure
16    = Online PnC Closed Loop
17    = Scenario & Evaluation Platform
17R   = Failure-driven Robustness Refinement
17R2  = Earlier Risk Handling + Benchmark Reliability
P3    = Lateral Optimization + Dynamics-in-the-Loop Validation
P4    = Reactive World + Statistical Credibility

             Deterministic / Future External World
                         ↓
                  Tracked Objects
                         ↓
             CV / CA / optional LSTM
                         ↓
               Cost Decision + FSM
                         ↓
       Observed-Motion Planning Guard   ← measured acceleration
                         ↓
              Frenet Lateral Planning
                         ↓
                     ST Graph
                         ↓
          DP + dynamic following headway
                         ↓
             QP smoothing / risk bypass
                         ↓
                    Trajectory
                         ↓
           PP / Stanley / LQR / MPC
                         ↓
          Longitudinal Safety Supervisor
                         ↓
                   Vehicle Plant
                         ↓
                   Vehicle State
                         └──────────────→ replan

Stage 16 以后车辆每次运动都会反馈新状态，Prediction / Decision / Planning / Control 按不同频率持续运行，而不是“规划一次，执行到底”。

Refined v1 已经让 stop_and_go 从碰撞变成无碰撞，但 Benchmark 暴露出一个新的工程事实：

Safety Supervisor ON + legacy nominal planner
collision_events            = 0
safety_override_count       = 84
safety_unavoidable_count    = 77
emergency_brake_plan_count  = 9
emergency ST-unsafe         = 8

这意味着“最终没撞”很大程度上依赖 20 Hz Safety Supervisor，5 Hz nominal planner 仍然太晚理解前车急减速。

Refined v2 的目标不是删除 Safety Supervisor，而是重新分工：

Nominal Planner
  尽早识别风险并产生可执行减速轨迹
        ↓
Controller
        ↓
Independent Safety Supervisor
  只处理 planning tick 之间的快速风险和最后防线

CV / CA / LSTM 的原始输出和 ADE/FDE 评价保持不变。Planner 额外利用连续 Tracked Object 速度变化估计：

[ a_{obs}\approx \frac{v_k-v_{k-1}}{\Delta t} ]

只在危险方向增强 Prediction envelope：

• 前车明显减速：未来纵向位置取更靠后的一侧；
• 后车明显加速：未来纵向位置取更靠前的一侧；
• lateral prediction 仍来自原 Prediction model。

所以它是：

prediction output
      ↓
planning-only conservative envelope

而不是偷偷修改 Prediction benchmark。

核心文件：

src/pnc/application/planning/motion_guard.py

如果 AEB 实际施加：

[ a=-4m/s^2 ]

下一次 Planning 不再继续相信旧 trajectory 中的 nominal acceleration。Runtime 会把当前实际 control acceleration 传给 PlanningContext；若与旧轨迹相差太大，Stitcher 拒绝继续沿用旧 prefix，从 measured state 重新规划。

这样避免：

Safety 已强制刹车
      ↓
Planner 却继续从“未刹车”的旧轨迹状态出发

对位于障碍物后方的 Ego，Stage 17 在线模式可以增加：

[ s_{ego}+T_h v_{ego} \le s_{obs}^{lower}-d_{extra} ]

它是方向性的 following constraint，不是简单把 ST obstacle 前后都对称扩大。

DP 在 segment collision check 中使用动态 margin；QP 使用对应的线性不等式，因此仍保持线性 corridor constraint。

Stage 07 历史默认配置仍然是：

following_time_headway_sec = 0
following_extra_gap_m      = 0

不会破坏旧教学实验。

Time-headway 是更保守的期望跟车 envelope，不等于物理碰撞边界。

如果：

DP + desired headway infeasible

系统先降级到：

DP + original geometric ST collision constraints

只有连基础 ST topology 也无解，才进入 Emergency minimum-risk planning。

因此现在明确区分：

[ \text{desired following margin infeasible} \neq \text{no collision-free ST path} ]

当 observed front braking 已触发，或者 desired headway 已经被迫 relax 时，系统可以保留 DP 风险规避结果，而不强制 QP 再向 cruise-speed reference 平滑。

normal risk
  DP → QP

observed longitudinal risk
  DP → keep DP profile

这不是永久取消 QP，而是风险状态下的明确 degradation policy。

当前 planner_risk_guard_ablation：

核心结论有两层：

1. Risk-aware nominal planner 大幅降低了对最终 Safety layer 的依赖；
2. 不能因此删除 Safety Supervisor：关闭 Safety 后虽然这一 deterministic run 没产生 collision event，但最小 clearance 只有约 3.3 cm，并且 16 个 emergency plan 中 15 个仍与 ST occupancy 相交。

因此当前推荐结构仍然是：

[ \boxed{ Risk\text{-}aware\ Planning \rightarrow Controller \rightarrow Independent\ Safety\ Supervisor } ]

完整报告：

artifacts/benchmark/planner_risk_guard_ablation/REPORT.md

车辆停车后允许：

[ (x_k,y_k)=(x_{k+1},y_{k+1}) ]

因此 Cartesian cumulative path distance 是 non-decreasing，而不是必须 strictly increasing。

20 Hz AEB-style Application service，支持：

• observed lead braking；
• TTC / physical gap trigger；
• 10 Hz object snapshot 外推到 20 Hz control time；
• minimum modification braking search；
• unavoidable=True 的显式语义。

FSM 支持：

ABORT_LEFT
ABORT_RIGHT

早期执行阶段发现目标车道风险恶化时，可以回到原车道。

Abort 时：

• 禁止继续 stitch 旧危险 lane-change prefix；
• 使用 shorter feasible recovery path；
• safety recovery 允许更高 lateral acceleration envelope，但仍保留 curvature hard limit。

Nominal DP 真实无解、且行为 topology 允许纵向制动降低风险时，生成 non-reversing minimum-risk braking trajectory，并独立记录 ST 后验是否安全。

free_cruise
curved_free_cruise
high_curvature_cruise
curved_left_overtake
curved_hard_brake
nominal_left_overtake
right_overtake
both_sides_blocked
left_gap_opens
left_gap_closes_prepare
brief_left_gap_flicker
lead_hard_brake
stop_and_go
front_cut_in
front_cut_out
target_lane_rear_accelerates
accelerating_lead
object_dropout
planner_fault
idm_lead_chain
idm_cut_in_brake
idm_rear_follower

当前 scenario_screening：

[ \boxed{22/22\ explicit\ expectation\ PASS} ]

但 22/22 的含义只是：

当前 22 个 deterministic curated scenarios 的显式回归条件全部满足。

它绝不等于“安全率 100%”。例如：

• lead_hard_brake 仍有 7 次 Safety override；
• stop_and_go 仍有少量 sub-cycle Safety override；
• target_lane_rear_accelerates 需要 1 次 Abort 和 10 次 Safety override；
• planner_fault 仍按设计产生 5 次 planning failure，再由 Runtime 恢复。

详见 docs/SCENARIO_CATALOG.md。

项目不默认做全部笛卡尔积，而使用 controlled ablation。

scenario_screening
controller_ablation
prediction_ablation
speed_ablation
fsm_ablation
stitching_ablation
safety_ablation
planner_risk_guard_ablation
mpc_tuning_ablation
uncertainty_ablation
realism_ablation
lateral_ablation
dynamic_plant_ablation
reactive_traffic_ablation
external_baseline_ablation
selected_stacks

为了真正比较：

DP only
vs
DP + QP

该 suite 明确关闭：

Safety Supervisor
Planning Risk Guard
Dynamic headway overlay

否则风险状态下 QP 可能被 bypass，实验就不再是纯 DP/QP 对比。

当前结果：

当前结论不再是“MPC tracking 一定最好”。在新的 risk-aware closed loop 下，LQR lateral RMSE 略低，而 balanced MPC 显著降低 jerk 并取得更高 progress。它体现的是闭环 trade-off，而不是单指标排名。

当前 balanced profile：

r_accel        = 0.50
rd_accel       = 12.0
max_accel_rate = 1.0 m/s³

Stage 10 Core 默认参数没有被改写；Runtime profile 显式选择 tuning。

CV   ADE ≈ 0.587 m
CA   ADE ≈ 0.645 m
LSTM ADE ≈ 1.424 m

LSTM 在本仓库中的定位必须说清楚：这是一个接口与训练流程的演示实现，不是性能主张。

在这个配置下 CV 仍是最稳健 baseline，这对「3 秒预测时域 + 恒定速度假设成立」的场景是合理结果： 学习方法要在该任务上取得优势，需要真实数据与交互/地图上下文，超出当前工程范围。 把这条负结果写进仓库，是为了让预测模块的评测口径保持诚实 —— evaluation_pass 只衡量闭环契约， 不能反过来当作「LSTM 有效」的证据。

Raw semantic switches = 2
FSM semantic switches = 0

说明 Stitching 主要改善 repeated-replan continuity，而不保证每个 tracking metric 都更低。

采样晶格定拓扑后，分段 jerk 路径 QP 在候选 ±0.35 m 走廊内平滑（OSQP，与纵向 DP→QP 同构）：

换道纵向 jerk RMS -31.9%，安全指标持平或更好；lead_hard_brake（不换道）逐位零回归，curved_free_cruise 舒适指标持平。同批次配对测量下 latqp 相对 lqr 的规划 p95 开销约 +0.7 ms（横向 QP 经 Hessian 对角化构造 + OSQP 实例缓存优化，单次独立求解约 6 ms，5 Hz 预算内）。

把 plant 从运动学自行车换成动力学自行车（线性轮胎 + μ·Fz 饱和），在陡 S 弯（最小半径约 83 m，16 m/s 下约 3.1 m/s² 横向）上对比控制器：

两个结论：① 模型失配的代价随"模型-时域"组合变化——运动学模型的 0.5 s MPC 在动力学 plant 陡弯上跑偏出车道，2.0 s 时域或动力学模型都能解决；② 在动力学 plant 上， kinematic LQR 靠反馈反而最稳，动力学 MPC 的价值在 curved_left_overtake 上体现为 +8.6% 进度（76.8 vs 70.7 m）。dyn_mpc 的 FAIL 按仓库方法论保留可见，不隐藏。

交通参与者可按 IDM 控制律对同车道真实前车（含 ego）闭环跟驰反应（IDMReactiveSpec），世界不再是对手剧本。idm_lead_chain（ego → IDM 跟随车 → 慢卡车链）上的预测消融：

结论：预测模型的价值排序不是普适常数，而是交通统计特性的函数——这正是反应式交通流消融相对剧本消融的增量信息。idm_rear_follower（后车反应式跟随 ego 制动）在 risk-guard 栈下 32 次安全干预、2 次应急规划仍保持零碰撞，realistic_lqr（RTK 噪声 + 0.12 s 滞后）同样通过。

collision events / samples
min geometric clearance
min TTC
safety override
safety unavoidable
emergency plan
emergency ST-unsafe

progress
mean / min / final speed
final lane

acceleration RMS / max
jerk RMS / max
steering RMS / max
steering-rate RMS
lateral acceleration RMS / max

lateral RMSE
heading RMSE
speed RMSE
tracking evaluated samples

behavior switches
lane-change commands
lane-change abort episodes
planning success / failure
QP→DP fallback
stitched plans
actual-acceleration feedback cycles
risk-guard plans / guarded objects
headway relaxation
risk QP bypass

online ADE
online FDE
number of evaluated predicted trajectories

planning p50 / p95 / p99 / max latency
deadline misses
module failures
MPC solver success / iterations

Stage 17 原始 batch runner 曾经把太多重型数值 Case 留在一个 Python 进程中，可能造成 NumPy/SciPy allocator/solver 状态跨 Case 累积。

Refined v2 使用：

Long Suite
   ↓
Fresh Batch Worker (default ≤ 5 cases)
   ↓
run case → release bundle → gc
   ↓
Fresh next Batch Worker

这样同时限制：

• 单个数值进程的生命周期；
• 大对象历史的累计；
• 连续启动大量重型子进程的压力。

如果希望严格 one-process-per-case：

python examples/benchmark.py \
  --suite scenario_screening \
  --chunk-size 1

python examples/benchmark.py \
  --suite scenario_screening \
  --resume

中间结果原子写入：

artifacts/benchmark/<suite>/.partial_results.json

Checkpoint 包含当前 suite config 的 SHA-256 signature；如果 StackConfig / Case 定义发生变化，旧 checkpoint 会拒绝复用，避免无意混入过期数据。成功完成 suite 后 partial checkpoint 自动删除。

算法主线完全不依赖 ROS2：

python3 -m venv .venv
source .venv/bin/activate
python -m pip install -e ".[all]"

测试：

pytest

查看场景 / Stack / Suite：

python examples/benchmark.py --list

单 Case：

python examples/benchmark.py \
  --scenario stop_and_go \
  --stack risk_guard_lqr

核心 Benchmark：

make benchmark-screening-resume
make benchmark-controller
make benchmark-prediction
make benchmark-speed
make benchmark-fsm
make benchmark-stitching
make benchmark-safety
make benchmark-planner-risk
make benchmark-mpc-tuning

本项目是 ROS-independent 架构：算法主线完全不依赖 ROS2。早期验证阶段的冻结 ROS2 适配骨架（ros2_ws/）已经移除；分层适配边界见 docs/ARCHITECTURE.md 与 docs/LAYERED_ARCHITECTURE_GUIDE.md。

当前开发主线：

macOS native Python
  → PnC algorithms
  → online runtime
  → scenarios / benchmark
  → robustness / performance / richer road geometry

当前回归口径：

fast suite (unit + architecture)  = 153 PASS
integration tests                 =  25 PASS
------------------------------------------
total                             = 178 PASS

数值型 integration tests 使用 每个 integration 文件一个 fresh pytest 进程 的方式执行（make test-integration），避免重新制造长生命周期 numerical-worker 累积问题。最重的 robustness 回归拆成 stop_and_go / lane_abort / mpc_profile / risk_guard 四个独立文件。

静态与架构检查（make audit，与 CI 同一套）：

Architecture boundary check            PASS
Python source files                     200+
parse errors                              0
TODO/FIXME/HACK                            0
bare except                                0
star imports                               0
mutable literal function defaults          0
compileall                               PASS

历史版本的回归日志保留在 git 历史中；CI 在每次 push 时执行同一套 fast suite + architecture + static audit。

1. lead_hard_brake 仍有较多 Safety override，说明 5 Hz planning 对极快 braking onset 还可继续提前化；
2. stop_and_go 已显著降低 Safety 依赖，但仍有少量 planning tick 之间的 AEB intervention；
3. Lane-change Abort 主要覆盖 early return，超过 progress threshold 仍缺 late escape / minimum-risk lateral strategy；
4. target_lane_rear_accelerates 仍需要较多 Safety intervention，recovery comfort 也不理想；
5. Observed Motion Guard 目前主要依据纵向加速度，不是 probabilistic intent / uncertainty model；
6. 当前 dynamic headway 是工程启发式，不是 RSS / CBF / formal safety proof；
7. Python + SciPy planner 仍属于原型实时性能；
8. 场景仍以直线结构化多车道为主；
9. Localization noise / delay、actuator lag、低附着、vehicle parameter mismatch 尚未进入默认 Scenario Matrix；
10. Dynamic Bicycle / Dynamic MPC 尚未成为 Stage 17 Benchmark Matrix 的默认 end-to-end plant/control 组合。

先看 docs/README.md 文档索引；如果按主线学习：

1. docs/ARCHITECTURE.md
2. docs/ONLINE_CLOSED_LOOP_GUIDE.md
3. docs/SCENARIO_BENCHMARK_GUIDE.md
4. docs/PLANNING_RISK_REFINEMENT.md
5. docs/ROBUSTNESS_REFINEMENT.md
6. docs/SCENARIO_CATALOG.md
7. docs/BENCHMARK_METHODOLOGY.md
8. docs/ALGORITHM_COMPARISON.md
9. docs/CODE_REVIEW_01_17.md

当前版本可以描述为：

一个 ROS2-independent、支持多频率在线 Replanning 的自动驾驶 PnC Mini-Stack，具备 deterministic scenario regression、controlled algorithm ablation、trajectory stitching、lane-change abort、risk-aware longitudinal planning、minimum-risk degradation、独立 AEB-style safety supervision，以及可断点续跑的 benchmark harness。

下一步优先级仍然不是继续堆控制算法，而是：

1. 继续降低 hard-brake / recovery 场景的 Safety intervention
2. 做 curved-road + noise + delay + actuator dynamics scenarios
3. 做 planning profiling / solver optimization
4. 再考虑更复杂道路语义或外部 simulator