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);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,149 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using EMPlannerVerificationHost;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
internal static class LongitudinalModelChecks
|
||||
{
|
||||
public static void Run()
|
||||
{
|
||||
VerifiesFinitePathSIndexedSpeedEnvelope();
|
||||
VerifiesStoppingPrecheckBeforeQpAssembly();
|
||||
VerifiesReferenceHorizonSelectionKeepsTheCurrentSegmentBoundary();
|
||||
}
|
||||
|
||||
private static void VerifiesFinitePathSIndexedSpeedEnvelope()
|
||||
{
|
||||
LateralPath directionPath = CreatePath(new[]
|
||||
{
|
||||
new PathFixture(0d, 0d, 0d, 0d),
|
||||
new PathFixture(1d, 1d, 0d, 0d),
|
||||
});
|
||||
var directionInput = new LongitudinalPlanningInput(directionPath, TravelDirection.Forward, 0d, 0d,
|
||||
EmTerminalType.Goal, EmPlannerConfiguration.CreateDefault(), Array.Empty<double>(), Array.Empty<double>());
|
||||
EmPlanningStatus directionStatus = new PathSpeedLimitBuilder().Build(directionInput,
|
||||
out PathSpeedLimit directionEnvelope, out string directionFailureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, directionStatus, "default direction speed limit status: " +
|
||||
directionFailureReason);
|
||||
Verification.NearlyEqual(0.20d, directionEnvelope.DirectionMaximumSpeedMetersPerSecond,
|
||||
"default direction speed limit");
|
||||
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
|
||||
configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
|
||||
LateralPath path = CreatePath(new[]
|
||||
{
|
||||
new PathFixture(10d, 0d, 0d, 0d),
|
||||
new PathFixture(20d, 2d, 2d, 4d),
|
||||
new PathFixture(20.5d, 4d, 20d, 0d),
|
||||
new PathFixture(21d, 5d, 0d, 0d),
|
||||
});
|
||||
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d,
|
||||
EmTerminalType.Goal, configuration, Array.Empty<double>(), Array.Empty<double>());
|
||||
|
||||
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
|
||||
out string failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "speed envelope status: " + failureReason);
|
||||
Verification.NearlyEqual(1d, envelope.DirectionMaximumSpeedMetersPerSecond, "overridden direction speed limit");
|
||||
Verification.NearlyEqual(Math.Sqrt(0.20d / 2d), envelope.LateralAccelerationLimitAt(2d),
|
||||
"curvature lateral-acceleration limit");
|
||||
Verification.NearlyEqual(0.50d / 4d, envelope.CurvatureRateLimitAt(2d), "curvature-rate limit");
|
||||
Verification.True(double.IsFinite(envelope.LateralAccelerationLimitAt(0d)) &&
|
||||
double.IsFinite(envelope.CurvatureRateLimitAt(0d)), "zero curvature limits stay finite");
|
||||
Verification.NearlyEqual(Math.Sqrt(2d * 0.30d * (5d - 4d)), envelope.StoppingLimitAt(4d),
|
||||
"stopping speed limit");
|
||||
Verification.NearlyEqual(Math.Sqrt(0.20d / 20d), envelope.MaximumSpeedAt(4d),
|
||||
"combined limit chooses the finite minimum");
|
||||
Verification.NearlyEqual((envelope.MaximumSpeedAt(0d) + envelope.MaximumSpeedAt(2d)) / 2d,
|
||||
envelope.MaximumSpeedAt(1d), "speed envelope interpolates by PathS rather than ReferenceS");
|
||||
Verification.NearlyEqual(0d, envelope.MaximumSpeedAt(5d), "terminal speed is exactly zero");
|
||||
}
|
||||
|
||||
private static void VerifiesStoppingPrecheckBeforeQpAssembly()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
LateralPath shortPath = CreatePath(new[]
|
||||
{
|
||||
new PathFixture(0d, 0d, 0d, 0d),
|
||||
new PathFixture(100d, 0.01d, 0d, 0d),
|
||||
});
|
||||
var input = new LongitudinalPlanningInput(shortPath, TravelDirection.Forward, 0.20d, 0.20d,
|
||||
EmTerminalType.RollingSafetyStop, configuration, Array.Empty<double>(), Array.Empty<double>());
|
||||
|
||||
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
|
||||
out string failureReason);
|
||||
Verification.Equal(EmPlanningStatus.StoppingDistanceInsufficient, status,
|
||||
"jerk/deceleration stopping precheck status");
|
||||
Verification.True(envelope == null, "stopping-distance failure does not create a speed envelope");
|
||||
Verification.True(failureReason.Length != 0, "stopping-distance failure explains the rejection");
|
||||
}
|
||||
|
||||
private static void VerifiesReferenceHorizonSelectionKeepsTheCurrentSegmentBoundary()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
DirectionSegmentView shortGoal = CreateSegment(0.25d, EmBoundaryType.Goal);
|
||||
EmPlanningStatus status = new PlanningHorizonSelector().Select(shortGoal, 0d, 0d, 0d, configuration,
|
||||
out PlanningHorizonSelection goalSelection, out string goalFailure);
|
||||
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "goal horizon status: " + goalFailure);
|
||||
Verification.Equal(EmTerminalType.Goal, goalSelection.TerminalType, "goal terminal type");
|
||||
Verification.NearlyEqual(0.25d, goalSelection.TerminalReferenceS, "goal terminal reference S");
|
||||
|
||||
DirectionSegmentView longSegment = CreateSegment(4d, EmBoundaryType.GearSwitchApproach);
|
||||
status = new PlanningHorizonSelector().Select(longSegment, 0d, 0d, 0d, configuration,
|
||||
out PlanningHorizonSelection rollingSelection, out string rollingFailure);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "rolling horizon status: " + rollingFailure);
|
||||
Verification.Equal(EmTerminalType.RollingSafetyStop, rollingSelection.TerminalType, "rolling terminal type");
|
||||
Verification.True(rollingSelection.TerminalReferenceS >= 0d && rollingSelection.TerminalReferenceS < 4d,
|
||||
"rolling horizon remains within the current segment");
|
||||
}
|
||||
|
||||
private static LateralPath CreatePath(IReadOnlyList<PathFixture> fixtures)
|
||||
{
|
||||
var points = new List<LateralPathPoint>(fixtures.Count);
|
||||
for (int index = 0; index < fixtures.Count; index++)
|
||||
{
|
||||
PathFixture fixture = fixtures[index];
|
||||
points.Add(new LateralPathPoint(fixture.ReferenceS, fixture.PathS, 0d, 0d, 0d, 0d, fixture.PathS, 0d,
|
||||
0d, fixture.Curvature, fixture.Curvature, fixture.CurvatureDerivative));
|
||||
}
|
||||
return new LateralPath(points, true);
|
||||
}
|
||||
|
||||
private static DirectionSegmentView CreateSegment(double length, EmBoundaryType endBoundaryType)
|
||||
{
|
||||
var points = new List<SmoothedPathPoint>
|
||||
{
|
||||
Point(0d, 0d),
|
||||
Point(length, length),
|
||||
};
|
||||
return new DirectionSegmentView(0, TravelDirection.Forward, points,
|
||||
new ReferenceBoundary(0, 0d, EmBoundaryType.None, 0d),
|
||||
new ReferenceBoundary(0, length, endBoundaryType, length), 0d);
|
||||
}
|
||||
|
||||
private static SmoothedPathPoint Point(double x, double s)
|
||||
{
|
||||
return new SmoothedPathPoint(x, 0d, 0d, 0d, s, TravelDirection.Forward, 0d, 0d, 0d, 1d, false,
|
||||
SmoothedPathPointSource.Anchor);
|
||||
}
|
||||
|
||||
private sealed class PathFixture
|
||||
{
|
||||
public PathFixture(double referenceS, double pathS, double curvature, double curvatureDerivative)
|
||||
{
|
||||
ReferenceS = referenceS;
|
||||
PathS = pathS;
|
||||
Curvature = curvature;
|
||||
CurvatureDerivative = curvatureDerivative;
|
||||
}
|
||||
|
||||
public double ReferenceS { get; }
|
||||
public double PathS { get; }
|
||||
public double Curvature { get; }
|
||||
public double CurvatureDerivative { get; }
|
||||
}
|
||||
}
|
||||
@@ -9,7 +9,8 @@ internal static class Program
|
||||
if (args.Length != 1 || (args[0] != "foundation" && args[0] != "segmentation" && args[0] != "frenet" &&
|
||||
args[0] != "corridor" && args[0] != "optimization" && args[0] != "osqp" && args[0] != "osqp-loader" && args[0] != "osqp-probe" &&
|
||||
args[0] != "all-foundation" && args[0] != "lateral-model" && args[0] != "lateral-integration" &&
|
||||
args[0] != "lateral-real-osqp" && args[0] != "lateral-real-osqp-probe" && args[0] != "lateral-all"))
|
||||
args[0] != "lateral-real-osqp" && args[0] != "lateral-real-osqp-probe" && args[0] != "lateral-all" &&
|
||||
args[0] != "longitudinal-model"))
|
||||
{
|
||||
Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|optimization|osqp|osqp-loader|all-foundation|lateral-model|lateral-integration|lateral-real-osqp|lateral-all");
|
||||
return 2;
|
||||
@@ -71,6 +72,11 @@ internal static class Program
|
||||
MultiWheelC.TrajectoryPlanning.EMPlanner.LateralIntegrationChecks.RunRealOsqpInCleanPluginBundle();
|
||||
Console.WriteLine("PASS lateral-real-osqp");
|
||||
}
|
||||
if (args[0] == "longitudinal-model")
|
||||
{
|
||||
MultiWheelC.TrajectoryPlanning.EMPlanner.LongitudinalModelChecks.Run();
|
||||
Console.WriteLine("PASS longitudinal-model");
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
catch (Exception exception)
|
||||
|
||||
Reference in New Issue
Block a user