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/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs
+38
View File
@@ -10,6 +10,8 @@ internal static class LongitudinalModelChecks
{
public static void Run()
{
VerifiesJerkLimitedStoppingProfileEndsAtRest();
VerifiesStoppedReachabilityUsesTheSameJerkModel();
VerifiesFinitePathSIndexedSpeedEnvelope();
VerifiesStoppingEnvelopeIsRefinedOnActualPathS();
VerifiesStoppingEnvelopeUsesDiscreteTimeTailStations();
@@ -18,6 +20,42 @@ internal static class LongitudinalModelChecks
VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints();
}
private static void VerifiesJerkLimitedStoppingProfileEndsAtRest()
{
Verification.True(JerkLimitedStoppingMath.TryCalculate(
0.20d, 0d, 0.30d, 0.50d,
out JerkLimitedStoppingProfile profile, out string failure),
"jerk-limited stop builds: " + failure);
Verification.True(profile.DistanceMeters > 0d, "stop distance is positive");
Verification.True(profile.DurationSeconds > 0d, "stop duration is positive");
Verification.NearlyEqual(0d, profile.FinalSpeedMetersPerSecond,
"stop ends at zero speed");
Verification.NearlyEqual(0d, profile.FinalAccelerationMetersPerSecondSquared,
"stop releases acceleration to zero");
Verification.True(JerkLimitedStoppingMath.TryCalculate(
0.20d, 0.20d, 0.30d, 0.50d,
out JerkLimitedStoppingProfile accelerating, out failure),
"positive-acceleration stop builds: " + failure);
Verification.True(accelerating.DistanceMeters > profile.DistanceMeters,
"positive initial acceleration needs more stopping distance");
Verification.NearlyEqual(0d, accelerating.FinalAccelerationMetersPerSecondSquared,
"positive-acceleration stop also releases acceleration");
}
private static void VerifiesStoppedReachabilityUsesTheSameJerkModel()
{
double maximumDistance = JerkLimitedStoppingMath.CalculateMaximumStoppedDistance(
0d, 0d, 0.20d, 0.20d, 0.30d, 0.50d, 2d);
Verification.True(maximumDistance > 0d && maximumDistance < 0.40d,
"two-second stopped reach is finite and below unconstrained cruise distance");
double cap = JerkLimitedStoppingMath.MaximumInitialSpeedForDistance(
maximumDistance, 0.20d, 0.30d, 0.50d, 0.20d);
Verification.True(cap >= 0d && cap <= 0.20d,
"distance inversion stays inside the direction speed range");
}
private static void VerifiesFinitePathSIndexedSpeedEnvelope()
{
LateralPath directionPath = CreatePath(new[]