First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat: seed rolling and exact-stop ST profiles

Browse Source
This commit is contained in:
梁薄云
2026-08-05 18:18:49 +08:00
parent 26bd822ec9
commit 59d13e5783
3 changed files with 526 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs
+117 -31
View File
@@ -12,10 +12,12 @@ internal static class LongitudinalIntegrationChecks
{
public static void Run()
{
VerifiesRollingOptimizationKeepsANonzeroTerminalSpeed();
VerifiesExactStopIncludesAStabilizationTail();
VerifiesLastStrictCandidateSurvivesLaterTimeout();
VerifiesInvalidAndInaccurateCandidatesNeverBecomeFallbacks();
VerifiesEnvelopeLinearizationAdvancesAfterStrictRejection();
VerifiesRejectedTerminalPathSStillUpdatesEnvelope();
VerifiesExactStopTailDoesNotDistortEnvelope();
VerifiesValidatedEndpointsAreCanonical();
VerifiesWarmStartAndFiveIterationLimit();
VerifiesNonzeroSpeedSeedIsStrictlyFeasible();
@@ -23,6 +25,75 @@ internal static class LongitudinalIntegrationChecks
RunRealOsqpInCleanPluginBundle();
}
private static void VerifiesRollingOptimizationKeepsANonzeroTerminalSpeed()
{
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
configuration.Scheduling.TimeHorizonSeconds = 2d;
configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
configuration.Scheduling.SolverTimeoutSeconds = 1d;
LateralPath path = new LateralPath(new[]
{
Point(0d, 0d, 0d),
Point(1d, 2.5d, 0d),
Point(2d, 5d, 0d),
}, true);
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.10d, 0d,
EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration,
Array.Empty<double>(), Array.Empty<double>());
var solver = new FakeQpSolver(Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 1d));
LongitudinalPlanningResult result = new SequentialLongitudinalOptimizer(solver).Optimize(input,
CancellationToken.None);
Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
"rolling seed remains a strict timeout fallback");
LongitudinalCandidate candidate = result.Candidate ??
throw new InvalidOperationException("Rolling timeout fallback candidate was missing.");
Verification.True(candidate.S[candidate.S.Count - 1] < input.PathUpperBoundS,
"two-second ST does not consume the five-metre LS window");
Verification.True(candidate.U[candidate.U.Count - 1] > 0.01d,
"rolling ST keeps nonzero terminal speed");
}
private static void VerifiesExactStopIncludesAStabilizationTail()
{
EmPlannerConfiguration configuration = CreateExactStopSeedConfiguration();
LateralPath path = new LateralPath(new[]
{
Point(0d, 0d, 0d),
Point(1d, 0.00375d, 0d),
Point(2d, 0.0075d, 0d),
}, true);
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.05d, 0d,
EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
Array.Empty<double>(), Array.Empty<double>());
var solver = new FakeQpSolver(Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 1d));
LongitudinalPlanningResult result = new SequentialLongitudinalOptimizer(solver).Optimize(input,
CancellationToken.None);
IReadOnlyList<double> times = LongitudinalCandidate.CreateKnotTimes(
input.Configuration.Scheduling.TimeHorizonSeconds, input.Configuration.Scheduling.OutputTimeStepSeconds);
LongitudinalCandidate seed = FromPrimal(times, solver.WarmStarts[0]);
EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
out string speedFailure);
Verification.Equal(EmPlanningStatus.Success, speedStatus, "exact-stop seed envelope: " + speedFailure);
Verification.True(new LongitudinalSolutionValidator().TryValidate(input, envelope, seed, out _,
out string validationFailure), "exact-stop seed is strictly feasible: " + validationFailure);
Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
"exact-stop seed remains a strict timeout fallback");
LongitudinalCandidate candidate = result.Candidate ??
throw new InvalidOperationException("Exact-stop timeout fallback candidate was missing.");
int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
candidate.KnotTimes, input.Configuration.Scheduling.OutputTimeStepSeconds);
for (int index = stabilizationStart; index < candidate.S.Count; index++)
{
Verification.NearlyEqual(input.StopBoundaryPathS, candidate.S[index], "stop-tail S " + index);
Verification.NearlyEqual(0d, candidate.U[index], "stop-tail U " + index);
Verification.NearlyEqual(0d, candidate.A[index], "stop-tail A " + index);
}
}
public static void RunRealOsqp()
{
foreach (LongitudinalScenario scenario in CreateRealOsqpScenarios())
@@ -110,9 +181,10 @@ internal static class LongitudinalIntegrationChecks
});
LongitudinalPlanningResult invalidResult = new SequentialLongitudinalOptimizer(invalidSolver).Optimize(input,
CancellationToken.None);
Verification.Equal(EmPlanningStatus.SolverTimedOut, invalidResult.Status,
"invalid solver vector cannot become fallback");
Verification.True(invalidResult.Candidate == null, "invalid solver vector publishes no candidate");
Verification.Equal(EmPlanningStatus.SuccessWithFallback, invalidResult.Status,
"invalid solver vector cannot replace the strict initial fallback");
Verification.NearlyEqual(valid.U[1], invalidResult.Candidate?.U[1] ?? double.NaN,
"invalid solver vector does not become the fallback candidate");
var inaccurateSolver = new FakeQpSolver(new[]
{
@@ -121,9 +193,10 @@ internal static class LongitudinalIntegrationChecks
});
LongitudinalPlanningResult inaccurateResult = new SequentialLongitudinalOptimizer(inaccurateSolver).Optimize(input,
CancellationToken.None);
Verification.Equal(EmPlanningStatus.SolverTimedOut, inaccurateResult.Status,
"inaccurate residual candidate cannot become fallback");
Verification.True(inaccurateResult.Candidate == null, "inaccurate residual publishes no candidate");
Verification.Equal(EmPlanningStatus.SuccessWithFallback, inaccurateResult.Status,
"inaccurate residual candidate cannot replace the strict initial fallback");
Verification.NearlyEqual(valid.U[1], inaccurateResult.Candidate?.U[1] ?? double.NaN,
"inaccurate residual does not become the fallback candidate");
double[] inaccuratePrimal = ToPrimal(valid);
for (int index = 0; index < inaccuratePrimal.Length; index++)
{
@@ -166,7 +239,7 @@ internal static class LongitudinalIntegrationChecks
{
Point(0d, 0d, 0d),
Point(1d, candidate.S[1], 10000d),
Point(2d, baseline.TerminalPathS, 0d),
Point(2d, baseline.PathUpperBoundS, 0d),
}, true);
var input = new LongitudinalPlanningInput(curvedPath, TravelDirection.Forward,
baseline.InitialProgressSpeedMetersPerSecond, baseline.InitialAccelerationMetersPerSecondSquared,
@@ -186,17 +259,11 @@ internal static class LongitudinalIntegrationChecks
Verification.Equal(2, solver.SolveCallCount, "rejected candidate reaches the next envelope iteration");
var layout = new LongitudinalVariableLayout(candidate.KnotTimes.Count);
FindSingleVariableBounds(solver.Problems[1], layout.U(1), out _, out double secondUpper);
double initialProgress = solver.WarmStarts[0][layout.S(1)];
double timeStep = candidate.KnotTimes[2] - candidate.KnotTimes[1];
double expectedAdvance = Math.Max(Math.Max(0d, candidate.S[1] - initialProgress), candidate.U[1] * timeStep);
double expectedProgress = candidate.S[1] + Math.Min(expectedAdvance,
0.5d * (input.TerminalPathS - candidate.S[1]));
Verification.True(expectedProgress > candidate.S[1], "scripted rejection advances the ST PathS envelope probe");
Verification.NearlyEqual(envelope.MaximumSpeedAt(expectedProgress), secondUpper,
"next ST QP samples the bounded forward-extrapolated PathS envelope");
Verification.True(Math.Abs(secondUpper - envelope.MaximumSpeedAt(candidate.S[1])) > 1e-12d,
"scripted rejection advances the ST PathS envelope probe");
}
private static void VerifiesRejectedTerminalPathSStillUpdatesEnvelope()
private static void VerifiesExactStopTailDoesNotDistortEnvelope()
{
LongitudinalPlanningInput baseline = CreateFakeInput(out LongitudinalCandidate valid);
double[] perturbedProgress = new double[valid.S.Count];
@@ -208,7 +275,7 @@ internal static class LongitudinalIntegrationChecks
{
Point(0d, 0d, 0d),
Point(1d, valid.S[1], 10000d),
Point(2d, baseline.TerminalPathS, 0d),
Point(2d, baseline.PathUpperBoundS, 0d),
}, true);
var input = new LongitudinalPlanningInput(curvedPath, TravelDirection.Forward,
baseline.InitialProgressSpeedMetersPerSecond, baseline.InitialAccelerationMetersPerSecondSquared,
@@ -224,8 +291,8 @@ internal static class LongitudinalIntegrationChecks
var layout = new LongitudinalVariableLayout(valid.KnotTimes.Count);
FindSingleVariableBounds(solver.Problems[0], layout.U(1), out _, out double firstUpper);
FindSingleVariableBounds(solver.Problems[1], layout.U(1), out _, out double secondUpper);
Verification.True(Math.Abs(firstUpper - secondUpper) > 1e-12d,
"rejected terminal PathS is projected before the next envelope sample");
Verification.NearlyEqual(firstUpper, secondUpper,
"exact stop-tail perturbations do not distort a moving-knot envelope sample");
}
private static void VerifiesCancellationInfeasibilityAndPlannerDelegation()
@@ -244,7 +311,9 @@ internal static class LongitudinalIntegrationChecks
var infeasibleSolver = new FakeQpSolver(Result(QpSolveStatus.PrimalInfeasible, Array.Empty<double>(), 1d));
LongitudinalPlanningResult infeasible = new SequentialLongitudinalOptimizer(infeasibleSolver).Optimize(input,
CancellationToken.None);
Verification.Equal(EmPlanningStatus.LongitudinalInfeasible, infeasible.Status, "QP infeasibility is longitudinal");
Verification.Equal(EmPlanningStatus.SuccessWithFallback, infeasible.Status,
"QP infeasibility preserves the strict mode-specific seed");
Verification.True(infeasible.Candidate != null, "QP infeasibility retains a safe fallback profile");
var plannerSolver = new FakeQpSolver(new[]
{
@@ -258,12 +327,14 @@ internal static class LongitudinalIntegrationChecks
private static void VerifiesNonzeroSpeedSeedIsStrictlyFeasible()
{
LongitudinalPlanningInput input = CreateRealScenario("seed", TravelDirection.Forward, 0.20d, 0d, 0.20d, 0d).Input;
LongitudinalPlanningInput input = CreateFakeInput(out _);
var solver = new FakeQpSolver(Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 1d));
LongitudinalPlanningResult result = new SequentialLongitudinalOptimizer(solver).Optimize(input,
CancellationToken.None);
IReadOnlyList<double> times = LongitudinalCandidate.CreateKnotTimes(
input.Configuration.Scheduling.TimeHorizonSeconds, input.Configuration.Scheduling.OutputTimeStepSeconds);
Verification.True(solver.WarmStarts.Count > 0,
"nonzero-speed seed reaches the ST solver: " + result.Status + " " + result.FailureReason);
LongitudinalCandidate seed = FromPrimal(times, solver.WarmStarts[0]);
EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
out string speedFailure);
@@ -303,7 +374,7 @@ internal static class LongitudinalIntegrationChecks
"canonical strict candidate remains the timeout fallback");
LongitudinalCandidate canonical = result.Candidate ??
throw new InvalidOperationException("Canonical fallback candidate was missing.");
Verification.Equal(input.TerminalPathS, canonical.S[canonical.S.Count - 1],
Verification.Equal(input.StopBoundaryPathS, canonical.S[canonical.S.Count - 1],
"validated terminal PathS is canonicalized exactly");
Verification.Equal(0d, canonical.U[canonical.U.Count - 1],
"validated terminal speed is canonicalized exactly");
@@ -316,7 +387,7 @@ internal static class LongitudinalIntegrationChecks
CreateRealScenario("forward", TravelDirection.Forward, 0.50d, 0d, 0d, 0d),
CreateRealScenario("reverse", TravelDirection.Reverse, 0.50d, 0d, 0d, 0d),
CreateRealScenario("curvature-limited", TravelDirection.Forward, 0.35d, 20d, 0d, 0d),
CreateRealScenario("jerk-limited-stop", TravelDirection.Forward, 0.20d, 0d, 0.20d, 0d),
CreateRealScenario("jerk-limited-stop", TravelDirection.Forward, 0.50d, 0d, 0.05d, 0d),
CreateRealScenario("short-segment", TravelDirection.Forward, 0.05d, 0d, 0d, 0d),
CreateRealScenario("zero-start-speed", TravelDirection.Forward, 0.50d, 0d, 0d, 0d),
};
@@ -335,7 +406,7 @@ internal static class LongitudinalIntegrationChecks
Point(2d, terminalPathS, 0d),
};
return new LongitudinalScenario(name, new LongitudinalPlanningInput(new LateralPath(points, true), direction,
initialSpeed, initialAcceleration, EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary,
initialSpeed, initialAcceleration, EmTerminalType.Goal, EmLongitudinalMode.ApproachStopBoundary,
configuration, Array.Empty<double>(), Array.Empty<double>()));
}
@@ -344,9 +415,8 @@ internal static class LongitudinalIntegrationChecks
Verification.True(result.Status == EmPlanningStatus.Success || result.Status == EmPlanningStatus.SuccessWithFallback,
scenario.Name + " returns a strict profile: " + result.FailureReason);
LongitudinalCandidate candidate = result.Candidate ?? throw new InvalidOperationException(scenario.Name + " candidate missing.");
Verification.NearlyEqual(scenario.Input.TerminalPathS, candidate.S[candidate.S.Count - 1],
scenario.Name + " exact terminal PathS");
Verification.NearlyEqual(0d, candidate.U[candidate.U.Count - 1], scenario.Name + " exact terminal speed");
Verification.True(candidate.S[candidate.S.Count - 1] <= scenario.Input.PathUpperBoundS,
scenario.Name + " remains inside the PathS window");
PathSpeedLimitBuilder builder = new PathSpeedLimitBuilder();
EmPlanningStatus speedStatus = builder.Build(scenario.Input, out PathSpeedLimit envelope, out string speedFailure);
Verification.Equal(EmPlanningStatus.Success, speedStatus, scenario.Name + " envelope: " + speedFailure);
@@ -377,14 +447,14 @@ internal static class LongitudinalIntegrationChecks
configuration.Scheduling.SolverTimeoutSeconds = 1d;
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1d;
configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1e-6d;
configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 1d;
configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 4d;
configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared = 1d;
configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1d;
IReadOnlyList<double> times = LongitudinalCandidate.CreateKnotTimes(1d, 0.25d);
valid = LongitudinalCandidate.Integrate(times, 0d, 0.10d, 0d,
new[] { -0.45714285714285714d, 0d, 0d, 0d });
new[] { -0.8d, 0d, 0.8d, 0d });
double terminalPathS = valid.S[valid.S.Count - 1];
var path = new LateralPath(new[]
{
@@ -396,6 +466,22 @@ internal static class LongitudinalIntegrationChecks
EmLongitudinalMode.ExactStopAtBoundary, configuration, Array.Empty<double>(), Array.Empty<double>());
}
private static EmPlannerConfiguration CreateExactStopSeedConfiguration()
{
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
configuration.Scheduling.TimeHorizonSeconds = 0.40d;
configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
configuration.Scheduling.SolverTimeoutSeconds = 1d;
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1e-6d;
configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 20d;
configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared = 1d;
configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1d;
return configuration;
}
private static LateralPathPoint Point(double referenceS, double pathS, double curvature)
{
return new LateralPathPoint(referenceS, pathS, 0d, 0d, 0d, 0d, pathS, 0d, 0d, curvature, curvature, 0d);