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

feat: apply ST stop constraints only at real boundaries

Browse Source
This commit is contained in:
梁薄云
2026-08-05 17:42:41 +08:00
parent 94a9be9c02
commit efa03c1f3a
3 changed files with 186 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs
+40 -9
View File
@@ -56,13 +56,14 @@ public sealed class LongitudinalSolutionValidator
double speed = candidate.U[index];
double acceleration = candidate.A[index];
if (!IsFinite(progress) || !IsFinite(speed) || !IsFinite(acceleration) || progress < -tolerance ||
progress > input.TerminalPathS + tolerance || speed < -tolerance ||
progress > input.PathUpperBoundS + tolerance || speed < -tolerance ||
acceleration < -maximumDeceleration - tolerance || acceleration > maximumAcceleration + tolerance)
{
failureReason = "ST candidate violates physical bounds at knot " + index + ".";
return false;
}
double speedLimitAtProgress = speedLimit.MaximumSpeedAt(Math.Max(0d, Math.Min(input.TerminalPathS, progress)));
double speedLimitAtProgress = speedLimit.MaximumSpeedAt(
Math.Max(0d, Math.Min(input.PathUpperBoundS, progress)));
if (speed > speedLimitAtProgress + tolerance)
{
failureReason = "ST candidate violates the actual-PathS speed envelope at knot " + index +
@@ -83,12 +84,35 @@ public sealed class LongitudinalSolutionValidator
return false;
}
}
int terminalIndex = candidate.S.Count - 1;
if (!AreClose(candidate.S[terminalIndex], input.TerminalPathS, tolerance) ||
!AreClose(candidate.U[terminalIndex], 0d, tolerance))
int stabilizationStart = candidate.S.Count;
if (input.Mode == EmLongitudinalMode.ExactStopAtBoundary)
{
failureReason = "ST candidate does not satisfy the exact zero-speed terminal.";
return false;
stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
candidate.KnotTimes, input.Configuration.Scheduling.OutputTimeStepSeconds);
for (int index = stabilizationStart; index < candidate.S.Count; index++)
{
if (!AreClose(candidate.S[index], input.StopBoundaryPathS, tolerance) ||
!AreClose(candidate.U[index], 0d, tolerance) ||
!AreClose(candidate.A[index], 0d, tolerance))
{
failureReason = "ST candidate does not satisfy the exact stabilized S/U/A stop tail at knot " +
index + ".";
return false;
}
}
}
if (input.Mode != EmLongitudinalMode.RollingContinuation)
{
for (int index = 0; index < candidate.S.Count; index++)
{
if (!JerkLimitedStoppingMath.TryCalculate(candidate.U[index], candidate.A[index],
maximumDeceleration, maximumJerk, out JerkLimitedStoppingProfile stop, out _) ||
candidate.S[index] + stop.DistanceMeters > input.StopBoundaryPathS + tolerance)
{
failureReason = "ST candidate leaves the jerk-limited stoppable set at knot " + index + ".";
return false;
}
}
}
var canonicalS = new double[candidate.S.Count];
@@ -103,8 +127,15 @@ public sealed class LongitudinalSolutionValidator
canonicalS[0] = 0d;
canonicalU[0] = input.InitialProgressSpeedMetersPerSecond;
canonicalA[0] = input.InitialAccelerationMetersPerSecondSquared;
canonicalS[terminalIndex] = input.TerminalPathS;
canonicalU[terminalIndex] = 0d;
if (input.Mode == EmLongitudinalMode.ExactStopAtBoundary)
{
for (int index = stabilizationStart; index < candidate.S.Count; index++)
{
canonicalS[index] = input.StopBoundaryPathS;
canonicalU[index] = 0d;
canonicalA[index] = 0d;
}
}
var canonicalCandidate = new LongitudinalCandidate(candidate.KnotTimes, canonicalS, canonicalU, canonicalA,
candidate.J);
if (!canonicalCandidate.SatisfiesExactDiscreteDynamics(tolerance))