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,