实现蟹行轨迹跟踪测试并优化底盘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;
         }
 

MedullaAdapter/obj/MedullaAdapter.csproj.nuget.dgspec.json

@@ -10,7 +10,7 @@
         "projectUniqueName": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\MedullaAdapter\\MedullaAdapter.csproj",
         "projectName": "MedullaAdapter",
         "projectPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\MedullaAdapter\\MedullaAdapter.csproj",
-        "packagesPath": "C:\\Users\\CodexSandboxOffline\\.nuget\\packages\\",
+        "packagesPath": "C:\\Users\\admin\\.nuget\\packages\\",
         "outputPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\MedullaAdapter\\obj\\",
         "projectStyle": "PackageReference",
         "fallbackFolders": [

MedullaAdapter/obj/MedullaAdapter.csproj.nuget.g.props

@@ -4,13 +4,13 @@
     <RestoreSuccess Condition=" '$(RestoreSuccess)' == '' ">True</RestoreSuccess>
     <RestoreTool Condition=" '$(RestoreTool)' == '' ">NuGet</RestoreTool>
     <ProjectAssetsFile Condition=" '$(ProjectAssetsFile)' == '' ">$(MSBuildThisFileDirectory)project.assets.json</ProjectAssetsFile>
-    <NuGetPackageRoot Condition=" '$(NuGetPackageRoot)' == '' ">C:\Users\CodexSandboxOffline\.nuget\packages\</NuGetPackageRoot>
-    <NuGetPackageFolders Condition=" '$(NuGetPackageFolders)' == '' ">C:\Users\CodexSandboxOffline\.nuget\packages\;C:\Program Files (x86)\Microsoft Visual Studio\Shared\NuGetPackages</NuGetPackageFolders>
+    <NuGetPackageRoot Condition=" '$(NuGetPackageRoot)' == '' ">$(UserProfile)\.nuget\packages\</NuGetPackageRoot>
+    <NuGetPackageFolders Condition=" '$(NuGetPackageFolders)' == '' ">C:\Users\admin\.nuget\packages\;C:\Program Files (x86)\Microsoft Visual Studio\Shared\NuGetPackages</NuGetPackageFolders>
     <NuGetProjectStyle Condition=" '$(NuGetProjectStyle)' == '' ">PackageReference</NuGetProjectStyle>
     <NuGetToolVersion Condition=" '$(NuGetToolVersion)' == '' ">6.14.3</NuGetToolVersion>
   </PropertyGroup>
   <ItemGroup Condition=" '$(ExcludeRestorePackageImports)' != 'true' ">
-    <SourceRoot Include="C:\Users\CodexSandboxOffline\.nuget\packages\" />
+    <SourceRoot Include="C:\Users\admin\.nuget\packages\" />
     <SourceRoot Include="C:\Program Files (x86)\Microsoft Visual Studio\Shared\NuGetPackages\" />
   </ItemGroup>
 </Project>

MedullaAdapter/obj/project.assets.json

@@ -8,7 +8,7 @@
     "net8.0": []
   },
   "packageFolders": {
-    "C:\\Users\\CodexSandboxOffline\\.nuget\\packages\\": {},
+    "C:\\Users\\admin\\.nuget\\packages\\": {},
     "C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\NuGetPackages": {}
   },
   "project": {
@@ -17,7 +17,7 @@
       "projectUniqueName": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\MedullaAdapter\\MedullaAdapter.csproj",
       "projectName": "MedullaAdapter",
       "projectPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\MedullaAdapter\\MedullaAdapter.csproj",
-      "packagesPath": "C:\\Users\\CodexSandboxOffline\\.nuget\\packages\\",
+      "packagesPath": "C:\\Users\\admin\\.nuget\\packages\\",
       "outputPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\MedullaAdapter\\obj\\",
       "projectStyle": "PackageReference",
       "fallbackFolders": [

MedullaAdapter/obj/project.nuget.cache

@@ -1,6 +1,6 @@
 {
   "version": 2,
-  "dgSpecHash": "b9v8vkN2ac8=",
+  "dgSpecHash": "4fABnQtycfA=",
   "success": true,
   "projectFilePath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\MedullaAdapter\\MedullaAdapter.csproj",
   "expectedPackageFiles": [],