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/LongitudinalConstraintBuilder.cs
+31 -14
View File
@@ -3,7 +3,7 @@ using System.Collections.Generic;
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
/// <summary>Builds one normalized ST QP with exact constant-jerk integration and hard terminal conditions.</summary>
/// <summary>Builds one normalized ST QP with exact constant-jerk integration and mode-specific stop conditions.</summary>
public sealed class LongitudinalConstraintBuilder
{
private readonly LongitudinalObjectiveBuilder _objectiveBuilder;
@@ -22,8 +22,8 @@ public sealed class LongitudinalConstraintBuilder
{
if (input == null || speedLimit == null || iterate == null)
throw new ArgumentException("ST input, speed envelope, and iterate are required.");
if (Math.Abs(speedLimit.TerminalPathS - input.TerminalPathS) > 1e-12d)
throw new ArgumentException("The speed envelope terminal must match actual lateral PathS.");
if (Math.Abs(speedLimit.PathUpperBoundS - input.PathUpperBoundS) > 1e-12d)
throw new ArgumentException("The speed envelope upper bound must match actual lateral PathS.");
IReadOnlyList<double> expectedTimes = LongitudinalCandidate.CreateKnotTimes(
input.Configuration.Scheduling.TimeHorizonSeconds, input.Configuration.Scheduling.OutputTimeStepSeconds);
@@ -51,16 +51,24 @@ public sealed class LongitudinalConstraintBuilder
var hessian = new SparseTripletBuilder(layout.VariableCount, layout.VariableCount, true);
var linearCost = new double[layout.VariableCount];
_objectiveBuilder.AddTerms(input, speedLimit, layout, iterate, hessian, linearCost);
var constraints = new SparseTripletBuilder(8 * layout.KnotCount, layout.VariableCount);
var lower = new List<double>(8 * layout.KnotCount);
var upper = new List<double>(8 * layout.KnotCount);
int stabilizationStart = input.Mode == EmLongitudinalMode.ExactStopAtBoundary
? LongitudinalTerminalSchedule.GetStabilizationStartIndex(expectedTimes,
input.Configuration.Scheduling.OutputTimeStepSeconds)
: layout.KnotCount;
int stationaryKnotCount = layout.KnotCount - stabilizationStart;
int expectedRows = 8 * layout.KnotCount - 2 + 3 * stationaryKnotCount;
var constraints = new SparseTripletBuilder(expectedRows, layout.VariableCount);
var lower = new List<double>(expectedRows);
var upper = new List<double>(expectedRows);
int row = 0;
AddVariableBounds(input, speedLimit, iterate, layout, maximumAcceleration, maximumDeceleration, maximumJerk,
constraints, lower, upper, ref row);
AddMonotonicProgress(layout, constraints, lower, upper, ref row);
AddExactDynamics(expectedTimes, layout, constraints, lower, upper, ref row);
AddExactStartAndTerminal(input, layout, constraints, lower, upper, ref row);
if (row != 8 * layout.KnotCount)
AddExactStart(input, layout, constraints, lower, upper, ref row);
if (input.Mode == EmLongitudinalMode.ExactStopAtBoundary)
AddExactStopTail(input, layout, stabilizationStart, constraints, lower, upper, ref row);
if (row != expectedRows)
throw new InvalidOperationException("ST constraint row accounting is inconsistent.");
problem = new QuadraticProgram(hessian.Build(), linearCost, constraints.Build(), lower, upper);
return true;
@@ -79,9 +87,9 @@ public sealed class LongitudinalConstraintBuilder
{
for (int index = 0; index < layout.KnotCount; index++)
{
if (iterate.S[index] < 0d || iterate.S[index] > input.TerminalPathS)
if (iterate.S[index] < 0d || iterate.S[index] > input.PathUpperBoundS)
throw new ArgumentException("The ST iterate progress lies outside actual PathS bounds.");
AddSingleVariableRow(constraints, lower, upper, layout.S(index), 0d, input.TerminalPathS, ref row);
AddSingleVariableRow(constraints, lower, upper, layout.S(index), 0d, input.PathUpperBoundS, ref row);
AddSingleVariableRow(constraints, lower, upper, layout.U(index), 0d,
Math.Min(input.DirectionMaximumSpeedMetersPerSecond, speedLimit.MaximumSpeedAt(iterate.S[index])), ref row);
AddSingleVariableRow(constraints, lower, upper, layout.A(index), -maximumDeceleration, maximumAcceleration,
@@ -132,7 +140,7 @@ public sealed class LongitudinalConstraintBuilder
}
}
private static void AddExactStartAndTerminal(LongitudinalPlanningInput input, LongitudinalVariableLayout layout,
private static void AddExactStart(LongitudinalPlanningInput input, LongitudinalVariableLayout layout,
SparseTripletBuilder constraints, IList<double> lower, IList<double> upper, ref int row)
{
AddSingleVariableRow(constraints, lower, upper, layout.S(0), 0d, 0d, ref row);
@@ -140,9 +148,18 @@ public sealed class LongitudinalConstraintBuilder
input.InitialProgressSpeedMetersPerSecond, ref row);
AddSingleVariableRow(constraints, lower, upper, layout.A(0), input.InitialAccelerationMetersPerSecondSquared,
input.InitialAccelerationMetersPerSecondSquared, ref row);
AddSingleVariableRow(constraints, lower, upper, layout.S(layout.KnotCount - 1), input.TerminalPathS,
input.TerminalPathS, ref row);
AddSingleVariableRow(constraints, lower, upper, layout.U(layout.KnotCount - 1), 0d, 0d, ref row);
}
private static void AddExactStopTail(LongitudinalPlanningInput input, LongitudinalVariableLayout layout,
int stabilizationStart, SparseTripletBuilder constraints, IList<double> lower, IList<double> upper, ref int row)
{
for (int index = stabilizationStart; index < layout.KnotCount; index++)
{
AddSingleVariableRow(constraints, lower, upper, layout.S(index), input.StopBoundaryPathS,
input.StopBoundaryPathS, ref row);
AddSingleVariableRow(constraints, lower, upper, layout.U(index), 0d, 0d, ref row);
AddSingleVariableRow(constraints, lower, upper, layout.A(index), 0d, 0d, ref row);
}
}
private static void AddSingleVariableRow(SparseTripletBuilder constraints, IList<double> lower, IList<double> upper,
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))