First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

chore: save current workspace progress

Browse Source
This commit is contained in:
梁薄云
2026-08-09 22:13:18 +08:00
parent 650c2ab0e3
commit 2f4fd15e52
449 changed files with 76593 additions and 971 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralObjectiveBuilder.cs
+17 -91
View File
@@ -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; }
}
}