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/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs
+100 -21
View File
@@ -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<double>();
var maximum = new List<double>();
var lateral = new List<double>();
var curvatureRate = new List<double>();
var stopping = new List<double>();
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<double>(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<double> 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<double> pathS, IList<double> maximum, IList<double> lateral, IList<double> curvatureRate, IList<double> 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)
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();