213 lines
11 KiB
C#
213 lines
11 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
|
|
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)
|
|
{
|
|
speedLimit = null;
|
|
failureReason = string.Empty;
|
|
if (input == null)
|
|
{
|
|
failureReason = "Longitudinal planning input is required.";
|
|
return EmPlanningStatus.InvalidInput;
|
|
}
|
|
|
|
if (!TryGetLimits(input, out double directionMaximum, out double maximumAcceleration, out double maximumDeceleration,
|
|
out double maximumJerk, out double maximumLateralAcceleration, out double maximumCurvatureRate,
|
|
out failureReason))
|
|
{
|
|
return EmPlanningStatus.InvalidInput;
|
|
}
|
|
if (input.InitialProgressSpeedMetersPerSecond > directionMaximum + StopDistanceToleranceMeters ||
|
|
input.InitialAccelerationMetersPerSecondSquared < -maximumDeceleration - StopDistanceToleranceMeters ||
|
|
input.InitialAccelerationMetersPerSecondSquared > maximumAcceleration + StopDistanceToleranceMeters)
|
|
{
|
|
failureReason = "The initial longitudinal state violates the configured hard bounds.";
|
|
return EmPlanningStatus.InvalidInput;
|
|
}
|
|
|
|
if (!JerkLimitedStoppingMath.TryCalculate(input.InitialProgressSpeedMetersPerSecond,
|
|
input.InitialAccelerationMetersPerSecondSquared, maximumDeceleration, maximumJerk,
|
|
out JerkLimitedStoppingProfile stopProfile, out failureReason))
|
|
{
|
|
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 (input.Configuration.Scheduling == null || !IsPositiveFinite(input.Configuration.Scheduling.OutputTimeStepSeconds))
|
|
{
|
|
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
|
|
{
|
|
speedLimit = new PathSpeedLimit(pathS, maximum, lateral, curvatureRate, stopping, directionMaximum);
|
|
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 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)
|
|
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);
|
|
}
|
|
}
|