210 lines
12 KiB
C#
210 lines
12 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
/// <summary>Derives bounded full-direction ST knots from the physical PathS speed and stopping envelope.</summary>
|
|
public sealed class FullDirectionSegmentScheduleBuilder
|
|
{
|
|
private const double Tolerance = 1e-10d;
|
|
|
|
public EmPlanningStatus TryBuild(LateralPath path, PathSpeedLimit speedLimit,
|
|
double initialProgressSpeedMetersPerSecond, double initialAccelerationMetersPerSecondSquared,
|
|
double desiredSpeedMetersPerSecond, EmPlannerConfiguration configuration,
|
|
out LongitudinalKnotSchedule schedule, out string failureReason)
|
|
{
|
|
schedule = null;
|
|
failureReason = string.Empty;
|
|
if (path == null || speedLimit == null || configuration == null || configuration.Scheduling == null ||
|
|
configuration.Longitudinal == null || !path.IsIndependentlyValidated || path.Points.Count < 2 ||
|
|
!IsFinite(initialProgressSpeedMetersPerSecond) || initialProgressSpeedMetersPerSecond < 0d ||
|
|
!IsFinite(initialAccelerationMetersPerSecondSquared) || !IsPositiveFinite(desiredSpeedMetersPerSecond))
|
|
{
|
|
failureReason = "Full-direction schedule inputs are invalid.";
|
|
return EmPlanningStatus.InvalidInput;
|
|
}
|
|
if (!speedLimit.HasStopBoundary || Math.Abs(speedLimit.PathUpperBoundS -
|
|
path.Points[path.Points.Count - 1].PathS) > Tolerance)
|
|
{
|
|
failureReason = "A full-direction schedule requires the matching real stop-boundary speed envelope.";
|
|
return EmPlanningStatus.InvalidInput;
|
|
}
|
|
|
|
SchedulingConfiguration scheduling = configuration.Scheduling;
|
|
LongitudinalConfiguration longitudinal = configuration.Longitudinal;
|
|
if (!IsPositiveFinite(scheduling.MaximumOptimizationTimeStepSeconds) ||
|
|
!IsPositiveFinite(scheduling.MaximumOptimizationSpatialStepMeters) ||
|
|
scheduling.MaximumOptimizationKnotCount < 3 ||
|
|
!IsPositiveFinite(longitudinal.MaximumAccelerationMetersPerSecondSquared) ||
|
|
!IsPositiveFinite(longitudinal.MaximumDecelerationMetersPerSecondSquared) ||
|
|
!IsPositiveFinite(longitudinal.MaximumJerkMetersPerSecondCubed))
|
|
{
|
|
failureReason = "Full-direction schedule limits are invalid.";
|
|
return EmPlanningStatus.InvalidInput;
|
|
}
|
|
|
|
int stationCount = speedLimit.PathS.Count;
|
|
var speeds = new double[stationCount];
|
|
double desired = Math.Min(desiredSpeedMetersPerSecond, speedLimit.DirectionMaximumSpeedMetersPerSecond);
|
|
speeds[0] = Math.Min(initialProgressSpeedMetersPerSecond, Math.Min(desired,
|
|
speedLimit.MaximumSpeedMetersPerSecond[0]));
|
|
for (int index = 1; index < stationCount; index++)
|
|
{
|
|
double distance = speedLimit.PathS[index] - speedLimit.PathS[index - 1];
|
|
double reachable = Math.Sqrt(Math.Max(0d, speeds[index - 1] * speeds[index - 1] +
|
|
2d * longitudinal.MaximumAccelerationMetersPerSecondSquared * distance));
|
|
speeds[index] = Math.Min(reachable, Math.Min(desired, speedLimit.MaximumSpeedMetersPerSecond[index]));
|
|
}
|
|
speeds[stationCount - 1] = 0d;
|
|
for (int index = stationCount - 2; index >= 0; index--)
|
|
{
|
|
double remainingDistance = speedLimit.PathUpperBoundS - speedLimit.PathS[index];
|
|
double stopCap = JerkLimitedStoppingMath.MaximumInitialSpeedForDistance(remainingDistance,
|
|
Math.Max(0d, initialAccelerationMetersPerSecondSquared), longitudinal.MaximumDecelerationMetersPerSecondSquared,
|
|
longitudinal.MaximumJerkMetersPerSecondCubed, speedLimit.DirectionMaximumSpeedMetersPerSecond);
|
|
double distance = speedLimit.PathS[index + 1] - speedLimit.PathS[index];
|
|
double decelerationCap = Math.Sqrt(Math.Max(0d, speeds[index + 1] * speeds[index + 1] +
|
|
2d * longitudinal.MaximumDecelerationMetersPerSecondSquared * distance));
|
|
speeds[index] = Math.Min(speeds[index], Math.Min(stopCap, decelerationCap));
|
|
}
|
|
|
|
var times = new List<double> { 0d };
|
|
var pathS = new List<double> { 0d };
|
|
var referenceSpeeds = new List<double> { speeds[0] };
|
|
IReadOnlyList<int> scheduleStations = SelectScheduleStations(speedLimit.PathS, speeds);
|
|
int minimumIntervalsPerSegment = Math.Max(1,
|
|
(3 + scheduleStations.Count - 2) / (scheduleStations.Count - 1));
|
|
for (int stationIndex = 1; stationIndex < scheduleStations.Count; stationIndex++)
|
|
{
|
|
int startIndex = scheduleStations[stationIndex - 1];
|
|
int endIndex = scheduleStations[stationIndex];
|
|
double startS = speedLimit.PathS[startIndex];
|
|
double endS = speedLimit.PathS[endIndex];
|
|
double startSpeed = speeds[startIndex];
|
|
double endSpeed = speeds[endIndex];
|
|
double distance = endS - startS;
|
|
double denominator = startSpeed + endSpeed;
|
|
double duration = denominator > Tolerance ? 2d * distance / denominator :
|
|
Math.Sqrt(2d * distance / Math.Max(Tolerance, longitudinal.MaximumAccelerationMetersPerSecondSquared));
|
|
int subdivisionCount = Math.Max(minimumIntervalsPerSegment, Math.Max(
|
|
checked((int)Math.Ceiling(distance / scheduling.MaximumOptimizationSpatialStepMeters)),
|
|
checked((int)Math.Ceiling(duration / scheduling.MaximumOptimizationTimeStepSeconds))));
|
|
for (int subdivision = 1; subdivision <= subdivisionCount; subdivision++)
|
|
{
|
|
double fraction = (double)subdivision / subdivisionCount;
|
|
times.Add(times[times.Count - 1] + duration / subdivisionCount);
|
|
pathS.Add(startS + distance * fraction);
|
|
referenceSpeeds.Add(startSpeed + (endSpeed - startSpeed) * fraction);
|
|
}
|
|
}
|
|
referenceSpeeds[referenceSpeeds.Count - 1] = 0d;
|
|
pathS[pathS.Count - 1] = speedLimit.PathUpperBoundS;
|
|
EnsureJerkReachableReferenceTimes(times, referenceSpeeds, longitudinal);
|
|
EnsureMinimumExactStopDuration(times, speedLimit.PathUpperBoundS, initialProgressSpeedMetersPerSecond,
|
|
initialAccelerationMetersPerSecondSquared, longitudinal);
|
|
if (!IsFinite(longitudinal.ZeroSpeedHoldSeconds) || longitudinal.ZeroSpeedHoldSeconds < 0d)
|
|
{
|
|
failureReason = "The full-direction zero-speed hold duration is invalid.";
|
|
return EmPlanningStatus.InvalidInput;
|
|
}
|
|
int terminalHoldStartIndex = times.Count - 1;
|
|
double remainingHold = longitudinal.ZeroSpeedHoldSeconds;
|
|
while (remainingHold > Tolerance)
|
|
{
|
|
double holdStep = Math.Min(remainingHold, scheduling.MaximumOptimizationTimeStepSeconds);
|
|
times.Add(times[times.Count - 1] + holdStep);
|
|
pathS.Add(speedLimit.PathUpperBoundS);
|
|
referenceSpeeds.Add(0d);
|
|
remainingHold -= holdStep;
|
|
}
|
|
if (times.Count > scheduling.MaximumOptimizationKnotCount)
|
|
{
|
|
failureReason = "Full-direction schedule required knots=" + times.Count + ", configured maximum=" +
|
|
scheduling.MaximumOptimizationKnotCount + ".";
|
|
return EmPlanningStatus.FullSegmentResourceLimitExceeded;
|
|
}
|
|
try
|
|
{
|
|
schedule = LongitudinalKnotSchedule.CreateAdaptive(times, pathS, referenceSpeeds,
|
|
terminalHoldStartIndex);
|
|
return EmPlanningStatus.Success;
|
|
}
|
|
catch (ArgumentException exception)
|
|
{
|
|
failureReason = exception.Message;
|
|
return EmPlanningStatus.InvalidInput;
|
|
}
|
|
}
|
|
|
|
private static void EnsureMinimumExactStopDuration(IList<double> times, double stopBoundaryPathS,
|
|
double initialSpeed, double initialAcceleration, LongitudinalConfiguration configuration)
|
|
{
|
|
if (initialSpeed <= Tolerance || !JerkLimitedStoppingMath.TryCalculate(initialSpeed, initialAcceleration,
|
|
configuration.MaximumDecelerationMetersPerSecondSquared,
|
|
configuration.MaximumJerkMetersPerSecondCubed, out JerkLimitedStoppingProfile stop, out _))
|
|
{
|
|
return;
|
|
}
|
|
double cruiseDistance = Math.Max(0d, stopBoundaryPathS - stop.DistanceMeters);
|
|
double requiredDuration = stop.DurationSeconds + cruiseDistance / initialSpeed;
|
|
double stopSpeedTolerance = configuration.StopSpeedToleranceMetersPerSecond;
|
|
if (IsPositiveFinite(stopSpeedTolerance) && JerkLimitedStoppingMath.TryCalculate(stopSpeedTolerance, 0d,
|
|
configuration.MaximumDecelerationMetersPerSecondSquared,
|
|
configuration.MaximumJerkMetersPerSecondCubed, out JerkLimitedStoppingProfile settlingStop, out _))
|
|
{
|
|
double envelopeTraverseDuration = 2d * stopBoundaryPathS / (initialSpeed + stopSpeedTolerance);
|
|
requiredDuration = Math.Max(requiredDuration, envelopeTraverseDuration + settlingStop.DurationSeconds);
|
|
}
|
|
double currentDuration = times[times.Count - 1];
|
|
if (currentDuration + Tolerance >= requiredDuration)
|
|
return;
|
|
double scale = requiredDuration / currentDuration;
|
|
for (int index = 1; index < times.Count; index++)
|
|
times[index] *= scale;
|
|
}
|
|
|
|
private static void EnsureJerkReachableReferenceTimes(IList<double> times, IReadOnlyList<double> referenceSpeeds,
|
|
LongitudinalConfiguration configuration)
|
|
{
|
|
double adjustedTime = 0d;
|
|
double requestedPreviousTime = times[0];
|
|
for (int index = 1; index < times.Count; index++)
|
|
{
|
|
double requestedTime = times[index];
|
|
double requestedDuration = requestedTime - requestedPreviousTime;
|
|
requestedPreviousTime = requestedTime;
|
|
double speedChange = Math.Abs(referenceSpeeds[index] - referenceSpeeds[index - 1]);
|
|
double accelerationLimit = referenceSpeeds[index] >= referenceSpeeds[index - 1]
|
|
? configuration.MaximumAccelerationMetersPerSecondSquared
|
|
: configuration.MaximumDecelerationMetersPerSecondSquared;
|
|
double accelerationDuration = speedChange / accelerationLimit;
|
|
double triangularJerkDuration = speedChange <= Tolerance
|
|
? 0d
|
|
: 2d * Math.Sqrt(speedChange / configuration.MaximumJerkMetersPerSecondCubed);
|
|
adjustedTime += Math.Max(requestedDuration, Math.Max(accelerationDuration, triangularJerkDuration));
|
|
times[index] = adjustedTime;
|
|
}
|
|
}
|
|
|
|
private static IReadOnlyList<int> SelectScheduleStations(IReadOnlyList<double> pathS,
|
|
IReadOnlyList<double> speeds)
|
|
{
|
|
var stations = new List<int> { 0 };
|
|
for (int index = 1; index < pathS.Count - 1; index++)
|
|
{
|
|
double previousSlope = (speeds[index] - speeds[index - 1]) / (pathS[index] - pathS[index - 1]);
|
|
double nextSlope = (speeds[index + 1] - speeds[index]) / (pathS[index + 1] - pathS[index]);
|
|
bool changesDirection = previousSlope * nextSlope < 0d;
|
|
bool entersCruise = previousSlope > Tolerance && nextSlope <= Tolerance;
|
|
bool leavesCruise = previousSlope >= -Tolerance && nextSlope < -Tolerance;
|
|
if (changesDirection || entersCruise || leavesCruise)
|
|
stations.Add(index);
|
|
}
|
|
stations.Add(pathS.Count - 1);
|
|
return stations;
|
|
}
|
|
|
|
private static bool IsFinite(double value) => !double.IsNaN(value) && !double.IsInfinity(value);
|
|
private static bool IsPositiveFinite(double value) => IsFinite(value) && value > 0d;
|
|
}
|