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);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user