102 lines
5.2 KiB
C#
102 lines
5.2 KiB
C#
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);
|
|
}
|
|
}
|