chore: save current workspace progress
This commit is contained in:
梁薄云
@@ -35,7 +35,7 @@ public sealed class LateralConstraintBuilder
|
||||
_objectiveBuilder.AddTerms(input, layout, linearization, hessian, linearCost);
|
||||
|
||||
int terminalRows = input.TerminalType == EmTerminalType.RollingSafetyStop ? 0 : 2;
|
||||
var constraints = new SparseTripletBuilder(7 * layout.StationCount - 2 + terminalRows, layout.VariableCount);
|
||||
var constraints = new SparseTripletBuilder(8 * layout.StationCount - 2 + terminalRows, layout.VariableCount);
|
||||
var lower = new List<double>();
|
||||
var upper = new List<double>();
|
||||
int row = 0;
|
||||
@@ -43,12 +43,13 @@ public sealed class LateralConstraintBuilder
|
||||
if (!TryAddLateralBounds(input, layout, linearization, constraints, lower, upper, ref row, out failureReason))
|
||||
return false;
|
||||
AddDerivativeBounds(input, layout, constraints, lower, upper, ref row);
|
||||
AddCurvatureBounds(input, layout, linearization, constraints, lower, upper, ref row);
|
||||
AddStartConstraints(input, layout, constraints, lower, upper, ref row);
|
||||
AddExactDynamics(input.ReferenceStations, layout, constraints, lower, upper, ref row);
|
||||
if (input.TerminalType != EmTerminalType.RollingSafetyStop)
|
||||
AddTerminalConstraints(layout, constraints, lower, upper, ref row);
|
||||
|
||||
if (row != 7 * layout.StationCount - 2 + terminalRows)
|
||||
if (row != 8 * layout.StationCount - 2 + terminalRows)
|
||||
throw new InvalidOperationException("Lateral constraint row accounting is inconsistent.");
|
||||
problem = new QuadraticProgram(hessian.Build(), linearCost, constraints.Build(), lower, upper);
|
||||
return true;
|
||||
@@ -131,6 +132,21 @@ public sealed class LateralConstraintBuilder
|
||||
AddSingleVariableRow(constraints, lower, upper, ref row, layout.DDDL(interval), -third, third);
|
||||
}
|
||||
|
||||
private static void AddCurvatureBounds(LateralPlanningInput input, LateralVariableLayout layout,
|
||||
LateralCandidate linearization, SparseTripletBuilder constraints, IList<double> lower, IList<double> upper,
|
||||
ref int row)
|
||||
{
|
||||
double maximumCurvature = LateralCurvatureLinearization.GetMaximumVehicleCurvature(input.Vehicle);
|
||||
IReadOnlyList<LateralCurvatureLinearization> affines = LateralCurvatureLinearization.Create(input, layout,
|
||||
linearization);
|
||||
for (int station = 0; station < affines.Count; station++)
|
||||
{
|
||||
LateralCurvatureLinearization affine = affines[station];
|
||||
AddRow(constraints, lower, upper, ref row, -maximumCurvature - affine.Constant,
|
||||
maximumCurvature - affine.Constant, affine.VariableIndices, affine.Gradient);
|
||||
}
|
||||
}
|
||||
|
||||
private static void AddStartConstraints(LateralPlanningInput input, LateralVariableLayout layout,
|
||||
SparseTripletBuilder constraints, IList<double> lower, IList<double> upper, ref int row)
|
||||
{
|
||||
|
||||
@@ -0,0 +1,101 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Shared affine vehicle-curvature model used by the LS objective and hard QP constraints.</summary>
|
||||
internal sealed class LateralCurvatureLinearization
|
||||
{
|
||||
private LateralCurvatureLinearization(int[] variableIndices, double[] gradient, double constant)
|
||||
{
|
||||
VariableIndices = variableIndices;
|
||||
Gradient = gradient;
|
||||
Constant = constant;
|
||||
}
|
||||
|
||||
internal IReadOnlyList<int> VariableIndices { get; }
|
||||
|
||||
internal IReadOnlyList<double> Gradient { get; }
|
||||
|
||||
internal double Constant { get; }
|
||||
|
||||
internal static IReadOnlyList<LateralCurvatureLinearization> Create(LateralPlanningInput input,
|
||||
LateralVariableLayout layout, LateralCandidate linearization)
|
||||
{
|
||||
if (input == null) throw new ArgumentNullException(nameof(input));
|
||||
if (layout == null) throw new ArgumentNullException(nameof(layout));
|
||||
if (linearization == null) throw new ArgumentNullException(nameof(linearization));
|
||||
if (layout.StationCount != input.ReferenceStations.Count ||
|
||||
linearization.ReferenceStations.Count != layout.StationCount)
|
||||
{
|
||||
throw new ArgumentException("Curvature linearization stations must match the lateral layout.", nameof(linearization));
|
||||
}
|
||||
|
||||
double directionSign = input.ReferenceSegment.Direction == TravelDirection.Forward ? 1d : -1d;
|
||||
var affines = new List<LateralCurvatureLinearization>(layout.StationCount);
|
||||
for (int station = 0; station < layout.StationCount; station++)
|
||||
{
|
||||
FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(input.ReferenceSegment,
|
||||
input.ReferenceStations[station]);
|
||||
double l = linearization.L[station];
|
||||
double dl = linearization.DL[station];
|
||||
double ddl = linearization.DDL[station];
|
||||
double referenceCurvature = reference.GeometricCurvature;
|
||||
double referenceCurvatureDerivative = directionSign * reference.VehicleCurvatureDerivative;
|
||||
double a = 1d - referenceCurvature * l;
|
||||
double denominatorSquared = a * a + dl * dl;
|
||||
if (!IsFinite(denominatorSquared) || denominatorSquared <= 0d)
|
||||
throw new ArgumentException("Curvature linearization denominator is invalid.", nameof(linearization));
|
||||
|
||||
double denominatorPow3Over2 = denominatorSquared * Math.Sqrt(denominatorSquared);
|
||||
double denominatorPow5Over2 = denominatorPow3Over2 * denominatorSquared;
|
||||
double numerator = a * a * referenceCurvature + a * ddl +
|
||||
referenceCurvatureDerivative * l * dl + 2d * referenceCurvature * dl * dl;
|
||||
double geometricCurvature = numerator / denominatorPow3Over2;
|
||||
double dNumeratorDLateral = -2d * a * referenceCurvature * referenceCurvature -
|
||||
referenceCurvature * ddl + referenceCurvatureDerivative * dl;
|
||||
double dNumeratorDSlope = referenceCurvatureDerivative * l + 4d * referenceCurvature * dl;
|
||||
double dDenominatorSquaredDLateral = -2d * a * referenceCurvature;
|
||||
double dDenominatorSquaredDSlope = 2d * dl;
|
||||
double dGeometricDLateral = dNumeratorDLateral / denominatorPow3Over2 -
|
||||
1.5d * numerator * dDenominatorSquaredDLateral / denominatorPow5Over2;
|
||||
double dGeometricDSlope = dNumeratorDSlope / denominatorPow3Over2 -
|
||||
1.5d * numerator * dDenominatorSquaredDSlope / denominatorPow5Over2;
|
||||
double dGeometricDSecondDerivative = a / denominatorPow3Over2;
|
||||
double vehicleCurvature = directionSign * geometricCurvature;
|
||||
double[] gradient =
|
||||
{
|
||||
directionSign * dGeometricDLateral,
|
||||
directionSign * dGeometricDSlope,
|
||||
directionSign * dGeometricDSecondDerivative,
|
||||
};
|
||||
double constant = vehicleCurvature - gradient[0] * l - gradient[1] * dl - gradient[2] * ddl;
|
||||
if (!IsFinite(vehicleCurvature) || !IsFinite(constant) || !IsFinite(gradient[0]) ||
|
||||
!IsFinite(gradient[1]) || !IsFinite(gradient[2]))
|
||||
{
|
||||
throw new ArgumentException("Curvature linearization is non-finite.", nameof(linearization));
|
||||
}
|
||||
affines.Add(new LateralCurvatureLinearization(
|
||||
new[] { layout.L(station), layout.DL(station), layout.DDL(station) }, gradient, constant));
|
||||
}
|
||||
return affines;
|
||||
}
|
||||
|
||||
internal static double GetMaximumVehicleCurvature(VehicleParameters vehicle)
|
||||
{
|
||||
if (vehicle == null) throw new ArgumentNullException(nameof(vehicle));
|
||||
double maximum = vehicle.MaximumCurvaturePerMeter ??
|
||||
(vehicle.MinimumTurningRadiusMeters.HasValue && vehicle.MinimumTurningRadiusMeters.Value > 0d
|
||||
? 1d / vehicle.MinimumTurningRadiusMeters.Value
|
||||
: double.NaN);
|
||||
if (!IsFinite(maximum) || maximum <= 0d)
|
||||
throw new ArgumentException("Vehicle maximum curvature is required.", nameof(vehicle));
|
||||
return maximum;
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
@@ -27,7 +27,8 @@ public sealed class LateralObjectiveBuilder
|
||||
double slopeScale = RequirePositive(lateral.MaximumLateralSlope, "slope scale");
|
||||
double secondDerivativeScale = RequirePositive(lateral.MaximumLateralSecondDerivativePerMeter, "second-derivative scale");
|
||||
double thirdDerivativeScale = RequirePositive(lateral.MaximumLateralThirdDerivativePerSquareMeter, "third-derivative scale");
|
||||
double curvatureScale = RequirePositive(GetMaximumVehicleCurvature(input.Vehicle), "curvature scale");
|
||||
double curvatureScale = RequirePositive(LateralCurvatureLinearization.GetMaximumVehicleCurvature(input.Vehicle),
|
||||
"curvature scale");
|
||||
double curvatureVariationScale = GetCurvatureVariationScale(input);
|
||||
|
||||
for (int station = 0; station < layout.StationCount; station++)
|
||||
@@ -46,10 +47,11 @@ public sealed class LateralObjectiveBuilder
|
||||
}
|
||||
|
||||
AddPreviousTrajectoryTerms(input, layout, hessian, linearCost, weights.PreviousTrajectory, lateralScale);
|
||||
CurvatureAffine[] curvature = CreateCurvatureAffines(input, layout, linearization);
|
||||
for (int station = 0; station < curvature.Length; station++)
|
||||
IReadOnlyList<LateralCurvatureLinearization> curvature = LateralCurvatureLinearization.Create(input, layout,
|
||||
linearization);
|
||||
for (int station = 0; station < curvature.Count; station++)
|
||||
{
|
||||
AddSquaredResidual(hessian, linearCost, curvature[station].Indices, curvature[station].Gradient,
|
||||
AddSquaredResidual(hessian, linearCost, curvature[station].VariableIndices, curvature[station].Gradient,
|
||||
curvature[station].Constant, weights.Curvature, curvatureScale);
|
||||
}
|
||||
AddCurvatureVariationTerms(input.ReferenceStations, curvature, hessian, linearCost, weights.CurvatureVariation,
|
||||
@@ -75,79 +77,27 @@ public sealed class LateralObjectiveBuilder
|
||||
}
|
||||
}
|
||||
|
||||
private static CurvatureAffine[] CreateCurvatureAffines(LateralPlanningInput input, LateralVariableLayout layout,
|
||||
LateralCandidate linearization)
|
||||
{
|
||||
var affines = new CurvatureAffine[layout.StationCount];
|
||||
double directionSign = input.ReferenceSegment.Direction == TravelDirection.Forward ? 1d : -1d;
|
||||
for (int station = 0; station < layout.StationCount; station++)
|
||||
{
|
||||
FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(input.ReferenceSegment,
|
||||
input.ReferenceStations[station]);
|
||||
double l = linearization.L[station];
|
||||
double dl = linearization.DL[station];
|
||||
double ddl = linearization.DDL[station];
|
||||
double referenceCurvature = reference.GeometricCurvature;
|
||||
double referenceCurvatureDerivative = directionSign * reference.VehicleCurvatureDerivative;
|
||||
double a = 1d - referenceCurvature * l;
|
||||
double denominatorSquared = a * a + dl * dl;
|
||||
if (!IsFinite(denominatorSquared) || denominatorSquared <= 0d)
|
||||
throw new ArgumentException("Curvature linearization denominator is invalid.", nameof(linearization));
|
||||
|
||||
double denominatorPow3Over2 = denominatorSquared * Math.Sqrt(denominatorSquared);
|
||||
double denominatorPow5Over2 = denominatorPow3Over2 * denominatorSquared;
|
||||
double numerator = a * a * referenceCurvature + a * ddl +
|
||||
referenceCurvatureDerivative * l * dl + 2d * referenceCurvature * dl * dl;
|
||||
double geometricCurvature = numerator / denominatorPow3Over2;
|
||||
double dNumeratorDLateral = -2d * a * referenceCurvature * referenceCurvature -
|
||||
referenceCurvature * ddl + referenceCurvatureDerivative * dl;
|
||||
double dNumeratorDSlope = referenceCurvatureDerivative * l + 4d * referenceCurvature * dl;
|
||||
double dDenominatorSquaredDLateral = -2d * a * referenceCurvature;
|
||||
double dDenominatorSquaredDSlope = 2d * dl;
|
||||
double dGeometricDLateral = dNumeratorDLateral / denominatorPow3Over2 -
|
||||
1.5d * numerator * dDenominatorSquaredDLateral / denominatorPow5Over2;
|
||||
double dGeometricDSlope = dNumeratorDSlope / denominatorPow3Over2 -
|
||||
1.5d * numerator * dDenominatorSquaredDSlope / denominatorPow5Over2;
|
||||
double dGeometricDSecondDerivative = a / denominatorPow3Over2;
|
||||
double vehicleCurvature = directionSign * geometricCurvature;
|
||||
double[] gradient =
|
||||
{
|
||||
directionSign * dGeometricDLateral,
|
||||
directionSign * dGeometricDSlope,
|
||||
directionSign * dGeometricDSecondDerivative,
|
||||
};
|
||||
double constant = vehicleCurvature - gradient[0] * l - gradient[1] * dl - gradient[2] * ddl;
|
||||
if (!IsFinite(vehicleCurvature) || !IsFinite(constant) || !IsFinite(gradient[0]) ||
|
||||
!IsFinite(gradient[1]) || !IsFinite(gradient[2]))
|
||||
{
|
||||
throw new ArgumentException("Curvature linearization is non-finite.", nameof(linearization));
|
||||
}
|
||||
affines[station] = new CurvatureAffine(new[] { layout.L(station), layout.DL(station), layout.DDL(station) },
|
||||
gradient, constant);
|
||||
}
|
||||
return affines;
|
||||
}
|
||||
|
||||
private static void AddCurvatureVariationTerms(IReadOnlyList<double> stations, CurvatureAffine[] curvature,
|
||||
private static void AddCurvatureVariationTerms(IReadOnlyList<double> stations,
|
||||
IReadOnlyList<LateralCurvatureLinearization> curvature,
|
||||
SparseTripletBuilder hessian, IList<double> linearCost, double weight, double scale)
|
||||
{
|
||||
for (int station = 0; station < curvature.Length; station++)
|
||||
for (int station = 0; station < curvature.Count; station++)
|
||||
{
|
||||
int lower = station == 0 ? 0 : station - 1;
|
||||
int upper = station == curvature.Length - 1 ? curvature.Length - 1 : station + 1;
|
||||
int upper = station == curvature.Count - 1 ? curvature.Count - 1 : station + 1;
|
||||
double ds = stations[upper] - stations[lower];
|
||||
if (!IsFinite(ds) || ds <= 0d)
|
||||
throw new ArgumentException("Curvature variation requires strictly increasing stations.", nameof(stations));
|
||||
CurvatureAffine left = curvature[lower];
|
||||
CurvatureAffine right = curvature[upper];
|
||||
var indices = new int[left.Indices.Length + right.Indices.Length];
|
||||
LateralCurvatureLinearization left = curvature[lower];
|
||||
LateralCurvatureLinearization right = curvature[upper];
|
||||
var indices = new int[left.VariableIndices.Count + right.VariableIndices.Count];
|
||||
var gradient = new double[indices.Length];
|
||||
for (int index = 0; index < left.Indices.Length; index++)
|
||||
for (int index = 0; index < left.VariableIndices.Count; index++)
|
||||
{
|
||||
indices[index] = left.Indices[index];
|
||||
indices[index] = left.VariableIndices[index];
|
||||
gradient[index] = -left.Gradient[index] / ds;
|
||||
indices[left.Indices.Length + index] = right.Indices[index];
|
||||
gradient[left.Indices.Length + index] = right.Gradient[index] / ds;
|
||||
indices[left.VariableIndices.Count + index] = right.VariableIndices[index];
|
||||
gradient[left.VariableIndices.Count + index] = right.Gradient[index] / ds;
|
||||
}
|
||||
AddSquaredResidual(hessian, linearCost, indices, gradient, (right.Constant - left.Constant) / ds, weight, scale);
|
||||
}
|
||||
@@ -209,17 +159,6 @@ public sealed class LateralObjectiveBuilder
|
||||
return Math.Max(1d, maximum);
|
||||
}
|
||||
|
||||
private static double GetMaximumVehicleCurvature(VehicleParameters vehicle)
|
||||
{
|
||||
if (vehicle == null)
|
||||
throw new ArgumentNullException(nameof(vehicle));
|
||||
if (vehicle.MaximumCurvaturePerMeter.HasValue)
|
||||
return vehicle.MaximumCurvaturePerMeter.Value;
|
||||
if (vehicle.MinimumTurningRadiusMeters.HasValue && vehicle.MinimumTurningRadiusMeters.Value > 0d)
|
||||
return 1d / vehicle.MinimumTurningRadiusMeters.Value;
|
||||
throw new ArgumentException("Vehicle maximum curvature is required.", nameof(vehicle));
|
||||
}
|
||||
|
||||
private static double RequirePositive(double value, string name)
|
||||
{
|
||||
if (!IsFinite(value) || value <= 0d)
|
||||
@@ -232,17 +171,4 @@ public sealed class LateralObjectiveBuilder
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
|
||||
private sealed class CurvatureAffine
|
||||
{
|
||||
public CurvatureAffine(int[] indices, double[] gradient, double constant)
|
||||
{
|
||||
Indices = indices;
|
||||
Gradient = gradient;
|
||||
Constant = constant;
|
||||
}
|
||||
|
||||
public int[] Indices { get; }
|
||||
public double[] Gradient { get; }
|
||||
public double Constant { get; }
|
||||
}
|
||||
}
|
||||
|
||||
@@ -68,6 +68,8 @@ public sealed class SequentialConvexOptimizer
|
||||
remainingBudget, settings.EnableWarmStart, settings.EnablePolishing, settings.EnableNativeVerboseOutput),
|
||||
warmStart, cancellationToken);
|
||||
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
return Failed(EmPlanningStatus.Cancelled, "Lateral SQP was cancelled after the QP solve.");
|
||||
if (solved == null)
|
||||
return FallbackOrFailure(lastValidatedPath, EmPlanningStatus.Failed, "The lateral QP solver returned no result.");
|
||||
|
||||
@@ -100,16 +102,27 @@ public sealed class SequentialConvexOptimizer
|
||||
continue;
|
||||
if (!_geometryEvaluator.TryEvaluate(input, candidate, out LateralPath evaluatedPath, out _))
|
||||
continue;
|
||||
|
||||
// A geometrically evaluable QP solution that remains inside the static
|
||||
// corridor is the next SQP linearization point, even when the independent
|
||||
// validator rejects it. Otherwise the next outer pass rebuilds the identical
|
||||
// convex subproblem and cannot correct that result. An out-of-corridor vector
|
||||
// must not become an iterate: it can make the following trust region infeasible.
|
||||
LateralCandidate previousIterate = iterate;
|
||||
if (RespectsStaticCorridor(input, candidate))
|
||||
{
|
||||
iterate = candidate;
|
||||
warmStart = CopyValues(solved.Primal);
|
||||
}
|
||||
|
||||
if (!_solutionValidator.TryValidate(input, candidate, evaluatedPath, out LateralPath validatedPath, out _))
|
||||
continue;
|
||||
|
||||
double maximumLateralChange = MaximumLateralChange(iterate, candidate);
|
||||
double maximumLateralChange = MaximumLateralChange(previousIterate, candidate);
|
||||
double relativeObjectiveImprovement = hasPreviousObjective
|
||||
? RelativeObjectiveImprovement(previousObjective, solved.Objective)
|
||||
: double.PositiveInfinity;
|
||||
lastValidatedPath = CopyPath(validatedPath);
|
||||
iterate = candidate;
|
||||
warmStart = CopyValues(solved.Primal);
|
||||
previousObjective = solved.Objective;
|
||||
hasPreviousObjective = true;
|
||||
|
||||
@@ -119,7 +132,10 @@ public sealed class SequentialConvexOptimizer
|
||||
|
||||
return lastValidatedPath == null
|
||||
? Failed(EmPlanningStatus.LateralInfeasible, "No independently validated lateral candidate was found.")
|
||||
: new LateralPlanningResult(EmPlanningStatus.Success, lastValidatedPath, string.Empty);
|
||||
: new LateralPlanningResult(
|
||||
EmPlanningStatus.SuccessWithFallback,
|
||||
lastValidatedPath,
|
||||
"Lateral SQP reached its outer-iteration limit before strict convergence.");
|
||||
}
|
||||
|
||||
private static bool TryCreateSettings(LateralPlanningInput input, out QpSolverSettings settings, out TimeSpan totalBudget,
|
||||
@@ -226,6 +242,17 @@ public sealed class SequentialConvexOptimizer
|
||||
return maximum;
|
||||
}
|
||||
|
||||
private static bool RespectsStaticCorridor(LateralPlanningInput input, LateralCandidate candidate)
|
||||
{
|
||||
for (int index = 0; index < candidate.L.Count; index++)
|
||||
{
|
||||
LateralInterval interval = input.Corridor.Stations[index];
|
||||
if (candidate.L[index] < interval.MinimumL || candidate.L[index] > interval.MaximumL)
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static double RelativeObjectiveImprovement(double previous, double current)
|
||||
{
|
||||
return Math.Abs(previous - current) / Math.Max(1d, Math.Abs(previous));
|
||||
@@ -233,7 +260,7 @@ public sealed class SequentialConvexOptimizer
|
||||
|
||||
private static LateralPlanningResult FallbackOrFailure(LateralPath path, EmPlanningStatus failureStatus, string failureReason)
|
||||
{
|
||||
return path == null
|
||||
return failureStatus == EmPlanningStatus.Cancelled || path == null
|
||||
? Failed(failureStatus, failureReason)
|
||||
: new LateralPlanningResult(EmPlanningStatus.SuccessWithFallback, path, failureReason);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user