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

2026-08-07 18:13:55 +08:00
parent bc37e71ad0
commit 88f651e0c2
25 changed files with 310 additions and 105 deletions
@@ -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目标转角必须是有限值。");
            }
        }
    }
@@ -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>
@@ -33,7 +33,7 @@ namespace MultiWheelC.Control.Execution
        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,
@@ -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(
-                CrossTrackGainPerSecond *
-                context.LateralErrorMeters /
-                speedMagnitude);
+            // 横向误差生成前后同向的共同转角，使四舵轮车辆平稳靠近轨迹。
+            var crossTrackCorrectionRadians =
+                ClampSymmetric(
+                    Math.Atan(
+                        CrossTrackGainPerSecond *
+                        context.LateralErrorMeters /
+                        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控制器的几何尺寸、速度和角度限制必须是正有限值。");
            }
        }

@@ -47,7 +47,7 @@ namespace MultiWheelC
        /// <summary>
        /// 获取或设置参考速度减速度，单位为m/s²。
        /// </summary>
-        public double DecelerationMetersPerSecondSquared = 0.12;
+        public double DecelerationMetersPerSecondSquared = 0.10;

        /// <summary>
        /// 获取或设置离散轨迹点间距，单位为m。
@@ -173,7 +173,12 @@ namespace MultiWheelC

                    foreach (var keepRunning in movement.Get())
                    {
-                        yield return keepRunning;
+                        if (!keepRunning)
+                        {
+                            break;
+                        }
+
+                        yield return true;
                    }

                    continue;
@@ -219,7 +224,12 @@ namespace MultiWheelC

                    foreach (var keepRunning in movement.Get())
                    {
-                        yield return keepRunning;
+                        if (!keepRunning)
+                        {
+                            break;
+                        }
+
+                        yield return true;
                    }

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

+把前后GCP转角分解成两个模态：
+共同转角 = (前GCP转角 + 后GCP转角) / 2
+差动转角 = (前GCP转角 - 后GCP转角) / 2
+
+
+
+