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

feat: seed rolling and exact-stop ST profiles

Browse Source
This commit is contained in:
梁薄云
2026-08-05 18:18:49 +08:00
parent 26bd822ec9
commit 59d13e5783
3 changed files with 526 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalObjectiveBuilder.cs
+8 -4
View File
@@ -21,11 +21,12 @@ public sealed class LongitudinalObjectiveBuilder
double accelerationScale = RequirePositive(Math.Max(configuration.MaximumAccelerationMetersPerSecondSquared,
configuration.MaximumDecelerationMetersPerSecondSquared), nameof(accelerationScale));
double jerkScale = RequirePositive(configuration.MaximumJerkMetersPerSecondCubed, nameof(jerkScale));
double progressScale = input.TerminalPathS > 0d ? input.TerminalPathS : 1d;
double progressScale = input.PathUpperBoundS > 0d ? input.PathUpperBoundS : 1d;
for (int index = 0; index < layout.KnotCount; index++)
{
AddSquaredResidual(hessian, linearCost, layout.U(index), speedLimit.MaximumSpeedAt(iterate.S[index]),
double iteratePathS = Math.Max(0d, Math.Min(input.PathUpperBoundS, iterate.S[index]));
AddSquaredResidual(hessian, linearCost, layout.U(index), speedLimit.MaximumSpeedAt(iteratePathS),
weights.ReferenceSpeed, speedScale);
AddSquaredResidual(hessian, linearCost, layout.A(index), 0d, weights.Acceleration, accelerationScale);
if (index < layout.KnotCount - 1 && index < input.PreviousPathS.Count)
@@ -38,8 +39,11 @@ public sealed class LongitudinalObjectiveBuilder
}
for (int index = 0; index < layout.KnotCount - 1; index++)
AddSquaredResidual(hessian, linearCost, layout.J(index), 0d, weights.Jerk, jerkScale);
AddSquaredResidual(hessian, linearCost, layout.A(layout.KnotCount - 1), 0d, weights.TerminalAcceleration,
accelerationScale);
if (input.Mode != EmLongitudinalMode.ExactStopAtBoundary)
{
AddSquaredResidual(hessian, linearCost, layout.A(layout.KnotCount - 1), 0d,
weights.TerminalAcceleration, accelerationScale);
}
}
private static void AddSquaredResidual(SparseTripletBuilder hessian, IList<double> linearCost, int variable,