chore: save current workspace progress

2026-08-09 22:13:18 +08:00
parent 650c2ab0e3
commit 2f4fd15e52
449 changed files with 76593 additions and 971 deletions
@@ -1,4 +1,4 @@
-namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

 public sealed partial class EmPlannerConfiguration
 {
@@ -17,8 +17,8 @@ public sealed partial class EmPlannerConfiguration
            Scheduling = new SchedulingConfiguration
            {
                ReplanPeriodSeconds = 0.20d,
-                TimeHorizonSeconds = 6d,
-                DistanceHorizonMeters = 5d,
+                TimeHorizonSeconds = 6d,                //窗口的规划时间长度。单位s
+                DistanceHorizonMeters = 500d,           //最大探索s里程距离，单位m
                OutputTimeStepSeconds = 0.05d,
                SolverTimeoutSeconds = 0.10d,
                HandoffLookaheadSeconds = 0.30d,
@@ -1,5 +1,6 @@
 using System;
 using System.Collections.Generic;
+using System.Diagnostics;
 using System.Threading;
 using MultiWheelC.TrajectoryPlanning.CoarsePath;

@@ -43,11 +44,19 @@ public sealed class EmPlanningService : IEmPlanningService
            EmitDebug(request, "direction-segment selection succeeded");

            var projector = new FrenetProjector();
-            if (!projector.TryProject(request.VehicleState.Pose, segment, 0d, segment.LengthMeters,
+            double startProjectionUpperS = request.PlanningScope == EmPlanningScope.FullDirectionSegment
+                ? Math.Min(segment.LengthMeters, configuration.Frenet.MaximumProjectionDistanceMeters)
+                : segment.LengthMeters;
+            if (!projector.TryProject(request.VehicleState.Pose, segment, 0d, startProjectionUpperS,
                    configuration.Frenet.MaximumProjectionDistanceMeters, 0d, out FrenetProjection startProjection))
            {
                return Failure(EmPlanningStatus.ProjectionFailed, request,
-                    "Vehicle pose could not be projected inside the selected direction segment.");
+                    "Vehicle pose could not be projected at an admissible start of the selected direction segment.");
+            }
+            if (Math.Abs(startProjection.HeadingError) >= Math.PI / 2d)
+            {
+                return Failure(EmPlanningStatus.ProjectionFailed, request,
+                    "Vehicle travel heading differs by at least 90 degrees from the selected direction segment.");
            }
            EmitDebug(request, "bounded ego projection succeeded");

@@ -78,9 +87,13 @@ public sealed class EmPlanningService : IEmPlanningService

            var lateralInput = new LateralPlanningInput(segment, corridor, startProjection, horizon.TerminalType,
                request.Vehicle, configuration, previousSeed);
+            TimeSpan totalSolveBudget = TimeSpan.FromSeconds(configuration.Scheduling.SolverTimeoutSeconds);
+            var solveBudgetStopwatch = Stopwatch.StartNew();
            LateralPlanningResult lateral = new LateralPlanner(qpSolver).Plan(lateralInput, cancellationToken);
            if (!IsSuccess(lateral.Status))
                return Failure(lateral.Status, request, lateral.FailureReason);
+            if (cancellationToken.IsCancellationRequested)
+                return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled after LS optimization.");
            EmitDebug(request, "LS optimization and validation succeeded");

            EmPlanningStatus envelopeStatus = new PathSpeedLimitBuilder().Build(lateral.Path, segment.Direction,
@@ -106,8 +119,16 @@ public sealed class EmPlanningService : IEmPlanningService
                new LongitudinalPreviousTrajectorySeedBuilder().Build(
                    request.PreviousTrajectory, lateral.Path, request.EffectiveAtUtc, knotSchedule,
                    segment.SegmentIndex, segment.Direction);
+            TimeSpan remainingSolveBudget = totalSolveBudget - solveBudgetStopwatch.Elapsed;
+            if (remainingSolveBudget <= TimeSpan.Zero)
+            {
+                return Failure(EmPlanningStatus.SolverTimedOut, request,
+                    "LS/ST optimization exhausted the shared solve budget before ST optimization.");
+            }
+            EmPlannerConfiguration longitudinalConfiguration = configuration.Copy();
+            longitudinalConfiguration.Scheduling.SolverTimeoutSeconds = remainingSolveBudget.TotalSeconds;
            var longitudinalInput = new LongitudinalPlanningInput(lateral.Path, segment.Direction, initialProgressSpeed,
-                initialAcceleration, horizon.TerminalType, horizon.LongitudinalMode, configuration,
+                initialAcceleration, horizon.TerminalType, horizon.LongitudinalMode, longitudinalConfiguration,
                request.PlanningScope, knotSchedule,
                previousLongitudinalSeed.PathS, previousLongitudinalSeed.ProgressSpeedMetersPerSecond);
            envelopeStatus = new PathSpeedLimitBuilder().Build(longitudinalInput, out _, out envelopeReason);
@@ -118,6 +139,8 @@ public sealed class EmPlanningService : IEmPlanningService
            LongitudinalPlanningResult longitudinal = new LongitudinalPlanner(qpSolver).Plan(longitudinalInput, cancellationToken);
            if (!IsSuccess(longitudinal.Status))
                return Failure(longitudinal.Status, request, longitudinal.FailureReason);
+            if (cancellationToken.IsCancellationRequested)
+                return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled after ST optimization.");
            EmitDebug(request, "ST optimization and validation succeeded");

            var metadata = new EmTrajectoryMetadata(request.OutputTrajectoryId, request.RequestedAtUtc, request.EffectiveAtUtc,
@@ -128,6 +151,8 @@ public sealed class EmPlanningService : IEmPlanningService
                longitudinal, metadata, out EmTrajectory trajectory, out string assemblyFailure);
            if (assemblyStatus != EmPlanningStatus.Success)
                return Failure(assemblyStatus, request, assemblyFailure);
+            if (cancellationToken.IsCancellationRequested)
+                return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled after trajectory assembly.");
            EmitDebug(request, "trajectory assembly succeeded");

            EmBoundaryType terminalBoundary = slice.TerminalBoundary.BoundaryType;
@@ -135,7 +160,7 @@ public sealed class EmPlanningService : IEmPlanningService
                ? TerminalPose(lateral.Path)
                : null;
            EmTrajectoryValidationResult publication = new EmTrajectoryValidator().Validate(trajectory, request.Map,
-                request.Vehicle, configuration, segment.SegmentIndex, longitudinalInput.PathUpperBoundS,
+                request.Vehicle, configuration, segment.SegmentIndex, segment.LengthMeters, longitudinalInput.PathUpperBoundS,
                terminalPose, terminalBoundary);
            if (!publication.IsValid)
            {
@@ -144,6 +169,9 @@ public sealed class EmPlanningService : IEmPlanningService
            }
            EmitDebug(request, "world-space publication validation succeeded");

+            if (cancellationToken.IsCancellationRequested)
+                return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled before trajectory publication.");
+
            EmPlanningStatus finalStatus = lateral.Status == EmPlanningStatus.SuccessWithFallback ||
                longitudinal.Status == EmPlanningStatus.SuccessWithFallback
                ? EmPlanningStatus.SuccessWithFallback
@@ -35,7 +35,7 @@ public sealed class LateralConstraintBuilder
            _objectiveBuilder.AddTerms(input, layout, linearization, hessian, linearCost);

            int terminalRows = input.TerminalType == EmTerminalType.RollingSafetyStop ? 0 : 2;
-            var constraints = new SparseTripletBuilder(7 * layout.StationCount - 2 + terminalRows, layout.VariableCount);
+            var constraints = new SparseTripletBuilder(8 * layout.StationCount - 2 + terminalRows, layout.VariableCount);
            var lower = new List<double>();
            var upper = new List<double>();
            int row = 0;
@@ -43,12 +43,13 @@ public sealed class LateralConstraintBuilder
            if (!TryAddLateralBounds(input, layout, linearization, constraints, lower, upper, ref row, out failureReason))
                return false;
            AddDerivativeBounds(input, layout, constraints, lower, upper, ref row);
+            AddCurvatureBounds(input, layout, linearization, constraints, lower, upper, ref row);
            AddStartConstraints(input, layout, constraints, lower, upper, ref row);
            AddExactDynamics(input.ReferenceStations, layout, constraints, lower, upper, ref row);
            if (input.TerminalType != EmTerminalType.RollingSafetyStop)
                AddTerminalConstraints(layout, constraints, lower, upper, ref row);

-            if (row != 7 * layout.StationCount - 2 + terminalRows)
+            if (row != 8 * layout.StationCount - 2 + terminalRows)
                throw new InvalidOperationException("Lateral constraint row accounting is inconsistent.");
            problem = new QuadraticProgram(hessian.Build(), linearCost, constraints.Build(), lower, upper);
            return true;
@@ -131,6 +132,21 @@ public sealed class LateralConstraintBuilder
            AddSingleVariableRow(constraints, lower, upper, ref row, layout.DDDL(interval), -third, third);
    }

+    private static void AddCurvatureBounds(LateralPlanningInput input, LateralVariableLayout layout,
+        LateralCandidate linearization, SparseTripletBuilder constraints, IList<double> lower, IList<double> upper,
+        ref int row)
+    {
+        double maximumCurvature = LateralCurvatureLinearization.GetMaximumVehicleCurvature(input.Vehicle);
+        IReadOnlyList<LateralCurvatureLinearization> affines = LateralCurvatureLinearization.Create(input, layout,
+            linearization);
+        for (int station = 0; station < affines.Count; station++)
+        {
+            LateralCurvatureLinearization affine = affines[station];
+            AddRow(constraints, lower, upper, ref row, -maximumCurvature - affine.Constant,
+                maximumCurvature - affine.Constant, affine.VariableIndices, affine.Gradient);
+        }
+    }
+
    private static void AddStartConstraints(LateralPlanningInput input, LateralVariableLayout layout,
        SparseTripletBuilder constraints, IList<double> lower, IList<double> upper, ref int row)
    {
@@ -0,0 +1,101 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Shared affine vehicle-curvature model used by the LS objective and hard QP constraints.</summary>
+internal sealed class LateralCurvatureLinearization
+{
+    private LateralCurvatureLinearization(int[] variableIndices, double[] gradient, double constant)
+    {
+        VariableIndices = variableIndices;
+        Gradient = gradient;
+        Constant = constant;
+    }
+
+    internal IReadOnlyList<int> VariableIndices { get; }
+
+    internal IReadOnlyList<double> Gradient { get; }
+
+    internal double Constant { get; }
+
+    internal static IReadOnlyList<LateralCurvatureLinearization> Create(LateralPlanningInput input,
+        LateralVariableLayout layout, LateralCandidate linearization)
+    {
+        if (input == null) throw new ArgumentNullException(nameof(input));
+        if (layout == null) throw new ArgumentNullException(nameof(layout));
+        if (linearization == null) throw new ArgumentNullException(nameof(linearization));
+        if (layout.StationCount != input.ReferenceStations.Count ||
+            linearization.ReferenceStations.Count != layout.StationCount)
+        {
+            throw new ArgumentException("Curvature linearization stations must match the lateral layout.", nameof(linearization));
+        }
+
+        double directionSign = input.ReferenceSegment.Direction == TravelDirection.Forward ? 1d : -1d;
+        var affines = new List<LateralCurvatureLinearization>(layout.StationCount);
+        for (int station = 0; station < layout.StationCount; station++)
+        {
+            FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(input.ReferenceSegment,
+                input.ReferenceStations[station]);
+            double l = linearization.L[station];
+            double dl = linearization.DL[station];
+            double ddl = linearization.DDL[station];
+            double referenceCurvature = reference.GeometricCurvature;
+            double referenceCurvatureDerivative = directionSign * reference.VehicleCurvatureDerivative;
+            double a = 1d - referenceCurvature * l;
+            double denominatorSquared = a * a + dl * dl;
+            if (!IsFinite(denominatorSquared) || denominatorSquared <= 0d)
+                throw new ArgumentException("Curvature linearization denominator is invalid.", nameof(linearization));
+
+            double denominatorPow3Over2 = denominatorSquared * Math.Sqrt(denominatorSquared);
+            double denominatorPow5Over2 = denominatorPow3Over2 * denominatorSquared;
+            double numerator = a * a * referenceCurvature + a * ddl +
+                referenceCurvatureDerivative * l * dl + 2d * referenceCurvature * dl * dl;
+            double geometricCurvature = numerator / denominatorPow3Over2;
+            double dNumeratorDLateral = -2d * a * referenceCurvature * referenceCurvature -
+                referenceCurvature * ddl + referenceCurvatureDerivative * dl;
+            double dNumeratorDSlope = referenceCurvatureDerivative * l + 4d * referenceCurvature * dl;
+            double dDenominatorSquaredDLateral = -2d * a * referenceCurvature;
+            double dDenominatorSquaredDSlope = 2d * dl;
+            double dGeometricDLateral = dNumeratorDLateral / denominatorPow3Over2 -
+                1.5d * numerator * dDenominatorSquaredDLateral / denominatorPow5Over2;
+            double dGeometricDSlope = dNumeratorDSlope / denominatorPow3Over2 -
+                1.5d * numerator * dDenominatorSquaredDSlope / denominatorPow5Over2;
+            double dGeometricDSecondDerivative = a / denominatorPow3Over2;
+            double vehicleCurvature = directionSign * geometricCurvature;
+            double[] gradient =
+            {
+                directionSign * dGeometricDLateral,
+                directionSign * dGeometricDSlope,
+                directionSign * dGeometricDSecondDerivative,
+            };
+            double constant = vehicleCurvature - gradient[0] * l - gradient[1] * dl - gradient[2] * ddl;
+            if (!IsFinite(vehicleCurvature) || !IsFinite(constant) || !IsFinite(gradient[0]) ||
+                !IsFinite(gradient[1]) || !IsFinite(gradient[2]))
+            {
+                throw new ArgumentException("Curvature linearization is non-finite.", nameof(linearization));
+            }
+            affines.Add(new LateralCurvatureLinearization(
+                new[] { layout.L(station), layout.DL(station), layout.DDL(station) }, gradient, constant));
+        }
+        return affines;
+    }
+
+    internal static double GetMaximumVehicleCurvature(VehicleParameters vehicle)
+    {
+        if (vehicle == null) throw new ArgumentNullException(nameof(vehicle));
+        double maximum = vehicle.MaximumCurvaturePerMeter ??
+            (vehicle.MinimumTurningRadiusMeters.HasValue && vehicle.MinimumTurningRadiusMeters.Value > 0d
+                ? 1d / vehicle.MinimumTurningRadiusMeters.Value
+                : double.NaN);
+        if (!IsFinite(maximum) || maximum <= 0d)
+            throw new ArgumentException("Vehicle maximum curvature is required.", nameof(vehicle));
+        return maximum;
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}
@@ -27,7 +27,8 @@ public sealed class LateralObjectiveBuilder
        double slopeScale = RequirePositive(lateral.MaximumLateralSlope, "slope scale");
        double secondDerivativeScale = RequirePositive(lateral.MaximumLateralSecondDerivativePerMeter, "second-derivative scale");
        double thirdDerivativeScale = RequirePositive(lateral.MaximumLateralThirdDerivativePerSquareMeter, "third-derivative scale");
-        double curvatureScale = RequirePositive(GetMaximumVehicleCurvature(input.Vehicle), "curvature scale");
+        double curvatureScale = RequirePositive(LateralCurvatureLinearization.GetMaximumVehicleCurvature(input.Vehicle),
+            "curvature scale");
        double curvatureVariationScale = GetCurvatureVariationScale(input);

        for (int station = 0; station < layout.StationCount; station++)
@@ -46,10 +47,11 @@ public sealed class LateralObjectiveBuilder
        }

        AddPreviousTrajectoryTerms(input, layout, hessian, linearCost, weights.PreviousTrajectory, lateralScale);
-        CurvatureAffine[] curvature = CreateCurvatureAffines(input, layout, linearization);
-        for (int station = 0; station < curvature.Length; station++)
+        IReadOnlyList<LateralCurvatureLinearization> curvature = LateralCurvatureLinearization.Create(input, layout,
+            linearization);
+        for (int station = 0; station < curvature.Count; station++)
        {
-            AddSquaredResidual(hessian, linearCost, curvature[station].Indices, curvature[station].Gradient,
+            AddSquaredResidual(hessian, linearCost, curvature[station].VariableIndices, curvature[station].Gradient,
                curvature[station].Constant, weights.Curvature, curvatureScale);
        }
        AddCurvatureVariationTerms(input.ReferenceStations, curvature, hessian, linearCost, weights.CurvatureVariation,
@@ -75,79 +77,27 @@ public sealed class LateralObjectiveBuilder
        }
    }

-    private static CurvatureAffine[] CreateCurvatureAffines(LateralPlanningInput input, LateralVariableLayout layout,
-        LateralCandidate linearization)
-    {
-        var affines = new CurvatureAffine[layout.StationCount];
-        double directionSign = input.ReferenceSegment.Direction == TravelDirection.Forward ? 1d : -1d;
-        for (int station = 0; station < layout.StationCount; station++)
-        {
-            FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(input.ReferenceSegment,
-                input.ReferenceStations[station]);
-            double l = linearization.L[station];
-            double dl = linearization.DL[station];
-            double ddl = linearization.DDL[station];
-            double referenceCurvature = reference.GeometricCurvature;
-            double referenceCurvatureDerivative = directionSign * reference.VehicleCurvatureDerivative;
-            double a = 1d - referenceCurvature * l;
-            double denominatorSquared = a * a + dl * dl;
-            if (!IsFinite(denominatorSquared) || denominatorSquared <= 0d)
-                throw new ArgumentException("Curvature linearization denominator is invalid.", nameof(linearization));
-
-            double denominatorPow3Over2 = denominatorSquared * Math.Sqrt(denominatorSquared);
-            double denominatorPow5Over2 = denominatorPow3Over2 * denominatorSquared;
-            double numerator = a * a * referenceCurvature + a * ddl +
-                referenceCurvatureDerivative * l * dl + 2d * referenceCurvature * dl * dl;
-            double geometricCurvature = numerator / denominatorPow3Over2;
-            double dNumeratorDLateral = -2d * a * referenceCurvature * referenceCurvature -
-                referenceCurvature * ddl + referenceCurvatureDerivative * dl;
-            double dNumeratorDSlope = referenceCurvatureDerivative * l + 4d * referenceCurvature * dl;
-            double dDenominatorSquaredDLateral = -2d * a * referenceCurvature;
-            double dDenominatorSquaredDSlope = 2d * dl;
-            double dGeometricDLateral = dNumeratorDLateral / denominatorPow3Over2 -
-                1.5d * numerator * dDenominatorSquaredDLateral / denominatorPow5Over2;
-            double dGeometricDSlope = dNumeratorDSlope / denominatorPow3Over2 -
-                1.5d * numerator * dDenominatorSquaredDSlope / denominatorPow5Over2;
-            double dGeometricDSecondDerivative = a / denominatorPow3Over2;
-            double vehicleCurvature = directionSign * geometricCurvature;
-            double[] gradient =
-            {
-                directionSign * dGeometricDLateral,
-                directionSign * dGeometricDSlope,
-                directionSign * dGeometricDSecondDerivative,
-            };
-            double constant = vehicleCurvature - gradient[0] * l - gradient[1] * dl - gradient[2] * ddl;
-            if (!IsFinite(vehicleCurvature) || !IsFinite(constant) || !IsFinite(gradient[0]) ||
-                !IsFinite(gradient[1]) || !IsFinite(gradient[2]))
-            {
-                throw new ArgumentException("Curvature linearization is non-finite.", nameof(linearization));
-            }
-            affines[station] = new CurvatureAffine(new[] { layout.L(station), layout.DL(station), layout.DDL(station) },
-                gradient, constant);
-        }
-        return affines;
-    }
-
-    private static void AddCurvatureVariationTerms(IReadOnlyList<double> stations, CurvatureAffine[] curvature,
+    private static void AddCurvatureVariationTerms(IReadOnlyList<double> stations,
+        IReadOnlyList<LateralCurvatureLinearization> curvature,
        SparseTripletBuilder hessian, IList<double> linearCost, double weight, double scale)
    {
-        for (int station = 0; station < curvature.Length; station++)
+        for (int station = 0; station < curvature.Count; station++)
        {
            int lower = station == 0 ? 0 : station - 1;
-            int upper = station == curvature.Length - 1 ? curvature.Length - 1 : station + 1;
+            int upper = station == curvature.Count - 1 ? curvature.Count - 1 : station + 1;
            double ds = stations[upper] - stations[lower];
            if (!IsFinite(ds) || ds <= 0d)
                throw new ArgumentException("Curvature variation requires strictly increasing stations.", nameof(stations));
-            CurvatureAffine left = curvature[lower];
-            CurvatureAffine right = curvature[upper];
-            var indices = new int[left.Indices.Length + right.Indices.Length];
+            LateralCurvatureLinearization left = curvature[lower];
+            LateralCurvatureLinearization right = curvature[upper];
+            var indices = new int[left.VariableIndices.Count + right.VariableIndices.Count];
            var gradient = new double[indices.Length];
-            for (int index = 0; index < left.Indices.Length; index++)
+            for (int index = 0; index < left.VariableIndices.Count; index++)
            {
-                indices[index] = left.Indices[index];
+                indices[index] = left.VariableIndices[index];
                gradient[index] = -left.Gradient[index] / ds;
-                indices[left.Indices.Length + index] = right.Indices[index];
-                gradient[left.Indices.Length + index] = right.Gradient[index] / ds;
+                indices[left.VariableIndices.Count + index] = right.VariableIndices[index];
+                gradient[left.VariableIndices.Count + index] = right.Gradient[index] / ds;
            }
            AddSquaredResidual(hessian, linearCost, indices, gradient, (right.Constant - left.Constant) / ds, weight, scale);
        }
@@ -209,17 +159,6 @@ public sealed class LateralObjectiveBuilder
        return Math.Max(1d, maximum);
    }

-    private static double GetMaximumVehicleCurvature(VehicleParameters vehicle)
-    {
-        if (vehicle == null)
-            throw new ArgumentNullException(nameof(vehicle));
-        if (vehicle.MaximumCurvaturePerMeter.HasValue)
-            return vehicle.MaximumCurvaturePerMeter.Value;
-        if (vehicle.MinimumTurningRadiusMeters.HasValue && vehicle.MinimumTurningRadiusMeters.Value > 0d)
-            return 1d / vehicle.MinimumTurningRadiusMeters.Value;
-        throw new ArgumentException("Vehicle maximum curvature is required.", nameof(vehicle));
-    }
-
    private static double RequirePositive(double value, string name)
    {
        if (!IsFinite(value) || value <= 0d)
@@ -232,17 +171,4 @@ public sealed class LateralObjectiveBuilder
        return !double.IsNaN(value) && !double.IsInfinity(value);
    }

-    private sealed class CurvatureAffine
-    {
-        public CurvatureAffine(int[] indices, double[] gradient, double constant)
-        {
-            Indices = indices;
-            Gradient = gradient;
-            Constant = constant;
-        }
-
-        public int[] Indices { get; }
-        public double[] Gradient { get; }
-        public double Constant { get; }
-    }
 }
@@ -68,6 +68,8 @@ public sealed class SequentialConvexOptimizer
                    remainingBudget, settings.EnableWarmStart, settings.EnablePolishing, settings.EnableNativeVerboseOutput),
                warmStart, cancellationToken);

+            if (cancellationToken.IsCancellationRequested)
+                return Failed(EmPlanningStatus.Cancelled, "Lateral SQP was cancelled after the QP solve.");
            if (solved == null)
                return FallbackOrFailure(lastValidatedPath, EmPlanningStatus.Failed, "The lateral QP solver returned no result.");

@@ -100,16 +102,27 @@ public sealed class SequentialConvexOptimizer
                continue;
            if (!_geometryEvaluator.TryEvaluate(input, candidate, out LateralPath evaluatedPath, out _))
                continue;
+
+            // A geometrically evaluable QP solution that remains inside the static
+            // corridor is the next SQP linearization point, even when the independent
+            // validator rejects it.  Otherwise the next outer pass rebuilds the identical
+            // convex subproblem and cannot correct that result.  An out-of-corridor vector
+            // must not become an iterate: it can make the following trust region infeasible.
+            LateralCandidate previousIterate = iterate;
+            if (RespectsStaticCorridor(input, candidate))
+            {
+                iterate = candidate;
+                warmStart = CopyValues(solved.Primal);
+            }
+
            if (!_solutionValidator.TryValidate(input, candidate, evaluatedPath, out LateralPath validatedPath, out _))
                continue;

-            double maximumLateralChange = MaximumLateralChange(iterate, candidate);
+            double maximumLateralChange = MaximumLateralChange(previousIterate, candidate);
            double relativeObjectiveImprovement = hasPreviousObjective
                ? RelativeObjectiveImprovement(previousObjective, solved.Objective)
                : double.PositiveInfinity;
            lastValidatedPath = CopyPath(validatedPath);
-            iterate = candidate;
-            warmStart = CopyValues(solved.Primal);
            previousObjective = solved.Objective;
            hasPreviousObjective = true;

@@ -119,7 +132,10 @@ public sealed class SequentialConvexOptimizer

        return lastValidatedPath == null
            ? Failed(EmPlanningStatus.LateralInfeasible, "No independently validated lateral candidate was found.")
-            : new LateralPlanningResult(EmPlanningStatus.Success, lastValidatedPath, string.Empty);
+            : new LateralPlanningResult(
+                EmPlanningStatus.SuccessWithFallback,
+                lastValidatedPath,
+                "Lateral SQP reached its outer-iteration limit before strict convergence.");
    }

    private static bool TryCreateSettings(LateralPlanningInput input, out QpSolverSettings settings, out TimeSpan totalBudget,
@@ -226,6 +242,17 @@ public sealed class SequentialConvexOptimizer
        return maximum;
    }

+    private static bool RespectsStaticCorridor(LateralPlanningInput input, LateralCandidate candidate)
+    {
+        for (int index = 0; index < candidate.L.Count; index++)
+        {
+            LateralInterval interval = input.Corridor.Stations[index];
+            if (candidate.L[index] < interval.MinimumL || candidate.L[index] > interval.MaximumL)
+                return false;
+        }
+        return true;
+    }
+
    private static double RelativeObjectiveImprovement(double previous, double current)
    {
        return Math.Abs(previous - current) / Math.Max(1d, Math.Abs(previous));
@@ -233,7 +260,7 @@ public sealed class SequentialConvexOptimizer

    private static LateralPlanningResult FallbackOrFailure(LateralPath path, EmPlanningStatus failureStatus, string failureReason)
    {
-        return path == null
+        return failureStatus == EmPlanningStatus.Cancelled || path == null
            ? Failed(failureStatus, failureReason)
            : new LateralPlanningResult(EmPlanningStatus.SuccessWithFallback, path, failureReason);
    }
@@ -119,6 +119,8 @@ public sealed class SequentialLongitudinalOptimizer
                    settings.EnablePolishing,
                    settings.EnableNativeVerboseOutput),
                warmStart, cancellationToken);
+            if (cancellationToken.IsCancellationRequested)
+                return Failed(EmPlanningStatus.Cancelled, "Longitudinal optimization was cancelled after the QP solve.");
            if (solved == null)
                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Failed, "The longitudinal QP solver returned no result.");
            if (solved.Status == QpSolveStatus.TimeLimit || solved.Status == QpSolveStatus.MaximumIterations)
@@ -293,6 +295,12 @@ public sealed class SequentialLongitudinalOptimizer
                    settings.EnablePolishing, settings.EnableNativeVerboseOutput),
                ToPrimal(linearizationIterate), cancellationToken);
            projectionSolveCount++;
+            if (cancellationToken.IsCancellationRequested)
+            {
+                failureStatus = EmPlanningStatus.Cancelled;
+                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection was cancelled after the QP solve.");
+                return false;
+            }
            if (solved == null)
            {
                failureStatus = EmPlanningStatus.Failed;
@@ -1196,7 +1204,7 @@ public sealed class SequentialLongitudinalOptimizer
    private static LongitudinalPlanningResult FallbackOrFailure(LongitudinalCandidate candidate,
        EmPlanningStatus failureStatus, string failureReason)
    {
-        return candidate == null
+        return failureStatus == EmPlanningStatus.Cancelled || candidate == null
            ? Failed(failureStatus, failureReason)
            : new LongitudinalPlanningResult(EmPlanningStatus.SuccessWithFallback, candidate, failureReason);
    }
@@ -81,7 +81,7 @@ public sealed class EmTrajectoryAssembler
            EmBoundaryType boundaryType = isTerminalAnchor ? ToBoundaryType(metadata.TerminalType) : EmBoundaryType.None;
            double signedSpeed = directionSign * sample.ProgressSpeed;
            points.Add(new EmTrajectoryPoint(geometry.X, geometry.Y, geometry.Yaw, signedSpeed, sample.TimeFromStart,
-                geometry.VehicleCurvature, metadata.SegmentIndex, sample.PathS, sample.PathS, metadata.Direction,
+                geometry.VehicleCurvature, metadata.SegmentIndex, geometry.ReferenceS, sample.PathS, metadata.Direction,
                boundaryType, sample.Acceleration, sample.Jerk));
        }

@@ -46,6 +46,7 @@ internal sealed class LateralPathInterpolator
                LateralPathPoint left = path.Points[index - 1];
                double ratio = (pathS - left.PathS) / (right.PathS - left.PathS);
                return new InterpolatedLateralPathPoint(
+                    Linear(left.ReferenceS, right.ReferenceS, ratio),
                    Linear(left.X, right.X, ratio),
                    Linear(left.Y, right.Y, ratio),
                    NormalizeYaw(left.VehicleYaw + ratio * NormalizeYaw(right.VehicleYaw - left.VehicleYaw)),
@@ -58,7 +59,8 @@ internal sealed class LateralPathInterpolator

    private static InterpolatedLateralPathPoint From(LateralPathPoint point)
    {
-        return new InterpolatedLateralPathPoint(point.X, point.Y, NormalizeYaw(point.VehicleYaw), point.VehicleCurvature);
+        return new InterpolatedLateralPathPoint(point.ReferenceS, point.X, point.Y, NormalizeYaw(point.VehicleYaw),
+            point.VehicleCurvature);
    }

    private static double Linear(double left, double right, double ratio)
@@ -84,14 +86,16 @@ internal sealed class LateralPathInterpolator

 internal sealed class InterpolatedLateralPathPoint
 {
-    public InterpolatedLateralPathPoint(double x, double y, double yaw, double vehicleCurvature)
+    public InterpolatedLateralPathPoint(double referenceS, double x, double y, double yaw, double vehicleCurvature)
    {
+        ReferenceS = referenceS;
        X = x;
        Y = y;
        Yaw = yaw;
        VehicleCurvature = vehicleCurvature;
    }

+    public double ReferenceS { get; }
    public double X { get; }
    public double Y { get; }
    public double Yaw { get; }
@@ -81,7 +81,7 @@ public sealed class EmTrajectoryValidator
    public EmTrajectoryValidationResult Validate(EmTrajectory trajectory, PlanningGridMap map, VehicleParameters vehicle,
        EmPlannerConfiguration configuration, int segmentIndex, double pathUpperBoundS, EmBoundaryType terminalBoundary)
    {
-        return ValidateCore(trajectory, map, vehicle, configuration, segmentIndex, pathUpperBoundS, null,
+        return ValidateCore(trajectory, map, vehicle, configuration, segmentIndex, double.PositiveInfinity, pathUpperBoundS, null,
            terminalBoundary, false);
    }

@@ -89,12 +89,34 @@ public sealed class EmTrajectoryValidator
        EmPlannerConfiguration configuration, int segmentIndex, double pathUpperBoundS, Pose2D terminalPose,
        EmBoundaryType terminalBoundary)
    {
-        return ValidateCore(trajectory, map, vehicle, configuration, segmentIndex, pathUpperBoundS, terminalPose,
+        return ValidateCore(trajectory, map, vehicle, configuration, segmentIndex, double.PositiveInfinity, pathUpperBoundS, terminalPose,
            terminalBoundary, true);
    }

+    public EmTrajectoryValidationResult Validate(EmTrajectory trajectory, PlanningGridMap map, VehicleParameters vehicle,
+        EmPlannerConfiguration configuration, int segmentIndex, double segmentUpperBoundS, double pathUpperBoundS,
+        EmBoundaryType terminalBoundary)
+    {
+        if (!IsFinite(segmentUpperBoundS) || segmentUpperBoundS < 0d)
+            return EmTrajectoryValidationResult.Reject(EmTrajectoryValidationFailure.InvalidInput, -1,
+                "Segment-local trajectory bounds are invalid.");
+        return ValidateCore(trajectory, map, vehicle, configuration, segmentIndex, segmentUpperBoundS, pathUpperBoundS,
+            null, terminalBoundary, false);
+    }
+
+    public EmTrajectoryValidationResult Validate(EmTrajectory trajectory, PlanningGridMap map, VehicleParameters vehicle,
+        EmPlannerConfiguration configuration, int segmentIndex, double segmentUpperBoundS, double pathUpperBoundS,
+        Pose2D terminalPose, EmBoundaryType terminalBoundary)
+    {
+        if (!IsFinite(segmentUpperBoundS) || segmentUpperBoundS < 0d)
+            return EmTrajectoryValidationResult.Reject(EmTrajectoryValidationFailure.InvalidInput, -1,
+                "Segment-local trajectory bounds are invalid.");
+        return ValidateCore(trajectory, map, vehicle, configuration, segmentIndex, segmentUpperBoundS, pathUpperBoundS,
+            terminalPose, terminalBoundary, true);
+    }
+
    private EmTrajectoryValidationResult ValidateCore(EmTrajectory trajectory, PlanningGridMap map, VehicleParameters vehicle,
-        EmPlannerConfiguration configuration, int segmentIndex, double pathUpperBoundS, Pose2D terminalPose,
+        EmPlannerConfiguration configuration, int segmentIndex, double segmentUpperBoundS, double pathUpperBoundS, Pose2D terminalPose,
        EmBoundaryType terminalBoundary, bool requiresTerminalPose)
    {
        if (trajectory == null || map == null || vehicle == null || configuration == null || configuration.Validation == null ||
@@ -111,7 +133,7 @@ public sealed class EmTrajectoryValidator
            EmTrajectoryPoint point = trajectory.Points[index];
            if (point == null || !HasOnlyFiniteValues(point))
                return Reject(EmTrajectoryValidationFailure.NonFinite, index, "Trajectory contains a non-finite point.");
-            if (point.SegmentIndex != segmentIndex || point.SegmentLocalS > pathUpperBoundS + limits.SpatialTolerance ||
+            if (point.SegmentIndex != segmentIndex || point.SegmentLocalS > segmentUpperBoundS + limits.SpatialTolerance ||
                point.PathS > pathUpperBoundS + limits.SpatialTolerance)
            {
                return Reject(EmTrajectoryValidationFailure.SegmentBoundaryExceeded, index,