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

fix: refine EM stopping speed limits

Browse Source
This commit is contained in:
梁薄云
2026-08-04 10:42:52 +08:00
parent c01d0d5b47
commit 25742ab052
2 changed files with 165 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs
+65 -2
View File
@@ -11,6 +11,8 @@ internal static class LongitudinalModelChecks
public static void Run()
{
VerifiesFinitePathSIndexedSpeedEnvelope();
VerifiesStoppingEnvelopeIsRefinedOnActualPathS();
VerifiesStoppingEnvelopeUsesDiscreteTimeTailStations();
VerifiesStoppingPrecheckBeforeQpAssembly();
VerifiesReferenceHorizonSelectionKeepsTheCurrentSegmentBoundary();
VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints();
@@ -58,11 +60,72 @@ internal static class LongitudinalModelChecks
"stopping speed limit");
Verification.NearlyEqual(Math.Sqrt(0.20d / 20d), envelope.MaximumSpeedAt(4d),
"combined limit chooses the finite minimum");
Verification.NearlyEqual((envelope.MaximumSpeedAt(0d) + envelope.MaximumSpeedAt(2d)) / 2d,
envelope.MaximumSpeedAt(1d), "speed envelope interpolates by PathS rather than ReferenceS");
double interpolationQueryPathS = 1.013d;
int upperStation = 1;
while (envelope.PathS[upperStation] < interpolationQueryPathS)
upperStation++;
double lowerPathS = envelope.PathS[upperStation - 1];
double upperPathS = envelope.PathS[upperStation];
double fraction = (interpolationQueryPathS - lowerPathS) / (upperPathS - lowerPathS);
double expectedInterpolatedSpeed = envelope.MaximumSpeedMetersPerSecond[upperStation - 1] +
(envelope.MaximumSpeedMetersPerSecond[upperStation] - envelope.MaximumSpeedMetersPerSecond[upperStation - 1]) *
fraction;
Verification.NearlyEqual(expectedInterpolatedSpeed, envelope.MaximumSpeedAt(interpolationQueryPathS),
"speed envelope interpolates by PathS rather than ReferenceS");
Verification.NearlyEqual(0d, envelope.MaximumSpeedAt(5d), "terminal speed is exactly zero");
}
private static void VerifiesStoppingEnvelopeIsRefinedOnActualPathS()
{
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
LateralPath path = CreatePath(new[]
{
new PathFixture(0d, 0d, 0d, 0d),
new PathFixture(100d, 2d, 0d, 0d),
});
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d,
EmTerminalType.Goal, configuration, Array.Empty<double>(), Array.Empty<double>());
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
out string failureReason);
Verification.Equal(EmPlanningStatus.Success, status, "refined stopping envelope status: " + failureReason);
Verification.True(envelope.PathS.Count > path.Points.Count, "stopping envelope inserts actual-PathS refinement stations");
Verification.NearlyEqual(Math.Sqrt(2d * 0.30d * (2d - 1.5d)), envelope.MaximumSpeedAt(1.5d),
"refined stopping envelope avoids a sparse terminal chord");
}
private static void VerifiesStoppingEnvelopeUsesDiscreteTimeTailStations()
{
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
LateralPath path = CreatePath(new[]
{
new PathFixture(0d, 0d, 0d, 0d),
new PathFixture(100d, 2d, 0d, 0d),
});
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d,
EmTerminalType.Goal, configuration, Array.Empty<double>(), Array.Empty<double>());
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
out string failureReason);
Verification.Equal(EmPlanningStatus.Success, status, "discrete stopping-tail status: " + failureReason);
double timeStep = configuration.Scheduling.OutputTimeStepSeconds;
double deceleration = configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared;
double firstTailDistance = 0.5d * deceleration * timeStep * timeStep;
double firstTailPathS = input.TerminalPathS - firstTailDistance;
Verification.NearlyEqual(deceleration * timeStep, envelope.StoppingLimitAt(firstTailPathS),
"first stopping-tail station matches one discrete deceleration time step");
double jerk = configuration.Longitudinal.MaximumJerkMetersPerSecondCubed;
double firstJerkTailDistance = jerk * timeStep * timeStep * timeStep / 6d;
double firstJerkTailPathS = input.TerminalPathS - firstJerkTailDistance;
Verification.NearlyEqual(Math.Sqrt(2d * deceleration * firstJerkTailDistance),
envelope.StoppingLimitAt(firstJerkTailPathS),
"first stopping-tail station matches one discrete jerk-release time step");
}
private static void VerifiesStoppingPrecheckBeforeQpAssembly()
{
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();