From 25742ab05260d474dc2d8ddd4e5da291c35d35c9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Tue, 4 Aug 2026 10:42:52 +0800 Subject: [PATCH] fix: refine EM stopping speed limits --- .../Longitudinal/PathSpeedLimitBuilder.cs | 121 +++++++++++++++--- .../LongitudinalModelChecks.cs | 67 +++++++++- 2 files changed, 165 insertions(+), 23 deletions(-) diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs index 6fa0d57..74dbf6c 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs @@ -8,6 +8,7 @@ public sealed class PathSpeedLimitBuilder { internal const double CurvatureEpsilon = 1e-10d; private const double StopDistanceToleranceMeters = 1e-8d; + private const double StationMergeToleranceMeters = 1e-12d; public EmPlanningStatus Build(LongitudinalPlanningInput input, out PathSpeedLimit speedLimit, out string failureReason) { @@ -40,28 +41,46 @@ public sealed class PathSpeedLimitBuilder failureReason = "The available actual PathS distance is insufficient for the jerk-limited stop."; return EmPlanningStatus.StoppingDistanceInsufficient; } - - int count = input.Path.Points.Count; - var pathS = new double[count]; - var maximum = new double[count]; - var lateral = new double[count]; - var curvatureRate = new double[count]; - var stopping = new double[count]; - for (int index = 0; index < count; index++) + if (input.Configuration.Scheduling == null || !IsPositiveFinite(input.Configuration.Scheduling.OutputTimeStepSeconds)) { - LateralPathPoint point = input.Path.Points[index]; - pathS[index] = point.PathS; - double lateralLimit = Math.Sqrt(maximumLateralAcceleration / - Math.Max(Math.Abs(point.VehicleCurvature), CurvatureEpsilon)); - double curvatureRateLimit = maximumCurvatureRate / - Math.Max(Math.Abs(point.VehicleCurvatureDerivative), CurvatureEpsilon); - double remainingDistance = Math.Max(0d, input.TerminalPathS - point.PathS); - double stoppingLimit = Math.Sqrt(2d * maximumDeceleration * remainingDistance); - lateral[index] = ClampFinite(lateralLimit, directionMaximum); - curvatureRate[index] = ClampFinite(curvatureRateLimit, directionMaximum); - stopping[index] = index == count - 1 ? 0d : ClampFinite(stoppingLimit, directionMaximum); - maximum[index] = index == count - 1 ? 0d : Math.Min(directionMaximum, - Math.Min(lateral[index], Math.Min(curvatureRate[index], stopping[index]))); + failureReason = "The output time step required to refine the PathS speed envelope is invalid."; + return EmPlanningStatus.InvalidInput; + } + + double maximumStationSpacing = directionMaximum * input.Configuration.Scheduling.OutputTimeStepSeconds; + var pathS = new List(); + var maximum = new List(); + var lateral = new List(); + var curvatureRate = new List(); + var stopping = new List(); + for (int segmentIndex = 0; segmentIndex < input.Path.Points.Count - 1; segmentIndex++) + { + LateralPathPoint lowerPoint = input.Path.Points[segmentIndex]; + LateralPathPoint upperPoint = input.Path.Points[segmentIndex + 1]; + double span = upperPoint.PathS - lowerPoint.PathS; + int subdivisions = Math.Max(1, checked((int)Math.Ceiling(span / maximumStationSpacing))); + var segmentStations = new List(subdivisions + 16); + for (int subdivision = segmentIndex == 0 ? 0 : 1; subdivision <= subdivisions; subdivision++) + segmentStations.Add(Interpolate(lowerPoint.PathS, upperPoint.PathS, (double)subdivision / subdivisions)); + AddDiscreteStoppingTailStations(lowerPoint.PathS, upperPoint.PathS, input.TerminalPathS, directionMaximum, + maximumDeceleration, maximumJerk, input.Configuration.Scheduling.OutputTimeStepSeconds, + segmentIndex == 0, segmentStations); + segmentStations.Sort(); + double previousStation = double.NegativeInfinity; + for (int stationIndex = 0; stationIndex < segmentStations.Count; stationIndex++) + { + double samplePathS = segmentStations[stationIndex]; + if (samplePathS <= previousStation + StationMergeToleranceMeters) + continue; + previousStation = samplePathS; + double fraction = (samplePathS - lowerPoint.PathS) / span; + double curvature = Interpolate(lowerPoint.VehicleCurvature, upperPoint.VehicleCurvature, fraction); + double curvatureDerivative = Interpolate(lowerPoint.VehicleCurvatureDerivative, + upperPoint.VehicleCurvatureDerivative, fraction); + AddLimitSample(samplePathS, curvature, curvatureDerivative, input.TerminalPathS, directionMaximum, + maximumDeceleration, maximumLateralAcceleration, maximumCurvatureRate, pathS, maximum, lateral, + curvatureRate, stopping); + } } try @@ -110,6 +129,66 @@ public sealed class PathSpeedLimitBuilder return true; } + private static void AddDiscreteStoppingTailStations(double lowerPathS, double upperPathS, double terminalPathS, + double directionMaximum, double maximumDeceleration, double maximumJerk, double timeStep, bool includeLower, + IList stations) + { + double speedIncrement = maximumDeceleration * timeStep; + int tailStationCount = Math.Max(1, checked((int)Math.Ceiling(directionMaximum / speedIncrement))); + for (int step = 1; step <= tailStationCount; step++) + { + double stopDuration = step * timeStep; + double remainingDistance = 0.5d * maximumDeceleration * stopDuration * stopDuration; + if (remainingDistance >= terminalPathS) + break; + double station = terminalPathS - remainingDistance; + bool aboveLower = includeLower + ? station >= lowerPathS - StationMergeToleranceMeters + : station > lowerPathS + StationMergeToleranceMeters; + if (aboveLower && station <= upperPathS + StationMergeToleranceMeters) + stations.Add(Math.Max(lowerPathS, Math.Min(upperPathS, station))); + } + int jerkTailStationCount = Math.Max(1, checked((int)Math.Ceiling( + maximumDeceleration / (maximumJerk * timeStep)))); + for (int step = 1; step <= jerkTailStationCount; step++) + { + double releaseDuration = step * timeStep; + double remainingDistance = maximumJerk * releaseDuration * releaseDuration * releaseDuration / 6d; + if (remainingDistance >= terminalPathS) + break; + double station = terminalPathS - remainingDistance; + bool aboveLower = includeLower + ? station >= lowerPathS - StationMergeToleranceMeters + : station > lowerPathS + StationMergeToleranceMeters; + if (aboveLower && station <= upperPathS + StationMergeToleranceMeters) + stations.Add(Math.Max(lowerPathS, Math.Min(upperPathS, station))); + } + } + + private static void AddLimitSample(double samplePathS, double curvature, double curvatureDerivative, double terminalPathS, + double directionMaximum, double maximumDeceleration, double maximumLateralAcceleration, double maximumCurvatureRate, + IList pathS, IList maximum, IList lateral, IList curvatureRate, IList stopping) + { + bool terminal = samplePathS >= terminalPathS; + double lateralLimit = Math.Sqrt(maximumLateralAcceleration / Math.Max(Math.Abs(curvature), CurvatureEpsilon)); + double curvatureRateLimit = maximumCurvatureRate / Math.Max(Math.Abs(curvatureDerivative), CurvatureEpsilon); + double stoppingLimit = Math.Sqrt(2d * maximumDeceleration * Math.Max(0d, terminalPathS - samplePathS)); + double lateralValue = ClampFinite(lateralLimit, directionMaximum); + double curvatureRateValue = ClampFinite(curvatureRateLimit, directionMaximum); + double stoppingValue = terminal ? 0d : ClampFinite(stoppingLimit, directionMaximum); + pathS.Add(samplePathS); + lateral.Add(lateralValue); + curvatureRate.Add(curvatureRateValue); + stopping.Add(stoppingValue); + maximum.Add(terminal ? 0d : Math.Min(directionMaximum, + Math.Min(lateralValue, Math.Min(curvatureRateValue, stoppingValue)))); + } + + private static double Interpolate(double lower, double upper, double fraction) + { + return lower + (upper - lower) * fraction; + } + private static double ClampFinite(double value, double maximum) { if (!IsFinite(value) || value < 0d) diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs index 26dea78..f607bf9 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs @@ -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(), Array.Empty()); + + 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(), Array.Empty()); + + 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();