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

2026-08-11 20:35:59 +08:00
parent 569de5f13c
commit 1903e71fc1
522 changed files with 4188 additions and 119188 deletions
@@ -0,0 +1,217 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+internal sealed class LongitudinalQpAuditResult
+{
+    internal LongitudinalQpAuditResult(bool isFeasible, double maximumResidual, int worstRow,
+        string category, string unit)
+    {
+        IsFeasible = isFeasible;
+        MaximumResidual = maximumResidual;
+        WorstRow = worstRow;
+        Category = category;
+        Unit = unit;
+    }
+
+    internal bool IsFeasible { get; }
+
+    internal double MaximumResidual { get; }
+
+    internal int WorstRow { get; }
+
+    internal string Category { get; }
+
+    internal string Unit { get; }
+}
+
+internal static class LongitudinalQpFeasibilityAudit
+{
+    internal static LongitudinalQpAuditResult Evaluate(QuadraticProgram problem,
+        LongitudinalCandidate candidate, double tolerance, LongitudinalVariableLayout layout,
+        int stabilizationStart)
+    {
+        if (problem == null)
+            throw new ArgumentNullException(nameof(problem));
+        if (candidate == null)
+            throw new ArgumentNullException(nameof(candidate));
+        if (layout == null)
+            throw new ArgumentNullException(nameof(layout));
+        if (problem.VariableCount != layout.VariableCount || candidate.S.Count != layout.KnotCount ||
+            candidate.U.Count != layout.KnotCount || candidate.A.Count != layout.KnotCount ||
+            candidate.J.Count != layout.KnotCount - 1)
+        {
+            throw new ArgumentException("The QP, candidate, and longitudinal layout must have matching dimensions.");
+        }
+
+        double[] primal = ToPrimal(candidate, layout);
+        var activity = new double[problem.ConstraintCount];
+        bool allFinite = IsFinite(tolerance);
+        SparseCscMatrix matrix = problem.ConstraintMatrix;
+        for (int column = 0; column < matrix.ColumnCount; column++)
+        {
+            double value = primal[column];
+            allFinite &= IsFinite(value);
+            for (int entry = matrix.ColumnPointers[column]; entry < matrix.ColumnPointers[column + 1]; entry++)
+                activity[matrix.RowIndices[entry]] += matrix.Values[entry] * value;
+        }
+
+        double maximumResidual = 0d;
+        int worstRow = problem.ConstraintCount == 0 ? -1 : 0;
+        for (int row = 0; row < problem.ConstraintCount; row++)
+        {
+            double residual;
+            if (!IsFinite(activity[row]))
+            {
+                allFinite = false;
+                residual = double.PositiveInfinity;
+            }
+            else
+            {
+                residual = Math.Max(0d, Math.Max(
+                    problem.LowerBounds[row] - activity[row],
+                    activity[row] - problem.UpperBounds[row]));
+            }
+            if (row == 0 || residual > maximumResidual)
+            {
+                maximumResidual = residual;
+                worstRow = row;
+            }
+        }
+
+        DescribeRow(worstRow, layout.KnotCount, stabilizationStart, out string category, out string unit);
+        return new LongitudinalQpAuditResult(allFinite && maximumResidual <= tolerance,
+            maximumResidual, worstRow, category, unit);
+    }
+
+    private static double[] ToPrimal(LongitudinalCandidate candidate, LongitudinalVariableLayout layout)
+    {
+        var primal = new double[layout.VariableCount];
+        for (int index = 0; index < layout.KnotCount; index++)
+        {
+            primal[layout.S(index)] = candidate.S[index];
+            primal[layout.U(index)] = candidate.U[index];
+            primal[layout.A(index)] = candidate.A[index];
+        }
+        for (int index = 0; index < layout.KnotCount - 1; index++)
+            primal[layout.J(index)] = candidate.J[index];
+        return primal;
+    }
+
+    private static void DescribeRow(int targetRow, int knotCount, int stabilizationStart,
+        out string category, out string unit)
+    {
+        int row = 0;
+        for (int index = 0; index < knotCount; index++)
+        {
+            if (targetRow == row++)
+            {
+                category = "PathS trust";
+                unit = "m";
+                return;
+            }
+            if (targetRow == row++)
+            {
+                category = "speed";
+                unit = "m/s";
+                return;
+            }
+            if (index > 0 && targetRow == row++)
+            {
+                category = "speed envelope";
+                unit = "m/s";
+                return;
+            }
+            if (targetRow == row++)
+            {
+                category = "acceleration";
+                unit = "m/s^2";
+                return;
+            }
+        }
+
+        for (int index = 0; index < knotCount - 1; index++)
+        {
+            if (targetRow == row++)
+            {
+                category = "jerk";
+                unit = "m/s^3";
+                return;
+            }
+        }
+        for (int index = 0; index < knotCount; index++)
+        {
+            if (targetRow == row++)
+            {
+                category = "low-speed deceleration release";
+                unit = "m/s";
+                return;
+            }
+        }
+        for (int index = 0; index < knotCount - 1; index++)
+        {
+            if (targetRow == row++)
+            {
+                category = "monotonic progress";
+                unit = "m";
+                return;
+            }
+        }
+        for (int index = 0; index < knotCount - 1; index++)
+        {
+            if (targetRow == row++)
+            {
+                category = "exact dynamics";
+                unit = "m/s^2";
+                return;
+            }
+            if (targetRow == row++)
+            {
+                category = "exact dynamics";
+                unit = "m/s";
+                return;
+            }
+            if (targetRow == row++)
+            {
+                category = "exact dynamics";
+                unit = "m";
+                return;
+            }
+        }
+
+        string[] stateUnits = { "m", "m/s", "m/s^2" };
+        for (int state = 0; state < stateUnits.Length; state++)
+        {
+            if (targetRow == row++)
+            {
+                category = "exact start";
+                unit = stateUnits[state];
+                return;
+            }
+        }
+
+        int stopKnotCount = stabilizationStart >= 0 && stabilizationStart < knotCount
+            ? knotCount - stabilizationStart
+            : 0;
+        for (int index = 0; index < stopKnotCount; index++)
+        {
+            for (int state = 0; state < stateUnits.Length; state++)
+            {
+                if (targetRow == row++)
+                {
+                    category = "exact stop";
+                    unit = stateUnits[state];
+                    return;
+                }
+            }
+        }
+
+        category = "row accounting";
+        unit = string.Empty;
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}