控制器测试横向跟踪，增加记录

MultiWheelC/Control/Execution/ParkingGeometricController.cs

@@ -47,11 +47,11 @@ namespace MultiWheelC.Control.Execution
             ILongitudinalController longitudinalController,
             GcpCommandAllocator gcpAllocator,
             GcpCommandExecutor commandExecutor,
-            double finishDistanceMeters = 0.03,
+            double finishDistanceMeters = 0.04,
             double finishSpeedMetersPerSecond = 0.02,
             double finishHeadingToleranceRadians =
                 3.0 * Math.PI / 180.0,
-            double maximumDistanceToTrajectoryMeters = 0.50)
+            double maximumDistanceToTrajectoryMeters = 0.30)
         {
             _stateProvider = stateProvider ??
                 throw new ArgumentNullException(

MultiWheelC/Control/Lateral/StanleyLateralController.cs

@@ -73,7 +73,7 @@ namespace MultiWheelC.Control.Lateral
         public double MinimumSpeedMetersPerSecond { get; }
 
         /// <summary>
-        /// 获取是否优先使用Detour估算的实际纵向速度计算横向修正。
+        /// 获取是否优先使用当前状态源提供的实际纵向速度计算横向修正。
         /// </summary>
         public bool UseActualSpeedForGain { get; }
 

MultiWheelC/Experiments/CompositeMotionPlanTests.cs

@@ -16,7 +16,7 @@ namespace MultiWheelC
     /// <summary>
     /// 测试平滑左转轨迹、停车原地左转90°和再次直行的组合运动执行过程。
     /// </summary>
-    [MovementTest(name = "新版控制器：曲线-停车自转-直线组合测试")]
+    [MovementTest(name = "新版控制器：直线-圆弧-折线组合测试")]
     public sealed class CompositeStopTurnGoTest : MovementTest
     {
         private const float MillimetersPerMeter = 1000f;
@@ -52,6 +52,13 @@ namespace MultiWheelC
                 return;
             }
 
+            if (!TrajectoryExperimentInput
+                    .TryReadLateralOffsetMeters(
+                        out var lateralOffsetMeters))
+            {
+                return;
+            }
+
             if (!MovementTestPreparation.AreWheelsForward())
             {
                 return;
@@ -65,19 +72,30 @@ namespace MultiWheelC
                 return;
             }
 
-            var stateProvider = new DetourVehicleStateProvider();
+            var detourStateProvider =
-            if (!stateProvider.TryGetState(out var initialState))
+                new DetourVehicleStateProvider();
+            if (!detourStateProvider.TryGetState(
+                    out var initialState))
             {
                 Console.WriteLine(
                     "无法读取组合运动起点位姿：" +
-                    stateProvider.LastFailureReason);
+                    detourStateProvider.LastFailureReason);
                 return;
             }
 
+            var stateProvider =
+                new WheelFeedbackVehicleStateProvider(
+                    detourStateProvider,
+                    chassis);
+
+            var planStartPose =
+                TrajectoryExperimentInput.OffsetPoseLaterally(
+                    initialState.PoseInWorld,
+                    lateralOffsetMeters);
             var firstTrajectory =
                 TestTrajectoryFactory
                     .CreateStraightSmoothLeftTurnStraight(
-                        initialState.PoseInWorld,
+                        planStartPose,
                         StraightLengthMeters,
                         TurnRadiusMeters,
                         AngleMath.DegreesToRadians(
@@ -133,7 +151,9 @@ namespace MultiWheelC
             _recorder = new TrackingExperimentRecorder(
                 controllerName: "NewStanleyPidComposite",
                 trajectoryName:
-                    "SmoothTurnStopRotateStraight",
+                    TrajectoryExperimentInput.BuildTrajectoryName(
+                        "SmoothTurnStopRotateStraight",
+                        lateralOffsetMeters),
                 trialNumber: TrialNumber,
                 referenceStart: ToMillimeterVector(
                     firstTrajectory.StartPoint.PoseInWorld),
@@ -145,7 +165,8 @@ namespace MultiWheelC
                 referenceAccelerationMetersPerSecondSquared:
                     (float)AccelerationMetersPerSecondSquared,
                 referenceDecelerationMetersPerSecondSquared:
-                    (float)DecelerationMetersPerSecondSquared);
+                    (float)DecelerationMetersPerSecondSquared,
+                diagnosticChassis: chassis);
             _recorder.Start();
 
             var controlPointRadiusMeters =
@@ -157,6 +178,7 @@ namespace MultiWheelC
                 StateProvider = stateProvider,
                 ConfigureTrackingMovement = tracking =>
                 {
+                    tracking.StanleyUsesActualSpeed = true;
                     tracking.MaximumCommandSpeedMetersPerSecond =
                         StraightMaximumSpeedMetersPerSecond;
                     tracking
@@ -166,6 +188,7 @@ namespace MultiWheelC
                 SegmentStarted = (index, segment) =>
                 {
                     _recorder?.ClearControlReference();
+                    _recorder?.ClearGcpCommand();
                     _recorder?.UpdateCommand(0f, 0f);
                     Console.WriteLine(
                         $"组合运动开始第{index + 1}段：" +
@@ -174,7 +197,8 @@ namespace MultiWheelC
                 TrackingCycleObserver = (index, controller) =>
                     RecordTrackingCycle(
                         controller,
-                        controlPointRadiusMeters),
+                        controlPointRadiusMeters,
+                        stateProvider),
                 RotationCommandObserver = (index, omega) =>
                     _recorder?.UpdateCommand(
                         0f,
@@ -276,7 +300,8 @@ namespace MultiWheelC
         /// </summary>
         private void RecordTrackingCycle(
             ParkingGeometricController controller,
-            double controlPointRadiusMeters)
+            double controlPointRadiusMeters,
+            WheelFeedbackVehicleStateProvider stateProvider)
         {
             if (controller.LastVehicleState.HasValue)
             {
@@ -284,6 +309,21 @@ namespace MultiWheelC
                     controller.LastVehicleState.Value);
             }
 
+            if (stateProvider.TryGetLatestVelocityDiagnostics(
+                    out var detourBodyVx,
+                    out var detourVelocityValid,
+                    out var rawWheelBodyVx,
+                    out var filteredWheelBodyVx,
+                    out var wheelVelocityValid))
+            {
+                _recorder?.UpdateVelocityDiagnostics(
+                    detourBodyVx,
+                    detourVelocityValid,
+                    rawWheelBodyVx,
+                    filteredWheelBodyVx,
+                    wheelVelocityValid);
+            }
+
             if (!controller.LastCommand.HasValue)
             {
                 return;
@@ -305,6 +345,9 @@ namespace MultiWheelC
             }
 
             var command = controller.LastCommand.Value;
+            _recorder?.UpdateGcpCommand(
+                command.FrontAngleRadians,
+                command.RearAngleRadians);
             var curvaturePerMeter = Math.Tan(
                 command.FrontAngleRadians) /
                 controlPointRadiusMeters;

MultiWheelC/Experiments/NewControllerTrackingTests.cs

@@ -1,5 +1,6 @@
 using System;
 using System.Drawing;
+using System.Globalization;
 using System.Numerics;
 using System.Threading;
 using ClumsyCore;
@@ -13,6 +14,82 @@ using MyParking.Shared;
 
 namespace MultiWheelC
 {
+    /// <summary>
+    /// 统一读取轨迹实验的有符号横向偏移，并将车体局部偏移转换到世界坐标系。
+    /// </summary>
+    internal static class TrajectoryExperimentInput
+    {
+        private const double MaximumOffsetCentimeters = 30.0;
+
+        /// <summary>
+        /// 从Clumsy输入框读取车体左正右负的横向偏移，单位转换为m。
+        /// </summary>
+        public static bool TryReadLateralOffsetMeters(
+            out double lateralOffsetMeters)
+        {
+            lateralOffsetMeters = 0.0;
+            var input = UI.GetInput(
+                "输入轨迹横向偏移（cm，左正右负，范围-30～30）：");
+            var parsed = double.TryParse(
+                input,
+                NumberStyles.Float,
+                CultureInfo.CurrentCulture,
+                out var offsetCentimeters) ||
+                double.TryParse(
+                    input,
+                    NumberStyles.Float,
+                    CultureInfo.InvariantCulture,
+                    out offsetCentimeters);
+
+            if (!parsed ||
+                double.IsNaN(offsetCentimeters) ||
+                double.IsInfinity(offsetCentimeters) ||
+                Math.Abs(offsetCentimeters) >
+                    MaximumOffsetCentimeters)
+            {
+                Console.WriteLine(
+                    "轨迹横向偏移必须是-30～30cm之间的有限数值，测试未启动。");
+                return false;
+            }
+
+            lateralOffsetMeters =
+                offsetCentimeters / 100.0;
+            return true;
+        }
+
+        /// <summary>
+        /// 沿初始车体左方向平移参考轨迹起点，同时保持世界坐标航向不变。
+        /// </summary>
+        public static Pose2D OffsetPoseLaterally(
+            Pose2D poseInWorld,
+            double lateralOffsetMeters)
+        {
+            var yawRadians = poseInWorld.YawRadians;
+            return new Pose2D(
+                poseInWorld.XMeters -
+                    Math.Sin(yawRadians) *
+                    lateralOffsetMeters,
+                poseInWorld.YMeters +
+                    Math.Cos(yawRadians) *
+                    lateralOffsetMeters,
+                yawRadians);
+        }
+
+        /// <summary>
+        /// 生成带毫米偏移标识的实验轨迹名称。
+        /// </summary>
+        public static string BuildTrajectoryName(
+            string baseName,
+            double lateralOffsetMeters)
+        {
+            return baseName +
+                   "_Offset" +
+                   (lateralOffsetMeters * 1000.0)
+                   .ToString("+0;-0;0", CultureInfo.InvariantCulture) +
+                   "mm";
+        }
+    }
+
     /// <summary>
     /// 从当前Detour位姿开始执行新版控制器4m直线跟踪并保存实验数据。
     /// </summary>
@@ -27,7 +104,7 @@ namespace MultiWheelC
 
         private DriveTask _task;
         private TrackingExperimentRecorder _recorder;
-        private DetourVehicleStateProvider _stateProvider;
+        private IVehicleStateProvider _stateProvider;
 
         /// <summary>
         /// 获取或设置本次测试编号，用于区分重复实验CSV。
@@ -37,7 +114,7 @@ namespace MultiWheelC
         /// <summary>
         /// 获取或设置4m直线的巡航参考速度，单位为m/s。
         /// </summary>
-        public double CruiseSpeedMetersPerSecond = 0.30;
+        public double CruiseSpeedMetersPerSecond = 0.40;
 
         /// <summary>
         /// 获取或设置参考速度加速度，单位为m/s²。
@@ -66,6 +143,13 @@ namespace MultiWheelC
                 return;
             }
 
+            if (!TrajectoryExperimentInput
+                    .TryReadLateralOffsetMeters(
+                        out var lateralOffsetMeters))
+            {
+                return;
+            }
+
             if (!MovementTestPreparation.AreWheelsForward())
             {
                 return;
@@ -80,21 +164,30 @@ namespace MultiWheelC
                 return;
             }
 
-            _stateProvider =
+            var detourStateProvider =
                 new DetourVehicleStateProvider();
-            if (!_stateProvider.TryGetState(
+            if (!detourStateProvider.TryGetState(
                     out var initialState))
             {
                 Console.WriteLine(
                     "无法读取有效Detour起点位姿：" +
-                    _stateProvider.LastFailureReason);
+                    detourStateProvider.LastFailureReason);
                 _stateProvider = null;
                 return;
             }
 
+            _stateProvider =
+                new WheelFeedbackVehicleStateProvider(
+                    detourStateProvider,
+                    chassis);
+
+            var trajectoryStartPose =
+                TrajectoryExperimentInput.OffsetPoseLaterally(
+                    initialState.PoseInWorld,
+                    lateralOffsetMeters);
             var trajectory =
                 TestTrajectoryFactory.CreateStraight4Meters(
-                    initialState.PoseInWorld,
+                    trajectoryStartPose,
                     CruiseSpeedMetersPerSecond,
                     AccelerationMetersPerSecondSquared,
                     DecelerationMetersPerSecondSquared,
@@ -109,7 +202,10 @@ namespace MultiWheelC
             var recorder =
                 new TrackingExperimentRecorder(
                     controllerName: "NewStanleyPid",
-                    trajectoryName: "ProfiledStraight4m",
+                    trajectoryName:
+                        TrajectoryExperimentInput.BuildTrajectoryName(
+                            "ProfiledStraight4m",
+                            lateralOffsetMeters),
                     trialNumber: TrialNumber,
                     referenceStart: referenceStart,
                     referenceEnd: referenceEnd,
@@ -119,7 +215,8 @@ namespace MultiWheelC
                     referenceAccelerationMetersPerSecondSquared:
                         (float)AccelerationMetersPerSecondSquared,
                     referenceDecelerationMetersPerSecondSquared:
-                        (float)DecelerationMetersPerSecondSquared);
+                        (float)DecelerationMetersPerSecondSquared,
+                    diagnosticChassis: chassis);
             _recorder = recorder;
 
             var controlPointRadiusMeters =
@@ -130,12 +227,15 @@ namespace MultiWheelC
                 {
                     Trajectory = trajectory,
                     StateProvider = _stateProvider,
+                    StanleyUsesActualSpeed = true,
                     MaximumCommandSpeedMetersPerSecond = 0.50,
                     CycleObserver = controller =>
                         RecordControlCycle(
                             recorder,
                             controller,
-                            controlPointRadiusMeters)
+                            controlPointRadiusMeters,
+                            _stateProvider as
+                                WheelFeedbackVehicleStateProvider)
                 };
 
             recorder.Start();
@@ -239,7 +339,8 @@ namespace MultiWheelC
         private static void RecordControlCycle(
             TrackingExperimentRecorder recorder,
             ParkingGeometricController controller,
-            double controlPointRadiusMeters)
+            double controlPointRadiusMeters,
+            WheelFeedbackVehicleStateProvider stateProvider)
         {
             if (controller.LastVehicleState.HasValue)
             {
@@ -247,6 +348,10 @@ namespace MultiWheelC
                     controller.LastVehicleState.Value);
             }
 
+            UpdateVelocityDiagnostics(
+                recorder,
+                stateProvider);
+
             if (!controller.LastCommand.HasValue)
             {
                 return;
@@ -267,6 +372,9 @@ namespace MultiWheelC
             }
 
             var command = controller.LastCommand.Value;
+            recorder.UpdateGcpCommand(
+                command.FrontAngleRadians,
+                command.RearAngleRadians);
             var curvaturePerMeter = Math.Tan(
                 command.FrontAngleRadians) /
                 controlPointRadiusMeters;
@@ -279,6 +387,32 @@ namespace MultiWheelC
                 (float)angularSpeedRadiansPerSecond);
         }
 
+        /// <summary>
+        /// 将同一周期的Detour速度和轮速解算速度写入实验记录器。
+        /// </summary>
+        private static void UpdateVelocityDiagnostics(
+            TrackingExperimentRecorder recorder,
+            WheelFeedbackVehicleStateProvider stateProvider)
+        {
+            if (stateProvider == null ||
+                !stateProvider.TryGetLatestVelocityDiagnostics(
+                    out var detourBodyVx,
+                    out var detourVelocityValid,
+                    out var rawWheelBodyVx,
+                    out var filteredWheelBodyVx,
+                    out var wheelVelocityValid))
+            {
+                return;
+            }
+
+            recorder.UpdateVelocityDiagnostics(
+                detourBodyVx,
+                detourVelocityValid,
+                rawWheelBodyVx,
+                filteredWheelBodyVx,
+                wheelVelocityValid);
+        }
+
         /// <summary>
         /// 将Shared世界坐标系米制位姿转换为Clumsy绘图和旧记录器使用的毫米坐标。
         /// </summary>
@@ -293,6 +427,7 @@ namespace MultiWheelC
                     poseInWorld.YMeters *
                     MillimetersPerMeter));
         }
+
     }
 
     /// <summary>
@@ -310,7 +445,7 @@ namespace MultiWheelC
 
         private DriveTask _task;
         private TrackingExperimentRecorder _recorder;
-        private DetourVehicleStateProvider _stateProvider;
+        private IVehicleStateProvider _stateProvider;
 
         /// <summary>
         /// 获取或设置本次测试编号，用于区分重复实验CSV。
@@ -369,6 +504,13 @@ namespace MultiWheelC
                 return;
             }
 
+            if (!TrajectoryExperimentInput
+                    .TryReadLateralOffsetMeters(
+                        out var lateralOffsetMeters))
+            {
+                return;
+            }
+
             if (!MovementTestPreparation.AreWheelsForward())
             {
                 return;
@@ -383,22 +525,31 @@ namespace MultiWheelC
                 return;
             }
 
-            _stateProvider =
+            var detourStateProvider =
                 new DetourVehicleStateProvider();
-            if (!_stateProvider.TryGetState(
+            if (!detourStateProvider.TryGetState(
                     out var initialState))
             {
                 Console.WriteLine(
                     "无法读取有效Detour起点位姿：" +
-                    _stateProvider.LastFailureReason);
+                    detourStateProvider.LastFailureReason);
                 _stateProvider = null;
                 return;
             }
 
+            _stateProvider =
+                new WheelFeedbackVehicleStateProvider(
+                    detourStateProvider,
+                    chassis);
+
+            var trajectoryStartPose =
+                TrajectoryExperimentInput.OffsetPoseLaterally(
+                    initialState.PoseInWorld,
+                    lateralOffsetMeters);
             var trajectory =
                 TestTrajectoryFactory
                     .CreateStraightLeftSemicircleStraight(
-                        initialState.PoseInWorld,
+                        trajectoryStartPose,
                         StraightLengthMeters,
                         TurnRadiusMeters,
                         CurvatureTransitionLengthMeters,
@@ -418,7 +569,9 @@ namespace MultiWheelC
                 new TrackingExperimentRecorder(
                     controllerName: "NewStanleyPid",
                     trajectoryName:
-                        "ProfiledStraightSmoothLeftTurnStraight",
+                        TrajectoryExperimentInput.BuildTrajectoryName(
+                            "ProfiledStraightSmoothLeftTurnStraight",
+                            lateralOffsetMeters),
                     trialNumber: TrialNumber,
                     referenceStart: referenceStart,
                     referenceEnd: referenceEnd,
@@ -428,7 +581,8 @@ namespace MultiWheelC
                     referenceAccelerationMetersPerSecondSquared:
                         (float)AccelerationMetersPerSecondSquared,
                     referenceDecelerationMetersPerSecondSquared:
-                        (float)DecelerationMetersPerSecondSquared);
+                        (float)DecelerationMetersPerSecondSquared,
+                    diagnosticChassis: chassis);
             _recorder = recorder;
 
             var controlPointRadiusMeters =
@@ -439,13 +593,16 @@ namespace MultiWheelC
                 {
                     Trajectory = trajectory,
                     StateProvider = _stateProvider,
+                    StanleyUsesActualSpeed = true,
                     MaximumCommandSpeedMetersPerSecond =
                         StraightMaximumSpeedMetersPerSecond,
                     CycleObserver = controller =>
                         RecordControlCycle(
                             recorder,
                             controller,
-                            controlPointRadiusMeters)
+                            controlPointRadiusMeters,
+                            _stateProvider as
+                                WheelFeedbackVehicleStateProvider)
                 };
 
             recorder.Start();
@@ -549,7 +706,8 @@ namespace MultiWheelC
         private static void RecordControlCycle(
             TrackingExperimentRecorder recorder,
             ParkingGeometricController controller,
-            double controlPointRadiusMeters)
+            double controlPointRadiusMeters,
+            WheelFeedbackVehicleStateProvider stateProvider)
         {
             if (controller.LastVehicleState.HasValue)
             {
@@ -557,6 +715,10 @@ namespace MultiWheelC
                     controller.LastVehicleState.Value);
             }
 
+            UpdateVelocityDiagnostics(
+                recorder,
+                stateProvider);
+
             if (!controller.LastCommand.HasValue)
             {
                 return;
@@ -577,6 +739,9 @@ namespace MultiWheelC
             }
 
             var command = controller.LastCommand.Value;
+            recorder.UpdateGcpCommand(
+                command.FrontAngleRadians,
+                command.RearAngleRadians);
             var curvaturePerMeter = Math.Tan(
                 command.FrontAngleRadians) /
                 controlPointRadiusMeters;
@@ -589,6 +754,32 @@ namespace MultiWheelC
                 (float)angularSpeedRadiansPerSecond);
         }
 
+        /// <summary>
+        /// 将同一周期的Detour速度和轮速解算速度写入实验记录器。
+        /// </summary>
+        private static void UpdateVelocityDiagnostics(
+            TrackingExperimentRecorder recorder,
+            WheelFeedbackVehicleStateProvider stateProvider)
+        {
+            if (stateProvider == null ||
+                !stateProvider.TryGetLatestVelocityDiagnostics(
+                    out var detourBodyVx,
+                    out var detourVelocityValid,
+                    out var rawWheelBodyVx,
+                    out var filteredWheelBodyVx,
+                    out var wheelVelocityValid))
+            {
+                return;
+            }
+
+            recorder.UpdateVelocityDiagnostics(
+                detourBodyVx,
+                detourVelocityValid,
+                rawWheelBodyVx,
+                filteredWheelBodyVx,
+                wheelVelocityValid);
+        }
+
         /// <summary>
         /// 将Shared世界坐标系米制位姿转换为Clumsy绘图和记录器使用的毫米坐标。
         /// </summary>

MultiWheelC/Experiments/TrackingExperimentRecorder.cs

@@ -7,6 +7,7 @@ using System.IO;
 using System.Numerics;
 using System.Text;
 using System.Threading;
+using CommonUsage.Chassis;
 using MyParking.Shared;
 using MultiWheelC.StateEstimation;
 
@@ -47,6 +48,28 @@ namespace MultiWheelC
         public double ControlHeadingErrorRadians;
         public double ControlDistanceToTrajectoryMeters;
         public double ControlRemainingDistanceMeters;
+
+        // 并列保存Detour速度与轮速解算速度，避免StateBodyVx的数据来源产生歧义。
+        public bool HasVelocityDiagnostics;
+        public double DetourEstimatedBodyVxMetersPerSecond;
+        public bool DetourVelocityEstimateValid;
+        public double WheelFeedbackRawBodyVxMetersPerSecond;
+        public double WheelFeedbackFilteredBodyVxMetersPerSecond;
+        public bool WheelFeedbackVelocityEstimateValid;
+
+        // 四舵轮机械角使用deg，前后虚拟GCP命令角使用rad。
+        public bool HasSteeringDiagnostics;
+        public double TargetSteerLeftFrontDegrees;
+        public double TargetSteerLeftRearDegrees;
+        public double TargetSteerRightFrontDegrees;
+        public double TargetSteerRightRearDegrees;
+        public double ActualSteerLeftFrontDegrees;
+        public double ActualSteerLeftRearDegrees;
+        public double ActualSteerRightFrontDegrees;
+        public double ActualSteerRightRearDegrees;
+        public bool HasGcpCommand;
+        public double CommandFrontGcpAngleRadians;
+        public double CommandRearGcpAngleRadians;
     }
 
     // C层实验工具：统一采集并保存轨迹跟踪实验数据。
@@ -63,6 +86,7 @@ namespace MultiWheelC
         private readonly float _referenceAccelerationMetersPerSecondSquared;
         private readonly float _referenceDecelerationMetersPerSecondSquared;
         private readonly int _sampleIntervalMs;
+        private readonly MultiWheelChassis _diagnosticChassis;
 
         private readonly List<TrackingSample> _samples =
             new List<TrackingSample>();
@@ -97,6 +121,15 @@ namespace MultiWheelC
         private double _controlHeadingErrorRadians;
         private double _controlDistanceToTrajectoryMeters;
         private double _controlRemainingDistanceMeters;
+        private bool _hasVelocityDiagnostics;
+        private double _detourEstimatedBodyVxMetersPerSecond;
+        private bool _detourVelocityEstimateValid;
+        private double _wheelFeedbackRawBodyVxMetersPerSecond;
+        private double _wheelFeedbackFilteredBodyVxMetersPerSecond;
+        private bool _wheelFeedbackVelocityEstimateValid;
+        private bool _hasGcpCommand;
+        private double _commandFrontGcpAngleRadians;
+        private double _commandRearGcpAngleRadians;
 
         public TrackingExperimentRecorder(
             string controllerName,
@@ -109,7 +142,8 @@ namespace MultiWheelC
             int sampleIntervalMs = 50,
             float referenceMotionFrameYawDegrees = 0f,
             float referenceAccelerationMetersPerSecondSquared = 0f,
-            float referenceDecelerationMetersPerSecondSquared = 0f)
+            float referenceDecelerationMetersPerSecondSquared = 0f,
+            MultiWheelChassis diagnosticChassis = null)
         {
             if (string.IsNullOrWhiteSpace(controllerName))
                 throw new ArgumentException(
@@ -140,6 +174,7 @@ namespace MultiWheelC
             _referenceDecelerationMetersPerSecondSquared =
                 referenceDecelerationMetersPerSecondSquared;
             _sampleIntervalMs = sampleIntervalMs;
+            _diagnosticChassis = diagnosticChassis;
         }
 
         // 保存成功后的CSV绝对路径；尚未保存时为空。
@@ -221,6 +256,60 @@ namespace MultiWheelC
             }
         }
 
+        /// <summary>
+        /// 保存同一控制周期的Detour纵向速度以及轮速解算的原始和滤波纵向速度。
+        /// </summary>
+        public void UpdateVelocityDiagnostics(
+            double detourEstimatedBodyVxMetersPerSecond,
+            bool detourVelocityEstimateValid,
+            double wheelFeedbackRawBodyVxMetersPerSecond,
+            double wheelFeedbackFilteredBodyVxMetersPerSecond,
+            bool wheelFeedbackVelocityEstimateValid)
+        {
+            lock (_stateSyncRoot)
+            {
+                _detourEstimatedBodyVxMetersPerSecond =
+                    detourEstimatedBodyVxMetersPerSecond;
+                _detourVelocityEstimateValid =
+                    detourVelocityEstimateValid;
+                _wheelFeedbackRawBodyVxMetersPerSecond =
+                    wheelFeedbackRawBodyVxMetersPerSecond;
+                _wheelFeedbackFilteredBodyVxMetersPerSecond =
+                    wheelFeedbackFilteredBodyVxMetersPerSecond;
+                _wheelFeedbackVelocityEstimateValid =
+                    wheelFeedbackVelocityEstimateValid;
+                _hasVelocityDiagnostics = true;
+            }
+        }
+
+        /// <summary>
+        /// 保存经过GCP角速度限制后实际交给底盘的前后虚拟控制点转角。
+        /// </summary>
+        public void UpdateGcpCommand(
+            double frontGcpAngleRadians,
+            double rearGcpAngleRadians)
+        {
+            lock (_stateSyncRoot)
+            {
+                _commandFrontGcpAngleRadians =
+                    frontGcpAngleRadians;
+                _commandRearGcpAngleRadians =
+                    rearGcpAngleRadians;
+                _hasGcpCommand = true;
+            }
+        }
+
+        /// <summary>
+        /// 清除上一轨迹段的GCP命令，避免停车或原地自转阶段沿用旧角度。
+        /// </summary>
+        public void ClearGcpCommand()
+        {
+            lock (_stateSyncRoot)
+            {
+                _hasGcpCommand = false;
+            }
+        }
+
         /// <summary>
         /// 保存新版控制器本周期实际使用的轨迹投影、误差和参考速度。
         /// </summary>
@@ -331,6 +420,15 @@ namespace MultiWheelC
                 double controlHeadingErrorRadians;
                 double controlDistanceToTrajectoryMeters;
                 double controlRemainingDistanceMeters;
+                bool hasVelocityDiagnostics;
+                double detourEstimatedBodyVxMetersPerSecond;
+                bool detourVelocityEstimateValid;
+                double wheelFeedbackRawBodyVxMetersPerSecond;
+                double wheelFeedbackFilteredBodyVxMetersPerSecond;
+                bool wheelFeedbackVelocityEstimateValid;
+                bool hasGcpCommand;
+                double commandFrontGcpAngleRadians;
+                double commandRearGcpAngleRadians;
 
                 lock (_commandSyncRoot)
                 {
@@ -382,6 +480,23 @@ namespace MultiWheelC
                         _controlDistanceToTrajectoryMeters;
                     controlRemainingDistanceMeters =
                         _controlRemainingDistanceMeters;
+                    hasVelocityDiagnostics =
+                        _hasVelocityDiagnostics;
+                    detourEstimatedBodyVxMetersPerSecond =
+                        _detourEstimatedBodyVxMetersPerSecond;
+                    detourVelocityEstimateValid =
+                        _detourVelocityEstimateValid;
+                    wheelFeedbackRawBodyVxMetersPerSecond =
+                        _wheelFeedbackRawBodyVxMetersPerSecond;
+                    wheelFeedbackFilteredBodyVxMetersPerSecond =
+                        _wheelFeedbackFilteredBodyVxMetersPerSecond;
+                    wheelFeedbackVelocityEstimateValid =
+                        _wheelFeedbackVelocityEstimateValid;
+                    hasGcpCommand = _hasGcpCommand;
+                    commandFrontGcpAngleRadians =
+                        _commandFrontGcpAngleRadians;
+                    commandRearGcpAngleRadians =
+                        _commandRearGcpAngleRadians;
                 }
 
                 var sample = new TrackingSample
@@ -411,9 +526,28 @@ namespace MultiWheelC
                     ControlDistanceToTrajectoryMeters =
                         controlDistanceToTrajectoryMeters,
                     ControlRemainingDistanceMeters =
-                        controlRemainingDistanceMeters
+                        controlRemainingDistanceMeters,
+                    HasVelocityDiagnostics =
+                        hasVelocityDiagnostics,
+                    DetourEstimatedBodyVxMetersPerSecond =
+                        detourEstimatedBodyVxMetersPerSecond,
+                    DetourVelocityEstimateValid =
+                        detourVelocityEstimateValid,
+                    WheelFeedbackRawBodyVxMetersPerSecond =
+                        wheelFeedbackRawBodyVxMetersPerSecond,
+                    WheelFeedbackFilteredBodyVxMetersPerSecond =
+                        wheelFeedbackFilteredBodyVxMetersPerSecond,
+                    WheelFeedbackVelocityEstimateValid =
+                        wheelFeedbackVelocityEstimateValid,
+                    HasGcpCommand = hasGcpCommand,
+                    CommandFrontGcpAngleRadians =
+                        commandFrontGcpAngleRadians,
+                    CommandRearGcpAngleRadians =
+                        commandRearGcpAngleRadians
                 };
 
+                CaptureSteeringDiagnostics(sample);
+
                 if (processedState.HasValue)
                 {
                     var state = processedState.Value;
@@ -452,6 +586,90 @@ namespace MultiWheelC
             }
         }
 
+        /// <summary>
+        /// 按舵轮物理安装位置记录四轮目标角和实际反馈角。
+        /// </summary>
+        private void CaptureSteeringDiagnostics(
+            TrackingSample sample)
+        {
+            if (_diagnosticChassis == null)
+            {
+                return;
+            }
+
+#pragma warning disable CS0612, CS0618
+            var wheels = _diagnosticChassis.GetSteerWheels();
+#pragma warning restore CS0612, CS0618
+
+            var leftFront = FindWheel(
+                wheels,
+                requireFront: true,
+                requireLeft: true);
+            var leftRear = FindWheel(
+                wheels,
+                requireFront: false,
+                requireLeft: true);
+            var rightFront = FindWheel(
+                wheels,
+                requireFront: true,
+                requireLeft: false);
+            var rightRear = FindWheel(
+                wheels,
+                requireFront: false,
+                requireLeft: false);
+
+            if (leftFront == null ||
+                leftRear == null ||
+                rightFront == null ||
+                rightRear == null)
+            {
+                return;
+            }
+
+            sample.HasSteeringDiagnostics = true;
+            sample.TargetSteerLeftFrontDegrees =
+                leftFront.GetSendAngle();
+            sample.TargetSteerLeftRearDegrees =
+                leftRear.GetSendAngle();
+            sample.TargetSteerRightFrontDegrees =
+                rightFront.GetSendAngle();
+            sample.TargetSteerRightRearDegrees =
+                rightRear.GetSendAngle();
+            sample.ActualSteerLeftFrontDegrees =
+                leftFront.ReadAngle();
+            sample.ActualSteerLeftRearDegrees =
+                leftRear.ReadAngle();
+            sample.ActualSteerRightFrontDegrees =
+                rightFront.ReadAngle();
+            sample.ActualSteerRightRearDegrees =
+                rightRear.ReadAngle();
+        }
+
+        /// <summary>
+        /// 根据车体X向前、Y向左的物理坐标查找指定象限中的舵轮。
+        /// </summary>
+        private static SteerWheel FindWheel(
+            IReadOnlyList<SteerWheel> wheels,
+            bool requireFront,
+            bool requireLeft)
+        {
+            foreach (var wheel in wheels)
+            {
+                var isFront =
+                    wheel.PhysicalPosition.X >= 0f;
+                var isLeft =
+                    wheel.PhysicalPosition.Y >= 0f;
+
+                if (isFront == requireFront &&
+                    isLeft == requireLeft)
+                {
+                    return wheel;
+                }
+            }
+
+            return null;
+        }
+
         // 将内存中的采样数据写入CSV。
         private void SaveCsv()
         {
@@ -523,7 +741,25 @@ namespace MultiWheelC
                     "ControlLateralErrorMeters," +
                     "ControlHeadingErrorRadians," +
                     "ControlDistanceToTrajectoryMeters," +
-                    "ControlRemainingDistanceMeters");
+                    "ControlRemainingDistanceMeters," +
+                    "HasVelocityDiagnostics," +
+                    "DetourEstimatedBodyVxMetersPerSecond," +
+                    "DetourVelocityEstimateValid," +
+                    "WheelFeedbackRawBodyVxMetersPerSecond," +
+                    "WheelFeedbackFilteredBodyVxMetersPerSecond," +
+                    "WheelFeedbackVelocityEstimateValid," +
+                    "HasSteeringDiagnostics," +
+                    "TargetSteerLeftFrontDegrees," +
+                    "TargetSteerLeftRearDegrees," +
+                    "TargetSteerRightFrontDegrees," +
+                    "TargetSteerRightRearDegrees," +
+                    "ActualSteerLeftFrontDegrees," +
+                    "ActualSteerLeftRearDegrees," +
+                    "ActualSteerRightFrontDegrees," +
+                    "ActualSteerRightRearDegrees," +
+                    "HasGcpCommand," +
+                    "CommandFrontGcpAngleRadians," +
+                    "CommandRearGcpAngleRadians");
 
                 foreach (var sample in snapshot)
                 {
@@ -610,7 +846,65 @@ namespace MultiWheelC
                             sample.ControlDistanceToTrajectoryMeters),
                         FormatOptional(
                             sample.HasControlReference,
-                            sample.ControlRemainingDistanceMeters)));
+                            sample.ControlRemainingDistanceMeters),
+                        sample.HasVelocityDiagnostics
+                            ? "1"
+                            : "0",
+                        FormatOptional(
+                            sample.HasVelocityDiagnostics,
+                            sample.DetourEstimatedBodyVxMetersPerSecond),
+                        sample.HasVelocityDiagnostics
+                            ? sample.DetourVelocityEstimateValid
+                                ? "1"
+                                : "0"
+                            : string.Empty,
+                        FormatOptional(
+                            sample.HasVelocityDiagnostics,
+                            sample.WheelFeedbackRawBodyVxMetersPerSecond),
+                        FormatOptional(
+                            sample.HasVelocityDiagnostics,
+                            sample.WheelFeedbackFilteredBodyVxMetersPerSecond),
+                        sample.HasVelocityDiagnostics
+                            ? sample.WheelFeedbackVelocityEstimateValid
+                                ? "1"
+                                : "0"
+                            : string.Empty,
+                        sample.HasSteeringDiagnostics
+                            ? "1"
+                            : "0",
+                        FormatOptional(
+                            sample.HasSteeringDiagnostics,
+                            sample.TargetSteerLeftFrontDegrees),
+                        FormatOptional(
+                            sample.HasSteeringDiagnostics,
+                            sample.TargetSteerLeftRearDegrees),
+                        FormatOptional(
+                            sample.HasSteeringDiagnostics,
+                            sample.TargetSteerRightFrontDegrees),
+                        FormatOptional(
+                            sample.HasSteeringDiagnostics,
+                            sample.TargetSteerRightRearDegrees),
+                        FormatOptional(
+                            sample.HasSteeringDiagnostics,
+                            sample.ActualSteerLeftFrontDegrees),
+                        FormatOptional(
+                            sample.HasSteeringDiagnostics,
+                            sample.ActualSteerLeftRearDegrees),
+                        FormatOptional(
+                            sample.HasSteeringDiagnostics,
+                            sample.ActualSteerRightFrontDegrees),
+                        FormatOptional(
+                            sample.HasSteeringDiagnostics,
+                            sample.ActualSteerRightRearDegrees),
+                        sample.HasGcpCommand
+                            ? "1"
+                            : "0",
+                        FormatOptional(
+                            sample.HasGcpCommand,
+                            sample.CommandFrontGcpAngleRadians),
+                        FormatOptional(
+                            sample.HasGcpCommand,
+                            sample.CommandRearGcpAngleRadians)));
                 }
             }
         }

MultiWheelC/Movements/TrajectoryTrackingMovement.cs

@@ -51,7 +51,7 @@ namespace MultiWheelC
         public double StanleyMinimumSpeedMetersPerSecond = 0.15;
 
         /// <summary>
-        /// 获取或设置Stanley是否优先使用Detour估算的实际速度。
+        /// 获取或设置Stanley是否优先使用当前状态源提供的实际纵向速度。
         /// </summary>
         public bool StanleyUsesActualSpeed = true;
 
@@ -129,7 +129,7 @@ namespace MultiWheelC
         /// <summary>
         /// 车辆允许偏离参考轨迹的最大欧氏距离，单位为m。
         /// </summary>
-        public double MaximumDistanceToTrajectoryMeters = 0.50;
+        public double MaximumDistanceToTrajectoryMeters = 0.30;
 
         /// <summary>
         /// 单次轨迹动作允许的最长执行时间，单位为s。

MultiWheelC/PilotConfig.cs

@@ -38,7 +38,7 @@ public class PilotConfig : MultiWheelPilotConfig
 
     #region 单车-临时
     [FieldMember(desc = "原地旋转Kp")]
-    public float InPlaceRotateKp = 1.1f;
+    public float InPlaceRotateKp = 1.05f;
 
     [FieldMember(desc = "原地旋转Ki")]
     public float InPlaceRotateKi = 0f;

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

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

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

@@ -1,4 +1,4 @@
-"""为新版4m直线控制器实验CSV生成轨迹、横向/航向误差和速度响应图。"""
+"""为新版控制器实验CSV生成包含轨迹、误差、速度和转角的六子图总图。"""
 
 from __future__ import annotations
 
@@ -293,13 +293,87 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
     )
     if np.any(has_control_reference):
         reference_speed[~has_control_reference] = np.nan
-    actual_speed = numeric_column(frame, "StateBodyVxMetersPerSecond")
+    state_body_vx = numeric_column(frame, "StateBodyVxMetersPerSecond")
     velocity_valid = (
         numeric_column(frame, "StateVelocityEstimateValid", 0.0) > 0.5
     )
-    actual_speed[~velocity_valid] = np.nan
+    state_body_vx[~velocity_valid] = np.nan
+    has_velocity_diagnostics = (
+        numeric_column(frame, "HasVelocityDiagnostics", 0.0) > 0.5
+    )
+    detour_speed = numeric_column(
+        frame,
+        "DetourEstimatedBodyVxMetersPerSecond",
+    )
+    detour_speed_valid = (
+        has_velocity_diagnostics
+        & (
+            numeric_column(
+                frame,
+                "DetourVelocityEstimateValid",
+                0.0,
+            )
+            > 0.5
+        )
+    )
+    detour_speed[~detour_speed_valid] = np.nan
+    wheel_raw_speed = numeric_column(
+        frame,
+        "WheelFeedbackRawBodyVxMetersPerSecond",
+    )
+    wheel_filtered_speed = numeric_column(
+        frame,
+        "WheelFeedbackFilteredBodyVxMetersPerSecond",
+    )
+    wheel_speed_valid = (
+        has_velocity_diagnostics
+        & (
+            numeric_column(
+                frame,
+                "WheelFeedbackVelocityEstimateValid",
+                0.0,
+            )
+            > 0.5
+        )
+    )
+    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,
+    )
     command_speed = numeric_column(frame, "CommandSpeed")
 
+    has_steering_diagnostics = (
+        numeric_column(frame, "HasSteeringDiagnostics", 0.0) > 0.5
+    )
+    steering_angles_degrees = {}
+    for wheel_name in (
+        "LeftFront",
+        "LeftRear",
+        "RightFront",
+        "RightRear",
+    ):
+        values = numeric_column(
+            frame,
+            f"ActualSteer{wheel_name}Degrees",
+        )
+        values[~has_steering_diagnostics] = np.nan
+        steering_angles_degrees[wheel_name] = values
+
+    has_gcp_command = (
+        numeric_column(frame, "HasGcpCommand", 0.0) > 0.5
+    )
+    front_gcp_degrees = np.rad2deg(
+        numeric_column(frame, "CommandFrontGcpAngleRadians")
+    )
+    rear_gcp_degrees = np.rad2deg(
+        numeric_column(frame, "CommandRearGcpAngleRadians")
+    )
+    front_gcp_degrees[~has_gcp_command] = np.nan
+    rear_gcp_degrees[~has_gcp_command] = np.nan
+
     return {
         "frame": frame,
         "time": time_seconds,
@@ -316,7 +390,13 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
         "heading_error": heading_error,
         "reference_speed": reference_speed,
         "actual_speed": actual_speed,
+        "detour_speed": detour_speed,
+        "wheel_raw_speed": wheel_raw_speed,
+        "wheel_filtered_speed": wheel_filtered_speed,
         "command_speed": command_speed,
+        "steering_angles_degrees": steering_angles_degrees,
+        "front_gcp_degrees": front_gcp_degrees,
+        "rear_gcp_degrees": rear_gcp_degrees,
         "controller_name": first_text(
             frame,
             "ControllerName",
@@ -342,7 +422,7 @@ def save_figure(
     show: bool,
 ) -> None:
     """保存并关闭一张实验图。"""
-    fig.tight_layout()
+    fig.tight_layout(rect=(0.0, 0.0, 1.0, 0.97))
     fig.savefig(destination, dpi=300, bbox_inches="tight")
     if show:
         plt.show()
@@ -354,14 +434,16 @@ def plot_experiment(
     output_directory: Path,
     show: bool,
 ) -> list[Path]:
-    """为单份新版控制器CSV生成四类对比图。"""
+    """为单份新版控制器CSV生成一张包含六个子图的实验总图。"""
     data = load_experiment(csv_path)
     output_directory.mkdir(parents=True, exist_ok=True)
     title = f"{data['controller_name']} - {data['trajectory_name']}"
-    destinations: list[Path] = []
+    fig, axes = plt.subplots(3, 2, figsize=(18.0, 16.0))
+    fig.suptitle(title, fontsize=16)
 
+    # 1. 期望轨迹与实际轨迹。
+    axis = axes[0, 0]
     valid_position = data["valid_position"]
-    fig, axis = plt.subplots(figsize=(9.0, 6.5))
     valid_reference_position = data["valid_reference_position"]
     if np.count_nonzero(valid_reference_position) >= 2:
         axis.plot(
@@ -390,47 +472,42 @@ def plot_experiment(
     axis.set_aspect("equal", adjustable="box")
     axis.set_xlabel("世界坐标X / m")
     axis.set_ylabel("世界坐标Y / m")
-    axis.set_title(f"期望轨迹与实际轨迹对比\n{title}")
+    axis.set_title("期望轨迹与实际轨迹对比")
     axis.grid(True, alpha=0.3)
-    axis.legend()
+    axis.legend(fontsize=8)
-    destination = output_directory / f"{csv_path.stem}_trajectory.png"
-    save_figure(fig, destination, show)
-    destinations.append(destination)
 
+    # 2. 横向误差。
     lateral_mm = data["lateral_error"] * 1000.0
     lateral_rmse_mm = finite_rmse(lateral_mm)
-    fig, axis = plt.subplots(figsize=(10.0, 5.5))
+    axis = axes[0, 1]
     axis.plot(data["time"], lateral_mm, linewidth=1.5)
     axis.axhline(0.0, color="black", linewidth=0.8)
     axis.set_xlabel("时间 / s")
     axis.set_ylabel("横向误差 / mm")
     axis.set_title(
-        f"横向误差（轨迹在车辆左侧为正）\n{title}，RMSE={lateral_rmse_mm:.2f}mm"
+        "横向误差（轨迹在车辆左侧为正）\n"
+        f"RMSE={lateral_rmse_mm:.2f}mm"
     )
     axis.grid(True, alpha=0.3)
-    destination = output_directory / f"{csv_path.stem}_lateral_error.png"
-    save_figure(fig, destination, show)
-    destinations.append(destination)
 
+    # 3. 航向误差。
     heading_degrees = np.rad2deg(data["heading_error"])
     heading_rmse_degrees = finite_rmse(heading_degrees)
-    fig, axis = plt.subplots(figsize=(10.0, 5.5))
+    axis = axes[1, 0]
     axis.plot(data["time"], heading_degrees, linewidth=1.5)
     axis.axhline(0.0, color="black", linewidth=0.8)
     axis.set_xlabel("时间 / s")
     axis.set_ylabel("航向角偏差 / °")
     axis.set_title(
         "航向角偏差：参考轨迹航向－实际车体航向（逆时针为正）\n"
-        f"{title}，RMSE={heading_rmse_degrees:.3f}°"
+        f"RMSE={heading_rmse_degrees:.3f}°"
     )
     axis.grid(True, alpha=0.3)
-    destination = output_directory / f"{csv_path.stem}_heading_error.png"
-    save_figure(fig, destination, show)
-    destinations.append(destination)
 
+    # 4. 参考、命令、Detour估计和轮速解算速度。
     speed_error = data["actual_speed"] - data["reference_speed"]
     speed_rmse = finite_rmse(speed_error)
-    fig, axis = plt.subplots(figsize=(10.0, 5.8))
+    axis = axes[1, 1]
     axis.plot(
         data["time"],
         data["reference_speed"],
@@ -444,29 +521,118 @@ def plot_experiment(
         linewidth=1.3,
         label="纵向控制器下发速度",
     )
-    axis.plot(
+    if np.any(np.isfinite(data["detour_speed"])):
-        data["time"],
+        axis.plot(
-        data["actual_speed"],
+            data["time"],
-        linewidth=1.5,
+            data["detour_speed"],
-        label="状态估计实际车体纵向速度",
+            ":",
-    )
+            linewidth=1.2,
+            label="Detour估计Vx",
+        )
+    if np.any(np.isfinite(data["wheel_filtered_speed"])):
+        axis.plot(
+            data["time"],
+            data["wheel_filtered_speed"],
+            linewidth=1.5,
+            label="轮速解算滤波Vx（控制使用）",
+        )
+    else:
+        axis.plot(
+            data["time"],
+            data["actual_speed"],
+            linewidth=1.5,
+            label="控制器实际纵向速度",
+        )
     axis.set_xlabel("时间 / s")
     axis.set_ylabel("速度 / (m/s)")
-    axis.set_title(f"参考速度与实际速度对比\n{title}，RMSE={speed_rmse:.4f}m/s")
+    axis.set_title(
+        "参考速度、控制命令与观测速度\n"
+        f"轮速Vx相对参考速度RMSE={speed_rmse:.4f}m/s"
+    )
     axis.grid(True, alpha=0.3)
-    axis.legend()
+    axis.legend(fontsize=8)
-    destination = output_directory / f"{csv_path.stem}_speed_response.png"
+
+    # 5. 四个舵轮的实际机械转角。
+    axis = axes[2, 0]
+    wheel_labels = {
+        "LeftFront": "左前轮",
+        "LeftRear": "左后轮",
+        "RightFront": "右前轮",
+        "RightRear": "右后轮",
+    }
+    steering_data_available = False
+    for wheel_name, wheel_label in wheel_labels.items():
+        wheel_angles = data["steering_angles_degrees"][wheel_name]
+        if np.any(np.isfinite(wheel_angles)):
+            steering_data_available = True
+            axis.plot(
+                data["time"],
+                wheel_angles,
+                linewidth=1.2,
+                label=wheel_label,
+            )
+    if steering_data_available:
+        axis.axhline(0.0, color="black", linewidth=0.8)
+        axis.legend(fontsize=8, ncol=2)
+    else:
+        axis.text(
+            0.5,
+            0.5,
+            "CSV不含四舵轮转角诊断数据",
+            ha="center",
+            va="center",
+            transform=axis.transAxes,
+        )
+    axis.set_xlabel("时间 / s")
+    axis.set_ylabel("实际舵角 / °")
+    axis.set_title("四个舵轮实际反馈转角")
+    axis.grid(True, alpha=0.3)
+
+    # 6. 经过角速度限制后实际发送的前、后虚拟GCP转角。
+    axis = axes[2, 1]
+    gcp_data_available = (
+        np.any(np.isfinite(data["front_gcp_degrees"]))
+        or np.any(np.isfinite(data["rear_gcp_degrees"]))
+    )
+    if gcp_data_available:
+        axis.plot(
+            data["time"],
+            data["front_gcp_degrees"],
+            linewidth=1.4,
+            label="前GCP",
+        )
+        axis.plot(
+            data["time"],
+            data["rear_gcp_degrees"],
+            linewidth=1.4,
+            label="后GCP",
+        )
+        axis.axhline(0.0, color="black", linewidth=0.8)
+        axis.legend(fontsize=8)
+    else:
+        axis.text(
+            0.5,
+            0.5,
+            "CSV不含前后GCP转角数据",
+            ha="center",
+            va="center",
+            transform=axis.transAxes,
+        )
+    axis.set_xlabel("时间 / s")
+    axis.set_ylabel("GCP命令角 / °")
+    axis.set_title("前后虚拟GCP实际发送转角")
+    axis.grid(True, alpha=0.3)
+
+    destination = output_directory / f"{csv_path.stem}_summary_6plots.png"
     save_figure(fig, destination, show)
-    destinations.append(destination)
 
     print(
         f"{csv_path.name}: 横向RMSE={lateral_rmse_mm:.3f}mm, "
         f"航向RMSE={heading_rmse_degrees:.4f}°, "
         f"速度RMSE={speed_rmse:.5f}m/s"
     )
-    for destination in destinations:
+    print(f"已生成六子图总图：{destination}")
-        print(f"已生成：{destination}")
+    return [destination]
-    return destinations
 
 
 def discover_csv_files(arguments: list[str]) -> list[Path]:
@@ -487,7 +653,7 @@ def discover_csv_files(arguments: list[str]) -> list[Path]:
 def main() -> None:
     """解析命令行并批量处理新版控制器实验CSV。"""
     parser = argparse.ArgumentParser(
-        description="绘制新版控制器轨迹实验的四类对比图。"
+        description="绘制新版控制器轨迹实验的六子图总图。"
     )
     parser.add_argument("csv", nargs="*", help="需要处理的CSV文件路径。")
     parser.add_argument(