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

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/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

@@ -224,6 +224,9 @@ namespace MultiWheelC
                     referenceSpeed:
                         (float)CruiseSpeedMetersPerSecond,
                     sampleIntervalMs: 50,
+                    referenceMotionFrameYawDegrees:
+                        (float)AngleMath.RadiansToDegrees(
+                            MotionDirectionInBodyRadians),
                     referenceAccelerationMetersPerSecondSquared:
                         (float)AccelerationMetersPerSecondSquared,
                     referenceDecelerationMetersPerSecondSquared:
@@ -432,6 +435,8 @@ namespace MultiWheelC
                     out var detourVelocityValid,
                     out var rawWheelBodyVx,
                     out var filteredWheelBodyVx,
+                    out var rawWheelBodyVy,
+                    out var filteredWheelBodyVy,
                     out var wheelVelocityValid))
             {
                 return;
@@ -442,6 +447,8 @@ namespace MultiWheelC
                 detourVelocityValid,
                 rawWheelBodyVx,
                 filteredWheelBodyVx,
+                rawWheelBodyVy,
+                filteredWheelBodyVy,
                 wheelVelocityValid);
         }
 
@@ -855,6 +862,8 @@ namespace MultiWheelC
                     out var detourVelocityValid,
                     out var rawWheelBodyVx,
                     out var filteredWheelBodyVx,
+                    out var rawWheelBodyVy,
+                    out var filteredWheelBodyVy,
                     out var wheelVelocityValid))
             {
                 return;
@@ -865,6 +874,8 @@ namespace MultiWheelC
                 detourVelocityValid,
                 rawWheelBodyVx,
                 filteredWheelBodyVx,
+                rawWheelBodyVy,
+                filteredWheelBodyVy,
                 wheelVelocityValid);
         }
 

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/StateEstimation/ParkingVehicleStateProviderFactory.cs

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

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,20 +93,27 @@ namespace MultiWheelC.StateEstimation
                 {
                     var actualCarSpeed =
                         _chassis.GetCarSpeed(true);
-                    var rawLongitudinalSpeedMetersPerSecond =
+                    var rawBodyVxMetersPerSecond =
                         (double)actualCarSpeed.Vx;
+                    var rawBodyVyMetersPerSecond =
+                        (double)actualCarSpeed.Vy;
 
                     NumericGuard.EnsureFinite(
-                        rawLongitudinalSpeedMetersPerSecond,
+                        rawBodyVxMetersPerSecond,
                         "电机反馈车体纵向速度");
+                    NumericGuard.EnsureFinite(
+                        rawBodyVyMetersPerSecond,
+                        "电机反馈车体横向速度");
 
                     var wheelSpeedTimestampSeconds =
                         _wheelSpeedClock.Elapsed.TotalSeconds;
 
-                    var filteredLongitudinalSpeedMetersPerSecond =
-                        UpdateLongitudinalSpeedFilter(
-                            rawLongitudinalSpeedMetersPerSecond,
+                    UpdateBodyVelocityFilters(
+                        rawBodyVxMetersPerSecond,
+                        rawBodyVyMetersPerSecond,
                         wheelSpeedTimestampSeconds,
+                        out var filteredBodyVxMetersPerSecond,
+                        out var filteredBodyVyMetersPerSecond,
                         out var hasValidWheelSpeedEstimate);
 
                     _latestDetourBodyVxMetersPerSecond =
@@ -108,19 +121,22 @@ namespace MultiWheelC.StateEstimation
                     _latestDetourVelocityValid =
                         poseState.HasValidVelocityEstimate;
                     _latestRawWheelBodyVxMetersPerSecond =
-                        rawLongitudinalSpeedMetersPerSecond;
+                        rawBodyVxMetersPerSecond;
                     _latestFilteredWheelBodyVxMetersPerSecond =
-                        filteredLongitudinalSpeedMetersPerSecond;
+                        filteredBodyVxMetersPerSecond;
+                    _latestRawWheelBodyVyMetersPerSecond =
+                        rawBodyVyMetersPerSecond;
+                    _latestFilteredWheelBodyVyMetersPerSecond =
+                        filteredBodyVyMetersPerSecond;
                     _latestWheelVelocityValid =
                         hasValidWheelSpeedEstimate;
                     _hasVelocityDiagnostics = true;
 
-                    // 第一阶段只替换控制器使用的车体纵向速度；横向速度和角速度
-                    // 继续使用Detour估计，避免轮速差和舵角误差放大Vy与Omega噪声。
+                    // 车体平面线速度来自四轮电机和舵角反馈；角速度继续使用Detour，
+                    // 避免轮速差和舵角误差放大Omega噪声。
                     var twistInBody = new Twist2D(
-                        filteredLongitudinalSpeedMetersPerSecond,
-                        poseState.TwistInBody
-                            .VyMetersPerSecond,
+                        filteredBodyVxMetersPerSecond,
+                        filteredBodyVyMetersPerSecond,
                         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,17 +215,22 @@ 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(
-            double rawLongitudinalSpeedMetersPerSecond,
+        private void UpdateBodyVelocityFilters(
+            double rawBodyVxMetersPerSecond,
+            double rawBodyVyMetersPerSecond,
             double timestampSeconds,
+            out double filteredBodyVxMetersPerSecond,
+            out double filteredBodyVyMetersPerSecond,
             out bool hasValidWheelSpeedEstimate)
         {
             NumericGuard.EnsureFiniteNonNegative(
@@ -212,11 +240,17 @@ namespace MultiWheelC.StateEstimation
             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,14 +261,25 @@ 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(
-                rawLongitudinalSpeedMetersPerSecond,
+            filteredBodyVxMetersPerSecond =
+                _longitudinalSpeedFilter.Update(
+                    rawBodyVxMetersPerSecond,
+                    deltaTimeSeconds);
+            filteredBodyVyMetersPerSecond =
+                _lateralSpeedFilter.Update(
+                    rawBodyVyMetersPerSecond,
                     deltaTimeSeconds);
         }
 

Shared/Models/MotionModels.cs

@@ -0,0 +1,154 @@
+// 纯数据层：只描述坐标、速度和命令
+// 定义二维坐标、位姿、速度、车队布局和单车底盘命令。
+// Shared层统一使用SI单位：位置m、线速度m/s、角度rad、角速度rad/s。
+// 车体坐标系采用右手系：X向前、Y向左、逆时针角度和角速度为正。
+// 命名约定：XxxInYyy表示Xxx在Yyy坐标系中的表达。
+
+// 共享的二维运动模型，不包含车辆路由或底盘执行策略。
+namespace MyParking.Shared
+{
+    /// <summary>
+    /// 二维坐标点，X、Y单位均为米。
+    /// </summary>
+    public readonly struct Point2D
+    {
+        public Point2D(double xMeters, double yMeters)
+        {
+            XMeters = xMeters;
+            YMeters = yMeters;
+        }
+
+        public double XMeters { get; }
+
+        public double YMeters { get; }
+
+        public static Point2D Zero => new Point2D(0.0, 0.0);
+    }
+
+    /// <summary>
+    /// 二维局部坐标系在父坐标系中的位姿。
+    /// 位置单位为米，朝向单位为弧度，逆时针为正。
+    /// 具体父子关系由变量名称说明，例如RadarPoseInBody。
+    /// </summary>
+    public readonly struct Pose2D
+    {
+        public Pose2D(
+            double xMeters,
+            double yMeters,
+            double yawRadians)
+        {
+            XMeters = xMeters;
+            YMeters = yMeters;
+            YawRadians = yawRadians;
+        }
+
+        public double XMeters { get; }
+
+        public double YMeters { get; }
+
+        public double YawRadians { get; }
+
+        public Point2D Position =>
+            new Point2D(XMeters, YMeters);
+
+        public static Pose2D Identity =>
+            new Pose2D(0.0, 0.0, 0.0);
+    }
+
+    /// <summary>
+    /// 二维刚体速度。
+    /// 线速度单位为m/s，角速度单位为rad/s。
+    /// 速度所属坐标系由持有该Twist2D的外层类型或变量名称确定。
+    /// </summary>
+    public readonly struct Twist2D
+    {
+        public Twist2D(
+            double vxMetersPerSecond,
+            double vyMetersPerSecond,
+            double omegaRadiansPerSecond)
+        {
+            VxMetersPerSecond = vxMetersPerSecond;
+            VyMetersPerSecond = vyMetersPerSecond;
+            OmegaRadiansPerSecond = omegaRadiansPerSecond;
+        }
+
+        public double VxMetersPerSecond { get; }
+
+        public double VyMetersPerSecond { get; }
+
+        public double OmegaRadiansPerSecond { get; }
+
+        public static Twist2D Zero =>
+            new Twist2D(0.0, 0.0, 0.0);
+    }
+
+
+    /// <summary>
+    /// 单辆车的车体坐标系在车队坐标系中的位姿。
+    /// </summary>
+    public readonly struct VehicleLayout
+    {
+        public VehicleLayout(
+            int vehicleId,
+            Pose2D poseInFleet)
+        {
+            VehicleId = vehicleId;
+            PoseInFleet = poseInFleet;
+        }
+
+        public int VehicleId { get; }
+
+        public Pose2D PoseInFleet { get; }
+    }
+
+    /// <summary>
+    /// 车队整体运动命令，速度分量均在车队坐标系中表达。
+    /// </summary>
+    public readonly struct FleetMotionCommand
+    {
+        public FleetMotionCommand(
+            Point2D referencePointInFleet,
+            Twist2D twistAtReferencePoint)
+        {
+            ReferencePointInFleet = referencePointInFleet;
+            TwistAtReferencePoint = twistAtReferencePoint;
+        }
+
+        /// <summary>
+        /// 速度命令对应的参考点，也可作为自定义旋转中心。
+        /// </summary>
+        public Point2D ReferencePointInFleet { get; }
+
+        /// <summary>
+        /// 参考点处的车队速度。
+        /// </summary>
+        public Twist2D TwistAtReferencePoint { get; }
+
+        /// <summary>
+        /// 创建绕指定中心原地旋转的车队命令。
+        /// </summary>
+        public static FleetMotionCommand RotateAround(
+            Point2D rotationCenterInFleet,
+            double omegaRadiansPerSecond)
+        {
+            return new FleetMotionCommand(
+                rotationCenterInFleet,
+                new Twist2D(
+                    0.0,
+                    0.0,
+                    omegaRadiansPerSecond));
+        }
+
+        /// <summary>
+        /// 创建车队停止命令。
+        /// </summary>
+        public static FleetMotionCommand Stop()
+        {
+            return new FleetMotionCommand(
+                Point2D.Zero,
+                Twist2D.Zero);
+        }
+    }
+
+
+}

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)