fix: stabilize stopped-start EM planning and add txt diagnostics

2026-08-07 15:51:31 +08:00
parent f2970d0aff
commit d378ad6257
13 changed files with 373 additions and 53 deletions
@@ -96,9 +96,9 @@ public sealed partial class EmPlannerConfiguration
            Validation = new ValidationConfiguration
            {
                SpatialToleranceMeters = 1e-8d,
-                KinematicTolerance = 1e-8d,
-                TerminalPositionToleranceMeters = 0.03d,
-                TerminalYawToleranceRadians = 5d * System.Math.PI / 180d,
+                KinematicTolerance = 1e-5d,
+                TerminalPositionToleranceMeters = 0d,
+                TerminalYawToleranceRadians = 0d,
            },
        };
    }
@@ -1,6 +1,7 @@
 using System;
 using System.Collections.Generic;
 using System.Diagnostics;
+using System.Globalization;
 using System.Threading;
 using MultiWheelC.TrajectoryPlanning.CoarsePath;

@@ -38,6 +39,21 @@ public sealed class SequentialLongitudinalOptimizer
            return Failed(EmPlanningStatus.InvalidInput, "Longitudinal planning input is required.");
        if (cancellationToken.IsCancellationRequested)
            return Failed(EmPlanningStatus.Cancelled, "Longitudinal optimization was cancelled.");
+        if (input.PlanningScope == EmPlanningScope.FullDirectionSegment &&
+            input.Mode == EmLongitudinalMode.ExactStopAtBoundary)
+        {
+            double staticStartSpeedTolerance = Math.Max(input.Configuration.Validation.SpatialToleranceMeters,
+                input.Configuration.Longitudinal.StopSpeedToleranceMetersPerSecond);
+            if (input.InitialProgressSpeedMetersPerSecond <= staticStartSpeedTolerance &&
+                Math.Abs(input.InitialAccelerationMetersPerSecondSquared) <=
+                    input.Configuration.Validation.KinematicTolerance)
+            {
+                input = new LongitudinalPlanningInput(input.Path, input.Direction, 0d,
+                    input.InitialAccelerationMetersPerSecondSquared, input.TerminalType, input.Mode,
+                    input.Configuration, input.PlanningScope, input.KnotSchedule,
+                    input.PreviousPathS, input.PreviousProgressSpeedMetersPerSecond);
+            }
+        }
        if (!TryCreateSettings(input, out QpSolverSettings settings, out TimeSpan totalBudget, out double convergenceTolerance,
                out int iterationLimit, out string configurationFailure))
        {
@@ -223,14 +239,27 @@ public sealed class SequentialLongitudinalOptimizer
        projectionSolveCount = 0;
        failureStatus = EmPlanningStatus.LongitudinalInfeasible;
        failureReason = string.Empty;
-        if (input.InitialProgressSpeedMetersPerSecond <= input.Configuration.Validation.SpatialToleranceMeters &&
+        double staticStartSpeedTolerance = Math.Max(input.Configuration.Validation.SpatialToleranceMeters,
+            input.Configuration.Longitudinal.StopSpeedToleranceMetersPerSecond);
+        bool staticStartEligible = input.InitialProgressSpeedMetersPerSecond <= staticStartSpeedTolerance &&
            Math.Abs(input.InitialAccelerationMetersPerSecondSquared) <=
-                input.Configuration.Validation.KinematicTolerance &&
-            TryCreateStaticStartSeed(input, speedLimit, out LongitudinalCandidate staticStartSeed))
+                input.Configuration.Validation.KinematicTolerance;
+        bool staticStartSeedUsed = false;
+        string staticStartSeedFailure = string.Empty;
+        if (staticStartEligible && TryCreateStaticStartSeed(input, speedLimit, out LongitudinalCandidate staticStartSeed,
+                out staticStartSeedFailure))
        {
+            staticStartSeedUsed = true;
            candidate = staticStartSeed;
            return true;
        }
+        string WithStaticSeedDiagnostic(string reason)
+        {
+            return staticStartEligible && !staticStartSeedUsed
+                ? "staticStartSeed=failed (" + staticStartSeedFailure + "); " + reason
+                : reason;
+        }
+
        LongitudinalCandidate linearizationIterate = CreateScheduleReferenceIterate(input);
        string lastRejection = string.Empty;
        for (int iteration = 0; iteration < iterationLimit; iteration++)
@@ -238,21 +267,21 @@ public sealed class SequentialLongitudinalOptimizer
            if (cancellationToken.IsCancellationRequested)
            {
                failureStatus = EmPlanningStatus.Cancelled;
-                failureReason = "Initial full-direction feasibility projection was cancelled.";
+                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection was cancelled.");
                return false;
            }
            TimeSpan remainingBudget = totalBudget - stopwatch.Elapsed;
            if (remainingBudget <= TimeSpan.Zero)
            {
                failureStatus = EmPlanningStatus.SolverTimedOut;
-                failureReason = "Initial full-direction feasibility projection exhausted the shared solve budget.";
+                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection exhausted the shared solve budget.");
                return false;
            }
            if (!_constraintBuilder.TryBuildInitialFeasibilityProjection(input, speedLimit, linearizationIterate,
                    out QuadraticProgram problem, out string buildFailure))
            {
                failureStatus = EmPlanningStatus.LongitudinalInfeasible;
-                failureReason = "Initial full-direction feasibility constraints are infeasible: " + buildFailure;
+                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility constraints are infeasible: " + buildFailure);
                return false;
            }

@@ -267,45 +296,45 @@ public sealed class SequentialLongitudinalOptimizer
            if (solved == null)
            {
                failureStatus = EmPlanningStatus.Failed;
-                failureReason = "The initial full-direction feasibility solver returned no result.";
+                failureReason = WithStaticSeedDiagnostic("The initial full-direction feasibility solver returned no result.");
                return false;
            }
            if (solved.Status == QpSolveStatus.TimeLimit || solved.Status == QpSolveStatus.MaximumIterations)
            {
                failureStatus = EmPlanningStatus.SolverTimedOut;
-                failureReason = "Initial full-direction feasibility projection timed out (status=" + solved.NativeStatus +
+                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection timed out (status=" + solved.NativeStatus +
                    ", iterations=" + solved.Iterations + ", primal=" + solved.PrimalResidual + ", dual=" +
-                    solved.DualResidual + "): " + solved.Diagnostic;
+                    solved.DualResidual + "): " + solved.Diagnostic);
                return false;
            }
            if (solved.Status == QpSolveStatus.Cancelled)
            {
                failureStatus = EmPlanningStatus.Cancelled;
-                failureReason = "Initial full-direction feasibility projection was cancelled: " + solved.Diagnostic;
+                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection was cancelled: " + solved.Diagnostic);
                return false;
            }
            if (solved.Status == QpSolveStatus.PrimalInfeasible || solved.Status == QpSolveStatus.DualInfeasible)
            {
                failureStatus = EmPlanningStatus.LongitudinalInfeasible;
-                failureReason = "Initial full-direction feasibility projection is infeasible: " + solved.Diagnostic;
+                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection is infeasible: " + solved.Diagnostic);
                return false;
            }
            if (solved.Status == QpSolveStatus.SolverUnavailable)
            {
                failureStatus = EmPlanningStatus.SolverUnavailable;
-                failureReason = "Initial full-direction feasibility solver is unavailable: " + solved.Diagnostic;
+                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility solver is unavailable: " + solved.Diagnostic);
                return false;
            }
            if (solved.Status != QpSolveStatus.Solved && solved.Status != QpSolveStatus.SolvedInaccurate)
            {
                failureStatus = EmPlanningStatus.Failed;
-                failureReason = "Initial full-direction feasibility solver failed: " + solved.Diagnostic;
+                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility solver failed: " + solved.Diagnostic);
                return false;
            }
            if (!TryCreateCandidate(input.KnotSchedule.KnotTimes, solved.Primal, out LongitudinalCandidate projected))
            {
                failureStatus = EmPlanningStatus.LongitudinalInfeasible;
-                failureReason = "Initial full-direction feasibility solver primal does not match the ST layout.";
+                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility solver primal does not match the ST layout.");
                return false;
            }
            if (solved.Status == QpSolveStatus.Solved || HasStrictResiduals(solved, convergenceTolerance))
@@ -319,7 +348,7 @@ public sealed class SequentialLongitudinalOptimizer
                if (validationStatus == EmPlanningStatus.NoProgress)
                {
                    failureStatus = validationStatus;
-                    failureReason = validationFailure;
+                    failureReason = WithStaticSeedDiagnostic(validationFailure);
                    return false;
                }
                lastRejection = validationFailure;
@@ -329,7 +358,7 @@ public sealed class SequentialLongitudinalOptimizer
                    out LongitudinalCandidate nextLinearization))
            {
                failureStatus = EmPlanningStatus.LongitudinalInfeasible;
-                failureReason = "Initial full-direction feasibility candidate could not be relinearized against the PathS envelope.";
+                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility candidate could not be relinearized against the PathS envelope.");
                return false;
            }
            linearizationIterate = nextLinearization;
@@ -339,8 +368,8 @@ public sealed class SequentialLongitudinalOptimizer
                lastRejection = "Initial feasibility projection violated the strict physical validator.";
        }
        failureStatus = EmPlanningStatus.LongitudinalInfeasible;
-        failureReason = "Initial full-direction feasibility projection exhausted the configured outer iterations. " +
-            lastRejection;
+        failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection exhausted the configured outer iterations. " +
+            lastRejection);
        return false;
    }

@@ -466,24 +495,31 @@ public sealed class SequentialLongitudinalOptimizer
    }

    private bool TryCreateStaticStartSeed(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
-        out LongitudinalCandidate candidate)
+        out LongitudinalCandidate candidate, out string failureReason)
    {
        candidate = null;
+        failureReason = "unknown";
        int stabilizationStart = input.KnotSchedule.TerminalHoldStartIndex;
        if (stabilizationStart < 5)
+        {
+            failureReason = "terminalHoldStartIndex=" + stabilizationStart.ToString(CultureInfo.InvariantCulture);
            return false;
+        }

        IReadOnlyList<double> times = input.KnotSchedule.KnotTimes;
        if (TryCreateExactJerkSeed(input, times, stabilizationStart, speedLimit, out candidate))
            return true;
+        failureReason = "exactJerkSeed=failed";
        double firstDuration = times[1] - times[0];
        double secondDuration = times[2] - times[1];
        LongitudinalConfiguration configuration = input.Configuration.Longitudinal;
        double maximumFirstJerk = Math.Min(configuration.MaximumJerkMetersPerSecondCubed,
            Math.Min(configuration.MaximumAccelerationMetersPerSecondSquared / firstDuration,
                configuration.MaximumJerkMetersPerSecondCubed * secondDuration / firstDuration));
-        for (int sample = 1; sample <= 256; sample++)
+        for (int sample = -256; sample <= 256; sample++)
        {
+            if (sample == 0)
+                continue;
            double firstJerk = maximumFirstJerk * sample / 256d;
            var jerk = new double[stabilizationStart];
            jerk[0] = firstJerk;
@@ -497,9 +533,11 @@ public sealed class SequentialLongitudinalOptimizer
                    out LongitudinalCandidate strict, out _))
            {
                candidate = strict;
+                failureReason = string.Empty;
                return true;
            }
        }
+        failureReason = "exactJerkSeed=failed; sampledSeeds=failed";
        return false;
    }

@@ -94,8 +94,8 @@ LS 的变量是 `l`、`dl`、`ddl` 和区间 `dddl`；它对静态走廊、导
 | 静态走廊、LS、ST 或停车条件不可行 | `CorridorInfeasible`、`LateralInfeasible`、`LongitudinalInfeasible`、`StoppingDistanceInsufficient` | 空 | 不发布；由上层决定重试、停车或重新选路 |
 | 求解器不可用、超时、取消或发布复核失败 | `SolverUnavailable`、`SolverTimedOut`、`Cancelled`、`ValidationFailed`、`Failed` | 空 | 读取 `FailureReason`；不得使用中间轨迹 |
 | 已被更新周期替代 | `Superseded` | 空 | 由滚动协调器忽略旧结果 |
-| 完整方向段无有效进度 | `NoProgress` | 空 | 不发布；仅真实终点容差内或受监督停车保持观察可合法零进度 |
-| 完整方向段终端位姿超差 | `TerminalPoseMismatch` | 空 | 不发布；位置误差超过 `0.03 m` 或归一化 yaw 误差超过 `5°` |
+| 完整方向段无有效进度 | `NoProgress` | 空 | 不发布；仅真实终点精确匹配或受监督停车保持观察可合法零进度 |
+| 完整方向段终端位姿超差 | `TerminalPoseMismatch` | 空 | 不发布；位置误差超过 `0 m` 或归一化 yaw 误差超过 `0 rad` |
 | 自适应 knots 或发布样本超限 | `FullSegmentResourceLimitExceeded` | 空 | 不发布；不得截断轨迹或回退滚动模式 |

 `FailureReason` 始终以 `map=...;reference=...;state=...;previous=...;segment=...` 开头，供调用方追踪地图、参考路径、状态、上一轨迹和方向段绑定。
@@ -165,7 +165,7 @@ EmTrajectory trajectory = result.Trajectory;

 - 对完整活动方向段做一次优化；`s_end` 来自已选 Local G2 的实际 `PathS`，`T_end` 由可行加速度、巡航、jerk 限速停止和零速 hold 推导，不依赖旧的距离/时间截断窗口。
 - 前进期望/硬上限为 `1.0 m/s`，倒车期望/硬上限为 `0.5 m/s`。
- 成功终端必须停止，世界位置误差不超过 `0.03 m`，归一化 yaw 误差不超过 `5°`（约 `0.0873 rad`）。
+- 成功终端必须停止，世界位置误差和归一化 yaw 误差均为 `0`（精确到达目标）。
 - `NoProgress`、`TerminalPoseMismatch`、`FullSegmentResourceLimitExceeded` 失败均返回空轨迹，不降级为滚动或截断发布。
 - `DistanceHorizonMeters` 与 `TimeHorizonSeconds` 作为兼容字段保留，但不用于截断 full 模式。

@@ -150,8 +150,8 @@ public static class EmPlanningRequestValidator
            !Positive(scheduling.MaximumOptimizationTimeStepSeconds) ||
            !Positive(scheduling.MaximumOptimizationSpatialStepMeters) ||
            scheduling.MaximumOptimizationKnotCount < 3 || scheduling.MaximumPublishedSampleCount < 2 ||
-            !Positive(validation.TerminalPositionToleranceMeters) ||
-            !Positive(validation.TerminalYawToleranceRadians) || validation.TerminalYawToleranceRadians > Math.PI ||
+            validation.TerminalPositionToleranceMeters < 0d ||
+            validation.TerminalYawToleranceRadians < 0d || validation.TerminalYawToleranceRadians > Math.PI ||
            !WeightsAreValid(configuration.Lateral.Weights) || !WeightsAreValid(configuration.Longitudinal.Weights))
            return false;