feat: separate rolling horizons from stop boundaries

2026-08-05 17:13:09 +08:00
parent e56220fc29
commit 21b20d09d4
9 changed files with 246 additions and 96 deletions
@@ -0,0 +1,8 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public enum EmLongitudinalMode
+{
+    RollingContinuation,
+    ApproachStopBoundary,
+    ExactStopAtBoundary,
+}
@@ -58,11 +58,11 @@ public sealed class EmPlanningService : IEmPlanningService
                initialAcceleration, configuration, out PlanningHorizonSelection horizon, out string horizonReason);
            if (horizonStatus != EmPlanningStatus.Success)
                return Failure(horizonStatus, request, horizonReason);
-            ReferenceHorizonSlice slice = ReferenceHorizonSlicer.Slice(segment, horizon.TerminalReferenceS);
+            ReferenceHorizonSlice slice = ReferenceHorizonSlicer.Slice(segment, horizon.WindowEndReferenceS);
            EmitDebug(request, "exact horizon and terminal selection succeeded");

            IReadOnlyList<FrenetProjection> previousSeed = ProjectPreviousTrajectorySeed(request.PreviousTrajectory, segment,
-                startProjection.ReferenceS, horizon.TerminalReferenceS, configuration.Frenet.MaximumProjectionDistanceMeters);
+                startProjection.ReferenceS, horizon.WindowEndReferenceS, configuration.Frenet.MaximumProjectionDistanceMeters);
            var corridorSeed = new List<FrenetProjection>(previousSeed.Count + 1) { startProjection };
            for (int index = 0; index < previousSeed.Count; index++) corridorSeed.Add(previousSeed[index]);
            EmitDebug(request, "previous-trajectory seed projection completed");
@@ -83,7 +83,8 @@ public sealed class EmPlanningService : IEmPlanningService
            EmitDebug(request, "LS optimization and validation succeeded");

            var longitudinalInput = new LongitudinalPlanningInput(lateral.Path, segment.Direction, initialProgressSpeed,
-                initialAcceleration, horizon.TerminalType, configuration, Array.Empty<double>(), Array.Empty<double>());
+                initialAcceleration, horizon.TerminalType, horizon.LongitudinalMode, configuration,
+                Array.Empty<double>(), Array.Empty<double>());
            EmPlanningStatus envelopeStatus = new PathSpeedLimitBuilder().Build(longitudinalInput, out _, out string envelopeReason);
            if (envelopeStatus != EmPlanningStatus.Success)
                return Failure(envelopeStatus, request, envelopeReason);
@@ -11,7 +11,8 @@ public sealed class LongitudinalPlanningInput
    private const double PathSTolerance = 1e-12d;

    public LongitudinalPlanningInput(LateralPath path, TravelDirection direction, double initialProgressSpeedMetersPerSecond,
-        double initialAccelerationMetersPerSecondSquared, EmTerminalType terminalType, EmPlannerConfiguration configuration,
+        double initialAccelerationMetersPerSecondSquared, EmTerminalType terminalType, EmLongitudinalMode mode,
+        EmPlannerConfiguration configuration,
        IReadOnlyList<double> previousPathS, IReadOnlyList<double> previousProgressSpeedMetersPerSecond)
    {
        if (path == null || !path.IsIndependentlyValidated || path.Points.Count < 2)
@@ -25,6 +26,12 @@ public sealed class LongitudinalPlanningInput
            throw new ArgumentOutOfRangeException(nameof(initialAccelerationMetersPerSecondSquared));
        if (!Enum.IsDefined(typeof(EmTerminalType), terminalType))
            throw new ArgumentOutOfRangeException(nameof(terminalType));
+        if (!Enum.IsDefined(typeof(EmLongitudinalMode), mode))
+            throw new ArgumentOutOfRangeException(nameof(mode));
+        if (mode == EmLongitudinalMode.RollingContinuation && terminalType != EmTerminalType.RollingSafetyStop)
+            throw new ArgumentException("Rolling continuation requires a rolling window boundary.");
+        if (mode != EmLongitudinalMode.RollingContinuation && terminalType == EmTerminalType.RollingSafetyStop)
+            throw new ArgumentException("Stop-boundary modes require Goal or GearSwitch.");
        if (configuration == null)
            throw new ArgumentNullException(nameof(configuration));

@@ -33,6 +40,7 @@ public sealed class LongitudinalPlanningInput
        InitialProgressSpeedMetersPerSecond = initialProgressSpeedMetersPerSecond;
        InitialAccelerationMetersPerSecondSquared = initialAccelerationMetersPerSecondSquared;
        TerminalType = terminalType;
+        Mode = mode;
        Configuration = configuration.Copy();
        PreviousPathS = CopyFiniteNonnegative(previousPathS, nameof(previousPathS));
        PreviousProgressSpeedMetersPerSecond = CopyFiniteNonnegative(previousProgressSpeedMetersPerSecond,
@@ -42,6 +50,17 @@ public sealed class LongitudinalPlanningInput
                nameof(previousProgressSpeedMetersPerSecond));
    }

+    public LongitudinalPlanningInput(LateralPath path, TravelDirection direction, double initialProgressSpeedMetersPerSecond,
+        double initialAccelerationMetersPerSecondSquared, EmTerminalType terminalType, EmPlannerConfiguration configuration,
+        IReadOnlyList<double> previousPathS, IReadOnlyList<double> previousProgressSpeedMetersPerSecond)
+        : this(path, direction, initialProgressSpeedMetersPerSecond, initialAccelerationMetersPerSecondSquared, terminalType,
+            terminalType == EmTerminalType.RollingSafetyStop
+                ? EmLongitudinalMode.RollingContinuation
+                : EmLongitudinalMode.ExactStopAtBoundary,
+            configuration, previousPathS, previousProgressSpeedMetersPerSecond)
+    {
+    }
+
    public LateralPath Path { get; }

    public TravelDirection Direction { get; }
@@ -52,13 +71,33 @@ public sealed class LongitudinalPlanningInput

    public EmTerminalType TerminalType { get; }

+    public EmLongitudinalMode Mode { get; }
+
    public EmPlannerConfiguration Configuration { get; }

    public IReadOnlyList<double> PreviousPathS { get; }

    public IReadOnlyList<double> PreviousProgressSpeedMetersPerSecond { get; }

-    public double TerminalPathS { get { return Path.Points[Path.Points.Count - 1].PathS; } }
+    public double PathUpperBoundS { get { return Path.Points[Path.Points.Count - 1].PathS; } }
+
+    public bool HasStopBoundary { get { return TerminalType != EmTerminalType.RollingSafetyStop; } }
+
+    public double StopBoundaryPathS { get { return HasStopBoundary ? PathUpperBoundS : double.NaN; } }
+
+    public EmBoundaryType StopBoundaryType
+    {
+        get
+        {
+            return TerminalType == EmTerminalType.Goal
+                ? EmBoundaryType.Goal
+                : TerminalType == EmTerminalType.GearSwitch
+                    ? EmBoundaryType.GearSwitchApproach
+                    : EmBoundaryType.None;
+        }
+    }
+
+    public double TerminalPathS { get { return PathUpperBoundS; } }

    public double DirectionMaximumSpeedMetersPerSecond
    {
@@ -0,0 +1,39 @@
+using System;
+using System.Collections.Generic;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public static class LongitudinalTerminalSchedule
+{
+    public static int GetStabilizationStartIndex(IReadOnlyList<double> knotTimes,
+        double minimumStabilizationDurationSeconds)
+    {
+        if (knotTimes == null || knotTimes.Count < 3 ||
+            double.IsNaN(minimumStabilizationDurationSeconds) ||
+            double.IsInfinity(minimumStabilizationDurationSeconds) ||
+            minimumStabilizationDurationSeconds <= 0d)
+        {
+            throw new ArgumentException("A positive stabilization tail and at least three knots are required.");
+        }
+
+        double previous = double.NegativeInfinity;
+        for (int index = 0; index < knotTimes.Count; index++)
+        {
+            if (double.IsNaN(knotTimes[index]) || double.IsInfinity(knotTimes[index]) ||
+                knotTimes[index] <= previous)
+            {
+                throw new ArgumentException("Terminal-schedule knots must be finite and strictly increasing.");
+            }
+            previous = knotTimes[index];
+        }
+
+        double finalTime = knotTimes[knotTimes.Count - 1];
+        for (int index = knotTimes.Count - 2; index >= 1; index--)
+        {
+            if (finalTime - knotTimes[index] >= minimumStabilizationDurationSeconds - 1e-12d)
+                return index;
+        }
+
+        throw new ArgumentException("The time horizon cannot contain a full terminal stabilization interval.");
+    }
+}
@@ -1,4 +1,5 @@
 using System;
+using System.Collections.Generic;
 using MultiWheelC.TrajectoryPlanning.CoarsePath;

 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
@@ -6,15 +7,31 @@ namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 /// <summary>Reference-distance terminal chosen before LS without crossing the current direction segment.</summary>
 public sealed class PlanningHorizonSelection
 {
-    internal PlanningHorizonSelection(double terminalReferenceS, EmTerminalType terminalType)
+    internal PlanningHorizonSelection(double windowEndReferenceS, EmBoundaryType windowEndBoundaryType,
+        EmTerminalType terminalType, EmLongitudinalMode longitudinalMode,
+        double stopBoundaryReferenceS, bool hasStopBoundary)
    {
-        TerminalReferenceS = terminalReferenceS;
+        WindowEndReferenceS = windowEndReferenceS;
+        WindowEndBoundaryType = windowEndBoundaryType;
        TerminalType = terminalType;
+        LongitudinalMode = longitudinalMode;
+        StopBoundaryReferenceS = stopBoundaryReferenceS;
+        HasStopBoundary = hasStopBoundary;
    }

-    public double TerminalReferenceS { get; }
+    public double WindowEndReferenceS { get; }
+
+    public double TerminalReferenceS => WindowEndReferenceS;
+
+    public EmBoundaryType WindowEndBoundaryType { get; }

    public EmTerminalType TerminalType { get; }
+
+    public EmLongitudinalMode LongitudinalMode { get; }
+
+    public double StopBoundaryReferenceS { get; }
+
+    public bool HasStopBoundary { get; }
 }

 public sealed class PlanningHorizonSelector
@@ -73,75 +90,41 @@ public sealed class PlanningHorizonSelector
            return EmPlanningStatus.StoppingDistanceInsufficient;
        }

-        double timeReachable = CalculateReachableDistance(initialProgressSpeedMetersPerSecond,
-            initialAccelerationMetersPerSecondSquared, directionMaximum, longitudinal, scheduling.TimeHorizonSeconds);
-        double terminalReferenceS = currentSegmentReferenceS + Math.Min(remainingSegment,
-            Math.Min(scheduling.DistanceHorizonMeters, timeReachable));
-        if (terminalReferenceS >= segment.LengthMeters - BoundaryTolerance)
+        double windowEnd = Math.Min(currentSegmentReferenceS + scheduling.DistanceHorizonMeters, segment.LengthMeters);
+        bool windowReachesSegmentEnd = windowEnd >= segment.LengthMeters - BoundaryTolerance;
+        bool hasStopBoundary = windowReachesSegmentEnd && IsStopBoundary(segment.EndBoundary.BoundaryType);
+        EmBoundaryType windowEndBoundaryType = windowReachesSegmentEnd
+            ? segment.EndBoundary.BoundaryType
+            : EmBoundaryType.RollingSafetyStop;
+        if (!hasStopBoundary)
        {
-            terminalReferenceS = segment.LengthMeters;
-            selection = new PlanningHorizonSelection(terminalReferenceS, ToTerminalType(segment.EndBoundary.BoundaryType));
-        }
-        else
-        {
-            selection = new PlanningHorizonSelection(terminalReferenceS, EmTerminalType.RollingSafetyStop);
+            selection = new PlanningHorizonSelection(windowEnd, windowEndBoundaryType,
+                EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation,
+                segment.LengthMeters, false);
+            return EmPlanningStatus.Success;
        }
+
+        IReadOnlyList<double> knotTimes = LongitudinalCandidate.CreateKnotTimes(
+            scheduling.TimeHorizonSeconds, scheduling.OutputTimeStepSeconds);
+        int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
+            knotTimes, scheduling.OutputTimeStepSeconds);
+        double availableMotionTime = knotTimes[stabilizationStart];
+        double maximumStoppedDistance = JerkLimitedStoppingMath.CalculateMaximumStoppedDistance(
+            initialProgressSpeedMetersPerSecond, initialAccelerationMetersPerSecondSquared, directionMaximum,
+            longitudinal.MaximumAccelerationMetersPerSecondSquared,
+            longitudinal.MaximumDecelerationMetersPerSecondSquared,
+            longitudinal.MaximumJerkMetersPerSecondCubed, availableMotionTime);
+        EmLongitudinalMode mode = remainingSegment <= maximumStoppedDistance + BoundaryTolerance
+            ? EmLongitudinalMode.ExactStopAtBoundary
+            : EmLongitudinalMode.ApproachStopBoundary;
+        selection = new PlanningHorizonSelection(segment.LengthMeters, windowEndBoundaryType,
+            ToTerminalType(segment.EndBoundary.BoundaryType), mode, segment.LengthMeters, true);
        return EmPlanningStatus.Success;
    }

-    private static double CalculateReachableDistance(double initialSpeed, double initialAcceleration, double maximumSpeed,
-        LongitudinalConfiguration configuration, double timeHorizonSeconds)
+    private static bool IsStopBoundary(EmBoundaryType boundaryType)
    {
-        if (!JerkLimitedStoppingMath.TryCalculate(maximumSpeed, 0d,
-                configuration.MaximumDecelerationMetersPerSecondSquared,
-                configuration.MaximumJerkMetersPerSecondCubed,
-                out JerkLimitedStoppingProfile stopAtMaximumSpeed, out _))
-        {
-            throw new ArgumentOutOfRangeException(nameof(configuration));
-        }
-        double drivingDuration = timeHorizonSeconds - configuration.ZeroSpeedHoldSeconds - stopAtMaximumSpeed.DurationSeconds;
-        if (drivingDuration <= 0d)
-            return Math.Min(initialSpeed, maximumSpeed) * Math.Max(0d, timeHorizonSeconds - configuration.ZeroSpeedHoldSeconds);
-
-        double speed = initialSpeed;
-        double acceleration = initialAcceleration;
-        double distance = 0d;
-        const double simulationStepSeconds = 0.001d;
-        while (drivingDuration > 0d)
-        {
-            double step = Math.Min(simulationStepSeconds, drivingDuration);
-            double jerk = ChooseAccelerationJerk(speed, acceleration, maximumSpeed,
-                configuration.MaximumAccelerationMetersPerSecondSquared, configuration.MaximumJerkMetersPerSecondCubed);
-            double nextSpeed = speed + acceleration * step + 0.5d * jerk * step * step;
-            if (nextSpeed > maximumSpeed)
-            {
-                nextSpeed = maximumSpeed;
-                acceleration = 0d;
-                jerk = 0d;
-            }
-            distance += speed * step + 0.5d * acceleration * step * step + jerk * step * step * step / 6d;
-            acceleration += jerk * step;
-            speed = Math.Max(0d, nextSpeed);
-            drivingDuration -= step;
-        }
-        return Math.Max(0d, distance + stopAtMaximumSpeed.DistanceMeters);
-    }
-
-    private static double ChooseAccelerationJerk(double speed, double acceleration, double maximumSpeed,
-        double maximumAcceleration, double maximumJerk)
-    {
-        if (speed >= maximumSpeed - BoundaryTolerance)
-        {
-            if (acceleration > 0d)
-                return -maximumJerk;
-            return acceleration < 0d ? maximumJerk : 0d;
-        }
-        double speedToReduceAccelerationToZero = acceleration > 0d
-            ? acceleration * acceleration / (2d * maximumJerk)
-            : 0d;
-        if (speed + speedToReduceAccelerationToZero >= maximumSpeed - BoundaryTolerance)
-            return acceleration > 0d ? -maximumJerk : 0d;
-        return acceleration < maximumAcceleration - BoundaryTolerance ? maximumJerk : 0d;
+        return boundaryType == EmBoundaryType.Goal || boundaryType == EmBoundaryType.GearSwitchApproach;
    }

    private static EmTerminalType ToTerminalType(EmBoundaryType boundaryType)
@@ -1,4 +1,5 @@
 using System;
+using System.Collections.Generic;
 using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
 using MultiWheelC.TrajectoryPlanning.PathSmoothing;
 using MultiWheelC.TrajectoryPlanning.Utils;
@@ -142,6 +143,45 @@ public static class EmPlanningRequestValidator
            !WeightsAreValid(configuration.Lateral.Weights) || !WeightsAreValid(configuration.Longitudinal.Weights))
            return false;

+        if (!JerkLimitedStoppingMath.TryCalculate(longitudinal.MaximumForwardSpeedMetersPerSecond,
+                longitudinal.MaximumAccelerationMetersPerSecondSquared,
+                longitudinal.MaximumDecelerationMetersPerSecondSquared,
+                longitudinal.MaximumJerkMetersPerSecondCubed, out JerkLimitedStoppingProfile forwardStop,
+                out _) ||
+            !JerkLimitedStoppingMath.TryCalculate(longitudinal.MaximumReverseSpeedMetersPerSecond,
+                longitudinal.MaximumAccelerationMetersPerSecondSquared,
+                longitudinal.MaximumDecelerationMetersPerSecondSquared,
+                longitudinal.MaximumJerkMetersPerSecondCubed, out JerkLimitedStoppingProfile reverseStop,
+                out _))
+        {
+            failureReason = "Configuration cannot construct the jerk-limited stopping model.";
+            return false;
+        }
+
+        double maximumSpeed = Math.Max(longitudinal.MaximumForwardSpeedMetersPerSecond,
+            longitudinal.MaximumReverseSpeedMetersPerSecond);
+        double requiredDistanceHorizon = Math.Max(forwardStop.DistanceMeters, reverseStop.DistanceMeters) +
+            maximumSpeed * scheduling.ReplanPeriodSeconds;
+        if (scheduling.DistanceHorizonMeters + 1e-12d < requiredDistanceHorizon)
+        {
+            failureReason = "DistanceHorizonMeters is insufficient: required=" + requiredDistanceHorizon +
+                ";configured=" + scheduling.DistanceHorizonMeters;
+            return false;
+        }
+
+        try
+        {
+            IReadOnlyList<double> knotTimes = LongitudinalCandidate.CreateKnotTimes(
+                scheduling.TimeHorizonSeconds, scheduling.OutputTimeStepSeconds);
+            LongitudinalTerminalSchedule.GetStabilizationStartIndex(knotTimes,
+                scheduling.OutputTimeStepSeconds);
+        }
+        catch (ArgumentException exception)
+        {
+            failureReason = exception.Message;
+            return false;
+        }
+
        return true;
    }