fix: generate deterministic S-curve static-start ST seeds

2026-08-07 16:33:12 +08:00
parent d378ad6257
commit 2d406359d0
2 changed files with 117 additions and 1 deletions
@@ -537,7 +537,86 @@ public sealed class SequentialLongitudinalOptimizer
                return true;
            }
        }
-        failureReason = "exactJerkSeed=failed; sampledSeeds=failed";
+        if (TryCreateStaticStartScurveSeed(input, times, stabilizationStart, speedLimit, out candidate))
+        {
+            failureReason = string.Empty;
+            return true;
+        }
+        failureReason = "exactJerkSeed=failed; sampledSeeds=failed; scurveSeeds=failed";
+        return false;
+    }
+
+    private bool TryCreateStaticStartScurveSeed(LongitudinalPlanningInput input,
+        IReadOnlyList<double> times, int stabilizationStart, PathSpeedLimit speedLimit,
+        out LongitudinalCandidate candidate)
+    {
+        candidate = null;
+        int intervalCount = stabilizationStart;
+        int maximumRamp = Math.Max(1, intervalCount / 6);
+        var motionTimes = new double[intervalCount + 1];
+        for (int index = 0; index <= intervalCount; index++)
+            motionTimes[index] = times[index];
+
+        for (int ramp = 1; ramp <= maximumRamp; ramp++)
+        {
+            for (int plateau = 0; 4 * ramp + 2 * plateau <= intervalCount; plateau++)
+            {
+                int cruise = intervalCount - 4 * ramp - 2 * plateau;
+                var basisAccel = new double[intervalCount];
+                var basisBrake = new double[intervalCount];
+                var basisOffset = new double[intervalCount];
+                int cursor = 0;
+                for (int index = 0; index < ramp; index++)
+                    basisAccel[cursor++] = 1d;
+                for (int index = 0; index < plateau; index++)
+                    cursor++;
+                for (int index = 0; index < ramp; index++)
+                    basisAccel[cursor++] = -1d;
+                for (int index = 0; index < cruise; index++)
+                    cursor++;
+                for (int index = 0; index < ramp; index++)
+                    basisBrake[cursor++] = -1d;
+                for (int index = 0; index < plateau; index++)
+                    cursor++;
+                for (int index = 0; index < ramp; index++)
+                    basisBrake[cursor++] = 1d;
+                if (cursor != intervalCount)
+                    continue;
+                for (int index = 0; index < intervalCount; index++)
+                    basisOffset[index] = 1d;
+
+                LongitudinalCandidate accelResponse = LongitudinalCandidate.Integrate(
+                    motionTimes, 0d, 0d, 0d, basisAccel);
+                LongitudinalCandidate brakeResponse = LongitudinalCandidate.Integrate(
+                    motionTimes, 0d, 0d, 0d, basisBrake);
+                LongitudinalCandidate offsetResponse = LongitudinalCandidate.Integrate(
+                    motionTimes, 0d, 0d, 0d, basisOffset);
+                var influence = new double[3, 3];
+                influence[0, 0] = accelResponse.A[intervalCount];
+                influence[1, 0] = accelResponse.U[intervalCount];
+                influence[2, 0] = accelResponse.S[intervalCount];
+                influence[0, 1] = brakeResponse.A[intervalCount];
+                influence[1, 1] = brakeResponse.U[intervalCount];
+                influence[2, 1] = brakeResponse.S[intervalCount];
+                influence[0, 2] = offsetResponse.A[intervalCount];
+                influence[1, 2] = offsetResponse.U[intervalCount];
+                influence[2, 2] = offsetResponse.S[intervalCount];
+
+                double[] target = { 0d, 0d, input.StopBoundaryPathS };
+                if (!TrySolveThreeByThree(influence, target, out double[] multipliers))
+                    continue;
+
+                var jerk = new double[intervalCount];
+                for (int index = 0; index < intervalCount; index++)
+                {
+                    jerk[index] = multipliers[0] * basisAccel[index] +
+                        multipliers[1] * basisBrake[index] +
+                        multipliers[2] * basisOffset[index];
+                }
+                if (TryValidateExactSeed(input, times, stabilizationStart, speedLimit, jerk, out candidate))
+                    return true;
+            }
+        }
        return false;
    }