feat: build EM path speed limits
This commit is contained in:
梁薄云
@@ -0,0 +1,111 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Immutable longitudinal inputs derived only from an independently validated lateral PathS path.</summary>
|
||||
public sealed class LongitudinalPlanningInput
|
||||
{
|
||||
private const double PathSTolerance = 1e-12d;
|
||||
|
||||
public LongitudinalPlanningInput(LateralPath path, TravelDirection direction, double initialProgressSpeedMetersPerSecond,
|
||||
double initialAccelerationMetersPerSecondSquared, EmTerminalType terminalType, EmPlannerConfiguration configuration,
|
||||
IReadOnlyList<double> previousPathS, IReadOnlyList<double> previousProgressSpeedMetersPerSecond)
|
||||
{
|
||||
if (path == null || !path.IsIndependentlyValidated || path.Points.Count < 2)
|
||||
throw new ArgumentException("Longitudinal planning requires an independently validated lateral path with at least two points.",
|
||||
nameof(path));
|
||||
if (!Enum.IsDefined(typeof(TravelDirection), direction))
|
||||
throw new ArgumentOutOfRangeException(nameof(direction));
|
||||
if (!IsFinite(initialProgressSpeedMetersPerSecond) || initialProgressSpeedMetersPerSecond < 0d)
|
||||
throw new ArgumentOutOfRangeException(nameof(initialProgressSpeedMetersPerSecond));
|
||||
if (!IsFinite(initialAccelerationMetersPerSecondSquared))
|
||||
throw new ArgumentOutOfRangeException(nameof(initialAccelerationMetersPerSecondSquared));
|
||||
if (!Enum.IsDefined(typeof(EmTerminalType), terminalType))
|
||||
throw new ArgumentOutOfRangeException(nameof(terminalType));
|
||||
if (configuration == null)
|
||||
throw new ArgumentNullException(nameof(configuration));
|
||||
|
||||
Path = CopyAndValidatePath(path);
|
||||
Direction = direction;
|
||||
InitialProgressSpeedMetersPerSecond = initialProgressSpeedMetersPerSecond;
|
||||
InitialAccelerationMetersPerSecondSquared = initialAccelerationMetersPerSecondSquared;
|
||||
TerminalType = terminalType;
|
||||
Configuration = configuration.Copy();
|
||||
PreviousPathS = CopyFiniteNonnegative(previousPathS, nameof(previousPathS));
|
||||
PreviousProgressSpeedMetersPerSecond = CopyFiniteNonnegative(previousProgressSpeedMetersPerSecond,
|
||||
nameof(previousProgressSpeedMetersPerSecond));
|
||||
if (PreviousPathS.Count != PreviousProgressSpeedMetersPerSecond.Count)
|
||||
throw new ArgumentException("Previous path-S and progress-speed samples must have matching counts.",
|
||||
nameof(previousProgressSpeedMetersPerSecond));
|
||||
}
|
||||
|
||||
public LateralPath Path { get; }
|
||||
|
||||
public TravelDirection Direction { get; }
|
||||
|
||||
public double InitialProgressSpeedMetersPerSecond { get; }
|
||||
|
||||
public double InitialAccelerationMetersPerSecondSquared { get; }
|
||||
|
||||
public EmTerminalType TerminalType { get; }
|
||||
|
||||
public EmPlannerConfiguration Configuration { get; }
|
||||
|
||||
public IReadOnlyList<double> PreviousPathS { get; }
|
||||
|
||||
public IReadOnlyList<double> PreviousProgressSpeedMetersPerSecond { get; }
|
||||
|
||||
public double TerminalPathS { get { return Path.Points[Path.Points.Count - 1].PathS; } }
|
||||
|
||||
public double DirectionMaximumSpeedMetersPerSecond
|
||||
{
|
||||
get
|
||||
{
|
||||
return Direction == TravelDirection.Forward
|
||||
? Configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond
|
||||
: Configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond;
|
||||
}
|
||||
}
|
||||
|
||||
private static LateralPath CopyAndValidatePath(LateralPath source)
|
||||
{
|
||||
var copy = new List<LateralPathPoint>(source.Points.Count);
|
||||
double previousPathS = double.NegativeInfinity;
|
||||
for (int index = 0; index < source.Points.Count; index++)
|
||||
{
|
||||
LateralPathPoint point = source.Points[index];
|
||||
if (point == null || !IsFinite(point.PathS) || point.PathS <= previousPathS)
|
||||
throw new ArgumentException("Lateral PathS must be finite and strictly increasing for ST planning.", nameof(source));
|
||||
if (index == 0 && Math.Abs(point.PathS) > PathSTolerance)
|
||||
throw new ArgumentException("The lateral PathS supplied to ST must begin at zero.", nameof(source));
|
||||
copy.Add(new LateralPathPoint(point.ReferenceS, point.PathS, point.L, point.DL, point.DDL, point.DDDL,
|
||||
point.X, point.Y, point.VehicleYaw, point.GeometricCurvature, point.VehicleCurvature,
|
||||
point.VehicleCurvatureDerivative));
|
||||
previousPathS = point.PathS;
|
||||
}
|
||||
return new LateralPath(copy, true);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<double> CopyFiniteNonnegative(IReadOnlyList<double> source, string parameterName)
|
||||
{
|
||||
var copy = new List<double>(source == null ? 0 : source.Count);
|
||||
if (source != null)
|
||||
{
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
{
|
||||
if (!IsFinite(source[index]) || source[index] < 0d)
|
||||
throw new ArgumentOutOfRangeException(parameterName);
|
||||
copy.Add(source[index]);
|
||||
}
|
||||
}
|
||||
return new ReadOnlyCollection<double>(copy);
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,123 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Finite piecewise-linear speed limits indexed exclusively by actual lateral PathS.</summary>
|
||||
public sealed class PathSpeedLimit
|
||||
{
|
||||
private const double StationTolerance = 1e-12d;
|
||||
|
||||
internal PathSpeedLimit(IReadOnlyList<double> pathS, IReadOnlyList<double> maximumSpeed,
|
||||
IReadOnlyList<double> lateralAccelerationLimit, IReadOnlyList<double> curvatureRateLimit,
|
||||
IReadOnlyList<double> stoppingLimit, double directionMaximumSpeedMetersPerSecond)
|
||||
{
|
||||
PathS = CopyStrictStations(pathS, nameof(pathS));
|
||||
MaximumSpeedMetersPerSecond = CopyFiniteNonnegative(maximumSpeed, PathS.Count, nameof(maximumSpeed));
|
||||
LateralAccelerationSpeedLimitsMetersPerSecond = CopyFiniteNonnegative(lateralAccelerationLimit, PathS.Count,
|
||||
nameof(lateralAccelerationLimit));
|
||||
CurvatureRateSpeedLimitsMetersPerSecond = CopyFiniteNonnegative(curvatureRateLimit, PathS.Count,
|
||||
nameof(curvatureRateLimit));
|
||||
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)
|
||||
{
|
||||
throw new ArgumentException("Terminal PathS speed limits must be exactly zero.");
|
||||
}
|
||||
|
||||
DirectionMaximumSpeedMetersPerSecond = directionMaximumSpeedMetersPerSecond;
|
||||
}
|
||||
|
||||
public IReadOnlyList<double> PathS { get; }
|
||||
|
||||
public IReadOnlyList<double> MaximumSpeedMetersPerSecond { get; }
|
||||
|
||||
public IReadOnlyList<double> LateralAccelerationSpeedLimitsMetersPerSecond { get; }
|
||||
|
||||
public IReadOnlyList<double> CurvatureRateSpeedLimitsMetersPerSecond { get; }
|
||||
|
||||
public IReadOnlyList<double> StoppingSpeedLimitsMetersPerSecond { get; }
|
||||
|
||||
public double DirectionMaximumSpeedMetersPerSecond { get; }
|
||||
|
||||
public double TerminalPathS { get { return PathS[PathS.Count - 1]; } }
|
||||
|
||||
public double MaximumSpeedAt(double pathS)
|
||||
{
|
||||
return Interpolate(MaximumSpeedMetersPerSecond, pathS);
|
||||
}
|
||||
|
||||
public double LateralAccelerationLimitAt(double pathS)
|
||||
{
|
||||
return Interpolate(LateralAccelerationSpeedLimitsMetersPerSecond, pathS);
|
||||
}
|
||||
|
||||
public double CurvatureRateLimitAt(double pathS)
|
||||
{
|
||||
return Interpolate(CurvatureRateSpeedLimitsMetersPerSecond, pathS);
|
||||
}
|
||||
|
||||
public double StoppingLimitAt(double pathS)
|
||||
{
|
||||
return Interpolate(StoppingSpeedLimitsMetersPerSecond, pathS);
|
||||
}
|
||||
|
||||
private double Interpolate(IReadOnlyList<double> values, double pathS)
|
||||
{
|
||||
if (!IsFinite(pathS) || pathS < PathS[0] - StationTolerance || pathS > TerminalPathS + StationTolerance)
|
||||
throw new ArgumentOutOfRangeException(nameof(pathS));
|
||||
if (pathS <= PathS[0])
|
||||
return values[0];
|
||||
if (pathS >= TerminalPathS)
|
||||
return values[values.Count - 1];
|
||||
|
||||
for (int index = 1; index < PathS.Count; index++)
|
||||
{
|
||||
if (pathS <= PathS[index])
|
||||
{
|
||||
double fraction = (pathS - PathS[index - 1]) / (PathS[index] - PathS[index - 1]);
|
||||
return values[index - 1] + (values[index] - values[index - 1]) * fraction;
|
||||
}
|
||||
}
|
||||
return values[values.Count - 1];
|
||||
}
|
||||
|
||||
private static IReadOnlyList<double> CopyStrictStations(IReadOnlyList<double> source, string parameterName)
|
||||
{
|
||||
if (source == null || source.Count < 2)
|
||||
throw new ArgumentException("At least two PathS stations are required.", parameterName);
|
||||
var copy = new List<double>(source.Count);
|
||||
double previous = double.NegativeInfinity;
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
{
|
||||
if (!IsFinite(source[index]) || source[index] <= previous)
|
||||
throw new ArgumentException("PathS stations must be finite and strictly increasing.", parameterName);
|
||||
copy.Add(source[index]);
|
||||
previous = source[index];
|
||||
}
|
||||
return new ReadOnlyCollection<double>(copy);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<double> CopyFiniteNonnegative(IReadOnlyList<double> source, int expectedCount,
|
||||
string parameterName)
|
||||
{
|
||||
if (source == null || source.Count != expectedCount)
|
||||
throw new ArgumentException("Speed limit count must match PathS stations.", parameterName);
|
||||
var copy = new List<double>(source.Count);
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
{
|
||||
if (!IsFinite(source[index]) || source[index] < 0d)
|
||||
throw new ArgumentOutOfRangeException(parameterName);
|
||||
copy.Add(source[index]);
|
||||
}
|
||||
return new ReadOnlyCollection<double>(copy);
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,188 @@
|
||||
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;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
LongitudinalStoppingProfile stopProfile = LongitudinalStoppingMath.Calculate(input.InitialProgressSpeedMetersPerSecond,
|
||||
input.InitialAccelerationMetersPerSecondSquared, maximumDeceleration, maximumJerk);
|
||||
if (stopProfile.DistanceMeters + StopDistanceToleranceMeters > input.TerminalPathS)
|
||||
{
|
||||
failureReason = "The available actual PathS distance is insufficient for the jerk-limited stop.";
|
||||
return EmPlanningStatus.StoppingDistanceInsufficient;
|
||||
}
|
||||
|
||||
int count = input.Path.Points.Count;
|
||||
var pathS = new double[count];
|
||||
var maximum = new double[count];
|
||||
var lateral = new double[count];
|
||||
var curvatureRate = new double[count];
|
||||
var stopping = new double[count];
|
||||
for (int index = 0; index < count; index++)
|
||||
{
|
||||
LateralPathPoint point = input.Path.Points[index];
|
||||
pathS[index] = point.PathS;
|
||||
double lateralLimit = Math.Sqrt(maximumLateralAcceleration /
|
||||
Math.Max(Math.Abs(point.VehicleCurvature), CurvatureEpsilon));
|
||||
double curvatureRateLimit = maximumCurvatureRate /
|
||||
Math.Max(Math.Abs(point.VehicleCurvatureDerivative), CurvatureEpsilon);
|
||||
double remainingDistance = Math.Max(0d, input.TerminalPathS - point.PathS);
|
||||
double stoppingLimit = Math.Sqrt(2d * maximumDeceleration * remainingDistance);
|
||||
lateral[index] = ClampFinite(lateralLimit, directionMaximum);
|
||||
curvatureRate[index] = ClampFinite(curvatureRateLimit, directionMaximum);
|
||||
stopping[index] = index == count - 1 ? 0d : ClampFinite(stoppingLimit, directionMaximum);
|
||||
maximum[index] = index == count - 1 ? 0d : Math.Min(directionMaximum,
|
||||
Math.Min(lateral[index], Math.Min(curvatureRate[index], stopping[index])));
|
||||
}
|
||||
|
||||
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 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);
|
||||
}
|
||||
}
|
||||
|
||||
internal sealed class LongitudinalStoppingProfile
|
||||
{
|
||||
public LongitudinalStoppingProfile(double distanceMeters, double durationSeconds)
|
||||
{
|
||||
DistanceMeters = distanceMeters;
|
||||
DurationSeconds = durationSeconds;
|
||||
}
|
||||
|
||||
public double DistanceMeters { get; }
|
||||
|
||||
public double DurationSeconds { get; }
|
||||
}
|
||||
|
||||
internal static class LongitudinalStoppingMath
|
||||
{
|
||||
public static LongitudinalStoppingProfile Calculate(double speedMetersPerSecond, double accelerationMetersPerSecondSquared,
|
||||
double maximumDecelerationMetersPerSecondSquared, double maximumJerkMetersPerSecondCubed)
|
||||
{
|
||||
if (!IsFinite(speedMetersPerSecond) || !IsFinite(accelerationMetersPerSecondSquared) ||
|
||||
!IsPositiveFinite(maximumDecelerationMetersPerSecondSquared) || !IsPositiveFinite(maximumJerkMetersPerSecondCubed))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(speedMetersPerSecond));
|
||||
}
|
||||
if (speedMetersPerSecond <= 0d)
|
||||
return new LongitudinalStoppingProfile(0d, 0d);
|
||||
|
||||
double acceleration = Math.Max(-maximumDecelerationMetersPerSecondSquared, accelerationMetersPerSecondSquared);
|
||||
double rampDuration = (acceleration + maximumDecelerationMetersPerSecondSquared) / maximumJerkMetersPerSecondCubed;
|
||||
double speedAfterRamp = speedMetersPerSecond + acceleration * rampDuration -
|
||||
0.5d * maximumJerkMetersPerSecondCubed * rampDuration * rampDuration;
|
||||
if (speedAfterRamp <= 0d)
|
||||
{
|
||||
double root = (acceleration + Math.Sqrt(acceleration * acceleration + 2d * maximumJerkMetersPerSecondCubed *
|
||||
speedMetersPerSecond)) / maximumJerkMetersPerSecondCubed;
|
||||
double distance = speedMetersPerSecond * root + 0.5d * acceleration * root * root -
|
||||
maximumJerkMetersPerSecondCubed * root * root * root / 6d;
|
||||
return new LongitudinalStoppingProfile(Math.Max(0d, distance), root);
|
||||
}
|
||||
|
||||
double rampDistance = speedMetersPerSecond * rampDuration + 0.5d * acceleration * rampDuration * rampDuration -
|
||||
maximumJerkMetersPerSecondCubed * rampDuration * rampDuration * rampDuration / 6d;
|
||||
double constantDecelerationDuration = speedAfterRamp / maximumDecelerationMetersPerSecondSquared;
|
||||
double constantDecelerationDistance = speedAfterRamp * speedAfterRamp /
|
||||
(2d * maximumDecelerationMetersPerSecondSquared);
|
||||
return new LongitudinalStoppingProfile(rampDistance + constantDecelerationDistance,
|
||||
rampDuration + constantDecelerationDuration);
|
||||
}
|
||||
|
||||
private static bool IsPositiveFinite(double value)
|
||||
{
|
||||
return IsFinite(value) && value > 0d;
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,156 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Reference-distance terminal chosen before LS without crossing the current direction segment.</summary>
|
||||
public sealed class PlanningHorizonSelection
|
||||
{
|
||||
internal PlanningHorizonSelection(double terminalReferenceS, EmTerminalType terminalType)
|
||||
{
|
||||
TerminalReferenceS = terminalReferenceS;
|
||||
TerminalType = terminalType;
|
||||
}
|
||||
|
||||
public double TerminalReferenceS { get; }
|
||||
|
||||
public EmTerminalType TerminalType { get; }
|
||||
}
|
||||
|
||||
public sealed class PlanningHorizonSelector
|
||||
{
|
||||
private const double BoundaryTolerance = 1e-8d;
|
||||
|
||||
public EmPlanningStatus Select(DirectionSegmentView segment, double currentSegmentReferenceS,
|
||||
double initialProgressSpeedMetersPerSecond, double initialAccelerationMetersPerSecondSquared,
|
||||
EmPlannerConfiguration configuration, out PlanningHorizonSelection selection, out string failureReason)
|
||||
{
|
||||
selection = null;
|
||||
failureReason = string.Empty;
|
||||
if (segment == null || configuration == null || configuration.Scheduling == null || configuration.Longitudinal == null ||
|
||||
!IsFinite(currentSegmentReferenceS) || currentSegmentReferenceS < 0d ||
|
||||
currentSegmentReferenceS > segment.LengthMeters + BoundaryTolerance ||
|
||||
!IsFinite(initialProgressSpeedMetersPerSecond) || initialProgressSpeedMetersPerSecond < 0d ||
|
||||
!IsFinite(initialAccelerationMetersPerSecondSquared))
|
||||
{
|
||||
failureReason = "Planning horizon inputs are invalid.";
|
||||
return EmPlanningStatus.InvalidInput;
|
||||
}
|
||||
|
||||
LongitudinalConfiguration longitudinal = configuration.Longitudinal;
|
||||
SchedulingConfiguration scheduling = configuration.Scheduling;
|
||||
double directionMaximum = segment.Direction == TravelDirection.Forward
|
||||
? longitudinal.MaximumForwardSpeedMetersPerSecond
|
||||
: longitudinal.MaximumReverseSpeedMetersPerSecond;
|
||||
if (!IsPositiveFinite(directionMaximum) || !IsPositiveFinite(longitudinal.MaximumAccelerationMetersPerSecondSquared) ||
|
||||
!IsPositiveFinite(longitudinal.MaximumDecelerationMetersPerSecondSquared) ||
|
||||
!IsPositiveFinite(longitudinal.MaximumJerkMetersPerSecondCubed) ||
|
||||
!IsPositiveFinite(longitudinal.ZeroSpeedHoldSeconds) || !IsPositiveFinite(scheduling.TimeHorizonSeconds) ||
|
||||
!IsPositiveFinite(scheduling.DistanceHorizonMeters))
|
||||
{
|
||||
failureReason = "Planning horizon configuration is invalid.";
|
||||
return EmPlanningStatus.InvalidInput;
|
||||
}
|
||||
if (initialProgressSpeedMetersPerSecond > directionMaximum + BoundaryTolerance ||
|
||||
initialAccelerationMetersPerSecondSquared < -longitudinal.MaximumDecelerationMetersPerSecondSquared - BoundaryTolerance ||
|
||||
initialAccelerationMetersPerSecondSquared > longitudinal.MaximumAccelerationMetersPerSecondSquared + BoundaryTolerance)
|
||||
{
|
||||
failureReason = "The initial state violates longitudinal bounds.";
|
||||
return EmPlanningStatus.InvalidInput;
|
||||
}
|
||||
|
||||
double remainingSegment = Math.Max(0d, segment.LengthMeters - currentSegmentReferenceS);
|
||||
LongitudinalStoppingProfile initialStop = LongitudinalStoppingMath.Calculate(initialProgressSpeedMetersPerSecond,
|
||||
initialAccelerationMetersPerSecondSquared, longitudinal.MaximumDecelerationMetersPerSecondSquared,
|
||||
longitudinal.MaximumJerkMetersPerSecondCubed);
|
||||
if (initialStop.DistanceMeters + BoundaryTolerance > remainingSegment)
|
||||
{
|
||||
failureReason = "The current segment lacks the jerk-limited stopping distance.";
|
||||
return EmPlanningStatus.StoppingDistanceInsufficient;
|
||||
}
|
||||
|
||||
double timeReachable = CalculateReachableDistance(initialProgressSpeedMetersPerSecond,
|
||||
initialAccelerationMetersPerSecondSquared, directionMaximum, longitudinal, scheduling.TimeHorizonSeconds);
|
||||
double terminalReferenceS = currentSegmentReferenceS + Math.Min(remainingSegment,
|
||||
Math.Min(scheduling.DistanceHorizonMeters, timeReachable));
|
||||
if (terminalReferenceS >= segment.LengthMeters - BoundaryTolerance)
|
||||
{
|
||||
terminalReferenceS = segment.LengthMeters;
|
||||
selection = new PlanningHorizonSelection(terminalReferenceS, ToTerminalType(segment.EndBoundary.BoundaryType));
|
||||
}
|
||||
else
|
||||
{
|
||||
selection = new PlanningHorizonSelection(terminalReferenceS, EmTerminalType.RollingSafetyStop);
|
||||
}
|
||||
return EmPlanningStatus.Success;
|
||||
}
|
||||
|
||||
private static double CalculateReachableDistance(double initialSpeed, double initialAcceleration, double maximumSpeed,
|
||||
LongitudinalConfiguration configuration, double timeHorizonSeconds)
|
||||
{
|
||||
LongitudinalStoppingProfile stopAtMaximumSpeed = LongitudinalStoppingMath.Calculate(maximumSpeed, 0d,
|
||||
configuration.MaximumDecelerationMetersPerSecondSquared, configuration.MaximumJerkMetersPerSecondCubed);
|
||||
double drivingDuration = timeHorizonSeconds - configuration.ZeroSpeedHoldSeconds - stopAtMaximumSpeed.DurationSeconds;
|
||||
if (drivingDuration <= 0d)
|
||||
return Math.Min(initialSpeed, maximumSpeed) * Math.Max(0d, timeHorizonSeconds - configuration.ZeroSpeedHoldSeconds);
|
||||
|
||||
double speed = initialSpeed;
|
||||
double acceleration = initialAcceleration;
|
||||
double distance = 0d;
|
||||
const double simulationStepSeconds = 0.001d;
|
||||
while (drivingDuration > 0d)
|
||||
{
|
||||
double step = Math.Min(simulationStepSeconds, drivingDuration);
|
||||
double jerk = ChooseAccelerationJerk(speed, acceleration, maximumSpeed,
|
||||
configuration.MaximumAccelerationMetersPerSecondSquared, configuration.MaximumJerkMetersPerSecondCubed);
|
||||
double nextSpeed = speed + acceleration * step + 0.5d * jerk * step * step;
|
||||
if (nextSpeed > maximumSpeed)
|
||||
{
|
||||
nextSpeed = maximumSpeed;
|
||||
acceleration = 0d;
|
||||
jerk = 0d;
|
||||
}
|
||||
distance += speed * step + 0.5d * acceleration * step * step + jerk * step * step * step / 6d;
|
||||
acceleration += jerk * step;
|
||||
speed = Math.Max(0d, nextSpeed);
|
||||
drivingDuration -= step;
|
||||
}
|
||||
return Math.Max(0d, distance + stopAtMaximumSpeed.DistanceMeters);
|
||||
}
|
||||
|
||||
private static double ChooseAccelerationJerk(double speed, double acceleration, double maximumSpeed,
|
||||
double maximumAcceleration, double maximumJerk)
|
||||
{
|
||||
if (speed >= maximumSpeed - BoundaryTolerance)
|
||||
{
|
||||
if (acceleration > 0d)
|
||||
return -maximumJerk;
|
||||
return acceleration < 0d ? maximumJerk : 0d;
|
||||
}
|
||||
double speedToReduceAccelerationToZero = acceleration > 0d
|
||||
? acceleration * acceleration / (2d * maximumJerk)
|
||||
: 0d;
|
||||
if (speed + speedToReduceAccelerationToZero >= maximumSpeed - BoundaryTolerance)
|
||||
return acceleration > 0d ? -maximumJerk : 0d;
|
||||
return acceleration < maximumAcceleration - BoundaryTolerance ? maximumJerk : 0d;
|
||||
}
|
||||
|
||||
private static EmTerminalType ToTerminalType(EmBoundaryType boundaryType)
|
||||
{
|
||||
return boundaryType == EmBoundaryType.Goal
|
||||
? EmTerminalType.Goal
|
||||
: boundaryType == EmBoundaryType.GearSwitchApproach || boundaryType == EmBoundaryType.GearSwitchDeparture
|
||||
? EmTerminalType.GearSwitch
|
||||
: EmTerminalType.RollingSafetyStop;
|
||||
}
|
||||
|
||||
private static bool IsPositiveFinite(double value)
|
||||
{
|
||||
return IsFinite(value) && value > 0d;
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user