feat: derive adaptive full-segment ST schedule

2026-08-06 23:54:34 +08:00
parent dad4ff4b04
commit 3269d556b6
14 changed files with 1642 additions and 108 deletions
@@ -9,7 +9,7 @@ internal sealed class TrajectorySampleSchedule
    private const double ZeroTolerance = 1e-12d;

    public TrajectorySampleSchedule(LongitudinalCandidate candidate, double outputTimeStepSeconds, double holdDurationSeconds,
-        EmLongitudinalMode mode)
+        EmLongitudinalMode mode, bool resampleMotion)
    {
        if (candidate == null)
            throw new ArgumentNullException(nameof(candidate));
@@ -22,16 +22,25 @@ internal sealed class TrajectorySampleSchedule

        var samples = new List<TrajectorySample>(candidate.KnotTimes.Count + 4);
        double previousPathS = double.NegativeInfinity;
-        for (int index = 0; index < candidate.KnotTimes.Count; index++)
+        if (resampleMotion)
        {
-            if (candidate.S[index] < previousPathS)
-                throw new ArgumentException("Trajectory PathS cannot decrease.", nameof(candidate));
-            if (candidate.U[index] < -ZeroTolerance)
-                throw new ArgumentException("Longitudinal progress speed cannot be negative.", nameof(candidate));
-
-            samples.Add(new TrajectorySample(candidate.KnotTimes[index], candidate.S[index], Math.Max(0d, candidate.U[index]),
-                candidate.A[index], index < candidate.J.Count ? candidate.J[index] : 0d, false));
-            previousPathS = candidate.S[index];
+            double finalTime = candidate.KnotTimes[candidate.KnotTimes.Count - 1];
+            int sourceInterval = 0;
+            for (double sampleTime = 0d; sampleTime < finalTime - ZeroTolerance;
+                sampleTime += outputTimeStepSeconds)
+            {
+                AddSample(Interpolate(candidate, sampleTime, ref sourceInterval), samples, ref previousPathS, candidate);
+            }
+            AddSample(Interpolate(candidate, finalTime, ref sourceInterval), samples, ref previousPathS, candidate);
+        }
+        else
+        {
+            for (int index = 0; index < candidate.KnotTimes.Count; index++)
+            {
+                AddSample(new TrajectorySample(candidate.KnotTimes[index], candidate.S[index],
+                    Math.Max(0d, candidate.U[index]), candidate.A[index], index < candidate.J.Count ? candidate.J[index] : 0d,
+                    false), samples, ref previousPathS, candidate);
+            }
        }

        if (mode != EmLongitudinalMode.ExactStopAtBoundary)
@@ -41,14 +50,22 @@ internal sealed class TrajectorySampleSchedule
            return;
        }

-        int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(candidate.KnotTimes,
-            outputTimeStepSeconds);
-        double stopPathS = candidate.S[stabilizationStart];
-        for (int index = stabilizationStart; index < candidate.S.Count; index++)
+        int sourceStabilizationStart = resampleMotion
+            ? FindTerminalStationaryTailStart(candidate)
+            : LongitudinalTerminalSchedule.GetStabilizationStartIndex(candidate.KnotTimes, outputTimeStepSeconds);
+        double stabilizationStartTime = candidate.KnotTimes[sourceStabilizationStart];
+        int stabilizationStart = 0;
+        while (stabilizationStart < samples.Count - 1 &&
+            samples[stabilizationStart].TimeFromStart < stabilizationStartTime - ZeroTolerance)
        {
-            if (Math.Abs(candidate.S[index] - stopPathS) > ZeroTolerance ||
-                Math.Abs(candidate.U[index]) > ZeroTolerance || Math.Abs(candidate.A[index]) > ZeroTolerance ||
-                (index < candidate.J.Count && Math.Abs(candidate.J[index]) > ZeroTolerance))
+            stabilizationStart++;
+        }
+        double stopPathS = samples[stabilizationStart].PathS;
+        for (int index = stabilizationStart; index < samples.Count; index++)
+        {
+            if (Math.Abs(samples[index].PathS - stopPathS) > ZeroTolerance ||
+                Math.Abs(samples[index].ProgressSpeed) > ZeroTolerance || Math.Abs(samples[index].Acceleration) > ZeroTolerance ||
+                Math.Abs(samples[index].Jerk) > ZeroTolerance)
            {
                throw new ArgumentException("An exact stop requires a stationary S/U/A/J tail.", nameof(candidate));
            }
@@ -69,6 +86,57 @@ internal sealed class TrajectorySampleSchedule
    public IReadOnlyList<TrajectorySample> Samples { get; }
    public int TerminalAnchorSampleIndex { get; }

+    private static void AddSample(TrajectorySample sample, ICollection<TrajectorySample> samples,
+        ref double previousPathS, LongitudinalCandidate candidate)
+    {
+        if (sample.PathS < previousPathS)
+            throw new ArgumentException("Trajectory PathS cannot decrease.", nameof(candidate));
+        if (sample.ProgressSpeed < -ZeroTolerance)
+            throw new ArgumentException("Longitudinal progress speed cannot be negative.", nameof(candidate));
+        samples.Add(sample);
+        previousPathS = sample.PathS;
+    }
+
+    private static TrajectorySample Interpolate(LongitudinalCandidate candidate, double sampleTime, ref int sourceInterval)
+    {
+        int lastKnot = candidate.KnotTimes.Count - 1;
+        if (sampleTime >= candidate.KnotTimes[lastKnot] - ZeroTolerance)
+        {
+            return new TrajectorySample(candidate.KnotTimes[lastKnot], candidate.S[lastKnot],
+                Math.Max(0d, candidate.U[lastKnot]), candidate.A[lastKnot], 0d, false);
+        }
+        while (sourceInterval < lastKnot - 1 &&
+            sampleTime >= candidate.KnotTimes[sourceInterval + 1] - ZeroTolerance)
+        {
+            sourceInterval++;
+        }
+        if (Math.Abs(sampleTime - candidate.KnotTimes[sourceInterval]) <= ZeroTolerance)
+        {
+            return new TrajectorySample(candidate.KnotTimes[sourceInterval], candidate.S[sourceInterval],
+                Math.Max(0d, candidate.U[sourceInterval]), candidate.A[sourceInterval], candidate.J[sourceInterval], false);
+        }
+        double dt = sampleTime - candidate.KnotTimes[sourceInterval];
+        double jerk = candidate.J[sourceInterval];
+        double acceleration = candidate.A[sourceInterval] + jerk * dt;
+        double speed = candidate.U[sourceInterval] + candidate.A[sourceInterval] * dt + 0.5d * jerk * dt * dt;
+        double pathS = candidate.S[sourceInterval] + candidate.U[sourceInterval] * dt +
+            0.5d * candidate.A[sourceInterval] * dt * dt + jerk * dt * dt * dt / 6d;
+        return new TrajectorySample(sampleTime, pathS, Math.Max(0d, speed), acceleration, jerk, false);
+    }
+
+    private static int FindTerminalStationaryTailStart(LongitudinalCandidate candidate)
+    {
+        int start = candidate.KnotTimes.Count - 1;
+        double terminalPathS = candidate.S[start];
+        while (start > 0 && Math.Abs(candidate.S[start - 1] - terminalPathS) <= ZeroTolerance &&
+            Math.Abs(candidate.U[start - 1]) <= ZeroTolerance && Math.Abs(candidate.A[start - 1]) <= ZeroTolerance &&
+            Math.Abs(candidate.J[start - 1]) <= ZeroTolerance)
+        {
+            start--;
+        }
+        return start;
+    }
+
    private static bool IsFinite(double value)
    {
        return !double.IsNaN(value) && !double.IsInfinity(value);