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

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

Shared/MultiWheelChassisAdapter.cs

@@ -19,6 +19,40 @@ namespace MyParking.Shared
         /// </summary>
         public int VehicleId { get; }
 
+        /// <summary>
+        /// Maximum distance from the body origin to a wheel center, in metres.
+        /// </summary>
+        public double MaximumWheelRadiusMeters { get; }
+
+        /// <summary>
+        /// Maximum longitudinal wheel offset from the body origin, in metres.
+        /// For a symmetric four-wheel-steering chassis this is half the wheelbase.
+        /// </summary>
+        public double HalfWheelBaseMeters { get; }
+
+        /// <summary>
+        /// Width of the steering-alignment speed gate, in degrees.
+        /// </summary>
+        public double SteeringAlignmentSigmaDegrees
+        {
+            get => _chassis.SteeringAlignmentSigmaDegrees;
+            set
+            {
+                if (double.IsNaN(value) ||
+                    double.IsInfinity(value) ||
+                    value <= 0.0 ||
+                    value > float.MaxValue)
+                {
+                    throw new ArgumentOutOfRangeException(
+                        nameof(value),
+                        "Steering alignment sigma must be a positive finite value.");
+                }
+
+                _chassis.SteeringAlignmentSigmaDegrees =
+                    (float)value;
+            }
+        }
+
         /// <summary>
         /// 检查旧底盘是否仍处于无偏置的真实车体坐标系。
         /// </summary>
@@ -118,6 +152,31 @@ namespace MyParking.Shared
             }
             // 禁用旧版DirectionAngle/ZeroDirection坐标偏置，
             // 保证SendXYThSpeed直接使用真实车体坐标系。
+            var maximumWheelRadiusMillimeters = 0.0;
+            var maximumLongitudinalOffsetMillimeters = 0.0;
+            foreach (var wheel in wheels)
+            {
+                maximumWheelRadiusMillimeters = Math.Max(
+                    maximumWheelRadiusMillimeters,
+                    wheel.PhysicalPosition.Length());
+
+                maximumLongitudinalOffsetMillimeters = Math.Max(
+                    maximumLongitudinalOffsetMillimeters,
+                    Math.Abs(wheel.PhysicalPosition.X));
+            }
+
+            MaximumWheelRadiusMeters =
+                maximumWheelRadiusMillimeters / 1000.0;
+            HalfWheelBaseMeters =
+                maximumLongitudinalOffsetMillimeters / 1000.0;
+
+            if (MaximumWheelRadiusMeters <= 0.0 ||
+                HalfWheelBaseMeters <= 0.0)
+            {
+                throw new InvalidOperationException(
+                    "Wheel positions cannot produce valid chassis dimensions.");
+            }
+
             ResetToBodyFrame();
 
             // 停车机器人优先保持当前机械舵角，通过反转轮速表达反向运动，
@@ -269,12 +328,10 @@ namespace MyParking.Shared
             TimeSpan? interval = null)
         {
             EnsureBodyFrameIsActive();
-            _chassis.PredefinedDriveStop();
 
             var success =
-                _chassis.SendRotateMotion(
-                    0.0f,
-                    interval);
+                _chassis.PrepareRotateWheels(
+                    alignmentToleranceDegrees: 2.0f);
 
             if (!success)
             {