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

feat: keep rolling speed envelopes open

Browse Source
This commit is contained in:
梁薄云
2026-08-05 17:33:50 +08:00
parent 105ec4bdad
commit 94a9be9c02
3 changed files with 132 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimit.cs
+14 -7
View File
@@ -11,7 +11,7 @@ public sealed class PathSpeedLimit
internal PathSpeedLimit(IReadOnlyList<double> pathS, IReadOnlyList<double> maximumSpeed,
IReadOnlyList<double> lateralAccelerationLimit, IReadOnlyList<double> curvatureRateLimit,
IReadOnlyList<double> stoppingLimit, double directionMaximumSpeedMetersPerSecond)
IReadOnlyList<double> stoppingLimit, double directionMaximumSpeedMetersPerSecond, bool hasStopBoundary)
{
PathS = CopyStrictStations(pathS, nameof(pathS));
MaximumSpeedMetersPerSecond = CopyFiniteNonnegative(maximumSpeed, PathS.Count, nameof(maximumSpeed));
@@ -22,13 +22,15 @@ public sealed class PathSpeedLimit
StoppingSpeedLimitsMetersPerSecond = CopyFiniteNonnegative(stoppingLimit, PathS.Count, nameof(stoppingLimit));
if (!IsFinite(directionMaximumSpeedMetersPerSecond) || directionMaximumSpeedMetersPerSecond <= 0d)
throw new ArgumentOutOfRangeException(nameof(directionMaximumSpeedMetersPerSecond));
if (MaximumSpeedMetersPerSecond[MaximumSpeedMetersPerSecond.Count - 1] != 0d ||
StoppingSpeedLimitsMetersPerSecond[StoppingSpeedLimitsMetersPerSecond.Count - 1] != 0d)
if (hasStopBoundary &&
(MaximumSpeedMetersPerSecond[MaximumSpeedMetersPerSecond.Count - 1] != 0d ||
StoppingSpeedLimitsMetersPerSecond[StoppingSpeedLimitsMetersPerSecond.Count - 1] != 0d))
{
throw new ArgumentException("Terminal PathS speed limits must be exactly zero.");
throw new ArgumentException("A real stop boundary must have an exact zero speed limit.");
}
DirectionMaximumSpeedMetersPerSecond = directionMaximumSpeedMetersPerSecond;
HasStopBoundary = hasStopBoundary;
}
public IReadOnlyList<double> PathS { get; }
@@ -43,7 +45,12 @@ public sealed class PathSpeedLimit
public double DirectionMaximumSpeedMetersPerSecond { get; }
public double TerminalPathS { get { return PathS[PathS.Count - 1]; } }
public bool HasStopBoundary { get; }
public double PathUpperBoundS { get { return PathS[PathS.Count - 1]; } }
[Obsolete("Use PathUpperBoundS.")]
public double TerminalPathS { get { return PathUpperBoundS; } }
public double MaximumSpeedAt(double pathS)
{
@@ -67,11 +74,11 @@ public sealed class PathSpeedLimit
private double Interpolate(IReadOnlyList<double> values, double pathS)
{
if (!IsFinite(pathS) || pathS < PathS[0] - StationTolerance || pathS > TerminalPathS + StationTolerance)
if (!IsFinite(pathS) || pathS < PathS[0] - StationTolerance || pathS > PathUpperBoundS + StationTolerance)
throw new ArgumentOutOfRangeException(nameof(pathS));
if (pathS <= PathS[0])
return values[0];
if (pathS >= TerminalPathS)
if (pathS >= PathUpperBoundS)
return values[values.Count - 1];
for (int index = 1; index < PathS.Count; index++)
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs
+46 -46
View File
@@ -34,16 +34,19 @@ public sealed class PathSpeedLimitBuilder
return EmPlanningStatus.InvalidInput;
}
if (!JerkLimitedStoppingMath.TryCalculate(input.InitialProgressSpeedMetersPerSecond,
input.InitialAccelerationMetersPerSecondSquared, maximumDeceleration, maximumJerk,
out JerkLimitedStoppingProfile stopProfile, out failureReason))
if (input.HasStopBoundary)
{
return EmPlanningStatus.InvalidInput;
}
if (stopProfile.DistanceMeters + StopDistanceToleranceMeters > input.TerminalPathS)
{
failureReason = "The available actual PathS distance is insufficient for the jerk-limited stop.";
return EmPlanningStatus.StoppingDistanceInsufficient;
if (!JerkLimitedStoppingMath.TryCalculate(input.InitialProgressSpeedMetersPerSecond,
input.InitialAccelerationMetersPerSecondSquared, maximumDeceleration, maximumJerk,
out JerkLimitedStoppingProfile stopProfile, out failureReason))
{
return EmPlanningStatus.InvalidInput;
}
if (stopProfile.DistanceMeters + StopDistanceToleranceMeters > input.StopBoundaryPathS)
{
failureReason = "The available actual PathS distance is insufficient for the jerk-limited stop.";
return EmPlanningStatus.StoppingDistanceInsufficient;
}
}
if (input.Configuration.Scheduling == null || !IsPositiveFinite(input.Configuration.Scheduling.OutputTimeStepSeconds))
{
@@ -66,9 +69,12 @@ public sealed class PathSpeedLimitBuilder
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);
if (input.HasStopBoundary)
{
AddJerkLimitedStoppingStations(lowerPoint.PathS, upperPoint.PathS, input.StopBoundaryPathS,
directionMaximum, maximumAcceleration, maximumDeceleration, maximumJerk,
segmentIndex == 0, segmentStations);
}
segmentStations.Sort();
double previousStation = double.NegativeInfinity;
for (int stationIndex = 0; stationIndex < segmentStations.Count; stationIndex++)
@@ -81,15 +87,17 @@ public sealed class PathSpeedLimitBuilder
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,
AddLimitSample(samplePathS, curvature, curvatureDerivative, input.HasStopBoundary,
input.StopBoundaryPathS, directionMaximum, maximumAcceleration, maximumDeceleration,
maximumJerk, maximumLateralAcceleration, maximumCurvatureRate, pathS, maximum, lateral,
curvatureRate, stopping);
}
}
try
{
speedLimit = new PathSpeedLimit(pathS, maximum, lateral, curvatureRate, stopping, directionMaximum);
speedLimit = new PathSpeedLimit(pathS, maximum, lateral, curvatureRate, stopping, directionMaximum,
input.HasStopBoundary);
return EmPlanningStatus.Success;
}
catch (ArgumentException exception)
@@ -133,34 +141,20 @@ 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)
private static void AddJerkLimitedStoppingStations(double lowerPathS, double upperPathS,
double stopBoundaryPathS, double directionMaximum, double maximumAcceleration, double maximumDeceleration,
double maximumJerk, 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++)
const int stoppingSpeedSampleCount = 64;
for (int step = 0; step < stoppingSpeedSampleCount; 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;
double speed = directionMaximum * step / stoppingSpeedSampleCount;
if (!JerkLimitedStoppingMath.TryCalculate(speed, maximumAcceleration,
maximumDeceleration, maximumJerk, out JerkLimitedStoppingProfile stop, out _))
{
throw new ArgumentException("The configured jerk-limited stop envelope cannot be sampled.");
}
double station = stopBoundaryPathS - stop.DistanceMeters;
bool aboveLower = includeLower
? station >= lowerPathS - StationMergeToleranceMeters
: station > lowerPathS + StationMergeToleranceMeters;
@@ -169,14 +163,20 @@ public sealed class PathSpeedLimitBuilder
}
}
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)
private static void AddLimitSample(double samplePathS, double curvature, double curvatureDerivative,
bool hasStopBoundary, double stopBoundaryPathS, double directionMaximum, double maximumAcceleration,
double maximumDeceleration, double maximumJerk, double maximumLateralAcceleration,
double maximumCurvatureRate, IList<double> pathS, IList<double> maximum, IList<double> lateral,
IList<double> curvatureRate, IList<double> stopping)
{
bool terminal = samplePathS >= terminalPathS;
bool terminal = hasStopBoundary && samplePathS >= stopBoundaryPathS;
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 stoppingLimit = hasStopBoundary
? JerkLimitedStoppingMath.MaximumInitialSpeedForDistance(
Math.Max(0d, stopBoundaryPathS - samplePathS), maximumAcceleration,
maximumDeceleration, maximumJerk, directionMaximum)
: directionMaximum;
double lateralValue = ClampFinite(lateralLimit, directionMaximum);
double curvatureRateValue = ClampFinite(curvatureRateLimit, directionMaximum);
double stoppingValue = terminal ? 0d : ClampFinite(stoppingLimit, directionMaximum);