From 2d406359d0d6912282735c4bdf841ff9b2d86f4a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Fri, 7 Aug 2026 16:33:12 +0800 Subject: [PATCH] fix: generate deterministic S-curve static-start ST seeds --- .../SequentialLongitudinalOptimizer.cs | 81 ++++++++++++++++++- .../LongitudinalIntegrationChecks.cs | 37 +++++++++ 2 files changed, 117 insertions(+), 1 deletion(-) diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs index 1760ce3..a2bb074 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs @@ -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 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; } diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs index bb644ee..bb537ec 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs @@ -16,6 +16,7 @@ internal static class LongitudinalIntegrationChecks VerifiesFullDirectionScheduleIsIndependentFromPublicationCadence(); VerifiesFullDirectionStaticStartMakesProgress(); VerifiesFullDirectionNearZeroSpeedUsesStaticStartSeed(); + VerifiesFullDirectionCurvedNearZeroStartUsesStaticStartSeed(); VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold(); VerifiesExactStopIncludesAStabilizationTail(); VerifiesLastStrictCandidateSurvivesLaterTimeout(); @@ -187,6 +188,42 @@ internal static class LongitudinalIntegrationChecks "near-zero start stops at the terminal boundary"); } + private static void VerifiesFullDirectionCurvedNearZeroStartUsesStaticStartSeed() + { + EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault(); + configuration.Scheduling.SolverTimeoutSeconds = 5d; + configuration.Solver.MaximumOsqpIterations = 100000; + LateralPath path = new LateralPath(new[] + { + Point(0d, 0d, 0d), + Point(1d, 2.25d, 0.1d), + Point(2d, 4.5d, 0d), + }, true); + + EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0.0001d, + EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason); + Verification.Equal(EmPlanningStatus.Success, status, "curved near-zero envelope: " + failureReason); + status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.0001d, 0d, + configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration, + out LongitudinalKnotSchedule schedule, out failureReason); + Verification.Equal(EmPlanningStatus.Success, status, "curved near-zero schedule: " + failureReason); + + var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.0001d, 0d, + EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration, + EmPlanningScope.FullDirectionSegment, schedule, Array.Empty(), Array.Empty()); + LongitudinalPlanningResult result = new LongitudinalPlanner(new OsqpNativeSolver()).Plan(input, + CancellationToken.None); + + Verification.True(result.Status == EmPlanningStatus.Success || result.Status == EmPlanningStatus.SuccessWithFallback, + "curved near-zero full-direction start succeeds: " + result.FailureReason); + LongitudinalCandidate candidate = result.Candidate ?? + throw new InvalidOperationException("Curved near-zero full-direction candidate was missing."); + Verification.NearlyEqual(path.Points[path.Points.Count - 1].PathS, candidate.S[candidate.S.Count - 1], + "curved near-zero start reaches the terminal boundary"); + Verification.NearlyEqual(0d, candidate.U[candidate.U.Count - 1], + "curved near-zero start stops at the terminal boundary"); + } + private static void VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold() { EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();