test: verify lateral LS scenarios

2026-08-04 08:52:05 +08:00
parent f19df53f73
commit 4d83ed2036
4 changed files with 249 additions and 5 deletions
@@ -41,7 +41,7 @@ public sealed class SequentialConvexOptimizer
        }

        LateralCandidate iterate = CreateInitialIterate(input);
-        var warmStart = Array.Empty<double>();
+        var warmStart = new double[new LateralVariableLayout(input.ReferenceStations.Count).VariableCount];
        LateralPath lastValidatedPath = null;
        double previousObjective = 0d;
        bool hasPreviousObjective = false;
@@ -167,3 +167,50 @@ dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerifi
 PASS osqp-solve
 PASS osqp-loader
 ```
+
+## LS 横向 SQP
+
+LS 只对单个 `DirectionSegmentView` 的 `ReferenceS` 站点求解；`l > 0` 在前进和倒车时
+都表示行驶方向左侧。每个有 `N` 个站点的问题使用连续区间的变量布局：
+
+```text
+l[0..N-1], dl[0..N-1], ddl[0..N-1], dddl[0..N-2]
+```
+
+相邻站点之间按实际 `ds = ReferenceS[i+1]-ReferenceS[i]` 精确满足三阶积分关系：
+
+```text
+ddl[i+1] = ddl[i] + ds*dddl[i]
+dl[i+1]  = dl[i] + ds*ddl[i] + 0.5*ds^2*dddl[i]
+l[i+1]   = l[i] + ds*dl[i] + 0.5*ds^2*ddl[i] + ds^3*dddl[i]/6
+```
+
+每轮 QP 将以下项目作为硬约束：静态走廊、最大横向偏移、以当前迭代为中心且半径不超过
+`0.05 m` 的 `l` 信赖域、`1-referenceK*l >= 0.20`、`dl`/`ddl`/`dddl` 上限、起始
+`l` 与 `dl`，以及线性化的车辆曲率约束。`Goal` 和 `GearSwitch` 末端额外强制
+`l_end=0`、`dl_end=0`；`RollingSafetyStop` 不添加这两个等式，而是使用软终端回归代价。
+
+目标函数采用 OSQP 的 `0.5*x'P*x + q'x` 形式。所有平方残差先除以相应物理尺度的平方，
+再乘权重：`l` 使用最大横向偏移，`dl`、`ddl`、`dddl` 分别使用对应导数上限，曲率使用
+车辆最大曲率，曲率变化使用 `max(1, max |dk/ds|)`。代价覆盖参考线、航向、二阶导、三阶导、
+线性化曲率、曲率变化、上一轨迹种子及滚动终端；走廊安全绝不软化为代价。
+
+`SequentialConvexOptimizer` 最多运行五轮，通过 `IQpSolver` 取得完整 primal 向量并将它用作
+下一轮 warm start。每个解都先由完整 Frenet 公式重建为世界坐标，再由独立验证器复核走廊、
+起点/终端、导数、分母、曲率、有限值和严格递增弧长。只有该复核通过的深拷贝候选才能保留。
+后续超时、取消或失败不会发布未验证的最后求解器向量：若已有候选则返回
+`SuccessWithFallback`，否则返回最具体的失败状态。
+
+`ReferenceS` 是 LS 的独立变量，不能被当作行驶距离。重建后以世界坐标相邻弦长重新累计
+`PathS`，因此输出 `PathS[0]=0` 且严格递增；这个实际几何 `PathS` 才是后续纵向规划可消费
+的距离契约。
+
+可重复横向验证：
+
+```powershell
+dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- lateral-all
+```
+
+该门禁依次验证 LS 模型、脚本化 SQP 状态机，以及在干净复制 plugin bundle 中运行的真实 OSQP
+固定场景：前进/倒车直线、缓弯、静态障碍收窄的种子连通走廊、换向终端和滚动终端。每个真实
+场景运行两次，状态、点数和全部数值输出必须在 `1e-10` 内一致。