完善轨迹跟踪测试并添加实验数据记录与绘图分析工具

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

.gitignore

@@ -88,3 +88,5 @@ _ReSharper*/
 *.db
 *.sqlite
 *.sqlite3
+
+*.csv

ClumsyPilot/MovementTests.cs

@@ -2,125 +2,619 @@ using ClumsyCore;
 using ClumsyCore.DTools;
 using ClumsyCore.Interfaces;
 using ClumsyCore.Pilot;
+using CommonUsage.Chassis;
 using MDCSToolBox.Commons.Controllers;
+using MDCSToolBox.Clumsy.Tracks;
+using MyParking.Shared;
 using System;
 using System.Numerics;
+using System.Threading;
 
 namespace MultiWheelC
 {
-    public abstract class DstTrackerTestBase : MovementTest
+    internal static class MovementTestPreparation
     {
-        public bool UseInteractivePick = true;
+        // 在测试正式开始前，将四个舵轮稳定回正到车体前向。
-        public float srcX;
+        public static bool AlignWheelsForward(
-        public float srcY;
+            ref DriveTask activeTask)
-        public float dstX;
-        public float dstY;
-        public float carDirectionBias;
-
-        private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
-        private DriveTask _dt;
-
-        protected DstTrackerTestBase(float defaultCarDirectionBias)
         {
-            carDirectionBias = defaultCarDirectionBias;
+            var preparation = new PrepareWheelsForward();
-        }
+            var task = new DriveTask(preparation.Get());
+            activeTask = task;
 
-        public override void TestStop()
+            try
-        {
-            _dt?.Stop();
-            _painter?.Clear();
-        }
-
-        public override void Test()
-        {
-            Vector2 p1;
-            Vector2 p2;
-            if (UseInteractivePick)
             {
-                p1 = UI.GetPoint("point1");
+                task.Wait();
-                p2 = UI.GetPoint("point2");
+                return preparation.Completed;
             }
-            else
+            catch (Exception ex)
-            {
-                p1 = new Vector2(srcX, srcY);
-                p2 = new Vector2(dstX, dstY);
-            }
-
-            _painter.Clear();
-            _dt = new DriveTask(new DstTracker
-            {
-                Src = p1,
-                Dst = p2,
-                CarDirectionBias = carDirectionBias,
-            }.Get());
-            _dt.Wait();
-        }
-    }
-
-    [MovementTest(name = "测试终点跟踪动作-前进")]
-    public sealed class DstTrackerForward : DstTrackerTestBase
-    {
-        public DstTrackerForward() : base(0f) { }
-    }
-
-    [MovementTest(name = "测试终点跟踪动作-后退")]
-    public sealed class DstTrackerBackward : DstTrackerTestBase
-    {
-        public DstTrackerBackward() : base(180f) { }
-    }
-
-    [MovementTest(name = "底盘旋转测试")]
-    public class RotateToAngleTest : MovementTest
-    {
-        private DriveTask _dt;
-
-        // 停止当前正在执行的底盘原地旋转任务。
-        public override void TestStop()
-        {
-            _dt?.Stop();
-        }
-
-        // 交互输入目标角度后执行底盘原地旋转测试。
-        public override void Test()
-        {
-            var input = UI.GetInput("输入旋转角度：");
-            if (!float.TryParse(input, out var angleTarget))
             {
                 Console.WriteLine(
-                    $"旋转测试输入无效：{input}");
+                    $"测试前舵轮回正失败：{ex.Message}");
+                return false;
+            }
+            finally
+            {
+                task.Stop();
+                if (ReferenceEquals(activeTask, task))
+                    activeTask = null;
+            }
+        }
+
+        // 只读取实际舵角，检查四个舵轮是否已与车头方向一致。
+        public static bool AreWheelsForward(
+            float toleranceDegrees = 2f)
+        {
+            var chassis =
+                PilotDefinition.Chassis as MultiWheelChassis;
+            if (chassis == null)
+            {
+                Console.WriteLine(
+                    "当前底盘不是MultiWheelChassis，无法检查舵轮方向。");
+                return false;
+            }
+
+            try
+            {
+                var adapter = new MultiWheelChassisAdapter(
+                    chassis,
+                    PilotDefinition.Self.CarNum);
+                var toleranceRadians =
+                    toleranceDegrees * Math.PI / 180.0;
+
+                if (adapter.AreParallelWheelsAligned(
+                        0.0,
+                        toleranceRadians))
+                {
+                    return true;
+                }
+
+                Console.WriteLine(
+                    "四个舵轮尚未与车头方向一致，请先执行“准备：四个舵轮与车头方向一致”。");
+                return false;
+            }
+            catch (Exception ex)
+            {
+                Console.WriteLine(
+                    $"检查舵轮方向失败：{ex.Message}");
+                return false;
+            }
+        }
+    }
+
+    [MovementTest(name = "准备：四个舵轮与车头方向一致")]
+    public class AlignWheelsForwardTest : MovementTest
+    {
+        private DriveTask _task;
+
+        // 单独将四个舵轮转到车体前向0°并等待实际反馈稳定到位。
+        public override void Test()
+        {
+            MovementTestPreparation.AlignWheelsForward(
+                ref _task);
+        }
+
+        // 停止正在执行的舵轮回正任务并清零底盘运动命令。
+        public override void TestStop()
+        {
+            _task?.Stop();
+            _task = null;
+        }
+    }
+
+    [MovementTest(name = "测试连续前进4m")]
+    public class TestForward4m : MovementTest
+    {
+        public float DistanceMillimeters = 4000f; // 测试距离，单位mm。
+        public float CruiseSpeed = 0.3f;  // 巡航速度上限，单位m/s。
+        public int TrialNumber = 1; // 重复实验编号。
+        private DriveTask _task;
+        private TrackingExperimentRecorder _recorder;
+        // 从当前Detour位置沿车头方向生成4m连续直线并记录测试数据。
+        public override void Test()
+        {
+            if (!MovementTestPreparation.AreWheelsForward())
+            {
                 return;
             }
-            // 防止重复启动测试时，上一项旋转任务仍在运行。
-            _dt?.Stop();
-            var task = new DriveTask(
-                new MultiWheelRotateInPlace
-                {
-                    AngleTarget = angleTarget,
 
-                    PidparamsRead = () => new PIDParams
+            var location = DetourInterface.getCartLocation();
-                    {
+            if (double.IsNaN(location.x) ||
-                        Kp = PilotDefinition.Conf.InPlaceRotateKp,
+                double.IsInfinity(location.x) ||
-                        Ki = PilotDefinition.Conf.InPlaceRotateKi,
+                double.IsNaN(location.y) ||
-                        Kd = PilotDefinition.Conf.InPlaceRotateKd,
+                double.IsInfinity(location.y) ||
-                        DeadZone = PilotDefinition.Conf.InPlaceRotateArriveDeg,
+                double.IsNaN(location.th) ||
-                        SpeedAccPerSec = PilotDefinition.Conf.InPlaceRotateAcc,
+                double.IsInfinity(location.th))
-                        OutputUpperThreshold = PilotDefinition.Conf.InPlaceRotateMaxSpeed,
+            {
-                        MaxI = PilotDefinition.Conf.InPlaceRotateMaxI,
+                Console.WriteLine(
-                    }
+                    "Detour当前位姿无效，取消连续前进4m测试。");
+                return;
+            }
+            var source = new Vector2((float)location.x, (float)location.y);
+            // Detour航向单位是度，三角函数需要弧度。
+            var headingRadians = location.th * Math.PI / 180.0;
+            var destination = new Vector2(
+                source.X + DistanceMillimeters * (float)Math.Cos(headingRadians),
+                source.Y + DistanceMillimeters * (float)Math.Sin(headingRadians));
+            _recorder =
+                        new TrackingExperimentRecorder(
+                            controllerName: "Stanley",
+                            trajectoryName: "Straight4m",
+                            trialNumber: TrialNumber,
+                            referenceStart: source,
+                            referenceEnd: destination,
+                            referenceSpeed: CruiseSpeed);
+            _recorder.Start();
+            try
+            {
+                _task = new DriveTask(
+                new DstTracker
+                {
+                    Src = source,
+                    Dst = destination,
+                    CarDirectionBias = 0f,
+                    MaxSpeed = CruiseSpeed
                 }.Get());
-            _dt = task;
+                _task.Wait();
+                // 保留少量停车后数据，便于观察速度是否回到零。
+                Thread.Sleep(300);
+            }
+            finally
+            {
+                _task?.Stop();
+                _recorder?.UpdateCommand(0f, 0f);
+                _recorder?.StopAndSave();
+                _task = null;
+                _recorder = null;
+            }
+        }
+
+        public override void TestStop()
+        {
+            _task?.Stop();
+            _recorder?.UpdateCommand(0f, 0f);
+            _recorder?.StopAndSave();
+        }
+    }
+
+    [MovementTest(name = "测试原地自转90°")]
+    public class TestRotate90 : MovementTest
+    {
+        public float RelativeAngleDegrees = 90f; // 相对当前航向的旋转角度，逆时针为正。
+        public float MaxAngularSpeedDegreesPerSecond = 20f; // PID输出的最大角速度。
+        public int TrialNumber = 1; // 重复实验编号。
+
+        private DriveTask _task;
+        private TrackingExperimentRecorder _recorder;
+
+        // 从当前Detour航向开始，原地相对旋转指定角度并记录实验数据。
+        public override void Test()
+        {
+            if (float.IsNaN(RelativeAngleDegrees) ||
+                float.IsInfinity(RelativeAngleDegrees) ||
+                float.IsNaN(MaxAngularSpeedDegreesPerSecond) ||
+                float.IsInfinity(MaxAngularSpeedDegreesPerSecond) ||
+                MaxAngularSpeedDegreesPerSecond <= 0f)
+            {
+                Console.WriteLine("原地旋转测试参数无效。");
+                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当前位姿无效，取消原地旋转测试。");
+                return;
+            }
+
+            var rotationCenter =
+                new Vector2((float)location.x, (float)location.y);
+            var targetWorldAngle =
+                NormalizeDegrees(
+                    (float)location.th + RelativeAngleDegrees);
+
+            _recorder = new TrackingExperimentRecorder(
+                controllerName: "InPlaceRotatePID",
+                trajectoryName: "Rotate90",
+                trialNumber: TrialNumber,
+                referenceStart: rotationCenter,
+                referenceEnd: rotationCenter,
+                referenceSpeed:
+                    MaxAngularSpeedDegreesPerSecond);
+            _recorder.Start();
+
+            try
+            {
+                _task = new DriveTask(
+                    new MultiWheelRotateInPlace
+                    {
+                        // MultiWheelRotateInPlace接收世界坐标系绝对航向。
+                        AngleTarget = targetWorldAngle,
+                        PidparamsRead = () => new PIDParams
+                        {
+                            Kp =
+                                PilotDefinition.Conf.InPlaceRotateKp,
+                            Ki =
+                                PilotDefinition.Conf.InPlaceRotateKi,
+                            Kd =
+                                PilotDefinition.Conf.InPlaceRotateKd,
+                            DeadZone =
+                                PilotDefinition.Conf
+                                    .InPlaceRotateArriveDeg,
+                            SpeedAccPerSec =
+                                PilotDefinition.Conf.InPlaceRotateAcc,
+                            OutputUpperThreshold =
+                                MaxAngularSpeedDegreesPerSecond,
+                            MaxI =
+                                PilotDefinition.Conf.InPlaceRotateMaxI
+                        },
+                        CommandAngularSpeedObserver =
+                            commandAngularSpeed =>
+                                _recorder?.UpdateCommand(
+                                    0f,
+                                    commandAngularSpeed)
+                    }.Get());
+
+                _task.Wait();
+
+                // 保留少量停止后的样本，用于观察角速度是否回到零。
+                Thread.Sleep(300);
+            }
+            finally
+            {
+                _task?.Stop();
+                _recorder?.UpdateCommand(0f, 0f);
+                _recorder?.StopAndSave();
+                _task = null;
+                _recorder = null;
+            }
+        }
+
+        // 停止原地旋转并保存当前已经采集的实验数据。
+        public override void TestStop()
+        {
+            _task?.Stop();
+            _recorder?.UpdateCommand(0f, 0f);
+            _recorder?.StopAndSave();
+        }
+
+        // 将世界航向归一化到大约[-180°,180°]。
+        private static float NormalizeDegrees(float angleDegrees)
+        {
+            return (float)(
+                angleDegrees -
+                Math.Round(angleDegrees / 360.0) * 360.0);
+        }
+    }
+
+    [MovementTest(name = "测试左转90°圆弧")]
+    public class TestArcMovement : MovementTest
+    {
+        public float RadiusMillimeters = 2000f; // 左转圆的半径，单位mm。
+        public float CruiseSpeed = 0.3f; // 圆周运动速度上限，单位m/s。
+        public int TrialNumber = 1; // 重复实验编号。
+
+        private DriveTask _task;
+        private TrackingExperimentRecorder _recorder;
+
+        // 从当前位姿开始，沿半径2m的圆弧向左转弯90°。
+        public override void Test()
+        {
+            if (float.IsNaN(RadiusMillimeters) ||
+                float.IsInfinity(RadiusMillimeters) ||
+                RadiusMillimeters <= 0f ||
+                float.IsNaN(CruiseSpeed) ||
+                float.IsInfinity(CruiseSpeed) ||
+                CruiseSpeed <= 0f)
+            {
+                Console.WriteLine("圆弧运动测试参数无效。");
+                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当前位姿无效，取消圆弧运动测试。");
+                return;
+            }
+
+            var source =
+                new Vector2((float)location.x, (float)location.y);
+            var headingRadians =
+                location.th * Math.PI / 180.0;
+
+            // 根据世界航向求车体左法向，左转圆心位于车辆左侧。
+            var center = new Vector2(
+                source.X -
+                    RadiusMillimeters *
+                    (float)Math.Sin(headingRadians),
+                source.Y +
+                    RadiusMillimeters *
+                    (float)Math.Cos(headingRadians));
+
+            // 从圆心指向车辆起点的极角，比车辆切线航向小90°。
+            var startRadialAngleDegrees =
+                (float)location.th - 90f;
+
+            var controller = new ChassisController
+            {
+                BaseSpeed = CruiseSpeed
+            }.Get();
+            controller.FinishSpeed = 0f;
+
+            var arc = new CircularArcTrack(
+                center,
+                RadiusMillimeters,
+                startRadialAngleDegrees,
+                startRadialAngleDegrees + 90f,
+                direction: 1)
+            {
+                Speed = CruiseSpeed,
+                CarDirectionBias = 0f
+            };
+
+            if (!controller.AddTrack(arc, "LeftArc90Degrees"))
+            {
+                Console.WriteLine(
+                    "左转90°圆弧轨迹添加失败，取消测试。");
+                return;
+            }
+
+            _recorder = new TrackingExperimentRecorder(
+                controllerName: "GeometricController",
+                trajectoryName:
+                    $"LeftArc90_R{RadiusMillimeters:0}mm",
+                trialNumber: TrialNumber,
+                referenceStart: source,
+                referenceEnd: source,
+                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;
+            }
+        }
+
+        // 停止圆弧运动并保存当前已经采集的实验数据。
+        public override void TestStop()
+        {
+            _task?.Stop();
+            _recorder?.UpdateCommand(0f, 0f);
+            _recorder?.StopAndSave();
+        }
+    }
+
+    public abstract class ClampMovementTestBase : MovementTest
+    {
+        public float TimeoutSeconds = 30f; // 动作超时时间，单位s。
+
+        private DriveTask _task;
+        protected abstract bool Close { get; }
+
+        // 根据派生测试类型驱动左右夹臂同步夹紧或打开。
+        public override void Test()
+        {
+            var leftTarget = Close
+                ? PilotDefinition.Self.LeftArmUpperPos
+                : PilotDefinition.Self.LeftArmLowerPos;
+            var rightTarget = Close
+                ? PilotDefinition.Self.RightArmUpperPos
+                : PilotDefinition.Self.RightArmLowerPos;
+
+            if (float.IsNaN(leftTarget) ||
+                float.IsInfinity(leftTarget) ||
+                float.IsNaN(rightTarget) ||
+                float.IsInfinity(rightTarget))
+            {
+                Console.WriteLine(
+                    "夹臂目标位置无效，取消夹臂运动测试。");
+                return;
+            }
+
+            // 防止重复点击时上一项夹臂任务仍在运行。
+            TestStop();
+            Console.WriteLine(
+                $"开始夹臂{(Close ? "夹紧" : "打开")}测试：" +
+                $"左目标={leftTarget}，右目标={rightTarget}");
+
+            var task = new DriveTask(
+                new ClampToTarget
+                {
+                    LeftClampTarget = leftTarget,
+                    RightClampTarget = rightTarget,
+                    TimeoutSeconds = TimeoutSeconds
+                }.Get());
+            _task = task;
+
             try
             {
                 task.Wait();
             }
             finally
             {
-                // 防止旧任务结束时，错误清除后来启动的新任务。
+                PilotDefinition.Self.SpeedLeftArm = 0f;
-                if (ReferenceEquals(_dt, task))
+                PilotDefinition.Self.SpeedRightArm = 0f;
-                {
+                if (ReferenceEquals(_task, task))
-                    _dt = null;
+                    _task = null;
-                }
             }
         }
+
+        // 停止夹臂任务并立即清零左右夹臂下发速度。
+        public override void TestStop()
+        {
+            _task?.Stop();
+            _task = null;
+            PilotDefinition.Self.SpeedLeftArm = 0f;
+            PilotDefinition.Self.SpeedRightArm = 0f;
+        }
     }
+
+    [MovementTest(name = "夹臂关闭测试")]
+    public sealed class TestClampOpenMovement
+        : ClampMovementTestBase
+    {
+        protected override bool Close => false;
+    }
+
+    [MovementTest(name = "夹臂启动测试")]
+    public sealed class TestClampCloseMovement
+        : ClampMovementTestBase
+    {
+        protected override bool Close => true;
+    }
+
 }
+
+
+
+
+
+
+#region 旧版测试
+// public abstract class DstTrackerTestBase : MovementTest
+// {
+//     public bool UseInteractivePick = true;
+//     public float srcX;
+//     public float srcY;
+//     public float dstX;
+//     public float dstY;
+//     public float carDirectionBias;
+
+//     private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
+//     private DriveTask _dt;
+
+//     protected DstTrackerTestBase(float defaultCarDirectionBias)
+//     {
+//         carDirectionBias = defaultCarDirectionBias;
+//     }
+
+//     public override void TestStop()
+//     {
+//         _dt?.Stop();
+//         _painter?.Clear();
+//     }
+
+//     public override void Test()
+//     {
+//         Vector2 p1;
+//         Vector2 p2;
+//         if (UseInteractivePick)
+//         {
+//             p1 = UI.GetPoint("point1");
+//             p2 = UI.GetPoint("point2");
+//         }
+//         else
+//         {
+//             p1 = new Vector2(srcX, srcY);
+//             p2 = new Vector2(dstX, dstY);
+//         }
+
+//         _painter.Clear();
+//         _dt = new DriveTask(new DstTracker
+//         {
+//             Src = p1,
+//             Dst = p2,
+//             CarDirectionBias = carDirectionBias,
+//         }.Get());
+//         _dt.Wait();
+//     }
+// }
+
+// [MovementTest(name = "测试终点跟踪动作-前进")]
+// public sealed class DstTrackerForward : DstTrackerTestBase
+// {
+//     public DstTrackerForward() : base(0f) { }
+// }
+
+// [MovementTest(name = "测试终点跟踪动作-后退")]
+// public sealed class DstTrackerBackward : DstTrackerTestBase
+// {
+//     public DstTrackerBackward() : base(180f) { }
+// }
+
+// [MovementTest(name = "底盘旋转测试")]
+// public class RotateToAngleTest : MovementTest
+// {
+//     private DriveTask _dt;
+
+//     // 停止当前正在执行的底盘原地旋转任务。
+//     public override void TestStop()
+//     {
+//         _dt?.Stop();
+//     }
+
+//     // 交互输入目标角度后执行底盘原地旋转测试。
+//     public override void Test()
+//     {
+//         var input = UI.GetInput("输入旋转角度：");
+//         if (!float.TryParse(input, out var angleTarget))
+//         {
+//             Console.WriteLine(
+//                 $"旋转测试输入无效：{input}");
+//             return;
+//         }
+//         // 防止重复启动测试时，上一项旋转任务仍在运行。
+//         _dt?.Stop();
+//         var task = new DriveTask(
+//             new MultiWheelRotateInPlace
+//             {
+//                 AngleTarget = angleTarget,
+
+//                 PidparamsRead = () => new PIDParams
+//                 {
+//                     Kp = PilotDefinition.Conf.InPlaceRotateKp,
+//                     Ki = PilotDefinition.Conf.InPlaceRotateKi,
+//                     Kd = PilotDefinition.Conf.InPlaceRotateKd,
+//                     DeadZone = PilotDefinition.Conf.InPlaceRotateArriveDeg,
+//                     SpeedAccPerSec = PilotDefinition.Conf.InPlaceRotateAcc,
+//                     OutputUpperThreshold = PilotDefinition.Conf.InPlaceRotateMaxSpeed,
+//                     MaxI = PilotDefinition.Conf.InPlaceRotateMaxI,
+//                 }
+//             }.Get());
+//         _dt = task;
+//         try
+//         {
+//             task.Wait();
+//         }
+//         finally
+//         {
+//             // 防止旧任务结束时，错误清除后来启动的新任务。
+//             if (ReferenceEquals(_dt, task))
+//             {
+//                 _dt = null;
+//             }
+//         }
+//     }
+// }
+
+#endregion

ClumsyPilot/Movements.cs

@@ -11,44 +11,92 @@ using System.Collections.Generic;
 using System.Drawing;
 using System.Numerics;
 using System.Threading;
+using FundamentalLib;
+using MyParking.Shared;
 
 namespace MultiWheelC
 {
-    public class DstTracker : MovementDefinition
+    // C层测试准备：停车并等待四个舵轮稳定回到车体前向0°。
+    public class PrepareWheelsForward : MovementDefinition
     {
-        public Vector2 Src;
+        public float ToleranceDegrees = 2f;
-        public Vector2 Dst;
+        public float StableSeconds = 0.3f;
-        public float CarDirectionBias = 0f;
+        public float TimeoutSeconds = 10f;
-        public Painter Painter = UI.GetPainter("DstTracker");
+        public bool Completed { get; private set; }
+
         public override IEnumerable<bool> Get()
         {
-            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+            var chassis =
-            DriveTask task = null;
+                PilotDefinition.Chassis as MultiWheelChassis;
+            if (chassis == null)
+            {
+                throw new InvalidOperationException(
+                    "当前底盘不是MultiWheelChassis，无法执行舵轮回正。");
+            }
+
+            var adapter = new MultiWheelChassisAdapter(
+                chassis,
+                PilotDefinition.Self.CarNum);
+            var toleranceRadians =
+                ToleranceDegrees * Math.PI / 180.0;
+            var startTime = DateTime.UtcNow;
+            DateTime? alignedSince = null;
+
+            Completed = false;
+            if (!adapter.PrepareParallelDirection(0.0))
+            {
+                throw new InvalidOperationException(
+                    "无法将所有舵轮下发到车体前向0°。");
+            }
+
             try
             {
-                Console.WriteLine($"DstTracker src:({Src.X:F2}, {Src.Y:F2}) dst:({Dst.X:F2}, {Dst.Y:F2})");
+                while (true)
-                Painter.DrawLine(Color.Cyan, Src.X, Src.Y, Dst.X, Dst.Y, width: 3);
-
-                var tracker = new ChassisController().Get();
-                var linePath = new LineTrack(Src, Dst)
                 {
-                    CarDirectionBias = CarDirectionBias,
+                    var aligned =
-                    Speed = PilotDefinition.Conf.DstTrackerMaxSpeed
+                        adapter.AreParallelWheelsAligned(
-                };
+                            0.0,
-                tracker.AddTrack(linePath);
+                            toleranceRadians);
-                task = new DriveTask(tracker.Track());
+
-                task.Wait();
+                    if (aligned)
-                yield return false;
+                    {
+                        if (!alignedSince.HasValue)
+                            alignedSince = DateTime.UtcNow;
+
+                        if ((DateTime.UtcNow -
+                             alignedSince.Value).TotalSeconds >=
+                            StableSeconds)
+                        {
+                            Completed = true;
+                            yield break;
+                        }
+                    }
+                    else
+                    {
+                        alignedSince = null;
+                    }
+
+                    if (TimeoutSeconds > 0f &&
+                        (DateTime.UtcNow - startTime).TotalSeconds >
+                        TimeoutSeconds)
+                    {
+                        throw new TimeoutException(
+                            $"舵轮回正超过{TimeoutSeconds:F1}s，" +
+                            "测试已经取消。");
+                    }
+
+                    yield return true;
+                }
             }
             finally
             {
-                task?.Stop();
+                // 只清零驱动速度，保留已经下发的0°舵角。
-                chassis.SendXYThSpeed(0f, 0f, 0f);
+                adapter.StopImmediately();
             }
         }
-
     }
 
+    #region 功能项
     public class Sleep : MovementDefinition
     {
         public float Second = 2f;
@@ -71,7 +119,303 @@ namespace MultiWheelC
             yield return false;
         }
     }
+    public class DriverAble : MovementDefinition
+    {
+        public int WaitTimeoutMs = 2000;
+        public int PollIntervalMs = 50;
 
+        // C层单车硬件：请求全部驱动轮复位并恢复使能。
+        public override IEnumerable<bool> Get()
+        {
+            PilotDefinition.Self.ResetFromC = true;
+
+            try
+            {
+                var start = DateTime.Now;
+                var timeoutMs = Math.Max(0, WaitTimeoutMs);
+                var pollMs = Math.Max(1, PollIntervalMs);
+
+                // 至少保留一个调度周期，确保M层能收到复位请求。
+                yield return true;
+
+                while (!PilotDefinition.Self.WheelAbleState &&
+                        (DateTime.Now - start).TotalMilliseconds < timeoutMs)
+                {
+                    Thread.Sleep(pollMs);
+                    yield return true;
+                }
+            }
+            finally
+            {
+                PilotDefinition.Self.ResetFromC = false;
+            }
+        }
+    }
+    public class DriverDisable : MovementDefinition
+    {
+        public int WaitTimeoutMs = 3000;
+        public int PollIntervalMs = 20;
+
+        // C层单车硬件：请求驱动轮退出使能，并等待M层状态反馈。
+        public override IEnumerable<bool> Get()
+        {
+            var timeoutMs = Math.Max(0, WaitTimeoutMs);
+            var pollMs = Math.Max(1, PollIntervalMs);
+            var startTime = DateTime.UtcNow;
+            var success = false;
+
+            PilotDefinition.Self.DisableFromC = true;
+
+            try
+            {
+                // 至少保持一个C层调度周期，确保M层能收到下使能请求。
+                yield return true;
+
+                success = !PilotDefinition.Self.WheelAbleState;
+
+                while (!success &&
+                        (DateTime.UtcNow - startTime).TotalMilliseconds <
+                            timeoutMs)
+                {
+                    Thread.Sleep(pollMs);
+
+                    success =
+                        !PilotDefinition.Self.WheelAbleState;
+
+                    if (!success)
+                    {
+                        yield return true;
+                    }
+                }
+            }
+            finally
+            {
+                // 无论正常完成、超时、异常还是任务被停止，都撤销请求。
+                PilotDefinition.Self.DisableFromC = false;
+            }
+            if (success)
+            {
+                Console.WriteLine(
+                    $"驱动器下使能完成，" +
+                    $"WheelAbleState=" +
+                    $"{PilotDefinition.Self.WheelAbleState}");
+            }
+            else
+            {
+                Console.WriteLine(
+                    $"驱动器下使能超时，" +
+                    $"WheelAbleState=" +
+                    $"{PilotDefinition.Self.WheelAbleState}，" +
+                    $"等待{timeoutMs}ms");
+            }
+            yield return false;
+        }
+    }
+    #endregion
+
+    #region 直线运动
+    //在世界坐标系下，从路径起点追踪到终点并停车
+    public class DstTracker : MovementDefinition
+    {
+        public Vector2 Src;
+        public Vector2 Dst;
+        // 本次轨迹的巡航速度上限，单位m/s。
+        public float MaxSpeed = PilotDefinition.Conf.DstTrackerMaxSpeed;
+        public float CarDirectionBias = 0f;
+        public Painter Painter = UI.GetPainter("DstTracker");
+        public override IEnumerable<bool> Get()
+        {
+            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+            DriveTask task = null;
+            try
+            {
+                Console.WriteLine($"DstTracker src:({Src.X:F2}, {Src.Y:F2}) dst:({Dst.X:F2}, {Dst.Y:F2})");
+                Painter.DrawLine(Color.Cyan, Src.X, Src.Y, Dst.X, Dst.Y, width: 3);
+
+                var tracker = new ChassisController
+                {
+                    BaseSpeed = MaxSpeed
+                }.Get();
+                // 要求路径末端速度下降到零。
+                tracker.FinishSpeed = 0f;
+                var linePath = new LineTrack(Src, Dst)
+                {
+                    CarDirectionBias = CarDirectionBias,
+                    Speed = MaxSpeed
+                };
+                tracker.AddTrack(linePath);
+                task = new DriveTask(tracker.Track());
+                task.Wait();
+                yield return false;
+            }
+            finally
+            {
+                task?.Stop();
+                chassis.SendXYThSpeed(0f, 0f, 0f);
+            }
+        }
+
+    }
+    //直线行走基于轮里程
+    // C层单车底盘：按照车轮里程行驶指定的相对距离。
+    public class LineTracking : MovementDefinition
+    {
+        // 相对动作启动位置的行驶距离，单位mm。
+        // 正数表示前进，负数表示后退。
+        public float TargetDistance;
+        public float MaxSpeed = PilotDefinition.Conf.LineTrackMaxSpeed;
+        public float Kp = PilotDefinition.Conf.LineTrackKp;
+        public float Ki = PilotDefinition.Conf.LineTrackKi;
+        public float Kd = PilotDefinition.Conf.LineTrackKd;
+        public float DeadZone = PilotDefinition.Conf.LineTrackDeadZone;
+        public int SrcId = -1;
+        public int DstId = -1;
+        public Action<int> LeaveSrcFunction;
+        // 接近目标后是否保留速度，交给下一个动作接管。
+        public bool EnableHandover;
+        // 进入动作衔接的剩余距离，单位mm。
+        public float HandoverDistance = 80f;
+        // HandoverSpeed小于0时，使用MaxSpeed的此比例。
+        public float HandoverSpeedRatio = 0.5f;
+        // 大于等于0时，直接作为衔接速度，单位m/s。
+        public float HandoverSpeed = -1f;
+        public float MinHandoverSpeed = 0.05f;
+        private PIDController _pid;
+        // 读取当前单车直线行驶里程，单位mm。
+        private static float ReadPosition()
+        {
+            return
+                (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2f;
+        }
+
+        // 根据动作启动位置和目标距离执行直线里程闭环。
+        public override IEnumerable<bool> Get()
+        {
+            if (float.IsNaN(TargetDistance) || float.IsInfinity(TargetDistance))
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(TargetDistance),
+                    "目标行驶距离必须是有限值。");
+            }
+
+            if (float.IsNaN(MaxSpeed) || float.IsInfinity(MaxSpeed) || MaxSpeed <= 0f)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(MaxSpeed),
+                    "最大速度必须是大于零的有限值。");
+            }
+            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+            // 每次启动动作时重新读取起始编码器位置。
+            var startPosition = ReadPosition();
+            // PID仍然控制绝对编码器位置，但绝对目标由动作自动计算。
+            var targetPosition = startPosition + TargetDistance;
+            _pid = new PIDController(ReadPosition, Kp, Ki, Kd, 0, DeadZone, MaxSpeed)
+            {
+                SpeedAccPerSec = Math.Abs(MaxSpeed) / 2f
+            };
+            var handoverRequested = false;
+            var keepHandoverSpeed = false;
+            DLog.Log(
+                $"直线里程动作：" +
+                $"起点={startPosition:F1}mm，" +
+                $"距离={TargetDistance:F1}mm，" +
+                $"目标={targetPosition:F1}mm",
+                "straight_line");
+            try
+            {
+                while (true)
+                {
+                    var currentPosition = ReadPosition();
+                    var remainingDistance = targetPosition - currentPosition;
+                    // 接近目标后，保留一定速度交给后续动作。
+                    if (EnableHandover && Math.Abs(remainingDistance) <= Math.Max(1f, HandoverDistance))
+                    {
+                        var direction = Math.Sign(remainingDistance);
+                        if (direction == 0)
+                        {
+                            direction = Math.Sign(TargetDistance);
+                        }
+                        var requestedSpeed = HandoverSpeed >= 0f ? Math.Abs(HandoverSpeed) : Math.Abs(MaxSpeed) * HandoverSpeedRatio;
+                        var maximumSpeed = Math.Abs(MaxSpeed);
+                        var minimumSpeed = Math.Min(Math.Abs(MinHandoverSpeed), maximumSpeed);
+                        var limitedSpeed = Math.Max(minimumSpeed, Math.Min(requestedSpeed, maximumSpeed));
+                        var handoverSpeed = limitedSpeed * direction;
+                        chassis.SendXYThSpeed(handoverSpeed, 0f, 0f);
+                        handoverRequested = true;
+                        // 保持一个调度周期，让速度命令实际生效。
+                        yield return true;
+                        break;
+                    }
+                    var speed = _pid.GetResponse(targetPosition);
+                    chassis.SendXYThSpeed(speed, 0f, 0f);
+                    if (_pid.IsArrived())
+                    {
+                        break;
+                    }
+                    yield return true;
+                }
+                if (SrcId != -1 &&
+                    LeaveSrcFunction != null)
+                {
+                    LeaveSrcFunction(SrcId);
+                    DLog.Log($"释放放车点{SrcId}", "straight_line");
+                }
+                // 只有正常完成动作衔接时才允许保留非零速度。
+                keepHandoverSpeed = handoverRequested;
+            }
+            finally
+            {
+                // 普通完成、人工停止或异常退出时都必须停车。
+                if (!keepHandoverSpeed)
+                {
+                    chassis.SendXYThSpeed(0f, 0f, 0f);
+                }
+            }
+            yield return false;
+        }
+    }
+    //直线行走基于detour
+    public class LineTracking_based_detour : MovementDefinition
+    {
+        public float LineDistance = 1000f;
+        public int SrcId = -1;
+        public int DstId = -1;
+        public Action<int> LeaveSrcFunction = null;
+        public Painter painter = UI.GetPainter("Line", false);
+        // C层单车轨迹：执行早期版本的两点直线跟踪动作。
+        public override IEnumerable<bool> Get()
+        {
+            var curpose = DetourInterface.getCartLocation();
+            Console.WriteLine($"curpose.th:{curpose.th}");
+            var src = new Vector2((float)curpose.x, (float)curpose.y);
+            var headingRadians = curpose.th * Math.PI / 180.0;
+            var dst = new Vector2(
+                (float)(curpose.x +
+                    LineDistance * Math.Cos(headingRadians)),
+                (float)(curpose.y +
+                    LineDistance * Math.Sin(headingRadians)));
+            // var dst = new Vector2((float)curpose.x + LineDistance * (float)Math.Cos(curpose.th),
+            //                       (float)curpose.y + LineDistance * (float)Math.Sin(curpose.th));
+            Console.WriteLine($"src:{src.X} {src.Y}");
+            Console.WriteLine($"dst:{dst.X} {dst.Y}");
+            painter.DrawLine(Color.Green, src.X, src.Y, dst.X, dst.Y, width: 3);
+
+            var tracker = new ChassisController().Get();
+            var linePath = new LineTrack(src, dst) { CarDirectionBias = LineDistance > 0 ? 0 : 180 };
+            tracker.AddTrack(linePath);
+            var _dt = new DriveTask(tracker.Track());
+            _dt.Wait();
+            if (SrcId != -1 && LeaveSrcFunction != null)
+            {
+                LeaveSrcFunction(SrcId);
+                DLog.Log($"释放放车点{SrcId}", "straight_line");
+            }
+            yield return false;
+        }
+    }
+    #endregion
+
+    #region 旋转运动
     public class MultiWheelRotateInPlace : MovementDefinition
     {
         /// <summary>
@@ -89,7 +433,10 @@ namespace MultiWheelC
 
         public PIDController thPid;
 
-        // 将角度归一化到零到三百六十度范围内。
+        // 将本周期PID角速度输出提供给实验记录器，单位deg/s。
+        public Action<float> CommandAngularSpeedObserver;
+
+        // 归一化到大约 [-180°, 180°]
         private static float RangeAngle(float theta)
         {
             return (float)(theta - Math.Round(theta / 360.0f) * 360);
@@ -110,6 +457,7 @@ namespace MultiWheelC
                 {
                     var s = thPid.GetResponse(targetAngle, true);
                     Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
+                    CommandAngularSpeedObserver?.Invoke(s);
                     Chassis.SendXYThSpeed(0, 0, s);
                     if (thPid.IsArrived()) break;
                     yield return true;
@@ -119,10 +467,96 @@ namespace MultiWheelC
             }
             finally
             {
+                CommandAngularSpeedObserver?.Invoke(0f);
                 Chassis.SendXYThSpeed(0, 0, 0);
             }
         }
     }
+    #endregion
 
+    #region 夹臂运动
+    public class ClampToTarget : MovementDefinition
+    {
+        public float LeftClampTarget;
+        public float RightClampTarget;
+        public float MaxClampSpeed = PilotDefinition.Conf.MaxClampSpeed;
+        public float ClampKp = PilotDefinition.Conf.ClampControlKp;
+        public float ClampKi = PilotDefinition.Conf.ClampControlKi;
+        public float ClampKd = PilotDefinition.Conf.ClampControlKd;
+        public float ClampMaxI = PilotDefinition.Conf.ClampControlMaxI;
+        public float ClampSpeedAcc = PilotDefinition.Conf.ClampControlSpeedAcc;
+        public float ClampDeadZone = PilotDefinition.Conf.ClampControlDeadZone;
+        public float TimeoutSeconds = 30f;
+        private PIDController leftpid, rightpid;
 
+        // C层单车业务：驱动左右夹臂运动到夹紧或松开目标。
+        public override IEnumerable<bool> Get()
+        {
+            try
+            {
+                leftpid = new PIDController(
+                    () => PilotDefinition.Self.ActualPosLeftArm,
+                    ClampKp, ClampKi, ClampKd, ClampMaxI,
+                    ClampDeadZone, MaxClampSpeed)
+                {
+                    SpeedAccPerSec = ClampSpeedAcc
+                };
+
+                rightpid = new PIDController(
+                    () => PilotDefinition.Self.ActualPosRightArm,
+                    ClampKp, ClampKi, ClampKd, ClampMaxI,
+                    ClampDeadZone, MaxClampSpeed)
+                {
+                    SpeedAccPerSec = ClampSpeedAcc
+                };
+
+                var startTime = DateTime.UtcNow;
+                while (true)
+                {
+                    if (TimeoutSeconds > 0f &&
+                        (DateTime.UtcNow - startTime).TotalSeconds >
+                        TimeoutSeconds)
+                    {
+                        Console.WriteLine(
+                            $"夹臂运动超时({TimeoutSeconds:F1}s)，" +
+                            "停止左右夹臂。");
+                        yield break;
+                    }
+
+                    var leftspeed =
+                        leftpid.GetResponse(LeftClampTarget);
+                    var rightspeed =
+                        rightpid.GetResponse(RightClampTarget);
+                    Console.WriteLine(
+                        $"left arm speed:{leftspeed}  " +
+                        $"right arm speed:{rightspeed}");
+
+                    PilotDefinition.Self.SpeedLeftArm = leftspeed;
+                    PilotDefinition.Self.SpeedRightArm = rightspeed;
+
+                    var leftArrived = leftpid.IsArrived();
+                    var rightArrived = rightpid.IsArrived();
+                    if (leftArrived)
+                        PilotDefinition.Self.SpeedLeftArm = 0f;
+                    if (rightArrived)
+                        PilotDefinition.Self.SpeedRightArm = 0f;
+
+                    if (leftArrived && rightArrived)
+                        break;
+
+                    yield return true;
+                }
+
+                Console.WriteLine(
+                    $"left clamp to target:{LeftClampTarget}  " +
+                    $"right clamp to target:{RightClampTarget}");
+            }
+            finally
+            {
+                PilotDefinition.Self.SpeedLeftArm = 0f;
+                PilotDefinition.Self.SpeedRightArm = 0f;
+            }
+        }
+    }
+    #endregion
 }

ClumsyPilot/PilotConfig.cs

@@ -38,7 +38,7 @@ public class PilotConfig : MultiWheelPilotConfig
 
     #region 单车-临时
     [FieldMember(desc = "原地旋转Kp")]
-    public float InPlaceRotateKp = 0.05f;
+    public float InPlaceRotateKp = 0.2f;
 
     [FieldMember(desc = "原地旋转Ki")]
     public float InPlaceRotateKi = 0.01f;

ClumsyPilot/TrackingExperimentRecorder.cs

@@ -0,0 +1,394 @@
+using ClumsyCore.Interfaces;
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.Globalization;
+using System.IO;
+using System.Numerics;
+using System.Text;
+using System.Threading;
+
+namespace MultiWheelC
+{
+    // C层实验数据：保存一个采样时刻的定位与控制命令。
+    public sealed class TrackingSample
+    {
+        public double ElapsedSeconds;
+
+        // Detour位置单位为mm，航向单位为deg。
+        public double DetourX;
+        public double DetourY;
+        public double DetourTheta;
+
+        // 车体速度单位为m/s，角速度单位为deg/s。
+        public float CommandSpeed;
+        public float CommandVx;
+        public float CommandVy;
+        public float CommandAngularSpeed;
+    }
+
+    // C层实验工具：统一采集并保存轨迹跟踪实验数据。
+    public sealed class TrackingExperimentRecorder
+    {
+        private readonly string _controllerName;
+        private readonly string _trajectoryName;
+        private readonly int _trialNumber;
+        private readonly Vector2 _referenceStart;
+        private readonly Vector2 _referenceEnd;
+        private readonly float _referenceSpeed;
+        private readonly int _sampleIntervalMs;
+
+        private readonly List<TrackingSample> _samples =
+            new List<TrackingSample>();
+
+        private readonly object _sampleSyncRoot =
+            new object();
+
+        private readonly object _commandSyncRoot =
+            new object();
+
+        private readonly Stopwatch _stopwatch =
+            new Stopwatch();
+
+        private Thread _worker;
+        private volatile bool _running;
+        private int _started;
+        private int _saved;
+
+        private bool _hasExternalCommand;
+        private float _externalCommandSpeed;
+        private float _externalCommandVx;
+        private float _externalCommandVy;
+        private float _externalCommandAngularSpeed;
+
+        public TrackingExperimentRecorder(
+            string controllerName,
+            string trajectoryName,
+            int trialNumber,
+            Vector2 referenceStart,
+            Vector2 referenceEnd,
+            float referenceSpeed,
+            int sampleIntervalMs = 50)
+        {
+            if (string.IsNullOrWhiteSpace(controllerName))
+                throw new ArgumentException(
+                    "控制器名称不能为空。",
+                    nameof(controllerName));
+
+            if (string.IsNullOrWhiteSpace(trajectoryName))
+                throw new ArgumentException(
+                    "轨迹名称不能为空。",
+                    nameof(trajectoryName));
+
+            if (sampleIntervalMs <= 0)
+                throw new ArgumentOutOfRangeException(
+                    nameof(sampleIntervalMs),
+                    "采样周期必须大于零。");
+
+            _controllerName = controllerName;
+            _trajectoryName = trajectoryName;
+            _trialNumber = trialNumber;
+            _referenceStart = referenceStart;
+            _referenceEnd = referenceEnd;
+            _referenceSpeed = referenceSpeed;
+            _sampleIntervalMs = sampleIntervalMs;
+        }
+
+        // 保存成功后的CSV绝对路径；尚未保存时为空。
+        public string SavedFilePath { get; private set; }
+
+        // 启动后台采样线程。
+        public void Start()
+        {
+            if (Interlocked.Exchange(ref _started, 1) != 0)
+                return;
+
+            _stopwatch.Restart();
+            _running = true;
+
+            // 立即保存起点静止状态，避免第一帧被后台线程延迟。
+            CaptureSample();
+
+            _worker = new Thread(SamplingLoop)
+            {
+                IsBackground = true,
+                Name = "TrackingExperimentRecorder"
+            };
+            _worker.Start();
+        }
+
+        // 供Stanley/LQR控制器主动写入本周期最终速度命令。
+        // 调用后优先记录该命令，不再使用底盘反解值。
+        public void UpdateCommand(
+            float commandSpeed,
+            float commandAngularSpeed)
+        {
+            lock (_commandSyncRoot)
+            {
+                _externalCommandSpeed = commandSpeed;
+                _externalCommandVx = commandSpeed;
+                _externalCommandVy = 0f;
+                _externalCommandAngularSpeed =
+                    commandAngularSpeed;
+                _hasExternalCommand = true;
+            }
+        }
+
+        // 供全向、蟹行和曲线控制器写入完整车体速度命令。
+        public void UpdateBodyCommand(
+            float commandVx,
+            float commandVy,
+            float commandAngularSpeed)
+        {
+            lock (_commandSyncRoot)
+            {
+                _externalCommandVx = commandVx;
+                _externalCommandVy = commandVy;
+                _externalCommandSpeed =
+                    (float)Math.Sqrt(
+                        commandVx * commandVx +
+                        commandVy * commandVy);
+                _externalCommandAngularSpeed =
+                    commandAngularSpeed;
+                _hasExternalCommand = true;
+            }
+        }
+
+        // 停止采样并将本次实验保存为CSV；重复调用只保存一次。
+        public void StopAndSave()
+        {
+            if (Volatile.Read(ref _started) == 0)
+                return;
+
+            if (Interlocked.Exchange(ref _saved, 1) != 0)
+                return;
+
+            try
+            {
+                _running = false;
+
+                if (_worker != null &&
+                    _worker != Thread.CurrentThread)
+                {
+                    _worker.Join(
+                        Math.Max(1000, _sampleIntervalMs * 4));
+                }
+
+                // 保存停止时刻的最后一帧。
+                CaptureSample();
+                _stopwatch.Stop();
+                SaveCsv();
+
+                Console.WriteLine(
+                    $"轨迹实验数据已保存：{SavedFilePath}");
+            }
+            catch
+            {
+                // 保存失败后允许调用者再次尝试。
+                Interlocked.Exchange(ref _saved, 0);
+                throw;
+            }
+        }
+
+        // 按固定周期采集Detour位姿和控制命令。
+        private void SamplingLoop()
+        {
+            while (_running)
+            {
+                Thread.Sleep(_sampleIntervalMs);
+
+                if (!_running)
+                    break;
+
+                CaptureSample();
+            }
+        }
+
+        // 采集一帧Detour位姿和控制命令。
+        private void CaptureSample()
+        {
+            try
+            {
+                var location =
+                    DetourInterface.getCartLocation();
+
+                float commandSpeed;
+                float commandVx;
+                float commandVy;
+                float commandAngularSpeed;
+
+                lock (_commandSyncRoot)
+                {
+                    if (_hasExternalCommand)
+                    {
+                        commandSpeed =
+                            _externalCommandSpeed;
+                        commandVx =
+                            _externalCommandVx;
+                        commandVy =
+                            _externalCommandVy;
+                        commandAngularSpeed =
+                            _externalCommandAngularSpeed;
+                    }
+                    else
+                    {
+                        var command =
+                            PilotDefinition.Chassis
+                                .GetCarSpeed(false);
+
+                        commandVx = command.Vx;
+                        commandVy = command.Vy;
+                        commandAngularSpeed = command.Vw;
+                        commandSpeed = (float)Math.Sqrt(
+                            commandVx * commandVx +
+                            commandVy * commandVy);
+                    }
+                }
+
+                var sample = new TrackingSample
+                {
+                    ElapsedSeconds =
+                        _stopwatch.Elapsed.TotalSeconds,
+                    DetourX = location.x,
+                    DetourY = location.y,
+                    DetourTheta = location.th,
+                    CommandSpeed = commandSpeed,
+                    CommandVx = commandVx,
+                    CommandVy = commandVy,
+                    CommandAngularSpeed =
+                        commandAngularSpeed
+                };
+
+                lock (_sampleSyncRoot)
+                {
+                    _samples.Add(sample);
+                }
+            }
+            catch (Exception ex)
+            {
+                // 单帧读取失败不应终止车辆控制或整个记录线程。
+                Console.WriteLine(
+                    $"轨迹实验采样失败：{ex.Message}");
+            }
+        }
+
+        // 将内存中的采样数据写入CSV。
+        private void SaveCsv()
+        {
+            List<TrackingSample> snapshot;
+
+            lock (_sampleSyncRoot)
+            {
+                snapshot =
+                    new List<TrackingSample>(_samples);
+            }
+
+            var outputDirectory = Path.Combine(
+                AppContext.BaseDirectory,
+                "TrackingExperiments");
+
+            Directory.CreateDirectory(outputDirectory);
+
+            var fileName =
+                $"{DateTime.Now:yyyyMMdd_HHmmss_fff}_" +
+                $"{SanitizeFileName(_controllerName)}_" +
+                $"{SanitizeFileName(_trajectoryName)}_" +
+                $"Trial{_trialNumber}.csv";
+
+            SavedFilePath = Path.Combine(
+                outputDirectory,
+                fileName);
+
+            using (var writer = new StreamWriter(
+                SavedFilePath,
+                false,
+                new UTF8Encoding(true)))
+            {
+                writer.WriteLine(
+                    "ElapsedSeconds," +
+                    "ControllerName," +
+                    "TrajectoryName," +
+                    "TrialNumber," +
+                    "DetourX," +
+                    "DetourY," +
+                    "DetourTheta," +
+                    "CommandSpeed," +
+                    "CommandAngularSpeed," +
+                    "CommandVx," +
+                    "CommandVy," +
+                    "ReferenceStartX," +
+                    "ReferenceStartY," +
+                    "ReferenceEndX," +
+                    "ReferenceEndY," +
+                    "ReferenceSpeed");
+
+                foreach (var sample in snapshot)
+                {
+                    writer.WriteLine(string.Join(
+                        ",",
+                        Format(sample.ElapsedSeconds),
+                        EscapeCsv(_controllerName),
+                        EscapeCsv(_trajectoryName),
+                        _trialNumber.ToString(
+                            CultureInfo.InvariantCulture),
+                        Format(sample.DetourX),
+                        Format(sample.DetourY),
+                        Format(sample.DetourTheta),
+                        Format(sample.CommandSpeed),
+                        Format(sample.CommandAngularSpeed),
+                        Format(sample.CommandVx),
+                        Format(sample.CommandVy),
+                        Format(_referenceStart.X),
+                        Format(_referenceStart.Y),
+                        Format(_referenceEnd.X),
+                        Format(_referenceEnd.Y),
+                        Format(_referenceSpeed)));
+                }
+            }
+        }
+
+        // 将文件名中的非法字符替换为下划线。
+        private static string SanitizeFileName(string value)
+        {
+            var result = value;
+
+            foreach (var invalidCharacter in
+                     Path.GetInvalidFileNameChars())
+            {
+                result = result.Replace(
+                    invalidCharacter,
+                    '_');
+            }
+
+            return result;
+        }
+
+        // 按固定小数格式输出数值，避免系统区域设置改变CSV格式。
+        private static string Format(double value)
+        {
+            return value.ToString(
+                "0.######",
+                CultureInfo.InvariantCulture);
+        }
+
+        // 对CSV文本字段进行引号和逗号转义。
+        private static string EscapeCsv(string value)
+        {
+            if (value == null)
+                return string.Empty;
+
+            if (!value.Contains(",") &&
+                !value.Contains("\"") &&
+                !value.Contains("\r") &&
+                !value.Contains("\n"))
+            {
+                return value;
+            }
+
+            return
+                "\"" +
+                value.Replace("\"", "\"\"") +
+                "\"";
+        }
+    }
+}

ClumsyPilot/obj/Debug/ClumsyPilot.AssemblyInfo.cs

@@ -13,7 +13,7 @@ using System.Reflection;
 [assembly: System.Reflection.AssemblyCompanyAttribute("ClumsyPilot")]
 [assembly: System.Reflection.AssemblyConfigurationAttribute("Debug")]
 [assembly: System.Reflection.AssemblyFileVersionAttribute("1.0.0.0")]
-[assembly: System.Reflection.AssemblyInformationalVersionAttribute("1.0.0+580a936a830dcb7a25ef327cf553341405264033")]
+[assembly: System.Reflection.AssemblyInformationalVersionAttribute("1.0.0+e6b99c45b352f24ff58092f7855d0eadd8828d42")]
 [assembly: System.Reflection.AssemblyProductAttribute("ClumsyPilot")]
 [assembly: System.Reflection.AssemblyTitleAttribute("ClumsyPilot")]
 [assembly: System.Reflection.AssemblyVersionAttribute("1.0.0.0")]

ClumsyPilot/obj/Debug/ClumsyPilot.AssemblyInfoInputs.cache

@@ -1 +1 @@
-038d57c5b1714403d308c9343b385ef762951607b63a9fb275f4d7d2b8fd29cb
+3e5507bf56e38facdc245d5ff407e181c060d8094f38b94ad743b8c3305523f2

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

@@ -1 +1 @@
-3920568398d269aea7b71fb4581ad111777c15ca3f2f2bb272ee9e0a989215c4
+e5afef3021b83202643b31f468b83ac62f224cd367fe52dc0a1cbb98570c91e1

data_process/plot_angular_command.py

@@ -0,0 +1,100 @@
+"""绘制控制器下发角速度命令曲线。"""
+
+from __future__ import annotations
+
+import argparse
+from pathlib import Path
+
+import matplotlib
+matplotlib.use("Agg")
+import matplotlib.pyplot as plt
+import numpy as np
+
+from plot_trajectory_comparison import (
+    configure_matplotlib,
+    discover_csv_files,
+    load_and_resample,
+    output_path,
+    shade_localization_jump_windows,
+)
+
+
+def plot_angular_command(
+    csv_path: Path,
+    frequency_hz: float,
+    filter_window_seconds: float,
+    output_directory: str | None,
+    show: bool,
+) -> Path:
+    """生成单份CSV的命令角速度曲线。"""
+    frame, metadata = load_and_resample(
+        csv_path,
+        frequency_hz,
+        filter_window_seconds,
+    )
+    time = frame["TimeSeconds"].to_numpy(dtype=float)
+    angular_command = frame[
+        "CommandAngularSpeedDegPerSec"
+    ].to_numpy(dtype=float)
+    maximum = float(np.max(angular_command))
+    minimum = float(np.min(angular_command))
+
+    fig, ax = plt.subplots(figsize=(10.0, 5.5))
+    ax.plot(
+        time,
+        angular_command,
+        color="tab:red",
+        linewidth=1.6,
+        label="CommandAngularSpeed",
+    )
+    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_title(
+        f"角速度指令曲线\n"
+        f"{metadata['controller_name']} - "
+        f"{metadata['trajectory_name']}，"
+        f"范围=[{minimum:.3f}, {maximum:.3f}]°/s"
+    )
+    ax.grid(True, alpha=0.3)
+    ax.legend()
+    fig.tight_layout()
+
+    destination = output_path(
+        csv_path,
+        output_directory,
+        "angular_command",
+    )
+    fig.savefig(destination, dpi=300, bbox_inches="tight")
+    if show:
+        plt.show()
+    plt.close(fig)
+    return destination
+
+
+def main() -> None:
+    configure_matplotlib()
+    parser = argparse.ArgumentParser(
+        description="绘制控制器下发角速度命令曲线。"
+    )
+    parser.add_argument("files", nargs="*", help="一个或多个CSV文件")
+    parser.add_argument("--frequency", type=float, default=20.0)
+    parser.add_argument("--window", type=float, default=0.55)
+    parser.add_argument("--output-dir")
+    parser.add_argument("--show", action="store_true")
+    args = parser.parse_args()
+
+    for csv_path in discover_csv_files(args.files):
+        destination = plot_angular_command(
+            csv_path,
+            args.frequency,
+            args.window,
+            args.output_dir,
+            args.show,
+        )
+        print(f"已生成：{destination}")
+
+
+if __name__ == "__main__":
+    main()

data_process/plot_speed_response.py

@@ -0,0 +1,177 @@
+"""绘制控制器参考速度与Detour差分实际速度对比图。"""
+
+from __future__ import annotations
+
+import argparse
+from pathlib import Path
+
+import matplotlib
+matplotlib.use("Agg")
+import matplotlib.pyplot as plt
+import numpy as np
+
+from plot_trajectory_comparison import (
+    configure_matplotlib,
+    discover_csv_files,
+    load_and_resample,
+    output_path,
+    segmented_savgol,
+    shade_localization_jump_windows,
+)
+
+
+def calculate_actual_speed_mps(
+    frame,
+    filter_window_seconds: float,
+) -> np.ndarray:
+    """使用Savitzky-Golay求位置导数并计算Detour实际合速度。"""
+    time = frame["TimeSeconds"].to_numpy(dtype=float)
+    dt = float(np.median(np.diff(time)))
+    # 直接对固定频率重采样后的位置做SG求导，避免“先平滑再求导”
+    # 造成两次滤波和过度削弱速度峰值。
+    x_mm = frame["DetourXRawMm"].to_numpy(dtype=float)
+    y_mm = frame["DetourYRawMm"].to_numpy(dtype=float)
+    vx_mm_per_second = segmented_savgol(
+        x_mm,
+        dt,
+        filter_window_seconds,
+        derivative=1,
+    )
+    vy_mm_per_second = segmented_savgol(
+        y_mm,
+        dt,
+        filter_window_seconds,
+        derivative=1,
+    )
+
+    speed = np.hypot(
+        vx_mm_per_second,
+        vy_mm_per_second,
+    ) / 1000.0
+    speed[
+        frame["InvalidNearLocalizationJump"].to_numpy(dtype=bool)
+    ] = np.nan
+    return speed
+
+
+def plot_speed(
+    csv_path: Path,
+    frequency_hz: float,
+    filter_window_seconds: float,
+    output_directory: str | None,
+    show: bool,
+) -> Path:
+    """生成单份CSV的参考/实际速度响应图。"""
+    frame, metadata = load_and_resample(
+        csv_path,
+        frequency_hz,
+        filter_window_seconds,
+    )
+    time = frame["TimeSeconds"].to_numpy(dtype=float)
+    command_speed = frame["CommandSpeedMps"].to_numpy(dtype=float)
+    actual_speed = calculate_actual_speed_mps(
+        frame,
+        filter_window_seconds,
+    )
+    is_in_place_rotation = (
+        str(metadata["trajectory_name"])
+        .lower()
+        .startswith("rotate")
+    )
+    # 原地自转CSV中的ReferenceSpeed历史上保存的是角速度上限deg/s，
+    # 不能作为线速度m/s使用；其参考线速度应为0。
+    configured_speed = (
+        0.0
+        if is_in_place_rotation
+        else float(metadata["reference_speed_mps"])
+    )
+
+    moving = (
+        (command_speed > max(0.02, configured_speed * 0.1)) &
+        np.isfinite(actual_speed)
+    )
+    if np.any(moving):
+        speed_rmse = float(
+            np.sqrt(
+                np.mean(
+                    (actual_speed[moving] - command_speed[moving]) ** 2
+                )
+            )
+        )
+    else:
+        speed_rmse = float("nan")
+
+    fig, ax = plt.subplots(figsize=(10.0, 5.8))
+    ax.plot(
+        time,
+        command_speed,
+        linewidth=1.8,
+        label="控制器参考/下发线速度",
+    )
+    ax.plot(
+        time,
+        actual_speed,
+        linewidth=1.5,
+        label="Detour差分实际线速度（SG求导）",
+    )
+    ax.axhline(
+        configured_speed,
+        linestyle=":",
+        linewidth=1.3,
+        color="tab:green",
+        label=(
+            "原地自转参考线速度 0 m/s"
+            if is_in_place_rotation
+            else f"配置巡航速度 {configured_speed:.3f} m/s"
+        ),
+    )
+    shade_localization_jump_windows(ax, metadata)
+    ax.set_xlabel("时间 / s")
+    ax.set_ylabel("线速度 / (m/s)")
+    ax.set_title(
+        f"参考速度与实际速度对比\n"
+        f"{metadata['controller_name']} - "
+        f"{metadata['trajectory_name']}，"
+        f"运动段RMSE={speed_rmse:.4f} m/s"
+    )
+    ax.grid(True, alpha=0.3)
+    ax.legend()
+    fig.tight_layout()
+
+    destination = output_path(
+        csv_path,
+        output_directory,
+        "speed_response",
+    )
+    fig.savefig(destination, dpi=300, bbox_inches="tight")
+    if show:
+        plt.show()
+    plt.close(fig)
+    return destination
+
+
+def main() -> None:
+    configure_matplotlib()
+    parser = argparse.ArgumentParser(
+        description="绘制参考速度与Detour差分实际速度对比图。"
+    )
+    parser.add_argument("files", nargs="*", help="一个或多个CSV文件")
+    parser.add_argument("--frequency", type=float, default=20.0)
+    parser.add_argument("--window", type=float, default=0.55)
+    parser.add_argument("--output-dir")
+    parser.add_argument("--show", action="store_true")
+    args = parser.parse_args()
+
+    for csv_path in discover_csv_files(args.files):
+        destination = plot_speed(
+            csv_path,
+            args.frequency,
+            args.window,
+            args.output_dir,
+            args.show,
+        )
+        print(f"已生成：{destination}")
+
+
+if __name__ == "__main__":
+    main()

data_process/plot_tracking_errors.py

@@ -0,0 +1,172 @@
+"""绘制横向误差和航向误差随时间变化图。"""
+
+from __future__ import annotations
+
+import argparse
+from pathlib import Path
+
+import matplotlib
+matplotlib.use("Agg")
+import matplotlib.pyplot as plt
+import numpy as np
+
+from plot_trajectory_comparison import (
+    build_reference,
+    configure_matplotlib,
+    discover_csv_files,
+    load_and_resample,
+    output_path,
+    shade_localization_jump_windows,
+)
+
+
+def plot_errors(
+    csv_path: Path,
+    frequency_hz: float,
+    filter_window_seconds: float,
+    output_directory: str | None,
+    show: bool,
+) -> Path:
+    """生成单份CSV的横向/航向误差图。"""
+    frame, metadata = load_and_resample(
+        csv_path,
+        frequency_hz,
+        filter_window_seconds,
+    )
+    reference = build_reference(frame, metadata)
+    time = frame["TimeSeconds"].to_numpy()
+    lateral = np.asarray(reference["lateral_error_mm"])
+    heading = np.asarray(reference["heading_error_degrees"])
+    invalid = frame[
+        "InvalidNearLocalizationJump"
+    ].to_numpy(dtype=bool)
+    lateral_for_statistics = lateral.copy()
+    heading_for_statistics = heading.copy()
+    lateral_for_statistics[invalid] = np.nan
+    heading_for_statistics[invalid] = np.nan
+
+    lateral_rmse = float(
+        np.sqrt(np.nanmean(lateral_for_statistics**2))
+    )
+    heading_rmse = float(
+        np.sqrt(np.nanmean(heading_for_statistics**2))
+    )
+    lateral_max = float(
+        np.nanmax(np.abs(lateral_for_statistics))
+    )
+    heading_max = float(
+        np.nanmax(np.abs(heading_for_statistics))
+    )
+    is_in_place_rotation = (
+        reference["kind"] == "in_place_rotation"
+    )
+
+    fig, axes = plt.subplots(
+        2,
+        1,
+        figsize=(10.0, 7.0),
+        sharex=True,
+    )
+    axes[0].plot(time, lateral, linewidth=1.5)
+    axes[0].axhline(0.0, color="black", linewidth=0.8)
+    if is_in_place_rotation:
+        axes[0].set_ylabel("旋转中心位置漂移 / mm")
+        axes[0].set_title(
+            f"原地自转位置漂移：RMS={lateral_rmse:.2f} mm，"
+            f"最大值={lateral_max:.2f} mm"
+        )
+    else:
+        axes[0].set_ylabel("横向误差 / mm")
+        axes[0].set_title(
+            f"横向误差：RMSE={lateral_rmse:.2f} mm，"
+            f"最大绝对值={lateral_max:.2f} mm"
+        )
+    shade_localization_jump_windows(axes[0], metadata)
+    axes[0].grid(True, alpha=0.3)
+
+    axes[1].plot(
+        time,
+        heading,
+        color="tab:orange",
+        linewidth=1.5,
+    )
+    axes[1].axhline(0.0, color="black", linewidth=0.8)
+    axes[1].set_xlabel("时间 / s")
+    axes[1].set_ylabel(
+        "目标角度剩余误差 / °"
+        if is_in_place_rotation
+        else "航向误差 / °"
+    )
+    axes[1].set_title(
+        (
+            f"目标角度剩余误差：RMSE={heading_rmse:.2f}°，"
+            f"最大绝对值={heading_max:.2f}°"
+        )
+        if is_in_place_rotation
+        else (
+            f"航向误差：RMSE={heading_rmse:.2f}°，"
+            f"最大绝对值={heading_max:.2f}°"
+        )
+    )
+    shade_localization_jump_windows(axes[1], metadata)
+    axes[1].grid(True, alpha=0.3)
+    if metadata["localization_jump_events"]:
+        axes[1].legend(loc="best")
+
+    fig.suptitle(
+        f"横向/航向误差随时间变化\n"
+        f"{metadata['controller_name']} - "
+        f"{metadata['trajectory_name']}"
+    )
+    fig.tight_layout()
+
+    destination = output_path(
+        csv_path,
+        output_directory,
+        "tracking_errors",
+    )
+    fig.savefig(destination, dpi=300, bbox_inches="tight")
+    if show:
+        plt.show()
+    plt.close(fig)
+
+    if is_in_place_rotation:
+        print(
+            f"{csv_path.name}: position drift RMS="
+            f"{lateral_rmse:.3f} mm, "
+            f"target-angle error RMS={heading_rmse:.3f} deg"
+        )
+    else:
+        print(
+            f"{csv_path.name}: lateral RMSE="
+            f"{lateral_rmse:.3f} mm, "
+            f"heading RMSE={heading_rmse:.3f} deg"
+        )
+    return destination
+
+
+def main() -> None:
+    configure_matplotlib()
+    parser = argparse.ArgumentParser(
+        description="绘制横向误差和航向误差随时间变化图。"
+    )
+    parser.add_argument("files", nargs="*", help="一个或多个CSV文件")
+    parser.add_argument("--frequency", type=float, default=20.0)
+    parser.add_argument("--window", type=float, default=0.55)
+    parser.add_argument("--output-dir")
+    parser.add_argument("--show", action="store_true")
+    args = parser.parse_args()
+
+    for csv_path in discover_csv_files(args.files):
+        destination = plot_errors(
+            csv_path,
+            args.frequency,
+            args.window,
+            args.output_dir,
+            args.show,
+        )
+        print(f"已生成：{destination}")
+
+
+if __name__ == "__main__":
+    main()

data_process/plot_trajectory_comparison.py

@@ -0,0 +1,706 @@
+"""绘制理想轨迹与Detour实际轨迹对比图。
+
+不传CSV路径时，默认处理本脚本目录下的全部CSV文件。
+本文件也提供其余三个绘图脚本共用的数据预处理函数。
+"""
+
+from __future__ import annotations
+
+import argparse
+import re
+from pathlib import Path
+from typing import Any
+
+import matplotlib
+matplotlib.use("Agg")
+import matplotlib.pyplot as plt
+import numpy as np
+import pandas as pd
+from scipy.signal import savgol_filter
+
+
+SCRIPT_DIR = Path(__file__).resolve().parent
+REQUIRED_COLUMNS = {
+    "ElapsedSeconds",
+    "TrajectoryName",
+    "DetourX",
+    "DetourY",
+    "DetourTheta",
+    "CommandSpeed",
+    "CommandAngularSpeed",
+    "ReferenceStartX",
+    "ReferenceStartY",
+    "ReferenceEndX",
+    "ReferenceEndY",
+    "ReferenceSpeed",
+}
+
+
+def configure_matplotlib() -> None:
+    """配置中文字体和图片输出风格。"""
+    matplotlib.rcParams["font.sans-serif"] = [
+        "Microsoft YaHei",
+        "SimHei",
+        "Arial Unicode MS",
+        "DejaVu Sans",
+    ]
+    matplotlib.rcParams["axes.unicode_minus"] = False
+    matplotlib.rcParams["figure.dpi"] = 120
+
+
+def _odd_window_length(
+    sample_count: int,
+    sample_interval: float,
+    window_seconds: float,
+    polynomial_order: int = 2,
+) -> int | None:
+    """计算不超过数据长度的Savitzky-Golay奇数窗口。"""
+    requested = max(
+        polynomial_order + 2,
+        int(round(window_seconds / sample_interval)),
+    )
+    if requested % 2 == 0:
+        requested += 1
+
+    maximum = sample_count if sample_count % 2 == 1 else sample_count - 1
+    window = min(requested, maximum)
+    minimum = polynomial_order + 2
+    if minimum % 2 == 0:
+        minimum += 1
+
+    return window if window >= minimum else None
+
+
+def wrap_degrees(angle_degrees: np.ndarray) -> np.ndarray:
+    """将角度差归一化到[-180°, 180°)。"""
+    return (angle_degrees + 180.0) % 360.0 - 180.0
+
+
+def segmented_savgol(
+    values: np.ndarray,
+    sample_interval: float,
+    window_seconds: float,
+    derivative: int = 0,
+    polynomial_order: int = 2,
+) -> np.ndarray:
+    """对含NaN断点的数据逐段执行SG滤波或求导。"""
+    values = np.asarray(values, dtype=float)
+    result = np.full_like(values, np.nan)
+    finite_indices = np.flatnonzero(np.isfinite(values))
+    if finite_indices.size == 0:
+        return result
+
+    breaks = np.flatnonzero(np.diff(finite_indices) > 1)
+    starts = np.r_[0, breaks + 1]
+    ends = np.r_[breaks + 1, finite_indices.size]
+
+    for start_index, end_index in zip(starts, ends):
+        indices = finite_indices[start_index:end_index]
+        segment = values[indices]
+        window = _odd_window_length(
+            len(segment),
+            sample_interval,
+            window_seconds,
+            polynomial_order,
+        )
+        if window is not None:
+            result[indices] = savgol_filter(
+                segment,
+                window,
+                polynomial_order,
+                deriv=derivative,
+                delta=sample_interval,
+                mode="interp",
+            )
+        elif derivative == 0:
+            result[indices] = segment
+        elif len(segment) >= 2:
+            result[indices] = np.gradient(segment, sample_interval)
+
+    return result
+
+
+def shade_localization_jump_windows(
+    axis,
+    metadata: dict[str, Any],
+) -> None:
+    """在时间曲线中标记不应参与车辆动力学评价的定位跳变窗口。"""
+    for index, (start, end) in enumerate(
+        metadata["jump_exclusion_windows"]
+    ):
+        axis.axvspan(
+            start,
+            end,
+            color="tab:red",
+            alpha=0.12,
+            label="Detour定位跳变排除窗口" if index == 0 else None,
+        )
+
+
+def load_and_resample(
+    csv_path: Path,
+    frequency_hz: float = 20.0,
+    filter_window_seconds: float = 0.55,
+) -> tuple[pd.DataFrame, dict[str, Any]]:
+    """压缩Detour保持帧，检测定位跳变，再分段重采样和平滑。"""
+    if not np.isfinite(frequency_hz) or frequency_hz <= 0.0:
+        raise ValueError("重采样频率必须是正有限值。")
+
+    raw = pd.read_csv(csv_path)
+    missing = REQUIRED_COLUMNS.difference(raw.columns)
+    if missing:
+        raise ValueError(
+            f"{csv_path.name}缺少列：{', '.join(sorted(missing))}"
+        )
+
+    numeric_columns = [
+        "ElapsedSeconds",
+        "DetourX",
+        "DetourY",
+        "DetourTheta",
+        "CommandSpeed",
+        "CommandAngularSpeed",
+        "ReferenceStartX",
+        "ReferenceStartY",
+        "ReferenceEndX",
+        "ReferenceEndY",
+        "ReferenceSpeed",
+    ]
+    for column in numeric_columns:
+        raw[column] = pd.to_numeric(raw[column], errors="coerce")
+
+    raw = (
+        raw.dropna(subset=[
+            "ElapsedSeconds",
+            "DetourX",
+            "DetourY",
+            "DetourTheta",
+        ])
+        .sort_values("ElapsedSeconds")
+        .drop_duplicates("ElapsedSeconds", keep="last")
+        .reset_index(drop=True)
+    )
+    if len(raw) < 5:
+        raise ValueError(f"{csv_path.name}有效数据不足5行。")
+
+    time_raw = raw["ElapsedSeconds"].to_numpy(dtype=float)
+    time_raw = time_raw - time_raw[0]
+    raw["ElapsedSeconds"] = time_raw
+    duration = float(time_raw[-1])
+    sample_interval = 1.0 / frequency_hz
+    time_uniform = np.arange(
+        0.0,
+        duration + sample_interval * 0.5,
+        sample_interval,
+    )
+
+    def interpolate_command(column: str) -> np.ndarray:
+        values = raw[column].to_numpy(dtype=float)
+        return np.interp(time_uniform, time_raw, values)
+
+    # 记录器频率高于Detour更新频率，会得到A,A,B,B形式的保持帧。
+    # 速度估计前先保留真正发生位姿更新的样本。
+    x_all = raw["DetourX"].to_numpy(dtype=float)
+    y_all = raw["DetourY"].to_numpy(dtype=float)
+    theta_all = raw["DetourTheta"].to_numpy(dtype=float)
+    position_change = np.hypot(np.diff(x_all), np.diff(y_all))
+    heading_change = np.abs(wrap_degrees(np.diff(theta_all)))
+    update_mask = np.r_[
+        True,
+        (position_change > 1e-6) | (heading_change > 1e-6),
+    ]
+    updates = raw.loc[update_mask].copy().reset_index(drop=True)
+    if len(updates) < 3:
+        raise ValueError(f"{csv_path.name}有效Detour更新点不足3个。")
+
+    update_time = updates["ElapsedSeconds"].to_numpy(dtype=float)
+    update_x = updates["DetourX"].to_numpy(dtype=float)
+    update_y = updates["DetourY"].to_numpy(dtype=float)
+    update_theta = updates["DetourTheta"].to_numpy(dtype=float)
+    update_command_speed = np.abs(
+        updates["CommandSpeed"].to_numpy(dtype=float)
+    )
+    update_command_angular = np.abs(
+        updates["CommandAngularSpeed"].to_numpy(dtype=float)
+    )
+
+    # 自适应跳变阈值：正常移动允许达到参考位移的3倍并保留15mm余量；
+    # 低速阶段仍至少允许30mm，防止把普通定位噪声误判为跳变。
+    update_dt = np.diff(update_time)
+    update_distance = np.hypot(np.diff(update_x), np.diff(update_y))
+    expected_distance = (
+        0.5 *
+        (update_command_speed[1:] + update_command_speed[:-1]) *
+        update_dt *
+        1000.0
+    )
+    distance_threshold = np.maximum(
+        30.0,
+        expected_distance * 3.0 + 15.0,
+    )
+    update_heading_delta = np.abs(
+        wrap_degrees(np.diff(update_theta))
+    )
+    expected_heading_delta = (
+        0.5 *
+        (update_command_angular[1:] + update_command_angular[:-1]) *
+        update_dt
+    )
+    heading_threshold = np.maximum(
+        5.0,
+        expected_heading_delta * 3.0 + 2.0,
+    )
+    jump_before_current = (
+        (update_distance > distance_threshold) |
+        (update_heading_delta > heading_threshold)
+    )
+    jump_at_update = np.r_[False, jump_before_current]
+    segment_ids = np.cumsum(jump_at_update.astype(int))
+
+    jump_events: list[dict[str, float]] = []
+    for current_index in np.flatnonzero(jump_at_update):
+        previous_index = current_index - 1
+        jump_events.append({
+            "time_seconds": float(update_time[current_index]),
+            "distance_mm": float(update_distance[previous_index]),
+            "heading_change_degrees":
+                float(update_heading_delta[previous_index]),
+            "before_x_mm": float(update_x[previous_index]),
+            "before_y_mm": float(update_y[previous_index]),
+            "after_x_mm": float(update_x[current_index]),
+            "after_y_mm": float(update_y[current_index]),
+        })
+
+    # 不跨越定位跳变插值。跳变前后之间保留NaN，使轨迹图自然断线，
+    # 也防止SG滤波把坐标修正涂抹成车辆高速运动。
+    x_resampled = np.full_like(time_uniform, np.nan)
+    y_resampled = np.full_like(time_uniform, np.nan)
+    theta_resampled = np.full_like(time_uniform, np.nan)
+    update_theta_unwrapped = np.rad2deg(
+        np.unwrap(np.deg2rad(update_theta))
+    )
+    maximum_segment_id = int(segment_ids[-1])
+    for segment_id in range(maximum_segment_id + 1):
+        segment_mask = segment_ids == segment_id
+        segment_time = update_time[segment_mask]
+        if segment_time.size == 0:
+            continue
+
+        interval_start = (
+            0.0 if segment_id == 0 else float(segment_time[0])
+        )
+        interval_end = (
+            duration
+            if segment_id == maximum_segment_id
+            else float(segment_time[-1])
+        )
+        uniform_mask = (
+            (time_uniform >= interval_start) &
+            (time_uniform <= interval_end)
+        )
+        x_resampled[uniform_mask] = np.interp(
+            time_uniform[uniform_mask],
+            segment_time,
+            update_x[segment_mask],
+        )
+        y_resampled[uniform_mask] = np.interp(
+            time_uniform[uniform_mask],
+            segment_time,
+            update_y[segment_mask],
+        )
+        theta_resampled[uniform_mask] = np.interp(
+            time_uniform[uniform_mask],
+            segment_time,
+            update_theta_unwrapped[segment_mask],
+        )
+
+    x_filtered = segmented_savgol(
+        x_resampled,
+        sample_interval,
+        filter_window_seconds,
+    )
+    y_filtered = segmented_savgol(
+        y_resampled,
+        sample_interval,
+        filter_window_seconds,
+    )
+    theta_filtered = segmented_savgol(
+        theta_resampled,
+        sample_interval,
+        filter_window_seconds,
+    )
+
+    exclusion_half_width = max(
+        0.30,
+        filter_window_seconds * 0.5,
+    )
+    jump_exclusion_windows = [
+        (
+            max(0.0, event["time_seconds"] - exclusion_half_width),
+            min(duration, event["time_seconds"] + exclusion_half_width),
+        )
+        for event in jump_events
+    ]
+    invalid_near_jump = np.zeros(len(time_uniform), dtype=bool)
+    for start, end in jump_exclusion_windows:
+        invalid_near_jump |= (
+            (time_uniform >= start) & (time_uniform <= end)
+        )
+
+    frame = pd.DataFrame({
+        "TimeSeconds": time_uniform,
+        "DetourXRawMm": x_resampled,
+        "DetourYRawMm": y_resampled,
+        "DetourXFilteredMm": x_filtered,
+        "DetourYFilteredMm": y_filtered,
+        "DetourThetaUnwrappedDeg": theta_filtered,
+        "DetourThetaDeg": wrap_degrees(theta_filtered),
+        "CommandSpeedMps": interpolate_command("CommandSpeed"),
+        "CommandAngularSpeedDegPerSec":
+            interpolate_command("CommandAngularSpeed"),
+        "InvalidNearLocalizationJump": invalid_near_jump,
+    })
+
+    first = raw.iloc[0]
+    metadata: dict[str, Any] = {
+        "csv_path": csv_path,
+        "trajectory_name": str(first["TrajectoryName"]),
+        "controller_name": str(first.get("ControllerName", "")),
+        "trial_number": str(first.get("TrialNumber", "")),
+        "reference_start_mm": np.array(
+            [first["ReferenceStartX"], first["ReferenceStartY"]],
+            dtype=float,
+        ),
+        "reference_end_mm": np.array(
+            [first["ReferenceEndX"], first["ReferenceEndY"]],
+            dtype=float,
+        ),
+        "reference_speed_mps": float(first["ReferenceSpeed"]),
+        # 圆弧构造时使用了测试开始处Detour航向，因此这里取首帧航向。
+        "start_heading_degrees": float(first["DetourTheta"]),
+        "sample_interval_seconds": sample_interval,
+        "filter_window_seconds": filter_window_seconds,
+        "raw_sample_count": len(raw),
+        "detour_update_count": len(updates),
+        "held_sample_count": int(len(raw) - len(updates)),
+        "localization_jump_events": jump_events,
+        "jump_exclusion_windows": jump_exclusion_windows,
+    }
+    return frame, metadata
+
+
+def build_reference(
+    frame: pd.DataFrame,
+    metadata: dict[str, Any],
+) -> dict[str, np.ndarray | float | str]:
+    """根据CSV元数据建立直线、圆弧或原地自转参考及误差。"""
+    trajectory_name = str(metadata["trajectory_name"])
+    start = np.asarray(metadata["reference_start_mm"], dtype=float)
+    end = np.asarray(metadata["reference_end_mm"], dtype=float)
+    actual = frame[
+        ["DetourXFilteredMm", "DetourYFilteredMm"]
+    ].to_numpy(dtype=float)
+    actual_heading = frame["DetourThetaUnwrappedDeg"].to_numpy(dtype=float)
+
+    radius_match = re.search(
+        r"LeftArc(?P<sweep>[0-9.]+)_R(?P<radius>[0-9.]+)mm",
+        trajectory_name,
+        flags=re.IGNORECASE,
+    )
+    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)
+        center = start + radius * np.array(
+            [-np.sin(heading_radians), np.cos(heading_radians)]
+        )
+        start_radial_degrees = start_heading - 90.0
+
+        radial = actual - center
+        distance_to_center = np.linalg.norm(radial, axis=1)
+        radial_angle_degrees = np.rad2deg(
+            np.arctan2(radial[:, 1], radial[:, 0])
+        )
+        radial_angle_radians = np.deg2rad(radial_angle_degrees)
+        reference_points = center + radius * np.column_stack([
+            np.cos(radial_angle_radians),
+            np.sin(radial_angle_radians),
+        ])
+        # 对逆时针圆弧，正横向误差表示车辆位于轨迹左侧（圆内侧）。
+        lateral_error = radius - distance_to_center
+        reference_heading = radial_angle_degrees + 90.0
+        heading_error = wrap_degrees(
+            actual_heading - reference_heading
+        )
+
+        plot_angles = np.deg2rad(
+            np.linspace(
+                start_radial_degrees,
+                start_radial_degrees + sweep_degrees,
+                361,
+            )
+        )
+        ideal_plot = center + radius * np.column_stack([
+            np.cos(plot_angles),
+            np.sin(plot_angles),
+        ])
+        return {
+            "kind": "left_arc",
+            "ideal_plot_mm": ideal_plot,
+            "reference_points_mm": reference_points,
+            "reference_heading_degrees": reference_heading,
+            "lateral_error_mm": lateral_error,
+            "heading_error_degrees": heading_error,
+            "center_mm": center,
+            "radius_mm": radius,
+        }
+
+    line = end - start
+    length = float(np.linalg.norm(line))
+    if length <= 1e-6:
+        rotation_match = re.search(
+            r"Rotate(?P<angle>[+-]?[0-9.]+)",
+            trajectory_name,
+            flags=re.IGNORECASE,
+        )
+        if rotation_match:
+            relative_angle_degrees = float(
+                rotation_match.group("angle")
+            )
+            target_heading_degrees = (
+                float(metadata["start_heading_degrees"]) +
+                relative_angle_degrees
+            )
+            reference_points = np.repeat(
+                start[np.newaxis, :],
+                len(frame),
+                axis=0,
+            )
+            position_drift = np.linalg.norm(
+                actual - start,
+                axis=1,
+            )
+            reference_heading = np.full(
+                len(frame),
+                target_heading_degrees,
+            )
+            heading_error = wrap_degrees(
+                actual_heading - reference_heading
+            )
+            ideal_plot = np.repeat(
+                start[np.newaxis, :],
+                2,
+                axis=0,
+            )
+            return {
+                "kind": "in_place_rotation",
+                "ideal_plot_mm": ideal_plot,
+                "reference_points_mm": reference_points,
+                "reference_heading_degrees": reference_heading,
+                # 对原地自转，该字段表示偏离初始旋转中心的距离。
+                "lateral_error_mm": position_drift,
+                "heading_error_degrees": heading_error,
+                "rotation_center_mm": start,
+                "relative_angle_degrees": relative_angle_degrees,
+                "target_heading_degrees": target_heading_degrees,
+            }
+
+        raise ValueError(
+            f"{trajectory_name}无法识别为圆弧，且参考直线长度为0。"
+        )
+
+    tangent = line / length
+    left_normal = np.array([-tangent[1], tangent[0]])
+    displacement = actual - start
+    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(
+        np.arctan2(tangent[1], tangent[0])
+    )
+    reference_heading = np.full(len(frame), reference_heading_scalar)
+    heading_error = wrap_degrees(
+        actual_heading - reference_heading
+    )
+    ideal_plot = np.linspace(start, end, 361)
+    return {
+        "kind": "line",
+        "ideal_plot_mm": ideal_plot,
+        "reference_points_mm": reference_points,
+        "reference_heading_degrees": reference_heading,
+        "lateral_error_mm": lateral_error,
+        "heading_error_degrees": heading_error,
+    }
+
+
+def discover_csv_files(arguments: list[str]) -> list[Path]:
+    """解析命令行CSV；未指定时使用脚本目录下全部CSV。"""
+    if arguments:
+        files = [Path(item).expanduser().resolve() for item in arguments]
+    else:
+        files = sorted(SCRIPT_DIR.glob("*.csv"))
+    if not files:
+        raise FileNotFoundError("没有找到可处理的CSV文件。")
+    return files
+
+
+def output_path(
+    csv_path: Path,
+    output_directory: str | None,
+    suffix: str,
+) -> Path:
+    """构造图片输出路径并创建目录。"""
+    directory = (
+        Path(output_directory).expanduser().resolve()
+        if output_directory
+        else csv_path.parent / "plots"
+    )
+    directory.mkdir(parents=True, exist_ok=True)
+    return directory / f"{csv_path.stem}_{suffix}.png"
+
+
+def plot_trajectory(
+    csv_path: Path,
+    frequency_hz: float,
+    filter_window_seconds: float,
+    output_directory: str | None,
+    show: bool,
+) -> Path:
+    """生成单份CSV的理想/实际轨迹对比图。"""
+    frame, metadata = load_and_resample(
+        csv_path,
+        frequency_hz,
+        filter_window_seconds,
+    )
+    reference = build_reference(frame, metadata)
+
+    actual_x_m = frame["DetourXFilteredMm"].to_numpy() / 1000.0
+    actual_y_m = frame["DetourYFilteredMm"].to_numpy() / 1000.0
+    ideal_m = np.asarray(reference["ideal_plot_mm"]) / 1000.0
+
+    fig, ax = plt.subplots(figsize=(8.0, 7.0))
+    ax.plot(
+        ideal_m[:, 0],
+        ideal_m[:, 1],
+        "--",
+        linewidth=2.2,
+        label="理想轨迹",
+    )
+    ax.plot(
+        actual_x_m,
+        actual_y_m,
+        linewidth=1.8,
+        label="Detour实际轨迹（滤波后）",
+    )
+    if reference["kind"] == "in_place_rotation":
+        ax.scatter(
+            [ideal_m[0, 0]],
+            [ideal_m[0, 1]],
+            marker="*",
+            s=100,
+            label="理想旋转中心",
+            zorder=5,
+        )
+    else:
+        ax.scatter(
+            [ideal_m[0, 0]],
+            [ideal_m[0, 1]],
+            marker="o",
+            s=55,
+            label="起点",
+            zorder=5,
+        )
+        ax.scatter(
+            [ideal_m[-1, 0]],
+            [ideal_m[-1, 1]],
+            marker="x",
+            s=65,
+            label="终点",
+            zorder=5,
+        )
+    for event_index, event in enumerate(
+        metadata["localization_jump_events"]
+    ):
+        before = np.array([
+            event["before_x_mm"],
+            event["before_y_mm"],
+        ]) / 1000.0
+        after = np.array([
+            event["after_x_mm"],
+            event["after_y_mm"],
+        ]) / 1000.0
+        ax.scatter(
+            [before[0], after[0]],
+            [before[1], after[1]],
+            marker="x",
+            color="tab:red",
+            s=55,
+            zorder=6,
+            label="Detour定位跳变前/后"
+            if event_index == 0 else None,
+        )
+        ax.annotate(
+            f"定位跳变 {event['distance_mm']:.1f} mm\n"
+            f"t={event['time_seconds']:.2f} s",
+            xy=(after[0], after[1]),
+            xytext=(8, 8),
+            textcoords="offset points",
+            color="tab:red",
+            fontsize=9,
+        )
+    ax.set_aspect("equal", adjustable="box")
+    ax.set_xlabel("世界坐标 X / m")
+    ax.set_ylabel("世界坐标 Y / m")
+    ax.set_title(
+        f"理想轨迹与实际轨迹对比\n"
+        f"{metadata['controller_name']} - "
+        f"{metadata['trajectory_name']}"
+    )
+    ax.grid(True, alpha=0.3)
+    ax.legend()
+    fig.tight_layout()
+
+    destination = output_path(
+        csv_path,
+        output_directory,
+        "trajectory_comparison",
+    )
+    fig.savefig(destination, dpi=300, bbox_inches="tight")
+    if show:
+        plt.show()
+    plt.close(fig)
+    print(
+        f"{csv_path.name}: 原始采样{metadata['raw_sample_count']}帧，"
+        f"有效Detour更新{metadata['detour_update_count']}帧，"
+        f"保持重复{metadata['held_sample_count']}帧，"
+        f"定位跳变{len(metadata['localization_jump_events'])}次"
+    )
+    return destination
+
+
+def main() -> None:
+    configure_matplotlib()
+    parser = argparse.ArgumentParser(
+        description="绘制理想轨迹与Detour实际轨迹对比图。"
+    )
+    parser.add_argument("files", nargs="*", help="一个或多个CSV文件")
+    parser.add_argument("--frequency", type=float, default=20.0)
+    parser.add_argument("--window", type=float, default=0.55)
+    parser.add_argument("--output-dir")
+    parser.add_argument("--show", action="store_true")
+    args = parser.parse_args()
+
+    for csv_path in discover_csv_files(args.files):
+        destination = plot_trajectory(
+            csv_path,
+            args.frequency,
+            args.window,
+            args.output_dir,
+            args.show,
+        )
+        print(f"已生成：{destination}")
+
+
+if __name__ == "__main__":
+    main()

data_process/requirements.txt

@@ -0,0 +1,4 @@
+numpy>=1.26
+pandas>=2.2
+matplotlib>=3.8
+scipy>=1.12

data_process/run_all_plots.py

@@ -0,0 +1,191 @@
+"""一次运行四个轨迹实验绘图脚本。"""
+
+from __future__ import annotations
+
+import argparse
+import os
+import subprocess
+import sys
+from concurrent.futures import ThreadPoolExecutor, as_completed
+from pathlib import Path
+
+
+SCRIPT_NAMES = (
+    "plot_trajectory_comparison.py",
+    "plot_tracking_errors.py",
+    "plot_speed_response.py",
+    "plot_angular_command.py",
+)
+
+
+def build_command(
+    script_path: Path,
+    files: list[str],
+    frequency_hz: float,
+    filter_window_seconds: float,
+    output_directory: str | None,
+    show: bool,
+) -> list[str]:
+    """为一个绘图脚本构造与统一入口一致的命令行参数。"""
+    command = [
+        sys.executable,
+        str(script_path),
+        *files,
+        "--frequency",
+        str(frequency_hz),
+        "--window",
+        str(filter_window_seconds),
+    ]
+
+    if output_directory:
+        command.extend(["--output-dir", output_directory])
+
+    if show:
+        command.append("--show")
+
+    return command
+
+
+def run_script(
+    script_path: Path,
+    files: list[str],
+    frequency_hz: float,
+    filter_window_seconds: float,
+    output_directory: str | None,
+    show: bool,
+) -> tuple[str, int, str, str]:
+    """运行一个绘图脚本并返回名称、退出码及标准输出和错误。"""
+    result = subprocess.run(
+        build_command(
+            script_path,
+            files,
+            frequency_hz,
+            filter_window_seconds,
+            output_directory,
+            show,
+        ),
+        cwd=script_path.parent,
+        capture_output=True,
+        text=True,
+        encoding="utf-8",
+        errors="replace",
+        env={
+            **os.environ,
+            "PYTHONIOENCODING": "utf-8",
+        },
+        check=False,
+    )
+
+    return (
+        script_path.name,
+        result.returncode,
+        result.stdout.strip(),
+        result.stderr.strip(),
+    )
+
+
+def main() -> None:
+    """并行执行四类实验图的生成任务。"""
+    parser = argparse.ArgumentParser(
+        description="一次生成轨迹、误差、速度响应和角速度指令四类图。"
+    )
+    parser.add_argument(
+        "files",
+        nargs="*",
+        help="一个或多个CSV文件；省略时处理data_process目录下全部CSV。",
+    )
+    parser.add_argument(
+        "--frequency",
+        type=float,
+        default=20.0,
+        help="固定重采样频率，默认20 Hz。",
+    )
+    parser.add_argument(
+        "--window",
+        type=float,
+        default=0.55,
+        help="Savitzky-Golay滤波窗口，默认0.55 s。",
+    )
+    parser.add_argument(
+        "--output-dir",
+        help="图片输出目录；省略时由各绘图脚本使用默认目录。",
+    )
+    parser.add_argument(
+        "--show",
+        action="store_true",
+        help="生成后请求显示图片。",
+    )
+    args = parser.parse_args()
+
+    if args.frequency <= 0.0:
+        parser.error("--frequency必须大于0。")
+
+    if args.window <= 0.0:
+        parser.error("--window必须大于0。")
+
+    script_directory = Path(__file__).resolve().parent
+    script_paths = [
+        script_directory / name
+        for name in SCRIPT_NAMES
+    ]
+    missing_scripts = [
+        str(path)
+        for path in script_paths
+        if not path.is_file()
+    ]
+    if missing_scripts:
+        parser.error(
+            "缺少绘图脚本：" + "，".join(missing_scripts)
+        )
+
+    print("开始并行生成四类实验图……")
+    failures: list[str] = []
+
+    with ThreadPoolExecutor(
+        max_workers=len(script_paths)
+    ) as executor:
+        futures = [
+            executor.submit(
+                run_script,
+                script_path,
+                args.files,
+                args.frequency,
+                args.window,
+                args.output_dir,
+                args.show,
+            )
+            for script_path in script_paths
+        ]
+
+        for future in as_completed(futures):
+            script_name, return_code, stdout, stderr = (
+                future.result()
+            )
+            print(f"\n[{script_name}]")
+            if stdout:
+                print(stdout)
+            if stderr:
+                print(stderr, file=sys.stderr)
+
+            if return_code == 0:
+                print("执行成功。")
+            else:
+                failures.append(script_name)
+                print(
+                    f"执行失败，退出码={return_code}。",
+                    file=sys.stderr,
+                )
+
+    if failures:
+        print(
+            "\n以下脚本执行失败：" +
+            "，".join(failures),
+            file=sys.stderr,
+        )
+        raise SystemExit(1)
+
+    print("\n四类实验图均已生成。")
+
+
+if __name__ == "__main__":
+    main()

测试方案.txt

@@ -0,0 +1,35 @@
+单个停车机器人轨迹测试方案
+### 测试对象
+单台停车机器人（+50kg负载）
+### 测试曲线
+1. 直线：前进/后退2m、速度0.3m/s、起点终点静止
+2. 转弯：左转/右转组合前进/后退、转弯半径1m、曲率1.0、速度0.3m/s
+3. 原地自转：±90°/±180°、角速度10°/s、20°/s、起点终点静止
+
+### 评价指标
+横向误差 RMSE、最大横向误差；航向误差 RMSE、最大航向误差；速度误差 RMSE、最大速度偏差；
+角速度或转角指令的变化曲线；最终位置误差、最终航向误差
+### 展示形式
+理想轨迹与实际轨迹对比图、横向/航向误差随时间变化图、参考速度与实际速度对比图、角速度指令曲线
+
+### LQR调参策略
+归一化状态/控制量：一般Q、R初始选择对应控制量最大值的平方的倒数
+或者使用Bryson’s Rule来给 Q、R 一个很好的初始猜测
+贝叶斯优化在仿真中自动调节Q、R参数
+ALQR：在线估计最新参数实时重新求解
+----------------------------------------
+不调参：学习式
+
+### 杂项
+1. M层获取实际小车的速度信息与位置信息并保存、使用python可视化来量化跟踪误差
+2. 自行车模型改动、过于局限于阿克曼小车的运动学限制
+3. 
+
+
+
+先用 Bryson’s Rule + 贝叶斯优化在仿真里把 Q/R 调到一个不错的基准。
+上实车时采用自适应 LQR：在线估计关键参数（尤其是轮胎刚度），实时更新 K。
+C# 实现的话：
+矩阵运算继续用 Math.NET
+贝叶斯优化可以调 Python 库，或者自己写简单版本
+在线参数估计（RLS）用 C# 写很轻松

记录.txt

@@ -0,0 +1,20 @@
+还可以把底盘的失败原因暴露出来：
+/// <summary>
+/// 获取最近一次底盘运动分解失败原因。
+/// </summary>
+public string LastFailureReason =>
+    _chassis.LastMotionDecomposeFailureReason;
+这样调用方可以打印：
+if (!adapter.Send(command))
+{
+    Console.WriteLine(
+        $"底盘命令执行失败：{adapter.LastFailureReason}");
+}
+
+
+需要注意，Detour 差分速度会有噪声，建议在 Python 中：
+按固定频率重新采样。
+对位置做轻微滤波或使用 Savitzky–Golay 求导。
+再计算速度，避免直接逐点差分产生尖峰。
+Stanley 和 LQR 必须使用相同的滤波和采样参数。
+