feat: apply ST stop constraints only at real boundaries
This commit is contained in:
梁薄云
+31
-14
@@ -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,
|
||||
|
||||
+40
-9
@@ -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))
|
||||
|
||||
@@ -19,6 +19,7 @@ internal static class LongitudinalModelChecks
|
||||
VerifiesStoppingPrecheckOnlyAppliesToRealStopBoundaries();
|
||||
VerifiesReferenceHorizonSelectionSeparatesSpaceAndTime();
|
||||
VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints();
|
||||
VerifiesModeSpecificSolutionValidation();
|
||||
}
|
||||
|
||||
private static void VerifiesJerkLimitedStoppingProfileEndsAtRest()
|
||||
@@ -313,6 +314,17 @@ internal static class LongitudinalModelChecks
|
||||
|
||||
Verification.True(new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder()).TryBuild(input, envelope,
|
||||
integrated, out QuadraticProgram problem, out string failureReason), "ST QP builds: " + failureReason);
|
||||
var rollingInput = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.10d, 0.02d,
|
||||
EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration,
|
||||
new[] { 0d, 0.10d, 0.20d, 0.30d, 0.40d },
|
||||
new[] { 0.20d, 0.20d, 0.20d, 0.20d, 0.20d });
|
||||
EmPlanningStatus rollingSpeedStatus = new PathSpeedLimitBuilder().Build(rollingInput,
|
||||
out PathSpeedLimit rollingEnvelope, out string rollingSpeedFailure);
|
||||
Verification.Equal(EmPlanningStatus.Success, rollingSpeedStatus,
|
||||
"unit-scale rolling speed envelope: " + rollingSpeedFailure);
|
||||
Verification.True(new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder()).TryBuild(
|
||||
rollingInput, rollingEnvelope, integrated, out QuadraticProgram rollingProblem, out string rollingFailure),
|
||||
"rolling ST QP builds: " + rollingFailure);
|
||||
Verification.NearlyEqual(30d, MatrixValue(problem.UpperTriangularP, layout.U(0), layout.U(0)),
|
||||
"normalized speed and previous-U P coefficient");
|
||||
Verification.NearlyEqual(2d, MatrixValue(problem.UpperTriangularP, layout.A(0), layout.A(0)),
|
||||
@@ -344,10 +356,29 @@ internal static class LongitudinalModelChecks
|
||||
"exact initial U");
|
||||
Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double> { { layout.A(0), 1d } }, 0.02d),
|
||||
"exact initial A");
|
||||
Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double> { { layout.S(4), 1d } }, 2d),
|
||||
"exact terminal S");
|
||||
Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double> { { layout.U(4), 1d } }, 0d),
|
||||
"exact terminal U");
|
||||
Verification.Equal(0, CountExactEqualityRows(rollingProblem,
|
||||
new Dictionary<int, double> { { layout.S(4), 1d } }, rollingInput.PathUpperBoundS),
|
||||
"rolling has no exact terminal S");
|
||||
Verification.Equal(0, CountExactEqualityRows(rollingProblem,
|
||||
new Dictionary<int, double> { { layout.U(4), 1d } }, 0d),
|
||||
"rolling has no exact terminal U");
|
||||
Verification.Equal(0, CountExactEqualityRows(rollingProblem,
|
||||
new Dictionary<int, double> { { layout.A(4), 1d } }, 0d),
|
||||
"rolling has no exact terminal A");
|
||||
int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
|
||||
integrated.KnotTimes, configuration.Scheduling.OutputTimeStepSeconds);
|
||||
for (int index = stabilizationStart; index < layout.KnotCount; index++)
|
||||
{
|
||||
Verification.Equal(1, CountExactEqualityRows(problem,
|
||||
new Dictionary<int, double> { { layout.S(index), 1d } }, input.StopBoundaryPathS),
|
||||
"stop tail exact S " + index);
|
||||
Verification.Equal(1, CountExactEqualityRows(problem,
|
||||
new Dictionary<int, double> { { layout.U(index), 1d } }, 0d),
|
||||
"stop tail exact U " + index);
|
||||
Verification.Equal(1, CountExactEqualityRows(problem,
|
||||
new Dictionary<int, double> { { layout.A(index), 1d } }, 0d),
|
||||
"stop tail exact A " + index);
|
||||
}
|
||||
Verification.Equal(1, CountBoundedRow(problem, new Dictionary<int, double>
|
||||
{
|
||||
{ layout.S(1), 1d }, { layout.S(0), -1d },
|
||||
@@ -367,6 +398,62 @@ internal static class LongitudinalModelChecks
|
||||
}, 0d), "exact ST progress equation");
|
||||
}
|
||||
|
||||
private static void VerifiesModeSpecificSolutionValidation()
|
||||
{
|
||||
EmPlannerConfiguration configuration = CreateTaskFourConfiguration();
|
||||
IReadOnlyList<double> exactTimes = LongitudinalCandidate.CreateKnotTimes(0.30d, 0.10d);
|
||||
LongitudinalCandidate nonstationaryExact = LongitudinalCandidate.Integrate(
|
||||
exactTimes, 0d, 0.10d, 0d, new[] { -4d, 0d, 0d });
|
||||
LateralPath exactPath = CreateStraightPath(nonstationaryExact.S[nonstationaryExact.S.Count - 1]);
|
||||
var exactInput = new LongitudinalPlanningInput(exactPath, TravelDirection.Forward, 0.10d, 0d,
|
||||
EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
|
||||
Array.Empty<double>(), Array.Empty<double>());
|
||||
var rollingForExact = new LongitudinalPlanningInput(exactPath, TravelDirection.Forward, 0.10d, 0d,
|
||||
EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration,
|
||||
Array.Empty<double>(), Array.Empty<double>());
|
||||
EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(rollingForExact,
|
||||
out PathSpeedLimit rollingEnvelope, out string speedFailure);
|
||||
Verification.Equal(EmPlanningStatus.Success, speedStatus, "exact-validation rolling envelope: " + speedFailure);
|
||||
|
||||
var validator = new LongitudinalSolutionValidator();
|
||||
Verification.True(!validator.TryValidate(exactInput, rollingEnvelope, nonstationaryExact,
|
||||
out _, out string exactFailure), "exact stops reject a nonstationary internal tail");
|
||||
Verification.True(exactFailure.IndexOf("exact stabilized", StringComparison.Ordinal) >= 0,
|
||||
"exact-stop failure identifies the stabilized tail: " + exactFailure);
|
||||
|
||||
LateralPath rollingPath = CreateStraightPath(0.10d);
|
||||
var rollingInput = new LongitudinalPlanningInput(rollingPath, TravelDirection.Forward, 0.10d, 0d,
|
||||
EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration,
|
||||
Array.Empty<double>(), Array.Empty<double>());
|
||||
speedStatus = new PathSpeedLimitBuilder().Build(rollingInput, out PathSpeedLimit openEnvelope, out speedFailure);
|
||||
Verification.Equal(EmPlanningStatus.Success, speedStatus, "rolling-validation envelope: " + speedFailure);
|
||||
LongitudinalCandidate rollingCandidate = LongitudinalCandidate.Integrate(
|
||||
exactTimes, 0d, 0.10d, 0d, new[] { 0d, 0d, 0d });
|
||||
Verification.True(validator.TryValidate(rollingInput, openEnvelope, rollingCandidate,
|
||||
out _, out string rollingFailure), "rolling nonzero terminal speed validates: " + rollingFailure);
|
||||
|
||||
EmPlannerConfiguration approachConfiguration = CreateTaskFourConfiguration();
|
||||
approachConfiguration.Scheduling.TimeHorizonSeconds = 0.15d;
|
||||
approachConfiguration.Scheduling.OutputTimeStepSeconds = 0.05d;
|
||||
IReadOnlyList<double> approachTimes = LongitudinalCandidate.CreateKnotTimes(0.15d, 0.05d);
|
||||
LongitudinalCandidate unstoppablyFastApproach = LongitudinalCandidate.Integrate(
|
||||
approachTimes, 0d, 0.20d, 0d, new[] { 0d, -10d, 10d });
|
||||
LateralPath approachPath = CreateStraightPath(0.035d);
|
||||
var approachInput = new LongitudinalPlanningInput(approachPath, TravelDirection.Forward, 0.20d, 0d,
|
||||
EmTerminalType.Goal, EmLongitudinalMode.ApproachStopBoundary, approachConfiguration,
|
||||
Array.Empty<double>(), Array.Empty<double>());
|
||||
var rollingForApproach = new LongitudinalPlanningInput(approachPath, TravelDirection.Forward, 0.20d, 0d,
|
||||
EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, approachConfiguration,
|
||||
Array.Empty<double>(), Array.Empty<double>());
|
||||
speedStatus = new PathSpeedLimitBuilder().Build(rollingForApproach,
|
||||
out PathSpeedLimit approachEnvelope, out speedFailure);
|
||||
Verification.Equal(EmPlanningStatus.Success, speedStatus, "approach-validation rolling envelope: " + speedFailure);
|
||||
Verification.True(!validator.TryValidate(approachInput, approachEnvelope, unstoppablyFastApproach,
|
||||
out _, out string approachFailure), "approach candidates outside the stoppable set are rejected");
|
||||
Verification.True(approachFailure.IndexOf("stoppable set", StringComparison.Ordinal) >= 0,
|
||||
"approach failure identifies the jerk-limited stoppable set");
|
||||
}
|
||||
|
||||
private static LateralPath CreatePath(IReadOnlyList<PathFixture> fixtures)
|
||||
{
|
||||
var points = new List<LateralPathPoint>(fixtures.Count);
|
||||
@@ -412,6 +499,30 @@ internal static class LongitudinalModelChecks
|
||||
return configuration;
|
||||
}
|
||||
|
||||
private static EmPlannerConfiguration CreateTaskFourConfiguration()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Scheduling.TimeHorizonSeconds = 0.30d;
|
||||
configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
|
||||
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
|
||||
configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
|
||||
configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1e-6d;
|
||||
configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
|
||||
configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 10d;
|
||||
configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared = 1d;
|
||||
configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1d;
|
||||
return configuration;
|
||||
}
|
||||
|
||||
private static LateralPath CreateStraightPath(double pathUpperBoundS)
|
||||
{
|
||||
return CreatePath(new[]
|
||||
{
|
||||
new PathFixture(0d, 0d, 0d, 0d),
|
||||
new PathFixture(pathUpperBoundS, pathUpperBoundS, 0d, 0d),
|
||||
});
|
||||
}
|
||||
|
||||
private static double MaximumJerkLimitedStopSpeed(LongitudinalPlanningInput input, double pathS)
|
||||
{
|
||||
LongitudinalConfiguration limits = input.Configuration.Longitudinal;
|
||||
|
||||
Reference in New Issue
Block a user