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/PathSpeedLimitBuilder.cs
+6 -61
View File
@@ -34,8 +34,12 @@ public sealed class PathSpeedLimitBuilder
return EmPlanningStatus.InvalidInput;
}
LongitudinalStoppingProfile stopProfile = LongitudinalStoppingMath.Calculate(input.InitialProgressSpeedMetersPerSecond,
input.InitialAccelerationMetersPerSecondSquared, maximumDeceleration, maximumJerk);
if (!JerkLimitedStoppingMath.TryCalculate(input.InitialProgressSpeedMetersPerSecond,
input.InitialAccelerationMetersPerSecondSquared, maximumDeceleration, maximumJerk,
out JerkLimitedStoppingProfile stopProfile, out failureReason))
{
return EmPlanningStatus.InvalidInput;
}
if (stopProfile.DistanceMeters + StopDistanceToleranceMeters > input.TerminalPathS)
{
failureReason = "The available actual PathS distance is insufficient for the jerk-limited stop.";
@@ -206,62 +210,3 @@ public sealed class PathSpeedLimitBuilder
return !double.IsNaN(value) && !double.IsInfinity(value);
}
}
internal sealed class LongitudinalStoppingProfile
{
public LongitudinalStoppingProfile(double distanceMeters, double durationSeconds)
{
DistanceMeters = distanceMeters;
DurationSeconds = durationSeconds;
}
public double DistanceMeters { get; }
public double DurationSeconds { get; }
}
internal static class LongitudinalStoppingMath
{
public static LongitudinalStoppingProfile Calculate(double speedMetersPerSecond, double accelerationMetersPerSecondSquared,
double maximumDecelerationMetersPerSecondSquared, double maximumJerkMetersPerSecondCubed)
{
if (!IsFinite(speedMetersPerSecond) || !IsFinite(accelerationMetersPerSecondSquared) ||
!IsPositiveFinite(maximumDecelerationMetersPerSecondSquared) || !IsPositiveFinite(maximumJerkMetersPerSecondCubed))
{
throw new ArgumentOutOfRangeException(nameof(speedMetersPerSecond));
}
if (speedMetersPerSecond <= 0d)
return new LongitudinalStoppingProfile(0d, 0d);
double acceleration = Math.Max(-maximumDecelerationMetersPerSecondSquared, accelerationMetersPerSecondSquared);
double rampDuration = (acceleration + maximumDecelerationMetersPerSecondSquared) / maximumJerkMetersPerSecondCubed;
double speedAfterRamp = speedMetersPerSecond + acceleration * rampDuration -
0.5d * maximumJerkMetersPerSecondCubed * rampDuration * rampDuration;
if (speedAfterRamp <= 0d)
{
double root = (acceleration + Math.Sqrt(acceleration * acceleration + 2d * maximumJerkMetersPerSecondCubed *
speedMetersPerSecond)) / maximumJerkMetersPerSecondCubed;
double distance = speedMetersPerSecond * root + 0.5d * acceleration * root * root -
maximumJerkMetersPerSecondCubed * root * root * root / 6d;
return new LongitudinalStoppingProfile(Math.Max(0d, distance), root);
}
double rampDistance = speedMetersPerSecond * rampDuration + 0.5d * acceleration * rampDuration * rampDuration -
maximumJerkMetersPerSecondCubed * rampDuration * rampDuration * rampDuration / 6d;
double constantDecelerationDuration = speedAfterRamp / maximumDecelerationMetersPerSecondSquared;
double constantDecelerationDistance = speedAfterRamp * speedAfterRamp /
(2d * maximumDecelerationMetersPerSecondSquared);
return new LongitudinalStoppingProfile(rampDistance + constantDecelerationDistance,
rampDuration + constantDecelerationDuration);
}
private static bool IsPositiveFinite(double value)
{
return IsFinite(value) && value > 0d;
}
private static bool IsFinite(double value)
{
return !double.IsNaN(value) && !double.IsInfinity(value);
}
}