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

feat: derive adaptive full-segment ST schedule

Browse Source
This commit is contained in:
梁薄云
2026-08-06 23:54:34 +08:00
parent dad4ff4b04
commit 3269d556b6
14 changed files with 1642 additions and 108 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalConstraintBuilder.cs
+128 -10
View File
@@ -15,6 +15,35 @@ public sealed class LongitudinalConstraintBuilder
public bool TryBuild(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate iterate,
out QuadraticProgram problem, out string failureReason)
{
return TryBuildCore(input, speedLimit, iterate, false, out problem, out failureReason);
}
/// <summary>Builds the bounded full-scope feasibility projection before objective optimization.</summary>
public bool TryBuildInitialFeasibilityProjection(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
out QuadraticProgram problem, out string failureReason)
{
return TryBuildInitialFeasibilityProjection(input, speedLimit,
input == null ? null : CreateScheduleReferenceIterate(input), out problem, out failureReason);
}
public bool TryBuildInitialFeasibilityProjection(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
LongitudinalCandidate linearizationIterate, out QuadraticProgram problem, out string failureReason)
{
problem = null;
failureReason = string.Empty;
if (input == null || input.PlanningScope != EmPlanningScope.FullDirectionSegment ||
input.Mode != EmLongitudinalMode.ExactStopAtBoundary || linearizationIterate == null)
{
failureReason = "Initial feasibility projection is only defined for full-direction exact-stop planning.";
return false;
}
return TryBuildCore(input, speedLimit, linearizationIterate, true, out problem,
out failureReason);
}
private bool TryBuildCore(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate iterate,
bool useScheduleReferenceObjective, out QuadraticProgram problem, out string failureReason)
{
problem = null;
failureReason = string.Empty;
@@ -25,10 +54,9 @@ public sealed class LongitudinalConstraintBuilder
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);
IReadOnlyList<double> expectedTimes = input.KnotSchedule.KnotTimes;
if (!HasMatchingTimes(iterate.KnotTimes, expectedTimes))
throw new ArgumentException("The ST iterate time knots do not match the configured horizon.");
throw new ArgumentException("The ST iterate time knots do not match the supplied knot schedule.");
var layout = new LongitudinalVariableLayout(expectedTimes.Count);
if (iterate.S.Count != layout.KnotCount || iterate.U.Count != layout.KnotCount ||
iterate.A.Count != layout.KnotCount || iterate.J.Count != layout.KnotCount - 1)
@@ -50,13 +78,13 @@ 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);
int stabilizationStart = input.Mode == EmLongitudinalMode.ExactStopAtBoundary
? LongitudinalTerminalSchedule.GetStabilizationStartIndex(expectedTimes,
input.Configuration.Scheduling.OutputTimeStepSeconds)
: layout.KnotCount;
if (useScheduleReferenceObjective)
AddInitialFeasibilityObjective(input, layout, hessian, linearCost);
else
_objectiveBuilder.AddTerms(input, speedLimit, layout, iterate, hessian, linearCost);
int stabilizationStart = GetStabilizationStart(input, expectedTimes, layout.KnotCount);
int stationaryKnotCount = layout.KnotCount - stabilizationStart;
int expectedRows = 8 * layout.KnotCount - 2 + 3 * stationaryKnotCount;
int expectedRows = 9 * layout.KnotCount - 3 + 3 * stationaryKnotCount;
var constraints = new SparseTripletBuilder(expectedRows, layout.VariableCount);
var lower = new List<double>(expectedRows);
var upper = new List<double>(expectedRows);
@@ -80,6 +108,42 @@ public sealed class LongitudinalConstraintBuilder
}
}
private static LongitudinalCandidate CreateScheduleReferenceIterate(LongitudinalPlanningInput input)
{
int knotCount = input.KnotSchedule.KnotTimes.Count;
return new LongitudinalCandidate(input.KnotSchedule.KnotTimes, input.KnotSchedule.ReferencePathS,
input.KnotSchedule.ReferenceSpeedMetersPerSecond, new double[knotCount], new double[knotCount - 1]);
}
private static void AddInitialFeasibilityObjective(LongitudinalPlanningInput input, LongitudinalVariableLayout layout,
SparseTripletBuilder hessian, IList<double> linearCost)
{
double progressScale = 1d;
double speedScale = 1d;
double accelerationScale = 1d;
double jerkScale = 1d;
for (int index = 0; index < layout.KnotCount; index++)
{
AddProjectionSquaredResidual(hessian, linearCost, layout.S(index), input.KnotSchedule.ReferencePathS[index],
1d, progressScale);
AddProjectionSquaredResidual(hessian, linearCost, layout.U(index),
input.KnotSchedule.ReferenceSpeedMetersPerSecond[index], 10d, speedScale);
AddProjectionSquaredResidual(hessian, linearCost, layout.A(index), 0d, 1e-3d, accelerationScale);
}
for (int index = 0; index < layout.KnotCount - 1; index++)
AddProjectionSquaredResidual(hessian, linearCost, layout.J(index), 0d, 1e-3d, jerkScale);
}
private static void AddProjectionSquaredResidual(SparseTripletBuilder hessian, IList<double> linearCost,
int variable, double reference, double weight, double scale)
{
if (!IsFinite(reference) || !IsFinite(weight) || weight <= 0d || !IsFinite(scale) || scale <= 0d)
throw new ArgumentOutOfRangeException(nameof(reference));
double coefficient = 2d * weight / (scale * scale);
hessian.Add(variable, variable, coefficient);
linearCost[variable] += -coefficient * reference;
}
private static void AddVariableBounds(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
LongitudinalCandidate iterate, LongitudinalVariableLayout layout, double maximumAcceleration,
double maximumDeceleration, double maximumJerk, SparseTripletBuilder constraints, IList<double> lower,
@@ -92,8 +156,11 @@ public sealed class LongitudinalConstraintBuilder
AddSingleVariableRow(constraints, lower, upper, layout.S(index), 0d, input.PathUpperBoundS, ref row);
double maximumSpeed = index == 0
? input.DirectionMaximumSpeedMetersPerSecond
: Math.Min(input.DirectionMaximumSpeedMetersPerSecond, speedLimit.MaximumSpeedAt(iterate.S[index]));
: input.DirectionMaximumSpeedMetersPerSecond;
AddSingleVariableRow(constraints, lower, upper, layout.U(index), 0d, maximumSpeed, ref row);
if (index > 0)
AddLinearizedSpeedEnvelopeRow(speedLimit, iterate.S[index], layout.S(index), layout.U(index),
constraints, lower, upper, ref row);
AddSingleVariableRow(constraints, lower, upper, layout.A(index), -maximumDeceleration, maximumAcceleration,
ref row);
}
@@ -101,6 +168,34 @@ public sealed class LongitudinalConstraintBuilder
AddSingleVariableRow(constraints, lower, upper, layout.J(index), -maximumJerk, maximumJerk, ref row);
}
private static void AddLinearizedSpeedEnvelopeRow(PathSpeedLimit speedLimit, double pathS, int pathSVariable,
int speedVariable, SparseTripletBuilder constraints, IList<double> lower, IList<double> upper, ref int row)
{
int segment = FindSpeedEnvelopeSegment(speedLimit, pathS);
double startS = speedLimit.PathS[segment];
double endS = speedLimit.PathS[segment + 1];
double startSpeed = speedLimit.MaximumSpeedMetersPerSecond[segment];
double endSpeed = speedLimit.MaximumSpeedMetersPerSecond[segment + 1];
double slope = (endSpeed - startSpeed) / (endS - startS);
double intercept = startSpeed - slope * startS;
AddRow(constraints, lower, upper, row, new[]
{
new Coefficient(speedVariable, 1d), new Coefficient(pathSVariable, -slope),
}, -QuadraticProgram.MaximumFiniteBound, intercept);
row++;
}
private static int FindSpeedEnvelopeSegment(PathSpeedLimit speedLimit, double pathS)
{
double clamped = Math.Max(speedLimit.PathS[0], Math.Min(speedLimit.PathUpperBoundS, pathS));
for (int index = 0; index < speedLimit.PathS.Count - 1; index++)
{
if (clamped <= speedLimit.PathS[index + 1])
return index;
}
return speedLimit.PathS.Count - 2;
}
private static void AddMonotonicProgress(LongitudinalVariableLayout layout, SparseTripletBuilder constraints,
IList<double> lower, IList<double> upper, ref int row)
{
@@ -164,6 +259,24 @@ public sealed class LongitudinalConstraintBuilder
}
}
private static int GetStabilizationStart(LongitudinalPlanningInput input, IReadOnlyList<double> times,
int knotCount)
{
if (input.Mode != EmLongitudinalMode.ExactStopAtBoundary)
return knotCount;
if (input.PlanningScope == EmPlanningScope.FullDirectionSegment)
{
if (input.KnotSchedule.TerminalHoldStartIndex < 1 ||
input.KnotSchedule.TerminalHoldStartIndex >= knotCount)
{
throw new ArgumentException("Full-direction exact-stop schedules require an explicit terminal hold boundary.");
}
return input.KnotSchedule.TerminalHoldStartIndex;
}
return LongitudinalTerminalSchedule.GetStabilizationStartIndex(times,
input.Configuration.Scheduling.OutputTimeStepSeconds);
}
private static void AddSingleVariableRow(SparseTripletBuilder constraints, IList<double> lower, IList<double> upper,
int variable, double minimum, double maximum, ref int row)
{
@@ -192,6 +305,11 @@ public sealed class LongitudinalConstraintBuilder
return true;
}
private static bool IsFinite(double value)
{
return !double.IsNaN(value) && !double.IsInfinity(value);
}
private readonly struct Coefficient
{
public Coefficient(int variable, double value)