重卡泊车 · NN加速版

神经网络热启动 · IPOPT精修 · 亚秒级泊车轨迹规划 · 垂直泊车

← 返回重卡泊车主页｜原版 Hybrid A* 演示

速度 2.0×

牵引车朝向 θ₁ 0°

挂车朝向 θ₂ 0°

精确输入： X Y θ₁ θ₂

牵引车朝向 θ₁

—

挂车朝向 θ₂

—

铰接角 β

—

求解时间

—

碰撞数

—

位置误差

—

轨迹长度

—

换挡次数

—

NN推理

—

💡 拖拽卡车设置起始位置，调节滑块设置朝向

NN加速泊车实验

本演示采用神经网络（NN）热启动加速铰接车辆泊车轨迹规划。NN 模型在毫秒级预测初始轨迹（紫色虚线），随后 IPOPT 精修保证无碰撞和终态精度（蓝色实线）。相比原始同伦延拓求解器，求解时间从 15-50 秒降至 < 5 秒。

NN 热启动 + IPOPT 精修求解器

本演示采用 神经网络热启动（NN Warm-Start） 策略，用训练好的 NN 模型快速生成初始轨迹猜测，替代原始版本中耗时的 4 阶段前向仿真 + 多阶段同伦延拓。IPOPT 从 NN 预测出发做 2 阶段 mini-homotopy 精修即可收敛到无碰撞解。

1. 两阶段求解管线

浏览器（拖拽 + 滑块）→ Nginx /api2/ → Flask :5781 → NN推理 + IPOPT精修 → JSON 轨迹 → Canvas 动画

阶段	方法	典型耗时	输出	可视化
① NN 预测	神经网络前向推理	5-20 ms	粗略轨迹（物理可行，可能碰撞）	紫色虚线
② IPOPT 精修	2阶段 mini-homotopy	3-7 s	无碰撞最优轨迹	蓝色实线

相比原版的同伦延拓（4-6 阶段，15-50 秒），NN 热启动只需 2 阶段精修（3+4 秒预算），因为 NN 预测已经提供了高质量的初始猜测——接近最终解的吸引域。

2. 与原版的对比

特性	原版（同伦延拓）	NN 加速版
初始猜测	4阶段前向仿真	NN 前向推理
IPOPT 求解	4-6 阶段同伦	2 阶段 mini-homotopy
A* 搜索	有（Hybrid A* 粗搜）	无（NN 替代）
典型求解时间	15-50 s	< 5 s
NN 推理时间	—	5-20 ms
终态精度	θ < 0.5°	θ < 0.5°（相同）
API 端点	/api/solve (:5780)	/api2/solve (:5781)

为什么 NN 热启动如此有效？同伦延拓的主要时间消耗在从松弛问题逐步收敛到严格约束的过程中——每阶段 IPOPT 都需要数百次迭代。NN 模型通过离线训练学会了从起点位姿直接映射到接近最优的轨迹形状，使 IPOPT 只需做局部微调（数十次迭代）即可收敛。

2.5 IPOPT 精修的 2 阶段 mini-homotopy

虽然 NN 提供了高质量初始猜测，但单次 IPOPT 求解仍难以同时兼顾终端精度和碰撞约束。实际实现采用 2 阶段 mini-homotopy——通过逐步施加约束，让 IPOPT 先收敛终端姿态，再解决碰撞：

阶段	终端权重 tw	碰撞权重 cw	预算	目标
Stage 0	10⁴	0（无碰撞项）	3s	先把末端姿态拉到位，忽略碰撞 → 更小的 NLP，更快收敛
Stage 1	10⁶	10⁶（硬约束）	4s	cw ≥ 10⁶ 触发硬碰撞约束，从 Stage 0 解出发微调，得到零碰撞解

Stage 0 的解作为 Stage 1 的初始猜测（热启动链）。y_front_max 根据起点距离自适应（远点 8.0、高近点 7.5、低点 99.0 不限制），限制牵引车前端高度防止摆入上方墙。

神经网络模型详解

上面介绍了"NN 热启动"的整体管线。这一节深入到模型内部：输入是什么、输出是什么、网络长什么样、怎么训练的。目标：让有基础编程经验但不懂深度学习的开发者也能看懂。

3. 模型的输入与输出

这个神经网络做的事情可以用一句话概括：给定卡车的起始位姿，直接输出 80 步的驾驶操作序列。

	维度	含义	范围
输入起始位姿	x ∈ ℝ	牵引车后轴横坐标（米）	[-30, 4]
	y ∈ ℝ	牵引车后轴纵坐标（米）	[-20, 4]
	θ₁ ∈ ℝ	牵引车朝向角（弧度）	通常 [−30°, 30°]
	θ₂ ∈ ℝ	挂车朝向角（弧度）	通常 [−30°, 30°]
输出控制序列	δ₁,δ₂,...,δ₈₀ (80 个值)	每步的方向盘转角（弧度） `δ > 0` = 右转，`δ < 0` = 左转	[−0.40, 0.40] rad ≈ [−23°, 23°]
输出控制序列	v₁,v₂,...,v₈₀ (80 个值)	每步的行驶速度（米/秒） `v > 0` = 前进，`v < 0` = 倒车	[−2.0, 2.0] m/s

所以模型的数学形式就是一个函数：

f : ℝ⁴ → ℝ¹⁶⁰ 输入：[x, y, θ₁, θ₂]（4 个数）输出：[δ₁,v₁, δ₂,v₂, ..., δ₈₀,v₈₀]（160 个数）

关键设计决策：为什么不直接输出轨迹（车的位置），而是输出"操作指令"？
如果让 NN 直接输出轨迹（即 80 步的位置 x,y,θ₁,θ₂），这些位置之间可能不满足物理约束——比如某一步车身角度突然跳变 90°，这在现实中是不可能的（卡车不可能瞬移）。

输出操作指令（方向盘 + 速度），再用 RK4 数值积分把它"翻译"成轨迹，保证轨迹一定满足物理运动学方程。这是本项目的核心设计原则之一，也是 IPOPT 精修能成功的前提——IPOPT 的约束方程里要求每一步都满足动力学，NN 提供的初始猜测也必须满足动力学，两者才能顺利衔接。

4. 网络结构详解：ResNet MLP

模型采用带残差连接（Residual Connection）的多层感知机（MLP）。之所以不用 Transformer 或 CNN，原因很简单：输入只有 4 个数（固定场景，障碍物不变），用处理变长序列的 Transformer 是杀鸡用牛刀。MLP + 残差连接是最简单且充分的架构。

4.1　全局架构一览

先看全景——数据从 4 维输入流经整个网络，最终变成 160 维输出的过程：

输入层
  [x, y, θ₁, θ₂]    ← 4 个数：卡车起始位姿
       │
       ▼  Linear(4 → 256) + GELU        ← 把 4 维"铺开"到 256 维
  [256]               ←  此后一直在 256 维空间里运算
       │
       ▼  ResBlock #1  ─────────────────────┐
       │  ResBlock #2                        │
       │  ResBlock #3   每个块内部有残差连接     │ 5 个块叠在一起
       │  ResBlock #4   （输入直接跳到输出相加）  │ = 10 层有效深度
       ▼  ResBlock #5  ─────────────────────┘
  [256]               ←  特征提取完毕
       │
       ▼  Linear(256 → 160)            ← 从 256 维"解码"回 160 维
  [160]
       │
       │   reshape 成 (2, 80) 的矩阵
       │     第 0 行 = [δ₁, δ₂, ..., δ₈₀]  ← 80 步方向盘转角
       │     第 1 行 = [v₁, v₂, ..., v₈₀]  ← 80 步速度
       │
       ▼  tanh 缩放到物理范围
  δᵢ = tanh(·) × 0.40 rad    ← 方向盘不超过 ±23°
  vᵢ = tanh(·) × 2.0 m/s     ← 速度不超过 ±2 m/s
       │
       ▼
输出: 80 步控制指令 [δ₁,v₁, δ₂,v₂, ..., δ₈₀,v₈₀]
    

层	类型	输入→输出	参数量	作用（通俗解释）
① 输入映射	Linear + GELU	4 → 256	1,280	把 4 个数字"铺开"成 256 个数字。就像把一张低清图片放大——虽然信息没变多，但给后续层提供了更大的操作空间
② ResBlock × 5	残差块（详见 4.2）	256 → 256	657,920 (131,584/块)	网络的核心。5 个块串联，在高维空间里逐步提取"从起点到操作序列"的映射关系。每块有两层全连接 + 残差连接
③ 输出映射	Linear	256 → 160	41,120	把 256 维特征"解码"成 160 个控制数值（80 步 × 2 控制量）。就像把高维特征重新"压缩"回物理世界
④ 输出约束	tanh 缩放	160 → 160	0	把原始数值"捏"到物理范围内（方向盘 ±23°，速度 ±2m/s）。无参数，纯数学运算
总计			≈ 700,320	≈ 0.7M 参数，模型文件 2.8 MB

4.2　残差块（ResBlock）—— 网络的核心组件

整个网络 0.7M 参数中有 94% 都在这 5 个残差块里。它是整个模型最重要的部分。下面拆开讲。

一个残差块内部的数据流：

输入 x (256 维)
  │
  ├──→ fc₁: Linear(256 → 256)     # 第一层全连接，256×256+256 = 65,792 参数
  │         │
  │         ▼
  │      GELU 激活                  # 引入非线性（见 4.3 详解）
  │         │
  │         ▼
  │      LayerNorm                  # 归一化，防止数值越练越大（见 4.4 详解）
  │         │
  │         ▼
  │      fc₂: Linear(256 → 256)     # 第二层全连接，65,792 参数
  │         │
  │         ▼  得到 h (256 维)
  │
  └──→  x + h                      ★ 残差连接：输入直接跳过两层，加到输出上
            │
            ▼
         GELU 激活                   # 最后再来一次非线性
            │
            ▼
       输出 (256 维)                 # 传给下一个 ResBlock

  每块参数 = 65,792 × 2 = 131,584
  5 块总计 = 657,920
    

① 残差连接（x + h）—— 为什么要让输入"跳过"两层？

先理解问题：如果把 10 层全连接直接堆起来（没有残差连接），训练时会出现梯度消失——反向传播的误差信号从输出层往回传，每经过一层就缩小一点，传到第 1 层时已经接近于 0，导致靠近输入的层几乎学不到东西。网络越深，问题越严重。

残差连接的解法很巧妙：让输入 x 直接加到输出上，相当于在网络内部开了一条"高速公路"：

# 没有残差连接（传统网络）：
output = fc₂(GELU(fc₁(x)))       # 信号必须穿过每一层

# 有残差连接（ResNet）：
output = GELU( x + fc₂(LN(GELU(fc₁(x)))) )
                 ↑
          这个 + 是关键

# 反向传播时，梯度可以走两条路：
∂L/∂x = ∂L/∂output × (1 + ∂fc₂(...)/∂x)
                                ↑              ↑
                           直通梯度      穿过层的梯度
                           （恒为 1，不衰减）

# "1" 这一项保证了不管网络多深，梯度总能无损传回输入层
    

这个想法来自何恺明 2015 年的 ResNet 论文（获 CVPR 最佳论文奖）。他最初是为了训练 152 层的超深网络——我们这里只有 10 层有效深度，问题没那么严重，但残差连接仍然带来了明显的训练稳定性和收敛速度提升。

直观点理解：残差连接让每个块学的是"输入和输出之间的差值"（即残差 residual），而不是从零开始学完整的映射。学"修正量"比学"完整答案"容易得多。如果某个块的工作其实很简单，它可以把 fc₂ 的权重训练到接近 0，输出就约等于输入——相当于这个块"跳过"了自己。网络自动决定哪些层需要做重活，哪些层只需微调。

② GELU 激活函数 —— 为什么不用 ReLU？

激活函数的作用是给网络引入非线性。没有激活函数，不管堆多少层全连接，整体仍然是一个线性变换（矩阵乘法的复合还是矩阵乘法）——网络就退化成了一个线性回归模型，啥也学不到。

# 三种主流激活函数对比：

ReLU(x)  = max(0, x)              # 负数直接归零，正数不变
                                    # 缺点：x=0 处不可导，负数梯度恒为 0（"死神经元"）

Sigmoid(x) = 1 / (1 + e⁻ˣ)        # 挤压到 (0,1)
                                    # 缺点：两端饱和，梯度消失

GELU(x)   = x · Φ(x)              # Φ(x) = 正态分布累积分布函数
          ≈ 0.5x(1 + tanh[√(2/π)(x + 0.044715x³)])   # 工程近似

# GELU 的行为：
#   x 很大时 → GELU(x) ≈ x     （跟 ReLU 一样，几乎不过渡）
#   x 很小时 → GELU(x) ≈ 0     （跟 ReLU 一样，抑制负值）
#   x ≈ 0 时 → GELU(x) 平滑过渡 （不像 ReLU 有硬折角，处处可导）
    

简单说，GELU = "平滑版 ReLU"。它在 0 附近没有硬折角（处处可导，梯度更稳定），同时正区间保持线性（不会像 Sigmoid 那样饱和）。GELU 是 GPT、BERT、GPT-4 等几乎所有现代大模型的默认激活函数。在本项目中选用 GELU 是因为可微 RK4 积分器已经引入了大量的 sin/cos/tan 运算，激活函数的平滑性有助于整体梯度的稳定传播。

③ LayerNorm —— 为什么要归一化？

训练过程中，全连接层的输出数值可能越来越大（或越来越小），导致激活函数进入饱和区、梯度消失。LayerNorm 在每个样本的特征维度上做标准化：

# 对一个 256 维向量 h 做 LayerNorm：

μ = mean(h)                        # 求这 256 个数的均值
σ = std(h)                         # 求标准差
h_normalized = (h - μ) / (σ + ε)   # 减均值、除标准差（ε=1e-5 防除零）

# 效果：把 256 个数拉回均值≈0、方差≈1 的分布
# 不管前一层输出多炸裂，经过 LN 后数值回到稳定区间

# 注意：LN 还有可学习的缩放参数 γ 和偏移 β（共 512 个参数）
output = γ · h_normalized + β      # 网络自己决定归一化的"力度"
    

这跟 BatchNorm 的区别：BatchNorm 是对一个 batch 内的同个特征做统计（依赖 batch size），LayerNorm 是对同个样本的所有特征做统计（跟 batch size 无关）。小数据集 + 小 batch（本项目 batch=32）场景下 LayerNorm 更稳定。Transformer 里也是用 LayerNorm。

4.3　输入层和输出层的设计哲学

输入层：Linear(4 → 256) —— "升维"。

4 维输入太"窄"了。如果直接在 4 维空间做运算，网络能表达的模式非常有限。把 4 维映射到 256 维，就像把一张黑白小图放大成彩色大图——虽然信息量没变多，但每个原始维度的影响被"展开"到了 256 个维度上，后续层有更大的空间去做复杂的非线性组合。这个技巧叫特征扩展（Feature Expansion），是深度学习的标配操作。

类比：原始 4 维 = 4 个旋钮的调音台 256 维 = 256 个旋钮的调音台输入的 4 个数（x, y, θ₁, θ₂）就像 4 个音源 Linear(4→256) 把每个音源分配到 256 个通道上残差块在 256 个通道上做精细调音最后 Linear(256→160) 把 256 个通道"混缩"成 160 个输出

输出层：Linear(256 → 160) + tanh 缩放 —— "解码" + "物理约束"。

# 输出层的两步处理：

# Step 1: Linear 把 256 维特征解码成 160 个原始数值
raw = Linear(features)              # raw.shape = (batch, 160)

# Step 2: reshape 成 (batch, 2, 80)，再分通道做 tanh 缩放
raw = raw.reshape(batch, 2, 80)     # 0=方向盘, 1=速度

delta = torch.tanh(raw[:, 0]) * 0.40   # δ ∈ [-0.40, 0.40] rad
v     = torch.tanh(raw[:, 1]) * 2.0     # v ∈ [-2.0, 2.0] m/s

controls = torch.stack([delta, v], dim=1)  # (batch, 2, 80)
    

tanh 把任何实数压到 (−1, 1) 区间，再乘以物理极限就保证输出永远不会越界。这个设计把物理约束刻进了模型结构——训练时不需要额外的惩罚项来限制方向盘角度和速度，模型天然就不可能输出物理上不可能的控制指令。

4.4　参数都在哪里？

0.7M 参数听上去不多，但理解它们的分布有助于判断模型是否"合理"：

组件	参数量	占比	直观理解
输入映射 Linear(4→256)	1,280	0.2%	"门卫"——只负责把 4 个入口分配到 256 个通道，参数极少
ResBlock × 5（含 LayerNorm）	659,460	94.1%	"主力"——5 个块 × (2 层全连接 + 1 层 LN)，几乎所有学习容量都在这里
输出映射 Linear(256→160)	41,120	5.9%	"出口"——把 256 维特征压缩到 160 维输出，参数适中
总计	≈ 700,320	100%	残差块占绝对主体，符合"输入输出层薄、中间层厚"的设计原则

这个参数量合理吗？
训练集只有 680 个样本，0.7M 参数意味着参数量是样本量的 1000 倍——这在传统统计学习看来严重过拟合。但深度学习有残差连接 + LayerNorm + 权重衰减 + 梯度裁剪四重正则化手段，实际验证集 loss（0.328）与训练集基本持平，没有明显过拟合。关键在于输入空间只有 4 维、且目标函数（轨迹优化）是连续光滑的——4 维空间的信息量不足以撑起更复杂的模型，0.7M 已经是足够的容量。

4.5　为什么不用 Transformer / CNN / RNN？

架构	擅长场景	本项目为什么不需要
Transformer	变长序列（NLP、代码）、需要注意力机制	输入固定 4 维、输出固定 160 维，没有"序列"概念，注意力机制无用武之地
CNN	图像、网格数据（有局部空间结构）	输入是一维向量 [x, y, θ₁, θ₂]，不存在"相邻像素"的空间结构供卷积核提取
RNN / LSTM	时序预测（逐步递推）	80 步输出是并行生成的（一次性算出全部 160 维），不是逐步递推，循环结构反而引入不必要的序列依赖
ResNet MLP ✅	固定维度的函数逼近	4→160 的映射就是一个固定维度的函数，MLP 是最直接的函数逼近器。残差连接提供深度，足以表达这个复杂映射

升级路径：如果将来泊车场景变成动态障碍物（比如旁边的车位有车开进开出），输入就不再是固定的 4 维了——需要把障碍物位置也作为输入。这时可以考虑 Conditional Neural Process（CNP） 或 Transformer（编码障碍物集合 + 解码控制序列）。但当前固定场景下，MLP 是最优选择——最简单、最快、最充分。

5. 训练数据

模型不凭空学习，它需要大量"起点 → 最优操作序列"的样本。这些样本是用现有的慢速求解器（纯 IPOPT 同伦延拓，每个约 21 秒）离线生成的。

项目	数值	说明
总生成尝试	1000 个随机起点	x∈[−20, 2], y∈[−2, 3], θ₁∈±20°, θ₂∈±15°
有效样本	756 个	成功率为 75.6%（IPOPT 未收敛或碰撞的样本被丢弃）
生成总耗时	≈ 6.3 小时	平均每样本 21 秒
训练/验证集	680 / 76 个	90% / 10% 随机划分
每条样本	start(4D) + U(2×80) + X(4×81)	起点 + 控制序列 + 对应轨迹

每个样本的生成过程：随机一个起点 → 跑完整 IPOPT 同伦求解 → 如果成功（0 碰撞、角度误差 < 0.5°）就保存 (start, controls, trajectory) 三元组为 .npz 文件。

6. 损失函数：三重约束

训练时，损失函数由三个部分加权求和：

L = L_control + λ_traj × L_trajectory + λ_coll × L_collision

损失项	公式	作用	权重
L_control 控制序列误差	`MSE(δ_pred, δ_true) + MSE(v_pred, v_true)`	让 NN 的输出直接逼近专家求解器的操作序列	1.0（基准）
L_trajectory 轨迹误差	`MSE(RK4(controls), trajectory_true)`	把 NN 输出的控制序列用 RK4 积分成轨迹，跟真实轨迹对比	λ_traj = 0.5
L_collision 碰撞惩罚	`Σ max(0, −d_min)²` 对所有车身点	惩罚车身（12 个检测点）与障碍物的重叠	λ_coll = 1.0

L_trajectory 是最精妙的部分——它通过一个可微分的 RK4 积分器把控制序列翻译成轨迹。这个积分器用 PyTorch 实现，梯度可以通过反向传播穿过整个积分过程（80 步），直接从"轨迹偏离"反向指导"控制序列"的优化。这就是学术界说的"端到端学习（End-to-End Learning）"。

碰撞检测：卡车车身被采样为 12 个关键点（挂车 8 个 + 牵引车 4 个），每个点对 3 个障碍物（左车、右车、墙壁）做 AABB 有符号距离判断。距离为负说明车身点在障碍物内部，用 hinge loss 的平方惩罚。

7. 训练过程

超参数	值	说明
优化器	AdamW	带动量 + 权重衰减的自适应优化器
学习率	1e-3 → 0	CosineAnnealing 余弦退火，从 0.001 平滑衰减到 0
权重衰减	1e-4	L2 正则化，防止过拟合
梯度裁剪	max_norm = 1.0	防止梯度爆炸
Batch Size	32	每批 32 个样本
Epochs	500（最佳 epoch 225）	早停：保存验证集 loss 最低的模型
训练设备	CPU（无 GPU）	0.7M 参数的 MLP 在 CPU 上训练完全可行
最终验证 loss	0.328	综合三项损失的加权值

8. NN 预测的实际表现

一个有趣的现象：NN 单独的预测并不完美，但已经足够好。

指标	NN 单独预测	NN + IPOPT 精修
推理耗时	5-20 ms	5-20 ms + 2-4 s
位置精度	较好（接近目标）	精确（误差 < 0.5m）
角度精度 θ₂	较差（可能偏 90°）	精确（误差 < 0.5°）
碰撞	有（几处碰擦）	0 碰撞
运动学可行性	✅ 满足（RK4 保证）	✅ 满足

这正是"NN 热启动"策略的精髓：不需要 NN 做到完美，只需要它提供一个"大方向正确"的起点。IPOPT 作为成熟的非线性求解器，从这个起点出发只需做局部微调（2 阶段，共 7 秒预算），就能修正所有碰撞和角度偏差。这比从零开始做 4-6 阶段同伦延拓（15-50 秒）快了一个数量级。

学术背景：这种"NN 提供初始猜测 → 传统优化器精修"的范式在运动规划领域有坚实的理论基础。PILOT（IROS 2021）用类似方法在 Cessi 双臂实验中实现了 7× 加速；Cauligi 等人（RSS 2021）证明了这种范式对通用轨迹优化问题都有效。本项目的创新点在于将其应用于铰接车辆的垂直泊车这一高约束非凸问题。