feat: optimize lateral paths with SQP
This commit is contained in:
梁薄云
@@ -0,0 +1,20 @@
|
||||
using System;
|
||||
using System.Threading;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Public lateral-planning entry point backed by the solver-neutral SQP optimizer.</summary>
|
||||
public sealed class LateralPlanner
|
||||
{
|
||||
private readonly SequentialConvexOptimizer _optimizer;
|
||||
|
||||
public LateralPlanner(IQpSolver qpSolver)
|
||||
{
|
||||
_optimizer = new SequentialConvexOptimizer(qpSolver ?? throw new ArgumentNullException(nameof(qpSolver)));
|
||||
}
|
||||
|
||||
public LateralPlanningResult Plan(LateralPlanningInput input, CancellationToken cancellationToken)
|
||||
{
|
||||
return _optimizer.Optimize(input, cancellationToken);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,294 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.Threading;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Runs bounded lateral SQP iterations and retains only independently validated candidates.</summary>
|
||||
public sealed class SequentialConvexOptimizer
|
||||
{
|
||||
private const double StationTolerance = 1e-12d;
|
||||
private readonly IQpSolver _qpSolver;
|
||||
private readonly LateralConstraintBuilder _constraintBuilder;
|
||||
private readonly LateralGeometryEvaluator _geometryEvaluator;
|
||||
private readonly LateralSolutionValidator _solutionValidator;
|
||||
|
||||
public SequentialConvexOptimizer(IQpSolver qpSolver)
|
||||
: this(qpSolver, new LateralConstraintBuilder(new LateralObjectiveBuilder()), new LateralGeometryEvaluator(),
|
||||
new LateralSolutionValidator())
|
||||
{
|
||||
}
|
||||
|
||||
internal SequentialConvexOptimizer(IQpSolver qpSolver, LateralConstraintBuilder constraintBuilder,
|
||||
LateralGeometryEvaluator geometryEvaluator, LateralSolutionValidator solutionValidator)
|
||||
{
|
||||
_qpSolver = qpSolver ?? throw new ArgumentNullException(nameof(qpSolver));
|
||||
_constraintBuilder = constraintBuilder ?? throw new ArgumentNullException(nameof(constraintBuilder));
|
||||
_geometryEvaluator = geometryEvaluator ?? throw new ArgumentNullException(nameof(geometryEvaluator));
|
||||
_solutionValidator = solutionValidator ?? throw new ArgumentNullException(nameof(solutionValidator));
|
||||
}
|
||||
|
||||
public LateralPlanningResult Optimize(LateralPlanningInput input, CancellationToken cancellationToken)
|
||||
{
|
||||
if (input == null)
|
||||
return Failed(EmPlanningStatus.InvalidInput, "Lateral planning input is required.");
|
||||
|
||||
if (!TryCreateSettings(input, out QpSolverSettings settings, out TimeSpan totalBudget, out double convergenceTolerance,
|
||||
out string configurationFailure))
|
||||
{
|
||||
return Failed(EmPlanningStatus.InvalidInput, configurationFailure);
|
||||
}
|
||||
|
||||
LateralCandidate iterate = CreateInitialIterate(input);
|
||||
var warmStart = Array.Empty<double>();
|
||||
LateralPath lastValidatedPath = null;
|
||||
double previousObjective = 0d;
|
||||
bool hasPreviousObjective = false;
|
||||
var stopwatch = Stopwatch.StartNew();
|
||||
|
||||
for (int iteration = 0; iteration < input.Configuration.Solver.MaximumOuterIterations; iteration++)
|
||||
{
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
return FallbackOrFailure(lastValidatedPath, EmPlanningStatus.Cancelled, "Lateral SQP was cancelled.");
|
||||
|
||||
TimeSpan remainingBudget = totalBudget - stopwatch.Elapsed;
|
||||
if (remainingBudget <= TimeSpan.Zero)
|
||||
return FallbackOrFailure(lastValidatedPath, EmPlanningStatus.SolverTimedOut,
|
||||
"Lateral SQP exhausted its solve budget.");
|
||||
|
||||
if (!_constraintBuilder.TryBuild(input, iterate, out QuadraticProgram problem, out string failureReason))
|
||||
{
|
||||
return FallbackOrFailure(lastValidatedPath, EmPlanningStatus.LateralInfeasible,
|
||||
"Lateral SQP constraints are infeasible: " + failureReason);
|
||||
}
|
||||
|
||||
QpSolveResult solved = _qpSolver.Solve(problem,
|
||||
new QpSolverSettings(settings.MaximumIterations, settings.AbsoluteTolerance, settings.RelativeTolerance,
|
||||
remainingBudget, settings.EnableWarmStart, settings.EnablePolishing, settings.EnableNativeVerboseOutput),
|
||||
warmStart, cancellationToken);
|
||||
|
||||
if (solved == null)
|
||||
return FallbackOrFailure(lastValidatedPath, EmPlanningStatus.Failed, "The lateral QP solver returned no result.");
|
||||
|
||||
if (solved.Status == QpSolveStatus.TimeLimit || solved.Status == QpSolveStatus.MaximumIterations)
|
||||
{
|
||||
return FallbackOrFailure(lastValidatedPath, EmPlanningStatus.SolverTimedOut,
|
||||
"The lateral QP solver timed out: " + solved.Diagnostic);
|
||||
}
|
||||
if (solved.Status == QpSolveStatus.Cancelled)
|
||||
return FallbackOrFailure(lastValidatedPath, EmPlanningStatus.Cancelled,
|
||||
"The lateral QP solver was cancelled: " + solved.Diagnostic);
|
||||
if (solved.Status == QpSolveStatus.PrimalInfeasible || solved.Status == QpSolveStatus.DualInfeasible)
|
||||
{
|
||||
return FallbackOrFailure(lastValidatedPath, EmPlanningStatus.LateralInfeasible,
|
||||
"The lateral QP solver reported infeasibility: " + solved.Diagnostic);
|
||||
}
|
||||
if (solved.Status == QpSolveStatus.SolverUnavailable)
|
||||
return FallbackOrFailure(lastValidatedPath, EmPlanningStatus.SolverUnavailable,
|
||||
"The lateral QP solver is unavailable: " + solved.Diagnostic);
|
||||
if (solved.Status != QpSolveStatus.Solved && solved.Status != QpSolveStatus.SolvedInaccurate)
|
||||
{
|
||||
return FallbackOrFailure(lastValidatedPath, EmPlanningStatus.Failed,
|
||||
"The lateral QP solver failed: " + solved.Diagnostic);
|
||||
}
|
||||
if (solved.Status == QpSolveStatus.SolvedInaccurate && !HasStrictResiduals(solved, input.Configuration.Solver.StrictResidualTolerance))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
if (!TryCreateCandidate(input.ReferenceStations, solved.Primal, out LateralCandidate candidate))
|
||||
continue;
|
||||
if (!_geometryEvaluator.TryEvaluate(input, candidate, out LateralPath evaluatedPath, out _))
|
||||
continue;
|
||||
if (!_solutionValidator.TryValidate(input, candidate, evaluatedPath, out LateralPath validatedPath, out _))
|
||||
continue;
|
||||
|
||||
double maximumLateralChange = MaximumLateralChange(iterate, candidate);
|
||||
double relativeObjectiveImprovement = hasPreviousObjective
|
||||
? RelativeObjectiveImprovement(previousObjective, solved.Objective)
|
||||
: double.PositiveInfinity;
|
||||
lastValidatedPath = CopyPath(validatedPath);
|
||||
iterate = candidate;
|
||||
warmStart = CopyValues(solved.Primal);
|
||||
previousObjective = solved.Objective;
|
||||
hasPreviousObjective = true;
|
||||
|
||||
if (maximumLateralChange <= convergenceTolerance && relativeObjectiveImprovement <= convergenceTolerance)
|
||||
return new LateralPlanningResult(EmPlanningStatus.Success, lastValidatedPath, string.Empty);
|
||||
}
|
||||
|
||||
return lastValidatedPath == null
|
||||
? Failed(EmPlanningStatus.LateralInfeasible, "No independently validated lateral candidate was found.")
|
||||
: new LateralPlanningResult(EmPlanningStatus.Success, lastValidatedPath, string.Empty);
|
||||
}
|
||||
|
||||
private static bool TryCreateSettings(LateralPlanningInput input, out QpSolverSettings settings, out TimeSpan totalBudget,
|
||||
out double convergenceTolerance, out string failureReason)
|
||||
{
|
||||
settings = null;
|
||||
totalBudget = TimeSpan.Zero;
|
||||
convergenceTolerance = 0d;
|
||||
failureReason = string.Empty;
|
||||
SolverConfiguration solver = input.Configuration.Solver;
|
||||
SchedulingConfiguration scheduling = input.Configuration.Scheduling;
|
||||
if (solver == null || scheduling == null || solver.MaximumOuterIterations <= 0 ||
|
||||
!IsPositiveFinite(solver.AbsoluteTolerance) || !IsPositiveFinite(solver.RelativeTolerance) ||
|
||||
!IsPositiveFinite(solver.StrictResidualTolerance) || !IsPositiveFinite(scheduling.SolverTimeoutSeconds))
|
||||
{
|
||||
failureReason = "The lateral SQP 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;
|
||||
return true;
|
||||
}
|
||||
catch (ArgumentException exception)
|
||||
{
|
||||
failureReason = exception.Message;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
private static LateralCandidate CreateInitialIterate(LateralPlanningInput input)
|
||||
{
|
||||
int stationCount = input.ReferenceStations.Count;
|
||||
var l = new double[stationCount];
|
||||
var dl = new double[stationCount];
|
||||
var ddl = new double[stationCount];
|
||||
var dddl = new double[stationCount - 1];
|
||||
bool coversAllStations = input.PreviousTrajectorySeed.Count >= 2 &&
|
||||
input.PreviousTrajectorySeed[0].ReferenceS <= input.ReferenceStations[0] + StationTolerance &&
|
||||
input.PreviousTrajectorySeed[input.PreviousTrajectorySeed.Count - 1].ReferenceS >=
|
||||
input.ReferenceStations[stationCount - 1] - StationTolerance;
|
||||
|
||||
for (int index = 0; index < stationCount; index++)
|
||||
{
|
||||
LateralInterval corridor = input.Corridor.Stations[index];
|
||||
l[index] = coversAllStations
|
||||
? InterpolateSeedL(input.PreviousTrajectorySeed, input.ReferenceStations[index])
|
||||
: Clamp(0d, corridor.MinimumL, corridor.MaximumL);
|
||||
}
|
||||
|
||||
double startDenominator = 1d - input.StartProjection.ReferencePoint.GeometricCurvature *
|
||||
input.StartProjection.LateralOffset;
|
||||
l[0] = input.StartProjection.LateralOffset;
|
||||
dl[0] = startDenominator * Math.Tan(input.StartProjection.HeadingError);
|
||||
return new LateralCandidate(input.ReferenceStations, l, dl, ddl, dddl);
|
||||
}
|
||||
|
||||
private static bool TryCreateCandidate(IReadOnlyList<double> stations, IReadOnlyList<double> primal,
|
||||
out LateralCandidate candidate)
|
||||
{
|
||||
candidate = null;
|
||||
if (primal == null)
|
||||
return false;
|
||||
try
|
||||
{
|
||||
var layout = new LateralVariableLayout(stations.Count);
|
||||
if (primal.Count != layout.VariableCount)
|
||||
return false;
|
||||
var l = new double[layout.StationCount];
|
||||
var dl = new double[layout.StationCount];
|
||||
var ddl = new double[layout.StationCount];
|
||||
var dddl = new double[layout.StationCount - 1];
|
||||
for (int index = 0; index < layout.StationCount; index++)
|
||||
{
|
||||
l[index] = primal[layout.L(index)];
|
||||
dl[index] = primal[layout.DL(index)];
|
||||
ddl[index] = primal[layout.DDL(index)];
|
||||
}
|
||||
for (int index = 0; index < dddl.Length; index++)
|
||||
dddl[index] = primal[layout.DDDL(index)];
|
||||
candidate = new LateralCandidate(stations, l, dl, ddl, dddl);
|
||||
return true;
|
||||
}
|
||||
catch (ArgumentException)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
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 double MaximumLateralChange(LateralCandidate previous, LateralCandidate current)
|
||||
{
|
||||
double maximum = 0d;
|
||||
for (int index = 0; index < previous.L.Count; index++)
|
||||
maximum = Math.Max(maximum, Math.Abs(current.L[index] - previous.L[index]));
|
||||
return maximum;
|
||||
}
|
||||
|
||||
private static double RelativeObjectiveImprovement(double previous, double current)
|
||||
{
|
||||
return Math.Abs(previous - current) / Math.Max(1d, Math.Abs(previous));
|
||||
}
|
||||
|
||||
private static LateralPlanningResult FallbackOrFailure(LateralPath path, EmPlanningStatus failureStatus, string failureReason)
|
||||
{
|
||||
return path == null
|
||||
? Failed(failureStatus, failureReason)
|
||||
: new LateralPlanningResult(EmPlanningStatus.SuccessWithFallback, path, failureReason);
|
||||
}
|
||||
|
||||
private static LateralPlanningResult Failed(EmPlanningStatus status, string reason)
|
||||
{
|
||||
return new LateralPlanningResult(status, null, reason);
|
||||
}
|
||||
|
||||
private static LateralPath CopyPath(LateralPath source)
|
||||
{
|
||||
var points = new List<LateralPathPoint>(source.Points.Count);
|
||||
for (int index = 0; index < source.Points.Count; index++)
|
||||
{
|
||||
LateralPathPoint point = source.Points[index];
|
||||
points.Add(new LateralPathPoint(point.ReferenceS, point.PathS, point.L, point.DL, point.DDL, point.DDDL,
|
||||
point.X, point.Y, point.VehicleYaw, point.GeometricCurvature, point.VehicleCurvature,
|
||||
point.VehicleCurvatureDerivative));
|
||||
}
|
||||
return new LateralPath(points, true);
|
||||
}
|
||||
|
||||
private static double[] CopyValues(IReadOnlyList<double> source)
|
||||
{
|
||||
var copy = new double[source.Count];
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
copy[index] = source[index];
|
||||
return copy;
|
||||
}
|
||||
|
||||
private static double InterpolateSeedL(IReadOnlyList<FrenetProjection> seed, double referenceS)
|
||||
{
|
||||
if (referenceS <= seed[0].ReferenceS)
|
||||
return seed[0].LateralOffset;
|
||||
for (int index = 1; index < seed.Count; index++)
|
||||
{
|
||||
if (referenceS <= seed[index].ReferenceS)
|
||||
{
|
||||
FrenetProjection lower = seed[index - 1];
|
||||
FrenetProjection upper = seed[index];
|
||||
double span = upper.ReferenceS - lower.ReferenceS;
|
||||
return span <= StationTolerance ? upper.LateralOffset : lower.LateralOffset +
|
||||
(upper.LateralOffset - lower.LateralOffset) * (referenceS - lower.ReferenceS) / span;
|
||||
}
|
||||
}
|
||||
return seed[seed.Count - 1].LateralOffset;
|
||||
}
|
||||
|
||||
private static double Clamp(double value, double minimum, double maximum)
|
||||
{
|
||||
return Math.Max(minimum, Math.Min(maximum, value));
|
||||
}
|
||||
|
||||
private static bool IsPositiveFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value) && value > 0d;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user