测试后可正常执行

MultiWheelC/Control/Execution/GcpCommandExecutor.cs

@@ -13,6 +13,7 @@ namespace MultiWheelC.Control.Execution
             1e-6;
 
         private readonly MultiWheelChassisAdapter _chassisAdapter;
+        private readonly double _motionDirectionInBodyRadians;
         private double _lastFrontAngleRadians;
         private double _lastRearAngleRadians;
 
@@ -22,7 +23,8 @@ namespace MultiWheelC.Control.Execution
         public GcpCommandExecutor(
             MultiWheelChassisAdapter chassisAdapter,
             double maximumGcpAngleRateRadiansPerSecond =
-                10.0 * Math.PI / 180.0)
+                10.0 * Math.PI / 180.0,
+            double motionDirectionInBodyRadians = 0.0)
         {
             _chassisAdapter = chassisAdapter ??
                 throw new ArgumentNullException(
@@ -30,9 +32,15 @@ namespace MultiWheelC.Control.Execution
             EnsureFinitePositive(
                 maximumGcpAngleRateRadiansPerSecond,
                 nameof(maximumGcpAngleRateRadiansPerSecond));
+            NumericGuard.EnsureFinite(
+                motionDirectionInBodyRadians,
+                nameof(motionDirectionInBodyRadians));
 
             MaximumGcpAngleRateRadiansPerSecond =
                 maximumGcpAngleRateRadiansPerSecond;
+            _motionDirectionInBodyRadians =
+                AngleMath.NormalizeRadians(
+                    motionDirectionInBodyRadians);
         }
 
         /// <summary>
@@ -103,10 +111,19 @@ namespace MultiWheelC.Control.Execution
                 _lastRearAngleRadians);
             LastSentCommand = limitedCommand;
 
-            var success = _chassisAdapter.SendGcpMotion(
-                limitedCommand.SpeedMetersPerSecond,
-                limitedCommand.FrontAngleRadians,
-                limitedCommand.RearAngleRadians,
+            var motionFrameTwist =
+                GcpKinematics.ToBodyTwist(
+                    limitedCommand,
+                    _chassisAdapter.ControlPointRadiusMeters);
+            var bodyTwist =
+                FrameTransform2D.TransformTwistAtSamePoint(
+                    new Pose2D(
+                        0.0,
+                        0.0,
+                        _motionDirectionInBodyRadians),
+                    motionFrameTwist);
+            var success = _chassisAdapter.SendBodyTwist(
+                bodyTwist,
                 TimeSpan.FromSeconds(deltaTimeSeconds));
 
             LastFailureReason = success

MultiWheelC/Control/Execution/ParkingGeometricController.cs

@@ -86,6 +86,7 @@ namespace MultiWheelC.Control.Execution
         private readonly ILongitudinalController _longitudinalController;
         private readonly GcpCommandAllocator _gcpAllocator;
         private readonly GcpCommandExecutor _commandExecutor;
+        private readonly double _motionDirectionInBodyRadians;
 
         private Trajectory2D _trajectory;
         private double _terminalTravelDirection = 1.0;
@@ -112,7 +113,8 @@ namespace MultiWheelC.Control.Execution
             double terminalApproachGainPerSecond = 0.8,
             double maximumTerminalApproachSpeedMetersPerSecond = 0.05,
             double curvaturePreviewSeconds = 0.20,
-            double maximumCurvaturePreviewMeters = 0.12)
+            double maximumCurvaturePreviewMeters = 0.12,
+            double motionDirectionInBodyRadians = 0.0)
         {
             _stateProvider = stateProvider ??
                 throw new ArgumentNullException(
@@ -160,6 +162,9 @@ namespace MultiWheelC.Control.Execution
             EnsureFiniteNonNegative(
                 maximumCurvaturePreviewMeters,
                 nameof(maximumCurvaturePreviewMeters));
+            NumericGuard.EnsureFinite(
+                motionDirectionInBodyRadians,
+                nameof(motionDirectionInBodyRadians));
 
             if (terminalApproachDistanceMeters <=
                 finishDistanceMeters)
@@ -187,6 +192,9 @@ namespace MultiWheelC.Control.Execution
             CurvaturePreviewSeconds = curvaturePreviewSeconds;
             MaximumCurvaturePreviewMeters =
                 maximumCurvaturePreviewMeters;
+            _motionDirectionInBodyRadians =
+                AngleMath.NormalizeRadians(
+                    motionDirectionInBodyRadians);
         }
 
         /// <summary>
@@ -484,7 +492,8 @@ namespace MultiWheelC.Control.Execution
                     projection,
                     controlReferenceSpeedMetersPerSecond,
                     feedforwardCurvaturePerMeter,
-                    deltaTimeSeconds);
+                    deltaTimeSeconds,
+                    _motionDirectionInBodyRadians);
                 var lateralCommand =
                     _lateralController.Compute(context);
                 var commandSpeedMetersPerSecond =
@@ -617,9 +626,8 @@ namespace MultiWheelC.Control.Execution
 
             var previewSpeedMetersPerSecond =
                 vehicleState.HasValidVelocityEstimate
-                    ? Math.Abs(
-                        vehicleState.TwistInBody
-                            .VxMetersPerSecond)
+                    ? CalculateActualLongitudinalSpeedMetersPerSecond(
+                        vehicleState)
                     : Math.Abs(
                         controlReferenceSpeedMetersPerSecond);
 
@@ -931,13 +939,16 @@ namespace MultiWheelC.Control.Execution
         }
 
         /// <summary>
-        /// 计算车体坐标系实际纵向速度的绝对值，单位为m/s。
+        /// 计算车辆沿当前运动坐标系X轴实际速度的绝对值，单位为m/s。
         /// </summary>
-        private static double CalculateActualLongitudinalSpeedMetersPerSecond(
+        private double CalculateActualLongitudinalSpeedMetersPerSecond(
             VehicleState vehicleState)
         {
             return Math.Abs(
-                vehicleState.TwistInBody.VxMetersPerSecond);
+                Math.Cos(_motionDirectionInBodyRadians) *
+                    vehicleState.TwistInBody.VxMetersPerSecond +
+                Math.Sin(_motionDirectionInBodyRadians) *
+                    vehicleState.TwistInBody.VyMetersPerSecond);
         }
 
         /// <summary>