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)