feat: build EM path speed limits

2026-08-04 09:16:17 +08:00
parent e91dbceb0b
commit 62ea9db8cd
6 changed files with 734 additions and 1 deletions
@@ -0,0 +1,156 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+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)
+    {
+        TerminalReferenceS = terminalReferenceS;
+        TerminalType = terminalType;
+    }
+
+    public double TerminalReferenceS { get; }
+
+    public EmTerminalType TerminalType { get; }
+}
+
+public sealed class PlanningHorizonSelector
+{
+    private const double BoundaryTolerance = 1e-8d;
+
+    public EmPlanningStatus Select(DirectionSegmentView segment, double currentSegmentReferenceS,
+        double initialProgressSpeedMetersPerSecond, double initialAccelerationMetersPerSecondSquared,
+        EmPlannerConfiguration configuration, out PlanningHorizonSelection selection, out string failureReason)
+    {
+        selection = null;
+        failureReason = string.Empty;
+        if (segment == null || configuration == null || configuration.Scheduling == null || configuration.Longitudinal == null ||
+            !IsFinite(currentSegmentReferenceS) || currentSegmentReferenceS < 0d ||
+            currentSegmentReferenceS > segment.LengthMeters + BoundaryTolerance ||
+            !IsFinite(initialProgressSpeedMetersPerSecond) || initialProgressSpeedMetersPerSecond < 0d ||
+            !IsFinite(initialAccelerationMetersPerSecondSquared))
+        {
+            failureReason = "Planning horizon inputs are invalid.";
+            return EmPlanningStatus.InvalidInput;
+        }
+
+        LongitudinalConfiguration longitudinal = configuration.Longitudinal;
+        SchedulingConfiguration scheduling = configuration.Scheduling;
+        double directionMaximum = segment.Direction == TravelDirection.Forward
+            ? longitudinal.MaximumForwardSpeedMetersPerSecond
+            : longitudinal.MaximumReverseSpeedMetersPerSecond;
+        if (!IsPositiveFinite(directionMaximum) || !IsPositiveFinite(longitudinal.MaximumAccelerationMetersPerSecondSquared) ||
+            !IsPositiveFinite(longitudinal.MaximumDecelerationMetersPerSecondSquared) ||
+            !IsPositiveFinite(longitudinal.MaximumJerkMetersPerSecondCubed) ||
+            !IsPositiveFinite(longitudinal.ZeroSpeedHoldSeconds) || !IsPositiveFinite(scheduling.TimeHorizonSeconds) ||
+            !IsPositiveFinite(scheduling.DistanceHorizonMeters))
+        {
+            failureReason = "Planning horizon configuration is invalid.";
+            return EmPlanningStatus.InvalidInput;
+        }
+        if (initialProgressSpeedMetersPerSecond > directionMaximum + BoundaryTolerance ||
+            initialAccelerationMetersPerSecondSquared < -longitudinal.MaximumDecelerationMetersPerSecondSquared - BoundaryTolerance ||
+            initialAccelerationMetersPerSecondSquared > longitudinal.MaximumAccelerationMetersPerSecondSquared + BoundaryTolerance)
+        {
+            failureReason = "The initial state violates longitudinal bounds.";
+            return EmPlanningStatus.InvalidInput;
+        }
+
+        double remainingSegment = Math.Max(0d, segment.LengthMeters - currentSegmentReferenceS);
+        LongitudinalStoppingProfile initialStop = LongitudinalStoppingMath.Calculate(initialProgressSpeedMetersPerSecond,
+            initialAccelerationMetersPerSecondSquared, longitudinal.MaximumDecelerationMetersPerSecondSquared,
+            longitudinal.MaximumJerkMetersPerSecondCubed);
+        if (initialStop.DistanceMeters + BoundaryTolerance > remainingSegment)
+        {
+            failureReason = "The current segment lacks the jerk-limited stopping distance.";
+            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)
+        {
+            terminalReferenceS = segment.LengthMeters;
+            selection = new PlanningHorizonSelection(terminalReferenceS, ToTerminalType(segment.EndBoundary.BoundaryType));
+        }
+        else
+        {
+            selection = new PlanningHorizonSelection(terminalReferenceS, EmTerminalType.RollingSafetyStop);
+        }
+        return EmPlanningStatus.Success;
+    }
+
+    private static double CalculateReachableDistance(double initialSpeed, double initialAcceleration, double maximumSpeed,
+        LongitudinalConfiguration configuration, double timeHorizonSeconds)
+    {
+        LongitudinalStoppingProfile stopAtMaximumSpeed = LongitudinalStoppingMath.Calculate(maximumSpeed, 0d,
+            configuration.MaximumDecelerationMetersPerSecondSquared, configuration.MaximumJerkMetersPerSecondCubed);
+        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;
+    }
+
+    private static EmTerminalType ToTerminalType(EmBoundaryType boundaryType)
+    {
+        return boundaryType == EmBoundaryType.Goal
+            ? EmTerminalType.Goal
+            : boundaryType == EmBoundaryType.GearSwitchApproach || boundaryType == EmBoundaryType.GearSwitchDeparture
+                ? EmTerminalType.GearSwitch
+                : EmTerminalType.RollingSafetyStop;
+    }
+
+    private static bool IsPositiveFinite(double value)
+    {
+        return IsFinite(value) && value > 0d;
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}