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,188 @@
+using System;
+using System.Collections.Generic;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Builds curvature-aware, stopping-aware speed limits over actual optimized PathS.</summary>
+public sealed class PathSpeedLimitBuilder
+{
+    internal const double CurvatureEpsilon = 1e-10d;
+    private const double StopDistanceToleranceMeters = 1e-8d;
+
+    public EmPlanningStatus Build(LongitudinalPlanningInput input, out PathSpeedLimit speedLimit, out string failureReason)
+    {
+        speedLimit = null;
+        failureReason = string.Empty;
+        if (input == null)
+        {
+            failureReason = "Longitudinal planning input is required.";
+            return EmPlanningStatus.InvalidInput;
+        }
+
+        if (!TryGetLimits(input, out double directionMaximum, out double maximumAcceleration, out double maximumDeceleration,
+                out double maximumJerk, out double maximumLateralAcceleration, out double maximumCurvatureRate,
+                out failureReason))
+        {
+            return EmPlanningStatus.InvalidInput;
+        }
+        if (input.InitialProgressSpeedMetersPerSecond > directionMaximum + StopDistanceToleranceMeters ||
+            input.InitialAccelerationMetersPerSecondSquared < -maximumDeceleration - StopDistanceToleranceMeters ||
+            input.InitialAccelerationMetersPerSecondSquared > maximumAcceleration + StopDistanceToleranceMeters)
+        {
+            failureReason = "The initial longitudinal state violates the configured hard bounds.";
+            return EmPlanningStatus.InvalidInput;
+        }
+
+        LongitudinalStoppingProfile stopProfile = LongitudinalStoppingMath.Calculate(input.InitialProgressSpeedMetersPerSecond,
+            input.InitialAccelerationMetersPerSecondSquared, maximumDeceleration, maximumJerk);
+        if (stopProfile.DistanceMeters + StopDistanceToleranceMeters > input.TerminalPathS)
+        {
+            failureReason = "The available actual PathS distance is insufficient for the jerk-limited stop.";
+            return EmPlanningStatus.StoppingDistanceInsufficient;
+        }
+
+        int count = input.Path.Points.Count;
+        var pathS = new double[count];
+        var maximum = new double[count];
+        var lateral = new double[count];
+        var curvatureRate = new double[count];
+        var stopping = new double[count];
+        for (int index = 0; index < count; index++)
+        {
+            LateralPathPoint point = input.Path.Points[index];
+            pathS[index] = point.PathS;
+            double lateralLimit = Math.Sqrt(maximumLateralAcceleration /
+                Math.Max(Math.Abs(point.VehicleCurvature), CurvatureEpsilon));
+            double curvatureRateLimit = maximumCurvatureRate /
+                Math.Max(Math.Abs(point.VehicleCurvatureDerivative), CurvatureEpsilon);
+            double remainingDistance = Math.Max(0d, input.TerminalPathS - point.PathS);
+            double stoppingLimit = Math.Sqrt(2d * maximumDeceleration * remainingDistance);
+            lateral[index] = ClampFinite(lateralLimit, directionMaximum);
+            curvatureRate[index] = ClampFinite(curvatureRateLimit, directionMaximum);
+            stopping[index] = index == count - 1 ? 0d : ClampFinite(stoppingLimit, directionMaximum);
+            maximum[index] = index == count - 1 ? 0d : Math.Min(directionMaximum,
+                Math.Min(lateral[index], Math.Min(curvatureRate[index], stopping[index])));
+        }
+
+        try
+        {
+            speedLimit = new PathSpeedLimit(pathS, maximum, lateral, curvatureRate, stopping, directionMaximum);
+            return EmPlanningStatus.Success;
+        }
+        catch (ArgumentException exception)
+        {
+            failureReason = exception.Message;
+            return EmPlanningStatus.InvalidInput;
+        }
+    }
+
+    internal static bool TryGetLimits(LongitudinalPlanningInput input, out double directionMaximum,
+        out double maximumAcceleration, out double maximumDeceleration, out double maximumJerk,
+        out double maximumLateralAcceleration, out double maximumCurvatureRate, out string failureReason)
+    {
+        directionMaximum = 0d;
+        maximumAcceleration = 0d;
+        maximumDeceleration = 0d;
+        maximumJerk = 0d;
+        maximumLateralAcceleration = 0d;
+        maximumCurvatureRate = 0d;
+        failureReason = string.Empty;
+        if (input.Configuration == null || input.Configuration.Longitudinal == null)
+        {
+            failureReason = "Longitudinal configuration is required.";
+            return false;
+        }
+
+        LongitudinalConfiguration configuration = input.Configuration.Longitudinal;
+        directionMaximum = input.DirectionMaximumSpeedMetersPerSecond;
+        maximumAcceleration = configuration.MaximumAccelerationMetersPerSecondSquared;
+        maximumDeceleration = configuration.MaximumDecelerationMetersPerSecondSquared;
+        maximumJerk = configuration.MaximumJerkMetersPerSecondCubed;
+        maximumLateralAcceleration = configuration.MaximumLateralAccelerationMetersPerSecondSquared;
+        maximumCurvatureRate = configuration.MaximumCurvatureRatePerMeterPerSecond;
+        if (!IsPositiveFinite(directionMaximum) || !IsPositiveFinite(maximumAcceleration) ||
+            !IsPositiveFinite(maximumDeceleration) || !IsPositiveFinite(maximumJerk) ||
+            !IsPositiveFinite(maximumLateralAcceleration) || !IsPositiveFinite(maximumCurvatureRate))
+        {
+            failureReason = "Longitudinal limits must be positive and finite.";
+            return false;
+        }
+        return true;
+    }
+
+    private static double ClampFinite(double value, double maximum)
+    {
+        if (!IsFinite(value) || value < 0d)
+            throw new ArgumentOutOfRangeException(nameof(value));
+        return Math.Min(maximum, value);
+    }
+
+    private static bool IsPositiveFinite(double value)
+    {
+        return IsFinite(value) && value > 0d;
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}
+
+internal sealed class LongitudinalStoppingProfile
+{
+    public LongitudinalStoppingProfile(double distanceMeters, double durationSeconds)
+    {
+        DistanceMeters = distanceMeters;
+        DurationSeconds = durationSeconds;
+    }
+
+    public double DistanceMeters { get; }
+
+    public double DurationSeconds { get; }
+}
+
+internal static class LongitudinalStoppingMath
+{
+    public static LongitudinalStoppingProfile Calculate(double speedMetersPerSecond, double accelerationMetersPerSecondSquared,
+        double maximumDecelerationMetersPerSecondSquared, double maximumJerkMetersPerSecondCubed)
+    {
+        if (!IsFinite(speedMetersPerSecond) || !IsFinite(accelerationMetersPerSecondSquared) ||
+            !IsPositiveFinite(maximumDecelerationMetersPerSecondSquared) || !IsPositiveFinite(maximumJerkMetersPerSecondCubed))
+        {
+            throw new ArgumentOutOfRangeException(nameof(speedMetersPerSecond));
+        }
+        if (speedMetersPerSecond <= 0d)
+            return new LongitudinalStoppingProfile(0d, 0d);
+
+        double acceleration = Math.Max(-maximumDecelerationMetersPerSecondSquared, accelerationMetersPerSecondSquared);
+        double rampDuration = (acceleration + maximumDecelerationMetersPerSecondSquared) / maximumJerkMetersPerSecondCubed;
+        double speedAfterRamp = speedMetersPerSecond + acceleration * rampDuration -
+            0.5d * maximumJerkMetersPerSecondCubed * rampDuration * rampDuration;
+        if (speedAfterRamp <= 0d)
+        {
+            double root = (acceleration + Math.Sqrt(acceleration * acceleration + 2d * maximumJerkMetersPerSecondCubed *
+                speedMetersPerSecond)) / maximumJerkMetersPerSecondCubed;
+            double distance = speedMetersPerSecond * root + 0.5d * acceleration * root * root -
+                maximumJerkMetersPerSecondCubed * root * root * root / 6d;
+            return new LongitudinalStoppingProfile(Math.Max(0d, distance), root);
+        }
+
+        double rampDistance = speedMetersPerSecond * rampDuration + 0.5d * acceleration * rampDuration * rampDuration -
+            maximumJerkMetersPerSecondCubed * rampDuration * rampDuration * rampDuration / 6d;
+        double constantDecelerationDuration = speedAfterRamp / maximumDecelerationMetersPerSecondSquared;
+        double constantDecelerationDistance = speedAfterRamp * speedAfterRamp /
+            (2d * maximumDecelerationMetersPerSecondSquared);
+        return new LongitudinalStoppingProfile(rampDistance + constantDecelerationDistance,
+            rampDuration + constantDecelerationDuration);
+    }
+
+    private static bool IsPositiveFinite(double value)
+    {
+        return IsFinite(value) && value > 0d;
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}