优化轨迹制动与轮速反馈并补充实验记录和中英文说明

MultiWheelC/Control/Execution/ParkingGeometricController.cs

@@ -51,7 +51,8 @@ namespace MultiWheelC.Control.Execution
             double finishSpeedMetersPerSecond = 0.02,
             double finishHeadingToleranceRadians =
                 3.0 * Math.PI / 180.0,
-            double maximumDistanceToTrajectoryMeters = 0.30)
+            double maximumDistanceToTrajectoryMeters = 0.30,
+            double terminalBrakingPreviewMeters = 0.02)
         {
             _stateProvider = stateProvider ??
                 throw new ArgumentNullException(
@@ -81,6 +82,9 @@ namespace MultiWheelC.Control.Execution
             EnsureFinitePositive(
                 maximumDistanceToTrajectoryMeters,
                 nameof(maximumDistanceToTrajectoryMeters));
+            EnsureFiniteNonNegative(
+                terminalBrakingPreviewMeters,
+                nameof(terminalBrakingPreviewMeters));
 
             FinishDistanceMeters = finishDistanceMeters;
             FinishSpeedMetersPerSecond =
@@ -89,6 +93,8 @@ namespace MultiWheelC.Control.Execution
                 finishHeadingToleranceRadians;
             MaximumDistanceToTrajectoryMeters =
                 maximumDistanceToTrajectoryMeters;
+            TerminalBrakingPreviewMeters =
+                terminalBrakingPreviewMeters;
         }
 
         /// <summary>
@@ -111,6 +117,11 @@ namespace MultiWheelC.Control.Execution
         /// </summary>
         public double MaximumDistanceToTrajectoryMeters { get; }
 
+        /// <summary>
+        /// 获取沿轨迹提前读取更低制动参考速度的距离，单位为m。
+        /// </summary>
+        public double TerminalBrakingPreviewMeters { get; }
+
         /// <summary>
         /// 获取控制器当前是否持有并正在执行一条轨迹。
         /// </summary>
@@ -288,7 +299,7 @@ namespace MultiWheelC.Control.Execution
         }
 
         /// <summary>
-        /// 在轨迹起点零速固定点处读取前方速度，并限制为低速起步命令。
+        /// 在轨迹起步区域内保持最低释放速度，避免空间速度曲线零速固定点。
         /// </summary>
         private double ResolveReferenceSpeedForControl(
             TrajectoryProjection projection)
@@ -297,15 +308,18 @@ namespace MultiWheelC.Control.Execution
                 projection.ReferencePoint
                     .ReferenceSpeedMetersPerSecond;
 
-            var requiresStartupRelease =
+            currentReferenceSpeed =
+                ApplyTerminalBrakingPreview(
+                    projection,
+                    currentReferenceSpeed);
+
+            var isInStartupRegion =
                 projection.ArcLengthMeters <=
                     StartupRegionMeters &&
                 projection.RemainingDistanceMeters >
-                    FinishDistanceMeters &&
-                Math.Abs(currentReferenceSpeed) <=
-                    ZeroReferenceSpeedToleranceMetersPerSecond;
+                    FinishDistanceMeters;
 
-            if (!requiresStartupRelease)
+            if (!isInStartupRegion)
             {
                 return currentReferenceSpeed;
             }
@@ -314,6 +328,7 @@ namespace MultiWheelC.Control.Execution
                 _trajectory.TotalLengthMeters,
                 projection.ArcLengthMeters +
                 StartupPreviewDistanceMeters);
+
             var previewReferenceSpeed =
                 _trajectory
                     .SampleAtArcLength(
@@ -323,13 +338,67 @@ namespace MultiWheelC.Control.Execution
             if (Math.Abs(previewReferenceSpeed) <=
                 ZeroReferenceSpeedToleranceMetersPerSecond)
             {
-                return 0.0;
+                return currentReferenceSpeed;
             }
 
-            return Math.Sign(previewReferenceSpeed) *
-                   Math.Min(
-                       Math.Abs(previewReferenceSpeed),
-                       MaximumStartupSpeedMetersPerSecond);
+            var startupReleaseSpeed =
+                Math.Sign(previewReferenceSpeed) *
+                Math.Min(
+                    Math.Abs(previewReferenceSpeed),
+                    MaximumStartupSpeedMetersPerSecond);
+
+            // 当前空间速度已经高于起步释放速度时，
+            // 正常交还给原始速度曲线。
+            if (Math.Sign(currentReferenceSpeed) ==
+                    Math.Sign(startupReleaseSpeed) &&
+                Math.Abs(currentReferenceSpeed) >=
+                    Math.Abs(startupReleaseSpeed))
+            {
+                return currentReferenceSpeed;
+            }
+
+            return startupReleaseSpeed;
+        }
+
+        /// <summary>
+        /// 采用前方更低的同方向参考速度，使车辆在终点减速段提前制动。
+        /// </summary>
+        private double ApplyTerminalBrakingPreview(
+            TrajectoryProjection projection,
+            double currentReferenceSpeed)
+        {
+            if (TerminalBrakingPreviewMeters <= 0.0)
+            {
+                return currentReferenceSpeed;
+            }
+
+            var previewArcLengthMeters = Math.Min(
+                _trajectory.TotalLengthMeters,
+                projection.ArcLengthMeters +
+                TerminalBrakingPreviewMeters);
+            var previewReferenceSpeed =
+                _trajectory
+                    .SampleAtArcLength(
+                        previewArcLengthMeters)
+                    .ReferenceSpeedMetersPerSecond;
+
+            var previewIsStop =
+                Math.Abs(previewReferenceSpeed) <=
+                ZeroReferenceSpeedToleranceMetersPerSecond;
+            var hasSameDirection =
+                Math.Sign(previewReferenceSpeed) ==
+                Math.Sign(currentReferenceSpeed);
+            var previewIsSlower =
+                Math.Abs(previewReferenceSpeed) <
+                Math.Abs(currentReferenceSpeed);
+
+            if (previewIsSlower &&
+                (previewIsStop || hasSameDirection))
+            {
+                return previewReferenceSpeed;
+            }
+
+            return currentReferenceSpeed;
         }
 
         /// <summary>

MultiWheelC/Experiments/CompositeMotionPlanTests.cs

@@ -37,7 +37,7 @@ namespace MultiWheelC
         public double StraightMaximumSpeedMetersPerSecond = 0.40; // 直线限速。
         public double CurveMaximumSpeedMetersPerSecond = 0.30; // 转弯和过渡段限速。
         public double AccelerationMetersPerSecondSquared = 0.20; // 参考加速度。
-        public double DecelerationMetersPerSecondSquared = 0.12; // 参考减速度。
+        public double DecelerationMetersPerSecondSquared = 0.08; // 参考减速度。
         public double PointSpacingMeters = 0.02; // 离散轨迹点间距。
 
         /// <summary>

MultiWheelC/Experiments/NewControllerTrackingTests.cs

@@ -124,7 +124,7 @@ namespace MultiWheelC
         /// <summary>
         /// 获取或设置参考速度减速度，单位为m/s²。
         /// </summary>
-        public double DecelerationMetersPerSecondSquared = 0.10;
+        public double DecelerationMetersPerSecondSquared = 0.08;
 
         /// <summary>
         /// 获取或设置离散轨迹点间距，单位为m。
@@ -485,7 +485,7 @@ namespace MultiWheelC
         /// <summary>
         /// 获取或设置参考速度减速度，单位为m/s²。
         /// </summary>
-        public double DecelerationMetersPerSecondSquared = 0.12;
+        public double DecelerationMetersPerSecondSquared = 0.08;
 
         /// <summary>
         /// 获取或设置离散轨迹点间距，单位为m。

MultiWheelC/Movements/TrajectoryTrackingMovement.cs

@@ -126,6 +126,11 @@ namespace MultiWheelC
         public double FinishHeadingToleranceRadians =
             AngleMath.DegreesToRadians(3.0);
 
+        /// <summary>
+        /// 终点减速阶段提前读取参考速度的距离，单位为m。
+        /// </summary>
+        public double TerminalBrakingPreviewMeters = 0.02;
+
         /// <summary>
         /// 车辆允许偏离参考轨迹的最大欧氏距离，单位为m。
         /// </summary>
@@ -203,7 +208,8 @@ namespace MultiWheelC
                 FinishDistanceMeters,
                 FinishSpeedMetersPerSecond,
                 FinishHeadingToleranceRadians,
-                MaximumDistanceToTrajectoryMeters);
+                MaximumDistanceToTrajectoryMeters,
+                TerminalBrakingPreviewMeters);
 
             var clock = Stopwatch.StartNew();
             var previousCycleSeconds =

MultiWheelC/StateEstimation/WheelFeedbackVehicleStateProvider.cs

@@ -0,0 +1,274 @@
+using System;
+using CommonUsage.Chassis;
+using MyParking.Shared;
+using System.Diagnostics;
+
+namespace MultiWheelC.StateEstimation
+{
+    /// <summary>
+    /// 保留外部状态源的Detour位姿，并以舵轮电机反馈解算的车体纵向速度替换Detour差分纵向速度。
+    /// </summary>
+    public sealed class WheelFeedbackVehicleStateProvider
+        : IVehicleStateProvider
+    {
+        private readonly Stopwatch _wheelSpeedClock = Stopwatch.StartNew();
+        public const double DefaultVelocityFilterTimeConstantSeconds =
+            0.10;
+
+        private readonly object _syncRoot = new object();
+        private readonly IVehicleStateProvider _poseProvider;
+        private readonly MultiWheelChassis _chassis;
+        private readonly FirstOrderLowPassFilter _longitudinalSpeedFilter;
+
+        private bool _hasPreviousTimestamp;
+        private double _previousTimestampSeconds;
+        private bool _hasVelocityDiagnostics;
+        private double _latestDetourBodyVxMetersPerSecond;
+        private bool _latestDetourVelocityValid;
+        private double _latestRawWheelBodyVxMetersPerSecond;
+        private double _latestFilteredWheelBodyVxMetersPerSecond;
+        private bool _latestWheelVelocityValid;
+
+        /// <summary>
+        /// 创建使用默认0.10s低通时间常数的电机反馈纵向速度状态源。
+        /// </summary>
+        public WheelFeedbackVehicleStateProvider(
+            IVehicleStateProvider poseProvider,
+            MultiWheelChassis chassis)
+            : this(
+                poseProvider,
+                chassis,
+                DefaultVelocityFilterTimeConstantSeconds)
+        {
+        }
+
+        /// <summary>
+        /// 创建使用指定低通时间常数的电机反馈纵向速度状态源。
+        /// </summary>
+        public WheelFeedbackVehicleStateProvider(
+            IVehicleStateProvider poseProvider,
+            MultiWheelChassis chassis,
+            double velocityFilterTimeConstantSeconds)
+        {
+            _poseProvider = poseProvider ??
+                throw new ArgumentNullException(
+                    nameof(poseProvider));
+            _chassis = chassis ??
+                throw new ArgumentNullException(
+                    nameof(chassis));
+            _longitudinalSpeedFilter =
+                new FirstOrderLowPassFilter(
+                    velocityFilterTimeConstantSeconds);
+        }
+
+        /// <summary>
+        /// 获取最近一次读取失败的原因，正常时为空字符串。
+        /// </summary>
+        public string LastFailureReason { get; private set; } =
+            string.Empty;
+
+        /// <summary>
+        /// 读取Detour位姿和电机反馈速度，并组合成统一车辆状态。
+        /// </summary>
+        public bool TryGetState(out VehicleState state)
+        {
+            lock (_syncRoot)
+            {
+                if (!_poseProvider.TryGetState(
+                        out var poseState))
+                {
+                    state = default;
+                    LastFailureReason =
+                        "基础位姿状态源暂时不可用。";
+                    return false;
+                }
+
+                try
+                {
+                    var actualCarSpeed =
+                        _chassis.GetCarSpeed(true);
+                    var rawLongitudinalSpeedMetersPerSecond =
+                        (double)actualCarSpeed.Vx;
+
+                    EnsureFinite(
+                        rawLongitudinalSpeedMetersPerSecond,
+                        "电机反馈车体纵向速度");
+
+                    var wheelSpeedTimestampSeconds =
+                        _wheelSpeedClock.Elapsed.TotalSeconds;
+
+                    var filteredLongitudinalSpeedMetersPerSecond =
+                        UpdateLongitudinalSpeedFilter(
+                            rawLongitudinalSpeedMetersPerSecond,
+                            wheelSpeedTimestampSeconds,
+                            out var hasValidWheelSpeedEstimate);
+
+                    _latestDetourBodyVxMetersPerSecond =
+                        poseState.TwistInBody.VxMetersPerSecond;
+                    _latestDetourVelocityValid =
+                        poseState.HasValidVelocityEstimate;
+                    _latestRawWheelBodyVxMetersPerSecond =
+                        rawLongitudinalSpeedMetersPerSecond;
+                    _latestFilteredWheelBodyVxMetersPerSecond =
+                        filteredLongitudinalSpeedMetersPerSecond;
+                    _latestWheelVelocityValid =
+                        hasValidWheelSpeedEstimate;
+                    _hasVelocityDiagnostics = true;
+
+                    // 第一阶段只替换控制器使用的车体纵向速度；横向速度和角速度
+                    // 继续使用Detour估计，避免轮速差和舵角误差放大Vy与Omega噪声。
+                    var twistInBody = new Twist2D(
+                        filteredLongitudinalSpeedMetersPerSecond,
+                        poseState.TwistInBody
+                            .VyMetersPerSecond,
+                        poseState.TwistInBody
+                            .OmegaRadiansPerSecond);
+
+                    var twistInWorld =
+                        FrameTransform2D
+                            .TransformTwistAtSamePoint(
+                                poseState.PoseInWorld,
+                                twistInBody);
+
+                    state = new VehicleState(
+                        poseState.SampleTimestampSeconds,
+                        poseState.PoseInWorld,
+                        twistInWorld,
+                        hasValidWheelSpeedEstimate);
+
+                    LastFailureReason = string.Empty;
+                    return true;
+                }
+                catch (Exception exception)
+                {
+                    state = default;
+                    LastFailureReason =
+                        "舵轮电机反馈车体速度解算失败：" +
+                        exception.Message;
+                    return false;
+                }
+            }
+        }
+
+        /// <summary>
+        /// 读取最近一帧Detour纵向速度和轮速解算纵向速度，供实验记录使用。
+        /// </summary>
+        public bool TryGetLatestVelocityDiagnostics(
+            out double detourBodyVxMetersPerSecond,
+            out bool detourVelocityValid,
+            out double rawWheelBodyVxMetersPerSecond,
+            out double filteredWheelBodyVxMetersPerSecond,
+            out bool wheelVelocityValid)
+        {
+            lock (_syncRoot)
+            {
+                detourBodyVxMetersPerSecond =
+                    _latestDetourBodyVxMetersPerSecond;
+                detourVelocityValid =
+                    _latestDetourVelocityValid;
+                rawWheelBodyVxMetersPerSecond =
+                    _latestRawWheelBodyVxMetersPerSecond;
+                filteredWheelBodyVxMetersPerSecond =
+                    _latestFilteredWheelBodyVxMetersPerSecond;
+                wheelVelocityValid =
+                    _latestWheelVelocityValid;
+                return _hasVelocityDiagnostics;
+            }
+        }
+
+        /// <summary>
+        /// 清除电机反馈速度的时间基准和低通滤波历史。
+        /// </summary>
+        public void Reset()
+        {
+            lock (_syncRoot)
+            {
+                _longitudinalSpeedFilter.Reset();
+                _wheelSpeedClock.Restart();
+                _hasPreviousTimestamp = false;
+                _previousTimestampSeconds = 0.0;
+                _hasVelocityDiagnostics = false;
+                _latestDetourBodyVxMetersPerSecond = 0.0;
+                _latestDetourVelocityValid = false;
+                _latestRawWheelBodyVxMetersPerSecond = 0.0;
+                _latestFilteredWheelBodyVxMetersPerSecond = 0.0;
+                _latestWheelVelocityValid = false;
+                LastFailureReason = string.Empty;
+            }
+        }
+
+        /// <summary>
+        /// 使用真实状态时间间隔更新纵向速度低通滤波，并在首帧建立基准。
+        /// </summary>
+        private double UpdateLongitudinalSpeedFilter(
+            double rawLongitudinalSpeedMetersPerSecond,
+            double timestampSeconds,
+            out bool hasValidWheelSpeedEstimate)
+        {
+            EnsureFiniteNonNegative(
+                timestampSeconds,
+                nameof(timestampSeconds));
+
+            if (!_hasPreviousTimestamp)
+            {
+                _longitudinalSpeedFilter.Reset(
+                    rawLongitudinalSpeedMetersPerSecond);
+                _previousTimestampSeconds = timestampSeconds;
+                _hasPreviousTimestamp = true;
+                hasValidWheelSpeedEstimate = false;
+                return rawLongitudinalSpeedMetersPerSecond;
+            }
+
+            var deltaTimeSeconds =
+                timestampSeconds -
+                _previousTimestampSeconds;
+            _previousTimestampSeconds = timestampSeconds;
+
+            if (deltaTimeSeconds <= 0.0)
+            {
+                _longitudinalSpeedFilter.Reset(
+                    rawLongitudinalSpeedMetersPerSecond);
+                hasValidWheelSpeedEstimate = false;
+                return rawLongitudinalSpeedMetersPerSecond;
+            }
+
+            hasValidWheelSpeedEstimate = true;
+            return _longitudinalSpeedFilter.Update(
+                rawLongitudinalSpeedMetersPerSecond,
+                deltaTimeSeconds);
+        }
+
+        /// <summary>
+        /// 检查采样时刻是否为非负有限值。
+        /// </summary>
+        private static void EnsureFiniteNonNegative(
+            double value,
+            string parameterName)
+        {
+            EnsureFinite(value, parameterName);
+
+            if (value < 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "采样时刻必须是非负有限值。");
+            }
+        }
+
+        /// <summary>
+        /// 检查状态输入是否为有限值。
+        /// </summary>
+        private static void EnsureFinite(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "车辆状态输入必须是有限值。");
+            }
+        }
+    }
+}

README.md

@@ -16,8 +16,8 @@
 | 阶段 | 当前状态 | 说明 |
 | --- | --- | --- |
 | 1. 单车基本功能 | 联调中 | 已接入运动控制、MCU 通信、轮组反馈、急停 IO、电池、灯光、遥控和诊断代码，仍需持续实车验证 |
-| 2. 增加停车功能 | 部分开展 | 已提供夹臂控制、限位、报警和测试入口；轮胎识别、钻车和完整停车流程尚未实现 |
-| 3. 优化跟踪方法 | 已启动 | 已加入直线、圆弧、S 型、蟹行测试、实验 CSV 记录和 Python 绘图工具 |
+| 2. 增加停车功能 | 部分开展 | 已接入夹臂控制、限位和报警；夹臂动作测试当前已注释，轮胎识别、钻车和完整停车流程尚未实现 |
+| 3. 优化跟踪方法 | 已启动 | 保留旧版 `SendMotion` 测试，并新增 Stanley 横向 + PID 纵向控制、组合运动计划、实验 CSV 和新版绘图工具 |
 | 4. 多车场景 | 暂不实施 | 多车参数和预研内容未参与当前编译，当前版本不提供多车联动 |
 
 ## 项目简介
@@ -41,7 +41,8 @@ MyParking 是一个面向多轮停车机器人底盘的 C# 工程，覆盖上层
 | 单车运动 | 直线、圆弧、S 型轨迹，前进、蟹行和原地旋转 |
 | 底盘命令 | `SendMotion`、`SendXYThSpeed` 和虚拟阿克曼测试后端 |
 | 模式切换 | 正常、蟹行、自转模式；切换时先停车、预转舵轮并等待到位 |
-| 跟踪控制 | 终点跟踪、直线跟踪、基于 Detour 的直线跟踪和蟹行运动坐标系跟踪 |
+| 跟踪控制 | 旧版终点/直线/蟹行跟踪；新版 Stanley 横向控制、PID 纵向控制、前后 GCP 分配、轨迹偏离保护和终点状态判定 |
+| 状态估计 | Detour 位姿与差分速度；新版实验可保留 Detour 位姿并用舵轮反馈解算、低通滤波后的车体纵向速度替代其差分纵向速度 |
 | 夹臂 | 左右夹臂速度命令、位置反馈、软限位、驱动报警、实体/虚拟遥控和目标位置动作 |
 | MCU 通信 | 串口桥打开、复位、版本/状态查询、数字 IO、CAN/串口同步收发和异步回调 |
 | 驱动与反馈 | 8 个驱动电机和 4 个舵轮的命令、速度/位置/舵角反馈及远程帧状态 |
@@ -99,7 +100,9 @@ MyParking/
 │   └── commonusage/                 # CommonUsage 公共底盘库源码
 ├── ref/                             # 构建生成的 CommonUsage.dll（勿手工覆盖）
 ├── data_process/
-│   ├── 轨迹测试处理/                # 轨迹对比、误差、速度与角速度绘图
+│   ├── plot_new_controller_experiment.py # 新版控制器实验六子图工具
+│   ├── 新版控制器轨迹测试处理/      # 新版绘图工具的 Python 依赖
+│   ├── 旧版控制器轨迹测试处理/      # 旧版轨迹对比、误差和响应绘图
 │   └── 电机响应处理/                # 舵轮响应快照分析
 ├── docs/
 │   ├── SteeringConstraintDesign.md  # 舵轮限位设计讨论
@@ -186,7 +189,7 @@ output/M/MedullaAdapter.dll
 | CAN | 1 路，`500000 bit/s`，重试时间 `10 ms` |
 | 串口 | 3 路，`9600 bit/s`，接收帧时间 `10 ms` |
 | 电池通信端口索引 | `3` |
-| 自转最大角速度 | `30 deg/s` |
+| 遥控自转最大角速度 | `30 deg/s` |
 | 轮速诊断目录 | `logs\wheel-speed` |
 
 `docs/chassis参考.json` 是底盘参数样例；源码中尚未发现自动加载该文件的入口，实车参数仍应以宿主实际配置为准。
@@ -195,20 +198,20 @@ output/M/MedullaAdapter.dll
 
 ## 单车测试入口
 
-`MultiWheelC/MovementTests.cs` 当前注册：
+`MultiWheelC/Experiments` 当前启用以下宿主测试入口：
 
 - `准备：四个舵轮与车头方向一致`
 - `SendMotion：连续前进4m`
-- `SendXYThSpeed：原地自转90°`
-- `SendXYThSpeed：原地自转180°`
+- `SendXYThSpeed：输入角度原地自转`
 - `SendMotion：左转90°半径2m圆弧`
 - `SendMotion：蟹行直线4m`
 - `SendMotion：蟹行左转90°半径2m圆弧`
 - `SendMotion：4m S型曲线`
-- `夹臂关闭测试`
-- `夹臂启动测试`
+- `新版控制器：4m直线轨迹跟踪`
+- `新版控制器：直线-左半圆-直线轨迹跟踪`
+- `新版控制器：直线-圆弧-折线组合测试`
 
-这些测试由 Clumsy 宿主的测试界面执行，并不是 `dotnet test` 自动化测试。运动测试会按配置记录实验编号、参考轨迹、Detour 位姿和控制命令。
+这些测试由 Clumsy 宿主的测试界面执行，并不是 `dotnet test` 自动化测试。运动测试会按配置记录实验编号、参考轨迹、Detour 位姿、轮速解算速度和控制命令。`MultiWheelC/Experiments/ClampTests.cs` 中的夹臂测试目前整段注释，不会注册到宿主。
 
 ## 实验数据分析
 
@@ -224,14 +227,23 @@ Medulla 的轮速诊断可通过 `StartWheelSpeedDiagnostic` / `StopWheelSpeedDi
 logs/wheel-speed/
 ```
 
-### 轨迹测试处理
+### 新版控制器轨迹处理
 
 ```powershell
-python -m pip install -r data_process\轨迹测试处理\requirements.txt
-python data_process\轨迹测试处理\run_all_plots.py "路径\实验1.csv" "路径\实验2.csv" --output-dir "路径\plots"
+python -m pip install -r data_process\新版控制器轨迹测试处理\requirements.txt
+python data_process\plot_new_controller_experiment.py "路径\实验1.csv" "路径\实验2.csv" --output-dir "路径\plots"
 ```
 
-默认重采样频率为 `20 Hz`，滤波窗口为 `0.55 s`，可通过 `--frequency` 和 `--window` 调整。
+该工具为每份新版控制器 CSV 生成一张六子图总图，包含轨迹、横向/航向误差、速度和前后 GCP/四舵轮转角。省略 CSV 参数时，它只扫描 `data_process` 根目录及其 `data` 子目录。
+
+### 旧版控制器轨迹处理
+
+```powershell
+python -m pip install -r data_process\旧版控制器轨迹测试处理\requirements.txt
+python data_process\旧版控制器轨迹测试处理\run_all_plots.py "路径\实验1.csv" "路径\实验2.csv" --output-dir "路径\plots"
+```
+
+旧版工具默认重采样频率为 `20 Hz`，滤波窗口为 `0.55 s`，可通过 `--frequency` 和 `--window` 调整。
 
 ### 电机响应处理
 

README_en.md

@@ -16,8 +16,8 @@ The current work remains focused on the **single robot** and is in chassis integ
 | Stage | Current status | Notes |
 | --- | --- | --- |
 | 1. Basic single-robot functions | Integration in progress | Motion control, MCU communication, wheel feedback, emergency-stop I/O, battery, lights, remote control, and diagnostics are connected in code; physical validation is ongoing |
-| 2. Add parking functions | Partially started | Clamp control, limits, alarms, and test entries exist; tire recognition, vehicle entry, and the complete parking workflow are not implemented |
-| 3. Improve tracking | Started | Straight, arc, S-curve, and crab tests, experiment CSV recording, and Python plotting tools are available |
+| 2. Add parking functions | Partially started | Clamp control, limits, and alarms are connected; the clamp movement tests are currently commented out, while tire recognition, vehicle entry, and the complete parking workflow are not implemented |
+| 3. Improve tracking | Started | Legacy `SendMotion` tests remain, with new Stanley lateral + PID longitudinal control, composite motion plans, experiment CSVs, and a new plotting tool |
 | 4. Multi-robot scenarios | Deferred | Multi-robot R&D settings are excluded from the build, and the current version provides no fleet coordination |
 
 ## Overview
@@ -41,7 +41,8 @@ No ROS/ROS 2, Docker, or Web simulator project is present. The plugins are loade
 | Single-robot motion | Straight, arc, and S-curve paths; forward, crab, and in-place rotation |
 | Chassis commands | `SendMotion`, `SendXYThSpeed`, and a virtual-Ackermann test backend |
 | Mode switching | Normal, crab, and spin modes; stop, pre-steer, and wait for wheel alignment before motion |
-| Tracking | Destination tracking, line tracking, Detour-based line tracking, and crab motion-frame tracking |
+| Tracking | Legacy destination, line, and crab tracking; new Stanley lateral control, PID longitudinal control, front/rear GCP allocation, path-deviation protection, and terminal-state checks |
+| State estimation | Detour pose and differentiated velocity; new experiments can retain the Detour pose while replacing its differentiated longitudinal velocity with a low-pass-filtered body velocity derived from steer-wheel feedback |
 | Clamp | Left/right speed commands, position feedback, soft limits, driver alarms, physical/virtual remote control, and target-position actions |
 | MCU communication | Bridge open/reset, version/state queries, digital I/O, synchronous serial/CAN access, and asynchronous callbacks |
 | Drive and feedback | Commands and speed/position/steering feedback for eight drive motors and four steer modules, plus remote-frame state |
@@ -99,7 +100,9 @@ MyParking/
 │   └── commonusage/                 # CommonUsage chassis-library source
 ├── ref/                             # Generated CommonUsage.dll (do not overwrite by hand)
 ├── data_process/
-│   ├── 轨迹测试处理/                # Trajectory comparison, error, speed, and yaw plots
+│   ├── plot_new_controller_experiment.py # Six-panel plots for new-controller trials
+│   ├── 新版控制器轨迹测试处理/      # Python dependencies for the new plotting tool
+│   ├── 旧版控制器轨迹测试处理/      # Legacy trajectory, error, and response plots
 │   └── 电机响应处理/                # Steering-response snapshot analysis
 ├── docs/
 │   ├── SteeringConstraintDesign.md  # Steering-limit design notes
@@ -186,7 +189,7 @@ MCU defaults confirmed from the current source:
 | CAN | One channel at `500000 bit/s`, with a `10 ms` retry time |
 | Serial | Three channels at `9600 bit/s`, with a `10 ms` receive-frame time |
 | Battery port index | `3` |
-| Maximum spin rate | `30 deg/s` |
+| Maximum remote-control spin rate | `30 deg/s` |
 | Wheel-speed diagnostic directory | `logs\wheel-speed` |
 
 `docs/chassis参考.json` is a chassis-parameter example. No automatic loader for it was found in the source. Treat the actual host configuration as authoritative.
@@ -195,20 +198,20 @@ Before physical testing, verify the port, vehicle ID, steering zero and limits,
 
 ## Single-Robot Test Entries
 
-`MultiWheelC/MovementTests.cs` currently registers:
+`MultiWheelC/Experiments` currently enables these host test entries:
 
 - `准备：四个舵轮与车头方向一致`
 - `SendMotion：连续前进4m`
-- `SendXYThSpeed：原地自转90°`
-- `SendXYThSpeed：原地自转180°`
+- `SendXYThSpeed：输入角度原地自转`
 - `SendMotion：左转90°半径2m圆弧`
 - `SendMotion：蟹行直线4m`
 - `SendMotion：蟹行左转90°半径2m圆弧`
 - `SendMotion：4m S型曲线`
-- `夹臂关闭测试`
-- `夹臂启动测试`
+- `新版控制器：4m直线轨迹跟踪`
+- `新版控制器：直线-左半圆-直线轨迹跟踪`
+- `新版控制器：直线-圆弧-折线组合测试`
 
-These are run through the Clumsy host's test interface and are not an automated `dotnet test` suite. Motion tests record the experiment number, reference path, Detour pose, and control commands according to their configuration.
+These are run through the Clumsy host's test interface and are not an automated `dotnet test` suite. Motion tests record the experiment number, reference path, Detour pose, wheel-derived velocity, and control commands according to their configuration. The clamp tests in `MultiWheelC/Experiments/ClampTests.cs` are currently commented out in full and are not registered with the host.
 
 ## Experiment Data Analysis
 
@@ -224,14 +227,23 @@ Medulla wheel-speed diagnostics can be controlled with the `StartWheelSpeedDiagn
 logs/wheel-speed/
 ```
 
-### Trajectory processing
+### New-controller trajectory processing
 
 ```powershell
-python -m pip install -r data_process\轨迹测试处理\requirements.txt
-python data_process\轨迹测试处理\run_all_plots.py "path\trial1.csv" "path\trial2.csv" --output-dir "path\plots"
+python -m pip install -r data_process\新版控制器轨迹测试处理\requirements.txt
+python data_process\plot_new_controller_experiment.py "path\trial1.csv" "path\trial2.csv" --output-dir "path\plots"
 ```
 
-The default resampling frequency is `20 Hz`, and the default filter window is `0.55 s`; use `--frequency` and `--window` to change them.
+This tool creates one six-panel summary for each new-controller CSV, covering the path, lateral/heading errors, speed, and front/rear GCP and four-wheel steering angles. When no CSV is passed, it scans only the `data_process` root and its `data` subdirectory.
+
+### Legacy-controller trajectory processing
+
+```powershell
+python -m pip install -r data_process\旧版控制器轨迹测试处理\requirements.txt
+python data_process\旧版控制器轨迹测试处理\run_all_plots.py "path\trial1.csv" "path\trial2.csv" --output-dir "path\plots"
+```
+
+The legacy tool defaults to `20 Hz` resampling and a `0.55 s` filter window; use `--frequency` and `--window` to change them.
 
 ### Steering-response processing
 

data_process/plot_new_controller_experiment.py

@@ -530,10 +530,16 @@ def plot_experiment(
             label="Detour估计Vx",
         )
     if np.any(np.isfinite(data["wheel_filtered_speed"])):
-        axis.plot(
-            data["time"],
-            data["wheel_filtered_speed"],
-            linewidth=1.5,
+        wheel_filtered_valid = np.isfinite(
+            data["wheel_filtered_speed"]
+        )
+        axis.scatter(
+            data["time"][wheel_filtered_valid],
+            data["wheel_filtered_speed"][wheel_filtered_valid],
+            color="tab:red",
+            s=14,
+            marker="o",
+            zorder=5,
             label="轮速解算滤波Vx（控制使用）",
         )
     else:

data_process/新版前后角解耦控制器测试处理/4m0.4偏10/记录.txt

@@ -0,0 +1,21 @@
+第一次：
+: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.040m，航向误差=0.15°。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.NewControllerStraight4mTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\NewControllerTrackingTests.cs:line 246
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.040m，航向误差=0.15°。, stack:
+   at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 257
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+第三次：
+
+: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.032m，航向误差=0.10°。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.NewControllerStraight4mTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\NewControllerTrackingTests.cs:line 246
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.032m，航向误差=0.10°。, stack:
+   at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 257
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+

data_process/新版前后角解耦控制器测试处理/直线圆弧折线/记录.txt

@@ -0,0 +1,22 @@
+第二次：
+: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.037m，航向误差=0.27°。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.CompositeStopTurnGoTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\CompositeMotionPlanTests.cs:line 211
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.037m，航向误差=0.27°。, stack:
+   at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 257
+   at MultiWheelC.MotionPlanExecutor.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\MotionPlanExecutor.cs:line 174
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+第二次：
+: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.033m，航向误差=0.27°。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.CompositeStopTurnGoTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\CompositeMotionPlanTests.cs:line 211
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.033m，航向误差=0.27°。, stack:
+   at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 257
+   at MultiWheelC.MotionPlanExecutor.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\MotionPlanExecutor.cs:line 174
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+

data_process/新版前后角解耦控制器测试处理/直线圆弧直线/记录.txt

@@ -0,0 +1,10 @@
+第二次：
+: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.034m，航向误差=0.11°。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.NewControllerStraightSemicircleStraightTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\NewControllerTrackingTests.cs:line 613
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.034m，航向误差=0.11°。, stack:
+   at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 257
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+