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

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

MedullaAdapter/DiverCartDefinition.cs

@@ -68,6 +68,8 @@ namespace MedullaAdapter
         [AsInitParam(desc = "遥控器速度上限")] public float TransmitterSpeedUpperLimit = 1.0f;
         [AsInitParam(desc = "遥控器速度下限")] public float TransmitterSpeedLowerLimit = 0.0f;
         [AsInitParam(desc = "手动控制夹臂速度系数")] public float ManualArmSpeedFac = 1.0f;
+        [AsInitParam(desc = "遥控转弯舵角同步限速宽度，单位为度")]
+        public float ManualSteeringAlignmentSigmaDegrees = 8.0f;
         [AsInitParam(desc = "左夹臂低限位")][AsLowerIO] public int LeftArmLowerPos = -10000;
         [AsInitParam(desc = "左夹臂高限位")][AsLowerIO] public int LeftArmUpperPos = 5927610;
         [AsInitParam(desc = "右夹臂低限位")][AsLowerIO] public int RightArmLowerPos = -17295;
@@ -225,21 +227,39 @@ namespace MedullaAdapter
             }
 
             var speed = speedThreshold * y;
-            var omega = CalculateManualOmega(speed, x);
+            var normalizedSteering =
+                (float)Math.Pow(
+                    Math.Abs(x),
+                    ManualThetaPow) *
+                Math.Sign(x);
+            var steeringDegrees =
+                -normalizedSteering * MaxManualTheta;
+            var omega = CalculateManualOmega(
+                speed,
+                steeringDegrees,
+                adapter.HalfWheelBaseMeters);
             ManualMode = (int)mode;
 
             switch (mode)
             {
                 case ManualControlMode.Normal:
-                    SendBodyCommand(vx: speed, vy: 0.0, omegaRadiansPerSecond: omega, interval);
+                    // 普通模式统一使用车体速度命令：
+                    // X向前，行驶中连续改变角速度时舵轮边转、车辆边走。
+                    SendBodyCommand(
+                        vx: speed,
+                        vy: 0.0,
+                        omegaRadiansPerSecond: omega,
+                        interval);
                     break;
                 case ManualControlMode.Crab:
                     SendBodyCommand(vx: 0.0, vy: speed, omegaRadiansPerSecond: omega, interval);
                     break;
                 case ManualControlMode.Spin:
+                    // 自转时speed表示最外侧舵轮的目标切向速度。
+                    // 根据v=omega*r换算角速度，不能把m/s与deg/s直接相乘。
                     var spinOmega =
-                        speed * MaxAngularSpeed *
-                        Math.PI / 180.0;
+                        speed /
+                        adapter.MaximumWheelRadiusMeters;
 
                     SendBodyCommand(
                         vx: 0.0,
@@ -341,27 +361,15 @@ namespace MedullaAdapter
 
         private double CalculateManualOmega(
             float speed,
-            float steeringInput)
+            float steeringDegrees,
+            double halfWheelBaseMeters)
         {
-            var normalizedSteering =
-                (float)Math.Pow(
-                    Math.Abs(steeringInput),
-                    ManualThetaPow) *
-                Math.Sign(steeringInput);
-
-            var steeringDegrees =
-                -normalizedSteering * MaxManualTheta;
-
             var steeringRadians =
                 steeringDegrees * Math.PI / 180.0;
 
-            // CommonUsage中的ControlPointRadius单位为毫米。
-            var halfWheelBaseMeters =
-                Math.Max(
-                    Chassis.ControlPointRadius / 1000.0,
-                    0.01);
-
-            return speed * Math.Tan(steeringRadians) / halfWheelBaseMeters;
+            return speed *
+                   Math.Tan(steeringRadians) /
+                   Math.Max(halfWheelBaseMeters, 0.01);
         }
 
         internal void SendBodyCommand(double vx, double vy, double omegaRadiansPerSecond, TimeSpan? interval = null)
@@ -398,6 +406,11 @@ namespace MedullaAdapter
                     new MultiWheelChassisAdapter(Chassis, CarNum);
             }
 
+            _chassisAdapter.SteeringAlignmentSigmaDegrees =
+                Math.Max(
+                    ManualSteeringAlignmentSigmaDegrees,
+                    0.1f);
+
             return _chassisAdapter;
         }