First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat: 发布 EM 轨迹规划首个版本

Browse Source
This commit is contained in:
jing.zhang
2026-08-11 20:35:59 +08:00
parent 569de5f13c
commit 1903e71fc1
522 changed files with 4188 additions and 119188 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs
+33 -13
View File
@@ -1,5 +1,6 @@
using System;
using System.Collections.Generic;
using System.Globalization;
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
@@ -48,6 +49,8 @@ public sealed class LongitudinalSolutionValidator
failureReason = "ST candidate violates exact constant-jerk dynamics.";
return false;
}
if (!LongitudinalContinuousProfileValidator.TryValidate(candidate, tolerance, out failureReason))
return false;
if (!AreClose(candidate.S[0], 0d, tolerance) ||
!AreClose(candidate.U[0], input.InitialProgressSpeedMetersPerSecond, tolerance) ||
!AreClose(candidate.A[0], input.InitialAccelerationMetersPerSecondSquared, tolerance))
@@ -56,6 +59,9 @@ public sealed class LongitudinalSolutionValidator
return false;
}
var canonicalS = new double[candidate.S.Count];
var canonicalU = new double[candidate.U.Count];
var canonicalA = new double[candidate.A.Count];
for (int index = 0; index < candidate.S.Count; index++)
{
double progress = candidate.S[index];
@@ -83,6 +89,11 @@ public sealed class LongitudinalSolutionValidator
failureReason = "ST candidate PathS decreases at knot " + index + ".";
return false;
}
canonicalS[index] = progress;
canonicalU[index] = speed < 0d ? 0d : speed;
canonicalA[index] = speed < 0d && acceleration < 0d && acceleration >= -tolerance
? 0d
: acceleration;
}
for (int index = 0; index < candidate.J.Count; index++)
{
@@ -122,25 +133,29 @@ public sealed class LongitudinalSolutionValidator
{
for (int index = 0; index < candidate.S.Count; index++)
{
if (!JerkLimitedStoppingMath.TryCalculate(candidate.U[index], candidate.A[index],
maximumDeceleration, maximumJerk, out JerkLimitedStoppingProfile stop, out _) ||
candidate.S[index] + stop.DistanceMeters > input.StopBoundaryPathS + tolerance)
bool hasStop = JerkLimitedStoppingMath.TryCalculate(canonicalU[index], canonicalA[index],
maximumDeceleration, maximumJerk, out JerkLimitedStoppingProfile stop,
out string stoppingFailure);
double stopDistance = hasStop ? stop.DistanceMeters : double.NaN;
double margin = hasStop
? input.StopBoundaryPathS - canonicalS[index] - stopDistance
: double.NaN;
if (!hasStop || canonicalS[index] + stopDistance > input.StopBoundaryPathS + tolerance)
{
failureReason = "ST candidate leaves the jerk-limited stoppable set at knot " + index + ".";
failureReason = "ST candidate leaves the jerk-limited stoppable set: knot=" +
index.ToString(CultureInfo.InvariantCulture) +
";S=" + Invariant(canonicalS[index]) +
";U=" + Invariant(canonicalU[index]) +
";A=" + Invariant(canonicalA[index]) +
";stopDistance=" + Invariant(stopDistance) +
";stopBoundary=" + Invariant(input.StopBoundaryPathS) +
";margin=" + Invariant(margin) +
(hasStop ? string.Empty : ";stoppingReason=" + stoppingFailure) + ".";
return false;
}
}
}
var canonicalS = new double[candidate.S.Count];
var canonicalU = new double[candidate.U.Count];
var canonicalA = new double[candidate.A.Count];
for (int index = 0; index < candidate.S.Count; index++)
{
canonicalS[index] = candidate.S[index];
canonicalU[index] = candidate.U[index];
canonicalA[index] = candidate.A[index];
}
canonicalS[0] = 0d;
canonicalU[0] = input.InitialProgressSpeedMetersPerSecond;
canonicalA[0] = input.InitialAccelerationMetersPerSecondSquared;
@@ -202,6 +217,11 @@ public sealed class LongitudinalSolutionValidator
return Math.Abs(actual - expected) <= tolerance;
}
private static string Invariant(double value)
{
return value.ToString("R", CultureInfo.InvariantCulture);
}
private static double RequireNonnegative(double value, string parameterName)
{
if (!IsFinite(value) || value < 0d)