feat: optimize longitudinal ST profiles
This commit is contained in:
梁薄云
@@ -0,0 +1,20 @@
|
||||
using System;
|
||||
using System.Threading;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Pure longitudinal planning facade that depends only on the solver-neutral IQpSolver boundary.</summary>
|
||||
public sealed class LongitudinalPlanner
|
||||
{
|
||||
private readonly SequentialLongitudinalOptimizer _optimizer;
|
||||
|
||||
public LongitudinalPlanner(IQpSolver qpSolver)
|
||||
{
|
||||
_optimizer = new SequentialLongitudinalOptimizer(qpSolver ?? throw new ArgumentNullException(nameof(qpSolver)));
|
||||
}
|
||||
|
||||
public LongitudinalPlanningResult Plan(LongitudinalPlanningInput input, CancellationToken cancellationToken)
|
||||
{
|
||||
return _optimizer.Optimize(input, cancellationToken);
|
||||
}
|
||||
}
|
||||
+153
@@ -0,0 +1,153 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Independently validates ST candidates directly in physical units before they may become fallbacks.</summary>
|
||||
public sealed class LongitudinalSolutionValidator
|
||||
{
|
||||
public bool TryValidate(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate candidate,
|
||||
out LongitudinalCandidate validatedCandidate, out string failureReason)
|
||||
{
|
||||
validatedCandidate = null;
|
||||
failureReason = string.Empty;
|
||||
if (input == null || speedLimit == null || candidate == null)
|
||||
{
|
||||
failureReason = "ST input, speed envelope, and candidate are required.";
|
||||
return false;
|
||||
}
|
||||
try
|
||||
{
|
||||
if (!PathSpeedLimitBuilder.TryGetLimits(input, out _, out double maximumAcceleration,
|
||||
out double maximumDeceleration, out double maximumJerk, out _, out _, out failureReason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
IReadOnlyList<double> expectedTimes = LongitudinalCandidate.CreateKnotTimes(
|
||||
input.Configuration.Scheduling.TimeHorizonSeconds, input.Configuration.Scheduling.OutputTimeStepSeconds);
|
||||
double tolerance = RequireNonnegative(input.Configuration.Validation.KinematicTolerance, nameof(tolerance));
|
||||
if (!HasMatchingTimes(candidate.KnotTimes, expectedTimes, tolerance))
|
||||
{
|
||||
failureReason = "ST candidate knot times do not match the configured horizon.";
|
||||
return false;
|
||||
}
|
||||
if (candidate.S.Count != expectedTimes.Count || candidate.U.Count != expectedTimes.Count ||
|
||||
candidate.A.Count != expectedTimes.Count || candidate.J.Count != expectedTimes.Count - 1)
|
||||
{
|
||||
failureReason = "ST candidate value counts do not match its time knots.";
|
||||
return false;
|
||||
}
|
||||
if (!candidate.SatisfiesExactDiscreteDynamics(tolerance))
|
||||
{
|
||||
failureReason = "ST candidate violates exact constant-jerk dynamics.";
|
||||
return false;
|
||||
}
|
||||
if (!AreClose(candidate.S[0], 0d, tolerance) ||
|
||||
!AreClose(candidate.U[0], input.InitialProgressSpeedMetersPerSecond, tolerance) ||
|
||||
!AreClose(candidate.A[0], input.InitialAccelerationMetersPerSecondSquared, tolerance))
|
||||
{
|
||||
failureReason = "ST candidate does not satisfy the exact initial state.";
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int index = 0; index < candidate.S.Count; index++)
|
||||
{
|
||||
double progress = candidate.S[index];
|
||||
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 ||
|
||||
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)));
|
||||
if (speed > speedLimitAtProgress + tolerance)
|
||||
{
|
||||
failureReason = "ST candidate violates the actual-PathS speed envelope at knot " + index +
|
||||
" (S=" + progress + ", U=" + speed + ", limit=" + speedLimitAtProgress + ").";
|
||||
return false;
|
||||
}
|
||||
if (index > 0 && progress < candidate.S[index - 1] - tolerance)
|
||||
{
|
||||
failureReason = "ST candidate PathS decreases at knot " + index + ".";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
for (int index = 0; index < candidate.J.Count; index++)
|
||||
{
|
||||
if (!IsFinite(candidate.J[index]) || Math.Abs(candidate.J[index]) > maximumJerk + tolerance)
|
||||
{
|
||||
failureReason = "ST candidate violates the jerk bound at interval " + index + ".";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
int terminalIndex = candidate.S.Count - 1;
|
||||
if (!AreClose(candidate.S[terminalIndex], input.TerminalPathS, tolerance) ||
|
||||
!AreClose(candidate.U[terminalIndex], 0d, tolerance))
|
||||
{
|
||||
failureReason = "ST candidate does not satisfy the exact zero-speed terminal.";
|
||||
return false;
|
||||
}
|
||||
|
||||
var canonicalS = new double[candidate.S.Count];
|
||||
var canonicalU = new double[candidate.U.Count];
|
||||
var canonicalA = new double[candidate.A.Count];
|
||||
for (int index = 0; index < candidate.S.Count; index++)
|
||||
{
|
||||
canonicalS[index] = candidate.S[index];
|
||||
canonicalU[index] = candidate.U[index];
|
||||
canonicalA[index] = candidate.A[index];
|
||||
}
|
||||
canonicalS[0] = 0d;
|
||||
canonicalU[0] = input.InitialProgressSpeedMetersPerSecond;
|
||||
canonicalA[0] = input.InitialAccelerationMetersPerSecondSquared;
|
||||
canonicalS[terminalIndex] = input.TerminalPathS;
|
||||
canonicalU[terminalIndex] = 0d;
|
||||
var canonicalCandidate = new LongitudinalCandidate(candidate.KnotTimes, canonicalS, canonicalU, canonicalA,
|
||||
candidate.J);
|
||||
if (!canonicalCandidate.SatisfiesExactDiscreteDynamics(tolerance))
|
||||
{
|
||||
failureReason = "Canonical ST hard-boundary values exceed the dynamics tolerance.";
|
||||
return false;
|
||||
}
|
||||
validatedCandidate = canonicalCandidate;
|
||||
return true;
|
||||
}
|
||||
catch (ArgumentException exception)
|
||||
{
|
||||
failureReason = exception.Message;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
private static bool HasMatchingTimes(IReadOnlyList<double> actual, IReadOnlyList<double> expected, double tolerance)
|
||||
{
|
||||
if (actual.Count != expected.Count)
|
||||
return false;
|
||||
for (int index = 0; index < expected.Count; index++)
|
||||
{
|
||||
if (!AreClose(actual[index], expected[index], tolerance))
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool AreClose(double actual, double expected, double tolerance)
|
||||
{
|
||||
return Math.Abs(actual - expected) <= tolerance;
|
||||
}
|
||||
|
||||
private static double RequireNonnegative(double value, string parameterName)
|
||||
{
|
||||
if (!IsFinite(value) || value < 0d)
|
||||
throw new ArgumentOutOfRangeException(parameterName);
|
||||
return value;
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
+461
@@ -0,0 +1,461 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.Threading;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Bounded ST envelope iteration retaining only independently validated physical candidates.</summary>
|
||||
public sealed class SequentialLongitudinalOptimizer
|
||||
{
|
||||
private const int MaximumEnvelopeIterations = 5;
|
||||
private const double OrdinaryEnvelopeProbeLookaheadSteps = 1d;
|
||||
private const double OrdinaryTerminalProbeFraction = 0.5d;
|
||||
private readonly IQpSolver _qpSolver;
|
||||
private readonly PathSpeedLimitBuilder _speedLimitBuilder;
|
||||
private readonly LongitudinalConstraintBuilder _constraintBuilder;
|
||||
private readonly LongitudinalSolutionValidator _solutionValidator;
|
||||
|
||||
public SequentialLongitudinalOptimizer(IQpSolver qpSolver)
|
||||
: this(qpSolver, new PathSpeedLimitBuilder(), new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder()),
|
||||
new LongitudinalSolutionValidator())
|
||||
{
|
||||
}
|
||||
|
||||
internal SequentialLongitudinalOptimizer(IQpSolver qpSolver, PathSpeedLimitBuilder speedLimitBuilder,
|
||||
LongitudinalConstraintBuilder constraintBuilder, LongitudinalSolutionValidator solutionValidator)
|
||||
{
|
||||
_qpSolver = qpSolver ?? throw new ArgumentNullException(nameof(qpSolver));
|
||||
_speedLimitBuilder = speedLimitBuilder ?? throw new ArgumentNullException(nameof(speedLimitBuilder));
|
||||
_constraintBuilder = constraintBuilder ?? throw new ArgumentNullException(nameof(constraintBuilder));
|
||||
_solutionValidator = solutionValidator ?? throw new ArgumentNullException(nameof(solutionValidator));
|
||||
}
|
||||
|
||||
public LongitudinalPlanningResult Optimize(LongitudinalPlanningInput input, CancellationToken cancellationToken)
|
||||
{
|
||||
if (input == null)
|
||||
return Failed(EmPlanningStatus.InvalidInput, "Longitudinal planning input is required.");
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
return Failed(EmPlanningStatus.Cancelled, "Longitudinal optimization was cancelled.");
|
||||
if (!TryCreateSettings(input, out QpSolverSettings settings, out TimeSpan totalBudget, out double convergenceTolerance,
|
||||
out int iterationLimit, out string configurationFailure))
|
||||
{
|
||||
return Failed(EmPlanningStatus.InvalidInput, configurationFailure);
|
||||
}
|
||||
|
||||
EmPlanningStatus speedStatus = _speedLimitBuilder.Build(input, out PathSpeedLimit speedLimit, out string speedFailure);
|
||||
if (speedStatus != EmPlanningStatus.Success)
|
||||
return Failed(speedStatus, speedFailure);
|
||||
|
||||
LongitudinalCandidate iterate = CreateInitialIterate(input);
|
||||
double[] warmStart = ToPrimal(iterate);
|
||||
bool hasDynamicsConsistentInitialWarmStart = iterate.SatisfiesExactDiscreteDynamics(1e-12d);
|
||||
LongitudinalCandidate lastStrictCandidate;
|
||||
if (!_solutionValidator.TryValidate(input, speedLimit, iterate, out lastStrictCandidate, out _))
|
||||
lastStrictCandidate = null;
|
||||
string lastCandidateRejection = string.Empty;
|
||||
bool hasPreviousObjective = false;
|
||||
double previousObjective = 0d;
|
||||
var stopwatch = Stopwatch.StartNew();
|
||||
|
||||
for (int iteration = 0; iteration < iterationLimit; iteration++)
|
||||
{
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Cancelled, "Longitudinal optimization was cancelled.");
|
||||
TimeSpan remainingBudget = totalBudget - stopwatch.Elapsed;
|
||||
if (remainingBudget <= TimeSpan.Zero)
|
||||
{
|
||||
return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.SolverTimedOut,
|
||||
"Longitudinal optimization exhausted its solve budget.");
|
||||
}
|
||||
if (!_constraintBuilder.TryBuild(input, speedLimit, iterate, out QuadraticProgram problem, out string buildFailure))
|
||||
{
|
||||
return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.LongitudinalInfeasible,
|
||||
"Longitudinal constraints are infeasible: " + buildFailure);
|
||||
}
|
||||
|
||||
QpSolveResult solved = _qpSolver.Solve(problem,
|
||||
new QpSolverSettings(settings.MaximumIterations, settings.AbsoluteTolerance, settings.RelativeTolerance,
|
||||
remainingBudget, settings.EnableWarmStart && (iteration > 0 || hasDynamicsConsistentInitialWarmStart),
|
||||
settings.EnablePolishing,
|
||||
settings.EnableNativeVerboseOutput),
|
||||
warmStart, cancellationToken);
|
||||
if (solved == null)
|
||||
return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Failed, "The longitudinal QP solver returned no result.");
|
||||
if (solved.Status == QpSolveStatus.TimeLimit || solved.Status == QpSolveStatus.MaximumIterations)
|
||||
{
|
||||
return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.SolverTimedOut,
|
||||
"The longitudinal QP solver timed out (status=" + solved.NativeStatus +
|
||||
", iterations=" + solved.Iterations + ", primal=" + solved.PrimalResidual +
|
||||
", dual=" + solved.DualResidual + "): " + solved.Diagnostic);
|
||||
}
|
||||
if (solved.Status == QpSolveStatus.Cancelled)
|
||||
return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Cancelled,
|
||||
"The longitudinal QP solver was cancelled: " + solved.Diagnostic);
|
||||
if (solved.Status == QpSolveStatus.PrimalInfeasible || solved.Status == QpSolveStatus.DualInfeasible)
|
||||
{
|
||||
return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.LongitudinalInfeasible,
|
||||
"The longitudinal QP solver reported infeasibility: " + solved.Diagnostic);
|
||||
}
|
||||
if (solved.Status == QpSolveStatus.SolverUnavailable)
|
||||
return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.SolverUnavailable,
|
||||
"The longitudinal QP solver is unavailable: " + solved.Diagnostic);
|
||||
if (solved.Status != QpSolveStatus.Solved && solved.Status != QpSolveStatus.SolvedInaccurate)
|
||||
{
|
||||
return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Failed,
|
||||
"The longitudinal QP solver failed: " + solved.Diagnostic);
|
||||
}
|
||||
if (solved.Status == QpSolveStatus.SolvedInaccurate && !HasStrictResiduals(solved, convergenceTolerance))
|
||||
{
|
||||
lastCandidateRejection = "SolvedInaccurate residuals exceed the strict acceptance tolerance" +
|
||||
" (primal=" + solved.PrimalResidual + ", dual=" + solved.DualResidual + ").";
|
||||
if (TryCreateCandidate(iterate.KnotTimes, solved.Primal, out LongitudinalCandidate inaccurateCandidate))
|
||||
warmStart = ToPrimal(inaccurateCandidate);
|
||||
continue;
|
||||
}
|
||||
if (!TryCreateCandidate(iterate.KnotTimes, solved.Primal, out LongitudinalCandidate candidate))
|
||||
{
|
||||
lastCandidateRejection = "The solver primal does not match the ST variable layout.";
|
||||
continue;
|
||||
}
|
||||
if (!_solutionValidator.TryValidate(input, speedLimit, candidate, out LongitudinalCandidate validated,
|
||||
out string validationFailure))
|
||||
{
|
||||
string rejection = validationFailure + CreateEnvelopeDiagnostic(speedLimit, iterate, candidate,
|
||||
iteration + 1);
|
||||
lastCandidateRejection = string.IsNullOrEmpty(lastCandidateRejection)
|
||||
? rejection
|
||||
: lastCandidateRejection + " | " + rejection;
|
||||
if (TryCreateEnvelopeIterate(input, iterate, candidate, out LongitudinalCandidate nextIterate))
|
||||
{
|
||||
iterate = nextIterate;
|
||||
warmStart = ToPrimal(candidate);
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
double maximumChange = MaximumProgressOrSpeedChange(iterate, validated);
|
||||
double relativeObjectiveImprovement = hasPreviousObjective
|
||||
? RelativeObjectiveImprovement(previousObjective, solved.Objective)
|
||||
: double.PositiveInfinity;
|
||||
lastStrictCandidate = CopyCandidate(validated);
|
||||
iterate = validated;
|
||||
warmStart = ToPrimal(validated);
|
||||
previousObjective = solved.Objective;
|
||||
hasPreviousObjective = true;
|
||||
if (maximumChange <= convergenceTolerance && relativeObjectiveImprovement <= convergenceTolerance)
|
||||
return new LongitudinalPlanningResult(EmPlanningStatus.Success, lastStrictCandidate, string.Empty);
|
||||
}
|
||||
|
||||
return lastStrictCandidate == null
|
||||
? Failed(EmPlanningStatus.LongitudinalInfeasible, "No strictly validated longitudinal candidate was found. " +
|
||||
lastCandidateRejection)
|
||||
: new LongitudinalPlanningResult(EmPlanningStatus.Success, lastStrictCandidate, string.Empty);
|
||||
}
|
||||
|
||||
private static bool TryCreateSettings(LongitudinalPlanningInput input, out QpSolverSettings settings,
|
||||
out TimeSpan totalBudget, out double convergenceTolerance, out int iterationLimit, out string failureReason)
|
||||
{
|
||||
settings = null;
|
||||
totalBudget = TimeSpan.Zero;
|
||||
convergenceTolerance = 0d;
|
||||
iterationLimit = 0;
|
||||
failureReason = string.Empty;
|
||||
if (input.Configuration == null || input.Configuration.Solver == null || input.Configuration.Scheduling == null)
|
||||
{
|
||||
failureReason = "Longitudinal solver configuration is required.";
|
||||
return false;
|
||||
}
|
||||
SolverConfiguration solver = input.Configuration.Solver;
|
||||
SchedulingConfiguration scheduling = input.Configuration.Scheduling;
|
||||
if (solver.MaximumOuterIterations <= 0 || solver.MaximumOsqpIterations <= 0 ||
|
||||
!IsPositiveFinite(solver.AbsoluteTolerance) || !IsPositiveFinite(solver.RelativeTolerance) ||
|
||||
!IsPositiveFinite(solver.StrictResidualTolerance) || !IsPositiveFinite(scheduling.SolverTimeoutSeconds))
|
||||
{
|
||||
failureReason = "Longitudinal solver configuration is invalid.";
|
||||
return false;
|
||||
}
|
||||
try
|
||||
{
|
||||
totalBudget = TimeSpan.FromSeconds(scheduling.SolverTimeoutSeconds);
|
||||
settings = new QpSolverSettings(solver.MaximumOsqpIterations, solver.AbsoluteTolerance, solver.RelativeTolerance,
|
||||
totalBudget, solver.WarmStart, solver.Polish, solver.NativeVerbose);
|
||||
convergenceTolerance = solver.StrictResidualTolerance;
|
||||
iterationLimit = Math.Min(MaximumEnvelopeIterations, solver.MaximumOuterIterations);
|
||||
return true;
|
||||
}
|
||||
catch (ArgumentException exception)
|
||||
{
|
||||
failureReason = exception.Message;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
private static LongitudinalCandidate CreateInitialIterate(LongitudinalPlanningInput input)
|
||||
{
|
||||
IReadOnlyList<double> times = LongitudinalCandidate.CreateKnotTimes(input.Configuration.Scheduling.TimeHorizonSeconds,
|
||||
input.Configuration.Scheduling.OutputTimeStepSeconds);
|
||||
if (TryCreateCruiseThenBrakeSeed(input, times, out LongitudinalCandidate brakingSeed))
|
||||
return brakingSeed;
|
||||
int knotCount = times.Count;
|
||||
var s = new double[knotCount];
|
||||
var u = new double[knotCount];
|
||||
var a = new double[knotCount];
|
||||
var j = new double[knotCount - 1];
|
||||
double horizon = times[knotCount - 1];
|
||||
double requestedSpeed = Math.Min(input.DirectionMaximumSpeedMetersPerSecond,
|
||||
Math.Max(0d, input.TerminalPathS / horizon));
|
||||
LongitudinalStoppingProfile terminalStop = LongitudinalStoppingMath.Calculate(requestedSpeed, 0d,
|
||||
input.Configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared,
|
||||
input.Configuration.Longitudinal.MaximumJerkMetersPerSecondCubed);
|
||||
double cruiseDistance = Math.Max(0d, input.TerminalPathS - terminalStop.DistanceMeters);
|
||||
for (int index = 0; index < knotCount; index++)
|
||||
{
|
||||
double fraction = (double)index / (knotCount - 1);
|
||||
s[index] = Math.Min(input.TerminalPathS, cruiseDistance * fraction + terminalStop.DistanceMeters * fraction * fraction);
|
||||
u[index] = index == 0 ? input.InitialProgressSpeedMetersPerSecond : requestedSpeed;
|
||||
a[index] = index == 0 ? input.InitialAccelerationMetersPerSecondSquared : 0d;
|
||||
}
|
||||
s[0] = 0d;
|
||||
s[knotCount - 1] = input.TerminalPathS;
|
||||
u[knotCount - 1] = 0d;
|
||||
a[knotCount - 1] = 0d;
|
||||
return new LongitudinalCandidate(times, s, u, a, j);
|
||||
}
|
||||
|
||||
private static bool TryCreateCruiseThenBrakeSeed(LongitudinalPlanningInput input, IReadOnlyList<double> times,
|
||||
out LongitudinalCandidate candidate)
|
||||
{
|
||||
candidate = null;
|
||||
double initialSpeed = input.InitialProgressSpeedMetersPerSecond;
|
||||
double initialAcceleration = input.InitialAccelerationMetersPerSecondSquared;
|
||||
if (initialSpeed <= 0d || Math.Abs(initialAcceleration) > 1e-12d)
|
||||
return false;
|
||||
double timeStep = times[1] - times[0];
|
||||
for (int index = 1; index < times.Count - 1; index++)
|
||||
{
|
||||
if (Math.Abs((times[index + 1] - times[index]) - timeStep) > 1e-12d)
|
||||
return false;
|
||||
}
|
||||
LongitudinalConfiguration configuration = input.Configuration.Longitudinal;
|
||||
int intervalCount = times.Count - 1;
|
||||
int maximumRampIntervals = Math.Min(intervalCount / 2, checked((int)Math.Floor(
|
||||
configuration.MaximumDecelerationMetersPerSecondSquared /
|
||||
(configuration.MaximumJerkMetersPerSecondCubed * timeStep))));
|
||||
for (int rampIntervals = maximumRampIntervals; rampIntervals >= 1; rampIntervals--)
|
||||
{
|
||||
for (int plateauIntervals = 0; 2 * rampIntervals + plateauIntervals <= intervalCount; plateauIntervals++)
|
||||
{
|
||||
double jerkMagnitude = initialSpeed / (rampIntervals * (rampIntervals + plateauIntervals) *
|
||||
timeStep * timeStep);
|
||||
double peakDeceleration = jerkMagnitude * rampIntervals * timeStep;
|
||||
if (jerkMagnitude > configuration.MaximumJerkMetersPerSecondCubed + 1e-12d ||
|
||||
peakDeceleration > configuration.MaximumDecelerationMetersPerSecondSquared + 1e-12d)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
int brakingIntervals = 2 * rampIntervals + plateauIntervals;
|
||||
double brakingDistance = 0.5d * initialSpeed * brakingIntervals * timeStep;
|
||||
if (brakingDistance > input.TerminalPathS + 1e-12d)
|
||||
continue;
|
||||
int maximumCruiseIntervals = intervalCount - brakingIntervals;
|
||||
int cruiseIntervals = Math.Min(maximumCruiseIntervals, Math.Max(0, checked((int)Math.Floor(
|
||||
(input.TerminalPathS - brakingDistance) / (initialSpeed * timeStep) + 1e-12d))));
|
||||
var jerk = new double[intervalCount];
|
||||
int cursor = cruiseIntervals;
|
||||
for (int index = 0; index < rampIntervals; index++)
|
||||
jerk[cursor++] = -jerkMagnitude;
|
||||
cursor += plateauIntervals;
|
||||
for (int index = 0; index < rampIntervals; index++)
|
||||
jerk[cursor++] = jerkMagnitude;
|
||||
LongitudinalCandidate integrated = LongitudinalCandidate.Integrate(times, 0d, initialSpeed, 0d, jerk);
|
||||
if (integrated.S[integrated.S.Count - 1] <= input.TerminalPathS + 1e-12d)
|
||||
{
|
||||
candidate = integrated;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
private static bool TryCreateCandidate(IReadOnlyList<double> times, IReadOnlyList<double> primal,
|
||||
out LongitudinalCandidate candidate)
|
||||
{
|
||||
candidate = null;
|
||||
if (primal == null)
|
||||
return false;
|
||||
try
|
||||
{
|
||||
var layout = new LongitudinalVariableLayout(times.Count);
|
||||
if (primal.Count != layout.VariableCount)
|
||||
return false;
|
||||
var s = new double[layout.KnotCount];
|
||||
var u = new double[layout.KnotCount];
|
||||
var a = new double[layout.KnotCount];
|
||||
var j = new double[layout.KnotCount - 1];
|
||||
for (int index = 0; index < layout.KnotCount; index++)
|
||||
{
|
||||
s[index] = primal[layout.S(index)];
|
||||
u[index] = primal[layout.U(index)];
|
||||
a[index] = primal[layout.A(index)];
|
||||
}
|
||||
for (int index = 0; index < layout.KnotCount - 1; index++)
|
||||
j[index] = primal[layout.J(index)];
|
||||
candidate = new LongitudinalCandidate(times, s, u, a, j);
|
||||
return true;
|
||||
}
|
||||
catch (ArgumentException)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
private static double[] ToPrimal(LongitudinalCandidate candidate)
|
||||
{
|
||||
var layout = new LongitudinalVariableLayout(candidate.KnotTimes.Count);
|
||||
var primal = new double[layout.VariableCount];
|
||||
for (int index = 0; index < layout.KnotCount; index++)
|
||||
{
|
||||
primal[layout.S(index)] = candidate.S[index];
|
||||
primal[layout.U(index)] = candidate.U[index];
|
||||
primal[layout.A(index)] = candidate.A[index];
|
||||
}
|
||||
for (int index = 0; index < layout.KnotCount - 1; index++)
|
||||
primal[layout.J(index)] = candidate.J[index];
|
||||
return primal;
|
||||
}
|
||||
|
||||
private static bool TryCreateEnvelopeIterate(LongitudinalPlanningInput input, LongitudinalCandidate previous,
|
||||
LongitudinalCandidate candidate, out LongitudinalCandidate nextIterate)
|
||||
{
|
||||
nextIterate = null;
|
||||
if (candidate.S.Count != previous.S.Count)
|
||||
return false;
|
||||
var candidateProgressSamples = new double[candidate.S.Count];
|
||||
double priorProgress = double.NegativeInfinity;
|
||||
double priorPreviousProgress = double.NegativeInfinity;
|
||||
for (int index = 0; index < candidate.S.Count; index++)
|
||||
{
|
||||
double candidateProgress = candidate.S[index];
|
||||
double previousProgress = previous.S[index];
|
||||
if (!IsFinite(previousProgress) || previousProgress < 0d || previousProgress > input.TerminalPathS ||
|
||||
previousProgress < priorPreviousProgress)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if (!IsFinite(candidateProgress))
|
||||
{
|
||||
candidateProgress = previousProgress;
|
||||
}
|
||||
candidateProgress = Math.Max(0d, Math.Min(input.TerminalPathS, candidateProgress));
|
||||
if (index == candidate.S.Count - 1)
|
||||
candidateProgress = input.TerminalPathS;
|
||||
candidateProgress = Math.Max(priorProgress, candidateProgress);
|
||||
candidateProgressSamples[index] = candidateProgress;
|
||||
priorProgress = candidateProgress;
|
||||
priorPreviousProgress = previousProgress;
|
||||
}
|
||||
var progress = new double[candidate.S.Count];
|
||||
double previousNextProgress = 0d;
|
||||
for (int index = 0; index < progress.Length; index++)
|
||||
{
|
||||
double candidateProgress = candidateProgressSamples[index];
|
||||
if (index == 0 || index == progress.Length - 1 || candidateProgress >= input.TerminalPathS)
|
||||
{
|
||||
progress[index] = candidateProgress;
|
||||
}
|
||||
else
|
||||
{
|
||||
double timeStep = candidate.KnotTimes[index + 1] - candidate.KnotTimes[index];
|
||||
double iterationAdvance = Math.Max(0d, candidateProgress - previous.S[index]);
|
||||
double candidateSpeed = IsFinite(candidate.U[index]) ? Math.Max(0d, candidate.U[index]) : 0d;
|
||||
double lookaheadAdvance = IsFinite(candidate.U[index])
|
||||
? OrdinaryEnvelopeProbeLookaheadSteps * candidateSpeed * timeStep
|
||||
: 0d;
|
||||
double terminalLimitedAdvance = OrdinaryTerminalProbeFraction *
|
||||
(input.TerminalPathS - candidateProgress);
|
||||
double advance = Math.Min(Math.Max(iterationAdvance, lookaheadAdvance), terminalLimitedAdvance);
|
||||
progress[index] = candidateProgress + advance;
|
||||
}
|
||||
progress[index] = Math.Max(previousNextProgress, progress[index]);
|
||||
previousNextProgress = progress[index];
|
||||
}
|
||||
nextIterate = new LongitudinalCandidate(candidate.KnotTimes, progress, previous.U, previous.A, previous.J);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool HasStrictResiduals(QpSolveResult result, double tolerance)
|
||||
{
|
||||
return IsPositiveFinite(tolerance) && result.PrimalResidual >= 0d && result.DualResidual >= 0d &&
|
||||
result.PrimalResidual <= tolerance && result.DualResidual <= tolerance;
|
||||
}
|
||||
|
||||
private static string CreateEnvelopeDiagnostic(PathSpeedLimit speedLimit, LongitudinalCandidate iterate,
|
||||
LongitudinalCandidate candidate, int iteration)
|
||||
{
|
||||
int worstIndex = -1;
|
||||
double worstExcess = double.NegativeInfinity;
|
||||
for (int index = 0; index < candidate.S.Count; index++)
|
||||
{
|
||||
if (!IsFinite(candidate.S[index]) || !IsFinite(candidate.U[index]))
|
||||
continue;
|
||||
double candidateProgress = Math.Max(0d, Math.Min(speedLimit.TerminalPathS, candidate.S[index]));
|
||||
double limit = speedLimit.MaximumSpeedAt(candidateProgress);
|
||||
double excess = candidate.U[index] - limit;
|
||||
if (excess > worstExcess)
|
||||
{
|
||||
worstExcess = excess;
|
||||
worstIndex = index;
|
||||
}
|
||||
}
|
||||
if (worstIndex < 0)
|
||||
return "";
|
||||
return " Envelope iteration " + iteration + " used PathS=" + iterate.S[worstIndex] +
|
||||
" and produced PathS=" + candidate.S[worstIndex] + " at its largest speed-envelope excess.";
|
||||
}
|
||||
|
||||
private static double MaximumProgressOrSpeedChange(LongitudinalCandidate previous, LongitudinalCandidate current)
|
||||
{
|
||||
double maximum = 0d;
|
||||
for (int index = 0; index < previous.S.Count; index++)
|
||||
{
|
||||
maximum = Math.Max(maximum, Math.Abs(current.S[index] - previous.S[index]));
|
||||
maximum = Math.Max(maximum, Math.Abs(current.U[index] - previous.U[index]));
|
||||
}
|
||||
return maximum;
|
||||
}
|
||||
|
||||
private static double RelativeObjectiveImprovement(double previous, double current)
|
||||
{
|
||||
return Math.Abs(previous - current) / Math.Max(1d, Math.Abs(previous));
|
||||
}
|
||||
|
||||
private static LongitudinalPlanningResult FallbackOrFailure(LongitudinalCandidate candidate,
|
||||
EmPlanningStatus failureStatus, string failureReason)
|
||||
{
|
||||
return candidate == null
|
||||
? Failed(failureStatus, failureReason)
|
||||
: new LongitudinalPlanningResult(EmPlanningStatus.SuccessWithFallback, candidate, failureReason);
|
||||
}
|
||||
|
||||
private static LongitudinalPlanningResult Failed(EmPlanningStatus status, string reason)
|
||||
{
|
||||
return new LongitudinalPlanningResult(status, null, reason);
|
||||
}
|
||||
|
||||
private static LongitudinalCandidate CopyCandidate(LongitudinalCandidate source)
|
||||
{
|
||||
return new LongitudinalCandidate(source.KnotTimes, source.S, source.U, source.A, source.J);
|
||||
}
|
||||
|
||||
private static bool IsPositiveFinite(double value)
|
||||
{
|
||||
return IsFinite(value) && value > 0d;
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user