V3.1 轻量精修 + 闭环跟踪架构
V3.1 是 V3（障碍物感知 NN + 3 阶段同伦精修）的轻量化改进版。核心思路：用参数化 NLP 预编译消除 CasADi 重建开销，用单阶段求解替代 3 阶段同伦，用前端闭环跟踪补偿轻量化带来的精度损失。
1. 三层架构总览
浏览器（拖拽起点 + 编辑障碍物）→ Nginx /api3_1/ → Flask :5786
┌─ ① NN 热启动 (~10ms) ──────────────────────────────────────┐
│ 复用 V3 的 parking_mlp_v3.pth（只读加载） │
│ 输入: start(4D) + obstacles(25D) = 29D │
│ 输出: 80 步控制序列 → RK4 积分 → 初始轨迹 │
│ 可视化: 紫色虚线 │
│ │
├─ ② 轻量单阶段 IPOPT (~1.2s) ──────────────────────────────┤
│ 参数化 NLP：起点+障碍物作为运行时参数（预编译一次） │
│ 单阶段: tw=10⁶, 硬碰撞约束(coll_step=2), 50 iter max │
│ max_wall_time=1.2s（V3 是 1+1.5+2=4.5s 三阶段） │
│ 无同伦兜底 → 有碰撞则拒绝（前端仍可显示） │
│ 可视化: 蓝色实线（规划轨迹） │
│ │
├─ ③ 闭环 MPC 跟踪（前端，实时） ────────────────────────────┤
│ 动画播放时，不是简单回放规划轨迹，而是： │
│ 1. 计算当前卡车与最近航点的横向偏差 e_y 和航向偏差 e_θ │
│ 2. PD 控制器计算修正转向角 δ_correct │
│ 3. 用修正后的轨迹驱动卡车动画 │
│ 4. 每帧检查碰撞 → 碰撞则停车 │
│ 可视化: 绿色虚线（实际跟踪轨迹） │
└─────────────────────────────────────────────────────────────┘
2. 参数化 NLP 预编译（核心加速）
V3 的最大性能瓶颈不是 IPOPT 迭代本身，而是 CasADi 每次调用 ca.nlpsol() 时的 NLP 符号编译（~1.6s）。V3.1 将起点状态和障碍物坐标提取为运行时参数（ca.MX.sym('p', ...)），NLP 在启动时编译一次（3.4s），之后每次 solve 只需传入参数值。
| 项目 | V3（每次重建） | V3.1（参数化预编译） |
|---|
| NLP 编译 | 1.6s × 每次调用 | 3.4s × 仅启动时一次 |
| IPOPT 迭代 | 1-3s（3阶段） | 1.2s（单阶段，50 iter） |
| 碰撞约束步长 | coll_step=1（每步检查） | coll_step=2（隔步检查，约束数减半） |
| 总延迟 | 4.5-15s | 1.2-2.0s |
| 碰撞率 | 0%（同伦兜底） | 0%（默认场景，硬约束） |
# V3: 每次调用重建 NLP（慢）
def solve_v3(start, obstacles):
w, f, g, ... = _build_nlp_v3(start, ...) # 1.6s 符号编译
solver = ca.nlpsol('solver', 'ipopt', nlp, opts) # 每次新建
res = solver(x0=w0, ...)
# V3.1: 参数化 NLP，启动时编译一次（快）
def build_parametric_nlp(): # 启动时调用一次
p_start = ca.MX.sym('p_start', 4) # 起点参数
p_boxes = ca.MX.sym('p_boxes', 20) # 障碍物参数
# ... NLP 表达式引用 p_start, p_boxes ...
solver = ca.nlpsol('solver', 'ipopt', nlp, opts)
return solver # 缓存
def solve_v31(start, obstacles):
solver(x0=w0, p=start+boxes, ...) # 只传参数，无重编译
3. 闭环 MPC 跟踪（前端 PD 控制器）
这是 V1-V4 全都没有的创新。后端返回 81 个航点 [x, y, θ₁, θ₂] 构成的规划轨迹，前端动画播放时执行一个简化的纯跟踪（Pure Pursuit）控制器，模拟闭环纠偏：
1. 取当前卡车位置，找最近的规划航点索引 k
2. 前瞻 lookahead=3 个航点，取 target = path[k + 3]
3. 计算横向偏差 e_y 和航向偏差 e_θ：
e_y = (target.x − current.x) × sin(current.θ) − (target.y − current.y) × cos(current.θ)
e_θ = normalize(target.θ − current.θ)
4. PD 控制器输出修正转角：
δ_correct = clamp(Kp_lat × e_y + Kp_theta × e_θ, −0.15, 0.15)
Kp_lat = 0.5, Kd_lat = 0.1, Kp_theta = 2.0
5. 用 δ_correct 叠加到规划航向，驱动卡车动画
6. 每帧检查碰撞 → 碰撞则停车显示警告
设计意图：闭环跟踪是纯视觉演示层面的，不涉及后端实时通信。它模拟了真实自动驾驶系统中的 MPC 跟踪控制器行为——即使规划轨迹完美，执行过程中也会有偏差，需要闭环纠偏。蓝色实线是规划轨迹，绿色虚线是带闭环纠偏的实际跟踪轨迹。
4. 实时碰撞监测与停车
动画过程中，每一帧对跟踪轨迹的卡车位置进行碰撞检测（使用与后端相同的 AABB 有符号距离方法，12 个车身采样点，margin=0.15m）。如果检测到碰撞：
function checkCollisionJS(x, y, th1, th2, obstacles) {
var bodyPts = getBodyPoints(x, y, th1, th2);
for (var p of bodyPts) {
for (var o of obstacles) {
var hw = o.w/2, hh = o.h/2;
if (p.x > o.cx-hw-margin && p.x < o.cx+hw+margin &&
p.y > o.cy-hh-margin && p.y < o.cy+hh+margin)
return true;
}
}
return false;
}
// 动画循环中：
if (checkCollisionJS(trackX, trackY, trackTh1, trackTh2, obstacles)) {
isAnimating = false;
setStatus('⚠️ 检测到碰撞风险，已停车', 'error');
}
5. V3 vs V3.1 对比
| 特性 | V3 | V3.1 |
|---|
| NLP 构建 | 每次调用重建 | 参数化预编译（启动一次） |
| IPOPT 阶段 | 3 阶段 mini-homotopy | 单阶段 |
| IPOPT 预算 | 1 + 1.5 + 2 = 4.5s | 1.2s |
| 最大迭代 | 500 | 50 |
| 碰撞约束步长 | coll_step=1 | coll_step=2 |
| 同伦兜底 | ✓（有碰撞时回退到完整同伦） | ✗（有碰撞直接拒绝或返回） |
| 闭环跟踪 | ✗ | ✓ 前端 PD 纯跟踪 |
| 碰撞监测 | 后端评估 | 前端实时检测+停车 |
| 总延迟 | 4.5-15s | 1.2-2.0s |
| NN 模型 | parking_mlp_v3.pth | 同上（只读复用） |
| 端口 | :5782 | :5786 |
| API 路径 | /api3/ | /api3_1/ |
为什么 V3.1 更快？V3 的延迟主要来自两个地方：① CasADi NLP 符号编译（1.6s × 3 阶段 = 4.8s），② IPOPT 迭代本身（每个阶段 0.5-1s）。V3.1 消除了①（参数化预编译），削减了②（单阶段 50 iter），同时用 coll_step=2 将碰撞约束数减半。总延迟从 4.5-15s 降至 1.2-2.0s。
精度权衡：V3.1 的单阶段 IPOPT 在大多数场景下能达到 0 碰撞（得益于 NN 热启动的高质量初始猜测），但在极端障碍物布局下可能不如 V3 的 3 阶段同伦稳健。V3.1 不做同伦兜底——这是有意的速度-精度权衡。前端闭环跟踪部分补偿了这个精度损失。