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,123 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Finite piecewise-linear speed limits indexed exclusively by actual lateral PathS.</summary>
+public sealed class PathSpeedLimit
+{
+    private const double StationTolerance = 1e-12d;
+
+    internal PathSpeedLimit(IReadOnlyList<double> pathS, IReadOnlyList<double> maximumSpeed,
+        IReadOnlyList<double> lateralAccelerationLimit, IReadOnlyList<double> curvatureRateLimit,
+        IReadOnlyList<double> stoppingLimit, double directionMaximumSpeedMetersPerSecond)
+    {
+        PathS = CopyStrictStations(pathS, nameof(pathS));
+        MaximumSpeedMetersPerSecond = CopyFiniteNonnegative(maximumSpeed, PathS.Count, nameof(maximumSpeed));
+        LateralAccelerationSpeedLimitsMetersPerSecond = CopyFiniteNonnegative(lateralAccelerationLimit, PathS.Count,
+            nameof(lateralAccelerationLimit));
+        CurvatureRateSpeedLimitsMetersPerSecond = CopyFiniteNonnegative(curvatureRateLimit, PathS.Count,
+            nameof(curvatureRateLimit));
+        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)
+        {
+            throw new ArgumentException("Terminal PathS speed limits must be exactly zero.");
+        }
+
+        DirectionMaximumSpeedMetersPerSecond = directionMaximumSpeedMetersPerSecond;
+    }
+
+    public IReadOnlyList<double> PathS { get; }
+
+    public IReadOnlyList<double> MaximumSpeedMetersPerSecond { get; }
+
+    public IReadOnlyList<double> LateralAccelerationSpeedLimitsMetersPerSecond { get; }
+
+    public IReadOnlyList<double> CurvatureRateSpeedLimitsMetersPerSecond { get; }
+
+    public IReadOnlyList<double> StoppingSpeedLimitsMetersPerSecond { get; }
+
+    public double DirectionMaximumSpeedMetersPerSecond { get; }
+
+    public double TerminalPathS { get { return PathS[PathS.Count - 1]; } }
+
+    public double MaximumSpeedAt(double pathS)
+    {
+        return Interpolate(MaximumSpeedMetersPerSecond, pathS);
+    }
+
+    public double LateralAccelerationLimitAt(double pathS)
+    {
+        return Interpolate(LateralAccelerationSpeedLimitsMetersPerSecond, pathS);
+    }
+
+    public double CurvatureRateLimitAt(double pathS)
+    {
+        return Interpolate(CurvatureRateSpeedLimitsMetersPerSecond, pathS);
+    }
+
+    public double StoppingLimitAt(double pathS)
+    {
+        return Interpolate(StoppingSpeedLimitsMetersPerSecond, pathS);
+    }
+
+    private double Interpolate(IReadOnlyList<double> values, double pathS)
+    {
+        if (!IsFinite(pathS) || pathS < PathS[0] - StationTolerance || pathS > TerminalPathS + StationTolerance)
+            throw new ArgumentOutOfRangeException(nameof(pathS));
+        if (pathS <= PathS[0])
+            return values[0];
+        if (pathS >= TerminalPathS)
+            return values[values.Count - 1];
+
+        for (int index = 1; index < PathS.Count; index++)
+        {
+            if (pathS <= PathS[index])
+            {
+                double fraction = (pathS - PathS[index - 1]) / (PathS[index] - PathS[index - 1]);
+                return values[index - 1] + (values[index] - values[index - 1]) * fraction;
+            }
+        }
+        return values[values.Count - 1];
+    }
+
+    private static IReadOnlyList<double> CopyStrictStations(IReadOnlyList<double> source, string parameterName)
+    {
+        if (source == null || source.Count < 2)
+            throw new ArgumentException("At least two PathS stations are required.", parameterName);
+        var copy = new List<double>(source.Count);
+        double previous = double.NegativeInfinity;
+        for (int index = 0; index < source.Count; index++)
+        {
+            if (!IsFinite(source[index]) || source[index] <= previous)
+                throw new ArgumentException("PathS stations must be finite and strictly increasing.", parameterName);
+            copy.Add(source[index]);
+            previous = source[index];
+        }
+        return new ReadOnlyCollection<double>(copy);
+    }
+
+    private static IReadOnlyList<double> CopyFiniteNonnegative(IReadOnlyList<double> source, int expectedCount,
+        string parameterName)
+    {
+        if (source == null || source.Count != expectedCount)
+            throw new ArgumentException("Speed limit count must match PathS stations.", parameterName);
+        var copy = new List<double>(source.Count);
+        for (int index = 0; index < source.Count; index++)
+        {
+            if (!IsFinite(source[index]) || source[index] < 0d)
+                throw new ArgumentOutOfRangeException(parameterName);
+            copy.Add(source[index]);
+        }
+        return new ReadOnlyCollection<double>(copy);
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}