From 4d83ed2036a17129b23ce55cd5fa4b19ad3efd90 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Tue, 4 Aug 2026 08:52:05 +0800 Subject: [PATCH] test: verify lateral LS scenarios --- .../Lateral/SequentialConvexOptimizer.cs | 2 +- .../ParkrobTrajplanner/EMPlanner/README.md | 47 +++++ .../LateralIntegrationChecks.cs | 186 ++++++++++++++++++ .../EMPlannerVerificationHost/Program.cs | 19 +- 4 files changed, 249 insertions(+), 5 deletions(-) diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/SequentialConvexOptimizer.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/SequentialConvexOptimizer.cs index 3693481..846aec1 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/SequentialConvexOptimizer.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/SequentialConvexOptimizer.cs @@ -41,7 +41,7 @@ public sealed class SequentialConvexOptimizer } LateralCandidate iterate = CreateInitialIterate(input); - var warmStart = Array.Empty(); + var warmStart = new double[new LateralVariableLayout(input.ReferenceStations.Count).VariableCount]; LateralPath lastValidatedPath = null; double previousObjective = 0d; bool hasPreviousObjective = false; diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md index 1c38ed5..fa7ac22 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md @@ -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` 内一致。 diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/LateralIntegrationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/LateralIntegrationChecks.cs index 030ae09..04f20da 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/LateralIntegrationChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/LateralIntegrationChecks.cs @@ -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 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(stations.Length); + var intervals = new List(stations.Length); + var seed = new List(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 @@ -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; } + } } diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs index 1f1c4e6..2b5b2d6 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs @@ -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)