269 lines
14 KiB
C#
269 lines
14 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
/// <summary>Builds curvature-aware, stopping-aware speed limits over actual optimized PathS.</summary>
|
|
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)
|
|
{
|
|
if (input == null)
|
|
{
|
|
speedLimit = null;
|
|
failureReason = "Longitudinal planning input is required.";
|
|
return EmPlanningStatus.InvalidInput;
|
|
}
|
|
return BuildCore(input.Path, input.Direction, input.InitialProgressSpeedMetersPerSecond,
|
|
input.InitialAccelerationMetersPerSecondSquared, input.TerminalType, input.Configuration,
|
|
out speedLimit, out failureReason);
|
|
}
|
|
|
|
public EmPlanningStatus Build(LateralPath path, TravelDirection direction,
|
|
double initialProgressSpeedMetersPerSecond, EmTerminalType terminalType,
|
|
EmPlannerConfiguration configuration, out PathSpeedLimit speedLimit, out string failureReason)
|
|
{
|
|
return BuildCore(path, direction, initialProgressSpeedMetersPerSecond, 0d, terminalType, configuration,
|
|
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,
|
|
out string failureReason)
|
|
{
|
|
speedLimit = null;
|
|
failureReason = string.Empty;
|
|
if (path == null || !path.IsIndependentlyValidated || path.Points.Count < 2 ||
|
|
!Enum.IsDefined(typeof(TravelDirection), direction) || !Enum.IsDefined(typeof(EmTerminalType), terminalType) ||
|
|
configuration == null || !IsFinite(initialProgressSpeedMetersPerSecond) ||
|
|
initialProgressSpeedMetersPerSecond < 0d || !IsFinite(initialAccelerationMetersPerSecondSquared))
|
|
{
|
|
failureReason = "Longitudinal planning input is required.";
|
|
return EmPlanningStatus.InvalidInput;
|
|
}
|
|
|
|
if (configuration.Longitudinal == null)
|
|
{
|
|
failureReason = "Longitudinal configuration is required.";
|
|
return EmPlanningStatus.InvalidInput;
|
|
}
|
|
LongitudinalConfiguration longitudinal = configuration.Longitudinal;
|
|
double directionMaximum = direction == TravelDirection.Forward
|
|
? longitudinal.MaximumForwardSpeedMetersPerSecond
|
|
: longitudinal.MaximumReverseSpeedMetersPerSecond;
|
|
double maximumAcceleration = longitudinal.MaximumAccelerationMetersPerSecondSquared;
|
|
double maximumDeceleration = longitudinal.MaximumDecelerationMetersPerSecondSquared;
|
|
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))
|
|
{
|
|
failureReason = "Longitudinal limits must be positive and finite.";
|
|
return EmPlanningStatus.InvalidInput;
|
|
}
|
|
if (initialProgressSpeedMetersPerSecond > directionMaximum + StopDistanceToleranceMeters ||
|
|
initialAccelerationMetersPerSecondSquared < -maximumDeceleration - StopDistanceToleranceMeters ||
|
|
initialAccelerationMetersPerSecondSquared > maximumAcceleration + StopDistanceToleranceMeters)
|
|
{
|
|
failureReason = "The initial longitudinal state violates the configured hard bounds.";
|
|
return EmPlanningStatus.InvalidInput;
|
|
}
|
|
|
|
bool hasStopBoundary = terminalType != EmTerminalType.RollingSafetyStop;
|
|
double stopBoundaryPathS = path.Points[path.Points.Count - 1].PathS;
|
|
if (hasStopBoundary)
|
|
{
|
|
if (!JerkLimitedStoppingMath.TryCalculate(initialProgressSpeedMetersPerSecond,
|
|
initialAccelerationMetersPerSecondSquared, maximumDeceleration, maximumJerk,
|
|
out JerkLimitedStoppingProfile stopProfile, out failureReason))
|
|
{
|
|
return EmPlanningStatus.InvalidInput;
|
|
}
|
|
if (stopProfile.DistanceMeters + StopDistanceToleranceMeters > stopBoundaryPathS)
|
|
{
|
|
failureReason = "The available actual PathS distance is insufficient for the jerk-limited stop.";
|
|
return EmPlanningStatus.StoppingDistanceInsufficient;
|
|
}
|
|
}
|
|
if (configuration.Scheduling == null ||
|
|
!IsPositiveFinite(configuration.Scheduling.MaximumOptimizationSpatialStepMeters))
|
|
{
|
|
failureReason = "The optimization spatial step required to refine the PathS speed envelope is invalid.";
|
|
return EmPlanningStatus.InvalidInput;
|
|
}
|
|
|
|
double maximumStationSpacing = configuration.Scheduling.MaximumOptimizationSpatialStepMeters;
|
|
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 < path.Points.Count - 1; segmentIndex++)
|
|
{
|
|
LateralPathPoint lowerPoint = path.Points[segmentIndex];
|
|
LateralPathPoint upperPoint = 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));
|
|
if (hasStopBoundary)
|
|
{
|
|
AddJerkLimitedStoppingStations(lowerPoint.PathS, upperPoint.PathS, stopBoundaryPathS,
|
|
directionMaximum, stoppingAcceleration, maximumDeceleration, maximumJerk,
|
|
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, hasStopBoundary,
|
|
stopBoundaryPathS, directionMaximum, stoppingAcceleration, maximumDeceleration,
|
|
maximumJerk, maximumLateralAcceleration, maximumCurvatureRate, pathS, maximum, lateral,
|
|
curvatureRate, stopping);
|
|
}
|
|
}
|
|
|
|
try
|
|
{
|
|
speedLimit = new PathSpeedLimit(pathS, maximum, lateral, curvatureRate, stopping, directionMaximum,
|
|
hasStopBoundary);
|
|
return EmPlanningStatus.Success;
|
|
}
|
|
catch (ArgumentException exception)
|
|
{
|
|
failureReason = exception.Message;
|
|
return EmPlanningStatus.InvalidInput;
|
|
}
|
|
}
|
|
|
|
internal static bool TryGetLimits(LongitudinalPlanningInput input, out double directionMaximum,
|
|
out double maximumAcceleration, out double maximumDeceleration, out double maximumJerk,
|
|
out double maximumLateralAcceleration, out double maximumCurvatureRate, out string failureReason)
|
|
{
|
|
directionMaximum = 0d;
|
|
maximumAcceleration = 0d;
|
|
maximumDeceleration = 0d;
|
|
maximumJerk = 0d;
|
|
maximumLateralAcceleration = 0d;
|
|
maximumCurvatureRate = 0d;
|
|
failureReason = string.Empty;
|
|
if (input.Configuration == null || input.Configuration.Longitudinal == null)
|
|
{
|
|
failureReason = "Longitudinal configuration is required.";
|
|
return false;
|
|
}
|
|
|
|
LongitudinalConfiguration configuration = input.Configuration.Longitudinal;
|
|
directionMaximum = input.DirectionMaximumSpeedMetersPerSecond;
|
|
maximumAcceleration = configuration.MaximumAccelerationMetersPerSecondSquared;
|
|
maximumDeceleration = configuration.MaximumDecelerationMetersPerSecondSquared;
|
|
maximumJerk = configuration.MaximumJerkMetersPerSecondCubed;
|
|
maximumLateralAcceleration = configuration.MaximumLateralAccelerationMetersPerSecondSquared;
|
|
maximumCurvatureRate = configuration.MaximumCurvatureRatePerMeterPerSecond;
|
|
if (!IsPositiveFinite(directionMaximum) || !IsPositiveFinite(maximumAcceleration) ||
|
|
!IsPositiveFinite(maximumDeceleration) || !IsPositiveFinite(maximumJerk) ||
|
|
!IsPositiveFinite(maximumLateralAcceleration) || !IsPositiveFinite(maximumCurvatureRate))
|
|
{
|
|
failureReason = "Longitudinal limits must be positive and finite.";
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
private static void AddJerkLimitedStoppingStations(double lowerPathS, double upperPathS,
|
|
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, stoppingAcceleration,
|
|
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;
|
|
if (aboveLower && station <= upperPathS + StationMergeToleranceMeters)
|
|
stations.Add(Math.Max(lowerPathS, Math.Min(upperPathS, station)));
|
|
}
|
|
}
|
|
|
|
private static void AddLimitSample(double samplePathS, double curvature, double curvatureDerivative,
|
|
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)
|
|
{
|
|
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 = hasStopBoundary
|
|
? JerkLimitedStoppingMath.MaximumInitialSpeedForDistance(
|
|
Math.Max(0d, stopBoundaryPathS - samplePathS), stoppingAcceleration,
|
|
maximumDeceleration, maximumJerk, directionMaximum)
|
|
: directionMaximum;
|
|
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)
|
|
throw new ArgumentOutOfRangeException(nameof(value));
|
|
return Math.Min(maximum, value);
|
|
}
|
|
|
|
private static bool IsPositiveFinite(double value)
|
|
{
|
|
return IsFinite(value) && value > 0d;
|
|
}
|
|
|
|
private static bool IsFinite(double value)
|
|
{
|
|
return !double.IsNaN(value) && !double.IsInfinity(value);
|
|
}
|
|
}
|