feat: 发布 EM 轨迹规划首个版本
This commit is contained in:
+274
@@ -0,0 +1,274 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
internal sealed class LongitudinalEnvelopeTrustRegion
|
||||
{
|
||||
internal LongitudinalEnvelopeTrustRegion(IReadOnlyList<double> minimumPathS, IReadOnlyList<double> maximumPathS,
|
||||
IReadOnlyList<double> speedSlope, IReadOnlyList<double> speedIntercept,
|
||||
IReadOnlyList<int> envelopeSegmentIndex, double scale)
|
||||
{
|
||||
MinimumPathS = Copy(minimumPathS, nameof(minimumPathS));
|
||||
MaximumPathS = Copy(maximumPathS, nameof(maximumPathS));
|
||||
SpeedSlope = Copy(speedSlope, nameof(speedSlope));
|
||||
SpeedIntercept = Copy(speedIntercept, nameof(speedIntercept));
|
||||
EnvelopeSegmentIndex = Copy(envelopeSegmentIndex, nameof(envelopeSegmentIndex));
|
||||
Scale = scale;
|
||||
}
|
||||
|
||||
internal IReadOnlyList<double> MinimumPathS { get; }
|
||||
|
||||
internal IReadOnlyList<double> MaximumPathS { get; }
|
||||
|
||||
internal IReadOnlyList<double> SpeedSlope { get; }
|
||||
|
||||
internal IReadOnlyList<double> SpeedIntercept { get; }
|
||||
|
||||
internal IReadOnlyList<int> EnvelopeSegmentIndex { get; }
|
||||
|
||||
internal double Scale { get; }
|
||||
|
||||
internal bool CanShrinkTo(double nextScale, double minimumActiveWidthMeters, out string failureReason)
|
||||
{
|
||||
failureReason = string.Empty;
|
||||
if (!IsFinite(nextScale) || nextScale <= 0d || nextScale >= Scale)
|
||||
{
|
||||
failureReason = "Next trust-region scale must be finite, positive, and smaller than the current scale.";
|
||||
return false;
|
||||
}
|
||||
if (!IsFinite(minimumActiveWidthMeters) || minimumActiveWidthMeters <= 0d)
|
||||
{
|
||||
failureReason = "Minimum active width must be finite and positive.";
|
||||
return false;
|
||||
}
|
||||
double ratio = nextScale / Scale;
|
||||
for (int index = 0; index < MinimumPathS.Count; index++)
|
||||
{
|
||||
double currentWidth = MaximumPathS[index] - MinimumPathS[index];
|
||||
if (currentWidth > 0d && currentWidth * ratio < minimumActiveWidthMeters)
|
||||
{
|
||||
failureReason = "The next trust-region scale would fall below the minimum width at knot " + index + ".";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static IReadOnlyList<double> Copy(IReadOnlyList<double> source, string parameterName)
|
||||
{
|
||||
if (source == null)
|
||||
throw new ArgumentNullException(parameterName);
|
||||
var copy = new List<double>(source.Count);
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
copy.Add(source[index]);
|
||||
return new ReadOnlyCollection<double>(copy);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<int> Copy(IReadOnlyList<int> source, string parameterName)
|
||||
{
|
||||
if (source == null)
|
||||
throw new ArgumentNullException(parameterName);
|
||||
var copy = new List<int>(source.Count);
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
copy.Add(source[index]);
|
||||
return new ReadOnlyCollection<int>(copy);
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
|
||||
internal sealed class LongitudinalEnvelopeTrustRegionBuilder
|
||||
{
|
||||
private const double ScheduleProgressTolerance = 1e-12d;
|
||||
private const double StationSelectionTolerance = 1e-12d;
|
||||
|
||||
internal bool TryBuild(PathSpeedLimit speedLimit, LongitudinalCandidate anchor,
|
||||
IReadOnlyList<double> referencePathS, int terminalHoldStartIndex, double scale,
|
||||
double minimumActiveWidthMeters, out LongitudinalEnvelopeTrustRegion region,
|
||||
out string failureReason)
|
||||
{
|
||||
region = null;
|
||||
failureReason = string.Empty;
|
||||
if (speedLimit == null || anchor == null || referencePathS == null)
|
||||
{
|
||||
failureReason = "Trust region inputs must be present.";
|
||||
return false;
|
||||
}
|
||||
if (!IsSupportedScale(scale))
|
||||
{
|
||||
failureReason = "Trust-region scale must be one of 1, 0.5, 0.25, or 0.125.";
|
||||
return false;
|
||||
}
|
||||
if (!IsFinite(minimumActiveWidthMeters) || minimumActiveWidthMeters <= 0d)
|
||||
{
|
||||
failureReason = "Minimum active width must be finite and positive.";
|
||||
return false;
|
||||
}
|
||||
if (referencePathS.Count != anchor.S.Count)
|
||||
{
|
||||
failureReason = "Reference PathS count must match the anchor knot count.";
|
||||
return false;
|
||||
}
|
||||
if (terminalHoldStartIndex == -1)
|
||||
terminalHoldStartIndex = anchor.S.Count;
|
||||
if (terminalHoldStartIndex < 1 || terminalHoldStartIndex > anchor.S.Count)
|
||||
{
|
||||
failureReason = "Terminal-hold start index is outside the anchor knot range.";
|
||||
return false;
|
||||
}
|
||||
if (!TryValidateAnchorAndReference(speedLimit, anchor, referencePathS, out failureReason))
|
||||
return false;
|
||||
|
||||
int knotCount = anchor.S.Count;
|
||||
var minimumPathS = new double[knotCount];
|
||||
var maximumPathS = new double[knotCount];
|
||||
var speedSlope = new double[knotCount];
|
||||
var speedIntercept = new double[knotCount];
|
||||
var segmentIndex = new int[knotCount];
|
||||
for (int index = 0; index < knotCount; index++)
|
||||
{
|
||||
double anchorS = anchor.S[index];
|
||||
bool fixedKnot = index == 0 || index >= terminalHoldStartIndex;
|
||||
int segment = SelectSegment(speedLimit, anchorS, referencePathS, index, fixedKnot);
|
||||
FindMaximalExactAffineRun(speedLimit, segment, out int firstSegment, out int lastSegment,
|
||||
out double slope, out double intercept);
|
||||
double lower = speedLimit.PathS[firstSegment];
|
||||
double upper = speedLimit.PathS[lastSegment + 1];
|
||||
if (anchorS < lower && lower - anchorS <= StationSelectionTolerance)
|
||||
lower = anchorS;
|
||||
if (anchorS > upper && anchorS - upper <= StationSelectionTolerance)
|
||||
upper = anchorS;
|
||||
double trustedLower = fixedKnot ? anchorS : anchorS - scale * (anchorS - lower);
|
||||
double trustedUpper = fixedKnot ? anchorS : anchorS + scale * (upper - anchorS);
|
||||
if (!fixedKnot && scale < 1d && trustedUpper - trustedLower < minimumActiveWidthMeters)
|
||||
{
|
||||
failureReason = "Active trust-region interval is narrower than the minimum width at knot " + index + ".";
|
||||
return false;
|
||||
}
|
||||
|
||||
minimumPathS[index] = trustedLower;
|
||||
maximumPathS[index] = trustedUpper;
|
||||
speedSlope[index] = slope;
|
||||
speedIntercept[index] = intercept;
|
||||
segmentIndex[index] = segment;
|
||||
}
|
||||
|
||||
region = new LongitudinalEnvelopeTrustRegion(minimumPathS, maximumPathS, speedSlope, speedIntercept,
|
||||
segmentIndex, scale);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryValidateAnchorAndReference(PathSpeedLimit speedLimit, LongitudinalCandidate anchor,
|
||||
IReadOnlyList<double> referencePathS, out string failureReason)
|
||||
{
|
||||
if (anchor.S[0] != 0d)
|
||||
{
|
||||
failureReason = "The anchor must begin at exact PathS zero.";
|
||||
return false;
|
||||
}
|
||||
|
||||
double minimumPathS = speedLimit.PathS[0];
|
||||
double maximumPathS = speedLimit.PathS[speedLimit.PathS.Count - 1];
|
||||
double previousAnchorS = double.NegativeInfinity;
|
||||
for (int index = 0; index < anchor.S.Count; index++)
|
||||
{
|
||||
double anchorS = anchor.S[index];
|
||||
double referenceS = referencePathS[index];
|
||||
if (!IsFinite(anchorS) || anchorS < minimumPathS || anchorS > maximumPathS)
|
||||
{
|
||||
failureReason = "Anchor PathS is outside the speed-limit range at knot " + index + ".";
|
||||
return false;
|
||||
}
|
||||
if (anchorS < previousAnchorS)
|
||||
{
|
||||
failureReason = "Anchor PathS must be nondecreasing.";
|
||||
return false;
|
||||
}
|
||||
if (!IsFinite(referenceS))
|
||||
{
|
||||
failureReason = "Reference PathS must be finite.";
|
||||
return false;
|
||||
}
|
||||
previousAnchorS = anchorS;
|
||||
}
|
||||
|
||||
failureReason = string.Empty;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static int SelectSegment(PathSpeedLimit speedLimit, double anchorS, IReadOnlyList<double> referencePathS,
|
||||
int knotIndex, bool terminalHold)
|
||||
{
|
||||
int lastSegment = speedLimit.PathS.Count - 2;
|
||||
if (Math.Abs(anchorS - speedLimit.PathS[0]) <= StationSelectionTolerance)
|
||||
return 0;
|
||||
if (Math.Abs(anchorS - speedLimit.PathS[speedLimit.PathS.Count - 1]) <= StationSelectionTolerance)
|
||||
return lastSegment;
|
||||
for (int index = 1; index < speedLimit.PathS.Count - 1; index++)
|
||||
{
|
||||
if (Math.Abs(anchorS - speedLimit.PathS[index]) <= StationSelectionTolerance)
|
||||
{
|
||||
if (terminalHold)
|
||||
return index;
|
||||
double scheduleDelta = referencePathS[knotIndex] - referencePathS[knotIndex - 1];
|
||||
if (scheduleDelta > ScheduleProgressTolerance)
|
||||
return index;
|
||||
if (scheduleDelta < -ScheduleProgressTolerance)
|
||||
return index - 1;
|
||||
double leftWidth = speedLimit.PathS[index] - speedLimit.PathS[index - 1];
|
||||
double rightWidth = speedLimit.PathS[index + 1] - speedLimit.PathS[index];
|
||||
return rightWidth >= leftWidth ? index : index - 1;
|
||||
}
|
||||
if (anchorS < speedLimit.PathS[index])
|
||||
return index - 1;
|
||||
}
|
||||
return lastSegment;
|
||||
}
|
||||
|
||||
private static void GetAffineLine(PathSpeedLimit limit, int segment,
|
||||
out double slope, out double intercept)
|
||||
{
|
||||
double lower = limit.PathS[segment];
|
||||
double upper = limit.PathS[segment + 1];
|
||||
slope = (limit.MaximumSpeedMetersPerSecond[segment + 1] -
|
||||
limit.MaximumSpeedMetersPerSecond[segment]) / (upper - lower);
|
||||
intercept = limit.MaximumSpeedMetersPerSecond[segment] - slope * lower;
|
||||
}
|
||||
|
||||
private static void FindMaximalExactAffineRun(PathSpeedLimit limit, int selectedSegment,
|
||||
out int firstSegment, out int lastSegment, out double slope, out double intercept)
|
||||
{
|
||||
GetAffineLine(limit, selectedSegment, out slope, out intercept);
|
||||
firstSegment = selectedSegment;
|
||||
while (firstSegment > 0)
|
||||
{
|
||||
GetAffineLine(limit, firstSegment - 1, out double candidateSlope, out double candidateIntercept);
|
||||
if (candidateSlope != slope || candidateIntercept != intercept)
|
||||
break;
|
||||
firstSegment--;
|
||||
}
|
||||
lastSegment = selectedSegment;
|
||||
while (lastSegment < limit.PathS.Count - 2)
|
||||
{
|
||||
GetAffineLine(limit, lastSegment + 1, out double candidateSlope, out double candidateIntercept);
|
||||
if (candidateSlope != slope || candidateIntercept != intercept)
|
||||
break;
|
||||
lastSegment++;
|
||||
}
|
||||
}
|
||||
|
||||
private static bool IsSupportedScale(double scale)
|
||||
{
|
||||
return scale == 1d || scale == 0.5d || scale == 0.25d || scale == 0.125d;
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user