feat: keep rolling speed envelopes open

2026-08-05 17:33:50 +08:00
parent 105ec4bdad
commit 94a9be9c02
3 changed files with 132 additions and 77 deletions
@@ -11,7 +11,7 @@ public sealed class PathSpeedLimit

    internal PathSpeedLimit(IReadOnlyList<double> pathS, IReadOnlyList<double> maximumSpeed,
        IReadOnlyList<double> lateralAccelerationLimit, IReadOnlyList<double> curvatureRateLimit,
-        IReadOnlyList<double> stoppingLimit, double directionMaximumSpeedMetersPerSecond)
+        IReadOnlyList<double> stoppingLimit, double directionMaximumSpeedMetersPerSecond, bool hasStopBoundary)
    {
        PathS = CopyStrictStations(pathS, nameof(pathS));
        MaximumSpeedMetersPerSecond = CopyFiniteNonnegative(maximumSpeed, PathS.Count, nameof(maximumSpeed));
@@ -22,13 +22,15 @@ public sealed class PathSpeedLimit
        StoppingSpeedLimitsMetersPerSecond = CopyFiniteNonnegative(stoppingLimit, PathS.Count, nameof(stoppingLimit));
        if (!IsFinite(directionMaximumSpeedMetersPerSecond) || directionMaximumSpeedMetersPerSecond <= 0d)
            throw new ArgumentOutOfRangeException(nameof(directionMaximumSpeedMetersPerSecond));
-        if (MaximumSpeedMetersPerSecond[MaximumSpeedMetersPerSecond.Count - 1] != 0d ||
-            StoppingSpeedLimitsMetersPerSecond[StoppingSpeedLimitsMetersPerSecond.Count - 1] != 0d)
+        if (hasStopBoundary &&
+            (MaximumSpeedMetersPerSecond[MaximumSpeedMetersPerSecond.Count - 1] != 0d ||
+             StoppingSpeedLimitsMetersPerSecond[StoppingSpeedLimitsMetersPerSecond.Count - 1] != 0d))
        {
-            throw new ArgumentException("Terminal PathS speed limits must be exactly zero.");
+            throw new ArgumentException("A real stop boundary must have an exact zero speed limit.");
        }

        DirectionMaximumSpeedMetersPerSecond = directionMaximumSpeedMetersPerSecond;
+        HasStopBoundary = hasStopBoundary;
    }

    public IReadOnlyList<double> PathS { get; }
@@ -43,7 +45,12 @@ public sealed class PathSpeedLimit

    public double DirectionMaximumSpeedMetersPerSecond { get; }

-    public double TerminalPathS { get { return PathS[PathS.Count - 1]; } }
+    public bool HasStopBoundary { get; }
+
+    public double PathUpperBoundS { get { return PathS[PathS.Count - 1]; } }
+
+    [Obsolete("Use PathUpperBoundS.")]
+    public double TerminalPathS { get { return PathUpperBoundS; } }

    public double MaximumSpeedAt(double pathS)
    {
@@ -67,11 +74,11 @@ public sealed class PathSpeedLimit

    private double Interpolate(IReadOnlyList<double> values, double pathS)
    {
-        if (!IsFinite(pathS) || pathS < PathS[0] - StationTolerance || pathS > TerminalPathS + StationTolerance)
+        if (!IsFinite(pathS) || pathS < PathS[0] - StationTolerance || pathS > PathUpperBoundS + StationTolerance)
            throw new ArgumentOutOfRangeException(nameof(pathS));
        if (pathS <= PathS[0])
            return values[0];
-        if (pathS >= TerminalPathS)
+        if (pathS >= PathUpperBoundS)
            return values[values.Count - 1];

        for (int index = 1; index < PathS.Count; index++)
@@ -34,17 +34,20 @@ public sealed class PathSpeedLimitBuilder
            return EmPlanningStatus.InvalidInput;
        }

+        if (input.HasStopBoundary)
+        {
            if (!JerkLimitedStoppingMath.TryCalculate(input.InitialProgressSpeedMetersPerSecond,
                    input.InitialAccelerationMetersPerSecondSquared, maximumDeceleration, maximumJerk,
                    out JerkLimitedStoppingProfile stopProfile, out failureReason))
            {
                return EmPlanningStatus.InvalidInput;
            }
-        if (stopProfile.DistanceMeters + StopDistanceToleranceMeters > input.TerminalPathS)
+            if (stopProfile.DistanceMeters + StopDistanceToleranceMeters > input.StopBoundaryPathS)
            {
                failureReason = "The available actual PathS distance is insufficient for the jerk-limited stop.";
                return EmPlanningStatus.StoppingDistanceInsufficient;
            }
+        }
        if (input.Configuration.Scheduling == null || !IsPositiveFinite(input.Configuration.Scheduling.OutputTimeStepSeconds))
        {
            failureReason = "The output time step required to refine the PathS speed envelope is invalid.";
@@ -66,9 +69,12 @@ public sealed class PathSpeedLimitBuilder
            var segmentStations = new List<double>(subdivisions + 16);
            for (int subdivision = segmentIndex == 0 ? 0 : 1; subdivision <= subdivisions; subdivision++)
                segmentStations.Add(Interpolate(lowerPoint.PathS, upperPoint.PathS, (double)subdivision / subdivisions));
-            AddDiscreteStoppingTailStations(lowerPoint.PathS, upperPoint.PathS, input.TerminalPathS, directionMaximum,
-                maximumDeceleration, maximumJerk, input.Configuration.Scheduling.OutputTimeStepSeconds,
+            if (input.HasStopBoundary)
+            {
+                AddJerkLimitedStoppingStations(lowerPoint.PathS, upperPoint.PathS, input.StopBoundaryPathS,
+                    directionMaximum, maximumAcceleration, maximumDeceleration, maximumJerk,
                    segmentIndex == 0, segmentStations);
+            }
            segmentStations.Sort();
            double previousStation = double.NegativeInfinity;
            for (int stationIndex = 0; stationIndex < segmentStations.Count; stationIndex++)
@@ -81,15 +87,17 @@ public sealed class PathSpeedLimitBuilder
                double curvature = Interpolate(lowerPoint.VehicleCurvature, upperPoint.VehicleCurvature, fraction);
                double curvatureDerivative = Interpolate(lowerPoint.VehicleCurvatureDerivative,
                    upperPoint.VehicleCurvatureDerivative, fraction);
-                AddLimitSample(samplePathS, curvature, curvatureDerivative, input.TerminalPathS, directionMaximum,
-                    maximumDeceleration, maximumLateralAcceleration, maximumCurvatureRate, pathS, maximum, lateral,
+                AddLimitSample(samplePathS, curvature, curvatureDerivative, input.HasStopBoundary,
+                    input.StopBoundaryPathS, directionMaximum, maximumAcceleration, maximumDeceleration,
+                    maximumJerk, maximumLateralAcceleration, maximumCurvatureRate, pathS, maximum, lateral,
                    curvatureRate, stopping);
            }
        }

        try
        {
-            speedLimit = new PathSpeedLimit(pathS, maximum, lateral, curvatureRate, stopping, directionMaximum);
+            speedLimit = new PathSpeedLimit(pathS, maximum, lateral, curvatureRate, stopping, directionMaximum,
+                input.HasStopBoundary);
            return EmPlanningStatus.Success;
        }
        catch (ArgumentException exception)
@@ -133,34 +141,20 @@ public sealed class PathSpeedLimitBuilder
        return true;
    }

-    private static void AddDiscreteStoppingTailStations(double lowerPathS, double upperPathS, double terminalPathS,
-        double directionMaximum, double maximumDeceleration, double maximumJerk, double timeStep, bool includeLower,
-        IList<double> stations)
+    private static void AddJerkLimitedStoppingStations(double lowerPathS, double upperPathS,
+        double stopBoundaryPathS, double directionMaximum, double maximumAcceleration, double maximumDeceleration,
+        double maximumJerk, bool includeLower, IList<double> stations)
    {
-        double speedIncrement = maximumDeceleration * timeStep;
-        int tailStationCount = Math.Max(1, checked((int)Math.Ceiling(directionMaximum / speedIncrement)));
-        for (int step = 1; step <= tailStationCount; step++)
+        const int stoppingSpeedSampleCount = 64;
+        for (int step = 0; step < stoppingSpeedSampleCount; step++)
        {
-            double stopDuration = step * timeStep;
-            double remainingDistance = 0.5d * maximumDeceleration * stopDuration * stopDuration;
-            if (remainingDistance >= terminalPathS)
-                break;
-            double station = terminalPathS - remainingDistance;
-            bool aboveLower = includeLower
-                ? station >= lowerPathS - StationMergeToleranceMeters
-                : station > lowerPathS + StationMergeToleranceMeters;
-            if (aboveLower && station <= upperPathS + StationMergeToleranceMeters)
-                stations.Add(Math.Max(lowerPathS, Math.Min(upperPathS, station)));
+            double speed = directionMaximum * step / stoppingSpeedSampleCount;
+            if (!JerkLimitedStoppingMath.TryCalculate(speed, maximumAcceleration,
+                    maximumDeceleration, maximumJerk, out JerkLimitedStoppingProfile stop, out _))
+            {
+                throw new ArgumentException("The configured jerk-limited stop envelope cannot be sampled.");
            }
-        int jerkTailStationCount = Math.Max(1, checked((int)Math.Ceiling(
-            maximumDeceleration / (maximumJerk * timeStep))));
-        for (int step = 1; step <= jerkTailStationCount; step++)
-        {
-            double releaseDuration = step * timeStep;
-            double remainingDistance = maximumJerk * releaseDuration * releaseDuration * releaseDuration / 6d;
-            if (remainingDistance >= terminalPathS)
-                break;
-            double station = terminalPathS - remainingDistance;
+            double station = stopBoundaryPathS - stop.DistanceMeters;
            bool aboveLower = includeLower
                ? station >= lowerPathS - StationMergeToleranceMeters
                : station > lowerPathS + StationMergeToleranceMeters;
@@ -169,14 +163,20 @@ public sealed class PathSpeedLimitBuilder
        }
    }

-    private static void AddLimitSample(double samplePathS, double curvature, double curvatureDerivative, double terminalPathS,
-        double directionMaximum, double maximumDeceleration, double maximumLateralAcceleration, double maximumCurvatureRate,
-        IList<double> pathS, IList<double> maximum, IList<double> lateral, IList<double> curvatureRate, IList<double> stopping)
+    private static void AddLimitSample(double samplePathS, double curvature, double curvatureDerivative,
+        bool hasStopBoundary, double stopBoundaryPathS, double directionMaximum, double maximumAcceleration,
+        double maximumDeceleration, double maximumJerk, double maximumLateralAcceleration,
+        double maximumCurvatureRate, IList<double> pathS, IList<double> maximum, IList<double> lateral,
+        IList<double> curvatureRate, IList<double> stopping)
    {
-        bool terminal = samplePathS >= terminalPathS;
+        bool terminal = hasStopBoundary && samplePathS >= stopBoundaryPathS;
        double lateralLimit = Math.Sqrt(maximumLateralAcceleration / Math.Max(Math.Abs(curvature), CurvatureEpsilon));
        double curvatureRateLimit = maximumCurvatureRate / Math.Max(Math.Abs(curvatureDerivative), CurvatureEpsilon);
-        double stoppingLimit = Math.Sqrt(2d * maximumDeceleration * Math.Max(0d, terminalPathS - samplePathS));
+        double stoppingLimit = hasStopBoundary
+            ? JerkLimitedStoppingMath.MaximumInitialSpeedForDistance(
+                Math.Max(0d, stopBoundaryPathS - samplePathS), maximumAcceleration,
+                maximumDeceleration, maximumJerk, directionMaximum)
+            : directionMaximum;
        double lateralValue = ClampFinite(lateralLimit, directionMaximum);
        double curvatureRateValue = ClampFinite(curvatureRateLimit, directionMaximum);
        double stoppingValue = terminal ? 0d : ClampFinite(stoppingLimit, directionMaximum);
@@ -12,10 +12,11 @@ internal static class LongitudinalModelChecks
    {
        VerifiesJerkLimitedStoppingProfileEndsAtRest();
        VerifiesStoppedReachabilityUsesTheSameJerkModel();
+        VerifiesRollingEnvelopeDoesNotStopAtWindowEnd();
        VerifiesFinitePathSIndexedSpeedEnvelope();
        VerifiesStoppingEnvelopeIsRefinedOnActualPathS();
-        VerifiesStoppingEnvelopeUsesDiscreteTimeTailStations();
-        VerifiesStoppingPrecheckBeforeQpAssembly();
+        VerifiesStoppingEnvelopeUsesJerkLimitedStoppingMath();
+        VerifiesStoppingPrecheckOnlyAppliesToRealStopBoundaries();
        VerifiesReferenceHorizonSelectionSeparatesSpaceAndTime();
        VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints();
    }
@@ -56,6 +57,40 @@ internal static class LongitudinalModelChecks
            "distance inversion stays inside the direction speed range");
    }

+    private static void VerifiesRollingEnvelopeDoesNotStopAtWindowEnd()
+    {
+        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+        LateralPath path = CreatePath(new[]
+        {
+            new PathFixture(0d, 0d, 0d, 0d),
+            new PathFixture(1d, 1d, 0d, 0d),
+        });
+        var rolling = new LongitudinalPlanningInput(path, TravelDirection.Forward,
+            0d, 0d, EmTerminalType.RollingSafetyStop,
+            EmLongitudinalMode.RollingContinuation, configuration,
+            Array.Empty<double>(), Array.Empty<double>());
+
+        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(
+            rolling, out PathSpeedLimit envelope, out string failure);
+
+        Verification.Equal(EmPlanningStatus.Success, status, "rolling envelope: " + failure);
+        Verification.True(envelope.MaximumSpeedAt(rolling.PathUpperBoundS) > 0d,
+            "rolling window end keeps a nonzero speed allowance");
+        Verification.True(!envelope.HasStopBoundary, "rolling envelope has no stop boundary");
+        Verification.NearlyEqual(rolling.PathUpperBoundS, envelope.PathUpperBoundS,
+            "rolling envelope reports its PathS upper bound");
+
+        var approach = new LongitudinalPlanningInput(path, TravelDirection.Forward,
+            0d, 0d, EmTerminalType.Goal,
+            EmLongitudinalMode.ApproachStopBoundary, configuration,
+            Array.Empty<double>(), Array.Empty<double>());
+        status = new PathSpeedLimitBuilder().Build(approach, out PathSpeedLimit approachEnvelope, out failure);
+        Verification.Equal(EmPlanningStatus.Success, status, "approach envelope: " + failure);
+        Verification.True(approachEnvelope.HasStopBoundary, "approach envelope retains its real stop boundary");
+        Verification.NearlyEqual(0d, approachEnvelope.StoppingLimitAt(approach.StopBoundaryPathS),
+            "approach stop boundary has an exact zero stopping limit");
+    }
+
    private static void VerifiesFinitePathSIndexedSpeedEnvelope()
    {
        LateralPath directionPath = CreatePath(new[]
@@ -96,8 +131,8 @@ internal static class LongitudinalModelChecks
        Verification.NearlyEqual(0.50d / 4d, envelope.CurvatureRateLimitAt(2d), "curvature-rate limit");
        Verification.True(double.IsFinite(envelope.LateralAccelerationLimitAt(0d)) &&
            double.IsFinite(envelope.CurvatureRateLimitAt(0d)), "zero curvature limits stay finite");
-        Verification.NearlyEqual(Math.Sqrt(2d * 0.30d * (5d - 4d)), envelope.StoppingLimitAt(4d),
-            "stopping speed limit");
+        Verification.NearlyEqual(MaximumJerkLimitedStopSpeed(input, 4d), envelope.StoppingLimitAt(4d),
+            "stopping speed limit uses the complete jerk-limited model");
        Verification.NearlyEqual(Math.Sqrt(0.20d / 20d), envelope.MaximumSpeedAt(4d),
            "combined limit chooses the finite minimum");
        double interpolationQueryPathS = 1.013d;
@@ -133,11 +168,11 @@ internal static class LongitudinalModelChecks
            out string failureReason);
        Verification.Equal(EmPlanningStatus.Success, status, "refined stopping envelope status: " + failureReason);
        Verification.True(envelope.PathS.Count > path.Points.Count, "stopping envelope inserts actual-PathS refinement stations");
-        Verification.NearlyEqual(Math.Sqrt(2d * 0.30d * (2d - 1.5d)), envelope.MaximumSpeedAt(1.5d),
-            "refined stopping envelope avoids a sparse terminal chord");
+        Verification.NearlyEqual(MaximumJerkLimitedStopSpeed(input, 1.5d), envelope.MaximumSpeedAt(1.5d),
+            "refined stopping envelope uses the complete jerk-limited stopping cap");
    }

-    private static void VerifiesStoppingEnvelopeUsesDiscreteTimeTailStations()
+    private static void VerifiesStoppingEnvelopeUsesJerkLimitedStoppingMath()
    {
        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
        configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
@@ -153,22 +188,16 @@ internal static class LongitudinalModelChecks

        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
            out string failureReason);
-        Verification.Equal(EmPlanningStatus.Success, status, "discrete stopping-tail status: " + failureReason);
-        double timeStep = configuration.Scheduling.OutputTimeStepSeconds;
-        double deceleration = configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared;
-        double firstTailDistance = 0.5d * deceleration * timeStep * timeStep;
-        double firstTailPathS = input.TerminalPathS - firstTailDistance;
-        Verification.NearlyEqual(deceleration * timeStep, envelope.StoppingLimitAt(firstTailPathS),
-            "first stopping-tail station matches one discrete deceleration time step");
-        double jerk = configuration.Longitudinal.MaximumJerkMetersPerSecondCubed;
-        double firstJerkTailDistance = jerk * timeStep * timeStep * timeStep / 6d;
-        double firstJerkTailPathS = input.TerminalPathS - firstJerkTailDistance;
-        Verification.NearlyEqual(Math.Sqrt(2d * deceleration * firstJerkTailDistance),
-            envelope.StoppingLimitAt(firstJerkTailPathS),
-            "first stopping-tail station matches one discrete jerk-release time step");
+        Verification.Equal(EmPlanningStatus.Success, status, "jerk-limited stopping-tail status: " + failureReason);
+        double nearBoundaryPathS = input.StopBoundaryPathS - 0.005d;
+        Verification.NearlyEqual(MaximumJerkLimitedStopSpeed(input, nearBoundaryPathS),
+            envelope.StoppingLimitAt(nearBoundaryPathS),
+            "near-boundary speed cap uses jerk-limited distance inversion");
+        Verification.NearlyEqual(0d, envelope.StoppingLimitAt(input.StopBoundaryPathS),
+            "real stop boundary keeps an exact zero stopping cap");
    }

-    private static void VerifiesStoppingPrecheckBeforeQpAssembly()
+    private static void VerifiesStoppingPrecheckOnlyAppliesToRealStopBoundaries()
    {
        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
        LateralPath shortPath = CreatePath(new[]
@@ -176,14 +205,22 @@ internal static class LongitudinalModelChecks
            new PathFixture(0d, 0d, 0d, 0d),
            new PathFixture(100d, 0.01d, 0d, 0d),
        });
-        var input = new LongitudinalPlanningInput(shortPath, TravelDirection.Forward, 0.20d, 0.20d,
+        var rolling = new LongitudinalPlanningInput(shortPath, TravelDirection.Forward, 0.20d, 0.20d,
            EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration,
            Array.Empty<double>(), Array.Empty<double>());

-        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
+        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(rolling, out PathSpeedLimit envelope,
            out string failureReason);
+        Verification.Equal(EmPlanningStatus.Success, status,
+            "rolling windows do not require a stop inside their local PathS extent: " + failureReason);
+        Verification.True(envelope != null, "rolling speed envelope is created despite the short local window");
+
+        var approach = new LongitudinalPlanningInput(shortPath, TravelDirection.Forward, 0.20d, 0.20d,
+            EmTerminalType.Goal, EmLongitudinalMode.ApproachStopBoundary, configuration,
+            Array.Empty<double>(), Array.Empty<double>());
+        status = new PathSpeedLimitBuilder().Build(approach, out envelope, out failureReason);
        Verification.Equal(EmPlanningStatus.StoppingDistanceInsufficient, status,
-            "jerk/deceleration stopping precheck status");
+            "real stop-boundary jerk/deceleration stopping precheck status");
        Verification.True(envelope == null, "stopping-distance failure does not create a speed envelope");
        Verification.True(failureReason.Length != 0, "stopping-distance failure explains the rejection");
    }
@@ -375,6 +412,17 @@ internal static class LongitudinalModelChecks
        return configuration;
    }

+    private static double MaximumJerkLimitedStopSpeed(LongitudinalPlanningInput input, double pathS)
+    {
+        LongitudinalConfiguration limits = input.Configuration.Longitudinal;
+        return JerkLimitedStoppingMath.MaximumInitialSpeedForDistance(
+            Math.Max(0d, input.StopBoundaryPathS - pathS),
+            limits.MaximumAccelerationMetersPerSecondSquared,
+            limits.MaximumDecelerationMetersPerSecondSquared,
+            limits.MaximumJerkMetersPerSecondCubed,
+            input.DirectionMaximumSpeedMetersPerSecond);
+    }
+
    private static double MatrixValue(SparseCscMatrix matrix, int row, int column)
    {
        for (int index = matrix.ColumnPointers[column]; index < matrix.ColumnPointers[column + 1]; index++)