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

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

.gitignore

@@ -89,4 +89,5 @@ _ReSharper*/
 *.sqlite
 *.sqlite3
 
-*.csv
+*.csv
+*.png

ClumsyPilot/CrabMotionFrameTracker.cs

@@ -0,0 +1,667 @@
+using ClumsyCore;
+using ClumsyCore.DTools;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using CommonUsage.Chassis;
+using MyParking.Shared;
+using System;
+using System.Collections.Generic;
+using System.Numerics;
+
+namespace MultiWheelC
+{
+    // C层单车测试：在可配置的运动坐标系中统一跟踪直线、圆弧或S型曲线。
+    public sealed class CrabMotionFrameTracker : MovementDefinition
+    {
+        public enum ReferencePathKind
+        {
+            Straight = 0,
+            LeftArc = 1,
+            SCurve = 2
+        }
+
+        public ReferencePathKind PathKind;
+        public Vector2 StartPosition;
+        public double InitialBodyYawRadians;
+        public float LengthMillimeters = 4000f;
+        public float RadiusMillimeters = 2000f;
+        public float SCurveLateralOffsetMillimeters = 400f;
+        public double ArcSweepRadians = Math.PI / 2.0;
+        public float CruiseSpeed = 0.2f;
+        public float SlowDistanceMillimeters = 600f;
+        public float FinishDistanceMillimeters = 30f;
+        public float MinimumSpeed = 0.04f;
+        public double LateralGainPerSecond = 0.8;
+        public double MaximumLateralCorrection = 0.12;
+        public double HeadingGainPerSecond = 1.5;
+        public double MaximumAngularSpeedRadiansPerSecond =
+            30.0 * Math.PI / 180.0;
+        public float WheelAlignmentToleranceDegrees = 2f;
+        public float WheelAlignmentStableSeconds = 0.3f;
+        public float WheelAlignmentTimeoutSeconds = 10f;
+        public float TrackingTimeoutSeconds = 60f;
+        public Action<float, float, float> CommandObserver;
+
+        // 运动坐标系相对车体坐标系的朝向：普通模式为0，蟹行为π/2。
+        public double MotionFrameYawInBodyRadians = Math.PI / 2.0;
+        private double _lastSCurveProgress;
+
+        public override IEnumerable<bool> Get()
+        {
+            ValidateParameters();
+
+            var chassis =
+                PilotDefinition.Chassis as MultiWheelChassis;
+            if (chassis == null)
+                throw new InvalidOperationException(
+                    "当前底盘不是MultiWheelChassis，无法执行运动坐标系轨迹测试。");
+
+            var adapter = new MultiWheelChassisAdapter(
+                chassis,
+                PilotDefinition.Self.CarNum);
+
+            var lastCommandTime = DateTime.Now;
+
+            try
+            {
+                // 模式切换阶段只转舵轮，驱动速度始终保持为零。
+                var alignmentStarted = DateTime.Now;
+                DateTime? stableSince = null;
+                while (true)
+                {
+                    if (!adapter.PrepareParallelDirection(
+                            MotionFrameYawInBodyRadians))
+                        throw new InvalidOperationException(
+                            "无法生成运动坐标系对应的舵轮准备姿态。");
+
+                    var aligned =
+                        adapter.AreParallelWheelsAligned(
+                            MotionFrameYawInBodyRadians,
+                            WheelAlignmentToleranceDegrees *
+                            Math.PI / 180.0);
+
+                    if (aligned)
+                    {
+                        if (stableSince == null)
+                            stableSince = DateTime.Now;
+
+                        if ((DateTime.Now - stableSince.Value)
+                                .TotalSeconds >=
+                            WheelAlignmentStableSeconds)
+                            break;
+                    }
+                    else
+                    {
+                        stableSince = null;
+                    }
+
+                    if ((DateTime.Now - alignmentStarted)
+                            .TotalSeconds >
+                        WheelAlignmentTimeoutSeconds)
+                        throw new TimeoutException(
+                            "舵轮在限定时间内未稳定到达运动坐标系初始方向。");
+
+                    yield return true;
+                }
+
+                var trackingStarted = DateTime.Now;
+                while (true)
+                {
+                    if ((DateTime.Now - trackingStarted)
+                            .TotalSeconds >
+                        TrackingTimeoutSeconds)
+                        throw new TimeoutException(
+                            "蟹行轨迹在限定时间内未完成。");
+
+                    var location =
+                        DetourInterface.getCartLocation();
+                    if (!IsFinite(location.x) ||
+                        !IsFinite(location.y) ||
+                        !IsFinite(location.th))
+                        throw new InvalidOperationException(
+                            "蟹行轨迹测试期间Detour位姿无效。");
+
+                    var currentPosition = new Vector2(
+                        (float)location.x,
+                        (float)location.y);
+                    var currentBodyYaw =
+                        location.th * Math.PI / 180.0;
+
+                    CalculateReference(
+                        currentPosition,
+                        out var tangentYaw,
+                        out var referencePoint,
+                        out var remainingMillimeters,
+                        out var referenceCurvature);
+
+                    if (remainingMillimeters <=
+                        FinishDistanceMillimeters)
+                        break;
+
+                    var speed =
+                        CalculateSpeed(remainingMillimeters);
+                    var tangent = new Vector2(
+                        (float)Math.Cos(tangentYaw),
+                        (float)Math.Sin(tangentYaw));
+                    var leftNormal = new Vector2(
+                        -tangent.Y,
+                        tangent.X);
+                    var positionError =
+                        currentPosition - referencePoint;
+                    var lateralErrorMeters =
+                        Vector2.Dot(
+                            positionError,
+                            leftNormal) / 1000.0;
+                    var normalCorrection =
+                        Limit(
+                            -LateralGainPerSecond *
+                            lateralErrorMeters,
+                            MaximumLateralCorrection);
+
+                    // 先在世界坐标中组合切向速度与横向纠偏速度。
+                    var worldVx =
+                        tangent.X * speed +
+                        leftNormal.X * (float)normalCorrection;
+                    var worldVy =
+                        tangent.Y * speed +
+                        leftNormal.Y * (float)normalCorrection;
+
+                    // 将世界速度表达为当前蟹行运动坐标系速度。
+                    var motionYaw =
+                        currentBodyYaw +
+                        MotionFrameYawInBodyRadians;
+                    var motionCos = Math.Cos(motionYaw);
+                    var motionSin = Math.Sin(motionYaw);
+                    var vxInMotion =
+                        motionCos * worldVx +
+                        motionSin * worldVy;
+                    var vyInMotion =
+                        -motionSin * worldVx +
+                        motionCos * worldVy;
+
+                    var desiredBodyYaw =
+                        tangentYaw -
+                        MotionFrameYawInBodyRadians;
+                    var headingError =
+                        FrameTransform2D
+                            .ShortestAngleDifference(
+                                desiredBodyYaw,
+                                currentBodyYaw);
+                    var omega =
+                        speed * referenceCurvature +
+                        HeadingGainPerSecond * headingError;
+                    omega = Limit(
+                        omega,
+                        MaximumAngularSpeedRadiansPerSecond);
+
+                    // 运动坐标系相对车体系旋转+90°：
+                    // 运动系正向速度会转换成车体系+Y速度。
+                    var bodyTwist =
+                        FrameTransform2D
+                            .TransformTwistAtSamePoint(
+                                new Pose2D(
+                                    0.0,
+                                    0.0,
+                                    MotionFrameYawInBodyRadians),
+                                new Twist2D(
+                                    vxInMotion,
+                                    vyInMotion,
+                                    omega));
+
+                    var now = DateTime.Now;
+                    var interval = now - lastCommandTime;
+                    lastCommandTime = now;
+                    var command = new ChassisCommand(
+                        PilotDefinition.Self.CarNum,
+                        bodyTwist);
+
+                    if (!adapter.Send(command, interval))
+                        throw new InvalidOperationException(
+                            "蟹行轨迹底盘解算失败：" +
+                            adapter.LastFailureReason);
+
+                    CommandObserver?.Invoke(
+                        (float)bodyTwist.VxMetersPerSecond,
+                        (float)bodyTwist.VyMetersPerSecond,
+                        (float)bodyTwist
+                            .OmegaRadiansPerSecond);
+
+                    yield return true;
+                }
+            }
+            finally
+            {
+                adapter.StopImmediately();
+                CommandObserver?.Invoke(0f, 0f, 0f);
+            }
+
+            yield return false;
+        }
+
+        // 计算当前点在直线或圆弧上的参考点、切线和剩余距离。
+        private void CalculateReference(
+            Vector2 currentPosition,
+            out double tangentYaw,
+            out Vector2 referencePoint,
+            out float remainingMillimeters,
+            out double curvaturePerMeter)
+        {
+            var initialMotionYaw =
+                InitialBodyYawRadians +
+                MotionFrameYawInBodyRadians;
+
+            if (PathKind == ReferencePathKind.Straight)
+            {
+                var tangent = new Vector2(
+                    (float)Math.Cos(initialMotionYaw),
+                    (float)Math.Sin(initialMotionYaw));
+                var relative = currentPosition - StartPosition;
+                var progress =
+                    Vector2.Dot(relative, tangent);
+                var clampedProgress =
+                    Math.Max(
+                        0f,
+                        Math.Min(progress, LengthMillimeters));
+
+                tangentYaw = initialMotionYaw;
+                referencePoint =
+                    StartPosition +
+                    tangent * clampedProgress;
+                remainingMillimeters =
+                    Math.Max(
+                        0f,
+                        LengthMillimeters - progress);
+                curvaturePerMeter = 0.0;
+                return;
+            }
+
+            if (PathKind == ReferencePathKind.SCurve)
+            {
+                CalculateSCurveReference(
+                    currentPosition,
+                    initialMotionYaw,
+                    out tangentYaw,
+                    out referencePoint,
+                    out remainingMillimeters,
+                    out curvaturePerMeter);
+                return;
+            }
+
+            var center = GetArcCenter();
+            var startRadialYaw =
+                initialMotionYaw - Math.PI / 2.0;
+            var radial = currentPosition - center;
+            var currentRadialYaw =
+                Math.Atan2(radial.Y, radial.X);
+            var progressRadians =
+                FrameTransform2D.NormalizeAngle(
+                    currentRadialYaw - startRadialYaw);
+
+            // 测试圆弧只有+90°，起点附近的轻微负噪声按0处理。
+            if (progressRadians < 0.0)
+                progressRadians = 0.0;
+
+            var clampedProgressRadians =
+                Math.Min(
+                    progressRadians,
+                    ArcSweepRadians);
+            var referenceRadialYaw =
+                startRadialYaw +
+                clampedProgressRadians;
+            referencePoint = center + new Vector2(
+                RadiusMillimeters *
+                    (float)Math.Cos(referenceRadialYaw),
+                RadiusMillimeters *
+                    (float)Math.Sin(referenceRadialYaw));
+            tangentYaw =
+                referenceRadialYaw + Math.PI / 2.0;
+            remainingMillimeters =
+                (float)Math.Max(
+                    0.0,
+                    (ArcSweepRadians - progressRadians) *
+                    RadiusMillimeters);
+            curvaturePerMeter =
+                1000.0 / RadiusMillimeters;
+        }
+
+        // 通过离散最近点和解析导数计算两段三次贝塞尔S曲线的参考状态。
+        private void CalculateSCurveReference(
+            Vector2 currentPosition,
+            double initialMotionYaw,
+            out double tangentYaw,
+            out Vector2 referencePoint,
+            out float remainingMillimeters,
+            out double curvaturePerMeter)
+        {
+            const int nearestPointSamples = 200;
+            var searchStart =
+                Math.Max(
+                    0.0,
+                    _lastSCurveProgress - 0.02);
+            var bestProgress = _lastSCurveProgress;
+            var bestDistanceSquared = double.MaxValue;
+
+            for (var i = 0;
+                 i <= nearestPointSamples;
+                 i++)
+            {
+                var progress =
+                    searchStart +
+                    (1.0 - searchStart) *
+                    i / nearestPointSamples;
+                EvaluateSCurve(
+                    progress,
+                    out var localPoint,
+                    out _,
+                    out _);
+                var worldPoint =
+                    LocalPathPointToWorld(
+                        localPoint,
+                        initialMotionYaw);
+                var distanceSquared =
+                    Vector2.DistanceSquared(
+                        currentPosition,
+                        worldPoint);
+
+                if (distanceSquared <
+                    bestDistanceSquared)
+                {
+                    bestDistanceSquared =
+                        distanceSquared;
+                    bestProgress = progress;
+                }
+            }
+
+            // 轨迹进度不允许因定位噪声倒退，防止控制目标跳回上一段曲线。
+            _lastSCurveProgress =
+                Math.Max(
+                    _lastSCurveProgress,
+                    bestProgress);
+            EvaluateSCurve(
+                _lastSCurveProgress,
+                out var bestLocalPoint,
+                out var firstDerivative,
+                out var secondDerivative);
+            referencePoint =
+                LocalPathPointToWorld(
+                    bestLocalPoint,
+                    initialMotionYaw);
+            tangentYaw =
+                initialMotionYaw +
+                Math.Atan2(
+                    firstDerivative.Y,
+                    firstDerivative.X);
+
+            var derivativeMagnitude =
+                Math.Sqrt(
+                    firstDerivative.X *
+                    firstDerivative.X +
+                    firstDerivative.Y *
+                    firstDerivative.Y);
+            if (derivativeMagnitude < 1e-6)
+            {
+                curvaturePerMeter = 0.0;
+            }
+            else
+            {
+                // 导数单位为mm，乘1000后将曲率从1/mm转换成1/m。
+                curvaturePerMeter =
+                    (firstDerivative.X *
+                     secondDerivative.Y -
+                     firstDerivative.Y *
+                     secondDerivative.X) *
+                    1000.0 /
+                    Math.Pow(
+                        derivativeMagnitude,
+                        3.0);
+            }
+
+            remainingMillimeters =
+                ApproximateSCurveRemainingLength(
+                    _lastSCurveProgress);
+        }
+
+        // 计算与普通4m S型测试完全一致的三段三次贝塞尔完整S曲线。
+        private void EvaluateSCurve(
+            double progress,
+            out Vector2 point,
+            out Vector2 firstDerivative,
+            out Vector2 secondDerivative)
+        {
+            progress =
+                Math.Max(
+                    0.0,
+                    Math.Min(progress, 1.0));
+
+            Vector2 p0;
+            Vector2 p1;
+            Vector2 p2;
+            Vector2 p3;
+            double t;
+
+            if (progress <= 0.25)
+            {
+                t = progress * 4.0;
+                p0 = new Vector2(0f, 0f);
+                p1 = new Vector2(
+                    LengthMillimeters / 12f,
+                    0f);
+                p2 = new Vector2(
+                    LengthMillimeters / 6f,
+                    SCurveLateralOffsetMillimeters);
+                p3 = new Vector2(
+                    LengthMillimeters * 0.25f,
+                    SCurveLateralOffsetMillimeters);
+            }
+            else if (progress <= 0.75)
+            {
+                t = (progress - 0.25) * 2.0;
+                p0 = new Vector2(
+                    LengthMillimeters * 0.25f,
+                    SCurveLateralOffsetMillimeters);
+                p1 = new Vector2(
+                    LengthMillimeters / 3f,
+                    SCurveLateralOffsetMillimeters);
+                p2 = new Vector2(
+                    LengthMillimeters * 2f / 3f,
+                    -SCurveLateralOffsetMillimeters);
+                p3 = new Vector2(
+                    LengthMillimeters * 0.75f,
+                    -SCurveLateralOffsetMillimeters);
+            }
+            else
+            {
+                t = (progress - 0.75) * 4.0;
+                p0 = new Vector2(
+                    LengthMillimeters * 0.75f,
+                    -SCurveLateralOffsetMillimeters);
+                p1 = new Vector2(
+                    LengthMillimeters * 5f / 6f,
+                    -SCurveLateralOffsetMillimeters);
+                p2 = new Vector2(
+                    LengthMillimeters * 11f / 12f,
+                    0f);
+                p3 = new Vector2(
+                    LengthMillimeters,
+                    0f);
+            }
+
+            var oneMinusT = 1.0 - t;
+            point =
+                p0 * (float)(
+                    oneMinusT *
+                    oneMinusT *
+                    oneMinusT) +
+                p1 * (float)(
+                    3.0 *
+                    oneMinusT *
+                    oneMinusT *
+                    t) +
+                p2 * (float)(
+                    3.0 *
+                    oneMinusT *
+                    t *
+                    t) +
+                p3 * (float)(t * t * t);
+            firstDerivative =
+                (p1 - p0) *
+                    (float)(
+                        3.0 *
+                        oneMinusT *
+                        oneMinusT) +
+                (p2 - p1) *
+                    (float)(
+                        6.0 *
+                        oneMinusT *
+                        t) +
+                (p3 - p2) *
+                    (float)(3.0 * t * t);
+            secondDerivative =
+                (p2 - 2f * p1 + p0) *
+                    (float)(6.0 * oneMinusT) +
+                (p3 - 2f * p2 + p1) *
+                    (float)(6.0 * t);
+        }
+
+        // 通过分段采样估算从当前S曲线进度到终点的实际弧长。
+        private float ApproximateSCurveRemainingLength(
+            double startProgress)
+        {
+            const int lengthSamples = 100;
+            EvaluateSCurve(
+                startProgress,
+                out var previousPoint,
+                out _,
+                out _);
+            var length = 0f;
+
+            for (var i = 1;
+                 i <= lengthSamples;
+                 i++)
+            {
+                var progress =
+                    startProgress +
+                    (1.0 - startProgress) *
+                    i / lengthSamples;
+                EvaluateSCurve(
+                    progress,
+                    out var point,
+                    out _,
+                    out _);
+                length +=
+                    Vector2.Distance(
+                        previousPoint,
+                        point);
+                previousPoint = point;
+            }
+
+            return length;
+        }
+
+        // 将以初始蟹行方向为X轴的局部路径点转换到Detour世界坐标。
+        private Vector2 LocalPathPointToWorld(
+            Vector2 localPoint,
+            double initialMotionYaw)
+        {
+            var cos =
+                (float)Math.Cos(initialMotionYaw);
+            var sin =
+                (float)Math.Sin(initialMotionYaw);
+
+            return StartPosition + new Vector2(
+                localPoint.X * cos -
+                localPoint.Y * sin,
+                localPoint.X * sin +
+                localPoint.Y * cos);
+        }
+
+        // 获取蟹行左转圆弧圆心；它位于初始运动方向的左侧。
+        public Vector2 GetArcCenter()
+        {
+            var initialMotionYaw =
+                InitialBodyYawRadians +
+                MotionFrameYawInBodyRadians;
+            return StartPosition + new Vector2(
+                -RadiusMillimeters *
+                    (float)Math.Sin(initialMotionYaw),
+                RadiusMillimeters *
+                    (float)Math.Cos(initialMotionYaw));
+        }
+
+        // 获取圆弧测试的理论终点。
+        public Vector2 GetArcDestination()
+        {
+            var initialMotionYaw =
+                InitialBodyYawRadians +
+                MotionFrameYawInBodyRadians;
+            var startRadialYaw =
+                initialMotionYaw - Math.PI / 2.0;
+            var endRadialYaw =
+                startRadialYaw + ArcSweepRadians;
+            var center = GetArcCenter();
+
+            return center + new Vector2(
+                RadiusMillimeters *
+                    (float)Math.Cos(endRadialYaw),
+                RadiusMillimeters *
+                    (float)Math.Sin(endRadialYaw));
+        }
+
+        // 根据剩余路径长度生成终点减速速度。
+        private float CalculateSpeed(
+            float remainingMillimeters)
+        {
+            if (remainingMillimeters >=
+                SlowDistanceMillimeters)
+                return CruiseSpeed;
+
+            var ratio =
+                remainingMillimeters /
+                Math.Max(
+                    SlowDistanceMillimeters,
+                    1f);
+            return Math.Max(
+                MinimumSpeed,
+                CruiseSpeed * ratio);
+        }
+
+        private void ValidateParameters()
+        {
+            if (CruiseSpeed <= 0f ||
+                !IsFinite(CruiseSpeed) ||
+                LengthMillimeters <= 0f ||
+                !IsFinite(LengthMillimeters) ||
+                RadiusMillimeters <= 0f ||
+                !IsFinite(RadiusMillimeters) ||
+                SCurveLateralOffsetMillimeters <= 0f ||
+                !IsFinite(
+                    SCurveLateralOffsetMillimeters) ||
+                ArcSweepRadians <= 0.0 ||
+                !IsFinite(ArcSweepRadians) ||
+                SlowDistanceMillimeters <= 0f ||
+                !IsFinite(SlowDistanceMillimeters) ||
+                FinishDistanceMillimeters < 0f ||
+                !IsFinite(FinishDistanceMillimeters) ||
+                TrackingTimeoutSeconds <= 0f ||
+                !IsFinite(TrackingTimeoutSeconds))
+                throw new ArgumentOutOfRangeException(
+                    "蟹行轨迹测试参数无效。");
+        }
+
+        private static double Limit(
+            double value,
+            double absoluteLimit)
+        {
+            return Math.Max(
+                -absoluteLimit,
+                Math.Min(value, absoluteLimit));
+        }
+
+        private static bool IsFinite(double value)
+        {
+            return
+                !double.IsNaN(value) &&
+                !double.IsInfinity(value);
+        }
+    }
+}

ClumsyPilot/MovementTests.cs

@@ -7,6 +7,7 @@ using MDCSToolBox.Commons.Controllers;
 using MDCSToolBox.Clumsy.Tracks;
 using MyParking.Shared;
 using System;
+using System.Collections.Generic;
 using System.Numerics;
 using System.Threading;
 
@@ -102,7 +103,7 @@ namespace MultiWheelC
         }
     }
 
-    [MovementTest(name = "测试连续前进4m")]
+    [MovementTest(name = "旧版SendMotion：连续前进4m")]
     public class TestForward4m : MovementTest
     {
         public float DistanceMillimeters = 4000f; // 测试距离，单位mm。
@@ -138,8 +139,8 @@ namespace MultiWheelC
                 source.Y + DistanceMillimeters * (float)Math.Sin(headingRadians));
             _recorder =
                         new TrackingExperimentRecorder(
-                            controllerName: "Stanley",
-                            trajectoryName: "Straight4m",
+                            controllerName: "LegacyGeometricController",
+                            trajectoryName: "LegacyStraight4m",
                             trialNumber: TrialNumber,
                             referenceStart: source,
                             referenceEnd: destination,
@@ -225,8 +226,10 @@ namespace MultiWheelC
                 trialNumber: TrialNumber,
                 referenceStart: rotationCenter,
                 referenceEnd: rotationCenter,
-                referenceSpeed:
-                    MaxAngularSpeedDegreesPerSecond);
+                referenceSpeed: 0f,
+                referenceAngularSpeed:
+                    MaxAngularSpeedDegreesPerSecond *
+                    (float)Math.PI / 180f);
             _recorder.Start();
 
             try
@@ -258,7 +261,8 @@ namespace MultiWheelC
                             commandAngularSpeed =>
                                 _recorder?.UpdateCommand(
                                     0f,
-                                    commandAngularSpeed)
+                                    commandAngularSpeed *
+                                    (float)Math.PI / 180f)
                     }.Get());
 
                 _task.Wait();
@@ -293,7 +297,7 @@ namespace MultiWheelC
         }
     }
 
-    [MovementTest(name = "测试左转90°圆弧")]
+    [MovementTest(name = "旧版SendMotion：左转90°圆弧")]
     public class TestArcMovement : MovementTest
     {
         public float RadiusMillimeters = 2000f; // 左转圆的半径，单位mm。
@@ -370,6 +374,13 @@ namespace MultiWheelC
                 CarDirectionBias = 0f
             };
 
+            // 左转90°后，圆心到终点的径向方向等于起始车头方向。
+            var destination = center + new Vector2(
+                RadiusMillimeters *
+                    (float)Math.Cos(headingRadians),
+                RadiusMillimeters *
+                    (float)Math.Sin(headingRadians));
+
             if (!controller.AddTrack(arc, "LeftArc90Degrees"))
             {
                 Console.WriteLine(
@@ -378,12 +389,12 @@ namespace MultiWheelC
             }
 
             _recorder = new TrackingExperimentRecorder(
-                controllerName: "GeometricController",
+                controllerName: "LegacyGeometricController",
                 trajectoryName:
-                    $"LeftArc90_R{RadiusMillimeters:0}mm",
+                    $"LegacyLeftArc90_R{RadiusMillimeters:0}mm",
                 trialNumber: TrialNumber,
                 referenceStart: source,
-                referenceEnd: source,
+                referenceEnd: destination,
                 referenceSpeed: CruiseSpeed);
             _recorder.Start();
 
@@ -414,6 +425,723 @@ namespace MultiWheelC
         }
     }
 
+    [MovementTest(name = "测试蟹行前进4m")]
+    public class TestCrabForward4m : MovementTest
+    {
+        public float DistanceMillimeters = 4000f;
+        public float CruiseSpeed = 0.2f;
+        public int TrialNumber = 1;
+
+        private DriveTask _task;
+        private TrackingExperimentRecorder _recorder;
+
+        // 将车体左侧作为运动前向，沿直线蟹行4m并记录Detour实验数据。
+        public override void Test()
+        {
+            if (!TryReadStartPose(
+                    out var source,
+                    out var bodyYawRadians))
+                return;
+
+            var motionYaw =
+                bodyYawRadians + Math.PI / 2.0;
+            var destination = new Vector2(
+                source.X +
+                    DistanceMillimeters *
+                    (float)Math.Cos(motionYaw),
+                source.Y +
+                    DistanceMillimeters *
+                    (float)Math.Sin(motionYaw));
+
+            var tracker = new CrabMotionFrameTracker
+            {
+                PathKind =
+                    CrabMotionFrameTracker
+                        .ReferencePathKind.Straight,
+                StartPosition = source,
+                InitialBodyYawRadians =
+                    bodyYawRadians,
+                LengthMillimeters =
+                    DistanceMillimeters,
+                CruiseSpeed = CruiseSpeed
+            };
+
+            _recorder = new TrackingExperimentRecorder(
+                controllerName:
+                    "CrabMotionFrameTracker",
+                trajectoryName:
+                    "CrabStraight4m",
+                trialNumber: TrialNumber,
+                referenceStart: source,
+                referenceEnd: destination,
+                referenceSpeed: CruiseSpeed,
+                referenceMotionFrameYawDegrees: 90f);
+            tracker.CommandObserver =
+                (vx, vy, omega) =>
+                    _recorder?.UpdateBodyCommand(
+                        vx,
+                        vy,
+                        omega);
+            _recorder.Start();
+
+            try
+            {
+                _task = new DriveTask(tracker.Get());
+                _task.Wait();
+                Thread.Sleep(300);
+            }
+            finally
+            {
+                _task?.Stop();
+                _recorder?.UpdateBodyCommand(
+                    0f,
+                    0f,
+                    0f);
+                _recorder?.StopAndSave();
+                _task = null;
+                _recorder = null;
+            }
+        }
+
+        public override void TestStop()
+        {
+            _task?.Stop();
+            _recorder?.UpdateBodyCommand(
+                0f,
+                0f,
+                0f);
+            _recorder?.StopAndSave();
+        }
+
+        // 读取并校验测试开始时的Detour世界位姿。
+        private static bool TryReadStartPose(
+            out Vector2 source,
+            out double bodyYawRadians)
+        {
+            var location =
+                DetourInterface.getCartLocation();
+            if (double.IsNaN(location.x) ||
+                double.IsInfinity(location.x) ||
+                double.IsNaN(location.y) ||
+                double.IsInfinity(location.y) ||
+                double.IsNaN(location.th) ||
+                double.IsInfinity(location.th))
+            {
+                Console.WriteLine(
+                    "Detour当前位姿无效，取消蟹行直线测试。");
+                source = Vector2.Zero;
+                bodyYawRadians = 0.0;
+                return false;
+            }
+
+            source = new Vector2(
+                (float)location.x,
+                (float)location.y);
+            bodyYawRadians =
+                location.th * Math.PI / 180.0;
+            return true;
+        }
+    }
+
+    [MovementTest(name = "测试蟹行左转90°圆弧")]
+    public class TestCrabLeftArc90 : MovementTest
+    {
+        public float RadiusMillimeters = 2000f;
+        public float CruiseSpeed = 0.2f;
+        public int TrialNumber = 1;
+
+        private DriveTask _task;
+        private TrackingExperimentRecorder _recorder;
+
+        // 将车体左侧作为运动前向，沿半径2m的左转圆弧运动90°。
+        public override void Test()
+        {
+            var location =
+                DetourInterface.getCartLocation();
+            if (double.IsNaN(location.x) ||
+                double.IsInfinity(location.x) ||
+                double.IsNaN(location.y) ||
+                double.IsInfinity(location.y) ||
+                double.IsNaN(location.th) ||
+                double.IsInfinity(location.th))
+            {
+                Console.WriteLine(
+                    "Detour当前位姿无效，取消蟹行圆弧测试。");
+                return;
+            }
+
+            var source = new Vector2(
+                (float)location.x,
+                (float)location.y);
+            var bodyYawRadians =
+                location.th * Math.PI / 180.0;
+            var tracker = new CrabMotionFrameTracker
+            {
+                PathKind =
+                    CrabMotionFrameTracker
+                        .ReferencePathKind.LeftArc,
+                StartPosition = source,
+                InitialBodyYawRadians =
+                    bodyYawRadians,
+                RadiusMillimeters =
+                    RadiusMillimeters,
+                ArcSweepRadians = Math.PI / 2.0,
+                CruiseSpeed = CruiseSpeed
+            };
+            var destination =
+                tracker.GetArcDestination();
+
+            _recorder = new TrackingExperimentRecorder(
+                controllerName:
+                    "CrabMotionFrameTracker",
+                trajectoryName:
+                    $"CrabLeftArc90_R{RadiusMillimeters:0}mm",
+                trialNumber: TrialNumber,
+                referenceStart: source,
+                referenceEnd: destination,
+                referenceSpeed: CruiseSpeed,
+                referenceMotionFrameYawDegrees: 90f);
+            tracker.CommandObserver =
+                (vx, vy, omega) =>
+                    _recorder?.UpdateBodyCommand(
+                        vx,
+                        vy,
+                        omega);
+            _recorder.Start();
+
+            try
+            {
+                _task = new DriveTask(tracker.Get());
+                _task.Wait();
+                Thread.Sleep(300);
+            }
+            finally
+            {
+                _task?.Stop();
+                _recorder?.UpdateBodyCommand(
+                    0f,
+                    0f,
+                    0f);
+                _recorder?.StopAndSave();
+                _task = null;
+                _recorder = null;
+            }
+        }
+
+        public override void TestStop()
+        {
+            _task?.Stop();
+            _recorder?.UpdateBodyCommand(
+                0f,
+                0f,
+                0f);
+            _recorder?.StopAndSave();
+        }
+    }
+
+    [MovementTest(name = "测试蟹行4m S型曲线")]
+    public class TestCrabSCurve4m : MovementTest
+    {
+        public float LengthMillimeters = 4000f;
+        public float LateralOffsetMillimeters = 400f;
+        public float CruiseSpeed = 0.2f;
+        public int TrialNumber = 1;
+
+        private DriveTask _task;
+        private TrackingExperimentRecorder _recorder;
+
+        // 将车体左侧作为运动前向，跟踪与普通测试参数一致的4m S型曲线。
+        public override void Test()
+        {
+            if (float.IsNaN(LengthMillimeters) ||
+                float.IsInfinity(LengthMillimeters) ||
+                LengthMillimeters <= 0f ||
+                float.IsNaN(
+                    LateralOffsetMillimeters) ||
+                float.IsInfinity(
+                    LateralOffsetMillimeters) ||
+                LateralOffsetMillimeters <= 0f ||
+                float.IsNaN(CruiseSpeed) ||
+                float.IsInfinity(CruiseSpeed) ||
+                CruiseSpeed <= 0f)
+            {
+                Console.WriteLine(
+                    "蟹行S型曲线测试参数无效。");
+                return;
+            }
+
+            var location =
+                DetourInterface.getCartLocation();
+            if (double.IsNaN(location.x) ||
+                double.IsInfinity(location.x) ||
+                double.IsNaN(location.y) ||
+                double.IsInfinity(location.y) ||
+                double.IsNaN(location.th) ||
+                double.IsInfinity(location.th))
+            {
+                Console.WriteLine(
+                    "Detour当前位姿无效，取消蟹行S型曲线测试。");
+                return;
+            }
+
+            var source = new Vector2(
+                (float)location.x,
+                (float)location.y);
+            var bodyYawRadians =
+                location.th * Math.PI / 180.0;
+            var initialMotionYaw =
+                bodyYawRadians + Math.PI / 2.0;
+            var destination = new Vector2(
+                source.X +
+                    LengthMillimeters *
+                    (float)Math.Cos(
+                        initialMotionYaw),
+                source.Y +
+                    LengthMillimeters *
+                    (float)Math.Sin(
+                        initialMotionYaw));
+            var tracker = new CrabMotionFrameTracker
+            {
+                PathKind =
+                    CrabMotionFrameTracker
+                        .ReferencePathKind.SCurve,
+                StartPosition = source,
+                InitialBodyYawRadians =
+                    bodyYawRadians,
+                LengthMillimeters =
+                    LengthMillimeters,
+                SCurveLateralOffsetMillimeters =
+                    LateralOffsetMillimeters,
+                CruiseSpeed = CruiseSpeed
+            };
+
+            _recorder = new TrackingExperimentRecorder(
+                controllerName:
+                    "CrabMotionFrameTracker",
+                trajectoryName:
+                    $"CrabSCurve4m_A{LateralOffsetMillimeters:0}mm",
+                trialNumber: TrialNumber,
+                referenceStart: source,
+                referenceEnd: destination,
+                referenceSpeed: CruiseSpeed,
+                referenceMotionFrameYawDegrees: 90f);
+            tracker.CommandObserver =
+                (vx, vy, omega) =>
+                    _recorder?.UpdateBodyCommand(
+                        vx,
+                        vy,
+                        omega);
+            _recorder.Start();
+
+            try
+            {
+                _task = new DriveTask(tracker.Get());
+                _task.Wait();
+                Thread.Sleep(300);
+            }
+            finally
+            {
+                _task?.Stop();
+                _recorder?.UpdateBodyCommand(
+                    0f,
+                    0f,
+                    0f);
+                _recorder?.StopAndSave();
+                _task = null;
+                _recorder = null;
+            }
+        }
+
+        public override void TestStop()
+        {
+            _task?.Stop();
+            _recorder?.UpdateBodyCommand(
+                0f,
+                0f,
+                0f);
+            _recorder?.StopAndSave();
+        }
+    }
+
+    [MovementTest(name = "旧版SendMotion：4m S型曲线")]
+    public class TestSCurve4m : MovementTest
+    {
+        public float LengthMillimeters = 4000f; // S型曲线纵向长度，单位mm。
+        public float LateralOffsetMillimeters = 400f; // S型曲线左右两侧的最大偏移，单位mm。
+        public float CruiseSpeed = 0.3f; // 首次实车测试建议使用0.3m/s。
+        public int TrialNumber = 1; // 重复实验编号。
+
+        private DriveTask _task;
+        private TrackingExperimentRecorder _recorder;
+
+        // 从当前Detour位姿开始，沿车头方向跟踪先左偏、再右偏并最终回中的完整S型曲线。
+        public override void Test()
+        {
+            if (float.IsNaN(LengthMillimeters) ||
+                float.IsInfinity(LengthMillimeters) ||
+                LengthMillimeters <= 0f ||
+                float.IsNaN(LateralOffsetMillimeters) ||
+                float.IsInfinity(LateralOffsetMillimeters) ||
+                LateralOffsetMillimeters <= 0f ||
+                float.IsNaN(CruiseSpeed) ||
+                float.IsInfinity(CruiseSpeed) ||
+                CruiseSpeed <= 0f)
+            {
+                Console.WriteLine("S型曲线测试参数无效。");
+                return;
+            }
+
+            if (!MovementTestPreparation.AreWheelsForward())
+                return;
+
+            var location = DetourInterface.getCartLocation();
+            if (double.IsNaN(location.x) ||
+                double.IsInfinity(location.x) ||
+                double.IsNaN(location.y) ||
+                double.IsInfinity(location.y) ||
+                double.IsNaN(location.th) ||
+                double.IsInfinity(location.th))
+            {
+                Console.WriteLine(
+                    "Detour当前位姿无效，取消4m S型曲线测试。");
+                return;
+            }
+
+            var source =
+                new Vector2((float)location.x, (float)location.y);
+            var headingRadians =
+                location.th * Math.PI / 180.0;
+            var length = LengthMillimeters;
+            var offset = LateralOffsetMillimeters;
+
+            // 三段三次贝塞尔依次经过左侧峰值、中心线和右侧峰值，
+            // 起点、两个峰值和终点的切线均沿初始前向，连接处没有折角。
+            var firstControlPoints = new List<Vector2>
+            {
+                LocalToWorld(source, headingRadians, 0f, 0f),
+                LocalToWorld(
+                    source, headingRadians,
+                    length / 12f, 0f),
+                LocalToWorld(
+                    source, headingRadians,
+                    length / 6f, offset),
+                LocalToWorld(
+                    source, headingRadians,
+                    length * 0.25f, offset)
+            };
+            var secondControlPoints = new List<Vector2>
+            {
+                LocalToWorld(
+                    source, headingRadians,
+                    length * 0.25f, offset),
+                LocalToWorld(
+                    source, headingRadians,
+                    length / 3f, offset),
+                LocalToWorld(
+                    source, headingRadians,
+                    length * 2f / 3f, -offset),
+                LocalToWorld(
+                    source, headingRadians,
+                    length * 0.75f, -offset)
+            };
+            var thirdControlPoints = new List<Vector2>
+            {
+                LocalToWorld(
+                    source, headingRadians,
+                    length * 0.75f, -offset),
+                LocalToWorld(
+                    source, headingRadians,
+                    length * 5f / 6f, -offset),
+                LocalToWorld(
+                    source, headingRadians,
+                    length * 11f / 12f, 0f),
+                LocalToWorld(
+                    source, headingRadians,
+                    length, 0f)
+            };
+
+            var firstTrack = new BezierTrack(firstControlPoints)
+            {
+                Speed = CruiseSpeed,
+                CarDirectionBias = 0f
+            };
+            var secondTrack = new BezierTrack(secondControlPoints)
+            {
+                Speed = CruiseSpeed,
+                CarDirectionBias = 0f
+            };
+            var thirdTrack = new BezierTrack(thirdControlPoints)
+            {
+                Speed = CruiseSpeed,
+                CarDirectionBias = 0f
+            };
+
+            var controller = new ChassisController
+            {
+                BaseSpeed = CruiseSpeed
+            }.Get();
+            controller.FinishSpeed = 0f;
+
+            if (!controller.AddTrack(
+                    firstTrack,
+                    "SCurve4m-Part1") ||
+                !controller.AddTrack(
+                    secondTrack,
+                    "SCurve4m-Part2") ||
+                !controller.AddTrack(
+                    thirdTrack,
+                    "SCurve4m-Part3"))
+            {
+                Console.WriteLine(
+                    "4m S型曲线轨迹添加失败，取消测试。");
+                return;
+            }
+
+            var destination =
+                LocalToWorld(
+                    source,
+                    headingRadians,
+                    length,
+                    0f);
+            _recorder = new TrackingExperimentRecorder(
+                controllerName: "LegacyGeometricController",
+                trajectoryName:
+                    $"LegacySCurve4m_A{LateralOffsetMillimeters:0}mm",
+                trialNumber: TrialNumber,
+                referenceStart: source,
+                referenceEnd: destination,
+                referenceSpeed: CruiseSpeed);
+            _recorder.Start();
+
+            try
+            {
+                _task = new DriveTask(controller.Track());
+                _task.Wait();
+
+                // 保留少量停止后的数据，用于观察速度是否回到零。
+                Thread.Sleep(300);
+            }
+            finally
+            {
+                _task?.Stop();
+                _recorder?.UpdateCommand(0f, 0f);
+                _recorder?.StopAndSave();
+                _task = null;
+                _recorder = null;
+            }
+        }
+
+        // 停止S型曲线测试并保存当前已经采集的数据。
+        public override void TestStop()
+        {
+            _task?.Stop();
+            _recorder?.UpdateCommand(0f, 0f);
+            _recorder?.StopAndSave();
+        }
+
+        // 将车体起点局部坐标转换为Detour世界坐标，X向前、Y向左。
+        private static Vector2 LocalToWorld(
+            Vector2 origin,
+            double headingRadians,
+            float localX,
+            float localY)
+        {
+            var cos = (float)Math.Cos(headingRadians);
+            var sin = (float)Math.Sin(headingRadians);
+
+            return new Vector2(
+                origin.X + localX * cos - localY * sin,
+                origin.Y + localX * sin + localY * cos);
+        }
+    }
+
+    // C层单车测试：统一使用车体速度命令和SendXYThSpeed跟踪普通模式轨迹。
+    public abstract class XYThNormalTrajectoryTestBase : MovementTest
+    {
+        public float LengthMillimeters = 4000f;
+        public float RadiusMillimeters = 2000f;
+        public float LateralOffsetMillimeters = 400f;
+        public float CruiseSpeed = 0.3f;
+        public int TrialNumber = 1;
+
+        private DriveTask _task;
+        private TrackingExperimentRecorder _recorder;
+
+        protected abstract CrabMotionFrameTracker.ReferencePathKind ReferencePath { get; }
+
+        protected abstract string TrajectoryName { get; }
+
+        // C层单车测试：读取Detour起点并执行普通模式SendXYThSpeed轨迹。
+        public override void Test()
+        {
+            ValidateParameters();
+
+            if (!TryReadDetourPose(
+                    out var source,
+                    out var initialBodyYawRadians))
+            {
+                throw new InvalidOperationException(
+                    "Detour当前位置或航向无效，无法开始新版SendXYThSpeed测试。");
+            }
+
+            var tracker = new CrabMotionFrameTracker
+            {
+                PathKind = ReferencePath,
+                // 普通模式的运动坐标系与车体坐标系重合。
+                MotionFrameYawInBodyRadians = 0.0,
+                StartPosition = source,
+                InitialBodyYawRadians = initialBodyYawRadians,
+                LengthMillimeters = LengthMillimeters,
+                RadiusMillimeters = RadiusMillimeters,
+                ArcSweepRadians = Math.PI / 2.0,
+                SCurveLateralOffsetMillimeters =
+                    LateralOffsetMillimeters,
+                CruiseSpeed = CruiseSpeed,
+            };
+
+            var destination = ReferencePath ==
+                              CrabMotionFrameTracker.ReferencePathKind
+                                  .LeftArc
+                ? tracker.GetArcDestination()
+                : new Vector2(
+                    source.X +
+                    LengthMillimeters *
+                    (float)Math.Cos(initialBodyYawRadians),
+                    source.Y +
+                    LengthMillimeters *
+                    (float)Math.Sin(initialBodyYawRadians));
+
+            _recorder = new TrackingExperimentRecorder(
+                controllerName: "UnifiedXYThTracker",
+                trajectoryName: TrajectoryName,
+                trialNumber: TrialNumber,
+                referenceStart: source,
+                referenceEnd: destination,
+                referenceSpeed: CruiseSpeed);
+
+            tracker.CommandObserver =
+                (vx, vy, omegaRadiansPerSecond) =>
+                    _recorder?.UpdateBodyCommand(
+                        vx,
+                        vy,
+                        omegaRadiansPerSecond);
+
+            _recorder.Start();
+            _task = new DriveTask(tracker.Get());
+
+            try
+            {
+                _task.Wait();
+            }
+            finally
+            {
+                _recorder?.UpdateBodyCommand(0f, 0f, 0f);
+                _recorder?.StopAndSave();
+                _recorder = null;
+                _task = null;
+            }
+        }
+
+        // C层单车测试：停止新版SendXYThSpeed轨迹并保存已有记录。
+        public override void TestStop()
+        {
+            _task?.Stop();
+            _recorder?.UpdateBodyCommand(0f, 0f, 0f);
+            _recorder?.StopAndSave();
+        }
+
+        // C层单车测试：检查新版轨迹的长度、半径、偏移和速度参数。
+        private void ValidateParameters()
+        {
+            if (!IsPositiveFinite(LengthMillimeters))
+                throw new ArgumentOutOfRangeException(
+                    nameof(LengthMillimeters),
+                    "轨迹长度必须是正有限值。");
+
+            if (!IsPositiveFinite(RadiusMillimeters))
+                throw new ArgumentOutOfRangeException(
+                    nameof(RadiusMillimeters),
+                    "圆弧半径必须是正有限值。");
+
+            if (!IsPositiveFinite(LateralOffsetMillimeters))
+                throw new ArgumentOutOfRangeException(
+                    nameof(LateralOffsetMillimeters),
+                    "S型曲线横向偏移必须是正有限值。");
+
+            if (!IsPositiveFinite(CruiseSpeed))
+                throw new ArgumentOutOfRangeException(
+                    nameof(CruiseSpeed),
+                    "巡航速度必须是正有限值。");
+        }
+
+        // C层单车测试：读取并验证Detour毫米坐标和角度制航向。
+        private static bool TryReadDetourPose(
+            out Vector2 position,
+            out double yawRadians)
+        {
+            var location = DetourInterface.getCartLocation();
+            var x = location.x;
+            var y = location.y;
+            var thetaDegrees = location.th;
+
+            position = new Vector2((float)x, (float)y);
+            yawRadians = thetaDegrees * Math.PI / 180.0;
+
+            return IsFinite(x) &&
+                   IsFinite(y) &&
+                   IsFinite(thetaDegrees);
+        }
+
+        // C层单车测试：判断浮点参数是否为有限值。
+        private static bool IsFinite(double value)
+        {
+            return !double.IsNaN(value) &&
+                   !double.IsInfinity(value);
+        }
+
+        // C层单车测试：判断浮点参数是否为正有限值。
+        private static bool IsPositiveFinite(double value)
+        {
+            return IsFinite(value) && value > 0.0;
+        }
+    }
+
+    [MovementTest(name = "新版SendXYThSpeed：连续前进4m")]
+    public sealed class TestXYThForward4m :
+        XYThNormalTrajectoryTestBase
+    {
+        protected override CrabMotionFrameTracker.ReferencePathKind
+            ReferencePath =>
+            CrabMotionFrameTracker.ReferencePathKind.Straight;
+
+        protected override string TrajectoryName =>
+            "XYThStraight4m";
+    }
+
+    [MovementTest(name = "新版SendXYThSpeed：左转90°圆弧")]
+    public sealed class TestXYThLeftArc90 :
+        XYThNormalTrajectoryTestBase
+    {
+        protected override CrabMotionFrameTracker.ReferencePathKind
+            ReferencePath =>
+            CrabMotionFrameTracker.ReferencePathKind.LeftArc;
+
+        protected override string TrajectoryName =>
+            $"XYThLeftArc90_R{RadiusMillimeters:0}mm";
+    }
+
+    [MovementTest(name = "新版SendXYThSpeed：4m S型曲线")]
+    public sealed class TestXYThSCurve4m :
+        XYThNormalTrajectoryTestBase
+    {
+        protected override CrabMotionFrameTracker.ReferencePathKind
+            ReferencePath =>
+            CrabMotionFrameTracker.ReferencePathKind.SCurve;
+
+        protected override string TrajectoryName =>
+            $"XYThSCurve4m_A{LateralOffsetMillimeters:0}mm";
+    }
+
     public abstract class ClampMovementTestBase : MovementTest
     {
         public float TimeoutSeconds = 30f; // 动作超时时间，单位s。

ClumsyPilot/Movements.cs

@@ -251,7 +251,7 @@ namespace MultiWheelC
             finally
             {
                 task?.Stop();
-                chassis.SendXYThSpeed(0f, 0f, 0f);
+                chassis.PredefinedDriveStop();
             }
         }
 
@@ -458,7 +458,12 @@ namespace MultiWheelC
                     var s = thPid.GetResponse(targetAngle, true);
                     Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
                     CommandAngularSpeedObserver?.Invoke(s);
-                    Chassis.SendXYThSpeed(0, 0, s);
+                    if (!Chassis.SendRotateMotion(s))
+                    {
+                        throw new InvalidOperationException(
+                            "原地旋转底盘解算失败：" +
+                            Chassis.LastMotionDecomposeFailureReason);
+                    }
                     if (thPid.IsArrived()) break;
                     yield return true;
                 }
@@ -468,7 +473,7 @@ namespace MultiWheelC
             finally
             {
                 CommandAngularSpeedObserver?.Invoke(0f);
-                Chassis.SendXYThSpeed(0, 0, 0);
+                Chassis.PredefinedDriveStop();
             }
         }
     }

ClumsyPilot/PilotConfig.cs

@@ -39,9 +39,11 @@ public class PilotConfig : MultiWheelPilotConfig
     #region 单车-临时
     [FieldMember(desc = "原地旋转Kp")]
     public float InPlaceRotateKp = 0.2f;
+    // public float InPlaceRotateKp = 0.2f;
 
     [FieldMember(desc = "原地旋转Ki")]
     public float InPlaceRotateKi = 0.01f;
+    // public float InPlaceRotateKi = 0.01f;
 
     [FieldMember(desc = "原地旋转Kd")]
     public float InPlaceRotateKd = 0f;

ClumsyPilot/TrackingExperimentRecorder.cs

@@ -20,7 +20,7 @@ namespace MultiWheelC
         public double DetourY;
         public double DetourTheta;
 
-        // 车体速度单位为m/s，角速度单位为deg/s。
+        // 车体速度单位为m/s，角速度统一使用rad/s。
         public float CommandSpeed;
         public float CommandVx;
         public float CommandVy;
@@ -36,6 +36,8 @@ namespace MultiWheelC
         private readonly Vector2 _referenceStart;
         private readonly Vector2 _referenceEnd;
         private readonly float _referenceSpeed;
+        private readonly float _referenceAngularSpeed;
+        private readonly float _referenceMotionFrameYawDegrees;
         private readonly int _sampleIntervalMs;
 
         private readonly List<TrackingSample> _samples =
@@ -68,7 +70,9 @@ namespace MultiWheelC
             Vector2 referenceStart,
             Vector2 referenceEnd,
             float referenceSpeed,
-            int sampleIntervalMs = 50)
+            float referenceAngularSpeed = 0f,
+            int sampleIntervalMs = 50,
+            float referenceMotionFrameYawDegrees = 0f)
         {
             if (string.IsNullOrWhiteSpace(controllerName))
                 throw new ArgumentException(
@@ -91,6 +95,9 @@ namespace MultiWheelC
             _referenceStart = referenceStart;
             _referenceEnd = referenceEnd;
             _referenceSpeed = referenceSpeed;
+            _referenceAngularSpeed = referenceAngularSpeed;
+            _referenceMotionFrameYawDegrees =
+                referenceMotionFrameYawDegrees;
             _sampleIntervalMs = sampleIntervalMs;
         }
 
@@ -238,7 +245,11 @@ namespace MultiWheelC
 
                         commandVx = command.Vx;
                         commandVy = command.Vy;
-                        commandAngularSpeed = command.Vw;
+                        // CommonUsage.GetCarSpeed().Vw的单位为deg/s，
+                        // 记录器内部统一转换为rad/s。
+                        commandAngularSpeed =
+                            command.Vw *
+                            (float)Math.PI / 180f;
                         commandSpeed = (float)Math.Sqrt(
                             commandVx * commandVx +
                             commandVy * commandVy);
@@ -313,14 +324,18 @@ namespace MultiWheelC
                     "DetourY," +
                     "DetourTheta," +
                     "CommandSpeed," +
+                    // 保留旧列（deg/s）供历史Python脚本兼容。
                     "CommandAngularSpeed," +
+                    "CommandAngularSpeedRadPerSecond," +
                     "CommandVx," +
                     "CommandVy," +
                     "ReferenceStartX," +
                     "ReferenceStartY," +
                     "ReferenceEndX," +
                     "ReferenceEndY," +
-                    "ReferenceSpeed");
+                    "ReferenceSpeed," +
+                    "ReferenceAngularSpeedRadPerSecond," +
+                    "ReferenceMotionFrameYawDegrees");
 
                 foreach (var sample in snapshot)
                 {
@@ -335,6 +350,9 @@ namespace MultiWheelC
                         Format(sample.DetourY),
                         Format(sample.DetourTheta),
                         Format(sample.CommandSpeed),
+                        Format(
+                            sample.CommandAngularSpeed *
+                            180.0 / Math.PI),
                         Format(sample.CommandAngularSpeed),
                         Format(sample.CommandVx),
                         Format(sample.CommandVy),
@@ -342,7 +360,9 @@ namespace MultiWheelC
                         Format(_referenceStart.Y),
                         Format(_referenceEnd.X),
                         Format(_referenceEnd.Y),
-                        Format(_referenceSpeed)));
+                        Format(_referenceSpeed),
+                        Format(_referenceAngularSpeed),
+                        Format(_referenceMotionFrameYawDegrees)));
                 }
             }
         }

ClumsyPilot/obj/Debug/ClumsyPilot.AssemblyInfo.cs

@@ -1,9 +1,10 @@
 //------------------------------------------------------------------------------
 // <auto-generated>
-//     This code was generated by a tool.
+//     此代码由工具生成。
+//     运行时版本:4.0.30319.42000
 //
-//     Changes to this file may cause incorrect behavior and will be lost if
-//     the code is regenerated.
+//     对此文件的更改可能会导致不正确的行为，并且如果
+//     重新生成代码，这些更改将会丢失。
 // </auto-generated>
 //------------------------------------------------------------------------------
 

ClumsyPilot/obj/Debug/ClumsyPilot.csproj.CoreCompileInputs.cache

@@ -1 +1 @@
-e972a413d047c4137a8ce86cbff54a8d2e2558806d9d974d3d6312467ee8ba4d
+1093d2ea159af831cb6cf39a28abbec1d032f5760b7f90d76a2cda9dbd80e1a4

CommonUsage-MultiVehicleSync/commonusage/Chassis/MultiWheelChassis.cs

@@ -693,6 +693,89 @@ namespace CommonUsage.Chassis
             return true;
         }
 
+		/// <summary>
+		/// 停车并将四个舵轮转到绕当前坐标原点自转所需的切线方向。
+		/// 只下发舵角，不下发驱动速度。
+		/// </summary>
+		public bool PrepareRotateWheels(float alignmentToleranceDegrees = 2.0f)
+		{
+			if (!Valid)
+				return FailMotionDecomposition(
+					"PrepareRotateWheels",
+					"invalid chassis",
+					null);
+
+			if (float.IsNaN(alignmentToleranceDegrees) ||
+			    float.IsInfinity(alignmentToleranceDegrees) ||
+			    alignmentToleranceDegrees < 0.0f)
+				throw new ArgumentOutOfRangeException(
+					nameof(alignmentToleranceDegrees),
+					"自转舵轮到位容差必须是非负有限值。");
+
+			if (_steerWheels.Count == 0)
+				return FailMotionDecomposition(
+					"PrepareRotateWheels",
+					"no steer wheels",
+					null);
+
+			// 模式切换期间必须保持驱动轮停止。
+			PredefinedDriveStop();
+
+			var targetAngles = new float[_steerWheels.Count];
+			var directions = new int[_steerWheels.Count];
+
+			// 先完成全部舵角解算，再统一下发，避免只转动部分舵轮。
+			for (var i = 0; i < _steerWheels.Count; i++)
+			{
+				var wheel = _steerWheels[i];
+				var px = (double)wheel.Position.X;
+				var py = (double)wheel.Position.Y;
+
+				// 逆时针绕原点旋转时，该舵轮的切向方向为(-py, px)。
+				var tangentDegrees =
+					(float)(Math.Atan2(px, -py) /
+					        Math.PI * 180.0);
+				tangentDegrees = CommonMath.ThDiff(
+					tangentDegrees,
+					wheel.ZeroDirection);
+
+				if (!TryResolveWheelAngle(
+					    i,
+					    tangentDegrees,
+					    "PrepareRotateWheels",
+					    out targetAngles[i],
+					    out directions[i],
+					    out var reason))
+					return FailMotionDecomposition(
+						"PrepareRotateWheels",
+						reason,
+						null);
+			}
+
+			for (var i = 0; i < _steerWheels.Count; i++)
+			{
+				_wheelDirs[i] = directions[i];
+				SendTh(i, targetAngles[i]);
+			}
+
+			var allAligned = true;
+			for (var i = 0; i < _steerWheels.Count; i++)
+			{
+				var actualAngle = _steerWheels[i].ReadAngle();
+				var angleError = targetAngles[i] - actualAngle;
+
+				// 这里比较受机械限位约束的真实舵角，不能使用圆周最短角度差。
+				if (float.IsNaN(actualAngle) ||
+				    float.IsInfinity(actualAngle) ||
+				    Math.Abs(angleError) > alignmentToleranceDegrees)
+					allAligned = false;
+			}
+
+			LastRotateAligned = allAligned;
+			LastMotionDecomposeFailureReason = "";
+			return true;
+		}
+
 		/// <summary>
 		/// 绕"已被 SetOriginBias 偏置到车队中心的原点"做原地旋转，可叠加一个车体系小幅纠偏旋量。
 		/// </summary>
@@ -781,14 +864,19 @@ namespace CommonUsage.Chassis
             var maxDth = 0f;
             for (var i = 0; i < _steerWheels.Count; ++i)
             {
-                var dth = Math.Abs(CommonMath.ThDiff(ths[i], _steerWheels[i].ReadAngle()));
+                var actualAngle = _steerWheels[i].ReadAngle();
+                var dth = Math.Abs(ths[i] - actualAngle);
                 maxDth = Math.Max(maxDth, dth);
-                slowFac = Math.Min(slowFac, CommonMath.gaussmf(dth, rotSync, 0));
-     //            if (dth > 5)
-     //            {
-					// allWheelAligned = false;
-					// break;
-     //            }
+                slowFac = Math.Min(
+                    slowFac,
+                    CommonMath.gaussmf(
+                        dth,
+                        Math.Max(SteeringAlignmentSigmaDegrees, 0.1f),
+                        0));
+                if (float.IsNaN(actualAngle) ||
+                    float.IsInfinity(actualAngle) ||
+                    dth > 2.0f)
+                    allWheelAligned = false;
             }
             LastRotateAligned = allWheelAligned; // 供上层做积分抗饱和
 
@@ -953,8 +1041,6 @@ namespace CommonUsage.Chassis
         {
             var vRotX = -vth / 180 * (float)Math.PI * pos.Y / 1000;
             var vRotY = vth / 180 * (float)Math.PI * pos.X / 1000;
-            Hedingben.ToastText($"vRot:({vRotX:F3},{vRotY:F3}) pos:({pos.X:F1},{pos.Y:F1})",
-                $"SendXYThSpeed-VectorVelocity{i}");
             return new Vector2(vx + vRotX, vy + vRotY);
         }
 		/// <summary>
@@ -971,38 +1057,61 @@ namespace CommonUsage.Chassis
             return ((float)(Math.Atan2(v.Y, v.X) / Math.PI * 180), v.Length());
         }
 
-        private float w = 0;
-        private float wAcc = 0.1f;
-        private float rotSync = 3f;
+        /// <summary>
+        /// 原地旋转时舵角误差对应的速度衰减宽度，单位为度。
+        /// </summary>
+        public float SteeringAlignmentSigmaDegrees { get; set; } = 8f;
         private bool XYThActive = false;
+        private bool _xyThWheelsAligned = false;
+        private DateTime _xyThDiagnosticsLastTime = DateTime.MinValue;
 
         public bool SendXYThSpeed(float vx, float vy, float vth, TimeSpan? deltaTime = null)
         {
             if (!Valid) return FailMotionDecomposition("SendXYThSpeed", "invalid chassis", deltaTime);
+
+            if (Math.Abs(vx) < 1e-6f && Math.Abs(vy) < 1e-6f && Math.Abs(vth) < 1e-6f)
+            {
+                RampStop(deltaTime);
+                XYThActive = false;
+                _xyThWheelsAligned = false;
+                GoingActive = false;
+                RotatingActive = false;
+                LastMotionDecomposeFailureReason = "";
+                return true;
+            }
+
 			if (!XYThActive)
             {
                 ResetMotionState();
-                w = 0;
+                _xyThWheelsAligned = false;
             }
 			XYThActive = true;
             GoingActive = false;
             RotatingActive = false;
 
-            if (Math.Abs(vx) < 1e-6f && Math.Abs(vy) < 1e-6f && Math.Abs(vth) < 1e-6f)
-            {
-                RampStop(deltaTime);
-                LastMotionDecomposeFailureReason = "";
-                return true;
-            }
-   
-            w = w + wAcc;
-            if (w > 1) w = 1;
             float[] sendSpeed = new float[_steerWheels.Count];
+            var allWheelsAligned = true;
+            var maximumAngleError = 0f;
+            const float initialAlignmentToleranceDegrees = 2f;
+            var writeDiagnostics =
+                Debug &&
+                (DateTime.Now - _xyThDiagnosticsLastTime)
+                    .TotalMilliseconds >= 250.0;
+
             for (var i = 0; i < _steerWheels.Count; i++)
             {
                 var sw = _steerWheels[i];
                 var (angle, speed) = AngleAndSpeed(sw.Position, vx, vy, vth, i);
                 var actualTh = sw.ReadAngle();
+                if (float.IsNaN(actualTh) ||
+                    float.IsInfinity(actualTh))
+                {
+                    return FailMotionDecomposition(
+                        "SendXYThSpeed",
+                        $"wheel {i} angle feedback is invalid: {actualTh}",
+                        deltaTime);
+                }
+
                 if (!TryResolveWheelAngle(i, CommonMath.ThDiff(angle, sw.ZeroDirection), "SendXYThSpeed",
                         out var useAngle, out var dir, out var resolveReason))
                     return FailMotionDecomposition("SendXYThSpeed", resolveReason, deltaTime);
@@ -1012,12 +1121,53 @@ namespace CommonUsage.Chassis
                 sendSpeed[i] = speed;
                 if (speed!=0)
                     SendTh(i, useAngle);
-                w = (float)Math.Min(w, CommonMath.gaussmf(CommonMath.ThDiff(actualTh, _sendAngle[i]), rotSync, 0));
 
-                Hedingben.ToastText($"w:{w:F2} s:{speed:F3} th:{_sendAngle[i]:F1} actualTh:{actualTh:F1}", $"SendXYThSpeed-{i}");
+                // 这里比较受机械限位约束的实际舵角，不使用圆周最短角。
+                var angleError =
+                    Math.Abs(_sendAngle[i] - actualTh);
+                maximumAngleError =
+                    Math.Max(maximumAngleError, angleError);
+                if (angleError >
+                    initialAlignmentToleranceDegrees)
+                {
+                    allWheelsAligned = false;
+                }
+
+                if (writeDiagnostics)
+                {
+                    Hedingben.ToastText(
+                        $"ready:{_xyThWheelsAligned} err:{angleError:F1} " +
+                        $"s:{speed:F3} th:{_sendAngle[i]:F1} actualTh:{actualTh:F1}",
+                        $"SendXYThSpeed-{i}");
+                }
             }
+
+            // 仅在一段XYTh运动刚开始时等待舵轮到位。
+            // 连续运动开始后，正常改变vx/vy/vth时允许舵轮边转、车辆边走，
+            // 避免每次打方向都重新把驱动速度压到零。
+            if (!_xyThWheelsAligned &&
+                allWheelsAligned)
+            {
+                _xyThWheelsAligned = true;
+            }
+
+            var driveEnabled = _xyThWheelsAligned;
             for (var i = 0; i < _steerWheels.Count; i++)
-                AccumulateSpeed(i, w * sendSpeed[i],false,new Vector2(0f,0f), deltaTime);
+                AccumulateSpeed(
+                    i,
+                    driveEnabled ? sendSpeed[i] : 0f,
+                    false,
+                    new Vector2(0f,0f),
+                    deltaTime);
+
+            if (writeDiagnostics)
+            {
+                _xyThDiagnosticsLastTime = DateTime.Now;
+                Hedingben.ToastText(
+                    $"ready:{_xyThWheelsAligned} maxErr:{maximumAngleError:F1} " +
+                    $"cmd:({vx:F3},{vy:F3},{vth:F1})",
+                    "SendXYThSpeed-alignment");
+            }
             //todo 计算rotCenter填入
             LastMoveTime = DateTime.Now;
             LastMotionDecomposeFailureReason = "";

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": [],

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)
             {

data_process/plot_angular_command.py

@@ -34,7 +34,7 @@ def plot_angular_command(
     )
     time = frame["TimeSeconds"].to_numpy(dtype=float)
     angular_command = frame[
-        "CommandAngularSpeedDegPerSec"
+        "CommandAngularSpeedRadPerSec"
     ].to_numpy(dtype=float)
     maximum = float(np.max(angular_command))
     minimum = float(np.min(angular_command))
@@ -50,12 +50,12 @@ def plot_angular_command(
     ax.axhline(0.0, color="black", linewidth=0.8)
     shade_localization_jump_windows(ax, metadata)
     ax.set_xlabel("时间 / s")
-    ax.set_ylabel("命令角速度 / (°/s)")
+    ax.set_ylabel("命令角速度 / (rad/s)")
     ax.set_title(
         f"角速度指令曲线\n"
         f"{metadata['controller_name']} - "
         f"{metadata['trajectory_name']}，"
-        f"范围=[{minimum:.3f}, {maximum:.3f}]°/s"
+        f"范围=[{minimum:.3f}, {maximum:.3f}]rad/s"
     )
     ax.grid(True, alpha=0.3)
     ax.legend()

data_process/plot_trajectory_comparison.py

@@ -76,6 +76,92 @@ def wrap_degrees(angle_degrees: np.ndarray) -> np.ndarray:
     return (angle_degrees + 180.0) % 360.0 - 180.0
 
 
+def build_complete_s_curve(
+    start: np.ndarray,
+    end: np.ndarray,
+    offset_mm: float,
+    samples_per_segment: int = 120,
+) -> tuple[np.ndarray, np.ndarray]:
+    """重建测试使用的三段三次贝塞尔完整S曲线及各点切线航向。"""
+    line = end - start
+    length = float(np.linalg.norm(line))
+    if length <= 1e-6:
+        raise ValueError("S型曲线的起点和终点不能重合。")
+
+    forward = line / length
+    left = np.array([-forward[1], forward[0]])
+    controls = [
+        np.array([
+            [0.0, 0.0],
+            [length / 12.0, 0.0],
+            [length / 6.0, offset_mm],
+            [length * 0.25, offset_mm],
+        ]),
+        np.array([
+            [length * 0.25, offset_mm],
+            [length / 3.0, offset_mm],
+            [length * 2.0 / 3.0, -offset_mm],
+            [length * 0.75, -offset_mm],
+        ]),
+        np.array([
+            [length * 0.75, -offset_mm],
+            [length * 5.0 / 6.0, -offset_mm],
+            [length * 11.0 / 12.0, 0.0],
+            [length, 0.0],
+        ]),
+    ]
+
+    local_parts: list[np.ndarray] = []
+    derivative_parts: list[np.ndarray] = []
+    for index, points in enumerate(controls):
+        t = np.linspace(0.0, 1.0, samples_per_segment + 1)
+        if index > 0:
+            t = t[1:]
+        one_minus_t = 1.0 - t
+        local = (
+            one_minus_t[:, None] ** 3 * points[0]
+            + 3.0
+            * one_minus_t[:, None] ** 2
+            * t[:, None]
+            * points[1]
+            + 3.0
+            * one_minus_t[:, None]
+            * t[:, None] ** 2
+            * points[2]
+            + t[:, None] ** 3 * points[3]
+        )
+        derivative = (
+            3.0
+            * one_minus_t[:, None] ** 2
+            * (points[1] - points[0])
+            + 6.0
+            * one_minus_t[:, None]
+            * t[:, None]
+            * (points[2] - points[1])
+            + 3.0
+            * t[:, None] ** 2
+            * (points[3] - points[2])
+        )
+        local_parts.append(local)
+        derivative_parts.append(derivative)
+
+    local_points = np.vstack(local_parts)
+    local_derivatives = np.vstack(derivative_parts)
+    world_points = (
+        start
+        + local_points[:, 0, None] * forward
+        + local_points[:, 1, None] * left
+    )
+    world_derivatives = (
+        local_derivatives[:, 0, None] * forward
+        + local_derivatives[:, 1, None] * left
+    )
+    headings = np.rad2deg(
+        np.arctan2(world_derivatives[:, 1], world_derivatives[:, 0])
+    )
+    return world_points, headings
+
+
 def segmented_savgol(
     values: np.ndarray,
     sample_interval: float,
@@ -166,6 +252,16 @@ def load_and_resample(
         "ReferenceEndY",
         "ReferenceSpeed",
     ]
+    optional_numeric_columns = [
+        "CommandAngularSpeedRadPerSecond",
+        "ReferenceAngularSpeedRadPerSecond",
+        "ReferenceMotionFrameYawDegrees",
+    ]
+    numeric_columns.extend(
+        column
+        for column in optional_numeric_columns
+        if column in raw.columns
+    )
     for column in numeric_columns:
         raw[column] = pd.to_numeric(raw[column], errors="coerce")
 
@@ -220,9 +316,21 @@ def load_and_resample(
     update_command_speed = np.abs(
         updates["CommandSpeed"].to_numpy(dtype=float)
     )
-    update_command_angular = np.abs(
-        updates["CommandAngularSpeed"].to_numpy(dtype=float)
-    )
+    if "CommandAngularSpeedRadPerSecond" in updates.columns:
+        update_command_angular_rad = np.abs(
+            updates[
+                "CommandAngularSpeedRadPerSecond"
+            ].to_numpy(dtype=float)
+        )
+    else:
+        # 旧CSV中的CommandAngularSpeed单位为deg/s。
+        update_command_angular_rad = np.deg2rad(
+            np.abs(
+                updates[
+                    "CommandAngularSpeed"
+                ].to_numpy(dtype=float)
+            )
+        )
 
     # 自适应跳变阈值：正常移动允许达到参考位移的3倍并保留15mm余量；
     # 低速阶段仍至少允许30mm，防止把普通定位噪声误判为跳变。
@@ -243,8 +351,12 @@ def load_and_resample(
     )
     expected_heading_delta = (
         0.5 *
-        (update_command_angular[1:] + update_command_angular[:-1]) *
-        update_dt
+        (
+            update_command_angular_rad[1:] +
+            update_command_angular_rad[:-1]
+        ) *
+        update_dt *
+        180.0 / np.pi
     )
     heading_threshold = np.maximum(
         5.0,
@@ -347,6 +459,15 @@ def load_and_resample(
             (time_uniform >= start) & (time_uniform <= end)
         )
 
+    if "CommandAngularSpeedRadPerSecond" in raw.columns:
+        angular_command_rad = interpolate_command(
+            "CommandAngularSpeedRadPerSecond"
+        )
+    else:
+        angular_command_rad = np.deg2rad(
+            interpolate_command("CommandAngularSpeed")
+        )
+
     frame = pd.DataFrame({
         "TimeSeconds": time_uniform,
         "DetourXRawMm": x_resampled,
@@ -356,8 +477,8 @@ def load_and_resample(
         "DetourThetaUnwrappedDeg": theta_filtered,
         "DetourThetaDeg": wrap_degrees(theta_filtered),
         "CommandSpeedMps": interpolate_command("CommandSpeed"),
-        "CommandAngularSpeedDegPerSec":
-            interpolate_command("CommandAngularSpeed"),
+        "CommandAngularSpeedRadPerSec":
+            angular_command_rad,
         "InvalidNearLocalizationJump": invalid_near_jump,
     })
 
@@ -367,6 +488,25 @@ def load_and_resample(
         "trajectory_name": str(first["TrajectoryName"]),
         "controller_name": str(first.get("ControllerName", "")),
         "trial_number": str(first.get("TrialNumber", "")),
+        # 蟹行轨迹的运动前向相对车体X轴逆时针偏置90°。
+        # DetourTheta始终是车体航向，计算航向误差时必须扣除该偏置。
+        "motion_frame_yaw_degrees": float(
+            first["ReferenceMotionFrameYawDegrees"]
+            if (
+                "ReferenceMotionFrameYawDegrees" in raw.columns
+                and pd.notna(
+                    first["ReferenceMotionFrameYawDegrees"]
+                )
+            )
+            else (
+                90.0
+                if "crab" in (
+                    str(first["TrajectoryName"]) +
+                    str(first.get("ControllerName", ""))
+                ).lower()
+                else 0.0
+            )
+        ),
         "reference_start_mm": np.array(
             [first["ReferenceStartX"], first["ReferenceStartY"]],
             dtype=float,
@@ -376,6 +516,12 @@ def load_and_resample(
             dtype=float,
         ),
         "reference_speed_mps": float(first["ReferenceSpeed"]),
+        "reference_angular_speed_rad_per_second": float(
+            first.get(
+                "ReferenceAngularSpeedRadPerSecond",
+                0.0,
+            )
+        ),
         # 圆弧构造时使用了测试开始处Detour航向，因此这里取首帧航向。
         "start_heading_degrees": float(first["DetourTheta"]),
         "sample_interval_seconds": sample_interval,
@@ -393,10 +539,13 @@ def build_reference(
     frame: pd.DataFrame,
     metadata: dict[str, Any],
 ) -> dict[str, np.ndarray | float | str]:
-    """根据CSV元数据建立直线、圆弧或原地自转参考及误差。"""
+    """根据CSV元数据建立直线、圆弧、完整S曲线或原地自转参考及误差。"""
     trajectory_name = str(metadata["trajectory_name"])
     start = np.asarray(metadata["reference_start_mm"], dtype=float)
     end = np.asarray(metadata["reference_end_mm"], dtype=float)
+    motion_frame_yaw_degrees = float(
+        metadata.get("motion_frame_yaw_degrees", 0.0)
+    )
     actual = frame[
         ["DetourXFilteredMm", "DetourYFilteredMm"]
     ].to_numpy(dtype=float)
@@ -410,12 +559,15 @@ def build_reference(
     if radius_match:
         radius = float(radius_match.group("radius"))
         sweep_degrees = float(radius_match.group("sweep"))
-        start_heading = float(metadata["start_heading_degrees"])
-        heading_radians = np.deg2rad(start_heading)
+        start_body_heading = float(metadata["start_heading_degrees"])
+        start_motion_heading = (
+            start_body_heading + motion_frame_yaw_degrees
+        )
+        heading_radians = np.deg2rad(start_motion_heading)
         center = start + radius * np.array(
             [-np.sin(heading_radians), np.cos(heading_radians)]
         )
-        start_radial_degrees = start_heading - 90.0
+        start_radial_degrees = start_motion_heading - 90.0
 
         radial = actual - center
         distance_to_center = np.linalg.norm(radial, axis=1)
@@ -429,7 +581,10 @@ def build_reference(
         ])
         # 对逆时针圆弧，正横向误差表示车辆位于轨迹左侧（圆内侧）。
         lateral_error = radius - distance_to_center
-        reference_heading = radial_angle_degrees + 90.0
+        reference_motion_heading = radial_angle_degrees + 90.0
+        reference_heading = (
+            reference_motion_heading - motion_frame_yaw_degrees
+        )
         heading_error = wrap_degrees(
             actual_heading - reference_heading
         )
@@ -450,12 +605,60 @@ def build_reference(
             "ideal_plot_mm": ideal_plot,
             "reference_points_mm": reference_points,
             "reference_heading_degrees": reference_heading,
+            "reference_motion_heading_degrees":
+                reference_motion_heading,
             "lateral_error_mm": lateral_error,
             "heading_error_degrees": heading_error,
             "center_mm": center,
             "radius_mm": radius,
         }
 
+    s_curve_match = re.search(
+        r"SCurve(?P<length>[0-9.]+)m_A(?P<offset>[0-9.]+)mm",
+        trajectory_name,
+        flags=re.IGNORECASE,
+    )
+    if s_curve_match:
+        offset_mm = float(s_curve_match.group("offset"))
+        ideal_plot, ideal_heading = build_complete_s_curve(
+            start,
+            end,
+            offset_mm,
+        )
+        delta = actual[:, np.newaxis, :] - ideal_plot[np.newaxis, :, :]
+        nearest_indices = np.argmin(
+            np.sum(delta * delta, axis=2),
+            axis=1,
+        )
+        reference_points = ideal_plot[nearest_indices]
+        reference_motion_heading = ideal_heading[nearest_indices]
+        reference_heading = (
+            reference_motion_heading - motion_frame_yaw_degrees
+        )
+        heading_radians = np.deg2rad(reference_motion_heading)
+        left_normals = np.column_stack([
+            -np.sin(heading_radians),
+            np.cos(heading_radians),
+        ])
+        lateral_error = np.sum(
+            (actual - reference_points) * left_normals,
+            axis=1,
+        )
+        heading_error = wrap_degrees(
+            actual_heading - reference_heading
+        )
+        return {
+            "kind": "s_curve",
+            "ideal_plot_mm": ideal_plot,
+            "reference_points_mm": reference_points,
+            "reference_heading_degrees": reference_heading,
+            "reference_motion_heading_degrees":
+                reference_motion_heading,
+            "lateral_error_mm": lateral_error,
+            "heading_error_degrees": heading_error,
+            "offset_mm": offset_mm,
+        }
+
     line = end - start
     length = float(np.linalg.norm(line))
     if length <= 1e-6:
@@ -516,10 +719,17 @@ def build_reference(
     progress = np.clip(displacement @ tangent, 0.0, length)
     reference_points = start + np.outer(progress, tangent)
     lateral_error = (actual - reference_points) @ left_normal
-    reference_heading_scalar = np.rad2deg(
+    reference_motion_heading_scalar = np.rad2deg(
         np.arctan2(tangent[1], tangent[0])
     )
-    reference_heading = np.full(len(frame), reference_heading_scalar)
+    reference_heading_scalar = (
+        reference_motion_heading_scalar -
+        motion_frame_yaw_degrees
+    )
+    reference_heading = np.full(
+        len(frame),
+        reference_heading_scalar,
+    )
     heading_error = wrap_degrees(
         actual_heading - reference_heading
     )
@@ -529,6 +739,10 @@ def build_reference(
         "ideal_plot_mm": ideal_plot,
         "reference_points_mm": reference_points,
         "reference_heading_degrees": reference_heading,
+        "reference_motion_heading_degrees": np.full(
+            len(frame),
+            reference_motion_heading_scalar,
+        ),
         "lateral_error_mm": lateral_error,
         "heading_error_degrees": heading_error,
     }

测试方案.txt

@@ -9,6 +9,7 @@
 ### 评价指标
 横向误差 RMSE、最大横向误差；航向误差 RMSE、最大航向误差；速度误差 RMSE、最大速度偏差；
 角速度或转角指令的变化曲线；最终位置误差、最终航向误差
+
 ### 展示形式
 理想轨迹与实际轨迹对比图、横向/航向误差随时间变化图、参考速度与实际速度对比图、角速度指令曲线
 
@@ -26,7 +27,6 @@ ALQR：在线估计最新参数实时重新求解
 3. 
 
 
-
 先用 Bryson’s Rule + 贝叶斯优化在仿真里把 Q/R 调到一个不错的基准。
 上实车时采用自适应 LQR：在线估计关键参数（尤其是轮胎刚度），实时更新 K。
 C# 实现的话：