增加GCP运动学并完善车体平面速度反馈分析

MedullaAdapter/DiverCartDefinition.cs

@@ -283,8 +283,6 @@ namespace MedullaAdapter
                 Math.Sign(x);
             var steeringDegrees =
                 -normalizedSteering * MaxManualTheta;
-            var frontTh = steeringDegrees;
-            var rearTh = -steeringDegrees;
             ManualMode = (int)mode;
 
             switch (mode)
@@ -292,16 +290,24 @@ namespace MedullaAdapter
                 case ManualControlMode.Normal:
                     // 普通模式统一使用车体速度命令：
                     // X向前，行驶中连续改变角速度时舵轮边转、车辆边走。
-                    // SendBodyCommand(
+                    var normalOmegaRadiansPerSecond =
-                    //     vx: speed,
+                        speed *
-                    //     vy: 0.0,
+                        Math.Tan(
-                    //     omegaRadiansPerSecond: omega,
+                            AngleMath.DegreesToRadians(
-                    //     interval);
+                                steeringDegrees)) /
-                    Chassis.SendMotion(
+                        adapter.ControlPointRadiusMeters;
+                    if (!adapter.SendBodyTwist(
+                            new Twist2D(
                                 speed,
-                                frontTh,
+                                0.0,
-                                rearTh,
+                                normalOmegaRadiansPerSecond),
-                                interval);
+                            interval))
+                    {
+                        adapter.StopImmediately();
+                        Console.WriteLine(
+                            "Normal SendMotion decomposition failed: " +
+                            adapter.LastFailureReason);
+                    }
                     break;
                 case ManualControlMode.Crab:
                     // 舵轮机械范围为[-120°,120°]。
@@ -332,13 +338,15 @@ namespace MedullaAdapter
 
                     // 将车体左侧作为虚拟阿克曼车头，并在该运动坐标系中
                     // 复用与普通模式相同的SendMotion前后控制点解算。
-                    if (!adapter.SendVirtualAckermannMotion(
+                    var crabOmegaRadiansPerSecond =
-                            motionDirectionRadians:
+                        speed *
-                                Math.PI / 2.0,
+                        Math.Tan(crabSteeringRadians) /
-                            speedMetersPerSecond:
+                        adapter.ControlPointRadiusMeters;
+                    if (!adapter.SendBodyTwist(
+                            new Twist2D(
+                                0.0,
                                 speed,
-                            steeringRadians:
+                                crabOmegaRadiansPerSecond),
-                                crabSteeringRadians,
                             interval))
                     {
                         adapter.StopImmediately();
@@ -380,13 +388,11 @@ namespace MedullaAdapter
 
                     // 普通安全版SendXYThSpeed只下发角速度，
                     // 四轮实际舵角未到位时不会开放驱动速度。
-                    if (!adapter.Send(
+                    if (!adapter.SendBodyTwist(
-                            new ChassisCommand(
+                            new Twist2D(
-                                CarNum,
+                                0.0,
-                                new Twist2D(
+                                0.0,
-                                    0.0,
+                                spinOmegaRadiansPerSecond),
-                                    0.0,
-                                    spinOmegaRadiansPerSecond)),
                             interval))
                     {
                         adapter.StopImmediately();

MedullaAdapter/MedullaAdapter.csproj

@@ -42,8 +42,8 @@
   </ItemGroup>
 
   <ItemGroup>
-  <Compile Include="..\Shared\Models\ChassisCommand.cs"
+  <Compile Include="..\Shared\Models\MotionModels.cs"
-           Link="Shared\Models\ChassisCommand.cs" />
+           Link="Shared\Models\MotionModels.cs" />
 
   <Compile Include="..\Shared\Mathematics\FrameTransform2D.cs"
            Link="Shared\Mathematics\FrameTransform2D.cs" />

MultiWheelC/Control/Abstractions/PathTrackingContext.cs

@@ -1,6 +1,7 @@
 using System;
 using MultiWheelC.StateEstimation;
 using MultiWheelC.Trajectory;
+using MyParking.Shared;
 
 namespace MultiWheelC.Control.Abstractions
 {
@@ -17,7 +18,8 @@ namespace MultiWheelC.Control.Abstractions
             TrajectoryProjection projection,
             double controlReferenceSpeedMetersPerSecond,
             double feedforwardCurvaturePerMeter,
-            double deltaTimeSeconds)
+            double deltaTimeSeconds,
+            double motionDirectionInBodyRadians = 0.0)
         {
             EnsureFinite(
                 controlReferenceSpeedMetersPerSecond,
@@ -28,6 +30,9 @@ namespace MultiWheelC.Control.Abstractions
             EnsureFinitePositive(
                 deltaTimeSeconds,
                 nameof(deltaTimeSeconds));
+            EnsureFinite(
+                motionDirectionInBodyRadians,
+                nameof(motionDirectionInBodyRadians));
 
             VehicleState = vehicleState;
             Projection = projection;
@@ -36,6 +41,9 @@ namespace MultiWheelC.Control.Abstractions
             FeedforwardCurvaturePerMeter =
                 feedforwardCurvaturePerMeter;
             DeltaTimeSeconds = deltaTimeSeconds;
+            MotionDirectionInBodyRadians =
+                AngleMath.NormalizeRadians(
+                    motionDirectionInBodyRadians);
         }
 
         /// <summary>
@@ -59,11 +67,18 @@ namespace MultiWheelC.Control.Abstractions
         public double ControlReferenceSpeedMetersPerSecond { get; }
 
         /// <summary>
-        /// 获取车辆在车体X轴方向上的实际纵向速度，单位为m/s。
+        /// 获取车辆沿当前运动坐标系X轴方向的实际纵向速度，单位为m/s。
         /// </summary>
         public double ActualLongitudinalSpeedMetersPerSecond =>
-            VehicleState.TwistInBody
+            Math.Cos(MotionDirectionInBodyRadians) *
-                .VxMetersPerSecond;
+                VehicleState.TwistInBody.VxMetersPerSecond +
+            Math.Sin(MotionDirectionInBodyRadians) *
+                VehicleState.TwistInBody.VyMetersPerSecond;
+
+        /// <summary>
+        /// 获取当前运动坐标系X轴在车体系中的方向，单位为rad。
+        /// </summary>
+        public double MotionDirectionInBodyRadians { get; }
 
         /// <summary>
         /// 获取沿轨迹执行点序定义的参考曲率，单位为1/m，左弯为正。

MultiWheelC/Control/Allocation/GcpKinematics.cs

@@ -0,0 +1,125 @@
+using System;
+using MyParking.Shared;
+
+namespace MultiWheelC.Control.Allocation
+{
+    /// <summary>
+    /// 在对称前后GCP方向命令与车体中心刚体速度之间执行纯几何转换。
+    /// </summary>
+    public static class GcpKinematics
+    {
+        private const double ParallelDirectionTolerance = 1e-9;
+        private const double StopSpeedDeadbandMetersPerSecond = 1e-6;
+
+        /// <summary>
+        /// 将有符号中心速度和前后GCP方向转换为真实车体坐标系中的Twist2D。
+        /// </summary>
+        public static Twist2D ToBodyTwist(
+            GcpMotionCommand command,
+            double controlPointRadiusMeters)
+        {
+            NumericGuard.EnsureFinitePositive(
+                controlPointRadiusMeters,
+                nameof(controlPointRadiusMeters));
+
+            if (Math.Abs(command.SpeedMetersPerSecond) <=
+                StopSpeedDeadbandMetersPerSecond)
+            {
+                return Twist2D.Zero;
+            }
+
+            var frontAngleRadians =
+                command.FrontAngleRadians;
+            var rearAngleRadians =
+                command.RearAngleRadians;
+            var directionDeterminant =
+                Math.Sin(
+                    rearAngleRadians -
+                    frontAngleRadians);
+
+            if (Math.Abs(directionDeterminant) <=
+                ParallelDirectionTolerance)
+            {
+                var averageDirectionRadians =
+                    Math.Atan2(
+                        Math.Sin(frontAngleRadians) +
+                        Math.Sin(rearAngleRadians),
+                        Math.Cos(frontAngleRadians) +
+                        Math.Cos(rearAngleRadians));
+
+                return new Twist2D(
+                    command.SpeedMetersPerSecond *
+                    Math.Cos(averageDirectionRadians),
+                    command.SpeedMetersPerSecond *
+                    Math.Sin(averageDirectionRadians),
+                    0.0);
+            }
+
+            var frontCosine =
+                Math.Cos(frontAngleRadians);
+            var frontSine =
+                Math.Sin(frontAngleRadians);
+            var rearCosine =
+                Math.Cos(rearAngleRadians);
+            var rearSine =
+                Math.Sin(rearAngleRadians);
+
+            // 两个GCP速度方向的法线交点就是瞬时旋转中心，坐标位于真实车体系。
+            var rotationCenterXMeters =
+                controlPointRadiusMeters *
+                (frontCosine * rearSine +
+                 frontSine * rearCosine) /
+                directionDeterminant;
+            var rotationCenterYMeters =
+                -2.0 *
+                controlPointRadiusMeters *
+                frontCosine *
+                rearCosine /
+                directionDeterminant;
+            var centerRadiusMeters =
+                Math.Sqrt(
+                    rotationCenterXMeters *
+                    rotationCenterXMeters +
+                    rotationCenterYMeters *
+                    rotationCenterYMeters);
+
+            if (!NumericGuard.IsFinite(centerRadiusMeters) ||
+                centerRadiusMeters <= 0.0)
+            {
+                throw new InvalidOperationException(
+                    "前后GCP方向不能生成有效的车体中心旋转半径。");
+            }
+
+            // 用有符号速度决定绕ICR的实际转向，避免倒车时把同一组GCP轴向解释成反向运动。
+            var requestedDirectionSign =
+                Math.Sign(command.SpeedMetersPerSecond);
+            var positiveAngularFrontVelocityXMetersPerSecond =
+                rotationCenterYMeters;
+            var positiveAngularFrontVelocityYMetersPerSecond =
+                controlPointRadiusMeters -
+                rotationCenterXMeters;
+            var frontDirectionAlignment =
+                positiveAngularFrontVelocityXMetersPerSecond *
+                requestedDirectionSign *
+                frontCosine +
+                positiveAngularFrontVelocityYMetersPerSecond *
+                requestedDirectionSign *
+                frontSine;
+            var omegaSign =
+                frontDirectionAlignment >= 0.0
+                    ? 1.0
+                    : -1.0;
+            var omegaRadiansPerSecond =
+                omegaSign *
+                Math.Abs(command.SpeedMetersPerSecond) /
+                centerRadiusMeters;
+
+            return new Twist2D(
+                omegaRadiansPerSecond *
+                rotationCenterYMeters,
+                -omegaRadiansPerSecond *
+                rotationCenterXMeters,
+                omegaRadiansPerSecond);
+        }
+    }
+}

MultiWheelC/Control/Execution/GcpCommandExecutor.cs

@@ -13,6 +13,7 @@ namespace MultiWheelC.Control.Execution
             1e-6;
 
         private readonly MultiWheelChassisAdapter _chassisAdapter;
+        private readonly double _motionDirectionInBodyRadians;
         private double _lastFrontAngleRadians;
         private double _lastRearAngleRadians;
 
@@ -22,7 +23,8 @@ namespace MultiWheelC.Control.Execution
         public GcpCommandExecutor(
             MultiWheelChassisAdapter chassisAdapter,
             double maximumGcpAngleRateRadiansPerSecond =
-                10.0 * Math.PI / 180.0)
+                10.0 * Math.PI / 180.0,
+            double motionDirectionInBodyRadians = 0.0)
         {
             _chassisAdapter = chassisAdapter ??
                 throw new ArgumentNullException(
@@ -30,9 +32,15 @@ namespace MultiWheelC.Control.Execution
             EnsureFinitePositive(
                 maximumGcpAngleRateRadiansPerSecond,
                 nameof(maximumGcpAngleRateRadiansPerSecond));
+            NumericGuard.EnsureFinite(
+                motionDirectionInBodyRadians,
+                nameof(motionDirectionInBodyRadians));
 
             MaximumGcpAngleRateRadiansPerSecond =
                 maximumGcpAngleRateRadiansPerSecond;
+            _motionDirectionInBodyRadians =
+                AngleMath.NormalizeRadians(
+                    motionDirectionInBodyRadians);
         }
 
         /// <summary>
@@ -103,10 +111,19 @@ namespace MultiWheelC.Control.Execution
                 _lastRearAngleRadians);
             LastSentCommand = limitedCommand;
 
-            var success = _chassisAdapter.SendGcpMotion(
+            var motionFrameTwist =
-                limitedCommand.SpeedMetersPerSecond,
+                GcpKinematics.ToBodyTwist(
-                limitedCommand.FrontAngleRadians,
+                    limitedCommand,
-                limitedCommand.RearAngleRadians,
+                    _chassisAdapter.ControlPointRadiusMeters);
+            var bodyTwist =
+                FrameTransform2D.TransformTwistAtSamePoint(
+                    new Pose2D(
+                        0.0,
+                        0.0,
+                        _motionDirectionInBodyRadians),
+                    motionFrameTwist);
+            var success = _chassisAdapter.SendBodyTwist(
+                bodyTwist,
                 TimeSpan.FromSeconds(deltaTimeSeconds));
 
             LastFailureReason = success

MultiWheelC/Control/Execution/ParkingGeometricController.cs

@@ -86,6 +86,7 @@ namespace MultiWheelC.Control.Execution
         private readonly ILongitudinalController _longitudinalController;
         private readonly GcpCommandAllocator _gcpAllocator;
         private readonly GcpCommandExecutor _commandExecutor;
+        private readonly double _motionDirectionInBodyRadians;
 
         private Trajectory2D _trajectory;
         private double _terminalTravelDirection = 1.0;
@@ -112,7 +113,8 @@ namespace MultiWheelC.Control.Execution
             double terminalApproachGainPerSecond = 0.8,
             double maximumTerminalApproachSpeedMetersPerSecond = 0.05,
             double curvaturePreviewSeconds = 0.20,
-            double maximumCurvaturePreviewMeters = 0.12)
+            double maximumCurvaturePreviewMeters = 0.12,
+            double motionDirectionInBodyRadians = 0.0)
         {
             _stateProvider = stateProvider ??
                 throw new ArgumentNullException(
@@ -160,6 +162,9 @@ namespace MultiWheelC.Control.Execution
             EnsureFiniteNonNegative(
                 maximumCurvaturePreviewMeters,
                 nameof(maximumCurvaturePreviewMeters));
+            NumericGuard.EnsureFinite(
+                motionDirectionInBodyRadians,
+                nameof(motionDirectionInBodyRadians));
 
             if (terminalApproachDistanceMeters <=
                 finishDistanceMeters)
@@ -187,6 +192,9 @@ namespace MultiWheelC.Control.Execution
             CurvaturePreviewSeconds = curvaturePreviewSeconds;
             MaximumCurvaturePreviewMeters =
                 maximumCurvaturePreviewMeters;
+            _motionDirectionInBodyRadians =
+                AngleMath.NormalizeRadians(
+                    motionDirectionInBodyRadians);
         }
 
         /// <summary>
@@ -484,7 +492,8 @@ namespace MultiWheelC.Control.Execution
                     projection,
                     controlReferenceSpeedMetersPerSecond,
                     feedforwardCurvaturePerMeter,
-                    deltaTimeSeconds);
+                    deltaTimeSeconds,
+                    _motionDirectionInBodyRadians);
                 var lateralCommand =
                     _lateralController.Compute(context);
                 var commandSpeedMetersPerSecond =
@@ -617,9 +626,8 @@ namespace MultiWheelC.Control.Execution
 
             var previewSpeedMetersPerSecond =
                 vehicleState.HasValidVelocityEstimate
-                    ? Math.Abs(
+                    ? CalculateActualLongitudinalSpeedMetersPerSecond(
-                        vehicleState.TwistInBody
+                        vehicleState)
-                            .VxMetersPerSecond)
                     : Math.Abs(
                         controlReferenceSpeedMetersPerSecond);
 
@@ -931,13 +939,16 @@ namespace MultiWheelC.Control.Execution
         }
 
         /// <summary>
-        /// 计算车体坐标系实际纵向速度的绝对值，单位为m/s。
+        /// 计算车辆沿当前运动坐标系X轴实际速度的绝对值，单位为m/s。
         /// </summary>
-        private static double CalculateActualLongitudinalSpeedMetersPerSecond(
+        private double CalculateActualLongitudinalSpeedMetersPerSecond(
             VehicleState vehicleState)
         {
             return Math.Abs(
-                vehicleState.TwistInBody.VxMetersPerSecond);
+                Math.Cos(_motionDirectionInBodyRadians) *
+                    vehicleState.TwistInBody.VxMetersPerSecond +
+                Math.Sin(_motionDirectionInBodyRadians) *
+                    vehicleState.TwistInBody.VyMetersPerSecond);
         }
 
         /// <summary>

MultiWheelC/Experiments/CompositeMotionPlanTests.cs

@@ -307,6 +307,8 @@ namespace MultiWheelC
                     out var detourVelocityValid,
                     out var rawWheelBodyVx,
                     out var filteredWheelBodyVx,
+                    out var rawWheelBodyVy,
+                    out var filteredWheelBodyVy,
                     out var wheelVelocityValid))
             {
                 _recorder?.UpdateVelocityDiagnostics(
@@ -314,6 +316,8 @@ namespace MultiWheelC
                     detourVelocityValid,
                     rawWheelBodyVx,
                     filteredWheelBodyVx,
+                    rawWheelBodyVy,
+                    filteredWheelBodyVy,
                     wheelVelocityValid);
             }
 

MultiWheelC/Experiments/NewControllerTrackingTests.cs

@@ -137,6 +137,18 @@ namespace MultiWheelC
         protected virtual string ExperimentTrajectoryBaseName =>
             "ProfiledStraight4m";
 
+        /// <summary>
+        /// 获取直线主运动方向相对车头的夹角，单位为rad。
+        /// </summary>
+        protected virtual double MotionDirectionInBodyRadians =>
+            0.0;
+
+        /// <summary>
+        /// 获取轨迹完成后是否需要将舵轮主动恢复到车头方向。
+        /// </summary>
+        protected virtual bool ReturnWheelsForwardAfterCompletion =>
+            false;
+
         /// <summary>
         /// 读取当前位姿、绘制离散轨迹并启动新版轨迹跟踪动作。
         /// </summary>
@@ -190,7 +202,8 @@ namespace MultiWheelC
                     CruiseSpeedMetersPerSecond,
                     AccelerationMetersPerSecondSquared,
                     DecelerationMetersPerSecondSquared,
-                    PointSpacingMeters);
+                    PointSpacingMeters,
+                    MotionDirectionInBodyRadians);
 
             DrawTrajectory(trajectory);
 
@@ -211,6 +224,9 @@ namespace MultiWheelC
                     referenceSpeed:
                         (float)CruiseSpeedMetersPerSecond,
                     sampleIntervalMs: 50,
+                    referenceMotionFrameYawDegrees:
+                        (float)AngleMath.RadiansToDegrees(
+                            MotionDirectionInBodyRadians),
                     referenceAccelerationMetersPerSecondSquared:
                         (float)AccelerationMetersPerSecondSquared,
                     referenceDecelerationMetersPerSecondSquared:
@@ -226,6 +242,10 @@ namespace MultiWheelC
                 {
                     Trajectory = trajectory,
                     StateProvider = _stateProvider,
+                    MotionDirectionInBodyRadians =
+                        MotionDirectionInBodyRadians,
+                    ReturnWheelsForwardAfterCompletion =
+                        ReturnWheelsForwardAfterCompletion,
                     CycleObserver = controller =>
                         RecordControlCycle(
                             recorder,
@@ -415,6 +435,8 @@ namespace MultiWheelC
                     out var detourVelocityValid,
                     out var rawWheelBodyVx,
                     out var filteredWheelBodyVx,
+                    out var rawWheelBodyVy,
+                    out var filteredWheelBodyVy,
                     out var wheelVelocityValid))
             {
                 return;
@@ -425,6 +447,8 @@ namespace MultiWheelC
                 detourVelocityValid,
                 rawWheelBodyVx,
                 filteredWheelBodyVx,
+                rawWheelBodyVy,
+                filteredWheelBodyVy,
                 wheelVelocityValid);
         }
 
@@ -467,6 +491,32 @@ namespace MultiWheelC
             "ProfiledReverseStraight4m";
     }
 
+    /// <summary>
+    /// 将舵轮准备到车体左前45°，以0.4m/s跟踪4m直线，停车后再恢复车头方向。
+    /// </summary>
+    [MovementTest(name = "新版控制器：45°蟹行4m直线轨迹跟踪")]
+    public sealed class NewControllerCrab45Straight4mTest
+        : NewControllerStraight4mTest
+    {
+        /// <summary>
+        /// 使用车体左前45°作为本次直线轨迹的固定运动方向。
+        /// </summary>
+        protected override double MotionDirectionInBodyRadians =>
+            Math.PI / 4.0;
+
+        /// <summary>
+        /// 蟹行轨迹正常完成后主动将四个舵轮恢复到车头方向。
+        /// </summary>
+        protected override bool ReturnWheelsForwardAfterCompletion =>
+            true;
+
+        /// <summary>
+        /// 将45°蟹行实验与普通前进和倒车实验的CSV名称明确区分。
+        /// </summary>
+        protected override string ExperimentTrajectoryBaseName =>
+            "ProfiledCrab45Straight4m";
+    }
+
     /// <summary>
     /// 从当前Detour位姿开始执行“3m直线—左半圆—3m直线”新版控制器跟踪实验。
     /// </summary>
@@ -812,6 +862,8 @@ namespace MultiWheelC
                     out var detourVelocityValid,
                     out var rawWheelBodyVx,
                     out var filteredWheelBodyVx,
+                    out var rawWheelBodyVy,
+                    out var filteredWheelBodyVy,
                     out var wheelVelocityValid))
             {
                 return;
@@ -822,6 +874,8 @@ namespace MultiWheelC
                 detourVelocityValid,
                 rawWheelBodyVx,
                 filteredWheelBodyVx,
+                rawWheelBodyVy,
+                filteredWheelBodyVy,
                 wheelVelocityValid);
         }
 

MultiWheelC/Experiments/TestTrajectoryFactory.cs

@@ -20,7 +20,8 @@ namespace MultiWheelC
             double cruiseSpeedMetersPerSecond = 0.30,
             double accelerationMetersPerSecondSquared = 0.20,
             double decelerationMetersPerSecondSquared = 0.20,
-            double pointSpacingMeters = 0.02)
+            double pointSpacingMeters = 0.02,
+            double motionDirectionInBodyRadians = 0.0)
         {
             return CreateStraight(
                 startPoseInWorld,
@@ -28,7 +29,8 @@ namespace MultiWheelC
                 cruiseSpeedMetersPerSecond,
                 accelerationMetersPerSecondSquared,
                 decelerationMetersPerSecondSquared,
-                pointSpacingMeters);
+                pointSpacingMeters,
+                motionDirectionInBodyRadians);
         }
 
         /// <summary>
@@ -40,7 +42,8 @@ namespace MultiWheelC
             double cruiseSpeedMetersPerSecond = 0.30,
             double accelerationMetersPerSecondSquared = 0.20,
             double decelerationMetersPerSecondSquared = 0.20,
-            double pointSpacingMeters = 0.02)
+            double pointSpacingMeters = 0.02,
+            double motionDirectionInBodyRadians = 0.0)
         {
             NumericGuard.EnsureFinite(
                 startPoseInWorld,
@@ -60,6 +63,9 @@ namespace MultiWheelC
             NumericGuard.EnsureFinitePositive(
                 pointSpacingMeters,
                 nameof(pointSpacingMeters));
+            NumericGuard.EnsureFinite(
+                motionDirectionInBodyRadians,
+                nameof(motionDirectionInBodyRadians));
 
             if (pointSpacingMeters > lengthMeters)
             {
@@ -73,10 +79,13 @@ namespace MultiWheelC
                 pointSpacingMeters);
             var points = new List<TrajectoryPoint>(
                 segmentCount + 1);
+            var worldMotionYawRadians =
+                startPoseInWorld.YawRadians +
+                motionDirectionInBodyRadians;
             var directionX = travelDirection *
-                Math.Cos(startPoseInWorld.YawRadians);
+                Math.Cos(worldMotionYawRadians);
             var directionY = travelDirection *
-                Math.Sin(startPoseInWorld.YawRadians);
+                Math.Sin(worldMotionYawRadians);
 
             for (var index = 0;
                  index <= segmentCount;

MultiWheelC/Experiments/TrackingExperimentRecorder.cs

@@ -53,12 +53,14 @@ namespace MultiWheelC
         public double CurvaturePreviewDistanceMeters;
         public double FeedforwardCurvaturePerMeter;
 
-        // 并列保存Detour速度与轮速解算速度，避免StateBodyVx的数据来源产生歧义。
+        // 并列保存Detour速度与轮速解算速度，避免StateBodyVx/Vy的数据来源产生歧义。
         public bool HasVelocityDiagnostics;
         public double DetourEstimatedBodyVxMetersPerSecond;
         public bool DetourVelocityEstimateValid;
         public double WheelFeedbackRawBodyVxMetersPerSecond;
         public double WheelFeedbackFilteredBodyVxMetersPerSecond;
+        public double WheelFeedbackRawBodyVyMetersPerSecond;
+        public double WheelFeedbackFilteredBodyVyMetersPerSecond;
         public bool WheelFeedbackVelocityEstimateValid;
 
         // 四舵轮机械角使用deg，前后虚拟GCP命令角使用rad。
@@ -176,6 +178,8 @@ namespace MultiWheelC
         private bool _detourVelocityEstimateValid;
         private double _wheelFeedbackRawBodyVxMetersPerSecond;
         private double _wheelFeedbackFilteredBodyVxMetersPerSecond;
+        private double _wheelFeedbackRawBodyVyMetersPerSecond;
+        private double _wheelFeedbackFilteredBodyVyMetersPerSecond;
         private bool _wheelFeedbackVelocityEstimateValid;
         private bool _hasGcpCommand;
         private double _requestedFrontGcpAngleRadians;
@@ -340,13 +344,15 @@ namespace MultiWheelC
         }
 
         /// <summary>
-        /// 保存同一控制周期的Detour纵向速度以及轮速解算的原始和滤波纵向速度。
+        /// 保存同一控制周期的Detour纵向速度以及轮速解算的原始和滤波平面速度。
         /// </summary>
         public void UpdateVelocityDiagnostics(
             double detourEstimatedBodyVxMetersPerSecond,
             bool detourVelocityEstimateValid,
             double wheelFeedbackRawBodyVxMetersPerSecond,
             double wheelFeedbackFilteredBodyVxMetersPerSecond,
+            double wheelFeedbackRawBodyVyMetersPerSecond,
+            double wheelFeedbackFilteredBodyVyMetersPerSecond,
             bool wheelFeedbackVelocityEstimateValid)
         {
             lock (_stateSyncRoot)
@@ -359,6 +365,10 @@ namespace MultiWheelC
                     wheelFeedbackRawBodyVxMetersPerSecond;
                 _wheelFeedbackFilteredBodyVxMetersPerSecond =
                     wheelFeedbackFilteredBodyVxMetersPerSecond;
+                _wheelFeedbackRawBodyVyMetersPerSecond =
+                    wheelFeedbackRawBodyVyMetersPerSecond;
+                _wheelFeedbackFilteredBodyVyMetersPerSecond =
+                    wheelFeedbackFilteredBodyVyMetersPerSecond;
                 _wheelFeedbackVelocityEstimateValid =
                     wheelFeedbackVelocityEstimateValid;
                 _hasVelocityDiagnostics = true;
@@ -543,6 +553,8 @@ namespace MultiWheelC
                 bool detourVelocityEstimateValid;
                 double wheelFeedbackRawBodyVxMetersPerSecond;
                 double wheelFeedbackFilteredBodyVxMetersPerSecond;
+                double wheelFeedbackRawBodyVyMetersPerSecond;
+                double wheelFeedbackFilteredBodyVyMetersPerSecond;
                 bool wheelFeedbackVelocityEstimateValid;
                 bool hasGcpCommand;
                 double requestedFrontGcpAngleRadians;
@@ -618,6 +630,10 @@ namespace MultiWheelC
                         _wheelFeedbackRawBodyVxMetersPerSecond;
                     wheelFeedbackFilteredBodyVxMetersPerSecond =
                         _wheelFeedbackFilteredBodyVxMetersPerSecond;
+                    wheelFeedbackRawBodyVyMetersPerSecond =
+                        _wheelFeedbackRawBodyVyMetersPerSecond;
+                    wheelFeedbackFilteredBodyVyMetersPerSecond =
+                        _wheelFeedbackFilteredBodyVyMetersPerSecond;
                     wheelFeedbackVelocityEstimateValid =
                         _wheelFeedbackVelocityEstimateValid;
                     hasGcpCommand = _hasGcpCommand;
@@ -681,6 +697,10 @@ namespace MultiWheelC
                         wheelFeedbackRawBodyVxMetersPerSecond,
                     WheelFeedbackFilteredBodyVxMetersPerSecond =
                         wheelFeedbackFilteredBodyVxMetersPerSecond,
+                    WheelFeedbackRawBodyVyMetersPerSecond =
+                        wheelFeedbackRawBodyVyMetersPerSecond,
+                    WheelFeedbackFilteredBodyVyMetersPerSecond =
+                        wheelFeedbackFilteredBodyVyMetersPerSecond,
                     WheelFeedbackVelocityEstimateValid =
                         wheelFeedbackVelocityEstimateValid,
                     HasGcpCommand = hasGcpCommand,
@@ -905,6 +925,8 @@ namespace MultiWheelC
                     "DetourVelocityEstimateValid," +
                     "WheelFeedbackRawBodyVxMetersPerSecond," +
                     "WheelFeedbackFilteredBodyVxMetersPerSecond," +
+                    "WheelFeedbackRawBodyVyMetersPerSecond," +
+                    "WheelFeedbackFilteredBodyVyMetersPerSecond," +
                     "WheelFeedbackVelocityEstimateValid," +
                     "HasSteeringDiagnostics," +
                     "TargetSteerLeftFrontDegrees," +
@@ -1034,6 +1056,12 @@ namespace MultiWheelC
                         FormatOptional(
                             sample.HasVelocityDiagnostics,
                             sample.WheelFeedbackFilteredBodyVxMetersPerSecond),
+                        FormatOptional(
+                            sample.HasVelocityDiagnostics,
+                            sample.WheelFeedbackRawBodyVyMetersPerSecond),
+                        FormatOptional(
+                            sample.HasVelocityDiagnostics,
+                            sample.WheelFeedbackFilteredBodyVyMetersPerSecond),
                         sample.HasVelocityDiagnostics
                             ? sample.WheelFeedbackVelocityEstimateValid
                                 ? "1"

MultiWheelC/Movements/PrepareWheelsForward.cs

@@ -14,6 +14,11 @@ namespace MultiWheelC
     /// </summary>
     public class PrepareWheelsForward : MovementDefinition
     {
+        /// <summary>
+        /// 获取或设置舵轮需要对准的车体方向，单位为rad；0表示车头方向。
+        /// </summary>
+        public double DirectionRadians;
+
         /// <summary>
         /// 获取或设置本次动作的回正到位容差覆盖值，单位为deg；为空时读取车辆配置。
         /// </summary>
@@ -36,6 +41,10 @@ namespace MultiWheelC
         /// </summary>
         public override IEnumerable<bool> Get()
         {
+            NumericGuard.EnsureFinite(
+                DirectionRadians,
+                nameof(DirectionRadians));
+
             var config = PilotDefinition.Conf;
             var toleranceDegrees =
                 ToleranceDegrees ??
@@ -75,10 +84,11 @@ namespace MultiWheelC
             DateTime? alignedSince = null;
 
             Completed = false;
-            if (!adapter.PrepareParallelDirection(0.0))
+            if (!adapter.PrepareParallelDirection(
+                    DirectionRadians))
             {
                 throw new InvalidOperationException(
-                    "无法将所有舵轮下发到车体前向0°。");
+                    "无法将所有舵轮下发到指定运动方向。");
             }
 
             try
@@ -87,7 +97,7 @@ namespace MultiWheelC
                 {
                     var aligned =
                         adapter.AreParallelWheelsAligned(
-                            0.0,
+                            DirectionRadians,
                             toleranceRadians);
 
                     if (aligned)
@@ -122,7 +132,7 @@ namespace MultiWheelC
             }
             finally
             {
-                // 只清零驱动速度，保留已经下发的0°舵角。
+                // 只清零驱动速度，保留已经下发的目标舵角。
                 adapter.StopImmediately();
             }
         }

MultiWheelC/Movements/RotateInPlace.cs

@@ -233,17 +233,14 @@ namespace MultiWheelC
                     var interval = now - lastCommandTime;
                     lastCommandTime = now;
 
-                    // PID输出s为deg/s，Shared命令统一使用rad/s。
+                    // PID输出s为deg/s，Shared统一使用车体坐标系Twist2D和rad/s。
-                    // adapter.Send最终调用普通安全版SendXYThSpeed。
                     var omegaRadiansPerSecond =
                         (float)AngleMath.DegreesToRadians(s);
-                    if (!adapter.Send(
+                    if (!adapter.SendBodyTwist(
-                            new ChassisCommand(
+                            new Twist2D(
-                                PilotDefinition.Self.CarNum,
+                                0.0,
-                                new Twist2D(
+                                0.0,
-                                    0.0,
+                                omegaRadiansPerSecond),
-                                    0.0,
-                                    omegaRadiansPerSecond)),
                             interval))
                     {
                         throw new InvalidOperationException(

MultiWheelC/Movements/TrajectoryTrackingMovement.cs

@@ -42,6 +42,16 @@ namespace MultiWheelC
         /// </summary>
         public Action<ParkingGeometricController> CycleObserver;
 
+        /// <summary>
+        /// 获取或设置本动作运动坐标系X轴在车体系中的方向，单位为rad；0表示车头方向。
+        /// </summary>
+        public double MotionDirectionInBodyRadians;
+
+        /// <summary>
+        /// 获取或设置轨迹正常完成后是否停车并将舵轮主动恢复到车头方向。
+        /// </summary>
+        public bool ReturnWheelsForwardAfterCompletion;
+
         /// <summary>
         /// 获取或设置本次动作的Stanley横向误差增益覆盖值，单位为1/s；为空时读取车辆配置。
         /// </summary>
@@ -270,7 +280,11 @@ namespace MultiWheelC
             }
 
             var wheelPreparation =
-                new PrepareWheelsForward();
+                new PrepareWheelsForward
+                {
+                    DirectionRadians =
+                        MotionDirectionInBodyRadians
+                };
             foreach (var keepRunning in wheelPreparation.Get())
             {
                 if (!keepRunning)
@@ -284,15 +298,15 @@ namespace MultiWheelC
             if (!wheelPreparation.Completed)
             {
                 throw new InvalidOperationException(
-                    "轨迹跟踪开始前舵轮未能稳定回到车头方向。");
+                    "轨迹跟踪开始前舵轮未能稳定到达目标运动方向。");
             }
 
             var adapter = new MultiWheelChassisAdapter(
                 chassis,
                 PilotDefinition.Self.CarNum);
 
-            // 新版GCP控制统一以真实车头为车体X正方向，避免继承上一次蟹行偏置。
+            adapter.ActivateMotionFrame(
-            adapter.ResetToBodyFrame();
+                MotionDirectionInBodyRadians);
 
             var stateProvider =
                 StateProvider ??
@@ -333,7 +347,8 @@ namespace MultiWheelC
             var commandExecutor =
                 new GcpCommandExecutor(
                     adapter,
-                    maximumGcpAngleRateRadiansPerSecond);
+                    maximumGcpAngleRateRadiansPerSecond,
+                    MotionDirectionInBodyRadians);
 
             Controller = new ParkingGeometricController(
                 stateProvider,
@@ -350,7 +365,8 @@ namespace MultiWheelC
                 terminalApproachGainPerSecond,
                 maximumTerminalApproachSpeedMetersPerSecond,
                 stanleyCurvaturePreviewSeconds,
-                stanleyMaximumCurvaturePreviewMeters);
+                stanleyMaximumCurvaturePreviewMeters,
+                MotionDirectionInBodyRadians);
 
             var clock = Stopwatch.StartNew();
             var previousCycleSeconds =
@@ -425,6 +441,28 @@ namespace MultiWheelC
                 Controller.Cancel();
             }
 
+            if (ReturnWheelsForwardAfterCompletion)
+            {
+                var forwardPreparation =
+                    new PrepareWheelsForward();
+                foreach (var keepRunning in
+                         forwardPreparation.Get())
+                {
+                    if (!keepRunning)
+                    {
+                        break;
+                    }
+
+                    yield return true;
+                }
+
+                if (!forwardPreparation.Completed)
+                {
+                    throw new InvalidOperationException(
+                        "轨迹完成后舵轮未能稳定回到车头方向。");
+                }
+            }
+
             yield return false;
         }
 
@@ -440,6 +478,10 @@ namespace MultiWheelC
                     "新版轨迹跟踪动作没有设置Trajectory。");
             }
 
+            NumericGuard.EnsureFinite(
+                MotionDirectionInBodyRadians,
+                nameof(MotionDirectionInBodyRadians));
+
             if (double.IsNaN(executionTimeoutSeconds) ||
                 double.IsInfinity(executionTimeoutSeconds) ||
                 executionTimeoutSeconds <= 0.0)

MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs

@@ -80,25 +80,25 @@ namespace MultiWheelC.StateEstimation
                 throw new ArgumentNullException(
                     nameof(velocityEstimator));
 
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 maximumLinearSpeedMetersPerSecond,
                 nameof(maximumLinearSpeedMetersPerSecond));
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 maximumAngularSpeedRadiansPerSecond,
                 nameof(maximumAngularSpeedRadiansPerSecond));
-            EnsureFiniteNonNegative(
+            NumericGuard.EnsureFiniteNonNegative(
                 positionJumpMarginMeters,
                 nameof(positionJumpMarginMeters));
-            EnsureFiniteNonNegative(
+            NumericGuard.EnsureFiniteNonNegative(
                 headingJumpMarginRadians,
                 nameof(headingJumpMarginRadians));
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 velocityPositionResidualMeters,
                 nameof(velocityPositionResidualMeters));
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 velocityHeadingResidualRadians,
                 nameof(velocityHeadingResidualRadians));
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 stationaryConfirmationSeconds,
                 nameof(stationaryConfirmationSeconds));
 
@@ -238,9 +238,9 @@ namespace MultiWheelC.StateEstimation
             var location =
                 DetourInterface.getCartLocation();
 
-            EnsureFinite(location.x, "DetourX");
+            NumericGuard.EnsureFinite(location.x, "DetourX");
-            EnsureFinite(location.y, "DetourY");
+            NumericGuard.EnsureFinite(location.y, "DetourY");
-            EnsureFinite(location.th, "DetourTheta");
+            NumericGuard.EnsureFinite(location.th, "DetourTheta");
 
             return new Pose2D(
                 location.x / MillimetersPerMeter,
@@ -268,25 +268,6 @@ namespace MultiWheelC.StateEstimation
             return _latestState;
         }
 
-        /// <summary>
-        /// 接受跳变后的新位姿基准，但不让该位移进入速度差分和低通滤波器。
-        /// </summary>
-        private VehicleState AcceptPoseAfterVelocityRebase(
-            Pose2D poseInWorld,
-            double timestampSeconds)
-        {
-            _latestState =
-                _velocityEstimator
-                    .RebasePreservingVelocity(
-                        poseInWorld,
-                        timestampSeconds);
-            _acceptedPoseInWorld = poseInWorld;
-            _acceptedTimestampSeconds =
-                timestampSeconds;
-            _stationaryHoldActive = false;
-            return _latestState;
-        }
-
         /// <summary>
         /// 接受首帧或静止后的首个新位姿并重新建立零速差分基准。
         /// </summary>
@@ -340,7 +321,7 @@ namespace MultiWheelC.StateEstimation
             Pose2D endPoseInWorld,
             double deltaTimeSeconds)
         {
-            if (!IsFinite(deltaTimeSeconds) ||
+            if (!NumericGuard.IsFinite(deltaTimeSeconds) ||
                 deltaTimeSeconds <= 0.0)
             {
                 return false;
@@ -448,62 +429,5 @@ namespace MultiWheelC.StateEstimation
                    HeadingEqualityToleranceRadians;
         }
 
-        /// <summary>
-        /// 检查数值是否为正有限值。
-        /// </summary>
-        private static void EnsureFinitePositive(
-            double value,
-            string parameterName)
-        {
-            EnsureFinite(value, parameterName);
-
-            if (value <= 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "状态源参数必须是正有限值。");
-            }
-        }
-
-        /// <summary>
-        /// 检查数值是否为非负有限值。
-        /// </summary>
-        private static void EnsureFiniteNonNegative(
-            double value,
-            string parameterName)
-        {
-            EnsureFinite(value, parameterName);
-
-            if (value < 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "状态源参数必须是非负有限值。");
-            }
-        }
-
-        /// <summary>
-        /// 检查数值是否为有限值。
-        /// </summary>
-        private static void EnsureFinite(
-            double value,
-            string parameterName)
-        {
-            if (!IsFinite(value))
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "状态源参数和Detour位姿必须是有限值。");
-            }
-        }
-
-        /// <summary>
-        /// 判断数值是否可用于状态估计。
-        /// </summary>
-        private static bool IsFinite(double value)
-        {
-            return !double.IsNaN(value) &&
-                   !double.IsInfinity(value);
-        }
     }
 }

MultiWheelC/StateEstimation/FirstOrderLowPassFilter.cs

@@ -1,4 +1,4 @@
-using System;
+using MyParking.Shared;
 
 namespace MultiWheelC.StateEstimation
 {
@@ -17,7 +17,7 @@ namespace MultiWheelC.StateEstimation
         public FirstOrderLowPassFilter(
             double timeConstantSeconds)
         {
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 timeConstantSeconds,
                 nameof(timeConstantSeconds));
 
@@ -25,35 +25,6 @@ namespace MultiWheelC.StateEstimation
                 timeConstantSeconds;
         }
 
-        /// <summary>
-        /// 获取滤波时间常数，单位为s；数值越大，滤波越强但响应越慢。
-        /// </summary>
-        public double TimeConstantSeconds =>
-            _timeConstantSeconds;
-
-        /// <summary>
-        /// 获取滤波器是否已经接收过有效初值。
-        /// </summary>
-        public bool IsInitialized =>
-            _isInitialized;
-
-        /// <summary>
-        /// 获取当前滤波输出；尚未初始化时读取会抛出异常。
-        /// </summary>
-        public double Value
-        {
-            get
-            {
-                if (!_isInitialized)
-                {
-                    throw new InvalidOperationException(
-                        "一阶低通滤波器尚未初始化。");
-                }
-
-                return _value;
-            }
-        }
-
         /// <summary>
         /// 使用当前输入和真实采样间隔更新滤波结果。
         /// </summary>
@@ -61,10 +32,10 @@ namespace MultiWheelC.StateEstimation
             double input,
             double deltaTimeSeconds)
         {
-            EnsureFinite(
+            NumericGuard.EnsureFinite(
                 input,
                 nameof(input));
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 deltaTimeSeconds,
                 nameof(deltaTimeSeconds));
 
@@ -98,7 +69,7 @@ namespace MultiWheelC.StateEstimation
         /// </summary>
         public void Reset(double initialValue)
         {
-            EnsureFinite(
+            NumericGuard.EnsureFinite(
                 initialValue,
                 nameof(initialValue));
 
@@ -106,37 +77,5 @@ namespace MultiWheelC.StateEstimation
             _isInitialized = true;
         }
 
-        /// <summary>
-        /// 检查数值是否为正有限值。
-        /// </summary>
-        private static void EnsureFinitePositive(
-            double value,
-            string parameterName)
-        {
-            EnsureFinite(value, parameterName);
-
-            if (value <= 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "滤波时间常数和采样间隔必须是正有限值。");
-            }
-        }
-
-        /// <summary>
-        /// 检查数值是否为有限值。
-        /// </summary>
-        private static void EnsureFinite(
-            double value,
-            string parameterName)
-        {
-            if (double.IsNaN(value) ||
-                double.IsInfinity(value))
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "滤波输入必须是有限值。");
-            }
-        }
     }
 }

MultiWheelC/StateEstimation/ParkingVehicleStateProviderFactory.cs

@@ -5,7 +5,7 @@ using MyParking.Shared;
 namespace MultiWheelC.StateEstimation
 {
     /// <summary>
-    /// 根据车载配置创建Detour位姿过滤与电机反馈纵向速度组合的停车状态源。
+    /// 根据车载配置创建Detour位姿过滤与电机反馈平面速度组合的停车状态源。
     /// </summary>
     public static class ParkingVehicleStateProviderFactory
     {

MultiWheelC/StateEstimation/VehicleState.cs

@@ -1,4 +1,3 @@
-using System;
 using MyParking.Shared;
 
 namespace MultiWheelC.StateEstimation
@@ -17,13 +16,13 @@ namespace MultiWheelC.StateEstimation
             Twist2D twistInWorld,
             bool hasValidVelocityEstimate)
         {
-            EnsureFiniteNonNegative(
+            NumericGuard.EnsureFiniteNonNegative(
                 sampleTimestampSeconds,
                 nameof(sampleTimestampSeconds));
-            EnsureFinitePose(
+            NumericGuard.EnsureFinite(
                 poseInWorld,
                 nameof(poseInWorld));
-            EnsureFiniteTwist(
+            NumericGuard.EnsureFinite(
                 twistInWorld,
                 nameof(twistInWorld));
 
@@ -73,66 +72,9 @@ namespace MultiWheelC.StateEstimation
         public Twist2D TwistInBody { get; }
 
         /// <summary>
-        /// 获取当前速度是否已由至少两个连续有效定位样本估算得到。
+        /// 获取当前速度估计是否已经初始化并可用于闭环控制。
         /// </summary>
         public bool HasValidVelocityEstimate { get; }
 
-        /// <summary>
-        /// 检查位姿是否由有限数值组成。
-        /// </summary>
-        private static void EnsureFinitePose(
-            Pose2D pose,
-            string parameterName)
-        {
-            if (!IsFinite(pose.XMeters) ||
-                !IsFinite(pose.YMeters) ||
-                !IsFinite(pose.YawRadians))
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "车辆位姿必须由有限数值组成。");
-            }
-        }
-
-        /// <summary>
-        /// 检查速度是否由有限数值组成。
-        /// </summary>
-        private static void EnsureFiniteTwist(
-            Twist2D twist,
-            string parameterName)
-        {
-            if (!IsFinite(twist.VxMetersPerSecond) ||
-                !IsFinite(twist.VyMetersPerSecond) ||
-                !IsFinite(twist.OmegaRadiansPerSecond))
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "车辆速度必须由有限数值组成。");
-            }
-        }
-
-        /// <summary>
-        /// 检查数值是否为非负有限值。
-        /// </summary>
-        private static void EnsureFiniteNonNegative(
-            double value,
-            string parameterName)
-        {
-            if (!IsFinite(value) || value < 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "采样时刻必须是非负有限值。");
-            }
-        }
-
-        /// <summary>
-        /// 判断数值是否可用于车辆状态计算。
-        /// </summary>
-        private static bool IsFinite(double value)
-        {
-            return !double.IsNaN(value) &&
-                   !double.IsInfinity(value);
-        }
     }
 }

MultiWheelC/StateEstimation/VelocityEstimator2D.cs

@@ -52,12 +52,6 @@ namespace MultiWheelC.StateEstimation
                     angularFilterTimeConstantSeconds);
         }
 
-        /// <summary>
-        /// 获取是否已经保存了可用于下一次差分的位姿基准。
-        /// </summary>
-        public bool HasPreviousSample =>
-            _hasPreviousSample;
-
         /// <summary>
         /// 使用一个新的有效定位样本更新并返回车辆状态。
         /// </summary>
@@ -65,10 +59,10 @@ namespace MultiWheelC.StateEstimation
             Pose2D poseInWorld,
             double sampleTimestampSeconds)
         {
-            EnsureFinitePose(
+            NumericGuard.EnsureFinite(
                 poseInWorld,
                 nameof(poseInWorld));
-            EnsureFiniteNonNegative(
+            NumericGuard.EnsureFiniteNonNegative(
                 sampleTimestampSeconds,
                 nameof(sampleTimestampSeconds));
 
@@ -135,53 +129,6 @@ namespace MultiWheelC.StateEstimation
                 true);
         }
 
-        /// <summary>
-        /// 更新位姿差分基准但保留当前滤波速度，避免定位跳变形成虚假速度尖峰。
-        /// </summary>
-        public VehicleState RebasePreservingVelocity(
-            Pose2D poseInWorld,
-            double sampleTimestampSeconds)
-        {
-            EnsureFinitePose(
-                poseInWorld,
-                nameof(poseInWorld));
-            EnsureFiniteNonNegative(
-                sampleTimestampSeconds,
-                nameof(sampleTimestampSeconds));
-
-            var normalizedPoseInWorld =
-                new Pose2D(
-                    poseInWorld.XMeters,
-                    poseInWorld.YMeters,
-                    AngleMath.NormalizeRadians(
-                        poseInWorld.YawRadians));
-
-            _previousPoseInWorld =
-                normalizedPoseInWorld;
-            _previousTimestampSeconds =
-                sampleTimestampSeconds;
-            _hasPreviousSample = true;
-
-            var hasValidVelocityEstimate =
-                _worldVelocityXFilter.IsInitialized &&
-                _worldVelocityYFilter.IsInitialized &&
-                _angularVelocityFilter.IsInitialized;
-
-            var retainedTwistInWorld =
-                hasValidVelocityEstimate
-                    ? new Twist2D(
-                        _worldVelocityXFilter.Value,
-                        _worldVelocityYFilter.Value,
-                        _angularVelocityFilter.Value)
-                    : Twist2D.Zero;
-
-            return new VehicleState(
-                sampleTimestampSeconds,
-                normalizedPoseInWorld,
-                retainedTwistInWorld,
-                hasValidVelocityEstimate);
-        }
-
         /// <summary>
         /// 使用当前定位重新建立差分基准，并返回速度无效的零速状态。
         /// </summary>
@@ -189,10 +136,10 @@ namespace MultiWheelC.StateEstimation
             Pose2D poseInWorld,
             double sampleTimestampSeconds)
         {
-            EnsureFinitePose(
+            NumericGuard.EnsureFinite(
                 poseInWorld,
                 nameof(poseInWorld));
-            EnsureFiniteNonNegative(
+            NumericGuard.EnsureFiniteNonNegative(
                 sampleTimestampSeconds,
                 nameof(sampleTimestampSeconds));
 
@@ -231,45 +178,5 @@ namespace MultiWheelC.StateEstimation
             _angularVelocityFilter.Reset();
         }
 
-        /// <summary>
-        /// 检查位姿是否由有限数值组成。
-        /// </summary>
-        private static void EnsureFinitePose(
-            Pose2D pose,
-            string parameterName)
-        {
-            if (!IsFinite(pose.XMeters) ||
-                !IsFinite(pose.YMeters) ||
-                !IsFinite(pose.YawRadians))
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "速度估计使用的车辆位姿必须由有限数值组成。");
-            }
-        }
-
-        /// <summary>
-        /// 检查数值是否为非负有限值。
-        /// </summary>
-        private static void EnsureFiniteNonNegative(
-            double value,
-            string parameterName)
-        {
-            if (!IsFinite(value) || value < 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "速度估计使用的采样时刻必须是非负有限值。");
-            }
-        }
-
-        /// <summary>
-        /// 判断数值是否可用于速度估计。
-        /// </summary>
-        private static bool IsFinite(double value)
-        {
-            return !double.IsNaN(value) &&
-                   !double.IsInfinity(value);
-        }
     }
 }

MultiWheelC/StateEstimation/WheelFeedbackVehicleStateProvider.cs

@@ -6,7 +6,7 @@ using System.Diagnostics;
 namespace MultiWheelC.StateEstimation
 {
     /// <summary>
-    /// 保留外部状态源的Detour位姿，并以舵轮电机反馈解算的车体纵向速度替换Detour差分纵向速度。
+    /// 保留外部状态源的Detour位姿，并以舵轮电机反馈解算的车体平面速度替换Detour差分线速度。
     /// </summary>
     public sealed class WheelFeedbackVehicleStateProvider
         : IVehicleStateProvider
@@ -19,6 +19,7 @@ namespace MultiWheelC.StateEstimation
         private readonly IVehicleStateProvider _poseProvider;
         private readonly MultiWheelChassis _chassis;
         private readonly FirstOrderLowPassFilter _longitudinalSpeedFilter;
+        private readonly FirstOrderLowPassFilter _lateralSpeedFilter;
 
         private bool _hasPreviousTimestamp;
         private double _previousTimestampSeconds;
@@ -27,10 +28,12 @@ namespace MultiWheelC.StateEstimation
         private bool _latestDetourVelocityValid;
         private double _latestRawWheelBodyVxMetersPerSecond;
         private double _latestFilteredWheelBodyVxMetersPerSecond;
+        private double _latestRawWheelBodyVyMetersPerSecond;
+        private double _latestFilteredWheelBodyVyMetersPerSecond;
         private bool _latestWheelVelocityValid;
 
         /// <summary>
-        /// 创建使用默认0.10s低通时间常数的电机反馈纵向速度状态源。
+        /// 创建使用默认0.10s低通时间常数的电机反馈平面速度状态源。
         /// </summary>
         public WheelFeedbackVehicleStateProvider(
             IVehicleStateProvider poseProvider,
@@ -43,7 +46,7 @@ namespace MultiWheelC.StateEstimation
         }
 
         /// <summary>
-        /// 创建使用指定低通时间常数的电机反馈纵向速度状态源。
+        /// 创建使用指定低通时间常数的电机反馈平面速度状态源。
         /// </summary>
         public WheelFeedbackVehicleStateProvider(
             IVehicleStateProvider poseProvider,
@@ -59,6 +62,9 @@ namespace MultiWheelC.StateEstimation
             _longitudinalSpeedFilter =
                 new FirstOrderLowPassFilter(
                     velocityFilterTimeConstantSeconds);
+            _lateralSpeedFilter =
+                new FirstOrderLowPassFilter(
+                    velocityFilterTimeConstantSeconds);
         }
 
         /// <summary>
@@ -87,40 +93,50 @@ namespace MultiWheelC.StateEstimation
                 {
                     var actualCarSpeed =
                         _chassis.GetCarSpeed(true);
-                    var rawLongitudinalSpeedMetersPerSecond =
+                    var rawBodyVxMetersPerSecond =
                         (double)actualCarSpeed.Vx;
+                    var rawBodyVyMetersPerSecond =
+                        (double)actualCarSpeed.Vy;
 
-                    EnsureFinite(
+                    NumericGuard.EnsureFinite(
-                        rawLongitudinalSpeedMetersPerSecond,
+                        rawBodyVxMetersPerSecond,
                         "电机反馈车体纵向速度");
+                    NumericGuard.EnsureFinite(
+                        rawBodyVyMetersPerSecond,
+                        "电机反馈车体横向速度");
 
                     var wheelSpeedTimestampSeconds =
                         _wheelSpeedClock.Elapsed.TotalSeconds;
 
-                    var filteredLongitudinalSpeedMetersPerSecond =
+                    UpdateBodyVelocityFilters(
-                        UpdateLongitudinalSpeedFilter(
+                        rawBodyVxMetersPerSecond,
-                            rawLongitudinalSpeedMetersPerSecond,
+                        rawBodyVyMetersPerSecond,
-                            wheelSpeedTimestampSeconds,
+                        wheelSpeedTimestampSeconds,
-                            out var hasValidWheelSpeedEstimate);
+                        out var filteredBodyVxMetersPerSecond,
+                        out var filteredBodyVyMetersPerSecond,
+                        out var hasValidWheelSpeedEstimate);
 
                     _latestDetourBodyVxMetersPerSecond =
                         poseState.TwistInBody.VxMetersPerSecond;
                     _latestDetourVelocityValid =
                         poseState.HasValidVelocityEstimate;
                     _latestRawWheelBodyVxMetersPerSecond =
-                        rawLongitudinalSpeedMetersPerSecond;
+                        rawBodyVxMetersPerSecond;
                     _latestFilteredWheelBodyVxMetersPerSecond =
-                        filteredLongitudinalSpeedMetersPerSecond;
+                        filteredBodyVxMetersPerSecond;
+                    _latestRawWheelBodyVyMetersPerSecond =
+                        rawBodyVyMetersPerSecond;
+                    _latestFilteredWheelBodyVyMetersPerSecond =
+                        filteredBodyVyMetersPerSecond;
                     _latestWheelVelocityValid =
                         hasValidWheelSpeedEstimate;
                     _hasVelocityDiagnostics = true;
 
-                    // 第一阶段只替换控制器使用的车体纵向速度；横向速度和角速度
+                    // 车体平面线速度来自四轮电机和舵角反馈；角速度继续使用Detour，
-                    // 继续使用Detour估计，避免轮速差和舵角误差放大Vy与Omega噪声。
+                    // 避免轮速差和舵角误差放大Omega噪声。
                     var twistInBody = new Twist2D(
-                        filteredLongitudinalSpeedMetersPerSecond,
+                        filteredBodyVxMetersPerSecond,
-                        poseState.TwistInBody
+                        filteredBodyVyMetersPerSecond,
-                            .VyMetersPerSecond,
                         poseState.TwistInBody
                             .OmegaRadiansPerSecond);
 
@@ -151,13 +167,15 @@ namespace MultiWheelC.StateEstimation
         }
 
         /// <summary>
-        /// 读取最近一帧Detour纵向速度和轮速解算纵向速度，供实验记录使用。
+        /// 读取最近一帧Detour纵向速度和轮速解算平面速度，供实验记录使用。
         /// </summary>
         public bool TryGetLatestVelocityDiagnostics(
             out double detourBodyVxMetersPerSecond,
             out bool detourVelocityValid,
             out double rawWheelBodyVxMetersPerSecond,
             out double filteredWheelBodyVxMetersPerSecond,
+            out double rawWheelBodyVyMetersPerSecond,
+            out double filteredWheelBodyVyMetersPerSecond,
             out bool wheelVelocityValid)
         {
             lock (_syncRoot)
@@ -170,6 +188,10 @@ namespace MultiWheelC.StateEstimation
                     _latestRawWheelBodyVxMetersPerSecond;
                 filteredWheelBodyVxMetersPerSecond =
                     _latestFilteredWheelBodyVxMetersPerSecond;
+                rawWheelBodyVyMetersPerSecond =
+                    _latestRawWheelBodyVyMetersPerSecond;
+                filteredWheelBodyVyMetersPerSecond =
+                    _latestFilteredWheelBodyVyMetersPerSecond;
                 wheelVelocityValid =
                     _latestWheelVelocityValid;
                 return _hasVelocityDiagnostics;
@@ -184,6 +206,7 @@ namespace MultiWheelC.StateEstimation
             lock (_syncRoot)
             {
                 _longitudinalSpeedFilter.Reset();
+                _lateralSpeedFilter.Reset();
                 _wheelSpeedClock.Restart();
                 _hasPreviousTimestamp = false;
                 _previousTimestampSeconds = 0.0;
@@ -192,31 +215,42 @@ namespace MultiWheelC.StateEstimation
                 _latestDetourVelocityValid = false;
                 _latestRawWheelBodyVxMetersPerSecond = 0.0;
                 _latestFilteredWheelBodyVxMetersPerSecond = 0.0;
+                _latestRawWheelBodyVyMetersPerSecond = 0.0;
+                _latestFilteredWheelBodyVyMetersPerSecond = 0.0;
                 _latestWheelVelocityValid = false;
                 LastFailureReason = string.Empty;
             }
         }
 
         /// <summary>
-        /// 使用真实状态时间间隔更新纵向速度低通滤波，并在首帧建立基准。
+        /// 使用同一个真实采样间隔更新车体Vx和Vy低通滤波，并在首帧建立共同时间基准。
         /// </summary>
-        private double UpdateLongitudinalSpeedFilter(
+        private void UpdateBodyVelocityFilters(
-            double rawLongitudinalSpeedMetersPerSecond,
+            double rawBodyVxMetersPerSecond,
+            double rawBodyVyMetersPerSecond,
             double timestampSeconds,
+            out double filteredBodyVxMetersPerSecond,
+            out double filteredBodyVyMetersPerSecond,
             out bool hasValidWheelSpeedEstimate)
         {
-            EnsureFiniteNonNegative(
+            NumericGuard.EnsureFiniteNonNegative(
                 timestampSeconds,
                 nameof(timestampSeconds));
 
             if (!_hasPreviousTimestamp)
             {
                 _longitudinalSpeedFilter.Reset(
-                    rawLongitudinalSpeedMetersPerSecond);
+                    rawBodyVxMetersPerSecond);
+                _lateralSpeedFilter.Reset(
+                    rawBodyVyMetersPerSecond);
                 _previousTimestampSeconds = timestampSeconds;
                 _hasPreviousTimestamp = true;
                 hasValidWheelSpeedEstimate = false;
-                return rawLongitudinalSpeedMetersPerSecond;
+                filteredBodyVxMetersPerSecond =
+                    rawBodyVxMetersPerSecond;
+                filteredBodyVyMetersPerSecond =
+                    rawBodyVyMetersPerSecond;
+                return;
             }
 
             var deltaTimeSeconds =
@@ -227,48 +261,27 @@ namespace MultiWheelC.StateEstimation
             if (deltaTimeSeconds <= 0.0)
             {
                 _longitudinalSpeedFilter.Reset(
-                    rawLongitudinalSpeedMetersPerSecond);
+                    rawBodyVxMetersPerSecond);
+                _lateralSpeedFilter.Reset(
+                    rawBodyVyMetersPerSecond);
                 hasValidWheelSpeedEstimate = false;
-                return rawLongitudinalSpeedMetersPerSecond;
+                filteredBodyVxMetersPerSecond =
+                    rawBodyVxMetersPerSecond;
+                filteredBodyVyMetersPerSecond =
+                    rawBodyVyMetersPerSecond;
+                return;
             }
 
             hasValidWheelSpeedEstimate = true;
-            return _longitudinalSpeedFilter.Update(
+            filteredBodyVxMetersPerSecond =
-                rawLongitudinalSpeedMetersPerSecond,
+                _longitudinalSpeedFilter.Update(
-                deltaTimeSeconds);
+                    rawBodyVxMetersPerSecond,
+                    deltaTimeSeconds);
+            filteredBodyVyMetersPerSecond =
+                _lateralSpeedFilter.Update(
+                    rawBodyVyMetersPerSecond,
+                    deltaTimeSeconds);
         }
 
-        /// <summary>
-        /// 检查采样时刻是否为非负有限值。
-        /// </summary>
-        private static void EnsureFiniteNonNegative(
-            double value,
-            string parameterName)
-        {
-            EnsureFinite(value, parameterName);
-
-            if (value < 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "采样时刻必须是非负有限值。");
-            }
-        }
-
-        /// <summary>
-        /// 检查状态输入是否为有限值。
-        /// </summary>
-        private static void EnsureFinite(
-            double value,
-            string parameterName)
-        {
-            if (double.IsNaN(value) ||
-                double.IsInfinity(value))
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "车辆状态输入必须是有限值。");
-            }
-        }
     }
 }

Shared/Chassis/MultiWheelChassisAdapter.cs

@@ -11,9 +11,10 @@ namespace MyParking.Shared
     public sealed class MultiWheelChassisAdapter
     {
         #region 辅助内容
-        private const double RadiansToDegrees = 180.0 / Math.PI;
         private const float BiasTolerance = 0.001f;
+        private const double MotionDeadband = 1e-6;
         private readonly MultiWheelChassis _chassis;
+        private double _activeMotionDirectionRadians;
         /// <summary>
         /// 当前适配器对应的车辆编号。
         /// </summary>
@@ -32,10 +33,22 @@ namespace MyParking.Shared
 
         /// <summary>
         /// 车体原点到最外侧舵轮中心的最大横向距离，单位为米。
-        /// 对称四舵轮底盘中，它也是蟹行虚拟阿克曼模型的半轴距。
+        /// 对称四舵轮底盘中，它通常等于物理轮距的一半。
         /// </summary>
         public double HalfTrackWidthMeters { get; }
 
+        /// <summary>
+        /// 获取旧版SendMotion使用的对称前后GCP半径，单位为m。
+        /// </summary>
+        public double ControlPointRadiusMeters =>
+            _chassis.ControlPointRadius / 1000.0;
+
+        /// <summary>
+        /// 获取当前已经准备并激活的滚动运动系X轴在真实车体系中的方向，单位为rad。
+        /// </summary>
+        public double ActiveMotionDirectionRadians =>
+            _activeMotionDirectionRadians;
+
         /// <summary>
         /// Width of the steering-alignment speed gate, in degrees.
         /// </summary>
@@ -44,15 +57,12 @@ namespace MyParking.Shared
             get => _chassis.SteeringAlignmentSigmaDegrees;
             set
             {
-                if (double.IsNaN(value) ||
+                NumericGuard.EnsureFinitePositive(
-                    double.IsInfinity(value) ||
+                    value,
-                    value <= 0.0 ||
+                    nameof(value));
-                    value > float.MaxValue)
+                EnsureRepresentableAsSingle(
-                {
+                    value,
-                    throw new ArgumentOutOfRangeException(
+                    nameof(value));
-                        nameof(value),
-                        "Steering alignment sigma must be a positive finite value.");
-                }
 
                 _chassis.SteeringAlignmentSigmaDegrees =
                     (float)value;
@@ -74,18 +84,18 @@ namespace MyParking.Shared
         private void EnsureMotionFrameIsActive(
             double motionDirectionRadians)
         {
-            ValidateFinite(
+            NumericGuard.EnsureFinite(
                 motionDirectionRadians,
                 nameof(motionDirectionRadians));
 
             var expectedBiasDegrees =
-                (float)(
+                ConvertRadiansToSingleDegrees(
-                    -FrameTransform2D.NormalizeAngle(
+                    -AngleMath.NormalizeRadians(
-                        motionDirectionRadians) *
+                        motionDirectionRadians),
-                    RadiansToDegrees);
+                    nameof(motionDirectionRadians));
             var bias = _chassis.GetOriginBias();
             var angleErrorDegrees =
-                NormalizeDegrees(
+                AngleMath.NormalizeDegrees(
                     bias.Z - expectedBiasDegrees);
 
             if (Math.Abs(bias.X) <= BiasTolerance &&
@@ -102,46 +112,32 @@ namespace MyParking.Shared
                 $"期望Th={expectedBiasDegrees}°。");
         }
 
-        /// <summary>
-        /// 将角度归一化到[-180°,180°]附近。
-        /// </summary>
-        private static float NormalizeDegrees(float degrees)
-        {
-            return (float)(
-                degrees -
-                Math.Round(degrees / 360.0) * 360.0);
-        }
         /// <summary>
         /// 检查底盘命令是否包含无效数值。
         /// </summary>
         private static void ValidateTwist(Twist2D twist)
         {
-            ValidateFinite(
+            EnsureRepresentableAsSingle(
                 twist.VxMetersPerSecond,
                 nameof(twist.VxMetersPerSecond));
 
-            ValidateFinite(
+            EnsureRepresentableAsSingle(
                 twist.VyMetersPerSecond,
                 nameof(twist.VyMetersPerSecond));
 
-            ValidateFinite(
+            EnsureRepresentableAsSingle(
-                twist.OmegaRadiansPerSecond,
+                AngleMath.RadiansToDegrees(
+                    twist.OmegaRadiansPerSecond),
                 nameof(twist.OmegaRadiansPerSecond));
         }
         /// <summary>
-        /// 检查数值是否为有限值。
+        /// 检查数值是否为有限值且可安全转换为float。
         /// </summary>
-        private static void ValidateFinite(
+        private static void EnsureRepresentableAsSingle(
             double value,
             string parameterName)
         {
-            if (double.IsNaN(value) ||
+            NumericGuard.EnsureFinite(value, parameterName);
-                double.IsInfinity(value))
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "底盘速度命令不能是NaN或无穷大。");
-            }
 
             if (value > float.MaxValue ||
                 value < -float.MaxValue)
@@ -152,6 +148,21 @@ namespace MyParking.Shared
             }
         }
 
+        /// <summary>
+        /// 将有限弧度值转换为float可表示的角度值。
+        /// </summary>
+        private static float ConvertRadiansToSingleDegrees(
+            double angleRadians,
+            string parameterName)
+        {
+            var angleDegrees =
+                AngleMath.RadiansToDegrees(angleRadians);
+            EnsureRepresentableAsSingle(
+                angleDegrees,
+                parameterName);
+            return (float)angleDegrees;
+        }
+
         /// <summary>
         /// 获取最近一次底盘运动分解失败原因。
         /// </summary>
@@ -171,19 +182,23 @@ namespace MyParking.Shared
         /// <summary>
         /// 激活指定运动方向对应的SendMotion坐标系。
         /// 0表示真实车头，正90度表示将车体左侧作为虚拟车头。
+        /// 调用方必须先停车，并确认舵轮已经按该方向完成预对齐。
         /// </summary>
         public void ActivateMotionFrame(
             double motionDirectionRadians)
         {
-            ValidateFinite(
+            NumericGuard.EnsureFinite(
                 motionDirectionRadians,
                 nameof(motionDirectionRadians));
 
+            var normalizedDirectionRadians =
+                AngleMath.NormalizeRadians(
+                    motionDirectionRadians);
+
             var biasDegrees =
-                (float)(
+                ConvertRadiansToSingleDegrees(
-                    -FrameTransform2D.NormalizeAngle(
+                    -normalizedDirectionRadians,
-                        motionDirectionRadians) *
+                    nameof(motionDirectionRadians));
-                    RadiansToDegrees);
             var currentBias =
                 _chassis.GetOriginBias();
 
@@ -192,11 +207,13 @@ namespace MyParking.Shared
                 Math.Abs(currentBias.Y) <=
                     BiasTolerance &&
                 Math.Abs(
-                    NormalizeDegrees(
+                    AngleMath.NormalizeDegrees(
                         currentBias.Z -
                         biasDegrees)) <=
                     BiasTolerance)
             {
+                SetActiveMotionDirection(
+                    normalizedDirectionRadians);
                 return;
             }
 
@@ -204,6 +221,19 @@ namespace MyParking.Shared
                 x: 0.0f,
                 y: 0.0f,
                 th: biasDegrees);
+            SetActiveMotionDirection(
+                normalizedDirectionRadians);
+        }
+
+        /// <summary>
+        /// 缓存当前运动坐标系方向，供控制周期内转换车体速度。
+        /// </summary>
+        private void SetActiveMotionDirection(
+            double motionDirectionRadians)
+        {
+            _activeMotionDirectionRadians =
+                AngleMath.NormalizeRadians(
+                    motionDirectionRadians);
         }
         public MultiWheelChassisAdapter(MultiWheelChassis chassis, int vehicleId)
         {
@@ -254,94 +284,146 @@ namespace MyParking.Shared
 
             if (MaximumWheelRadiusMeters <= 0.0 ||
                 HalfWheelBaseMeters <= 0.0 ||
-                HalfTrackWidthMeters <= 0.0)
+                HalfTrackWidthMeters <= 0.0 ||
+                ControlPointRadiusMeters <= 0.0)
             {
                 throw new InvalidOperationException(
-                    "Wheel positions cannot produce valid chassis dimensions.");
+                    "Wheel positions and ControlPointRadius must produce valid chassis dimensions.");
             }
 
+            var initialBias = _chassis.GetOriginBias();
+            SetActiveMotionDirection(
+                -AngleMath.DegreesToRadians(
+                    initialBias.Z));
+
             // 通过反转轮速表达反向运动，避免蟹行正反切换时舵轮无意义地旋转180°。
             _chassis.PreferMinimumSteeringTravel = true;
         }
 
         /// <summary>
-        /// 将车体坐标系速度命令发送给多舵轮底盘。
+        /// 将车体坐标系刚体速度统一转换为滚动SendMotion、原地自转或停车命令。
+        /// 非零平移命令使用调用方在运动段开始前已经准备并激活的β运动坐标系。
         /// </summary>
-        public bool Send(ChassisCommand command, TimeSpan? interval = null)
+        public bool SendBodyTwist(
-        {
+            Twist2D bodyTwist,
-            if (command.VehicleId != VehicleId)
-            {
-                throw new InvalidOperationException(
-                    $"命令车辆编号{command.VehicleId}与适配器车辆编号" +
-                    $"{VehicleId}不一致。");
-            }
-            ValidateTwist(command.BodyTwist);
-            // 防止其他旧逻辑再次调用DirectionAngle或
-            // SetOriginBias改变底盘坐标语义。
-            EnsureBodyFrameIsActive();
-            var vxMetersPerSecond =
-                            (float)command.BodyTwist.VxMetersPerSecond;
-            var vyMetersPerSecond =
-                (float)command.BodyTwist.VyMetersPerSecond;
-            var omegaDegreesPerSecond =
-                (float)(
-                    command.BodyTwist.OmegaRadiansPerSecond *
-                    RadiansToDegrees);
-            var success = _chassis.SendXYThSpeed(
-                                vxMetersPerSecond,
-                                vyMetersPerSecond,
-                                omegaDegreesPerSecond,
-                                interval,
-                                enableDifferentialSteerFeedforward: true);
-            if (!success)
-            {
-                // 防止分解失败后继续执行上一条运动命令。
-                _chassis.PredefinedDriveStop();
-            }
-            return success;
-        }
-
-
-        /// <summary>
-        /// 在已经激活的运动坐标系中使用SendMotion执行虚拟阿克曼运动。
-        /// 转向角均相对该运动坐标系表达；正90度运动系对应车体左侧蟹行。
-        /// </summary>
-        public bool SendVirtualAckermannMotion(
-            double motionDirectionRadians,
-            double speedMetersPerSecond,
-            double steeringRadians,
             TimeSpan? interval = null)
         {
-            ValidateFinite(
+            ValidateTwist(bodyTwist);
-                motionDirectionRadians,
-                nameof(motionDirectionRadians));
-            ValidateFinite(
-                speedMetersPerSecond,
-                nameof(speedMetersPerSecond));
-            ValidateFinite(
-                steeringRadians,
-                nameof(steeringRadians));
-            EnsureMotionFrameIsActive(
-                motionDirectionRadians);
 
-            if (Math.Abs(steeringRadians) >=
+            var linearSpeedMetersPerSecond =
-                Math.PI / 2.0)
+                Math.Sqrt(
+                    bodyTwist.VxMetersPerSecond *
+                    bodyTwist.VxMetersPerSecond +
+                    bodyTwist.VyMetersPerSecond *
+                    bodyTwist.VyMetersPerSecond);
+
+            if (linearSpeedMetersPerSecond <= MotionDeadband)
             {
-                throw new ArgumentOutOfRangeException(
+                if (Math.Abs(
-                    nameof(steeringRadians),
+                        bodyTwist.OmegaRadiansPerSecond) <=
-                    "虚拟阿克曼转向角必须位于正负90度以内。");
+                    MotionDeadband)
+                {
+                    StopImmediately();
+                    return true;
+                }
+
+                return SendPureRotation(
+                    bodyTwist.OmegaRadiansPerSecond,
+                    interval);
             }
 
-            var steeringDegrees =
+            return SendRollingTwistInActiveMotionFrame(
-                (float)(
+                bodyTwist,
-                    steeringRadians *
+                linearSpeedMetersPerSecond,
-                    RadiansToDegrees);
+                interval);
-            var success =
+        }
-                _chassis.SendMotion(
+
-                    (float)speedMetersPerSecond,
+        /// <summary>
-                    steeringDegrees,
+        /// 将车体刚体速度转换到已准备的运动坐标系，并生成该坐标系中的前后GCP方向。
-                    -steeringDegrees,
+        /// </summary>
-                    interval);
+        private bool SendRollingTwistInActiveMotionFrame(
+            Twist2D bodyTwist,
+            double linearSpeedMetersPerSecond,
+            TimeSpan? interval)
+        {
+            EnsureMotionFrameIsActive(
+                _activeMotionDirectionRadians);
+
+            var bodyPoseInMotionFrame =
+                new Pose2D(
+                    0.0,
+                    0.0,
+                    -_activeMotionDirectionRadians);
+            var motionTwist =
+                FrameTransform2D.TransformTwistAtSamePoint(
+                    bodyPoseInMotionFrame,
+                    bodyTwist);
+            var motionVxMetersPerSecond =
+                motionTwist.VxMetersPerSecond;
+            var motionVyMetersPerSecond =
+                motionTwist.VyMetersPerSecond;
+
+            if (Math.Abs(motionVxMetersPerSecond) <=
+                MotionDeadband)
+            {
+                StopImmediately();
+                throw new InvalidOperationException(
+                    "当前车体速度几乎垂直于已经准备的运动坐标系，" +
+                    "无法由方向型前后GCP稳定表示。请停车后按目标主运动方向重新准备并激活β。");
+            }
+
+            var travelDirection =
+                Math.Sign(
+                    motionVxMetersPerSecond);
+            var signedCenterSpeedMetersPerSecond =
+                travelDirection *
+                linearSpeedMetersPerSecond;
+            var frontVelocityYMetersPerSecond =
+                motionVyMetersPerSecond +
+                bodyTwist.OmegaRadiansPerSecond *
+                ControlPointRadiusMeters;
+            var rearVelocityYMetersPerSecond =
+                motionVyMetersPerSecond -
+                bodyTwist.OmegaRadiansPerSecond *
+                ControlPointRadiusMeters;
+            var directedVxMetersPerSecond =
+                travelDirection *
+                motionVxMetersPerSecond;
+            var frontAngleRadians =
+                Math.Atan2(
+                    travelDirection *
+                    frontVelocityYMetersPerSecond,
+                    directedVxMetersPerSecond);
+            var rearAngleRadians =
+                Math.Atan2(
+                    travelDirection *
+                    rearVelocityYMetersPerSecond,
+                    directedVxMetersPerSecond);
+
+            return SendGcpMotionInActiveFrame(
+                signedCenterSpeedMetersPerSecond,
+                frontAngleRadians,
+                rearAngleRadians,
+                interval);
+        }
+
+        /// <summary>
+        /// 将已经完成自转舵轮准备的纯角速度命令交给XYTh底盘解算。
+        /// </summary>
+        private bool SendPureRotation(
+            double omegaRadiansPerSecond,
+            TimeSpan? interval)
+        {
+            EnsureBodyFrameIsActive();
+
+            var success = _chassis.SendXYThSpeed(
+                0.0f,
+                0.0f,
+                ConvertRadiansToSingleDegrees(
+                    omegaRadiansPerSecond,
+                    nameof(omegaRadiansPerSecond)),
+                interval,
+                enableDifferentialSteerFeedforward: true);
 
             if (!success)
             {
@@ -350,26 +432,26 @@ namespace MyParking.Shared
 
             return success;
         }
-
         /// <summary>
-        /// 在真实车体坐标系中将有符号速度和独立前后GCP角度发送给旧版SendMotion。
+        /// 在当前已激活的运动坐标系中将有符号速度和前后GCP角度发送给旧版SendMotion。
         /// </summary>
-        public bool SendGcpMotion(
+        private bool SendGcpMotionInActiveFrame(
             double speedMetersPerSecond,
             double frontAngleRadians,
             double rearAngleRadians,
             TimeSpan? interval = null)
         {
-            ValidateFinite(
+            EnsureRepresentableAsSingle(
                 speedMetersPerSecond,
                 nameof(speedMetersPerSecond));
-            ValidateFinite(
+            NumericGuard.EnsureFinite(
                 frontAngleRadians,
                 nameof(frontAngleRadians));
-            ValidateFinite(
+            NumericGuard.EnsureFinite(
                 rearAngleRadians,
                 nameof(rearAngleRadians));
-            EnsureBodyFrameIsActive();
+            EnsureMotionFrameIsActive(
+                _activeMotionDirectionRadians);
 
             if (Math.Abs(frontAngleRadians) >=
                     Math.PI / 2.0 ||
@@ -383,10 +465,12 @@ namespace MyParking.Shared
 
             var success = _chassis.SendMotion(
                 (float)speedMetersPerSecond,
-                (float)(frontAngleRadians *
+                ConvertRadiansToSingleDegrees(
-                        RadiansToDegrees),
+                    frontAngleRadians,
-                (float)(rearAngleRadians *
+                    nameof(frontAngleRadians)),
-                        RadiansToDegrees),
+                ConvertRadiansToSingleDegrees(
+                    rearAngleRadians,
+                    nameof(rearAngleRadians)),
                 interval);
 
             if (!success)
@@ -422,8 +506,11 @@ namespace MyParking.Shared
             double directionRadians)
         {
             EnsureBodyFrameIsActive();
-            var targetDegrees = (float)(FrameTransform2D.NormalizeAngle(directionRadians) *
+            var targetDegrees =
-                                    RadiansToDegrees);
+                ConvertRadiansToSingleDegrees(
+                    AngleMath.NormalizeRadians(
+                        directionRadians),
+                    nameof(directionRadians));
 
 #pragma warning disable CS0612, CS0618
             var wheels = _chassis.GetSteerWheels();
@@ -463,23 +550,21 @@ namespace MyParking.Shared
             double directionRadians,
             double toleranceRadians)
         {
-            if (double.IsNaN(toleranceRadians) ||
+            NumericGuard.EnsureFiniteNonNegative(
-        double.IsInfinity(toleranceRadians) ||
+                toleranceRadians,
-        toleranceRadians < 0.0)
+                nameof(toleranceRadians));
-            {
-                throw new ArgumentOutOfRangeException(
-                    nameof(toleranceRadians),
-                    "舵轮到位容差必须是非负有限值。");
-            }
 
             EnsureBodyFrameIsActive();
-            var targetDegrees = (float)(
+            var targetDegrees =
-                FrameTransform2D.NormalizeAngle(directionRadians) *
+                ConvertRadiansToSingleDegrees(
-                180.0 / Math.PI);
+                    AngleMath.NormalizeRadians(
+                        directionRadians),
+                    nameof(directionRadians));
 
-            var toleranceDegrees = (float)(
+            var toleranceDegrees =
-                Math.Abs(toleranceRadians) *
+                ConvertRadiansToSingleDegrees(
-                180.0 / Math.PI);
+                    toleranceRadians,
+                    nameof(toleranceRadians));
 
 #pragma warning disable CS0612, CS0618
             var wheels = _chassis.GetSteerWheels();
@@ -507,16 +592,12 @@ namespace MyParking.Shared
             TimeSpan? interval = null,
             double alignmentToleranceDegrees = 2.0)
         {
-            ValidateFinite(
+            NumericGuard.EnsureFiniteNonNegative(
+                alignmentToleranceDegrees,
+                nameof(alignmentToleranceDegrees));
+            EnsureRepresentableAsSingle(
                 alignmentToleranceDegrees,
                 nameof(alignmentToleranceDegrees));
-
-            if (alignmentToleranceDegrees < 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    nameof(alignmentToleranceDegrees),
-                    "自转舵轮到位容差必须是非负有限值。");
-            }
 
             EnsureBodyFrameIsActive();
 
@@ -540,23 +621,18 @@ namespace MyParking.Shared
             double toleranceRadians =
                 2.0 * Math.PI / 180.0)
         {
-            if (double.IsNaN(toleranceRadians) ||
+            NumericGuard.EnsureFiniteNonNegative(
-                double.IsInfinity(toleranceRadians) ||
+                toleranceRadians,
-                toleranceRadians < 0.0)
+                nameof(toleranceRadians));
-            {
-                throw new ArgumentOutOfRangeException(
-                    nameof(toleranceRadians),
-                    "自转状态交接容差必须是非负有限值。");
-            }
 
             EnsureBodyFrameIsActive();
 
             var success =
                 _chassis
                     .AdoptPreparedRotateWheelsForXYTh(
-                        (float)(
+                        ConvertRadiansToSingleDegrees(
-                            toleranceRadians *
+                            toleranceRadians,
-                            RadiansToDegrees));
+                            nameof(toleranceRadians)));
 
             if (!success)
             {

Shared/Mathematics/FrameTransform2D.cs

@@ -9,27 +9,6 @@ namespace MyParking.Shared
     /// </summary>
     public static class FrameTransform2D
     {
-        /// <summary>
-        /// 将角度归一化到[-π, π)范围。
-        /// </summary>
-        public static double NormalizeAngle(double angleRadians)
-        {
-            return AngleMath.NormalizeRadians(angleRadians);
-        }
-
-        /// <summary>
-        /// 计算从current到target的最短角度差。
-        /// 返回正值表示逆时针旋转。
-        /// </summary>
-        public static double ShortestAngleDifference(
-            double targetRadians,
-            double currentRadians)
-        {
-            return AngleMath.ShortestDifferenceRadians(
-                targetRadians,
-                currentRadians);
-        }
-
         /// <summary>
         /// 将源坐标系中的点变换到目标坐标系。
         /// sourcePoseInTarget表示源坐标系在目标坐标系中的位姿。
@@ -106,7 +85,7 @@ namespace MyParking.Shared
             return new Pose2D(
                 childPositionInParent.XMeters,
                 childPositionInParent.YMeters,
-                NormalizeAngle(
+                AngleMath.NormalizeRadians(
                     parentFromMiddle.YawRadians +
                     middleFromChild.YawRadians));
         }
@@ -127,7 +106,7 @@ namespace MyParking.Shared
                 sin * childPoseInParent.XMeters -
                 cos * childPoseInParent.YMeters,
 
-                NormalizeAngle(
+                AngleMath.NormalizeRadians(
                     -childPoseInParent.YawRadians));
         }
 

Shared/Models/MotionModels.cs

@@ -4,6 +4,7 @@
 // 车体坐标系采用右手系：X向前、Y向左、逆时针角度和角速度为正。
 // 命名约定：XxxInYyy表示Xxx在Yyy坐标系中的表达。
 
+// 共享的二维运动模型，不包含车辆路由或底盘执行策略。
 namespace MyParking.Shared
 {
     /// <summary>
@@ -81,36 +82,6 @@ namespace MyParking.Shared
             new Twist2D(0.0, 0.0, 0.0);
     }
 
-    /// <summary>
-    /// 发送给单辆车的车体坐标系速度命令。
-    /// </summary>
-    public readonly struct ChassisCommand
-    {
-        public ChassisCommand(
-            int vehicleId,
-            Twist2D bodyTwist)
-        {
-            VehicleId = vehicleId;
-            BodyTwist = bodyTwist;
-        }
-
-        public int VehicleId { get; }
-
-        /// <summary>
-        /// 单车车体坐标系速度：X向前、Y向左、逆时针旋转为正。
-        /// </summary>
-        public Twist2D BodyTwist { get; }
-
-        /// <summary>
-        /// 创建指定车辆的停止命令。
-        /// </summary>
-        public static ChassisCommand Stop(int vehicleId)
-        {
-            return new ChassisCommand(
-                vehicleId,
-                Twist2D.Zero);
-        }
-    }
 
     /// <summary>
     /// 单辆车的车体坐标系在车队坐标系中的位姿。

Shared/Validation/NumericGuard.cs

@@ -7,6 +7,15 @@ namespace MyParking.Shared
     /// </summary>
     public static class NumericGuard
     {
+        /// <summary>
+        /// 判断指定浮点数是否既不是NaN也不是无穷大。
+        /// </summary>
+        public static bool IsFinite(double value)
+        {
+            return !double.IsNaN(value) &&
+                   !double.IsInfinity(value);
+        }
+
         /// <summary>
         /// 确保指定浮点数不是NaN或无穷大。
         /// </summary>
@@ -14,8 +23,7 @@ namespace MyParking.Shared
             double value,
             string parameterName)
         {
-            if (double.IsNaN(value) ||
+            if (!IsFinite(value))
-                double.IsInfinity(value))
             {
                 throw new ArgumentOutOfRangeException(
                     parameterName,
@@ -64,17 +72,31 @@ namespace MyParking.Shared
             Pose2D pose,
             string parameterName)
         {
-            if (double.IsNaN(pose.XMeters) ||
+            if (!IsFinite(pose.XMeters) ||
-                double.IsInfinity(pose.XMeters) ||
+                !IsFinite(pose.YMeters) ||
-                double.IsNaN(pose.YMeters) ||
+                !IsFinite(pose.YawRadians))
-                double.IsInfinity(pose.YMeters) ||
-                double.IsNaN(pose.YawRadians) ||
-                double.IsInfinity(pose.YawRadians))
             {
                 throw new ArgumentOutOfRangeException(
                     parameterName,
                     "二维位姿必须由有限值组成。");
             }
         }
+
+        /// <summary>
+        /// 确保二维刚体速度的两个线速度分量和角速度均为有限值。
+        /// </summary>
+        public static void EnsureFinite(
+            Twist2D twist,
+            string parameterName)
+        {
+            if (!IsFinite(twist.VxMetersPerSecond) ||
+                !IsFinite(twist.VyMetersPerSecond) ||
+                !IsFinite(twist.OmegaRadiansPerSecond))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "二维刚体速度必须由有限值组成。");
+            }
+        }
     }
 }

data_process/plot_new_controller_experiment.py

@@ -293,11 +293,22 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
     )
     if np.any(has_control_reference):
         reference_speed[~has_control_reference] = np.nan
+    motion_frame_yaw_radians = np.deg2rad(
+        numeric_column(frame, "ReferenceMotionFrameYawDegrees", 0.0)
+    )
+    motion_direction_cosine = np.cos(motion_frame_yaw_radians)
+    motion_direction_sine = np.sin(motion_frame_yaw_radians)
     state_body_vx = numeric_column(frame, "StateBodyVxMetersPerSecond")
+    state_body_vy = numeric_column(frame, "StateBodyVyMetersPerSecond")
     velocity_valid = (
         numeric_column(frame, "StateVelocityEstimateValid", 0.0) > 0.5
     )
     state_body_vx[~velocity_valid] = np.nan
+    state_body_vy[~velocity_valid] = np.nan
+    state_motion_speed = (
+        state_body_vx * motion_direction_cosine
+        + state_body_vy * motion_direction_sine
+    )
     has_velocity_diagnostics = (
         numeric_column(frame, "HasVelocityDiagnostics", 0.0) > 0.5
     )
@@ -317,14 +328,22 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
         )
     )
     detour_speed[~detour_speed_valid] = np.nan
-    wheel_raw_speed = numeric_column(
+    wheel_raw_body_vx = numeric_column(
         frame,
         "WheelFeedbackRawBodyVxMetersPerSecond",
     )
-    wheel_filtered_speed = numeric_column(
+    wheel_filtered_body_vx = numeric_column(
         frame,
         "WheelFeedbackFilteredBodyVxMetersPerSecond",
     )
+    wheel_raw_body_vy = numeric_column(
+        frame,
+        "WheelFeedbackRawBodyVyMetersPerSecond",
+    )
+    wheel_filtered_body_vy = numeric_column(
+        frame,
+        "WheelFeedbackFilteredBodyVyMetersPerSecond",
+    )
     wheel_speed_valid = (
         has_velocity_diagnostics
         & (
@@ -336,12 +355,33 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
             > 0.5
         )
     )
+    wheel_raw_speed = (
+        wheel_raw_body_vx * motion_direction_cosine
+        + wheel_raw_body_vy * motion_direction_sine
+    )
+    wheel_filtered_speed = (
+        wheel_filtered_body_vx * motion_direction_cosine
+        + wheel_filtered_body_vy * motion_direction_sine
+    )
+
+    # 兼容尚未记录轮速Vy的旧版β=0实验；非零β缺少Vy时不能伪造投影速度。
+    body_x_motion = np.abs(motion_direction_sine) <= 1e-12
+    missing_raw_projection = ~np.isfinite(wheel_raw_speed)
+    missing_filtered_projection = ~np.isfinite(wheel_filtered_speed)
+    wheel_raw_speed[body_x_motion & missing_raw_projection] = (
+        wheel_raw_body_vx[body_x_motion & missing_raw_projection]
+    )
+    wheel_filtered_speed[
+        body_x_motion & missing_filtered_projection
+    ] = wheel_filtered_body_vx[
+        body_x_motion & missing_filtered_projection
+    ]
     wheel_raw_speed[~wheel_speed_valid] = np.nan
     wheel_filtered_speed[~wheel_speed_valid] = np.nan
     actual_speed = np.where(
         np.isfinite(wheel_filtered_speed),
         wheel_filtered_speed,
-        state_body_vx,
+        state_motion_speed,
     )
     command_speed = numeric_column(frame, "CommandSpeed")
 
@@ -504,7 +544,7 @@ def plot_experiment(
     )
     axis.grid(True, alpha=0.3)
 
-    # 4. 参考、命令、Detour估计和轮速解算速度。
+    # 4. 参考、命令、Detour车头分量和沿β投影的轮速解算速度。
     speed_error = data["actual_speed"] - data["reference_speed"]
     speed_rmse = finite_rmse(speed_error)
     axis = axes[1, 1]
@@ -527,7 +567,7 @@ def plot_experiment(
             data["detour_speed"],
             ":",
             linewidth=1.2,
-            label="Detour估计Vx",
+            label="Detour估计Vx（车头分量）",
         )
     if np.any(np.isfinite(data["wheel_filtered_speed"])):
         wheel_filtered_valid = np.isfinite(
@@ -540,7 +580,7 @@ def plot_experiment(
             s=14,
             marker="o",
             zorder=5,
-            label="轮速解算滤波Vx（控制使用）",
+            label="轮速解算β方向速度（控制使用）",
         )
     else:
         axis.plot(
@@ -553,7 +593,7 @@ def plot_experiment(
     axis.set_ylabel("速度 / (m/s)")
     axis.set_title(
         "参考速度、控制命令与观测速度\n"
-        f"轮速Vx相对参考速度RMSE={speed_rmse:.4f}m/s"
+        f"轮速β方向速度相对参考速度RMSE={speed_rmse:.4f}m/s"
     )
     axis.grid(True, alpha=0.3)
     axis.legend(fontsize=8)

docs/数据融合.txt

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