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
@@ -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)