实现蟹行轨迹跟踪测试并优化底盘XYTh与原地旋转舵角控制

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

ClumsyPilot/TrackingExperimentRecorder.cs

@@ -20,7 +20,7 @@ namespace MultiWheelC
         public double DetourY;
         public double DetourTheta;
 
-        // 车体速度单位为m/s，角速度单位为deg/s。
+        // 车体速度单位为m/s，角速度统一使用rad/s。
         public float CommandSpeed;
         public float CommandVx;
         public float CommandVy;
@@ -36,6 +36,8 @@ namespace MultiWheelC
         private readonly Vector2 _referenceStart;
         private readonly Vector2 _referenceEnd;
         private readonly float _referenceSpeed;
+        private readonly float _referenceAngularSpeed;
+        private readonly float _referenceMotionFrameYawDegrees;
         private readonly int _sampleIntervalMs;
 
         private readonly List<TrackingSample> _samples =
@@ -68,7 +70,9 @@ namespace MultiWheelC
             Vector2 referenceStart,
             Vector2 referenceEnd,
             float referenceSpeed,
-            int sampleIntervalMs = 50)
+            float referenceAngularSpeed = 0f,
+            int sampleIntervalMs = 50,
+            float referenceMotionFrameYawDegrees = 0f)
         {
             if (string.IsNullOrWhiteSpace(controllerName))
                 throw new ArgumentException(
@@ -91,6 +95,9 @@ namespace MultiWheelC
             _referenceStart = referenceStart;
             _referenceEnd = referenceEnd;
             _referenceSpeed = referenceSpeed;
+            _referenceAngularSpeed = referenceAngularSpeed;
+            _referenceMotionFrameYawDegrees =
+                referenceMotionFrameYawDegrees;
             _sampleIntervalMs = sampleIntervalMs;
         }
 
@@ -238,7 +245,11 @@ namespace MultiWheelC
 
                         commandVx = command.Vx;
                         commandVy = command.Vy;
-                        commandAngularSpeed = command.Vw;
+                        // CommonUsage.GetCarSpeed().Vw的单位为deg/s，
+                        // 记录器内部统一转换为rad/s。
+                        commandAngularSpeed =
+                            command.Vw *
+                            (float)Math.PI / 180f;
                         commandSpeed = (float)Math.Sqrt(
                             commandVx * commandVx +
                             commandVy * commandVy);
@@ -313,14 +324,18 @@ namespace MultiWheelC
                     "DetourY," +
                     "DetourTheta," +
                     "CommandSpeed," +
+                    // 保留旧列（deg/s）供历史Python脚本兼容。
                     "CommandAngularSpeed," +
+                    "CommandAngularSpeedRadPerSecond," +
                     "CommandVx," +
                     "CommandVy," +
                     "ReferenceStartX," +
                     "ReferenceStartY," +
                     "ReferenceEndX," +
                     "ReferenceEndY," +
-                    "ReferenceSpeed");
+                    "ReferenceSpeed," +
+                    "ReferenceAngularSpeedRadPerSecond," +
+                    "ReferenceMotionFrameYawDegrees");
 
                 foreach (var sample in snapshot)
                 {
@@ -335,6 +350,9 @@ namespace MultiWheelC
                         Format(sample.DetourY),
                         Format(sample.DetourTheta),
                         Format(sample.CommandSpeed),
+                        Format(
+                            sample.CommandAngularSpeed *
+                            180.0 / Math.PI),
                         Format(sample.CommandAngularSpeed),
                         Format(sample.CommandVx),
                         Format(sample.CommandVy),
@@ -342,7 +360,9 @@ namespace MultiWheelC
                         Format(_referenceStart.Y),
                         Format(_referenceEnd.X),
                         Format(_referenceEnd.Y),
-                        Format(_referenceSpeed)));
+                        Format(_referenceSpeed),
+                        Format(_referenceAngularSpeed),
+                        Format(_referenceMotionFrameYawDegrees)));
                 }
             }
         }