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

feat: add complete jerk-limited stopping math

Browse Source
This commit is contained in:
梁薄云
2026-08-05 17:07:54 +08:00
parent 6c2f4066c8
commit e56220fc29
5 changed files with 268 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs
+7 -3
View File
@@ -205,9 +205,13 @@ public sealed class SequentialLongitudinalOptimizer
double horizon = times[knotCount - 1];
double requestedSpeed = Math.Min(input.DirectionMaximumSpeedMetersPerSecond,
Math.Max(0d, input.TerminalPathS / horizon));
LongitudinalStoppingProfile terminalStop = LongitudinalStoppingMath.Calculate(requestedSpeed, 0d,
input.Configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared,
input.Configuration.Longitudinal.MaximumJerkMetersPerSecondCubed);
if (!JerkLimitedStoppingMath.TryCalculate(requestedSpeed, 0d,
input.Configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared,
input.Configuration.Longitudinal.MaximumJerkMetersPerSecondCubed,
out JerkLimitedStoppingProfile terminalStop, out _))
{
throw new ArgumentOutOfRangeException(nameof(input));
}
double cruiseDistance = Math.Max(0d, input.TerminalPathS - terminalStop.DistanceMeters);
for (int index = 0; index < knotCount; index++)
{