feat: 发布 EM 轨迹规划首个版本

2026-08-11 20:35:59 +08:00
parent 569de5f13c
commit 1903e71fc1
522 changed files with 4188 additions and 119188 deletions
@@ -0,0 +1,136 @@
+using System;
+using System.Collections.Generic;
+using System.Globalization;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Proves the constant-jerk profile used by publication is continuously forward-progressing.</summary>
+internal static class LongitudinalContinuousProfileValidator
+{
+    public static bool TryValidate(LongitudinalCandidate candidate, double tolerance, out string failureReason)
+    {
+        failureReason = string.Empty;
+        if (candidate == null)
+        {
+            failureReason = "A longitudinal candidate is required.";
+            return false;
+        }
+        if (!IsFinite(tolerance) || tolerance < 0d)
+        {
+            failureReason = "A finite nonnegative continuous-profile tolerance is required.";
+            return false;
+        }
+
+        double highWater = candidate.S[0];
+        for (int interval = 0; interval < candidate.J.Count; interval++)
+        {
+            double duration = candidate.KnotTimes[interval + 1] - candidate.KnotTimes[interval];
+            double initialS = candidate.S[interval];
+            double initialU = candidate.U[interval];
+            double initialA = candidate.A[interval];
+            double jerk = candidate.J[interval];
+            var evaluationTimes = new List<double>(5) { 0d, duration };
+            if (jerk != 0d)
+                AddIfInside(evaluationTimes, -initialA / jerk, duration);
+            AddSpeedRoots(evaluationTimes, initialU, initialA, jerk, duration);
+            evaluationTimes.Sort();
+
+            double previousTime = double.NegativeInfinity;
+            for (int point = 0; point < evaluationTimes.Count; point++)
+            {
+                double localTime = evaluationTimes[point];
+                if (localTime == previousTime)
+                    continue;
+                previousTime = localTime;
+                Evaluate(initialS, initialU, initialA, jerk, localTime,
+                    out double progress, out double speed, out double acceleration);
+                double regression = highWater - progress;
+                if (!IsFinite(progress) || !IsFinite(speed) || !IsFinite(acceleration) ||
+                    speed < -tolerance || regression > tolerance)
+                {
+                    string kind = !IsFinite(progress) || !IsFinite(speed) || !IsFinite(acceleration)
+                        ? "non-finite"
+                        : speed < -tolerance ? "negative-speed" : "PathS-regression";
+                    failureReason = FormatFailure(kind, interval, localTime, progress, speed,
+                        acceleration, jerk, regression, string.Empty);
+                    return false;
+                }
+                if (progress > highWater)
+                    highWater = progress;
+            }
+
+            Evaluate(initialS, initialU, initialA, jerk, duration,
+                out double integratedS, out double integratedU, out double integratedA);
+            double sMismatch = Math.Abs(integratedS - candidate.S[interval + 1]);
+            double uMismatch = Math.Abs(integratedU - candidate.U[interval + 1]);
+            double aMismatch = Math.Abs(integratedA - candidate.A[interval + 1]);
+            if (!IsFinite(sMismatch) || !IsFinite(uMismatch) || !IsFinite(aMismatch) ||
+                sMismatch > tolerance || uMismatch > tolerance || aMismatch > tolerance)
+            {
+                string detail = ";endpointMismatchS=" + Invariant(sMismatch) +
+                    ";endpointMismatchU=" + Invariant(uMismatch) +
+                    ";endpointMismatchA=" + Invariant(aMismatch);
+                failureReason = FormatFailure("endpoint-mismatch", interval, duration, integratedS,
+                    integratedU, integratedA, jerk, highWater - integratedS, detail);
+                return false;
+            }
+        }
+        return true;
+    }
+
+    private static void AddSpeedRoots(ICollection<double> times, double initialU, double initialA,
+        double jerk, double duration)
+    {
+        if (jerk == 0d)
+        {
+            if (initialA != 0d)
+                AddIfInside(times, -initialU / initialA, duration);
+            return;
+        }
+
+        double discriminant = initialA * initialA - 2d * jerk * initialU;
+        if (!IsFinite(discriminant) || discriminant < 0d)
+            return;
+        double rootTerm = Math.Sqrt(discriminant);
+        AddIfInside(times, (-initialA - rootTerm) / jerk, duration);
+        AddIfInside(times, (-initialA + rootTerm) / jerk, duration);
+    }
+
+    private static void AddIfInside(ICollection<double> times, double localTime, double duration)
+    {
+        if (IsFinite(localTime) && localTime >= 0d && localTime <= duration)
+            times.Add(localTime);
+    }
+
+    private static void Evaluate(double initialS, double initialU, double initialA, double jerk,
+        double localTime, out double progress, out double speed, out double acceleration)
+    {
+        acceleration = initialA + jerk * localTime;
+        speed = initialU + initialA * localTime + 0.5d * jerk * localTime * localTime;
+        progress = initialS + initialU * localTime + 0.5d * initialA * localTime * localTime +
+            jerk * localTime * localTime * localTime / 6d;
+    }
+
+    private static string FormatFailure(string kind, int interval, double localTime, double progress,
+        double speed, double acceleration, double jerk, double regression, string detail)
+    {
+        return "Continuous ST profile rejected: kind=" + kind +
+            ";interval=" + interval.ToString(CultureInfo.InvariantCulture) +
+            ";localTime=" + Invariant(localTime) +
+            ";S=" + Invariant(progress) +
+            ";U=" + Invariant(speed) +
+            ";A=" + Invariant(acceleration) +
+            ";J=" + Invariant(jerk) +
+            ";regression=" + Invariant(regression) + detail + ".";
+    }
+
+    private static string Invariant(double value)
+    {
+        return value.ToString("R", CultureInfo.InvariantCulture);
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}