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

fix: stabilize stopped-start EM planning and add txt diagnostics

Browse Source
This commit is contained in:
梁薄云
2026-08-07 15:51:31 +08:00
parent f2970d0aff
commit d378ad6257
13 changed files with 373 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs
+58 -20
View File
@@ -1,6 +1,7 @@
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Globalization;
using System.Threading;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
@@ -38,6 +39,21 @@ public sealed class SequentialLongitudinalOptimizer
return Failed(EmPlanningStatus.InvalidInput, "Longitudinal planning input is required.");
if (cancellationToken.IsCancellationRequested)
return Failed(EmPlanningStatus.Cancelled, "Longitudinal optimization was cancelled.");
if (input.PlanningScope == EmPlanningScope.FullDirectionSegment &&
input.Mode == EmLongitudinalMode.ExactStopAtBoundary)
{
double staticStartSpeedTolerance = Math.Max(input.Configuration.Validation.SpatialToleranceMeters,
input.Configuration.Longitudinal.StopSpeedToleranceMetersPerSecond);
if (input.InitialProgressSpeedMetersPerSecond <= staticStartSpeedTolerance &&
Math.Abs(input.InitialAccelerationMetersPerSecondSquared) <=
input.Configuration.Validation.KinematicTolerance)
{
input = new LongitudinalPlanningInput(input.Path, input.Direction, 0d,
input.InitialAccelerationMetersPerSecondSquared, input.TerminalType, input.Mode,
input.Configuration, input.PlanningScope, input.KnotSchedule,
input.PreviousPathS, input.PreviousProgressSpeedMetersPerSecond);
}
}
if (!TryCreateSettings(input, out QpSolverSettings settings, out TimeSpan totalBudget, out double convergenceTolerance,
out int iterationLimit, out string configurationFailure))
{
@@ -223,14 +239,27 @@ public sealed class SequentialLongitudinalOptimizer
projectionSolveCount = 0;
failureStatus = EmPlanningStatus.LongitudinalInfeasible;
failureReason = string.Empty;
if (input.InitialProgressSpeedMetersPerSecond <= input.Configuration.Validation.SpatialToleranceMeters &&
double staticStartSpeedTolerance = Math.Max(input.Configuration.Validation.SpatialToleranceMeters,
input.Configuration.Longitudinal.StopSpeedToleranceMetersPerSecond);
bool staticStartEligible = input.InitialProgressSpeedMetersPerSecond <= staticStartSpeedTolerance &&
Math.Abs(input.InitialAccelerationMetersPerSecondSquared) <=
input.Configuration.Validation.KinematicTolerance &&
TryCreateStaticStartSeed(input, speedLimit, out LongitudinalCandidate staticStartSeed))
input.Configuration.Validation.KinematicTolerance;
bool staticStartSeedUsed = false;
string staticStartSeedFailure = string.Empty;
if (staticStartEligible && TryCreateStaticStartSeed(input, speedLimit, out LongitudinalCandidate staticStartSeed,
out staticStartSeedFailure))
{
staticStartSeedUsed = true;
candidate = staticStartSeed;
return true;
}
string WithStaticSeedDiagnostic(string reason)
{
return staticStartEligible && !staticStartSeedUsed
? "staticStartSeed=failed (" + staticStartSeedFailure + "); " + reason
: reason;
}
LongitudinalCandidate linearizationIterate = CreateScheduleReferenceIterate(input);
string lastRejection = string.Empty;
for (int iteration = 0; iteration < iterationLimit; iteration++)
@@ -238,21 +267,21 @@ public sealed class SequentialLongitudinalOptimizer
if (cancellationToken.IsCancellationRequested)
{
failureStatus = EmPlanningStatus.Cancelled;
failureReason = "Initial full-direction feasibility projection was cancelled.";
failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection was cancelled.");
return false;
}
TimeSpan remainingBudget = totalBudget - stopwatch.Elapsed;
if (remainingBudget <= TimeSpan.Zero)
{
failureStatus = EmPlanningStatus.SolverTimedOut;
failureReason = "Initial full-direction feasibility projection exhausted the shared solve budget.";
failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection exhausted the shared solve budget.");
return false;
}
if (!_constraintBuilder.TryBuildInitialFeasibilityProjection(input, speedLimit, linearizationIterate,
out QuadraticProgram problem, out string buildFailure))
{
failureStatus = EmPlanningStatus.LongitudinalInfeasible;
failureReason = "Initial full-direction feasibility constraints are infeasible: " + buildFailure;
failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility constraints are infeasible: " + buildFailure);
return false;
}
@@ -267,45 +296,45 @@ public sealed class SequentialLongitudinalOptimizer
if (solved == null)
{
failureStatus = EmPlanningStatus.Failed;
failureReason = "The initial full-direction feasibility solver returned no result.";
failureReason = WithStaticSeedDiagnostic("The initial full-direction feasibility solver returned no result.");
return false;
}
if (solved.Status == QpSolveStatus.TimeLimit || solved.Status == QpSolveStatus.MaximumIterations)
{
failureStatus = EmPlanningStatus.SolverTimedOut;
failureReason = "Initial full-direction feasibility projection timed out (status=" + solved.NativeStatus +
failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection timed out (status=" + solved.NativeStatus +
", iterations=" + solved.Iterations + ", primal=" + solved.PrimalResidual + ", dual=" +
solved.DualResidual + "): " + solved.Diagnostic;
solved.DualResidual + "): " + solved.Diagnostic);
return false;
}
if (solved.Status == QpSolveStatus.Cancelled)
{
failureStatus = EmPlanningStatus.Cancelled;
failureReason = "Initial full-direction feasibility projection was cancelled: " + solved.Diagnostic;
failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection was cancelled: " + solved.Diagnostic);
return false;
}
if (solved.Status == QpSolveStatus.PrimalInfeasible || solved.Status == QpSolveStatus.DualInfeasible)
{
failureStatus = EmPlanningStatus.LongitudinalInfeasible;
failureReason = "Initial full-direction feasibility projection is infeasible: " + solved.Diagnostic;
failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection is infeasible: " + solved.Diagnostic);
return false;
}
if (solved.Status == QpSolveStatus.SolverUnavailable)
{
failureStatus = EmPlanningStatus.SolverUnavailable;
failureReason = "Initial full-direction feasibility solver is unavailable: " + solved.Diagnostic;
failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility solver is unavailable: " + solved.Diagnostic);
return false;
}
if (solved.Status != QpSolveStatus.Solved && solved.Status != QpSolveStatus.SolvedInaccurate)
{
failureStatus = EmPlanningStatus.Failed;
failureReason = "Initial full-direction feasibility solver failed: " + solved.Diagnostic;
failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility solver failed: " + solved.Diagnostic);
return false;
}
if (!TryCreateCandidate(input.KnotSchedule.KnotTimes, solved.Primal, out LongitudinalCandidate projected))
{
failureStatus = EmPlanningStatus.LongitudinalInfeasible;
failureReason = "Initial full-direction feasibility solver primal does not match the ST layout.";
failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility solver primal does not match the ST layout.");
return false;
}
if (solved.Status == QpSolveStatus.Solved || HasStrictResiduals(solved, convergenceTolerance))
@@ -319,7 +348,7 @@ public sealed class SequentialLongitudinalOptimizer
if (validationStatus == EmPlanningStatus.NoProgress)
{
failureStatus = validationStatus;
failureReason = validationFailure;
failureReason = WithStaticSeedDiagnostic(validationFailure);
return false;
}
lastRejection = validationFailure;
@@ -329,7 +358,7 @@ public sealed class SequentialLongitudinalOptimizer
out LongitudinalCandidate nextLinearization))
{
failureStatus = EmPlanningStatus.LongitudinalInfeasible;
failureReason = "Initial full-direction feasibility candidate could not be relinearized against the PathS envelope.";
failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility candidate could not be relinearized against the PathS envelope.");
return false;
}
linearizationIterate = nextLinearization;
@@ -339,8 +368,8 @@ public sealed class SequentialLongitudinalOptimizer
lastRejection = "Initial feasibility projection violated the strict physical validator.";
}
failureStatus = EmPlanningStatus.LongitudinalInfeasible;
failureReason = "Initial full-direction feasibility projection exhausted the configured outer iterations. " +
lastRejection;
failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection exhausted the configured outer iterations. " +
lastRejection);
return false;
}
@@ -466,24 +495,31 @@ public sealed class SequentialLongitudinalOptimizer
}
private bool TryCreateStaticStartSeed(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
out LongitudinalCandidate candidate)
out LongitudinalCandidate candidate, out string failureReason)
{
candidate = null;
failureReason = "unknown";
int stabilizationStart = input.KnotSchedule.TerminalHoldStartIndex;
if (stabilizationStart < 5)
{
failureReason = "terminalHoldStartIndex=" + stabilizationStart.ToString(CultureInfo.InvariantCulture);
return false;
}
IReadOnlyList<double> times = input.KnotSchedule.KnotTimes;
if (TryCreateExactJerkSeed(input, times, stabilizationStart, speedLimit, out candidate))
return true;
failureReason = "exactJerkSeed=failed";
double firstDuration = times[1] - times[0];
double secondDuration = times[2] - times[1];
LongitudinalConfiguration configuration = input.Configuration.Longitudinal;
double maximumFirstJerk = Math.Min(configuration.MaximumJerkMetersPerSecondCubed,
Math.Min(configuration.MaximumAccelerationMetersPerSecondSquared / firstDuration,
configuration.MaximumJerkMetersPerSecondCubed * secondDuration / firstDuration));
for (int sample = 1; sample <= 256; sample++)
for (int sample = -256; sample <= 256; sample++)
{
if (sample == 0)
continue;
double firstJerk = maximumFirstJerk * sample / 256d;
var jerk = new double[stabilizationStart];
jerk[0] = firstJerk;
@@ -497,9 +533,11 @@ public sealed class SequentialLongitudinalOptimizer
out LongitudinalCandidate strict, out _))
{
candidate = strict;
failureReason = string.Empty;
return true;
}
}
failureReason = "exactJerkSeed=failed; sampledSeeds=failed";
return false;
}