feat: optimize lateral paths with SQP

2026-08-04 08:46:24 +08:00
parent f5c69c252b
commit f19df53f73
5 changed files with 583 additions and 5 deletions
@@ -8,11 +8,24 @@ namespace EMPlannerVerificationHost;

 internal sealed class FakeQpSolver : IQpSolver
 {
-    private readonly QpSolveResult _result;
+    private readonly Queue<QpSolveResult> _results;
+    private readonly List<QuadraticProgram> _problems = new List<QuadraticProgram>();
+    private readonly List<IReadOnlyList<double>> _warmStarts = new List<IReadOnlyList<double>>();

    public FakeQpSolver(QpSolveResult result)
+        : this(new[] { result })
    {
-        _result = result ?? throw new ArgumentNullException(nameof(result));
+    }
+
+    public FakeQpSolver(IEnumerable<QpSolveResult> results)
+    {
+        if (results == null)
+            throw new ArgumentNullException(nameof(results));
+        _results = new Queue<QpSolveResult>();
+        foreach (QpSolveResult result in results)
+            _results.Enqueue(result ?? throw new ArgumentException("Fake solver results cannot contain null values.", nameof(results)));
+        if (_results.Count == 0)
+            throw new ArgumentException("At least one fake solver result is required.", nameof(results));
        LastWarmStart = Array.Empty<double>();
    }

@@ -22,6 +35,12 @@ internal sealed class FakeQpSolver : IQpSolver

    public IReadOnlyList<double> LastWarmStart { get; private set; }

+    public IReadOnlyList<QuadraticProgram> Problems => new ReadOnlyCollection<QuadraticProgram>(_problems);
+
+    public IReadOnlyList<IReadOnlyList<double>> WarmStarts => new ReadOnlyCollection<IReadOnlyList<double>>(_warmStarts);
+
+    public int SolveCallCount => _problems.Count;
+
    public QpSolveResult Solve(QuadraticProgram problem, QpSolverSettings settings, IReadOnlyList<double> warmStart,
        CancellationToken cancellationToken)
    {
@@ -34,6 +53,10 @@ internal sealed class FakeQpSolver : IQpSolver
                copy.Add(warmStart[index]);
        }
        LastWarmStart = new ReadOnlyCollection<double>(copy);
-        return _result;
+        _problems.Add(LastProblem);
+        _warmStarts.Add(LastWarmStart);
+        if (_results.Count == 0)
+            throw new InvalidOperationException("Fake solver was called more often than its scripted result sequence.");
+        return _results.Dequeue();
    }
 }
@@ -0,0 +1,236 @@
+using System;
+using System.Collections.Generic;
+using System.Threading;
+using EMPlannerVerificationHost;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+internal static class LateralIntegrationChecks
+{
+    public static void Run()
+    {
+        VerifiesValidatedCandidateSurvivesLaterTimeout();
+        VerifiesInvalidVectorsAndInaccurateResidualsNeverBecomeFallbacks();
+        VerifiesTrustRegionWarmStartAndOuterIterationLimit();
+        VerifiesCancellationAndTimeoutWithoutCandidate();
+        VerifiesLateralPlannerDelegatesToTheSequentialOptimizer();
+    }
+
+    private static void VerifiesValidatedCandidateSurvivesLaterTimeout()
+    {
+        LateralPlanningInput input = CreateInput();
+        double[] valid = CreatePrimal(input, 0.02d);
+        var solver = new FakeQpSolver(new[]
+        {
+            Result(QpSolveStatus.Solved, valid, 10d),
+            Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 10d),
+        });
+
+        LateralPlanningResult result = new SequentialConvexOptimizer(solver).Optimize(input, CancellationToken.None);
+
+        Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
+            "timeout after an independently validated candidate returns fallback success");
+        LateralPath fallbackPath = result.Path ?? throw new InvalidOperationException("Fallback path was not returned.");
+        Verification.True(fallbackPath.IsIndependentlyValidated,
+            "fallback path remains independently validated");
+        Verification.NearlyEqual(0.02d, fallbackPath.Points[1].L,
+            "first valid candidate remains the fallback path");
+    }
+
+    private static void VerifiesInvalidVectorsAndInaccurateResidualsNeverBecomeFallbacks()
+    {
+        LateralPlanningInput input = CreateInput();
+        double[] valid = CreatePrimal(input, 0.02d);
+        double[] invalid = CreatePrimal(input, 0.40d);
+
+        var solver = new FakeQpSolver(new[]
+        {
+            Result(QpSolveStatus.Solved, valid, 10d),
+            Result(QpSolveStatus.Solved, invalid, 9d),
+            Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 9d),
+        });
+        LateralPlanningResult preserved = new SequentialConvexOptimizer(solver).Optimize(input, CancellationToken.None);
+        Verification.Equal(EmPlanningStatus.SuccessWithFallback, preserved.Status,
+            "invalid solved vector does not discard an earlier fallback");
+        Verification.NearlyEqual(0.02d, preserved.Path.Points[1].L,
+            "invalid solved vector does not replace the fallback candidate");
+
+        var inaccurateResidual = new FakeQpSolver(new[]
+        {
+            Result(QpSolveStatus.SolvedInaccurate, valid, 10d, 2e-5d, 0d),
+            Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 10d),
+        });
+        LateralPlanningResult rejectedResidual = new SequentialConvexOptimizer(inaccurateResidual).Optimize(input,
+            CancellationToken.None);
+        Verification.Equal(EmPlanningStatus.SolverTimedOut, rejectedResidual.Status,
+            "SolvedInaccurate above strict residual threshold is rejected");
+        Verification.True(ReferenceEquals(null, rejectedResidual.Path),
+            "rejected inaccurate result does not publish a path");
+
+        var inaccurateGeometry = new FakeQpSolver(new[]
+        {
+            Result(QpSolveStatus.SolvedInaccurate, invalid, 10d, 0d, 0d),
+            Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 10d),
+        });
+        LateralPlanningResult rejectedGeometry = new SequentialConvexOptimizer(inaccurateGeometry).Optimize(input,
+            CancellationToken.None);
+        Verification.Equal(EmPlanningStatus.SolverTimedOut, rejectedGeometry.Status,
+            "SolvedInaccurate still requires full independent lateral validation");
+    }
+
+    private static void VerifiesTrustRegionWarmStartAndOuterIterationLimit()
+    {
+        LateralPlanningInput input = CreateInput();
+        var trustSolver = new FakeQpSolver(new[]
+        {
+            Result(QpSolveStatus.Solved, CreatePrimal(input, 0.02d), 10d),
+            Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 10d),
+        });
+        new SequentialConvexOptimizer(trustSolver).Optimize(input, CancellationToken.None);
+
+        var layout = new LateralVariableLayout(input.ReferenceStations.Count);
+        FindSingleVariableBounds(trustSolver.Problems[0], layout.L(1), out double initialLower, out double initialUpper);
+        FindSingleVariableBounds(trustSolver.Problems[1], layout.L(1), out double nextLower, out double nextUpper);
+        Verification.NearlyEqual(-0.05d, initialLower, "initial trust-region lower bound");
+        Verification.NearlyEqual(0.05d, initialUpper, "initial trust-region upper bound");
+        Verification.NearlyEqual(-0.03d, nextLower, "trust region is centered on previous iterate");
+        Verification.NearlyEqual(0.07d, nextUpper, "trust region never exceeds 0.05m around previous iterate");
+        Verification.Equal(layout.VariableCount, trustSolver.WarmStarts[1].Count,
+            "next QP receives the complete previous primal warm start");
+        Verification.NearlyEqual(0.02d, trustSolver.WarmStarts[1][layout.L(1)],
+            "warm start retains the prior lateral iterate");
+
+        var limitResults = new List<QpSolveResult>();
+        for (int index = 1; index <= 5; index++)
+            limitResults.Add(Result(QpSolveStatus.Solved, CreatePrimal(input, 0.02d * index), 100d - index));
+        var limitSolver = new FakeQpSolver(limitResults);
+        LateralPlanningResult limited = new SequentialConvexOptimizer(limitSolver).Optimize(input, CancellationToken.None);
+        Verification.Equal(5, limitSolver.SolveCallCount, "outer loop stops after at most five QP calls");
+        Verification.Equal(EmPlanningStatus.Success, limited.Status, "last feasible candidate succeeds at outer iteration limit");
+    }
+
+    private static void VerifiesCancellationAndTimeoutWithoutCandidate()
+    {
+        LateralPlanningInput input = CreateInput();
+        var cancellationSolver = new FakeQpSolver(Result(QpSolveStatus.Solved, CreatePrimal(input, 0d), 1d));
+        using var cancellation = new CancellationTokenSource();
+        cancellation.Cancel();
+        LateralPlanningResult cancelled = new SequentialConvexOptimizer(cancellationSolver).Optimize(input,
+            cancellation.Token);
+        Verification.Equal(EmPlanningStatus.Cancelled, cancelled.Status, "cancellation before a solver call is cancelled");
+        Verification.Equal(0, cancellationSolver.SolveCallCount, "cancelled solve does not invoke the solver");
+
+        var timeoutSolver = new FakeQpSolver(Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 1d));
+        LateralPlanningResult timeout = new SequentialConvexOptimizer(timeoutSolver).Optimize(input, CancellationToken.None);
+        Verification.Equal(EmPlanningStatus.SolverTimedOut, timeout.Status,
+            "timeout without a feasible candidate is solver timed out");
+        Verification.True(ReferenceEquals(null, timeout.Path), "timeout without candidate does not publish a path");
+    }
+
+    private static void VerifiesLateralPlannerDelegatesToTheSequentialOptimizer()
+    {
+        LateralPlanningInput input = CreateInput();
+        double[] zero = CreatePrimal(input, 0d);
+        var solver = new FakeQpSolver(new[]
+        {
+            Result(QpSolveStatus.Solved, zero, 1d),
+            Result(QpSolveStatus.Solved, zero, 1d),
+        });
+        LateralPlanningResult result = new LateralPlanner(solver).Plan(input, CancellationToken.None);
+        Verification.Equal(EmPlanningStatus.Success, result.Status, "lateral planner returns SQP success");
+    }
+
+    private static LateralPlanningInput CreateInput()
+    {
+        var points = new List<SmoothedPathPoint>
+        {
+            Point(0d, 0d),
+            Point(1d, 1d),
+            Point(2d, 2d),
+        };
+        var segment = new DirectionSegmentView(0, TravelDirection.Forward, points,
+            new ReferenceBoundary(0, 0d, EmBoundaryType.None, 0d),
+            new ReferenceBoundary(0, 2d, EmBoundaryType.Goal, 2d), 0d);
+        var corridor = new StaticCorridor(new[]
+        {
+            new LateralInterval(0d, -0.3d, 0.3d, 0d),
+            new LateralInterval(1d, -0.3d, 0.3d, 0d),
+            new LateralInterval(2d, -0.3d, 0.3d, 0d),
+        });
+        var vehicle = new VehicleParameters
+        {
+            LengthMeters = 0.1d,
+            WidthMeters = 0.1d,
+            SafetyMarginMeters = 0d,
+            MaximumCurvaturePerMeter = 1d,
+        };
+        return new LateralPlanningInput(segment, corridor,
+            new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 0d), 0d, 0d, 0d),
+            EmTerminalType.Goal, vehicle, EmPlannerConfiguration.CreateDefault(), Array.Empty<FrenetProjection>());
+    }
+
+    private static SmoothedPathPoint Point(double x, double pathS)
+    {
+        return new SmoothedPathPoint(x, 0d, 0d, 0d, pathS, TravelDirection.Forward, 0d, 0d, 0d, 1d,
+            false, SmoothedPathPointSource.Anchor);
+    }
+
+    private static QpSolveResult Result(QpSolveStatus status, IReadOnlyList<double> primal, double objective,
+        double primalResidual = 0d, double dualResidual = 0d)
+    {
+        return new QpSolveResult(status, primal, objective, primalResidual, dualResidual, 1, TimeSpan.Zero,
+            status.ToString(), string.Empty);
+    }
+
+    private static double[] CreatePrimal(LateralPlanningInput input, double middleL)
+    {
+        var layout = new LateralVariableLayout(input.ReferenceStations.Count);
+        double c = 6d * middleL;
+        var primal = new double[layout.VariableCount];
+        primal[layout.L(0)] = 0d;
+        primal[layout.L(1)] = middleL;
+        primal[layout.L(2)] = 0d;
+        primal[layout.DL(0)] = 0d;
+        primal[layout.DL(1)] = 0d;
+        primal[layout.DL(2)] = 0d;
+        primal[layout.DDL(0)] = c;
+        primal[layout.DDL(1)] = -c;
+        primal[layout.DDL(2)] = c;
+        primal[layout.DDDL(0)] = -2d * c;
+        primal[layout.DDDL(1)] = 2d * c;
+        return primal;
+    }
+
+    private static void FindSingleVariableBounds(QuadraticProgram problem, int variable, out double lower, out double upper)
+    {
+        for (int row = 0; row < problem.ConstraintCount; row++)
+        {
+            int matchingEntries = 0;
+            double coefficient = 0d;
+            for (int column = 0; column < problem.ConstraintMatrix.ColumnCount; column++)
+            {
+                for (int index = problem.ConstraintMatrix.ColumnPointers[column];
+                    index < problem.ConstraintMatrix.ColumnPointers[column + 1]; index++)
+                {
+                    if (problem.ConstraintMatrix.RowIndices[index] == row)
+                    {
+                        matchingEntries++;
+                        if (column == variable)
+                            coefficient = problem.ConstraintMatrix.Values[index];
+                    }
+                }
+            }
+            if (matchingEntries == 1 && Math.Abs(coefficient - 1d) <= 1e-12d &&
+                Math.Abs(problem.LowerBounds[row] - problem.UpperBounds[row]) > 1e-12d)
+            {
+                lower = problem.LowerBounds[row];
+                upper = problem.UpperBounds[row];
+                return;
+            }
+        }
+        throw new InvalidOperationException("Expected single-variable lateral trust-region row was not found.");
+    }
+}
@@ -8,9 +8,9 @@ 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] != "all-foundation" && args[0] != "lateral-model" && args[0] != "lateral-integration"))
        {
-            Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|optimization|osqp|osqp-loader|all-foundation|lateral-model");
+            Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|optimization|osqp|osqp-loader|all-foundation|lateral-model|lateral-integration");
            return 2;
        }

@@ -55,6 +55,11 @@ internal static class Program
                MultiWheelC.TrajectoryPlanning.EMPlanner.LateralModelChecks.Run();
                Console.WriteLine("PASS lateral-model");
            }
+            if (args[0] == "lateral-integration")
+            {
+                MultiWheelC.TrajectoryPlanning.EMPlanner.LateralIntegrationChecks.Run();
+                Console.WriteLine("PASS lateral-integration");
+            }
            return 0;
        }
        catch (Exception exception)