feat: derive adaptive full-segment ST schedule

2026-08-06 23:54:34 +08:00
parent dad4ff4b04
commit 3269d556b6
14 changed files with 1642 additions and 108 deletions
@@ -47,18 +47,40 @@ public sealed class SequentialLongitudinalOptimizer
        if (speedStatus != EmPlanningStatus.Success)
            return Failed(speedStatus, speedFailure);

-        LongitudinalCandidate iterate = CreateInitialIterate(input, speedLimit);
+        var stopwatch = Stopwatch.StartNew();
+        LongitudinalCandidate iterate;
+        LongitudinalCandidate lastStrictCandidate = null;
+        int remainingObjectiveIterations = iterationLimit;
+        if (input.PlanningScope == EmPlanningScope.FullDirectionSegment &&
+            input.Mode == EmLongitudinalMode.ExactStopAtBoundary)
+        {
+            if (!TryCreateInitialFeasibleCandidate(input, speedLimit, settings, totalBudget, convergenceTolerance,
+                    iterationLimit, stopwatch, cancellationToken, out iterate, out int projectionSolveCount,
+                    out EmPlanningStatus projectionStatus, out string projectionFailure))
+            {
+                return Failed(projectionStatus, projectionFailure);
+            }
+            lastStrictCandidate = CopyCandidate(iterate);
+            remainingObjectiveIterations -= projectionSolveCount;
+            if (remainingObjectiveIterations <= 0)
+            {
+                return new LongitudinalPlanningResult(EmPlanningStatus.SuccessWithFallback, lastStrictCandidate,
+                    "The strict initial feasibility projection consumed the configured outer-iteration budget.");
+            }
+        }
+        else
+        {
+            iterate = CreateInitialIterate(input, speedLimit);
+            if (!_solutionValidator.TryValidate(input, speedLimit, iterate, out lastStrictCandidate, out _))
+                lastStrictCandidate = null;
+        }
        double[] warmStart = ToPrimal(iterate);
        bool hasDynamicsConsistentInitialWarmStart = iterate.SatisfiesExactDiscreteDynamics(1e-12d);
-        LongitudinalCandidate lastStrictCandidate;
-        if (!_solutionValidator.TryValidate(input, speedLimit, iterate, out lastStrictCandidate, out _))
-            lastStrictCandidate = null;
        string lastCandidateRejection = string.Empty;
        bool hasPreviousObjective = false;
        double previousObjective = 0d;
-        var stopwatch = Stopwatch.StartNew();

-        for (int iteration = 0; iteration < iterationLimit; iteration++)
+        for (int iteration = 0; iteration < remainingObjectiveIterations; iteration++)
        {
            if (cancellationToken.IsCancellationRequested)
                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Cancelled, "Longitudinal optimization was cancelled.");
@@ -191,10 +213,132 @@ public sealed class SequentialLongitudinalOptimizer
        }
    }

+    private bool TryCreateInitialFeasibleCandidate(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
+        QpSolverSettings settings, TimeSpan totalBudget, double convergenceTolerance, int iterationLimit,
+        Stopwatch stopwatch, CancellationToken cancellationToken, out LongitudinalCandidate candidate,
+        out int projectionSolveCount, out EmPlanningStatus failureStatus, out string failureReason)
+    {
+        candidate = null;
+        projectionSolveCount = 0;
+        failureStatus = EmPlanningStatus.LongitudinalInfeasible;
+        failureReason = string.Empty;
+        LongitudinalCandidate linearizationIterate = CreateScheduleReferenceIterate(input);
+        string lastRejection = string.Empty;
+        for (int iteration = 0; iteration < iterationLimit; iteration++)
+        {
+            if (cancellationToken.IsCancellationRequested)
+            {
+                failureStatus = EmPlanningStatus.Cancelled;
+                failureReason = "Initial full-direction feasibility projection was cancelled.";
+                return false;
+            }
+            TimeSpan remainingBudget = totalBudget - stopwatch.Elapsed;
+            if (remainingBudget <= TimeSpan.Zero)
+            {
+                failureStatus = EmPlanningStatus.SolverTimedOut;
+                failureReason = "Initial full-direction feasibility projection exhausted the shared solve budget.";
+                return false;
+            }
+            if (!_constraintBuilder.TryBuildInitialFeasibilityProjection(input, speedLimit, linearizationIterate,
+                    out QuadraticProgram problem, out string buildFailure))
+            {
+                failureStatus = EmPlanningStatus.LongitudinalInfeasible;
+                failureReason = "Initial full-direction feasibility constraints are infeasible: " + buildFailure;
+                return false;
+            }
+
+            double projectionTolerance = Math.Min(settings.AbsoluteTolerance,
+                input.Configuration.Validation.KinematicTolerance * 0.1d);
+            QpSolveResult solved = _qpSolver.Solve(problem,
+                new QpSolverSettings(settings.MaximumIterations, projectionTolerance, projectionTolerance,
+                    remainingBudget, settings.EnableWarmStart && linearizationIterate.SatisfiesExactDiscreteDynamics(1e-12d),
+                    settings.EnablePolishing, settings.EnableNativeVerboseOutput),
+                ToPrimal(linearizationIterate), cancellationToken);
+            projectionSolveCount++;
+            if (solved == null)
+            {
+                failureStatus = EmPlanningStatus.Failed;
+                failureReason = "The initial full-direction feasibility solver returned no result.";
+                return false;
+            }
+            if (solved.Status == QpSolveStatus.TimeLimit || solved.Status == QpSolveStatus.MaximumIterations)
+            {
+                failureStatus = EmPlanningStatus.SolverTimedOut;
+                failureReason = "Initial full-direction feasibility projection timed out (status=" + solved.NativeStatus +
+                    ", iterations=" + solved.Iterations + ", primal=" + solved.PrimalResidual + ", dual=" +
+                    solved.DualResidual + "): " + solved.Diagnostic;
+                return false;
+            }
+            if (solved.Status == QpSolveStatus.Cancelled)
+            {
+                failureStatus = EmPlanningStatus.Cancelled;
+                failureReason = "Initial full-direction feasibility projection was cancelled: " + solved.Diagnostic;
+                return false;
+            }
+            if (solved.Status == QpSolveStatus.PrimalInfeasible || solved.Status == QpSolveStatus.DualInfeasible)
+            {
+                failureStatus = EmPlanningStatus.LongitudinalInfeasible;
+                failureReason = "Initial full-direction feasibility projection is infeasible: " + solved.Diagnostic;
+                return false;
+            }
+            if (solved.Status == QpSolveStatus.SolverUnavailable)
+            {
+                failureStatus = EmPlanningStatus.SolverUnavailable;
+                failureReason = "Initial full-direction feasibility solver is unavailable: " + solved.Diagnostic;
+                return false;
+            }
+            if (solved.Status != QpSolveStatus.Solved && solved.Status != QpSolveStatus.SolvedInaccurate)
+            {
+                failureStatus = EmPlanningStatus.Failed;
+                failureReason = "Initial full-direction feasibility solver failed: " + solved.Diagnostic;
+                return false;
+            }
+            if (!TryCreateCandidate(input.KnotSchedule.KnotTimes, solved.Primal, out LongitudinalCandidate projected))
+            {
+                failureStatus = EmPlanningStatus.LongitudinalInfeasible;
+                failureReason = "Initial full-direction feasibility solver primal does not match the ST layout.";
+                return false;
+            }
+            if (solved.Status == QpSolveStatus.Solved || HasStrictResiduals(solved, convergenceTolerance))
+            {
+                if (_solutionValidator.TryValidate(input, speedLimit, projected, out LongitudinalCandidate strict,
+                        out string validationFailure))
+                {
+                    candidate = strict;
+                    return true;
+                }
+                lastRejection = validationFailure;
+            }
+
+            if (!TryCreateFeasibilityEnvelopeIterate(input, projected,
+                    out LongitudinalCandidate nextLinearization))
+            {
+                failureStatus = EmPlanningStatus.LongitudinalInfeasible;
+                failureReason = "Initial full-direction feasibility candidate could not be relinearized against the PathS envelope.";
+                return false;
+            }
+            linearizationIterate = nextLinearization;
+            if (solved.Status == QpSolveStatus.SolvedInaccurate)
+                lastRejection = "Initial feasibility projection residuals exceed the strict acceptance tolerance.";
+            else if (string.IsNullOrEmpty(lastRejection))
+                lastRejection = "Initial feasibility projection violated the strict physical validator.";
+        }
+        failureStatus = EmPlanningStatus.LongitudinalInfeasible;
+        failureReason = "Initial full-direction feasibility projection exhausted the configured outer iterations. " +
+            lastRejection;
+        return false;
+    }
+
+    private static LongitudinalCandidate CreateScheduleReferenceIterate(LongitudinalPlanningInput input)
+    {
+        int knotCount = input.KnotSchedule.KnotTimes.Count;
+        return new LongitudinalCandidate(input.KnotSchedule.KnotTimes, input.KnotSchedule.ReferencePathS,
+            input.KnotSchedule.ReferenceSpeedMetersPerSecond, new double[knotCount], new double[knotCount - 1]);
+    }
+
    private LongitudinalCandidate CreateInitialIterate(LongitudinalPlanningInput input, PathSpeedLimit speedLimit)
    {
-        IReadOnlyList<double> times = LongitudinalCandidate.CreateKnotTimes(input.Configuration.Scheduling.TimeHorizonSeconds,
-            input.Configuration.Scheduling.OutputTimeStepSeconds);
+        IReadOnlyList<double> times = input.KnotSchedule.KnotTimes;
        switch (input.Mode)
        {
            case EmLongitudinalMode.RollingContinuation:
@@ -269,6 +413,10 @@ public sealed class SequentialLongitudinalOptimizer
    private LongitudinalCandidate CreateExactStopSeed(LongitudinalPlanningInput input,
        IReadOnlyList<double> times, PathSpeedLimit speedLimit)
    {
+        if (input.PlanningScope == EmPlanningScope.FullDirectionSegment)
+        {
+            throw new InvalidOperationException("Full-direction exact-stop planning requires the initial feasibility projection.");
+        }
        int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(times,
            input.Configuration.Scheduling.OutputTimeStepSeconds);
        var motionTimes = new double[stabilizationStart + 1];
@@ -364,7 +512,7 @@ public sealed class SequentialLongitudinalOptimizer
        for (int index = 0; index < motionTimes.Length; index++)
            motionTimes[index] = times[index];

-        LongitudinalCandidate baseline = CreateApproachSeed(input, motionTimes, speedLimit);
+        LongitudinalCandidate baseline = CreateScheduleReferenceSeed(input, motionTimes, speedLimit);
        var influence = new double[3, intervalCount];
        for (int interval = 0; interval < intervalCount; interval++)
        {
@@ -461,6 +609,30 @@ public sealed class SequentialLongitudinalOptimizer
        return AppendExactStopTail(times, stabilizationStart, input.StopBoundaryPathS, motion);
    }

+    private static LongitudinalCandidate CreateScheduleReferenceSeed(LongitudinalPlanningInput input,
+        IReadOnlyList<double> times, PathSpeedLimit speedLimit)
+    {
+        LongitudinalConfiguration configuration = input.Configuration.Longitudinal;
+        var jerk = new double[times.Count - 1];
+        double speed = input.InitialProgressSpeedMetersPerSecond;
+        double acceleration = input.InitialAccelerationMetersPerSecondSquared;
+        for (int index = 0; index < jerk.Length; index++)
+        {
+            double dt = times[index + 1] - times[index];
+            double targetSpeed = input.KnotSchedule.ReferenceSpeedMetersPerSecond[index + 1];
+            double lowerJerk = Math.Max(-configuration.MaximumJerkMetersPerSecondCubed,
+                (-configuration.MaximumDecelerationMetersPerSecondSquared - acceleration) / dt);
+            double upperJerk = Math.Min(configuration.MaximumJerkMetersPerSecondCubed,
+                (configuration.MaximumAccelerationMetersPerSecondSquared - acceleration) / dt);
+            double requestedJerk = 2d * (targetSpeed - speed - acceleration * dt) / (dt * dt);
+            double selectedJerk = Clamp(requestedJerk, lowerJerk, upperJerk);
+            jerk[index] = selectedJerk;
+            IntegrateStep(0d, speed, acceleration, selectedJerk, dt, out _, out speed, out acceleration);
+        }
+        return LongitudinalCandidate.Integrate(times, 0d, input.InitialProgressSpeedMetersPerSecond,
+            input.InitialAccelerationMetersPerSecondSquared, jerk);
+    }
+
    private static double[] CreateEndpointNullspaceDirection(double[,] influence, double[,] gram, int basisIndex)
    {
        int intervalCount = influence.GetLength(1);
@@ -679,10 +851,12 @@ public sealed class SequentialLongitudinalOptimizer
        nextIterate = null;
        if (candidate.S.Count != previous.S.Count)
            return false;
-        int stabilizationStart = input.Mode == EmLongitudinalMode.ExactStopAtBoundary
-            ? LongitudinalTerminalSchedule.GetStabilizationStartIndex(candidate.KnotTimes,
-                input.Configuration.Scheduling.OutputTimeStepSeconds)
-            : candidate.S.Count;
+        int stabilizationStart = input.Mode != EmLongitudinalMode.ExactStopAtBoundary
+            ? candidate.S.Count
+            : input.PlanningScope == EmPlanningScope.FullDirectionSegment
+                ? input.KnotSchedule.TerminalHoldStartIndex
+                : LongitudinalTerminalSchedule.GetStabilizationStartIndex(candidate.KnotTimes,
+                    input.Configuration.Scheduling.OutputTimeStepSeconds);
        var candidateProgressSamples = new double[candidate.S.Count];
        double priorProgress = double.NegativeInfinity;
        double priorPreviousProgress = double.NegativeInfinity;
@@ -737,6 +911,30 @@ public sealed class SequentialLongitudinalOptimizer
        return true;
    }

+    private static bool TryCreateFeasibilityEnvelopeIterate(LongitudinalPlanningInput input,
+        LongitudinalCandidate candidate, out LongitudinalCandidate nextIterate)
+    {
+        nextIterate = null;
+        int stabilizationStart = input.KnotSchedule.TerminalHoldStartIndex;
+        var pathS = new double[candidate.S.Count];
+        double previousPathS = double.NegativeInfinity;
+        double tolerance = input.Configuration.Validation.KinematicTolerance;
+        for (int index = 0; index < pathS.Length; index++)
+        {
+            double value = candidate.S[index];
+            if (!IsFinite(value) || value < -tolerance || value > input.PathUpperBoundS + tolerance ||
+                value < previousPathS - tolerance)
+            {
+                return false;
+            }
+            value = Math.Max(0d, Math.Min(input.PathUpperBoundS, value));
+            pathS[index] = index >= stabilizationStart ? input.StopBoundaryPathS : Math.Max(previousPathS, value);
+            previousPathS = pathS[index];
+        }
+        nextIterate = new LongitudinalCandidate(candidate.KnotTimes, pathS, candidate.U, candidate.A, candidate.J);
+        return true;
+    }
+
    private static bool HasStrictResiduals(QpSolveResult result, double tolerance)
    {
        return IsPositiveFinite(tolerance) && result.PrimalResidual >= 0d && result.DualResidual >= 0d &&