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` 内一致。
@@ -1,5 +1,7 @@
 using System;
 using System.Collections.Generic;
+using System.Diagnostics;
+using System.IO;
 using System.Threading;
 using EMPlannerVerificationHost;
 using MultiWheelC.TrajectoryPlanning.CoarsePath;
@@ -19,6 +21,69 @@ internal static class LateralIntegrationChecks
        VerifiesLateralPlannerDelegatesToTheSequentialOptimizer();
    }

+    public static void RunRealOsqp()
+    {
+        foreach (LateralScenario scenario in CreateRealOsqpScenarios())
+        {
+            LateralPlanningResult first = new LateralPlanner(new OsqpNativeSolver()).Plan(scenario.Input,
+                CancellationToken.None);
+            LateralPlanningResult second = new LateralPlanner(new OsqpNativeSolver()).Plan(scenario.Input,
+                CancellationToken.None);
+            VerifyRealScenarioResult(scenario, first);
+            VerifyRealScenarioResult(scenario, second);
+            VerifyDeterministicResult(scenario.Name, first, second);
+            if (scenario.RequiresSeedConnectedInterval)
+            {
+                for (int index = 0; index < first.Path.Points.Count; index++)
+                    Verification.True(first.Path.Points[index].L <= -0.05d + 1e-10d,
+                        scenario.Name + " remains in the seed-connected obstacle corridor");
+            }
+        }
+    }
+
+    public static void RunRealOsqpInCleanPluginBundle()
+    {
+        string pluginDirectory = Path.Combine(Path.GetTempPath(), "em-planner-lateral-real-" + Guid.NewGuid().ToString("N"));
+        try
+        {
+            Directory.CreateDirectory(pluginDirectory);
+            foreach (string sourcePath in Directory.GetFiles(AppContext.BaseDirectory))
+                File.Copy(sourcePath, Path.Combine(pluginDirectory, Path.GetFileName(sourcePath)), false);
+            string nativeSource = Path.GetFullPath(Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot", "ThirdParty",
+                "OSQP", "win-x64", "osqp.dll"));
+            Verification.True(File.Exists(nativeSource), "pinned OSQP DLL is available for the real lateral bundle");
+            File.Copy(nativeSource, Path.Combine(pluginDirectory, "osqp.dll"), false);
+
+            var startInfo = new ProcessStartInfo
+            {
+                FileName = Path.Combine(pluginDirectory, "EMPlannerVerificationHost.exe"),
+                Arguments = "lateral-real-osqp-probe",
+                WorkingDirectory = pluginDirectory,
+                UseShellExecute = false,
+                CreateNoWindow = true,
+                RedirectStandardOutput = true,
+                RedirectStandardError = true,
+            };
+            using (var process = new Process { StartInfo = startInfo })
+            {
+                process.Start();
+                string standardOutput = process.StandardOutput.ReadToEnd();
+                string standardError = process.StandardError.ReadToEnd();
+                process.WaitForExit();
+                if (process.ExitCode != 0 || standardOutput.IndexOf("PASS lateral-real-osqp", StringComparison.Ordinal) < 0)
+                {
+                    throw new InvalidOperationException("Real lateral OSQP clean-plugin probe exited " + process.ExitCode + ": " +
+                        standardError + standardOutput);
+                }
+            }
+        }
+        finally
+        {
+            if (Directory.Exists(pluginDirectory))
+                Directory.Delete(pluginDirectory, true);
+        }
+    }
+
    private static void VerifiesValidatedCandidateSurvivesLaterTimeout()
    {
        LateralPlanningInput input = CreateInput();
@@ -143,6 +208,113 @@ internal static class LateralIntegrationChecks
        Verification.Equal(EmPlanningStatus.Success, result.Status, "lateral planner returns SQP success");
    }

+    private static IReadOnlyList<LateralScenario> CreateRealOsqpScenarios()
+    {
+        return new[]
+        {
+            CreateScenario("straight-empty-forward", TravelDirection.Forward, 0d, EmTerminalType.Goal, -0.3d, 0.3d, 0d, false),
+            CreateScenario("straight-empty-reverse", TravelDirection.Reverse, 0d, EmTerminalType.Goal, -0.3d, 0.3d, 0d, false),
+            CreateScenario("gentle-curve", TravelDirection.Forward, 0.05d, EmTerminalType.Goal, -0.3d, 0.3d, 0d, false),
+            CreateScenario("static-obstacle-narrowing", TravelDirection.Forward, 0d, EmTerminalType.RollingSafetyStop,
+                -0.3d, -0.05d, -0.10d, true),
+            CreateScenario("gear-switch-terminal", TravelDirection.Forward, 0d, EmTerminalType.GearSwitch, -0.3d, 0.3d, 0d, false),
+            CreateScenario("rolling-terminal", TravelDirection.Forward, 0d, EmTerminalType.RollingSafetyStop, -0.3d, 0.3d, 0d, false),
+        };
+    }
+
+    private static LateralScenario CreateScenario(string name, TravelDirection direction, double geometricCurvature,
+        EmTerminalType terminal, double corridorMinimum, double corridorMaximum, double seedL,
+        bool requiresSeedConnectedInterval)
+    {
+        double[] stations = { 0d, 0.5d, 1d, 1.5d, 2d };
+        var points = new List<SmoothedPathPoint>(stations.Length);
+        var intervals = new List<LateralInterval>(stations.Length);
+        var seed = new List<FrenetProjection>(requiresSeedConnectedInterval ? stations.Length : 0);
+        for (int index = 0; index < stations.Length; index++)
+        {
+            double referenceS = stations[index];
+            double travelYaw = geometricCurvature * referenceS;
+            double x = Math.Abs(geometricCurvature) <= 1e-12d ? referenceS : Math.Sin(travelYaw) / geometricCurvature;
+            double y = Math.Abs(geometricCurvature) <= 1e-12d ? 0d : (1d - Math.Cos(travelYaw)) / geometricCurvature;
+            double vehicleYaw = direction == TravelDirection.Forward ? travelYaw : travelYaw - Math.PI;
+            points.Add(new SmoothedPathPoint(x, y, vehicleYaw, vehicleYaw, referenceS, direction, geometricCurvature,
+                direction == TravelDirection.Forward ? geometricCurvature : -geometricCurvature, 0d, 1d, false,
+                SmoothedPathPointSource.Anchor));
+            intervals.Add(new LateralInterval(referenceS, corridorMinimum, corridorMaximum, seedL));
+        }
+        var segment = new DirectionSegmentView(0, direction, points,
+            new ReferenceBoundary(0, 0d, EmBoundaryType.None, 0d),
+            new ReferenceBoundary(0, 2d, terminal == EmTerminalType.GearSwitch ? EmBoundaryType.GearSwitchApproach : EmBoundaryType.Goal,
+                2d), 0d);
+        if (requiresSeedConnectedInterval)
+        {
+            for (int index = 0; index < stations.Length; index++)
+                seed.Add(new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, stations[index]), seedL, 0d, 0d));
+        }
+        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+        configuration.Scheduling.SolverTimeoutSeconds = 1d;
+        configuration.Validation.SpatialToleranceMeters = 1e-5d;
+        configuration.Validation.KinematicTolerance = 1e-5d;
+        var vehicle = new VehicleParameters
+        {
+            LengthMeters = 0.1d,
+            WidthMeters = 0.1d,
+            SafetyMarginMeters = 0d,
+            MaximumCurvaturePerMeter = 1d,
+        };
+        return new LateralScenario(name, new LateralPlanningInput(segment, new StaticCorridor(intervals),
+            new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 0d), seedL, 0d, 0d), terminal, vehicle,
+            configuration, seed), requiresSeedConnectedInterval);
+    }
+
+    private static void VerifyRealScenarioResult(LateralScenario scenario, LateralPlanningResult result)
+    {
+        Verification.True(result.Status == EmPlanningStatus.Success || result.Status == EmPlanningStatus.SuccessWithFallback,
+            scenario.Name + " is solved or has a documented fallback: " + result.FailureReason);
+        LateralPath path = result.Path ?? throw new InvalidOperationException(scenario.Name + " returned no lateral path.");
+        Verification.True(path.IsIndependentlyValidated, scenario.Name + " path is independently validated");
+        Verification.Equal(scenario.Input.ReferenceStations.Count, path.Points.Count, scenario.Name + " point count");
+        double maximumCurvature = scenario.Input.Vehicle.MaximumCurvaturePerMeter.GetValueOrDefault();
+        Verification.True(maximumCurvature > 0d, scenario.Name + " has a maximum vehicle curvature");
+        for (int index = 0; index < path.Points.Count; index++)
+        {
+            LateralPathPoint point = path.Points[index];
+            LateralInterval interval = scenario.Input.Corridor.Stations[index];
+            Verification.True(point.L >= interval.MinimumL - 1e-10d && point.L <= interval.MaximumL + 1e-10d,
+                scenario.Name + " remains in corridor at station " + index);
+            Verification.True(Math.Abs(point.VehicleCurvature) <= maximumCurvature + 1e-10d,
+                scenario.Name + " respects vehicle curvature at station " + index);
+        }
+        Verification.NearlyEqual(scenario.Input.ReferenceStations[scenario.Input.ReferenceStations.Count - 1],
+            path.Points[path.Points.Count - 1].ReferenceS, scenario.Name + " ends at exact ReferenceS anchor");
+    }
+
+    private static void VerifyDeterministicResult(string name, LateralPlanningResult first, LateralPlanningResult second)
+    {
+        Verification.Equal(first.Status, second.Status, name + " deterministic status");
+        LateralPath firstPath = first.Path ?? throw new InvalidOperationException(name + " first path was missing.");
+        LateralPath secondPath = second.Path ?? throw new InvalidOperationException(name + " second path was missing.");
+        Verification.Equal(firstPath.Points.Count, secondPath.Points.Count, name + " deterministic point count");
+        for (int index = 0; index < firstPath.Points.Count; index++)
+            ComparePoint(firstPath.Points[index], secondPath.Points[index], name + " deterministic point " + index);
+    }
+
+    private static void ComparePoint(LateralPathPoint left, LateralPathPoint right, string name)
+    {
+        double[] leftValues =
+        {
+            left.ReferenceS, left.PathS, left.L, left.DL, left.DDL, left.DDDL, left.X, left.Y, left.VehicleYaw,
+            left.GeometricCurvature, left.VehicleCurvature, left.VehicleCurvatureDerivative,
+        };
+        double[] rightValues =
+        {
+            right.ReferenceS, right.PathS, right.L, right.DL, right.DDL, right.DDDL, right.X, right.Y, right.VehicleYaw,
+            right.GeometricCurvature, right.VehicleCurvature, right.VehicleCurvatureDerivative,
+        };
+        for (int index = 0; index < leftValues.Length; index++)
+            Verification.True(Math.Abs(leftValues[index] - rightValues[index]) <= 1e-10d, name + " value " + index);
+    }
+
    private static LateralPlanningInput CreateInput()
    {
        var points = new List<SmoothedPathPoint>
@@ -233,4 +405,18 @@ internal static class LateralIntegrationChecks
        }
        throw new InvalidOperationException("Expected single-variable lateral trust-region row was not found.");
    }
+
+    private sealed class LateralScenario
+    {
+        public LateralScenario(string name, LateralPlanningInput input, bool requiresSeedConnectedInterval)
+        {
+            Name = name;
+            Input = input;
+            RequiresSeedConnectedInterval = requiresSeedConnectedInterval;
+        }
+
+        public string Name { get; }
+        public LateralPlanningInput Input { get; }
+        public bool RequiresSeedConnectedInterval { get; }
+    }
 }
@@ -8,9 +8,10 @@ internal static class Program
    {
        if (args.Length != 1 || (args[0] != "foundation" && args[0] != "segmentation" && args[0] != "frenet" &&
            args[0] != "corridor" && args[0] != "optimization" && args[0] != "osqp" && args[0] != "osqp-loader" && args[0] != "osqp-probe" &&
-            args[0] != "all-foundation" && args[0] != "lateral-model" && args[0] != "lateral-integration"))
+            args[0] != "all-foundation" && args[0] != "lateral-model" && args[0] != "lateral-integration" &&
+            args[0] != "lateral-real-osqp" && args[0] != "lateral-real-osqp-probe" && args[0] != "lateral-all"))
        {
-            Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|optimization|osqp|osqp-loader|all-foundation|lateral-model|lateral-integration");
+            Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|optimization|osqp|osqp-loader|all-foundation|lateral-model|lateral-integration|lateral-real-osqp|lateral-all");
            return 2;
        }

@@ -50,16 +51,26 @@ internal static class Program
            {
                MultiWheelC.TrajectoryPlanning.EMPlanner.OsqpChecks.RunProbe();
            }
-            if (args[0] == "lateral-model")
+            if (args[0] == "lateral-model" || args[0] == "lateral-all")
            {
                MultiWheelC.TrajectoryPlanning.EMPlanner.LateralModelChecks.Run();
                Console.WriteLine("PASS lateral-model");
            }
-            if (args[0] == "lateral-integration")
+            if (args[0] == "lateral-integration" || args[0] == "lateral-all")
            {
                MultiWheelC.TrajectoryPlanning.EMPlanner.LateralIntegrationChecks.Run();
                Console.WriteLine("PASS lateral-integration");
            }
+            if (args[0] == "lateral-real-osqp-probe")
+            {
+                MultiWheelC.TrajectoryPlanning.EMPlanner.LateralIntegrationChecks.RunRealOsqp();
+                Console.WriteLine("PASS lateral-real-osqp");
+            }
+            if (args[0] == "lateral-real-osqp" || args[0] == "lateral-all")
+            {
+                MultiWheelC.TrajectoryPlanning.EMPlanner.LateralIntegrationChecks.RunRealOsqpInCleanPluginBundle();
+                Console.WriteLine("PASS lateral-real-osqp");
+            }
            return 0;
        }
        catch (Exception exception)