增加GCP运动学并完善车体平面速度反馈分析

2026-08-14 17:45:17 +08:00
parent 9fe8901c4c
commit d8de901a80
21 changed files with 445 additions and 38 deletions
@@ -0,0 +1,125 @@
+using System;
+using MyParking.Shared;
+
+namespace MultiWheelC.Control.Allocation
+{
+    /// <summary>
+    /// 在对称前后GCP方向命令与车体中心刚体速度之间执行纯几何转换。
+    /// </summary>
+    public static class GcpKinematics
+    {
+        private const double ParallelDirectionTolerance = 1e-9;
+        private const double StopSpeedDeadbandMetersPerSecond = 1e-6;
+
+        /// <summary>
+        /// 将有符号中心速度和前后GCP方向转换为真实车体坐标系中的Twist2D。
+        /// </summary>
+        public static Twist2D ToBodyTwist(
+            GcpMotionCommand command,
+            double controlPointRadiusMeters)
+        {
+            NumericGuard.EnsureFinitePositive(
+                controlPointRadiusMeters,
+                nameof(controlPointRadiusMeters));
+
+            if (Math.Abs(command.SpeedMetersPerSecond) <=
+                StopSpeedDeadbandMetersPerSecond)
+            {
+                return Twist2D.Zero;
+            }
+
+            var frontAngleRadians =
+                command.FrontAngleRadians;
+            var rearAngleRadians =
+                command.RearAngleRadians;
+            var directionDeterminant =
+                Math.Sin(
+                    rearAngleRadians -
+                    frontAngleRadians);
+
+            if (Math.Abs(directionDeterminant) <=
+                ParallelDirectionTolerance)
+            {
+                var averageDirectionRadians =
+                    Math.Atan2(
+                        Math.Sin(frontAngleRadians) +
+                        Math.Sin(rearAngleRadians),
+                        Math.Cos(frontAngleRadians) +
+                        Math.Cos(rearAngleRadians));
+
+                return new Twist2D(
+                    command.SpeedMetersPerSecond *
+                    Math.Cos(averageDirectionRadians),
+                    command.SpeedMetersPerSecond *
+                    Math.Sin(averageDirectionRadians),
+                    0.0);
+            }
+
+            var frontCosine =
+                Math.Cos(frontAngleRadians);
+            var frontSine =
+                Math.Sin(frontAngleRadians);
+            var rearCosine =
+                Math.Cos(rearAngleRadians);
+            var rearSine =
+                Math.Sin(rearAngleRadians);
+
+            // 两个GCP速度方向的法线交点就是瞬时旋转中心，坐标位于真实车体系。
+            var rotationCenterXMeters =
+                controlPointRadiusMeters *
+                (frontCosine * rearSine +
+                 frontSine * rearCosine) /
+                directionDeterminant;
+            var rotationCenterYMeters =
+                -2.0 *
+                controlPointRadiusMeters *
+                frontCosine *
+                rearCosine /
+                directionDeterminant;
+            var centerRadiusMeters =
+                Math.Sqrt(
+                    rotationCenterXMeters *
+                    rotationCenterXMeters +
+                    rotationCenterYMeters *
+                    rotationCenterYMeters);
+
+            if (!NumericGuard.IsFinite(centerRadiusMeters) ||
+                centerRadiusMeters <= 0.0)
+            {
+                throw new InvalidOperationException(
+                    "前后GCP方向不能生成有效的车体中心旋转半径。");
+            }
+
+            // 用有符号速度决定绕ICR的实际转向，避免倒车时把同一组GCP轴向解释成反向运动。
+            var requestedDirectionSign =
+                Math.Sign(command.SpeedMetersPerSecond);
+            var positiveAngularFrontVelocityXMetersPerSecond =
+                rotationCenterYMeters;
+            var positiveAngularFrontVelocityYMetersPerSecond =
+                controlPointRadiusMeters -
+                rotationCenterXMeters;
+            var frontDirectionAlignment =
+                positiveAngularFrontVelocityXMetersPerSecond *
+                requestedDirectionSign *
+                frontCosine +
+                positiveAngularFrontVelocityYMetersPerSecond *
+                requestedDirectionSign *
+                frontSine;
+            var omegaSign =
+                frontDirectionAlignment >= 0.0
+                    ? 1.0
+                    : -1.0;
+            var omegaRadiansPerSecond =
+                omegaSign *
+                Math.Abs(command.SpeedMetersPerSecond) /
+                centerRadiusMeters;
+
+            return new Twist2D(
+                omegaRadiansPerSecond *
+                rotationCenterYMeters,
+                -omegaRadiansPerSecond *
+                rotationCenterXMeters,
+                omegaRadiansPerSecond);
+        }
+    }
+}