feat: assemble longitudinal ST quadratic programs
This commit is contained in:
梁薄云
@@ -0,0 +1,124 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Immutable time-knot s/u/a/j iterate used for ST linearization and strict validation.</summary>
|
||||
public sealed class LongitudinalCandidate
|
||||
{
|
||||
public LongitudinalCandidate(IReadOnlyList<double> knotTimes, IReadOnlyList<double> s, IReadOnlyList<double> u,
|
||||
IReadOnlyList<double> a, IReadOnlyList<double> j)
|
||||
{
|
||||
KnotTimes = CopyTimes(knotTimes);
|
||||
S = CopyValues(s, KnotTimes.Count, nameof(s));
|
||||
U = CopyValues(u, KnotTimes.Count, nameof(u));
|
||||
A = CopyValues(a, KnotTimes.Count, nameof(a));
|
||||
J = CopyValues(j, KnotTimes.Count - 1, nameof(j));
|
||||
}
|
||||
|
||||
public IReadOnlyList<double> KnotTimes { get; }
|
||||
|
||||
public IReadOnlyList<double> S { get; }
|
||||
|
||||
public IReadOnlyList<double> U { get; }
|
||||
|
||||
public IReadOnlyList<double> A { get; }
|
||||
|
||||
public IReadOnlyList<double> J { get; }
|
||||
|
||||
public static LongitudinalCandidate Integrate(IReadOnlyList<double> knotTimes, double initialS, double initialU,
|
||||
double initialA, IReadOnlyList<double> jerk)
|
||||
{
|
||||
IReadOnlyList<double> times = CopyTimes(knotTimes);
|
||||
if (!IsFinite(initialS) || !IsFinite(initialU) || !IsFinite(initialA))
|
||||
throw new ArgumentOutOfRangeException(nameof(initialS));
|
||||
IReadOnlyList<double> copiedJerk = CopyValues(jerk, times.Count - 1, nameof(jerk));
|
||||
var s = new double[times.Count];
|
||||
var u = new double[times.Count];
|
||||
var a = new double[times.Count];
|
||||
s[0] = initialS;
|
||||
u[0] = initialU;
|
||||
a[0] = initialA;
|
||||
for (int index = 0; index < copiedJerk.Count; index++)
|
||||
{
|
||||
double dt = times[index + 1] - times[index];
|
||||
double currentJerk = copiedJerk[index];
|
||||
a[index + 1] = a[index] + dt * currentJerk;
|
||||
u[index + 1] = u[index] + dt * a[index] + 0.5d * dt * dt * currentJerk;
|
||||
s[index + 1] = s[index] + dt * u[index] + 0.5d * dt * dt * a[index] +
|
||||
dt * dt * dt * currentJerk / 6d;
|
||||
}
|
||||
return new LongitudinalCandidate(times, s, u, a, copiedJerk);
|
||||
}
|
||||
|
||||
public bool SatisfiesExactDiscreteDynamics(double tolerance)
|
||||
{
|
||||
if (!IsFinite(tolerance) || tolerance < 0d)
|
||||
throw new ArgumentOutOfRangeException(nameof(tolerance));
|
||||
for (int index = 0; index < J.Count; index++)
|
||||
{
|
||||
double dt = KnotTimes[index + 1] - KnotTimes[index];
|
||||
if (Math.Abs(A[index + 1] - (A[index] + dt * J[index])) > tolerance ||
|
||||
Math.Abs(U[index + 1] - (U[index] + dt * A[index] + 0.5d * dt * dt * J[index])) > tolerance ||
|
||||
Math.Abs(S[index + 1] - (S[index] + dt * U[index] + 0.5d * dt * dt * A[index] +
|
||||
dt * dt * dt * J[index] / 6d)) > tolerance)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
public static IReadOnlyList<double> CreateKnotTimes(double timeHorizonSeconds, double outputTimeStepSeconds)
|
||||
{
|
||||
if (!IsFinite(timeHorizonSeconds) || !IsFinite(outputTimeStepSeconds) || timeHorizonSeconds <= 0d ||
|
||||
outputTimeStepSeconds <= 0d)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(timeHorizonSeconds));
|
||||
}
|
||||
int intervalCount = checked((int)Math.Ceiling(timeHorizonSeconds / outputTimeStepSeconds));
|
||||
var times = new double[intervalCount + 1];
|
||||
for (int index = 0; index < intervalCount; index++)
|
||||
times[index] = index * outputTimeStepSeconds;
|
||||
times[intervalCount] = timeHorizonSeconds;
|
||||
return new ReadOnlyCollection<double>(times);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<double> CopyTimes(IReadOnlyList<double> source)
|
||||
{
|
||||
if (source == null || source.Count < 2)
|
||||
throw new ArgumentException("At least two strictly increasing time knots are required.", nameof(source));
|
||||
var copy = new List<double>(source.Count);
|
||||
double previous = double.NegativeInfinity;
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
{
|
||||
if (!IsFinite(source[index]) || source[index] <= previous)
|
||||
throw new ArgumentException("Time knots must be finite and strictly increasing.", nameof(source));
|
||||
if (index == 0 && Math.Abs(source[index]) > 1e-12d)
|
||||
throw new ArgumentException("The first time knot must be exact zero.", nameof(source));
|
||||
copy.Add(source[index]);
|
||||
previous = source[index];
|
||||
}
|
||||
return new ReadOnlyCollection<double>(copy);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<double> CopyValues(IReadOnlyList<double> source, int expectedCount, string parameterName)
|
||||
{
|
||||
if (source == null || source.Count != expectedCount)
|
||||
throw new ArgumentException("Longitudinal values do not match the time-knot layout.", parameterName);
|
||||
var copy = new List<double>(source.Count);
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
{
|
||||
if (!IsFinite(source[index]))
|
||||
throw new ArgumentOutOfRangeException(parameterName);
|
||||
copy.Add(source[index]);
|
||||
}
|
||||
return new ReadOnlyCollection<double>(copy);
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
+188
@@ -0,0 +1,188 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Builds one normalized ST QP with exact constant-jerk integration and hard terminal conditions.</summary>
|
||||
public sealed class LongitudinalConstraintBuilder
|
||||
{
|
||||
private readonly LongitudinalObjectiveBuilder _objectiveBuilder;
|
||||
|
||||
public LongitudinalConstraintBuilder(LongitudinalObjectiveBuilder objectiveBuilder)
|
||||
{
|
||||
_objectiveBuilder = objectiveBuilder ?? throw new ArgumentNullException(nameof(objectiveBuilder));
|
||||
}
|
||||
|
||||
public bool TryBuild(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate iterate,
|
||||
out QuadraticProgram problem, out string failureReason)
|
||||
{
|
||||
problem = null;
|
||||
failureReason = string.Empty;
|
||||
try
|
||||
{
|
||||
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.");
|
||||
|
||||
IReadOnlyList<double> expectedTimes = LongitudinalCandidate.CreateKnotTimes(
|
||||
input.Configuration.Scheduling.TimeHorizonSeconds, input.Configuration.Scheduling.OutputTimeStepSeconds);
|
||||
if (!HasMatchingTimes(iterate.KnotTimes, expectedTimes))
|
||||
throw new ArgumentException("The ST iterate time knots do not match the configured horizon.");
|
||||
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)
|
||||
{
|
||||
throw new ArgumentException("The ST iterate does not match the configured knot layout.");
|
||||
}
|
||||
if (!PathSpeedLimitBuilder.TryGetLimits(input, out double directionMaximum, out double maximumAcceleration,
|
||||
out double maximumDeceleration, out double maximumJerk, out _, out _, out failureReason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if (input.InitialProgressSpeedMetersPerSecond > speedLimit.MaximumSpeedAt(0d) + 1e-12d ||
|
||||
input.InitialAccelerationMetersPerSecondSquared < -maximumDeceleration - 1e-12d ||
|
||||
input.InitialAccelerationMetersPerSecondSquared > maximumAcceleration + 1e-12d)
|
||||
{
|
||||
failureReason = "The initial ST state violates hard bounds.";
|
||||
return false;
|
||||
}
|
||||
|
||||
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 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)
|
||||
throw new InvalidOperationException("ST constraint row accounting is inconsistent.");
|
||||
problem = new QuadraticProgram(hessian.Build(), linearCost, constraints.Build(), lower, upper);
|
||||
return true;
|
||||
}
|
||||
catch (ArgumentException exception)
|
||||
{
|
||||
failureReason = exception.Message;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
private static void AddVariableBounds(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
|
||||
LongitudinalCandidate iterate, LongitudinalVariableLayout layout, double maximumAcceleration,
|
||||
double maximumDeceleration, double maximumJerk, SparseTripletBuilder constraints, IList<double> lower,
|
||||
IList<double> upper, ref int row)
|
||||
{
|
||||
for (int index = 0; index < layout.KnotCount; index++)
|
||||
{
|
||||
if (iterate.S[index] < 0d || iterate.S[index] > input.TerminalPathS)
|
||||
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.U(index), 0d,
|
||||
Math.Min(input.DirectionMaximumSpeedMetersPerSecond, speedLimit.MaximumSpeedAt(iterate.S[index])), ref row);
|
||||
AddSingleVariableRow(constraints, lower, upper, layout.A(index), -maximumDeceleration, maximumAcceleration,
|
||||
ref row);
|
||||
}
|
||||
for (int index = 0; index < layout.KnotCount - 1; index++)
|
||||
AddSingleVariableRow(constraints, lower, upper, layout.J(index), -maximumJerk, maximumJerk, ref row);
|
||||
}
|
||||
|
||||
private static void AddMonotonicProgress(LongitudinalVariableLayout layout, SparseTripletBuilder constraints,
|
||||
IList<double> lower, IList<double> upper, ref int row)
|
||||
{
|
||||
for (int index = 0; index < layout.KnotCount - 1; index++)
|
||||
{
|
||||
AddRow(constraints, lower, upper, row, new[]
|
||||
{
|
||||
new Coefficient(layout.S(index + 1), 1d), new Coefficient(layout.S(index), -1d),
|
||||
}, 0d, QuadraticProgram.MaximumFiniteBound);
|
||||
row++;
|
||||
}
|
||||
}
|
||||
|
||||
private static void AddExactDynamics(IReadOnlyList<double> times, LongitudinalVariableLayout layout,
|
||||
SparseTripletBuilder constraints, IList<double> lower, IList<double> upper, ref int row)
|
||||
{
|
||||
for (int index = 0; index < layout.KnotCount - 1; index++)
|
||||
{
|
||||
double dt = times[index + 1] - times[index];
|
||||
AddRow(constraints, lower, upper, row, new[]
|
||||
{
|
||||
new Coefficient(layout.A(index + 1), 1d), new Coefficient(layout.A(index), -1d),
|
||||
new Coefficient(layout.J(index), -dt),
|
||||
}, 0d, 0d);
|
||||
row++;
|
||||
AddRow(constraints, lower, upper, row, new[]
|
||||
{
|
||||
new Coefficient(layout.U(index + 1), 1d), new Coefficient(layout.U(index), -1d),
|
||||
new Coefficient(layout.A(index), -dt), new Coefficient(layout.J(index), -0.5d * dt * dt),
|
||||
}, 0d, 0d);
|
||||
row++;
|
||||
AddRow(constraints, lower, upper, row, new[]
|
||||
{
|
||||
new Coefficient(layout.S(index + 1), 1d), new Coefficient(layout.S(index), -1d),
|
||||
new Coefficient(layout.U(index), -dt), new Coefficient(layout.A(index), -0.5d * dt * dt),
|
||||
new Coefficient(layout.J(index), -dt * dt * dt / 6d),
|
||||
}, 0d, 0d);
|
||||
row++;
|
||||
}
|
||||
}
|
||||
|
||||
private static void AddExactStartAndTerminal(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);
|
||||
AddSingleVariableRow(constraints, lower, upper, layout.U(0), input.InitialProgressSpeedMetersPerSecond,
|
||||
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 AddSingleVariableRow(SparseTripletBuilder constraints, IList<double> lower, IList<double> upper,
|
||||
int variable, double minimum, double maximum, ref int row)
|
||||
{
|
||||
AddRow(constraints, lower, upper, row, new[] { new Coefficient(variable, 1d) }, minimum, maximum);
|
||||
row++;
|
||||
}
|
||||
|
||||
private static void AddRow(SparseTripletBuilder constraints, IList<double> lower, IList<double> upper, int row,
|
||||
IReadOnlyList<Coefficient> coefficients, double minimum, double maximum)
|
||||
{
|
||||
for (int index = 0; index < coefficients.Count; index++)
|
||||
constraints.Add(row, coefficients[index].Variable, coefficients[index].Value);
|
||||
lower.Add(minimum);
|
||||
upper.Add(maximum);
|
||||
}
|
||||
|
||||
private static bool HasMatchingTimes(IReadOnlyList<double> actual, IReadOnlyList<double> expected)
|
||||
{
|
||||
if (actual.Count != expected.Count)
|
||||
return false;
|
||||
for (int index = 0; index < expected.Count; index++)
|
||||
{
|
||||
if (Math.Abs(actual[index] - expected[index]) > 1e-12d)
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private readonly struct Coefficient
|
||||
{
|
||||
public Coefficient(int variable, double value)
|
||||
{
|
||||
Variable = variable;
|
||||
Value = value;
|
||||
}
|
||||
|
||||
public int Variable { get; }
|
||||
|
||||
public double Value { get; }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,68 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Normalized 0.5*x'Px+q'x ST objective terms with no terminal-progress reward.</summary>
|
||||
public sealed class LongitudinalObjectiveBuilder
|
||||
{
|
||||
public void AddTerms(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalVariableLayout layout,
|
||||
LongitudinalCandidate iterate, SparseTripletBuilder hessian, IList<double> linearCost)
|
||||
{
|
||||
if (input == null || speedLimit == null || layout == null || iterate == null || hessian == null || linearCost == null ||
|
||||
linearCost.Count != layout.VariableCount || iterate.KnotTimes.Count != layout.KnotCount)
|
||||
{
|
||||
throw new ArgumentException("ST objective inputs do not match the variable layout.");
|
||||
}
|
||||
|
||||
LongitudinalConfiguration configuration = input.Configuration.Longitudinal;
|
||||
LongitudinalWeights weights = configuration.Weights ?? throw new ArgumentException("Longitudinal weights are required.");
|
||||
double speedScale = RequirePositive(input.DirectionMaximumSpeedMetersPerSecond, nameof(speedScale));
|
||||
double accelerationScale = RequirePositive(Math.Max(configuration.MaximumAccelerationMetersPerSecondSquared,
|
||||
configuration.MaximumDecelerationMetersPerSecondSquared), nameof(accelerationScale));
|
||||
double jerkScale = RequirePositive(configuration.MaximumJerkMetersPerSecondCubed, nameof(jerkScale));
|
||||
double progressScale = input.TerminalPathS > 0d ? input.TerminalPathS : 1d;
|
||||
|
||||
for (int index = 0; index < layout.KnotCount; index++)
|
||||
{
|
||||
AddSquaredResidual(hessian, linearCost, layout.U(index), speedLimit.MaximumSpeedAt(iterate.S[index]),
|
||||
weights.ReferenceSpeed, speedScale);
|
||||
AddSquaredResidual(hessian, linearCost, layout.A(index), 0d, weights.Acceleration, accelerationScale);
|
||||
if (index < layout.KnotCount - 1 && index < input.PreviousPathS.Count)
|
||||
{
|
||||
AddSquaredResidual(hessian, linearCost, layout.S(index), input.PreviousPathS[index],
|
||||
weights.PreviousTrajectory, progressScale);
|
||||
AddSquaredResidual(hessian, linearCost, layout.U(index), input.PreviousProgressSpeedMetersPerSecond[index],
|
||||
weights.PreviousTrajectory, speedScale);
|
||||
}
|
||||
}
|
||||
for (int index = 0; index < layout.KnotCount - 1; index++)
|
||||
AddSquaredResidual(hessian, linearCost, layout.J(index), 0d, weights.Jerk, jerkScale);
|
||||
AddSquaredResidual(hessian, linearCost, layout.A(layout.KnotCount - 1), 0d, weights.TerminalAcceleration,
|
||||
accelerationScale);
|
||||
}
|
||||
|
||||
private static void AddSquaredResidual(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));
|
||||
if (weight == 0d)
|
||||
return;
|
||||
double coefficient = 2d * weight / (scale * scale);
|
||||
hessian.Add(variable, variable, coefficient);
|
||||
linearCost[variable] += -coefficient * reference;
|
||||
}
|
||||
|
||||
private static double RequirePositive(double value, string name)
|
||||
{
|
||||
if (!IsFinite(value) || value <= 0d)
|
||||
throw new ArgumentOutOfRangeException(name);
|
||||
return value;
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,29 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Immutable longitudinal solve result; only successful statuses may expose a candidate profile.</summary>
|
||||
public sealed class LongitudinalPlanningResult
|
||||
{
|
||||
public LongitudinalPlanningResult(EmPlanningStatus status, LongitudinalCandidate candidate, string failureReason)
|
||||
{
|
||||
if (!Enum.IsDefined(typeof(EmPlanningStatus), status))
|
||||
throw new ArgumentOutOfRangeException(nameof(status));
|
||||
bool successful = status == EmPlanningStatus.Success || status == EmPlanningStatus.SuccessWithFallback;
|
||||
if (successful && candidate == null)
|
||||
throw new ArgumentException("Successful longitudinal results require a candidate.", nameof(candidate));
|
||||
if (!successful && candidate != null)
|
||||
throw new ArgumentException("Failed longitudinal results cannot expose a candidate.", nameof(candidate));
|
||||
|
||||
Status = status;
|
||||
Candidate = candidate == null ? null : new LongitudinalCandidate(candidate.KnotTimes, candidate.S, candidate.U,
|
||||
candidate.A, candidate.J);
|
||||
FailureReason = failureReason ?? string.Empty;
|
||||
}
|
||||
|
||||
public EmPlanningStatus Status { get; }
|
||||
|
||||
public LongitudinalCandidate Candidate { get; }
|
||||
|
||||
public string FailureReason { get; }
|
||||
}
|
||||
@@ -0,0 +1,52 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Deterministic contiguous variable ranges for one time-domain longitudinal QP.</summary>
|
||||
public sealed class LongitudinalVariableLayout
|
||||
{
|
||||
public LongitudinalVariableLayout(int knotCount)
|
||||
{
|
||||
if (knotCount < 2)
|
||||
throw new ArgumentOutOfRangeException(nameof(knotCount), "At least two time knots are required.");
|
||||
|
||||
KnotCount = knotCount;
|
||||
SStart = 0;
|
||||
UStart = knotCount;
|
||||
AStart = 2 * knotCount;
|
||||
JStart = 3 * knotCount;
|
||||
VariableCount = 4 * knotCount - 1;
|
||||
}
|
||||
|
||||
public int KnotCount { get; }
|
||||
|
||||
public int SStart { get; }
|
||||
|
||||
public int UStart { get; }
|
||||
|
||||
public int AStart { get; }
|
||||
|
||||
public int JStart { get; }
|
||||
|
||||
public int VariableCount { get; }
|
||||
|
||||
public int S(int knotIndex) { return RequireKnotIndex(knotIndex, SStart); }
|
||||
|
||||
public int U(int knotIndex) { return RequireKnotIndex(knotIndex, UStart); }
|
||||
|
||||
public int A(int knotIndex) { return RequireKnotIndex(knotIndex, AStart); }
|
||||
|
||||
public int J(int intervalIndex)
|
||||
{
|
||||
if (intervalIndex < 0 || intervalIndex >= KnotCount - 1)
|
||||
throw new ArgumentOutOfRangeException(nameof(intervalIndex));
|
||||
return JStart + intervalIndex;
|
||||
}
|
||||
|
||||
private int RequireKnotIndex(int knotIndex, int start)
|
||||
{
|
||||
if (knotIndex < 0 || knotIndex >= KnotCount)
|
||||
throw new ArgumentOutOfRangeException(nameof(knotIndex));
|
||||
return start + knotIndex;
|
||||
}
|
||||
}
|
||||
@@ -13,6 +13,7 @@ internal static class LongitudinalModelChecks
|
||||
VerifiesFinitePathSIndexedSpeedEnvelope();
|
||||
VerifiesStoppingPrecheckBeforeQpAssembly();
|
||||
VerifiesReferenceHorizonSelectionKeepsTheCurrentSegmentBoundary();
|
||||
VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints();
|
||||
}
|
||||
|
||||
private static void VerifiesFinitePathSIndexedSpeedEnvelope()
|
||||
@@ -101,6 +102,105 @@ internal static class LongitudinalModelChecks
|
||||
"rolling horizon remains within the current segment");
|
||||
}
|
||||
|
||||
private static void VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints()
|
||||
{
|
||||
var layout = new LongitudinalVariableLayout(5);
|
||||
Verification.Equal(19, layout.VariableCount, "ST variable count");
|
||||
for (int index = 0; index < 5; index++)
|
||||
{
|
||||
Verification.Equal(index, layout.S(index), "s index " + index);
|
||||
Verification.Equal(5 + index, layout.U(index), "u index " + index);
|
||||
Verification.Equal(10 + index, layout.A(index), "a index " + index);
|
||||
}
|
||||
for (int index = 0; index < 4; index++)
|
||||
Verification.Equal(15 + index, layout.J(index), "j index " + index);
|
||||
|
||||
double[] times = { 0d, 0.05d, 0.10d, 0.15d, 0.20d };
|
||||
double[] jerk = { 0.30d, -0.10d, 0.20d, -0.20d };
|
||||
LongitudinalCandidate integrated = LongitudinalCandidate.Integrate(times, 0d, 0.10d, 0.02d, jerk);
|
||||
for (int index = 0; index < jerk.Length; index++)
|
||||
{
|
||||
double dt = times[index + 1] - times[index];
|
||||
Verification.NearlyEqual(integrated.A[index] + dt * integrated.J[index], integrated.A[index + 1],
|
||||
"exact ST acceleration dynamics " + index);
|
||||
Verification.NearlyEqual(integrated.U[index] + dt * integrated.A[index] + 0.5d * dt * dt * integrated.J[index],
|
||||
integrated.U[index + 1], "exact ST speed dynamics " + index);
|
||||
Verification.NearlyEqual(integrated.S[index] + dt * integrated.U[index] +
|
||||
0.5d * dt * dt * integrated.A[index] + dt * dt * dt * integrated.J[index] / 6d,
|
||||
integrated.S[index + 1], "exact ST progress dynamics " + index);
|
||||
}
|
||||
Verification.True(integrated.SatisfiesExactDiscreteDynamics(1e-12d), "integrated ST candidate validates dynamics");
|
||||
|
||||
EmPlannerConfiguration configuration = CreateUnitScaleConfiguration();
|
||||
LateralPath path = CreatePath(new[]
|
||||
{
|
||||
new PathFixture(0d, 0d, 0d, 0d),
|
||||
new PathFixture(1d, 1d, 0d, 0d),
|
||||
new PathFixture(2d, 2d, 0d, 0d),
|
||||
});
|
||||
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.10d, 0.02d,
|
||||
EmTerminalType.Goal, configuration, new[] { 0d, 0.10d, 0.20d, 0.30d, 0.40d },
|
||||
new[] { 0.20d, 0.20d, 0.20d, 0.20d, 0.20d });
|
||||
EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
|
||||
out string speedFailure);
|
||||
Verification.Equal(EmPlanningStatus.Success, speedStatus, "unit-scale speed envelope: " + speedFailure);
|
||||
|
||||
Verification.True(new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder()).TryBuild(input, envelope,
|
||||
integrated, out QuadraticProgram problem, out string failureReason), "ST QP builds: " + failureReason);
|
||||
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)),
|
||||
"normalized acceleration P coefficient");
|
||||
Verification.NearlyEqual(20d, MatrixValue(problem.UpperTriangularP, layout.J(0), layout.J(0)),
|
||||
"normalized jerk P coefficient");
|
||||
Verification.NearlyEqual(2.5d, MatrixValue(problem.UpperTriangularP, layout.S(0), layout.S(0)),
|
||||
"normalized previous-S P coefficient");
|
||||
Verification.NearlyEqual(0d, MatrixValue(problem.UpperTriangularP, layout.S(4), layout.S(4)),
|
||||
"fixed terminal S has no progress-reward coefficient");
|
||||
|
||||
FindSingleVariableBounds(problem, layout.S(0), out double sLower, out double sUpper);
|
||||
Verification.NearlyEqual(0d, sLower, "S lower bound");
|
||||
Verification.NearlyEqual(2d, sUpper, "S upper bound");
|
||||
FindSingleVariableBounds(problem, layout.U(1), out double uLower, out double uUpper);
|
||||
Verification.NearlyEqual(0d, uLower, "U nonnegative bound");
|
||||
Verification.NearlyEqual(envelope.MaximumSpeedAt(integrated.S[1]), uUpper,
|
||||
"U upper bound samples envelope at current S iterate");
|
||||
FindSingleVariableBounds(problem, layout.A(1), out double aLower, out double aUpper);
|
||||
Verification.NearlyEqual(-1d, aLower, "deceleration lower bound");
|
||||
Verification.NearlyEqual(1d, aUpper, "acceleration upper bound");
|
||||
FindSingleVariableBounds(problem, layout.J(1), out double jLower, out double jUpper);
|
||||
Verification.NearlyEqual(-1d, jLower, "jerk lower bound");
|
||||
Verification.NearlyEqual(1d, jUpper, "jerk upper bound");
|
||||
|
||||
Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double> { { layout.S(0), 1d } }, 0d),
|
||||
"exact initial S");
|
||||
Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double> { { layout.U(0), 1d } }, 0.10d),
|
||||
"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(1, CountBoundedRow(problem, new Dictionary<int, double>
|
||||
{
|
||||
{ layout.S(1), 1d }, { layout.S(0), -1d },
|
||||
}, 0d, QuadraticProgram.MaximumFiniteBound), "monotonic S hard constraint");
|
||||
Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double>
|
||||
{
|
||||
{ layout.A(1), 1d }, { layout.A(0), -1d }, { layout.J(0), -0.05d },
|
||||
}, 0d), "exact ST acceleration equation");
|
||||
Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double>
|
||||
{
|
||||
{ layout.U(1), 1d }, { layout.U(0), -1d }, { layout.A(0), -0.05d }, { layout.J(0), -0.00125d },
|
||||
}, 0d), "exact ST speed equation");
|
||||
Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double>
|
||||
{
|
||||
{ layout.S(1), 1d }, { layout.S(0), -1d }, { layout.U(0), -0.05d }, { layout.A(0), -0.00125d },
|
||||
{ layout.J(0), -0.000020833333333333333d },
|
||||
}, 0d), "exact ST progress equation");
|
||||
}
|
||||
|
||||
private static LateralPath CreatePath(IReadOnlyList<PathFixture> fixtures)
|
||||
{
|
||||
var points = new List<LateralPathPoint>(fixtures.Count);
|
||||
@@ -131,6 +231,87 @@ internal static class LongitudinalModelChecks
|
||||
SmoothedPathPointSource.Anchor);
|
||||
}
|
||||
|
||||
private static EmPlannerConfiguration CreateUnitScaleConfiguration()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Scheduling.TimeHorizonSeconds = 0.20d;
|
||||
configuration.Scheduling.OutputTimeStepSeconds = 0.05d;
|
||||
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
|
||||
configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
|
||||
configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1d;
|
||||
configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
|
||||
configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 1d;
|
||||
configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared = 1d;
|
||||
configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1d;
|
||||
return configuration;
|
||||
}
|
||||
|
||||
private static double MatrixValue(SparseCscMatrix matrix, int row, int column)
|
||||
{
|
||||
for (int index = matrix.ColumnPointers[column]; index < matrix.ColumnPointers[column + 1]; index++)
|
||||
{
|
||||
if (matrix.RowIndices[index] == row)
|
||||
return matrix.Values[index];
|
||||
}
|
||||
return 0d;
|
||||
}
|
||||
|
||||
private static void FindSingleVariableBounds(QuadraticProgram problem, int variable, out double lower, out double upper)
|
||||
{
|
||||
for (int row = 0; row < problem.ConstraintCount; row++)
|
||||
{
|
||||
if (RowMatches(problem.ConstraintMatrix, row, new Dictionary<int, double> { { variable, 1d } }))
|
||||
{
|
||||
lower = problem.LowerBounds[row];
|
||||
upper = problem.UpperBounds[row];
|
||||
return;
|
||||
}
|
||||
}
|
||||
throw new InvalidOperationException("No single-variable bounds were found for variable " + variable + ".");
|
||||
}
|
||||
|
||||
private static int CountExactEqualityRows(QuadraticProgram problem, IReadOnlyDictionary<int, double> expected,
|
||||
double bound)
|
||||
{
|
||||
return CountBoundedRow(problem, expected, bound, bound);
|
||||
}
|
||||
|
||||
private static int CountBoundedRow(QuadraticProgram problem, IReadOnlyDictionary<int, double> expected,
|
||||
double lower, double upper)
|
||||
{
|
||||
int count = 0;
|
||||
for (int row = 0; row < problem.ConstraintCount; row++)
|
||||
{
|
||||
if (Math.Abs(problem.LowerBounds[row] - lower) <= 1e-12d &&
|
||||
Math.Abs(problem.UpperBounds[row] - upper) <= 1e-12d && RowMatches(problem.ConstraintMatrix, row, expected))
|
||||
{
|
||||
count++;
|
||||
}
|
||||
}
|
||||
return count;
|
||||
}
|
||||
|
||||
private static bool RowMatches(SparseCscMatrix matrix, int targetRow, IReadOnlyDictionary<int, double> expected)
|
||||
{
|
||||
var actual = new Dictionary<int, double>();
|
||||
for (int column = 0; column < matrix.ColumnCount; column++)
|
||||
{
|
||||
for (int index = matrix.ColumnPointers[column]; index < matrix.ColumnPointers[column + 1]; index++)
|
||||
{
|
||||
if (matrix.RowIndices[index] == targetRow)
|
||||
actual[column] = matrix.Values[index];
|
||||
}
|
||||
}
|
||||
if (actual.Count != expected.Count)
|
||||
return false;
|
||||
foreach (KeyValuePair<int, double> pair in expected)
|
||||
{
|
||||
if (!actual.TryGetValue(pair.Key, out double actualValue) || Math.Abs(actualValue - pair.Value) > 1e-12d)
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private sealed class PathFixture
|
||||
{
|
||||
public PathFixture(double referenceS, double pathS, double curvature, double curvatureDerivative)
|
||||
|
||||
Reference in New Issue
Block a user