重构横向控制器输出前后GCP目标转角并简化命令分配器

Co-authored-by: Cursor <cursoragent@cursor.com>

MultiWheelC/Control/Abstractions/LateralControlCommand.cs

@@ -3,37 +3,62 @@ using System;
 namespace MultiWheelC.Control.Abstractions
 {
     /// <summary>
-    /// 表示横向控制器生成的车体中心目标曲率命令。
+    /// 表示横向控制器生成的前、后GCP目标转角，单位为rad，逆时针为正。
     /// </summary>
     public readonly struct LateralControlCommand
     {
         /// <summary>
-        /// 创建统一使用SI单位和左转为正约定的横向控制命令。
+        /// 创建前、后GCP目标转角命令。
         /// </summary>
         public LateralControlCommand(
-            double targetCurvaturePerMeter)
+            double frontGcpAngleRadians,
+            double rearGcpAngleRadians)
         {
             EnsureFinite(
-                targetCurvaturePerMeter,
-                nameof(targetCurvaturePerMeter));
+                frontGcpAngleRadians,
+                nameof(frontGcpAngleRadians));
+            EnsureFinite(
+                rearGcpAngleRadians,
+                nameof(rearGcpAngleRadians));
 
-            TargetCurvaturePerMeter =
-                targetCurvaturePerMeter;
+            FrontGcpAngleRadians =
+                frontGcpAngleRadians;
+            RearGcpAngleRadians =
+                rearGcpAngleRadians;
         }
 
         /// <summary>
-        /// 获取车体中心目标轨迹曲率，单位为1/m，左转为正、右转为负。
+        /// 获取前GCP目标转角，单位为rad，逆时针为正。
         /// </summary>
-        public double TargetCurvaturePerMeter { get; }
+        public double FrontGcpAngleRadians { get; }
 
         /// <summary>
-        /// 创建保持直线行驶的零曲率命令。
+        /// 获取后GCP目标转角，单位为rad，逆时针为正。
+        /// </summary>
+        public double RearGcpAngleRadians { get; }
+
+        /// <summary>
+        /// 获取前后GCP的共同转角分量，主要用于横向平移修正。
+        /// </summary>
+        public double CommonAngleRadians =>
+            (FrontGcpAngleRadians +
+             RearGcpAngleRadians) / 2.0;
+
+        /// <summary>
+        /// 获取前后GCP的差动转角分量，主要用于曲率前馈和航向修正。
+        /// </summary>
+        public double DifferentialAngleRadians =>
+            (FrontGcpAngleRadians -
+             RearGcpAngleRadians) / 2.0;
+
+        /// <summary>
+        /// 创建前后GCP均保持车头方向的直线命令。
         /// </summary>
         public static LateralControlCommand Straight =>
-            new LateralControlCommand(0.0);
+            new LateralControlCommand(0.0, 0.0);
 
         /// <summary>
-        /// 检查横向曲率命令是否为有限值。
+        /// 检查GCP目标转角是否为有限值。
         /// </summary>
         private static void EnsureFinite(
             double value,
@@ -44,7 +69,7 @@ namespace MultiWheelC.Control.Abstractions
             {
                 throw new ArgumentOutOfRangeException(
                     parameterName,
-                    "横向控制目标曲率必须是有限值。");
+                    "GCP目标转角必须是有限值。");
             }
         }
     }

MultiWheelC/Control/Allocation/GcpCommandAllocator.cs

@@ -4,57 +4,36 @@ using MultiWheelC.Control.Abstractions;
 namespace MultiWheelC.Control.Allocation
 {
     /// <summary>
-    /// 将车体中心目标曲率按对称前后转向策略转换为旧版底盘的前后GCP方向。
+    /// 独立限制前后GCP目标转角并与纵向速度组合成底盘运动命令。
     /// </summary>
-    public sealed class AckermannGcpAllocator
+    public sealed class GcpCommandAllocator
     {
         /// <summary>
-        /// 创建使用指定GCP半间距和最大GCP转角的对称转向分配器。
+        /// 创建使用指定前后GCP最大转角的命令分配器。
         /// </summary>
-        public AckermannGcpAllocator(
-            double controlPointRadiusMeters,
-            double maximumGcpAngleRadians)
+        public GcpCommandAllocator(double maximumGcpAngleRadians)
         {
-            EnsureFinitePositive(
-                controlPointRadiusMeters,
-                nameof(controlPointRadiusMeters));
             EnsureFinitePositive(
                 maximumGcpAngleRadians,
                 nameof(maximumGcpAngleRadians));
 
-            if (maximumGcpAngleRadians >=
-                Math.PI / 2.0)
+            if (maximumGcpAngleRadians >= Math.PI / 2.0)
             {
                 throw new ArgumentOutOfRangeException(
                     nameof(maximumGcpAngleRadians),
-                    "最大GCP转角必须小于π/2，避免曲率换算出现奇异值。");
+                    "最大GCP转角必须小于π/2。");
             }
 
-            ControlPointRadiusMeters =
-                controlPointRadiusMeters;
-            MaximumGcpAngleRadians =
-                maximumGcpAngleRadians;
+            MaximumGcpAngleRadians = maximumGcpAngleRadians;
         }
 
-        /// <summary>
-        /// 获取车体中心到前、后GCP的距离，单位为m。
-        /// </summary>
-        public double ControlPointRadiusMeters { get; }
-
         /// <summary>
         /// 获取前后GCP允许的最大转角绝对值，单位为rad。
         /// </summary>
         public double MaximumGcpAngleRadians { get; }
 
         /// <summary>
-        /// 获取当前GCP几何和转角限制允许的最大车体中心曲率，单位为1/m。
-        /// </summary>
-        public double MaximumCurvaturePerMeter =>
-            Math.Tan(MaximumGcpAngleRadians) /
-            ControlPointRadiusMeters;
-
-        /// <summary>
-        /// 将纵向命令速度和车体中心目标曲率分配为前后GCP运动命令。
+        /// 将纵向速度和前后GCP转角组合为底盘运动命令。
         /// </summary>
         public GcpMotionCommand Allocate(
             double speedMetersPerSecond,
@@ -64,19 +43,12 @@ namespace MultiWheelC.Control.Allocation
                 speedMetersPerSecond,
                 nameof(speedMetersPerSecond));
 
-            var limitedCurvaturePerMeter =
-                Clamp(
-                    lateralCommand
-                        .TargetCurvaturePerMeter,
-                    -MaximumCurvaturePerMeter,
-                    MaximumCurvaturePerMeter);
-
-            var frontAngleRadians =
-                Math.Atan(
-                    limitedCurvaturePerMeter *
-                    ControlPointRadiusMeters);
-            var rearAngleRadians =
-                -frontAngleRadians;
+            var frontAngleRadians = ClampSymmetric(
+                lateralCommand.FrontGcpAngleRadians,
+                MaximumGcpAngleRadians);
+            var rearAngleRadians = ClampSymmetric(
+                lateralCommand.RearGcpAngleRadians,
+                MaximumGcpAngleRadians);
 
             return new GcpMotionCommand(
                 speedMetersPerSecond,
@@ -85,16 +57,15 @@ namespace MultiWheelC.Control.Allocation
         }
 
         /// <summary>
-        /// 将数值限制在指定闭区间内。
+        /// 将数值按正负对称方式限制在指定绝对值内。
         /// </summary>
-        private static double Clamp(
+        private static double ClampSymmetric(
             double value,
-            double minimum,
-            double maximum)
+            double maximumAbsoluteValue)
         {
             return Math.Max(
-                minimum,
-                Math.Min(maximum, value));
+                -maximumAbsoluteValue,
+                Math.Min(maximumAbsoluteValue, value));
         }
 
         /// <summary>

MultiWheelC/Control/Execution/ParkingGeometricController.cs

@@ -28,12 +28,12 @@ namespace MultiWheelC.Control.Execution
             1e-6;
         private const double StartupRegionMeters = 0.02;
         private const double StartupPreviewDistanceMeters = 0.05;
-        private const double MaximumStartupSpeedMetersPerSecond = 0.05;
+        private const double MaximumStartupSpeedMetersPerSecond = 0.08;
 
         private readonly IVehicleStateProvider _stateProvider;
         private readonly ILateralController _lateralController;
         private readonly ILongitudinalController _longitudinalController;
-        private readonly AckermannGcpAllocator _gcpAllocator;
+        private readonly GcpCommandAllocator _gcpAllocator;
         private readonly GcpCommandExecutor _commandExecutor;
 
         private Trajectory2D _trajectory;
@@ -45,7 +45,7 @@ namespace MultiWheelC.Control.Execution
             IVehicleStateProvider stateProvider,
             ILateralController lateralController,
             ILongitudinalController longitudinalController,
-            AckermannGcpAllocator gcpAllocator,
+            GcpCommandAllocator gcpAllocator,
             GcpCommandExecutor commandExecutor,
             double finishDistanceMeters = 0.03,
             double finishSpeedMetersPerSecond = 0.02,

MultiWheelC/Control/Lateral/StanleyLateralController.cs

@@ -4,22 +4,23 @@ using MultiWheelC.Control.Abstractions;
 namespace MultiWheelC.Control.Lateral
 {
     /// <summary>
-    /// 使用参考曲率前馈、航向误差和横向误差计算车体中心目标曲率。
+    /// 将参考曲率、横向误差和航向误差分别转换为前、后GCP目标转角。
     /// </summary>
     public sealed class StanleyLateralController : ILateralController
     {
-        private const double MaximumMathematicalAngleRadians =
-            Math.PI / 2.0 - 1e-3;
-
         /// <summary>
-        /// 创建使用指定GCP几何、Stanley增益和低速保护参数的横向控制器。
+        /// 创建使用指定GCP几何、Stanley增益和转角保护参数的横向控制器。
         /// </summary>
         public StanleyLateralController(
             double controlPointRadiusMeters,
             double crossTrackGainPerSecond,
             double headingErrorGain,
             double minimumSpeedMetersPerSecond,
-            bool useActualSpeedForGain = true)
+            bool useActualSpeedForGain = true,
+            double maximumCrossTrackCorrectionRadians =
+                10.0 * Math.PI / 180.0,
+            double maximumHeadingCorrectionRadians =
+                10.0 * Math.PI / 180.0)
         {
             EnsureFinitePositive(
                 controlPointRadiusMeters,
@@ -33,12 +34,22 @@ namespace MultiWheelC.Control.Lateral
             EnsureFinitePositive(
                 minimumSpeedMetersPerSecond,
                 nameof(minimumSpeedMetersPerSecond));
+            EnsureFinitePositive(
+                maximumCrossTrackCorrectionRadians,
+                nameof(maximumCrossTrackCorrectionRadians));
+            EnsureFinitePositive(
+                maximumHeadingCorrectionRadians,
+                nameof(maximumHeadingCorrectionRadians));
 
             ControlPointRadiusMeters = controlPointRadiusMeters;
             CrossTrackGainPerSecond = crossTrackGainPerSecond;
             HeadingErrorGain = headingErrorGain;
             MinimumSpeedMetersPerSecond = minimumSpeedMetersPerSecond;
             UseActualSpeedForGain = useActualSpeedForGain;
+            MaximumCrossTrackCorrectionRadians =
+                maximumCrossTrackCorrectionRadians;
+            MaximumHeadingCorrectionRadians =
+                maximumHeadingCorrectionRadians;
         }
 
         /// <summary>
@@ -62,12 +73,22 @@ namespace MultiWheelC.Control.Lateral
         public double MinimumSpeedMetersPerSecond { get; }
 
         /// <summary>
-        /// 获取是否优先使用Detour估算的实际纵向速度计算横向误差项。
+        /// 获取是否优先使用Detour估算的实际纵向速度计算横向修正。
         /// </summary>
         public bool UseActualSpeedForGain { get; }
 
         /// <summary>
-        /// 根据参考曲率、航向误差和横向误差计算车体中心目标曲率。
+        /// 获取横向误差共同转角分量的最大绝对值，单位为rad。
+        /// </summary>
+        public double MaximumCrossTrackCorrectionRadians { get; }
+
+        /// <summary>
+        /// 获取航向误差差动转角分量的最大绝对值，单位为rad。
+        /// </summary>
+        public double MaximumHeadingCorrectionRadians { get; }
+
+        /// <summary>
+        /// 分别计算横向共同转角以及曲率和航向差动转角，并生成前后GCP命令。
         /// </summary>
         public LateralControlCommand Compute(
             PathTrackingContext context)
@@ -78,33 +99,40 @@ namespace MultiWheelC.Control.Lateral
                 MinimumSpeedMetersPerSecond);
             var travelDirection = SelectTravelDirection(context);
 
-            // 参考曲率提供前馈；没有跟踪误差时也能沿曲线行驶。
+            // 参考曲率决定前后反向的差动转角，使无跟踪误差时也能沿曲线行驶。
             var feedforwardAngleRadians = Math.Atan(
                 context.ReferenceCurvaturePerMeter *
                 ControlPointRadiusMeters);
 
-            // 轨迹位于车辆左侧时横向误差为正，对应正的左转修正。
-            var crossTrackCorrectionRadians = Math.Atan(
+            // 横向误差生成前后同向的共同转角，使四舵轮车辆平稳靠近轨迹。
+            var crossTrackCorrectionRadians =
+                ClampSymmetric(
+                    Math.Atan(
                         CrossTrackGainPerSecond *
                         context.LateralErrorMeters /
-                speedMagnitude);
+                        speedMagnitude),
+                    MaximumCrossTrackCorrectionRadians);
 
-            // 倒车时需要反转反馈修正方向；参考曲率前馈仍由轨迹本身决定。
-            var feedbackAngleRadians = travelDirection *
-                (HeadingErrorGain * context.HeadingErrorRadians +
-                 crossTrackCorrectionRadians);
+            // 航向误差生成前后反向的差动转角，只负责调整车身朝向。
+            var headingCorrectionRadians =
+                ClampSymmetric(
+                    HeadingErrorGain *
+                    context.HeadingErrorRadians,
+                    MaximumHeadingCorrectionRadians);
 
-            // 这里只避开tan奇点，实际GCP机械限制由AckermannGcpAllocator处理。
-            var targetEquivalentAngleRadians = Clamp(
-                feedforwardAngleRadians + feedbackAngleRadians,
-                -MaximumMathematicalAngleRadians,
-                MaximumMathematicalAngleRadians);
-            var targetCurvaturePerMeter = Math.Tan(
-                targetEquivalentAngleRadians) /
-                ControlPointRadiusMeters;
+            var commonAngleRadians =
+                travelDirection *
+                crossTrackCorrectionRadians;
+            var differentialAngleRadians =
+                feedforwardAngleRadians +
+                travelDirection *
+                headingCorrectionRadians;
 
             return new LateralControlCommand(
-                targetCurvaturePerMeter);
+                commonAngleRadians +
+                differentialAngleRadians,
+                commonAngleRadians -
+                differentialAngleRadians);
         }
 
         /// <summary>
@@ -115,7 +143,7 @@ namespace MultiWheelC.Control.Lateral
         }
 
         /// <summary>
-        /// 选择Stanley横向误差项使用的实际速度或旧版参考速度。
+        /// 选择Stanley横向误差项使用的实际速度或参考速度。
         /// </summary>
         private double SelectSpeedForGain(
             PathTrackingContext context)
@@ -131,7 +159,7 @@ namespace MultiWheelC.Control.Lateral
         }
 
         /// <summary>
-        /// 根据有符号参考速度确定前进或倒车的反馈修正方向。
+        /// 根据有符号参考速度确定前进或倒车时的反馈修正方向。
         /// </summary>
         private static double SelectTravelDirection(
             PathTrackingContext context)
@@ -158,16 +186,15 @@ namespace MultiWheelC.Control.Lateral
         }
 
         /// <summary>
-        /// 将数值限制在指定闭区间内。
+        /// 将数值按正负对称方式限制在指定绝对值内。
         /// </summary>
-        private static double Clamp(
+        private static double ClampSymmetric(
             double value,
-            double minimum,
-            double maximum)
+            double maximumAbsoluteValue)
         {
             return Math.Max(
-                minimum,
-                Math.Min(maximum, value));
+                -maximumAbsoluteValue,
+                Math.Min(maximumAbsoluteValue, value));
         }
 
         /// <summary>
@@ -183,7 +210,7 @@ namespace MultiWheelC.Control.Lateral
             {
                 throw new ArgumentOutOfRangeException(
                     parameterName,
-                    "Stanley控制器的几何尺寸和最小速度必须是正有限值。");
+                    "Stanley控制器的几何尺寸、速度和角度限制必须是正有限值。");
             }
         }
 

MultiWheelC/Control/Longitudinal/PidLongitudinalController.cs

@@ -23,11 +23,15 @@ namespace MultiWheelC.Control.Longitudinal
             double integralGainPerSecond,
             double derivativeGainSeconds,
             double maximumIntegralCorrectionMetersPerSecond,
-            double maximumCommandSpeedMetersPerSecond)
+            double maximumCommandSpeedMetersPerSecond,
+            double speedErrorDeadbandMetersPerSecond = 0.025)
         {
             EnsureFinitePositive(
                 maximumCommandSpeedMetersPerSecond,
                 nameof(maximumCommandSpeedMetersPerSecond));
+            EnsureFiniteNonNegative(
+                speedErrorDeadbandMetersPerSecond,
+                nameof(speedErrorDeadbandMetersPerSecond));
 
             _feedbackPid = new PidController(
                 proportionalGain,
@@ -37,6 +41,8 @@ namespace MultiWheelC.Control.Longitudinal
                 derivativeOnMeasurement: true);
             MaximumCommandSpeedMetersPerSecond =
                 maximumCommandSpeedMetersPerSecond;
+            SpeedErrorDeadbandMetersPerSecond =
+                speedErrorDeadbandMetersPerSecond;
         }
 
         /// <summary>
@@ -49,6 +55,11 @@ namespace MultiWheelC.Control.Longitudinal
         /// </summary>
         public double MaximumCommandSpeedMetersPerSecond { get; }
 
+        /// <summary>
+        /// 获取不触发纵向PID修正的速度误差死区，单位为m/s。
+        /// </summary>
+        public double SpeedErrorDeadbandMetersPerSecond { get; }
+
         /// <summary>
         /// 获取最近一次有效控制周期的参考速度减实际速度，单位为m/s。
         /// </summary>
@@ -98,6 +109,20 @@ namespace MultiWheelC.Control.Longitudinal
                     referenceSpeedMetersPerSecond);
             }
 
+            var speedErrorMetersPerSecond =
+                referenceSpeedMetersPerSecond -
+                context.ActualLongitudinalSpeedMetersPerSecond;
+
+            // Detour差分速度在参考速度附近会有小幅波动；死区内只使用速度前馈，
+            // 同时清除PID历史，避免噪声持续积累后产生突发修正。
+            if (Math.Abs(speedErrorMetersPerSecond) <=
+                SpeedErrorDeadbandMetersPerSecond)
+            {
+                Reset();
+                return LimitReferenceSpeed(
+                    referenceSpeedMetersPerSecond);
+            }
+
             GetCorrectionOutputRange(
                 referenceSpeedMetersPerSecond,
                 out var minimumCorrectionMetersPerSecond,
@@ -178,5 +203,22 @@ namespace MultiWheelC.Control.Longitudinal
                     "纵向控制器最大命令速度必须是正有限值。");
             }
         }
+
+        /// <summary>
+        /// 检查速度误差死区是否为非负有限值。
+        /// </summary>
+        private static void EnsureFiniteNonNegative(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value) ||
+                value < 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "纵向控制器速度误差死区必须是非负有限值。");
+            }
+        }
     }
 }

MultiWheelC/Experiments/ClampTests.cs

@@ -1,90 +1,90 @@
 
 
-using System;
-using ClumsyCore;
-using ClumsyCore.Pilot;
-using FundamentalLib;
-using MDCSToolBox.Clumsy.Movements;
-using MDCSToolBox.Clumsy.Pilot;
+// using System;
+// using ClumsyCore;
+// using ClumsyCore.Pilot;
+// using FundamentalLib;
+// using MDCSToolBox.Clumsy.Movements;
+// using MDCSToolBox.Clumsy.Pilot;
 
-namespace MultiWheelC
-{
-    public abstract class ClampMovementTestBase : MovementTest
-    {
-        public float TimeoutSeconds = 30f; // 动作超时时间，单位s。
+// namespace MultiWheelC
+// {
+//     public abstract class ClampMovementTestBase : MovementTest
+//     {
+//         public float TimeoutSeconds = 30f; // 动作超时时间，单位s。
 
-        private DriveTask _task;
-        protected abstract bool Close { get; }
+//         private DriveTask _task;
+//         protected abstract bool Close { get; }
 
-        // 根据派生测试类型驱动左右夹臂同步夹紧或打开。
-        public override void Test()
-        {
-            var leftTarget = Close
-                ? PilotDefinition.Self.LeftArmUpperPos
-                : PilotDefinition.Self.LeftArmLowerPos;
-            var rightTarget = Close
-                ? PilotDefinition.Self.RightArmUpperPos
-                : PilotDefinition.Self.RightArmLowerPos;
+//         // 根据派生测试类型驱动左右夹臂同步夹紧或打开。
+//         public override void Test()
+//         {
+//             var leftTarget = Close
+//                 ? PilotDefinition.Self.LeftArmUpperPos
+//                 : PilotDefinition.Self.LeftArmLowerPos;
+//             var rightTarget = Close
+//                 ? PilotDefinition.Self.RightArmUpperPos
+//                 : PilotDefinition.Self.RightArmLowerPos;
 
-            if (float.IsNaN(leftTarget) ||
-                float.IsInfinity(leftTarget) ||
-                float.IsNaN(rightTarget) ||
-                float.IsInfinity(rightTarget))
-            {
-                Console.WriteLine(
-                    "夹臂目标位置无效，取消夹臂运动测试。");
-                return;
-            }
+//             if (float.IsNaN(leftTarget) ||
+//                 float.IsInfinity(leftTarget) ||
+//                 float.IsNaN(rightTarget) ||
+//                 float.IsInfinity(rightTarget))
+//             {
+//                 Console.WriteLine(
+//                     "夹臂目标位置无效，取消夹臂运动测试。");
+//                 return;
+//             }
 
-            // 防止重复点击时上一项夹臂任务仍在运行。
-            TestStop();
-            Console.WriteLine(
-                $"开始夹臂{(Close ? "夹紧" : "打开")}测试：" +
-                $"左目标={leftTarget}，右目标={rightTarget}");
+//             // 防止重复点击时上一项夹臂任务仍在运行。
+//             TestStop();
+//             Console.WriteLine(
+//                 $"开始夹臂{(Close ? "夹紧" : "打开")}测试：" +
+//                 $"左目标={leftTarget}，右目标={rightTarget}");
 
-            var task = new DriveTask(
-                new ClampToTarget
-                {
-                    LeftClampTarget = leftTarget,
-                    RightClampTarget = rightTarget,
-                    TimeoutSeconds = TimeoutSeconds
-                }.Get());
-            _task = task;
+//             var task = new DriveTask(
+//                 new ClampToTarget
+//                 {
+//                     LeftClampTarget = leftTarget,
+//                     RightClampTarget = rightTarget,
+//                     TimeoutSeconds = TimeoutSeconds
+//                 }.Get());
+//             _task = task;
 
-            try
-            {
-                task.Wait();
-            }
-            finally
-            {
-                PilotDefinition.Self.SpeedLeftArm = 0f;
-                PilotDefinition.Self.SpeedRightArm = 0f;
-                if (ReferenceEquals(_task, task))
-                    _task = null;
-            }
-        }
+//             try
+//             {
+//                 task.Wait();
+//             }
+//             finally
+//             {
+//                 PilotDefinition.Self.SpeedLeftArm = 0f;
+//                 PilotDefinition.Self.SpeedRightArm = 0f;
+//                 if (ReferenceEquals(_task, task))
+//                     _task = null;
+//             }
+//         }
 
-        // 停止夹臂任务并立即清零左右夹臂下发速度。
-        public override void TestStop()
-        {
-            _task?.Stop();
-            _task = null;
-            PilotDefinition.Self.SpeedLeftArm = 0f;
-            PilotDefinition.Self.SpeedRightArm = 0f;
-        }
-    }
+//         // 停止夹臂任务并立即清零左右夹臂下发速度。
+//         public override void TestStop()
+//         {
+//             _task?.Stop();
+//             _task = null;
+//             PilotDefinition.Self.SpeedLeftArm = 0f;
+//             PilotDefinition.Self.SpeedRightArm = 0f;
+//         }
+//     }
 
-    [MovementTest(name = "夹臂关闭测试")]
-    public sealed class TestClampCloseMovement
-        : ClampMovementTestBase
-    {
-        protected override bool Close => false;
-    }
+//     [MovementTest(name = "夹臂关闭测试")]
+//     public sealed class TestClampCloseMovement
+//         : ClampMovementTestBase
+//     {
+//         protected override bool Close => false;
+//     }
 
-    [MovementTest(name = "夹臂启动测试")]
-    public sealed class TestClampOpenMovement
-        : ClampMovementTestBase
-    {
-        protected override bool Close => true;
-    }
-}
+//     [MovementTest(name = "夹臂启动测试")]
+//     public sealed class TestClampOpenMovement
+//         : ClampMovementTestBase
+//     {
+//         protected override bool Close => true;
+//     }
+// }

MultiWheelC/Experiments/CompositeMotionPlanTests.cs

@@ -0,0 +1,332 @@
+using System;
+using System.Drawing;
+using System.Numerics;
+using ClumsyCore;
+using ClumsyCore.DTools;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using CommonUsage.Chassis;
+using MultiWheelC.Control.Execution;
+using MultiWheelC.StateEstimation;
+using MultiWheelC.Trajectory;
+using MyParking.Shared;
+
+namespace MultiWheelC
+{
+    /// <summary>
+    /// 测试平滑左转轨迹、停车原地左转90°和再次直行的组合运动执行过程。
+    /// </summary>
+    [MovementTest(name = "新版控制器：曲线-停车自转-直线组合测试")]
+    public sealed class CompositeStopTurnGoTest : MovementTest
+    {
+        private const float MillimetersPerMeter = 1000f;
+
+        private readonly Painter _painter =
+            UI.GetPainter("CompositeStopTurnGoTest");
+
+        private DriveTask _task;
+        private TrackingExperimentRecorder _recorder;
+
+        public int TrialNumber = 1; // 重复实验编号。
+        public double StraightLengthMeters = 2.0; // 圆弧前后直线长度，单位m。
+        public double TurnRadiusMeters = 2.0; // 平滑左转名义半径，单位m。
+        public double TurnAngleDegrees = 90.0; // 含过渡段在内的总左转角度。
+        public double CurvatureTransitionLengthMeters = 0.80; // 单侧过渡长度，单位m。
+        public double InPlaceLeftTurnDegrees = 90.0; // 停车后的原地左转角度。
+        public double FinalStraightLengthMeters = 4.5; // 自转后的直线长度，单位m。
+        public double StraightMaximumSpeedMetersPerSecond = 0.40; // 直线限速。
+        public double CurveMaximumSpeedMetersPerSecond = 0.30; // 转弯和过渡段限速。
+        public double AccelerationMetersPerSecondSquared = 0.20; // 参考加速度。
+        public double DecelerationMetersPerSecondSquared = 0.12; // 参考减速度。
+        public double PointSpacingMeters = 0.02; // 离散轨迹点间距。
+
+        /// <summary>
+        /// 从当前Detour位姿构造完整计划并依次执行连续跟踪、原地自转和最终直线。
+        /// </summary>
+        public override void Test()
+        {
+            if (_task != null)
+            {
+                Console.WriteLine(
+                    "曲线-停车自转-直线组合测试已经在运行。");
+                return;
+            }
+
+            if (!MovementTestPreparation.AreWheelsForward())
+            {
+                return;
+            }
+
+            var chassis = PilotDefinition.Chassis as MultiWheelChassis;
+            if (chassis == null)
+            {
+                Console.WriteLine(
+                    "当前底盘不是MultiWheelChassis，无法执行组合运动测试。");
+                return;
+            }
+
+            var stateProvider = new DetourVehicleStateProvider();
+            if (!stateProvider.TryGetState(out var initialState))
+            {
+                Console.WriteLine(
+                    "无法读取组合运动起点位姿：" +
+                    stateProvider.LastFailureReason);
+                return;
+            }
+
+            var firstTrajectory =
+                TestTrajectoryFactory
+                    .CreateStraightSmoothLeftTurnStraight(
+                        initialState.PoseInWorld,
+                        StraightLengthMeters,
+                        TurnRadiusMeters,
+                        AngleMath.DegreesToRadians(
+                            TurnAngleDegrees),
+                        CurvatureTransitionLengthMeters,
+                        StraightMaximumSpeedMetersPerSecond,
+                        CurveMaximumSpeedMetersPerSecond,
+                        AccelerationMetersPerSecondSquared,
+                        DecelerationMetersPerSecondSquared,
+                        PointSpacingMeters);
+
+            var firstStopPose =
+                firstTrajectory.EndPoint.PoseInWorld;
+            var finalStraightYawRadians =
+                AngleMath.NormalizeRadians(
+                    firstStopPose.YawRadians +
+                    AngleMath.DegreesToRadians(
+                        InPlaceLeftTurnDegrees));
+            var finalStraightStartPose =
+                new Pose2D(
+                    firstStopPose.XMeters,
+                    firstStopPose.YMeters,
+                    finalStraightYawRadians);
+            var finalTrajectory =
+                TestTrajectoryFactory.CreateStraight(
+                    finalStraightStartPose,
+                    FinalStraightLengthMeters,
+                    StraightMaximumSpeedMetersPerSecond,
+                    AccelerationMetersPerSecondSquared,
+                    DecelerationMetersPerSecondSquared,
+                    PointSpacingMeters);
+
+            DrawPlan(
+                firstTrajectory,
+                finalTrajectory,
+                firstStopPose);
+
+            var plan = new MotionPlanSegment[]
+            {
+                new TrackMotionPlanSegment(firstTrajectory)
+                {
+                    // 中间停车点允许后续原地转向和末段跟踪继续收敛位置误差。
+                    FinishDistanceMeters = 0.05,
+                    FinishSpeedMetersPerSecond = 0.03,
+                    FinishHeadingToleranceRadians =
+                        AngleMath.DegreesToRadians(3.0)
+                },
+                new RotateInPlaceMotionPlanSegment(
+                    finalStraightYawRadians),
+                new TrackMotionPlanSegment(finalTrajectory)
+            };
+
+            _recorder = new TrackingExperimentRecorder(
+                controllerName: "NewStanleyPidComposite",
+                trajectoryName:
+                    "SmoothTurnStopRotateStraight",
+                trialNumber: TrialNumber,
+                referenceStart: ToMillimeterVector(
+                    firstTrajectory.StartPoint.PoseInWorld),
+                referenceEnd: ToMillimeterVector(
+                    finalTrajectory.EndPoint.PoseInWorld),
+                referenceSpeed:
+                    (float)StraightMaximumSpeedMetersPerSecond,
+                sampleIntervalMs: 50,
+                referenceAccelerationMetersPerSecondSquared:
+                    (float)AccelerationMetersPerSecondSquared,
+                referenceDecelerationMetersPerSecondSquared:
+                    (float)DecelerationMetersPerSecondSquared);
+            _recorder.Start();
+
+            var controlPointRadiusMeters =
+                chassis.ControlPointRadius /
+                MillimetersPerMeter;
+            var movement = new MotionPlanExecutor
+            {
+                Segments = plan,
+                StateProvider = stateProvider,
+                ConfigureTrackingMovement = tracking =>
+                {
+                    tracking.MaximumCommandSpeedMetersPerSecond =
+                        StraightMaximumSpeedMetersPerSecond;
+                    tracking
+                        .LongitudinalSpeedErrorDeadbandMetersPerSecond =
+                        0.025;
+                },
+                SegmentStarted = (index, segment) =>
+                {
+                    _recorder?.ClearControlReference();
+                    _recorder?.UpdateCommand(0f, 0f);
+                    Console.WriteLine(
+                        $"组合运动开始第{index + 1}段：" +
+                        segment.GetType().Name);
+                },
+                TrackingCycleObserver = (index, controller) =>
+                    RecordTrackingCycle(
+                        controller,
+                        controlPointRadiusMeters),
+                RotationCommandObserver = (index, omega) =>
+                    _recorder?.UpdateCommand(
+                        0f,
+                        (float)omega)
+            };
+
+            try
+            {
+                _task = new DriveTask(movement.Get());
+                _task.Wait();
+            }
+            finally
+            {
+                _task?.Stop();
+                _recorder?.UpdateCommand(0f, 0f);
+                _recorder?.StopAndSave();
+                _task = null;
+                _recorder = null;
+                _painter?.Clear();
+            }
+        }
+
+        /// <summary>
+        /// 停止组合运动、保存已有实验数据并清除计划轨迹。
+        /// </summary>
+        public override void TestStop()
+        {
+            _task?.Stop();
+            _recorder?.UpdateCommand(0f, 0f);
+            _recorder?.StopAndSave();
+            _task = null;
+            _recorder = null;
+            _painter?.Clear();
+        }
+
+        /// <summary>
+        /// 将两段轨迹和中间原地自转位置绘制到Clumsy界面。
+        /// </summary>
+        private void DrawPlan(
+            Trajectory2D firstTrajectory,
+            Trajectory2D finalTrajectory,
+            Pose2D rotationPoseInWorld)
+        {
+            _painter.Clear();
+            DrawTrajectory(
+                firstTrajectory,
+                Color.DeepSkyBlue);
+            DrawTrajectory(
+                finalTrajectory,
+                Color.Gold);
+
+            var rotationPoint =
+                ToMillimeterVector(rotationPoseInWorld);
+            _painter.DrawDot(
+                Color.Magenta,
+                rotationPoint.X,
+                rotationPoint.Y,
+                10f);
+        }
+
+        /// <summary>
+        /// 绘制一段离散世界坐标系轨迹。
+        /// </summary>
+        private void DrawTrajectory(
+            Trajectory2D trajectory,
+            Color color)
+        {
+            for (var index = 0;
+                 index < trajectory.Count;
+                 index++)
+            {
+                var point = ToMillimeterVector(
+                    trajectory[index].PoseInWorld);
+                _painter.DrawDot(
+                    color,
+                    point.X,
+                    point.Y,
+                    3f);
+
+                if (index == 0)
+                {
+                    continue;
+                }
+
+                var previousPoint = ToMillimeterVector(
+                    trajectory[index - 1].PoseInWorld);
+                _painter.DrawLine(
+                    color,
+                    previousPoint.X,
+                    previousPoint.Y,
+                    point.X,
+                    point.Y,
+                    width: 2);
+            }
+        }
+
+        /// <summary>
+        /// 将轨迹控制周期使用的状态、参考误差和最终GCP命令写入记录器。
+        /// </summary>
+        private void RecordTrackingCycle(
+            ParkingGeometricController controller,
+            double controlPointRadiusMeters)
+        {
+            if (controller.LastVehicleState.HasValue)
+            {
+                _recorder?.UpdateProcessedState(
+                    controller.LastVehicleState.Value);
+            }
+
+            if (!controller.LastCommand.HasValue)
+            {
+                return;
+            }
+
+            if (controller.LastProjection.HasValue &&
+                controller.LastReferenceSpeedMetersPerSecond.HasValue)
+            {
+                var projection =
+                    controller.LastProjection.Value;
+                _recorder?.UpdateControlReference(
+                    projection.ArcLengthMeters,
+                    controller
+                        .LastReferenceSpeedMetersPerSecond.Value,
+                    projection.LateralErrorMeters,
+                    projection.HeadingErrorRadians,
+                    projection.DistanceToTrajectoryMeters,
+                    projection.RemainingDistanceMeters);
+            }
+
+            var command = controller.LastCommand.Value;
+            var curvaturePerMeter = Math.Tan(
+                command.FrontAngleRadians) /
+                controlPointRadiusMeters;
+            var angularSpeedRadiansPerSecond =
+                command.SpeedMetersPerSecond *
+                curvaturePerMeter;
+            _recorder?.UpdateCommand(
+                (float)command.SpeedMetersPerSecond,
+                (float)angularSpeedRadiansPerSecond);
+        }
+
+        /// <summary>
+        /// 将米制世界位姿转换为Clumsy绘图和记录使用的毫米坐标。
+        /// </summary>
+        private static Vector2 ToMillimeterVector(
+            Pose2D poseInWorld)
+        {
+            return new Vector2(
+                (float)(poseInWorld.XMeters *
+                    MillimetersPerMeter),
+                (float)(poseInWorld.YMeters *
+                    MillimetersPerMeter));
+        }
+    }
+}

MultiWheelC/Experiments/NewControllerTrackingTests.cs

@@ -47,7 +47,7 @@ namespace MultiWheelC
         /// <summary>
         /// 获取或设置参考速度减速度，单位为m/s²。
         /// </summary>
-        public double DecelerationMetersPerSecondSquared = 0.12;
+        public double DecelerationMetersPerSecondSquared = 0.10;
 
         /// <summary>
         /// 获取或设置离散轨迹点间距，单位为m。
@@ -330,17 +330,17 @@ namespace MultiWheelC
         /// <summary>
         /// 获取或设置直线与等曲率转弯之间的曲率过渡长度，单位为m。
         /// </summary>
-        public double CurvatureTransitionLengthMeters = 0.60;
+        public double CurvatureTransitionLengthMeters = 0.70;
 
         /// <summary>
         /// 获取或设置两段直线的最大参考速度，单位为m/s。
         /// </summary>
-        public double StraightMaximumSpeedMetersPerSecond = 0.30;
+        public double StraightMaximumSpeedMetersPerSecond = 0.40;
 
         /// <summary>
         /// 获取或设置半圆段的最大参考速度，单位为m/s。
         /// </summary>
-        public double SemicircleMaximumSpeedMetersPerSecond = 0.25;
+        public double SemicircleMaximumSpeedMetersPerSecond = 0.30;
 
         /// <summary>
         /// 获取或设置参考速度加速度，单位为m/s²。

MultiWheelC/Experiments/RotationTests.cs

@@ -1,6 +1,7 @@
 
 
 using System;
+using System.Globalization;
 using System.Numerics;
 using System.Threading;
 using ClumsyCore;
@@ -17,7 +18,6 @@ namespace MultiWheelC
     public abstract class InPlaceRotateTestBase : MovementTest
     {
         public float RelativeAngleDegrees; // 相对当前航向的旋转角度，逆时针为正。
-        public float MaxAngularSpeedDegreesPerSecond = 20f; // PID输出的最大角速度。
         public int TrialNumber = 1; // 重复实验编号。
 
         private DriveTask _task;
@@ -37,11 +37,18 @@ namespace MultiWheelC
         // 从当前Detour航向开始，原地相对旋转指定角度并记录实验数据。
         public override void Test()
         {
+            var config = PilotDefinition.Conf;
+
             if (float.IsNaN(RelativeAngleDegrees) ||
                 float.IsInfinity(RelativeAngleDegrees) ||
-                float.IsNaN(MaxAngularSpeedDegreesPerSecond) ||
-                float.IsInfinity(MaxAngularSpeedDegreesPerSecond) ||
-                MaxAngularSpeedDegreesPerSecond <= 0f)
+                float.IsNaN(config.InPlaceRotateMaxSpeed) ||
+                float.IsInfinity(config.InPlaceRotateMaxSpeed) ||
+                config.InPlaceRotateMaxSpeed <= 0f ||
+                float.IsNaN(config.InPlaceRotateMinimumSpeed) ||
+                float.IsInfinity(config.InPlaceRotateMinimumSpeed) ||
+                config.InPlaceRotateMinimumSpeed <= 0f ||
+                config.InPlaceRotateMinimumSpeed >
+                    config.InPlaceRotateMaxSpeed)
             {
                 Console.WriteLine("原地旋转测试参数无效。");
                 return;
@@ -66,8 +73,25 @@ namespace MultiWheelC
                 (float)AngleMath.NormalizeDegrees(
                     location.th + RelativeAngleDegrees);
 
+            Console.WriteLine(
+                "原地自转实际参数：" +
+                $"Kp={config.InPlaceRotateKp:F3}，" +
+                $"Ki={config.InPlaceRotateKi:F3}，" +
+                $"Kd={config.InPlaceRotateKd:F3}，" +
+                $"到位误差={config.InPlaceRotateArriveDeg:F2}°，" +
+                $"最小角速度={config.InPlaceRotateMinimumSpeed:F2}°/s，" +
+                $"最大角速度={config.InPlaceRotateMaxSpeed:F2}°/s，" +
+                $"角加速度={config.InPlaceRotateAcc:F2}°/s²，" +
+                $"舵轮到位误差={config.InPlaceRotateWheelAlignDeg:F2}°，" +
+                $"旋转超时={config.InPlaceRotateTimeoutSec:F1}s；" +
+                $"起点航向={location.th:F2}°，" +
+                $"目标航向={targetWorldAngle:F2}°。");
+            Console.WriteLine(
+                "原地自转CSV保存目录：" +
+                TrackingExperimentRecorder.DefaultOutputDirectory);
+
             _recorder = new TrackingExperimentRecorder(
-                controllerName: "InPlaceRotatePID",
+                controllerName: "InPlaceRotateFilteredPID",
                 trajectoryName: _trajectoryName,
                 trialNumber: TrialNumber,
                 referenceStart: rotationCenter,
@@ -75,7 +99,7 @@ namespace MultiWheelC
                 referenceSpeed: 0f,
                 referenceAngularSpeed:
                     (float)AngleMath.DegreesToRadians(
-                        MaxAngularSpeedDegreesPerSecond));
+                        config.InPlaceRotateMaxSpeed));
             _recorder.Start();
 
             try
@@ -88,21 +112,26 @@ namespace MultiWheelC
                         PidparamsRead = () => new PIDParams
                         {
                             Kp =
-                                PilotDefinition.Conf.InPlaceRotateKp,
+                                config.InPlaceRotateKp,
                             Ki =
-                                PilotDefinition.Conf.InPlaceRotateKi,
+                                config.InPlaceRotateKi,
                             Kd =
-                                PilotDefinition.Conf.InPlaceRotateKd,
+                                config.InPlaceRotateKd,
                             DeadZone =
-                                PilotDefinition.Conf
-                                    .InPlaceRotateArriveDeg,
+                                config.InPlaceRotateArriveDeg,
                             SpeedAccPerSec =
-                                PilotDefinition.Conf.InPlaceRotateAcc,
+                                config.InPlaceRotateAcc,
                             OutputUpperThreshold =
-                                MaxAngularSpeedDegreesPerSecond,
+                                config.InPlaceRotateMaxSpeed,
                             MaxI =
-                                PilotDefinition.Conf.InPlaceRotateMaxI
+                                config.InPlaceRotateMaxI
                         },
+                        MinimumAngularSpeedDegreesPerSecond =
+                            config.InPlaceRotateMinimumSpeed,
+                        WheelAlignmentToleranceDegrees =
+                            config.InPlaceRotateWheelAlignDeg,
+                        RotationTimeoutSeconds =
+                            config.InPlaceRotateTimeoutSec,
                         CommandAngularSpeedObserver =
                             commandAngularSpeed =>
                                 _recorder?.UpdateCommand(
@@ -136,23 +165,58 @@ namespace MultiWheelC
 
     }
 
-    [MovementTest(name = "SendXYThSpeed：原地自转90°")]
-    public sealed class TestRotate90 :
+    [MovementTest(name = "SendXYThSpeed：输入角度原地自转")]
+    public sealed class TestRotateAngle :
         InPlaceRotateTestBase
     {
-        public TestRotate90()
-            : base(90f, "Rotate90")
+        public TestRotateAngle()
+            : base(0f, "RotateCustomAngle")
         {
         }
+
+        /// <summary>
+        /// 读取相对旋转角度并按正值逆时针、负值顺时针执行原地自转。
+        /// </summary>
+        public override void Test()
+        {
+            var input = UI.GetInput(
+                "输入相对旋转角度（deg，正数逆时针，负数顺时针，范围-180到180之间）：");
+
+            if ((!float.TryParse(
+                     input,
+                     NumberStyles.Float,
+                     CultureInfo.CurrentCulture,
+                     out var relativeAngleDegrees) &&
+                 !float.TryParse(
+                     input,
+                     NumberStyles.Float,
+                     CultureInfo.InvariantCulture,
+                     out relativeAngleDegrees)) ||
+                float.IsNaN(relativeAngleDegrees) ||
+                float.IsInfinity(relativeAngleDegrees))
+            {
+                Console.WriteLine("旋转角度输入无效，测试已经取消。");
+                return;
+            }
+
+            if (Math.Abs(relativeAngleDegrees) < 1e-3f)
+            {
+                Console.WriteLine("旋转角度不能为0，测试已经取消。");
+                return;
+            }
+
+            // 当前控制器按照圆周最短角旋转；精确±180°的方向存在二义性。
+            if (Math.Abs(relativeAngleDegrees) >= 180f)
+            {
+                Console.WriteLine(
+                    "输入角度必须满足-180° < angle < 180°；" +
+                    "当前最短角控制不支持指定精确±180°的旋转方向。");
+                return;
+            }
+
+            RelativeAngleDegrees = relativeAngleDegrees;
+            base.Test();
+        }
     }
 
-    [MovementTest(name = "SendXYThSpeed：原地自转180°")]
-    public sealed class TestRotate180 :
-        InPlaceRotateTestBase
-    {
-        public TestRotate180()
-            : base(180f, "Rotate180")
-        {
-        }
-    }
 }

MultiWheelC/Experiments/TestTrajectoryFactory.cs

@@ -21,10 +21,33 @@ namespace MultiWheelC
             double accelerationMetersPerSecondSquared = 0.20,
             double decelerationMetersPerSecondSquared = 0.20,
             double pointSpacingMeters = 0.02)
+        {
+            return CreateStraight(
+                startPoseInWorld,
+                StraightLengthMeters,
+                cruiseSpeedMetersPerSecond,
+                accelerationMetersPerSecondSquared,
+                decelerationMetersPerSecondSquared,
+                pointSpacingMeters);
+        }
+
+        /// <summary>
+        /// 从给定车体中心位姿沿当前航向生成指定长度并在终点停车的直线轨迹。
+        /// </summary>
+        public static Trajectory2D CreateStraight(
+            Pose2D startPoseInWorld,
+            double lengthMeters,
+            double cruiseSpeedMetersPerSecond = 0.30,
+            double accelerationMetersPerSecondSquared = 0.20,
+            double decelerationMetersPerSecondSquared = 0.20,
+            double pointSpacingMeters = 0.02)
         {
             EnsureFinitePose(
                 startPoseInWorld,
                 nameof(startPoseInWorld));
+            EnsureFinitePositive(
+                lengthMeters,
+                nameof(lengthMeters));
             EnsureFinitePositive(
                 cruiseSpeedMetersPerSecond,
                 nameof(cruiseSpeedMetersPerSecond));
@@ -38,7 +61,7 @@ namespace MultiWheelC
                 pointSpacingMeters,
                 nameof(pointSpacingMeters));
 
-            if (pointSpacingMeters > StraightLengthMeters)
+            if (pointSpacingMeters > lengthMeters)
             {
                 throw new ArgumentOutOfRangeException(
                     nameof(pointSpacingMeters),
@@ -46,7 +69,7 @@ namespace MultiWheelC
             }
 
             var segmentCount = (int)Math.Ceiling(
-                StraightLengthMeters /
+                lengthMeters /
                 pointSpacingMeters);
             var points = new List<TrajectoryPoint>(
                 segmentCount + 1);
@@ -59,13 +82,13 @@ namespace MultiWheelC
                  index <= segmentCount;
                  index++)
             {
-                // 均分后最后一个点严格落在4m终点，避免浮点累加越界。
+                // 均分后最后一个点严格落在指定终点，避免浮点累加越界。
                 var arcLengthMeters =
-                    StraightLengthMeters *
+                    lengthMeters *
                     index /
                     segmentCount;
                 var remainingDistanceMeters =
-                    StraightLengthMeters -
+                    lengthMeters -
                     arcLengthMeters;
                 var referenceSpeedMetersPerSecond =
                     CalculateReferenceSpeed(
@@ -105,6 +128,34 @@ namespace MultiWheelC
             double accelerationMetersPerSecondSquared = 0.20,
             double decelerationMetersPerSecondSquared = 0.12,
             double pointSpacingMeters = 0.02)
+        {
+            return CreateStraightSmoothLeftTurnStraight(
+                startPoseInWorld,
+                straightLengthMeters,
+                turnRadiusMeters,
+                Math.PI,
+                curvatureTransitionLengthMeters,
+                straightMaximumSpeedMetersPerSecond,
+                semicircleMaximumSpeedMetersPerSecond,
+                accelerationMetersPerSecondSquared,
+                decelerationMetersPerSecondSquared,
+                pointSpacingMeters);
+        }
+
+        /// <summary>
+        /// 生成“直线、平滑左转、直线”轨迹，并使总转向角严格等于指定角度。
+        /// </summary>
+        public static Trajectory2D CreateStraightSmoothLeftTurnStraight(
+            Pose2D startPoseInWorld,
+            double straightLengthMeters,
+            double turnRadiusMeters,
+            double turnAngleRadians,
+            double curvatureTransitionLengthMeters,
+            double straightMaximumSpeedMetersPerSecond,
+            double turnMaximumSpeedMetersPerSecond,
+            double accelerationMetersPerSecondSquared,
+            double decelerationMetersPerSecondSquared,
+            double pointSpacingMeters)
         {
             EnsureFinitePose(
                 startPoseInWorld,
@@ -115,6 +166,9 @@ namespace MultiWheelC
             EnsureFinitePositive(
                 turnRadiusMeters,
                 nameof(turnRadiusMeters));
+            EnsureFinitePositive(
+                turnAngleRadians,
+                nameof(turnAngleRadians));
             EnsureFinitePositive(
                 curvatureTransitionLengthMeters,
                 nameof(curvatureTransitionLengthMeters));
@@ -122,8 +176,8 @@ namespace MultiWheelC
                 straightMaximumSpeedMetersPerSecond,
                 nameof(straightMaximumSpeedMetersPerSecond));
             EnsureFinitePositive(
-                semicircleMaximumSpeedMetersPerSecond,
-                nameof(semicircleMaximumSpeedMetersPerSecond));
+                turnMaximumSpeedMetersPerSecond,
+                nameof(turnMaximumSpeedMetersPerSecond));
             EnsureFinitePositive(
                 accelerationMetersPerSecondSquared,
                 nameof(accelerationMetersPerSecondSquared));
@@ -134,21 +188,28 @@ namespace MultiWheelC
                 pointSpacingMeters,
                 nameof(pointSpacingMeters));
 
-            var originalSemicircleLengthMeters =
-                Math.PI * turnRadiusMeters;
+            if (turnAngleRadians > 2.0 * Math.PI)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(turnAngleRadians),
+                    "单段平滑左转角度不能大于2π。");
+            }
+
+            var nominalTurnArcLengthMeters =
+                turnAngleRadians * turnRadiusMeters;
             var constantCurvatureLengthMeters =
-                originalSemicircleLengthMeters -
+                nominalTurnArcLengthMeters -
                 curvatureTransitionLengthMeters;
 
             if (constantCurvatureLengthMeters <= 0.0)
             {
                 throw new ArgumentOutOfRangeException(
                     nameof(curvatureTransitionLengthMeters),
-                    "曲率过渡段长度必须小于半径对应的原始半圆弧长。");
+                    "曲率过渡段长度必须小于指定转角对应的圆弧长度。");
             }
 
             // 两段平滑过渡的平均曲率均为最大曲率的一半；
-            // 将等曲率段缩短一个过渡长度后，总曲率积分仍严格等于π。
+            // 将等曲率段缩短一个过渡长度后，总曲率积分仍严格等于指定转角。
             var turnLengthMeters =
                 2.0 * curvatureTransitionLengthMeters +
                 constantCurvatureLengthMeters;
@@ -184,7 +245,7 @@ namespace MultiWheelC
                         turnStartArcLengthMeters &&
                     arcLengthMeters <=
                         turnEndArcLengthMeters
-                        ? semicircleMaximumSpeedMetersPerSecond
+                        ? turnMaximumSpeedMetersPerSecond
                         : straightMaximumSpeedMetersPerSecond;
             }
 
@@ -341,7 +402,7 @@ namespace MultiWheelC
         }
 
         /// <summary>
-        /// 计算180°左转中连续变化的参考曲率，过渡段两端的曲率变化率均为零。
+        /// 计算平滑左转中连续变化的参考曲率，过渡段两端的曲率变化率均为零。
         /// </summary>
         private static double CalculateSmoothTurnCurvature(
             double distanceInTurnMeters,

MultiWheelC/Experiments/TrackingExperimentRecorder.cs

@@ -145,6 +145,14 @@ namespace MultiWheelC
         // 保存成功后的CSV绝对路径；尚未保存时为空。
         public string SavedFilePath { get; private set; }
 
+        /// <summary>
+        /// 获取Clumsy当前运行目录下统一保存轨迹实验CSV的文件夹。
+        /// </summary>
+        public static string DefaultOutputDirectory =>
+            Path.Combine(
+                AppContext.BaseDirectory,
+                "TrackingExperiments");
+
         // 启动后台采样线程。
         public void Start()
         {
@@ -242,6 +250,17 @@ namespace MultiWheelC
             }
         }
 
+        /// <summary>
+        /// 清除上一轨迹段参考量，避免停车或原地自转期间沿用已经结束的投影结果。
+        /// </summary>
+        public void ClearControlReference()
+        {
+            lock (_stateSyncRoot)
+            {
+                _hasControlReference = false;
+            }
+        }
+
         // 停止采样并将本次实验保存为CSV；重复调用只保存一次。
         public void StopAndSave()
         {
@@ -444,9 +463,8 @@ namespace MultiWheelC
                     new List<TrackingSample>(_samples);
             }
 
-            var outputDirectory = Path.Combine(
-                AppContext.BaseDirectory,
-                "TrackingExperiments");
+            var outputDirectory =
+                DefaultOutputDirectory;
 
             Directory.CreateDirectory(outputDirectory);
 

MultiWheelC/Movements/MotionPlanExecutor.cs

@@ -0,0 +1,246 @@
+using System;
+using System.Collections.Generic;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using MDCSToolBox.Commons.Controllers;
+using MultiWheelC.Control.Execution;
+using MultiWheelC.StateEstimation;
+using MultiWheelC.Trajectory;
+using MyParking.Shared;
+
+namespace MultiWheelC
+{
+    /// <summary>
+    /// 表示组合运动计划中由一种控制方式完整执行的单个动作段。
+    /// </summary>
+    public abstract class MotionPlanSegment
+    {
+    }
+
+    /// <summary>
+    /// 表示使用新版几何控制器跟踪一条连续二维轨迹的动作段。
+    /// </summary>
+    public sealed class TrackMotionPlanSegment : MotionPlanSegment
+    {
+        public TrackMotionPlanSegment(Trajectory2D trajectory)
+        {
+            Trajectory = trajectory ??
+                throw new ArgumentNullException(nameof(trajectory));
+        }
+
+        public Trajectory2D Trajectory { get; }
+
+        /// <summary>
+        /// 获取或设置本段轨迹独立的终点距离容差；为空时沿用轨迹动作默认值。
+        /// </summary>
+        public double? FinishDistanceMeters { get; set; }
+
+        /// <summary>
+        /// 获取或设置本段轨迹独立的停车速度容差；为空时沿用轨迹动作默认值。
+        /// </summary>
+        public double? FinishSpeedMetersPerSecond { get; set; }
+
+        /// <summary>
+        /// 获取或设置本段轨迹独立的终点航向容差；为空时沿用轨迹动作默认值。
+        /// </summary>
+        public double? FinishHeadingToleranceRadians { get; set; }
+    }
+
+    /// <summary>
+    /// 表示车辆停车后原地旋转到指定世界航向的动作段。
+    /// </summary>
+    public sealed class RotateInPlaceMotionPlanSegment
+        : MotionPlanSegment
+    {
+        public RotateInPlaceMotionPlanSegment(
+            double targetYawRadians)
+        {
+            if (double.IsNaN(targetYawRadians) ||
+                double.IsInfinity(targetYawRadians))
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(targetYawRadians),
+                    "原地自转目标航向必须是有限值。");
+            }
+
+            TargetYawRadians =
+                AngleMath.NormalizeRadians(targetYawRadians);
+        }
+
+        public double TargetYawRadians { get; }
+    }
+
+    /// <summary>
+    /// 顺序执行连续轨迹和原地自转动作，并在动作段边界完成停车与控制器切换。
+    /// </summary>
+    public sealed class MotionPlanExecutor : MovementDefinition
+    {
+        /// <summary>
+        /// 获取或设置一次性提交并按顺序执行的组合运动计划。
+        /// </summary>
+        public IReadOnlyList<MotionPlanSegment> Segments;
+
+        /// <summary>
+        /// 获取或设置所有动作段共享的车辆状态源；为空时使用Detour状态源。
+        /// </summary>
+        public IVehicleStateProvider StateProvider;
+
+        /// <summary>
+        /// 获取或设置创建每段轨迹动作后应用参数的回调。
+        /// </summary>
+        public Action<TrajectoryTrackingMovement>
+            ConfigureTrackingMovement;
+
+        /// <summary>
+        /// 获取或设置创建每段原地自转动作后应用参数的回调。
+        /// </summary>
+        public Action<MultiWheelRotateInPlace>
+            ConfigureRotationMovement;
+
+        /// <summary>
+        /// 获取或设置动作段开始前的通知，参数依次为索引和动作段。
+        /// </summary>
+        public Action<int, MotionPlanSegment> SegmentStarted;
+
+        /// <summary>
+        /// 获取或设置轨迹段每个有效控制周期后的诊断通知。
+        /// </summary>
+        public Action<int, ParkingGeometricController>
+            TrackingCycleObserver;
+
+        /// <summary>
+        /// 获取或设置自转段角速度命令通知，角速度单位为rad/s。
+        /// </summary>
+        public Action<int, double> RotationCommandObserver;
+
+        /// <summary>
+        /// 按计划顺序执行各动作段，任一动作失败时停止后续动作。
+        /// </summary>
+        public override IEnumerable<bool> Get()
+        {
+            if (Segments == null || Segments.Count == 0)
+            {
+                throw new InvalidOperationException(
+                    "组合运动计划至少需要包含一个动作段。");
+            }
+
+            var stateProvider =
+                StateProvider ??
+                new DetourVehicleStateProvider();
+
+            for (var index = 0;
+                 index < Segments.Count;
+                 index++)
+            {
+                var segment = Segments[index] ??
+                    throw new InvalidOperationException(
+                        $"组合运动计划第{index}段为空。");
+
+                SegmentStarted?.Invoke(index, segment);
+
+                if (segment is TrackMotionPlanSegment track)
+                {
+                    var movement =
+                        new TrajectoryTrackingMovement
+                        {
+                            Trajectory = track.Trajectory,
+                            StateProvider = stateProvider,
+                            CycleObserver = controller =>
+                                TrackingCycleObserver?.Invoke(
+                                    index,
+                                    controller)
+                        };
+                    ConfigureTrackingMovement?.Invoke(movement);
+
+                    // 单段参数后应用，确保中间连接段可以覆盖组合动作的公共配置。
+                    if (track.FinishDistanceMeters.HasValue)
+                    {
+                        movement.FinishDistanceMeters =
+                            track.FinishDistanceMeters.Value;
+                    }
+
+                    if (track.FinishSpeedMetersPerSecond.HasValue)
+                    {
+                        movement.FinishSpeedMetersPerSecond =
+                            track.FinishSpeedMetersPerSecond.Value;
+                    }
+
+                    if (track.FinishHeadingToleranceRadians.HasValue)
+                    {
+                        movement.FinishHeadingToleranceRadians =
+                            track.FinishHeadingToleranceRadians.Value;
+                    }
+
+                    foreach (var keepRunning in movement.Get())
+                    {
+                        if (!keepRunning)
+                        {
+                            break;
+                        }
+
+                        yield return true;
+                    }
+
+                    continue;
+                }
+
+                if (segment is RotateInPlaceMotionPlanSegment rotate)
+                {
+                    var config = PilotDefinition.Conf;
+                    var movement =
+                        new MultiWheelRotateInPlace
+                        {
+                            AngleTarget =
+                                (float)AngleMath.RadiansToDegrees(
+                                    rotate.TargetYawRadians),
+                            StateProvider = stateProvider,
+                            PidparamsRead = () => new PIDParams
+                            {
+                                Kp = config.InPlaceRotateKp,
+                                Ki = config.InPlaceRotateKi,
+                                Kd = config.InPlaceRotateKd,
+                                DeadZone =
+                                    config.InPlaceRotateArriveDeg,
+                                SpeedAccPerSec =
+                                    config.InPlaceRotateAcc,
+                                OutputUpperThreshold =
+                                    config.InPlaceRotateMaxSpeed,
+                                MaxI = config.InPlaceRotateMaxI
+                            },
+                            MinimumAngularSpeedDegreesPerSecond =
+                                config.InPlaceRotateMinimumSpeed,
+                            WheelAlignmentToleranceDegrees =
+                                config.InPlaceRotateWheelAlignDeg,
+                            RotationTimeoutSeconds =
+                                config.InPlaceRotateTimeoutSec,
+                            CommandAngularSpeedObserver =
+                                commandDegreesPerSecond =>
+                                    RotationCommandObserver?.Invoke(
+                                        index,
+                                        AngleMath.DegreesToRadians(
+                                            commandDegreesPerSecond))
+                        };
+                    ConfigureRotationMovement?.Invoke(movement);
+
+                    foreach (var keepRunning in movement.Get())
+                    {
+                        if (!keepRunning)
+                        {
+                            break;
+                        }
+
+                        yield return true;
+                    }
+
+                    continue;
+                }
+
+                throw new NotSupportedException(
+                    $"组合运动计划不支持动作段类型：{segment.GetType().FullName}。");
+            }
+
+            // 所有子动作均已完成后，才向外层DriveTask发送组合计划结束信号。
+            yield return false;
+        }
+    }
+}

MultiWheelC/Movements/RotateInPlace.cs

@@ -7,6 +7,7 @@ using ClumsyCore.Pilot;
 using CommonUsage.Chassis;
 using MDCSToolBox.Commons.Controllers;
 using MyParking.Shared;
+using MultiWheelC.StateEstimation;
 
 namespace MultiWheelC
 {
@@ -17,7 +18,11 @@ namespace MultiWheelC
         /// </summary>
         public float AngleTarget;
 
-        public Func<float> ThetaReader = () => (float)DetourInterface.getCartLocation().th;
+        // 留作标定或单元测试时显式替换；为空时使用经过校验的Detour状态源。
+        public Func<float> ThetaReader;
+
+        public IVehicleStateProvider StateProvider =
+            new DetourVehicleStateProvider();
 
         public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis;
 
@@ -37,6 +42,12 @@ namespace MultiWheelC
         // 自转舵轮准备超时时间，单位s。
         public float WheelAlignmentTimeoutSeconds = 10f;
 
+        // 航向尚未到位时允许下发的最小有效角速度，单位deg/s。
+        public float MinimumAngularSpeedDegreesPerSecond = 1f;
+
+        // 舵轮到位后执行航向闭环允许的最长时间，单位s。
+        public float RotationTimeoutSeconds = 15f;
+
         // 先准备自转舵角，再通过安全版SendXYThSpeed闭环旋转到目标角度。
         public override IEnumerable<bool> Get()
         {
@@ -44,6 +55,8 @@ namespace MultiWheelC
                 throw new InvalidOperationException(
                     "当前底盘不是MultiWheelChassis，无法执行原地自转。");
 
+            ValidateParameters();
+
             var adapter = new MultiWheelChassisAdapter(
                 Chassis,
                 PilotDefinition.Self.CarNum);
@@ -55,7 +68,9 @@ namespace MultiWheelC
                 DateTime? alignedSince = null;
                 while (true)
                 {
-                    if (!adapter.PrepareSpin())
+                    if (!adapter.PrepareSpin(
+                            alignmentToleranceDegrees:
+                                WheelAlignmentToleranceDegrees))
                         throw new InvalidOperationException(
                             "无法生成原地自转舵轮目标：" +
                             adapter.LastFailureReason);
@@ -84,18 +99,86 @@ namespace MultiWheelC
                     yield return true;
                 }
 
+                var alignmentToleranceRadians =
+                    AngleMath.DegreesToRadians(
+                        WheelAlignmentToleranceDegrees);
+                if (!adapter.AdoptPreparedSpinForXYTh(
+                        alignmentToleranceRadians))
+                {
+                    throw new InvalidOperationException(
+                        "无法将已到位的自转舵角交接给XYTh：" +
+                        adapter.LastFailureReason);
+                }
+
                 var targetAngle =
                     (float)AngleMath.NormalizeDegrees(AngleTarget);
                 var p = PidparamsRead();
-                thPid = new PIDController(ThetaReader, p.Kp);
+                var currentAngle = ReadCurrentAngleDegrees();
+                var cachedCurrentAngle = currentAngle;
+                thPid = new PIDController(
+                    () => cachedCurrentAngle,
+                    p.Kp);
                 thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone,
                     p.OutputUpperThreshold, p.SpeedAccPerSec);
                 var lastCommandTime = DateTime.Now;
+                var rotationStarted = DateTime.Now;
 
                 while (true)
                 {
+                    if ((DateTime.Now - rotationStarted)
+                            .TotalSeconds >
+                        RotationTimeoutSeconds)
+                    {
+                        throw new TimeoutException(
+                            $"原地自转超过{RotationTimeoutSeconds:F1}s仍未到位。");
+                    }
+
+                    currentAngle = ReadCurrentAngleDegrees();
+                    cachedCurrentAngle = currentAngle;
                     var s = thPid.GetResponse(targetAngle, true);
-                    Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
+                    var angleErrorDegrees =
+                        (float)AngleMath
+                            .ShortestDifferenceDegrees(
+                                targetAngle,
+                                currentAngle);
+
+                    // PID进入到位死区后等待其0.3s稳定确认；等待期间
+                    // 只清零驱动速度，不清除已经准备好的自转舵角状态。
+                    if (Math.Abs(angleErrorDegrees) <=
+                        p.DeadZone)
+                    {
+                        CommandAngularSpeedObserver?.Invoke(0f);
+                        adapter
+                            .StopXYThDrivePreserveSteeringState();
+
+                        if (thPid.IsArrived())
+                            break;
+
+                        yield return true;
+                        continue;
+                    }
+
+                    // PID输出低于底盘有效轮速范围时提高到最小可执行值，
+                    // 避免接近目标时反复出现微小命令但车辆实际不动。
+                    if (Math.Abs(s) > 1e-6f &&
+                        Math.Abs(s) <
+                            MinimumAngularSpeedDegreesPerSecond)
+                    {
+                        s = Math.Sign(angleErrorDegrees) *
+                            MinimumAngularSpeedDegreesPerSecond;
+                    }
+
+                    // PID加速限制在首周期可能暂时输出零；此时保留
+                    // 已交接的自转状态，等待下一周期产生有效角速度。
+                    if (Math.Abs(s) <= 1e-6f)
+                    {
+                        CommandAngularSpeedObserver?.Invoke(0f);
+                        adapter
+                            .StopXYThDrivePreserveSteeringState();
+                        yield return true;
+                        continue;
+                    }
+
                     CommandAngularSpeedObserver?.Invoke(s);
                     var now = DateTime.Now;
                     var interval = now - lastCommandTime;
@@ -118,7 +201,6 @@ namespace MultiWheelC
                             "安全XYTh原地旋转底盘解算失败：" +
                             adapter.LastFailureReason);
                     }
-                    if (thPid.IsArrived()) break;
                     yield return true;
                 }
 
@@ -130,5 +212,109 @@ namespace MultiWheelC
                 adapter.StopImmediately();
             }
         }
+
+        /// <summary>
+        /// 检查原地自转的舵轮准备、最小速度和超时参数是否可执行。
+        /// </summary>
+        private void ValidateParameters()
+        {
+            EnsureFinitePositive(
+                WheelAlignmentToleranceDegrees,
+                nameof(WheelAlignmentToleranceDegrees),
+                allowZero: true);
+            EnsureFinitePositive(
+                WheelAlignmentStableSeconds,
+                nameof(WheelAlignmentStableSeconds),
+                allowZero: true);
+            EnsureFinitePositive(
+                WheelAlignmentTimeoutSeconds,
+                nameof(WheelAlignmentTimeoutSeconds));
+            EnsureFinitePositive(
+                MinimumAngularSpeedDegreesPerSecond,
+                nameof(MinimumAngularSpeedDegreesPerSecond));
+            EnsureFinitePositive(
+                RotationTimeoutSeconds,
+                nameof(RotationTimeoutSeconds));
+
+            var pidParameters = PidparamsRead();
+            if (pidParameters == null)
+            {
+                throw new InvalidOperationException(
+                    "原地自转PID参数读取结果为空。");
+            }
+
+            EnsureFinitePositive(
+                pidParameters.DeadZone,
+                "PidparamsRead.DeadZone");
+            EnsureFinitePositive(
+                pidParameters.OutputUpperThreshold,
+                "PidparamsRead.OutputUpperThreshold");
+            EnsureFinitePositive(
+                pidParameters.SpeedAccPerSec,
+                "PidparamsRead.SpeedAccPerSec");
+            EnsureFinitePositive(
+                pidParameters.Kp,
+                "PidparamsRead.Kp");
+
+            if (MinimumAngularSpeedDegreesPerSecond >
+                pidParameters.OutputUpperThreshold)
+            {
+                throw new InvalidOperationException(
+                    "原地自转最小有效角速度不能大于最大角速度。");
+            }
+        }
+
+        /// <summary>
+        /// 读取经过状态源校验的世界航向，显式设置ThetaReader时优先使用替代读数。
+        /// </summary>
+        private float ReadCurrentAngleDegrees()
+        {
+            if (ThetaReader != null)
+            {
+                var angleDegrees = ThetaReader();
+                if (float.IsNaN(angleDegrees) ||
+                    float.IsInfinity(angleDegrees))
+                {
+                    throw new InvalidOperationException(
+                        "自定义航向读取结果不是有效角度。");
+                }
+
+                return (float)AngleMath.NormalizeDegrees(
+                    angleDegrees);
+            }
+
+            if (StateProvider == null ||
+                !StateProvider.TryGetState(out var state))
+            {
+                throw new InvalidOperationException(
+                    "无法从Detour状态源读取有效车辆航向。" +
+                    (StateProvider is DetourVehicleStateProvider provider
+                        ? provider.LastFailureReason
+                        : ""));
+            }
+
+            return (float)AngleMath.RadiansToDegrees(
+                state.PoseInWorld.YawRadians);
+        }
+
+        /// <summary>
+        /// 检查原地自转参数是否为正有限值，部分时间和容差参数允许为零。
+        /// </summary>
+        private static void EnsureFinitePositive(
+            float value,
+            string parameterName,
+            bool allowZero = false)
+        {
+            if (float.IsNaN(value) ||
+                float.IsInfinity(value) ||
+                (allowZero
+                    ? value < 0f
+                    : value <= 0f))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "原地自转参数必须是有效的正数。");
+            }
+        }
     }
 }

MultiWheelC/Movements/TrajectoryTrackingMovement.cs

@@ -55,6 +55,18 @@ namespace MultiWheelC
         /// </summary>
         public bool StanleyUsesActualSpeed = true;
 
+        /// <summary>
+        /// Stanley横向误差共同转角分量的最大绝对值，单位为rad。
+        /// </summary>
+        public double MaximumCrossTrackCorrectionRadians =
+            AngleMath.DegreesToRadians(10.0);
+
+        /// <summary>
+        /// Stanley航向误差差动转角分量的最大绝对值，单位为rad。
+        /// </summary>
+        public double MaximumHeadingCorrectionRadians =
+            AngleMath.DegreesToRadians(10.0);
+
         /// <summary>
         /// 纵向速度外环比例增益。
         /// </summary>
@@ -75,6 +87,12 @@ namespace MultiWheelC
         /// </summary>
         public double MaximumIntegralCorrectionMetersPerSecond = 0.05;
 
+        /// <summary>
+        /// 纵向PID不进行反馈修正的速度误差死区，单位为m/s。
+        /// </summary>
+        public double LongitudinalSpeedErrorDeadbandMetersPerSecond =
+            0.025;
+
         /// <summary>
         /// 底盘纵向命令速度绝对值上限，单位为m/s。
         /// </summary>
@@ -90,7 +108,7 @@ namespace MultiWheelC
         /// 前后GCP目标转角最大变化率，单位为rad/s。
         /// </summary>
         public double MaximumGcpAngleRateRadiansPerSecond =
-            AngleMath.DegreesToRadians(10.0);
+            AngleMath.DegreesToRadians(15.0);
 
         /// <summary>
         /// 终点位置和剩余弧长的完成容差，单位为m。
@@ -157,17 +175,19 @@ namespace MultiWheelC
                     StanleyCrossTrackGainPerSecond,
                     StanleyHeadingErrorGain,
                     StanleyMinimumSpeedMetersPerSecond,
-                    StanleyUsesActualSpeed);
+                    StanleyUsesActualSpeed,
+                    MaximumCrossTrackCorrectionRadians,
+                    MaximumHeadingCorrectionRadians);
             var longitudinalController =
                 new PidLongitudinalController(
                     LongitudinalKp,
                     LongitudinalKiPerSecond,
                     LongitudinalKdSeconds,
                     MaximumIntegralCorrectionMetersPerSecond,
-                    MaximumCommandSpeedMetersPerSecond);
+                    MaximumCommandSpeedMetersPerSecond,
+                    LongitudinalSpeedErrorDeadbandMetersPerSecond);
             var gcpAllocator =
-                new AckermannGcpAllocator(
-                    controlPointRadiusMeters,
+                new GcpCommandAllocator(
                     MaximumGcpAngleRadians);
             var commandExecutor =
                 new GcpCommandExecutor(

MultiWheelC/PilotConfig.cs

@@ -26,7 +26,7 @@ public class PilotConfig : MultiWheelPilotConfig
     public float InPlaceRotateSpeed = 30f;
 
     [FieldMember(desc = "原地旋转：到位角度精度(deg)")]
-    public float InPlaceRotateArriveDeg = 1f;
+    public float InPlaceRotateArriveDeg = 1.5f;
 
     [FieldMember(desc = "原地旋转：起转前舵轮对齐精度(deg)")]
     public float InPlaceRotateWheelAlignDeg = 2f;
@@ -38,24 +38,25 @@ public class PilotConfig : MultiWheelPilotConfig
 
     #region 单车-临时
     [FieldMember(desc = "原地旋转Kp")]
-    public float InPlaceRotateKp = 0.2f;
-    // public float InPlaceRotateKp = 0.2f;
+    public float InPlaceRotateKp = 1.1f;
 
     [FieldMember(desc = "原地旋转Ki")]
-    public float InPlaceRotateKi = 0.01f;
-    // public float InPlaceRotateKi = 0.01f;
+    public float InPlaceRotateKi = 0f;
 
     [FieldMember(desc = "原地旋转Kd")]
     public float InPlaceRotateKd = 0f;
 
     [FieldMember(desc = "原地旋转积分限幅")]
-    public float InPlaceRotateMaxI = 0.01f;
+    public float InPlaceRotateMaxI = 0f;
+
+    [FieldMember(desc = "原地旋转最小有效角速度(deg/s)")]
+    public float InPlaceRotateMinimumSpeed = 1f;
 
     [FieldMember(desc = "原地旋转最大角速度(deg/s)")]
-    public float InPlaceRotateMaxSpeed = 30f;
+    public float InPlaceRotateMaxSpeed = 47.5f;
 
     [FieldMember(desc = "原地旋转角加速度(deg/s²)")]
-    public float InPlaceRotateAcc = 30f;
+    public float InPlaceRotateAcc = 60f;
 
     [FieldMember(desc = "原地旋转超时(s)")]
     public float InPlaceRotateTimeoutSec = 15f;

MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs

@@ -188,11 +188,11 @@ namespace MultiWheelC.StateEstimation
                             poseInWorld,
                             elapsedSeconds))
                     {
-                        state = AcceptPoseAfterVelocityRebase(
+                        state = AcceptPoseAfterReset(
                             poseInWorld,
                             timestampSeconds);
                         LastFailureReason =
-                            "Detour位姿偏离上一速度预测，本次只更新位姿基准并保留滤波速度。";
+                            "Detour位姿偏离速度预测，已重新建立速度估计基准。";
                         return true;
                     }
 

Shared/Chassis/MultiWheelChassisAdapter.cs

@@ -504,13 +504,26 @@ namespace MyParking.Shared
         /// 返回是否成功生成舵轮目标。
         /// </summary>
         public bool PrepareSpin(
-            TimeSpan? interval = null)
+            TimeSpan? interval = null,
+            double alignmentToleranceDegrees = 2.0)
         {
+            ValidateFinite(
+                alignmentToleranceDegrees,
+                nameof(alignmentToleranceDegrees));
+
+            if (alignmentToleranceDegrees < 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(alignmentToleranceDegrees),
+                    "自转舵轮到位容差必须是非负有限值。");
+            }
+
             EnsureBodyFrameIsActive();
 
             var success =
                 _chassis.PrepareRotateWheels(
-                    alignmentToleranceDegrees: 2.0f);
+                    alignmentToleranceDegrees:
+                        (float)alignmentToleranceDegrees);
 
             if (!success)
             {

data_process/新版前后角解耦控制器测试处理/4m直线0.3/记录.txt

@@ -0,0 +1,17 @@
+第一次：
+ok
+
+第二次：
+: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.031m，航向误差=0.03°。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.NewControllerStraight4mTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\NewControllerTrackingTests.cs:line 146
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.031m，航向误差=0.03°。, stack:
+   at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 257
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+
+第三次：
+ok

data_process/新版控制器轨迹测试处理/plot_new_controller_experiment.py

@@ -208,10 +208,13 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
     has_control_reference = (
         numeric_column(frame, "HasControlReference", 0.0) > 0.5
     )
-    lateral_error = np.where(
-        has_control_reference & np.isfinite(recorded_lateral_error),
-        recorded_lateral_error,
-        derived_lateral_error,
+    recorded_lateral_valid = (
+        has_control_reference & np.isfinite(recorded_lateral_error)
+    )
+    lateral_error = (
+        np.where(recorded_lateral_valid, recorded_lateral_error, np.nan)
+        if np.any(recorded_lateral_valid)
+        else derived_lateral_error
     )
 
     state_yaw = numeric_column(frame, "StateYawRadians")
@@ -230,10 +233,28 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
         frame,
         "ControlHeadingErrorRadians",
     )
-    heading_error = np.where(
-        has_control_reference & np.isfinite(recorded_heading_error),
-        recorded_heading_error,
-        derived_heading_error,
+    recorded_heading_valid = (
+        has_control_reference & np.isfinite(recorded_heading_error)
+    )
+    heading_error = (
+        np.where(recorded_heading_valid, recorded_heading_error, np.nan)
+        if np.any(recorded_heading_valid)
+        else derived_heading_error
+    )
+
+    # 投影定义满足：参考点 = 车体位置 + 横向误差 × 参考航向左法向。
+    # 因此无需假设轨迹类型，即可从有效控制周期还原车辆实际使用的参考轨迹。
+    projected_reference_yaw = actual_yaw + heading_error
+    reference_x = (
+        actual_x - lateral_error * np.sin(projected_reference_yaw)
+    )
+    reference_y = (
+        actual_y + lateral_error * np.cos(projected_reference_yaw)
+    )
+    valid_reference_position = (
+        has_control_reference
+        & np.isfinite(reference_x)
+        & np.isfinite(reference_y)
     )
 
     cruise_speed = first_finite(
@@ -270,6 +291,8 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
         numeric_column(frame, "ControlReferenceSpeedMetersPerSecond"),
         ideal_speed,
     )
+    if np.any(has_control_reference):
+        reference_speed[~has_control_reference] = np.nan
     actual_speed = numeric_column(frame, "StateBodyVxMetersPerSecond")
     velocity_valid = (
         numeric_column(frame, "StateVelocityEstimateValid", 0.0) > 0.5
@@ -283,6 +306,9 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
         "actual_x": actual_x,
         "actual_y": actual_y,
         "valid_position": valid_position,
+        "reference_x": reference_x,
+        "reference_y": reference_y,
+        "valid_reference_position": valid_reference_position,
         "start": start,
         "end": end,
         "length": length_meters,
@@ -336,12 +362,22 @@ def plot_experiment(
 
     valid_position = data["valid_position"]
     fig, axis = plt.subplots(figsize=(9.0, 6.5))
+    valid_reference_position = data["valid_reference_position"]
+    if np.count_nonzero(valid_reference_position) >= 2:
+        axis.plot(
+            data["reference_x"][valid_reference_position],
+            data["reference_y"][valid_reference_position],
+            "--",
+            linewidth=2.0,
+            label="控制器实际使用的参考轨迹",
+        )
+    else:
         axis.plot(
             [data["start"][0], data["end"][0]],
             [data["start"][1], data["end"][1]],
             "--",
             linewidth=2.0,
-        label="期望4m直线轨迹",
+            label="参考起终点连线",
         )
     axis.plot(
         data["actual_x"][valid_position],
@@ -451,7 +487,7 @@ def discover_csv_files(arguments: list[str]) -> list[Path]:
 def main() -> None:
     """解析命令行并批量处理新版控制器实验CSV。"""
     parser = argparse.ArgumentParser(
-        description="绘制新版控制器4m直线实验的四类对比图。"
+        description="绘制新版控制器轨迹实验的四类对比图。"
     )
     parser.add_argument("csv", nargs="*", help="需要处理的CSV文件路径。")
     parser.add_argument(

data_process/新版控制器轨迹测试处理/混合测试/第一次/记录.txt

@@ -0,0 +1,10 @@
+: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.033m，航向误差=0.19°。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.CompositeStopTurnGoTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\CompositeMotionPlanTests.cs:line 180
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.033m，航向误差=0.19°。, stack:
+   at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 244
+   at MultiWheelC.MotionPlanExecutor.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\MotionPlanExecutor.cs:line 140
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+

data_process/新版控制器轨迹测试处理/混合测试/第三次/记录.txt

@@ -0,0 +1,9 @@
+put meobj `clumsy_detour_backplate_lines` @ 95c33350
+apply gltf class `basic_car`, vtx=19456
+put meobj `CarInWorld` @ 965e05e0
+组合运动开始第1段：TrackMotionPlanSegment
+put meobj `CompositeStopTurnGoTest_pc` @ a511aa30
+put meobj `CompositeStopTurnGoTest_lines` @ a5281610
+轨迹实验数据已保存：C:\Users\Administrator\Desktop\MDCS2\Clumsy自动\TrackingExperiments\20260807_134158_374_NewStanleyPidComposite_SmoothTurnStopRotateStraight_Trial1.csv
+Declare P2 on T0(local)
+P2(on T0) decide to close

data_process/新版控制器轨迹测试处理/混合测试/第四次/记录.txt

@@ -0,0 +1,29 @@
+第一次
+: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.033m，航向误差=0.75°。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.CompositeStopTurnGoTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\CompositeMotionPlanTests.cs:line 187
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.033m，航向误差=0.75°。, stack:
+   at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 244
+   at MultiWheelC.MotionPlanExecutor.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\MotionPlanExecutor.cs:line 174
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+第二次：
+
+: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.041m，航向误差=0.24°。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.CompositeStopTurnGoTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\CompositeMotionPlanTests.cs:line 187
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.041m，航向误差=0.24°。, stack:
+   at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 244
+   at MultiWheelC.MotionPlanExecutor.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\MotionPlanExecutor.cs:line 174
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+第三次：
+ok
+
+

data_process/新版控制器轨迹测试处理/直线-圆弧-直线测试/第一组-失败/记录.txt

@@ -0,0 +1,19 @@
+: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.328m，航向误差=8.10°。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.NewControllerStraightSemicircleStraightTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\NewControllerTrackingTests.cs:line 449
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.328m，航向误差=8.10°。, stack:
+   at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 237
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+
+最主要的问题：曲率不连续
+目前轨迹几何是：
+直线：曲率 0
+→ 瞬间进入半径2m圆弧：曲率 0.5 1/m
+→ 瞬间离开圆弧：曲率重新变成0
+位置和航向是连续的，因此轨迹不会断开；但是曲率不连续。
+
+主要问题是“直线与圆弧的曲率瞬间跳变”，而你的 GCP 转角又被限制为每秒最多变化 10°，车辆无法瞬间进入或退出半径2m的圆弧。随后 Stanley 反馈不断补偿，形成明显振荡；速度偏高又进一步放大了问题。

data_process/新版控制器轨迹测试处理/直线测试/第一组4m测试/记录.txt

@@ -0,0 +1,18 @@
+: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.063m，航向误差=3.91°。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.NewControllerStraight4mTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\NewControllerTrackingTests.cs:line 146
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.063m，航向误差=3.91°。, stack:
+   at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 237
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+当前 Stanley 参数与车辆转向动态不匹配，形成了低频蛇形振荡。
+数据很有代表性：
+横向误差 RMSE：约 12.3 mm
+航向误差 RMSE：约 1.98°
+航向误差范围：约 -3.24°～+4.82°
+角速度命令共明显换向约5次，不是 Detour 高频噪声
+Stanley 目标角与经过 GCP 限速后实际发送角的平均差仅约 0.26°
+所以确实是“中心贴线，但车身左右摆”。

data_process/新版控制器轨迹测试处理/直线测试/第三组4m测试/记录.txt

@@ -0,0 +1,15 @@
+误差主要集中在轨迹 s=0.7～2.3m：
+该区间：
+横向RMSE约22.4mm
+航向RMSE约1.91°
+
+s>2.5m以后：
+横向RMSE约4.58mm
+航向RMSE约0.57°
+也就是说，第三组不是持续蛇形，而是在中途发生了一次：
+车辆向一侧偏移
+→ 控制器给出修正
+→ 实际角速度响应滞后
+→ 修正稍微过头
+→ 随后重新稳定
+第三组终点成功、后半段精度很好，说明控制器没有结构性错误。纵向 PID 也暂时不用调整。

data_process/新版控制器轨迹测试处理/直线测试/第二组4m测试/记录.txt

@@ -0,0 +1,22 @@
+: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.048m，航向误差=0.08°。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.NewControllerStraight4mTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\NewControllerTrackingTests.cs:line 146
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.048m，航向误差=0.08°。, stack:
+   at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 237
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+第二组在终点前的情况是：
+距离终点约97mm：
+参考速度 0.197m/s
+命令速度 0.135m/s
+实际速度约0.320m/s
+
+距离终点约12mm：
+参考速度 0.053m/s
+命令速度已经为0
+实际速度仍约0.286m/s
+说明控制器已经要求停车，但车辆和 M 层速度斜坡来不及完全降速，最终越过终点约48 mm。
+这不是 Stanley 横向控制问题，而是参考速度规划的减速距离不够。

data_process/新版控制器轨迹测试处理/直线测试/第五组4m测试3次/记录.txt

@@ -0,0 +1,15 @@
+第一次：
+: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.034m，航向误差=0.43°。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.NewControllerStraight4mTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\NewControllerTrackingTests.cs:line 146
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.034m，航向误差=0.43°。, stack:
+   at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 237
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+第二次：
+
+第三次：
+

data_process/新版控制器轨迹测试处理/直线测试/第四组4m测试3次/记录.txt

@@ -0,0 +1,57 @@
+第一次：
+: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.045m，航向误差=0.51°。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.NewControllerStraight4mTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\NewControllerTrackingTests.cs:line 146
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.045m，航向误差=0.51°。, stack:
+   at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 237
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+
+第二次：
+: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.032m，航向误差=0.48°。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.NewControllerStraight4mTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\NewControllerTrackingTests.cs:line 146
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.032m，航向误差=0.48°。, stack:
+   at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 237
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+
+第三次：
+
+: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.041m，航向误差=0.07°。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.NewControllerStraight4mTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\NewControllerTrackingTests.cs:line 146
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.041m，航向误差=0.07°。, stack:
+   at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 237
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+次数	横向 RMSE	航向 RMSE	速度 RMSE	终点误差
+第1次	19.70 mm	1.42°	0.0492 m/s	45.2 mm
+第2次	6.08 mm	0.45°	0.0515 m/s	31.7 mm
+第3次	8.66 mm	0.72°	0.0540 m/s	40.7 mm
+
+第二、三次横向效果不错；第一次数值较差。
+第一次变差的主要原因
+第一次在：
+t ≈ 9.74s
+s ≈ 2.22m
+出现了一次约 33.8mm 的 Detour 横向位置突变。突变后的位置持续保持在新的坐标基准上，不是单帧尖峰。
+控制器随后正常纠偏，但由于车辆转向响应存在滞后，形成了一次明显摆动：
+Detour横向位置突然变化
+→ Stanley认为车辆偏离约30mm
+→ 给出较大转向修正
+→ 实际角速度滞后
+→ 航向和横向误差产生一次超调
+因此第一次不是 Stanley 自己无缘无故发散，而是定位突变触发了欠阻尼响应。
+
+
+

docs/clumsy参考.json

@@ -0,0 +1,534 @@
+{
+  "layout": {
+    "chassis": {
+      "width": 1100.0,
+      "length": 1550.0,
+      "contour": [
+        -775.0,
+        550.0,
+        775.0,
+        550.0,
+        775.0,
+        -550.0,
+        -775.0,
+        -550.0
+      ]
+    },
+    "components": [
+      {
+        "type": "wheel",
+        "options": {
+          "platform": 0,
+          "scale": 1.0,
+          "radius": 200.0,
+          "group": null,
+          "id": 1,
+          "name": "w1",
+          "x": 0.0,
+          "y": 300.0,
+          "yaw": 0.0,
+          "z": 0.0,
+          "pitch": 0.0,
+          "roll": 0.0
+        }
+      },
+      {
+        "type": "wheel",
+        "options": {
+          "platform": 6,
+          "scale": 1.0,
+          "radius": 200.0,
+          "group": null,
+          "id": 2,
+          "name": "w2",
+          "x": 0.0,
+          "y": -300.0,
+          "yaw": 0.0,
+          "z": 0.0,
+          "pitch": 0.0,
+          "roll": 0.0
+        }
+      },
+      {
+        "type": "lidarssc",
+        "options": {
+          "usingLidars": "frontlidar",
+          "stopDist": 100.0,
+          "directionX": 1.0,
+          "directionY": 0.0,
+          "thresDot": 9999,
+          "contour": [
+            0.0,
+            0.0
+          ],
+          "group": [
+            "0",
+            "stop"
+          ],
+          "id": 411683697,
+          "name": "autoStop",
+          "x": 0.0,
+          "y": 0.0,
+          "yaw": 0.0,
+          "z": 0.0,
+          "pitch": 0.0,
+          "roll": 0.0
+        }
+      },
+      {
+        "type": "lidarssc",
+        "options": {
+          "usingLidars": "frontlidar",
+          "stopDist": 100.0,
+          "directionX": 1.0,
+          "directionY": 0.0,
+          "thresDot": 999,
+          "contour": [
+            0.0,
+            0.0
+          ],
+          "group": [
+            "0",
+            "slow"
+          ],
+          "id": 726862610,
+          "name": "autoSlow",
+          "x": 0.0,
+          "y": 0.0,
+          "yaw": 0.0,
+          "z": 0.0,
+          "pitch": 0.0,
+          "roll": 0.0
+        }
+      },
+      {
+        "type": "lidar2d",
+        "options": {
+          "isCircle": true,
+          "ignoreDist": 10.0,
+          "maxDist": 200000.0,
+          "useFilter": "",
+          "filterChassis": true,
+          "afterImageFilterOutN": 7,
+          "afterImageFilterOutDeg": 2.0,
+          "reflexThres": 0.4,
+          "reflexFilterWndSz": 30,
+          "reflexDistWnd": 50.0,
+          "reflexChunkThres": 2.5,
+          "BindLidar2dName": "",
+          "BindRelativeX": -4.9166203,
+          "BindRelativeY": 938.9871,
+          "BindRelativeTh": 1.300003,
+          "group": null,
+          "id": 1444795304,
+          "name": "rightlidar",
+          "x": -749.0,
+          "y": -475.0,
+          "yaw": 180.8,
+          "z": 0.0,
+          "pitch": 0.0,
+          "roll": 0.0
+        }
+      },
+      {
+        "type": "lidar2d",
+        "options": {
+          "isCircle": true,
+          "ignoreDist": 10.0,
+          "maxDist": 200000.0,
+          "useFilter": "",
+          "filterChassis": true,
+          "afterImageFilterOutN": 7,
+          "afterImageFilterOutDeg": 2.0,
+          "reflexThres": 0.4,
+          "reflexFilterWndSz": 30,
+          "reflexDistWnd": 50.0,
+          "reflexChunkThres": 2.5,
+          "BindLidar2dName": "",
+          "BindRelativeX": 0.0,
+          "BindRelativeY": 0.0,
+          "BindRelativeTh": 0.0,
+          "group": null,
+          "id": 1983955111,
+          "name": "leftlidar",
+          "x": -734.0,
+          "y": 475.0,
+          "yaw": 179.8,
+          "z": 0.0,
+          "pitch": 0.0,
+          "roll": 0.0
+        }
+      },
+      {
+        "type": "lidar3d",
+        "options": {
+          "ignoreDist": 5.0,
+          "maxDist": 200000.0,
+          "reduce": false,
+          "voxelSize": 70.0,
+          "pcklen": 82560,
+          "angleSgn": -1,
+          "endAngle": 0.0,
+          "RotationMatrix": [
+            1.0,
+            0.0,
+            0.0,
+            0.0,
+            1.0,
+            0.0,
+            -0.0,
+            0.0,
+            1.0
+          ],
+          "group": null,
+          "id": 1000069841,
+          "name": "frontlidar3d",
+          "x": 752.5,
+          "y": 0.0,
+          "yaw": 0.0,
+          "z": 0.0,
+          "pitch": 0.0,
+          "roll": 0.0
+        }
+      },
+      {
+        "type": "plannar3dlidarzrange",
+        "options": {
+          "zmin": -65.0,
+          "zmax": 7.0,
+          "useAbsolute": true,
+          "samples": 1024,
+          "lidar3dName": "frontlidar3d",
+          "isCircle": true,
+          "ignoreDist": 10.0,
+          "maxDist": 200000.0,
+          "useFilter": "",
+          "filterChassis": true,
+          "afterImageFilterOutN": 7,
+          "afterImageFilterOutDeg": 2.0,
+          "reflexThres": 0.4,
+          "reflexFilterWndSz": 30,
+          "reflexDistWnd": 50.0,
+          "reflexChunkThres": 2.5,
+          "BindLidar2dName": "",
+          "BindRelativeX": 0.0,
+          "BindRelativeY": 0.0,
+          "BindRelativeTh": 0.0,
+          "group": null,
+          "id": 1954892242,
+          "name": "frontlidar",
+          "x": 752.5,
+          "y": 0.0,
+          "yaw": 0.0,
+          "z": 0.0,
+          "pitch": 0.0,
+          "roll": 0.0
+        }
+      },
+      {
+        "type": "lidarssc",
+        "options": {
+          "usingLidars": "frontlidar",
+          "stopDist": 100.0,
+          "directionX": 1.0,
+          "directionY": 0.0,
+          "thresDot": 10,
+          "contour": [
+            600.0,
+            -650.0,
+            600.0,
+            650.0,
+            1200.0,
+            650.0,
+            1200.0,
+            -650.0
+          ],
+          "group": [
+            "1",
+            "stop"
+          ],
+          "id": 1519627219,
+          "name": "fstop1",
+          "x": 230.0,
+          "y": 0.0,
+          "yaw": 0.0,
+          "z": 0.0,
+          "pitch": 0.0,
+          "roll": 0.0
+        }
+      },
+      {
+        "type": "lidarssc",
+        "options": {
+          "usingLidars": "frontlidar",
+          "stopDist": 100.0,
+          "directionX": 1.0,
+          "directionY": 0.0,
+          "thresDot": 10,
+          "contour": [
+            1200.0,
+            -650.0,
+            1200.0,
+            650.0,
+            2600.0,
+            650.0,
+            2600.0,
+            -650.0
+          ],
+          "group": [
+            "1",
+            "slow"
+          ],
+          "id": 771141297,
+          "name": "fslow1",
+          "x": 230.0,
+          "y": 0.0,
+          "yaw": 0.0,
+          "z": 0.0,
+          "pitch": 0.0,
+          "roll": 0.0
+        }
+      }
+    ]
+  },
+  "DriveTaskInterval": 30,
+  "DriveTaskTimeout": 9999.0,
+  "basicSpeed": 0.7,
+  "msConf": {
+    "SyncThAccPerSec": 5.0,
+    "TestCarSyncDistance": 2893.0,
+    "TestCarSyncTh": 0.0,
+    "ManualCarSyncVxFac": 0.3,
+    "ManualCarSyncVyFac": 1.0,
+    "ManualCarSyncVthFac": 30.0,
+    "MultiVehicleCrabSteerLimitDeg": 120.0,
+    "DeltaDetectCenter": 742.5,
+    "MultiVehicleSyncUseDetour": true,
+    "MultiVehicleManualUseDetourCorrection": true,
+    "MultiVehicleFleetNum": 2,
+    "MultiVehicleSyncInterval": 25,
+    "MultiVehicleMasterEndpoint": "/",
+    "SimpleIp": "192.168.1.101",
+    "MultiVehicleSelfEndpoint": "",
+    "MultiVehicleUseDetect": false,
+    "MultiVehicleControlRadius": 0.0,
+    "MultiVehicleAutoCmdTimeoutMs": 9999,
+    "MultiVehicleMemberTtlMs": 0,
+    "MultiVehicleAutoUseIdealCenter": true,
+    "MultiVehicleAutoRequireFleetCenter": true,
+    "MultiVehiclePosBiasXFac": 0.005,
+    "MultiVehiclePosBiasYFac": 0.15,
+    "MultiVehiclePosBiasThFac": 0.1,
+    "MultiVehiclePosBiasXThreshold": 0.05,
+    "MultiVehiclePosBiasYThreshold": 10.0,
+    "MultiVehiclePosBiasThThreshold": 5.0,
+    "MultiVehicleDetectBiasXFac": 0.0,
+    "MultiVehicleDetectBiasYFac": 0.0,
+    "MultiVehicleDetectBiasThFac": 0.0,
+    "MultiVehicleDetectBiasXThreshold": 0.0,
+    "MultiVehicleDetectBiasYThreshold": 0.0,
+    "MultiVehicleDetectBiasThThreshold": 0.0,
+    "MultiVehicleRotateCompXyFac": 0.003,
+    "MultiVehicleRotateCompXyIFac": 0.01,
+    "MultiVehicleRotateCompXyMax": 3.0,
+    "MultiVehicleRotateCompThFac": 0.1,
+    "MultiVehicleRotateCompThIFac": 0.01,
+    "MultiVehicleRotateCompThMax": 3.0,
+    "MultiVehicleRotateActiveOmega": 0.5,
+    "MultiVehicleRotateCompTangentFrac": 0.1,
+    "SingleCarSyncPrecisionXy": 10.0,
+    "SingleCarSyncPrecisionTh": 0.1,
+    "PlaygroundWebApiUrl": "http://localhost:18090",
+    "MultiVehicleRotatePoseWebApiDiagEnabled": false,
+    "PlaygroundRobotName": "agv_multi_1",
+    "PlaygroundNeighborRobotName": "agv_multi_2",
+    "WebApiTranslateMm": 100.0,
+    "WebApiRotateDeg": 5.0,
+    "InPlaceRotateTargetWorldDeg": 90.0,
+    "InPlaceRotateSpeed": 30.0,
+    "InPlaceRotateArriveDeg": 1.0,
+    "InPlaceRotateWheelAlignDeg": 2.0,
+    "InPlaceRotateActiveWheelAlignDeg": 10.0,
+    "FleetRotateOmega": 6.0,
+    "FleetRotateTargetDeltaDeg": 90.0,
+    "FleetRotateArriveDeg": 1.5,
+    "FleetRotateSlowDeg": 10.0,
+    "FleetRotateMinOmega": 0.5,
+    "FleetRotateAccel": 1.0,
+    "FleetRotateSettleSec": 0.5,
+    "FleetRotateUseDetourHeading": true,
+    "FleetCrabAngleDeg": 90.0,
+    "FleetCrabBodyWorldHeadingDeg": 0.0,
+    "FleetCrabLengthMm": 2000.0,
+    "FleetCrabSpeed": 0.35,
+    "FleetCrabAccel": 0.1,
+    "FleetCrabStartAccel": 0.02,
+    "FleetCrabSlowDistance": 800.0,
+    "FleetCrabFinishDistance": 10.0,
+    "FleetCrabFinishSpeed": 0.0,
+    "FleetCrabSlowingPow": 0.7,
+    "FleetCrabGcpThetaThreshold": 120.0,
+    "FleetCrabDthLinearFac": 3.3,
+    "FleetCrabDthLinearThreshold": 10.0,
+    "FleetCrabStartSyncTimeoutSec": 9999.0,
+    "FleetCrabStartWheelAlignDeg": 2.0,
+    "TwoLegLidarName": "leftlidar,rearlidar",
+    "TwoLegGuessX": -2000.0,
+    "TwoLegWidth": 450.0,
+    "TwoLegWidthErr": 50.0,
+    "TwoLegBlobDist": 100.0,
+    "TwoLegBlobSize": 200.0,
+    "TwoLegBlobPtCount": 10,
+    "TwoLegPadding": 5,
+    "TwoLegPillarFindingScope": 20,
+    "TwoLegSgnDir": 1,
+    "TwoLegCenterChangeX": 0.0,
+    "TwoLegOutputBiasX": -15.0,
+    "TwoLegOutputBiasY": 0.0,
+    "TwoLegFilterLength": 500.0,
+    "TwoLegFilterWidth": 800.0,
+    "TireFilterLength": 1000.0,
+    "TireFilterWidth": 1900.0,
+    "TireTwoLegWidth": 1470.0,
+    "TireTwoLegWidthErr": 200.0,
+    "TireTwoLegBlobPtCount": 10,
+    "TireFrontTwoLegBlobDist": 100.0,
+    "TireFrontTwoLegBlobSize": 200.0,
+    "TireFrontPadding": 10,
+    "TireFrontTwoLegPillarFindingScope": 10,
+    "TireFrontTwoLegSgnDir": 1,
+    "TireFrontTwoLegCenterChangeX": 0.0,
+    "TireBackTwoLegBlobDist": 100.0,
+    "TireBackTwoLegBlobSize": 200.0,
+    "TireBackPadding": 5,
+    "TireBackTwoLegPillarFindingScope": 20,
+    "TireBackTwoLegSgnDir": 1,
+    "TireBackTwoLegCenterChangeX": 0.0,
+    "ClampControlKp": 0.0015,
+    "ClampControlKi": 0.0,
+    "ClampControlKd": 0.0,
+    "ClampControlMaxI": 0.02,
+    "ClampControlSpeedAcc": 2.0,
+    "ClampControlThresh": 0.2,
+    "ClampControlDeadZone": 30.0,
+    "MaxClampSpeed": 12.0,
+    "LineTrackDistance": 1000.0,
+    "LineTrackMaxSpeed": 0.3,
+    "LineTrackKp": 0.001,
+    "LineTrackKi": 0.0,
+    "LineTrackKd": 0.0,
+    "LineTrackDeadZone": 10.0,
+    "TireFollowingWalkBlindSwitchingDistance": 1300.0,
+    "TireFollowingStage1GuessX": 2200.0,
+    "TireFollowingStage2GuessX": 2600.0,
+    "TireFollowingWalkBlindFinishDistance": 10.0,
+    "TireFollowingSlowDistance": 750.0,
+    "TireFollowingMaxSpeed": 0.3,
+    "TireFollowingFrontLidarPathTransformationX": 160.0,
+    "TireFollowingFrontLidarPathTransformationY": 3.0,
+    "TireFollowingFrontLidarWalkBlindTh": 0.0,
+    "TireFollowingBackLidarPathTransformationX": 155.0,
+    "TireFollowingBackLidarPathTransformationY": 0.0,
+    "TireFollowingBackLidarWalkBlindTh": 0.0,
+    "TireFollowingLeaveCarBackLidarPathTransformationX": 1700.0,
+    "TireFollowingLeaveCarWalkBlindSwitchingDistance": 2400.0,
+    "TireFollowingTireNum": 2,
+    "TireFollowingCloseDistance": 1400.0,
+    "TireFollowingAngleIgnoreThr": 1.0,
+    "TireFollowingYAverageFrameCount": 6,
+    "DstTrackerMaxSpeed": 0.3,
+    "GcpThetaThreshold": 70.0,
+    "DthLinearFac": 0.75,
+    "DthLinearThreshold": 25.0,
+    "BiasFac": 0.5,
+    "BiasThreshold": 15.0,
+    "BiasControlGainFac": 1.0,
+    "BiasSlowSigma": 55.0,
+    "LineMagKp": 0.1,
+    "LineMagKi": 0.0,
+    "LineMagKd": 0.1,
+    "MagMaxI": 10.0,
+    "MagDeadZone": 1.0,
+    "LineMagThresh": 35.0,
+    "CurveMagKp": 0.4,
+    "CurveMagKi": 0.0,
+    "CurveMagKd": 0.1,
+    "CurveMagThresh": 65.0,
+    "MotionDebugPrint": true,
+    "DebugCurvature": false,
+    "SlowDistance": 1000.0,
+    "SlowingPow": 0.8,
+    "FinishDistance": 5.0,
+    "FinishSpeed": 0.02,
+    "FirstThAccuracy": 2.0,
+    "ThContinuousThreshold": 10.0,
+    "FirstRotateSpeedFac": 1.0,
+    "FirstRotateMaxSpeed": 30.0,
+    "FirstRotateAcc": 20.0,
+    "FirstRotateDeAcc": 30.0,
+    "SpeedAccPerSecond": 0.1,
+    "SpeedDeAccPerSecond": 1.0,
+    "NotContinuousAngle": 3.0,
+    "PowerSteeringLookAhead": 100.0,
+    "SpeedLookAhead": 1500.0,
+    "SpeedLookAheadCurveDiff": 1000.0,
+    "SpeedLookBackCurveDiff": 200.0,
+    "SpeedLimitCurveDiffMin": 0.2,
+    "SpeedLimitCurveMin": 0.2,
+    "MaxRotateSpeedCurveLimit": 30.0,
+    "MaxRotateAccCurveLimit": 30.0,
+    "BaisAlarmValue": 1500.0,
+    "DthAlarmValue": 150.0,
+    "UseAutoAvoidance": false,
+    "ObstacleStopDistance": 1000.0,
+    "ObstacleSlowDistance": 2500.0,
+    "CoefficientOfExpansion": 1.0,
+    "TargetSpeed": 0.5,
+    "EmptyCartLength": 1550.0,
+    "EmptyCartWidth": 1100.0,
+    "RotateStopFac": 1.3,
+    "RotateSlowFac": 1.8,
+    "SlowPow": 1.2,
+    "ShieldAutoObstacle": false,
+    "LidarName": "frontlidar",
+    "ObstacleRecoveryTime": 500,
+    "UseCameraAvoidance": false,
+    "UseManualContorolAvoidance": true,
+    "ManualAutoAvoidcaneSlowDistance": 500.0,
+    "ManualAutoAvoidcaneStopDistance": 300.0,
+    "ShieldAutoAvoidance": false,
+    "UpCamPoseX": 0.0,
+    "UpCamPoseY": 0.0,
+    "UpCamPoseTh": 0.0,
+    "DownCamPoseX": 0.0,
+    "DownCamPoseY": 0.0,
+    "DownCamPoseTh": 0.0,
+    "OutMapEnable": true,
+    "GroundLossThreshold": 5.0,
+    "LaserLossThreshold": 15.0,
+    "RiskSlowdownThreshold": 15.0,
+    "UseSimpleDetector": false,
+    "LoseConnectionTime": 5000,
+    "GroundCameraDisconnectAlarmTime": 200,
+    "FrontLidarName": "null",
+    "RearLidarName": "null",
+    "UseSkidDetector": true,
+    "SkidTimeThreshold": 3000,
+    "SkidFacThreshold": 3.0,
+    "MissionWarningTime": 3000,
+    "UseGyrosDetector": false,
+    "GyrosErrorTime": 3000,
+    "GyrosErrorFac": 10,
+    "ObstacleStopDec": 0.1
+  },
+  "script": "MultiWheelC.dll",
+  "guru": {
+    "MaxLogFiles": 20,
+    "interpreter": "javascript",
+    "throwSAIError": true
+  },
+  "locationTimeout": 100,
+  "IOCheckIntegrity": true,
+  "detourHost": "127.0.0.1",
+  "detourPort": 4321
+}

docs/数据融合.txt

@@ -0,0 +1,94 @@
+是的，强烈建议做系统辨识，尤其是你这种要把电机反馈和 SLAM 融合的场景。
+为什么需要系统辨识？
+卡尔曼滤波（或 EKF）的效果很大程度上取决于过程模型有多准。模型不准的话，会出现：
+
+预测步持续往错误方向跑
+滤波器过度依赖测量（SLAM），或者反过来过度信任错误的模型
+速度估计系统性偏大/偏小
+原地自转时航向纠正效果变差
+
+你现在已经知道电机反馈“偏大”，这本身就是典型的模型参数问题（可能是轮胎半径、减速比、编码器标定、打滑补偿等）。
+建议辨识的主要参数
+针对四轮差速车，优先考虑这些：
+
+运动学参数
+有效轮半径（或左右轮半径）
+轮距（左右轮中心距离）
+可能的轴距相关项（如果不是纯差速）
+
+速度相关
+电机速度到实际车速的缩放系数（你提到的“偏大”问题）
+左右轮速度的不对称性（如果存在）
+低速段的非线性（静摩擦、死区）
+
+可选但有用的
+速度偏置 / 零点漂移
+简单的打滑系数（如果经常在特定地面跑）
+
+
+怎么做比较实际？
+不需要一开始就上很复杂的在线辨识，可以分两步：
+离线标定（推荐先做）
+
+让车在平坦地面以不同速度直线行驶、原地旋转，记录：
+四个电机反馈速度
+SLAM 给出的位置和航向变化
+
+用最小二乘或简单优化，拟合出轮半径、轮距、速度缩放系数等
+原地自转特别适合标定轮距和左右轮差异
+
+在线微调（可选）
+
+把关键参数（比如速度缩放系数、轮半径）也放进状态向量里一起估计（Augmented EKF）
+或者用简单的自适应方法，定期用 SLAM 和电机速度的一致性去微调缩放因子
+
+实际建议顺序
+
+先建立基本运动学模型 + EKF 框架
+做一轮离线系统辨识，把明显偏差（尤其是速度偏大）修正掉
+再跑融合，看残差（innovation）是否变小、速度估计是否更合理
+如果还有残余误差，再考虑把个别参数做成在线估计
+
+总结：建完模型后做系统辨识，是让卡尔曼滤波真正好用的关键一步。不做的话，即使融合了电机和 SLAM，效果也容易打折扣。需要的话，我可以给你一个针对差速车的简单离线标定思路（需要记录哪些数据、怎么拟合）。
+
+有没有必要做卡尔曼/传感器融合？
+分两层看：
+
+如果只是为了速度闭环，且当前绿色线已经能让车跑得比较稳
+→ 性价比一般。
+你现在的处理已经解决了最严重的问题，继续花时间做完整的 EKF + 电机融合，短期内收益可能没有那么大。可以先把绿色线再优化一下（比如调整低通截止频率、加一点自适应滤波，或者用更温和的滑动平均），看能不能把残余抖动再压低一点。
+如果你对速度精度、平滑度要求比较高，或者后面还要做更精细的控制（轨迹跟踪、力控、高精度定位等）
+→ 值得做，性价比会变高。
+原因是：
+绿色线本质还是“从位置差分出来的速度”，天然会有延迟和残余噪声。
+电机反馈是更直接的速度源，频率高、相位更好。
+把电机速度和经过处理的 SLAM 差分速度做融合（哪怕是简单的互补滤波或一阶卡尔曼），通常能同时得到：
+更低的噪声
+更小的滞后
+对电机“偏大”问题的一定校正能力
+
+
+
+实际建议（按性价比排序）
+
+先低成本优化现有绿色线（最快）
+检查低通滤波的截止频率是否合适（太低会滞后，太高会留噪声）。
+可以尝试一阶低通 + 中值滤波组合，或者简单的互补滤波（位置差分 + 电机速度）。
+看看闭环时车的实际表现，如果抖动已经可接受，就先用着。
+
+中等成本：做简单融合（推荐优先考虑）
+不用完整 EKF，先做一个互补滤波或一维卡尔曼只融速度：
+高频信任电机反馈速度
+低频用处理后的 SLAM 差分速度去校正偏差
+
+实现简单，效果通常比单纯低通好一截，滞后也更小。
+
+完整 EKF + 系统辨识
+只有在你需要同时提升位置、航向、速度的整体一致性，或者后面要上更复杂的控制时，再上这个。
+工作量明显更大，但长期收益最高。
+
+
+总结
+从你这张图来看，目前绿色线已经能用，不是“必须立刻上卡尔曼”的紧急情况。
+但如果你觉得绿色线的残余抖动还是影响闭环效果，或者想要更干净、滞后更小的速度，那么把四个电机反馈融进去（哪怕先做简单融合）是性价比不错的下一步，比继续死磕低通滤波更有前途。
+你现在速度闭环时，车实际表现如何？是觉得绿色线的抖动已经导致控制不稳，还是只是看着不舒服？这能帮助判断要不要马上上融合。

docs/记录.txt

@@ -55,3 +55,10 @@ ParkingGeometricController.cs
 增加起步航向对齐状态。
 第一阶段建议采用第二种，简单、安全。
 
+把前后GCP转角分解成两个模态：
+共同转角 = (前GCP转角 + 后GCP转角) / 2
+差动转角 = (前GCP转角 - 后GCP转角) / 2
+
+
+
+