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

fix: accelerate EM trajectories from rest

Browse Source
This commit is contained in:
梁薄云
2026-08-07 07:51:43 +08:00
parent 162f1a24e6
commit 0bba8d7e61
9 changed files with 273 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs
+16 -6
View File
@@ -32,6 +32,15 @@ public sealed class PathSpeedLimitBuilder
out speedLimit, out failureReason);
}
public EmPlanningStatus Build(LateralPath path, TravelDirection direction,
double initialProgressSpeedMetersPerSecond, double initialAccelerationMetersPerSecondSquared,
EmTerminalType terminalType, EmPlannerConfiguration configuration, out PathSpeedLimit speedLimit,
out string failureReason)
{
return BuildCore(path, direction, initialProgressSpeedMetersPerSecond,
initialAccelerationMetersPerSecondSquared, terminalType, configuration, out speedLimit, out failureReason);
}
private EmPlanningStatus BuildCore(LateralPath path, TravelDirection direction,
double initialProgressSpeedMetersPerSecond, double initialAccelerationMetersPerSecondSquared,
EmTerminalType terminalType, EmPlannerConfiguration configuration, out PathSpeedLimit speedLimit,
@@ -62,6 +71,7 @@ public sealed class PathSpeedLimitBuilder
double maximumJerk = longitudinal.MaximumJerkMetersPerSecondCubed;
double maximumLateralAcceleration = longitudinal.MaximumLateralAccelerationMetersPerSecondSquared;
double maximumCurvatureRate = longitudinal.MaximumCurvatureRatePerMeterPerSecond;
double stoppingAcceleration = Math.Max(0d, initialAccelerationMetersPerSecondSquared);
if (!IsPositiveFinite(directionMaximum) || !IsPositiveFinite(maximumAcceleration) ||
!IsPositiveFinite(maximumDeceleration) || !IsPositiveFinite(maximumJerk) ||
!IsPositiveFinite(maximumLateralAcceleration) || !IsPositiveFinite(maximumCurvatureRate))
@@ -118,7 +128,7 @@ public sealed class PathSpeedLimitBuilder
if (hasStopBoundary)
{
AddJerkLimitedStoppingStations(lowerPoint.PathS, upperPoint.PathS, stopBoundaryPathS,
directionMaximum, maximumAcceleration, maximumDeceleration, maximumJerk,
directionMaximum, stoppingAcceleration, maximumDeceleration, maximumJerk,
segmentIndex == 0, segmentStations);
}
segmentStations.Sort();
@@ -134,7 +144,7 @@ public sealed class PathSpeedLimitBuilder
double curvatureDerivative = Interpolate(lowerPoint.VehicleCurvatureDerivative,
upperPoint.VehicleCurvatureDerivative, fraction);
AddLimitSample(samplePathS, curvature, curvatureDerivative, hasStopBoundary,
stopBoundaryPathS, directionMaximum, maximumAcceleration, maximumDeceleration,
stopBoundaryPathS, directionMaximum, stoppingAcceleration, maximumDeceleration,
maximumJerk, maximumLateralAcceleration, maximumCurvatureRate, pathS, maximum, lateral,
curvatureRate, stopping);
}
@@ -188,14 +198,14 @@ public sealed class PathSpeedLimitBuilder
}
private static void AddJerkLimitedStoppingStations(double lowerPathS, double upperPathS,
double stopBoundaryPathS, double directionMaximum, double maximumAcceleration, double maximumDeceleration,
double stopBoundaryPathS, double directionMaximum, double stoppingAcceleration, double maximumDeceleration,
double maximumJerk, bool includeLower, IList<double> stations)
{
const int stoppingSpeedSampleCount = 64;
for (int step = 0; step < stoppingSpeedSampleCount; step++)
{
double speed = directionMaximum * step / stoppingSpeedSampleCount;
if (!JerkLimitedStoppingMath.TryCalculate(speed, maximumAcceleration,
if (!JerkLimitedStoppingMath.TryCalculate(speed, stoppingAcceleration,
maximumDeceleration, maximumJerk, out JerkLimitedStoppingProfile stop, out _))
{
throw new ArgumentException("The configured jerk-limited stop envelope cannot be sampled.");
@@ -210,7 +220,7 @@ public sealed class PathSpeedLimitBuilder
}
private static void AddLimitSample(double samplePathS, double curvature, double curvatureDerivative,
bool hasStopBoundary, double stopBoundaryPathS, double directionMaximum, double maximumAcceleration,
bool hasStopBoundary, double stopBoundaryPathS, double directionMaximum, double stoppingAcceleration,
double maximumDeceleration, double maximumJerk, double maximumLateralAcceleration,
double maximumCurvatureRate, IList<double> pathS, IList<double> maximum, IList<double> lateral,
IList<double> curvatureRate, IList<double> stopping)
@@ -220,7 +230,7 @@ public sealed class PathSpeedLimitBuilder
double curvatureRateLimit = maximumCurvatureRate / Math.Max(Math.Abs(curvatureDerivative), CurvatureEpsilon);
double stoppingLimit = hasStopBoundary
? JerkLimitedStoppingMath.MaximumInitialSpeedForDistance(
Math.Max(0d, stopBoundaryPathS - samplePathS), maximumAcceleration,
Math.Max(0d, stopBoundaryPathS - samplePathS), stoppingAcceleration,
maximumDeceleration, maximumJerk, directionMaximum)
: directionMaximum;
double lateralValue = ClampFinite(lateralLimit, directionMaximum);