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

2026-08-11 20:35:59 +08:00
parent 569de5f13c
commit 1903e71fc1
522 changed files with 4188 additions and 119188 deletions
@@ -1,5 +1,6 @@
 using System;
 using System.Collections.Generic;
+using System.Globalization;

 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

@@ -16,7 +17,15 @@ public sealed class LongitudinalConstraintBuilder
    public bool TryBuild(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate iterate,
        out QuadraticProgram problem, out string failureReason)
    {
-        return TryBuildCore(input, speedLimit, iterate, false, out problem, out failureReason);
+        return TryBuildCore(input, speedLimit, iterate, false, null, null, 0d, out problem, out failureReason);
+    }
+
+    internal bool TryBuildTrusted(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
+        LongitudinalCandidate anchor, LongitudinalEnvelopeTrustRegion trustRegion, double strictTolerance,
+        out QuadraticProgram problem, out string failureReason)
+    {
+        return TryBuildCore(input, speedLimit, anchor, false, trustRegion, anchor, strictTolerance,
+            out problem, out failureReason);
    }

    /// <summary>Builds the bounded full-scope feasibility projection before objective optimization.</summary>
@@ -38,12 +47,14 @@ public sealed class LongitudinalConstraintBuilder
            failureReason = "Initial feasibility projection is only defined for full-direction exact-stop planning.";
            return false;
        }
-        return TryBuildCore(input, speedLimit, linearizationIterate, true, out problem,
-            out failureReason);
+        return TryBuildCore(input, speedLimit, linearizationIterate, true, null, null, 0d,
+            out problem, out failureReason);
    }

    private bool TryBuildCore(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate iterate,
-        bool useScheduleReferenceObjective, out QuadraticProgram problem, out string failureReason)
+        bool useScheduleReferenceObjective, LongitudinalEnvelopeTrustRegion trustRegion,
+        LongitudinalCandidate trustedAnchor, double strictTolerance, out QuadraticProgram problem,
+        out string failureReason)
    {
        problem = null;
        failureReason = string.Empty;
@@ -63,6 +74,15 @@ public sealed class LongitudinalConstraintBuilder
            {
                throw new ArgumentException("The ST iterate does not match the configured knot layout.");
            }
+            if ((trustRegion == null) != (trustedAnchor == null))
+                throw new ArgumentException("Trusted QP construction requires both a trust region and anchor.");
+            if (trustRegion != null && (trustRegion.MinimumPathS.Count != layout.KnotCount ||
+                trustRegion.MaximumPathS.Count != layout.KnotCount ||
+                trustRegion.SpeedSlope.Count != layout.KnotCount ||
+                trustRegion.SpeedIntercept.Count != layout.KnotCount))
+            {
+                throw new ArgumentException("The trust region does not match the configured knot layout.");
+            }
            if (!PathSpeedLimitBuilder.TryGetLimits(input, out double directionMaximum, out double maximumAcceleration,
                    out double maximumDeceleration, out double maximumJerk, out _, out _, out failureReason))
            {
@@ -84,12 +104,14 @@ public sealed class LongitudinalConstraintBuilder
                _objectiveBuilder.AddTerms(input, speedLimit, layout, iterate, hessian, linearCost);
            int stabilizationStart = GetStabilizationStart(input, expectedTimes, layout.KnotCount);
            int stationaryKnotCount = layout.KnotCount - stabilizationStart;
-            int expectedRows = 9 * layout.KnotCount - 3 + 3 * stationaryKnotCount;
+            int expectedRows = 10 * layout.KnotCount - 3 + 3 * stationaryKnotCount;
            var constraints = new SparseTripletBuilder(expectedRows, layout.VariableCount);
            var lower = new List<double>(expectedRows);
            var upper = new List<double>(expectedRows);
            int row = 0;
-            AddVariableBounds(input, speedLimit, iterate, layout, maximumAcceleration, maximumDeceleration, maximumJerk,
+            AddVariableBounds(input, speedLimit, iterate, trustRegion, layout, maximumAcceleration,
+                maximumDeceleration, maximumJerk, constraints, lower, upper, ref row);
+            AddLowSpeedDecelerationReleaseEnvelope(layout, iterate, maximumJerk,
                constraints, lower, upper, ref row);
            AddMonotonicProgress(layout, constraints, lower, upper, ref row);
            AddExactDynamics(expectedTimes, layout, constraints, lower, upper, ref row);
@@ -99,6 +121,20 @@ public sealed class LongitudinalConstraintBuilder
            if (row != expectedRows)
                throw new InvalidOperationException("ST constraint row accounting is inconsistent.");
            problem = new QuadraticProgram(hessian.Build(), linearCost, constraints.Build(), lower, upper);
+            if (trustedAnchor != null)
+            {
+                LongitudinalQpAuditResult audit = LongitudinalQpFeasibilityAudit.Evaluate(problem, trustedAnchor,
+                    strictTolerance, layout, stabilizationStart);
+                if (!audit.IsFeasible)
+                {
+                    problem = null;
+                    failureReason = "Planner invariant failure: strict anchor is outside trusted QP" +
+                        ";row=" + audit.WorstRow + ";category=" + audit.Category +
+                        ";residual=" + audit.MaximumResidual.ToString("R", CultureInfo.InvariantCulture) +
+                        audit.Unit + ";tolerance=" + strictTolerance.ToString("R", CultureInfo.InvariantCulture);
+                    return false;
+                }
+            }
            return true;
        }
        catch (ArgumentException exception)
@@ -145,7 +181,8 @@ public sealed class LongitudinalConstraintBuilder
    }

    private static void AddVariableBounds(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
-        LongitudinalCandidate iterate, LongitudinalVariableLayout layout, double maximumAcceleration,
+        LongitudinalCandidate iterate, LongitudinalEnvelopeTrustRegion trustRegion,
+        LongitudinalVariableLayout layout, double maximumAcceleration,
        double maximumDeceleration, double maximumJerk, SparseTripletBuilder constraints, IList<double> lower,
        IList<double> upper, ref int row)
    {
@@ -153,14 +190,32 @@ public sealed class LongitudinalConstraintBuilder
        {
            if (iterate.S[index] < 0d || iterate.S[index] > input.PathUpperBoundS)
                throw new ArgumentException("The ST iterate progress lies outside actual PathS bounds.");
-            AddSingleVariableRow(constraints, lower, upper, layout.S(index), 0d, input.PathUpperBoundS, ref row);
+            if (trustRegion == null)
+                AddSingleVariableRow(constraints, lower, upper, layout.S(index), 0d, input.PathUpperBoundS, ref row);
+            else
+                AddSingleVariableRow(constraints, lower, upper, layout.S(index),
+                    trustRegion.MinimumPathS[index], trustRegion.MaximumPathS[index], ref row);
            double maximumSpeed = index == 0
                ? input.DirectionMaximumSpeedMetersPerSecond
                : input.DirectionMaximumSpeedMetersPerSecond;
            AddSingleVariableRow(constraints, lower, upper, layout.U(index), 0d, maximumSpeed, ref row);
            if (index > 0)
-                AddLinearizedSpeedEnvelopeRow(speedLimit, iterate.S[index], layout.S(index), layout.U(index),
-                    constraints, lower, upper, ref row);
+            {
+                if (trustRegion == null)
+                {
+                    AddLinearizedSpeedEnvelopeRow(speedLimit, iterate.S[index], layout.S(index), layout.U(index),
+                        constraints, lower, upper, ref row);
+                }
+                else
+                {
+                    AddRow(constraints, lower, upper, row, new[]
+                    {
+                        new Coefficient(layout.U(index), 1d),
+                        new Coefficient(layout.S(index), -trustRegion.SpeedSlope[index]),
+                    }, -QuadraticProgram.MaximumFiniteBound, trustRegion.SpeedIntercept[index]);
+                    row++;
+                }
+            }
            AddSingleVariableRow(constraints, lower, upper, layout.A(index), -maximumDeceleration, maximumAcceleration,
                ref row);
        }
@@ -209,6 +264,34 @@ public sealed class LongitudinalConstraintBuilder
        }
    }

+    internal static void CalculateLowSpeedDecelerationReleaseTangent(
+        double anchorAcceleration, double maximumJerk,
+        out double accelerationCoefficient, out double lowerBound)
+    {
+        if (!IsFinite(anchorAcceleration) || !IsFinite(maximumJerk) || maximumJerk <= 0d)
+            throw new ArgumentOutOfRangeException(nameof(anchorAcceleration));
+        double a0 = Math.Min(0d, anchorAcceleration);
+        accelerationCoefficient = -a0 / maximumJerk;
+        lowerBound = -(a0 * a0) / (2d * maximumJerk);
+    }
+
+    private static void AddLowSpeedDecelerationReleaseEnvelope(LongitudinalVariableLayout layout,
+        LongitudinalCandidate iterate, double maximumJerk, SparseTripletBuilder constraints,
+        IList<double> lower, IList<double> upper, ref int row)
+    {
+        for (int index = 0; index < layout.KnotCount; index++)
+        {
+            CalculateLowSpeedDecelerationReleaseTangent(iterate.A[index], maximumJerk,
+                out double accelerationCoefficient, out double lowerBound);
+            AddRow(constraints, lower, upper, row, new[]
+            {
+                new Coefficient(layout.U(index), 1d),
+                new Coefficient(layout.A(index), accelerationCoefficient),
+            }, lowerBound, QuadraticProgram.MaximumFiniteBound);
+            row++;
+        }
+    }
+
    private static void AddExactDynamics(IReadOnlyList<double> times, LongitudinalVariableLayout layout,
        SparseTripletBuilder constraints, IList<double> lower, IList<double> upper, ref int row)
    {