贯通车队运行链并支持轨迹自动推导β

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

.codex/skills/commit/SKILL.md

@@ -1,77 +0,0 @@
----
-name: commit
-description: 自动生成中文 git commit 信息并提交推送。读取当前改动，用简洁的中文一句话概括改动内容，然后自动执行 git add、commit、push。当用户说"提交""commit""提交代码""推送"时使用。
-allowed-tools: Bash(git status:*), Bash(git diff:*), Bash(git add:*), Bash(git commit:*), Bash(git push:*), Bash(git log:*), Bash(git branch:*)
----
-
-# 自动 commit 并 push
-
-读取当前 git 改动，生成简洁的中文 commit 信息，然后自动提交并推送。
-
-## 执行步骤
-
-### 1. 查看当前状态
-
-先了解仓库当前情况：
-
-```bash
-git status
-git diff --stat        # 看改动了哪些文件、改动量
-git diff               # 看未暂存的具体改动
-git diff --staged      # 看已暂存的具体改动
-git log --oneline -5   # 看最近几次提交风格，保持一致
-```
-
-### 2. 分析改动
-
-基于 diff 内容，理解这次改动**实际做了什么**：
-
-- 新增了什么功能/文件
-- 修改/修复了什么
-- 删除/重构了什么
-- 是文档、配置还是代码改动
-
-**不要凭文件名猜测，要看实际 diff 内容。**
-
-### 3. 生成 commit 信息
-
-要求：
-
-- **中文**，简洁，**一句话**概括这次改动的核心内容
-- **不要前缀**（不用 feat/fix/docs 这种 Conventional Commits 前缀）
-- 直接描述做了什么，动词开头，如"添加 ALNS 自适应大邻域搜索算法"、"修复 POX 交叉中的索引越界问题"、"重构 FJSP 解码逻辑去掉 AGV 部分"
-- 如果一次改动包含多个不相关的事情，提示用户是否要分开提交（但默认仍按一条处理）
-- 长度控制在一行能看完，不写冗长描述
-
-### 4. 自动提交并推送
-
-确认 commit 信息后，依次执行：
-
-```bash
-git add -A                          # 暂存所有改动
-git commit -m "生成的中文commit信息"
-git push                            # 推送到当前分支的远程
-```
-
-### 5. 处理常见情况
-
-- **没有改动**：如果 `git status` 显示没有改动，告知用户无需提交，停止
-- **push 失败**：
-  - 如果是因为远程有新提交（需要先 pull），告知用户，建议先 `git pull` 或 `git pull --rebase`，**不要自动强推**
-  - 如果是没有配置远程或没有 upstream 分支，提示用户，给出 `git push -u origin <分支名>` 的建议命令
-  - 如果是认证问题，告知用户检查凭证
-- **当前在重要分支**（如 main/master）：正常执行，但在输出里提示一下当前分支名，让用户心里有数
-
-### 6. 输出
-
-完成后简要报告：
-
-- 生成的 commit 信息
-- 提交到了哪个分支
-- push 是否成功
-
-## 注意事项
-
-- commit 信息必须如实反映 diff 内容，不编造
-- push 失败时不要用 `--force` 强推，交给用户决定
-- 如果改动很大很杂，主动提示用户考虑拆分提交，但不强制

.codex/skills/readme/SKILL.md

@@ -1,92 +0,0 @@
----
-name: readme
-description: 为当前项目生成适配 Gitee / 公司内部代码仓库的中英文双语 README。默认生成 README.md（中文，Gitee 默认展示）和 README_en.md（英文）两个文件，顶部互相链接切换语言。适用于公司项目、算法项目、机器人项目、工程代码仓库。当用户说“写个README”“生成项目介绍”“生成Gitee README”“make a readme”时使用。
----
-
-# Gitee 双语 README 生成
-
-为当前项目生成两个互相链接的 README 文件：
-
-- `README.md`：简体中文，作为 Gitee 默认展示文件
-- `README_en.md`：英文版，供中英文切换使用
-
-如果项目中已经存在 `README_zh.md`、`Readme_zh.md`、`Readme_en.md` 等命名，先读取已有文件，并尽量沿用当前仓库已有命名规范；如果没有明确规范，默认使用 `README.md` + `README_en.md`。
-
-## 执行目标
-
-生成符合公司内部 Gitee 仓库风格的 README，不写成 GitHub 开源宣传页。
-
-README 应该让新同事或项目参与者快速知道：
-
-- 项目是什么
-- 面向什么设备 / 平台 / 场景
-- 软件架构大概是什么
-- 如何安装依赖
-- 如何编译 / 运行 / 启动
-- 代码目录怎么组织
-- 如何按公司流程参与开发
-
-## 执行步骤
-
-### 1. 调研项目
-
-先充分了解项目，不要凭空编造内容。
-
-必须优先读取和分析：
-
-- 项目根目录结构
-- 已有 README / 文档
-- 主入口脚本
-- 启动脚本
-- `CMakeLists.txt`
-- `package.xml`
-- `requirements.txt`
-- `pyproject.toml`
-- `package.json`
-- `docker-compose.yml`
-- `Dockerfile`
-- 配置文件
-- launch 文件
-- ROS / ROS2 相关目录
-- 核心源码目录
-- 设备通信、底盘控制、导航、感知、驱动相关代码
-
-需要识别：
-
-- 项目名称
-- 项目用途
-- 运行平台
-- 技术栈
-- 编程语言
-- ROS / ROS2 版本（如果存在）
-- 构建方式
-- 启动方式
-- 主要模块
-- 依赖项
-- 是否有实际设备、仿真环境、域控一体机、阿克曼底盘、CAN、串口、网络通信等内容
-
-**重要：只写代码和文档中真实存在的内容。**
-
-不要编造：
-
-- 未确认的算法
-- 未确认的性能指标
-- 未确认的硬件型号
-- 未确认的 ROS 版本
-- 未确认的启动命令
-- 未确认的部署流程
-- 未确认的许可证
-
-如果信息不足，用“待补充”明确标注，不要用通用模板假装完整。
-
----
-
-## 2. 文件命名与语言切换
-
-### 默认文件
-
-生成：
-
-```text
-README.md
-README_en.md

.gitignore

@@ -1,93 +1,56 @@
-##################################################
-# Visual Studio
-##################################################
+# .NET / MSBuild生成目录
+**/bin/
+**/obj/
+**/build/
+**/publish/
+artifacts/
+TestResults/
+*.nupkg
+packages/
 
-# Visual Studio 工作区缓存
+# MyParking构建脚本生成的部署文件
+/output/
+/ref/CommonUsage.dll
+
+# Python缓存和本地虚拟环境
+**/__pycache__/
+*.py[cod]
+.pytest_cache/
+.mypy_cache/
+.venv/
+venv/
+
+# 实验生成数据；保留脚本、requirements和README
+/data_process/**/*.csv
+/data_process/**/*.png
+/data_process/**/plots/
+/logs/
+
+# IDE和用户配置
 .vs/
-**/.vs/
-
-# 用户配置
+.idea/
+.vscode/
 *.user
 *.suo
 *.userosscache
 *.sln.docstates
-
-##################################################
-# Build 输出
-##################################################
-
-# 编译输出目录
-bin/
-obj/
-**/bin/
-**/obj/
-
-##################################################
-# Rider / VS Code
-##################################################
-
-.idea/
-.vscode/
-
-##################################################
-# NuGet
-##################################################
-
-*.nupkg
-packages/
-
-##################################################
-# 日志
-##################################################
-
-*.log
-
-##################################################
-# 临时文件
-##################################################
-
-*.tmp
-*.temp
-
-##################################################
-# 测试结果
-##################################################
-
-TestResults/
-
-##################################################
-# 发布目录
-##################################################
-
-publish/
-
-##################################################
-# Windows
-##################################################
-
-Thumbs.db
-Desktop.ini
-
-##################################################
-# JetBrains
-##################################################
-
 _ReSharper*/
 *.DotSettings.user
 
-##################################################
-# 缓存
-##################################################
-
+# 日志、临时文件和本地缓存
+*.log
+*.tmp
+*.temp
 *.cache
 
-##################################################
-# 数据库（如果有）
-##################################################
-
+# 本地数据库
 *.db
 *.sqlite
 *.sqlite3
 
-*.csv
-*.png
+# 操作系统生成文件
+Thumbs.db
+Desktop.ini
+.DS_Store
+
+# 不要全局忽略*.dll：MedullaAdapter/ref和MultiWheelC/ref中的宿主依赖需要保留。

AGENTS.md

@@ -22,6 +22,13 @@
 
 # MyParking项目规则
 
+## 项目定位与事实来源
+
+- `MyParking`是当前正式开发的停车机器人项目，结论优先依据本目录中的当前代码和实际运行配置。
+- 工作区中的旧版停车机器人、MDCS源码和轨迹规划项目只能作为辅助参考，不能覆盖当前实现所表达的事实。
+- 不能从代码、配置或用户提供资料确认的信息统一标记为“待确认”，不得自行补全或编造。
+- 修改代码后检查实际diff，并运行与改动风险相匹配的最相关编译或测试；不主动修改任务范围之外的代码。
+
 ## 代码边界
 
 - `CommonUsage-MultiVehicleSync`是独立的通用底盘库，不反向依赖`Shared`、M层或C层。
@@ -57,3 +64,24 @@ powershell -NoProfile -ExecutionPolicy Bypass -File .\build-and-package.ps1
 - 报告各项目的警告和错误，并确认M/C部署包使用同一份`CommonUsage.dll`。
 - 不直接编辑`bin`、`obj`、`build`和`output`中的产物。
 - 不手工覆盖`ref/CommonUsage.dll`，由构建脚本统一更新。
+
+# 项目知识库规则
+
+## 按需读取
+
+- 默认只读取`docs/INDEX.md`，再根据当前任务选择最相关的知识文档。
+- 严禁在每个任务开始时读取整个`docs/`；初始只读取与任务直接相关的1～2个文档，信息不足时再扩大范围。
+- 当前任务不依赖项目背景或长期知识时，可以不读取`INDEX.md`之外的文档。
+- 同一会话中已经读取且没有变化的知识文档不要重复读取。
+- 除非任务确实涉及旧版实现或MDCS底层，不读取工作区中的参考项目。
+- 优先使用关键词、类名、方法名和文件路径定位代码，不进行无目的的全库扫描。
+- 不扫描`.git`、`bin`、`obj`、`build`、`output`、日志、缓存、编译产物和第三方依赖。
+
+## 增量更新
+
+- 只有产生了已经确认、长期有效的新知识时，才更新对应文档。
+- 普通代码修改、临时调试、失败尝试和一般问答不需要更新知识库。
+- 每次只读取和更新与当前任务直接相关的文档，采用局部增量修改，不重写无关内容。
+- 不把大段源码、日志、终端输出或聊天记录复制到知识库；使用路径、类型名、方法名和精炼结论。
+- 单纯进度变化只更新`docs/progress.md`中的对应小段。
+- 没有值得长期保存的信息时，不为了形式要求强行更新文档。

CommonUsage-MultiVehicleSync/commonusage/Chassis/MultiWheelChassis.cs

@@ -1190,7 +1190,8 @@ namespace CommonUsage.Chassis
             float vx,
             float vy,
             float vth,
-            TimeSpan? deltaTime = null)
+            TimeSpan? deltaTime = null,
+            bool enableDifferentialSteerFeedforward = false)
         {
             const string operationName = "SendXYThSpeed";
 
@@ -1313,13 +1314,42 @@ namespace CommonUsage.Chassis
             var driveScale = _xyThWheelsAligned
                 ? alignmentSpeedScale
                 : 0f;
+            #region 新增虚拟旋转中心
+            // vth传入单位为deg/s，这里换算成rad/s。
+            var omegaRadiansPerSecond =
+                vth * (float)Math.PI / 180f;
+
+            // 只有调用者主动开启并且存在旋转运动时，
+            // 才启用差速舵轮左右轮的几何速度前馈。
+            var useDifferentialSteerFeedforward =
+                enableDifferentialSteerFeedforward &&
+                Math.Abs(omegaRadiansPerSecond) > 1e-4f;
+
+            var rotationCenter = Vector2.Zero;
+
+            if (useDifferentialSteerFeedforward)
+            {
+                // 根据车体速度场：
+                // vx(point) = vx - omega * y
+                // vy(point) = vy + omega * x
+                // 计算车体坐标系中的瞬时旋转中心。
+                //
+                // vx、vy单位为m/s，计算结果原本是m；
+                // 舵轮Position使用mm，因此乘以1000。
+                rotationCenter = new Vector2(
+                    -vy / omegaRadiansPerSecond * 1000f,
+                     vx / omegaRadiansPerSecond * 1000f);
+            }
+            #endregion
+
+
             for (var i = 0; i < _steerWheels.Count; i++)
                 AccumulateSpeed(
                     i,
                     driveScale *
                     sendSpeed[i],
-                    false,
-                    new Vector2(0f, 0f),
+                    useDifferentialSteerFeedforward,
+                    rotationCenter,
                     deltaTime);
 
             if (writeDiagnostics)

CommonUsage-MultiVehicleSync/commonusage/CommonUsage.csproj

@@ -34,7 +34,7 @@
 
   <ItemGroup>
     <Reference Include="FundamentalLib">
-      <HintPath>..\..\MedullaAdapter\ref\RefFundamentalLib.dll</HintPath>
+      <HintPath>.\ref\RefFundamentalLib.dll</HintPath>
     </Reference>
     <!-- <Reference Include="ClumsyCore">
       <HintPath>..\..\MultiWheelC\ref\RefClumsyCore.dll</HintPath>

MedullaAdapter/DiverCartDefinition.cs

@@ -64,9 +64,22 @@ namespace MedullaAdapter
         #endregion
 
         #region  初始参数
+
+        [AsInitParam(desc = "差速转舵目标角速度前馈增益")]
+        // public float DiffSteerRateFeedforwardGain = 0.9f;
+        public float DiffSteerRateFeedforwardGain = 0f;
+
+        [AsInitParam(desc = "差速舵轮左右轮间距，单位mm")]
+        public float DiffSteerWheelDistanceMillimeters = 85f;
+
+        [AsInitParam(desc = "差速转舵前馈最大速度，单位m/s")]
+        public float DiffSteerRateFeedforwardMaximumSpeed = 0.03f;
+
         [AsInitParam(desc = "MCU端口号")] public string MCUPort = "COM4";
         [AsInitParam(desc = "遥控器速度上限")] public float TransmitterSpeedUpperLimit = 1.0f;
         [AsInitParam(desc = "遥控器速度下限")] public float TransmitterSpeedLowerLimit = 0.0f;
+        [AsInitParam(desc = "实体遥控器SB模式防抖时间，单位ms")]
+        public int TransmitterModeDebounceMilliseconds = 200;
         [AsInitParam(desc = "手动控制夹臂速度系数")] public float ManualArmSpeedFac = 1.0f;
         [AsInitParam(desc = "遥控转弯舵角同步限速宽度，单位为度")]
         public float ManualSteeringAlignmentSigmaDegrees = 8.0f;
@@ -98,12 +111,61 @@ namespace MedullaAdapter
         [IOObjectMonitor(desc = "左后舵轮转向PID输出")] public float DiffSteerOutputLeftRear;
         [IOObjectMonitor(desc = "右前舵轮转向PID输出")] public float DiffSteerOutputRightFront;
         [IOObjectMonitor(desc = "右后舵轮转向PID输出")] public float DiffSteerOutputRightRear;
+        [IOObjectMonitor(desc = "左前舵轮目标角速度前馈输出")] public float DiffSteerRateFeedforwardLeftFront;
+        [IOObjectMonitor(desc = "左后舵轮目标角速度前馈输出")] public float DiffSteerRateFeedforwardLeftRear;
+        [IOObjectMonitor(desc = "右前舵轮目标角速度前馈输出")] public float DiffSteerRateFeedforwardRightFront;
+        [IOObjectMonitor(desc = "右后舵轮目标角速度前馈输出")] public float DiffSteerRateFeedforwardRightRear;
+        [IOObjectMonitor(desc = "左前舵轮转向合成差速输出")] public float DiffSteerTotalOutputLeftFront;
+        [IOObjectMonitor(desc = "左后舵轮转向合成差速输出")] public float DiffSteerTotalOutputLeftRear;
+        [IOObjectMonitor(desc = "右前舵轮转向合成差速输出")] public float DiffSteerTotalOutputRightFront;
+        [IOObjectMonitor(desc = "右后舵轮转向合成差速输出")] public float DiffSteerTotalOutputRightRear;
         [IOObjectMonitor(desc = "灯光模式")] public int LightMode = 0;
         [IOObjectMonitor(desc = "实体遥控器当前速度倍率")] public float TransmitterSpeed = 0.3f;
         [IOObjectMonitor(desc = "轮速诊断记录已启用")]
         public bool WheelSpeedDiagnosticEnabled;
         [IOObjectMonitor(desc = "轮速诊断记录状态")]
         public string WheelSpeedDiagnosticStatus = "未启动";
+
+        // M层辨识日志：以下字段只保存控制和反馈事件的单调时钟快照，
+        // 不参与底盘控制、限幅或模式切换。
+        internal long DiffSteerControlTimestamp;
+        internal long DiffSteerControlSequence;
+        internal long WheelCommandTimestamp;
+        internal long WheelCommandSequence;
+        internal float SentSpeedLFL;
+        internal float SentSpeedLFR;
+        internal float SentSpeedLRL;
+        internal float SentSpeedLRR;
+        internal float SentSpeedRFL;
+        internal float SentSpeedRFR;
+        internal float SentSpeedRRL;
+        internal float SentSpeedRRR;
+        internal bool WheelCommandLimitedLFL;
+        internal bool WheelCommandLimitedLFR;
+        internal bool WheelCommandLimitedLRL;
+        internal bool WheelCommandLimitedLRR;
+        internal bool WheelCommandLimitedRFL;
+        internal bool WheelCommandLimitedRFR;
+        internal bool WheelCommandLimitedRRL;
+        internal bool WheelCommandLimitedRRR;
+        internal bool WheelCommandSuppressed;
+        internal float DiffSteerTargetRateLeftFrontDegreesPerSecond;
+        internal float DiffSteerTargetRateLeftRearDegreesPerSecond;
+        internal float DiffSteerTargetRateRightFrontDegreesPerSecond;
+        internal float DiffSteerTargetRateRightRearDegreesPerSecond;
+        internal float DiffSteerFeedforwardDeltaTimeMilliseconds;
+        internal bool DiffSteerFeedforwardLimitedLeftFront;
+        internal bool DiffSteerFeedforwardLimitedLeftRear;
+        internal bool DiffSteerFeedforwardLimitedRightFront;
+        internal bool DiffSteerFeedforwardLimitedRightRear;
+        internal long ActualThLeftFrontTimestamp;
+        internal long ActualThLeftFrontSequence;
+        internal long ActualThLeftRearTimestamp;
+        internal long ActualThLeftRearSequence;
+        internal long ActualThRightFrontTimestamp;
+        internal long ActualThRightFrontSequence;
+        internal long ActualThRightRearTimestamp;
+        internal long ActualThRightRearSequence;
         [IOObjectMonitor(desc = "左前左驱动器远程帧701")] public byte LFLRemoteCode = 0;
         [IOObjectMonitor(desc = "左前右驱动器远程帧702")] public byte LFRRemoteCode = 0;
         [IOObjectMonitor(desc = "右前左驱动器远程帧703")] public byte RFLRemoteCode = 0;
@@ -263,8 +325,6 @@ namespace MedullaAdapter
                 Math.Sign(x);
             var steeringDegrees =
                 -normalizedSteering * MaxManualTheta;
-            var frontTh = steeringDegrees;
-            var rearTh = -steeringDegrees;
             ManualMode = (int)mode;
 
             switch (mode)
@@ -272,16 +332,24 @@ namespace MedullaAdapter
                 case ManualControlMode.Normal:
                     // 普通模式统一使用车体速度命令：
                     // X向前，行驶中连续改变角速度时舵轮边转、车辆边走。
-                    // SendBodyCommand(
-                    //     vx: speed,
-                    //     vy: 0.0,
-                    //     omegaRadiansPerSecond: omega,
-                    //     interval);
-                    Chassis.SendMotion(
+                    var normalOmegaRadiansPerSecond =
+                        speed *
+                        Math.Tan(
+                            AngleMath.DegreesToRadians(
+                                steeringDegrees)) /
+                        adapter.ControlPointRadiusMeters;
+                    if (!adapter.SendBodyTwist(
+                            new Twist2D(
                                 speed,
-                                frontTh,
-                                rearTh,
-                                interval);
+                                0.0,
+                                normalOmegaRadiansPerSecond),
+                            interval))
+                    {
+                        adapter.StopImmediately();
+                        Console.WriteLine(
+                            "Normal SendMotion decomposition failed: " +
+                            adapter.LastFailureReason);
+                    }
                     break;
                 case ManualControlMode.Crab:
                     // 舵轮机械范围为[-120°,120°]。
@@ -312,13 +380,15 @@ namespace MedullaAdapter
 
                     // 将车体左侧作为虚拟阿克曼车头，并在该运动坐标系中
                     // 复用与普通模式相同的SendMotion前后控制点解算。
-                    if (!adapter.SendVirtualAckermannMotion(
-                            motionDirectionRadians:
-                                Math.PI / 2.0,
-                            speedMetersPerSecond:
+                    var crabOmegaRadiansPerSecond =
+                        speed *
+                        Math.Tan(crabSteeringRadians) /
+                        adapter.ControlPointRadiusMeters;
+                    if (!adapter.SendBodyTwist(
+                            new Twist2D(
+                                0.0,
                                 speed,
-                            steeringRadians:
-                                crabSteeringRadians,
+                                crabOmegaRadiansPerSecond),
                             interval))
                     {
                         adapter.StopImmediately();
@@ -360,13 +430,11 @@ namespace MedullaAdapter
 
                     // 普通安全版SendXYThSpeed只下发角速度，
                     // 四轮实际舵角未到位时不会开放驱动速度。
-                    if (!adapter.Send(
-                            new ChassisCommand(
-                                CarNum,
-                                new Twist2D(
-                                    0.0,
-                                    0.0,
-                                    spinOmegaRadiansPerSecond)),
+                    if (!adapter.SendBodyTwist(
+                            new Twist2D(
+                                0.0,
+                                0.0,
+                                spinOmegaRadiansPerSecond),
                             interval))
                     {
                         adapter.StopImmediately();

MedullaAdapter/MCURoutine.cs

@@ -4,7 +4,9 @@ using FundamentalLib;
 using MCUSerialBridgeCLR;
 using System;
 using System.Collections.Generic;
+using System.Diagnostics;
 using System.IO;
+using System.Threading;
 
 namespace MedullaAdapter
 {
@@ -51,6 +53,19 @@ namespace MedullaAdapter
         {
             return BitConverter.ToInt32(payload, offset) * 1875f / 512f / 10000f;
         }
+
+        /// <summary>
+        /// 保存异步CAN反馈的本机单调接收时刻和递增序号，仅供辨识日志使用。
+        /// </summary>
+        private static void MarkFeedbackReceived(
+            ref long timestamp,
+            ref long sequence)
+        {
+            Interlocked.Exchange(
+                ref timestamp,
+                Stopwatch.GetTimestamp());
+            Interlocked.Increment(ref sequence);
+        }
         // M层硬件主循环：交换IO、发送轮组指令并更新车辆反馈状态。
         public override void Operation(int iteration)
         {
@@ -394,6 +409,44 @@ namespace MedullaAdapter
             var sendLArm = BitConverter.GetBytes((int)Math.Round(v9 * 512f * 10000f / 1875f));
             var sendRArm = BitConverter.GetBytes((int)Math.Round(v10 * 512f * 10000f / 1875f));
 
+            var wheelCommandSuppressed =
+                cart.AlarmLevel == 2 ||
+                cart.WaitStart ||
+                cart.PreparaStart ||
+                _driversDisabled;
+            var speedLimit = Math.Max(0f, cart.SendThresSpeed);
+
+            cart.SentSpeedLFL = wheelCommandSuppressed ? 0f : lfl;
+            cart.SentSpeedLFR = wheelCommandSuppressed ? 0f : lfr;
+            cart.SentSpeedLRL = wheelCommandSuppressed ? 0f : lrl;
+            cart.SentSpeedLRR = wheelCommandSuppressed ? 0f : lrr;
+            cart.SentSpeedRFL = wheelCommandSuppressed ? 0f : rfl;
+            cart.SentSpeedRFR = wheelCommandSuppressed ? 0f : rfr;
+            cart.SentSpeedRRL = wheelCommandSuppressed ? 0f : rrl;
+            cart.SentSpeedRRR = wheelCommandSuppressed ? 0f : rrr;
+            cart.WheelCommandLimitedLFL =
+                Math.Abs(cart.SpeedLFL) > speedLimit;
+            cart.WheelCommandLimitedLFR =
+                Math.Abs(cart.SpeedLFR) > speedLimit;
+            cart.WheelCommandLimitedLRL =
+                Math.Abs(cart.SpeedLRL) > speedLimit;
+            cart.WheelCommandLimitedLRR =
+                Math.Abs(cart.SpeedLRR) > speedLimit;
+            cart.WheelCommandLimitedRFL =
+                Math.Abs(cart.SpeedRFL) > speedLimit;
+            cart.WheelCommandLimitedRFR =
+                Math.Abs(cart.SpeedRFR) > speedLimit;
+            cart.WheelCommandLimitedRRL =
+                Math.Abs(cart.SpeedRRL) > speedLimit;
+            cart.WheelCommandLimitedRRR =
+                Math.Abs(cart.SpeedRRR) > speedLimit;
+            cart.WheelCommandSuppressed = wheelCommandSuppressed;
+            Interlocked.Exchange(
+                ref cart.WheelCommandTimestamp,
+                Stopwatch.GetTimestamp());
+            Interlocked.Increment(
+                ref cart.WheelCommandSequence);
+
             if (iteration % 2 == 0)
             {
                 //SendNodeGuardRequests(SendCan);
@@ -685,13 +738,18 @@ namespace MedullaAdapter
                     if (payload == null || payload.Length < 8) return;
                     var rpm = DecodeRpmFromPayload(payload);
                     var speed = ConvertRpm2Mps(rpm);
+                    var positionMillimeters =
+                        ConvertR2MM(
+                            BitConverter.ToInt32(payload, 0) /
+                            10000f);
                     cart.ActualSpeedLeftFrontLeft = speed;
+                    cart.LFLActualPos = positionMillimeters;
                     _wheelSpeedLogger.RecordCanFeedback(
                         0x281,
                         "LFL",
                         rpm,
-                        speed);
-                    cart.LFLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
+                        speed,
+                        positionMillimeters);
                 },
                 [0x282] = (msg) =>
                 {
@@ -700,13 +758,18 @@ namespace MedullaAdapter
                     if (payload == null || payload.Length < 8) return;
                     var rpm = DecodeRpmFromPayload(payload);
                     var speed = -ConvertRpm2Mps(rpm);
+                    var positionMillimeters =
+                        ConvertR2MM(
+                            -BitConverter.ToInt32(payload, 0) /
+                            10000f);
                     cart.ActualSpeedLeftFrontRight = speed;
+                    cart.LFRActualPos = positionMillimeters;
                     _wheelSpeedLogger.RecordCanFeedback(
                         0x282,
                         "LFR",
                         rpm,
-                        speed);
-                    cart.LFRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
+                        speed,
+                        positionMillimeters);
                 },
                 [0x283] = (msg) =>
                 {
@@ -715,13 +778,18 @@ namespace MedullaAdapter
                     if (payload == null || payload.Length < 8) return;
                     var rpm = DecodeRpmFromPayload(payload);
                     var speed = ConvertRpm2Mps(rpm);
+                    var positionMillimeters =
+                        ConvertR2MM(
+                            BitConverter.ToInt32(payload, 0) /
+                            10000f);
                     cart.ActualSpeedRightFrontLeft = speed;
+                    cart.RFLActualPos = positionMillimeters;
                     _wheelSpeedLogger.RecordCanFeedback(
                         0x283,
                         "RFL",
                         rpm,
-                        speed);
-                    cart.RFLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
+                        speed,
+                        positionMillimeters);
                 },
                 [0x284] = (msg) =>
                 {
@@ -730,13 +798,18 @@ namespace MedullaAdapter
                     if (payload == null || payload.Length < 8) return;
                     var rpm = DecodeRpmFromPayload(payload);
                     var speed = -ConvertRpm2Mps(rpm);
+                    var positionMillimeters =
+                        ConvertR2MM(
+                            -BitConverter.ToInt32(payload, 0) /
+                            10000f);
                     cart.ActualSpeedRightFrontRight = speed;
+                    cart.RFRActualPos = positionMillimeters;
                     _wheelSpeedLogger.RecordCanFeedback(
                         0x284,
                         "RFR",
                         rpm,
-                        speed);
-                    cart.RFRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
+                        speed,
+                        positionMillimeters);
                 },
                 [0x285] = (msg) =>
                 {
@@ -745,13 +818,18 @@ namespace MedullaAdapter
                     if (payload == null || payload.Length < 8) return;
                     var rpm = DecodeRpmFromPayload(payload);
                     var speed = ConvertRpm2Mps(rpm);
+                    var positionMillimeters =
+                        ConvertR2MM(
+                            BitConverter.ToInt32(payload, 0) /
+                            10000f);
                     cart.ActualSpeedLeftRearLeft = speed;
+                    cart.LRLActualPos = positionMillimeters;
                     _wheelSpeedLogger.RecordCanFeedback(
                         0x285,
                         "LRL",
                         rpm,
-                        speed);
-                    cart.LRLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
+                        speed,
+                        positionMillimeters);
                 },
                 [0x286] = (msg) =>
                 {
@@ -760,13 +838,18 @@ namespace MedullaAdapter
                     if (payload == null || payload.Length < 8) return;
                     var rpm = DecodeRpmFromPayload(payload);
                     var speed = -ConvertRpm2Mps(rpm);
+                    var positionMillimeters =
+                        ConvertR2MM(
+                            -BitConverter.ToInt32(payload, 0) /
+                            10000f);
                     cart.ActualSpeedLeftRearRight = speed;
+                    cart.LRRActualPos = positionMillimeters;
                     _wheelSpeedLogger.RecordCanFeedback(
                         0x286,
                         "LRR",
                         rpm,
-                        speed);
-                    cart.LRRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
+                        speed,
+                        positionMillimeters);
                 },
                 [0x287] = (msg) =>
                 {
@@ -775,13 +858,18 @@ namespace MedullaAdapter
                     if (payload == null || payload.Length < 8) return;
                     var rpm = DecodeRpmFromPayload(payload);
                     var speed = ConvertRpm2Mps(rpm);
+                    var positionMillimeters =
+                        ConvertR2MM(
+                            BitConverter.ToInt32(payload, 0) /
+                            10000f);
                     cart.ActualSpeedRightRearLeft = speed;
+                    cart.RRLActualPos = positionMillimeters;
                     _wheelSpeedLogger.RecordCanFeedback(
                         0x287,
                         "RRL",
                         rpm,
-                        speed);
-                    cart.RRLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
+                        speed,
+                        positionMillimeters);
                 },
                 [0x288] = (msg) =>
                 {
@@ -790,13 +878,18 @@ namespace MedullaAdapter
                     if (payload == null || payload.Length < 8) return;
                     var rpm = DecodeRpmFromPayload(payload);
                     var speed = -ConvertRpm2Mps(rpm);
+                    var positionMillimeters =
+                        ConvertR2MM(
+                            -BitConverter.ToInt32(payload, 0) /
+                            10000f);
                     cart.ActualSpeedRightRearRight = speed;
+                    cart.RRRActualPos = positionMillimeters;
                     _wheelSpeedLogger.RecordCanFeedback(
                         0x288,
                         "RRR",
                         rpm,
-                        speed);
-                    cart.RRRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
+                        speed,
+                        positionMillimeters);
                 },
                 [0x289] = (msg) =>
                 {
@@ -914,10 +1007,18 @@ namespace MedullaAdapter
                     //Console.WriteLine("Received 0x18B CAN Message");
                     var payload = msg.Payload;
                     if (payload == null || payload.Length < 4) return;
-                    cart.ActualThLeftFront = BitConverter.ToInt32(payload, 0);
-                    cart.ActualThLeftFront = cart.ActualThLeftFront >= 16384
-                    ? (cart.ActualThLeftFront - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftFront
-                    : cart.ActualThLeftFront / 4096f / 5f * 360 - cart.ThBiasLeftFront;
+                    var raw = BitConverter.ToInt32(payload, 0);
+                    cart.ActualThLeftFront = raw >= 16384
+                    ? (raw - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftFront
+                    : raw / 4096f / 5f * 360 - cart.ThBiasLeftFront;
+                    MarkFeedbackReceived(
+                        ref cart.ActualThLeftFrontTimestamp,
+                        ref cart.ActualThLeftFrontSequence);
+                    _wheelSpeedLogger.RecordSteeringAngleFeedback(
+                        0x18B,
+                        "LF",
+                        raw,
+                        cart.ActualThLeftFront);
                 },
                 [0x18C] = (msg) =>
                 {
@@ -927,6 +1028,14 @@ namespace MedullaAdapter
                     cart.ActualThRightFront = raw >= 16384
                         ? (raw - 98303) / 4096f / 5f * 360 - cart.ThBiasRightFront
                         : raw / 4096f / 5f * 360 - cart.ThBiasRightFront;
+                    MarkFeedbackReceived(
+                        ref cart.ActualThRightFrontTimestamp,
+                        ref cart.ActualThRightFrontSequence);
+                    _wheelSpeedLogger.RecordSteeringAngleFeedback(
+                        0x18C,
+                        "RF",
+                        raw,
+                        cart.ActualThRightFront);
                     //DLog.Log($"RF raw=0x{raw:X8}({raw}) angle={cart.ActualThRightFront:F2}", "0x18C");
                 },
                 [0x18D] = (msg) =>
@@ -934,20 +1043,36 @@ namespace MedullaAdapter
                     //Console.WriteLine($"Received 0x18D CAN Message {DateTime.Now:yyyy-MM-dd HH:mm:ss.ffffff}"); 
                     var payload = msg.Payload;
                     if (payload == null || payload.Length < 4) return;
-                    cart.ActualThLeftRear = BitConverter.ToInt32(payload, 0);
-                    cart.ActualThLeftRear = cart.ActualThLeftRear >= 16384
-                        ? (cart.ActualThLeftRear - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftRear
-                        : cart.ActualThLeftRear / 4096f / 5f * 360 - cart.ThBiasLeftRear;
+                    var raw = BitConverter.ToInt32(payload, 0);
+                    cart.ActualThLeftRear = raw >= 16384
+                        ? (raw - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftRear
+                        : raw / 4096f / 5f * 360 - cart.ThBiasLeftRear;
+                    MarkFeedbackReceived(
+                        ref cart.ActualThLeftRearTimestamp,
+                        ref cart.ActualThLeftRearSequence);
+                    _wheelSpeedLogger.RecordSteeringAngleFeedback(
+                        0x18D,
+                        "LR",
+                        raw,
+                        cart.ActualThLeftRear);
                 },
                 [0x18E] = (msg) =>
                 {
                     //Console.WriteLine("Received 0x18E CAN Message");
                     var payload = msg.Payload;
                     if (payload == null || payload.Length < 4) return;
-                    cart.ActualThRightRear = BitConverter.ToInt32(payload, 0);
-                    cart.ActualThRightRear = cart.ActualThRightRear >= 16384
-                        ? (cart.ActualThRightRear - 98303) / 4096f / 5f * 360 - cart.ThBiasRightRear
-                        : cart.ActualThRightRear / 4096f / 5f * 360 - cart.ThBiasRightRear;
+                    var raw = BitConverter.ToInt32(payload, 0);
+                    cart.ActualThRightRear = raw >= 16384
+                        ? (raw - 98303) / 4096f / 5f * 360 - cart.ThBiasRightRear
+                        : raw / 4096f / 5f * 360 - cart.ThBiasRightRear;
+                    MarkFeedbackReceived(
+                        ref cart.ActualThRightRearTimestamp,
+                        ref cart.ActualThRightRearSequence);
+                    _wheelSpeedLogger.RecordSteeringAngleFeedback(
+                        0x18E,
+                        "RR",
+                        raw,
+                        cart.ActualThRightRear);
                 },
 
                 // 远程帧

MedullaAdapter/MedullaAdapter.csproj

@@ -42,8 +42,8 @@
   </ItemGroup>
 
   <ItemGroup>
-  <Compile Include="..\Shared\Models\ChassisCommand.cs"
-           Link="Shared\Models\ChassisCommand.cs" />
+  <Compile Include="..\Shared\Models\MotionModels.cs"
+           Link="Shared\Models\MotionModels.cs" />
 
   <Compile Include="..\Shared\Mathematics\FrameTransform2D.cs"
            Link="Shared\Mathematics\FrameTransform2D.cs" />
@@ -51,6 +51,9 @@
   <Compile Include="..\Shared\Mathematics\AngleMath.cs"
            Link="Shared\Mathematics\AngleMath.cs" />
 
+  <Compile Include="..\Shared\Validation\NumericGuard.cs"
+           Link="Shared\Validation\NumericGuard.cs" />
+
   <Compile Include="..\Shared\Chassis\MultiWheelChassisAdapter.cs"
            Link="Shared\Chassis\MultiWheelChassisAdapter.cs" />
 </ItemGroup>

MedullaAdapter/MotorRoutine.cs

@@ -3,14 +3,27 @@ using CartActivator;
 using FundamentalLib;
 using MDCSToolBox.Commons;
 using System;
+using System.Diagnostics;
+using System.Threading;
 using static MDCSToolBox.Medulla.Chassis.BasicCartDefinition;
 
 namespace MedullaAdapter
 {
     public class MotorRoutine : LadderLogic<DiverCartDefinition>
     {
+        private const double MaximumFeedforwardIntervalSeconds = 0.2;
         private bool _wasTransmitterControlling;
         private DateTime _lastMoveTime = DateTime.Now;
+        private bool _diffSteerFeedforwardInitialized;
+        private long _lastDiffSteerFeedforwardTimestamp;
+        private float _previousThLeftFront;
+        private float _previousThLeftRear;
+        private float _previousThRightFront;
+        private float _previousThRightRear;
+        private DiverCartDefinition.ManualControlMode?
+            _pendingTransmitterControlMode;
+        private DateTime _pendingTransmitterControlModeSince =
+            DateTime.MinValue;
         public override void Operation(int iteration)
         {
             if (!cart.GhostMode && cart.State == -1) return;
@@ -63,38 +76,91 @@ namespace MedullaAdapter
             cart.ClumsyControl = CartDefinition.currentPriority == 0;
             // 计算四个舵轮PID和8个驱动电机最终速度。
             UpdateDiffSteerWheelSpeeds();
+            Interlocked.Exchange(
+                ref cart.DiffSteerControlTimestamp,
+                Stopwatch.GetTimestamp());
+            Interlocked.Increment(
+                ref cart.DiffSteerControlSequence);
             // 平滑更新硬件速度限制。
             UpdateSendSpeedLimit();
             // 更新红黄绿灯状态。
             UpdateLightMode();
         }
+        // M层物理遥控器：只有SB档位连续稳定指定时间后才确认模式切换。
+        private bool TryGetStableTransmitterControlMode(
+            out DiverCartDefinition.ManualControlMode stableMode)
+        {
+            stableMode = cart.TransmitterControlMode;
+
+            DiverCartDefinition.ManualControlMode? requestedMode;
+
+            switch (cart.Transmitter_SB)
+            {
+                case TransmitterState.Mode0:
+                    requestedMode =
+                        DiverCartDefinition.ManualControlMode.Normal;
+                    break;
+
+                case TransmitterState.Mode1:
+                    requestedMode =
+                        DiverCartDefinition.ManualControlMode.Crab;
+                    break;
+
+                case TransmitterState.Mode2:
+                    requestedMode =
+                        DiverCartDefinition.ManualControlMode.Spin;
+                    break;
+
+                default:
+                    _pendingTransmitterControlMode = null;
+                    _pendingTransmitterControlModeSince =
+                        DateTime.MinValue;
+                    return false;
+            }
+
+            if (_pendingTransmitterControlMode != requestedMode)
+            {
+                _pendingTransmitterControlMode = requestedMode;
+                _pendingTransmitterControlModeSince = DateTime.Now;
+                return false;
+            }
+
+            var debounceMilliseconds = Math.Max(
+                0,
+                cart.TransmitterModeDebounceMilliseconds);
+
+            if ((DateTime.Now -
+                 _pendingTransmitterControlModeSince)
+                .TotalMilliseconds < debounceMilliseconds)
+            {
+                return false;
+            }
+
+            stableMode = requestedMode.Value;
+            return true;
+        }
+
         // 物理遥控器设置
         public void TransmitterChassisControl()
         {
             var interval = DateTime.Now - cart.TransmitterLastTime;
-            switch (cart.Transmitter_SB)
+
+            if (!TryGetStableTransmitterControlMode(
+                    out var stableControlMode))
             {
-                case TransmitterState.Mode0:
-                    cart.TransmitterControlMode =
-                    DiverCartDefinition.ManualControlMode.Normal;
-                    break;
-                case TransmitterState.Mode1:
-                    cart.TransmitterControlMode =
-                        DiverCartDefinition.ManualControlMode.Crab;
-                    break;
-                case TransmitterState.Mode2:
-                    cart.TransmitterControlMode =
-                        DiverCartDefinition.ManualControlMode.Spin;
-                    break;
-                default:
-                    cart.ManualControl(
-                        cart.TransmitterControlMode,
-                        0, 0, 0,
-                        cart.TransmitterSpeed,
-                        interval);
-                    StopClampArms();
-                    return;
+                // SB处于中间档或尚未稳定时立即停车，
+                // 保持当前模式，不下发新的模式目标角。
+                cart.ManualControl(
+                    cart.TransmitterControlMode,
+                    0, 0, 0,
+                    cart.TransmitterSpeed,
+                    interval);
+                StopClampArms();
+                return;
             }
+
+            cart.TransmitterControlMode = stableControlMode;
+
             // SA关闭后立即停车。
             if (!cart.Transmitter_SA)
             {
@@ -161,7 +227,9 @@ namespace MedullaAdapter
             cart.SpeedRightArm = 0;
         }
 
-        // M层单车底盘：根据四个舵轮的目标角度和实际角度修正8个驱动电机速度。
+        /// <summary>
+        /// 根据四个舵轮的目标角速度前馈和实际角度反馈修正八个驱动电机速度。
+        /// </summary>
         private void UpdateDiffSteerWheelSpeeds()
         {
             if (cart.LeftFrontPid == null ||
@@ -177,6 +245,7 @@ namespace MedullaAdapter
                 cart.SpeedLRR = 0;
                 cart.SpeedRRL = 0;
                 cart.SpeedRRR = 0;
+                ResetDiffSteerRateFeedforward();
                 return;
             }
 
@@ -220,26 +289,45 @@ namespace MedullaAdapter
                 cart.DiffSteerThresh,
                 cart.DiffSteerSpeedAcc);
 
-            // 根据实际舵角计算四条腿的差速修正量。
-            var diffLf = cart.LeftFrontPid.GetResponse(
+            // 根据实际舵角计算四条腿的PID反馈修正量。
+            var feedbackLf = cart.LeftFrontPid.GetResponse(
                 cart.ThLeftFront, false, false, "LF");
 
-            var diffLr = cart.LeftRearPid.GetResponse(
+            var feedbackLr = cart.LeftRearPid.GetResponse(
                 cart.ThLeftRear, false, false, "LR");
 
-            var diffRf = cart.RightFrontPid.GetResponse(
+            var feedbackRf = cart.RightFrontPid.GetResponse(
                 cart.ThRightFront, false, false, "RF");
 
-            var diffRr = cart.RightRearPid.GetResponse(
+            var feedbackRr = cart.RightRearPid.GetResponse(
                 cart.ThRightRear, false, false, "RR");
 
-            // 保存四个转向PID的本周期修正量，供M层监控和舵轮响应CSV记录使用。
-            cart.DiffSteerOutputLeftFront = diffLf;
-            cart.DiffSteerOutputLeftRear = diffLr;
-            cart.DiffSteerOutputRightFront = diffRf;
-            cart.DiffSteerOutputRightRear = diffRr;
+            CalculateDiffSteerRateFeedforward(
+                out var feedforwardLf,
+                out var feedforwardLr,
+                out var feedforwardRf,
+                out var feedforwardRr);
 
-            // 左前腿：左右电机施加方向相反的PID修正量。
+            var diffLf = feedbackLf + feedforwardLf;
+            var diffLr = feedbackLr + feedforwardLr;
+            var diffRf = feedbackRf + feedforwardRf;
+            var diffRr = feedbackRr + feedforwardRr;
+
+            // 分别保留PID、前馈和合成差速，便于独立标定与诊断。
+            cart.DiffSteerOutputLeftFront = feedbackLf;
+            cart.DiffSteerOutputLeftRear = feedbackLr;
+            cart.DiffSteerOutputRightFront = feedbackRf;
+            cart.DiffSteerOutputRightRear = feedbackRr;
+            cart.DiffSteerRateFeedforwardLeftFront = feedforwardLf;
+            cart.DiffSteerRateFeedforwardLeftRear = feedforwardLr;
+            cart.DiffSteerRateFeedforwardRightFront = feedforwardRf;
+            cart.DiffSteerRateFeedforwardRightRear = feedforwardRr;
+            cart.DiffSteerTotalOutputLeftFront = diffLf;
+            cart.DiffSteerTotalOutputLeftRear = diffLr;
+            cart.DiffSteerTotalOutputRightFront = diffRf;
+            cart.DiffSteerTotalOutputRightRear = diffRr;
+
+            // 左前腿：左右电机施加方向相反的合成差速修正量。
             cart.SpeedLFL = cart.SpeedLeftFrontLeft - diffLf;
             cart.SpeedLFR = cart.SpeedLeftFrontRight + diffLf;
 
@@ -256,6 +344,188 @@ namespace MedullaAdapter
             cart.SpeedRRR = cart.SpeedRightRearRight + diffRr;
         }
 
+        /// <summary>
+        /// 根据四个机械目标舵角的实际变化率计算本周期差速轮线速度前馈。
+        /// </summary>
+        private void CalculateDiffSteerRateFeedforward(
+            out float leftFront,
+            out float leftRear,
+            out float rightFront,
+            out float rightRear)
+        {
+            leftFront = 0f;
+            leftRear = 0f;
+            rightFront = 0f;
+            rightRear = 0f;
+            cart.DiffSteerTargetRateLeftFrontDegreesPerSecond = 0f;
+            cart.DiffSteerTargetRateLeftRearDegreesPerSecond = 0f;
+            cart.DiffSteerTargetRateRightFrontDegreesPerSecond = 0f;
+            cart.DiffSteerTargetRateRightRearDegreesPerSecond = 0f;
+            cart.DiffSteerFeedforwardDeltaTimeMilliseconds = 0f;
+            cart.DiffSteerFeedforwardLimitedLeftFront = false;
+            cart.DiffSteerFeedforwardLimitedLeftRear = false;
+            cart.DiffSteerFeedforwardLimitedRightFront = false;
+            cart.DiffSteerFeedforwardLimitedRightRear = false;
+
+            var currentTimestamp = Stopwatch.GetTimestamp();
+
+            if (_diffSteerFeedforwardInitialized)
+            {
+                var deltaTimeSeconds =
+                    (currentTimestamp -
+                     _lastDiffSteerFeedforwardTimestamp) /
+                    (double)Stopwatch.Frequency;
+
+                if (deltaTimeSeconds > 0.0 &&
+                    deltaTimeSeconds <=
+                    MaximumFeedforwardIntervalSeconds)
+                {
+                    cart.DiffSteerFeedforwardDeltaTimeMilliseconds =
+                        (float)(deltaTimeSeconds * 1000.0);
+                    leftFront = CalculateDiffSteerRateFeedforward(
+                        cart.ThLeftFront,
+                        _previousThLeftFront,
+                        deltaTimeSeconds,
+                        out var targetRateLf,
+                        out var limitedLf);
+                    leftRear = CalculateDiffSteerRateFeedforward(
+                        cart.ThLeftRear,
+                        _previousThLeftRear,
+                        deltaTimeSeconds,
+                        out var targetRateLr,
+                        out var limitedLr);
+                    rightFront = CalculateDiffSteerRateFeedforward(
+                        cart.ThRightFront,
+                        _previousThRightFront,
+                        deltaTimeSeconds,
+                        out var targetRateRf,
+                        out var limitedRf);
+                    rightRear = CalculateDiffSteerRateFeedforward(
+                        cart.ThRightRear,
+                        _previousThRightRear,
+                        deltaTimeSeconds,
+                        out var targetRateRr,
+                        out var limitedRr);
+
+                    cart.DiffSteerTargetRateLeftFrontDegreesPerSecond =
+                        targetRateLf;
+                    cart.DiffSteerTargetRateLeftRearDegreesPerSecond =
+                        targetRateLr;
+                    cart.DiffSteerTargetRateRightFrontDegreesPerSecond =
+                        targetRateRf;
+                    cart.DiffSteerTargetRateRightRearDegreesPerSecond =
+                        targetRateRr;
+                    cart.DiffSteerFeedforwardLimitedLeftFront = limitedLf;
+                    cart.DiffSteerFeedforwardLimitedLeftRear = limitedLr;
+                    cart.DiffSteerFeedforwardLimitedRightFront = limitedRf;
+                    cart.DiffSteerFeedforwardLimitedRightRear = limitedRr;
+                }
+            }
+
+            _previousThLeftFront = cart.ThLeftFront;
+            _previousThLeftRear = cart.ThLeftRear;
+            _previousThRightFront = cart.ThRightFront;
+            _previousThRightRear = cart.ThRightRear;
+            _lastDiffSteerFeedforwardTimestamp = currentTimestamp;
+            _diffSteerFeedforwardInitialized = true;
+        }
+
+        /// <summary>
+        /// 将单个机械目标舵角变化率转换为带限幅的左右轮差速线速度前馈。
+        /// </summary>
+        private float CalculateDiffSteerRateFeedforward(
+            float targetAngleDegrees,
+            float previousTargetAngleDegrees,
+            double deltaTimeSeconds,
+            out float targetRateDegreesPerSecond,
+            out bool limited)
+        {
+            targetRateDegreesPerSecond = 0f;
+            limited = false;
+
+            var gain = cart.DiffSteerRateFeedforwardGain;
+            var wheelDistanceMillimeters =
+                cart.DiffSteerWheelDistanceMillimeters;
+            var maximumSpeed =
+                cart.DiffSteerRateFeedforwardMaximumSpeed;
+
+            if (!IsFinite(targetAngleDegrees) ||
+                !IsFinite(previousTargetAngleDegrees) ||
+                !double.IsFinite(deltaTimeSeconds) ||
+                deltaTimeSeconds <= 0.0)
+            {
+                return 0f;
+            }
+
+            // 机械舵角受限，必须使用直接差值而不是圆周最短角差。
+            targetRateDegreesPerSecond =
+                (float)((targetAngleDegrees -
+                         previousTargetAngleDegrees) /
+                        deltaTimeSeconds);
+
+            if (!IsFinite(gain) || gain <= 0f ||
+                !IsFinite(wheelDistanceMillimeters) ||
+                wheelDistanceMillimeters <= 0f ||
+                !IsFinite(maximumSpeed) || maximumSpeed <= 0f)
+            {
+                return 0f;
+            }
+
+            var targetRateRadiansPerSecond =
+                targetRateDegreesPerSecond *
+                Math.PI / 180.0;
+            var wheelDistanceMeters =
+                wheelDistanceMillimeters / 1000.0;
+            var feedforwardSpeed =
+                0.5 *
+                wheelDistanceMeters *
+                targetRateRadiansPerSecond *
+                gain;
+
+            var limitedSpeed = (float)Math.Clamp(
+                feedforwardSpeed,
+                -maximumSpeed,
+                maximumSpeed);
+            limited = Math.Abs(
+                feedforwardSpeed - limitedSpeed) > 1e-9;
+            return limitedSpeed;
+        }
+
+        /// <summary>
+        /// 清除差速转舵前馈历史和监控输出，避免恢复控制时使用过期目标角。
+        /// </summary>
+        private void ResetDiffSteerRateFeedforward()
+        {
+            _diffSteerFeedforwardInitialized = false;
+            _lastDiffSteerFeedforwardTimestamp = 0;
+            cart.DiffSteerRateFeedforwardLeftFront = 0f;
+            cart.DiffSteerRateFeedforwardLeftRear = 0f;
+            cart.DiffSteerRateFeedforwardRightFront = 0f;
+            cart.DiffSteerRateFeedforwardRightRear = 0f;
+            cart.DiffSteerTargetRateLeftFrontDegreesPerSecond = 0f;
+            cart.DiffSteerTargetRateLeftRearDegreesPerSecond = 0f;
+            cart.DiffSteerTargetRateRightFrontDegreesPerSecond = 0f;
+            cart.DiffSteerTargetRateRightRearDegreesPerSecond = 0f;
+            cart.DiffSteerFeedforwardDeltaTimeMilliseconds = 0f;
+            cart.DiffSteerFeedforwardLimitedLeftFront = false;
+            cart.DiffSteerFeedforwardLimitedLeftRear = false;
+            cart.DiffSteerFeedforwardLimitedRightFront = false;
+            cart.DiffSteerFeedforwardLimitedRightRear = false;
+            cart.DiffSteerTotalOutputLeftFront = 0f;
+            cart.DiffSteerTotalOutputLeftRear = 0f;
+            cart.DiffSteerTotalOutputRightFront = 0f;
+            cart.DiffSteerTotalOutputRightRear = 0f;
+        }
+
+        /// <summary>
+        /// 判断单精度参数是否可安全参与底盘控制计算。
+        /// </summary>
+        private static bool IsFinite(float value)
+        {
+            return !float.IsNaN(value) &&
+                   !float.IsInfinity(value);
+        }
+
         // M层单车限速：按照加速度和减速度平滑更新实际下发速度上限。
         private void UpdateSendSpeedLimit()
         {

MedullaAdapter/WheelSpeedDiagnosticLogger.cs

@@ -38,8 +38,10 @@ namespace MedullaAdapter
         private volatile bool _isRunning;
         private int _queuedRecordCount;
         private long _receiveSequence;
+        private long _snapshotSequence;
         private long _droppedRecordCount;
         private double _lastSnapshotMilliseconds = double.NegativeInfinity;
+        private long _startTimestamp;
 
         public bool IsRunning => _isRunning;
 
@@ -79,19 +81,43 @@ namespace MedullaAdapter
                 _snapshotWriter = CreateWriter(SnapshotLogPath);
 
                 _canWriter.WriteLine(
-                    "ElapsedMs,ReceiveSequence,CanId,MotorName,RawRpm,SpeedMps");
+                    "ElapsedMs,ReceiveSequence,CanId,EventType,ChannelName," +
+                    "RawRpm,SpeedMps,PositionMm,RawAngle,AngleDegrees");
 
                 _snapshotWriter.WriteLine(
-                    "ElapsedMs,CarNum,ManualControlMode,ManualMode,SendThresSpeed," +
+                    "ElapsedMs,SnapshotSequence," +
+                    "ControlElapsedMs,ControlSequence,ControlAgeMs," +
+                    "WheelCommandElapsedMs,WheelCommandSequence,WheelCommandAgeMs," +
+                    "CarNum,ManualControlMode,ManualMode,SendThresSpeed," +
+                    "VoltageV,AlarmLevel,ChassisMode,WheelAbleState," +
                     "DiffSteerKp,DiffSteerKi,DiffSteerKd,DiffSteerMaxI,DiffSteerDeadZone,DiffSteerThresh,DiffSteerSpeedAcc," +
+                    "DiffSteerRateFeedforwardGain,DiffSteerWheelDistanceMillimeters,DiffSteerRateFeedforwardMaximumSpeed," +
+                    "FeedforwardDeltaTimeMs," +
+                    "TargetRateThLeftFrontDegreesPerSecond,TargetRateThLeftRearDegreesPerSecond," +
+                    "TargetRateThRightFrontDegreesPerSecond,TargetRateThRightRearDegreesPerSecond," +
+                    "FeedforwardLimitedLeftFront,FeedforwardLimitedLeftRear,FeedforwardLimitedRightFront,FeedforwardLimitedRightRear," +
                     "PidOutLeftFront,PidOutLeftRear,PidOutRightFront,PidOutRightRear," +
+                    "RateFeedforwardLeftFront,RateFeedforwardLeftRear,RateFeedforwardRightFront,RateFeedforwardRightRear," +
+                    "TotalDiffLeftFront,TotalDiffLeftRear,TotalDiffRightFront,TotalDiffRightRear," +
                     "CmdLFL,CmdLFR,CmdLRL,CmdLRR,CmdRFL,CmdRFR,CmdRRL,CmdRRR," +
                     "PidLFL,PidLFR,PidLRL,PidLRR,PidRFL,PidRFR,PidRRL,PidRRR," +
+                    "SentLFLMps,SentLFRMps,SentLRLMps,SentLRRMps,SentRFLMps,SentRFRMps,SentRRLMps,SentRRRMps," +
+                    "CommandLimitedLFL,CommandLimitedLFR,CommandLimitedLRL,CommandLimitedLRR," +
+                    "CommandLimitedRFL,CommandLimitedRFR,CommandLimitedRRL,CommandLimitedRRR," +
+                    "PairCommandLimitedLeftFront,PairCommandLimitedLeftRear," +
+                    "PairCommandLimitedRightFront,PairCommandLimitedRightRear,WheelCommandSuppressed," +
                     "ActualLFL,ActualLFR,ActualLRL,ActualLRR,ActualRFL,ActualRFR,ActualRRL,ActualRRR," +
                     "ActualLeftFront,ActualLeftRear,ActualRightFront,ActualRightRear," +
+                    "PositionLFL,PositionLFR,PositionLRL,PositionLRR,PositionRFL,PositionRFR,PositionRRL,PositionRRR," +
+                    "CurrentLFLAmps,CurrentLFRAmps,CurrentLRLAmps,CurrentLRRAmps," +
+                    "CurrentRFLAmps,CurrentRFRAmps,CurrentRRLAmps,CurrentRRRAmps," +
                     "TargetThLeftFront,TargetThLeftRear,TargetThRightFront,TargetThRightRear," +
                     "ActualThLeftFront,ActualThLeftRear,ActualThRightFront,ActualThRightRear," +
-                    "ErrorThLeftFront,ErrorThLeftRear,ErrorThRightFront,ErrorThRightRear");
+                    "ErrorThLeftFront,ErrorThLeftRear,ErrorThRightFront,ErrorThRightRear," +
+                    "ActualThLeftFrontReceiveElapsedMs,ActualThLeftFrontReceiveSequence,ActualThLeftFrontAgeMs," +
+                    "ActualThLeftRearReceiveElapsedMs,ActualThLeftRearReceiveSequence,ActualThLeftRearAgeMs," +
+                    "ActualThRightFrontReceiveElapsedMs,ActualThRightFrontReceiveSequence,ActualThRightFrontAgeMs," +
+                    "ActualThRightRearReceiveElapsedMs,ActualThRightRearReceiveSequence,ActualThRightRearAgeMs");
 
                 while (_records.TryDequeue(out _))
                 {
@@ -99,9 +125,11 @@ namespace MedullaAdapter
 
                 _queuedRecordCount = 0;
                 _receiveSequence = 0;
+                _snapshotSequence = 0;
                 _droppedRecordCount = 0;
                 _lastSnapshotMilliseconds =
                     double.NegativeInfinity;
+                _startTimestamp = Stopwatch.GetTimestamp();
                 _stopwatch = Stopwatch.StartNew();
                 _isRunning = true;
 
@@ -154,13 +182,15 @@ namespace MedullaAdapter
             ushort canId,
             string motorName,
             float rawRpm,
-            float speedMetersPerSecond)
+            float speedMetersPerSecond,
+            float positionMillimeters)
         {
             if (!_isRunning)
                 return;
 
             var elapsedMilliseconds =
-                _stopwatch.Elapsed.TotalMilliseconds;
+                GetElapsedMilliseconds(
+                    Stopwatch.GetTimestamp());
             var receiveSequence =
                 Interlocked.Increment(
                     ref _receiveSequence);
@@ -171,9 +201,52 @@ namespace MedullaAdapter
                 receiveSequence.ToString(
                     CultureInfo.InvariantCulture),
                 $"0x{canId:X3}",
+                "MotorSpeedPosition",
                 motorName,
                 Format(rawRpm),
-                Format(speedMetersPerSecond));
+                Format(speedMetersPerSecond),
+                Format(positionMillimeters),
+                "",
+                "");
+
+            Enqueue(new LogRecord(
+                isCanEvent: true,
+                line));
+        }
+
+        /// <summary>
+        /// 按CAN回调到达时刻记录一帧舵角原始值和换算后的机械角度。
+        /// </summary>
+        public void RecordSteeringAngleFeedback(
+            ushort canId,
+            string wheelName,
+            int rawAngle,
+            float angleDegrees)
+        {
+            if (!_isRunning)
+                return;
+
+            var elapsedMilliseconds =
+                GetElapsedMilliseconds(
+                    Stopwatch.GetTimestamp());
+            var receiveSequence =
+                Interlocked.Increment(
+                    ref _receiveSequence);
+
+            var line = string.Join(
+                ",",
+                Format(elapsedMilliseconds),
+                receiveSequence.ToString(
+                    CultureInfo.InvariantCulture),
+                $"0x{canId:X3}",
+                "SteeringAngle",
+                wheelName,
+                "",
+                "",
+                "",
+                rawAngle.ToString(
+                    CultureInfo.InvariantCulture),
+                Format(angleDegrees));
 
             Enqueue(new LogRecord(
                 isCanEvent: true,
@@ -189,8 +262,9 @@ namespace MedullaAdapter
             if (!_isRunning || cart == null)
                 return;
 
+            var snapshotTimestamp = Stopwatch.GetTimestamp();
             var elapsedMilliseconds =
-                _stopwatch.Elapsed.TotalMilliseconds;
+                GetElapsedMilliseconds(snapshotTimestamp);
 
             if (elapsedMilliseconds -
                 _lastSnapshotMilliseconds <
@@ -202,14 +276,72 @@ namespace MedullaAdapter
             _lastSnapshotMilliseconds =
                 elapsedMilliseconds;
 
+            var snapshotSequence =
+                Interlocked.Increment(
+                    ref _snapshotSequence);
+            var controlTimestamp =
+                Interlocked.Read(
+                    ref cart.DiffSteerControlTimestamp);
+            var controlSequence =
+                Interlocked.Read(
+                    ref cart.DiffSteerControlSequence);
+            var commandTimestamp =
+                Interlocked.Read(
+                    ref cart.WheelCommandTimestamp);
+            var commandSequence =
+                Interlocked.Read(
+                    ref cart.WheelCommandSequence);
+            var actualThLeftFrontTimestamp =
+                Interlocked.Read(
+                    ref cart.ActualThLeftFrontTimestamp);
+            var actualThLeftFrontSequence =
+                Interlocked.Read(
+                    ref cart.ActualThLeftFrontSequence);
+            var actualThLeftRearTimestamp =
+                Interlocked.Read(
+                    ref cart.ActualThLeftRearTimestamp);
+            var actualThLeftRearSequence =
+                Interlocked.Read(
+                    ref cart.ActualThLeftRearSequence);
+            var actualThRightFrontTimestamp =
+                Interlocked.Read(
+                    ref cart.ActualThRightFrontTimestamp);
+            var actualThRightFrontSequence =
+                Interlocked.Read(
+                    ref cart.ActualThRightFrontSequence);
+            var actualThRightRearTimestamp =
+                Interlocked.Read(
+                    ref cart.ActualThRightRearTimestamp);
+            var actualThRightRearSequence =
+                Interlocked.Read(
+                    ref cart.ActualThRightRearSequence);
+
             var line = string.Join(
                 ",",
                 Format(elapsedMilliseconds),
+                snapshotSequence.ToString(
+                    CultureInfo.InvariantCulture),
+                FormatEventElapsedMilliseconds(controlTimestamp),
+                controlSequence.ToString(
+                    CultureInfo.InvariantCulture),
+                FormatEventAgeMilliseconds(
+                    snapshotTimestamp,
+                    controlTimestamp),
+                FormatEventElapsedMilliseconds(commandTimestamp),
+                commandSequence.ToString(
+                    CultureInfo.InvariantCulture),
+                FormatEventAgeMilliseconds(
+                    snapshotTimestamp,
+                    commandTimestamp),
                 cart.CarNum.ToString(
                     CultureInfo.InvariantCulture),
                 Format((int)cart.TransmitterControlMode),
                 Format(cart.ManualMode),
                 Format(cart.SendThresSpeed),
+                Format(cart.Voltage),
+                Format(cart.AlarmLevel),
+                Format(cart.ChassisMode),
+                FormatBoolean(cart.WheelAbleState),
                 Format(cart.DiffSteerKp),
                 Format(cart.DiffSteerKi),
                 Format(cart.DiffSteerKd),
@@ -217,10 +349,30 @@ namespace MedullaAdapter
                 Format(cart.DiffSteerDeadZone),
                 Format(cart.DiffSteerThresh),
                 Format(cart.DiffSteerSpeedAcc),
+                Format(cart.DiffSteerRateFeedforwardGain),
+                Format(cart.DiffSteerWheelDistanceMillimeters),
+                Format(cart.DiffSteerRateFeedforwardMaximumSpeed),
+                Format(cart.DiffSteerFeedforwardDeltaTimeMilliseconds),
+                Format(cart.DiffSteerTargetRateLeftFrontDegreesPerSecond),
+                Format(cart.DiffSteerTargetRateLeftRearDegreesPerSecond),
+                Format(cart.DiffSteerTargetRateRightFrontDegreesPerSecond),
+                Format(cart.DiffSteerTargetRateRightRearDegreesPerSecond),
+                FormatBoolean(cart.DiffSteerFeedforwardLimitedLeftFront),
+                FormatBoolean(cart.DiffSteerFeedforwardLimitedLeftRear),
+                FormatBoolean(cart.DiffSteerFeedforwardLimitedRightFront),
+                FormatBoolean(cart.DiffSteerFeedforwardLimitedRightRear),
                 Format(cart.DiffSteerOutputLeftFront),
                 Format(cart.DiffSteerOutputLeftRear),
                 Format(cart.DiffSteerOutputRightFront),
                 Format(cart.DiffSteerOutputRightRear),
+                Format(cart.DiffSteerRateFeedforwardLeftFront),
+                Format(cart.DiffSteerRateFeedforwardLeftRear),
+                Format(cart.DiffSteerRateFeedforwardRightFront),
+                Format(cart.DiffSteerRateFeedforwardRightRear),
+                Format(cart.DiffSteerTotalOutputLeftFront),
+                Format(cart.DiffSteerTotalOutputLeftRear),
+                Format(cart.DiffSteerTotalOutputRightFront),
+                Format(cart.DiffSteerTotalOutputRightRear),
                 Format(cart.SpeedLeftFrontLeft),
                 Format(cart.SpeedLeftFrontRight),
                 Format(cart.SpeedLeftRearLeft),
@@ -237,6 +389,35 @@ namespace MedullaAdapter
                 Format(cart.SpeedRFR),
                 Format(cart.SpeedRRL),
                 Format(cart.SpeedRRR),
+                Format(cart.SentSpeedLFL),
+                Format(cart.SentSpeedLFR),
+                Format(cart.SentSpeedLRL),
+                Format(cart.SentSpeedLRR),
+                Format(cart.SentSpeedRFL),
+                Format(cart.SentSpeedRFR),
+                Format(cart.SentSpeedRRL),
+                Format(cart.SentSpeedRRR),
+                FormatBoolean(cart.WheelCommandLimitedLFL),
+                FormatBoolean(cart.WheelCommandLimitedLFR),
+                FormatBoolean(cart.WheelCommandLimitedLRL),
+                FormatBoolean(cart.WheelCommandLimitedLRR),
+                FormatBoolean(cart.WheelCommandLimitedRFL),
+                FormatBoolean(cart.WheelCommandLimitedRFR),
+                FormatBoolean(cart.WheelCommandLimitedRRL),
+                FormatBoolean(cart.WheelCommandLimitedRRR),
+                FormatBoolean(
+                    cart.WheelCommandLimitedLFL ||
+                    cart.WheelCommandLimitedLFR),
+                FormatBoolean(
+                    cart.WheelCommandLimitedLRL ||
+                    cart.WheelCommandLimitedLRR),
+                FormatBoolean(
+                    cart.WheelCommandLimitedRFL ||
+                    cart.WheelCommandLimitedRFR),
+                FormatBoolean(
+                    cart.WheelCommandLimitedRRL ||
+                    cart.WheelCommandLimitedRRR),
+                FormatBoolean(cart.WheelCommandSuppressed),
                 Format(cart.ActualSpeedLeftFrontLeft),
                 Format(cart.ActualSpeedLeftFrontRight),
                 Format(cart.ActualSpeedLeftRearLeft),
@@ -249,6 +430,22 @@ namespace MedullaAdapter
                 Format(cart.ActualSpeedLeftRear),
                 Format(cart.ActualSpeedRightFront),
                 Format(cart.ActualSpeedRightRear),
+                Format(cart.LFLActualPos),
+                Format(cart.LFRActualPos),
+                Format(cart.LRLActualPos),
+                Format(cart.LRRActualPos),
+                Format(cart.RFLActualPos),
+                Format(cart.RFRActualPos),
+                Format(cart.RRLActualPos),
+                Format(cart.RRRActualPos),
+                Format(cart.LeftFrontLeftElectric),
+                Format(cart.LeftFrontRightElectric),
+                Format(cart.LeftRearLeftElectric),
+                Format(cart.LeftRearRightElectric),
+                Format(cart.RightFrontLeftElectric),
+                Format(cart.RightFrontRightElectric),
+                Format(cart.RightRearLeftElectric),
+                Format(cart.RightRearRightElectric),
                 Format(cart.ThLeftFront),
                 Format(cart.ThLeftRear),
                 Format(cart.ThRightFront),
@@ -260,7 +457,35 @@ namespace MedullaAdapter
                 Format(cart.ThLeftFront - cart.ActualThLeftFront),
                 Format(cart.ThLeftRear - cart.ActualThLeftRear),
                 Format(cart.ThRightFront - cart.ActualThRightFront),
-                Format(cart.ThRightRear - cart.ActualThRightRear));
+                Format(cart.ThRightRear - cart.ActualThRightRear),
+                FormatEventElapsedMilliseconds(
+                    actualThLeftFrontTimestamp),
+                actualThLeftFrontSequence.ToString(
+                    CultureInfo.InvariantCulture),
+                FormatEventAgeMilliseconds(
+                    snapshotTimestamp,
+                    actualThLeftFrontTimestamp),
+                FormatEventElapsedMilliseconds(
+                    actualThLeftRearTimestamp),
+                actualThLeftRearSequence.ToString(
+                    CultureInfo.InvariantCulture),
+                FormatEventAgeMilliseconds(
+                    snapshotTimestamp,
+                    actualThLeftRearTimestamp),
+                FormatEventElapsedMilliseconds(
+                    actualThRightFrontTimestamp),
+                actualThRightFrontSequence.ToString(
+                    CultureInfo.InvariantCulture),
+                FormatEventAgeMilliseconds(
+                    snapshotTimestamp,
+                    actualThRightFrontTimestamp),
+                FormatEventElapsedMilliseconds(
+                    actualThRightRearTimestamp),
+                actualThRightRearSequence.ToString(
+                    CultureInfo.InvariantCulture),
+                FormatEventAgeMilliseconds(
+                    snapshotTimestamp,
+                    actualThRightRearTimestamp));
 
             Enqueue(new LogRecord(
                 isCanEvent: false,
@@ -372,6 +597,54 @@ namespace MedullaAdapter
                 CultureInfo.InvariantCulture);
         }
 
+        /// <summary>
+        /// 将诊断布尔值写成便于MATLAB直接读取的0或1。
+        /// </summary>
+        private static string FormatBoolean(bool value)
+        {
+            return value ? "1" : "0";
+        }
+
+        /// <summary>
+        /// 将本机单调时钟值换算为相对本次日志开始的毫秒数。
+        /// </summary>
+        private double GetElapsedMilliseconds(long timestamp)
+        {
+            return (timestamp - _startTimestamp) *
+                   1000.0 /
+                   Stopwatch.Frequency;
+        }
+
+        /// <summary>
+        /// 格式化发生在本次记录期间的事件时刻，记录前事件返回空字段。
+        /// </summary>
+        private string FormatEventElapsedMilliseconds(long timestamp)
+        {
+            if (timestamp < _startTimestamp)
+                return "";
+
+            return Format(GetElapsedMilliseconds(timestamp));
+        }
+
+        /// <summary>
+        /// 计算快照时刻相对最近一次控制或反馈事件的数据年龄。
+        /// </summary>
+        private static string FormatEventAgeMilliseconds(
+            long currentTimestamp,
+            long eventTimestamp)
+        {
+            if (eventTimestamp <= 0 ||
+                eventTimestamp > currentTimestamp)
+            {
+                return "";
+            }
+
+            return Format(
+                (currentTimestamp - eventTimestamp) *
+                1000.0 /
+                Stopwatch.Frequency);
+        }
+
         public void Dispose()
         {
             Stop();

MedullaAdapter/WirelessFleetTransport.cs

@@ -0,0 +1 @@
+// M层负责串口读写、校验、收发状态

MultiWheelC.Tests/FleetControllerTests.cs

@@ -0,0 +1,374 @@
+using System;
+using MultiWheelC.Control.Abstractions;
+using MultiWheelC.Control.Allocation;
+using MultiWheelC.Fleet;
+using MultiWheelC.Trajectory;
+using MyParking.Shared;
+
+namespace MultiWheelC.Tests
+{
+    internal static class FleetControllerTests
+    {
+        private const double Tolerance = 1e-9;
+
+        public static void Run()
+        {
+            VerifyInactiveControllerStops();
+            VerifyStraightCommand();
+            VerifyFortyFiveDegreeMotionDirection();
+            VerifyNinetyDegreeMotionDirection();
+            VerifyInvalidVelocityUsesReferenceSpeed();
+            VerifyCompletionStops();
+            VerifyExcessiveTrackingErrorFaults();
+            VerifyCancelStops();
+
+            Console.WriteLine(
+                "FleetController车队中心控制测试通过。共8个场景。");
+        }
+
+        private static void VerifyInactiveControllerStops()
+        {
+            var controller = CreateController();
+            var result = controller.ComputeCommand(
+                CreateState(0.5, 0.0, 0.0, true),
+                0.02,
+                out var command);
+
+            AssertResult(
+                result,
+                FleetControlCycleResult.Inactive,
+                "未启动控制器");
+            AssertStop(command, "未启动控制器");
+        }
+
+        private static void VerifyStraightCommand()
+        {
+            var controller = CreateController();
+            controller.Start(CreateStraightTrajectory());
+
+            var result = controller.ComputeCommand(
+                CreateState(0.5, 0.0, 0.4, true),
+                0.02,
+                out var command);
+
+            AssertResult(
+                result,
+                FleetControlCycleResult.CommandGenerated,
+                "直线控制");
+            AssertNear(
+                command.ReferencePointInFleet.XMeters,
+                0.0,
+                "直线控制参考点X");
+            AssertNear(
+                command.ReferencePointInFleet.YMeters,
+                0.0,
+                "直线控制参考点Y");
+            AssertTwist(
+                command.TwistAtReferencePoint,
+                0.4,
+                0.0,
+                0.0,
+                "直线控制");
+        }
+
+        private static void VerifyInvalidVelocityUsesReferenceSpeed()
+        {
+            var controller = CreateController();
+            controller.Start(CreateStraightTrajectory());
+
+            var result = controller.ComputeCommand(
+                CreateState(0.5, 0.0, 0.0, false),
+                0.02,
+                out var command);
+
+            AssertResult(
+                result,
+                FleetControlCycleResult.CommandGenerated,
+                "速度尚未初始化");
+            AssertTwist(
+                command.TwistAtReferencePoint,
+                0.4,
+                0.0,
+                0.0,
+                "速度尚未初始化");
+        }
+
+        private static void VerifyFortyFiveDegreeMotionDirection()
+        {
+            VerifyMotionDirection(
+                Math.PI / 4.0,
+                "45度运动方向");
+        }
+
+        private static void VerifyNinetyDegreeMotionDirection()
+        {
+            VerifyMotionDirection(
+                Math.PI / 2.0,
+                "90度运动方向");
+        }
+
+        private static void VerifyMotionDirection(
+            double motionDirectionInFleetRadians,
+            string scenario)
+        {
+            var controller = CreateController(
+                motionDirectionInFleetRadians);
+            controller.Start(
+                CreateStraightTrajectory(
+                    motionDirectionInFleetRadians));
+
+            var directionX =
+                Math.Cos(motionDirectionInFleetRadians);
+            var directionY =
+                Math.Sin(motionDirectionInFleetRadians);
+            var result = controller.ComputeCommand(
+                CreateState(
+                    0.5 * directionX,
+                    0.5 * directionY,
+                    0.4 * directionX,
+                    0.4 * directionY,
+                    true),
+                0.02,
+                out var command);
+
+            AssertResult(
+                result,
+                FleetControlCycleResult.CommandGenerated,
+                scenario);
+            AssertTwist(
+                command.TwistAtReferencePoint,
+                0.4 * directionX,
+                0.4 * directionY,
+                0.0,
+                scenario);
+        }
+
+        private static void VerifyCompletionStops()
+        {
+            var controller = CreateController();
+            controller.Start(CreateStraightTrajectory());
+
+            var result = controller.ComputeCommand(
+                CreateState(1.0, 0.0, 0.0, true),
+                0.02,
+                out var command);
+
+            AssertResult(
+                result,
+                FleetControlCycleResult.Completed,
+                "终点完成");
+            AssertStop(command, "终点完成");
+
+            if (controller.IsActive || !controller.IsCompleted)
+            {
+                throw new InvalidOperationException(
+                    "终点完成后控制器状态错误。");
+            }
+        }
+
+        private static void VerifyExcessiveTrackingErrorFaults()
+        {
+            var controller = CreateController();
+            controller.Start(CreateStraightTrajectory());
+
+            var result = controller.ComputeCommand(
+                CreateState(0.5, 0.5, 0.0, true),
+                0.02,
+                out var command);
+
+            AssertResult(
+                result,
+                FleetControlCycleResult.Faulted,
+                "轨迹偏离保护");
+            AssertStop(command, "轨迹偏离保护");
+
+            if (string.IsNullOrWhiteSpace(
+                    controller.LastFailureReason))
+            {
+                throw new InvalidOperationException(
+                    "轨迹偏离故障没有保存原因。");
+            }
+        }
+
+        private static void VerifyCancelStops()
+        {
+            var controller = CreateController();
+            controller.Start(CreateStraightTrajectory());
+            controller.Cancel();
+
+            var result = controller.ComputeCommand(
+                CreateState(0.5, 0.0, 0.0, true),
+                0.02,
+                out var command);
+
+            AssertResult(
+                result,
+                FleetControlCycleResult.Inactive,
+                "取消控制");
+            AssertStop(command, "取消控制");
+        }
+
+        private static FleetController CreateController(
+            double motionDirectionInFleetRadians = 0.0)
+        {
+            return new FleetController(
+                new StraightLateralController(),
+                new ReferenceLongitudinalController(),
+                new GcpCommandAllocator(
+                    Math.PI / 4.0),
+                virtualControlPointRadiusMeters: 0.5,
+                motionDirectionInFleetRadians:
+                    motionDirectionInFleetRadians);
+        }
+
+        private static Trajectory2D CreateStraightTrajectory(
+            double motionDirectionInFleetRadians = 0.0)
+        {
+            var directionX =
+                Math.Cos(motionDirectionInFleetRadians);
+            var directionY =
+                Math.Sin(motionDirectionInFleetRadians);
+
+            return new Trajectory2D(
+                new[]
+                {
+                    new TrajectoryPoint(
+                        0.0,
+                        Pose2D.Identity,
+                        0.0,
+                        0.4),
+                    new TrajectoryPoint(
+                        1.0,
+                        new Pose2D(
+                            directionX,
+                            directionY,
+                            0.0),
+                        0.0,
+                        0.4)
+                });
+        }
+
+        private static FleetState CreateState(
+            double xMeters,
+            double yMeters,
+            double vxMetersPerSecond,
+            bool hasValidVelocityEstimate)
+        {
+            return CreateState(
+                xMeters,
+                yMeters,
+                vxMetersPerSecond,
+                0.0,
+                hasValidVelocityEstimate);
+        }
+
+        private static FleetState CreateState(
+            double xMeters,
+            double yMeters,
+            double vxMetersPerSecond,
+            double vyMetersPerSecond,
+            bool hasValidVelocityEstimate)
+        {
+            return new FleetState(
+                sampleTimestampSeconds: 1.0,
+                fleetPoseInWorld: new Pose2D(
+                    xMeters,
+                    yMeters,
+                    0.0),
+                twistAtFleetOriginInWorld: new Twist2D(
+                    vxMetersPerSecond,
+                    vyMetersPerSecond,
+                    0.0),
+                hasValidVelocityEstimate:
+                    hasValidVelocityEstimate);
+        }
+
+        private static void AssertResult(
+            FleetControlCycleResult actual,
+            FleetControlCycleResult expected,
+            string scenario)
+        {
+            if (actual != expected)
+            {
+                throw new InvalidOperationException(
+                    $"{scenario}结果错误：" +
+                    $"actual={actual}, expected={expected}。");
+            }
+        }
+
+        private static void AssertStop(
+            FleetMotionCommand command,
+            string scenario)
+        {
+            AssertTwist(
+                command.TwistAtReferencePoint,
+                0.0,
+                0.0,
+                0.0,
+                scenario);
+        }
+
+        private static void AssertTwist(
+            Twist2D actual,
+            double expectedVx,
+            double expectedVy,
+            double expectedOmega,
+            string scenario)
+        {
+            AssertNear(
+                actual.VxMetersPerSecond,
+                expectedVx,
+                $"{scenario} Vx");
+            AssertNear(
+                actual.VyMetersPerSecond,
+                expectedVy,
+                $"{scenario} Vy");
+            AssertNear(
+                actual.OmegaRadiansPerSecond,
+                expectedOmega,
+                $"{scenario} Omega");
+        }
+
+        private static void AssertNear(
+            double actual,
+            double expected,
+            string valueName)
+        {
+            if (Math.Abs(actual - expected) > Tolerance)
+            {
+                throw new InvalidOperationException(
+                    $"{valueName}错误：" +
+                    $"actual={actual:F9}, expected={expected:F9}。");
+            }
+        }
+
+        private sealed class StraightLateralController :
+            ILateralController
+        {
+            public LateralControlCommand Compute(
+                PathTrackingContext context)
+            {
+                return LateralControlCommand.Straight;
+            }
+
+            public void Reset()
+            {
+            }
+        }
+
+        private sealed class ReferenceLongitudinalController :
+            ILongitudinalController
+        {
+            public double ComputeSpeedMetersPerSecond(
+                PathTrackingContext context)
+            {
+                return context
+                    .ControlReferenceSpeedMetersPerSecond;
+            }
+
+            public void Reset()
+            {
+            }
+        }
+    }
+}

MultiWheelC.Tests/FleetCoordinatorTests.cs

@@ -0,0 +1,654 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.Control.Abstractions;
+using MultiWheelC.Control.Allocation;
+using MultiWheelC.Fleet;
+using MultiWheelC.Trajectory;
+using MyParking.Shared;
+
+namespace MultiWheelC.Tests
+{
+    internal static class FleetCoordinatorTests
+    {
+        private const double Tolerance = 1e-9;
+
+        public static void Run()
+        {
+            VerifyCommandCycle();
+            VerifySmallLayoutErrorIsCorrected();
+            VerifyWarningRangeScalesAllCommands();
+            VerifyUnavailableStateStopsAndRecovers();
+            VerifyUnsafeLayoutFaultsAndLatches();
+            VerifyCompletionStops();
+            VerifyTrackingFaultStops();
+            VerifyCancelReturnsInactive();
+
+            Console.WriteLine(
+                "FleetCoordinator车队协调测试通过。共8个场景。");
+        }
+
+        private static void VerifyCommandCycle()
+        {
+            var layout = CreateLayout();
+            var coordinator = CreateCoordinator();
+            coordinator.Start(layout, CreateTrajectory());
+
+            var result = coordinator.ExecuteCycle(
+                CreateRigidMemberStates(
+                    layout,
+                    new Pose2D(0.5, 0.0, 0.0),
+                    new Twist2D(0.4, 0.0, 0.0),
+                    1.0),
+                targetTimestampSeconds: 1.0,
+                deltaTimeSeconds: 0.02,
+                out var output);
+
+            AssertResult(
+                result,
+                FleetCoordinationCycleResult.CommandGenerated,
+                "正常协调周期");
+            if (!output.State.HasValue)
+            {
+                throw new InvalidOperationException(
+                    "正常协调周期没有返回车队状态。");
+            }
+
+            AssertNear(
+                output.State.Value.FleetPoseInWorld.XMeters,
+                0.5,
+                "正常协调周期中心X");
+            AssertNear(
+                output.SpeedScale,
+                1.0,
+                "正常协调周期速度比例");
+            AssertTwist(
+                output.FleetCommand.TwistAtReferencePoint,
+                0.4,
+                0.0,
+                0.0,
+                "正常协调周期车队命令");
+            AssertTwist(
+                FindCommand(output.MemberCommands, 1)
+                    .TwistInVehicleBody,
+                0.4,
+                0.0,
+                0.0,
+                "正常协调周期车辆1");
+            AssertTwist(
+                FindCommand(output.MemberCommands, 2)
+                    .TwistInVehicleBody,
+                -0.4,
+                0.0,
+                0.0,
+                "正常协调周期车辆2");
+        }
+
+        private static void VerifySmallLayoutErrorIsCorrected()
+        {
+            var layout = CreateLayout();
+            var coordinator = CreateCoordinator();
+            coordinator.Start(layout, CreateTrajectory());
+            var result = coordinator.ExecuteCycle(
+                new[]
+                {
+                    CreateMemberState(
+                        1,
+                        new Pose2D(-0.99, 0.0, 0.0),
+                        1.0),
+                    CreateMemberState(
+                        2,
+                        new Pose2D(0.99, 0.0, Math.PI),
+                        1.0)
+                },
+                targetTimestampSeconds: 1.0,
+                deltaTimeSeconds: 0.02,
+                out var output);
+
+            AssertResult(
+                result,
+                FleetCoordinationCycleResult.CommandGenerated,
+                "小范围布局误差纠偏");
+            AssertNear(
+                output.SpeedScale,
+                1.0,
+                "小范围布局误差速度比例");
+            AssertTwist(
+                FindCommand(output.BaseMemberCommands, 1)
+                    .TwistInVehicleBody,
+                0.4,
+                0.0,
+                0.0,
+                "车辆1基础命令");
+            AssertTwist(
+                FindCommand(output.BaseMemberCommands, 2)
+                    .TwistInVehicleBody,
+                -0.4,
+                0.0,
+                0.0,
+                "车辆2基础命令");
+            AssertTwist(
+                FindCommand(output.MemberCommands, 1)
+                    .TwistInVehicleBody,
+                0.395,
+                0.0,
+                0.0,
+                "车辆1纠偏后命令");
+            AssertTwist(
+                FindCommand(output.MemberCommands, 2)
+                    .TwistInVehicleBody,
+                -0.405,
+                0.0,
+                0.0,
+                "车辆2纠偏后命令");
+        }
+
+        private static void VerifyWarningRangeScalesAllCommands()
+        {
+            var layout = CreateLayout();
+            var coordinator = CreateCoordinator();
+            coordinator.Start(layout, CreateTrajectory());
+            var result = coordinator.ExecuteCycle(
+                new[]
+                {
+                    CreateMemberState(
+                        1,
+                        new Pose2D(-0.965, 0.0, 0.0),
+                        1.0),
+                    CreateMemberState(
+                        2,
+                        new Pose2D(0.965, 0.0, Math.PI),
+                        1.0)
+                },
+                targetTimestampSeconds: 1.0,
+                deltaTimeSeconds: 0.02,
+                out var output);
+
+            AssertResult(
+                result,
+                FleetCoordinationCycleResult.CommandGenerated,
+                "警告区间统一缩放");
+            AssertNear(
+                output.SpeedScale,
+                0.5,
+                "警告区间速度比例");
+            AssertTwist(
+                output.FleetCommand.TwistAtReferencePoint,
+                0.2,
+                0.0,
+                0.0,
+                "警告区间车队命令");
+            AssertTwist(
+                FindCommand(output.MemberCommands, 1)
+                    .TwistInVehicleBody,
+                0.2,
+                0.0,
+                0.0,
+                "警告区间车辆1");
+            AssertTwist(
+                FindCommand(output.MemberCommands, 2)
+                    .TwistInVehicleBody,
+                -0.2,
+                0.0,
+                0.0,
+                "警告区间车辆2");
+
+            if (string.IsNullOrWhiteSpace(output.Reason))
+            {
+                throw new InvalidOperationException(
+                    "警告区间缩放没有返回限制原因。");
+            }
+        }
+
+        private static void VerifyUnavailableStateStopsAndRecovers()
+        {
+            var layout = CreateLayout();
+            var coordinator = CreateCoordinator();
+            coordinator.Start(layout, CreateTrajectory());
+            var unavailableStates = CreateRigidMemberStates(
+                layout,
+                new Pose2D(0.5, 0.0, 0.0),
+                new Twist2D(0.4, 0.0, 0.0),
+                1.0);
+            unavailableStates[1] = new FleetMemberStateSample(
+                unavailableStates[1].VehicleId,
+                unavailableStates[1].SampleTimestampSeconds,
+                unavailableStates[1].PoseInWorld,
+                unavailableStates[1].TwistAtVehicleOriginInWorld,
+                isStateAvailable: false,
+                hasValidVelocityEstimate: true);
+
+            var waitingResult = coordinator.ExecuteCycle(
+                unavailableStates,
+                targetTimestampSeconds: 1.0,
+                deltaTimeSeconds: 0.02,
+                out var waitingOutput);
+
+            AssertResult(
+                waitingResult,
+                FleetCoordinationCycleResult.WaitingForState,
+                "状态暂时不可用");
+            AssertStop(waitingOutput, layout.VehicleCount);
+
+            if (!coordinator.IsActive)
+            {
+                throw new InvalidOperationException(
+                    "状态暂时不可用不应取消车队控制器。");
+            }
+
+            var recoveredResult = coordinator.ExecuteCycle(
+                CreateRigidMemberStates(
+                    layout,
+                    new Pose2D(0.5, 0.0, 0.0),
+                    new Twist2D(0.4, 0.0, 0.0),
+                    1.02),
+                targetTimestampSeconds: 1.02,
+                deltaTimeSeconds: 0.02,
+                out _);
+
+            AssertResult(
+                recoveredResult,
+                FleetCoordinationCycleResult.CommandGenerated,
+                "状态恢复");
+        }
+
+        private static void VerifyUnsafeLayoutFaultsAndLatches()
+        {
+            var layout = CreateLayout();
+            var coordinator = CreateCoordinator();
+            coordinator.Start(layout, CreateTrajectory());
+            var deformedStates = new[]
+            {
+                CreateMemberState(
+                    1,
+                    new Pose2D(-0.9, 0.0, 0.0),
+                    1.0),
+                CreateMemberState(
+                    2,
+                    new Pose2D(0.9, 0.0, Math.PI),
+                    1.0)
+            };
+
+            var result = coordinator.ExecuteCycle(
+                deformedStates,
+                targetTimestampSeconds: 1.0,
+                deltaTimeSeconds: 0.02,
+                out var output);
+
+            AssertResult(
+                result,
+                FleetCoordinationCycleResult.Faulted,
+                "布局误差超限");
+            AssertStop(output, layout.VehicleCount);
+
+            if (!coordinator.IsFaulted ||
+                string.IsNullOrWhiteSpace(
+                    coordinator.LastFailureReason))
+            {
+                throw new InvalidOperationException(
+                    "布局误差故障没有被锁存。");
+            }
+
+            var latchedResult = coordinator.ExecuteCycle(
+                CreateRigidMemberStates(
+                    layout,
+                    new Pose2D(0.5, 0.0, 0.0),
+                    new Twist2D(0.4, 0.0, 0.0),
+                    1.02),
+                targetTimestampSeconds: 1.02,
+                deltaTimeSeconds: 0.02,
+                out var latchedOutput);
+
+            AssertResult(
+                latchedResult,
+                FleetCoordinationCycleResult.Faulted,
+                "布局误差故障锁存");
+            AssertStop(latchedOutput, layout.VehicleCount);
+        }
+
+        private static void VerifyCompletionStops()
+        {
+            var layout = CreateLayout();
+            var coordinator = CreateCoordinator();
+            coordinator.Start(layout, CreateTrajectory());
+
+            var result = coordinator.ExecuteCycle(
+                CreateRigidMemberStates(
+                    layout,
+                    new Pose2D(1.0, 0.0, 0.0),
+                    Twist2D.Zero,
+                    1.0),
+                targetTimestampSeconds: 1.0,
+                deltaTimeSeconds: 0.02,
+                out var output);
+
+            AssertResult(
+                result,
+                FleetCoordinationCycleResult.Completed,
+                "车队轨迹完成");
+            AssertStop(output, layout.VehicleCount);
+
+            if (!coordinator.IsCompleted)
+            {
+                throw new InvalidOperationException(
+                    "车队轨迹完成状态没有被保存。");
+            }
+        }
+
+        private static void VerifyTrackingFaultStops()
+        {
+            var layout = CreateLayout();
+            var coordinator = CreateCoordinator();
+            coordinator.Start(layout, CreateTrajectory());
+
+            var result = coordinator.ExecuteCycle(
+                CreateRigidMemberStates(
+                    layout,
+                    new Pose2D(0.5, 0.5, 0.0),
+                    Twist2D.Zero,
+                    1.0),
+                targetTimestampSeconds: 1.0,
+                deltaTimeSeconds: 0.02,
+                out var output);
+
+            AssertResult(
+                result,
+                FleetCoordinationCycleResult.Faulted,
+                "车队中心跟踪故障");
+            AssertStop(output, layout.VehicleCount);
+
+            if (!coordinator.IsFaulted)
+            {
+                throw new InvalidOperationException(
+                    "车队中心跟踪故障没有传递到协调器。");
+            }
+        }
+
+        private static void VerifyCancelReturnsInactive()
+        {
+            var layout = CreateLayout();
+            var coordinator = CreateCoordinator();
+            coordinator.Start(layout, CreateTrajectory());
+            coordinator.Cancel();
+
+            var result = coordinator.ExecuteCycle(
+                CreateRigidMemberStates(
+                    layout,
+                    new Pose2D(0.5, 0.0, 0.0),
+                    Twist2D.Zero,
+                    1.0),
+                targetTimestampSeconds: 1.0,
+                deltaTimeSeconds: 0.02,
+                out var output);
+
+            AssertResult(
+                result,
+                FleetCoordinationCycleResult.Inactive,
+                "取消车队协调");
+            AssertStop(output, layout.VehicleCount);
+        }
+
+        private static FleetCoordinator CreateCoordinator()
+        {
+            var estimator = new FleetStateEstimator(
+                maximumMemberStateAgeSeconds: 0.25,
+                maximumPositionDisagreementMeters: 0.5,
+                maximumYawDisagreementRadians:
+                    AngleMath.DegreesToRadians(10.0));
+            var controller = new FleetController(
+                new StraightLateralController(),
+                new ReferenceLongitudinalController(),
+                new GcpCommandAllocator(Math.PI / 4.0),
+                virtualControlPointRadiusMeters: 0.5);
+            var commandCorrector =
+                new FleetMemberCommandCorrector(
+                    longitudinalPositionGainPerSecond: 1.0,
+                    lateralPositionGainPerSecond: 1.0,
+                    yawGainPerSecond: 1.0,
+                    positionErrorDeadbandMeters: 0.005,
+                    yawErrorDeadbandRadians:
+                        AngleMath.DegreesToRadians(0.5),
+                    maximumLinearCorrectionMetersPerSecond:
+                        0.03,
+                    maximumAngularCorrectionRadiansPerSecond:
+                        AngleMath.DegreesToRadians(2.0));
+
+            return new FleetCoordinator(
+                estimator,
+                controller,
+                commandCorrector,
+                memberPositionErrorWarningMeters: 0.02,
+                maximumMemberPositionErrorMeters: 0.05,
+                memberYawErrorWarningRadians:
+                    AngleMath.DegreesToRadians(1.0),
+                maximumMemberYawErrorRadians:
+                    AngleMath.DegreesToRadians(3.0));
+        }
+
+        private static FleetLayout CreateLayout()
+        {
+            return new FleetLayout(
+                new[]
+                {
+                    new VehicleLayout(
+                        1,
+                        new Pose2D(-1.0, 0.0, 0.0)),
+                    new VehicleLayout(
+                        2,
+                        new Pose2D(1.0, 0.0, Math.PI))
+                });
+        }
+
+        private static Trajectory2D CreateTrajectory()
+        {
+            return new Trajectory2D(
+                new[]
+                {
+                    new TrajectoryPoint(
+                        0.0,
+                        Pose2D.Identity,
+                        0.0,
+                        0.4),
+                    new TrajectoryPoint(
+                        1.0,
+                        new Pose2D(1.0, 0.0, 0.0),
+                        0.0,
+                        0.4)
+                });
+        }
+
+        private static FleetMemberStateSample[]
+            CreateRigidMemberStates(
+                FleetLayout layout,
+                Pose2D fleetPoseInWorld,
+                Twist2D twistAtFleetOriginInWorld,
+                double timestampSeconds)
+        {
+            var states =
+                new FleetMemberStateSample[layout.VehicleCount];
+
+            for (var index = 0;
+                 index < layout.Vehicles.Count;
+                 index++)
+            {
+                var vehicle = layout.Vehicles[index];
+                var poseInWorld = FrameTransform2D.Compose(
+                    fleetPoseInWorld,
+                    vehicle.PoseInFleet);
+                var offsetX =
+                    poseInWorld.XMeters -
+                    fleetPoseInWorld.XMeters;
+                var offsetY =
+                    poseInWorld.YMeters -
+                    fleetPoseInWorld.YMeters;
+                var twistInWorld = new Twist2D(
+                    twistAtFleetOriginInWorld.VxMetersPerSecond -
+                    twistAtFleetOriginInWorld.OmegaRadiansPerSecond *
+                    offsetY,
+                    twistAtFleetOriginInWorld.VyMetersPerSecond +
+                    twistAtFleetOriginInWorld.OmegaRadiansPerSecond *
+                    offsetX,
+                    twistAtFleetOriginInWorld.OmegaRadiansPerSecond);
+
+                states[index] = new FleetMemberStateSample(
+                    vehicle.VehicleId,
+                    timestampSeconds,
+                    poseInWorld,
+                    twistInWorld,
+                    isStateAvailable: true,
+                    hasValidVelocityEstimate: true);
+            }
+
+            return states;
+        }
+
+        private static FleetMemberStateSample CreateMemberState(
+            int vehicleId,
+            Pose2D poseInWorld,
+            double timestampSeconds)
+        {
+            return new FleetMemberStateSample(
+                vehicleId,
+                timestampSeconds,
+                poseInWorld,
+                Twist2D.Zero,
+                isStateAvailable: true,
+                hasValidVelocityEstimate: true);
+        }
+
+        private static FleetMemberCommand FindCommand(
+            IReadOnlyList<FleetMemberCommand> commands,
+            int vehicleId)
+        {
+            for (var index = 0;
+                 index < commands.Count;
+                 index++)
+            {
+                if (commands[index].VehicleId == vehicleId)
+                {
+                    return commands[index];
+                }
+            }
+
+            throw new InvalidOperationException(
+                $"没有找到车辆{vehicleId}的成员命令。");
+        }
+
+        private static void AssertStop(
+            FleetCoordinationCycleOutput output,
+            int expectedMemberCount)
+        {
+            AssertTwist(
+                output.FleetCommand.TwistAtReferencePoint,
+                0.0,
+                0.0,
+                0.0,
+                "车队停止命令");
+
+            if (output.MemberCommands.Count != expectedMemberCount)
+            {
+                throw new InvalidOperationException(
+                    "停止输出的成员命令数量错误。");
+            }
+
+            if (output.BaseMemberCommands.Count != expectedMemberCount)
+            {
+                throw new InvalidOperationException(
+                    "停止输出的成员基础命令数量错误。");
+            }
+
+            for (var index = 0;
+                 index < output.MemberCommands.Count;
+                 index++)
+            {
+                AssertTwist(
+                    output.BaseMemberCommands[index]
+                        .TwistInVehicleBody,
+                    0.0,
+                    0.0,
+                    0.0,
+                    "成员基础停止命令");
+                AssertTwist(
+                    output.MemberCommands[index].TwistInVehicleBody,
+                    0.0,
+                    0.0,
+                    0.0,
+                    "成员停止命令");
+            }
+        }
+
+        private static void AssertResult(
+            FleetCoordinationCycleResult actual,
+            FleetCoordinationCycleResult expected,
+            string scenario)
+        {
+            if (actual != expected)
+            {
+                throw new InvalidOperationException(
+                    $"{scenario}结果错误：" +
+                    $"actual={actual}, expected={expected}。");
+            }
+        }
+
+        private static void AssertTwist(
+            Twist2D actual,
+            double expectedVx,
+            double expectedVy,
+            double expectedOmega,
+            string scenario)
+        {
+            AssertNear(
+                actual.VxMetersPerSecond,
+                expectedVx,
+                scenario + " Vx");
+            AssertNear(
+                actual.VyMetersPerSecond,
+                expectedVy,
+                scenario + " Vy");
+            AssertNear(
+                actual.OmegaRadiansPerSecond,
+                expectedOmega,
+                scenario + " Omega");
+        }
+
+        private static void AssertNear(
+            double actual,
+            double expected,
+            string valueName)
+        {
+            if (Math.Abs(actual - expected) > Tolerance)
+            {
+                throw new InvalidOperationException(
+                    $"{valueName}错误：" +
+                    $"actual={actual:F9}, expected={expected:F9}。");
+            }
+        }
+
+        private sealed class StraightLateralController :
+            ILateralController
+        {
+            public LateralControlCommand Compute(
+                PathTrackingContext context)
+            {
+                return LateralControlCommand.Straight;
+            }
+
+            public void Reset()
+            {
+            }
+        }
+
+        private sealed class ReferenceLongitudinalController :
+            ILongitudinalController
+        {
+            public double ComputeSpeedMetersPerSecond(
+                PathTrackingContext context)
+            {
+                return context.ControlReferenceSpeedMetersPerSecond;
+            }
+
+            public void Reset()
+            {
+            }
+        }
+    }
+}

MultiWheelC.Tests/FleetKinematicsTests.cs

@@ -0,0 +1,139 @@
+using System;
+using System.Collections.Generic;
+using MyParking.Shared;
+
+namespace MultiWheelC.Tests
+{
+    internal static class FleetKinematicsTests
+    {
+        private const double Tolerance = 1e-9;
+
+        public static void Run()
+        {
+            var layout = CreateTailToTailLayout();
+
+            VerifyTranslation(layout);
+            VerifyRotationAroundFleetCenter(layout);
+            VerifyRotationAroundFirstVehicle(layout);
+            VerifyStop(layout);
+
+            Console.WriteLine(
+                "FleetKinematics刚体速度分配测试通过。共4个场景。");
+        }
+
+        private static FleetLayout CreateTailToTailLayout()
+        {
+            return new FleetLayout(
+                new[]
+                {
+                    new VehicleLayout(
+                        1,
+                        new Pose2D(1.0, 0.0, 0.0)),
+                    new VehicleLayout(
+                        2,
+                        new Pose2D(-1.0, 0.0, Math.PI))
+                });
+        }
+
+        private static void VerifyTranslation(FleetLayout layout)
+        {
+            var commands = FleetKinematics.Decompose(
+                layout,
+                new FleetMotionCommand(
+                    Point2D.Zero,
+                    new Twist2D(0.4, 0.0, 0.0)));
+
+            AssertTwist(Find(commands, 1), 0.4, 0.0, 0.0);
+            AssertTwist(Find(commands, 2), -0.4, 0.0, 0.0);
+        }
+
+        private static void VerifyRotationAroundFleetCenter(
+            FleetLayout layout)
+        {
+            var commands = FleetKinematics.Decompose(
+                layout,
+                FleetMotionCommand.RotateAround(
+                    Point2D.Zero,
+                    0.2));
+
+            AssertTwist(Find(commands, 1), 0.0, 0.2, 0.2);
+            AssertTwist(Find(commands, 2), 0.0, 0.2, 0.2);
+        }
+
+        private static void VerifyRotationAroundFirstVehicle(
+            FleetLayout layout)
+        {
+            var commands = FleetKinematics.Decompose(
+                layout,
+                FleetMotionCommand.RotateAround(
+                    new Point2D(1.0, 0.0),
+                    0.2));
+
+            AssertTwist(Find(commands, 1), 0.0, 0.0, 0.2);
+            AssertTwist(Find(commands, 2), 0.0, 0.4, 0.2);
+        }
+
+        private static void VerifyStop(FleetLayout layout)
+        {
+            var commands = FleetKinematics.Decompose(
+                layout,
+                FleetMotionCommand.Stop());
+
+            AssertTwist(Find(commands, 1), 0.0, 0.0, 0.0);
+            AssertTwist(Find(commands, 2), 0.0, 0.0, 0.0);
+        }
+
+        private static FleetMemberCommand Find(
+            IReadOnlyList<FleetMemberCommand> commands,
+            int vehicleId)
+        {
+            for (var index = 0; index < commands.Count; index++)
+            {
+                if (commands[index].VehicleId == vehicleId)
+                {
+                    return commands[index];
+                }
+            }
+
+            throw new InvalidOperationException(
+                $"没有找到车辆{vehicleId}的分配命令。");
+        }
+
+        private static void AssertTwist(
+            FleetMemberCommand command,
+            double expectedVx,
+            double expectedVy,
+            double expectedOmega)
+        {
+            AssertNear(
+                command.TwistInVehicleBody.VxMetersPerSecond,
+                expectedVx,
+                command.VehicleId,
+                "Vx");
+            AssertNear(
+                command.TwistInVehicleBody.VyMetersPerSecond,
+                expectedVy,
+                command.VehicleId,
+                "Vy");
+            AssertNear(
+                command.TwistInVehicleBody.OmegaRadiansPerSecond,
+                expectedOmega,
+                command.VehicleId,
+                "Omega");
+        }
+
+        private static void AssertNear(
+            double actual,
+            double expected,
+            int vehicleId,
+            string valueName)
+        {
+            if (Math.Abs(actual - expected) > Tolerance)
+            {
+                throw new InvalidOperationException(
+                    $"车辆{vehicleId}的{valueName}错误：" +
+                    $"actual={actual:F9}, expected={expected:F9}。");
+            }
+        }
+    }
+}

MultiWheelC.Tests/FleetLayoutCaptureTests.cs

@@ -0,0 +1,268 @@
+using System;
+using MultiWheelC.Fleet;
+using MyParking.Shared;
+
+namespace MultiWheelC.Tests
+{
+    internal static class FleetLayoutCaptureTests
+    {
+        private const double Tolerance = 1e-9;
+
+        public static void Run()
+        {
+            VerifySymmetricTailToTailLayout();
+            VerifyAsymmetricLayoutAndPoseReconstruction();
+            VerifyInputOrderDoesNotChangeResult();
+            VerifyEmptyInputIsRejected();
+            VerifyDuplicateVehicleIdIsRejected();
+            VerifyMissingLeaderIsRejected();
+
+            Console.WriteLine(
+                "FleetLayoutCapture布局建立测试通过。共6个场景。");
+        }
+
+        private static void VerifySymmetricTailToTailLayout()
+        {
+            var result = FleetLayoutCapture.Capture(
+                new[]
+                {
+                    new FleetMemberPose(
+                        1,
+                        new Pose2D(-1.2, 0.0, 0.0)),
+                    new FleetMemberPose(
+                        2,
+                        new Pose2D(1.2, 0.0, Math.PI))
+                },
+                leaderVehicleId: 1);
+
+            AssertPose(
+                result.FleetPoseInWorld,
+                Pose2D.Identity,
+                "对称双车中心");
+            AssertVehicleLayout(
+                result.Layout,
+                1,
+                new Pose2D(-1.2, 0.0, 0.0),
+                "对称双车主车布局");
+            AssertVehicleLayout(
+                result.Layout,
+                2,
+                new Pose2D(1.2, 0.0, Math.PI),
+                "对称双车从车布局");
+        }
+
+        private static void VerifyAsymmetricLayoutAndPoseReconstruction()
+        {
+            var members = new[]
+            {
+                new FleetMemberPose(
+                    3,
+                    new Pose2D(1.0, 1.0, 0.4)),
+                new FleetMemberPose(
+                    1,
+                    new Pose2D(4.0, 1.0, 0.4)),
+                new FleetMemberPose(
+                    2,
+                    new Pose2D(1.0, 4.0, -0.8))
+            };
+            var result = FleetLayoutCapture.Capture(
+                members,
+                leaderVehicleId: 1);
+
+            AssertPose(
+                result.FleetPoseInWorld,
+                new Pose2D(2.0, 2.0, 0.4),
+                "非对称三车中心");
+
+            for (var index = 0;
+                 index < members.Length;
+                 index++)
+            {
+                if (!result.Layout.TryGetVehicle(
+                        members[index].VehicleId,
+                        out var vehicleLayout))
+                {
+                    throw new InvalidOperationException(
+                        "非对称布局缺少成员车。" +
+                        members[index].VehicleId);
+                }
+
+                var reconstructedPoseInWorld =
+                    FrameTransform2D.Compose(
+                        result.FleetPoseInWorld,
+                        vehicleLayout.PoseInFleet);
+                AssertPose(
+                    reconstructedPoseInWorld,
+                    members[index].PoseInWorld,
+                    "非对称布局世界位姿还原");
+            }
+        }
+
+        private static void VerifyInputOrderDoesNotChangeResult()
+        {
+            var first = FleetLayoutCapture.Capture(
+                new[]
+                {
+                    new FleetMemberPose(
+                        1,
+                        new Pose2D(2.0, 3.0, 0.6)),
+                    new FleetMemberPose(
+                        2,
+                        new Pose2D(4.0, 5.0, -1.0))
+                },
+                leaderVehicleId: 1);
+            var second = FleetLayoutCapture.Capture(
+                new[]
+                {
+                    new FleetMemberPose(
+                        2,
+                        new Pose2D(4.0, 5.0, -1.0)),
+                    new FleetMemberPose(
+                        1,
+                        new Pose2D(2.0, 3.0, 0.6))
+                },
+                leaderVehicleId: 1);
+
+            AssertPose(
+                first.FleetPoseInWorld,
+                second.FleetPoseInWorld,
+                "输入顺序不变中心");
+            AssertSameLayout(first.Layout, second.Layout);
+        }
+
+        private static void VerifyEmptyInputIsRejected()
+        {
+            ExpectException<ArgumentException>(
+                () => FleetLayoutCapture.Capture(
+                    Array.Empty<FleetMemberPose>(),
+                    leaderVehicleId: 1),
+                "空成员集合");
+        }
+
+        private static void VerifyDuplicateVehicleIdIsRejected()
+        {
+            ExpectException<ArgumentException>(
+                () => FleetLayoutCapture.Capture(
+                    new[]
+                    {
+                        new FleetMemberPose(
+                            1,
+                            Pose2D.Identity),
+                        new FleetMemberPose(
+                            1,
+                            new Pose2D(1.0, 0.0, 0.0))
+                    },
+                    leaderVehicleId: 1),
+                "重复车号");
+        }
+
+        private static void VerifyMissingLeaderIsRejected()
+        {
+            ExpectException<ArgumentException>(
+                () => FleetLayoutCapture.Capture(
+                    new[]
+                    {
+                        new FleetMemberPose(
+                            2,
+                            Pose2D.Identity)
+                    },
+                    leaderVehicleId: 1),
+                "缺少主车");
+        }
+
+        private static void AssertSameLayout(
+            FleetLayout first,
+            FleetLayout second)
+        {
+            if (first.VehicleCount != second.VehicleCount)
+            {
+                throw new InvalidOperationException(
+                    "输入顺序变化后成员数量发生变化。");
+            }
+
+            for (var index = 0;
+                 index < first.Vehicles.Count;
+                 index++)
+            {
+                var vehicle = first.Vehicles[index];
+                AssertVehicleLayout(
+                    second,
+                    vehicle.VehicleId,
+                    vehicle.PoseInFleet,
+                    "输入顺序不变布局");
+            }
+        }
+
+        private static void AssertVehicleLayout(
+            FleetLayout layout,
+            int vehicleId,
+            Pose2D expectedPoseInFleet,
+            string scenario)
+        {
+            if (!layout.TryGetVehicle(
+                    vehicleId,
+                    out var vehicle))
+            {
+                throw new InvalidOperationException(
+                    $"{scenario}缺少车辆{vehicleId}。");
+            }
+
+            AssertPose(
+                vehicle.PoseInFleet,
+                expectedPoseInFleet,
+                scenario);
+        }
+
+        private static void AssertPose(
+            Pose2D actual,
+            Pose2D expected,
+            string scenario)
+        {
+            AssertNear(
+                actual.XMeters,
+                expected.XMeters,
+                scenario + " X");
+            AssertNear(
+                actual.YMeters,
+                expected.YMeters,
+                scenario + " Y");
+            AssertNear(
+                AngleMath.NormalizeRadians(
+                    actual.YawRadians -
+                    expected.YawRadians),
+                0.0,
+                scenario + " Yaw");
+        }
+
+        private static void AssertNear(
+            double actual,
+            double expected,
+            string valueName)
+        {
+            if (Math.Abs(actual - expected) > Tolerance)
+            {
+                throw new InvalidOperationException(
+                    $"{valueName}错误：" +
+                    $"actual={actual:F9}, expected={expected:F9}。");
+            }
+        }
+
+        private static void ExpectException<TException>(
+            Action action,
+            string scenario)
+            where TException : Exception
+        {
+            try
+            {
+                action();
+            }
+            catch (TException)
+            {
+                return;
+            }
+
+            throw new InvalidOperationException(
+                $"{scenario}没有抛出{typeof(TException).Name}。");
+        }
+    }
+}

MultiWheelC.Tests/FleetMemberAgentTests.cs

@@ -0,0 +1,240 @@
+using System;
+using System.Numerics;
+using CommonUsage.Chassis;
+using MultiWheelC.Fleet;
+using MyParking.Shared;
+
+namespace MultiWheelC.Tests
+{
+    internal static class FleetMemberAgentTests
+    {
+        private const long PlanId = 11;
+        private const int VehicleId = 2;
+
+        public static void Run()
+        {
+            VerifyActiveWatchdogExpires();
+            VerifyValidMotionCommandRefreshesDeadline();
+            VerifyRepeatedActivationDoesNotRefreshDeadline();
+            VerifyLateActivationCannotResumeMotion();
+            VerifyStopClearsWatchdog();
+
+            Console.WriteLine(
+                "FleetMemberAgent本地命令看门狗测试通过，共5个场景。");
+        }
+
+        private static void VerifyActiveWatchdogExpires()
+        {
+            var agent = CreateReadyAgent();
+            AssertTrue(
+                agent.Activate(
+                    PlanId,
+                    commandReceivedTimeSeconds: 10.0,
+                    validForSeconds: 0.5),
+                "成员车应当成功激活");
+
+            AssertTrue(
+                agent.UpdateCommandWatchdog(10.5),
+                "截止时刻仍应视为有效");
+            AssertFalse(
+                agent.UpdateCommandWatchdog(10.501),
+                "超过命令有效期后应停车");
+            AssertState(
+                agent,
+                FleetMemberAgentState.Faulted,
+                "命令超时");
+            AssertTrue(
+                !string.IsNullOrWhiteSpace(
+                    agent.LastFailureReason),
+                "命令超时应保留故障原因");
+        }
+
+        private static void VerifyValidMotionCommandRefreshesDeadline()
+        {
+            var agent = CreateReadyAgent();
+            agent.Activate(
+                PlanId,
+                commandReceivedTimeSeconds: 20.0,
+                validForSeconds: 0.5);
+
+            var accepted = agent.Execute(
+                PlanId,
+                new FleetMemberCommand(
+                    VehicleId,
+                    Twist2D.Zero),
+                commandReceivedTimeSeconds: 20.4,
+                validForSeconds: 0.7);
+
+            AssertTrue(
+                accepted,
+                "有效速度命令应被接受");
+            AssertNear(
+                agent.LastAcceptedCommandTimeSeconds,
+                20.4,
+                "最近命令接收时间");
+            AssertNear(
+                agent.CommandDeadlineSeconds,
+                21.1,
+                "速度命令刷新后的截止时间");
+            AssertTrue(
+                agent.UpdateCommandWatchdog(20.8),
+                "刷新截止时间后车辆应保持Active");
+        }
+
+        private static void VerifyRepeatedActivationDoesNotRefreshDeadline()
+        {
+            var agent = CreateReadyAgent();
+            agent.Activate(
+                PlanId,
+                commandReceivedTimeSeconds: 30.0,
+                validForSeconds: 0.5);
+
+            AssertTrue(
+                agent.Activate(
+                    PlanId,
+                    commandReceivedTimeSeconds: 30.4,
+                    validForSeconds: 0.5),
+                "未超时的重复激活命令应允许幂等确认");
+            AssertNear(
+                agent.CommandDeadlineSeconds,
+                30.5,
+                "重复激活不能替代运动命令刷新截止时间");
+            AssertFalse(
+                agent.UpdateCommandWatchdog(30.501),
+                "没有收到运动命令时仍应按最初激活期限停车");
+        }
+
+        private static void VerifyLateActivationCannotResumeMotion()
+        {
+            var agent = CreateReadyAgent();
+            agent.Activate(
+                PlanId,
+                commandReceivedTimeSeconds: 40.0,
+                validForSeconds: 0.5);
+
+            AssertFalse(
+                agent.Activate(
+                    PlanId,
+                    commandReceivedTimeSeconds: 40.6,
+                    validForSeconds: 0.5),
+                "迟到的激活命令不能恢复已经失联的车辆");
+            AssertState(
+                agent,
+                FleetMemberAgentState.Faulted,
+                "迟到激活命令");
+        }
+
+        private static void VerifyStopClearsWatchdog()
+        {
+            var agent = CreateReadyAgent();
+            agent.Activate(
+                PlanId,
+                commandReceivedTimeSeconds: 50.0,
+                validForSeconds: 0.5);
+
+            agent.Stop();
+
+            AssertState(
+                agent,
+                FleetMemberAgentState.Idle,
+                "正常停止");
+            AssertTrue(
+                !agent.LastAcceptedCommandTimeSeconds.HasValue &&
+                !agent.CommandDeadlineSeconds.HasValue,
+                "正常停止后应清除命令看门狗");
+        }
+
+        private static FleetMemberAgent CreateReadyAgent()
+        {
+            var chassis = new MultiWheelChassis();
+            chassis.AddWheel(CreateWheel(-500f, 300f));
+            chassis.AddWheel(CreateWheel(-500f, -300f));
+            chassis.AddWheel(CreateWheel(500f, 300f));
+            chassis.AddWheel(CreateWheel(500f, -300f));
+            chassis.Initialize();
+
+            var agent = new FleetMemberAgent(
+                new MultiWheelChassisAdapter(
+                    chassis,
+                    VehicleId),
+                alignmentToleranceRadians:
+                    AngleMath.DegreesToRadians(1.0),
+                alignmentStableSeconds: 0.0);
+
+            AssertTrue(
+                agent.BeginRollingPreparation(
+                    PlanId,
+                    motionDirectionInBodyRadians: 0.0),
+                "滚动运动系准备命令应被接受");
+            AssertState(
+                agent,
+                FleetMemberAgentState.Preparing,
+                "开始准备");
+            AssertTrue(
+                agent.UpdatePreparation(0.01) ==
+                FleetMemberAgentState.Ready,
+                "内存底盘舵轮应立即准备完成");
+
+            return agent;
+        }
+
+        private static SteerWheel CreateWheel(
+            float xMillimeters,
+            float yMillimeters)
+        {
+            var speed = 0f;
+            var angle = 0f;
+
+            return new SteerWheel(
+                new Vector2(
+                    xMillimeters,
+                    yMillimeters),
+                angleLowerLimit: -120f,
+                angleUpperLimit: 120f,
+                speedWriter: value => speed = value,
+                speedReader: () => speed,
+                angleWriter: value => angle = value,
+                angleReader: () => angle);
+        }
+
+        private static void AssertState(
+            FleetMemberAgent agent,
+            FleetMemberAgentState expected,
+            string scenario)
+        {
+            AssertTrue(
+                agent.State == expected,
+                $"{scenario}后的状态应为{expected}，" +
+                $"实际为{agent.State}。");
+        }
+
+        private static void AssertNear(
+            double? actual,
+            double expected,
+            string name)
+        {
+            AssertTrue(
+                actual.HasValue &&
+                Math.Abs(actual.Value - expected) <= 1e-9,
+                $"{name}不正确，期望{expected:F6}，" +
+                $"实际{actual?.ToString("F6") ?? "null"}。");
+        }
+
+        private static void AssertTrue(
+            bool condition,
+            string message)
+        {
+            if (!condition)
+            {
+                throw new InvalidOperationException(message);
+            }
+        }
+
+        private static void AssertFalse(
+            bool condition,
+            string message)
+        {
+            AssertTrue(!condition, message);
+        }
+    }
+}

MultiWheelC.Tests/FleetMemberCommandCorrectorTests.cs

@@ -0,0 +1,302 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.Fleet;
+using MyParking.Shared;
+
+namespace MultiWheelC.Tests
+{
+    internal static class FleetMemberCommandCorrectorTests
+    {
+        private const double Tolerance = 1e-9;
+
+        public static void Run()
+        {
+            VerifyZeroErrorsPreserveBaseCommands();
+            VerifyRelativePositionErrorProducesOpposingCorrection();
+            VerifyCommonTranslationIsRemoved();
+            VerifyCommonRotationIsRemoved();
+            VerifyDeadbandSuppressesSmallErrors();
+            VerifyCorrectionLimits();
+
+            Console.WriteLine(
+                "FleetMemberCommandCorrector测试通过，共6个场景。");
+        }
+
+        private static void VerifyZeroErrorsPreserveBaseCommands()
+        {
+            var layout = CreateLayout();
+            var baseCommands = FleetKinematics.Decompose(
+                layout,
+                new FleetMotionCommand(
+                    Point2D.Zero,
+                    new Twist2D(0.4, 0.1, 0.05)));
+            var corrected = CreateCorrector().Correct(
+                layout,
+                baseCommands,
+                CreateErrors(Pose2D.Identity, Pose2D.Identity));
+
+            AssertCommandsEqual(
+                corrected,
+                baseCommands,
+                "零布局误差");
+        }
+
+        private static void
+            VerifyRelativePositionErrorProducesOpposingCorrection()
+        {
+            var layout = CreateLayout();
+            var corrected = CreateCorrector().Correct(
+                layout,
+                CreateStopCommands(layout),
+                CreateErrors(
+                    new Pose2D(0.02, 0.0, 0.0),
+                    new Pose2D(0.02, 0.0, 0.0)));
+
+            AssertTwist(
+                FindCommand(corrected, 1).TwistInVehicleBody,
+                -0.02,
+                0.0,
+                0.0,
+                "车辆1相对位置纠偏");
+            AssertTwist(
+                FindCommand(corrected, 2).TwistInVehicleBody,
+                -0.02,
+                0.0,
+                0.0,
+                "车辆2相对位置纠偏");
+        }
+
+        private static void VerifyCommonTranslationIsRemoved()
+        {
+            var layout = CreateLayout();
+            var corrected = CreateCorrector().Correct(
+                layout,
+                CreateStopCommands(layout),
+                CreateErrors(
+                    new Pose2D(0.02, 0.0, 0.0),
+                    new Pose2D(-0.02, 0.0, 0.0)));
+
+            AssertAllStopped(
+                corrected,
+                "共同平移不应成为成员相对纠偏");
+        }
+
+        private static void VerifyCommonRotationIsRemoved()
+        {
+            const double fleetYawErrorRadians = 0.02;
+            var layout = CreateLayout();
+            var commonRotationError = new Pose2D(
+                0.0,
+                -fleetYawErrorRadians,
+                fleetYawErrorRadians);
+            var corrected = CreateCorrector().Correct(
+                layout,
+                CreateStopCommands(layout),
+                CreateErrors(
+                    commonRotationError,
+                    commonRotationError));
+
+            AssertAllStopped(
+                corrected,
+                "共同旋转不应成为成员相对纠偏");
+        }
+
+        private static void VerifyDeadbandSuppressesSmallErrors()
+        {
+            var layout = CreateLayout();
+            var corrector = new FleetMemberCommandCorrector(
+                longitudinalPositionGainPerSecond: 1.0,
+                lateralPositionGainPerSecond: 1.0,
+                yawGainPerSecond: 1.0,
+                positionErrorDeadbandMeters: 0.005,
+                yawErrorDeadbandRadians: 0.02,
+                maximumLinearCorrectionMetersPerSecond: 1.0,
+                maximumAngularCorrectionRadiansPerSecond: 1.0);
+            var corrected = corrector.Correct(
+                layout,
+                CreateStopCommands(layout),
+                CreateErrors(
+                    new Pose2D(0.004, 0.003, 0.01),
+                    new Pose2D(0.004, -0.003, -0.01)));
+
+            AssertAllStopped(corrected, "布局误差死区");
+        }
+
+        private static void VerifyCorrectionLimits()
+        {
+            var layout = CreateLayout();
+            var corrector = new FleetMemberCommandCorrector(
+                longitudinalPositionGainPerSecond: 1.0,
+                lateralPositionGainPerSecond: 1.0,
+                yawGainPerSecond: 1.0,
+                positionErrorDeadbandMeters: 0.0,
+                yawErrorDeadbandRadians: 0.0,
+                maximumLinearCorrectionMetersPerSecond: 0.03,
+                maximumAngularCorrectionRadiansPerSecond: 0.05);
+            var corrected = corrector.Correct(
+                layout,
+                CreateStopCommands(layout),
+                CreateErrors(
+                    new Pose2D(0.2, 0.0, 0.2),
+                    new Pose2D(0.2, 0.0, -0.2)));
+
+            for (var index = 0; index < corrected.Count; index++)
+            {
+                var twist = corrected[index].TwistInVehicleBody;
+                var linearMagnitude = Math.Sqrt(
+                    twist.VxMetersPerSecond *
+                    twist.VxMetersPerSecond +
+                    twist.VyMetersPerSecond *
+                    twist.VyMetersPerSecond);
+
+                AssertNear(
+                    linearMagnitude,
+                    0.03,
+                    "线速度纠偏限幅");
+                AssertNear(
+                    Math.Abs(twist.OmegaRadiansPerSecond),
+                    0.05,
+                    "角速度纠偏限幅");
+            }
+        }
+
+        private static FleetMemberCommandCorrector CreateCorrector()
+        {
+            return new FleetMemberCommandCorrector(
+                longitudinalPositionGainPerSecond: 1.0,
+                lateralPositionGainPerSecond: 1.0,
+                yawGainPerSecond: 1.0,
+                positionErrorDeadbandMeters: 0.0,
+                yawErrorDeadbandRadians: 0.0,
+                maximumLinearCorrectionMetersPerSecond: 1.0,
+                maximumAngularCorrectionRadiansPerSecond: 1.0);
+        }
+
+        private static FleetLayout CreateLayout()
+        {
+            return new FleetLayout(
+                new[]
+                {
+                    new VehicleLayout(
+                        1,
+                        new Pose2D(-1.0, 0.0, 0.0)),
+                    new VehicleLayout(
+                        2,
+                        new Pose2D(1.0, 0.0, Math.PI))
+                });
+        }
+
+        private static IReadOnlyList<FleetMemberCommand>
+            CreateStopCommands(FleetLayout layout)
+        {
+            return FleetKinematics.Decompose(
+                layout,
+                FleetMotionCommand.Stop());
+        }
+
+        private static FleetMemberLayoutError[] CreateErrors(
+            Pose2D vehicle1Error,
+            Pose2D vehicle2Error)
+        {
+            return new[]
+            {
+                new FleetMemberLayoutError(1, vehicle1Error),
+                new FleetMemberLayoutError(2, vehicle2Error)
+            };
+        }
+
+        private static FleetMemberCommand FindCommand(
+            IReadOnlyList<FleetMemberCommand> commands,
+            int vehicleId)
+        {
+            for (var index = 0; index < commands.Count; index++)
+            {
+                if (commands[index].VehicleId == vehicleId)
+                {
+                    return commands[index];
+                }
+            }
+
+            throw new InvalidOperationException(
+                $"没有找到车辆{vehicleId}的成员命令。");
+        }
+
+        private static void AssertCommandsEqual(
+            IReadOnlyList<FleetMemberCommand> actual,
+            IReadOnlyList<FleetMemberCommand> expected,
+            string scenario)
+        {
+            if (actual.Count != expected.Count)
+            {
+                throw new InvalidOperationException(
+                    $"{scenario}的命令数量不一致。");
+            }
+
+            for (var index = 0; index < expected.Count; index++)
+            {
+                var expectedCommand = expected[index];
+                var actualCommand = FindCommand(
+                    actual,
+                    expectedCommand.VehicleId);
+                AssertTwist(
+                    actualCommand.TwistInVehicleBody,
+                    expectedCommand.TwistInVehicleBody
+                        .VxMetersPerSecond,
+                    expectedCommand.TwistInVehicleBody
+                        .VyMetersPerSecond,
+                    expectedCommand.TwistInVehicleBody
+                        .OmegaRadiansPerSecond,
+                    scenario);
+            }
+        }
+
+        private static void AssertAllStopped(
+            IReadOnlyList<FleetMemberCommand> commands,
+            string scenario)
+        {
+            for (var index = 0; index < commands.Count; index++)
+            {
+                AssertTwist(
+                    commands[index].TwistInVehicleBody,
+                    0.0,
+                    0.0,
+                    0.0,
+                    scenario);
+            }
+        }
+
+        private static void AssertTwist(
+            Twist2D actual,
+            double expectedVx,
+            double expectedVy,
+            double expectedOmega,
+            string scenario)
+        {
+            AssertNear(
+                actual.VxMetersPerSecond,
+                expectedVx,
+                scenario + " Vx");
+            AssertNear(
+                actual.VyMetersPerSecond,
+                expectedVy,
+                scenario + " Vy");
+            AssertNear(
+                actual.OmegaRadiansPerSecond,
+                expectedOmega,
+                scenario + " Omega");
+        }
+
+        private static void AssertNear(
+            double actual,
+            double expected,
+            string valueName)
+        {
+            if (Math.Abs(actual - expected) > Tolerance)
+            {
+                throw new InvalidOperationException(
+                    $"{valueName}错误：" +
+                    $"actual={actual:F9}, expected={expected:F9}。");
+            }
+        }
+    }
+}

MultiWheelC.Tests/FleetPreparationCoordinatorTests.cs

@@ -0,0 +1,310 @@
+using System;
+using MultiWheelC.Fleet;
+using MyParking.Shared;
+
+namespace MultiWheelC.Tests
+{
+    internal static class FleetPreparationCoordinatorTests
+    {
+        private const double Tolerance = 1e-9;
+
+        public static void Run()
+        {
+            VerifyLeaderYawExample();
+            VerifyTailToTailUsesEquivalentAxis();
+            VerifyAllMembersMustBeReady();
+            VerifyStaleStatusIsIgnored();
+            VerifyMemberFaultIsLatched();
+            VerifyFaultAfterAuthorizationIsLatched();
+            VerifyPrematureActiveIsRejected();
+            VerifyCancelClearsPlan();
+
+            Console.WriteLine(
+                "FleetPreparationCoordinator测试通过。共8个场景。");
+        }
+
+        private static void VerifyLeaderYawExample()
+        {
+            var capture = FleetLayoutCapture.Capture(
+                new[]
+                {
+                    new FleetMemberPose(
+                        1,
+                        new Pose2D(
+                            -1.0,
+                            0.0,
+                            AngleMath.DegreesToRadians(20.0))),
+                    new FleetMemberPose(
+                        2,
+                        new Pose2D(1.0, 0.0, 0.0))
+                },
+                leaderVehicleId: 1);
+            var coordinator =
+                new FleetPreparationCoordinator();
+
+            coordinator.StartRollingPreparation(
+                planId: 1,
+                capture.Layout,
+                motionDirectionInFleetRadians: 0.0);
+
+            AssertTargetDegrees(coordinator, 1, 0.0);
+            AssertTargetDegrees(coordinator, 2, 20.0);
+        }
+
+        private static void VerifyTailToTailUsesEquivalentAxis()
+        {
+            var coordinator =
+                new FleetPreparationCoordinator();
+            coordinator.StartRollingPreparation(
+                planId: 2,
+                CreateTailToTailLayout(),
+                motionDirectionInFleetRadians: 0.0);
+
+            AssertTargetDegrees(coordinator, 1, 0.0);
+            AssertTargetDegrees(coordinator, 2, 0.0);
+        }
+
+        private static void VerifyAllMembersMustBeReady()
+        {
+            var coordinator = CreateStartedCoordinator(3);
+
+            AssertState(
+                coordinator.ReportMemberStatus(
+                    CreateStatus(
+                        3,
+                        1,
+                        FleetMemberAgentState.Ready)),
+                FleetPreparationCoordinatorState
+                    .WaitingForMembers,
+                "仅一辆车Ready");
+            AssertState(
+                coordinator.ReportMemberStatus(
+                    CreateStatus(
+                        3,
+                        2,
+                        FleetMemberAgentState.Ready)),
+                FleetPreparationCoordinatorState
+                    .ReadyToActivate,
+                "全部成员Ready");
+            AssertState(
+                coordinator.ReportMemberStatus(
+                    CreateStatus(
+                        3,
+                        1,
+                        FleetMemberAgentState.Preparing)),
+                FleetPreparationCoordinatorState
+                    .WaitingForMembers,
+                "成员失去Ready");
+            AssertState(
+                coordinator.ReportMemberStatus(
+                    CreateStatus(
+                        3,
+                        1,
+                        FleetMemberAgentState.Ready)),
+                FleetPreparationCoordinatorState
+                    .ReadyToActivate,
+                "成员重新Ready");
+
+            if (coordinator.TryAuthorizeActivation(4))
+            {
+                throw new InvalidOperationException(
+                    "错误任务编号不应获得激活授权。");
+            }
+
+            if (!coordinator.TryAuthorizeActivation(3))
+            {
+                throw new InvalidOperationException(
+                    "全部成员Ready后没有获得激活授权。");
+            }
+
+            AssertState(
+                coordinator.State,
+                FleetPreparationCoordinatorState
+                    .ActivationAuthorized,
+                "统一激活授权");
+        }
+
+        private static void VerifyStaleStatusIsIgnored()
+        {
+            var coordinator = CreateStartedCoordinator(5);
+            var state = coordinator.ReportMemberStatus(
+                CreateStatus(
+                    4,
+                    1,
+                    FleetMemberAgentState.Ready));
+
+            AssertState(
+                state,
+                FleetPreparationCoordinatorState
+                    .WaitingForMembers,
+                "旧任务状态报告");
+        }
+
+        private static void VerifyMemberFaultIsLatched()
+        {
+            var coordinator = CreateStartedCoordinator(6);
+            var state = coordinator.ReportMemberStatus(
+                CreateStatus(
+                    6,
+                    2,
+                    FleetMemberAgentState.Faulted,
+                    "舵轮未到位"));
+
+            AssertState(
+                state,
+                FleetPreparationCoordinatorState.Faulted,
+                "成员准备故障");
+            if (string.IsNullOrWhiteSpace(
+                    coordinator.LastFailureReason))
+            {
+                throw new InvalidOperationException(
+                    "成员准备故障没有保存原因。");
+            }
+        }
+
+        private static void VerifyFaultAfterAuthorizationIsLatched()
+        {
+            var coordinator = CreateStartedCoordinator(9);
+            coordinator.ReportMemberStatus(
+                CreateStatus(
+                    9,
+                    1,
+                    FleetMemberAgentState.Ready));
+            coordinator.ReportMemberStatus(
+                CreateStatus(
+                    9,
+                    2,
+                    FleetMemberAgentState.Ready));
+            coordinator.TryAuthorizeActivation(9);
+
+            var state = coordinator.ReportMemberStatus(
+                CreateStatus(
+                    9,
+                    2,
+                    FleetMemberAgentState.Faulted,
+                    "激活失败"));
+
+            AssertState(
+                state,
+                FleetPreparationCoordinatorState.Faulted,
+                "授权后的成员故障");
+        }
+
+        private static void VerifyPrematureActiveIsRejected()
+        {
+            var coordinator = CreateStartedCoordinator(7);
+            var state = coordinator.ReportMemberStatus(
+                CreateStatus(
+                    7,
+                    1,
+                    FleetMemberAgentState.Active));
+
+            AssertState(
+                state,
+                FleetPreparationCoordinatorState.Faulted,
+                "成员提前运动");
+        }
+
+        private static void VerifyCancelClearsPlan()
+        {
+            var coordinator = CreateStartedCoordinator(8);
+            coordinator.Cancel();
+
+            AssertState(
+                coordinator.State,
+                FleetPreparationCoordinatorState.Idle,
+                "取消准备任务");
+            if (coordinator.CurrentPlanId != 0 ||
+                coordinator.Targets.Count != 0)
+            {
+                throw new InvalidOperationException(
+                    "取消后没有清除准备任务数据。");
+            }
+        }
+
+        private static FleetPreparationCoordinator
+            CreateStartedCoordinator(long planId)
+        {
+            var coordinator =
+                new FleetPreparationCoordinator();
+            coordinator.StartRollingPreparation(
+                planId,
+                CreateTailToTailLayout(),
+                motionDirectionInFleetRadians: 0.0);
+            return coordinator;
+        }
+
+        private static FleetLayout CreateTailToTailLayout()
+        {
+            return new FleetLayout(
+                new[]
+                {
+                    new VehicleLayout(
+                        1,
+                        new Pose2D(-1.0, 0.0, 0.0)),
+                    new VehicleLayout(
+                        2,
+                        new Pose2D(1.0, 0.0, Math.PI))
+                });
+        }
+
+        private static FleetMemberPreparationStatus CreateStatus(
+            long planId,
+            int vehicleId,
+            FleetMemberAgentState state,
+            string failureReason = "")
+        {
+            return new FleetMemberPreparationStatus(
+                planId,
+                vehicleId,
+                state,
+                failureReason);
+        }
+
+        private static void AssertTargetDegrees(
+            FleetPreparationCoordinator coordinator,
+            int vehicleId,
+            double expectedDegrees)
+        {
+            if (!coordinator.TryGetTarget(
+                    vehicleId,
+                    out var target))
+            {
+                throw new InvalidOperationException(
+                    $"没有找到车辆{vehicleId}的准备目标。");
+            }
+
+            AssertNear(
+                AngleMath.RadiansToDegrees(
+                    target.MotionDirectionInBodyRadians),
+                expectedDegrees,
+                $"车辆{vehicleId}本地β");
+        }
+
+        private static void AssertState(
+            FleetPreparationCoordinatorState actual,
+            FleetPreparationCoordinatorState expected,
+            string scenario)
+        {
+            if (actual != expected)
+            {
+                throw new InvalidOperationException(
+                    $"{scenario}状态错误：" +
+                    $"actual={actual}, expected={expected}。");
+            }
+        }
+
+        private static void AssertNear(
+            double actual,
+            double expected,
+            string valueName)
+        {
+            if (Math.Abs(actual - expected) > Tolerance)
+            {
+                throw new InvalidOperationException(
+                    $"{valueName}错误：" +
+                    $"actual={actual:F9}, expected={expected:F9}。");
+            }
+        }
+    }
+}

MultiWheelC.Tests/FleetRuntimeTests.cs

@@ -0,0 +1,430 @@
+using System;
+using System.Numerics;
+using CommonUsage.Chassis;
+using MultiWheelC.Control.Abstractions;
+using MultiWheelC.Control.Allocation;
+using MultiWheelC.Fleet;
+using MultiWheelC.StateEstimation;
+using MultiWheelC.Trajectory;
+using MyParking.Shared;
+
+namespace MultiWheelC.Tests
+{
+    internal static class FleetRuntimeTests
+    {
+        private const long PlanId = 21;
+
+        public static void Run()
+        {
+            VerifyTwoVehiclePlanBecomesActive();
+            VerifyStaleCommandIsIgnored();
+            VerifyInvalidCommandLatchesMemberFault();
+            VerifyMissingReportFaultsLeader();
+            VerifyMemberWatchdogStopsLocally();
+            VerifyUnavailableMemberStateFaultsFleet();
+
+            Console.WriteLine(
+                "FleetRuntime端到端测试通过，共6个场景。");
+        }
+
+        private static void VerifyTwoVehiclePlanBecomesActive()
+        {
+            var fleet = CreateFleet();
+            StartAndActivate(fleet);
+
+            AssertState(
+                fleet.LeaderRuntime,
+                FleetRuntimeState.Active,
+                "主车正常激活");
+            AssertState(
+                fleet.MemberRuntime,
+                FleetRuntimeState.Active,
+                "从车正常激活");
+            AssertTrue(
+                fleet.LeaderRuntime.LastCoordinationOutput != null,
+                "主车激活后应产生首周期协调输出");
+            AssertTrue(
+                fleet.MemberRuntime.LastAppliedCommandSequence > 0,
+                "从车应确认已经执行主车命令");
+        }
+
+        private static void VerifyStaleCommandIsIgnored()
+        {
+            var fleet = CreateFleet();
+            StartAndActivate(fleet);
+            var appliedSequence =
+                fleet.MemberRuntime.LastAppliedCommandSequence;
+
+            fleet.LeaderTransport.SendCommand(
+                new FleetCommand(
+                    PlanId,
+                    appliedSequence,
+                    targetVehicleId: 2,
+                    FleetCommandKind.Motion,
+                    motionDirectionInBodyRadians: 0.0,
+                    twistInVehicleBody:
+                        new Twist2D(1.0, 0.0, 0.0),
+                    validForSeconds: 0.2));
+            fleet.MemberProvider.SetTimestamp(0.04);
+            fleet.MemberRuntime.Update(0.04, 0.02);
+
+            AssertState(
+                fleet.MemberRuntime,
+                FleetRuntimeState.Active,
+                "忽略旧序列命令");
+            AssertTrue(
+                fleet.MemberRuntime.LastAppliedCommandSequence ==
+                appliedSequence,
+                "旧序列命令不应更新最近执行序号");
+        }
+
+        private static void VerifyMissingReportFaultsLeader()
+        {
+            var fleet = CreateFleet();
+            AssertTrue(
+                fleet.LeaderRuntime.StartRollingPlan(
+                    PlanId,
+                    fleet.Layout,
+                    CreateTrajectory()),
+                "主车应成功启动测试任务");
+
+            fleet.LeaderRuntime.Update(0.0, 0.02);
+            fleet.LeaderProvider.SetTimestamp(0.25);
+            fleet.LeaderRuntime.Update(0.25, 0.02);
+
+            AssertState(
+                fleet.LeaderRuntime,
+                FleetRuntimeState.Faulted,
+                "成员报告超时");
+            AssertTrue(
+                fleet.LeaderRuntime.LastFailureReason.Contains(
+                    "未收到成员车2"),
+                "通信超时应指出缺失的成员车");
+            AssertTrue(
+                fleet.LeaderAgent.State ==
+                FleetMemberAgentState.Idle,
+                "主车故障后必须立即停止本车执行器");
+        }
+
+        private static void VerifyInvalidCommandLatchesMemberFault()
+        {
+            var fleet = CreateFleet();
+            StartAndActivate(fleet);
+
+            fleet.LeaderTransport.SendCommand(
+                new FleetCommand(
+                    PlanId,
+                    fleet.MemberRuntime.LastAppliedCommandSequence + 1,
+                    targetVehicleId: 2,
+                    FleetCommandKind.Motion,
+                    motionDirectionInBodyRadians: 0.0,
+                    twistInVehicleBody: Twist2D.Zero,
+                    validForSeconds: double.NaN));
+            fleet.MemberRuntime.Update(0.04, 0.02);
+
+            AssertState(
+                fleet.MemberRuntime,
+                FleetRuntimeState.Faulted,
+                "非法命令字段触发并锁存本地故障");
+        }
+
+        private static void VerifyMemberWatchdogStopsLocally()
+        {
+            var fleet = CreateFleet();
+            StartAndActivate(fleet);
+
+            fleet.MemberProvider.SetTimestamp(0.25);
+            fleet.MemberRuntime.Update(0.25, 0.02);
+
+            AssertState(
+                fleet.MemberRuntime,
+                FleetRuntimeState.Faulted,
+                "从车命令看门狗超时");
+            AssertTrue(
+                fleet.MemberRuntime.LastFailureReason.Contains(
+                    "超时"),
+                "从车看门狗应保留超时原因");
+        }
+
+        private static void VerifyUnavailableMemberStateFaultsFleet()
+        {
+            var fleet = CreateFleet();
+            StartAndActivate(fleet);
+
+            fleet.MemberProvider.IsAvailable = false;
+            fleet.MemberRuntime.Update(0.04, 0.02);
+            fleet.LeaderProvider.SetTimestamp(0.04);
+            fleet.LeaderRuntime.Update(0.04, 0.02);
+
+            AssertState(
+                fleet.MemberRuntime,
+                FleetRuntimeState.Faulted,
+                "从车状态不可用时本地停车");
+            AssertState(
+                fleet.LeaderRuntime,
+                FleetRuntimeState.Faulted,
+                "从车状态不可用时整队停车");
+        }
+
+        private static void StartAndActivate(TestFleet fleet)
+        {
+            AssertTrue(
+                fleet.LeaderRuntime.StartRollingPlan(
+                    PlanId,
+                    fleet.Layout,
+                    CreateTrajectory()),
+                "主车应成功启动测试任务");
+
+            fleet.MemberRuntime.Update(0.0, 0.02);
+            fleet.LeaderRuntime.Update(0.0, 0.02);
+            fleet.MemberProvider.SetTimestamp(0.02);
+            fleet.MemberRuntime.Update(0.02, 0.02);
+        }
+
+        private static TestFleet CreateFleet()
+        {
+            var layout = new FleetLayout(
+                new[]
+                {
+                    new VehicleLayout(
+                        1,
+                        new Pose2D(-0.5, 0.0, 0.0)),
+                    new VehicleLayout(
+                        2,
+                        new Pose2D(0.5, 0.0, 0.0))
+                });
+            var network = new InMemoryFleetTransportNetwork(
+                new[] { 1, 2 },
+                leaderVehicleId: 1);
+            var leaderTransport = network.CreateEndpoint(1);
+            var memberTransport = network.CreateEndpoint(2);
+            var leaderProvider = new MutableStateProvider(
+                new Pose2D(-0.5, 0.0, 0.0));
+            var memberProvider = new MutableStateProvider(
+                new Pose2D(0.5, 0.0, 0.0));
+            var leaderAgent = CreateAgent(1);
+            var memberAgent = CreateAgent(2);
+
+            var leaderRuntime = new FleetRuntime(
+                selfVehicleId: 1,
+                leaderVehicleId: 1,
+                leaderTransport,
+                leaderAgent,
+                leaderProvider,
+                new FleetPreparationCoordinator(),
+                CreateCoordinator(),
+                new FleetSafetySupervisor(
+                    communicationTimeoutSeconds: 0.2),
+                commandValidForSeconds: 0.2,
+                preparationTimeoutSeconds: 1.0);
+            var memberRuntime = new FleetRuntime(
+                selfVehicleId: 2,
+                leaderVehicleId: 1,
+                memberTransport,
+                memberAgent,
+                memberProvider,
+                commandValidForSeconds: 0.2);
+
+            return new TestFleet(
+                layout,
+                leaderTransport,
+                leaderProvider,
+                memberProvider,
+                leaderAgent,
+                leaderRuntime,
+                memberRuntime);
+        }
+
+        private static FleetCoordinator CreateCoordinator()
+        {
+            return new FleetCoordinator(
+                new FleetStateEstimator(
+                    maximumMemberStateAgeSeconds: 0.5,
+                    maximumPositionDisagreementMeters: 0.2,
+                    maximumYawDisagreementRadians:
+                        AngleMath.DegreesToRadians(5.0)),
+                new FleetController(
+                    new StraightLateralController(),
+                    new ZeroLongitudinalController(),
+                    new GcpCommandAllocator(Math.PI / 4.0),
+                    virtualControlPointRadiusMeters: 0.5),
+                new FleetMemberCommandCorrector(
+                    longitudinalPositionGainPerSecond: 1.0,
+                    lateralPositionGainPerSecond: 1.0,
+                    yawGainPerSecond: 1.0,
+                    positionErrorDeadbandMeters: 0.005,
+                    yawErrorDeadbandRadians:
+                        AngleMath.DegreesToRadians(0.5),
+                    maximumLinearCorrectionMetersPerSecond: 0.03,
+                    maximumAngularCorrectionRadiansPerSecond:
+                        AngleMath.DegreesToRadians(2.0)),
+                memberPositionErrorWarningMeters: 0.02,
+                maximumMemberPositionErrorMeters: 0.05,
+                memberYawErrorWarningRadians:
+                    AngleMath.DegreesToRadians(1.0),
+                maximumMemberYawErrorRadians:
+                    AngleMath.DegreesToRadians(3.0));
+        }
+
+        private static Trajectory2D CreateTrajectory()
+        {
+            return new Trajectory2D(
+                new[]
+                {
+                    new TrajectoryPoint(
+                        0.0,
+                        Pose2D.Identity,
+                        0.0,
+                        0.2),
+                    new TrajectoryPoint(
+                        1.0,
+                        new Pose2D(1.0, 0.0, 0.0),
+                        0.0,
+                        0.2)
+                });
+        }
+
+        private static FleetMemberAgent CreateAgent(int vehicleId)
+        {
+            var chassis = new MultiWheelChassis();
+            chassis.AddWheel(CreateWheel(-500f, 300f));
+            chassis.AddWheel(CreateWheel(-500f, -300f));
+            chassis.AddWheel(CreateWheel(500f, 300f));
+            chassis.AddWheel(CreateWheel(500f, -300f));
+            chassis.Initialize();
+
+            return new FleetMemberAgent(
+                new MultiWheelChassisAdapter(chassis, vehicleId),
+                alignmentToleranceRadians:
+                    AngleMath.DegreesToRadians(1.0),
+                alignmentStableSeconds: 0.0);
+        }
+
+        private static SteerWheel CreateWheel(float x, float y)
+        {
+            var speed = 0f;
+            var angle = 0f;
+            return new SteerWheel(
+                new Vector2(x, y),
+                angleLowerLimit: -120f,
+                angleUpperLimit: 120f,
+                speedWriter: value => speed = value,
+                speedReader: () => speed,
+                angleWriter: value => angle = value,
+                angleReader: () => angle);
+        }
+
+        private static void AssertState(
+            FleetRuntime runtime,
+            FleetRuntimeState expected,
+            string scenario)
+        {
+            AssertTrue(
+                runtime.State == expected,
+                $"{scenario}状态错误：" +
+                $"actual={runtime.State}, expected={expected}，" +
+                $"reason={runtime.LastFailureReason}");
+        }
+
+        private static void AssertTrue(bool condition, string message)
+        {
+            if (!condition)
+            {
+                throw new InvalidOperationException(message);
+            }
+        }
+
+        private sealed class MutableStateProvider :
+            IVehicleStateProvider
+        {
+            private readonly Pose2D _poseInWorld;
+            private double _timestampSeconds;
+
+            public MutableStateProvider(Pose2D poseInWorld)
+            {
+                _poseInWorld = poseInWorld;
+                IsAvailable = true;
+            }
+
+            public bool IsAvailable { get; set; }
+
+            public void SetTimestamp(double timestampSeconds)
+            {
+                _timestampSeconds = timestampSeconds;
+            }
+
+            public bool TryGetState(out VehicleState state)
+            {
+                state = new VehicleState(
+                    _timestampSeconds,
+                    _poseInWorld,
+                    Twist2D.Zero,
+                    hasValidVelocityEstimate: true);
+                return IsAvailable;
+            }
+        }
+
+        private sealed class StraightLateralController :
+            ILateralController
+        {
+            public LateralControlCommand Compute(
+                PathTrackingContext context)
+            {
+                return LateralControlCommand.Straight;
+            }
+
+            public void Reset()
+            {
+            }
+        }
+
+        private sealed class ZeroLongitudinalController :
+            ILongitudinalController
+        {
+            public double ComputeSpeedMetersPerSecond(
+                PathTrackingContext context)
+            {
+                return 0.0;
+            }
+
+            public void Reset()
+            {
+            }
+        }
+
+        private sealed class TestFleet
+        {
+            public TestFleet(
+                FleetLayout layout,
+                InMemoryFleetTransport leaderTransport,
+                MutableStateProvider leaderProvider,
+                MutableStateProvider memberProvider,
+                FleetMemberAgent leaderAgent,
+                FleetRuntime leaderRuntime,
+                FleetRuntime memberRuntime)
+            {
+                Layout = layout;
+                LeaderTransport = leaderTransport;
+                LeaderProvider = leaderProvider;
+                MemberProvider = memberProvider;
+                LeaderAgent = leaderAgent;
+                LeaderRuntime = leaderRuntime;
+                MemberRuntime = memberRuntime;
+            }
+
+            public FleetLayout Layout { get; }
+
+            public InMemoryFleetTransport LeaderTransport { get; }
+
+            public MutableStateProvider LeaderProvider { get; }
+
+            public MutableStateProvider MemberProvider { get; }
+
+            public FleetMemberAgent LeaderAgent { get; }
+
+            public FleetRuntime LeaderRuntime { get; }
+
+            public FleetRuntime MemberRuntime { get; }
+        }
+    }
+}

MultiWheelC.Tests/FleetSafetySupervisorTests.cs

@@ -0,0 +1,297 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.Fleet;
+using MyParking.Shared;
+
+namespace MultiWheelC.Tests
+{
+    internal static class FleetSafetySupervisorTests
+    {
+        private const long PlanId = 7;
+        private const double CurrentTimeSeconds = 10.0;
+        private const double CommunicationTimeoutSeconds = 0.5;
+
+        public static void Run()
+        {
+            VerifyHealthyFleetContinues();
+            VerifyMissingMemberStopsFleet();
+            VerifyCommunicationTimeoutStopsFleet();
+            VerifyUnavailableStateStopsFleet();
+            VerifyMemberFaultStopsFleet();
+            VerifyFailureCodeStopsFleet();
+            VerifyPlanMismatchStopsFleet();
+            VerifyStopIsLatchedUntilNewPlanStarts();
+
+            Console.WriteLine(
+                "FleetSafetySupervisor测试通过，共8个场景。");
+        }
+
+        private static void VerifyHealthyFleetContinues()
+        {
+            var supervisor = CreateStartedSupervisor();
+
+            var decision = supervisor.Evaluate(
+                CreateLayout(),
+                CreateHealthyStatuses(),
+                CurrentTimeSeconds);
+
+            AssertFalse(
+                decision.ShouldStop,
+                "成员状态健康时不应停车");
+        }
+
+        private static void VerifyMissingMemberStopsFleet()
+        {
+            var supervisor = CreateStartedSupervisor();
+
+            var decision = supervisor.Evaluate(
+                CreateLayout(),
+                new[] { CreateHealthyStatus(1) },
+                CurrentTimeSeconds);
+
+            AssertStopFromVehicle(
+                decision,
+                2,
+                "缺少成员报告");
+        }
+
+        private static void VerifyCommunicationTimeoutStopsFleet()
+        {
+            var supervisor = CreateStartedSupervisor();
+            var statuses = new[]
+            {
+                CreateHealthyStatus(1),
+                new FleetMemberSafetyStatus(
+                    vehicleId: 2,
+                    planId: PlanId,
+                    isStateAvailable: true,
+                    isFaulted: false,
+                    failureCode: 0,
+                    lastAcceptedReportTimeSeconds: 9.4)
+            };
+
+            var decision = supervisor.Evaluate(
+                CreateLayout(),
+                statuses,
+                CurrentTimeSeconds);
+
+            AssertStopFromVehicle(
+                decision,
+                2,
+                "通信超时");
+        }
+
+        private static void VerifyUnavailableStateStopsFleet()
+        {
+            var supervisor = CreateStartedSupervisor();
+            var statuses = new[]
+            {
+                CreateHealthyStatus(1),
+                new FleetMemberSafetyStatus(
+                    vehicleId: 2,
+                    planId: PlanId,
+                    isStateAvailable: false,
+                    isFaulted: false,
+                    failureCode: 0,
+                    lastAcceptedReportTimeSeconds: 9.9)
+            };
+
+            var decision = supervisor.Evaluate(
+                CreateLayout(),
+                statuses,
+                CurrentTimeSeconds);
+
+            AssertStopFromVehicle(
+                decision,
+                2,
+                "状态不可用");
+        }
+
+        private static void VerifyMemberFaultStopsFleet()
+        {
+            var supervisor = CreateStartedSupervisor();
+            var statuses = new[]
+            {
+                CreateHealthyStatus(1),
+                new FleetMemberSafetyStatus(
+                    vehicleId: 2,
+                    planId: PlanId,
+                    isStateAvailable: true,
+                    isFaulted: true,
+                    failureCode: 0,
+                    lastAcceptedReportTimeSeconds: 9.9)
+            };
+
+            var decision = supervisor.Evaluate(
+                CreateLayout(),
+                statuses,
+                CurrentTimeSeconds);
+
+            AssertStopFromVehicle(
+                decision,
+                2,
+                "成员故障状态");
+        }
+
+        private static void VerifyFailureCodeStopsFleet()
+        {
+            var supervisor = CreateStartedSupervisor();
+            var statuses = new[]
+            {
+                CreateHealthyStatus(1),
+                new FleetMemberSafetyStatus(
+                    vehicleId: 2,
+                    planId: PlanId,
+                    isStateAvailable: true,
+                    isFaulted: false,
+                    failureCode: 42,
+                    lastAcceptedReportTimeSeconds: 9.9)
+            };
+
+            var decision = supervisor.Evaluate(
+                CreateLayout(),
+                statuses,
+                CurrentTimeSeconds);
+
+            AssertStopFromVehicle(
+                decision,
+                2,
+                "成员故障码");
+        }
+
+        private static void VerifyPlanMismatchStopsFleet()
+        {
+            var supervisor = CreateStartedSupervisor();
+            var statuses = new[]
+            {
+                CreateHealthyStatus(1),
+                new FleetMemberSafetyStatus(
+                    vehicleId: 2,
+                    planId: PlanId - 1,
+                    isStateAvailable: true,
+                    isFaulted: false,
+                    failureCode: 0,
+                    lastAcceptedReportTimeSeconds: 9.9)
+            };
+
+            var decision = supervisor.Evaluate(
+                CreateLayout(),
+                statuses,
+                CurrentTimeSeconds);
+
+            AssertStopFromVehicle(
+                decision,
+                2,
+                "任务编号不一致");
+        }
+
+        private static void VerifyStopIsLatchedUntilNewPlanStarts()
+        {
+            var supervisor = CreateStartedSupervisor();
+            supervisor.Evaluate(
+                CreateLayout(),
+                new[] { CreateHealthyStatus(1) },
+                CurrentTimeSeconds);
+
+            var latchedDecision = supervisor.Evaluate(
+                CreateLayout(),
+                CreateHealthyStatuses(),
+                CurrentTimeSeconds);
+            AssertTrue(
+                latchedDecision.ShouldStop,
+                "故障恢复后停车决定仍应锁存");
+
+            supervisor.Start(PlanId + 1);
+            var recoveredDecision = supervisor.Evaluate(
+                CreateLayout(),
+                new[]
+                {
+                    CreateHealthyStatus(1, PlanId + 1),
+                    CreateHealthyStatus(2, PlanId + 1)
+                },
+                CurrentTimeSeconds);
+            AssertFalse(
+                recoveredDecision.ShouldStop,
+                "开始新任务后应清除旧任务停车锁存");
+        }
+
+        private static FleetSafetySupervisor
+            CreateStartedSupervisor()
+        {
+            var supervisor = new FleetSafetySupervisor(
+                CommunicationTimeoutSeconds);
+            supervisor.Start(PlanId);
+            return supervisor;
+        }
+
+        private static FleetLayout CreateLayout()
+        {
+            return new FleetLayout(
+                new[]
+                {
+                    new VehicleLayout(
+                        1,
+                        new Pose2D(-1.0, 0.0, 0.0)),
+                    new VehicleLayout(
+                        2,
+                        new Pose2D(1.0, 0.0, Math.PI))
+                });
+        }
+
+        private static IReadOnlyList<FleetMemberSafetyStatus>
+            CreateHealthyStatuses()
+        {
+            return new[]
+            {
+                CreateHealthyStatus(1),
+                CreateHealthyStatus(2)
+            };
+        }
+
+        private static FleetMemberSafetyStatus CreateHealthyStatus(
+            int vehicleId,
+            long planId = PlanId)
+        {
+            return new FleetMemberSafetyStatus(
+                vehicleId,
+                planId,
+                isStateAvailable: true,
+                isFaulted: false,
+                failureCode: 0,
+                lastAcceptedReportTimeSeconds: 9.9);
+        }
+
+        private static void AssertStopFromVehicle(
+            FleetSafetyDecision decision,
+            int expectedVehicleId,
+            string scenario)
+        {
+            AssertTrue(
+                decision.ShouldStop,
+                $"{scenario}时应停车");
+            AssertTrue(
+                decision.SourceVehicleId == expectedVehicleId,
+                $"{scenario}的来源车辆不正确");
+            AssertTrue(
+                !string.IsNullOrWhiteSpace(decision.Reason),
+                $"{scenario}应提供停车原因");
+        }
+
+        private static void AssertTrue(
+            bool condition,
+            string message)
+        {
+            if (!condition)
+            {
+                throw new InvalidOperationException(message);
+            }
+        }
+
+        private static void AssertFalse(
+            bool condition,
+            string message)
+        {
+            AssertTrue(!condition, message);
+        }
+    }
+}

MultiWheelC.Tests/FleetStateEstimatorTests.cs

@@ -0,0 +1,430 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.Fleet;
+using MyParking.Shared;
+
+namespace MultiWheelC.Tests
+{
+    internal static class FleetStateEstimatorTests
+    {
+        private const double Tolerance = 1e-9;
+
+        public static void Run()
+        {
+            VerifyRigidStateIsRecovered();
+            VerifyOlderSamplesAreAligned();
+            VerifyYawWrapAroundIsAveraged();
+            VerifySmallLayoutErrorIsReported();
+            VerifyInconsistentCentersAreRejected();
+            VerifyMissingMemberIsRejected();
+            VerifyInvalidVelocityRemainsExplicit();
+
+            Console.WriteLine(
+                "FleetStateEstimator车队状态估计测试通过。共7个场景。");
+        }
+
+        private static void VerifyRigidStateIsRecovered()
+        {
+            var layout = CreateLayout();
+            var fleetPose = new Pose2D(4.0, -2.0, 0.4);
+            var fleetTwist = new Twist2D(0.3, -0.1, 0.2);
+            var result = CreateEstimator().Estimate(
+                layout,
+                CreateRigidMemberStates(
+                    layout,
+                    fleetPose,
+                    fleetTwist,
+                    sampleTimestampSeconds: 5.0,
+                    hasValidVelocityEstimate: true),
+                targetTimestampSeconds: 5.0);
+
+            var state = RequireState(result, "刚体状态还原");
+            AssertPose(
+                state.FleetPoseInWorld,
+                fleetPose,
+                "刚体状态还原");
+            AssertTwist(
+                state.TwistAtFleetOriginInWorld,
+                fleetTwist,
+                "刚体速度还原");
+
+            for (var index = 0;
+                 index < result.MemberErrors.Count;
+                 index++)
+            {
+                AssertPose(
+                    result.MemberErrors[index]
+                        .ActualPoseInExpectedVehicleFrame,
+                    Pose2D.Identity,
+                    "刚体布局误差");
+            }
+        }
+
+        private static void VerifyOlderSamplesAreAligned()
+        {
+            var layout = CreateLayout();
+            var sampleFleetPose =
+                new Pose2D(1.0, 2.0, 0.3);
+            var fleetTwist =
+                new Twist2D(0.4, -0.2, 0.0);
+            var result = CreateEstimator().Estimate(
+                layout,
+                CreateRigidMemberStates(
+                    layout,
+                    sampleFleetPose,
+                    fleetTwist,
+                    sampleTimestampSeconds: 0.9,
+                    hasValidVelocityEstimate: true),
+                targetTimestampSeconds: 1.0);
+
+            var state = RequireState(result, "成员时间对齐");
+            AssertPose(
+                state.FleetPoseInWorld,
+                new Pose2D(1.04, 1.98, 0.3),
+                "成员时间对齐");
+            AssertTwist(
+                state.TwistAtFleetOriginInWorld,
+                fleetTwist,
+                "时间对齐后速度");
+        }
+
+        private static void VerifySmallLayoutErrorIsReported()
+        {
+            var layout = CreateLayout();
+            var result = CreateEstimator().Estimate(
+                layout,
+                new[]
+                {
+                    CreateMemberState(
+                        1,
+                        new Pose2D(-0.98, 0.0, 0.0),
+                        Twist2D.Zero,
+                        1.0,
+                        true),
+                    CreateMemberState(
+                        2,
+                        new Pose2D(0.99, 0.0, Math.PI),
+                        Twist2D.Zero,
+                        1.0,
+                        true)
+                },
+                targetTimestampSeconds: 1.0);
+
+            var state = RequireState(result, "小范围布局误差");
+            AssertNear(
+                state.FleetPoseInWorld.XMeters,
+                0.005,
+                "小范围布局误差中心X");
+            AssertNear(
+                FindError(result.MemberErrors, 1)
+                    .ActualPoseInExpectedVehicleFrame.XMeters,
+                0.015,
+                "车辆1布局误差X");
+            AssertNear(
+                FindError(result.MemberErrors, 2)
+                    .ActualPoseInExpectedVehicleFrame.XMeters,
+                0.015,
+                "车辆2布局误差X");
+        }
+
+        private static void VerifyYawWrapAroundIsAveraged()
+        {
+            var layout = CreateLayout();
+            var firstCandidate = new Pose2D(
+                0.0,
+                0.0,
+                AngleMath.DegreesToRadians(179.0));
+            var secondCandidate = new Pose2D(
+                0.0,
+                0.0,
+                AngleMath.DegreesToRadians(-179.0));
+            var result = CreateEstimator().Estimate(
+                layout,
+                new[]
+                {
+                    CreateMemberState(
+                        1,
+                        FrameTransform2D.Compose(
+                            firstCandidate,
+                            layout.Vehicles[0].PoseInFleet),
+                        Twist2D.Zero,
+                        1.0,
+                        true),
+                    CreateMemberState(
+                        2,
+                        FrameTransform2D.Compose(
+                            secondCandidate,
+                            layout.Vehicles[1].PoseInFleet),
+                        Twist2D.Zero,
+                        1.0,
+                        true)
+                },
+                targetTimestampSeconds: 1.0);
+
+            var state = RequireState(result, "跨正负π航向平均");
+            AssertNear(
+                Math.Abs(state.FleetPoseInWorld.YawRadians),
+                Math.PI,
+                "跨正负π航向平均");
+        }
+
+        private static void VerifyInconsistentCentersAreRejected()
+        {
+            var result = CreateEstimator().Estimate(
+                CreateLayout(),
+                new[]
+                {
+                    CreateMemberState(
+                        1,
+                        new Pose2D(-1.0, 0.0, 0.0),
+                        Twist2D.Zero,
+                        1.0,
+                        true),
+                    CreateMemberState(
+                        2,
+                        new Pose2D(1.3, 0.0, Math.PI),
+                        Twist2D.Zero,
+                        1.0,
+                        true)
+                },
+                targetTimestampSeconds: 1.0);
+
+            AssertUnavailable(result, "候选中心冲突");
+        }
+
+        private static void VerifyMissingMemberIsRejected()
+        {
+            var result = CreateEstimator().Estimate(
+                CreateLayout(),
+                new[]
+                {
+                    CreateMemberState(
+                        1,
+                        new Pose2D(-1.0, 0.0, 0.0),
+                        Twist2D.Zero,
+                        1.0,
+                        true)
+                },
+                targetTimestampSeconds: 1.0);
+
+            AssertUnavailable(result, "成员缺失");
+        }
+
+        private static void VerifyInvalidVelocityRemainsExplicit()
+        {
+            var layout = CreateLayout();
+            var result = CreateEstimator().Estimate(
+                layout,
+                CreateRigidMemberStates(
+                    layout,
+                    Pose2D.Identity,
+                    new Twist2D(0.4, 0.0, 0.0),
+                    sampleTimestampSeconds: 1.0,
+                    hasValidVelocityEstimate: false),
+                targetTimestampSeconds: 1.0);
+
+            var state = RequireState(result, "速度未初始化");
+            if (state.HasValidVelocityEstimate)
+            {
+                throw new InvalidOperationException(
+                    "成员速度无效时车队速度不应标记为有效。");
+            }
+
+            AssertTwist(
+                state.TwistAtFleetOriginInWorld,
+                Twist2D.Zero,
+                "速度未初始化");
+        }
+
+        private static FleetStateEstimator CreateEstimator()
+        {
+            return new FleetStateEstimator(
+                maximumMemberStateAgeSeconds: 0.25,
+                maximumPositionDisagreementMeters: 0.1,
+                maximumYawDisagreementRadians:
+                    AngleMath.DegreesToRadians(5.0));
+        }
+
+        private static FleetLayout CreateLayout()
+        {
+            return new FleetLayout(
+                new[]
+                {
+                    new VehicleLayout(
+                        1,
+                        new Pose2D(-1.0, 0.0, 0.0)),
+                    new VehicleLayout(
+                        2,
+                        new Pose2D(1.0, 0.0, Math.PI))
+                });
+        }
+
+        private static FleetMemberStateSample[]
+            CreateRigidMemberStates(
+                FleetLayout layout,
+                Pose2D fleetPoseInWorld,
+                Twist2D twistAtFleetOriginInWorld,
+                double sampleTimestampSeconds,
+                bool hasValidVelocityEstimate)
+        {
+            var states =
+                new FleetMemberStateSample[layout.VehicleCount];
+
+            for (var index = 0;
+                 index < layout.Vehicles.Count;
+                 index++)
+            {
+                var vehicle = layout.Vehicles[index];
+                var memberPoseInWorld =
+                    FrameTransform2D.Compose(
+                        fleetPoseInWorld,
+                        vehicle.PoseInFleet);
+                var xFromFleetOrigin =
+                    memberPoseInWorld.XMeters -
+                    fleetPoseInWorld.XMeters;
+                var yFromFleetOrigin =
+                    memberPoseInWorld.YMeters -
+                    fleetPoseInWorld.YMeters;
+                var memberTwistInWorld = new Twist2D(
+                    twistAtFleetOriginInWorld
+                        .VxMetersPerSecond -
+                    twistAtFleetOriginInWorld
+                        .OmegaRadiansPerSecond *
+                    yFromFleetOrigin,
+                    twistAtFleetOriginInWorld
+                        .VyMetersPerSecond +
+                    twistAtFleetOriginInWorld
+                        .OmegaRadiansPerSecond *
+                    xFromFleetOrigin,
+                    twistAtFleetOriginInWorld
+                        .OmegaRadiansPerSecond);
+
+                states[index] = new FleetMemberStateSample(
+                    vehicle.VehicleId,
+                    sampleTimestampSeconds,
+                    memberPoseInWorld,
+                    memberTwistInWorld,
+                    isStateAvailable: true,
+                    hasValidVelocityEstimate:
+                        hasValidVelocityEstimate);
+            }
+
+            return states;
+        }
+
+        private static FleetMemberStateSample CreateMemberState(
+            int vehicleId,
+            Pose2D poseInWorld,
+            Twist2D twistInWorld,
+            double timestampSeconds,
+            bool hasValidVelocityEstimate)
+        {
+            return new FleetMemberStateSample(
+                vehicleId,
+                timestampSeconds,
+                poseInWorld,
+                twistInWorld,
+                isStateAvailable: true,
+                hasValidVelocityEstimate:
+                    hasValidVelocityEstimate);
+        }
+
+        private static FleetState RequireState(
+            FleetStateEstimateResult result,
+            string scenario)
+        {
+            if (!result.IsAvailable || !result.State.HasValue)
+            {
+                throw new InvalidOperationException(
+                    $"{scenario}应产生可用状态：" +
+                    result.UnavailableReason);
+            }
+
+            return result.State.Value;
+        }
+
+        private static void AssertUnavailable(
+            FleetStateEstimateResult result,
+            string scenario)
+        {
+            if (result.IsAvailable ||
+                string.IsNullOrWhiteSpace(
+                    result.UnavailableReason))
+            {
+                throw new InvalidOperationException(
+                    $"{scenario}应返回带原因的不可用结果。");
+            }
+        }
+
+        private static FleetMemberLayoutError FindError(
+            IReadOnlyList<FleetMemberLayoutError> errors,
+            int vehicleId)
+        {
+            for (var index = 0;
+                 index < errors.Count;
+                 index++)
+            {
+                if (errors[index].VehicleId == vehicleId)
+                {
+                    return errors[index];
+                }
+            }
+
+            throw new InvalidOperationException(
+                $"没有找到车辆{vehicleId}的布局误差。");
+        }
+
+        private static void AssertPose(
+            Pose2D actual,
+            Pose2D expected,
+            string scenario)
+        {
+            AssertNear(
+                actual.XMeters,
+                expected.XMeters,
+                scenario + " X");
+            AssertNear(
+                actual.YMeters,
+                expected.YMeters,
+                scenario + " Y");
+            AssertNear(
+                AngleMath.ShortestDifferenceRadians(
+                    actual.YawRadians,
+                    expected.YawRadians),
+                0.0,
+                scenario + " Yaw");
+        }
+
+        private static void AssertTwist(
+            Twist2D actual,
+            Twist2D expected,
+            string scenario)
+        {
+            AssertNear(
+                actual.VxMetersPerSecond,
+                expected.VxMetersPerSecond,
+                scenario + " Vx");
+            AssertNear(
+                actual.VyMetersPerSecond,
+                expected.VyMetersPerSecond,
+                scenario + " Vy");
+            AssertNear(
+                actual.OmegaRadiansPerSecond,
+                expected.OmegaRadiansPerSecond,
+                scenario + " Omega");
+        }
+
+        private static void AssertNear(
+            double actual,
+            double expected,
+            string valueName)
+        {
+            if (Math.Abs(actual - expected) > Tolerance)
+            {
+                throw new InvalidOperationException(
+                    $"{valueName}错误：" +
+                    $"actual={actual:F9}, expected={expected:F9}。");
+            }
+        }
+    }
+}

MultiWheelC.Tests/InMemoryFleetTransport.cs

@@ -0,0 +1,240 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.Fleet;
+using MyParking.Shared;
+
+namespace MultiWheelC.Tests
+{
+    // 为同一测试进程中的各车辆端点提供共享FIFO消息队列。
+    internal sealed class InMemoryFleetTransportNetwork
+    {
+        private readonly SharedState _sharedState;
+        private readonly HashSet<int> _createdEndpointIds =
+            new HashSet<int>();
+
+        public InMemoryFleetTransportNetwork(
+            IReadOnlyList<int> vehicleIds,
+            int leaderVehicleId)
+        {
+            _sharedState = new SharedState(
+                vehicleIds,
+                leaderVehicleId);
+        }
+
+        public InMemoryFleetTransport CreateEndpoint(
+            int vehicleId)
+        {
+            lock (_sharedState.SyncRoot)
+            {
+                if (!_sharedState.CommandQueues.ContainsKey(
+                        vehicleId))
+                {
+                    throw new ArgumentOutOfRangeException(
+                        nameof(vehicleId),
+                        $"车辆{vehicleId}不属于当前内存车队网络。");
+                }
+
+                if (!_createdEndpointIds.Add(vehicleId))
+                {
+                    throw new InvalidOperationException(
+                        $"车辆{vehicleId}的内存通信端点已经创建。");
+                }
+            }
+
+            return new InMemoryFleetTransport(
+                _sharedState,
+                vehicleId);
+        }
+
+        internal sealed class SharedState
+        {
+            public SharedState(
+                IReadOnlyList<int> vehicleIds,
+                int leaderVehicleId)
+            {
+                if (vehicleIds == null)
+                {
+                    throw new ArgumentNullException(
+                        nameof(vehicleIds));
+                }
+
+                if (vehicleIds.Count == 0)
+                {
+                    throw new ArgumentException(
+                        "内存车队网络至少需要一辆车。",
+                        nameof(vehicleIds));
+                }
+
+                if (leaderVehicleId <= 0)
+                {
+                    throw new ArgumentOutOfRangeException(
+                        nameof(leaderVehicleId),
+                        "主车编号必须大于零。");
+                }
+
+                CommandQueues =
+                    new Dictionary<int, Queue<FleetCommand>>();
+
+                for (var index = 0;
+                     index < vehicleIds.Count;
+                     index++)
+                {
+                    var vehicleId = vehicleIds[index];
+                    if (vehicleId <= 0)
+                    {
+                        throw new ArgumentOutOfRangeException(
+                            nameof(vehicleIds),
+                            $"第{index}辆车的编号必须大于零。");
+                    }
+
+                    if (CommandQueues.ContainsKey(vehicleId))
+                    {
+                        throw new ArgumentException(
+                            $"内存车队网络包含重复车号{vehicleId}。",
+                            nameof(vehicleIds));
+                    }
+
+                    CommandQueues.Add(
+                        vehicleId,
+                        new Queue<FleetCommand>());
+                }
+
+                if (!CommandQueues.ContainsKey(
+                        leaderVehicleId))
+                {
+                    throw new ArgumentException(
+                        $"主车{leaderVehicleId}不在车辆列表中。",
+                        nameof(leaderVehicleId));
+                }
+
+                LeaderVehicleId = leaderVehicleId;
+            }
+
+            public object SyncRoot { get; } = new object();
+
+            public int LeaderVehicleId { get; }
+
+            public Dictionary<int, Queue<FleetCommand>>
+                CommandQueues { get; }
+
+            public Queue<FleetMemberReport> ReportQueue { get; } =
+                new Queue<FleetMemberReport>();
+        }
+    }
+
+    // 单辆模拟车辆持有的通信端点；只负责消息路由，不解释控制语义。
+    internal sealed class InMemoryFleetTransport : IFleetTransport
+    {
+        private readonly InMemoryFleetTransportNetwork.SharedState
+            _sharedState;
+        private readonly int _localVehicleId;
+
+        internal InMemoryFleetTransport(
+            InMemoryFleetTransportNetwork.SharedState sharedState,
+            int localVehicleId)
+        {
+            _sharedState = sharedState ??
+                throw new ArgumentNullException(
+                    nameof(sharedState));
+            _localVehicleId = localVehicleId;
+        }
+
+        public void SendCommand(FleetCommand command)
+        {
+            if (_localVehicleId !=
+                _sharedState.LeaderVehicleId)
+            {
+                throw new InvalidOperationException(
+                    "只有主车通信端点可以发送车队命令。");
+            }
+
+            lock (_sharedState.SyncRoot)
+            {
+                if (command.TargetVehicleId ==
+                    FleetProtocol.BroadcastVehicleId)
+                {
+                    foreach (var pair in
+                             _sharedState.CommandQueues)
+                    {
+                        // 主车本地命令由运行入口直接执行，不通过通信回环。
+                        if (pair.Key != _localVehicleId)
+                        {
+                            pair.Value.Enqueue(command);
+                        }
+                    }
+
+                    return;
+                }
+
+                if (!_sharedState.CommandQueues.TryGetValue(
+                        command.TargetVehicleId,
+                        out var queue))
+                {
+                    throw new ArgumentOutOfRangeException(
+                        nameof(command),
+                        $"目标车辆{command.TargetVehicleId}不存在。");
+                }
+
+                queue.Enqueue(command);
+            }
+        }
+
+        public void SendReport(FleetMemberReport report)
+        {
+            if (report.VehicleId != _localVehicleId)
+            {
+                throw new ArgumentException(
+                    $"车辆{_localVehicleId}不能发送属于车辆" +
+                    $"{report.VehicleId}的状态报告。",
+                    nameof(report));
+            }
+
+            lock (_sharedState.SyncRoot)
+            {
+                _sharedState.ReportQueue.Enqueue(report);
+            }
+        }
+
+        public bool TryReceiveCommand(
+            out FleetCommand command)
+        {
+            lock (_sharedState.SyncRoot)
+            {
+                var queue =
+                    _sharedState.CommandQueues[_localVehicleId];
+                if (queue.Count == 0)
+                {
+                    command = default;
+                    return false;
+                }
+
+                command = queue.Dequeue();
+                return true;
+            }
+        }
+
+        public bool TryReceiveReport(
+            out FleetMemberReport report)
+        {
+            if (_localVehicleId !=
+                _sharedState.LeaderVehicleId)
+            {
+                throw new InvalidOperationException(
+                    "只有主车通信端点可以接收成员状态报告。");
+            }
+
+            lock (_sharedState.SyncRoot)
+            {
+                if (_sharedState.ReportQueue.Count == 0)
+                {
+                    report = default;
+                    return false;
+                }
+
+                report =
+                    _sharedState.ReportQueue.Dequeue();
+                return true;
+            }
+        }
+    }
+}

MultiWheelC.Tests/InMemoryFleetTransportTests.cs

@@ -0,0 +1,198 @@
+using System;
+using MultiWheelC.Fleet;
+using MyParking.Shared;
+
+namespace MultiWheelC.Tests
+{
+    internal static class InMemoryFleetTransportTests
+    {
+        public static void Run()
+        {
+            VerifyTargetedCommandRoutingAndFifoOrder();
+            VerifyBroadcastReachesAllFollowersOnly();
+            VerifyMemberReportReturnsToLeader();
+            VerifyEndpointRolesAndVehicleIdentity();
+
+            Console.WriteLine(
+                "InMemoryFleetTransport测试通过，共4个场景。");
+        }
+
+        private static void VerifyTargetedCommandRoutingAndFifoOrder()
+        {
+            var network = CreateNetwork();
+            var leader = network.CreateEndpoint(1);
+            var member2 = network.CreateEndpoint(2);
+            var member3 = network.CreateEndpoint(3);
+
+            leader.SendCommand(CreateCommand(2, sequenceNumber: 1));
+            leader.SendCommand(CreateCommand(2, sequenceNumber: 2));
+
+            AssertTrue(
+                member2.TryReceiveCommand(out var first) &&
+                first.SequenceNumber == 1,
+                "定向命令第一条应到达目标车辆");
+            AssertTrue(
+                member2.TryReceiveCommand(out var second) &&
+                second.SequenceNumber == 2,
+                "定向命令应保持FIFO顺序");
+            AssertFalse(
+                member2.TryReceiveCommand(out _),
+                "目标车辆不应收到额外命令");
+            AssertFalse(
+                member3.TryReceiveCommand(out _),
+                "其他成员不应收到定向命令");
+        }
+
+        private static void VerifyBroadcastReachesAllFollowersOnly()
+        {
+            var network = CreateNetwork();
+            var leader = network.CreateEndpoint(1);
+            var member2 = network.CreateEndpoint(2);
+            var member3 = network.CreateEndpoint(3);
+
+            leader.SendCommand(
+                CreateCommand(
+                    FleetProtocol.BroadcastVehicleId,
+                    sequenceNumber: 3));
+
+            AssertTrue(
+                member2.TryReceiveCommand(out var command2) &&
+                command2.SequenceNumber == 3,
+                "广播命令应到达成员车2");
+            AssertTrue(
+                member3.TryReceiveCommand(out var command3) &&
+                command3.SequenceNumber == 3,
+                "广播命令应到达成员车3");
+            AssertFalse(
+                leader.TryReceiveCommand(out _),
+                "主车本地命令不应通过通信层回环");
+        }
+
+        private static void VerifyMemberReportReturnsToLeader()
+        {
+            var network = CreateNetwork();
+            var leader = network.CreateEndpoint(1);
+            var member2 = network.CreateEndpoint(2);
+
+            member2.SendReport(
+                CreateReport(
+                    vehicleId: 2,
+                    sequenceNumber: 8));
+
+            AssertTrue(
+                leader.TryReceiveReport(out var report),
+                "主车应收到成员报告");
+            AssertTrue(
+                report.VehicleId == 2 &&
+                report.SequenceNumber == 8,
+                "主车收到的成员报告内容不正确");
+            AssertFalse(
+                leader.TryReceiveReport(out _),
+                "报告队列取空后应返回false");
+        }
+
+        private static void VerifyEndpointRolesAndVehicleIdentity()
+        {
+            var network = CreateNetwork();
+            var leader = network.CreateEndpoint(1);
+            var member2 = network.CreateEndpoint(2);
+
+            AssertThrows<InvalidOperationException>(
+                () => member2.SendCommand(
+                    CreateCommand(1, sequenceNumber: 1)),
+                "从车不能发送车队命令");
+            AssertThrows<InvalidOperationException>(
+                () => member2.TryReceiveReport(out _),
+                "从车不能消费全队成员报告");
+            AssertThrows<ArgumentException>(
+                () => member2.SendReport(
+                    CreateReport(
+                        vehicleId: 1,
+                        sequenceNumber: 1)),
+                "端点不能冒用其他车辆身份");
+
+            leader.SendReport(
+                CreateReport(
+                    vehicleId: 1,
+                    sequenceNumber: 2));
+            AssertTrue(
+                leader.TryReceiveReport(out var leaderReport) &&
+                leaderReport.VehicleId == 1,
+                "主车作为成员时也应能上报本车状态");
+        }
+
+        private static InMemoryFleetTransportNetwork CreateNetwork()
+        {
+            return new InMemoryFleetTransportNetwork(
+                new[] { 1, 2, 3 },
+                leaderVehicleId: 1);
+        }
+
+        private static FleetCommand CreateCommand(
+            int targetVehicleId,
+            long sequenceNumber)
+        {
+            return new FleetCommand(
+                planId: 5,
+                sequenceNumber,
+                targetVehicleId,
+                FleetCommandKind.Stop,
+                motionDirectionInBodyRadians: 0.0,
+                twistInVehicleBody: Twist2D.Zero,
+                validForSeconds: 0.5);
+        }
+
+        private static FleetMemberReport CreateReport(
+            int vehicleId,
+            long sequenceNumber)
+        {
+            return new FleetMemberReport(
+                vehicleId,
+                planId: 5,
+                sequenceNumber,
+                sampleTimestampSeconds: 1.0,
+                poseInCommonWorld: Pose2D.Identity,
+                twistAtVehicleOriginInCommonWorld:
+                    Twist2D.Zero,
+                isStateAvailable: true,
+                hasValidVelocityEstimate: true,
+                FleetMemberState.Active,
+                lastAppliedCommandSequence: 1);
+        }
+
+        private static void AssertThrows<TException>(
+            Action action,
+            string scenario)
+            where TException : Exception
+        {
+            try
+            {
+                action();
+            }
+            catch (TException)
+            {
+                return;
+            }
+
+            throw new InvalidOperationException(
+                $"{scenario}时应抛出{typeof(TException).Name}。");
+        }
+
+        private static void AssertTrue(
+            bool condition,
+            string message)
+        {
+            if (!condition)
+            {
+                throw new InvalidOperationException(message);
+            }
+        }
+
+        private static void AssertFalse(
+            bool condition,
+            string message)
+        {
+            AssertTrue(!condition, message);
+        }
+    }
+}

MultiWheelC.Tests/MultiWheelC.Tests.csproj

@@ -0,0 +1,20 @@
+<Project Sdk="Microsoft.NET.Sdk">
+
+  <PropertyGroup>
+    <OutputType>Exe</OutputType>
+    <TargetFramework>net8.0</TargetFramework>
+    <LangVersion>10</LangVersion>
+    <IsPackable>false</IsPackable>
+  </PropertyGroup>
+
+  <ItemGroup>
+    <ProjectReference Include="..\MultiWheelC\MultiWheelC.csproj" />
+  </ItemGroup>
+
+  <ItemGroup>
+    <Reference Include="CommonUsage">
+      <HintPath>..\ref\CommonUsage.dll</HintPath>
+    </Reference>
+  </ItemGroup>
+
+</Project>

MultiWheelC.Tests/Program.cs

@@ -0,0 +1,209 @@
+using System;
+using MultiWheelC.Control.Abstractions;
+using MultiWheelC.Control.Lateral;
+using MultiWheelC.Trajectory;
+using MyParking.Shared;
+
+namespace MultiWheelC.Tests
+{
+    /// <summary>
+    /// 验证Stanley控制器在前进和倒车时的横向误差符号及差动转角方向。
+    /// </summary>
+    internal static class Program
+    {
+        private const double SpeedMagnitudeMetersPerSecond = 0.4;
+        private const double TestLateralErrorMeters = 0.1;
+        private const double TestHeadingErrorRadians = 0.1;
+        private const double TestCurvaturePerMeter = 0.2;
+
+        /// <summary>
+        /// 运行不依赖宿主、Detour或实车底盘的控制器数学测试。
+        /// </summary>
+        private static void Main()
+        {
+            foreach (var travelDirection in new[] { 1.0, -1.0 })
+            {
+                VerifyCrossTrackConvergence(
+                    travelDirection,
+                    TestLateralErrorMeters);
+                VerifyCrossTrackConvergence(
+                    travelDirection,
+                    -TestLateralErrorMeters);
+                VerifyHeadingDirection(travelDirection);
+                VerifyCurvatureDirection(travelDirection);
+            }
+
+            Console.WriteLine(
+                "Stanley前进/倒车横向符号测试通过。共8个场景。");
+            FleetLayoutCaptureTests.Run();
+            FleetStateEstimatorTests.Run();
+            FleetKinematicsTests.Run();
+            FleetControllerTests.Run();
+            FleetMemberCommandCorrectorTests.Run();
+            FleetCoordinatorTests.Run();
+            FleetPreparationCoordinatorTests.Run();
+            FleetMemberAgentTests.Run();
+            FleetSafetySupervisorTests.Run();
+            InMemoryFleetTransportTests.Run();
+            FleetRuntimeTests.Run();
+        }
+
+        /// <summary>
+        /// 验证共同转角产生的横向速度始终使轨迹点序横向误差绝对值减小。
+        /// </summary>
+        private static void VerifyCrossTrackConvergence(
+            double travelDirection,
+            double lateralErrorMeters)
+        {
+            var signedSpeedMetersPerSecond =
+                travelDirection *
+                SpeedMagnitudeMetersPerSecond;
+            var controller = CreateController();
+            var command = controller.Compute(
+                CreateContext(
+                    signedSpeedMetersPerSecond,
+                    lateralErrorMeters,
+                    headingErrorRadians: 0.0,
+                    feedforwardCurvaturePerMeter: 0.0));
+
+            var bodyLateralSpeedMetersPerSecond =
+                signedSpeedMetersPerSecond *
+                Math.Sin(command.CommonAngleRadians);
+
+            // 轨迹执行方向在倒车时与车体X轴相反，因此需要先把车体
+            // 横向速度换算到轨迹点序坐标系，再计算参考轨迹相对车辆的误差变化率。
+            var lateralErrorDerivativeMetersPerSecond =
+                -travelDirection *
+                bodyLateralSpeedMetersPerSecond;
+
+            AssertTrue(
+                lateralErrorMeters *
+                lateralErrorDerivativeMetersPerSecond < 0.0,
+                $"横向误差没有收敛：direction={travelDirection}，" +
+                $"error={lateralErrorMeters:F3}m，" +
+                $"common={command.CommonAngleRadians:F6}rad，" +
+                $"errorDerivative={lateralErrorDerivativeMetersPerSecond:F6}m/s。");
+        }
+
+        /// <summary>
+        /// 验证航向误差差动转角在倒车时仍按行驶方向反号。
+        /// </summary>
+        private static void VerifyHeadingDirection(
+            double travelDirection)
+        {
+            var command = CreateController().Compute(
+                CreateContext(
+                    travelDirection *
+                    SpeedMagnitudeMetersPerSecond,
+                    lateralErrorMeters: 0.0,
+                    headingErrorRadians:
+                        TestHeadingErrorRadians,
+                    feedforwardCurvaturePerMeter: 0.0));
+
+            AssertSameSign(
+                command.DifferentialAngleRadians,
+                travelDirection,
+                "航向误差差动转角");
+        }
+
+        /// <summary>
+        /// 验证正曲率前馈差动转角在倒车时仍按行驶方向反号。
+        /// </summary>
+        private static void VerifyCurvatureDirection(
+            double travelDirection)
+        {
+            var command = CreateController().Compute(
+                CreateContext(
+                    travelDirection *
+                    SpeedMagnitudeMetersPerSecond,
+                    lateralErrorMeters: 0.0,
+                    headingErrorRadians: 0.0,
+                    feedforwardCurvaturePerMeter:
+                        TestCurvaturePerMeter));
+
+            AssertSameSign(
+                command.DifferentialAngleRadians,
+                travelDirection,
+                "曲率前馈差动转角");
+        }
+
+        /// <summary>
+        /// 创建使用固定参数的无状态Stanley控制器。
+        /// </summary>
+        private static StanleyLateralController CreateController()
+        {
+            return new StanleyLateralController(
+                controlPointRadiusMeters: 0.5,
+                crossTrackGainPerSecond: 1.0,
+                headingErrorGain: 1.0,
+                minimumSpeedMetersPerSecond: 0.05,
+                useActualSpeedForGain: true);
+        }
+
+        /// <summary>
+        /// 创建只包含本次符号测试所需字段的轨迹跟踪上下文。
+        /// </summary>
+        private static PathTrackingContext CreateContext(
+            double signedSpeedMetersPerSecond,
+            double lateralErrorMeters,
+            double headingErrorRadians,
+            double feedforwardCurvaturePerMeter)
+        {
+            var actualTwistInBody = new Twist2D(
+                signedSpeedMetersPerSecond,
+                0.0,
+                0.0);
+            var referencePoint = new TrajectoryPoint(
+                arcLengthMeters: 0.0,
+                poseInWorld: Pose2D.Identity,
+                curvaturePerMeter: 0.0,
+                referenceSpeedMetersPerSecond:
+                    signedSpeedMetersPerSecond);
+            var projection = new TrajectoryProjection(
+                segmentStartIndex: 0,
+                referencePoint: referencePoint,
+                lateralErrorMeters: lateralErrorMeters,
+                headingErrorRadians: headingErrorRadians,
+                distanceToTrajectoryMeters:
+                    Math.Abs(lateralErrorMeters),
+                remainingDistanceMeters: 1.0);
+
+            return new PathTrackingContext(
+                actualTwistInBody,
+                true,
+                projection,
+                signedSpeedMetersPerSecond,
+                feedforwardCurvaturePerMeter,
+                deltaTimeSeconds: 0.02);
+        }
+
+        /// <summary>
+        /// 验证实际值与预期符号一致。
+        /// </summary>
+        private static void AssertSameSign(
+            double actualValue,
+            double expectedSign,
+            string valueName)
+        {
+            AssertTrue(
+                Math.Sign(actualValue) ==
+                Math.Sign(expectedSign),
+                $"{valueName}方向错误：actual={actualValue:F6}，" +
+                $"expectedSign={expectedSign:F0}。");
+        }
+
+        /// <summary>
+        /// 条件不成立时抛出异常，使测试进程以非零状态结束。
+        /// </summary>
+        private static void AssertTrue(
+            bool condition,
+            string failureMessage)
+        {
+            if (!condition)
+            {
+                throw new InvalidOperationException(
+                    failureMessage);
+            }
+        }
+    }
+}

MultiWheelC/AGV.cs

@@ -1,21 +0,0 @@
-using ClumsyCore;
-using MDCSToolBox.Clumsy.AgvInterfaces;
-using MDCSToolBox.Clumsy.MotionControllers;
-
-namespace MultiWheelC
-{
-    public class AGV : MultiWheelInterface
-    {
-        public override AbstractGeometricController GetController()
-                => new ChassisController().Get();
-        public override MultiWheelMagTracker GetMagController()
-            => new MultiWheelMagTracker();
-        public override NaiveMagnetController GetNaiveMagnetController()
-            => new NaiveMagnetController();
-
-        public void Sleep(float seconds)
-        {
-            new DriveTask(new Sleep { Second = seconds }.Get()).Wait();
-        }
-    }
-}

MultiWheelC/ChassisController.cs

@@ -1,45 +0,0 @@
-using ClumsyCore;
-using ClumsyCore.Pilot;
-using MDCSToolBox.Clumsy.MotionControllers;
-using MDCSToolBox.Clumsy.Movements;
-using MDCSToolBox.Clumsy.Pilot;
-
-namespace MultiWheelC;
-
-public class ChassisController : MovementDefinition<MultiWheelGeometricController>
-{
-    public float BaseSpeed = Configuration.conf.basicSpeed;
-
-    // 创建单车几何跟踪控制器（直接控本车底盘，不走多车 Auto 通道）
-    public override MultiWheelGeometricController Get()
-    {
-        return new MultiWheelGeometricController
-        {
-            Chassis = BasicPilotBase.Chassis,
-            BaseSpeed = BaseSpeed,
-            SlowDistance = PilotDefinition.Conf.SlowDistance,
-            SlowingPow = PilotDefinition.Conf.SlowingPow,
-            FinishDistance = PilotDefinition.Conf.FinishDistance,
-            FinishSpeed = PilotDefinition.Conf.FinishSpeed,
-            FirstThAccuracy = PilotDefinition.Conf.FirstThAccuracy,
-            FirstRotateSpeedFac = PilotDefinition.Conf.FirstRotateSpeedFac,
-            FirstRotateMaxSpeed = PilotDefinition.Conf.FirstRotateMaxSpeed,
-            NotContinuousAngle = PilotDefinition.Conf.NotContinuousAngle,
-            DebugMode = PilotDefinition.Conf.MotionDebugPrint,
-            DebugCurvature = PilotDefinition.Conf.DebugCurvature,
-            PowerSteeringLookAhead = PilotDefinition.Conf.PowerSteeringLookAhead,
-            SpeedLookAhead = PilotDefinition.Conf.SpeedLookAhead,
-            SpeedLookAheadCurveDiff = PilotDefinition.Conf.SpeedLookAheadCurveDiff,
-            SpeedLookBackCurveDiff = PilotDefinition.Conf.SpeedLookBackCurveDiff,
-            SpeedLimitCurveDiffMin = PilotDefinition.Conf.SpeedLimitCurveDiffMin,
-            SpeedLimitCurveMin = PilotDefinition.Conf.SpeedLimitCurveMin,
-            MaxRotateSpeed = PilotDefinition.Conf.MaxRotateSpeedCurveLimit,
-            MaxRotateAcc = PilotDefinition.Conf.MaxRotateAccCurveLimit,
-            GcpThetaThreshold = PilotDefinition.Conf.GcpThetaThreshold,
-            DthLinearFac = PilotDefinition.Conf.DthLinearFac,
-            DthLinearThreshold = PilotDefinition.Conf.DthLinearThreshold,
-            BiasFac = PilotDefinition.Conf.BiasFac,
-            BiasThreshold = PilotDefinition.Conf.BiasThreshold,
-        };
-    }
-}

MultiWheelC/Configuration/PilotConfig.ParkingControl.cs

@@ -0,0 +1,204 @@
+using ClumsyCore;
+
+namespace MultiWheelC;
+
+/// <summary>
+/// 定义停车状态估计、轨迹跟踪和原地自转使用的车辆级参数。
+/// </summary>
+public partial class PilotConfig
+{
+    #region 停车控制-状态估计
+
+    [FieldMember(desc = "停车控制：Detour最大合理线速度(m/s)")]
+    public float ParkingDetourMaximumLinearSpeed = 1.20f;
+
+    [FieldMember(desc = "停车控制：Detour最大合理角速度(deg/s)")]
+    public float ParkingDetourMaximumAngularSpeedDegrees = 45f;
+
+    [FieldMember(desc = "停车控制：Detour位置跳变余量(m)")]
+    public float ParkingDetourPositionJumpMargin = 0.03f;
+
+    [FieldMember(desc = "停车控制：Detour航向跳变余量(deg)")]
+    public float ParkingDetourHeadingJumpMarginDegrees = 5f;
+
+    [FieldMember(desc = "停车控制：Detour速度预测位置残差(m)")]
+    public float ParkingDetourVelocityPositionResidual = 0.04f;
+
+    [FieldMember(desc = "停车控制：Detour速度预测航向残差(deg)")]
+    public float ParkingDetourVelocityHeadingResidualDegrees = 5f;
+
+    [FieldMember(desc = "停车控制：Detour航向异常确认新帧数")]
+    public int ParkingDetourHeadingOutlierConfirmationFrames = 3;
+
+    [FieldMember(desc = "停车控制：Detour航向异常短时预测超时(s)")]
+    public float ParkingDetourHeadingOutlierPredictionTimeoutSeconds =
+        0.30f;
+
+    [FieldMember(desc = "停车控制：Detour静止确认时间(s)")]
+    public float ParkingDetourStationaryConfirmationSeconds = 0.35f;
+
+    [FieldMember(desc = "停车控制：Detour跳变确认新帧数")]
+    public int ParkingDetourJumpConfirmationFrames = 3;
+
+    [FieldMember(desc = "停车控制：Detour跳变确认超时(s)")]
+    public float ParkingDetourJumpConfirmationTimeoutSeconds = 0.60f;
+
+    [FieldMember(desc = "停车控制：Detour自动坐标连续化最大平移(m)")]
+    public float ParkingDetourMaximumAutomaticFrameShift = 0.15f;
+
+    [FieldMember(desc = "停车控制：Detour自动坐标连续化最大航向变化(deg)")]
+    public float ParkingDetourMaximumAutomaticHeadingShiftDegrees = 5f;
+
+    [FieldMember(desc = "停车控制：Detour缓存帧最大允许时间(s)")]
+    public float ParkingDetourMaximumCachedFrameAgeSeconds = 0.50f;
+
+    [FieldMember(desc = "停车控制：Detour定位质量失效/恢复确认新帧数")]
+    public int ParkingDetourLocalizationQualityConfirmationFrames = 3;
+
+    [FieldMember(desc = "停车控制：Detour线速度滤波时间常数(s)")]
+    public float ParkingDetourLinearVelocityFilterSeconds = 0.15f;
+
+    [FieldMember(desc = "停车控制：Detour角速度滤波时间常数(s)")]
+    public float ParkingDetourAngularVelocityFilterSeconds = 0.20f;
+
+    [FieldMember(desc = "停车控制：电机反馈速度滤波时间常数(s)")]
+    public float ParkingWheelVelocityFilterSeconds = 0.10f;
+
+    #endregion
+
+    #region 停车控制-Stanley
+
+    [FieldMember(desc = "停车控制：Stanley横向误差增益(1/s)")]
+    public float ParkingStanleyCrossTrackGain = 0.4f;
+
+    [FieldMember(desc = "停车控制：Stanley航向误差增益")]
+    public float ParkingStanleyHeadingGain = 1.0f;
+
+    [FieldMember(desc = "停车控制：Stanley最低分母速度(m/s)")]
+    public float ParkingStanleyMinimumSpeed = 0.15f;
+
+    [FieldMember(desc = "停车控制：Stanley使用电机实际速度")]
+    public bool ParkingStanleyUseActualSpeed = true;
+
+    [FieldMember(desc = "停车控制：Stanley曲率前馈预瞄时间(s)，0为关闭")]
+    public float ParkingStanleyCurvaturePreviewSeconds = 0.15f;
+
+    [FieldMember(desc = "停车控制：Stanley曲率前馈最大预瞄距离(m)")]
+    public float ParkingStanleyMaximumCurvaturePreviewMeters = 0.12f;
+
+    [FieldMember(desc = "停车控制：Stanley横向修正上限(deg)")]
+    public float ParkingMaximumCrossTrackCorrectionDegrees = 10f;
+
+    [FieldMember(desc = "停车控制：Stanley航向修正上限(deg)")]
+    public float ParkingMaximumHeadingCorrectionDegrees = 10f;
+
+    #endregion
+
+    #region 停车控制-纵向PID
+
+    [FieldMember(desc = "停车控制：纵向速度Kp")]
+    public float ParkingLongitudinalKp = 0.5f;
+
+    [FieldMember(desc = "停车控制：纵向速度Ki(1/s)")]
+    public float ParkingLongitudinalKi = 0f;
+
+    [FieldMember(desc = "停车控制：纵向速度Kd(s)")]
+    public float ParkingLongitudinalKd = 0f;
+
+    [FieldMember(desc = "停车控制：纵向积分修正上限(m/s)")]
+    public float ParkingMaximumIntegralCorrection = 0.05f;
+
+    [FieldMember(desc = "停车控制：纵向速度误差死区(m/s)")]
+    public float ParkingLongitudinalSpeedErrorDeadband = 0.025f;
+
+    [FieldMember(desc = "停车控制：最大命令速度(m/s)")]
+    public float ParkingMaximumCommandSpeed = 0.50f;
+
+    #endregion
+
+    #region 停车控制-原地自转
+
+    [FieldMember(desc = "停车控制：原地自转Kp")]
+    public float InPlaceRotateKp = 1.0f;
+
+    [FieldMember(desc = "停车控制：原地自转Ki")]
+    public float InPlaceRotateKi = 0f;
+
+    [FieldMember(desc = "停车控制：原地自转Kd")]
+    public float InPlaceRotateKd = 0f;
+
+    [FieldMember(desc = "停车控制：原地自转积分限幅")]
+    public float InPlaceRotateMaxI = 0f;
+
+    [FieldMember(desc = "停车控制：原地自转到位角度容差(deg)")]
+    public float InPlaceRotateArriveDeg = 1.5f;
+
+    [FieldMember(desc = "停车控制：原地自转起转前舵轮对齐容差(deg)")]
+    public float InPlaceRotateWheelAlignDeg = 2f;
+
+    [FieldMember(desc = "停车控制：原地自转最小有效角速度(deg/s)")]
+    public float InPlaceRotateMinimumSpeed = 1f;
+
+    [FieldMember(desc = "停车控制：原地自转最大角速度(deg/s)")]
+    // public float InPlaceRotateMaxSpeed = 47.5f;
+    public float InPlaceRotateMaxSpeed = 45f;
+
+    [FieldMember(desc = "停车控制：原地自转角加速度(deg/s²)")]
+    // public float InPlaceRotateAcc = 60f;
+    public float InPlaceRotateAcc = 45f;
+
+    [FieldMember(desc = "停车控制：原地自转超时(s)")]
+    public float InPlaceRotateTimeoutSec = 15f;
+
+    #endregion
+
+    #region 停车控制-舵轮回正
+
+    [FieldMember(desc = "停车控制：舵轮回正到位容差(deg)")]
+    public float ParkingWheelForwardToleranceDegrees = 2f;
+
+    [FieldMember(desc = "停车控制：舵轮回正稳定确认时间(s)")]
+    public float ParkingWheelForwardStableSeconds = 0.3f;
+
+    [FieldMember(desc = "停车控制：舵轮回正超时(s，0关闭)")]
+    public float ParkingWheelForwardTimeoutSeconds = 10f;
+
+    #endregion
+
+    #region 停车控制-GCP与完成条件
+
+    [FieldMember(desc = "停车控制：最大GCP转角(deg)")]
+    public float ParkingMaximumGcpAngleDegrees = 45f;
+
+    [FieldMember(desc = "停车控制：最大GCP转角速度(deg/s)")]
+    public float ParkingMaximumGcpAngleRateDegreesPerSecond = 15f;
+
+    [FieldMember(desc = "停车控制：终点距离容差(m)")]
+    public float ParkingFinishDistance = 0.03f;
+
+    [FieldMember(desc = "停车控制：终点速度容差(m/s)")]
+    public float ParkingFinishSpeed = 0.02f;
+
+    [FieldMember(desc = "停车控制：终点航向容差(deg)")]
+    public float ParkingFinishHeadingToleranceDegrees = 3f;
+
+    [FieldMember(desc = "停车控制：终点制动预瞄距离(m)")]
+    public float ParkingTerminalBrakingPreview = 0.02f;
+
+    [FieldMember(desc = "停车控制：终点单向逼近范围(m)")]
+    public float ParkingTerminalApproachDistance = 0.10f;
+
+    [FieldMember(desc = "停车控制：终点单向逼近增益(1/s)")]
+    public float ParkingTerminalApproachGain = 0.8f;
+
+    [FieldMember(desc = "停车控制：终点单向逼近最大速度(m/s)")]
+    public float ParkingTerminalMaximumApproachSpeed = 0.05f;
+
+    [FieldMember(desc = "停车控制：最大轨迹偏离距离(m)")]
+    public float ParkingMaximumDistanceToTrajectory = 0.30f;
+
+    [FieldMember(desc = "停车控制：轨迹执行超时(s)")]
+    public float ParkingExecutionTimeoutSeconds = 120f;
+
+    #endregion
+}

MultiWheelC/Control/Abstractions/ILateralController.cs

@@ -0,0 +1,19 @@
+namespace MultiWheelC.Control.Abstractions
+{
+    /// <summary>
+    /// 定义Stanley、LQR和MPC等车体中心横向控制器的统一接口。
+    /// </summary>
+    public interface ILateralController
+    {
+        /// <summary>
+        /// 根据本周期车辆状态和轨迹误差计算车体中心目标曲率。
+        /// </summary>
+        LateralControlCommand Compute(
+            PathTrackingContext context);
+
+        /// <summary>
+        /// 清除控制器跨周期状态，以便开始新轨迹或异常恢复后重新运行。
+        /// </summary>
+        void Reset();
+    }
+}

MultiWheelC/Control/Abstractions/ILongitudinalController.cs

@@ -0,0 +1,19 @@
+namespace MultiWheelC.Control.Abstractions
+{
+    /// <summary>
+    /// 定义根据参考速度和实际纵向速度生成底盘命令速度的统一接口。
+    /// </summary>
+    public interface ILongitudinalController
+    {
+        /// <summary>
+        /// 根据本周期速度目标、速度反馈和时间间隔计算有符号底盘命令速度。
+        /// </summary>
+        double ComputeSpeedMetersPerSecond(
+            PathTrackingContext context);
+
+        /// <summary>
+        /// 清除积分、历史误差和其他跨周期状态，以便安全开始新的控制过程。
+        /// </summary>
+        void Reset();
+    }
+}

MultiWheelC/Control/Abstractions/LateralControlCommand.cs

@@ -0,0 +1,76 @@
+using System;
+
+namespace MultiWheelC.Control.Abstractions
+{
+    /// <summary>
+    /// 表示横向控制器生成的前、后GCP目标转角，单位为rad，逆时针为正。
+    /// </summary>
+    public readonly struct LateralControlCommand
+    {
+        /// <summary>
+        /// 创建前、后GCP目标转角命令。
+        /// </summary>
+        public LateralControlCommand(
+            double frontGcpAngleRadians,
+            double rearGcpAngleRadians)
+        {
+            EnsureFinite(
+                frontGcpAngleRadians,
+                nameof(frontGcpAngleRadians));
+            EnsureFinite(
+                rearGcpAngleRadians,
+                nameof(rearGcpAngleRadians));
+
+            FrontGcpAngleRadians =
+                frontGcpAngleRadians;
+            RearGcpAngleRadians =
+                rearGcpAngleRadians;
+        }
+
+        /// <summary>
+        /// 获取前GCP目标转角，单位为rad，逆时针为正。
+        /// </summary>
+        public double FrontGcpAngleRadians { get; }
+
+        /// <summary>
+        /// 获取后GCP目标转角，单位为rad，逆时针为正。
+        /// </summary>
+        public double RearGcpAngleRadians { get; }
+
+        /// <summary>
+        /// 获取前后GCP的共同转角分量，主要用于横向平移修正。
+        /// </summary>
+        public double CommonAngleRadians =>
+            (FrontGcpAngleRadians +
+             RearGcpAngleRadians) / 2.0;
+
+        /// <summary>
+        /// 获取前后GCP的差动转角分量，主要用于曲率前馈和航向修正。
+        /// </summary>
+        public double DifferentialAngleRadians =>
+            (FrontGcpAngleRadians -
+             RearGcpAngleRadians) / 2.0;
+
+        /// <summary>
+        /// 创建前后GCP均保持车头方向的直线命令。
+        /// </summary>
+        public static LateralControlCommand Straight =>
+            new LateralControlCommand(0.0, 0.0);
+
+        /// <summary>
+        /// 检查GCP目标转角是否为有限值。
+        /// </summary>
+        private static void EnsureFinite(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "GCP目标转角必须是有限值。");
+            }
+        }
+    }
+}

MultiWheelC/Control/Abstractions/PathTrackingContext.cs

@@ -0,0 +1,156 @@
+using System;
+using MultiWheelC.Trajectory;
+using MyParking.Shared;
+
+namespace MultiWheelC.Control.Abstractions
+{
+    /// <summary>
+    /// 保存一次轨迹跟踪控制周期使用的刚体速度、轨迹投影和真实时间间隔。
+    /// </summary>
+    public readonly struct PathTrackingContext
+    {
+        /// <summary>
+        /// 创建横向和纵向控制器共享的只读控制输入快照。
+        /// </summary>
+        public PathTrackingContext(
+            Twist2D actualTwistInBody,
+            bool hasValidVelocityEstimate,
+            TrajectoryProjection projection,
+            double controlReferenceSpeedMetersPerSecond,
+            double feedforwardCurvaturePerMeter,
+            double deltaTimeSeconds,
+            double motionDirectionInBodyRadians = 0.0)
+        {
+            EnsureFinite(
+                controlReferenceSpeedMetersPerSecond,
+                nameof(controlReferenceSpeedMetersPerSecond));
+            EnsureFinite(
+                feedforwardCurvaturePerMeter,
+                nameof(feedforwardCurvaturePerMeter));
+            EnsureFinitePositive(
+                deltaTimeSeconds,
+                nameof(deltaTimeSeconds));
+            EnsureFinite(
+                motionDirectionInBodyRadians,
+                nameof(motionDirectionInBodyRadians));
+            NumericGuard.EnsureFinite(
+                actualTwistInBody,
+                nameof(actualTwistInBody));
+
+            ActualTwistInBody = actualTwistInBody;
+            HasValidVelocityEstimate =
+                hasValidVelocityEstimate;
+            Projection = projection;
+            ControlReferenceSpeedMetersPerSecond =
+                controlReferenceSpeedMetersPerSecond;
+            FeedforwardCurvaturePerMeter =
+                feedforwardCurvaturePerMeter;
+            DeltaTimeSeconds = deltaTimeSeconds;
+            MotionDirectionInBodyRadians =
+                AngleMath.NormalizeRadians(
+                    motionDirectionInBodyRadians);
+        }
+
+        /// <summary>
+        /// 获取受控刚体坐标系下的实际速度。
+        /// </summary>
+        public Twist2D ActualTwistInBody { get; }
+
+        /// <summary>
+        /// 获取实际车体中心投影到参考轨迹后得到的参考状态和跟踪误差。
+        /// </summary>
+        public TrajectoryProjection Projection { get; }
+
+        /// <summary>
+        /// 获取本次控制计算距离上次计算的真实时间间隔，单位为s。
+        /// </summary>
+        public double DeltaTimeSeconds { get; }
+
+        /// <summary>
+        /// 获取轨迹原始速度经过起步释放和制动预瞄处理后，本控制周期实际使用的有符号参考速度，单位为m/s。
+        /// </summary>
+        public double ControlReferenceSpeedMetersPerSecond { get; }
+
+        /// <summary>
+        /// 获取车辆沿当前运动坐标系X轴方向的实际纵向速度，单位为m/s。
+        /// </summary>
+        public double ActualLongitudinalSpeedMetersPerSecond =>
+            Math.Cos(MotionDirectionInBodyRadians) *
+                ActualTwistInBody.VxMetersPerSecond +
+            Math.Sin(MotionDirectionInBodyRadians) *
+                ActualTwistInBody.VyMetersPerSecond;
+
+        /// <summary>
+        /// 获取当前运动坐标系X轴在车体系中的方向，单位为rad。
+        /// </summary>
+        public double MotionDirectionInBodyRadians { get; }
+
+        /// <summary>
+        /// 获取沿轨迹执行点序定义的参考曲率，单位为1/m，左弯为正。
+        /// </summary>
+        public double ReferenceCurvaturePerMeter =>
+            Projection.ReferencePoint
+                .CurvaturePerMeter;
+
+        /// <summary>
+        /// 获取沿轨迹点序适量预瞄后专供几何前馈使用的参考曲率，单位为1/m，左弯为正。
+        /// </summary>
+        public double FeedforwardCurvaturePerMeter { get; }
+
+        /// <summary>
+        /// 获取相对轨迹执行点序的有符号横向误差，单位为m，参考轨迹位于执行方向左侧时为正。
+        /// </summary>
+        public double LateralErrorMeters =>
+            Projection.LateralErrorMeters;
+
+        /// <summary>
+        /// 获取参考航向减实际车体航向的最短角差，单位为rad，逆时针为正。
+        /// </summary>
+        public double HeadingErrorRadians =>
+            Projection.HeadingErrorRadians;
+
+        /// <summary>
+        /// 获取当前投影位置沿参考轨迹到终点的剩余距离，单位为m。
+        /// </summary>
+        public double RemainingDistanceMeters =>
+            Projection.RemainingDistanceMeters;
+
+        /// <summary>
+        /// 获取本周期实际速度估计是否可供闭环控制使用。
+        /// </summary>
+        public bool HasValidVelocityEstimate { get; }
+
+        /// <summary>
+        /// 检查控制周期是否为正有限值。
+        /// </summary>
+        private static void EnsureFinitePositive(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value) ||
+                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,
+                    "轨迹跟踪参考速度必须是有限值。");
+            }
+        }
+    }
+}

MultiWheelC/Control/Allocation/GcpCommandAllocator.cs

@@ -0,0 +1,73 @@
+using System;
+using MultiWheelC.Control.Abstractions;
+using MyParking.Shared;
+// 限制目标角度的最大绝对值，例如不能超过60°。
+namespace MultiWheelC.Control.Allocation
+{
+    /// <summary>
+    /// 独立限制前后GCP目标转角并与纵向速度组合成底盘运动命令。
+    /// </summary>
+    public sealed class GcpCommandAllocator
+    {
+        /// <summary>
+        /// 创建使用指定前后GCP最大转角的命令分配器。
+        /// </summary>
+        public GcpCommandAllocator(double maximumGcpAngleRadians)
+        {
+            NumericGuard.EnsureFinitePositive(
+                maximumGcpAngleRadians,
+                nameof(maximumGcpAngleRadians));
+
+            if (maximumGcpAngleRadians >= Math.PI / 2.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(maximumGcpAngleRadians),
+                    "最大GCP转角必须小于π/2。");
+            }
+
+            MaximumGcpAngleRadians = maximumGcpAngleRadians;
+        }
+
+        /// <summary>
+        /// 获取前后GCP允许的最大转角绝对值，单位为rad。
+        /// </summary>
+        public double MaximumGcpAngleRadians { get; }
+
+        /// <summary>
+        /// 将纵向速度和前后GCP转角组合为底盘运动命令。
+        /// </summary>
+        public GcpMotionCommand Allocate(
+            double speedMetersPerSecond,
+            LateralControlCommand lateralCommand)
+        {
+            NumericGuard.EnsureFinite(
+                speedMetersPerSecond,
+                nameof(speedMetersPerSecond));
+
+            var frontAngleRadians = ClampSymmetric(
+                lateralCommand.FrontGcpAngleRadians,
+                MaximumGcpAngleRadians);
+            var rearAngleRadians = ClampSymmetric(
+                lateralCommand.RearGcpAngleRadians,
+                MaximumGcpAngleRadians);
+
+            return new GcpMotionCommand(
+                speedMetersPerSecond,
+                frontAngleRadians,
+                rearAngleRadians);
+        }
+
+        /// <summary>
+        /// 将数值按正负对称方式限制在指定绝对值内。
+        /// </summary>
+        private static double ClampSymmetric(
+            double value,
+            double maximumAbsoluteValue)
+        {
+            return Math.Max(
+                -maximumAbsoluteValue,
+                Math.Min(maximumAbsoluteValue, value));
+        }
+
+    }
+}

MultiWheelC/Control/Allocation/GcpKinematics.cs

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

MultiWheelC/Control/Allocation/GcpMotionCommand.cs

@@ -0,0 +1,52 @@
+using MyParking.Shared;
+
+namespace MultiWheelC.Control.Allocation
+{
+    /// <summary>
+    /// 表示发送给旧版多舵轮四轮解算前的有符号速度和前后GCP角度命令。
+    /// </summary>
+    public readonly struct GcpMotionCommand
+    {
+        /// <summary>
+        /// 创建统一使用m/s和rad的前后几何控制点运动命令。
+        /// </summary>
+        public GcpMotionCommand(
+            double speedMetersPerSecond,
+            double frontAngleRadians,
+            double rearAngleRadians)
+        {
+            NumericGuard.EnsureFinite(
+                speedMetersPerSecond,
+                nameof(speedMetersPerSecond));
+            NumericGuard.EnsureFinite(
+                frontAngleRadians,
+                nameof(frontAngleRadians));
+            NumericGuard.EnsureFinite(
+                rearAngleRadians,
+                nameof(rearAngleRadians));
+
+            SpeedMetersPerSecond =
+                speedMetersPerSecond;
+            FrontAngleRadians =
+                frontAngleRadians;
+            RearAngleRadians =
+                rearAngleRadians;
+        }
+
+        /// <summary>
+        /// 获取准备交给底盘的有符号纵向速度，单位为m/s，正值表示前进。
+        /// </summary>
+        public double SpeedMetersPerSecond { get; }
+
+        /// <summary>
+        /// 获取前几何控制点相对车体X轴的目标方向，单位为rad，逆时针为正。
+        /// </summary>
+        public double FrontAngleRadians { get; }
+
+        /// <summary>
+        /// 获取后几何控制点相对车体X轴的目标方向，单位为rad，逆时针为正。
+        /// </summary>
+        public double RearAngleRadians { get; }
+
+    }
+}

MultiWheelC/Control/Common/PidController.cs

@@ -0,0 +1,307 @@
+using System;
+
+namespace MultiWheelC.Control.Common
+{
+    /// <summary>
+    /// 使用真实控制周期计算带积分限幅、输出限幅和抗饱和的通用有状态PID输出。
+    /// </summary>
+    public sealed class PidController
+    {
+        private double _integralState;
+        private double _previousError;
+        private double _previousMeasurement;
+        private bool _hasPreviousSample;
+
+        /// <summary>
+        /// 创建具有指定增益、积分输出限制和微分形式的PID控制器。
+        /// </summary>
+        public PidController(
+            double proportionalGain,
+            double integralGainPerSecond,
+            double derivativeGainSeconds,
+            double maximumIntegralOutput,
+            bool derivativeOnMeasurement = true)
+        {
+            EnsureFiniteNonNegative(
+                proportionalGain,
+                nameof(proportionalGain));
+            EnsureFiniteNonNegative(
+                integralGainPerSecond,
+                nameof(integralGainPerSecond));
+            EnsureFiniteNonNegative(
+                derivativeGainSeconds,
+                nameof(derivativeGainSeconds));
+            EnsureFiniteNonNegative(
+                maximumIntegralOutput,
+                nameof(maximumIntegralOutput));
+
+            ProportionalGain = proportionalGain;
+            IntegralGainPerSecond = integralGainPerSecond;
+            DerivativeGainSeconds = derivativeGainSeconds;
+            MaximumIntegralOutput = maximumIntegralOutput;
+            DerivativeOnMeasurement = derivativeOnMeasurement;
+        }
+
+        /// <summary>
+        /// 获取比例增益。
+        /// </summary>
+        public double ProportionalGain { get; }
+
+        /// <summary>
+        /// 获取积分增益，单位为1/s。
+        /// </summary>
+        public double IntegralGainPerSecond { get; }
+
+        /// <summary>
+        /// 获取微分增益，单位为s。
+        /// </summary>
+        public double DerivativeGainSeconds { get; }
+
+        /// <summary>
+        /// 获取积分项允许产生的最大输出绝对值。
+        /// </summary>
+        public double MaximumIntegralOutput { get; }
+
+        /// <summary>
+        /// 获取微分项是否作用于测量值，以避免设定值变化产生微分冲击。
+        /// </summary>
+        public bool DerivativeOnMeasurement { get; }
+
+        /// <summary>
+        /// 获取最近一次设定值减测量值的误差。
+        /// </summary>
+        public double LastError { get; private set; }
+
+        /// <summary>
+        /// 获取最近一次比例项输出。
+        /// </summary>
+        public double LastProportionalOutput { get; private set; }
+
+        /// <summary>
+        /// 获取最近一次积分项输出。
+        /// </summary>
+        public double LastIntegralOutput { get; private set; }
+
+        /// <summary>
+        /// 获取最近一次微分项输出。
+        /// </summary>
+        public double LastDerivativeOutput { get; private set; }
+
+        /// <summary>
+        /// 获取最近一次经过输出范围限制后的PID输出。
+        /// </summary>
+        public double LastOutput { get; private set; }
+
+        /// <summary>
+        /// 根据设定值、测量值、真实时间间隔和本周期输出范围更新PID。
+        /// </summary>
+        public double Update(
+            double setPoint,
+            double measurement,
+            double deltaTimeSeconds,
+            double minimumOutput,
+            double maximumOutput)
+        {
+            EnsureFinite(setPoint, nameof(setPoint));
+            EnsureFinite(measurement, nameof(measurement));
+            EnsureFinitePositive(
+                deltaTimeSeconds,
+                nameof(deltaTimeSeconds));
+            EnsureFinite(minimumOutput, nameof(minimumOutput));
+            EnsureFinite(maximumOutput, nameof(maximumOutput));
+
+            if (minimumOutput > maximumOutput)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(minimumOutput),
+                    "PID最小输出不能大于最大输出。");
+            }
+
+            var error = setPoint - measurement;
+            var proportionalOutput =
+                ProportionalGain * error;
+            var derivativeOutput = CalculateDerivativeOutput(
+                error,
+                measurement,
+                deltaTimeSeconds);
+
+            var candidateIntegralState =
+                _integralState +
+                error * deltaTimeSeconds;
+            var integralOutput = CalculateIntegralOutput(
+                candidateIntegralState);
+
+            // 同步截断积分状态本身，避免积分输出虽已限幅、内部状态仍继续增长。
+            candidateIntegralState =
+                IntegralGainPerSecond > 0.0 &&
+                MaximumIntegralOutput > 0.0
+                    ? integralOutput /
+                      IntegralGainPerSecond
+                    : 0.0;
+
+            var unlimitedOutput =
+                proportionalOutput +
+                integralOutput +
+                derivativeOutput;
+            var output = Clamp(
+                unlimitedOutput,
+                minimumOutput,
+                maximumOutput);
+
+            // 根据实际允许输出反算积分项，避免执行器饱和期间继续积累误差。
+            if (IntegralGainPerSecond > 0.0 &&
+                output != unlimitedOutput)
+            {
+                integralOutput = Clamp(
+                    output -
+                    proportionalOutput -
+                    derivativeOutput,
+                    -MaximumIntegralOutput,
+                    MaximumIntegralOutput);
+                candidateIntegralState =
+                    integralOutput /
+                    IntegralGainPerSecond;
+            }
+
+            _integralState =
+                IntegralGainPerSecond > 0.0 &&
+                MaximumIntegralOutput > 0.0
+                    ? candidateIntegralState
+                    : 0.0;
+            _previousError = error;
+            _previousMeasurement = measurement;
+            _hasPreviousSample = true;
+
+            LastError = error;
+            LastProportionalOutput = proportionalOutput;
+            LastIntegralOutput = integralOutput;
+            LastDerivativeOutput = derivativeOutput;
+            LastOutput = output;
+
+            return output;
+        }
+
+        /// <summary>
+        /// 清除积分、历史采样和最近一次PID诊断输出。
+        /// </summary>
+        public void Reset()
+        {
+            _integralState = 0.0;
+            _previousError = 0.0;
+            _previousMeasurement = 0.0;
+            _hasPreviousSample = false;
+            LastError = 0.0;
+            LastProportionalOutput = 0.0;
+            LastIntegralOutput = 0.0;
+            LastDerivativeOutput = 0.0;
+            LastOutput = 0.0;
+        }
+
+        /// <summary>
+        /// 使用测量值微分或误差微分计算本周期微分项输出。
+        /// </summary>
+        private double CalculateDerivativeOutput(
+            double error,
+            double measurement,
+            double deltaTimeSeconds)
+        {
+            if (!_hasPreviousSample ||
+                DerivativeGainSeconds <= 0.0)
+            {
+                return 0.0;
+            }
+
+            if (DerivativeOnMeasurement)
+            {
+                return -DerivativeGainSeconds *
+                    (measurement - _previousMeasurement) /
+                    deltaTimeSeconds;
+            }
+
+            return DerivativeGainSeconds *
+                (error - _previousError) /
+                deltaTimeSeconds;
+        }
+
+        /// <summary>
+        /// 根据积分状态计算经过绝对值限制的积分项输出。
+        /// </summary>
+        private double CalculateIntegralOutput(
+            double integralState)
+        {
+            if (IntegralGainPerSecond <= 0.0 ||
+                MaximumIntegralOutput <= 0.0)
+            {
+                return 0.0;
+            }
+
+            return Clamp(
+                IntegralGainPerSecond * integralState,
+                -MaximumIntegralOutput,
+                MaximumIntegralOutput);
+        }
+
+        /// <summary>
+        /// 将数值限制在指定闭区间内。
+        /// </summary>
+        private static double Clamp(
+            double value,
+            double minimum,
+            double maximum)
+        {
+            return Math.Max(
+                minimum,
+                Math.Min(maximum, value));
+        }
+
+        /// <summary>
+        /// 检查参数是否为正有限值。
+        /// </summary>
+        private static void EnsureFinitePositive(
+            double value,
+            string parameterName)
+        {
+            EnsureFinite(value, parameterName);
+
+            if (value <= 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "PID时间间隔必须是正有限值。");
+            }
+        }
+
+        /// <summary>
+        /// 检查参数是否为非负有限值。
+        /// </summary>
+        private static void EnsureFiniteNonNegative(
+            double value,
+            string parameterName)
+        {
+            EnsureFinite(value, parameterName);
+
+            if (value < 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "PID增益和积分输出限幅必须是非负有限值。");
+            }
+        }
+
+        /// <summary>
+        /// 检查参数是否为有限值。
+        /// </summary>
+        private static void EnsureFinite(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "PID参数和输入必须是有限值。");
+            }
+        }
+    }
+}

MultiWheelC/Control/Execution/GcpCommandExecutor.cs

@@ -0,0 +1,194 @@
+using System;
+using MultiWheelC.Control.Allocation;
+using MyParking.Shared;
+
+namespace MultiWheelC.Control.Execution
+{
+    /// <summary>
+    /// 将SI单位的GCP运动命令安全转换为现有多舵轮底盘调用。
+    /// </summary>
+    public sealed class GcpCommandExecutor
+    {
+        private const double StopSpeedDeadbandMetersPerSecond =
+            1e-6;
+
+        private readonly MultiWheelChassisAdapter _chassisAdapter;
+        private readonly double _motionDirectionInBodyRadians;
+        private double _lastFrontAngleRadians;
+        private double _lastRearAngleRadians;
+
+        /// <summary>
+        /// 创建绑定指定单车底盘适配器的GCP命令执行器。
+        /// </summary>
+        public GcpCommandExecutor(
+            MultiWheelChassisAdapter chassisAdapter,
+            double maximumGcpAngleRateRadiansPerSecond =
+                10.0 * Math.PI / 180.0,
+            double motionDirectionInBodyRadians = 0.0)
+        {
+            _chassisAdapter = chassisAdapter ??
+                throw new ArgumentNullException(
+                    nameof(chassisAdapter));
+            EnsureFinitePositive(
+                maximumGcpAngleRateRadiansPerSecond,
+                nameof(maximumGcpAngleRateRadiansPerSecond));
+            NumericGuard.EnsureFinite(
+                motionDirectionInBodyRadians,
+                nameof(motionDirectionInBodyRadians));
+
+            MaximumGcpAngleRateRadiansPerSecond =
+                maximumGcpAngleRateRadiansPerSecond;
+            _motionDirectionInBodyRadians =
+                AngleMath.NormalizeRadians(
+                    motionDirectionInBodyRadians);
+        }
+
+        /// <summary>
+        /// 获取执行器绑定的车辆编号。
+        /// </summary>
+        public int VehicleId =>
+            _chassisAdapter.VehicleId;
+
+        /// <summary>
+        /// 获取前后GCP目标角度允许的最大变化率，单位为rad/s。
+        /// </summary>
+        public double MaximumGcpAngleRateRadiansPerSecond { get; }
+
+        /// <summary>
+        /// 获取最近一次控制器请求的未限速GCP命令。
+        /// </summary>
+        public GcpMotionCommand? LastRequestedCommand { get; private set; }
+
+        /// <summary>
+        /// 获取最近一次经过GCP角速度限制后实际发送给底盘的命令。
+        /// </summary>
+        public GcpMotionCommand? LastSentCommand { get; private set; }
+
+        /// <summary>
+        /// 获取最近一次旧版底盘运动分解失败原因。
+        /// </summary>
+        public string LastFailureReason { get; private set; } =
+            string.Empty;
+
+        /// <summary>
+        /// 使用真实控制周期执行一条GCP命令，并在分解失败时保持停车。
+        /// </summary>
+        public bool Execute(
+            GcpMotionCommand command,
+            double deltaTimeSeconds)
+        {
+            EnsureFinitePositive(
+                deltaTimeSeconds,
+                nameof(deltaTimeSeconds));
+            LastRequestedCommand = command;
+
+            if (Math.Abs(command.SpeedMetersPerSecond) <=
+                StopSpeedDeadbandMetersPerSecond)
+            {
+                Stop();
+                LastSentCommand = new GcpMotionCommand(
+                    0.0,
+                    _lastFrontAngleRadians,
+                    _lastRearAngleRadians);
+                return true;
+            }
+
+            var maximumAngleChangeRadians =
+                MaximumGcpAngleRateRadiansPerSecond *
+                deltaTimeSeconds;
+            _lastFrontAngleRadians = MoveTowards(
+                _lastFrontAngleRadians,
+                command.FrontAngleRadians,
+                maximumAngleChangeRadians);
+            _lastRearAngleRadians = MoveTowards(
+                _lastRearAngleRadians,
+                command.RearAngleRadians,
+                maximumAngleChangeRadians);
+
+            var limitedCommand = new GcpMotionCommand(
+                command.SpeedMetersPerSecond,
+                _lastFrontAngleRadians,
+                _lastRearAngleRadians);
+            LastSentCommand = limitedCommand;
+
+            var motionFrameTwist =
+                GcpKinematics.ToBodyTwist(
+                    limitedCommand,
+                    _chassisAdapter.ControlPointRadiusMeters);
+            var bodyTwist =
+                FrameTransform2D.TransformTwistAtSamePoint(
+                    new Pose2D(
+                        0.0,
+                        0.0,
+                        _motionDirectionInBodyRadians),
+                    motionFrameTwist);
+            var success = _chassisAdapter.SendBodyTwist(
+                bodyTwist,
+                TimeSpan.FromSeconds(deltaTimeSeconds));
+
+            LastFailureReason = success
+                ? string.Empty
+                : BuildFailureReason();
+
+            return success;
+        }
+
+        /// <summary>
+        /// 立即清零底盘驱动速度并清除执行器失败状态。
+        /// </summary>
+        public void Stop()
+        {
+            _chassisAdapter.StopImmediately();
+            LastFailureReason = string.Empty;
+        }
+
+        /// <summary>
+        /// 以不超过指定单周期变化量的速度使当前值接近目标值。
+        /// </summary>
+        private static double MoveTowards(
+            double current,
+            double target,
+            double maximumChange)
+        {
+            var difference = target - current;
+
+            if (Math.Abs(difference) <= maximumChange)
+            {
+                return target;
+            }
+
+            return current +
+                   Math.Sign(difference) *
+                   maximumChange;
+        }
+
+        /// <summary>
+        /// 将底盘返回的空失败原因替换为可诊断的默认说明。
+        /// </summary>
+        private string BuildFailureReason()
+        {
+            return string.IsNullOrWhiteSpace(
+                    _chassisAdapter.LastFailureReason)
+                ? "旧版SendMotion未能完成GCP运动分解。"
+                : _chassisAdapter.LastFailureReason;
+        }
+
+        /// <summary>
+        /// 检查控制周期是否为正有限值且能够转换为TimeSpan。
+        /// </summary>
+        private static void EnsureFinitePositive(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value) ||
+                value <= 0.0 ||
+                value > TimeSpan.MaxValue.TotalSeconds)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "GCP命令控制周期必须是TimeSpan可表示的正有限秒数。");
+            }
+        }
+    }
+}

MultiWheelC/Control/Execution/ParkingGeometricController.cs

@@ -0,0 +1,435 @@
+using System;
+using System.Diagnostics;
+using MultiWheelC.Control.Abstractions;
+using MultiWheelC.Control.Allocation;
+using MultiWheelC.StateEstimation;
+using MultiWheelC.Trajectory;
+using MyParking.Shared;
+
+namespace MultiWheelC.Control.Execution
+{
+    // 单周期轨迹控制结果。
+    public enum ParkingControlCycleResult
+    {
+        Inactive = 0,
+        CommandSent = 1,
+        Completed = 2,
+        StateUnavailable = 3,
+        Faulted = 4
+    }
+
+    // 单周期各阶段耗时及定位帧更新状态，用于区分控制计算与底层通信延迟。
+    public readonly struct ParkingControlCycleTiming
+    {
+        public ParkingControlCycleTiming(
+            long cycleIndex,
+            double cycleIntervalMilliseconds,
+            double stateReadMilliseconds,
+            double projectionMilliseconds,
+            double controllerComputeMilliseconds,
+            double commandSendMilliseconds,
+            double totalCycleMilliseconds,
+            bool hasStateTimestamp,
+            double stateTimestampSeconds,
+            bool stateTimestampChanged,
+            ParkingControlCycleResult result)
+        {
+            CycleIndex = cycleIndex;
+            CycleIntervalMilliseconds =
+                cycleIntervalMilliseconds;
+            StateReadMilliseconds = stateReadMilliseconds;
+            ProjectionMilliseconds = projectionMilliseconds;
+            ControllerComputeMilliseconds =
+                controllerComputeMilliseconds;
+            CommandSendMilliseconds = commandSendMilliseconds;
+            TotalCycleMilliseconds = totalCycleMilliseconds;
+            HasStateTimestamp = hasStateTimestamp;
+            StateTimestampSeconds = stateTimestampSeconds;
+            StateTimestampChanged = stateTimestampChanged;
+            Result = result;
+        }
+
+        public long CycleIndex { get; }
+        public double CycleIntervalMilliseconds { get; }
+        public double StateReadMilliseconds { get; }
+        public double ProjectionMilliseconds { get; }
+        public double ControllerComputeMilliseconds { get; }
+        public double CommandSendMilliseconds { get; }
+        public double TotalCycleMilliseconds { get; }
+        public bool HasStateTimestamp { get; }
+        public double StateTimestampSeconds { get; }
+        public bool StateTimestampChanged { get; }
+        public ParkingControlCycleResult Result { get; }
+    }
+
+    // 负责单车状态读取、公共轨迹计算和真实底盘命令发送。
+    public sealed class ParkingGeometricController
+    {
+        private readonly IVehicleStateProvider _stateProvider;
+        private readonly GcpCommandExecutor _commandExecutor;
+        private readonly PathTrackingCore _trackingCore;
+
+        private long _cycleIndex;
+        private bool _hasPreviousStateTimestamp;
+        private double _previousStateTimestampSeconds;
+
+        // 创建具有终点判定、轨迹偏离保护和曲率前馈预瞄的单车轨迹控制器。
+        public ParkingGeometricController(
+            IVehicleStateProvider stateProvider,
+            ILateralController lateralController,
+            ILongitudinalController longitudinalController,
+            GcpCommandAllocator gcpAllocator,
+            GcpCommandExecutor commandExecutor,
+            double finishDistanceMeters = 0.04,
+            double finishSpeedMetersPerSecond = 0.02,
+            double finishHeadingToleranceRadians =
+                3.0 * Math.PI / 180.0,
+            double maximumDistanceToTrajectoryMeters = 0.30,
+            double terminalBrakingPreviewMeters = 0.02,
+            double terminalApproachDistanceMeters = 0.10,
+            double terminalApproachGainPerSecond = 0.8,
+            double maximumTerminalApproachSpeedMetersPerSecond = 0.05,
+            double curvaturePreviewSeconds = 0.20,
+            double maximumCurvaturePreviewMeters = 0.12,
+            double motionDirectionInBodyRadians = 0.0)
+        {
+            _stateProvider = stateProvider ??
+                throw new ArgumentNullException(
+                    nameof(stateProvider));
+            _commandExecutor = commandExecutor ??
+                throw new ArgumentNullException(
+                    nameof(commandExecutor));
+            _trackingCore = new PathTrackingCore(
+                lateralController,
+                longitudinalController,
+                gcpAllocator,
+                finishDistanceMeters,
+                finishSpeedMetersPerSecond,
+                finishHeadingToleranceRadians,
+                maximumDistanceToTrajectoryMeters,
+                terminalBrakingPreviewMeters,
+                terminalApproachDistanceMeters,
+                terminalApproachGainPerSecond,
+                maximumTerminalApproachSpeedMetersPerSecond,
+                curvaturePreviewSeconds,
+                maximumCurvaturePreviewMeters,
+                motionDirectionInBodyRadians);
+        }
+
+        // 以下控制参数由公共轨迹核心统一持有。
+        public double FinishDistanceMeters =>
+            _trackingCore.FinishDistanceMeters;
+
+        public double FinishSpeedMetersPerSecond =>
+            _trackingCore.FinishSpeedMetersPerSecond;
+
+        public double FinishHeadingToleranceRadians =>
+            _trackingCore.FinishHeadingToleranceRadians;
+
+        public double MaximumDistanceToTrajectoryMeters =>
+            _trackingCore.MaximumDistanceToTrajectoryMeters;
+
+        public double TerminalBrakingPreviewMeters =>
+            _trackingCore.TerminalBrakingPreviewMeters;
+
+        public double TerminalApproachDistanceMeters =>
+            _trackingCore.TerminalApproachDistanceMeters;
+
+        public double TerminalApproachGainPerSecond =>
+            _trackingCore.TerminalApproachGainPerSecond;
+
+        public double MaximumTerminalApproachSpeedMetersPerSecond =>
+            _trackingCore.MaximumTerminalApproachSpeedMetersPerSecond;
+
+        public double CurvaturePreviewSeconds =>
+            _trackingCore.CurvaturePreviewSeconds;
+
+        public double MaximumCurvaturePreviewMeters =>
+            _trackingCore.MaximumCurvaturePreviewMeters;
+
+        public bool IsActive => _trackingCore.IsActive;
+
+        public bool IsCompleted => _trackingCore.IsCompleted;
+
+        public string LastFailureReason =>
+            _trackingCore.LastFailureReason;
+
+        public Exception LastException =>
+            _trackingCore.LastException;
+
+        // 最近一次有效车辆状态仍由单车外层保存。
+        public VehicleState? LastVehicleState { get; private set; }
+
+        public TrajectoryProjection? LastProjection =>
+            _trackingCore.LastProjection;
+
+        public GcpMotionCommand? LastRequestedCommand =>
+            _trackingCore.LastRequestedCommand;
+
+        // 经过GCP角速度限制后实际发送给底盘的最近一次命令。
+        public GcpMotionCommand? LastCommand { get; private set; }
+
+        public double? LastControlReferenceSpeedMetersPerSecond =>
+            _trackingCore.LastControlReferenceSpeedMetersPerSecond;
+
+        public double? LastCurvaturePreviewDistanceMeters =>
+            _trackingCore.LastCurvaturePreviewDistanceMeters;
+
+        public double? LastFeedforwardCurvaturePerMeter =>
+            _trackingCore.LastFeedforwardCurvaturePerMeter;
+
+        public ParkingControlCycleTiming? LastCycleTiming { get; private set; }
+
+        // 先停车，再从轨迹起点重置公共控制核心和单车执行诊断。
+        public void Start(Trajectory2D trajectory)
+        {
+            _commandExecutor.Stop();
+            _trackingCore.Start(trajectory);
+            ClearExecutionDiagnostics();
+        }
+
+        // 读取车辆状态、调用公共核心并发送一次真实底盘命令。
+        public ParkingControlCycleResult ExecuteCycle(
+            double deltaTimeSeconds)
+        {
+            NumericGuard.EnsureFinitePositive(
+                deltaTimeSeconds,
+                nameof(deltaTimeSeconds));
+
+            if (!_trackingCore.IsActive)
+            {
+                return ParkingControlCycleResult.Inactive;
+            }
+
+            var cycleIndex = ++_cycleIndex;
+            var cycleStartTimestamp =
+                Stopwatch.GetTimestamp();
+            var stateReadMilliseconds = 0.0;
+            var projectionMilliseconds = 0.0;
+            var controllerComputeMilliseconds = 0.0;
+            var commandSendMilliseconds = 0.0;
+            var hasStateTimestamp = false;
+            var stateTimestampSeconds = 0.0;
+            var stateTimestampChanged = false;
+            var cycleResult =
+                ParkingControlCycleResult.Faulted;
+
+            try
+            {
+                var stateReadStartTimestamp =
+                    Stopwatch.GetTimestamp();
+                bool stateAvailable;
+                VehicleState vehicleState;
+                try
+                {
+                    stateAvailable =
+                        _stateProvider.TryGetState(
+                            out vehicleState);
+                }
+                finally
+                {
+                    stateReadMilliseconds =
+                        GetElapsedMilliseconds(
+                            stateReadStartTimestamp);
+                }
+
+                if (!stateAvailable)
+                {
+                    var stopStartTimestamp =
+                        Stopwatch.GetTimestamp();
+                    try
+                    {
+                        StopForUnavailableState();
+                    }
+                    finally
+                    {
+                        commandSendMilliseconds =
+                            GetElapsedMilliseconds(
+                                stopStartTimestamp);
+                    }
+
+                    cycleResult = ParkingControlCycleResult
+                        .StateUnavailable;
+                    return cycleResult;
+                }
+
+                LastVehicleState = vehicleState;
+                hasStateTimestamp = true;
+                stateTimestampSeconds =
+                    vehicleState.SampleTimestampSeconds;
+                stateTimestampChanged =
+                    !_hasPreviousStateTimestamp ||
+                    stateTimestampSeconds !=
+                    _previousStateTimestampSeconds;
+                _previousStateTimestampSeconds =
+                    stateTimestampSeconds;
+                _hasPreviousStateTimestamp = true;
+
+                var output = _trackingCore.Compute(
+                    vehicleState.PoseInWorld,
+                    vehicleState.TwistInBody,
+                    vehicleState.HasValidVelocityEstimate,
+                    deltaTimeSeconds);
+                projectionMilliseconds =
+                    output.ProjectionMilliseconds;
+                controllerComputeMilliseconds =
+                    output.ControllerComputeMilliseconds;
+
+                if (output.Result ==
+                    PathTrackingCycleResult.Completed)
+                {
+                    commandSendMilliseconds =
+                        StopForCompletedTrajectory();
+                    cycleResult =
+                        ParkingControlCycleResult.Completed;
+                    return cycleResult;
+                }
+
+                if (output.Result ==
+                    PathTrackingCycleResult.Faulted)
+                {
+                    commandSendMilliseconds =
+                        StopForTrackingFault();
+                    cycleResult =
+                        ParkingControlCycleResult.Faulted;
+                    return cycleResult;
+                }
+
+                if (output.Result !=
+                        PathTrackingCycleResult.CommandGenerated ||
+                    !output.Command.HasValue)
+                {
+                    cycleResult =
+                        ParkingControlCycleResult.Inactive;
+                    return cycleResult;
+                }
+
+                var commandSendStartTimestamp =
+                    Stopwatch.GetTimestamp();
+                bool commandSucceeded;
+                try
+                {
+                    commandSucceeded =
+                        _commandExecutor.Execute(
+                            output.Command.Value,
+                            deltaTimeSeconds);
+                }
+                finally
+                {
+                    commandSendMilliseconds =
+                        GetElapsedMilliseconds(
+                            commandSendStartTimestamp);
+                }
+
+                if (!commandSucceeded)
+                {
+                    cycleResult = EnterFault(
+                        string.IsNullOrWhiteSpace(
+                            _commandExecutor.LastFailureReason)
+                            ? "GCP底盘命令执行失败。"
+                            : _commandExecutor.LastFailureReason);
+                    return cycleResult;
+                }
+
+                LastCommand =
+                    _commandExecutor.LastSentCommand;
+                cycleResult =
+                    ParkingControlCycleResult.CommandSent;
+                return cycleResult;
+            }
+            catch (Exception exception)
+            {
+                cycleResult = EnterFault(
+                    "停车机器人轨迹控制周期异常：" +
+                    exception.Message,
+                    exception);
+                return cycleResult;
+            }
+            finally
+            {
+                LastCycleTiming =
+                    new ParkingControlCycleTiming(
+                        cycleIndex,
+                        deltaTimeSeconds * 1000.0,
+                        stateReadMilliseconds,
+                        projectionMilliseconds,
+                        controllerComputeMilliseconds,
+                        commandSendMilliseconds,
+                        GetElapsedMilliseconds(
+                            cycleStartTimestamp),
+                        hasStateTimestamp,
+                        stateTimestampSeconds,
+                        stateTimestampChanged,
+                        cycleResult);
+            }
+        }
+
+        // 主动取消当前轨迹、立即停车并清除全部控制状态。
+        public void Cancel()
+        {
+            _commandExecutor.Stop();
+            _trackingCore.Cancel();
+            ClearExecutionDiagnostics();
+        }
+
+        // 状态不可用时停车并重置反馈历史，同时保留轨迹等待恢复。
+        private void StopForUnavailableState()
+        {
+            _commandExecutor.Stop();
+            _trackingCore.PauseForUnavailableState(
+                "当前无法获得有效车辆状态，底盘已停车并等待定位恢复。");
+            LastCommand = null;
+        }
+
+        // 公共核心完成轨迹后发送停车，并保留零命令供实验记录。
+        private double StopForCompletedTrajectory()
+        {
+            var startTimestamp = Stopwatch.GetTimestamp();
+            _commandExecutor.Stop();
+            LastCommand = new GcpMotionCommand(
+                0.0,
+                0.0,
+                0.0);
+            return GetElapsedMilliseconds(startTimestamp);
+        }
+
+        // 公共核心故障后只负责真实底盘停车，不覆盖核心保存的失败原因。
+        private double StopForTrackingFault()
+        {
+            var startTimestamp = Stopwatch.GetTimestamp();
+            _commandExecutor.Stop();
+            LastCommand = null;
+            return GetElapsedMilliseconds(startTimestamp);
+        }
+
+        // 将底盘执行或外层异常同步到公共核心，并立即停车。
+        private ParkingControlCycleResult EnterFault(
+            string reason,
+            Exception exception = null)
+        {
+            _commandExecutor.Stop();
+            _trackingCore.Fail(reason, exception);
+            LastCommand = null;
+            return ParkingControlCycleResult.Faulted;
+        }
+
+        // 清除只属于单车状态读取、发送和周期计时的诊断。
+        private void ClearExecutionDiagnostics()
+        {
+            _cycleIndex = 0;
+            _hasPreviousStateTimestamp = false;
+            _previousStateTimestampSeconds = 0.0;
+            LastVehicleState = null;
+            LastCommand = null;
+            LastCycleTiming = null;
+        }
+
+        private static double GetElapsedMilliseconds(
+            long startTimestamp)
+        {
+            return (Stopwatch.GetTimestamp() - startTimestamp) *
+                   1000.0 /
+                   Stopwatch.Frequency;
+        }
+    }
+}

MultiWheelC/Control/Execution/PathTrackingCore.cs

@@ -0,0 +1,793 @@
+using System;
+using System.Diagnostics;
+using MultiWheelC.Control.Abstractions;
+using MultiWheelC.Control.Allocation;
+using MultiWheelC.Trajectory;
+using MyParking.Shared;
+
+namespace MultiWheelC.Control.Execution
+{
+    // 表示公共轨迹跟踪核心单周期的计算结果，不包含底盘发送结果。
+    public enum PathTrackingCycleResult
+    {
+        Inactive = 0,
+        CommandGenerated = 1,
+        Completed = 2,
+        Faulted = 3
+    }
+
+    // 保存公共核心生成的GCP命令、轨迹投影和分阶段计算耗时。
+    public readonly struct PathTrackingCycleOutput
+    {
+        public PathTrackingCycleOutput(
+            PathTrackingCycleResult result,
+            GcpMotionCommand? command,
+            TrajectoryProjection? projection,
+            double projectionMilliseconds,
+            double controllerComputeMilliseconds)
+        {
+            Result = result;
+            Command = command;
+            Projection = projection;
+            ProjectionMilliseconds = projectionMilliseconds;
+            ControllerComputeMilliseconds =
+                controllerComputeMilliseconds;
+        }
+
+        public PathTrackingCycleResult Result { get; }
+
+        public GcpMotionCommand? Command { get; }
+
+        public TrajectoryProjection? Projection { get; }
+
+        public double ProjectionMilliseconds { get; }
+
+        public double ControllerComputeMilliseconds { get; }
+    }
+
+    // 统一处理单车和虚拟车队共有的轨迹投影、速度整形及GCP命令生成。
+    public sealed class PathTrackingCore
+    {
+        private const double ZeroReferenceSpeedToleranceMetersPerSecond =
+            1e-6;
+        private const double StartupRegionMeters = 0.02;
+        private const double StartupPreviewDistanceMeters = 0.05;
+        private const double MaximumStartupSpeedMetersPerSecond = 0.08;
+        private const double ProjectionBackwardSearchDistanceMeters =
+            0.10;
+        private const double ProjectionForwardSearchDistanceMeters =
+            1.00;
+
+        private readonly ILateralController _lateralController;
+        private readonly ILongitudinalController _longitudinalController;
+        private readonly GcpCommandAllocator _gcpAllocator;
+        private readonly double _motionDirectionInBodyRadians;
+
+        private Trajectory2D _trajectory;
+        private double _terminalTravelDirection = 1.0;
+
+        public PathTrackingCore(
+            ILateralController lateralController,
+            ILongitudinalController longitudinalController,
+            GcpCommandAllocator gcpAllocator,
+            double finishDistanceMeters = 0.04,
+            double finishSpeedMetersPerSecond = 0.02,
+            double finishHeadingToleranceRadians =
+                3.0 * Math.PI / 180.0,
+            double maximumDistanceToTrajectoryMeters = 0.30,
+            double terminalBrakingPreviewMeters = 0.02,
+            double terminalApproachDistanceMeters = 0.10,
+            double terminalApproachGainPerSecond = 0.8,
+            double maximumTerminalApproachSpeedMetersPerSecond = 0.05,
+            double curvaturePreviewSeconds = 0.20,
+            double maximumCurvaturePreviewMeters = 0.12,
+            double motionDirectionInBodyRadians = 0.0)
+        {
+            _lateralController = lateralController ??
+                throw new ArgumentNullException(
+                    nameof(lateralController));
+            _longitudinalController = longitudinalController ??
+                throw new ArgumentNullException(
+                    nameof(longitudinalController));
+            _gcpAllocator = gcpAllocator ??
+                throw new ArgumentNullException(
+                    nameof(gcpAllocator));
+
+            NumericGuard.EnsureFinitePositive(
+                finishDistanceMeters,
+                nameof(finishDistanceMeters));
+            NumericGuard.EnsureFiniteNonNegative(
+                finishSpeedMetersPerSecond,
+                nameof(finishSpeedMetersPerSecond));
+            NumericGuard.EnsureFinitePositive(
+                finishHeadingToleranceRadians,
+                nameof(finishHeadingToleranceRadians));
+            NumericGuard.EnsureFinitePositive(
+                maximumDistanceToTrajectoryMeters,
+                nameof(maximumDistanceToTrajectoryMeters));
+            NumericGuard.EnsureFiniteNonNegative(
+                terminalBrakingPreviewMeters,
+                nameof(terminalBrakingPreviewMeters));
+            NumericGuard.EnsureFinitePositive(
+                terminalApproachDistanceMeters,
+                nameof(terminalApproachDistanceMeters));
+            NumericGuard.EnsureFinitePositive(
+                terminalApproachGainPerSecond,
+                nameof(terminalApproachGainPerSecond));
+            NumericGuard.EnsureFinitePositive(
+                maximumTerminalApproachSpeedMetersPerSecond,
+                nameof(maximumTerminalApproachSpeedMetersPerSecond));
+            NumericGuard.EnsureFiniteNonNegative(
+                curvaturePreviewSeconds,
+                nameof(curvaturePreviewSeconds));
+            NumericGuard.EnsureFiniteNonNegative(
+                maximumCurvaturePreviewMeters,
+                nameof(maximumCurvaturePreviewMeters));
+            NumericGuard.EnsureFinite(
+                motionDirectionInBodyRadians,
+                nameof(motionDirectionInBodyRadians));
+
+            if (terminalApproachDistanceMeters <=
+                finishDistanceMeters)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(terminalApproachDistanceMeters),
+                    "终点单向逼近范围必须大于终点位置容差。");
+            }
+
+            FinishDistanceMeters = finishDistanceMeters;
+            FinishSpeedMetersPerSecond =
+                finishSpeedMetersPerSecond;
+            FinishHeadingToleranceRadians =
+                finishHeadingToleranceRadians;
+            MaximumDistanceToTrajectoryMeters =
+                maximumDistanceToTrajectoryMeters;
+            TerminalBrakingPreviewMeters =
+                terminalBrakingPreviewMeters;
+            TerminalApproachDistanceMeters =
+                terminalApproachDistanceMeters;
+            TerminalApproachGainPerSecond =
+                terminalApproachGainPerSecond;
+            MaximumTerminalApproachSpeedMetersPerSecond =
+                maximumTerminalApproachSpeedMetersPerSecond;
+            CurvaturePreviewSeconds = curvaturePreviewSeconds;
+            MaximumCurvaturePreviewMeters =
+                maximumCurvaturePreviewMeters;
+            _motionDirectionInBodyRadians =
+                AngleMath.NormalizeRadians(
+                    motionDirectionInBodyRadians);
+        }
+
+        public double FinishDistanceMeters { get; }
+
+        public double FinishSpeedMetersPerSecond { get; }
+
+        public double FinishHeadingToleranceRadians { get; }
+
+        public double MaximumDistanceToTrajectoryMeters { get; }
+
+        public double TerminalBrakingPreviewMeters { get; }
+
+        public double TerminalApproachDistanceMeters { get; }
+
+        public double TerminalApproachGainPerSecond { get; }
+
+        public double MaximumTerminalApproachSpeedMetersPerSecond { get; }
+
+        public double CurvaturePreviewSeconds { get; }
+
+        public double MaximumCurvaturePreviewMeters { get; }
+
+        public bool IsActive { get; private set; }
+
+        public bool IsCompleted { get; private set; }
+
+        public string LastFailureReason { get; private set; } =
+            string.Empty;
+
+        public Exception LastException { get; private set; }
+
+        public TrajectoryProjection? LastProjection { get; private set; }
+
+        public GcpMotionCommand? LastRequestedCommand { get; private set; }
+
+        public double? LastControlReferenceSpeedMetersPerSecond { get; private set; }
+
+        public double? LastCurvaturePreviewDistanceMeters { get; private set; }
+
+        public double? LastFeedforwardCurvaturePerMeter { get; private set; }
+
+        // 重置跨周期状态，并从轨迹起点开始新的跟踪过程。
+        public void Start(Trajectory2D trajectory)
+        {
+            if (trajectory == null)
+            {
+                throw new ArgumentNullException(
+                    nameof(trajectory));
+            }
+
+            var terminalTravelDirection =
+                ResolveTerminalTravelDirection(trajectory);
+
+            ResetFeedbackControllers();
+            _trajectory = trajectory;
+            _terminalTravelDirection =
+                terminalTravelDirection;
+            IsActive = true;
+            IsCompleted = false;
+            ClearDiagnostics();
+        }
+
+        // 将受控刚体的位姿和速度转换为本周期GCP命令。
+        public PathTrackingCycleOutput Compute(
+            Pose2D poseInWorld,
+            Twist2D actualTwistInBody,
+            bool hasValidVelocityEstimate,
+            double deltaTimeSeconds)
+        {
+            NumericGuard.EnsureFinite(
+                poseInWorld,
+                nameof(poseInWorld));
+            NumericGuard.EnsureFinite(
+                actualTwistInBody,
+                nameof(actualTwistInBody));
+            NumericGuard.EnsureFinitePositive(
+                deltaTimeSeconds,
+                nameof(deltaTimeSeconds));
+
+            if (!IsActive || _trajectory == null)
+            {
+                return new PathTrackingCycleOutput(
+                    PathTrackingCycleResult.Inactive,
+                    null,
+                    LastProjection,
+                    0.0,
+                    0.0);
+            }
+
+            var projectionStartTimestamp =
+                Stopwatch.GetTimestamp();
+            var projectionCompleted = false;
+            var projectionMilliseconds = 0.0;
+            var controllerComputeStartTimestamp = 0L;
+
+            try
+            {
+                var projection = LastProjection.HasValue
+                    ? TrajectoryProjector.Project(
+                        _trajectory,
+                        poseInWorld,
+                        LastProjection.Value.ArcLengthMeters,
+                        ProjectionBackwardSearchDistanceMeters,
+                        ProjectionForwardSearchDistanceMeters)
+                    : TrajectoryProjector.Project(
+                        _trajectory,
+                        poseInWorld);
+
+                projectionMilliseconds =
+                    GetElapsedMilliseconds(
+                        projectionStartTimestamp);
+                projectionCompleted = true;
+                LastProjection = projection;
+                controllerComputeStartTimestamp =
+                    Stopwatch.GetTimestamp();
+
+                if (projection.DistanceToTrajectoryMeters >
+                    MaximumDistanceToTrajectoryMeters)
+                {
+                    Fail(
+                        "受控刚体距离参考轨迹" +
+                        $"{projection.DistanceToTrajectoryMeters:F3}m，" +
+                        "超过允许值" +
+                        $"{MaximumDistanceToTrajectoryMeters:F3}m。");
+                    return CreateOutput(
+                        PathTrackingCycleResult.Faulted,
+                        null,
+                        projection,
+                        projectionMilliseconds,
+                        controllerComputeStartTimestamp);
+                }
+
+                if (HasReachedEnd(
+                        poseInWorld,
+                        actualTwistInBody,
+                        hasValidVelocityEstimate,
+                        projection))
+                {
+                    CompleteTrajectory();
+                    return CreateOutput(
+                        PathTrackingCycleResult.Completed,
+                        null,
+                        projection,
+                        projectionMilliseconds,
+                        controllerComputeStartTimestamp);
+                }
+
+                if (HasStoppedAtUnsatisfiedTerminal(
+                        poseInWorld,
+                        actualTwistInBody,
+                        hasValidVelocityEstimate,
+                        projection,
+                        out var terminalFailureReason))
+                {
+                    Fail(terminalFailureReason);
+                    return CreateOutput(
+                        PathTrackingCycleResult.Faulted,
+                        null,
+                        projection,
+                        projectionMilliseconds,
+                        controllerComputeStartTimestamp);
+                }
+
+                var controlReferenceSpeedMetersPerSecond =
+                    ResolveControlReferenceSpeed(
+                        poseInWorld,
+                        projection);
+                LastControlReferenceSpeedMetersPerSecond =
+                    controlReferenceSpeedMetersPerSecond;
+                var curvaturePreviewDistanceMeters =
+                    ResolveCurvaturePreviewDistanceMeters(
+                        actualTwistInBody,
+                        hasValidVelocityEstimate,
+                        controlReferenceSpeedMetersPerSecond);
+                LastCurvaturePreviewDistanceMeters =
+                    curvaturePreviewDistanceMeters;
+                var feedforwardCurvaturePerMeter =
+                    ResolveFeedforwardCurvaturePerMeter(
+                        projection,
+                        curvaturePreviewDistanceMeters);
+                LastFeedforwardCurvaturePerMeter =
+                    feedforwardCurvaturePerMeter;
+                var context = new PathTrackingContext(
+                    actualTwistInBody,
+                    hasValidVelocityEstimate,
+                    projection,
+                    controlReferenceSpeedMetersPerSecond,
+                    feedforwardCurvaturePerMeter,
+                    deltaTimeSeconds,
+                    _motionDirectionInBodyRadians);
+                var lateralCommand =
+                    _lateralController.Compute(context);
+                var commandSpeedMetersPerSecond =
+                    _longitudinalController
+                        .ComputeSpeedMetersPerSecond(context);
+                var command = _gcpAllocator.Allocate(
+                    commandSpeedMetersPerSecond,
+                    lateralCommand);
+
+                LastRequestedCommand = command;
+                LastFailureReason = string.Empty;
+                LastException = null;
+
+                return CreateOutput(
+                    PathTrackingCycleResult.CommandGenerated,
+                    command,
+                    projection,
+                    projectionMilliseconds,
+                    controllerComputeStartTimestamp);
+            }
+            catch (Exception exception)
+            {
+                if (!projectionCompleted)
+                {
+                    projectionMilliseconds =
+                        GetElapsedMilliseconds(
+                            projectionStartTimestamp);
+                }
+
+                Fail(
+                    "轨迹跟踪核心计算异常：" +
+                    exception.Message,
+                    exception);
+
+                return CreateOutput(
+                    PathTrackingCycleResult.Faulted,
+                    null,
+                    LastProjection,
+                    projectionMilliseconds,
+                    controllerComputeStartTimestamp);
+            }
+        }
+
+        // 状态暂不可用时重置反馈历史，但保留当前轨迹和投影进度等待恢复。
+        public void PauseForUnavailableState(string reason)
+        {
+            ResetFeedbackControllers();
+            LastRequestedCommand = null;
+            LastFailureReason = reason ?? string.Empty;
+            LastException = null;
+        }
+
+        // 将外层执行故障同步到公共核心，并终止当前轨迹。
+        public void Fail(
+            string reason,
+            Exception exception = null)
+        {
+            ResetFeedbackControllers();
+            _trajectory = null;
+            IsActive = false;
+            IsCompleted = false;
+            LastRequestedCommand = null;
+            LastFailureReason = reason ?? string.Empty;
+            LastException = exception;
+        }
+
+        // 取消当前轨迹并清除全部跟踪状态。
+        public void Cancel()
+        {
+            ResetFeedbackControllers();
+            _trajectory = null;
+            _terminalTravelDirection = 1.0;
+            IsActive = false;
+            IsCompleted = false;
+            ClearDiagnostics();
+        }
+
+        private double ResolveControlReferenceSpeed(
+            Pose2D poseInWorld,
+            TrajectoryProjection projection)
+        {
+            if (projection.RemainingDistanceMeters >
+                TerminalApproachDistanceMeters)
+            {
+                return ResolveReferenceSpeedForControl(
+                    projection);
+            }
+
+            return ResolveTerminalApproachSpeed(
+                poseInWorld);
+        }
+
+        private double ResolveCurvaturePreviewDistanceMeters(
+            Twist2D actualTwistInBody,
+            bool hasValidVelocityEstimate,
+            double controlReferenceSpeedMetersPerSecond)
+        {
+            if (CurvaturePreviewSeconds <= 0.0 ||
+                MaximumCurvaturePreviewMeters <= 0.0)
+            {
+                return 0.0;
+            }
+
+            var previewSpeedMetersPerSecond =
+                hasValidVelocityEstimate
+                    ? CalculateActualLongitudinalSpeedMetersPerSecond(
+                        actualTwistInBody)
+                    : Math.Abs(
+                        controlReferenceSpeedMetersPerSecond);
+
+            return Math.Min(
+                MaximumCurvaturePreviewMeters,
+                previewSpeedMetersPerSecond *
+                CurvaturePreviewSeconds);
+        }
+
+        private double ResolveFeedforwardCurvaturePerMeter(
+            TrajectoryProjection projection,
+            double previewDistanceMeters)
+        {
+            var previewArcLengthMeters = Math.Min(
+                _trajectory.TotalLengthMeters,
+                projection.ArcLengthMeters +
+                previewDistanceMeters);
+
+            return _trajectory
+                .SampleAtArcLength(previewArcLengthMeters)
+                .CurvaturePerMeter;
+        }
+
+        private double ResolveTerminalApproachSpeed(
+            Pose2D poseInWorld)
+        {
+            var distanceToEndMeters =
+                CalculateDistanceToEndMeters(
+                    poseInWorld);
+            var headingErrorToEndRadians =
+                CalculateHeadingErrorToEndRadians(
+                    poseInWorld);
+
+            if (distanceToEndMeters <=
+                    FinishDistanceMeters &&
+                headingErrorToEndRadians <=
+                    FinishHeadingToleranceRadians)
+            {
+                return 0.0;
+            }
+
+            var endPose = _trajectory.EndPoint.PoseInWorld;
+            var deltaX = endPose.XMeters -
+                         poseInWorld.XMeters;
+            var deltaY = endPose.YMeters -
+                         poseInWorld.YMeters;
+            var longitudinalErrorMeters =
+                deltaX * Math.Cos(endPose.YawRadians) +
+                deltaY * Math.Sin(endPose.YawRadians);
+            var remainingAlongTravelMeters =
+                _terminalTravelDirection *
+                longitudinalErrorMeters;
+
+            // 越过终点后不生成与原轨迹方向相反的修正速度。
+            if (remainingAlongTravelMeters <= 0.0)
+            {
+                return 0.0;
+            }
+
+            var speedMagnitudeMetersPerSecond =
+                Math.Min(
+                    MaximumTerminalApproachSpeedMetersPerSecond,
+                    TerminalApproachGainPerSecond *
+                    remainingAlongTravelMeters);
+
+            return _terminalTravelDirection *
+                   speedMagnitudeMetersPerSecond;
+        }
+
+        private double ResolveReferenceSpeedForControl(
+            TrajectoryProjection projection)
+        {
+            var currentReferenceSpeed =
+                ApplyTerminalBrakingPreview(
+                    projection,
+                    projection.ReferencePoint
+                        .ReferenceSpeedMetersPerSecond);
+            var isInStartupRegion =
+                projection.ArcLengthMeters <=
+                    StartupRegionMeters &&
+                projection.RemainingDistanceMeters >
+                    FinishDistanceMeters;
+
+            if (!isInStartupRegion)
+            {
+                return currentReferenceSpeed;
+            }
+
+            var previewArcLengthMeters = Math.Min(
+                _trajectory.TotalLengthMeters,
+                projection.ArcLengthMeters +
+                StartupPreviewDistanceMeters);
+            var previewReferenceSpeed =
+                _trajectory
+                    .SampleAtArcLength(previewArcLengthMeters)
+                    .ReferenceSpeedMetersPerSecond;
+
+            if (Math.Abs(previewReferenceSpeed) <=
+                ZeroReferenceSpeedToleranceMetersPerSecond)
+            {
+                return currentReferenceSpeed;
+            }
+
+            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;
+        }
+
+        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;
+        }
+
+        private static double ResolveTerminalTravelDirection(
+            Trajectory2D trajectory)
+        {
+            for (var index = trajectory.Count - 1;
+                 index >= 0;
+                 index--)
+            {
+                var referenceSpeedMetersPerSecond =
+                    trajectory[index]
+                        .ReferenceSpeedMetersPerSecond;
+
+                if (Math.Abs(referenceSpeedMetersPerSecond) >
+                    ZeroReferenceSpeedToleranceMetersPerSecond)
+                {
+                    return Math.Sign(
+                        referenceSpeedMetersPerSecond);
+                }
+            }
+
+            throw new ArgumentException(
+                "轨迹必须在终点前包含至少一个非零参考速度。",
+                nameof(trajectory));
+        }
+
+        private bool HasReachedEnd(
+            Pose2D poseInWorld,
+            Twist2D actualTwistInBody,
+            bool hasValidVelocityEstimate,
+            TrajectoryProjection projection)
+        {
+            if (!hasValidVelocityEstimate)
+            {
+                return false;
+            }
+
+            return projection.RemainingDistanceMeters <=
+                       FinishDistanceMeters &&
+                   CalculateDistanceToEndMeters(poseInWorld) <=
+                       FinishDistanceMeters &&
+                   CalculateHeadingErrorToEndRadians(poseInWorld) <=
+                       FinishHeadingToleranceRadians &&
+                   CalculateActualLongitudinalSpeedMetersPerSecond(
+                       actualTwistInBody) <=
+                       FinishSpeedMetersPerSecond;
+        }
+
+        private bool HasStoppedAtUnsatisfiedTerminal(
+            Pose2D poseInWorld,
+            Twist2D actualTwistInBody,
+            bool hasValidVelocityEstimate,
+            TrajectoryProjection projection,
+            out string failureReason)
+        {
+            failureReason = string.Empty;
+
+            var isTerminalZeroSpeedReference =
+                projection.RemainingDistanceMeters <=
+                    FinishDistanceMeters &&
+                Math.Abs(
+                    projection.ReferencePoint
+                        .ReferenceSpeedMetersPerSecond) <=
+                    ZeroReferenceSpeedToleranceMetersPerSecond;
+
+            if (!isTerminalZeroSpeedReference ||
+                !hasValidVelocityEstimate ||
+                CalculateActualLongitudinalSpeedMetersPerSecond(
+                    actualTwistInBody) >
+                FinishSpeedMetersPerSecond)
+            {
+                return false;
+            }
+
+            var positionErrorMeters =
+                CalculateDistanceToEndMeters(
+                    poseInWorld);
+            var headingErrorRadians =
+                CalculateHeadingErrorToEndRadians(
+                    poseInWorld);
+
+            failureReason =
+                "受控刚体已在终点零速参考处停稳，但终点精度不满足要求：" +
+                $"位置误差={positionErrorMeters:F3}m，" +
+                "航向误差=" +
+                $"{AngleMath.RadiansToDegrees(headingErrorRadians):F2}°。";
+            return true;
+        }
+
+        private double CalculateDistanceToEndMeters(
+            Pose2D poseInWorld)
+        {
+            var endPose = _trajectory.EndPoint.PoseInWorld;
+            var deltaX = poseInWorld.XMeters -
+                         endPose.XMeters;
+            var deltaY = poseInWorld.YMeters -
+                         endPose.YMeters;
+
+            return Math.Sqrt(
+                deltaX * deltaX +
+                deltaY * deltaY);
+        }
+
+        private double CalculateHeadingErrorToEndRadians(
+            Pose2D poseInWorld)
+        {
+            return Math.Abs(
+                AngleMath.ShortestDifferenceRadians(
+                    _trajectory.EndPoint
+                        .PoseInWorld.YawRadians,
+                    poseInWorld.YawRadians));
+        }
+
+        private double CalculateActualLongitudinalSpeedMetersPerSecond(
+            Twist2D actualTwistInBody)
+        {
+            return Math.Abs(
+                Math.Cos(_motionDirectionInBodyRadians) *
+                    actualTwistInBody.VxMetersPerSecond +
+                Math.Sin(_motionDirectionInBodyRadians) *
+                    actualTwistInBody.VyMetersPerSecond);
+        }
+
+        private void CompleteTrajectory()
+        {
+            ResetFeedbackControllers();
+            _trajectory = null;
+            IsActive = false;
+            IsCompleted = true;
+            LastRequestedCommand = new GcpMotionCommand(
+                0.0,
+                0.0,
+                0.0);
+            LastFailureReason = string.Empty;
+            LastException = null;
+        }
+
+        private void ResetFeedbackControllers()
+        {
+            _lateralController.Reset();
+            _longitudinalController.Reset();
+        }
+
+        private void ClearDiagnostics()
+        {
+            LastProjection = null;
+            LastRequestedCommand = null;
+            LastControlReferenceSpeedMetersPerSecond = null;
+            LastCurvaturePreviewDistanceMeters = null;
+            LastFeedforwardCurvaturePerMeter = null;
+            LastFailureReason = string.Empty;
+            LastException = null;
+        }
+
+        private static PathTrackingCycleOutput CreateOutput(
+            PathTrackingCycleResult result,
+            GcpMotionCommand? command,
+            TrajectoryProjection? projection,
+            double projectionMilliseconds,
+            long controllerComputeStartTimestamp)
+        {
+            var controllerComputeMilliseconds =
+                controllerComputeStartTimestamp == 0L
+                    ? 0.0
+                    : GetElapsedMilliseconds(
+                        controllerComputeStartTimestamp);
+
+            return new PathTrackingCycleOutput(
+                result,
+                command,
+                projection,
+                projectionMilliseconds,
+                controllerComputeMilliseconds);
+        }
+
+        private static double GetElapsedMilliseconds(
+            long startTimestamp)
+        {
+            return (Stopwatch.GetTimestamp() - startTimestamp) *
+                   1000.0 /
+                   Stopwatch.Frequency;
+        }
+    }
+}

MultiWheelC/Control/Lateral/StanleyLateralController.cs

@@ -0,0 +1,254 @@
+using System;
+using MultiWheelC.Control.Abstractions;
+
+namespace MultiWheelC.Control.Lateral
+{
+    /// <summary>
+    /// 将参考曲率、横向误差和航向误差分别转换为前、后GCP目标转角。
+    /// </summary>
+    public sealed class StanleyLateralController : ILateralController
+    {
+        /// <summary>
+        /// 创建使用指定GCP几何、Stanley增益和转角保护参数的横向控制器。
+        /// </summary>
+        public StanleyLateralController(
+            double controlPointRadiusMeters,
+            double crossTrackGainPerSecond,
+            double headingErrorGain,
+            double minimumSpeedMetersPerSecond,
+            bool useActualSpeedForGain = true,
+            double maximumCrossTrackCorrectionRadians =
+                10.0 * Math.PI / 180.0,
+            double maximumHeadingCorrectionRadians =
+                10.0 * Math.PI / 180.0)
+        {
+            EnsureFinitePositive(
+                controlPointRadiusMeters,
+                nameof(controlPointRadiusMeters));
+            EnsureFiniteNonNegative(
+                crossTrackGainPerSecond,
+                nameof(crossTrackGainPerSecond));
+            EnsureFiniteNonNegative(
+                headingErrorGain,
+                nameof(headingErrorGain));
+            EnsureFinitePositive(
+                minimumSpeedMetersPerSecond,
+                nameof(minimumSpeedMetersPerSecond));
+            EnsureFinitePositive(
+                maximumCrossTrackCorrectionRadians,
+                nameof(maximumCrossTrackCorrectionRadians));
+            EnsureFinitePositive(
+                maximumHeadingCorrectionRadians,
+                nameof(maximumHeadingCorrectionRadians));
+
+            ControlPointRadiusMeters = controlPointRadiusMeters;
+            CrossTrackGainPerSecond = crossTrackGainPerSecond;
+            HeadingErrorGain = headingErrorGain;
+            MinimumSpeedMetersPerSecond = minimumSpeedMetersPerSecond;
+            UseActualSpeedForGain = useActualSpeedForGain;
+            MaximumCrossTrackCorrectionRadians =
+                maximumCrossTrackCorrectionRadians;
+            MaximumHeadingCorrectionRadians =
+                maximumHeadingCorrectionRadians;
+        }
+
+        /// <summary>
+        /// 获取车体中心到前、后GCP的距离，单位为m。
+        /// </summary>
+        public double ControlPointRadiusMeters { get; }
+
+        /// <summary>
+        /// 获取横向误差增益，单位为1/s。
+        /// </summary>
+        public double CrossTrackGainPerSecond { get; }
+
+        /// <summary>
+        /// 获取航向误差的无量纲增益。
+        /// </summary>
+        public double HeadingErrorGain { get; }
+
+        /// <summary>
+        /// 获取Stanley分母使用的最小速度绝对值，单位为m/s。
+        /// </summary>
+        public double MinimumSpeedMetersPerSecond { get; }
+
+        /// <summary>
+        /// 获取是否优先使用当前状态源提供的实际纵向速度计算横向修正。
+        /// </summary>
+        public bool UseActualSpeedForGain { get; }
+
+        /// <summary>
+        /// 获取横向误差共同转角分量的最大绝对值，单位为rad。
+        /// </summary>
+        public double MaximumCrossTrackCorrectionRadians { get; }
+
+        /// <summary>
+        /// 获取航向误差差动转角分量的最大绝对值，单位为rad。
+        /// </summary>
+        public double MaximumHeadingCorrectionRadians { get; }
+
+        /// <summary>
+        /// 分别计算横向共同转角以及曲率和航向差动转角，并生成前后GCP命令。
+        /// </summary>
+        public LateralControlCommand Compute(
+            PathTrackingContext context)
+        {
+            var speedForGain = SelectSpeedForGain(context);
+            var speedMagnitude = Math.Max(
+                Math.Abs(speedForGain),
+                MinimumSpeedMetersPerSecond);
+            var travelDirection = SelectTravelDirection(context);
+
+            // 参考曲率按轨迹点序的实际行进方向定义；倒车时底盘有符号
+            // 纵向速度反向，因此GCP曲率前馈也必须反向才能保持相同几何曲率。
+            var feedforwardAngleRadians =
+                travelDirection *
+                Math.Atan(
+                    context.FeedforwardCurvaturePerMeter *
+                    ControlPointRadiusMeters);
+
+            // 横向误差生成前后同向的共同转角，使四舵轮车辆平稳靠近轨迹。
+            var crossTrackCorrectionRadians =
+                ClampSymmetric(
+                    Math.Atan(
+                        CrossTrackGainPerSecond *
+                        context.LateralErrorMeters /
+                        speedMagnitude),
+                    MaximumCrossTrackCorrectionRadians);
+
+            // 航向误差生成前后反向的差动转角，只负责调整车身朝向。
+            var headingCorrectionRadians =
+                ClampSymmetric(
+                    HeadingErrorGain *
+                    context.HeadingErrorRadians,
+                    MaximumHeadingCorrectionRadians);
+
+            // 横向误差已经按轨迹执行点序定义；倒车轨迹的点序会自然
+            // 翻转横向轴，因此共同转角不能再按行驶方向重复反号。
+            var commonAngleRadians =
+                crossTrackCorrectionRadians;
+            var differentialAngleRadians =
+                feedforwardAngleRadians +
+                travelDirection *
+                headingCorrectionRadians;
+
+            return new LateralControlCommand(
+                commonAngleRadians +
+                differentialAngleRadians,
+                commonAngleRadians -
+                differentialAngleRadians);
+        }
+
+        /// <summary>
+        /// 清除横向控制器状态；当前Stanley实现没有跨周期状态。
+        /// </summary>
+        public void Reset()
+        {
+        }
+
+        /// <summary>
+        /// 选择Stanley横向误差项使用的实际速度或参考速度。
+        /// </summary>
+        private double SelectSpeedForGain(
+            PathTrackingContext context)
+        {
+            if (UseActualSpeedForGain &&
+                context.HasValidVelocityEstimate)
+            {
+                return context
+                    .ActualLongitudinalSpeedMetersPerSecond;
+            }
+
+            return context.ControlReferenceSpeedMetersPerSecond;
+        }
+
+        /// <summary>
+        /// 根据有符号参考速度确定前进或倒车时的反馈修正方向。
+        /// </summary>
+        private static double SelectTravelDirection(
+            PathTrackingContext context)
+        {
+            const double directionDeadbandMetersPerSecond = 1e-6;
+
+            if (Math.Abs(context.ControlReferenceSpeedMetersPerSecond) >
+                directionDeadbandMetersPerSecond)
+            {
+                return Math.Sign(
+                    context.ControlReferenceSpeedMetersPerSecond);
+            }
+
+            if (context.HasValidVelocityEstimate &&
+                Math.Abs(
+                    context.ActualLongitudinalSpeedMetersPerSecond) >
+                directionDeadbandMetersPerSecond)
+            {
+                return Math.Sign(
+                    context.ActualLongitudinalSpeedMetersPerSecond);
+            }
+
+            return 1.0;
+        }
+
+        /// <summary>
+        /// 将数值按正负对称方式限制在指定绝对值内。
+        /// </summary>
+        private static double ClampSymmetric(
+            double value,
+            double maximumAbsoluteValue)
+        {
+            return Math.Max(
+                -maximumAbsoluteValue,
+                Math.Min(maximumAbsoluteValue, value));
+        }
+
+        /// <summary>
+        /// 检查控制参数是否为正有限值。
+        /// </summary>
+        private static void EnsureFinitePositive(
+            double value,
+            string parameterName)
+        {
+            EnsureFinite(value, parameterName);
+
+            if (value <= 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "Stanley控制器的几何尺寸、速度和角度限制必须是正有限值。");
+            }
+        }
+
+        /// <summary>
+        /// 检查控制增益是否为非负有限值。
+        /// </summary>
+        private static void EnsureFiniteNonNegative(
+            double value,
+            string parameterName)
+        {
+            EnsureFinite(value, parameterName);
+
+            if (value < 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "Stanley控制增益必须是非负有限值。");
+            }
+        }
+
+        /// <summary>
+        /// 检查控制参数是否为有限值。
+        /// </summary>
+        private static void EnsureFinite(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "Stanley控制参数必须是有限值。");
+            }
+        }
+    }
+}

MultiWheelC/Control/Longitudinal/PidLongitudinalController.cs

@@ -0,0 +1,224 @@
+using System;
+using MultiWheelC.Control.Abstractions;
+using MultiWheelC.Control.Common;
+
+namespace MultiWheelC.Control.Longitudinal
+{
+    /// <summary>
+    /// 将轨迹参考速度前馈与通用PID速度反馈组合为有符号底盘命令速度。
+    /// </summary>
+    public sealed class PidLongitudinalController
+        : ILongitudinalController
+    {
+        private const double ReferenceStopDeadbandMetersPerSecond =
+            1e-6;
+
+        private readonly PidController _feedbackPid;
+
+        /// <summary>
+        /// 创建具有积分抗饱和和命令速度限幅的纵向速度外环。
+        /// </summary>
+        public PidLongitudinalController(
+            double proportionalGain,
+            double integralGainPerSecond,
+            double derivativeGainSeconds,
+            double maximumIntegralCorrectionMetersPerSecond,
+            double maximumCommandSpeedMetersPerSecond,
+            double speedErrorDeadbandMetersPerSecond = 0.025)
+        {
+            EnsureFinitePositive(
+                maximumCommandSpeedMetersPerSecond,
+                nameof(maximumCommandSpeedMetersPerSecond));
+            EnsureFiniteNonNegative(
+                speedErrorDeadbandMetersPerSecond,
+                nameof(speedErrorDeadbandMetersPerSecond));
+
+            _feedbackPid = new PidController(
+                proportionalGain,
+                integralGainPerSecond,
+                derivativeGainSeconds,
+                maximumIntegralCorrectionMetersPerSecond,
+                derivativeOnMeasurement: true);
+            MaximumCommandSpeedMetersPerSecond =
+                maximumCommandSpeedMetersPerSecond;
+            SpeedErrorDeadbandMetersPerSecond =
+                speedErrorDeadbandMetersPerSecond;
+        }
+
+        /// <summary>
+        /// 获取负责计算速度误差修正量的通用PID控制器。
+        /// </summary>
+        public PidController FeedbackPid => _feedbackPid;
+
+        /// <summary>
+        /// 获取底盘命令速度的最大绝对值，单位为m/s。
+        /// </summary>
+        public double MaximumCommandSpeedMetersPerSecond { get; }
+
+        /// <summary>
+        /// 获取不触发纵向PID修正的速度误差死区，单位为m/s。
+        /// </summary>
+        public double SpeedErrorDeadbandMetersPerSecond { get; }
+
+        /// <summary>
+        /// 获取最近一次有效控制周期的参考速度减实际速度，单位为m/s。
+        /// </summary>
+        public double LastSpeedErrorMetersPerSecond =>
+            _feedbackPid.LastError;
+
+        /// <summary>
+        /// 获取最近一次比例项产生的速度修正，单位为m/s。
+        /// </summary>
+        public double LastProportionalCorrectionMetersPerSecond =>
+            _feedbackPid.LastProportionalOutput;
+
+        /// <summary>
+        /// 获取最近一次积分项产生的速度修正，单位为m/s。
+        /// </summary>
+        public double LastIntegralCorrectionMetersPerSecond =>
+            _feedbackPid.LastIntegralOutput;
+
+        /// <summary>
+        /// 获取最近一次微分项产生的速度修正，单位为m/s。
+        /// </summary>
+        public double LastDerivativeCorrectionMetersPerSecond =>
+            _feedbackPid.LastDerivativeOutput;
+
+        /// <summary>
+        /// 根据本周期控制参考速度和实际纵向速度计算底盘命令速度。
+        /// </summary>
+        public double ComputeSpeedMetersPerSecond(
+            PathTrackingContext context)
+        {
+            var controlReferenceSpeedMetersPerSecond =
+                context.ControlReferenceSpeedMetersPerSecond;
+
+            // 轨迹明确要求停车时直接输出零，防止速度反馈使车辆在终点反向纠偏。
+            if (Math.Abs(controlReferenceSpeedMetersPerSecond) <=
+                ReferenceStopDeadbandMetersPerSecond)
+            {
+                Reset();
+                return 0.0;
+            }
+
+            // 定位速度尚不可用时只透传参考速度，不使用无效反馈更新PID状态。
+            if (!context.HasValidVelocityEstimate)
+            {
+                Reset();
+                return LimitReferenceSpeed(
+                    controlReferenceSpeedMetersPerSecond);
+            }
+
+            var speedErrorMetersPerSecond =
+                controlReferenceSpeedMetersPerSecond -
+                context.ActualLongitudinalSpeedMetersPerSecond;
+
+            // Detour差分速度在参考速度附近会有小幅波动；死区内只使用速度前馈，
+            // 同时清除PID历史，避免噪声持续积累后产生突发修正。
+            if (Math.Abs(speedErrorMetersPerSecond) <=
+                SpeedErrorDeadbandMetersPerSecond)
+            {
+                Reset();
+                return LimitReferenceSpeed(
+                    controlReferenceSpeedMetersPerSecond);
+            }
+
+            GetCorrectionOutputRange(
+                controlReferenceSpeedMetersPerSecond,
+                out var minimumCorrectionMetersPerSecond,
+                out var maximumCorrectionMetersPerSecond);
+
+            var correctionMetersPerSecond =
+                _feedbackPid.Update(
+                    controlReferenceSpeedMetersPerSecond,
+                    context
+                        .ActualLongitudinalSpeedMetersPerSecond,
+                    context.DeltaTimeSeconds,
+                    minimumCorrectionMetersPerSecond,
+                    maximumCorrectionMetersPerSecond);
+
+            return controlReferenceSpeedMetersPerSecond +
+                   correctionMetersPerSecond;
+        }
+
+        /// <summary>
+        /// 清除纵向速度外环的积分、历史测量值和诊断输出。
+        /// </summary>
+        public void Reset()
+        {
+            _feedbackPid.Reset();
+        }
+
+        /// <summary>
+        /// 根据参考行驶方向计算PID修正量允许使用的动态输出范围。
+        /// </summary>
+        private void GetCorrectionOutputRange(
+            double referenceSpeedMetersPerSecond,
+            out double minimumCorrectionMetersPerSecond,
+            out double maximumCorrectionMetersPerSecond)
+        {
+            if (referenceSpeedMetersPerSecond > 0.0)
+            {
+                minimumCorrectionMetersPerSecond =
+                    -referenceSpeedMetersPerSecond;
+                maximumCorrectionMetersPerSecond =
+                    MaximumCommandSpeedMetersPerSecond -
+                    referenceSpeedMetersPerSecond;
+                return;
+            }
+
+            minimumCorrectionMetersPerSecond =
+                -MaximumCommandSpeedMetersPerSecond -
+                referenceSpeedMetersPerSecond;
+            maximumCorrectionMetersPerSecond =
+                -referenceSpeedMetersPerSecond;
+        }
+
+        /// <summary>
+        /// 在没有有效速度反馈时限制参考速度的绝对值。
+        /// </summary>
+        private double LimitReferenceSpeed(
+            double referenceSpeedMetersPerSecond)
+        {
+            return Math.Max(
+                -MaximumCommandSpeedMetersPerSecond,
+                Math.Min(
+                    MaximumCommandSpeedMetersPerSecond,
+                    referenceSpeedMetersPerSecond));
+        }
+
+        /// <summary>
+        /// 检查最大命令速度是否为正有限值。
+        /// </summary>
+        private static void EnsureFinitePositive(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value) ||
+                value <= 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "纵向控制器最大命令速度必须是正有限值。");
+            }
+        }
+
+        /// <summary>
+        /// 检查速度误差死区是否为非负有限值。
+        /// </summary>
+        private static void EnsureFiniteNonNegative(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value) ||
+                value < 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "纵向控制器速度误差死区必须是非负有限值。");
+            }
+        }
+    }
+}

MultiWheelC/CrabMotionFrameTracker.cs

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

MultiWheelC/Experiments/CompositeMotionPlanTests.cs

@@ -0,0 +1,381 @@
+using System;
+using System.Drawing;
+using System.Numerics;
+using ClumsyCore;
+using ClumsyCore.DTools;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using CommonUsage.Chassis;
+using MultiWheelC.Control.Execution;
+using MultiWheelC.StateEstimation;
+using MultiWheelC.Trajectory;
+using MyParking.Shared;
+
+namespace MultiWheelC
+{
+    /// <summary>
+    /// 测试平滑左转轨迹、停车原地左转90°和再次直行的组合运动执行过程。
+    /// </summary>
+    [MovementTest(name = "新版控制器：直线-圆弧-折线组合测试")]
+    public sealed class CompositeStopTurnGoTest : MovementTest
+    {
+        private const float MillimetersPerMeter = 1000f;
+
+        private readonly Painter _painter =
+            UI.GetPainter("CompositeStopTurnGoTest");
+
+        private DriveTask _task;
+        private TrackingExperimentRecorder _recorder;
+
+        public int TrialNumber = 1; // 重复实验编号。
+        public double StraightLengthMeters = 2.0; // 圆弧前后直线长度，单位m。
+        public double TurnRadiusMeters = 2.0; // 平滑左转名义半径，单位m。
+        public double TurnAngleDegrees = 90.0; // 含过渡段在内的总左转角度。
+        public double CurvatureTransitionLengthMeters = 0.80; // 单侧过渡长度，单位m。
+        public double InPlaceLeftTurnDegrees = 90.0; // 停车后的原地左转角度。
+        public double FinalStraightLengthMeters = 4.5; // 自转后的直线长度，单位m。
+        public double StraightMaximumSpeedMetersPerSecond = 0.40; // 直线限速。
+        public double CurveMaximumSpeedMetersPerSecond = 0.30; // 转弯和过渡段限速。
+        public double AccelerationMetersPerSecondSquared = 0.20; // 参考加速度。
+        public double DecelerationMetersPerSecondSquared = 0.08; // 参考减速度。
+        public double PointSpacingMeters = 0.02; // 离散轨迹点间距。
+
+        /// <summary>
+        /// 从当前Detour位姿构造完整计划并依次执行连续跟踪、原地自转和最终直线。
+        /// </summary>
+        public override void Test()
+        {
+            if (_task != null)
+            {
+                Console.WriteLine(
+                    "曲线-停车自转-直线组合测试已经在运行。");
+                return;
+            }
+
+            if (!TrajectoryExperimentInput
+                    .TryReadLateralOffsetMeters(
+                        out var lateralOffsetMeters))
+            {
+                return;
+            }
+
+            var chassis = PilotDefinition.Chassis as MultiWheelChassis;
+            if (chassis == null)
+            {
+                Console.WriteLine(
+                    "当前底盘不是MultiWheelChassis，无法执行组合运动测试。");
+                return;
+            }
+
+            var stateProvider =
+                ParkingVehicleStateProviderFactory.Create(
+                    chassis);
+            if (!stateProvider.TryGetState(
+                    out var initialState))
+            {
+                Console.WriteLine(
+                    "无法读取组合运动起点状态：" +
+                    stateProvider.LastFailureReason);
+                return;
+            }
+
+            var planStartPose =
+                TrajectoryExperimentInput.OffsetPoseLaterally(
+                    initialState.PoseInWorld,
+                    lateralOffsetMeters);
+            var firstTrajectory =
+                TestTrajectoryFactory
+                    .CreateStraightSmoothLeftTurnStraight(
+                        planStartPose,
+                        StraightLengthMeters,
+                        TurnRadiusMeters,
+                        AngleMath.DegreesToRadians(
+                            TurnAngleDegrees),
+                        CurvatureTransitionLengthMeters,
+                        StraightMaximumSpeedMetersPerSecond,
+                        CurveMaximumSpeedMetersPerSecond,
+                        AccelerationMetersPerSecondSquared,
+                        DecelerationMetersPerSecondSquared,
+                        PointSpacingMeters);
+
+            var firstStopPose =
+                firstTrajectory.EndPoint.PoseInWorld;
+            var finalStraightYawRadians =
+                AngleMath.NormalizeRadians(
+                    firstStopPose.YawRadians +
+                    AngleMath.DegreesToRadians(
+                        InPlaceLeftTurnDegrees));
+            var finalStraightStartPose =
+                new Pose2D(
+                    firstStopPose.XMeters,
+                    firstStopPose.YMeters,
+                    finalStraightYawRadians);
+            var finalTrajectory =
+                TestTrajectoryFactory.CreateStraight(
+                    finalStraightStartPose,
+                    FinalStraightLengthMeters,
+                    StraightMaximumSpeedMetersPerSecond,
+                    AccelerationMetersPerSecondSquared,
+                    DecelerationMetersPerSecondSquared,
+                    PointSpacingMeters);
+
+            DrawPlan(
+                firstTrajectory,
+                finalTrajectory,
+                firstStopPose);
+
+            var plan = new MotionPlanSegment[]
+            {
+                new TrackMotionPlanSegment(firstTrajectory)
+                {
+                    // 中间停车点允许后续原地转向和末段跟踪继续收敛位置误差。
+                    FinishDistanceMeters = 0.05,
+                    FinishSpeedMetersPerSecond = 0.03,
+                    FinishHeadingToleranceRadians =
+                        AngleMath.DegreesToRadians(3.0)
+                },
+                new RotateInPlaceMotionPlanSegment(
+                    finalStraightYawRadians),
+                new TrackMotionPlanSegment(finalTrajectory)
+            };
+
+            _recorder = new TrackingExperimentRecorder(
+                controllerName: "NewStanleyPidComposite",
+                trajectoryName:
+                    TrajectoryExperimentInput.BuildTrajectoryName(
+                        "SmoothTurnStopRotateStraight",
+                        lateralOffsetMeters),
+                trialNumber: TrialNumber,
+                referenceStart: ToMillimeterVector(
+                    firstTrajectory.StartPoint.PoseInWorld),
+                referenceEnd: ToMillimeterVector(
+                    finalTrajectory.EndPoint.PoseInWorld),
+                referenceSpeed:
+                    (float)StraightMaximumSpeedMetersPerSecond,
+                sampleIntervalMs: 50,
+                referenceAccelerationMetersPerSecondSquared:
+                    (float)AccelerationMetersPerSecondSquared,
+                referenceDecelerationMetersPerSecondSquared:
+                    (float)DecelerationMetersPerSecondSquared,
+                diagnosticChassis: chassis,
+                diagnosticStateProvider: stateProvider);
+            _recorder.Start();
+
+            var controlPointRadiusMeters =
+                chassis.ControlPointRadius /
+                MillimetersPerMeter;
+            var movement = new MotionPlanExecutor
+            {
+                Segments = plan,
+                StateProvider = stateProvider,
+                SegmentStarted = (index, segment) =>
+                {
+                    _recorder?.ClearControlReference();
+                    _recorder?.ClearGcpCommand();
+                    _recorder?.UpdateCommand(0f, 0f);
+                    Console.WriteLine(
+                        $"组合运动开始第{index + 1}段：" +
+                        segment.GetType().Name);
+                },
+                TrackingCycleObserver = (index, controller) =>
+                    RecordTrackingCycle(
+                        index,
+                        controller,
+                        controlPointRadiusMeters,
+                        stateProvider),
+                RotationCommandObserver = (index, omega) =>
+                    _recorder?.UpdateCommand(
+                        0f,
+                        (float)omega)
+            };
+
+            try
+            {
+                _task = new DriveTask(movement.Get());
+                _task.Wait();
+            }
+            finally
+            {
+                _task?.Stop();
+                _recorder?.UpdateCommand(0f, 0f);
+                _recorder?.StopAndSave();
+                _task = null;
+                _recorder = null;
+                _painter?.Clear();
+            }
+        }
+
+        /// <summary>
+        /// 停止组合运动、保存已有实验数据并清除计划轨迹。
+        /// </summary>
+        public override void TestStop()
+        {
+            _task?.Stop();
+            _recorder?.UpdateCommand(0f, 0f);
+            _recorder?.StopAndSave();
+            _task = null;
+            _recorder = null;
+            _painter?.Clear();
+        }
+
+        /// <summary>
+        /// 将两段轨迹和中间原地自转位置绘制到Clumsy界面。
+        /// </summary>
+        private void DrawPlan(
+            Trajectory2D firstTrajectory,
+            Trajectory2D finalTrajectory,
+            Pose2D rotationPoseInWorld)
+        {
+            _painter.Clear();
+            DrawTrajectory(
+                firstTrajectory,
+                Color.DeepSkyBlue);
+            DrawTrajectory(
+                finalTrajectory,
+                Color.Gold);
+
+            var rotationPoint =
+                ToMillimeterVector(rotationPoseInWorld);
+            _painter.DrawDot(
+                Color.Magenta,
+                rotationPoint.X,
+                rotationPoint.Y,
+                10f);
+        }
+
+        /// <summary>
+        /// 绘制一段离散世界坐标系轨迹。
+        /// </summary>
+        private void DrawTrajectory(
+            Trajectory2D trajectory,
+            Color color)
+        {
+            for (var index = 0;
+                 index < trajectory.Count;
+                 index++)
+            {
+                var point = ToMillimeterVector(
+                    trajectory[index].PoseInWorld);
+                _painter.DrawDot(
+                    color,
+                    point.X,
+                    point.Y,
+                    3f);
+
+                if (index == 0)
+                {
+                    continue;
+                }
+
+                var previousPoint = ToMillimeterVector(
+                    trajectory[index - 1].PoseInWorld);
+                _painter.DrawLine(
+                    color,
+                    previousPoint.X,
+                    previousPoint.Y,
+                    point.X,
+                    point.Y,
+                    width: 2);
+            }
+        }
+
+        /// <summary>
+        /// 将轨迹控制周期使用的状态、参考误差和最终GCP命令写入记录器。
+        /// </summary>
+        private void RecordTrackingCycle(
+            int motionSegmentIndex,
+            ParkingGeometricController controller,
+            double controlPointRadiusMeters,
+            WheelFeedbackVehicleStateProvider stateProvider)
+        {
+            if (controller.LastCycleTiming.HasValue)
+            {
+                _recorder?.RecordControlCycleTiming(
+                    controller.LastCycleTiming.Value,
+                    motionSegmentIndex,
+                    controller.LastRequestedCommand,
+                    controller.LastCommand);
+            }
+
+            if (controller.LastVehicleState.HasValue)
+            {
+                _recorder?.UpdateProcessedState(
+                    controller.LastVehicleState.Value);
+            }
+
+            if (stateProvider.TryGetLatestVelocityDiagnostics(
+                    out var detourBodyVx,
+                    out var detourVelocityValid,
+                    out var rawWheelBodyVx,
+                    out var filteredWheelBodyVx,
+                    out var rawWheelBodyVy,
+                    out var filteredWheelBodyVy,
+                    out var wheelVelocityValid))
+            {
+                _recorder?.UpdateVelocityDiagnostics(
+                    detourBodyVx,
+                    detourVelocityValid,
+                    rawWheelBodyVx,
+                    filteredWheelBodyVx,
+                    rawWheelBodyVy,
+                    filteredWheelBodyVy,
+                    wheelVelocityValid);
+            }
+
+            if (!controller.LastCommand.HasValue)
+            {
+                return;
+            }
+
+            if (controller.LastProjection.HasValue &&
+                controller.LastControlReferenceSpeedMetersPerSecond.HasValue)
+            {
+                var projection =
+                    controller.LastProjection.Value;
+                _recorder?.UpdateControlReference(
+                    projection.ArcLengthMeters,
+                    controller
+                        .LastControlReferenceSpeedMetersPerSecond.Value,
+                    projection.LateralErrorMeters,
+                    projection.HeadingErrorRadians,
+                    projection.DistanceToTrajectoryMeters,
+                    projection.RemainingDistanceMeters,
+                    controller.LastCurvaturePreviewDistanceMeters ?? 0.0,
+                    controller.LastFeedforwardCurvaturePerMeter ??
+                    projection.ReferencePoint.CurvaturePerMeter);
+            }
+
+            var requestedCommand =
+                controller.LastRequestedCommand ??
+                controller.LastCommand.Value;
+            var command = controller.LastCommand.Value;
+            _recorder?.UpdateGcpCommand(
+                requestedCommand.FrontAngleRadians,
+                requestedCommand.RearAngleRadians,
+                command.FrontAngleRadians,
+                command.RearAngleRadians);
+            var curvaturePerMeter = Math.Tan(
+                command.FrontAngleRadians) /
+                controlPointRadiusMeters;
+            var angularSpeedRadiansPerSecond =
+                command.SpeedMetersPerSecond *
+                curvaturePerMeter;
+            _recorder?.UpdateCommand(
+                (float)command.SpeedMetersPerSecond,
+                (float)angularSpeedRadiansPerSecond);
+        }
+
+        /// <summary>
+        /// 将米制世界位姿转换为Clumsy绘图和记录使用的毫米坐标。
+        /// </summary>
+        private static Vector2 ToMillimeterVector(
+            Pose2D poseInWorld)
+        {
+            return new Vector2(
+                (float)(poseInWorld.XMeters *
+                    MillimetersPerMeter),
+                (float)(poseInWorld.YMeters *
+                    MillimetersPerMeter));
+        }
+    }
+}

MultiWheelC/Experiments/DetourStaticDiagnosticTest.cs

@@ -0,0 +1,696 @@
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.Globalization;
+using System.IO;
+using System.Text;
+using System.Threading;
+using ClumsyCore;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using CommonUsage.Chassis;
+using FundamentalLib;
+using MyParking.Shared;
+
+namespace MultiWheelC
+{
+    /// <summary>
+    /// 从C层测试界面启动定时的Detour静态定位与轮组反馈诊断记录。
+    /// </summary>
+    [MovementTest(name = "诊断：Detour静态定位记录")]
+    public sealed class DetourStaticDiagnosticTest : MovementTest
+    {
+        private sealed class DiagnosticSample
+        {
+            public double ElapsedSeconds;
+            public string LocalTimestamp;
+            public bool DetourReadSucceeded;
+            public double DetourCallDurationMilliseconds;
+            public long? DetourTickRaw;
+            public string DetourTimestamp;
+            public bool DetourTimestampValid;
+            public double? DetourDataAgeMilliseconds;
+            public double? DetourLStep;
+            public double? DetourXMillimeters;
+            public double? DetourYMillimeters;
+            public double? DetourYawDegrees;
+            public double? LocalDeltaMilliseconds;
+            public double? DetourTickDeltaMilliseconds;
+            public double? DeltaXMillimeters;
+            public double? DeltaYMillimeters;
+            public double? DeltaYawDegrees;
+            public double? DeltaPositionMillimeters;
+            public bool IsRepeatedTick;
+            public bool IsRepeatedPose;
+            public bool IsOutOfOrderTick;
+            public bool WheelReadSucceeded;
+            public double? WheelBodyVxMetersPerSecond;
+            public double? WheelBodyVyMetersPerSecond;
+            public double? WheelBodyOmegaDegreesPerSecond;
+            public double? ActualSteerLeftFrontDegrees;
+            public double? ActualSteerLeftRearDegrees;
+            public double? ActualSteerRightFrontDegrees;
+            public double? ActualSteerRightRearDegrees;
+            public double? ActualSpeedLeftFrontMetersPerSecond;
+            public double? ActualSpeedLeftRearMetersPerSecond;
+            public double? ActualSpeedRightFrontMetersPerSecond;
+            public double? ActualSpeedRightRearMetersPerSecond;
+            public string FailureReason;
+        }
+
+        private readonly object _sampleSyncRoot = new object();
+        private readonly List<DiagnosticSample> _samples =
+            new List<DiagnosticSample>();
+        private readonly Stopwatch _clock = new Stopwatch();
+
+        private MultiWheelChassis _chassis;
+        private Thread _samplingThread;
+        private volatile bool _sampling;
+        private int _testRunning;
+        private int _stopRequested;
+        private int _sessionId;
+        private bool _hasPreviousDetourSample;
+        private double _previousElapsedSeconds;
+        private long _previousDetourTick;
+        private double _previousDetourXMillimeters;
+        private double _previousDetourYMillimeters;
+        private double _previousDetourYawDegrees;
+
+        /// <summary>
+        /// 获取或设置自动结束前的记录时长，单位为min。
+        /// </summary>
+        public double DurationMinutes = 20.0;
+
+        /// <summary>
+        /// 获取或设置本机主动读取Detour的周期，单位为ms。
+        /// </summary>
+        public int SampleIntervalMilliseconds = 50;
+
+        /// <summary>
+        /// 获取最近一次静态诊断CSV的完整路径。
+        /// </summary>
+        public string SavedFilePath { get; private set; } =
+            string.Empty;
+
+        /// <summary>
+        /// 停车后开始静态采样，并在到达设定时长时自动保存CSV。
+        /// </summary>
+        public override void Test()
+        {
+            if (Interlocked.CompareExchange(
+                    ref _testRunning,
+                    1,
+                    0) != 0)
+            {
+                Console.WriteLine("Detour静态诊断已经在运行。");
+                return;
+            }
+
+            var samplingStarted = false;
+            var completedAutomatically = false;
+            Exception testFailure = null;
+
+            try
+            {
+                ValidateSettings();
+                _chassis =
+                    PilotDefinition.Chassis as MultiWheelChassis;
+                if (_chassis == null)
+                {
+                    throw new InvalidOperationException(
+                        "当前底盘不是MultiWheelChassis，无法读取四轮反馈。");
+                }
+
+                var sessionId = ResetSession();
+                _chassis.PredefinedDriveStop();
+                _sampling = true;
+                _clock.Restart();
+                samplingStarted = true;
+
+                _samplingThread = new Thread(
+                    () => SamplingLoop(sessionId))
+                {
+                    IsBackground = true,
+                    Name = "DetourStaticDiagnostic"
+                };
+                _samplingThread.Start();
+
+                Console.WriteLine(
+                    $"Detour静态诊断开始：时长={DurationMinutes:F1}min，" +
+                    $"主动读取周期={SampleIntervalMilliseconds}ms，" +
+                    "车辆必须保持静止。");
+                Hedingben.ToastText(
+                    $"Detour静态诊断开始，预计{DurationMinutes:F1}分钟后自动结束。");
+
+                var durationSeconds = DurationMinutes * 60.0;
+                while (Volatile.Read(ref _stopRequested) == 0 &&
+                       _clock.Elapsed.TotalSeconds < durationSeconds)
+                {
+                    Thread.Sleep(100);
+                }
+
+                completedAutomatically =
+                    Volatile.Read(ref _stopRequested) == 0;
+            }
+            catch (Exception exception)
+            {
+                testFailure = exception;
+                Console.WriteLine(
+                    "Detour静态诊断失败：" +
+                    exception.Message);
+            }
+            finally
+            {
+                _sampling = false;
+                _chassis?.PredefinedDriveStop();
+
+                if (_samplingThread != null &&
+                    _samplingThread != Thread.CurrentThread)
+                {
+                    _samplingThread.Join(
+                        Math.Max(
+                            1000,
+                            SampleIntervalMilliseconds * 4));
+                }
+
+                _clock.Stop();
+
+                if (samplingStarted)
+                {
+                    try
+                    {
+                        SaveCsvAndReport(
+                            completedAutomatically,
+                            testFailure);
+                    }
+                    catch (Exception exception)
+                    {
+                        Console.WriteLine(
+                            "Detour静态诊断CSV保存失败：" +
+                            exception.Message);
+                        Hedingben.ToastText(
+                            "Detour静态诊断CSV保存失败：" +
+                            exception.Message);
+                    }
+                }
+
+                _samplingThread = null;
+                _chassis = null;
+                Interlocked.Exchange(ref _testRunning, 0);
+            }
+        }
+
+        /// <summary>
+        /// 请求提前停止采样；测试线程随后保存已经采集的数据。
+        /// </summary>
+        public override void TestStop()
+        {
+            Interlocked.Exchange(ref _stopRequested, 1);
+            _sampling = false;
+            _chassis?.PredefinedDriveStop();
+        }
+
+        /// <summary>
+        /// 清除上一次测试的样本、时间基准和输出路径。
+        /// </summary>
+        private int ResetSession()
+        {
+            lock (_sampleSyncRoot)
+            {
+                _samples.Clear();
+            }
+
+            Interlocked.Exchange(ref _stopRequested, 0);
+            _hasPreviousDetourSample = false;
+            _previousElapsedSeconds = 0.0;
+            _previousDetourTick = 0;
+            _previousDetourXMillimeters = 0.0;
+            _previousDetourYMillimeters = 0.0;
+            _previousDetourYawDegrees = 0.0;
+            SavedFilePath = string.Empty;
+            return Interlocked.Increment(ref _sessionId);
+        }
+
+        /// <summary>
+        /// 检查测试时长和主动采样周期是否适合执行。
+        /// </summary>
+        private void ValidateSettings()
+        {
+            NumericGuard.EnsureFinitePositive(
+                DurationMinutes,
+                nameof(DurationMinutes));
+
+            if (SampleIntervalMilliseconds < 20 ||
+                SampleIntervalMilliseconds > 5000)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(SampleIntervalMilliseconds),
+                    "Detour主动读取周期必须在20ms到5000ms之间。");
+            }
+        }
+
+        /// <summary>
+        /// 按设定周期持续采样，直至测试到时或收到停止请求。
+        /// </summary>
+        private void SamplingLoop(int sessionId)
+        {
+            while (_sampling &&
+                   sessionId == Volatile.Read(ref _sessionId))
+            {
+                CaptureSample(sessionId);
+                Thread.Sleep(SampleIntervalMilliseconds);
+            }
+        }
+
+        /// <summary>
+        /// 采集一帧原始Detour定位、接口耗时和四轮实际反馈。
+        /// </summary>
+        private void CaptureSample(int sessionId)
+        {
+            var sample = new DiagnosticSample
+            {
+                ElapsedSeconds = _clock.Elapsed.TotalSeconds,
+                LocalTimestamp =
+                    DateTimeOffset.Now.ToString(
+                        "O",
+                        CultureInfo.InvariantCulture),
+                FailureReason = string.Empty
+            };
+
+            CaptureDetour(sample, sessionId);
+
+            if (sessionId != Volatile.Read(ref _sessionId))
+            {
+                return;
+            }
+
+            CaptureWheelFeedback(sample);
+
+            if (!_sampling ||
+                sessionId != Volatile.Read(ref _sessionId))
+            {
+                return;
+            }
+
+            lock (_sampleSyncRoot)
+            {
+                _samples.Add(sample);
+            }
+        }
+
+        /// <summary>
+        /// 读取Detour原始字段并计算与上一成功读取之间的时间和位姿差。
+        /// </summary>
+        private void CaptureDetour(
+            DiagnosticSample sample,
+            int sessionId)
+        {
+            var callClock = Stopwatch.StartNew();
+
+            try
+            {
+                var location =
+                    DetourInterface.getCartLocation();
+                callClock.Stop();
+
+                sample.DetourCallDurationMilliseconds =
+                    callClock.Elapsed.TotalMilliseconds;
+                sample.DetourReadSucceeded = true;
+                sample.DetourTickRaw = Convert.ToInt64(
+                    location.tick,
+                    CultureInfo.InvariantCulture);
+                sample.DetourLStep = Convert.ToDouble(
+                    location.l_step,
+                    CultureInfo.InvariantCulture);
+                sample.DetourXMillimeters = Convert.ToDouble(
+                    location.x,
+                    CultureInfo.InvariantCulture);
+                sample.DetourYMillimeters = Convert.ToDouble(
+                    location.y,
+                    CultureInfo.InvariantCulture);
+                sample.DetourYawDegrees = Convert.ToDouble(
+                    location.th,
+                    CultureInfo.InvariantCulture);
+
+                if (sessionId != Volatile.Read(ref _sessionId))
+                {
+                    return;
+                }
+
+                CaptureDetourTimestamp(sample);
+                CaptureDetourDelta(sample);
+            }
+            catch (Exception exception)
+            {
+                callClock.Stop();
+                sample.DetourCallDurationMilliseconds =
+                    callClock.Elapsed.TotalMilliseconds;
+                AppendFailure(
+                    sample,
+                    "Detour读取失败：" +
+                    exception.Message);
+            }
+        }
+
+        /// <summary>
+        /// 将Detour原始tick按.NET DateTime ticks解释并记录数据年龄。
+        /// </summary>
+        private static void CaptureDetourTimestamp(
+            DiagnosticSample sample)
+        {
+            try
+            {
+                var detourTime = new DateTime(
+                    sample.DetourTickRaw.Value,
+                    DateTimeKind.Local);
+                sample.DetourTimestamp =
+                    detourTime.ToString(
+                        "O",
+                        CultureInfo.InvariantCulture);
+                sample.DetourDataAgeMilliseconds =
+                    (DateTime.Now - detourTime)
+                    .TotalMilliseconds;
+                sample.DetourTimestampValid = true;
+            }
+            catch (ArgumentOutOfRangeException)
+            {
+                sample.DetourTimestamp = string.Empty;
+            }
+        }
+
+        /// <summary>
+        /// 计算Detour帧间差并更新下一帧使用的原始基准。
+        /// </summary>
+        private void CaptureDetourDelta(
+            DiagnosticSample sample)
+        {
+            var tick = sample.DetourTickRaw.Value;
+            var xMillimeters = sample.DetourXMillimeters.Value;
+            var yMillimeters = sample.DetourYMillimeters.Value;
+            var yawDegrees = sample.DetourYawDegrees.Value;
+
+            if (_hasPreviousDetourSample)
+            {
+                sample.LocalDeltaMilliseconds =
+                    (sample.ElapsedSeconds -
+                     _previousElapsedSeconds) * 1000.0;
+                sample.DetourTickDeltaMilliseconds =
+                    (tick - _previousDetourTick) /
+                    (double)TimeSpan.TicksPerMillisecond;
+                sample.DeltaXMillimeters =
+                    xMillimeters - _previousDetourXMillimeters;
+                sample.DeltaYMillimeters =
+                    yMillimeters - _previousDetourYMillimeters;
+                sample.DeltaYawDegrees =
+                    AngleMath.ShortestDifferenceDegrees(
+                        yawDegrees,
+                        _previousDetourYawDegrees);
+                sample.DeltaPositionMillimeters = Math.Sqrt(
+                    sample.DeltaXMillimeters.Value *
+                    sample.DeltaXMillimeters.Value +
+                    sample.DeltaYMillimeters.Value *
+                    sample.DeltaYMillimeters.Value);
+                sample.IsRepeatedTick =
+                    tick == _previousDetourTick;
+                sample.IsOutOfOrderTick =
+                    tick < _previousDetourTick;
+                sample.IsRepeatedPose =
+                    sample.DeltaPositionMillimeters.Value <= 1e-6 &&
+                    Math.Abs(sample.DeltaYawDegrees.Value) <= 1e-9;
+            }
+
+            _hasPreviousDetourSample = true;
+            _previousElapsedSeconds = sample.ElapsedSeconds;
+            _previousDetourTick = tick;
+            _previousDetourXMillimeters = xMillimeters;
+            _previousDetourYMillimeters = yMillimeters;
+            _previousDetourYawDegrees = yawDegrees;
+        }
+
+        /// <summary>
+        /// 读取底盘反算速度与按物理安装位置识别的四轮实际反馈。
+        /// </summary>
+        private void CaptureWheelFeedback(
+            DiagnosticSample sample)
+        {
+            try
+            {
+                var carSpeed = _chassis.GetCarSpeed(true);
+                sample.WheelBodyVxMetersPerSecond = carSpeed.Vx;
+                sample.WheelBodyVyMetersPerSecond = carSpeed.Vy;
+                // CommonUsage的CarSpeed.Vw以deg/s表达。
+                sample.WheelBodyOmegaDegreesPerSecond = carSpeed.Vw;
+
+#pragma warning disable CS0612, CS0618
+                var wheels = _chassis.GetSteerWheels();
+#pragma warning restore CS0612, CS0618
+
+                var leftFront = FindWheel(wheels, true, true);
+                var leftRear = FindWheel(wheels, false, true);
+                var rightFront = FindWheel(wheels, true, false);
+                var rightRear = FindWheel(wheels, false, false);
+
+                if (leftFront == null || leftRear == null ||
+                    rightFront == null || rightRear == null)
+                {
+                    throw new InvalidOperationException(
+                        "未能按物理安装位置识别四个舵轮。");
+                }
+
+                sample.ActualSteerLeftFrontDegrees =
+                    leftFront.ReadAngle();
+                sample.ActualSteerLeftRearDegrees =
+                    leftRear.ReadAngle();
+                sample.ActualSteerRightFrontDegrees =
+                    rightFront.ReadAngle();
+                sample.ActualSteerRightRearDegrees =
+                    rightRear.ReadAngle();
+                sample.ActualSpeedLeftFrontMetersPerSecond =
+                    leftFront.ReadSpeed();
+                sample.ActualSpeedLeftRearMetersPerSecond =
+                    leftRear.ReadSpeed();
+                sample.ActualSpeedRightFrontMetersPerSecond =
+                    rightFront.ReadSpeed();
+                sample.ActualSpeedRightRearMetersPerSecond =
+                    rightRear.ReadSpeed();
+                sample.WheelReadSucceeded = true;
+            }
+            catch (Exception exception)
+            {
+                AppendFailure(
+                    sample,
+                    "四轮反馈读取失败：" +
+                    exception.Message);
+            }
+        }
+
+        /// <summary>
+        /// 根据真实车体X向前、Y向左的物理安装位置查找指定舵轮。
+        /// </summary>
+        private static SteerWheel FindWheel(
+            IReadOnlyList<SteerWheel> wheels,
+            bool requireFront,
+            bool requireLeft)
+        {
+            foreach (var wheel in wheels)
+            {
+                var isFront = wheel.PhysicalPosition.X >= 0f;
+                var isLeft = wheel.PhysicalPosition.Y >= 0f;
+
+                if (isFront == requireFront &&
+                    isLeft == requireLeft)
+                {
+                    return wheel;
+                }
+            }
+
+            return null;
+        }
+
+        /// <summary>
+        /// 追加本帧诊断失败原因且保留先前错误信息。
+        /// </summary>
+        private static void AppendFailure(
+            DiagnosticSample sample,
+            string reason)
+        {
+            sample.FailureReason =
+                string.IsNullOrWhiteSpace(sample.FailureReason)
+                    ? reason
+                    : sample.FailureReason + "；" + reason;
+        }
+
+        /// <summary>
+        /// 保存采样快照并输出自动结束或手动停止后的摘要提示。
+        /// </summary>
+        private void SaveCsvAndReport(
+            bool completedAutomatically,
+            Exception testFailure)
+        {
+            List<DiagnosticSample> snapshot;
+            lock (_sampleSyncRoot)
+            {
+                snapshot =
+                    new List<DiagnosticSample>(_samples);
+            }
+
+            var outputDirectory = Path.Combine(
+                AppContext.BaseDirectory,
+                "DetourStaticDiagnostics");
+            Directory.CreateDirectory(outputDirectory);
+            SavedFilePath = Path.Combine(
+                outputDirectory,
+                $"{DateTime.Now:yyyyMMdd_HHmmss_fff}_" +
+                "DetourStaticDiagnostic.csv");
+
+            using (var writer = new StreamWriter(
+                       SavedFilePath,
+                       false,
+                       new UTF8Encoding(true)))
+            {
+                WriteCsvRow(writer,
+                    "ElapsedSeconds", "LocalTimestamp",
+                    "DetourReadSucceeded", "DetourCallDurationMilliseconds",
+                    "DetourTickRaw", "DetourTimestamp",
+                    "DetourTimestampValid", "DetourDataAgeMilliseconds",
+                    "DetourLStep", "DetourXMillimeters",
+                    "DetourYMillimeters", "DetourYawDegrees",
+                    "LocalDeltaMilliseconds", "DetourTickDeltaMilliseconds",
+                    "DeltaXMillimeters", "DeltaYMillimeters",
+                    "DeltaYawDegrees", "DeltaPositionMillimeters",
+                    "IsRepeatedTick", "IsRepeatedPose", "IsOutOfOrderTick",
+                    "WheelReadSucceeded", "WheelBodyVxMetersPerSecond",
+                    "WheelBodyVyMetersPerSecond",
+                    "WheelBodyOmegaDegreesPerSecond",
+                    "ActualSteerLeftFrontDegrees",
+                    "ActualSteerLeftRearDegrees",
+                    "ActualSteerRightFrontDegrees",
+                    "ActualSteerRightRearDegrees",
+                    "ActualSpeedLeftFrontMetersPerSecond",
+                    "ActualSpeedLeftRearMetersPerSecond",
+                    "ActualSpeedRightFrontMetersPerSecond",
+                    "ActualSpeedRightRearMetersPerSecond",
+                    "FailureReason");
+
+                foreach (var sample in snapshot)
+                {
+                    WriteCsvRow(writer,
+                        sample.ElapsedSeconds, sample.LocalTimestamp,
+                        sample.DetourReadSucceeded,
+                        sample.DetourCallDurationMilliseconds,
+                        sample.DetourTickRaw, sample.DetourTimestamp,
+                        sample.DetourTimestampValid,
+                        sample.DetourDataAgeMilliseconds,
+                        sample.DetourLStep, sample.DetourXMillimeters,
+                        sample.DetourYMillimeters, sample.DetourYawDegrees,
+                        sample.LocalDeltaMilliseconds,
+                        sample.DetourTickDeltaMilliseconds,
+                        sample.DeltaXMillimeters, sample.DeltaYMillimeters,
+                        sample.DeltaYawDegrees,
+                        sample.DeltaPositionMillimeters,
+                        sample.IsRepeatedTick, sample.IsRepeatedPose,
+                        sample.IsOutOfOrderTick,
+                        sample.WheelReadSucceeded,
+                        sample.WheelBodyVxMetersPerSecond,
+                        sample.WheelBodyVyMetersPerSecond,
+                        sample.WheelBodyOmegaDegreesPerSecond,
+                        sample.ActualSteerLeftFrontDegrees,
+                        sample.ActualSteerLeftRearDegrees,
+                        sample.ActualSteerRightFrontDegrees,
+                        sample.ActualSteerRightRearDegrees,
+                        sample.ActualSpeedLeftFrontMetersPerSecond,
+                        sample.ActualSpeedLeftRearMetersPerSecond,
+                        sample.ActualSpeedRightFrontMetersPerSecond,
+                        sample.ActualSpeedRightRearMetersPerSecond,
+                        sample.FailureReason);
+                }
+            }
+
+            var successfulSamples = 0;
+            var maximumPositionStepMillimeters = 0.0;
+            var maximumAbsoluteYawStepDegrees = 0.0;
+            foreach (var sample in snapshot)
+            {
+                if (sample.DetourReadSucceeded)
+                {
+                    successfulSamples++;
+                }
+
+                maximumPositionStepMillimeters = Math.Max(
+                    maximumPositionStepMillimeters,
+                    sample.DeltaPositionMillimeters ?? 0.0);
+                maximumAbsoluteYawStepDegrees = Math.Max(
+                    maximumAbsoluteYawStepDegrees,
+                    Math.Abs(sample.DeltaYawDegrees ?? 0.0));
+            }
+
+            var completionReason = testFailure != null
+                ? "因异常提前结束"
+                : completedAutomatically
+                    ? "到达设定时长，已自动结束"
+                    : "收到手动停止请求";
+            var message =
+                $"Detour静态诊断{completionReason}；" +
+                $"样本={snapshot.Count}，有效Detour样本={successfulSamples}，" +
+                $"最大位置阶跃={maximumPositionStepMillimeters:F2}mm，" +
+                $"最大航向阶跃={maximumAbsoluteYawStepDegrees:F3}°；" +
+                $"CSV={SavedFilePath}";
+
+            Console.WriteLine(message);
+            Hedingben.ToastText(message);
+        }
+
+        /// <summary>
+        /// 使用InvariantCulture格式化并转义一行CSV字段。
+        /// </summary>
+        private static void WriteCsvRow(
+            TextWriter writer,
+            params object[] values)
+        {
+            var fields = new string[values.Length];
+            for (var index = 0; index < values.Length; index++)
+            {
+                fields[index] = FormatCsvValue(values[index]);
+            }
+
+            writer.WriteLine(string.Join(",", fields));
+        }
+
+        /// <summary>
+        /// 将单个值转换为区域无关且符合CSV转义规则的文本。
+        /// </summary>
+        private static string FormatCsvValue(object value)
+        {
+            if (value == null)
+            {
+                return string.Empty;
+            }
+
+            string text;
+            if (value is bool boolean)
+            {
+                text = boolean ? "1" : "0";
+            }
+            else if (value is IFormattable formattable)
+            {
+                text = formattable.ToString(
+                    null,
+                    CultureInfo.InvariantCulture);
+            }
+            else
+            {
+                text = value.ToString();
+            }
+
+            if (text.IndexOfAny(
+                    new[] { ',', '"', '\r', '\n' }) < 0)
+            {
+                return text;
+            }
+
+            return "\"" +
+                   text.Replace("\"", "\"\"") +
+                   "\"";
+        }
+    }
+}

MultiWheelC/Experiments/NewControllerTrackingTests.cs

@@ -0,0 +1,979 @@
+using System;
+using System.Drawing;
+using System.Globalization;
+using System.Numerics;
+using System.Threading;
+using ClumsyCore;
+using ClumsyCore.DTools;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using CommonUsage.Chassis;
+using MultiWheelC.Control.Execution;
+using MultiWheelC.StateEstimation;
+using MyParking.Shared;
+
+namespace MultiWheelC
+{
+    /// <summary>
+    /// 统一读取轨迹实验的有符号横向偏移，并将车体局部偏移转换到世界坐标系。
+    /// </summary>
+    internal static class TrajectoryExperimentInput
+    {
+        private const double MaximumOffsetCentimeters = 30.0;
+
+        /// <summary>
+        /// 从Clumsy输入框读取车体左正右负的横向偏移，单位转换为m。
+        /// </summary>
+        public static bool TryReadLateralOffsetMeters(
+            out double lateralOffsetMeters)
+        {
+            lateralOffsetMeters = 0.0;
+            var input = UI.GetInput(
+                "输入轨迹横向偏移（cm，左正右负，范围-30～30）：");
+            var parsed = double.TryParse(
+                input,
+                NumberStyles.Float,
+                CultureInfo.CurrentCulture,
+                out var offsetCentimeters) ||
+                double.TryParse(
+                    input,
+                    NumberStyles.Float,
+                    CultureInfo.InvariantCulture,
+                    out offsetCentimeters);
+
+            if (!parsed ||
+                double.IsNaN(offsetCentimeters) ||
+                double.IsInfinity(offsetCentimeters) ||
+                Math.Abs(offsetCentimeters) >
+                    MaximumOffsetCentimeters)
+            {
+                Console.WriteLine(
+                    "轨迹横向偏移必须是-30～30cm之间的有限数值，测试未启动。");
+                return false;
+            }
+
+            lateralOffsetMeters =
+                offsetCentimeters / 100.0;
+            return true;
+        }
+
+        /// <summary>
+        /// 沿初始车体左方向平移参考轨迹起点，同时保持世界坐标航向不变。
+        /// </summary>
+        public static Pose2D OffsetPoseLaterally(
+            Pose2D poseInWorld,
+            double lateralOffsetMeters)
+        {
+            var yawRadians = poseInWorld.YawRadians;
+            return new Pose2D(
+                poseInWorld.XMeters -
+                    Math.Sin(yawRadians) *
+                    lateralOffsetMeters,
+                poseInWorld.YMeters +
+                    Math.Cos(yawRadians) *
+                    lateralOffsetMeters,
+                yawRadians);
+        }
+
+        /// <summary>
+        /// 生成带毫米偏移标识的实验轨迹名称。
+        /// </summary>
+        public static string BuildTrajectoryName(
+            string baseName,
+            double lateralOffsetMeters)
+        {
+            return baseName +
+                   "_Offset" +
+                   (lateralOffsetMeters * 1000.0)
+                   .ToString("+0;-0;0", CultureInfo.InvariantCulture) +
+                   "mm";
+        }
+    }
+
+    /// <summary>
+    /// 从当前Detour位姿开始执行新版控制器4m直线跟踪并保存实验数据。
+    /// </summary>
+    [MovementTest(name = "新版控制器：4m直线轨迹跟踪")]
+    public class NewControllerStraight4mTest
+        : MovementTest
+    {
+        private const float MillimetersPerMeter = 1000f;
+
+        private readonly Painter _painter =
+            UI.GetPainter("NewControllerStraight4m");
+
+        private DriveTask _task;
+        private TrackingExperimentRecorder _recorder;
+        private IVehicleStateProvider _stateProvider;
+
+        /// <summary>
+        /// 获取或设置本次测试编号，用于区分重复实验CSV。
+        /// </summary>
+        public int TrialNumber = 1;
+
+        /// <summary>
+        /// 获取或设置4m直线的巡航参考速度，单位为m/s。
+        /// </summary>
+        public double CruiseSpeedMetersPerSecond = 0.40;
+
+        /// <summary>
+        /// 获取或设置参考速度加速度，单位为m/s²。
+        /// </summary>
+        public double AccelerationMetersPerSecondSquared = 0.20;
+
+        /// <summary>
+        /// 获取或设置参考速度减速度，单位为m/s²。
+        /// </summary>
+        public double DecelerationMetersPerSecondSquared = 0.08;
+
+        /// <summary>
+        /// 获取或设置离散轨迹点间距，单位为m。
+        /// </summary>
+        public double PointSpacingMeters = 0.02;
+
+        /// <summary>
+        /// 获取实验记录使用的轨迹基础名称，供同一套直线测试流程区分前进和倒车。
+        /// </summary>
+        protected virtual string ExperimentTrajectoryBaseName =>
+            "ProfiledStraight4m";
+
+        /// <summary>
+        /// 获取直线主运动方向相对车头的夹角，单位为rad。
+        /// </summary>
+        protected virtual double MotionDirectionInBodyRadians =>
+            0.0;
+
+        /// <summary>
+        /// 获取是否由生成后的轨迹自动推导底盘运动坐标系方向。
+        /// </summary>
+        protected virtual bool ResolveMotionDirectionFromTrajectory =>
+            false;
+
+        /// <summary>
+        /// 获取轨迹完成后是否需要将舵轮主动恢复到车头方向。
+        /// </summary>
+        protected virtual bool ReturnWheelsForwardAfterCompletion =>
+            false;
+
+        /// <summary>
+        /// 读取当前位姿、绘制离散轨迹并启动新版轨迹跟踪动作。
+        /// </summary>
+        public override void Test()
+        {
+            if (_task != null)
+            {
+                Console.WriteLine(
+                    "新版4m直线轨迹测试已经在运行，请先停止当前测试。");
+                return;
+            }
+
+            if (!TrajectoryExperimentInput
+                    .TryReadLateralOffsetMeters(
+                        out var lateralOffsetMeters))
+            {
+                return;
+            }
+
+            var chassis =
+                PilotDefinition.Chassis as MultiWheelChassis;
+            if (chassis == null)
+            {
+                Console.WriteLine(
+                    "当前底盘不是MultiWheelChassis，无法执行新版轨迹测试。");
+                return;
+            }
+
+            var stateProvider =
+                ParkingVehicleStateProviderFactory.Create(
+                    chassis);
+            if (!stateProvider.TryGetState(
+                    out var initialState))
+            {
+                Console.WriteLine(
+                    "无法读取有效停车状态起点位姿：" +
+                    stateProvider.LastFailureReason);
+                _stateProvider = null;
+                return;
+            }
+
+            _stateProvider = stateProvider;
+
+            var trajectoryStartPose =
+                TrajectoryExperimentInput.OffsetPoseLaterally(
+                    initialState.PoseInWorld,
+                    lateralOffsetMeters);
+            var trajectory =
+                TestTrajectoryFactory.CreateStraight4Meters(
+                    trajectoryStartPose,
+                    CruiseSpeedMetersPerSecond,
+                    AccelerationMetersPerSecondSquared,
+                    DecelerationMetersPerSecondSquared,
+                    PointSpacingMeters,
+                    MotionDirectionInBodyRadians);
+
+            DrawTrajectory(trajectory);
+
+            var referenceStart = ToMillimeterVector(
+                trajectory.StartPoint.PoseInWorld);
+            var referenceEnd = ToMillimeterVector(
+                trajectory.EndPoint.PoseInWorld);
+            var recorder =
+                new TrackingExperimentRecorder(
+                    controllerName: "NewStanleyPid",
+                    trajectoryName:
+                        TrajectoryExperimentInput.BuildTrajectoryName(
+                            ExperimentTrajectoryBaseName,
+                            lateralOffsetMeters),
+                    trialNumber: TrialNumber,
+                    referenceStart: referenceStart,
+                    referenceEnd: referenceEnd,
+                    referenceSpeed:
+                        (float)CruiseSpeedMetersPerSecond,
+                    sampleIntervalMs: 50,
+                    referenceMotionFrameYawDegrees:
+                        (float)AngleMath.RadiansToDegrees(
+                            MotionDirectionInBodyRadians),
+                    referenceAccelerationMetersPerSecondSquared:
+                        (float)AccelerationMetersPerSecondSquared,
+                    referenceDecelerationMetersPerSecondSquared:
+                        (float)DecelerationMetersPerSecondSquared,
+                    diagnosticChassis: chassis,
+                    diagnosticStateProvider: stateProvider);
+            _recorder = recorder;
+
+            var controlPointRadiusMeters =
+                chassis.ControlPointRadius /
+                MillimetersPerMeter;
+            var movement =
+                new TrajectoryTrackingMovement
+                {
+                    Trajectory = trajectory,
+                    StateProvider = _stateProvider,
+                    MotionDirectionInBodyRadians =
+                        ResolveMotionDirectionFromTrajectory
+                            ? (double?)null
+                            : MotionDirectionInBodyRadians,
+                    ReturnWheelsForwardAfterCompletion =
+                        ReturnWheelsForwardAfterCompletion,
+                    CycleObserver = controller =>
+                        RecordControlCycle(
+                            recorder,
+                            controller,
+                            controlPointRadiusMeters,
+                            _stateProvider as
+                                WheelFeedbackVehicleStateProvider)
+                };
+
+            recorder.Start();
+
+            try
+            {
+                _task = new DriveTask(movement.Get());
+                _task.Wait();
+
+                // 保留少量停车后原始Detour数据，并生成一帧处理后的静止状态。
+                Thread.Sleep(400);
+                recorder.UpdateCommand(0f, 0f);
+                if (_stateProvider.TryGetState(
+                        out var stoppedState))
+                {
+                    recorder.UpdateProcessedState(
+                        stoppedState);
+                }
+            }
+            finally
+            {
+                _task?.Stop();
+                recorder.UpdateCommand(0f, 0f);
+                recorder.StopAndSave();
+                _painter.Clear();
+                _task = null;
+                _recorder = null;
+                _stateProvider = null;
+            }
+        }
+
+        /// <summary>
+        /// 停止正在运行的测试、保存已有数据并清除Clumsy轨迹可视化。
+        /// </summary>
+        public override void TestStop()
+        {
+            _task?.Stop();
+            _recorder?.UpdateCommand(0f, 0f);
+            _recorder?.StopAndSave();
+            _painter.Clear();
+            _task = null;
+            _recorder = null;
+            _stateProvider = null;
+        }
+
+        /// <summary>
+        /// 将离散轨迹点和相邻线段从SI单位转换为Clumsy毫米坐标后绘制。
+        /// </summary>
+        private void DrawTrajectory(
+            Trajectory.Trajectory2D trajectory)
+        {
+            _painter.Clear();
+
+            for (var index = 0;
+                 index < trajectory.Count;
+                 index++)
+            {
+                var point = ToMillimeterVector(
+                    trajectory[index].PoseInWorld);
+                _painter.DrawDot(
+                    Color.Cyan,
+                    point.X,
+                    point.Y,
+                    3f);
+
+                if (index == 0)
+                {
+                    continue;
+                }
+
+                var previousPoint = ToMillimeterVector(
+                    trajectory[index - 1].PoseInWorld);
+                _painter.DrawLine(
+                    Color.DeepSkyBlue,
+                    previousPoint.X,
+                    previousPoint.Y,
+                    point.X,
+                    point.Y,
+                    width: 2);
+            }
+
+            var start = ToMillimeterVector(
+                trajectory.StartPoint.PoseInWorld);
+            var end = ToMillimeterVector(
+                trajectory.EndPoint.PoseInWorld);
+            _painter.DrawDot(
+                Color.LimeGreen,
+                start.X,
+                start.Y,
+                8f);
+            _painter.DrawDot(
+                Color.OrangeRed,
+                end.X,
+                end.Y,
+                8f);
+        }
+
+        /// <summary>
+        /// 将控制器本周期使用的状态和最终GCP命令同步给实验记录器。
+        /// </summary>
+        private static void RecordControlCycle(
+            TrackingExperimentRecorder recorder,
+            ParkingGeometricController controller,
+            double controlPointRadiusMeters,
+            WheelFeedbackVehicleStateProvider stateProvider)
+        {
+            if (controller.LastCycleTiming.HasValue)
+            {
+                recorder.RecordControlCycleTiming(
+                    controller.LastCycleTiming.Value,
+                    requestedCommand:
+                        controller.LastRequestedCommand,
+                    sentCommand:
+                        controller.LastCommand);
+            }
+
+            if (controller.LastVehicleState.HasValue)
+            {
+                recorder.UpdateProcessedState(
+                    controller.LastVehicleState.Value);
+            }
+
+            UpdateVelocityDiagnostics(
+                recorder,
+                stateProvider);
+
+            if (!controller.LastCommand.HasValue)
+            {
+                return;
+            }
+
+            if (controller.LastProjection.HasValue &&
+                controller.LastControlReferenceSpeedMetersPerSecond.HasValue)
+            {
+                var projection =
+                    controller.LastProjection.Value;
+                recorder.UpdateControlReference(
+                    projection.ArcLengthMeters,
+                    controller.LastControlReferenceSpeedMetersPerSecond.Value,
+                    projection.LateralErrorMeters,
+                    projection.HeadingErrorRadians,
+                    projection.DistanceToTrajectoryMeters,
+                    projection.RemainingDistanceMeters,
+                    controller.LastCurvaturePreviewDistanceMeters ?? 0.0,
+                    controller.LastFeedforwardCurvaturePerMeter ??
+                    projection.ReferencePoint.CurvaturePerMeter);
+            }
+
+            var requestedCommand =
+                controller.LastRequestedCommand ??
+                controller.LastCommand.Value;
+            var command = controller.LastCommand.Value;
+            recorder.UpdateGcpCommand(
+                requestedCommand.FrontAngleRadians,
+                requestedCommand.RearAngleRadians,
+                command.FrontAngleRadians,
+                command.RearAngleRadians);
+            var curvaturePerMeter = Math.Tan(
+                command.FrontAngleRadians) /
+                controlPointRadiusMeters;
+            var angularSpeedRadiansPerSecond =
+                command.SpeedMetersPerSecond *
+                curvaturePerMeter;
+
+            recorder.UpdateCommand(
+                (float)command.SpeedMetersPerSecond,
+                (float)angularSpeedRadiansPerSecond);
+        }
+
+        /// <summary>
+        /// 将同一周期的Detour速度和轮速解算速度写入实验记录器。
+        /// </summary>
+        private static void UpdateVelocityDiagnostics(
+            TrackingExperimentRecorder recorder,
+            WheelFeedbackVehicleStateProvider stateProvider)
+        {
+            if (stateProvider == null ||
+                !stateProvider.TryGetLatestVelocityDiagnostics(
+                    out var detourBodyVx,
+                    out var detourVelocityValid,
+                    out var rawWheelBodyVx,
+                    out var filteredWheelBodyVx,
+                    out var rawWheelBodyVy,
+                    out var filteredWheelBodyVy,
+                    out var wheelVelocityValid))
+            {
+                return;
+            }
+
+            recorder.UpdateVelocityDiagnostics(
+                detourBodyVx,
+                detourVelocityValid,
+                rawWheelBodyVx,
+                filteredWheelBodyVx,
+                rawWheelBodyVy,
+                filteredWheelBodyVy,
+                wheelVelocityValid);
+        }
+
+        /// <summary>
+        /// 将Shared世界坐标系米制位姿转换为Clumsy绘图和旧记录器使用的毫米坐标。
+        /// </summary>
+        private static Vector2 ToMillimeterVector(
+            Pose2D poseInWorld)
+        {
+            return new Vector2(
+                (float)(
+                    poseInWorld.XMeters *
+                    MillimetersPerMeter),
+                (float)(
+                    poseInWorld.YMeters *
+                    MillimetersPerMeter));
+        }
+
+    }
+
+    /// <summary>
+    /// 从当前Detour位姿开始，沿车体后方执行新版控制器4m直线倒车跟踪并保存实验数据。
+    /// </summary>
+    [MovementTest(name = "新版控制器：4m直线倒车轨迹跟踪")]
+    public sealed class NewControllerReverseStraight4mTest
+        : NewControllerStraight4mTest
+    {
+        /// <summary>
+        /// 使用负参考速度，使轨迹工厂沿车尾方向生成轨迹并触发倒车控制语义。
+        /// </summary>
+        public NewControllerReverseStraight4mTest()
+        {
+            CruiseSpeedMetersPerSecond = -0.40;
+        }
+
+        /// <summary>
+        /// 将倒车实验与前进直线实验的CSV名称明确区分。
+        /// </summary>
+        protected override string ExperimentTrajectoryBaseName =>
+            "ProfiledReverseStraight4m";
+    }
+
+    /// <summary>
+    /// 将舵轮准备到车体左前45°，以0.4m/s跟踪4m直线，停车后再恢复车头方向。
+    /// </summary>
+    [MovementTest(name = "新版控制器：45°蟹行4m直线轨迹跟踪")]
+    public sealed class NewControllerCrab45Straight4mTest
+        : NewControllerStraight4mTest
+    {
+        /// <summary>
+        /// 使用车体左前45°作为本次直线轨迹的固定运动方向。
+        /// </summary>
+        protected override double MotionDirectionInBodyRadians =>
+            Math.PI / 4.0;
+
+        /// <summary>
+        /// 只用45°定义参考轨迹，底盘β由轨迹切线和车身参考航向自动推导。
+        /// </summary>
+        protected override bool ResolveMotionDirectionFromTrajectory =>
+            true;
+
+        /// <summary>
+        /// 蟹行轨迹正常完成后主动将四个舵轮恢复到车头方向。
+        /// </summary>
+        protected override bool ReturnWheelsForwardAfterCompletion =>
+            true;
+
+        /// <summary>
+        /// 将45°蟹行实验与普通前进和倒车实验的CSV名称明确区分。
+        /// </summary>
+        protected override string ExperimentTrajectoryBaseName =>
+            "ProfiledCrab45Straight4m";
+    }
+
+    /// <summary>
+    /// 从当前Detour位姿开始执行“3m直线—左半圆—3m直线”新版控制器跟踪实验。
+    /// </summary>
+    [MovementTest(name = "新版控制器：直线-左半圆-直线轨迹跟踪")]
+    public class NewControllerStraightSemicircleStraightTest
+        : MovementTest
+    {
+        private const float MillimetersPerMeter = 1000f;
+
+        private readonly Painter _painter =
+            UI.GetPainter(
+                "NewControllerStraightSemicircleStraight");
+
+        private DriveTask _task;
+        private TrackingExperimentRecorder _recorder;
+        private IVehicleStateProvider _stateProvider;
+
+        /// <summary>
+        /// 获取或设置本次测试编号，用于区分重复实验CSV。
+        /// </summary>
+        public int TrialNumber = 1;
+
+        /// <summary>
+        /// 获取或设置半圆前后两段直线的长度，单位为m。
+        /// </summary>
+        public double StraightLengthMeters = 3.0;
+
+        /// <summary>
+        /// 获取或设置左转半圆的转弯半径，单位为m。
+        /// </summary>
+        public double TurnRadiusMeters = 2.0;
+
+        /// <summary>
+        /// 获取或设置直线与等曲率转弯之间的曲率过渡长度，单位为m。
+        /// </summary>
+        public double CurvatureTransitionLengthMeters = 0.70;
+
+        /// <summary>
+        /// 获取或设置两段直线的最大参考速度，单位为m/s。
+        /// </summary>
+        public double StraightMaximumSpeedMetersPerSecond = 0.40;
+
+        /// <summary>
+        /// 获取或设置半圆段的最大参考速度，单位为m/s。
+        /// </summary>
+        public double SemicircleMaximumSpeedMetersPerSecond = 0.30;
+
+        /// <summary>
+        /// 获取或设置参考速度加速度，单位为m/s²。
+        /// </summary>
+        public double AccelerationMetersPerSecondSquared = 0.20;
+
+        /// <summary>
+        /// 获取或设置参考速度减速度，单位为m/s²。
+        /// </summary>
+        public double DecelerationMetersPerSecondSquared = 0.08;
+
+        /// <summary>
+        /// 获取或设置离散轨迹点间距，单位为m。
+        /// </summary>
+        public double PointSpacingMeters = 0.02;
+
+        /// <summary>
+        /// 获取组合轨迹主运动方向相对车头的夹角，单位为rad。
+        /// </summary>
+        protected virtual double MotionDirectionInBodyRadians =>
+            0.0;
+
+        /// <summary>
+        /// 获取是否由生成后的轨迹自动推导底盘运动坐标系方向。
+        /// </summary>
+        protected virtual bool ResolveMotionDirectionFromTrajectory =>
+            false;
+
+        /// <summary>
+        /// 获取轨迹完成后是否需要将舵轮主动恢复到车头方向。
+        /// </summary>
+        protected virtual bool ReturnWheelsForwardAfterCompletion =>
+            false;
+
+        /// <summary>
+        /// 获取实验记录使用的轨迹基础名称。
+        /// </summary>
+        protected virtual string ExperimentTrajectoryBaseName =>
+            "ProfiledStraightSmoothLeftTurnStraight";
+
+        /// <summary>
+        /// 读取当前位姿、绘制组合轨迹并启动新版轨迹跟踪动作。
+        /// </summary>
+        public override void Test()
+        {
+            if (_task != null)
+            {
+                Console.WriteLine(
+                    "新版直线-左半圆-直线测试已经在运行，请先停止当前测试。");
+                return;
+            }
+
+            if (!TrajectoryExperimentInput
+                    .TryReadLateralOffsetMeters(
+                        out var lateralOffsetMeters))
+            {
+                return;
+            }
+
+            var chassis =
+                PilotDefinition.Chassis as MultiWheelChassis;
+            if (chassis == null)
+            {
+                Console.WriteLine(
+                    "当前底盘不是MultiWheelChassis，无法执行新版轨迹测试。");
+                return;
+            }
+
+            var stateProvider =
+                ParkingVehicleStateProviderFactory.Create(
+                    chassis);
+            if (!stateProvider.TryGetState(
+                    out var initialState))
+            {
+                Console.WriteLine(
+                    "无法读取有效停车状态起点位姿：" +
+                    stateProvider.LastFailureReason);
+                _stateProvider = null;
+                return;
+            }
+
+            _stateProvider = stateProvider;
+
+            var trajectoryStartPose =
+                TrajectoryExperimentInput.OffsetPoseLaterally(
+                    initialState.PoseInWorld,
+                    lateralOffsetMeters);
+            var trajectory =
+                TestTrajectoryFactory
+                    .CreateStraightLeftSemicircleStraight(
+                        trajectoryStartPose,
+                        StraightLengthMeters,
+                        TurnRadiusMeters,
+                        CurvatureTransitionLengthMeters,
+                        StraightMaximumSpeedMetersPerSecond,
+                        SemicircleMaximumSpeedMetersPerSecond,
+                        AccelerationMetersPerSecondSquared,
+                        DecelerationMetersPerSecondSquared,
+                        PointSpacingMeters,
+                        MotionDirectionInBodyRadians);
+
+            DrawTrajectory(trajectory);
+
+            var referenceStart = ToMillimeterVector(
+                trajectory.StartPoint.PoseInWorld);
+            var referenceEnd = ToMillimeterVector(
+                trajectory.EndPoint.PoseInWorld);
+            var recorder =
+                new TrackingExperimentRecorder(
+                    controllerName: "NewStanleyPid",
+                    trajectoryName:
+                        TrajectoryExperimentInput.BuildTrajectoryName(
+                            ExperimentTrajectoryBaseName,
+                            lateralOffsetMeters),
+                    trialNumber: TrialNumber,
+                    referenceStart: referenceStart,
+                    referenceEnd: referenceEnd,
+                    referenceSpeed:
+                        (float)StraightMaximumSpeedMetersPerSecond,
+                    sampleIntervalMs: 50,
+                    referenceMotionFrameYawDegrees:
+                        (float)AngleMath.RadiansToDegrees(
+                            MotionDirectionInBodyRadians),
+                    referenceAccelerationMetersPerSecondSquared:
+                        (float)AccelerationMetersPerSecondSquared,
+                    referenceDecelerationMetersPerSecondSquared:
+                        (float)DecelerationMetersPerSecondSquared,
+                    diagnosticChassis: chassis,
+                    diagnosticStateProvider: stateProvider);
+            _recorder = recorder;
+
+            var controlPointRadiusMeters =
+                chassis.ControlPointRadius /
+                MillimetersPerMeter;
+            var movement =
+                new TrajectoryTrackingMovement
+                {
+                    Trajectory = trajectory,
+                    StateProvider = _stateProvider,
+                    MotionDirectionInBodyRadians =
+                        ResolveMotionDirectionFromTrajectory
+                            ? (double?)null
+                            : MotionDirectionInBodyRadians,
+                    ReturnWheelsForwardAfterCompletion =
+                        ReturnWheelsForwardAfterCompletion,
+                    CycleObserver = controller =>
+                        RecordControlCycle(
+                            recorder,
+                            controller,
+                            controlPointRadiusMeters,
+                            _stateProvider as
+                                WheelFeedbackVehicleStateProvider)
+                };
+
+            recorder.Start();
+
+            try
+            {
+                _task = new DriveTask(movement.Get());
+                _task.Wait();
+
+                // 保留少量停车后原始Detour数据，并生成一帧处理后的静止状态。
+                Thread.Sleep(400);
+                recorder.UpdateCommand(0f, 0f);
+                if (_stateProvider.TryGetState(
+                        out var stoppedState))
+                {
+                    recorder.UpdateProcessedState(
+                        stoppedState);
+                }
+            }
+            finally
+            {
+                _task?.Stop();
+                recorder.UpdateCommand(0f, 0f);
+                recorder.StopAndSave();
+                _painter.Clear();
+                _task = null;
+                _recorder = null;
+                _stateProvider = null;
+            }
+        }
+
+        /// <summary>
+        /// 停止组合轨迹测试、保存已有数据并清除Clumsy轨迹可视化。
+        /// </summary>
+        public override void TestStop()
+        {
+            _task?.Stop();
+            _recorder?.UpdateCommand(0f, 0f);
+            _recorder?.StopAndSave();
+            _painter.Clear();
+            _task = null;
+            _recorder = null;
+            _stateProvider = null;
+        }
+
+        /// <summary>
+        /// 将组合轨迹的离散点和相邻线段转换为Clumsy毫米坐标后绘制。
+        /// </summary>
+        private void DrawTrajectory(
+            Trajectory.Trajectory2D trajectory)
+        {
+            _painter.Clear();
+
+            for (var index = 0;
+                 index < trajectory.Count;
+                 index++)
+            {
+                var point = ToMillimeterVector(
+                    trajectory[index].PoseInWorld);
+                _painter.DrawDot(
+                    Color.Cyan,
+                    point.X,
+                    point.Y,
+                    3f);
+
+                if (index == 0)
+                {
+                    continue;
+                }
+
+                var previousPoint = ToMillimeterVector(
+                    trajectory[index - 1].PoseInWorld);
+                _painter.DrawLine(
+                    Color.DeepSkyBlue,
+                    previousPoint.X,
+                    previousPoint.Y,
+                    point.X,
+                    point.Y,
+                    width: 2);
+            }
+
+            var start = ToMillimeterVector(
+                trajectory.StartPoint.PoseInWorld);
+            var end = ToMillimeterVector(
+                trajectory.EndPoint.PoseInWorld);
+            _painter.DrawDot(
+                Color.LimeGreen,
+                start.X,
+                start.Y,
+                8f);
+            _painter.DrawDot(
+                Color.OrangeRed,
+                end.X,
+                end.Y,
+                8f);
+        }
+
+        /// <summary>
+        /// 将组合轨迹控制周期的状态、参考量和最终GCP命令同步给实验记录器。
+        /// </summary>
+        private static void RecordControlCycle(
+            TrackingExperimentRecorder recorder,
+            ParkingGeometricController controller,
+            double controlPointRadiusMeters,
+            WheelFeedbackVehicleStateProvider stateProvider)
+        {
+            if (controller.LastCycleTiming.HasValue)
+            {
+                recorder.RecordControlCycleTiming(
+                    controller.LastCycleTiming.Value,
+                    requestedCommand:
+                        controller.LastRequestedCommand,
+                    sentCommand:
+                        controller.LastCommand);
+            }
+
+            if (controller.LastVehicleState.HasValue)
+            {
+                recorder.UpdateProcessedState(
+                    controller.LastVehicleState.Value);
+            }
+
+            UpdateVelocityDiagnostics(
+                recorder,
+                stateProvider);
+
+            if (!controller.LastCommand.HasValue)
+            {
+                return;
+            }
+
+            if (controller.LastProjection.HasValue &&
+                controller.LastControlReferenceSpeedMetersPerSecond.HasValue)
+            {
+                var projection =
+                    controller.LastProjection.Value;
+                recorder.UpdateControlReference(
+                    projection.ArcLengthMeters,
+                    controller.LastControlReferenceSpeedMetersPerSecond.Value,
+                    projection.LateralErrorMeters,
+                    projection.HeadingErrorRadians,
+                    projection.DistanceToTrajectoryMeters,
+                    projection.RemainingDistanceMeters,
+                    controller.LastCurvaturePreviewDistanceMeters ?? 0.0,
+                    controller.LastFeedforwardCurvaturePerMeter ??
+                    projection.ReferencePoint.CurvaturePerMeter);
+            }
+
+            var requestedCommand =
+                controller.LastRequestedCommand ??
+                controller.LastCommand.Value;
+            var command = controller.LastCommand.Value;
+            recorder.UpdateGcpCommand(
+                requestedCommand.FrontAngleRadians,
+                requestedCommand.RearAngleRadians,
+                command.FrontAngleRadians,
+                command.RearAngleRadians);
+            var curvaturePerMeter = Math.Tan(
+                command.FrontAngleRadians) /
+                controlPointRadiusMeters;
+            var angularSpeedRadiansPerSecond =
+                command.SpeedMetersPerSecond *
+                curvaturePerMeter;
+
+            recorder.UpdateCommand(
+                (float)command.SpeedMetersPerSecond,
+                (float)angularSpeedRadiansPerSecond);
+        }
+
+        /// <summary>
+        /// 将同一周期的Detour速度和轮速解算速度写入实验记录器。
+        /// </summary>
+        private static void UpdateVelocityDiagnostics(
+            TrackingExperimentRecorder recorder,
+            WheelFeedbackVehicleStateProvider stateProvider)
+        {
+            if (stateProvider == null ||
+                !stateProvider.TryGetLatestVelocityDiagnostics(
+                    out var detourBodyVx,
+                    out var detourVelocityValid,
+                    out var rawWheelBodyVx,
+                    out var filteredWheelBodyVx,
+                    out var rawWheelBodyVy,
+                    out var filteredWheelBodyVy,
+                    out var wheelVelocityValid))
+            {
+                return;
+            }
+
+            recorder.UpdateVelocityDiagnostics(
+                detourBodyVx,
+                detourVelocityValid,
+                rawWheelBodyVx,
+                filteredWheelBodyVx,
+                rawWheelBodyVy,
+                filteredWheelBodyVy,
+                wheelVelocityValid);
+        }
+
+        /// <summary>
+        /// 将Shared世界坐标系米制位姿转换为Clumsy绘图和记录器使用的毫米坐标。
+        /// </summary>
+        private static Vector2 ToMillimeterVector(
+            Pose2D poseInWorld)
+        {
+            return new Vector2(
+                (float)(
+                    poseInWorld.XMeters *
+                    MillimetersPerMeter),
+                (float)(
+                    poseInWorld.YMeters *
+                    MillimetersPerMeter));
+        }
+    }
+
+    /// <summary>
+    /// 将舵轮准备到车体左前45°，跟踪直线—左半圆—直线轨迹，并在停车后恢复车头方向。
+    /// </summary>
+    [MovementTest(name = "新版控制器：45°蟹行直线-左半圆-直线轨迹跟踪")]
+    public sealed class NewControllerCrab45StraightSemicircleStraightTest
+        : NewControllerStraightSemicircleStraightTest
+    {
+        /// <summary>
+        /// 使用车体左前45°作为组合轨迹的固定运动方向。
+        /// </summary>
+        protected override double MotionDirectionInBodyRadians =>
+            Math.PI / 4.0;
+
+        /// <summary>
+        /// 只用45°定义参考轨迹，底盘β由整段轨迹自动推导并检查一致性。
+        /// </summary>
+        protected override bool ResolveMotionDirectionFromTrajectory =>
+            true;
+
+        /// <summary>
+        /// 蟹行组合轨迹正常完成后主动将四个舵轮恢复到车头方向。
+        /// </summary>
+        protected override bool ReturnWheelsForwardAfterCompletion =>
+            true;
+
+        /// <summary>
+        /// 将45°蟹行组合实验与普通组合轨迹实验的CSV名称明确区分。
+        /// </summary>
+        protected override string ExperimentTrajectoryBaseName =>
+            "ProfiledCrab45StraightSmoothLeftTurnStraight";
+    }
+}

MultiWheelC/Experiments/RotationTests.cs

@@ -0,0 +1,277 @@
+
+
+using System;
+using System.Globalization;
+using System.Numerics;
+using System.Threading;
+using ClumsyCore;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using CommonUsage.Chassis;
+using FundamentalLib;
+using MDCSToolBox.Clumsy.Movements;
+using MDCSToolBox.Clumsy.Pilot;
+using MyParking.Shared;
+using MultiWheelC.StateEstimation;
+
+namespace MultiWheelC
+{
+    public abstract class InPlaceRotateTestBase : MovementTest
+    {
+        public float RelativeAngleDegrees; // 相对当前航向的旋转角度，逆时针为正。
+        public int TrialNumber = 1; // 重复实验编号。
+        public InPlaceRotationFeedbackMode FeedbackMode =
+            InPlaceRotationFeedbackMode.DetourAbsoluteHeading;
+
+        private DriveTask _task;
+        private TrackingExperimentRecorder _recorder;
+        private readonly string _trajectoryName;
+
+        protected InPlaceRotateTestBase(
+            float relativeAngleDegrees,
+            string trajectoryName)
+        {
+            RelativeAngleDegrees =
+                relativeAngleDegrees;
+            _trajectoryName =
+                trajectoryName;
+        }
+
+        // 从当前Detour航向开始，原地相对旋转指定角度并记录实验数据。
+        public override void Test()
+        {
+            var config = PilotDefinition.Conf;
+
+            if (float.IsNaN(RelativeAngleDegrees) ||
+                float.IsInfinity(RelativeAngleDegrees) ||
+                float.IsNaN(config.InPlaceRotateMaxSpeed) ||
+                float.IsInfinity(config.InPlaceRotateMaxSpeed) ||
+                config.InPlaceRotateMaxSpeed <= 0f ||
+                float.IsNaN(config.InPlaceRotateMinimumSpeed) ||
+                float.IsInfinity(config.InPlaceRotateMinimumSpeed) ||
+                config.InPlaceRotateMinimumSpeed <= 0f ||
+                config.InPlaceRotateMinimumSpeed >
+                    config.InPlaceRotateMaxSpeed)
+            {
+                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 chassis =
+                PilotDefinition.Chassis as MultiWheelChassis;
+            if (chassis == null)
+            {
+                Console.WriteLine(
+                    "当前底盘不是MultiWheelChassis，无法执行原地旋转测试。");
+                return;
+            }
+
+            var stateProvider =
+                ParkingVehicleStateProviderFactory.Create(
+                    chassis);
+            if (!stateProvider.TryGetState(out _))
+            {
+                Console.WriteLine(
+                    "无法读取原地旋转起点状态：" +
+                    stateProvider.LastFailureReason);
+                return;
+            }
+
+            var rotationCenter =
+                new Vector2((float)location.x, (float)location.y);
+            var targetWorldAngle =
+                (float)AngleMath.NormalizeDegrees(
+                    location.th + RelativeAngleDegrees);
+            var movementAngleTarget =
+                FeedbackMode ==
+                    InPlaceRotationFeedbackMode
+                        .RelativeWheelOdometry
+                    ? RelativeAngleDegrees
+                    : targetWorldAngle;
+
+            Console.WriteLine(
+                "原地自转实际参数：" +
+                $"Kp={config.InPlaceRotateKp:F3}，" +
+                $"Ki={config.InPlaceRotateKi:F3}，" +
+                $"Kd={config.InPlaceRotateKd:F3}，" +
+                $"到位误差={config.InPlaceRotateArriveDeg:F2}°，" +
+                $"最小角速度={config.InPlaceRotateMinimumSpeed:F2}°/s，" +
+                $"最大角速度={config.InPlaceRotateMaxSpeed:F2}°/s，" +
+                $"角加速度={config.InPlaceRotateAcc:F2}°/s²，" +
+                $"舵轮到位误差={config.InPlaceRotateWheelAlignDeg:F2}°，" +
+                $"旋转超时={config.InPlaceRotateTimeoutSec:F1}s；" +
+                $"起点航向={location.th:F2}°，" +
+                $"目标航向={targetWorldAngle:F2}°，" +
+                $"反馈模式={FeedbackMode}。");
+            Console.WriteLine(
+                "原地自转CSV保存目录：" +
+                TrackingExperimentRecorder.DefaultOutputDirectory);
+
+            _recorder = new TrackingExperimentRecorder(
+                controllerName:
+                    FeedbackMode ==
+                        InPlaceRotationFeedbackMode
+                            .RelativeWheelOdometry
+                        ? "InPlaceRotateWheelOdometry"
+                        : "InPlaceRotateFilteredPID",
+                trajectoryName: _trajectoryName,
+                trialNumber: TrialNumber,
+                referenceStart: rotationCenter,
+                referenceEnd: rotationCenter,
+                referenceSpeed: 0f,
+                referenceAngularSpeed:
+                    (float)AngleMath.DegreesToRadians(
+                        config.InPlaceRotateMaxSpeed),
+                diagnosticChassis: chassis,
+                diagnosticStateProvider: stateProvider);
+            _recorder.Start();
+
+            try
+            {
+                _task = new DriveTask(
+                    new MultiWheelRotateInPlace
+                    {
+                        AngleTarget = movementAngleTarget,
+                        FeedbackMode = FeedbackMode,
+                        Chassis = chassis,
+                        StateProvider = stateProvider,
+                        CommandAngularSpeedObserver =
+                            commandAngularSpeed =>
+                                _recorder?.UpdateCommand(
+                                    0f,
+                                    (float)AngleMath.DegreesToRadians(
+                                        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();
+        }
+
+        // 读取并校验测试使用的有符号相对旋转角度。
+        protected static bool TryReadRelativeAngleDegrees(
+            out float relativeAngleDegrees)
+        {
+            var input = UI.GetInput(
+                "输入相对旋转角度（deg，正数逆时针，负数顺时针，范围-180到180之间）：");
+
+            if ((!float.TryParse(
+                     input,
+                     NumberStyles.Float,
+                     CultureInfo.CurrentCulture,
+                     out relativeAngleDegrees) &&
+                 !float.TryParse(
+                     input,
+                     NumberStyles.Float,
+                     CultureInfo.InvariantCulture,
+                     out relativeAngleDegrees)) ||
+                float.IsNaN(relativeAngleDegrees) ||
+                float.IsInfinity(relativeAngleDegrees))
+            {
+                Console.WriteLine("旋转角度输入无效，测试已经取消。");
+                return false;
+            }
+
+            if (Math.Abs(relativeAngleDegrees) < 1e-3f)
+            {
+                Console.WriteLine("旋转角度不能为0，测试已经取消。");
+                return false;
+            }
+
+            if (Math.Abs(relativeAngleDegrees) >= 180f)
+            {
+                Console.WriteLine(
+                    "输入角度必须满足-180° < angle < 180°。");
+                return false;
+            }
+
+            return true;
+        }
+
+    }
+
+    [MovementTest(name = "SendXYThSpeed：输入角度原地自转")]
+    public sealed class TestRotateAngle :
+        InPlaceRotateTestBase
+    {
+        public TestRotateAngle()
+            : base(0f, "RotateCustomAngle")
+        {
+        }
+
+        /// <summary>
+        /// 读取相对旋转角度并按正值逆时针、负值顺时针执行原地自转。
+        /// </summary>
+        public override void Test()
+        {
+            if (!TryReadRelativeAngleDegrees(
+                    out var relativeAngleDegrees))
+            {
+                return;
+            }
+
+            RelativeAngleDegrees = relativeAngleDegrees;
+            base.Test();
+        }
+    }
+
+    [MovementTest(name = "轮组里程计：输入角度原地相对自转")]
+    public sealed class TestWheelOdometryRotateAngle :
+        InPlaceRotateTestBase
+    {
+        public TestWheelOdometryRotateAngle()
+            : base(0f, "RotateWheelOdometryCustomAngle")
+        {
+            FeedbackMode =
+                InPlaceRotationFeedbackMode
+                    .RelativeWheelOdometry;
+        }
+
+        /// <summary>
+        /// 读取相对角度并仅用滤波后的轮组角速度积分完成自转。
+        /// </summary>
+        public override void Test()
+        {
+            if (!TryReadRelativeAngleDegrees(
+                    out var relativeAngleDegrees))
+            {
+                return;
+            }
+
+            RelativeAngleDegrees = relativeAngleDegrees;
+            base.Test();
+        }
+    }
+
+}

MultiWheelC/Experiments/TestTrajectoryFactory.cs

@@ -0,0 +1,656 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.Trajectory;
+using MyParking.Shared;
+
+namespace MultiWheelC
+{
+    /// <summary>
+    /// 为新版控制器实验生成不依赖正式规划层的简单世界坐标系参考轨迹。
+    /// </summary>
+    public static class TestTrajectoryFactory
+    {
+        private const double StraightLengthMeters = 4.0;
+
+        /// <summary>
+        /// 从给定车体中心位姿按速度符号沿车头或车尾方向生成带梯形速度规划的4m直线轨迹。
+        /// </summary>
+        public static Trajectory2D CreateStraight4Meters(
+            Pose2D startPoseInWorld,
+            double cruiseSpeedMetersPerSecond = 0.30,
+            double accelerationMetersPerSecondSquared = 0.20,
+            double decelerationMetersPerSecondSquared = 0.20,
+            double pointSpacingMeters = 0.02,
+            double motionDirectionInBodyRadians = 0.0)
+        {
+            return CreateStraight(
+                startPoseInWorld,
+                StraightLengthMeters,
+                cruiseSpeedMetersPerSecond,
+                accelerationMetersPerSecondSquared,
+                decelerationMetersPerSecondSquared,
+                pointSpacingMeters,
+                motionDirectionInBodyRadians);
+        }
+
+        /// <summary>
+        /// 从给定车体中心位姿按速度符号沿车头或车尾方向生成指定长度并在终点停车的直线轨迹。
+        /// </summary>
+        public static Trajectory2D CreateStraight(
+            Pose2D startPoseInWorld,
+            double lengthMeters,
+            double cruiseSpeedMetersPerSecond = 0.30,
+            double accelerationMetersPerSecondSquared = 0.20,
+            double decelerationMetersPerSecondSquared = 0.20,
+            double pointSpacingMeters = 0.02,
+            double motionDirectionInBodyRadians = 0.0)
+        {
+            NumericGuard.EnsureFinite(
+                startPoseInWorld,
+                nameof(startPoseInWorld));
+            NumericGuard.EnsureFinitePositive(
+                lengthMeters,
+                nameof(lengthMeters));
+            var travelDirection = GetTravelDirection(
+                cruiseSpeedMetersPerSecond,
+                nameof(cruiseSpeedMetersPerSecond));
+            NumericGuard.EnsureFinitePositive(
+                accelerationMetersPerSecondSquared,
+                nameof(accelerationMetersPerSecondSquared));
+            NumericGuard.EnsureFinitePositive(
+                decelerationMetersPerSecondSquared,
+                nameof(decelerationMetersPerSecondSquared));
+            NumericGuard.EnsureFinitePositive(
+                pointSpacingMeters,
+                nameof(pointSpacingMeters));
+            NumericGuard.EnsureFinite(
+                motionDirectionInBodyRadians,
+                nameof(motionDirectionInBodyRadians));
+
+            if (pointSpacingMeters > lengthMeters)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(pointSpacingMeters),
+                    "直线轨迹点间距不能大于轨迹总长度。");
+            }
+
+            var segmentCount = (int)Math.Ceiling(
+                lengthMeters /
+                pointSpacingMeters);
+            var points = new List<TrajectoryPoint>(
+                segmentCount + 1);
+            var worldMotionYawRadians =
+                startPoseInWorld.YawRadians +
+                motionDirectionInBodyRadians;
+            var directionX = travelDirection *
+                Math.Cos(worldMotionYawRadians);
+            var directionY = travelDirection *
+                Math.Sin(worldMotionYawRadians);
+
+            for (var index = 0;
+                 index <= segmentCount;
+                 index++)
+            {
+                // 均分后最后一个点严格落在指定终点，避免浮点累加越界。
+                var arcLengthMeters =
+                    lengthMeters *
+                    index /
+                    segmentCount;
+                var remainingDistanceMeters =
+                    lengthMeters -
+                    arcLengthMeters;
+                var referenceSpeedMetersPerSecond =
+                    CalculateReferenceSpeed(
+                        arcLengthMeters,
+                        remainingDistanceMeters,
+                        cruiseSpeedMetersPerSecond,
+                        accelerationMetersPerSecondSquared,
+                        decelerationMetersPerSecondSquared);
+
+                points.Add(
+                    new TrajectoryPoint(
+                        arcLengthMeters,
+                        new Pose2D(
+                            startPoseInWorld.XMeters +
+                                directionX * arcLengthMeters,
+                            startPoseInWorld.YMeters +
+                                directionY * arcLengthMeters,
+                            startPoseInWorld.YawRadians),
+                        curvaturePerMeter: 0.0,
+                        referenceSpeedMetersPerSecond:
+                            referenceSpeedMetersPerSecond));
+            }
+
+            return new Trajectory2D(points);
+        }
+
+        /// <summary>
+        /// 从当前位姿沿指定车体运动方向生成“3m直线、平滑左弯180°、3m直线”的轨迹。
+        /// </summary>
+        public static Trajectory2D CreateStraightLeftSemicircleStraight(
+            Pose2D startPoseInWorld,
+            double straightLengthMeters = 3.0,
+            double turnRadiusMeters = 2.0,
+            double curvatureTransitionLengthMeters = 0.60,
+            double straightMaximumSpeedMetersPerSecond = 0.30,
+            double semicircleMaximumSpeedMetersPerSecond = 0.25,
+            double accelerationMetersPerSecondSquared = 0.20,
+            double decelerationMetersPerSecondSquared = 0.12,
+            double pointSpacingMeters = 0.02,
+            double motionDirectionInBodyRadians = 0.0)
+        {
+            return CreateStraightSmoothLeftTurnStraight(
+                startPoseInWorld,
+                straightLengthMeters,
+                turnRadiusMeters,
+                Math.PI,
+                curvatureTransitionLengthMeters,
+                straightMaximumSpeedMetersPerSecond,
+                semicircleMaximumSpeedMetersPerSecond,
+                accelerationMetersPerSecondSquared,
+                decelerationMetersPerSecondSquared,
+                pointSpacingMeters,
+                motionDirectionInBodyRadians);
+        }
+
+        /// <summary>
+        /// 沿指定车体运动方向生成“直线、平滑左弯、直线”轨迹，并使总转角严格等于指定角度。
+        /// </summary>
+        public static Trajectory2D CreateStraightSmoothLeftTurnStraight(
+            Pose2D startPoseInWorld,
+            double straightLengthMeters,
+            double turnRadiusMeters,
+            double turnAngleRadians,
+            double curvatureTransitionLengthMeters,
+            double straightMaximumSpeedMetersPerSecond,
+            double turnMaximumSpeedMetersPerSecond,
+            double accelerationMetersPerSecondSquared,
+            double decelerationMetersPerSecondSquared,
+            double pointSpacingMeters,
+            double motionDirectionInBodyRadians = 0.0)
+        {
+            NumericGuard.EnsureFinite(
+                startPoseInWorld,
+                nameof(startPoseInWorld));
+            NumericGuard.EnsureFinitePositive(
+                straightLengthMeters,
+                nameof(straightLengthMeters));
+            NumericGuard.EnsureFinitePositive(
+                turnRadiusMeters,
+                nameof(turnRadiusMeters));
+            NumericGuard.EnsureFinitePositive(
+                turnAngleRadians,
+                nameof(turnAngleRadians));
+            NumericGuard.EnsureFinitePositive(
+                curvatureTransitionLengthMeters,
+                nameof(curvatureTransitionLengthMeters));
+            var travelDirection = GetCommonTravelDirection(
+                straightMaximumSpeedMetersPerSecond,
+                nameof(straightMaximumSpeedMetersPerSecond),
+                turnMaximumSpeedMetersPerSecond,
+                nameof(turnMaximumSpeedMetersPerSecond));
+            NumericGuard.EnsureFinitePositive(
+                accelerationMetersPerSecondSquared,
+                nameof(accelerationMetersPerSecondSquared));
+            NumericGuard.EnsureFinitePositive(
+                decelerationMetersPerSecondSquared,
+                nameof(decelerationMetersPerSecondSquared));
+            NumericGuard.EnsureFinitePositive(
+                pointSpacingMeters,
+                nameof(pointSpacingMeters));
+            NumericGuard.EnsureFinite(
+                motionDirectionInBodyRadians,
+                nameof(motionDirectionInBodyRadians));
+
+            if (turnAngleRadians > 2.0 * Math.PI)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(turnAngleRadians),
+                    "单段平滑左转角度不能大于2π。");
+            }
+
+            var nominalTurnArcLengthMeters =
+                turnAngleRadians * turnRadiusMeters;
+            var constantCurvatureLengthMeters =
+                nominalTurnArcLengthMeters -
+                curvatureTransitionLengthMeters;
+
+            if (constantCurvatureLengthMeters <= 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(curvatureTransitionLengthMeters),
+                    "曲率过渡段长度必须小于指定转角对应的圆弧长度。");
+            }
+
+            // 两段平滑过渡的平均曲率均为最大曲率的一半；
+            // 将等曲率段缩短一个过渡长度后，总曲率积分仍严格等于指定转角。
+            var turnLengthMeters =
+                2.0 * curvatureTransitionLengthMeters +
+                constantCurvatureLengthMeters;
+            var turnStartArcLengthMeters =
+                straightLengthMeters;
+            var turnEndArcLengthMeters =
+                straightLengthMeters +
+                turnLengthMeters;
+            var maximumCurvaturePerMeter =
+                1.0 / turnRadiusMeters;
+
+            var sampleArcLengths =
+                BuildCompositeArcLengthSamples(
+                    straightLengthMeters,
+                    curvatureTransitionLengthMeters,
+                    constantCurvatureLengthMeters,
+                    pointSpacingMeters);
+            var speedLimits = new double[
+                sampleArcLengths.Count];
+            var referenceSpeeds = new double[
+                sampleArcLengths.Count];
+
+            for (var index = 0;
+                 index < sampleArcLengths.Count;
+                 index++)
+            {
+                var arcLengthMeters =
+                    sampleArcLengths[index];
+
+                // 整个转弯及两侧曲率过渡段采用转弯限速。
+                speedLimits[index] =
+                    arcLengthMeters >=
+                        turnStartArcLengthMeters &&
+                    arcLengthMeters <=
+                        turnEndArcLengthMeters
+                        ? Math.Abs(
+                            turnMaximumSpeedMetersPerSecond)
+                        : Math.Abs(
+                            straightMaximumSpeedMetersPerSecond);
+            }
+
+            ApplyAccelerationAndBrakingLimits(
+                sampleArcLengths,
+                speedLimits,
+                referenceSpeeds,
+                accelerationMetersPerSecondSquared,
+                decelerationMetersPerSecondSquared);
+
+            for (var index = 0;
+                 index < referenceSpeeds.Length;
+                 index++)
+            {
+                referenceSpeeds[index] *=
+                    travelDirection;
+            }
+
+            var points = new List<TrajectoryPoint>(
+                sampleArcLengths.Count);
+            var worldMotionStartYawRadians =
+                startPoseInWorld.YawRadians +
+                motionDirectionInBodyRadians;
+            var startCos = Math.Cos(
+                worldMotionStartYawRadians);
+            var startSin = Math.Sin(
+                worldMotionStartYawRadians);
+            var localX = 0.0;
+            var localY = 0.0;
+            var localYawRadians = 0.0;
+            var previousArcLengthMeters = 0.0;
+
+            for (var index = 0;
+                 index < sampleArcLengths.Count;
+                 index++)
+            {
+                var arcLengthMeters =
+                    sampleArcLengths[index];
+                if (index > 0)
+                {
+                    var segmentLengthMeters =
+                        arcLengthMeters -
+                        previousArcLengthMeters;
+                    var segmentMiddleArcLengthMeters =
+                        (arcLengthMeters +
+                         previousArcLengthMeters) /
+                        2.0;
+                    var segmentCurvaturePerMeter =
+                        CalculateSmoothTurnCurvature(
+                            segmentMiddleArcLengthMeters -
+                                turnStartArcLengthMeters,
+                            curvatureTransitionLengthMeters,
+                            constantCurvatureLengthMeters,
+                            maximumCurvaturePerMeter);
+                    var segmentYawChangeRadians =
+                        segmentCurvaturePerMeter *
+                        segmentLengthMeters;
+
+                    if (Math.Abs(segmentCurvaturePerMeter) <=
+                        1e-12)
+                    {
+                        localX += travelDirection *
+                            Math.Cos(localYawRadians) *
+                            segmentLengthMeters;
+                        localY += travelDirection *
+                            Math.Sin(localYawRadians) *
+                            segmentLengthMeters;
+                    }
+                    else
+                    {
+                        var nextYawRadians =
+                            localYawRadians +
+                            segmentYawChangeRadians;
+                        localX += travelDirection *
+                            (Math.Sin(nextYawRadians) -
+                             Math.Sin(localYawRadians)) /
+                            segmentCurvaturePerMeter;
+                        localY += travelDirection *
+                            (Math.Cos(localYawRadians) -
+                             Math.Cos(nextYawRadians)) /
+                            segmentCurvaturePerMeter;
+                    }
+
+                    localYawRadians +=
+                        segmentYawChangeRadians;
+                }
+
+                var curvaturePerMeter =
+                    CalculateSmoothTurnCurvature(
+                        arcLengthMeters -
+                            turnStartArcLengthMeters,
+                        curvatureTransitionLengthMeters,
+                        constantCurvatureLengthMeters,
+                        maximumCurvaturePerMeter);
+
+                var worldX =
+                    startPoseInWorld.XMeters +
+                    startCos * localX -
+                    startSin * localY;
+                var worldY =
+                    startPoseInWorld.YMeters +
+                    startSin * localX +
+                    startCos * localY;
+                var worldYawRadians =
+                    AngleMath.NormalizeRadians(
+                        startPoseInWorld.YawRadians +
+                        localYawRadians);
+
+                points.Add(
+                    new TrajectoryPoint(
+                        arcLengthMeters,
+                        new Pose2D(
+                            worldX,
+                            worldY,
+                            worldYawRadians),
+                        curvaturePerMeter,
+                        referenceSpeeds[index]));
+
+                previousArcLengthMeters =
+                    arcLengthMeters;
+            }
+
+            return new Trajectory2D(points);
+        }
+
+        /// <summary>
+        /// 分别采样直线、入弯过渡、等曲率段和出弯过渡，保证所有边界均为精确轨迹点。
+        /// </summary>
+        private static List<double> BuildCompositeArcLengthSamples(
+            double straightLengthMeters,
+            double curvatureTransitionLengthMeters,
+            double constantCurvatureLengthMeters,
+            double pointSpacingMeters)
+        {
+            var samples = new List<double> { 0.0 };
+            var accumulatedArcLengthMeters = 0.0;
+
+            AppendSectionArcLengthSamples(
+                samples,
+                ref accumulatedArcLengthMeters,
+                straightLengthMeters,
+                pointSpacingMeters);
+            AppendSectionArcLengthSamples(
+                samples,
+                ref accumulatedArcLengthMeters,
+                curvatureTransitionLengthMeters,
+                pointSpacingMeters);
+            AppendSectionArcLengthSamples(
+                samples,
+                ref accumulatedArcLengthMeters,
+                constantCurvatureLengthMeters,
+                pointSpacingMeters);
+            AppendSectionArcLengthSamples(
+                samples,
+                ref accumulatedArcLengthMeters,
+                curvatureTransitionLengthMeters,
+                pointSpacingMeters);
+            AppendSectionArcLengthSamples(
+                samples,
+                ref accumulatedArcLengthMeters,
+                straightLengthMeters,
+                pointSpacingMeters);
+
+            return samples;
+        }
+
+        /// <summary>
+        /// 计算平滑左转中连续变化的参考曲率，过渡段两端的曲率变化率均为零。
+        /// </summary>
+        private static double CalculateSmoothTurnCurvature(
+            double distanceInTurnMeters,
+            double transitionLengthMeters,
+            double constantCurvatureLengthMeters,
+            double maximumCurvaturePerMeter)
+        {
+            var totalTurnLengthMeters =
+                2.0 * transitionLengthMeters +
+                constantCurvatureLengthMeters;
+
+            if (distanceInTurnMeters <= 0.0 ||
+                distanceInTurnMeters >= totalTurnLengthMeters)
+            {
+                return 0.0;
+            }
+
+            if (distanceInTurnMeters < transitionLengthMeters)
+            {
+                return maximumCurvaturePerMeter *
+                       SmoothStep01(
+                           distanceInTurnMeters /
+                           transitionLengthMeters);
+            }
+
+            var exitTransitionStartMeters =
+                transitionLengthMeters +
+                constantCurvatureLengthMeters;
+            if (distanceInTurnMeters <=
+                exitTransitionStartMeters)
+            {
+                return maximumCurvaturePerMeter;
+            }
+
+            var exitRatio =
+                (distanceInTurnMeters -
+                 exitTransitionStartMeters) /
+                transitionLengthMeters;
+            return maximumCurvaturePerMeter *
+                   (1.0 - SmoothStep01(exitRatio));
+        }
+
+        /// <summary>
+        /// 将零到一的比例转换为两端一阶导数均为零的三次平滑比例。
+        /// </summary>
+        private static double SmoothStep01(double ratio)
+        {
+            var limitedRatio = Math.Max(
+                0.0,
+                Math.Min(1.0, ratio));
+            return limitedRatio *
+                   limitedRatio *
+                   (3.0 - 2.0 * limitedRatio);
+        }
+
+        /// <summary>
+        /// 将一段指定长度的轨迹追加为均匀弧长采样，并使最后一个点严格落在该段终点。
+        /// </summary>
+        private static void AppendSectionArcLengthSamples(
+            ICollection<double> samples,
+            ref double accumulatedArcLengthMeters,
+            double sectionLengthMeters,
+            double pointSpacingMeters)
+        {
+            var sectionStartArcLengthMeters =
+                accumulatedArcLengthMeters;
+            var segmentCount = (int)Math.Ceiling(
+                sectionLengthMeters /
+                pointSpacingMeters);
+
+            for (var index = 1;
+                 index <= segmentCount;
+                 index++)
+            {
+                samples.Add(
+                    sectionStartArcLengthMeters +
+                    sectionLengthMeters *
+                    index /
+                    segmentCount);
+            }
+
+            accumulatedArcLengthMeters =
+                sectionStartArcLengthMeters +
+                sectionLengthMeters;
+        }
+
+        /// <summary>
+        /// 对逐点速度幅值上限执行前向加速约束和反向制动约束，生成连续可执行的空间速度曲线。
+        /// </summary>
+        private static void ApplyAccelerationAndBrakingLimits(
+            IReadOnlyList<double> arcLengthsMeters,
+            IReadOnlyList<double> speedLimitsMetersPerSecond,
+            double[] referenceSpeedsMetersPerSecond,
+            double accelerationMetersPerSecondSquared,
+            double decelerationMetersPerSecondSquared)
+        {
+            referenceSpeedsMetersPerSecond[0] = 0.0;
+
+            for (var index = 1;
+                 index < arcLengthsMeters.Count;
+                 index++)
+            {
+                var segmentLengthMeters =
+                    arcLengthsMeters[index] -
+                    arcLengthsMeters[index - 1];
+                var accelerationLimitedSpeed = Math.Sqrt(
+                    referenceSpeedsMetersPerSecond[index - 1] *
+                    referenceSpeedsMetersPerSecond[index - 1] +
+                    2.0 *
+                    accelerationMetersPerSecondSquared *
+                    segmentLengthMeters);
+
+                referenceSpeedsMetersPerSecond[index] =
+                    Math.Min(
+                        speedLimitsMetersPerSecond[index],
+                        accelerationLimitedSpeed);
+            }
+
+            var finalIndex =
+                referenceSpeedsMetersPerSecond.Length - 1;
+            referenceSpeedsMetersPerSecond[finalIndex] = 0.0;
+
+            for (var index = finalIndex - 1;
+                 index >= 0;
+                 index--)
+            {
+                var segmentLengthMeters =
+                    arcLengthsMeters[index + 1] -
+                    arcLengthsMeters[index];
+                var brakingLimitedSpeed = Math.Sqrt(
+                    referenceSpeedsMetersPerSecond[index + 1] *
+                    referenceSpeedsMetersPerSecond[index + 1] +
+                    2.0 *
+                    decelerationMetersPerSecondSquared *
+                    segmentLengthMeters);
+
+                referenceSpeedsMetersPerSecond[index] =
+                    Math.Min(
+                        referenceSpeedsMetersPerSecond[index],
+                        brakingLimitedSpeed);
+            }
+        }
+
+        /// <summary>
+        /// 根据起步、巡航和制动能力计算指定弧长位置允许的有符号参考速度。
+        /// </summary>
+        private static double CalculateReferenceSpeed(
+            double arcLengthMeters,
+            double remainingDistanceMeters,
+            double cruiseSpeedMetersPerSecond,
+            double accelerationMetersPerSecondSquared,
+            double decelerationMetersPerSecondSquared)
+        {
+            // 由v²=2as分别得到从静止起步和到终点静止允许的速度上限。
+            var accelerationLimitedSpeed = Math.Sqrt(
+                2.0 *
+                accelerationMetersPerSecondSquared *
+                Math.Max(0.0, arcLengthMeters));
+            var brakingLimitedSpeed = Math.Sqrt(
+                2.0 *
+                decelerationMetersPerSecondSquared *
+                Math.Max(0.0, remainingDistanceMeters));
+
+            var travelDirection = GetTravelDirection(
+                cruiseSpeedMetersPerSecond,
+                nameof(cruiseSpeedMetersPerSecond));
+            var speedMagnitude = Math.Min(
+                Math.Abs(cruiseSpeedMetersPerSecond),
+                Math.Min(
+                    accelerationLimitedSpeed,
+                    brakingLimitedSpeed));
+
+            return travelDirection * speedMagnitude;
+        }
+
+        /// <summary>
+        /// 获取非零有符号速度表示的前进或倒车方向。
+        /// </summary>
+        private static double GetTravelDirection(
+            double signedSpeedMetersPerSecond,
+            string parameterName)
+        {
+            NumericGuard.EnsureFinite(
+                signedSpeedMetersPerSecond,
+                parameterName);
+
+            if (signedSpeedMetersPerSecond == 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "测试轨迹的最大速度不能为零；正值表示前进，负值表示倒车。");
+            }
+
+            return Math.Sign(
+                signedSpeedMetersPerSecond);
+        }
+
+        /// <summary>
+        /// 确保直线段和转弯段速度使用相同的前进或倒车方向。
+        /// </summary>
+        private static double GetCommonTravelDirection(
+            double firstSpeedMetersPerSecond,
+            string firstParameterName,
+            double secondSpeedMetersPerSecond,
+            string secondParameterName)
+        {
+            var firstDirection = GetTravelDirection(
+                firstSpeedMetersPerSecond,
+                firstParameterName);
+            var secondDirection = GetTravelDirection(
+                secondSpeedMetersPerSecond,
+                secondParameterName);
+
+            if (firstDirection != secondDirection)
+            {
+                throw new ArgumentException(
+                    "同一条测试轨迹的直线段和转弯段速度必须同号，不能在运动中直接切换前进与倒车方向。",
+                    firstParameterName);
+            }
+
+            return firstDirection;
+        }
+    }
+}

MultiWheelC/Experiments/WheelPreparationTests.cs

@@ -0,0 +1,70 @@
+
+
+using System;
+using ClumsyCore;
+using ClumsyCore.Pilot;
+using FundamentalLib;
+using MDCSToolBox.Clumsy.Movements;
+using MDCSToolBox.Clumsy.Pilot;
+
+namespace MultiWheelC
+{
+    /// <summary>
+    /// 为需要显式执行舵轮回正的测试管理准备动作及其DriveTask生命周期。
+    /// </summary>
+    internal static class MovementTestPreparation
+    {
+        /// <summary>
+        /// 执行舵轮回正动作并返回四轮是否已经稳定朝向车体前方。
+        /// </summary>
+        public static bool AlignWheelsForward(
+            ref DriveTask activeTask)
+        {
+            var preparation = new PrepareWheelsForward();
+            var task = new DriveTask(preparation.Get());
+            activeTask = task;
+
+            try
+            {
+                task.Wait();
+                return preparation.Completed;
+            }
+            catch (Exception ex)
+            {
+                Console.WriteLine(
+                    $"测试前舵轮回正失败：{ex.Message}");
+                return false;
+            }
+            finally
+            {
+                task.Stop();
+                if (ReferenceEquals(activeTask, task))
+                    activeTask = null;
+            }
+        }
+
+    }
+
+    /// <summary>
+    /// 提供可从测试界面单独触发的四舵轮回正动作。
+    /// </summary>
+    [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;
+        }
+    }
+}

MultiWheelC/Fleet/FleetController.cs

@@ -0,0 +1,219 @@
+using System;
+using MultiWheelC.Control.Abstractions;
+using MultiWheelC.Control.Allocation;
+using MultiWheelC.Control.Execution;
+using MultiWheelC.Trajectory;
+using MyParking.Shared;
+
+namespace MultiWheelC.Fleet
+{
+    // 表示车队中心单周期轨迹控制的计算结果，不包含通信发送结果。
+    public enum FleetControlCycleResult
+    {
+        Inactive = 0,
+        CommandGenerated = 1,
+        Completed = 2,
+        Faulted = 3
+    }
+
+    // 将公共轨迹核心生成的GCP命令转换为车队坐标系下的刚体速度命令。
+    public sealed class FleetController
+    {
+        private readonly PathTrackingCore _trackingCore;
+
+        // virtualControlPointRadiusMeters必须与横向控制器采用的虚拟车队GCP半径一致。
+        public FleetController(
+            ILateralController lateralController,
+            ILongitudinalController longitudinalController,
+            GcpCommandAllocator gcpAllocator,
+            double virtualControlPointRadiusMeters,
+            double finishDistanceMeters = 0.04,
+            double finishSpeedMetersPerSecond = 0.02,
+            double finishHeadingToleranceRadians =
+                3.0 * Math.PI / 180.0,
+            double maximumDistanceToTrajectoryMeters = 0.30,
+            double terminalBrakingPreviewMeters = 0.02,
+            double terminalApproachDistanceMeters = 0.10,
+            double terminalApproachGainPerSecond = 0.8,
+            double maximumTerminalApproachSpeedMetersPerSecond = 0.05,
+            double curvaturePreviewSeconds = 0.20,
+            double maximumCurvaturePreviewMeters = 0.12,
+            double motionDirectionInFleetRadians = 0.0)
+        {
+            NumericGuard.EnsureFinitePositive(
+                virtualControlPointRadiusMeters,
+                nameof(virtualControlPointRadiusMeters));
+            NumericGuard.EnsureFinite(
+                motionDirectionInFleetRadians,
+                nameof(motionDirectionInFleetRadians));
+
+            VirtualControlPointRadiusMeters =
+                virtualControlPointRadiusMeters;
+            MotionDirectionInFleetRadians =
+                AngleMath.NormalizeRadians(
+                    motionDirectionInFleetRadians);
+            _trackingCore = new PathTrackingCore(
+                lateralController,
+                longitudinalController,
+                gcpAllocator,
+                finishDistanceMeters,
+                finishSpeedMetersPerSecond,
+                finishHeadingToleranceRadians,
+                maximumDistanceToTrajectoryMeters,
+                terminalBrakingPreviewMeters,
+                terminalApproachDistanceMeters,
+                terminalApproachGainPerSecond,
+                maximumTerminalApproachSpeedMetersPerSecond,
+                curvaturePreviewSeconds,
+                maximumCurvaturePreviewMeters,
+                MotionDirectionInFleetRadians);
+        }
+
+        // 虚拟车队中心到前、后GCP的距离，单位为m。
+        public double VirtualControlPointRadiusMeters { get; }
+
+        // 当前运动坐标系+X轴相对车队坐标系+X轴的方向，单位为rad。
+        public double MotionDirectionInFleetRadians { get; }
+
+        public double FinishDistanceMeters =>
+            _trackingCore.FinishDistanceMeters;
+
+        public double FinishSpeedMetersPerSecond =>
+            _trackingCore.FinishSpeedMetersPerSecond;
+
+        public double FinishHeadingToleranceRadians =>
+            _trackingCore.FinishHeadingToleranceRadians;
+
+        public double MaximumDistanceToTrajectoryMeters =>
+            _trackingCore.MaximumDistanceToTrajectoryMeters;
+
+        public double TerminalBrakingPreviewMeters =>
+            _trackingCore.TerminalBrakingPreviewMeters;
+
+        public double TerminalApproachDistanceMeters =>
+            _trackingCore.TerminalApproachDistanceMeters;
+
+        public double TerminalApproachGainPerSecond =>
+            _trackingCore.TerminalApproachGainPerSecond;
+
+        public double MaximumTerminalApproachSpeedMetersPerSecond =>
+            _trackingCore.MaximumTerminalApproachSpeedMetersPerSecond;
+
+        public double CurvaturePreviewSeconds =>
+            _trackingCore.CurvaturePreviewSeconds;
+
+        public double MaximumCurvaturePreviewMeters =>
+            _trackingCore.MaximumCurvaturePreviewMeters;
+
+        public bool IsActive => _trackingCore.IsActive;
+
+        public bool IsCompleted => _trackingCore.IsCompleted;
+
+        public string LastFailureReason =>
+            _trackingCore.LastFailureReason;
+
+        public Exception LastException =>
+            _trackingCore.LastException;
+
+        public FleetState? LastFleetState { get; private set; }
+
+        public TrajectoryProjection? LastProjection =>
+            _trackingCore.LastProjection;
+
+        public GcpMotionCommand? LastGcpCommand =>
+            _trackingCore.LastRequestedCommand;
+
+        public FleetMotionCommand? LastCommand { get; private set; }
+
+        // 重置公共核心，并从轨迹起点开始跟踪虚拟车队中心。
+        public void Start(Trajectory2D trajectory)
+        {
+            _trackingCore.Start(trajectory);
+            LastFleetState = null;
+            LastCommand = null;
+        }
+
+        // 计算一周期车队中心命令；非运行状态、完成或故障时返回停车命令。
+        public FleetControlCycleResult ComputeCommand(
+            FleetState fleetState,
+            double deltaTimeSeconds,
+            out FleetMotionCommand command)
+        {
+            NumericGuard.EnsureFinitePositive(
+                deltaTimeSeconds,
+                nameof(deltaTimeSeconds));
+
+            command = FleetMotionCommand.Stop();
+
+            if (!_trackingCore.IsActive)
+            {
+                return FleetControlCycleResult.Inactive;
+            }
+
+            LastFleetState = fleetState;
+            var output = _trackingCore.Compute(
+                fleetState.FleetPoseInWorld,
+                fleetState.TwistAtFleetOriginInFleet,
+                fleetState.HasValidVelocityEstimate,
+                deltaTimeSeconds);
+
+            if (output.Result ==
+                PathTrackingCycleResult.Completed)
+            {
+                LastCommand = command;
+                return FleetControlCycleResult.Completed;
+            }
+
+            if (output.Result ==
+                PathTrackingCycleResult.Faulted)
+            {
+                LastCommand = command;
+                return FleetControlCycleResult.Faulted;
+            }
+
+            if (output.Result !=
+                    PathTrackingCycleResult.CommandGenerated ||
+                !output.Command.HasValue)
+            {
+                return FleetControlCycleResult.Inactive;
+            }
+
+            try
+            {
+                var twistAtFleetOriginInMotionFrame =
+                    GcpKinematics.ToBodyTwist(
+                        output.Command.Value,
+                        VirtualControlPointRadiusMeters);
+                var twistAtFleetOriginInFleet =
+                    FrameTransform2D.TransformTwistAtSamePoint(
+                        new Pose2D(
+                            0.0,
+                            0.0,
+                            MotionDirectionInFleetRadians),
+                        twistAtFleetOriginInMotionFrame);
+                command = new FleetMotionCommand(
+                    Point2D.Zero,
+                    twistAtFleetOriginInFleet);
+                LastCommand = command;
+                return FleetControlCycleResult.CommandGenerated;
+            }
+            catch (Exception exception)
+            {
+                _trackingCore.Fail(
+                    "车队中心GCP命令转换异常：" +
+                    exception.Message,
+                    exception);
+                LastCommand = command;
+                return FleetControlCycleResult.Faulted;
+            }
+        }
+
+        // 取消当前轨迹并使后续周期只生成停车命令。
+        public void Cancel()
+        {
+            _trackingCore.Cancel();
+            LastFleetState = null;
+            LastCommand = null;
+        }
+    }
+}

MultiWheelC/Fleet/FleetCoordinator.cs

@@ -0,0 +1,513 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.Trajectory;
+using MyParking.Shared;
+// 负责运动中：每周期计算每辆车的速度命令
+namespace MultiWheelC.Fleet
+{
+    // 主车单周期车队协调结果，不表示通信或成员底盘执行结果。
+    public enum FleetCoordinationCycleResult
+    {
+        Inactive = 0,
+        WaitingForState = 1,
+        CommandGenerated = 2,
+        Completed = 3,
+        Faulted = 4
+    }
+
+    // 保存本周期的状态估计、车队命令和成员基础命令。
+    public sealed class FleetCoordinationCycleOutput
+    {
+        internal FleetCoordinationCycleOutput(
+            FleetState? state,
+            IReadOnlyList<FleetMemberLayoutError> memberErrors,
+            FleetMotionCommand fleetCommand,
+            IReadOnlyList<FleetMemberCommand> baseMemberCommands,
+            IReadOnlyList<FleetMemberCommand> memberCommands,
+            double speedScale,
+            string reason)
+        {
+            NumericGuard.EnsureFiniteNonNegative(
+                speedScale,
+                nameof(speedScale));
+            if (speedScale > 1.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(speedScale),
+                    "车队统一速度比例不能大于1。");
+            }
+
+            State = state;
+            MemberErrors = memberErrors ??
+                throw new ArgumentNullException(
+                    nameof(memberErrors));
+            FleetCommand = fleetCommand;
+            BaseMemberCommands = baseMemberCommands ??
+                throw new ArgumentNullException(
+                    nameof(baseMemberCommands));
+            MemberCommands = memberCommands ??
+                throw new ArgumentNullException(
+                    nameof(memberCommands));
+            SpeedScale = speedScale;
+            Reason = reason ?? string.Empty;
+        }
+
+        public FleetState? State { get; }
+
+        public IReadOnlyList<FleetMemberLayoutError> MemberErrors { get; }
+
+        public FleetMotionCommand FleetCommand { get; }
+
+        // 仅由车队刚体命令分解得到，尚未叠加成员相对布局纠偏。
+        public IReadOnlyList<FleetMemberCommand> BaseMemberCommands { get; }
+
+        // 已转换到成员当前车体系并叠加小范围相对布局纠偏的最终命令。
+        public IReadOnlyList<FleetMemberCommand> MemberCommands { get; }
+
+        // 因成员布局误差施加到Vx、Vy和Omega的统一比例，范围为[0,1]。
+        public double SpeedScale { get; }
+
+        public string Reason { get; }
+    }
+
+    // 在主车上串联车队状态估计、中心轨迹控制和成员命令分解。
+    public sealed class FleetCoordinator
+    {
+        private static readonly IReadOnlyList<FleetMemberLayoutError>
+            EmptyMemberErrors = Array.AsReadOnly(
+                Array.Empty<FleetMemberLayoutError>());
+        private static readonly IReadOnlyList<FleetMemberCommand>
+            EmptyMemberCommands = Array.AsReadOnly(
+                Array.Empty<FleetMemberCommand>());
+
+        private readonly FleetStateEstimator _stateEstimator;
+        private readonly FleetController _fleetController;
+        private readonly FleetMemberCommandCorrector
+            _memberCommandCorrector;
+        private readonly double _memberPositionErrorWarningMeters;
+        private readonly double _maximumMemberPositionErrorMeters;
+        private readonly double _memberYawErrorWarningRadians;
+        private readonly double _maximumMemberYawErrorRadians;
+
+        private FleetLayout _activeLayout;
+        private bool _isCompleted;
+        private bool _isFaulted;
+
+        public FleetCoordinator(
+            FleetStateEstimator stateEstimator,
+            FleetController fleetController,
+            FleetMemberCommandCorrector memberCommandCorrector,
+            double memberPositionErrorWarningMeters,
+            double maximumMemberPositionErrorMeters,
+            double memberYawErrorWarningRadians,
+            double maximumMemberYawErrorRadians)
+        {
+            _stateEstimator = stateEstimator ??
+                throw new ArgumentNullException(
+                    nameof(stateEstimator));
+            _fleetController = fleetController ??
+                throw new ArgumentNullException(
+                    nameof(fleetController));
+            _memberCommandCorrector =
+                memberCommandCorrector ??
+                throw new ArgumentNullException(
+                    nameof(memberCommandCorrector));
+            NumericGuard.EnsureFiniteNonNegative(
+                memberPositionErrorWarningMeters,
+                nameof(memberPositionErrorWarningMeters));
+            NumericGuard.EnsureFinitePositive(
+                maximumMemberPositionErrorMeters,
+                nameof(maximumMemberPositionErrorMeters));
+            NumericGuard.EnsureFiniteNonNegative(
+                memberYawErrorWarningRadians,
+                nameof(memberYawErrorWarningRadians));
+            NumericGuard.EnsureFinitePositive(
+                maximumMemberYawErrorRadians,
+                nameof(maximumMemberYawErrorRadians));
+
+            if (memberPositionErrorWarningMeters >=
+                maximumMemberPositionErrorMeters)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(memberPositionErrorWarningMeters),
+                    "成员位置误差警告阈值必须小于停止阈值。");
+            }
+
+            if (memberYawErrorWarningRadians >=
+                maximumMemberYawErrorRadians)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(memberYawErrorWarningRadians),
+                    "成员航向误差警告阈值必须小于停止阈值。");
+            }
+
+            if (maximumMemberYawErrorRadians > Math.PI)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(maximumMemberYawErrorRadians),
+                    "成员航向误差上限不能大于π。");
+            }
+
+            _memberPositionErrorWarningMeters =
+                memberPositionErrorWarningMeters;
+            _maximumMemberPositionErrorMeters =
+                maximumMemberPositionErrorMeters;
+            _memberYawErrorWarningRadians =
+                memberYawErrorWarningRadians;
+            _maximumMemberYawErrorRadians =
+                maximumMemberYawErrorRadians;
+            LastFailureReason = string.Empty;
+        }
+
+        public FleetLayout ActiveLayout => _activeLayout;
+
+        public bool IsActive =>
+            _activeLayout != null &&
+            !_isCompleted &&
+            !_isFaulted &&
+            _fleetController.IsActive;
+
+        public bool IsCompleted => _isCompleted;
+
+        public bool IsFaulted => _isFaulted;
+
+        public string LastFailureReason { get; private set; }
+
+        // 运动前成员β准备必须与车队控制器采用同一个车队运动方向。
+        public double MotionDirectionInFleetRadians =>
+            _fleetController.MotionDirectionInFleetRadians;
+
+        public void Start(
+            FleetLayout layout,
+            Trajectory2D trajectory)
+        {
+            if (layout == null)
+            {
+                throw new ArgumentNullException(nameof(layout));
+            }
+
+            if (trajectory == null)
+            {
+                throw new ArgumentNullException(nameof(trajectory));
+            }
+
+            _fleetController.Start(trajectory);
+            _activeLayout = layout;
+            _isCompleted = false;
+            _isFaulted = false;
+            LastFailureReason = string.Empty;
+        }
+
+        public FleetCoordinationCycleResult ExecuteCycle(
+            IReadOnlyList<FleetMemberStateSample> memberStates,
+            double targetTimestampSeconds,
+            double deltaTimeSeconds,
+            out FleetCoordinationCycleOutput output)
+        {
+            if (memberStates == null)
+            {
+                throw new ArgumentNullException(nameof(memberStates));
+            }
+
+            NumericGuard.EnsureFiniteNonNegative(
+                targetTimestampSeconds,
+                nameof(targetTimestampSeconds));
+            NumericGuard.EnsureFinitePositive(
+                deltaTimeSeconds,
+                nameof(deltaTimeSeconds));
+
+            if (_activeLayout == null)
+            {
+                output = CreateOutput(
+                    null,
+                    EmptyMemberErrors,
+                    EmptyMemberCommands,
+                    "车队布局尚未激活。");
+                return FleetCoordinationCycleResult.Inactive;
+            }
+
+            if (_isFaulted)
+            {
+                output = CreateStopOutput(
+                    null,
+                    EmptyMemberErrors,
+                    LastFailureReason);
+                return FleetCoordinationCycleResult.Faulted;
+            }
+
+            if (_isCompleted)
+            {
+                output = CreateStopOutput(
+                    null,
+                    EmptyMemberErrors,
+                    string.Empty);
+                return FleetCoordinationCycleResult.Completed;
+            }
+
+            if (!_fleetController.IsActive)
+            {
+                output = CreateStopOutput(
+                    null,
+                    EmptyMemberErrors,
+                    "车队中心控制器尚未启动或已经取消。");
+                return FleetCoordinationCycleResult.Inactive;
+            }
+
+            var estimate = _stateEstimator.Estimate(
+                _activeLayout,
+                memberStates,
+                targetTimestampSeconds);
+            if (!estimate.IsAvailable ||
+                !estimate.State.HasValue)
+            {
+                output = CreateStopOutput(
+                    null,
+                    estimate.MemberErrors,
+                    estimate.UnavailableReason);
+                return FleetCoordinationCycleResult.WaitingForState;
+            }
+
+            var state = estimate.State.Value;
+            var speedScale = CalculateLayoutSpeedScale(
+                estimate.MemberErrors,
+                out var layoutFailureReason,
+                out var limitingReason);
+            if (layoutFailureReason != null)
+            {
+                return Fail(
+                    state,
+                    estimate.MemberErrors,
+                    layoutFailureReason,
+                    out output);
+            }
+
+            var controlResult =
+                _fleetController.ComputeCommand(
+                    state,
+                    deltaTimeSeconds,
+                    out var fleetCommand);
+
+            if (controlResult ==
+                FleetControlCycleResult.CommandGenerated)
+            {
+                var scaledFleetCommand = ScaleFleetCommand(
+                    fleetCommand,
+                    speedScale);
+                var baseMemberCommands =
+                    FleetKinematics.Decompose(
+                        _activeLayout,
+                        scaledFleetCommand);
+                var memberCommands =
+                    _memberCommandCorrector.Correct(
+                        _activeLayout,
+                        baseMemberCommands,
+                        estimate.MemberErrors,
+                        applyRelativeCorrection:
+                            speedScale >= 1.0);
+                output = new FleetCoordinationCycleOutput(
+                    state,
+                    estimate.MemberErrors,
+                    scaledFleetCommand,
+                    baseMemberCommands,
+                    memberCommands,
+                    speedScale,
+                    limitingReason);
+                return FleetCoordinationCycleResult.CommandGenerated;
+            }
+
+            if (controlResult ==
+                FleetControlCycleResult.Completed)
+            {
+                _isCompleted = true;
+                output = CreateStopOutput(
+                    state,
+                    estimate.MemberErrors,
+                    string.Empty);
+                return FleetCoordinationCycleResult.Completed;
+            }
+
+            if (controlResult ==
+                FleetControlCycleResult.Faulted)
+            {
+                var reason = string.IsNullOrWhiteSpace(
+                        _fleetController.LastFailureReason)
+                    ? "车队中心轨迹控制失败。"
+                    : _fleetController.LastFailureReason;
+                return Fail(
+                    state,
+                    estimate.MemberErrors,
+                    reason,
+                    out output,
+                    cancelController: false);
+            }
+
+            output = CreateStopOutput(
+                state,
+                estimate.MemberErrors,
+                "车队中心控制器当前未生成命令。");
+            return FleetCoordinationCycleResult.Inactive;
+        }
+
+        public void Cancel()
+        {
+            _fleetController.Cancel();
+            _isCompleted = false;
+            _isFaulted = false;
+            LastFailureReason = string.Empty;
+        }
+
+        private double CalculateLayoutSpeedScale(
+            IReadOnlyList<FleetMemberLayoutError> memberErrors,
+            out string failureReason,
+            out string limitingReason)
+        {
+            var speedScale = 1.0;
+            failureReason = null;
+            limitingReason = string.Empty;
+
+            for (var index = 0;
+                 index < memberErrors.Count;
+                 index++)
+            {
+                var memberError = memberErrors[index];
+                var poseError =
+                    memberError.ActualPoseInExpectedVehicleFrame;
+                var positionErrorMeters = Math.Sqrt(
+                    poseError.XMeters * poseError.XMeters +
+                    poseError.YMeters * poseError.YMeters);
+                var yawErrorRadians =
+                    Math.Abs(poseError.YawRadians);
+
+                if (positionErrorMeters >=
+                    _maximumMemberPositionErrorMeters)
+                {
+                    failureReason =
+                        $"车辆{memberError.VehicleId}相对布局位置误差" +
+                        $"{positionErrorMeters:F3}m达到停止阈值。";
+                    return 0.0;
+                }
+
+                if (yawErrorRadians >=
+                    _maximumMemberYawErrorRadians)
+                {
+                    failureReason =
+                        $"车辆{memberError.VehicleId}相对布局航向误差" +
+                        $"{AngleMath.RadiansToDegrees(yawErrorRadians):F2}°" +
+                        "达到停止阈值。";
+                    return 0.0;
+                }
+
+                var positionScale = CalculateScale(
+                    positionErrorMeters,
+                    _memberPositionErrorWarningMeters,
+                    _maximumMemberPositionErrorMeters);
+                if (positionScale < speedScale)
+                {
+                    speedScale = positionScale;
+                    limitingReason =
+                        $"车辆{memberError.VehicleId}相对布局位置误差" +
+                        $"{positionErrorMeters:F3}m，车队统一速度比例" +
+                        $"降至{speedScale:F3}。";
+                }
+
+                var yawScale = CalculateScale(
+                    yawErrorRadians,
+                    _memberYawErrorWarningRadians,
+                    _maximumMemberYawErrorRadians);
+                if (yawScale < speedScale)
+                {
+                    speedScale = yawScale;
+                    limitingReason =
+                        $"车辆{memberError.VehicleId}相对布局航向误差" +
+                        $"{AngleMath.RadiansToDegrees(yawErrorRadians):F2}°，" +
+                        $"车队统一速度比例降至{speedScale:F3}。";
+                }
+            }
+
+            return speedScale;
+        }
+
+        private static double CalculateScale(
+            double errorMagnitude,
+            double warningThreshold,
+            double stopThreshold)
+        {
+            if (errorMagnitude <= warningThreshold)
+            {
+                return 1.0;
+            }
+
+            return (stopThreshold - errorMagnitude) /
+                   (stopThreshold - warningThreshold);
+        }
+
+        private static FleetMotionCommand ScaleFleetCommand(
+            FleetMotionCommand command,
+            double speedScale)
+        {
+            var twist = command.TwistAtReferencePoint;
+            return new FleetMotionCommand(
+                command.ReferencePointInFleet,
+                new Twist2D(
+                    twist.VxMetersPerSecond * speedScale,
+                    twist.VyMetersPerSecond * speedScale,
+                    twist.OmegaRadiansPerSecond * speedScale));
+        }
+
+        private FleetCoordinationCycleResult Fail(
+            FleetState state,
+            IReadOnlyList<FleetMemberLayoutError> memberErrors,
+            string reason,
+            out FleetCoordinationCycleOutput output,
+            bool cancelController = true)
+        {
+            if (cancelController)
+            {
+                _fleetController.Cancel();
+            }
+
+            _isFaulted = true;
+            _isCompleted = false;
+            LastFailureReason = reason ?? string.Empty;
+            output = CreateStopOutput(
+                state,
+                memberErrors,
+                LastFailureReason);
+            return FleetCoordinationCycleResult.Faulted;
+        }
+
+        private FleetCoordinationCycleOutput CreateStopOutput(
+            FleetState? state,
+            IReadOnlyList<FleetMemberLayoutError> memberErrors,
+            string reason)
+        {
+            var stopCommands = _activeLayout == null
+                ? EmptyMemberCommands
+                : FleetKinematics.Decompose(
+                    _activeLayout,
+                    FleetMotionCommand.Stop());
+
+            return CreateOutput(
+                state,
+                memberErrors,
+                stopCommands,
+                reason);
+        }
+
+        private static FleetCoordinationCycleOutput CreateOutput(
+            FleetState? state,
+            IReadOnlyList<FleetMemberLayoutError> memberErrors,
+            IReadOnlyList<FleetMemberCommand> memberCommands,
+            string reason)
+        {
+            return new FleetCoordinationCycleOutput(
+                state,
+                memberErrors,
+                FleetMotionCommand.Stop(),
+                memberCommands,
+                memberCommands,
+                0.0,
+                reason);
+        }
+    }
+}

MultiWheelC/Fleet/FleetLayoutCapture.cs

@@ -0,0 +1,181 @@
+using System;
+using System.Collections.Generic;
+using MyParking.Shared;
+// 夹紧后只执行一次 → 建立固定布局
+namespace MultiWheelC.Fleet
+{
+    // 建立编队时使用的一辆成员车世界位姿快照。
+    public readonly struct FleetMemberPose
+    {
+        public FleetMemberPose(
+            int vehicleId,
+            Pose2D poseInWorld)
+        {
+            if (vehicleId <= 0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(vehicleId),
+                    "编队成员车号必须大于零。");
+            }
+
+            NumericGuard.EnsureFinite(
+                poseInWorld,
+                nameof(poseInWorld));
+
+            VehicleId = vehicleId;
+            PoseInWorld = new Pose2D(
+                poseInWorld.XMeters,
+                poseInWorld.YMeters,
+                AngleMath.NormalizeRadians(
+                    poseInWorld.YawRadians));
+        }
+
+        public int VehicleId { get; }
+
+        public Pose2D PoseInWorld { get; }
+    }
+
+    // 保存布局建立时的车队世界位姿和固定成员布局。
+    public readonly struct FleetLayoutCaptureResult
+    {
+        public FleetLayoutCaptureResult(
+            Pose2D fleetPoseInWorld,
+            FleetLayout layout)
+        {
+            NumericGuard.EnsureFinite(
+                fleetPoseInWorld,
+                nameof(fleetPoseInWorld));
+
+            FleetPoseInWorld = new Pose2D(
+                fleetPoseInWorld.XMeters,
+                fleetPoseInWorld.YMeters,
+                AngleMath.NormalizeRadians(
+                    fleetPoseInWorld.YawRadians));
+            Layout = layout ??
+                throw new ArgumentNullException(
+                    nameof(layout));
+        }
+
+        public Pose2D FleetPoseInWorld { get; }
+
+        public FleetLayout Layout { get; }
+    }
+
+    // 根据同一世界坐标系中的成员位姿建立车队几何中心和固定布局。
+    public static class FleetLayoutCapture
+    {
+        public static FleetLayoutCaptureResult Capture(
+            IReadOnlyList<FleetMemberPose> memberPoses,
+            int leaderVehicleId)
+        {
+            if (memberPoses == null)
+            {
+                throw new ArgumentNullException(
+                    nameof(memberPoses));
+            }
+
+            if (memberPoses.Count == 0)
+            {
+                throw new ArgumentException(
+                    "建立编队布局至少需要一辆成员车。",
+                    nameof(memberPoses));
+            }
+
+            if (leaderVehicleId <= 0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(leaderVehicleId),
+                    "主车车号必须大于零。");
+            }
+
+            var vehicleIds = new HashSet<int>();
+            var centerXMeters = 0.0;
+            var centerYMeters = 0.0;
+            var leaderFound = false;
+            var leaderYawRadians = 0.0;
+
+            for (var index = 0;
+                 index < memberPoses.Count;
+                 index++)
+            {
+                var memberPose = memberPoses[index];
+                if (memberPose.VehicleId <= 0)
+                {
+                    throw new ArgumentOutOfRangeException(
+                        nameof(memberPoses),
+                        $"第{index}辆成员车的车号必须大于零。");
+                }
+
+                NumericGuard.EnsureFinite(
+                    memberPose.PoseInWorld,
+                    $"{nameof(memberPoses)}[{index}]." +
+                    nameof(FleetMemberPose.PoseInWorld));
+
+                if (!vehicleIds.Add(memberPose.VehicleId))
+                {
+                    throw new ArgumentException(
+                        $"成员位姿包含重复车号{memberPose.VehicleId}。",
+                        nameof(memberPoses));
+                }
+
+                centerXMeters +=
+                    memberPose.PoseInWorld.XMeters;
+                centerYMeters +=
+                    memberPose.PoseInWorld.YMeters;
+
+                if (memberPose.VehicleId == leaderVehicleId)
+                {
+                    leaderFound = true;
+                    leaderYawRadians =
+                        memberPose.PoseInWorld.YawRadians;
+                }
+            }
+
+            if (!leaderFound)
+            {
+                throw new ArgumentException(
+                    $"成员位姿中不存在主车{leaderVehicleId}。",
+                    nameof(leaderVehicleId));
+            }
+
+            centerXMeters /= memberPoses.Count;
+            centerYMeters /= memberPoses.Count;
+            NumericGuard.EnsureFinite(
+                centerXMeters,
+                nameof(centerXMeters));
+            NumericGuard.EnsureFinite(
+                centerYMeters,
+                nameof(centerYMeters));
+
+            var fleetPoseInWorld = new Pose2D(
+                centerXMeters,
+                centerYMeters,
+                AngleMath.NormalizeRadians(
+                    leaderYawRadians));
+            var worldPoseInFleet =
+                FrameTransform2D.Inverse(
+                    fleetPoseInWorld);
+            var vehicleLayouts =
+                new VehicleLayout[memberPoses.Count];
+
+            for (var index = 0;
+                 index < memberPoses.Count;
+                 index++)
+            {
+                var memberPose = memberPoses[index];
+                var poseInFleet =
+                    FrameTransform2D.Compose(
+                        worldPoseInFleet,
+                        memberPose.PoseInWorld);
+                vehicleLayouts[index] =
+                    new VehicleLayout(
+                        memberPose.VehicleId,
+                        poseInFleet);
+            }
+
+            return new FleetLayoutCaptureResult(
+                fleetPoseInWorld,
+                new FleetLayout(vehicleLayouts));
+        }
+    }
+}

MultiWheelC/Fleet/FleetMemberAgent.cs

@@ -0,0 +1,569 @@
+using System;
+using MyParking.Shared;
+
+namespace MultiWheelC.Fleet
+{
+    /// <summary>表示成员车在一次车队动作中的本地执行阶段。</summary>
+    public enum FleetMemberAgentState
+    {
+        Idle = 0,
+        Preparing = 1,
+        Ready = 2,
+        Active = 3,
+        Faulted = 4
+    }
+
+    /// <summary>区分固定β滚动运动和车辆中心纯自转的准备方式。</summary>
+    public enum FleetMemberPreparationMode
+    {
+        Rolling = 0,
+        Spin = 1
+    }
+
+    /// <summary>负责一辆成员车的舵轮准备、激活和本车速度命令执行。</summary>
+    public sealed class FleetMemberAgent
+    {
+        private const double MotionDeadband = 1e-6;
+
+        private readonly MultiWheelChassisAdapter _adapter;
+        private readonly double _alignmentToleranceRadians;
+        private readonly double _alignmentStableSeconds;
+
+        private double _alignedDurationSeconds;
+        private double? _lastAcceptedCommandTimeSeconds;
+        private double? _commandDeadlineSeconds;
+
+        /// <summary>创建绑定到一辆多舵轮底盘的成员车执行器。</summary>
+        public FleetMemberAgent(
+            MultiWheelChassisAdapter adapter,
+            double alignmentToleranceRadians,
+            double alignmentStableSeconds)
+        {
+            _adapter = adapter ??
+                throw new ArgumentNullException(nameof(adapter));
+            NumericGuard.EnsureFinitePositive(
+                alignmentToleranceRadians,
+                nameof(alignmentToleranceRadians));
+            NumericGuard.EnsureFiniteNonNegative(
+                alignmentStableSeconds,
+                nameof(alignmentStableSeconds));
+
+            if (alignmentToleranceRadians > Math.PI)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(alignmentToleranceRadians),
+                    "舵轮到位容差不能大于π。");
+            }
+
+            _alignmentToleranceRadians =
+                alignmentToleranceRadians;
+            _alignmentStableSeconds =
+                alignmentStableSeconds;
+            State = FleetMemberAgentState.Idle;
+            LastFailureReason = string.Empty;
+        }
+
+        public int VehicleId => _adapter.VehicleId;
+
+        public FleetMemberAgentState State { get; private set; }
+
+        public FleetMemberPreparationMode? PreparationMode
+        {
+            get;
+            private set;
+        }
+
+        public long CurrentPlanId { get; private set; }
+
+        public double MotionDirectionInBodyRadians
+        {
+            get;
+            private set;
+        }
+
+        public string LastFailureReason { get; private set; }
+
+        // 使用从车本机单调时钟记录，不依赖主车或Detour时间戳。
+        public double? LastAcceptedCommandTimeSeconds =>
+            _lastAcceptedCommandTimeSeconds;
+
+        public double? CommandDeadlineSeconds =>
+            _commandDeadlineSeconds;
+
+        /// <summary>停车并开始准备本车固定β滚动运动系。</summary>
+        public bool BeginRollingPreparation(
+            long planId,
+            double motionDirectionInBodyRadians)
+        {
+            ValidatePlanId(planId);
+            NumericGuard.EnsureFinite(
+                motionDirectionInBodyRadians,
+                nameof(motionDirectionInBodyRadians));
+
+            return BeginPreparation(
+                planId,
+                FleetMemberPreparationMode.Rolling,
+                AngleMath.NormalizeRadians(
+                    motionDirectionInBodyRadians));
+        }
+
+        /// <summary>停车并开始准备车辆中心纯自转所需的舵轮方向。</summary>
+        public bool BeginSpinPreparation(long planId)
+        {
+            ValidatePlanId(planId);
+            return BeginPreparation(
+                planId,
+                FleetMemberPreparationMode.Spin,
+                motionDirectionInBodyRadians: 0.0);
+        }
+
+        /// <summary>检查舵轮是否已连续稳定到位；宿主应在准备阶段周期调用。</summary>
+        public FleetMemberAgentState UpdatePreparation(
+            double deltaTimeSeconds)
+        {
+            NumericGuard.EnsureFinitePositive(
+                deltaTimeSeconds,
+                nameof(deltaTimeSeconds));
+
+            if (State != FleetMemberAgentState.Preparing)
+            {
+                return State;
+            }
+
+            bool aligned;
+            try
+            {
+                aligned = UpdateAndCheckAlignment();
+            }
+            catch (InvalidOperationException exception)
+            {
+                Fail(exception.Message);
+                return State;
+            }
+            catch (ArgumentException exception)
+            {
+                Fail(exception.Message);
+                return State;
+            }
+
+            if (State == FleetMemberAgentState.Faulted)
+            {
+                return State;
+            }
+
+            _alignedDurationSeconds = aligned
+                ? _alignedDurationSeconds + deltaTimeSeconds
+                : 0.0;
+
+            if (aligned &&
+                _alignedDurationSeconds >=
+                _alignmentStableSeconds)
+            {
+                State = FleetMemberAgentState.Ready;
+                LastFailureReason = string.Empty;
+            }
+
+            return State;
+        }
+
+        /// <summary>在主车确认全队Ready后激活本车已经准备好的运动方式。</summary>
+        public bool Activate(
+            long planId,
+            double commandReceivedTimeSeconds,
+            double validForSeconds)
+        {
+            NumericGuard.EnsureFiniteNonNegative(
+                commandReceivedTimeSeconds,
+                nameof(commandReceivedTimeSeconds));
+            NumericGuard.EnsureFinitePositive(
+                validForSeconds,
+                nameof(validForSeconds));
+
+            if (planId != CurrentPlanId)
+            {
+                LastFailureReason =
+                    "激活任务编号与当前准备任务不一致。";
+                return false;
+            }
+
+            if (State == FleetMemberAgentState.Active)
+            {
+                // 重复激活只允许幂等确认，不能替代周期运动命令延长车辆运动时间。
+                return UpdateCommandWatchdog(
+                    commandReceivedTimeSeconds);
+            }
+
+            if (State != FleetMemberAgentState.Ready ||
+                !PreparationMode.HasValue)
+            {
+                return RejectWhileStopped(
+                    "成员车尚未完成舵轮准备。");
+            }
+
+            bool stillAligned;
+            try
+            {
+                stillAligned = ArePreparedWheelsStillAligned();
+            }
+            catch (InvalidOperationException exception)
+            {
+                return Fail(exception.Message);
+            }
+            catch (ArgumentException exception)
+            {
+                return Fail(exception.Message);
+            }
+
+            if (!stillAligned)
+            {
+                State = FleetMemberAgentState.Preparing;
+                _alignedDurationSeconds = 0.0;
+                return RejectWhileStopped(
+                    "成员车在激活前失去舵轮到位状态。");
+            }
+
+            try
+            {
+                if (PreparationMode.Value ==
+                    FleetMemberPreparationMode.Rolling)
+                {
+                    _adapter.ActivateMotionFrame(
+                        MotionDirectionInBodyRadians);
+                }
+                else if (!_adapter.AdoptPreparedSpinForXYTh(
+                             _alignmentToleranceRadians))
+                {
+                    return Fail(
+                        BuildAdapterFailureReason(
+                            "无法激活已经准备好的原地自转舵轮。"));
+                }
+            }
+            catch (InvalidOperationException exception)
+            {
+                return Fail(exception.Message);
+            }
+            catch (ArgumentException exception)
+            {
+                return Fail(exception.Message);
+            }
+
+            State = FleetMemberAgentState.Active;
+            AcceptCommandDeadline(
+                commandReceivedTimeSeconds,
+                validForSeconds);
+            LastFailureReason = string.Empty;
+            return true;
+        }
+
+        /// <summary>校验任务和车号后执行分配给本车的车体系速度命令。</summary>
+        public bool Execute(
+            long planId,
+            FleetMemberCommand command,
+            double commandReceivedTimeSeconds,
+            double validForSeconds,
+            TimeSpan? interval = null)
+        {
+            ValidatePlanId(planId);
+            NumericGuard.EnsureFinite(
+                command.TwistInVehicleBody,
+                nameof(command));
+            NumericGuard.EnsureFiniteNonNegative(
+                commandReceivedTimeSeconds,
+                nameof(commandReceivedTimeSeconds));
+            NumericGuard.EnsureFinitePositive(
+                validForSeconds,
+                nameof(validForSeconds));
+
+            if (planId != CurrentPlanId)
+            {
+                return Fail(
+                    "速度命令任务编号与当前激活任务不一致。");
+            }
+
+            if (command.VehicleId != VehicleId)
+            {
+                return Fail(
+                    $"速度命令属于车辆{command.VehicleId}，" +
+                    $"当前成员车号为{VehicleId}。");
+            }
+
+            if (State != FleetMemberAgentState.Active ||
+                !PreparationMode.HasValue)
+            {
+                return RejectWhileStopped(
+                    "成员车尚未激活，不能执行速度命令。");
+            }
+
+            // 先检查上一条命令是否已经过期，禁止失联后由迟到命令自动恢复运动。
+            if (!UpdateCommandWatchdog(
+                    commandReceivedTimeSeconds))
+            {
+                return false;
+            }
+
+            if (!IsCommandCompatibleWithPreparation(
+                    command.TwistInVehicleBody))
+            {
+                return Fail(
+                    "速度命令与本次舵轮准备方式不一致。");
+            }
+
+            try
+            {
+                if (!_adapter.SendBodyTwist(
+                        command.TwistInVehicleBody,
+                        interval))
+                {
+                    return Fail(
+                        BuildAdapterFailureReason(
+                            "成员车底盘拒绝执行速度命令。"));
+                }
+            }
+            catch (InvalidOperationException exception)
+            {
+                return Fail(exception.Message);
+            }
+            catch (ArgumentException exception)
+            {
+                return Fail(exception.Message);
+            }
+
+            AcceptCommandDeadline(
+                commandReceivedTimeSeconds,
+                validForSeconds);
+            LastFailureReason = string.Empty;
+            return true;
+        }
+
+        // 运行循环即使没有收到新命令也必须调用本方法，超时后会本地停车并锁存Faulted。
+        public bool UpdateCommandWatchdog(
+            double currentTimeSeconds)
+        {
+            NumericGuard.EnsureFiniteNonNegative(
+                currentTimeSeconds,
+                nameof(currentTimeSeconds));
+
+            if (State == FleetMemberAgentState.Faulted)
+            {
+                return false;
+            }
+
+            if (State != FleetMemberAgentState.Active)
+            {
+                return true;
+            }
+
+            if (!_lastAcceptedCommandTimeSeconds.HasValue ||
+                !_commandDeadlineSeconds.HasValue)
+            {
+                return Fail(
+                    "成员车已经激活，但本地命令看门狗尚未初始化。");
+            }
+
+            if (currentTimeSeconds <
+                _lastAcceptedCommandTimeSeconds.Value)
+            {
+                return Fail(
+                    "成员车本地单调时钟发生倒退，无法继续校验命令时效。");
+            }
+
+            if (currentTimeSeconds <=
+                _commandDeadlineSeconds.Value)
+            {
+                return true;
+            }
+
+            var commandAgeSeconds =
+                currentTimeSeconds -
+                _lastAcceptedCommandTimeSeconds.Value;
+            return Fail(
+                "成员车等待主车有效命令超时，" +
+                $"最近一次命令距今{commandAgeSeconds:F3}s。");
+        }
+
+        /// <summary>正常取消当前任务并立即停止驱动轮。</summary>
+        public void Stop()
+        {
+            _adapter.StopImmediately();
+            State = FleetMemberAgentState.Idle;
+            PreparationMode = null;
+            CurrentPlanId = 0;
+            MotionDirectionInBodyRadians = 0.0;
+            _alignedDurationSeconds = 0.0;
+            ClearCommandWatchdog();
+            LastFailureReason = string.Empty;
+        }
+
+        /// <summary>重置上一动作并下发本次滚动或自转舵轮准备目标。</summary>
+        private bool BeginPreparation(
+            long planId,
+            FleetMemberPreparationMode mode,
+            double motionDirectionInBodyRadians)
+        {
+            try
+            {
+                _adapter.StopImmediately();
+                _adapter.ResetToBodyFrame();
+
+                CurrentPlanId = planId;
+                PreparationMode = mode;
+                MotionDirectionInBodyRadians =
+                    motionDirectionInBodyRadians;
+                State = FleetMemberAgentState.Preparing;
+                LastFailureReason = string.Empty;
+                _alignedDurationSeconds = 0.0;
+                ClearCommandWatchdog();
+
+                var accepted = mode ==
+                    FleetMemberPreparationMode.Rolling
+                        ? _adapter.PrepareParallelDirection(
+                            motionDirectionInBodyRadians)
+                        : _adapter.PrepareSpin(
+                            alignmentToleranceDegrees:
+                                AngleMath.RadiansToDegrees(
+                                    _alignmentToleranceRadians));
+
+                if (!accepted)
+                {
+                    return Fail(
+                        BuildAdapterFailureReason(
+                            "成员车底盘拒绝舵轮准备目标。"));
+                }
+
+                return true;
+            }
+            catch (InvalidOperationException exception)
+            {
+                return Fail(exception.Message);
+            }
+            catch (ArgumentException exception)
+            {
+                return Fail(exception.Message);
+            }
+        }
+
+        /// <summary>更新当前准备目标并读取舵轮到位状态。</summary>
+        private bool UpdateAndCheckAlignment()
+        {
+            if (PreparationMode ==
+                FleetMemberPreparationMode.Rolling)
+            {
+                return _adapter.AreParallelWheelsAligned(
+                    MotionDirectionInBodyRadians,
+                    _alignmentToleranceRadians);
+            }
+
+            if (!_adapter.PrepareSpin(
+                    alignmentToleranceDegrees:
+                        AngleMath.RadiansToDegrees(
+                            _alignmentToleranceRadians)))
+            {
+                Fail(
+                    BuildAdapterFailureReason(
+                        "成员车底盘无法继续更新原地自转准备。"));
+                return false;
+            }
+
+            return _adapter.AreSpinWheelsAligned;
+        }
+
+        /// <summary>确认舵轮在全队释放前仍保持到位。</summary>
+        private bool ArePreparedWheelsStillAligned()
+        {
+            return PreparationMode ==
+                   FleetMemberPreparationMode.Rolling
+                ? _adapter.AreParallelWheelsAligned(
+                    MotionDirectionInBodyRadians,
+                    _alignmentToleranceRadians)
+                : _adapter.AreSpinWheelsAligned;
+        }
+
+        /// <summary>禁止滚动准备执行纯自转，也禁止自转准备执行平移。</summary>
+        private bool IsCommandCompatibleWithPreparation(
+            Twist2D bodyTwist)
+        {
+            var linearSpeed = Math.Sqrt(
+                bodyTwist.VxMetersPerSecond *
+                bodyTwist.VxMetersPerSecond +
+                bodyTwist.VyMetersPerSecond *
+                bodyTwist.VyMetersPerSecond);
+            var hasLinearMotion =
+                linearSpeed > MotionDeadband;
+            var hasAngularMotion =
+                Math.Abs(
+                    bodyTwist.OmegaRadiansPerSecond) >
+                MotionDeadband;
+
+            if (!hasLinearMotion && !hasAngularMotion)
+            {
+                return true;
+            }
+
+            return PreparationMode ==
+                   FleetMemberPreparationMode.Rolling
+                ? hasLinearMotion
+                : !hasLinearMotion && hasAngularMotion;
+        }
+
+        /// <summary>拒绝未满足执行条件的命令并保持车辆零速。</summary>
+        private bool RejectWhileStopped(string reason)
+        {
+            _adapter.StopImmediately();
+            LastFailureReason = reason ?? string.Empty;
+            return false;
+        }
+
+        private void AcceptCommandDeadline(
+            double commandReceivedTimeSeconds,
+            double validForSeconds)
+        {
+            var commandDeadlineSeconds =
+                commandReceivedTimeSeconds +
+                validForSeconds;
+            NumericGuard.EnsureFinite(
+                commandDeadlineSeconds,
+                nameof(validForSeconds));
+
+            _lastAcceptedCommandTimeSeconds =
+                commandReceivedTimeSeconds;
+            _commandDeadlineSeconds =
+                commandDeadlineSeconds;
+        }
+
+        private void ClearCommandWatchdog()
+        {
+            _lastAcceptedCommandTimeSeconds = null;
+            _commandDeadlineSeconds = null;
+        }
+
+        /// <summary>锁存成员车故障并立即清零驱动轮速度。</summary>
+        private bool Fail(string reason)
+        {
+            _adapter.StopImmediately();
+            State = FleetMemberAgentState.Faulted;
+            LastFailureReason = reason ?? string.Empty;
+            return false;
+        }
+
+        /// <summary>优先返回底盘提供的具体失败原因。</summary>
+        private string BuildAdapterFailureReason(
+            string fallbackReason)
+        {
+            return string.IsNullOrWhiteSpace(
+                    _adapter.LastFailureReason)
+                ? fallbackReason
+                : _adapter.LastFailureReason;
+        }
+
+        /// <summary>拒绝零值和负值任务编号。</summary>
+        private static void ValidatePlanId(long planId)
+        {
+            if (planId <= 0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(planId),
+                    "车队动作任务编号必须大于零。");
+            }
+        }
+    }
+}

MultiWheelC/Fleet/FleetMemberCommandCorrector.cs

@@ -0,0 +1,433 @@
+using System;
+using System.Collections.Generic;
+using MyParking.Shared;
+
+namespace MultiWheelC.Fleet
+{
+    // 将小范围成员布局误差转换为不改变车队整体刚体运动的相对速度修正。
+    public sealed class FleetMemberCommandCorrector
+    {
+        private const double GeometryTolerance = 1e-12;
+
+        private readonly double _longitudinalPositionGainPerSecond;
+        private readonly double _lateralPositionGainPerSecond;
+        private readonly double _yawGainPerSecond;
+        private readonly double _positionErrorDeadbandMeters;
+        private readonly double _yawErrorDeadbandRadians;
+        private readonly double _maximumLinearCorrectionMetersPerSecond;
+        private readonly double _maximumAngularCorrectionRadiansPerSecond;
+
+        public FleetMemberCommandCorrector(
+            double longitudinalPositionGainPerSecond,
+            double lateralPositionGainPerSecond,
+            double yawGainPerSecond,
+            double positionErrorDeadbandMeters,
+            double yawErrorDeadbandRadians,
+            double maximumLinearCorrectionMetersPerSecond,
+            double maximumAngularCorrectionRadiansPerSecond)
+        {
+            NumericGuard.EnsureFiniteNonNegative(
+                longitudinalPositionGainPerSecond,
+                nameof(longitudinalPositionGainPerSecond));
+            NumericGuard.EnsureFiniteNonNegative(
+                lateralPositionGainPerSecond,
+                nameof(lateralPositionGainPerSecond));
+            NumericGuard.EnsureFiniteNonNegative(
+                yawGainPerSecond,
+                nameof(yawGainPerSecond));
+            NumericGuard.EnsureFiniteNonNegative(
+                positionErrorDeadbandMeters,
+                nameof(positionErrorDeadbandMeters));
+            NumericGuard.EnsureFiniteNonNegative(
+                yawErrorDeadbandRadians,
+                nameof(yawErrorDeadbandRadians));
+            NumericGuard.EnsureFinitePositive(
+                maximumLinearCorrectionMetersPerSecond,
+                nameof(maximumLinearCorrectionMetersPerSecond));
+            NumericGuard.EnsureFinitePositive(
+                maximumAngularCorrectionRadiansPerSecond,
+                nameof(maximumAngularCorrectionRadiansPerSecond));
+
+            if (yawErrorDeadbandRadians > Math.PI)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(yawErrorDeadbandRadians),
+                    "成员航向误差死区不能大于π。");
+            }
+
+            _longitudinalPositionGainPerSecond =
+                longitudinalPositionGainPerSecond;
+            _lateralPositionGainPerSecond =
+                lateralPositionGainPerSecond;
+            _yawGainPerSecond = yawGainPerSecond;
+            _positionErrorDeadbandMeters =
+                positionErrorDeadbandMeters;
+            _yawErrorDeadbandRadians =
+                yawErrorDeadbandRadians;
+            _maximumLinearCorrectionMetersPerSecond =
+                maximumLinearCorrectionMetersPerSecond;
+            _maximumAngularCorrectionRadiansPerSecond =
+                maximumAngularCorrectionRadiansPerSecond;
+        }
+
+        public IReadOnlyList<FleetMemberCommand> Correct(
+            FleetLayout layout,
+            IReadOnlyList<FleetMemberCommand> baseCommands,
+            IReadOnlyList<FleetMemberLayoutError> memberErrors,
+            bool applyRelativeCorrection = true)
+        {
+            if (layout == null)
+            {
+                throw new ArgumentNullException(nameof(layout));
+            }
+
+            if (baseCommands == null)
+            {
+                throw new ArgumentNullException(nameof(baseCommands));
+            }
+
+            if (memberErrors == null)
+            {
+                throw new ArgumentNullException(nameof(memberErrors));
+            }
+
+            if (baseCommands.Count != layout.VehicleCount)
+            {
+                throw new ArgumentException(
+                    "成员基础命令数量必须与车队布局一致。",
+                    nameof(baseCommands));
+            }
+
+            if (memberErrors.Count != layout.VehicleCount)
+            {
+                throw new ArgumentException(
+                    "成员布局误差数量必须与车队布局一致。",
+                    nameof(memberErrors));
+            }
+
+            var commandsByVehicleId =
+                IndexCommands(baseCommands);
+            var errorsByVehicleId =
+                IndexErrors(memberErrors);
+            var orderedCommands =
+                new FleetMemberCommand[layout.VehicleCount];
+            var orderedErrors =
+                new FleetMemberLayoutError[layout.VehicleCount];
+            var rawCorrectionsInFleet =
+                new Twist2D[layout.VehicleCount];
+
+            for (var index = 0;
+                 index < layout.Vehicles.Count;
+                 index++)
+            {
+                var vehicleLayout = layout.Vehicles[index];
+                if (!commandsByVehicleId.TryGetValue(
+                        vehicleLayout.VehicleId,
+                        out var baseCommand))
+                {
+                    throw new ArgumentException(
+                        $"缺少车辆{vehicleLayout.VehicleId}的基础命令。",
+                        nameof(baseCommands));
+                }
+
+                if (!errorsByVehicleId.TryGetValue(
+                        vehicleLayout.VehicleId,
+                        out var memberError))
+                {
+                    throw new ArgumentException(
+                        $"缺少车辆{vehicleLayout.VehicleId}的布局误差。",
+                        nameof(memberErrors));
+                }
+
+                orderedCommands[index] = baseCommand;
+                orderedErrors[index] = memberError;
+                rawCorrectionsInFleet[index] =
+                    applyRelativeCorrection
+                        ? CalculateRawCorrectionInFleet(
+                            vehicleLayout,
+                            memberError)
+                        : Twist2D.Zero;
+            }
+
+            var relativeCorrectionsInFleet =
+                RemoveCommonRigidMotion(
+                    layout,
+                    rawCorrectionsInFleet);
+            var correctedCommands =
+                new FleetMemberCommand[layout.VehicleCount];
+
+            for (var index = 0;
+                 index < layout.Vehicles.Count;
+                 index++)
+            {
+                var vehicleLayout = layout.Vehicles[index];
+                var baseTwistInFleet =
+                    FrameTransform2D.TransformTwistAtSamePoint(
+                        vehicleLayout.PoseInFleet,
+                        orderedCommands[index].TwistInVehicleBody);
+                var correctionInFleet = LimitCorrection(
+                    relativeCorrectionsInFleet[index]);
+                var correctedTwistInFleet = Add(
+                    baseTwistInFleet,
+                    correctionInFleet);
+
+                // 使用成员当前相对姿态表达最终命令，避免小航向误差造成坐标表达偏差。
+                var actualPoseInFleet =
+                    FrameTransform2D.Compose(
+                        vehicleLayout.PoseInFleet,
+                        orderedErrors[index]
+                            .ActualPoseInExpectedVehicleFrame);
+                var fleetPoseInActualVehicle =
+                    FrameTransform2D.Inverse(
+                        actualPoseInFleet);
+                var correctedTwistInVehicleBody =
+                    FrameTransform2D.TransformTwistAtSamePoint(
+                        fleetPoseInActualVehicle,
+                        correctedTwistInFleet);
+
+                correctedCommands[index] =
+                    new FleetMemberCommand(
+                        vehicleLayout.VehicleId,
+                        correctedTwistInVehicleBody);
+            }
+
+            return Array.AsReadOnly(correctedCommands);
+        }
+
+        private Twist2D CalculateRawCorrectionInFleet(
+            VehicleLayout vehicleLayout,
+            FleetMemberLayoutError memberError)
+        {
+            var error =
+                memberError.ActualPoseInExpectedVehicleFrame;
+            var correctionInExpectedVehicle = new Twist2D(
+                -_longitudinalPositionGainPerSecond *
+                ApplyDeadband(
+                    error.XMeters,
+                    _positionErrorDeadbandMeters),
+                -_lateralPositionGainPerSecond *
+                ApplyDeadband(
+                    error.YMeters,
+                    _positionErrorDeadbandMeters),
+                -_yawGainPerSecond *
+                ApplyDeadband(
+                    error.YawRadians,
+                    _yawErrorDeadbandRadians));
+
+            return FrameTransform2D.TransformTwistAtSamePoint(
+                vehicleLayout.PoseInFleet,
+                correctionInExpectedVehicle);
+        }
+
+        private static Twist2D[] RemoveCommonRigidMotion(
+            FleetLayout layout,
+            IReadOnlyList<Twist2D> rawCorrectionsInFleet)
+        {
+            var count = layout.VehicleCount;
+            var meanX = 0.0;
+            var meanY = 0.0;
+            var meanVx = 0.0;
+            var meanVy = 0.0;
+            var meanOmega = 0.0;
+
+            for (var index = 0; index < count; index++)
+            {
+                var position = layout.Vehicles[index].PoseInFleet;
+                var correction = rawCorrectionsInFleet[index];
+                meanX += position.XMeters;
+                meanY += position.YMeters;
+                meanVx += correction.VxMetersPerSecond;
+                meanVy += correction.VyMetersPerSecond;
+                meanOmega += correction.OmegaRadiansPerSecond;
+            }
+
+            meanX /= count;
+            meanY /= count;
+            meanVx /= count;
+            meanVy /= count;
+            meanOmega /= count;
+
+            var rotationalNumerator = 0.0;
+            var rotationalDenominator = 0.0;
+            for (var index = 0; index < count; index++)
+            {
+                var position = layout.Vehicles[index].PoseInFleet;
+                var correction = rawCorrectionsInFleet[index];
+                var centeredX = position.XMeters - meanX;
+                var centeredY = position.YMeters - meanY;
+                var centeredVx =
+                    correction.VxMetersPerSecond - meanVx;
+                var centeredVy =
+                    correction.VyMetersPerSecond - meanVy;
+
+                rotationalNumerator +=
+                    -centeredY * centeredVx +
+                    centeredX * centeredVy;
+                rotationalDenominator +=
+                    centeredX * centeredX +
+                    centeredY * centeredY;
+            }
+
+            var commonOmegaFromTranslation =
+                rotationalDenominator <= GeometryTolerance
+                    ? 0.0
+                    : rotationalNumerator /
+                      rotationalDenominator;
+            var commonVxAtFleetOrigin =
+                meanVx +
+                commonOmegaFromTranslation * meanY;
+            var commonVyAtFleetOrigin =
+                meanVy -
+                commonOmegaFromTranslation * meanX;
+            var relativeCorrections = new Twist2D[count];
+
+            for (var index = 0; index < count; index++)
+            {
+                var position = layout.Vehicles[index].PoseInFleet;
+                var correction = rawCorrectionsInFleet[index];
+                var commonVxAtMember =
+                    commonVxAtFleetOrigin -
+                    commonOmegaFromTranslation *
+                    position.YMeters;
+                var commonVyAtMember =
+                    commonVyAtFleetOrigin +
+                    commonOmegaFromTranslation *
+                    position.XMeters;
+
+                relativeCorrections[index] = new Twist2D(
+                    correction.VxMetersPerSecond -
+                    commonVxAtMember,
+                    correction.VyMetersPerSecond -
+                    commonVyAtMember,
+                    correction.OmegaRadiansPerSecond -
+                    meanOmega);
+            }
+
+            return relativeCorrections;
+        }
+
+        private Twist2D LimitCorrection(Twist2D correction)
+        {
+            var linearMagnitude = Math.Sqrt(
+                correction.VxMetersPerSecond *
+                correction.VxMetersPerSecond +
+                correction.VyMetersPerSecond *
+                correction.VyMetersPerSecond);
+            var linearScale =
+                linearMagnitude <=
+                _maximumLinearCorrectionMetersPerSecond
+                    ? 1.0
+                    : _maximumLinearCorrectionMetersPerSecond /
+                      linearMagnitude;
+            var limitedOmega = Math.Max(
+                -_maximumAngularCorrectionRadiansPerSecond,
+                Math.Min(
+                    _maximumAngularCorrectionRadiansPerSecond,
+                    correction.OmegaRadiansPerSecond));
+
+            return new Twist2D(
+                correction.VxMetersPerSecond * linearScale,
+                correction.VyMetersPerSecond * linearScale,
+                limitedOmega);
+        }
+
+        private static Dictionary<int, FleetMemberCommand>
+            IndexCommands(
+                IReadOnlyList<FleetMemberCommand> commands)
+        {
+            var indexed =
+                new Dictionary<int, FleetMemberCommand>(
+                    commands.Count);
+
+            for (var index = 0; index < commands.Count; index++)
+            {
+                var command = commands[index];
+                if (command.VehicleId <= 0)
+                {
+                    throw new ArgumentOutOfRangeException(
+                        nameof(commands),
+                        $"第{index}个成员命令的车号无效。");
+                }
+
+                NumericGuard.EnsureFinite(
+                    command.TwistInVehicleBody,
+                    $"{nameof(commands)}[{index}]." +
+                    nameof(FleetMemberCommand.TwistInVehicleBody));
+
+                if (indexed.ContainsKey(command.VehicleId))
+                {
+                    throw new ArgumentException(
+                        $"成员命令包含重复车号{command.VehicleId}。",
+                        nameof(commands));
+                }
+
+                indexed.Add(command.VehicleId, command);
+            }
+
+            return indexed;
+        }
+
+        private static Dictionary<int, FleetMemberLayoutError>
+            IndexErrors(
+                IReadOnlyList<FleetMemberLayoutError> errors)
+        {
+            var indexed =
+                new Dictionary<int, FleetMemberLayoutError>(
+                    errors.Count);
+
+            for (var index = 0; index < errors.Count; index++)
+            {
+                var error = errors[index];
+                if (error.VehicleId <= 0)
+                {
+                    throw new ArgumentOutOfRangeException(
+                        nameof(errors),
+                        $"第{index}个布局误差的车号无效。");
+                }
+
+                NumericGuard.EnsureFinite(
+                    error.ActualPoseInExpectedVehicleFrame,
+                    $"{nameof(errors)}[{index}]." +
+                    nameof(FleetMemberLayoutError
+                        .ActualPoseInExpectedVehicleFrame));
+
+                if (indexed.ContainsKey(error.VehicleId))
+                {
+                    throw new ArgumentException(
+                        $"成员布局误差包含重复车号{error.VehicleId}。",
+                        nameof(errors));
+                }
+
+                indexed.Add(error.VehicleId, error);
+            }
+
+            return indexed;
+        }
+
+        private static double ApplyDeadband(
+            double value,
+            double deadband)
+        {
+            var magnitude = Math.Abs(value);
+            if (magnitude <= deadband)
+            {
+                return 0.0;
+            }
+
+            return Math.Sign(value) * (magnitude - deadband);
+        }
+
+        private static Twist2D Add(
+            Twist2D first,
+            Twist2D second)
+        {
+            return new Twist2D(
+                first.VxMetersPerSecond +
+                second.VxMetersPerSecond,
+                first.VyMetersPerSecond +
+                second.VyMetersPerSecond,
+                first.OmegaRadiansPerSecond +
+                second.OmegaRadiansPerSecond);
+        }
+    }
+}

MultiWheelC/Fleet/FleetPreparationCoordinator.cs

@@ -0,0 +1,373 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MyParking.Shared;
+// 负责运动前：所有车辆舵轮是否准备完成
+
+namespace MultiWheelC.Fleet
+{
+    /// <summary>表示主车侧车队运动准备的当前阶段。</summary>
+    public enum FleetPreparationCoordinatorState
+    {
+        Idle = 0,
+        WaitingForMembers = 1,
+        ReadyToActivate = 2,
+        ActivationAuthorized = 3,
+        Faulted = 4
+    }
+
+    /// <summary>保存一次滚动准备中分配给指定成员车的本地β目标。</summary>
+    public readonly struct FleetMemberPreparationTarget
+    {
+        /// <summary>创建一条属于指定任务和成员车的准备目标。</summary>
+        public FleetMemberPreparationTarget(
+            long planId,
+            int vehicleId,
+            double motionDirectionInBodyRadians)
+        {
+            if (planId <= 0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(planId),
+                    "车队动作任务编号必须大于零。");
+            }
+
+            if (vehicleId <= 0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(vehicleId),
+                    "成员车号必须大于零。");
+            }
+
+            NumericGuard.EnsureFinite(
+                motionDirectionInBodyRadians,
+                nameof(motionDirectionInBodyRadians));
+
+            PlanId = planId;
+            VehicleId = vehicleId;
+            MotionDirectionInBodyRadians =
+                AngleMath.NormalizeRadians(
+                    motionDirectionInBodyRadians);
+        }
+
+        public long PlanId { get; }
+
+        public int VehicleId { get; }
+
+        public double MotionDirectionInBodyRadians { get; }
+    }
+
+    /// <summary>保存成员车对某次准备任务上报的本地状态。</summary>
+    public readonly struct FleetMemberPreparationStatus
+    {
+        /// <summary>创建一条成员车准备状态报告。</summary>
+        public FleetMemberPreparationStatus(
+            long planId,
+            int vehicleId,
+            FleetMemberAgentState state,
+            string failureReason = "")
+        {
+            if (planId <= 0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(planId),
+                    "车队动作任务编号必须大于零。");
+            }
+
+            if (vehicleId <= 0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(vehicleId),
+                    "成员车号必须大于零。");
+            }
+
+            if (!Enum.IsDefined(
+                    typeof(FleetMemberAgentState),
+                    state))
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(state),
+                    "成员车准备状态无效。");
+            }
+
+            PlanId = planId;
+            VehicleId = vehicleId;
+            State = state;
+            FailureReason = failureReason ?? string.Empty;
+        }
+
+        public long PlanId { get; }
+
+        public int VehicleId { get; }
+
+        public FleetMemberAgentState State { get; }
+
+        public string FailureReason { get; }
+    }
+
+    /// <summary>在主车侧分配成员β并管理全队Ready统一激活屏障。</summary>
+    public sealed class FleetPreparationCoordinator
+    {
+        private static readonly IReadOnlyList<
+                FleetMemberPreparationTarget>
+            EmptyTargets = Array.AsReadOnly(
+                Array.Empty<FleetMemberPreparationTarget>());
+
+        private readonly Dictionary<int, FleetMemberAgentState>
+            _memberStates =
+                new Dictionary<int, FleetMemberAgentState>();
+
+        private IReadOnlyList<FleetMemberPreparationTarget>
+            _targets = EmptyTargets;
+
+        /// <summary>创建尚未激活准备任务的主车侧协调器。</summary>
+        public FleetPreparationCoordinator()
+        {
+            State = FleetPreparationCoordinatorState.Idle;
+            LastFailureReason = string.Empty;
+        }
+
+        public FleetPreparationCoordinatorState State
+        {
+            get;
+            private set;
+        }
+
+        public long CurrentPlanId { get; private set; }
+
+        public double MotionDirectionInFleetRadians
+        {
+            get;
+            private set;
+        }
+
+        public IReadOnlyList<FleetMemberPreparationTarget>
+            Targets => _targets;
+
+        public string LastFailureReason { get; private set; }
+
+        /// <summary>根据车队固定布局为全部成员建立本次滚动β准备目标。</summary>
+        public void StartRollingPreparation(
+            long planId,
+            FleetLayout layout,
+            double motionDirectionInFleetRadians)
+        {
+            if (planId <= 0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(planId),
+                    "车队动作任务编号必须大于零。");
+            }
+
+            if (layout == null)
+            {
+                throw new ArgumentNullException(nameof(layout));
+            }
+
+            NumericGuard.EnsureFinite(
+                motionDirectionInFleetRadians,
+                nameof(motionDirectionInFleetRadians));
+
+            var normalizedFleetDirection =
+                AngleMath.NormalizeRadians(
+                    motionDirectionInFleetRadians);
+            var targets =
+                new FleetMemberPreparationTarget[
+                    layout.VehicleCount];
+
+            _memberStates.Clear();
+            for (var index = 0;
+                 index < layout.Vehicles.Count;
+                 index++)
+            {
+                var vehicle = layout.Vehicles[index];
+                var rawDirectionInBody =
+                    AngleMath.NormalizeRadians(
+                        normalizedFleetDirection -
+                        vehicle.PoseInFleet.YawRadians);
+                var equivalentDirectionInBody =
+                    SelectSteeringAxisEquivalent(
+                        rawDirectionInBody);
+
+                targets[index] =
+                    new FleetMemberPreparationTarget(
+                        planId,
+                        vehicle.VehicleId,
+                        equivalentDirectionInBody);
+                _memberStates.Add(
+                    vehicle.VehicleId,
+                    FleetMemberAgentState.Idle);
+            }
+
+            CurrentPlanId = planId;
+            MotionDirectionInFleetRadians =
+                normalizedFleetDirection;
+            _targets = Array.AsReadOnly(targets);
+            State =
+                FleetPreparationCoordinatorState
+                    .WaitingForMembers;
+            LastFailureReason = string.Empty;
+        }
+
+        /// <summary>接收一辆成员车的状态并重新计算全队Ready状态。</summary>
+        public FleetPreparationCoordinatorState
+            ReportMemberStatus(
+                FleetMemberPreparationStatus status)
+        {
+            if (State ==
+                    FleetPreparationCoordinatorState.Idle ||
+                State ==
+                    FleetPreparationCoordinatorState.Faulted)
+            {
+                return State;
+            }
+
+            if (status.PlanId != CurrentPlanId)
+            {
+                return State;
+            }
+
+            if (!_memberStates.ContainsKey(status.VehicleId))
+            {
+                throw new ArgumentException(
+                    $"车辆{status.VehicleId}不属于当前车队布局。",
+                    nameof(status));
+            }
+
+            if (status.State ==
+                FleetMemberAgentState.Faulted)
+            {
+                return Fail(
+                    string.IsNullOrWhiteSpace(
+                        status.FailureReason)
+                        ? $"车辆{status.VehicleId}准备失败。"
+                        : $"车辆{status.VehicleId}准备失败：" +
+                          status.FailureReason);
+            }
+
+            if (State ==
+                FleetPreparationCoordinatorState
+                    .ActivationAuthorized)
+            {
+                return State;
+            }
+
+            if (status.State ==
+                FleetMemberAgentState.Active)
+            {
+                return Fail(
+                    $"车辆{status.VehicleId}在全队统一激活前已经进入Active。");
+            }
+
+            _memberStates[status.VehicleId] = status.State;
+            State = AreAllMembersReady()
+                ? FleetPreparationCoordinatorState
+                    .ReadyToActivate
+                : FleetPreparationCoordinatorState
+                    .WaitingForMembers;
+            LastFailureReason = string.Empty;
+            return State;
+        }
+
+        /// <summary>在全部成员Ready后授权外层向全队广播同一任务的激活命令。</summary>
+        public bool TryAuthorizeActivation(long planId)
+        {
+            if (planId != CurrentPlanId)
+            {
+                return false;
+            }
+
+            if (State ==
+                FleetPreparationCoordinatorState
+                    .ActivationAuthorized)
+            {
+                return true;
+            }
+
+            if (State !=
+                FleetPreparationCoordinatorState
+                    .ReadyToActivate)
+            {
+                return false;
+            }
+
+            State = FleetPreparationCoordinatorState
+                .ActivationAuthorized;
+            LastFailureReason = string.Empty;
+            return true;
+        }
+
+        /// <summary>查找指定成员车在当前任务中的本地β准备目标。</summary>
+        public bool TryGetTarget(
+            int vehicleId,
+            out FleetMemberPreparationTarget target)
+        {
+            for (var index = 0;
+                 index < _targets.Count;
+                 index++)
+            {
+                if (_targets[index].VehicleId == vehicleId)
+                {
+                    target = _targets[index];
+                    return true;
+                }
+            }
+
+            target = default;
+            return false;
+        }
+
+        /// <summary>取消当前准备任务并清除成员状态和β目标。</summary>
+        public void Cancel()
+        {
+            _memberStates.Clear();
+            _targets = EmptyTargets;
+            CurrentPlanId = 0;
+            MotionDirectionInFleetRadians = 0.0;
+            State = FleetPreparationCoordinatorState.Idle;
+            LastFailureReason = string.Empty;
+        }
+
+        /// <summary>判断当前任务中的每辆成员车是否都已报告Ready。</summary>
+        private bool AreAllMembersReady()
+        {
+            foreach (var state in _memberStates.Values)
+            {
+                if (state != FleetMemberAgentState.Ready)
+                {
+                    return false;
+                }
+            }
+
+            return _memberStates.Count > 0;
+        }
+
+        /// <summary>将有向β转换为±90°内的等效滚动轴，反向运动由轮速符号表达。</summary>
+        private static double SelectSteeringAxisEquivalent(
+            double directionRadians)
+        {
+            var equivalent = AngleMath.NormalizeRadians(
+                directionRadians);
+
+            if (equivalent > Math.PI / 2.0)
+            {
+                equivalent -= Math.PI;
+            }
+            else if (equivalent < -Math.PI / 2.0)
+            {
+                equivalent += Math.PI;
+            }
+
+            return AngleMath.NormalizeRadians(equivalent);
+        }
+
+        /// <summary>锁存准备故障，等待外层停止所有成员并取消任务。</summary>
+        private FleetPreparationCoordinatorState Fail(
+            string reason)
+        {
+            State = FleetPreparationCoordinatorState.Faulted;
+            LastFailureReason = reason ?? string.Empty;
+            return State;
+        }
+    }
+}

MultiWheelC/Fleet/FleetSafetySupervisor.cs

@@ -0,0 +1,286 @@
+using System;
+using System.Collections.Generic;
+using MyParking.Shared;
+
+namespace MultiWheelC.Fleet
+{
+    // 主车已经接收并接受的一辆成员车安全状态。
+    public readonly struct FleetMemberSafetyStatus
+    {
+        public FleetMemberSafetyStatus(
+            int vehicleId,
+            long planId,
+            bool isStateAvailable,
+            bool isFaulted,
+            int failureCode,
+            double lastAcceptedReportTimeSeconds)
+        {
+            if (vehicleId <= 0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(vehicleId),
+                    "成员车编号必须大于零。");
+            }
+
+            if (planId < 0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(planId),
+                    "任务编号不能为负数。");
+            }
+
+            NumericGuard.EnsureFiniteNonNegative(
+                lastAcceptedReportTimeSeconds,
+                nameof(lastAcceptedReportTimeSeconds));
+
+            VehicleId = vehicleId;
+            PlanId = planId;
+            IsStateAvailable = isStateAvailable;
+            IsFaulted = isFaulted;
+            FailureCode = failureCode;
+            LastAcceptedReportTimeSeconds =
+                lastAcceptedReportTimeSeconds;
+        }
+
+        public int VehicleId { get; }
+
+        public long PlanId { get; }
+
+        public bool IsStateAvailable { get; }
+
+        public bool IsFaulted { get; }
+
+        // 零表示成员车没有报告结构化故障。
+        public int FailureCode { get; }
+
+        // 使用主车本地单调时钟，不能直接填写从车上传的时间戳。
+        public double LastAcceptedReportTimeSeconds { get; }
+    }
+
+    // 一次安全检查的结果；ShouldStop可直接作为是否停车的判断标识。
+    public readonly struct FleetSafetyDecision
+    {
+        internal FleetSafetyDecision(
+            bool shouldStop,
+            int sourceVehicleId,
+            string reason)
+        {
+            ShouldStop = shouldStop;
+            SourceVehicleId = sourceVehicleId;
+            Reason = reason ?? string.Empty;
+        }
+
+        public bool ShouldStop { get; }
+
+        // 零表示原因属于整个车队，而不是某一辆成员车。
+        public int SourceVehicleId { get; }
+
+        public string Reason { get; }
+    }
+
+    // 检查成员通信和健康状态，并锁存需要整队停车的首个原因。
+    public sealed class FleetSafetySupervisor
+    {
+        private readonly double _communicationTimeoutSeconds;
+
+        private long _activePlanId;
+        private FleetSafetyDecision _latchedDecision;
+
+        public FleetSafetySupervisor(
+            double communicationTimeoutSeconds)
+        {
+            NumericGuard.EnsureFinitePositive(
+                communicationTimeoutSeconds,
+                nameof(communicationTimeoutSeconds));
+
+            _communicationTimeoutSeconds =
+                communicationTimeoutSeconds;
+            Reset();
+        }
+
+        public double CommunicationTimeoutSeconds =>
+            _communicationTimeoutSeconds;
+
+        public long ActivePlanId => _activePlanId;
+
+        public bool IsActive => _activePlanId > 0;
+
+        public bool IsStopLatched =>
+            _latchedDecision.ShouldStop;
+
+        public FleetSafetyDecision LastDecision =>
+            _latchedDecision;
+
+        // 开始一次新任务，同时清除上一任务留下的停车锁存。
+        public void Start(long planId)
+        {
+            if (planId <= 0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(planId),
+                    "活动任务编号必须大于零。");
+            }
+
+            _activePlanId = planId;
+            _latchedDecision = CreateContinueDecision();
+        }
+
+        // 返回ShouldStop；本类只负责判定，实际停车由后续运行入口执行。
+        public FleetSafetyDecision Evaluate(
+            FleetLayout layout,
+            IReadOnlyList<FleetMemberSafetyStatus> memberStatuses,
+            double currentTimeSeconds)
+        {
+            if (layout == null)
+            {
+                throw new ArgumentNullException(nameof(layout));
+            }
+
+            if (memberStatuses == null)
+            {
+                throw new ArgumentNullException(
+                    nameof(memberStatuses));
+            }
+
+            NumericGuard.EnsureFiniteNonNegative(
+                currentTimeSeconds,
+                nameof(currentTimeSeconds));
+
+            if (!IsActive)
+            {
+                return new FleetSafetyDecision(
+                    true,
+                    0,
+                    "车队安全监督器尚未启动活动任务。");
+            }
+
+            if (IsStopLatched)
+            {
+                return _latchedDecision;
+            }
+
+            var statusesByVehicleId =
+                new Dictionary<int, FleetMemberSafetyStatus>();
+
+            for (var index = 0;
+                 index < memberStatuses.Count;
+                 index++)
+            {
+                var status = memberStatuses[index];
+
+                // 非当前编队成员的状态不参与本次任务安全判定。
+                if (!layout.TryGetVehicle(
+                        status.VehicleId,
+                        out _))
+                {
+                    continue;
+                }
+
+                if (statusesByVehicleId.ContainsKey(
+                        status.VehicleId))
+                {
+                    return LatchStop(
+                        status.VehicleId,
+                        $"成员车{status.VehicleId}存在重复状态报告。");
+                }
+
+                statusesByVehicleId.Add(
+                    status.VehicleId,
+                    status);
+            }
+
+            for (var index = 0;
+                 index < layout.Vehicles.Count;
+                 index++)
+            {
+                var vehicleId =
+                    layout.Vehicles[index].VehicleId;
+
+                if (!statusesByVehicleId.TryGetValue(
+                        vehicleId,
+                        out var status))
+                {
+                    return LatchStop(
+                        vehicleId,
+                        $"未收到成员车{vehicleId}的状态报告。");
+                }
+
+                if (status.PlanId != _activePlanId)
+                {
+                    return LatchStop(
+                        vehicleId,
+                        $"成员车{vehicleId}报告的任务编号" +
+                        $"{status.PlanId}与当前任务" +
+                        $"{_activePlanId}不一致。");
+                }
+
+                if (status.LastAcceptedReportTimeSeconds >
+                    currentTimeSeconds)
+                {
+                    return LatchStop(
+                        vehicleId,
+                        $"成员车{vehicleId}的主车接收时间晚于当前时间。");
+                }
+
+                var reportAgeSeconds =
+                    currentTimeSeconds -
+                    status.LastAcceptedReportTimeSeconds;
+                if (reportAgeSeconds >
+                    _communicationTimeoutSeconds)
+                {
+                    return LatchStop(
+                        vehicleId,
+                        $"成员车{vehicleId}通信超时，" +
+                        $"最近有效报告距今" +
+                        $"{reportAgeSeconds:F3}s。");
+                }
+
+                if (status.IsFaulted ||
+                    status.FailureCode != 0)
+                {
+                    return LatchStop(
+                        vehicleId,
+                        $"成员车{vehicleId}报告故障，" +
+                        $"故障码为{status.FailureCode}。");
+                }
+
+                if (!status.IsStateAvailable)
+                {
+                    return LatchStop(
+                        vehicleId,
+                        $"成员车{vehicleId}状态不可用。");
+                }
+            }
+
+            _latchedDecision = CreateContinueDecision();
+            return _latchedDecision;
+        }
+
+        // 结束当前任务并清除锁存；未开始新任务前Evaluate仍会要求停车。
+        public void Reset()
+        {
+            _activePlanId = 0;
+            _latchedDecision = CreateContinueDecision();
+        }
+
+        private FleetSafetyDecision LatchStop(
+            int sourceVehicleId,
+            string reason)
+        {
+            _latchedDecision = new FleetSafetyDecision(
+                true,
+                sourceVehicleId,
+                reason);
+            return _latchedDecision;
+        }
+
+        private static FleetSafetyDecision
+            CreateContinueDecision()
+        {
+            return new FleetSafetyDecision(
+                false,
+                0,
+                string.Empty);
+        }
+    }
+}

MultiWheelC/Fleet/FleetState.cs

@@ -0,0 +1,64 @@
+using MyParking.Shared;
+
+namespace MultiWheelC.Fleet
+{
+    // 一次经过校验的虚拟车队原点状态快照。
+    public readonly struct FleetState
+    {
+        public FleetState(
+            double sampleTimestampSeconds,
+            Pose2D fleetPoseInWorld,
+            Twist2D twistAtFleetOriginInWorld,
+            bool hasValidVelocityEstimate)
+        {
+            NumericGuard.EnsureFiniteNonNegative(
+                sampleTimestampSeconds,
+                nameof(sampleTimestampSeconds));
+            NumericGuard.EnsureFinite(
+                fleetPoseInWorld,
+                nameof(fleetPoseInWorld));
+            NumericGuard.EnsureFinite(
+                twistAtFleetOriginInWorld,
+                nameof(twistAtFleetOriginInWorld));
+
+            SampleTimestampSeconds =
+                sampleTimestampSeconds;
+            FleetPoseInWorld = new Pose2D(
+                fleetPoseInWorld.XMeters,
+                fleetPoseInWorld.YMeters,
+                AngleMath.NormalizeRadians(
+                    fleetPoseInWorld.YawRadians));
+            HasValidVelocityEstimate =
+                hasValidVelocityEstimate;
+
+            // 位姿有效但速度尚未初始化时显式置零，避免控制器误用输入值。
+            TwistAtFleetOriginInWorld =
+                hasValidVelocityEstimate
+                    ? twistAtFleetOriginInWorld
+                    : Twist2D.Zero;
+
+            var worldPoseInFleet =
+                FrameTransform2D.Inverse(
+                    FleetPoseInWorld);
+            TwistAtFleetOriginInFleet =
+                FrameTransform2D.TransformTwistAtSamePoint(
+                    worldPoseInFleet,
+                    TwistAtFleetOriginInWorld);
+        }
+
+        // 状态源单调时钟中的采样时刻，单位为s。
+        public double SampleTimestampSeconds { get; }
+
+        // 车队坐标系原点在世界坐标系中的实际位姿。
+        public Pose2D FleetPoseInWorld { get; }
+
+        // 车队原点处的实际刚体速度，在世界坐标系中表达。
+        public Twist2D TwistAtFleetOriginInWorld { get; }
+
+        // 同一刚体速度在车队坐标系中表达，供车队控制器使用。
+        public Twist2D TwistAtFleetOriginInFleet { get; }
+
+        // 速度是否已经初始化并可用于闭环控制。
+        public bool HasValidVelocityEstimate { get; }
+    }
+}

MultiWheelC/Fleet/FleetStateEstimator.cs

@@ -0,0 +1,579 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MyParking.Shared;
+
+namespace MultiWheelC.Fleet
+{
+    // 一辆成员车在主车统一时间轴上的状态样本。
+    public readonly struct FleetMemberStateSample
+    {
+        public FleetMemberStateSample(
+            int vehicleId,
+            double sampleTimestampSeconds,
+            Pose2D poseInWorld,
+            Twist2D twistAtVehicleOriginInWorld,
+            bool isStateAvailable,
+            bool hasValidVelocityEstimate)
+        {
+            if (vehicleId <= 0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(vehicleId),
+                    "编队成员车号必须大于零。");
+            }
+
+            NumericGuard.EnsureFiniteNonNegative(
+                sampleTimestampSeconds,
+                nameof(sampleTimestampSeconds));
+            NumericGuard.EnsureFinite(
+                poseInWorld,
+                nameof(poseInWorld));
+            NumericGuard.EnsureFinite(
+                twistAtVehicleOriginInWorld,
+                nameof(twistAtVehicleOriginInWorld));
+
+            VehicleId = vehicleId;
+            SampleTimestampSeconds = sampleTimestampSeconds;
+            PoseInWorld = new Pose2D(
+                poseInWorld.XMeters,
+                poseInWorld.YMeters,
+                AngleMath.NormalizeRadians(
+                    poseInWorld.YawRadians));
+            TwistAtVehicleOriginInWorld =
+                twistAtVehicleOriginInWorld;
+            IsStateAvailable = isStateAvailable;
+            HasValidVelocityEstimate =
+                hasValidVelocityEstimate;
+        }
+
+        public int VehicleId { get; }
+
+        // 该时间戳必须已经换算到主车/协调器的单调时间轴。
+        public double SampleTimestampSeconds { get; }
+
+        public Pose2D PoseInWorld { get; }
+
+        // 成员车体中心处的实际速度，在世界坐标系中表达。
+        public Twist2D TwistAtVehicleOriginInWorld { get; }
+
+        public bool IsStateAvailable { get; }
+
+        public bool HasValidVelocityEstimate { get; }
+    }
+
+    // 成员实际位姿相对固定布局目标位姿的误差。
+    public readonly struct FleetMemberLayoutError
+    {
+        public FleetMemberLayoutError(
+            int vehicleId,
+            Pose2D actualPoseInExpectedVehicleFrame)
+        {
+            if (vehicleId <= 0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(vehicleId),
+                    "编队成员车号必须大于零。");
+            }
+
+            NumericGuard.EnsureFinite(
+                actualPoseInExpectedVehicleFrame,
+                nameof(actualPoseInExpectedVehicleFrame));
+
+            VehicleId = vehicleId;
+            ActualPoseInExpectedVehicleFrame =
+                new Pose2D(
+                    actualPoseInExpectedVehicleFrame.XMeters,
+                    actualPoseInExpectedVehicleFrame.YMeters,
+                    AngleMath.NormalizeRadians(
+                        actualPoseInExpectedVehicleFrame
+                            .YawRadians));
+        }
+
+        public int VehicleId { get; }
+
+        // 期望成员车体系中表达的实际成员位姿；理想刚体布局时为Identity。
+        public Pose2D ActualPoseInExpectedVehicleFrame { get; }
+    }
+
+    // 一次车队状态估计的结果；不可用时不提供FleetState。
+    public sealed class FleetStateEstimateResult
+    {
+        private static readonly IReadOnlyList<FleetMemberLayoutError>
+            EmptyMemberErrors = Array.AsReadOnly(
+                Array.Empty<FleetMemberLayoutError>());
+
+        private FleetStateEstimateResult(
+            bool isAvailable,
+            FleetState? state,
+            IReadOnlyList<FleetMemberLayoutError> memberErrors,
+            string unavailableReason)
+        {
+            IsAvailable = isAvailable;
+            State = state;
+            MemberErrors = memberErrors;
+            UnavailableReason = unavailableReason;
+        }
+
+        public bool IsAvailable { get; }
+
+        public FleetState? State { get; }
+
+        public IReadOnlyList<FleetMemberLayoutError> MemberErrors { get; }
+
+        public string UnavailableReason { get; }
+
+        internal static FleetStateEstimateResult Available(
+            FleetState state,
+            FleetMemberLayoutError[] memberErrors)
+        {
+            return new FleetStateEstimateResult(
+                true,
+                state,
+                Array.AsReadOnly(memberErrors),
+                string.Empty);
+        }
+
+        internal static FleetStateEstimateResult Unavailable(
+            string reason)
+        {
+            return new FleetStateEstimateResult(
+                false,
+                null,
+                EmptyMemberErrors,
+                reason ?? string.Empty);
+        }
+    }
+
+    // 从各成员状态反算并融合车队虚拟中心状态。
+    public sealed class FleetStateEstimator
+    {
+        private const double TimestampToleranceSeconds = 1e-9;
+        private const double MinimumCircularMeanMagnitude = 1e-12;
+
+        private readonly double _maximumMemberStateAgeSeconds;
+        private readonly double _maximumPositionDisagreementMeters;
+        private readonly double _maximumYawDisagreementRadians;
+
+        public FleetStateEstimator(
+            double maximumMemberStateAgeSeconds,
+            double maximumPositionDisagreementMeters,
+            double maximumYawDisagreementRadians)
+        {
+            NumericGuard.EnsureFinitePositive(
+                maximumMemberStateAgeSeconds,
+                nameof(maximumMemberStateAgeSeconds));
+            NumericGuard.EnsureFinitePositive(
+                maximumPositionDisagreementMeters,
+                nameof(maximumPositionDisagreementMeters));
+            NumericGuard.EnsureFinitePositive(
+                maximumYawDisagreementRadians,
+                nameof(maximumYawDisagreementRadians));
+
+            if (maximumYawDisagreementRadians > Math.PI)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(maximumYawDisagreementRadians),
+                    "车队候选航向差阈值不能大于π。");
+            }
+
+            _maximumMemberStateAgeSeconds =
+                maximumMemberStateAgeSeconds;
+            _maximumPositionDisagreementMeters =
+                maximumPositionDisagreementMeters;
+            _maximumYawDisagreementRadians =
+                maximumYawDisagreementRadians;
+        }
+
+        public FleetStateEstimateResult Estimate(
+            FleetLayout layout,
+            IReadOnlyList<FleetMemberStateSample> memberStates,
+            double targetTimestampSeconds)
+        {
+            if (layout == null)
+            {
+                throw new ArgumentNullException(nameof(layout));
+            }
+
+            if (memberStates == null)
+            {
+                throw new ArgumentNullException(nameof(memberStates));
+            }
+
+            NumericGuard.EnsureFiniteNonNegative(
+                targetTimestampSeconds,
+                nameof(targetTimestampSeconds));
+
+            if (memberStates.Count != layout.VehicleCount)
+            {
+                return FleetStateEstimateResult.Unavailable(
+                    $"成员状态数量{memberStates.Count}与布局数量" +
+                    $"{layout.VehicleCount}不一致。");
+            }
+
+            var statesByVehicleId =
+                new Dictionary<int, FleetMemberStateSample>(
+                    memberStates.Count);
+
+            for (var index = 0;
+                 index < memberStates.Count;
+                 index++)
+            {
+                var memberState = memberStates[index];
+                if (memberState.VehicleId <= 0)
+                {
+                    return FleetStateEstimateResult.Unavailable(
+                        $"第{index}个成员状态的车号无效。");
+                }
+
+                if (!layout.TryGetVehicle(
+                        memberState.VehicleId,
+                        out _))
+                {
+                    return FleetStateEstimateResult.Unavailable(
+                        $"成员状态包含布局外车辆" +
+                        $"{memberState.VehicleId}。");
+                }
+
+                if (statesByVehicleId.ContainsKey(
+                        memberState.VehicleId))
+                {
+                    return FleetStateEstimateResult.Unavailable(
+                        $"成员状态包含重复车号" +
+                        $"{memberState.VehicleId}。");
+                }
+
+                statesByVehicleId.Add(
+                    memberState.VehicleId,
+                    memberState);
+            }
+
+            var alignedMembers =
+                new AlignedMemberState[layout.VehicleCount];
+
+            for (var index = 0;
+                 index < layout.Vehicles.Count;
+                 index++)
+            {
+                var vehicleLayout = layout.Vehicles[index];
+                if (!statesByVehicleId.TryGetValue(
+                        vehicleLayout.VehicleId,
+                        out var memberState))
+                {
+                    return FleetStateEstimateResult.Unavailable(
+                        $"缺少车辆{vehicleLayout.VehicleId}的状态。");
+                }
+
+                var alignmentResult = AlignMemberState(
+                    memberState,
+                    targetTimestampSeconds,
+                    out var alignedPoseInWorld);
+                if (alignmentResult != null)
+                {
+                    return FleetStateEstimateResult.Unavailable(
+                        alignmentResult);
+                }
+
+                var candidateFleetPoseInWorld =
+                    FrameTransform2D.Compose(
+                        alignedPoseInWorld,
+                        FrameTransform2D.Inverse(
+                            vehicleLayout.PoseInFleet));
+
+                alignedMembers[index] =
+                    new AlignedMemberState(
+                        vehicleLayout,
+                        memberState,
+                        alignedPoseInWorld,
+                        candidateFleetPoseInWorld);
+            }
+
+            var disagreementReason =
+                FindCandidateDisagreement(alignedMembers);
+            if (disagreementReason != null)
+            {
+                return FleetStateEstimateResult.Unavailable(
+                    disagreementReason);
+            }
+
+            if (!TryAverageCandidateFleetPose(
+                    alignedMembers,
+                    out var fleetPoseInWorld))
+            {
+                return FleetStateEstimateResult.Unavailable(
+                    "成员候选航向无法形成唯一的车队平均航向。");
+            }
+
+            var hasValidVelocityEstimate =
+                TryAverageFleetOriginTwist(
+                    alignedMembers,
+                    fleetPoseInWorld,
+                    out var twistAtFleetOriginInWorld);
+            var fleetState = new FleetState(
+                targetTimestampSeconds,
+                fleetPoseInWorld,
+                twistAtFleetOriginInWorld,
+                hasValidVelocityEstimate);
+            var memberErrors = CalculateMemberErrors(
+                alignedMembers,
+                fleetPoseInWorld);
+
+            return FleetStateEstimateResult.Available(
+                fleetState,
+                memberErrors);
+        }
+
+        private string AlignMemberState(
+            FleetMemberStateSample memberState,
+            double targetTimestampSeconds,
+            out Pose2D alignedPoseInWorld)
+        {
+            alignedPoseInWorld = memberState.PoseInWorld;
+
+            if (!memberState.IsStateAvailable)
+            {
+                return $"车辆{memberState.VehicleId}状态不可用。";
+            }
+
+            var ageSeconds =
+                targetTimestampSeconds -
+                memberState.SampleTimestampSeconds;
+
+            if (ageSeconds < -TimestampToleranceSeconds)
+            {
+                return $"车辆{memberState.VehicleId}的状态时间晚于" +
+                       "本次估计目标时间。";
+            }
+
+            if (ageSeconds > _maximumMemberStateAgeSeconds)
+            {
+                return $"车辆{memberState.VehicleId}的状态已过期：" +
+                       $"{ageSeconds:F3}s。";
+            }
+
+            if (ageSeconds <= TimestampToleranceSeconds)
+            {
+                return null;
+            }
+
+            if (!memberState.HasValidVelocityEstimate)
+            {
+                return $"车辆{memberState.VehicleId}缺少时间对齐所需的" +
+                       "有效速度。";
+            }
+
+            var twist = memberState.TwistAtVehicleOriginInWorld;
+            alignedPoseInWorld = new Pose2D(
+                memberState.PoseInWorld.XMeters +
+                twist.VxMetersPerSecond * ageSeconds,
+                memberState.PoseInWorld.YMeters +
+                twist.VyMetersPerSecond * ageSeconds,
+                AngleMath.NormalizeRadians(
+                    memberState.PoseInWorld.YawRadians +
+                    twist.OmegaRadiansPerSecond * ageSeconds));
+            return null;
+        }
+
+        private string FindCandidateDisagreement(
+            IReadOnlyList<AlignedMemberState> alignedMembers)
+        {
+            for (var firstIndex = 0;
+                 firstIndex < alignedMembers.Count;
+                 firstIndex++)
+            {
+                var first = alignedMembers[firstIndex];
+                for (var secondIndex = firstIndex + 1;
+                     secondIndex < alignedMembers.Count;
+                     secondIndex++)
+                {
+                    var second = alignedMembers[secondIndex];
+                    var dx =
+                        first.CandidateFleetPoseInWorld.XMeters -
+                        second.CandidateFleetPoseInWorld.XMeters;
+                    var dy =
+                        first.CandidateFleetPoseInWorld.YMeters -
+                        second.CandidateFleetPoseInWorld.YMeters;
+                    var positionDifferenceMeters =
+                        Math.Sqrt(dx * dx + dy * dy);
+                    var yawDifferenceRadians = Math.Abs(
+                        AngleMath.ShortestDifferenceRadians(
+                            first.CandidateFleetPoseInWorld
+                                .YawRadians,
+                            second.CandidateFleetPoseInWorld
+                                .YawRadians));
+
+                    if (positionDifferenceMeters >
+                        _maximumPositionDisagreementMeters)
+                    {
+                        return $"车辆{first.VehicleLayout.VehicleId}与" +
+                               $"车辆{second.VehicleLayout.VehicleId}反算的" +
+                               $"车队中心相差{positionDifferenceMeters:F3}m，" +
+                               "超过允许值。";
+                    }
+
+                    if (yawDifferenceRadians >
+                        _maximumYawDisagreementRadians)
+                    {
+                        return $"车辆{first.VehicleLayout.VehicleId}与" +
+                               $"车辆{second.VehicleLayout.VehicleId}反算的" +
+                               $"车队航向相差" +
+                               $"{AngleMath.RadiansToDegrees(yawDifferenceRadians):F2}°，" +
+                               "超过允许值。";
+                    }
+                }
+            }
+
+            return null;
+        }
+
+        private static bool TryAverageCandidateFleetPose(
+            IReadOnlyList<AlignedMemberState> alignedMembers,
+            out Pose2D fleetPoseInWorld)
+        {
+            var xMeters = 0.0;
+            var yMeters = 0.0;
+            var yawCosineSum = 0.0;
+            var yawSineSum = 0.0;
+
+            for (var index = 0;
+                 index < alignedMembers.Count;
+                 index++)
+            {
+                var candidate =
+                    alignedMembers[index]
+                        .CandidateFleetPoseInWorld;
+                xMeters += candidate.XMeters;
+                yMeters += candidate.YMeters;
+                yawCosineSum += Math.Cos(candidate.YawRadians);
+                yawSineSum += Math.Sin(candidate.YawRadians);
+            }
+
+            var count = alignedMembers.Count;
+            var circularMeanMagnitude = Math.Sqrt(
+                yawCosineSum * yawCosineSum +
+                yawSineSum * yawSineSum);
+            if (circularMeanMagnitude <
+                MinimumCircularMeanMagnitude)
+            {
+                fleetPoseInWorld = Pose2D.Identity;
+                return false;
+            }
+
+            fleetPoseInWorld = new Pose2D(
+                xMeters / count,
+                yMeters / count,
+                Math.Atan2(yawSineSum, yawCosineSum));
+            return true;
+        }
+
+        private static bool TryAverageFleetOriginTwist(
+            IReadOnlyList<AlignedMemberState> alignedMembers,
+            Pose2D fleetPoseInWorld,
+            out Twist2D twistAtFleetOriginInWorld)
+        {
+            for (var index = 0;
+                 index < alignedMembers.Count;
+                 index++)
+            {
+                if (!alignedMembers[index]
+                    .MemberState
+                    .HasValidVelocityEstimate)
+                {
+                    twistAtFleetOriginInWorld = Twist2D.Zero;
+                    return false;
+                }
+            }
+
+            var vxMetersPerSecond = 0.0;
+            var vyMetersPerSecond = 0.0;
+            var omegaRadiansPerSecond = 0.0;
+
+            for (var index = 0;
+                 index < alignedMembers.Count;
+                 index++)
+            {
+                var member = alignedMembers[index];
+                var twist = member.MemberState
+                    .TwistAtVehicleOriginInWorld;
+                var memberXFromFleetOrigin =
+                    member.AlignedPoseInWorld.XMeters -
+                    fleetPoseInWorld.XMeters;
+                var memberYFromFleetOrigin =
+                    member.AlignedPoseInWorld.YMeters -
+                    fleetPoseInWorld.YMeters;
+
+                vxMetersPerSecond +=
+                    twist.VxMetersPerSecond +
+                    twist.OmegaRadiansPerSecond *
+                    memberYFromFleetOrigin;
+                vyMetersPerSecond +=
+                    twist.VyMetersPerSecond -
+                    twist.OmegaRadiansPerSecond *
+                    memberXFromFleetOrigin;
+                omegaRadiansPerSecond +=
+                    twist.OmegaRadiansPerSecond;
+            }
+
+            var count = alignedMembers.Count;
+            twistAtFleetOriginInWorld = new Twist2D(
+                vxMetersPerSecond / count,
+                vyMetersPerSecond / count,
+                omegaRadiansPerSecond / count);
+            return true;
+        }
+
+        private static FleetMemberLayoutError[] CalculateMemberErrors(
+            IReadOnlyList<AlignedMemberState> alignedMembers,
+            Pose2D fleetPoseInWorld)
+        {
+            var errors =
+                new FleetMemberLayoutError[alignedMembers.Count];
+
+            for (var index = 0;
+                 index < alignedMembers.Count;
+                 index++)
+            {
+                var member = alignedMembers[index];
+                var expectedPoseInWorld =
+                    FrameTransform2D.Compose(
+                        fleetPoseInWorld,
+                        member.VehicleLayout.PoseInFleet);
+                var actualPoseInExpectedVehicleFrame =
+                    FrameTransform2D.Compose(
+                        FrameTransform2D.Inverse(
+                            expectedPoseInWorld),
+                        member.AlignedPoseInWorld);
+
+                errors[index] = new FleetMemberLayoutError(
+                    member.VehicleLayout.VehicleId,
+                    actualPoseInExpectedVehicleFrame);
+            }
+
+            return errors;
+        }
+
+        private readonly struct AlignedMemberState
+        {
+            public AlignedMemberState(
+                VehicleLayout vehicleLayout,
+                FleetMemberStateSample memberState,
+                Pose2D alignedPoseInWorld,
+                Pose2D candidateFleetPoseInWorld)
+            {
+                VehicleLayout = vehicleLayout;
+                MemberState = memberState;
+                AlignedPoseInWorld = alignedPoseInWorld;
+                CandidateFleetPoseInWorld =
+                    candidateFleetPoseInWorld;
+            }
+
+            public VehicleLayout VehicleLayout { get; }
+
+            public FleetMemberStateSample MemberState { get; }
+
+            public Pose2D AlignedPoseInWorld { get; }
+
+            public Pose2D CandidateFleetPoseInWorld { get; }
+        }
+    }
+}

MultiWheelC/Fleet/IFleetTransport.cs

@@ -0,0 +1,16 @@
+using MyParking.Shared;
+
+namespace MultiWheelC.Fleet
+{
+    // 隔离车队运行逻辑与具体无线、串口或内存传输实现。
+    public interface IFleetTransport
+    {
+        void SendCommand(FleetCommand command);
+
+        void SendReport(FleetMemberReport report);
+
+        bool TryReceiveCommand(out FleetCommand command);
+
+        bool TryReceiveReport(out FleetMemberReport report);
+    }
+}

MultiWheelC/MovementTests.cs

@@ -1,949 +0,0 @@
-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.Collections.Generic;
-using System.Numerics;
-using System.Threading;
-
-namespace MultiWheelC
-{
-    internal static class MovementTestPreparation
-    {
-        // 在测试正式开始前，将四个舵轮稳定回正到车体前向。
-        public static bool AlignWheelsForward(
-            ref DriveTask activeTask)
-        {
-            var preparation = new PrepareWheelsForward();
-            var task = new DriveTask(preparation.Get());
-            activeTask = task;
-
-            try
-            {
-                task.Wait();
-                return preparation.Completed;
-            }
-            catch (Exception ex)
-            {
-                Console.WriteLine(
-                    $"测试前舵轮回正失败：{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 =
-                    AngleMath.DegreesToRadians(toleranceDegrees);
-
-                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 = "SendMotion：连续前进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;
-            }
-
-            var location = DetourInterface.getCartLocation();
-            if (double.IsNaN(location.x) ||
-                double.IsInfinity(location.x) ||
-                double.IsNaN(location.y) ||
-                double.IsInfinity(location.y) ||
-                double.IsNaN(location.th) ||
-                double.IsInfinity(location.th))
-            {
-                Console.WriteLine(
-                    "Detour当前位姿无效，取消连续前进4m测试。");
-                return;
-            }
-            var source = new Vector2((float)location.x, (float)location.y);
-            // Detour航向单位是度，三角函数需要弧度。
-            var headingRadians =
-                AngleMath.DegreesToRadians(location.th);
-            var destination = new Vector2(
-                source.X + DistanceMillimeters * (float)Math.Cos(headingRadians),
-                source.Y + DistanceMillimeters * (float)Math.Sin(headingRadians));
-            _recorder =
-                        new TrackingExperimentRecorder(
-                            controllerName: "LegacyGeometricController",
-                            trajectoryName: "LegacyStraight4m",
-                            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());
-                _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 InPlaceRotateTestBase : MovementTest
-    {
-        public float RelativeAngleDegrees; // 相对当前航向的旋转角度，逆时针为正。
-        public float MaxAngularSpeedDegreesPerSecond = 20f; // PID输出的最大角速度。
-        public int TrialNumber = 1; // 重复实验编号。
-
-        private DriveTask _task;
-        private TrackingExperimentRecorder _recorder;
-        private readonly string _trajectoryName;
-
-        protected InPlaceRotateTestBase(
-            float relativeAngleDegrees,
-            string trajectoryName)
-        {
-            RelativeAngleDegrees =
-                relativeAngleDegrees;
-            _trajectoryName =
-                trajectoryName;
-        }
-
-        // 从当前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 =
-                (float)AngleMath.NormalizeDegrees(
-                    location.th + RelativeAngleDegrees);
-
-            _recorder = new TrackingExperimentRecorder(
-                controllerName: "InPlaceRotatePID",
-                trajectoryName: _trajectoryName,
-                trialNumber: TrialNumber,
-                referenceStart: rotationCenter,
-                referenceEnd: rotationCenter,
-                referenceSpeed: 0f,
-                referenceAngularSpeed:
-                    (float)AngleMath.DegreesToRadians(
-                        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,
-                                    (float)AngleMath.DegreesToRadians(
-                                        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();
-        }
-
-    }
-
-    [MovementTest(name = "SendXYThSpeed：原地自转90°")]
-    public sealed class TestRotate90 :
-        InPlaceRotateTestBase
-    {
-        public TestRotate90()
-            : base(90f, "Rotate90")
-        {
-        }
-    }
-
-    [MovementTest(name = "SendXYThSpeed：原地自转180°")]
-    public sealed class TestRotate180 :
-        InPlaceRotateTestBase
-    {
-        public TestRotate180()
-            : base(180f, "Rotate180")
-        {
-        }
-    }
-
-    [MovementTest(name = "SendMotion：左转90°半径2m圆弧")]
-    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 =
-                AngleMath.DegreesToRadians(location.th);
-
-            // 根据世界航向求车体左法向，左转圆心位于车辆左侧。
-            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
-            };
-
-            // 左转90°后，圆心到终点的径向方向等于起始车头方向。
-            var destination = center + new Vector2(
-                RadiusMillimeters *
-                    (float)Math.Cos(headingRadians),
-                RadiusMillimeters *
-                    (float)Math.Sin(headingRadians));
-
-            if (!controller.AddTrack(arc, "LeftArc90Degrees"))
-            {
-                Console.WriteLine(
-                    "左转90°圆弧轨迹添加失败，取消测试。");
-                return;
-            }
-
-            _recorder = new TrackingExperimentRecorder(
-                controllerName: "LegacyGeometricController",
-                trajectoryName:
-                    $"LegacyLeftArc90_R{RadiusMillimeters:0}mm",
-                trialNumber: TrialNumber,
-                referenceStart: source,
-                referenceEnd: destination,
-                referenceSpeed: CruiseSpeed);
-            _recorder.Start();
-
-            try
-            {
-                _task = new DriveTask(controller.Track());
-                _task.Wait();
-
-                // 保留少量停车后的样本，用于观察速度是否回到零。
-                Thread.Sleep(300);
-            }
-            finally
-            {
-                _task?.Stop();
-                _recorder?.UpdateCommand(0f, 0f);
-                _recorder?.StopAndSave();
-                _task = null;
-                _recorder = null;
-            }
-        }
-
-        // 停止圆弧运动并保存当前已经采集的实验数据。
-        public override void TestStop()
-        {
-            _task?.Stop();
-            _recorder?.UpdateCommand(0f, 0f);
-            _recorder?.StopAndSave();
-        }
-    }
-
-    #region 蟹行运动测试
-    [MovementTest(name = "SendMotion：蟹行直线4m")]
-    public class TestCrabForward4m : MovementTest
-    {
-        public float DistanceMillimeters = 4000f;
-        public float CruiseSpeed = 0.2f;
-        public int TrialNumber = 1;
-
-        private DriveTask _task;
-        private TrackingExperimentRecorder _recorder;
-
-        // 将车体左侧作为运动前向，沿直线蟹行4m并记录Detour实验数据。
-        public override void Test()
-        {
-            if (!TryReadStartPose(
-                    out var source,
-                    out var bodyYawRadians))
-                return;
-
-            var motionYaw =
-                bodyYawRadians + Math.PI / 2.0;
-            var destination = new Vector2(
-                source.X +
-                    DistanceMillimeters *
-                    (float)Math.Cos(motionYaw),
-                source.Y +
-                    DistanceMillimeters *
-                    (float)Math.Sin(motionYaw));
-
-            var tracker = new CrabMotionFrameTracker
-            {
-                CommandBackend =
-                    CrabMotionFrameTracker
-                        .ChassisCommandBackend
-                        .SendMotion,
-                PathKind =
-                    CrabMotionFrameTracker
-                        .ReferencePathKind.Straight,
-                StartPosition = source,
-                InitialBodyYawRadians =
-                    bodyYawRadians,
-                LengthMillimeters =
-                    DistanceMillimeters,
-                CruiseSpeed = CruiseSpeed
-            };
-
-            _recorder = new TrackingExperimentRecorder(
-                controllerName:
-                    "CrabSendMotionTracker",
-                trajectoryName:
-                    "CrabStraight4m",
-                trialNumber: TrialNumber,
-                referenceStart: source,
-                referenceEnd: destination,
-                referenceSpeed: CruiseSpeed,
-                referenceMotionFrameYawDegrees: 90f);
-            tracker.CommandObserver =
-                (vx, vy, omega) =>
-                    _recorder?.UpdateBodyCommand(
-                        vx,
-                        vy,
-                        omega);
-            _recorder.Start();
-
-            try
-            {
-                _task = new DriveTask(tracker.Get());
-                _task.Wait();
-                Thread.Sleep(300);
-            }
-            finally
-            {
-                _task?.Stop();
-                _recorder?.UpdateBodyCommand(
-                    0f,
-                    0f,
-                    0f);
-                _recorder?.StopAndSave();
-                _task = null;
-                _recorder = null;
-            }
-        }
-
-        public override void TestStop()
-        {
-            _task?.Stop();
-            _recorder?.UpdateBodyCommand(
-                0f,
-                0f,
-                0f);
-            _recorder?.StopAndSave();
-        }
-
-        // 读取并校验测试开始时的Detour世界位姿。
-        private static bool TryReadStartPose(
-            out Vector2 source,
-            out double bodyYawRadians)
-        {
-            var location =
-                DetourInterface.getCartLocation();
-            if (double.IsNaN(location.x) ||
-                double.IsInfinity(location.x) ||
-                double.IsNaN(location.y) ||
-                double.IsInfinity(location.y) ||
-                double.IsNaN(location.th) ||
-                double.IsInfinity(location.th))
-            {
-                Console.WriteLine(
-                    "Detour当前位姿无效，取消蟹行直线测试。");
-                source = Vector2.Zero;
-                bodyYawRadians = 0.0;
-                return false;
-            }
-
-            source = new Vector2(
-                (float)location.x,
-                (float)location.y);
-            bodyYawRadians =
-                AngleMath.DegreesToRadians(location.th);
-            return true;
-        }
-    }
-
-    [MovementTest(name = "SendMotion：蟹行左转90°半径2m圆弧")]
-    public class TestCrabLeftArc90 : MovementTest
-    {
-        public float RadiusMillimeters = 2000f;
-        public float CruiseSpeed = 0.2f;
-        public int TrialNumber = 1;
-
-        private DriveTask _task;
-        private TrackingExperimentRecorder _recorder;
-
-        // 将车体左侧作为运动前向，沿半径2m的左转圆弧运动90°。
-        public override void Test()
-        {
-            var location =
-                DetourInterface.getCartLocation();
-            if (double.IsNaN(location.x) ||
-                double.IsInfinity(location.x) ||
-                double.IsNaN(location.y) ||
-                double.IsInfinity(location.y) ||
-                double.IsNaN(location.th) ||
-                double.IsInfinity(location.th))
-            {
-                Console.WriteLine(
-                    "Detour当前位姿无效，取消蟹行圆弧测试。");
-                return;
-            }
-
-            var source = new Vector2(
-                (float)location.x,
-                (float)location.y);
-            var bodyYawRadians =
-                AngleMath.DegreesToRadians(location.th);
-            var tracker = new CrabMotionFrameTracker
-            {
-                CommandBackend =
-                    CrabMotionFrameTracker
-                        .ChassisCommandBackend
-                        .SendMotion,
-                PathKind =
-                    CrabMotionFrameTracker
-                        .ReferencePathKind.LeftArc,
-                StartPosition = source,
-                InitialBodyYawRadians =
-                    bodyYawRadians,
-                RadiusMillimeters =
-                    RadiusMillimeters,
-                ArcSweepRadians = Math.PI / 2.0,
-                CruiseSpeed = CruiseSpeed
-            };
-            var destination =
-                tracker.GetArcDestination();
-
-            _recorder = new TrackingExperimentRecorder(
-                controllerName:
-                    "CrabSendMotionTracker",
-                trajectoryName:
-                    $"CrabLeftArc90_R{RadiusMillimeters:0}mm",
-                trialNumber: TrialNumber,
-                referenceStart: source,
-                referenceEnd: destination,
-                referenceSpeed: CruiseSpeed,
-                referenceMotionFrameYawDegrees: 90f);
-            tracker.CommandObserver =
-                (vx, vy, omega) =>
-                    _recorder?.UpdateBodyCommand(
-                        vx,
-                        vy,
-                        omega);
-            _recorder.Start();
-
-            try
-            {
-                _task = new DriveTask(tracker.Get());
-                _task.Wait();
-                Thread.Sleep(300);
-            }
-            finally
-            {
-                _task?.Stop();
-                _recorder?.UpdateBodyCommand(
-                    0f,
-                    0f,
-                    0f);
-                _recorder?.StopAndSave();
-                _task = null;
-                _recorder = null;
-            }
-        }
-
-        public override void TestStop()
-        {
-            _task?.Stop();
-            _recorder?.UpdateBodyCommand(
-                0f,
-                0f,
-                0f);
-            _recorder?.StopAndSave();
-        }
-    }
-
-    [MovementTest(name = "SendMotion：4m S型曲线")]
-    public class TestSCurve4m : MovementTest
-    {
-        public float LengthMillimeters = 4000f; // S型曲线纵向长度，单位mm。
-        public float LateralOffsetMillimeters = 400f; // S型曲线左右两侧的最大偏移，单位mm。
-        public float CruiseSpeed = 0.3f; // 首次实车测试建议使用0.3m/s。
-        public int TrialNumber = 1; // 重复实验编号。
-
-        private DriveTask _task;
-        private TrackingExperimentRecorder _recorder;
-
-        // 从当前Detour位姿开始，沿车头方向跟踪先左偏、再右偏并最终回中的完整S型曲线。
-        public override void Test()
-        {
-            if (float.IsNaN(LengthMillimeters) ||
-                float.IsInfinity(LengthMillimeters) ||
-                LengthMillimeters <= 0f ||
-                float.IsNaN(LateralOffsetMillimeters) ||
-                float.IsInfinity(LateralOffsetMillimeters) ||
-                LateralOffsetMillimeters <= 0f ||
-                float.IsNaN(CruiseSpeed) ||
-                float.IsInfinity(CruiseSpeed) ||
-                CruiseSpeed <= 0f)
-            {
-                Console.WriteLine("S型曲线测试参数无效。");
-                return;
-            }
-
-            if (!MovementTestPreparation.AreWheelsForward())
-                return;
-
-            var location = DetourInterface.getCartLocation();
-            if (double.IsNaN(location.x) ||
-                double.IsInfinity(location.x) ||
-                double.IsNaN(location.y) ||
-                double.IsInfinity(location.y) ||
-                double.IsNaN(location.th) ||
-                double.IsInfinity(location.th))
-            {
-                Console.WriteLine(
-                    "Detour当前位姿无效，取消4m S型曲线测试。");
-                return;
-            }
-
-            var source =
-                new Vector2((float)location.x, (float)location.y);
-            var headingRadians =
-                AngleMath.DegreesToRadians(location.th);
-            var length = LengthMillimeters;
-            var offset = LateralOffsetMillimeters;
-
-            // 三段三次贝塞尔依次经过左侧峰值、中心线和右侧峰值，
-            // 起点、两个峰值和终点的切线均沿初始前向，连接处没有折角。
-            var firstControlPoints = new List<Vector2>
-            {
-                LocalToWorld(source, headingRadians, 0f, 0f),
-                LocalToWorld(
-                    source, headingRadians,
-                    length / 12f, 0f),
-                LocalToWorld(
-                    source, headingRadians,
-                    length / 6f, offset),
-                LocalToWorld(
-                    source, headingRadians,
-                    length * 0.25f, offset)
-            };
-            var secondControlPoints = new List<Vector2>
-            {
-                LocalToWorld(
-                    source, headingRadians,
-                    length * 0.25f, offset),
-                LocalToWorld(
-                    source, headingRadians,
-                    length / 3f, offset),
-                LocalToWorld(
-                    source, headingRadians,
-                    length * 2f / 3f, -offset),
-                LocalToWorld(
-                    source, headingRadians,
-                    length * 0.75f, -offset)
-            };
-            var thirdControlPoints = new List<Vector2>
-            {
-                LocalToWorld(
-                    source, headingRadians,
-                    length * 0.75f, -offset),
-                LocalToWorld(
-                    source, headingRadians,
-                    length * 5f / 6f, -offset),
-                LocalToWorld(
-                    source, headingRadians,
-                    length * 11f / 12f, 0f),
-                LocalToWorld(
-                    source, headingRadians,
-                    length, 0f)
-            };
-
-            var firstTrack = new BezierTrack(firstControlPoints)
-            {
-                Speed = CruiseSpeed,
-                CarDirectionBias = 0f
-            };
-            var secondTrack = new BezierTrack(secondControlPoints)
-            {
-                Speed = CruiseSpeed,
-                CarDirectionBias = 0f
-            };
-            var thirdTrack = new BezierTrack(thirdControlPoints)
-            {
-                Speed = CruiseSpeed,
-                CarDirectionBias = 0f
-            };
-
-            var controller = new ChassisController
-            {
-                BaseSpeed = CruiseSpeed
-            }.Get();
-            controller.FinishSpeed = 0f;
-
-            if (!controller.AddTrack(
-                    firstTrack,
-                    "SCurve4m-Part1") ||
-                !controller.AddTrack(
-                    secondTrack,
-                    "SCurve4m-Part2") ||
-                !controller.AddTrack(
-                    thirdTrack,
-                    "SCurve4m-Part3"))
-            {
-                Console.WriteLine(
-                    "4m S型曲线轨迹添加失败，取消测试。");
-                return;
-            }
-
-            var destination =
-                LocalToWorld(
-                    source,
-                    headingRadians,
-                    length,
-                    0f);
-            _recorder = new TrackingExperimentRecorder(
-                controllerName: "LegacyGeometricController",
-                trajectoryName:
-                    $"LegacySCurve4m_A{LateralOffsetMillimeters:0}mm",
-                trialNumber: TrialNumber,
-                referenceStart: source,
-                referenceEnd: destination,
-                referenceSpeed: CruiseSpeed);
-            _recorder.Start();
-
-            try
-            {
-                _task = new DriveTask(controller.Track());
-                _task.Wait();
-
-                // 保留少量停止后的数据，用于观察速度是否回到零。
-                Thread.Sleep(300);
-            }
-            finally
-            {
-                _task?.Stop();
-                _recorder?.UpdateCommand(0f, 0f);
-                _recorder?.StopAndSave();
-                _task = null;
-                _recorder = null;
-            }
-        }
-
-        // 停止S型曲线测试并保存当前已经采集的数据。
-        public override void TestStop()
-        {
-            _task?.Stop();
-            _recorder?.UpdateCommand(0f, 0f);
-            _recorder?.StopAndSave();
-        }
-
-        // 将车体起点局部坐标转换为Detour世界坐标，X向前、Y向左。
-        private static Vector2 LocalToWorld(
-            Vector2 origin,
-            double headingRadians,
-            float localX,
-            float localY)
-        {
-            var cos = (float)Math.Cos(headingRadians);
-            var sin = (float)Math.Sin(headingRadians);
-
-            return new Vector2(
-                origin.X + localX * cos - localY * sin,
-                origin.Y + localX * sin + localY * cos);
-        }
-    }
-
-    #endregion
-    
-    #region 夹臂运动测试
-    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;
-                PilotDefinition.Self.SpeedRightArm = 0f;
-                if (ReferenceEquals(_task, task))
-                    _task = null;
-            }
-        }
-
-        // 停止夹臂任务并立即清零左右夹臂下发速度。
-        public override void TestStop()
-        {
-            _task?.Stop();
-            _task = null;
-            PilotDefinition.Self.SpeedLeftArm = 0f;
-            PilotDefinition.Self.SpeedRightArm = 0f;
-        }
-    }
-
-    [MovementTest(name = "夹臂关闭测试")]
-    public sealed class TestClampCloseMovement
-        : ClampMovementTestBase
-    {
-        protected override bool Close => false;
-    }
-
-    [MovementTest(name = "夹臂启动测试")]
-    public sealed class TestClampOpenMovement
-        : ClampMovementTestBase
-    {
-        protected override bool Close => true;
-    }
-    #endregion
-}

MultiWheelC/Movements.cs

@@ -1,629 +0,0 @@
-using ClumsyCore;
-using ClumsyCore.DTools;
-using ClumsyCore.Interfaces;
-using ClumsyCore.Pilot;
-using CommonUsage.Chassis;
-using MDCSToolBox.Clumsy.Movements;
-using MDCSToolBox.Clumsy.Tracks;
-using MDCSToolBox.Commons.Controllers;
-using System;
-using System.Collections.Generic;
-using System.Drawing;
-using System.Numerics;
-using System.Threading;
-using FundamentalLib;
-using MyParking.Shared;
-
-namespace MultiWheelC
-{
-    // C层测试准备：停车并等待四个舵轮稳定回到车体前向0°。
-    public class PrepareWheelsForward : MovementDefinition
-    {
-        public float ToleranceDegrees = 2f;
-        public float StableSeconds = 0.3f;
-        public float TimeoutSeconds = 10f;
-        public bool Completed { get; private set; }
-
-        public override IEnumerable<bool> Get()
-        {
-            var chassis =
-                PilotDefinition.Chassis as MultiWheelChassis;
-            if (chassis == null)
-            {
-                throw new InvalidOperationException(
-                    "当前底盘不是MultiWheelChassis，无法执行舵轮回正。");
-            }
-
-            var adapter = new MultiWheelChassisAdapter(
-                chassis,
-                PilotDefinition.Self.CarNum);
-            adapter.ResetToBodyFrame();
-            var toleranceRadians =
-                AngleMath.DegreesToRadians(ToleranceDegrees);
-            var startTime = DateTime.UtcNow;
-            DateTime? alignedSince = null;
-
-            Completed = false;
-            if (!adapter.PrepareParallelDirection(0.0))
-            {
-                throw new InvalidOperationException(
-                    "无法将所有舵轮下发到车体前向0°。");
-            }
-
-            try
-            {
-                while (true)
-                {
-                    var aligned =
-                        adapter.AreParallelWheelsAligned(
-                            0.0,
-                            toleranceRadians);
-
-                    if (aligned)
-                    {
-                        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
-            {
-                // 只清零驱动速度，保留已经下发的0°舵角。
-                adapter.StopImmediately();
-            }
-        }
-    }
-
-    #region 功能项
-    public class Sleep : MovementDefinition
-    {
-        public float Second = 2f;
-
-        public override IEnumerable<bool> Get()
-        {
-            if (Second <= 0)
-            {
-                yield return false;
-                yield break;
-            }
-
-            var endTime = DateTime.UtcNow.AddSeconds(Second);
-            while (DateTime.UtcNow < endTime)
-            {
-                Thread.Sleep(50);
-                yield return true;
-            }
-
-            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.PredefinedDriveStop();
-            }
-        }
-
-    }
-    //直线行走基于轮里程
-    // 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 =
-                AngleMath.DegreesToRadians(curpose.th);
-            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>
-        /// 旋转目标角度
-        /// </summary>
-        public float AngleTarget;
-
-        public Func<float> ThetaReader = () => (float)DetourInterface.getCartLocation().th;
-
-        public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis;
-
-        public Func<PIDParams> PidparamsRead = () => new PIDParams() { };
-
-        public PIDController thPid;
-
-        // 将本周期PID角速度输出提供给实验记录器，单位deg/s。
-        public Action<float> CommandAngularSpeedObserver;
-
-        // 自转前舵轮实际角度允许误差，单位deg。
-        public float WheelAlignmentToleranceDegrees = 2f;
-
-        // 自转舵轮连续保持到位的时间，单位s。
-        public float WheelAlignmentStableSeconds = 0.3f;
-
-        // 自转舵轮准备超时时间，单位s。
-        public float WheelAlignmentTimeoutSeconds = 10f;
-
-        // 先准备自转舵角，再通过安全版SendXYThSpeed闭环旋转到目标角度。
-        public override IEnumerable<bool> Get()
-        {
-            if (Chassis == null)
-                throw new InvalidOperationException(
-                    "当前底盘不是MultiWheelChassis，无法执行原地自转。");
-
-            var adapter = new MultiWheelChassisAdapter(
-                Chassis,
-                PilotDefinition.Self.CarNum);
-            adapter.ResetToBodyFrame();
-
-            try
-            {
-                var alignmentStarted = DateTime.Now;
-                DateTime? alignedSince = null;
-                while (true)
-                {
-                    if (!adapter.PrepareSpin())
-                        throw new InvalidOperationException(
-                            "无法生成原地自转舵轮目标：" +
-                            adapter.LastFailureReason);
-
-                    if (adapter.AreSpinWheelsAligned)
-                    {
-                        if (alignedSince == null)
-                            alignedSince = DateTime.Now;
-
-                        if ((DateTime.Now - alignedSince.Value)
-                                .TotalSeconds >=
-                            WheelAlignmentStableSeconds)
-                            break;
-                    }
-                    else
-                    {
-                        alignedSince = null;
-                    }
-
-                    if ((DateTime.Now - alignmentStarted)
-                            .TotalSeconds >
-                        WheelAlignmentTimeoutSeconds)
-                        throw new TimeoutException(
-                            "原地自转舵轮在限定时间内未稳定到位。");
-
-                    yield return true;
-                }
-
-                var targetAngle =
-                    (float)AngleMath.NormalizeDegrees(AngleTarget);
-                var p = PidparamsRead();
-                thPid = new PIDController(ThetaReader, p.Kp);
-                thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone,
-                    p.OutputUpperThreshold, p.SpeedAccPerSec);
-                var lastCommandTime = DateTime.Now;
-
-                while (true)
-                {
-                    var s = thPid.GetResponse(targetAngle, true);
-                    Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
-                    CommandAngularSpeedObserver?.Invoke(s);
-                    var now = DateTime.Now;
-                    var interval = now - lastCommandTime;
-                    lastCommandTime = now;
-
-                    // PID输出s为deg/s，Shared命令统一使用rad/s。
-                    // adapter.Send最终调用普通安全版SendXYThSpeed。
-                    var omegaRadiansPerSecond =
-                        (float)AngleMath.DegreesToRadians(s);
-                    if (!adapter.Send(
-                            new ChassisCommand(
-                                PilotDefinition.Self.CarNum,
-                                new Twist2D(
-                                    0.0,
-                                    0.0,
-                                    omegaRadiansPerSecond)),
-                            interval))
-                    {
-                        throw new InvalidOperationException(
-                            "安全XYTh原地旋转底盘解算失败：" +
-                            adapter.LastFailureReason);
-                    }
-                    if (thPid.IsArrived()) break;
-                    yield return true;
-                }
-
-                Console.WriteLine($"final rotate to {targetAngle}");
-            }
-            finally
-            {
-                CommandAngularSpeedObserver?.Invoke(0f);
-                adapter.StopImmediately();
-            }
-        }
-    }
-    #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
-}

MultiWheelC/Movements/MotionPlanExecutor.cs

@@ -0,0 +1,266 @@
+using System;
+using System.Collections.Generic;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using CommonUsage.Chassis;
+using MultiWheelC.Control.Execution;
+using MultiWheelC.StateEstimation;
+using MultiWheelC.Trajectory;
+using MyParking.Shared;
+
+namespace MultiWheelC
+{
+    /// <summary>
+    /// 表示组合运动计划中由一种控制方式完整执行的单个动作段。
+    /// </summary>
+    public abstract class MotionPlanSegment
+    {
+    }
+
+    /// <summary>
+    /// 表示使用新版几何控制器跟踪一条连续二维轨迹的动作段。
+    /// </summary>
+    public sealed class TrackMotionPlanSegment : MotionPlanSegment
+    {
+        public TrackMotionPlanSegment(Trajectory2D trajectory)
+        {
+            Trajectory = trajectory ??
+                throw new ArgumentNullException(nameof(trajectory));
+        }
+
+        public Trajectory2D Trajectory { get; }
+
+        /// <summary>
+        /// 获取或设置本段轨迹独立的终点距离容差；为空时沿用轨迹动作默认值。
+        /// </summary>
+        public double? FinishDistanceMeters { get; set; }
+
+        /// <summary>
+        /// 获取或设置本段轨迹独立的停车速度容差；为空时沿用轨迹动作默认值。
+        /// </summary>
+        public double? FinishSpeedMetersPerSecond { get; set; }
+
+        /// <summary>
+        /// 获取或设置本段轨迹独立的终点航向容差；为空时沿用轨迹动作默认值。
+        /// </summary>
+        public double? FinishHeadingToleranceRadians { get; set; }
+    }
+
+    /// <summary>
+    /// 表示车辆停车后原地旋转到指定世界航向的动作段。
+    /// </summary>
+    public sealed class RotateInPlaceMotionPlanSegment
+        : MotionPlanSegment
+    {
+        public RotateInPlaceMotionPlanSegment(
+            double targetYawRadians)
+        {
+            if (double.IsNaN(targetYawRadians) ||
+                double.IsInfinity(targetYawRadians))
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(targetYawRadians),
+                    "原地自转目标航向必须是有限值。");
+            }
+
+            TargetYawRadians =
+                AngleMath.NormalizeRadians(targetYawRadians);
+        }
+
+        public double TargetYawRadians { get; }
+    }
+
+    /// <summary>
+    /// 顺序执行连续轨迹和原地自转动作，并在动作段边界完成停车与控制器切换。
+    /// </summary>
+    public sealed class MotionPlanExecutor : MovementDefinition
+    {
+        /// <summary>
+        /// 获取或设置一次性提交并按顺序执行的组合运动计划。
+        /// </summary>
+        public IReadOnlyList<MotionPlanSegment> Segments;
+
+        /// <summary>
+        /// 获取或设置所有动作段共享的车辆状态源；为空时组合Detour位姿与电机反馈速度。
+        /// </summary>
+        public IVehicleStateProvider StateProvider;
+
+        /// <summary>
+        /// 获取或设置创建每段轨迹动作后应用参数的回调。
+        /// </summary>
+        public Action<TrajectoryTrackingMovement>
+            ConfigureTrackingMovement;
+
+        /// <summary>
+        /// 获取或设置创建每段原地自转动作后应用参数的回调。
+        /// </summary>
+        public Action<MultiWheelRotateInPlace>
+            ConfigureRotationMovement;
+
+        /// <summary>
+        /// 获取或设置动作段开始前的通知，参数依次为索引和动作段。
+        /// </summary>
+        public Action<int, MotionPlanSegment> SegmentStarted;
+
+        /// <summary>
+        /// 获取或设置轨迹段每个有效控制周期后的诊断通知。
+        /// </summary>
+        public Action<int, ParkingGeometricController>
+            TrackingCycleObserver;
+
+        /// <summary>
+        /// 获取或设置自转段角速度命令通知，角速度单位为rad/s。
+        /// </summary>
+        public Action<int, double> RotationCommandObserver;
+
+        /// <summary>
+        /// 按计划顺序执行各动作段，任一动作失败时停止后续动作。
+        /// </summary>
+        public override IEnumerable<bool> Get()
+        {
+            var segments = ValidateAndSnapshotSegments();
+
+            var chassis =
+                PilotDefinition.Chassis as MultiWheelChassis;
+            if (chassis == null)
+            {
+                throw new InvalidOperationException(
+                    "当前底盘不是MultiWheelChassis，无法执行组合运动计划。");
+            }
+
+            var stateProvider =
+                StateProvider ??
+                ParkingVehicleStateProviderFactory.Create(
+                    chassis);
+
+            for (var index = 0;
+                 index < segments.Count;
+                 index++)
+            {
+                var segment = segments[index];
+
+                SegmentStarted?.Invoke(index, segment);
+
+                if (segment is TrackMotionPlanSegment track)
+                {
+                    var movement =
+                        new TrajectoryTrackingMovement
+                        {
+                            Trajectory = track.Trajectory,
+                            StateProvider = stateProvider,
+                            CycleObserver = controller =>
+                                TrackingCycleObserver?.Invoke(
+                                    index,
+                                    controller)
+                        };
+                    ConfigureTrackingMovement?.Invoke(movement);
+
+                    // 单段参数后应用，确保中间连接段可以覆盖组合动作的公共配置。
+                    if (track.FinishDistanceMeters.HasValue)
+                    {
+                        movement.FinishDistanceMeters =
+                            track.FinishDistanceMeters.Value;
+                    }
+
+                    if (track.FinishSpeedMetersPerSecond.HasValue)
+                    {
+                        movement.FinishSpeedMetersPerSecond =
+                            track.FinishSpeedMetersPerSecond.Value;
+                    }
+
+                    if (track.FinishHeadingToleranceRadians.HasValue)
+                    {
+                        movement.FinishHeadingToleranceRadians =
+                            track.FinishHeadingToleranceRadians.Value;
+                    }
+
+                    foreach (var keepRunning in movement.Get())
+                    {
+                        if (!keepRunning)
+                        {
+                            break;
+                        }
+
+                        yield return true;
+                    }
+
+                    continue;
+                }
+
+                if (segment is RotateInPlaceMotionPlanSegment rotate)
+                {
+                    var movement =
+                        new MultiWheelRotateInPlace
+                        {
+                            AngleTarget =
+                                (float)AngleMath.RadiansToDegrees(
+                                    rotate.TargetYawRadians),
+                            StateProvider = stateProvider,
+                            CommandAngularSpeedObserver =
+                                commandDegreesPerSecond =>
+                                    RotationCommandObserver?.Invoke(
+                                        index,
+                                        AngleMath.DegreesToRadians(
+                                            commandDegreesPerSecond))
+                        };
+                    ConfigureRotationMovement?.Invoke(movement);
+
+                    foreach (var keepRunning in movement.Get())
+                    {
+                        if (!keepRunning)
+                        {
+                            break;
+                        }
+
+                        yield return true;
+                    }
+
+                    continue;
+                }
+
+                throw new NotSupportedException(
+                    $"组合运动计划不支持动作段类型：{segment.GetType().FullName}。");
+            }
+
+            // 所有子动作均已完成后，才向外层DriveTask发送组合计划结束信号。
+            yield return false;
+        }
+
+        /// <summary>
+        /// 在车辆动作开始前验证全部动作段，并创建本次执行使用的稳定快照。
+        /// </summary>
+        private IReadOnlyList<MotionPlanSegment>
+            ValidateAndSnapshotSegments()
+        {
+            if (Segments == null || Segments.Count == 0)
+            {
+                throw new InvalidOperationException(
+                    "组合运动计划至少需要包含一个动作段。");
+            }
+
+            var segments =
+                new MotionPlanSegment[Segments.Count];
+
+            for (var index = 0;
+                 index < Segments.Count;
+                 index++)
+            {
+                var segment = Segments[index] ??
+                    throw new InvalidOperationException(
+                        $"组合运动计划第{index}段为空。");
+
+                if (!(segment is TrackMotionPlanSegment) &&
+                    !(segment is RotateInPlaceMotionPlanSegment))
+                {
+                    throw new NotSupportedException(
+                        "组合运动计划不支持动作段类型：" +
+                        $"{segment.GetType().FullName}。");
+                }
+
+                segments[index] = segment;
+            }
+
+            return segments;
+        }
+    }
+}

MultiWheelC/Movements/PrepareWheelsForward.cs

@@ -0,0 +1,140 @@
+
+
+
+using System;
+using System.Collections.Generic;
+using ClumsyCore.Pilot;
+using CommonUsage.Chassis;
+using MyParking.Shared;
+
+namespace MultiWheelC
+{
+    /// <summary>
+    /// 停车并等待四个舵轮稳定回到车体前向0°。
+    /// </summary>
+    public class PrepareWheelsForward : MovementDefinition
+    {
+        /// <summary>
+        /// 获取或设置舵轮需要对准的车体方向，单位为rad；0表示车头方向。
+        /// </summary>
+        public double DirectionRadians;
+
+        /// <summary>
+        /// 获取或设置本次动作的回正到位容差覆盖值，单位为deg；为空时读取车辆配置。
+        /// </summary>
+        public float? ToleranceDegrees;
+
+        /// <summary>
+        /// 获取或设置本次动作的稳定确认时间覆盖值，单位为s；为空时读取车辆配置。
+        /// </summary>
+        public float? StableSeconds;
+
+        /// <summary>
+        /// 获取或设置本次动作的超时覆盖值，单位为s；为空时读取车辆配置，0表示关闭超时。
+        /// </summary>
+        public float? TimeoutSeconds;
+
+        public bool Completed { get; private set; }
+
+        /// <summary>
+        /// 读取一次有效配置并等待全部舵轮在容差内稳定保持车体前向0°。
+        /// </summary>
+        public override IEnumerable<bool> Get()
+        {
+            NumericGuard.EnsureFinite(
+                DirectionRadians,
+                nameof(DirectionRadians));
+
+            var config = PilotDefinition.Conf;
+            var toleranceDegrees =
+                ToleranceDegrees ??
+                config.ParkingWheelForwardToleranceDegrees;
+            var stableSeconds =
+                StableSeconds ??
+                config.ParkingWheelForwardStableSeconds;
+            var timeoutSeconds =
+                TimeoutSeconds ??
+                config.ParkingWheelForwardTimeoutSeconds;
+
+            NumericGuard.EnsureFiniteNonNegative(
+                toleranceDegrees,
+                nameof(ToleranceDegrees));
+            NumericGuard.EnsureFiniteNonNegative(
+                stableSeconds,
+                nameof(StableSeconds));
+            NumericGuard.EnsureFiniteNonNegative(
+                timeoutSeconds,
+                nameof(TimeoutSeconds));
+
+            var chassis =
+                PilotDefinition.Chassis as MultiWheelChassis;
+            if (chassis == null)
+            {
+                throw new InvalidOperationException(
+                    "当前底盘不是MultiWheelChassis，无法执行舵轮回正。");
+            }
+
+            var adapter = new MultiWheelChassisAdapter(
+                chassis,
+                PilotDefinition.Self.CarNum);
+            adapter.ResetToBodyFrame();
+            var toleranceRadians =
+                AngleMath.DegreesToRadians(toleranceDegrees);
+            var startTime = DateTime.UtcNow;
+            DateTime? alignedSince = null;
+
+            Completed = false;
+            if (!adapter.PrepareParallelDirection(
+                    DirectionRadians))
+            {
+                throw new InvalidOperationException(
+                    "无法将所有舵轮下发到指定运动方向。");
+            }
+
+            try
+            {
+                while (true)
+                {
+                    var aligned =
+                        adapter.AreParallelWheelsAligned(
+                            DirectionRadians,
+                            toleranceRadians);
+
+                    if (aligned)
+                    {
+                        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
+            {
+                // 只清零驱动速度，保留已经下发的目标舵角。
+                adapter.StopImmediately();
+            }
+        }
+    }
+}

MultiWheelC/Movements/RotateInPlace.cs

@@ -0,0 +1,655 @@
+
+
+using System;
+using System.Collections.Generic;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using CommonUsage.Chassis;
+using MDCSToolBox.Commons.Controllers;
+using MyParking.Shared;
+using MultiWheelC.StateEstimation;
+
+namespace MultiWheelC
+{
+    /// <summary>
+    /// 指定原地自转使用Detour绝对航向或轮组相对角度反馈。
+    /// </summary>
+    public enum InPlaceRotationFeedbackMode
+    {
+        DetourAbsoluteHeading,
+        RelativeWheelOdometry
+    }
+
+    /// <summary>
+    /// 将四个舵轮准备到自转姿态，按所选反馈旋转并在完成后等待舵轮回正。
+    /// </summary>
+    public class MultiWheelRotateInPlace : MovementDefinition
+    {
+        /// <summary>
+        /// Detour模式表示世界目标航向，轮组模式表示有符号相对旋转角度，单位deg。
+        /// </summary>
+        public float AngleTarget;
+
+        /// <summary>
+        /// 获取或设置原地自转反馈模式；默认保持现有Detour绝对航向闭环。
+        /// </summary>
+        public InPlaceRotationFeedbackMode FeedbackMode =
+            InPlaceRotationFeedbackMode.DetourAbsoluteHeading;
+
+        // 留作标定或单元测试时显式替换；为空时使用配置化Detour与电机反馈组合状态源。
+        public Func<float> ThetaReader;
+
+        public IVehicleStateProvider StateProvider;
+
+        public MultiWheelChassis Chassis =
+            PilotDefinition.Chassis as MultiWheelChassis;
+
+        /// <summary>
+        /// 获取或设置本次动作的PID参数读取覆盖；为空时读取车辆配置。
+        /// </summary>
+        public Func<PIDParams> PidparamsRead;
+
+        public PIDController thPid;
+
+        // 将本周期PID角速度输出提供给实验记录器，单位deg/s。
+        public Action<float> CommandAngularSpeedObserver;
+
+        // 自转前舵轮实际角度允许误差覆盖值，单位deg；为空时读取车辆配置。
+        public float? WheelAlignmentToleranceDegrees;
+
+        // 自转舵轮连续保持到位的时间，单位s。
+        public float WheelAlignmentStableSeconds = 0.3f;
+
+        // 自转舵轮准备超时时间，单位s。
+        public float WheelAlignmentTimeoutSeconds = 10f;
+
+        // 航向尚未到位时允许下发的最小有效角速度覆盖值，单位deg/s；为空时读取车辆配置。
+        public float? MinimumAngularSpeedDegreesPerSecond;
+
+        // 舵轮到位后执行航向闭环允许的最长时间覆盖值，单位s；为空时读取车辆配置。
+        public float? RotationTimeoutSeconds;
+
+        /// <summary>
+        /// 读取一次有效配置，闭环旋转到目标航向并在正常完成后等待舵轮稳定回正。
+        /// </summary>
+        public override IEnumerable<bool> Get()
+        {
+            if (Chassis == null)
+                throw new InvalidOperationException(
+                    "当前底盘不是MultiWheelChassis，无法执行原地自转。");
+
+            var config = PilotDefinition.Conf;
+            var pidParameters =
+                PidparamsRead == null
+                    ? new PIDParams
+                    {
+                        Kp = config.InPlaceRotateKp,
+                        Ki = config.InPlaceRotateKi,
+                        Kd = config.InPlaceRotateKd,
+                        MaxI = config.InPlaceRotateMaxI,
+                        DeadZone = config.InPlaceRotateArriveDeg,
+                        SpeedAccPerSec = config.InPlaceRotateAcc,
+                        OutputUpperThreshold =
+                            config.InPlaceRotateMaxSpeed
+                    }
+                    : PidparamsRead();
+            var wheelAlignmentToleranceDegrees =
+                WheelAlignmentToleranceDegrees ??
+                config.InPlaceRotateWheelAlignDeg;
+            var minimumAngularSpeedDegreesPerSecond =
+                MinimumAngularSpeedDegreesPerSecond ??
+                config.InPlaceRotateMinimumSpeed;
+            var rotationTimeoutSeconds =
+                RotationTimeoutSeconds ??
+                config.InPlaceRotateTimeoutSec;
+            var stateProvider =
+                StateProvider ??
+                ParkingVehicleStateProviderFactory.Create(
+                    Chassis,
+                    config);
+
+            ValidateParameters(
+                pidParameters,
+                wheelAlignmentToleranceDegrees,
+                minimumAngularSpeedDegreesPerSecond,
+                rotationTimeoutSeconds);
+
+            var adapter = new MultiWheelChassisAdapter(
+                Chassis,
+                PilotDefinition.Self.CarNum);
+            adapter.ResetToBodyFrame();
+
+            try
+            {
+                var alignmentStarted = DateTime.Now;
+                DateTime? alignedSince = null;
+                while (true)
+                {
+                    if (!adapter.PrepareSpin(
+                            alignmentToleranceDegrees:
+                                wheelAlignmentToleranceDegrees))
+                        throw new InvalidOperationException(
+                            "无法生成原地自转舵轮目标：" +
+                            adapter.LastFailureReason);
+
+                    if (adapter.AreSpinWheelsAligned)
+                    {
+                        if (alignedSince == null)
+                            alignedSince = DateTime.Now;
+
+                        if ((DateTime.Now - alignedSince.Value)
+                                .TotalSeconds >=
+                            WheelAlignmentStableSeconds)
+                            break;
+                    }
+                    else
+                    {
+                        alignedSince = null;
+                    }
+
+                    if ((DateTime.Now - alignmentStarted)
+                            .TotalSeconds >
+                        WheelAlignmentTimeoutSeconds)
+                        throw new TimeoutException(
+                            "原地自转舵轮在限定时间内未稳定到位。");
+
+                    yield return true;
+                }
+
+                var alignmentToleranceRadians =
+                    AngleMath.DegreesToRadians(
+                        wheelAlignmentToleranceDegrees);
+                if (!adapter.AdoptPreparedSpinForXYTh(
+                        alignmentToleranceRadians))
+                {
+                    throw new InvalidOperationException(
+                        "无法将已到位的自转舵角交接给XYTh：" +
+                        adapter.LastFailureReason);
+                }
+
+                var useRelativeWheelOdometry =
+                    FeedbackMode ==
+                    InPlaceRotationFeedbackMode
+                        .RelativeWheelOdometry;
+                var wheelStateProvider =
+                    useRelativeWheelOdometry
+                        ? stateProvider as
+                            WheelFeedbackVehicleStateProvider
+                        : null;
+                if (useRelativeWheelOdometry &&
+                    wheelStateProvider == null)
+                {
+                    throw new InvalidOperationException(
+                        "轮组相对角度自转需要" +
+                        "WheelFeedbackVehicleStateProvider。");
+                }
+
+                var targetAngle =
+                    useRelativeWheelOdometry
+                        ? AngleTarget
+                        : (float)AngleMath.NormalizeDegrees(
+                            AngleTarget);
+                var currentAngle =
+                    useRelativeWheelOdometry
+                        ? 0f
+                        : ReadCurrentAngleDegrees(stateProvider);
+                var cachedCurrentAngle = currentAngle;
+                thPid = new PIDController(
+                    () => cachedCurrentAngle,
+                    pidParameters.Kp);
+                thPid.ChangeParameters(
+                    pidParameters.Kp,
+                    pidParameters.Ki,
+                    pidParameters.Kd,
+                    pidParameters.MaxI,
+                    pidParameters.DeadZone,
+                    pidParameters.OutputUpperThreshold,
+                    pidParameters.SpeedAccPerSec);
+                var lastCommandTime = DateTime.Now;
+                var rotationStarted = DateTime.Now;
+                var accumulatedWheelAngleRadians = 0.0;
+                var previousWheelOmegaRadiansPerSecond = 0.0;
+                var previousWheelTimestampSeconds = 0.0;
+                var hasPreviousWheelSample = false;
+
+                while (true)
+                {
+                    if ((DateTime.Now - rotationStarted)
+                            .TotalSeconds >
+                        rotationTimeoutSeconds)
+                    {
+                        throw new TimeoutException(
+                            $"原地自转超过{rotationTimeoutSeconds:F1}s仍未到位。");
+                    }
+
+                    if (useRelativeWheelOdometry)
+                    {
+                        if (!wheelStateProvider.TryGetWheelTwist(
+                                out var wheelTwist,
+                                out var wheelTimestampSeconds))
+                        {
+                            CommandAngularSpeedObserver?.Invoke(0f);
+                            adapter
+                                .StopXYThDrivePreserveSteeringState();
+
+                            if (hasPreviousWheelSample)
+                            {
+                                throw new InvalidOperationException(
+                                    "原地自转期间轮组角速度不可用：" +
+                                    wheelStateProvider.LastFailureReason);
+                            }
+
+                            yield return true;
+                            continue;
+                        }
+
+                        if (hasPreviousWheelSample)
+                        {
+                            var wheelDeltaTimeSeconds =
+                                wheelTimestampSeconds -
+                                previousWheelTimestampSeconds;
+                            if (!NumericGuard.IsFinite(
+                                    wheelDeltaTimeSeconds) ||
+                                wheelDeltaTimeSeconds <= 0.0)
+                            {
+                                throw new InvalidOperationException(
+                                    "轮组角速度采样时间没有单调递增。");
+                            }
+
+                            accumulatedWheelAngleRadians +=
+                                0.5 *
+                                (previousWheelOmegaRadiansPerSecond +
+                                 wheelTwist
+                                     .OmegaRadiansPerSecond) *
+                                wheelDeltaTimeSeconds;
+                        }
+
+                        previousWheelOmegaRadiansPerSecond =
+                            wheelTwist.OmegaRadiansPerSecond;
+                        previousWheelTimestampSeconds =
+                            wheelTimestampSeconds;
+                        hasPreviousWheelSample = true;
+                        currentAngle =
+                            (float)AngleMath.RadiansToDegrees(
+                                accumulatedWheelAngleRadians);
+                    }
+                    else
+                    {
+                        currentAngle =
+                            ReadCurrentAngleDegrees(stateProvider);
+                    }
+
+                    cachedCurrentAngle = currentAngle;
+                    var s = thPid.GetResponse(
+                        targetAngle,
+                        !useRelativeWheelOdometry);
+                    var angleErrorDegrees =
+                        useRelativeWheelOdometry
+                            ? targetAngle - currentAngle
+                            : (float)AngleMath
+                                .ShortestDifferenceDegrees(
+                                    targetAngle,
+                                    currentAngle);
+
+                    // PID进入到位死区后等待其0.3s稳定确认；等待期间
+                    // 只清零驱动速度，不清除已经准备好的自转舵角状态。
+                    if (Math.Abs(angleErrorDegrees) <=
+                        pidParameters.DeadZone)
+                    {
+                        CommandAngularSpeedObserver?.Invoke(0f);
+                        adapter
+                            .StopXYThDrivePreserveSteeringState();
+
+                        // 到位稳定只依赖已经单独校验的航向。
+                        // 位置候选留到停车后处理，避免位置抖动中断航向闭环。
+                        if (thPid.IsArrived())
+                            break;
+
+                        yield return true;
+                        continue;
+                    }
+
+                    // PID输出低于底盘有效轮速范围时提高到最小可执行值，
+                    // 避免接近目标时反复出现微小命令但车辆实际不动。
+                    if (Math.Abs(s) > 1e-6f &&
+                        Math.Abs(s) <
+                            minimumAngularSpeedDegreesPerSecond)
+                    {
+                        s = Math.Sign(angleErrorDegrees) *
+                            minimumAngularSpeedDegreesPerSecond;
+                    }
+
+                    // PID加速限制在首周期可能暂时输出零；此时保留
+                    // 已交接的自转状态，等待下一周期产生有效角速度。
+                    if (Math.Abs(s) <= 1e-6f)
+                    {
+                        CommandAngularSpeedObserver?.Invoke(0f);
+                        adapter
+                            .StopXYThDrivePreserveSteeringState();
+                        yield return true;
+                        continue;
+                    }
+
+                    CommandAngularSpeedObserver?.Invoke(s);
+                    var now = DateTime.Now;
+                    var interval = now - lastCommandTime;
+                    lastCommandTime = now;
+
+                    // PID输出s为deg/s，Shared统一使用车体坐标系Twist2D和rad/s。
+                    var omegaRadiansPerSecond =
+                        (float)AngleMath.DegreesToRadians(s);
+                    if (!adapter.SendBodyTwist(
+                            new Twist2D(
+                                0.0,
+                                0.0,
+                                omegaRadiansPerSecond),
+                            interval))
+                    {
+                        throw new InvalidOperationException(
+                            "安全XYTh原地旋转底盘解算失败：" +
+                            adapter.LastFailureReason);
+                    }
+                    yield return true;
+                }
+
+                CommandAngularSpeedObserver?.Invoke(0f);
+                adapter.StopXYThDrivePreserveSteeringState();
+
+                if (!useRelativeWheelOdometry &&
+                    IsLocalizationRecoveryPending(
+                        stateProvider))
+                {
+                    BeginPostRotationPositionRecovery(
+                        stateProvider);
+                    var recoveryStarted = DateTime.Now;
+
+                    while (true)
+                    {
+                        adapter
+                            .StopXYThDrivePreserveSteeringState();
+
+                        if (stateProvider.TryGetState(out _) &&
+                            !IsLocalizationRecoveryPending(
+                                stateProvider))
+                        {
+                            break;
+                        }
+
+                        if ((DateTime.Now - recoveryStarted)
+                                .TotalSeconds >
+                            config
+                                .ParkingDetourJumpConfirmationTimeoutSeconds)
+                        {
+                            throw new InvalidOperationException(
+                                "原地自转完成后Detour位置在限定时间内未恢复。" +
+                                GetStateProviderFailureReason(
+                                    stateProvider));
+                        }
+
+                        yield return true;
+                    }
+                }
+
+                // 航向正常到位后复用统一回正动作；异常或取消会直接进入finally停车。
+                var wheelPreparation =
+                    new PrepareWheelsForward();
+                foreach (var keepRunning in wheelPreparation.Get())
+                {
+                    if (!keepRunning)
+                    {
+                        break;
+                    }
+
+                    yield return true;
+                }
+
+                if (!wheelPreparation.Completed)
+                {
+                    throw new InvalidOperationException(
+                        "原地自转完成后舵轮未能稳定回到车头方向。");
+                }
+
+                Console.WriteLine(
+                    useRelativeWheelOdometry
+                        ? "final relative wheel rotate to " +
+                          $"{currentAngle:F2}deg, wheels forward"
+                        : $"final rotate to {targetAngle}, wheels forward");
+            }
+            finally
+            {
+                CommandAngularSpeedObserver?.Invoke(0f);
+                adapter.StopImmediately();
+            }
+        }
+
+        /// <summary>
+        /// 检查原地自转的舵轮准备、最小速度和超时参数是否可执行。
+        /// </summary>
+        private void ValidateParameters(
+            PIDParams pidParameters,
+            float wheelAlignmentToleranceDegrees,
+            float minimumAngularSpeedDegreesPerSecond,
+            float rotationTimeoutSeconds)
+        {
+            EnsureFinitePositive(
+                wheelAlignmentToleranceDegrees,
+                nameof(WheelAlignmentToleranceDegrees),
+                allowZero: true);
+            EnsureFinitePositive(
+                WheelAlignmentStableSeconds,
+                nameof(WheelAlignmentStableSeconds),
+                allowZero: true);
+            EnsureFinitePositive(
+                WheelAlignmentTimeoutSeconds,
+                nameof(WheelAlignmentTimeoutSeconds));
+            EnsureFinitePositive(
+                minimumAngularSpeedDegreesPerSecond,
+                nameof(MinimumAngularSpeedDegreesPerSecond));
+            EnsureFinitePositive(
+                rotationTimeoutSeconds,
+                nameof(RotationTimeoutSeconds));
+
+            if (float.IsNaN(AngleTarget) ||
+                float.IsInfinity(AngleTarget))
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(AngleTarget),
+                    "原地自转目标角度必须是有限值。");
+            }
+
+            if (!Enum.IsDefined(
+                    typeof(InPlaceRotationFeedbackMode),
+                    FeedbackMode))
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(FeedbackMode),
+                    "原地自转反馈模式无效。");
+            }
+
+            if (FeedbackMode ==
+                    InPlaceRotationFeedbackMode
+                        .RelativeWheelOdometry &&
+                Math.Abs(AngleTarget) >= 180f)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(AngleTarget),
+                    "轮组相对自转角度必须满足-180° < angle < 180°。");
+            }
+
+            if (pidParameters == null)
+            {
+                throw new InvalidOperationException(
+                    "原地自转PID参数读取结果为空。");
+            }
+
+            EnsureFinitePositive(
+                pidParameters.DeadZone,
+                "PidparamsRead.DeadZone");
+            EnsureFinitePositive(
+                pidParameters.OutputUpperThreshold,
+                "PidparamsRead.OutputUpperThreshold");
+            EnsureFinitePositive(
+                pidParameters.SpeedAccPerSec,
+                "PidparamsRead.SpeedAccPerSec");
+            EnsureFinitePositive(
+                pidParameters.Kp,
+                "PidparamsRead.Kp");
+
+            if (minimumAngularSpeedDegreesPerSecond >
+                pidParameters.OutputUpperThreshold)
+            {
+                throw new InvalidOperationException(
+                    "原地自转最小有效角速度不能大于最大角速度。");
+            }
+        }
+
+        /// <summary>
+        /// 读取经过状态源校验的世界航向，显式设置ThetaReader时优先使用替代读数。
+        /// </summary>
+        private float ReadCurrentAngleDegrees(
+            IVehicleStateProvider stateProvider)
+        {
+            if (ThetaReader != null)
+            {
+                var angleDegrees = ThetaReader();
+                if (float.IsNaN(angleDegrees) ||
+                    float.IsInfinity(angleDegrees))
+                {
+                    throw new InvalidOperationException(
+                        "自定义航向读取结果不是有效角度。");
+                }
+
+                return (float)AngleMath.NormalizeDegrees(
+                    angleDegrees);
+            }
+
+            if (stateProvider is
+                WheelFeedbackVehicleStateProvider wheelProvider)
+            {
+                if (!wheelProvider.TryGetHeadingRadians(
+                        out var wheelHeadingRadians))
+                {
+                    throw new InvalidOperationException(
+                        "无法从Detour状态源读取有效车辆航向。" +
+                        wheelProvider.LastHeadingFailureReason);
+                }
+
+                return (float)AngleMath.RadiansToDegrees(
+                    wheelHeadingRadians);
+            }
+
+            if (stateProvider is
+                DetourVehicleStateProvider detourProvider)
+            {
+                if (!detourProvider.TryGetHeadingRadians(
+                        out var detourHeadingRadians))
+                {
+                    throw new InvalidOperationException(
+                        "无法从Detour状态源读取有效车辆航向。" +
+                        detourProvider.LastHeadingFailureReason);
+                }
+
+                return (float)AngleMath.RadiansToDegrees(
+                    detourHeadingRadians);
+            }
+
+            if (stateProvider == null ||
+                !stateProvider.TryGetState(out var state))
+            {
+                throw new InvalidOperationException(
+                    "无法从Detour状态源读取有效车辆航向。" +
+                    GetStateProviderFailureReason(
+                        stateProvider));
+            }
+
+            return (float)AngleMath.RadiansToDegrees(
+                state.PoseInWorld.YawRadians);
+        }
+
+        /// <summary>
+        /// 通知配置化状态源：车辆已经停车，可以重新确认旋转期间的位置候选。
+        /// </summary>
+        private static void BeginPostRotationPositionRecovery(
+            IVehicleStateProvider stateProvider)
+        {
+            if (stateProvider is
+                WheelFeedbackVehicleStateProvider wheelProvider)
+            {
+                wheelProvider.BeginPostRotationPositionRecovery();
+                return;
+            }
+
+            if (stateProvider is
+                DetourVehicleStateProvider detourProvider)
+            {
+                detourProvider.BeginPostRotationPositionRecovery();
+            }
+        }
+
+        /// <summary>
+        /// 获取已知停车状态源最近一次失败原因，未知实现返回空字符串。
+        /// </summary>
+        private static string GetStateProviderFailureReason(
+            IVehicleStateProvider stateProvider)
+        {
+            if (stateProvider is
+                WheelFeedbackVehicleStateProvider wheelProvider)
+            {
+                return wheelProvider.LastFailureReason;
+            }
+
+            if (stateProvider is
+                DetourVehicleStateProvider detourProvider)
+            {
+                return detourProvider.LastFailureReason;
+            }
+
+            return string.Empty;
+        }
+
+        /// <summary>
+        /// 判断Detour是否仍在使用轮组预测确认疑似位姿不连续。
+        /// </summary>
+        private static bool IsLocalizationRecoveryPending(
+            IVehicleStateProvider stateProvider)
+        {
+            if (stateProvider is
+                    WheelFeedbackVehicleStateProvider wheelProvider)
+            {
+                return wheelProvider.TryGetLatestDetourDiagnostics(
+                           out var jumpCandidateActive,
+                           out _,
+                           out _,
+                           out _,
+                           out _,
+                           out _,
+                           out _) &&
+                       jumpCandidateActive;
+            }
+
+            return stateProvider is
+                       DetourVehicleStateProvider detourProvider &&
+                   detourProvider.IsJumpCandidateActive;
+        }
+
+        /// <summary>
+        /// 检查原地自转参数是否为正有限值，部分时间和容差参数允许为零。
+        /// </summary>
+        private static void EnsureFinitePositive(
+            float value,
+            string parameterName,
+            bool allowZero = false)
+        {
+            if (float.IsNaN(value) ||
+                float.IsInfinity(value) ||
+                (allowZero
+                    ? value < 0f
+                    : value <= 0f))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "原地自转参数必须是有效的正数。");
+            }
+        }
+    }
+}

MultiWheelC/Movements/TrajectoryTrackingMovement.cs

@@ -0,0 +1,627 @@
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using CommonUsage.Chassis;
+using MultiWheelC.Control.Abstractions;
+using MultiWheelC.Control.Allocation;
+using MultiWheelC.Control.Execution;
+using MultiWheelC.Control.Lateral;
+using MultiWheelC.Control.Longitudinal;
+using MultiWheelC.StateEstimation;
+using MultiWheelC.Trajectory;
+using MyParking.Shared;
+
+namespace MultiWheelC
+{
+    /// <summary>
+    /// 使用新版横纵向控制器持续跟踪一条世界坐标系二维轨迹。
+    /// </summary>
+    public sealed class TrajectoryTrackingMovement
+        : MovementDefinition
+    {
+        private const double ReferenceSpeedDeadbandMetersPerSecond =
+            1e-6;
+        private const double FixedMotionDirectionToleranceRadians =
+            3.0 * Math.PI / 180.0;
+
+        /// <summary>
+        /// 获取或设置本次动作需要跟踪的世界坐标系轨迹。
+        /// </summary>
+        public Trajectory2D Trajectory;
+
+        /// <summary>
+        /// 获取或设置本次动作使用的车辆状态源；为空时组合Detour位姿与电机反馈速度。
+        /// </summary>
+        public IVehicleStateProvider StateProvider;
+
+        /// <summary>
+        /// 获取或设置横向控制器创建委托；参数为车辆控制点半径（m），为空时使用配置化Stanley控制器。
+        /// </summary>
+        public Func<double, ILateralController>
+            LateralControllerFactory;
+
+        /// <summary>
+        /// 获取或设置每个有效控制周期结束后的诊断数据观察回调。
+        /// </summary>
+        public Action<ParkingGeometricController> CycleObserver;
+
+        /// <summary>
+        /// 获取或设置本动作运动坐标系X轴在车体系中的方向，单位为rad；为空时从轨迹自动推导。
+        /// </summary>
+        public double? MotionDirectionInBodyRadians = 0.0;
+
+        /// <summary>
+        /// 获取本次执行最终采用的运动坐标系方向，动作尚未开始时为空。
+        /// </summary>
+        public double? ResolvedMotionDirectionInBodyRadians
+        {
+            get;
+            private set;
+        }
+
+        /// <summary>
+        /// 获取或设置轨迹正常完成后是否停车并将舵轮主动恢复到车头方向。
+        /// </summary>
+        public bool ReturnWheelsForwardAfterCompletion;
+
+        /// <summary>
+        /// 获取或设置本次动作的Stanley横向误差增益覆盖值，单位为1/s；为空时读取车辆配置。
+        /// </summary>
+        public double? StanleyCrossTrackGainPerSecond;
+
+        /// <summary>
+        /// 获取或设置本次动作的Stanley航向误差增益覆盖值；为空时读取车辆配置。
+        /// </summary>
+        public double? StanleyHeadingErrorGain;
+
+        /// <summary>
+        /// 获取或设置本次动作的Stanley低速分母保护速度覆盖值，单位为m/s；为空时读取车辆配置。
+        /// </summary>
+        public double? StanleyMinimumSpeedMetersPerSecond;
+
+        /// <summary>
+        /// 获取或设置本次动作是否使用实际纵向速度的覆盖值；为空时读取车辆配置。
+        /// </summary>
+        public bool? StanleyUsesActualSpeed;
+
+        /// <summary>
+        /// 获取或设置本次动作的Stanley曲率前馈预瞄时间覆盖值，单位为s，0为关闭；为空时读取车辆配置。
+        /// </summary>
+        public double? StanleyCurvaturePreviewSeconds;
+
+        /// <summary>
+        /// 获取或设置本次动作的Stanley曲率前馈最大预瞄距离覆盖值，单位为m；为空时读取车辆配置。
+        /// </summary>
+        public double? StanleyMaximumCurvaturePreviewMeters;
+
+        /// <summary>
+        /// 获取或设置本次动作的Stanley横向修正上限覆盖值，单位为rad；为空时读取车辆配置。
+        /// </summary>
+        public double? MaximumCrossTrackCorrectionRadians;
+
+        /// <summary>
+        /// 获取或设置本次动作的Stanley航向修正上限覆盖值，单位为rad；为空时读取车辆配置。
+        /// </summary>
+        public double? MaximumHeadingCorrectionRadians;
+
+        /// <summary>
+        /// 获取或设置本次动作的纵向速度比例增益覆盖值；为空时读取车辆配置。
+        /// </summary>
+        public double? LongitudinalKp;
+
+        /// <summary>
+        /// 获取或设置本次动作的纵向速度积分增益覆盖值，单位为1/s；为空时读取车辆配置。
+        /// </summary>
+        public double? LongitudinalKiPerSecond;
+
+        /// <summary>
+        /// 获取或设置本次动作的纵向速度微分增益覆盖值，单位为s；为空时读取车辆配置。
+        /// </summary>
+        public double? LongitudinalKdSeconds;
+
+        /// <summary>
+        /// 获取或设置本次动作的纵向积分修正上限覆盖值，单位为m/s；为空时读取车辆配置。
+        /// </summary>
+        public double? MaximumIntegralCorrectionMetersPerSecond;
+
+        /// <summary>
+        /// 获取或设置本次动作的纵向速度误差死区覆盖值，单位为m/s；为空时读取车辆配置。
+        /// </summary>
+        public double? LongitudinalSpeedErrorDeadbandMetersPerSecond;
+
+        /// <summary>
+        /// 获取或设置本次动作的底盘纵向命令速度上限覆盖值，单位为m/s；为空时读取车辆配置。
+        /// </summary>
+        public double? MaximumCommandSpeedMetersPerSecond;
+
+        /// <summary>
+        /// 获取或设置本次动作的GCP转角上限覆盖值，单位为rad；为空时读取车辆配置。
+        /// </summary>
+        public double? MaximumGcpAngleRadians;
+
+        /// <summary>
+        /// 获取或设置本次动作的GCP转角变化率上限覆盖值，单位为rad/s；为空时读取车辆配置。
+        /// </summary>
+        public double? MaximumGcpAngleRateRadiansPerSecond;
+
+        /// <summary>
+        /// 获取或设置本次动作的终点距离容差覆盖值，单位为m；为空时读取车辆配置。
+        /// </summary>
+        public double? FinishDistanceMeters;
+
+        /// <summary>
+        /// 获取或设置本次动作的终点速度容差覆盖值，单位为m/s；为空时读取车辆配置。
+        /// </summary>
+        public double? FinishSpeedMetersPerSecond;
+
+        /// <summary>
+        /// 获取或设置本次动作的终点航向容差覆盖值，单位为rad；为空时读取车辆配置。
+        /// </summary>
+        public double? FinishHeadingToleranceRadians;
+
+        /// <summary>
+        /// 获取或设置本次动作的终点制动预瞄距离覆盖值，单位为m；为空时读取车辆配置。
+        /// </summary>
+        public double? TerminalBrakingPreviewMeters;
+
+        /// <summary>
+        /// 获取或设置本次动作进入终点单向低速逼近的剩余弧长覆盖值，单位为m；为空时读取车辆配置。
+        /// </summary>
+        public double? TerminalApproachDistanceMeters;
+
+        /// <summary>
+        /// 获取或设置本次动作由终点纵向剩余距离生成低速参考的比例增益覆盖值，单位为1/s；为空时读取车辆配置。
+        /// </summary>
+        public double? TerminalApproachGainPerSecond;
+
+        /// <summary>
+        /// 获取或设置本次动作终点单向逼近参考速度的最大绝对值覆盖值，单位为m/s；为空时读取车辆配置。
+        /// </summary>
+        public double? MaximumTerminalApproachSpeedMetersPerSecond;
+
+        /// <summary>
+        /// 获取或设置本次动作的最大轨迹偏离距离覆盖值，单位为m；为空时读取车辆配置。
+        /// </summary>
+        public double? MaximumDistanceToTrajectoryMeters;
+
+        /// <summary>
+        /// 获取或设置本次动作的执行超时覆盖值，单位为s；为空时读取车辆配置。
+        /// </summary>
+        public double? ExecutionTimeoutSeconds;
+
+        /// <summary>
+        /// 获取本次动作创建的控制器，尚未开始时为空。
+        /// </summary>
+        public ParkingGeometricController Controller { get; private set; }
+
+        /// <summary>
+        /// 等待舵轮稳定回正后创建控制器并持续执行，直到轨迹完成、失败或动作被取消。
+        /// </summary>
+        public override IEnumerable<bool> Get()
+        {
+            var config = PilotDefinition.Conf;
+            var stanleyCrossTrackGainPerSecond =
+                StanleyCrossTrackGainPerSecond ??
+                config.ParkingStanleyCrossTrackGain;
+            var stanleyHeadingErrorGain =
+                StanleyHeadingErrorGain ??
+                config.ParkingStanleyHeadingGain;
+            var stanleyMinimumSpeedMetersPerSecond =
+                StanleyMinimumSpeedMetersPerSecond ??
+                config.ParkingStanleyMinimumSpeed;
+            var stanleyUsesActualSpeed =
+                StanleyUsesActualSpeed ??
+                config.ParkingStanleyUseActualSpeed;
+            var stanleyCurvaturePreviewSeconds =
+                StanleyCurvaturePreviewSeconds ??
+                config.ParkingStanleyCurvaturePreviewSeconds;
+            var stanleyMaximumCurvaturePreviewMeters =
+                StanleyMaximumCurvaturePreviewMeters ??
+                config.ParkingStanleyMaximumCurvaturePreviewMeters;
+            var maximumCrossTrackCorrectionRadians =
+                MaximumCrossTrackCorrectionRadians ??
+                AngleMath.DegreesToRadians(
+                    config.ParkingMaximumCrossTrackCorrectionDegrees);
+            var maximumHeadingCorrectionRadians =
+                MaximumHeadingCorrectionRadians ??
+                AngleMath.DegreesToRadians(
+                    config.ParkingMaximumHeadingCorrectionDegrees);
+            var longitudinalKp =
+                LongitudinalKp ??
+                config.ParkingLongitudinalKp;
+            var longitudinalKiPerSecond =
+                LongitudinalKiPerSecond ??
+                config.ParkingLongitudinalKi;
+            var longitudinalKdSeconds =
+                LongitudinalKdSeconds ??
+                config.ParkingLongitudinalKd;
+            var maximumIntegralCorrectionMetersPerSecond =
+                MaximumIntegralCorrectionMetersPerSecond ??
+                config.ParkingMaximumIntegralCorrection;
+            var maximumCommandSpeedMetersPerSecond =
+                MaximumCommandSpeedMetersPerSecond ??
+                config.ParkingMaximumCommandSpeed;
+            var longitudinalSpeedErrorDeadbandMetersPerSecond =
+                LongitudinalSpeedErrorDeadbandMetersPerSecond ??
+                config.ParkingLongitudinalSpeedErrorDeadband;
+            var maximumGcpAngleRadians =
+                MaximumGcpAngleRadians ??
+                AngleMath.DegreesToRadians(
+                    config.ParkingMaximumGcpAngleDegrees);
+            var maximumGcpAngleRateRadiansPerSecond =
+                MaximumGcpAngleRateRadiansPerSecond ??
+                AngleMath.DegreesToRadians(
+                    config.ParkingMaximumGcpAngleRateDegreesPerSecond);
+            var finishDistanceMeters =
+                FinishDistanceMeters ??
+                config.ParkingFinishDistance;
+            var finishSpeedMetersPerSecond =
+                FinishSpeedMetersPerSecond ??
+                config.ParkingFinishSpeed;
+            var finishHeadingToleranceRadians =
+                FinishHeadingToleranceRadians ??
+                AngleMath.DegreesToRadians(
+                    config.ParkingFinishHeadingToleranceDegrees);
+            var terminalBrakingPreviewMeters =
+                TerminalBrakingPreviewMeters ??
+                config.ParkingTerminalBrakingPreview;
+            var terminalApproachDistanceMeters =
+                TerminalApproachDistanceMeters ??
+                config.ParkingTerminalApproachDistance;
+            var terminalApproachGainPerSecond =
+                TerminalApproachGainPerSecond ??
+                config.ParkingTerminalApproachGain;
+            var maximumTerminalApproachSpeedMetersPerSecond =
+                MaximumTerminalApproachSpeedMetersPerSecond ??
+                config.ParkingTerminalMaximumApproachSpeed;
+            var maximumDistanceToTrajectoryMeters =
+                MaximumDistanceToTrajectoryMeters ??
+                config.ParkingMaximumDistanceToTrajectory;
+            var executionTimeoutSeconds =
+                ExecutionTimeoutSeconds ??
+                config.ParkingExecutionTimeoutSeconds;
+
+            ValidateParameters(executionTimeoutSeconds);
+            var motionDirectionInBodyRadians =
+                MotionDirectionInBodyRadians ??
+                ResolveFixedMotionDirectionInBodyRadians(
+                    Trajectory);
+            ResolvedMotionDirectionInBodyRadians =
+                motionDirectionInBodyRadians;
+
+            var chassis =
+                PilotDefinition.Chassis as MultiWheelChassis;
+            if (chassis == null)
+            {
+                throw new InvalidOperationException(
+                    "当前底盘不是MultiWheelChassis，无法执行新版轨迹跟踪动作。");
+            }
+
+            var wheelPreparation =
+                new PrepareWheelsForward
+                {
+                    DirectionRadians =
+                        motionDirectionInBodyRadians
+                };
+            foreach (var keepRunning in wheelPreparation.Get())
+            {
+                if (!keepRunning)
+                {
+                    break;
+                }
+
+                yield return true;
+            }
+
+            if (!wheelPreparation.Completed)
+            {
+                throw new InvalidOperationException(
+                    "轨迹跟踪开始前舵轮未能稳定到达目标运动方向。");
+            }
+
+            var adapter = new MultiWheelChassisAdapter(
+                chassis,
+                PilotDefinition.Self.CarNum);
+
+            adapter.ActivateMotionFrame(
+                motionDirectionInBodyRadians);
+
+            var stateProvider =
+                StateProvider ??
+                ParkingVehicleStateProviderFactory.Create(
+                    chassis,
+                    config);
+            var controlPointRadiusMeters =
+                chassis.ControlPointRadius / 1000.0;
+
+            var lateralController =
+                LateralControllerFactory == null
+                    ? new StanleyLateralController(
+                        controlPointRadiusMeters,
+                        stanleyCrossTrackGainPerSecond,
+                        stanleyHeadingErrorGain,
+                        stanleyMinimumSpeedMetersPerSecond,
+                        stanleyUsesActualSpeed,
+                        maximumCrossTrackCorrectionRadians,
+                        maximumHeadingCorrectionRadians)
+                    : LateralControllerFactory(
+                        controlPointRadiusMeters);
+            if (lateralController == null)
+            {
+                throw new InvalidOperationException(
+                    "横向控制器创建委托不能返回空值。");
+            }
+            var longitudinalController =
+                new PidLongitudinalController(
+                    longitudinalKp,
+                    longitudinalKiPerSecond,
+                    longitudinalKdSeconds,
+                    maximumIntegralCorrectionMetersPerSecond,
+                    maximumCommandSpeedMetersPerSecond,
+                    longitudinalSpeedErrorDeadbandMetersPerSecond);
+            var gcpAllocator =
+                new GcpCommandAllocator(
+                    maximumGcpAngleRadians);
+            var commandExecutor =
+                new GcpCommandExecutor(
+                    adapter,
+                    maximumGcpAngleRateRadiansPerSecond,
+                    motionDirectionInBodyRadians);
+
+            Controller = new ParkingGeometricController(
+                stateProvider,
+                lateralController,
+                longitudinalController,
+                gcpAllocator,
+                commandExecutor,
+                finishDistanceMeters,
+                finishSpeedMetersPerSecond,
+                finishHeadingToleranceRadians,
+                maximumDistanceToTrajectoryMeters,
+                terminalBrakingPreviewMeters,
+                terminalApproachDistanceMeters,
+                terminalApproachGainPerSecond,
+                maximumTerminalApproachSpeedMetersPerSecond,
+                stanleyCurvaturePreviewSeconds,
+                stanleyMaximumCurvaturePreviewMeters,
+                motionDirectionInBodyRadians);
+
+            var clock = Stopwatch.StartNew();
+            var previousCycleSeconds =
+                clock.Elapsed.TotalSeconds;
+            Controller.Start(Trajectory);
+
+            try
+            {
+                while (true)
+                {
+                    if (clock.Elapsed.TotalSeconds >
+                        executionTimeoutSeconds)
+                    {
+                        throw new TimeoutException(
+                            $"新版轨迹跟踪超过{executionTimeoutSeconds:F1}s仍未完成。");
+                    }
+
+                    var currentCycleSeconds =
+                        clock.Elapsed.TotalSeconds;
+                    var deltaTimeSeconds =
+                        currentCycleSeconds -
+                        previousCycleSeconds;
+                    previousCycleSeconds =
+                        currentCycleSeconds;
+
+                    // 极短首周期不参与PID和GCP角速度限制，等待调度器进入下一周期。
+                    if (deltaTimeSeconds <= 1e-6)
+                    {
+                        yield return true;
+                        continue;
+                    }
+
+                    var result =
+                        Controller.ExecuteCycle(
+                            deltaTimeSeconds);
+
+                    // 诊断观察器按真实控制周期触发，即使本周期状态不可用，
+                    // 也允许记录状态读取和主动停车所消耗的时间。
+                    CycleObserver?.Invoke(Controller);
+
+                    if (result ==
+                        ParkingControlCycleResult.Completed)
+                    {
+                        break;
+                    }
+
+                    if (result ==
+                        ParkingControlCycleResult.Faulted)
+                    {
+                        throw new InvalidOperationException(
+                            string.IsNullOrWhiteSpace(
+                                Controller.LastFailureReason)
+                                ? "新版轨迹跟踪控制器发生未知故障。"
+                                : Controller.LastFailureReason,
+                            Controller.LastException);
+                    }
+
+                    if (result ==
+                        ParkingControlCycleResult.Inactive)
+                    {
+                        throw new InvalidOperationException(
+                            "新版轨迹跟踪控制器在轨迹完成前意外停止活动。");
+                    }
+
+                    // CommandSent和短暂StateUnavailable均继续下一控制周期；
+                    // 后者已经由控制器主动停车，等待Detour恢复。
+                    yield return true;
+                }
+            }
+            finally
+            {
+                Controller.Cancel();
+            }
+
+            if (ReturnWheelsForwardAfterCompletion)
+            {
+                var forwardPreparation =
+                    new PrepareWheelsForward();
+                foreach (var keepRunning in
+                         forwardPreparation.Get())
+                {
+                    if (!keepRunning)
+                    {
+                        break;
+                    }
+
+                    yield return true;
+                }
+
+                if (!forwardPreparation.Completed)
+                {
+                    throw new InvalidOperationException(
+                        "轨迹完成后舵轮未能稳定回到车头方向。");
+                }
+            }
+
+            yield return false;
+        }
+
+        /// <summary>
+        /// 在接管实际底盘前检查动作自身无法由子控制器检查的参数。
+        /// </summary>
+        private void ValidateParameters(
+            double executionTimeoutSeconds)
+        {
+            if (Trajectory == null)
+            {
+                throw new InvalidOperationException(
+                    "新版轨迹跟踪动作没有设置Trajectory。");
+            }
+
+            if (MotionDirectionInBodyRadians.HasValue)
+            {
+                NumericGuard.EnsureFinite(
+                    MotionDirectionInBodyRadians.Value,
+                    nameof(MotionDirectionInBodyRadians));
+            }
+
+            if (double.IsNaN(executionTimeoutSeconds) ||
+                double.IsInfinity(executionTimeoutSeconds) ||
+                executionTimeoutSeconds <= 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(ExecutionTimeoutSeconds),
+                    "轨迹跟踪超时时间必须是正有限值。");
+            }
+        }
+
+        /// <summary>
+        /// 根据轨迹切线、参考车身航向和速度符号推导整段轨迹共同使用的固定运动方向。
+        /// </summary>
+        private static double ResolveFixedMotionDirectionInBodyRadians(
+            Trajectory2D trajectory)
+        {
+            double? resolvedDirectionRadians = null;
+
+            for (var index = 0;
+                 index < trajectory.Count - 1;
+                 index++)
+            {
+                var segmentStart = trajectory[index];
+                var segmentEnd = trajectory[index + 1];
+                var travelDirection = ResolveSegmentTravelDirection(
+                    segmentStart.ReferenceSpeedMetersPerSecond,
+                    segmentEnd.ReferenceSpeedMetersPerSecond,
+                    index);
+                if (travelDirection == 0.0)
+                {
+                    continue;
+                }
+
+                var tangentYawRadians = Math.Atan2(
+                    segmentEnd.PoseInWorld.YMeters -
+                        segmentStart.PoseInWorld.YMeters,
+                    segmentEnd.PoseInWorld.XMeters -
+                        segmentStart.PoseInWorld.XMeters);
+                var positiveMotionAxisYawRadians =
+                    travelDirection > 0.0
+                        ? tangentYawRadians
+                        : AngleMath.NormalizeRadians(
+                            tangentYawRadians + Math.PI);
+                var referenceBodyYawRadians =
+                    AngleMath.LerpRadians(
+                        segmentStart.PoseInWorld.YawRadians,
+                        segmentEnd.PoseInWorld.YawRadians,
+                        0.5);
+                var candidateDirectionRadians =
+                    AngleMath.ShortestDifferenceRadians(
+                        positiveMotionAxisYawRadians,
+                        referenceBodyYawRadians);
+
+                if (!resolvedDirectionRadians.HasValue)
+                {
+                    resolvedDirectionRadians =
+                        candidateDirectionRadians;
+                    continue;
+                }
+
+                var directionDifferenceRadians = Math.Abs(
+                    AngleMath.ShortestDifferenceRadians(
+                        candidateDirectionRadians,
+                        resolvedDirectionRadians.Value));
+                if (directionDifferenceRadians >
+                    FixedMotionDirectionToleranceRadians)
+                {
+                    throw new InvalidOperationException(
+                        "轨迹无法由一个固定运动坐标系执行：" +
+                        $"第{index + 1}段需要的方向与起始方向相差" +
+                        $"{AngleMath.RadiansToDegrees(directionDifferenceRadians):F2}°。" +
+                        "请拆分轨迹，或显式指定并验证MotionDirectionInBodyRadians。");
+                }
+            }
+
+            if (!resolvedDirectionRadians.HasValue)
+            {
+                throw new InvalidOperationException(
+                    "轨迹没有非零参考速度线段，无法自动确定运动坐标系方向。");
+            }
+
+            return resolvedDirectionRadians.Value;
+        }
+
+        /// <summary>
+        /// 从相邻轨迹点的有符号参考速度确定该线段的执行方向。
+        /// </summary>
+        private static double ResolveSegmentTravelDirection(
+            double startSpeedMetersPerSecond,
+            double endSpeedMetersPerSecond,
+            int segmentStartIndex)
+        {
+            var hasStartDirection =
+                Math.Abs(startSpeedMetersPerSecond) >
+                ReferenceSpeedDeadbandMetersPerSecond;
+            var hasEndDirection =
+                Math.Abs(endSpeedMetersPerSecond) >
+                ReferenceSpeedDeadbandMetersPerSecond;
+
+            if (hasStartDirection &&
+                hasEndDirection &&
+                Math.Sign(startSpeedMetersPerSecond) !=
+                Math.Sign(endSpeedMetersPerSecond))
+            {
+                throw new InvalidOperationException(
+                    $"轨迹第{segmentStartIndex + 1}段内参考速度发生正负切换，" +
+                    "无法自动确定固定运动坐标系；请在零速点拆分动作段。");
+            }
+
+            if (hasStartDirection)
+            {
+                return Math.Sign(startSpeedMetersPerSecond);
+            }
+
+            return hasEndDirection
+                ? Math.Sign(endSpeedMetersPerSecond)
+                : 0.0;
+        }
+    }
+}

MultiWheelC/Old/AGV.cs

@@ -0,0 +1,21 @@
+// using ClumsyCore;
+// using MDCSToolBox.Clumsy.AgvInterfaces;
+// using MDCSToolBox.Clumsy.MotionControllers;
+
+// namespace MultiWheelC
+// {
+//     public class AGV : MultiWheelInterface
+//     {
+//         public override AbstractGeometricController GetController()
+//                 => new ChassisController().Get();
+//         public override MultiWheelMagTracker GetMagController()
+//             => new MultiWheelMagTracker();
+//         public override NaiveMagnetController GetNaiveMagnetController()
+//             => new NaiveMagnetController();
+
+//         public void Sleep(float seconds)
+//         {
+//             new DriveTask(new Sleep { Second = seconds }.Get()).Wait();
+//         }
+//     }
+// }

MultiWheelC/Old/ChassisController.cs

@@ -0,0 +1,45 @@
+// using ClumsyCore;
+// using ClumsyCore.Pilot;
+// using MDCSToolBox.Clumsy.MotionControllers;
+// using MDCSToolBox.Clumsy.Movements;
+// using MDCSToolBox.Clumsy.Pilot;
+
+// namespace MultiWheelC;
+
+// public class ChassisController : MovementDefinition<MultiWheelGeometricController>
+// {
+//     public float BaseSpeed = Configuration.conf.basicSpeed;
+
+//     // 创建单车几何跟踪控制器（直接控本车底盘，不走多车 Auto 通道）
+//     public override MultiWheelGeometricController Get()
+//     {
+//         return new MultiWheelGeometricController
+//         {
+//             Chassis = BasicPilotBase.Chassis,
+//             BaseSpeed = BaseSpeed,
+//             SlowDistance = PilotDefinition.Conf.SlowDistance,
+//             SlowingPow = PilotDefinition.Conf.SlowingPow,
+//             FinishDistance = PilotDefinition.Conf.FinishDistance,
+//             FinishSpeed = PilotDefinition.Conf.FinishSpeed,
+//             FirstThAccuracy = PilotDefinition.Conf.FirstThAccuracy,
+//             FirstRotateSpeedFac = PilotDefinition.Conf.FirstRotateSpeedFac,
+//             FirstRotateMaxSpeed = PilotDefinition.Conf.FirstRotateMaxSpeed,
+//             NotContinuousAngle = PilotDefinition.Conf.NotContinuousAngle,
+//             DebugMode = PilotDefinition.Conf.MotionDebugPrint,
+//             DebugCurvature = PilotDefinition.Conf.DebugCurvature,
+//             PowerSteeringLookAhead = PilotDefinition.Conf.PowerSteeringLookAhead,
+//             SpeedLookAhead = PilotDefinition.Conf.SpeedLookAhead,
+//             SpeedLookAheadCurveDiff = PilotDefinition.Conf.SpeedLookAheadCurveDiff,
+//             SpeedLookBackCurveDiff = PilotDefinition.Conf.SpeedLookBackCurveDiff,
+//             SpeedLimitCurveDiffMin = PilotDefinition.Conf.SpeedLimitCurveDiffMin,
+//             SpeedLimitCurveMin = PilotDefinition.Conf.SpeedLimitCurveMin,
+//             MaxRotateSpeed = PilotDefinition.Conf.MaxRotateSpeedCurveLimit,
+//             MaxRotateAcc = PilotDefinition.Conf.MaxRotateAccCurveLimit,
+//             GcpThetaThreshold = PilotDefinition.Conf.GcpThetaThreshold,
+//             DthLinearFac = PilotDefinition.Conf.DthLinearFac,
+//             DthLinearThreshold = PilotDefinition.Conf.DthLinearThreshold,
+//             BiasFac = PilotDefinition.Conf.BiasFac,
+//             BiasThreshold = PilotDefinition.Conf.BiasThreshold,
+//         };
+//     }
+// }

MultiWheelC/Old/ClampTests.cs

@@ -0,0 +1,90 @@
+
+
+using System;
+using ClumsyCore;
+using ClumsyCore.Pilot;
+using FundamentalLib;
+using MDCSToolBox.Clumsy.Movements;
+using MDCSToolBox.Clumsy.Pilot;
+
+namespace MultiWheelC
+{
+    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;
+                PilotDefinition.Self.SpeedRightArm = 0f;
+                if (ReferenceEquals(_task, task))
+                    _task = null;
+            }
+        }
+
+        // 停止夹臂任务并立即清零左右夹臂下发速度。
+        public override void TestStop()
+        {
+            _task?.Stop();
+            _task = null;
+            PilotDefinition.Self.SpeedLeftArm = 0f;
+            PilotDefinition.Self.SpeedRightArm = 0f;
+        }
+    }
+
+    [MovementTest(name = "夹臂关闭测试")]
+    public sealed class TestClampCloseMovement
+        : ClampMovementTestBase
+    {
+        protected override bool Close => false;
+    }
+
+    [MovementTest(name = "夹臂启动测试")]
+    public sealed class TestClampOpenMovement
+        : ClampMovementTestBase
+    {
+        protected override bool Close => true;
+    }
+}

MultiWheelC/Old/ClampToTarget.cs

@@ -0,0 +1,91 @@
+using System;
+using System.Collections.Generic;
+using ClumsyCore.Pilot;
+using MDCSToolBox.Commons.Controllers;
+
+namespace MultiWheelC
+{
+    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;
+            }
+        }
+    }
+}

MultiWheelC/Old/CrabMotionFrameTracker.cs

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

MultiWheelC/Old/DriverMovements.cs

@@ -0,0 +1,123 @@
+// using System;
+// using System.Collections.Generic;
+// using System.Threading;
+// using ClumsyCore.Pilot;
+
+// namespace MultiWheelC
+// {
+//     public class Sleep : MovementDefinition
+//     {
+//         public float Second = 2f;
+
+//         public override IEnumerable<bool> Get()
+//         {
+//             if (Second <= 0)
+//             {
+//                 yield return false;
+//                 yield break;
+//             }
+
+//             var endTime = DateTime.UtcNow.AddSeconds(Second);
+//             while (DateTime.UtcNow < endTime)
+//             {
+//                 Thread.Sleep(50);
+//                 yield return true;
+//             }
+
+//             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;
+//         }
+//     }
+// }

MultiWheelC/Old/DstTracker.cs

@@ -0,0 +1,55 @@
+// using System;
+// using System.Collections.Generic;
+// using System.Drawing;
+// using System.Numerics;
+// using ClumsyCore;
+// using ClumsyCore.DTools;
+// using ClumsyCore.Pilot;
+// using CommonUsage.Chassis;
+// using MDCSToolBox.Clumsy.Tracks;
+
+// namespace MultiWheelC
+// {
+//     //在世界坐标系下，从路径起点追踪到终点并停车
+//     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.PredefinedDriveStop();
+//             }
+//         }
+
+//     }
+// }

MultiWheelC/Old/LineTracking.cs

@@ -0,0 +1,178 @@
+// using System;
+// using System.Collections.Generic;
+// using System.Drawing;
+// using System.Numerics;
+// using ClumsyCore;
+// using ClumsyCore.DTools;
+// using ClumsyCore.Interfaces;
+// using ClumsyCore.Pilot;
+// using CommonUsage.Chassis;
+// using FundamentalLib;
+// using MDCSToolBox.Clumsy.Tracks;
+// using MDCSToolBox.Commons.Controllers;
+// using MyParking.Shared;
+
+// namespace MultiWheelC
+// {
+//     // 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 =
+//                 AngleMath.DegreesToRadians(curpose.th);
+//             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;
+//         }
+//     }
+// }

MultiWheelC/Old/PathTrackingTests.cs

@@ -0,0 +1,635 @@
+
+
+// using System;
+// using System.Collections.Generic;
+// using System.Numerics;
+// using System.Threading;
+// using ClumsyCore;
+// using ClumsyCore.Interfaces;
+// using ClumsyCore.Pilot;
+// using FundamentalLib;
+// using MDCSToolBox.Clumsy.Movements;
+// using MDCSToolBox.Clumsy.Pilot;
+// using MDCSToolBox.Clumsy.Tracks;
+// using MyParking.Shared;
+
+// namespace MultiWheelC
+// {
+//     [MovementTest(name = "SendMotion：连续前进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;
+//             }
+
+//             var location = DetourInterface.getCartLocation();
+//             if (double.IsNaN(location.x) ||
+//                 double.IsInfinity(location.x) ||
+//                 double.IsNaN(location.y) ||
+//                 double.IsInfinity(location.y) ||
+//                 double.IsNaN(location.th) ||
+//                 double.IsInfinity(location.th))
+//             {
+//                 Console.WriteLine(
+//                     "Detour当前位姿无效，取消连续前进4m测试。");
+//                 return;
+//             }
+//             var source = new Vector2((float)location.x, (float)location.y);
+//             // Detour航向单位是度，三角函数需要弧度。
+//             var headingRadians =
+//                 AngleMath.DegreesToRadians(location.th);
+//             var destination = new Vector2(
+//                 source.X + DistanceMillimeters * (float)Math.Cos(headingRadians),
+//                 source.Y + DistanceMillimeters * (float)Math.Sin(headingRadians));
+//             _recorder =
+//                         new TrackingExperimentRecorder(
+//                             controllerName: "LegacyGeometricController",
+//                             trajectoryName: "LegacyStraight4m",
+//                             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());
+//                 _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 = "SendMotion：左转90°半径2m圆弧")]
+//     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 =
+//                 AngleMath.DegreesToRadians(location.th);
+
+//             // 根据世界航向求车体左法向，左转圆心位于车辆左侧。
+//             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
+//             };
+
+//             // 左转90°后，圆心到终点的径向方向等于起始车头方向。
+//             var destination = center + new Vector2(
+//                 RadiusMillimeters *
+//                     (float)Math.Cos(headingRadians),
+//                 RadiusMillimeters *
+//                     (float)Math.Sin(headingRadians));
+
+//             if (!controller.AddTrack(arc, "LeftArc90Degrees"))
+//             {
+//                 Console.WriteLine(
+//                     "左转90°圆弧轨迹添加失败，取消测试。");
+//                 return;
+//             }
+
+//             _recorder = new TrackingExperimentRecorder(
+//                 controllerName: "LegacyGeometricController",
+//                 trajectoryName:
+//                     $"LegacyLeftArc90_R{RadiusMillimeters:0}mm",
+//                 trialNumber: TrialNumber,
+//                 referenceStart: source,
+//                 referenceEnd: destination,
+//                 referenceSpeed: CruiseSpeed);
+//             _recorder.Start();
+
+//             try
+//             {
+//                 _task = new DriveTask(controller.Track());
+//                 _task.Wait();
+
+//                 // 保留少量停车后的样本，用于观察速度是否回到零。
+//                 Thread.Sleep(300);
+//             }
+//             finally
+//             {
+//                 _task?.Stop();
+//                 _recorder?.UpdateCommand(0f, 0f);
+//                 _recorder?.StopAndSave();
+//                 _task = null;
+//                 _recorder = null;
+//             }
+//         }
+
+//         // 停止圆弧运动并保存当前已经采集的实验数据。
+//         public override void TestStop()
+//         {
+//             _task?.Stop();
+//             _recorder?.UpdateCommand(0f, 0f);
+//             _recorder?.StopAndSave();
+//         }
+//     }
+
+//     [MovementTest(name = "SendMotion：蟹行直线4m")]
+//     public class TestCrabForward4m : MovementTest
+//     {
+//         public float DistanceMillimeters = 4000f;
+//         public float CruiseSpeed = 0.2f;
+//         public int TrialNumber = 1;
+
+//         private DriveTask _task;
+//         private TrackingExperimentRecorder _recorder;
+
+//         // 将车体左侧作为运动前向，沿直线蟹行4m并记录Detour实验数据。
+//         public override void Test()
+//         {
+//             if (!TryReadStartPose(
+//                     out var source,
+//                     out var bodyYawRadians))
+//                 return;
+
+//             var motionYaw =
+//                 bodyYawRadians + Math.PI / 2.0;
+//             var destination = new Vector2(
+//                 source.X +
+//                     DistanceMillimeters *
+//                     (float)Math.Cos(motionYaw),
+//                 source.Y +
+//                     DistanceMillimeters *
+//                     (float)Math.Sin(motionYaw));
+
+//             var tracker = new CrabMotionFrameTracker
+//             {
+//                 CommandBackend =
+//                     CrabMotionFrameTracker
+//                         .ChassisCommandBackend
+//                         .SendMotion,
+//                 PathKind =
+//                     CrabMotionFrameTracker
+//                         .ReferencePathKind.Straight,
+//                 StartPosition = source,
+//                 InitialBodyYawRadians =
+//                     bodyYawRadians,
+//                 LengthMillimeters =
+//                     DistanceMillimeters,
+//                 CruiseSpeed = CruiseSpeed
+//             };
+
+//             _recorder = new TrackingExperimentRecorder(
+//                 controllerName:
+//                     "CrabSendMotionTracker",
+//                 trajectoryName:
+//                     "CrabStraight4m",
+//                 trialNumber: TrialNumber,
+//                 referenceStart: source,
+//                 referenceEnd: destination,
+//                 referenceSpeed: CruiseSpeed,
+//                 referenceMotionFrameYawDegrees: 90f);
+//             tracker.CommandObserver =
+//                 (vx, vy, omega) =>
+//                     _recorder?.UpdateBodyCommand(
+//                         vx,
+//                         vy,
+//                         omega);
+//             _recorder.Start();
+
+//             try
+//             {
+//                 _task = new DriveTask(tracker.Get());
+//                 _task.Wait();
+//                 Thread.Sleep(300);
+//             }
+//             finally
+//             {
+//                 _task?.Stop();
+//                 _recorder?.UpdateBodyCommand(
+//                     0f,
+//                     0f,
+//                     0f);
+//                 _recorder?.StopAndSave();
+//                 _task = null;
+//                 _recorder = null;
+//             }
+//         }
+
+//         public override void TestStop()
+//         {
+//             _task?.Stop();
+//             _recorder?.UpdateBodyCommand(
+//                 0f,
+//                 0f,
+//                 0f);
+//             _recorder?.StopAndSave();
+//         }
+
+//         // 读取并校验测试开始时的Detour世界位姿。
+//         private static bool TryReadStartPose(
+//             out Vector2 source,
+//             out double bodyYawRadians)
+//         {
+//             var location =
+//                 DetourInterface.getCartLocation();
+//             if (double.IsNaN(location.x) ||
+//                 double.IsInfinity(location.x) ||
+//                 double.IsNaN(location.y) ||
+//                 double.IsInfinity(location.y) ||
+//                 double.IsNaN(location.th) ||
+//                 double.IsInfinity(location.th))
+//             {
+//                 Console.WriteLine(
+//                     "Detour当前位姿无效，取消蟹行直线测试。");
+//                 source = Vector2.Zero;
+//                 bodyYawRadians = 0.0;
+//                 return false;
+//             }
+
+//             source = new Vector2(
+//                 (float)location.x,
+//                 (float)location.y);
+//             bodyYawRadians =
+//                 AngleMath.DegreesToRadians(location.th);
+//             return true;
+//         }
+//     }
+
+//     [MovementTest(name = "SendMotion：蟹行左转90°半径2m圆弧")]
+//     public class TestCrabLeftArc90 : MovementTest
+//     {
+//         public float RadiusMillimeters = 2000f;
+//         public float CruiseSpeed = 0.2f;
+//         public int TrialNumber = 1;
+
+//         private DriveTask _task;
+//         private TrackingExperimentRecorder _recorder;
+
+//         // 将车体左侧作为运动前向，沿半径2m的左转圆弧运动90°。
+//         public override void Test()
+//         {
+//             var location =
+//                 DetourInterface.getCartLocation();
+//             if (double.IsNaN(location.x) ||
+//                 double.IsInfinity(location.x) ||
+//                 double.IsNaN(location.y) ||
+//                 double.IsInfinity(location.y) ||
+//                 double.IsNaN(location.th) ||
+//                 double.IsInfinity(location.th))
+//             {
+//                 Console.WriteLine(
+//                     "Detour当前位姿无效，取消蟹行圆弧测试。");
+//                 return;
+//             }
+
+//             var source = new Vector2(
+//                 (float)location.x,
+//                 (float)location.y);
+//             var bodyYawRadians =
+//                 AngleMath.DegreesToRadians(location.th);
+//             var tracker = new CrabMotionFrameTracker
+//             {
+//                 CommandBackend =
+//                     CrabMotionFrameTracker
+//                         .ChassisCommandBackend
+//                         .SendMotion,
+//                 PathKind =
+//                     CrabMotionFrameTracker
+//                         .ReferencePathKind.LeftArc,
+//                 StartPosition = source,
+//                 InitialBodyYawRadians =
+//                     bodyYawRadians,
+//                 RadiusMillimeters =
+//                     RadiusMillimeters,
+//                 ArcSweepRadians = Math.PI / 2.0,
+//                 CruiseSpeed = CruiseSpeed
+//             };
+//             var destination =
+//                 tracker.GetArcDestination();
+
+//             _recorder = new TrackingExperimentRecorder(
+//                 controllerName:
+//                     "CrabSendMotionTracker",
+//                 trajectoryName:
+//                     $"CrabLeftArc90_R{RadiusMillimeters:0}mm",
+//                 trialNumber: TrialNumber,
+//                 referenceStart: source,
+//                 referenceEnd: destination,
+//                 referenceSpeed: CruiseSpeed,
+//                 referenceMotionFrameYawDegrees: 90f);
+//             tracker.CommandObserver =
+//                 (vx, vy, omega) =>
+//                     _recorder?.UpdateBodyCommand(
+//                         vx,
+//                         vy,
+//                         omega);
+//             _recorder.Start();
+
+//             try
+//             {
+//                 _task = new DriveTask(tracker.Get());
+//                 _task.Wait();
+//                 Thread.Sleep(300);
+//             }
+//             finally
+//             {
+//                 _task?.Stop();
+//                 _recorder?.UpdateBodyCommand(
+//                     0f,
+//                     0f,
+//                     0f);
+//                 _recorder?.StopAndSave();
+//                 _task = null;
+//                 _recorder = null;
+//             }
+//         }
+
+//         public override void TestStop()
+//         {
+//             _task?.Stop();
+//             _recorder?.UpdateBodyCommand(
+//                 0f,
+//                 0f,
+//                 0f);
+//             _recorder?.StopAndSave();
+//         }
+//     }
+
+//     [MovementTest(name = "SendMotion：4m S型曲线")]
+//     public class TestSCurve4m : MovementTest
+//     {
+//         public float LengthMillimeters = 4000f; // S型曲线纵向长度，单位mm。
+//         public float LateralOffsetMillimeters = 400f; // S型曲线左右两侧的最大偏移，单位mm。
+//         public float CruiseSpeed = 0.3f; // 首次实车测试建议使用0.3m/s。
+//         public int TrialNumber = 1; // 重复实验编号。
+
+//         private DriveTask _task;
+//         private TrackingExperimentRecorder _recorder;
+
+//         // 从当前Detour位姿开始，沿车头方向跟踪先左偏、再右偏并最终回中的完整S型曲线。
+//         public override void Test()
+//         {
+//             if (float.IsNaN(LengthMillimeters) ||
+//                 float.IsInfinity(LengthMillimeters) ||
+//                 LengthMillimeters <= 0f ||
+//                 float.IsNaN(LateralOffsetMillimeters) ||
+//                 float.IsInfinity(LateralOffsetMillimeters) ||
+//                 LateralOffsetMillimeters <= 0f ||
+//                 float.IsNaN(CruiseSpeed) ||
+//                 float.IsInfinity(CruiseSpeed) ||
+//                 CruiseSpeed <= 0f)
+//             {
+//                 Console.WriteLine("S型曲线测试参数无效。");
+//                 return;
+//             }
+
+//             if (!MovementTestPreparation.AreWheelsForward())
+//                 return;
+
+//             var location = DetourInterface.getCartLocation();
+//             if (double.IsNaN(location.x) ||
+//                 double.IsInfinity(location.x) ||
+//                 double.IsNaN(location.y) ||
+//                 double.IsInfinity(location.y) ||
+//                 double.IsNaN(location.th) ||
+//                 double.IsInfinity(location.th))
+//             {
+//                 Console.WriteLine(
+//                     "Detour当前位姿无效，取消4m S型曲线测试。");
+//                 return;
+//             }
+
+//             var source =
+//                 new Vector2((float)location.x, (float)location.y);
+//             var headingRadians =
+//                 AngleMath.DegreesToRadians(location.th);
+//             var length = LengthMillimeters;
+//             var offset = LateralOffsetMillimeters;
+
+//             // 三段三次贝塞尔依次经过左侧峰值、中心线和右侧峰值，
+//             // 起点、两个峰值和终点的切线均沿初始前向，连接处没有折角。
+//             var firstControlPoints = new List<Vector2>
+//             {
+//                 LocalToWorld(source, headingRadians, 0f, 0f),
+//                 LocalToWorld(
+//                     source, headingRadians,
+//                     length / 12f, 0f),
+//                 LocalToWorld(
+//                     source, headingRadians,
+//                     length / 6f, offset),
+//                 LocalToWorld(
+//                     source, headingRadians,
+//                     length * 0.25f, offset)
+//             };
+//             var secondControlPoints = new List<Vector2>
+//             {
+//                 LocalToWorld(
+//                     source, headingRadians,
+//                     length * 0.25f, offset),
+//                 LocalToWorld(
+//                     source, headingRadians,
+//                     length / 3f, offset),
+//                 LocalToWorld(
+//                     source, headingRadians,
+//                     length * 2f / 3f, -offset),
+//                 LocalToWorld(
+//                     source, headingRadians,
+//                     length * 0.75f, -offset)
+//             };
+//             var thirdControlPoints = new List<Vector2>
+//             {
+//                 LocalToWorld(
+//                     source, headingRadians,
+//                     length * 0.75f, -offset),
+//                 LocalToWorld(
+//                     source, headingRadians,
+//                     length * 5f / 6f, -offset),
+//                 LocalToWorld(
+//                     source, headingRadians,
+//                     length * 11f / 12f, 0f),
+//                 LocalToWorld(
+//                     source, headingRadians,
+//                     length, 0f)
+//             };
+
+//             var firstTrack = new BezierTrack(firstControlPoints)
+//             {
+//                 Speed = CruiseSpeed,
+//                 CarDirectionBias = 0f
+//             };
+//             var secondTrack = new BezierTrack(secondControlPoints)
+//             {
+//                 Speed = CruiseSpeed,
+//                 CarDirectionBias = 0f
+//             };
+//             var thirdTrack = new BezierTrack(thirdControlPoints)
+//             {
+//                 Speed = CruiseSpeed,
+//                 CarDirectionBias = 0f
+//             };
+
+//             var controller = new ChassisController
+//             {
+//                 BaseSpeed = CruiseSpeed
+//             }.Get();
+//             controller.FinishSpeed = 0f;
+
+//             if (!controller.AddTrack(
+//                     firstTrack,
+//                     "SCurve4m-Part1") ||
+//                 !controller.AddTrack(
+//                     secondTrack,
+//                     "SCurve4m-Part2") ||
+//                 !controller.AddTrack(
+//                     thirdTrack,
+//                     "SCurve4m-Part3"))
+//             {
+//                 Console.WriteLine(
+//                     "4m S型曲线轨迹添加失败，取消测试。");
+//                 return;
+//             }
+
+//             var destination =
+//                 LocalToWorld(
+//                     source,
+//                     headingRadians,
+//                     length,
+//                     0f);
+//             _recorder = new TrackingExperimentRecorder(
+//                 controllerName: "LegacyGeometricController",
+//                 trajectoryName:
+//                     $"LegacySCurve4m_A{LateralOffsetMillimeters:0}mm",
+//                 trialNumber: TrialNumber,
+//                 referenceStart: source,
+//                 referenceEnd: destination,
+//                 referenceSpeed: CruiseSpeed);
+//             _recorder.Start();
+
+//             try
+//             {
+//                 _task = new DriveTask(controller.Track());
+//                 _task.Wait();
+
+//                 // 保留少量停止后的数据，用于观察速度是否回到零。
+//                 Thread.Sleep(300);
+//             }
+//             finally
+//             {
+//                 _task?.Stop();
+//                 _recorder?.UpdateCommand(0f, 0f);
+//                 _recorder?.StopAndSave();
+//                 _task = null;
+//                 _recorder = null;
+//             }
+//         }
+
+//         // 停止S型曲线测试并保存当前已经采集的数据。
+//         public override void TestStop()
+//         {
+//             _task?.Stop();
+//             _recorder?.UpdateCommand(0f, 0f);
+//             _recorder?.StopAndSave();
+//         }
+
+//         // 将车体起点局部坐标转换为Detour世界坐标，X向前、Y向左。
+//         private static Vector2 LocalToWorld(
+//             Vector2 origin,
+//             double headingRadians,
+//             float localX,
+//             float localY)
+//         {
+//             var cos = (float)Math.Cos(headingRadians);
+//             var sin = (float)Math.Sin(headingRadians);
+
+//             return new Vector2(
+//                 origin.X + localX * cos - localY * sin,
+//                 origin.Y + localX * sin + localY * cos);
+//         }
+//     }
+// }

MultiWheelC/PilotConfig.cs

@@ -4,7 +4,10 @@ using Newtonsoft.Json;
 
 namespace MultiWheelC;
 
-public class PilotConfig : MultiWheelPilotConfig
+/// <summary>
+/// 定义由MDCS显示、持久化并随车辆部署的运行参数。
+/// </summary>
+public partial class PilotConfig : MultiWheelPilotConfig
 {
     #region 单车-轨迹跟踪 LineTracking
 
@@ -18,52 +21,6 @@ public class PilotConfig : MultiWheelPilotConfig
     [FieldMember(desc = "终点跟踪:速度")] public float DstTrackerMaxSpeed = 0.3f;
     #endregion
 
-    #region 单车-原地旋转  暂时没用上
-    [FieldMember(desc = "原地旋转：目标朝向(世界坐标系, deg)")]
-    public float InPlaceRotateTargetWorldDeg = 90f;
-
-    [FieldMember(desc = "原地旋转：旋转角速度(deg/s)")]
-    public float InPlaceRotateSpeed = 30f;
-
-    [FieldMember(desc = "原地旋转：到位角度精度(deg)")]
-    public float InPlaceRotateArriveDeg = 1f;
-
-    [FieldMember(desc = "原地旋转：起转前舵轮对齐精度(deg)")]
-    public float InPlaceRotateWheelAlignDeg = 2f;
-
-    [FieldMember(desc = "原地旋转：旋转过程中舵轮偏差重对齐阈值(deg)")]
-    public float InPlaceRotateActiveWheelAlignDeg = 10f;
-
-    #endregion
-
-    #region 单车-临时
-    [FieldMember(desc = "原地旋转Kp")]
-    public float InPlaceRotateKp = 0.2f;
-    // public float InPlaceRotateKp = 0.2f;
-
-    [FieldMember(desc = "原地旋转Ki")]
-    public float InPlaceRotateKi = 0.01f;
-    // public float InPlaceRotateKi = 0.01f;
-
-    [FieldMember(desc = "原地旋转Kd")]
-    public float InPlaceRotateKd = 0f;
-
-    [FieldMember(desc = "原地旋转积分限幅")]
-    public float InPlaceRotateMaxI = 0.01f;
-
-    [FieldMember(desc = "原地旋转最大角速度(deg/s)")]
-    public float InPlaceRotateMaxSpeed = 30f;
-
-    [FieldMember(desc = "原地旋转角加速度(deg/s²)")]
-    public float InPlaceRotateAcc = 30f;
-
-    [FieldMember(desc = "原地旋转超时(s)")]
-    public float InPlaceRotateTimeoutSec = 15f;
-    #endregion
-
-
-
-
     #region 单车-钻车与夹抱
     [FieldMember(desc = "2腿检测：雷达名（逗号分隔可多个）")]
     public string TwoLegLidarName = "rear_left_lidar_1,rear_right_lidar_1";

MultiWheelC/PilotDefinition.cs

@@ -33,10 +33,10 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
     [AsLowerIO(desc = "右后左轮实际位置")] public float RRLActualPos;
     [AsLowerIO(desc = "右后右轮实际位置")] public float RRRActualPos;
 
-    [AsLowerIO(desc = "左夹臂低限位")] public float LeftArmLowerPos;
-    [AsLowerIO(desc = "左夹臂高限位")] public float LeftArmUpperPos;
-    [AsLowerIO(desc = "右夹臂低限位")] public float RightArmLowerPos;
-    [AsLowerIO(desc = "右夹臂高限位")] public float RightArmUpperPos;
+    [AsLowerIO(desc = "左夹臂低限位")] public int LeftArmLowerPos;
+    [AsLowerIO(desc = "左夹臂高限位")] public int LeftArmUpperPos;
+    [AsLowerIO(desc = "右夹臂低限位")] public int RightArmLowerPos;
+    [AsLowerIO(desc = "右夹臂高限位")] public int RightArmUpperPos;
 
     [AsUpperIO(desc = "从C往驱动器下使能")] public bool DisableFromC = false;
     [AsUpperIO(desc = "从C上复位")] public bool ResetFromC = false;

MultiWheelC/StateEstimation/FirstOrderLowPassFilter.cs

@@ -0,0 +1,81 @@
+using MyParking.Shared;
+
+namespace MultiWheelC.StateEstimation
+{
+    /// <summary>
+    /// 使用真实采样时间间隔对单个连续量执行在线一阶低通滤波。
+    /// </summary>
+    public sealed class FirstOrderLowPassFilter
+    {
+        private readonly double _timeConstantSeconds;
+        private bool _isInitialized;
+        private double _value;
+
+        /// <summary>
+        /// 创建使用指定时间常数的一阶低通滤波器。
+        /// </summary>
+        public FirstOrderLowPassFilter(
+            double timeConstantSeconds)
+        {
+            NumericGuard.EnsureFinitePositive(
+                timeConstantSeconds,
+                nameof(timeConstantSeconds));
+
+            _timeConstantSeconds =
+                timeConstantSeconds;
+        }
+
+        /// <summary>
+        /// 使用当前输入和真实采样间隔更新滤波结果。
+        /// </summary>
+        public double Update(
+            double input,
+            double deltaTimeSeconds)
+        {
+            NumericGuard.EnsureFinite(
+                input,
+                nameof(input));
+            NumericGuard.EnsureFinitePositive(
+                deltaTimeSeconds,
+                nameof(deltaTimeSeconds));
+
+            if (!_isInitialized)
+            {
+                _value = input;
+                _isInitialized = true;
+                return _value;
+            }
+
+            var alpha =
+                deltaTimeSeconds /
+                (_timeConstantSeconds +
+                 deltaTimeSeconds);
+
+            _value += alpha * (input - _value);
+            return _value;
+        }
+
+        /// <summary>
+        /// 清除历史输出，使下一次有效输入直接成为新的初值。
+        /// </summary>
+        public void Reset()
+        {
+            _value = 0.0;
+            _isInitialized = false;
+        }
+
+        /// <summary>
+        /// 将滤波器立即重置到指定的有限初值。
+        /// </summary>
+        public void Reset(double initialValue)
+        {
+            NumericGuard.EnsureFinite(
+                initialValue,
+                nameof(initialValue));
+
+            _value = initialValue;
+            _isInitialized = true;
+        }
+
+    }
+}

MultiWheelC/StateEstimation/IVehicleStateProvider.cs

@@ -0,0 +1,14 @@
+namespace MultiWheelC.StateEstimation
+{
+    /// <summary>
+    /// 为轨迹控制器提供与具体定位来源无关的统一车辆状态读取接口。
+    /// </summary>
+    public interface IVehicleStateProvider
+    {
+        /// <summary>
+        /// 尝试读取当前有效车辆状态；定位不可用或过期时返回false。
+        /// </summary>
+        bool TryGetState(out VehicleState state);
+    }
+}
+

MultiWheelC/StateEstimation/ParkingVehicleStateProviderFactory.cs

@@ -0,0 +1,78 @@
+using System;
+using CommonUsage.Chassis;
+using MyParking.Shared;
+
+namespace MultiWheelC.StateEstimation
+{
+    /// <summary>
+    /// 根据车载配置创建Detour位姿过滤与电机反馈平面速度组合的停车状态源。
+    /// </summary>
+    public static class ParkingVehicleStateProviderFactory
+    {
+        /// <summary>
+        /// 使用当前车辆运行配置创建停车轨迹控制的默认状态源。
+        /// </summary>
+        public static WheelFeedbackVehicleStateProvider Create(
+            MultiWheelChassis chassis)
+        {
+            return Create(
+                chassis,
+                PilotDefinition.Conf);
+        }
+
+        /// <summary>
+        /// 使用指定配置快照创建停车轨迹控制的默认状态源。
+        /// </summary>
+        public static WheelFeedbackVehicleStateProvider Create(
+            MultiWheelChassis chassis,
+            PilotConfig config)
+        {
+            if (chassis == null)
+            {
+                throw new ArgumentNullException(nameof(chassis));
+            }
+
+            if (config == null)
+            {
+                throw new ArgumentNullException(nameof(config));
+            }
+
+            var velocityEstimator =
+                new VelocityEstimator2D(
+                    config.ParkingDetourLinearVelocityFilterSeconds,
+                    config.ParkingDetourAngularVelocityFilterSeconds);
+            var detourStateProvider =
+                new DetourVehicleStateProvider(
+                    velocityEstimator,
+                    config.ParkingDetourMaximumLinearSpeed,
+                    AngleMath.DegreesToRadians(
+                        config
+                            .ParkingDetourMaximumAngularSpeedDegrees),
+                    config.ParkingDetourPositionJumpMargin,
+                    AngleMath.DegreesToRadians(
+                        config.ParkingDetourHeadingJumpMarginDegrees),
+                    config.ParkingDetourVelocityPositionResidual,
+                    AngleMath.DegreesToRadians(
+                        config
+                            .ParkingDetourVelocityHeadingResidualDegrees),
+                    config.ParkingDetourStationaryConfirmationSeconds,
+                    config.ParkingDetourHeadingOutlierConfirmationFrames,
+                    config
+                        .ParkingDetourHeadingOutlierPredictionTimeoutSeconds,
+                    config.ParkingDetourJumpConfirmationFrames,
+                    config.ParkingDetourJumpConfirmationTimeoutSeconds,
+                    config.ParkingDetourMaximumAutomaticFrameShift,
+                    AngleMath.DegreesToRadians(
+                        config
+                            .ParkingDetourMaximumAutomaticHeadingShiftDegrees),
+                    config.ParkingDetourMaximumCachedFrameAgeSeconds,
+                    config
+                        .ParkingDetourLocalizationQualityConfirmationFrames);
+
+            return new WheelFeedbackVehicleStateProvider(
+                detourStateProvider,
+                chassis,
+                config.ParkingWheelVelocityFilterSeconds);
+        }
+    }
+}

MultiWheelC/StateEstimation/VehicleState.cs

@@ -0,0 +1,81 @@
+using MyParking.Shared;
+
+namespace MultiWheelC.StateEstimation
+{
+    /// <summary>
+    /// 保存一次经过校验的车辆位姿和速度估计快照，统一使用SI单位。
+    /// </summary>
+    public readonly struct VehicleState
+    {
+        /// <summary>
+        /// 创建车辆状态，并将世界坐标速度同步转换到车体坐标系。
+        /// </summary>
+        public VehicleState(
+            double sampleTimestampSeconds,
+            Pose2D poseInWorld,
+            Twist2D twistInWorld,
+            bool hasValidVelocityEstimate)
+        {
+            NumericGuard.EnsureFiniteNonNegative(
+                sampleTimestampSeconds,
+                nameof(sampleTimestampSeconds));
+            NumericGuard.EnsureFinite(
+                poseInWorld,
+                nameof(poseInWorld));
+            NumericGuard.EnsureFinite(
+                twistInWorld,
+                nameof(twistInWorld));
+
+            SampleTimestampSeconds =
+                sampleTimestampSeconds;
+            PoseInWorld = new Pose2D(
+                poseInWorld.XMeters,
+                poseInWorld.YMeters,
+                AngleMath.NormalizeRadians(
+                    poseInWorld.YawRadians));
+            HasValidVelocityEstimate =
+                hasValidVelocityEstimate;
+
+            // 第一帧或定位重置后的速度不可用于闭环控制，
+            // 此时显式置零，避免调用方误用残留速度。
+            TwistInWorld = hasValidVelocityEstimate
+                ? twistInWorld
+                : Twist2D.Zero;
+
+            var worldPoseInBody =
+                FrameTransform2D.Inverse(
+                    PoseInWorld);
+            TwistInBody =
+                FrameTransform2D.TransformTwistAtSamePoint(
+                    worldPoseInBody,
+                    TwistInWorld);
+        }
+
+        /// <summary>
+        /// 获取状态源单调时钟中的采样时刻，单位为s。
+        /// </summary>
+        public double SampleTimestampSeconds { get; }
+
+        /// <summary>
+        /// 获取车体中心在状态源输出世界坐标系中的位姿，单位为m和rad。
+        /// Detour发生经确认的小幅坐标跳变后，该坐标系会保持任务内连续。
+        /// </summary>
+        public Pose2D PoseInWorld { get; }
+
+        /// <summary>
+        /// 获取在世界坐标系中表达的车辆速度，单位为m/s和rad/s。
+        /// </summary>
+        public Twist2D TwistInWorld { get; }
+
+        /// <summary>
+        /// 获取在车体坐标系中表达的车辆速度，X向前、Y向左、逆时针为正。
+        /// </summary>
+        public Twist2D TwistInBody { get; }
+
+        /// <summary>
+        /// 获取当前速度估计是否已经初始化并可用于闭环控制。
+        /// </summary>
+        public bool HasValidVelocityEstimate { get; }
+
+    }
+}

MultiWheelC/StateEstimation/VelocityEstimator2D.cs

@@ -0,0 +1,182 @@
+using System;
+using MyParking.Shared;
+
+namespace MultiWheelC.StateEstimation
+{
+    /// <summary>
+    /// 根据连续有效的Detour世界位姿和真实时间差估算车辆二维速度。
+    /// </summary>
+    public sealed class VelocityEstimator2D
+    {
+        public const double DefaultLinearFilterTimeConstantSeconds =
+            0.15;
+        public const double DefaultAngularFilterTimeConstantSeconds =
+            0.20;
+
+        private readonly FirstOrderLowPassFilter
+            _worldVelocityXFilter;
+        private readonly FirstOrderLowPassFilter
+            _worldVelocityYFilter;
+        private readonly FirstOrderLowPassFilter
+            _angularVelocityFilter;
+
+        private bool _hasPreviousSample;
+        private Pose2D _previousPoseInWorld;
+        private double _previousTimestampSeconds;
+
+        /// <summary>
+        /// 创建使用默认0.15s线速度和0.20s角速度时间常数的估计器。
+        /// </summary>
+        public VelocityEstimator2D()
+            : this(
+                DefaultLinearFilterTimeConstantSeconds,
+                DefaultAngularFilterTimeConstantSeconds)
+        {
+        }
+
+        /// <summary>
+        /// 创建使用指定线速度和角速度滤波时间常数的估计器。
+        /// </summary>
+        public VelocityEstimator2D(
+            double linearFilterTimeConstantSeconds,
+            double angularFilterTimeConstantSeconds)
+        {
+            _worldVelocityXFilter =
+                new FirstOrderLowPassFilter(
+                    linearFilterTimeConstantSeconds);
+            _worldVelocityYFilter =
+                new FirstOrderLowPassFilter(
+                    linearFilterTimeConstantSeconds);
+            _angularVelocityFilter =
+                new FirstOrderLowPassFilter(
+                    angularFilterTimeConstantSeconds);
+        }
+
+        /// <summary>
+        /// 使用一个新的有效定位样本更新并返回车辆状态。
+        /// </summary>
+        public VehicleState Update(
+            Pose2D poseInWorld,
+            double sampleTimestampSeconds)
+        {
+            NumericGuard.EnsureFinite(
+                poseInWorld,
+                nameof(poseInWorld));
+            NumericGuard.EnsureFiniteNonNegative(
+                sampleTimestampSeconds,
+                nameof(sampleTimestampSeconds));
+
+            var normalizedPoseInWorld =
+                new Pose2D(
+                    poseInWorld.XMeters,
+                    poseInWorld.YMeters,
+                    AngleMath.NormalizeRadians(
+                        poseInWorld.YawRadians));
+
+            if (!_hasPreviousSample)
+            {
+                return Reset(
+                    normalizedPoseInWorld,
+                    sampleTimestampSeconds);
+            }
+
+            var deltaTimeSeconds =
+                sampleTimestampSeconds -
+                _previousTimestampSeconds;
+
+            if (deltaTimeSeconds <= 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(sampleTimestampSeconds),
+                    "新定位样本的单调时间戳必须严格大于上一帧。");
+            }
+
+            var rawVelocityXInWorld =
+                (normalizedPoseInWorld.XMeters -
+                 _previousPoseInWorld.XMeters) /
+                deltaTimeSeconds;
+            var rawVelocityYInWorld =
+                (normalizedPoseInWorld.YMeters -
+                 _previousPoseInWorld.YMeters) /
+                deltaTimeSeconds;
+            var rawAngularVelocity =
+                AngleMath.ShortestDifferenceRadians(
+                    normalizedPoseInWorld.YawRadians,
+                    _previousPoseInWorld.YawRadians) /
+                deltaTimeSeconds;
+
+            var filteredTwistInWorld =
+                new Twist2D(
+                    _worldVelocityXFilter.Update(
+                        rawVelocityXInWorld,
+                        deltaTimeSeconds),
+                    _worldVelocityYFilter.Update(
+                        rawVelocityYInWorld,
+                        deltaTimeSeconds),
+                    _angularVelocityFilter.Update(
+                        rawAngularVelocity,
+                        deltaTimeSeconds));
+
+            _previousPoseInWorld =
+                normalizedPoseInWorld;
+            _previousTimestampSeconds =
+                sampleTimestampSeconds;
+
+            return new VehicleState(
+                sampleTimestampSeconds,
+                normalizedPoseInWorld,
+                filteredTwistInWorld,
+                true);
+        }
+
+        /// <summary>
+        /// 使用当前定位重新建立差分基准，并返回速度无效的零速状态。
+        /// </summary>
+        public VehicleState Reset(
+            Pose2D poseInWorld,
+            double sampleTimestampSeconds)
+        {
+            NumericGuard.EnsureFinite(
+                poseInWorld,
+                nameof(poseInWorld));
+            NumericGuard.EnsureFiniteNonNegative(
+                sampleTimestampSeconds,
+                nameof(sampleTimestampSeconds));
+
+            _previousPoseInWorld =
+                new Pose2D(
+                    poseInWorld.XMeters,
+                    poseInWorld.YMeters,
+                    AngleMath.NormalizeRadians(
+                        poseInWorld.YawRadians));
+            _previousTimestampSeconds =
+                sampleTimestampSeconds;
+            _hasPreviousSample = true;
+
+            _worldVelocityXFilter.Reset();
+            _worldVelocityYFilter.Reset();
+            _angularVelocityFilter.Reset();
+
+            return new VehicleState(
+                sampleTimestampSeconds,
+                _previousPoseInWorld,
+                Twist2D.Zero,
+                false);
+        }
+
+        /// <summary>
+        /// 清除差分基准和全部滤波历史，使下一帧重新初始化估计器。
+        /// </summary>
+        public void Reset()
+        {
+            _hasPreviousSample = false;
+            _previousPoseInWorld = Pose2D.Identity;
+            _previousTimestampSeconds = 0.0;
+
+            _worldVelocityXFilter.Reset();
+            _worldVelocityYFilter.Reset();
+            _angularVelocityFilter.Reset();
+        }
+
+    }
+}

MultiWheelC/StateEstimation/WheelFeedbackVehicleStateProvider.cs

@@ -0,0 +1,668 @@
+using System;
+using CommonUsage.Chassis;
+using MyParking.Shared;
+using System.Diagnostics;
+
+namespace MultiWheelC.StateEstimation
+{
+    /// <summary>
+    /// 保留外部状态源的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 readonly FirstOrderLowPassFilter _lateralSpeedFilter;
+        private readonly FirstOrderLowPassFilter _angularSpeedFilter;
+
+        private bool _hasPreviousTimestamp;
+        private double _previousTimestampSeconds;
+        private bool _hasVelocityDiagnostics;
+        private double _latestDetourBodyVxMetersPerSecond;
+        private bool _latestDetourVelocityValid;
+        private double _latestRawWheelBodyVxMetersPerSecond;
+        private double _latestFilteredWheelBodyVxMetersPerSecond;
+        private double _latestRawWheelBodyVyMetersPerSecond;
+        private double _latestFilteredWheelBodyVyMetersPerSecond;
+        private double _latestRawWheelBodyOmegaRadiansPerSecond;
+        private double _latestFilteredWheelBodyOmegaRadiansPerSecond;
+        private double _latestWheelSampleTimestampSeconds;
+        private bool _latestWheelVelocityValid;
+        private bool _latestWheelFeedbackReadSucceeded;
+
+        /// <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);
+            _lateralSpeedFilter =
+                new FirstOrderLowPassFilter(
+                    velocityFilterTimeConstantSeconds);
+            _angularSpeedFilter =
+                new FirstOrderLowPassFilter(
+                    velocityFilterTimeConstantSeconds);
+        }
+
+        /// <summary>
+        /// 获取最近一次读取失败的原因，正常时为空字符串。
+        /// </summary>
+        public string LastFailureReason { get; private set; } =
+            string.Empty;
+
+        /// <summary>
+        /// 获取最近一次航向读取失败的原因；位置单独异常时保持为空。
+        /// </summary>
+        public string LastHeadingFailureReason { get; private set; } =
+            string.Empty;
+
+        /// <summary>
+        /// 读取Detour位姿和电机反馈速度，并组合成统一车辆状态。
+        /// </summary>
+        public bool TryGetState(out VehicleState state)
+        {
+            lock (_syncRoot)
+            {
+                try
+                {
+                    ReadFilteredWheelTwist(
+                        out var filteredWheelTwist,
+                        out _,
+                        out var hasValidWheelSpeedEstimate);
+
+                    // Detour位姿跳变确认期间需要用轮速维持短时运动预测。
+                    if (_poseProvider is DetourVehicleStateProvider
+                            detourStateProvider)
+                    {
+                        detourStateProvider.UpdateWheelVelocityEstimate(
+                            filteredWheelTwist.VxMetersPerSecond,
+                            filteredWheelTwist.VyMetersPerSecond,
+                            filteredWheelTwist.OmegaRadiansPerSecond,
+                            hasValidWheelSpeedEstimate);
+                    }
+
+                    if (!_poseProvider.TryGetState(
+                            out var poseState))
+                    {
+                        state = default;
+                        LastFailureReason =
+                            GetPoseProviderFailureReason();
+                        return false;
+                    }
+
+                    _latestDetourBodyVxMetersPerSecond =
+                        poseState.TwistInBody.VxMetersPerSecond;
+                    _latestDetourVelocityValid =
+                        poseState.HasValidVelocityEstimate;
+                    _hasVelocityDiagnostics = true;
+
+                    // 轮组反馈有效后统一使用滤波后的平面速度；初始化期间
+                    // 暂时保留Detour角速度作为回退值。
+                    var omegaRadiansPerSecond =
+                        hasValidWheelSpeedEstimate
+                            ? filteredWheelTwist
+                                .OmegaRadiansPerSecond
+                            : poseState.TwistInBody
+                                .OmegaRadiansPerSecond;
+                    var twistInBody = new Twist2D(
+                        filteredWheelTwist.VxMetersPerSecond,
+                        filteredWheelTwist.VyMetersPerSecond,
+                        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)
+                {
+                    _latestWheelFeedbackReadSucceeded = false;
+                    state = default;
+                    LastFailureReason =
+                        "舵轮电机反馈车体速度解算失败：" +
+                        exception.Message;
+                    return false;
+                }
+            }
+        }
+
+        /// <summary>
+        /// 读取并滤波轮组反馈速度，不访问Detour；首帧仅建立滤波时间基准并返回false。
+        /// </summary>
+        public bool TryGetWheelTwist(
+            out Twist2D twistInBody,
+            out double sampleTimestampSeconds)
+        {
+            lock (_syncRoot)
+            {
+                try
+                {
+                    ReadFilteredWheelTwist(
+                        out var filteredWheelTwist,
+                        out sampleTimestampSeconds,
+                        out var hasValidWheelSpeedEstimate);
+
+                    if (!hasValidWheelSpeedEstimate)
+                    {
+                        twistInBody = Twist2D.Zero;
+                        LastFailureReason =
+                            "轮组速度估计正在建立采样时间基准。";
+                        return false;
+                    }
+
+                    twistInBody = filteredWheelTwist;
+                    LastFailureReason = string.Empty;
+                    return true;
+                }
+                catch (Exception exception)
+                {
+                    _latestWheelFeedbackReadSucceeded = false;
+                    twistInBody = Twist2D.Zero;
+                    sampleTimestampSeconds = 0.0;
+                    LastFailureReason =
+                        "舵轮电机反馈车体速度解算失败：" +
+                        exception.Message;
+                    return false;
+                }
+            }
+        }
+
+        /// <summary>
+        /// 读取Detour独立校验后的航向，同时保持轮组速度预测输入更新。
+        /// </summary>
+        public bool TryGetHeadingRadians(
+            out double headingRadians)
+        {
+            lock (_syncRoot)
+            {
+                TryGetState(out _);
+
+                if (!_latestWheelFeedbackReadSucceeded)
+                {
+                    headingRadians = 0.0;
+                    LastHeadingFailureReason =
+                        string.IsNullOrWhiteSpace(
+                            LastFailureReason)
+                            ? "舵轮反馈当前不可用，无法校验航向。"
+                            : LastFailureReason;
+                    return false;
+                }
+
+                if (_poseProvider is DetourVehicleStateProvider
+                        detourStateProvider)
+                {
+                    var success = detourStateProvider
+                        .TryGetLatestReliableHeadingRadians(
+                            out headingRadians);
+                    LastHeadingFailureReason = success
+                        ? string.Empty
+                        : detourStateProvider
+                            .LastHeadingFailureReason;
+                    return success;
+                }
+
+                if (_poseProvider.TryGetState(
+                        out var poseState))
+                {
+                    headingRadians =
+                        poseState.PoseInWorld.YawRadians;
+                    LastHeadingFailureReason = string.Empty;
+                    return true;
+                }
+
+                headingRadians = 0.0;
+                LastHeadingFailureReason =
+                    GetPoseProviderFailureReason();
+                return false;
+            }
+        }
+
+        /// <summary>
+        /// 原地自转停车后，允许基础Detour状态源重新确认有限位置偏移。
+        /// </summary>
+        public void BeginPostRotationPositionRecovery()
+        {
+            lock (_syncRoot)
+            {
+                if (_poseProvider is DetourVehicleStateProvider
+                        detourStateProvider)
+                {
+                    detourStateProvider
+                        .BeginPostRotationPositionRecovery();
+                }
+            }
+        }
+
+        private string GetPoseProviderFailureReason()
+        {
+            if (_poseProvider is DetourVehicleStateProvider
+                    detourStateProvider &&
+                !string.IsNullOrWhiteSpace(
+                    detourStateProvider.LastFailureReason))
+            {
+                return detourStateProvider.LastFailureReason;
+            }
+
+            return "基础位姿状态源暂时不可用。";
+        }
+
+        /// <summary>
+        /// 读取最近一帧Detour纵向速度和轮速解算平面速度，供实验记录使用。
+        /// </summary>
+        public bool TryGetLatestVelocityDiagnostics(
+            out double detourBodyVxMetersPerSecond,
+            out bool detourVelocityValid,
+            out double rawWheelBodyVxMetersPerSecond,
+            out double filteredWheelBodyVxMetersPerSecond,
+            out double rawWheelBodyVyMetersPerSecond,
+            out double filteredWheelBodyVyMetersPerSecond,
+            out bool wheelVelocityValid)
+        {
+            lock (_syncRoot)
+            {
+                detourBodyVxMetersPerSecond =
+                    _latestDetourBodyVxMetersPerSecond;
+                detourVelocityValid =
+                    _latestDetourVelocityValid;
+                rawWheelBodyVxMetersPerSecond =
+                    _latestRawWheelBodyVxMetersPerSecond;
+                filteredWheelBodyVxMetersPerSecond =
+                    _latestFilteredWheelBodyVxMetersPerSecond;
+                rawWheelBodyVyMetersPerSecond =
+                    _latestRawWheelBodyVyMetersPerSecond;
+                filteredWheelBodyVyMetersPerSecond =
+                    _latestFilteredWheelBodyVyMetersPerSecond;
+                wheelVelocityValid =
+                    _latestWheelVelocityValid;
+                return _hasVelocityDiagnostics;
+            }
+        }
+
+        /// <summary>
+        /// 读取最近一帧Detour纵向速度及轮组原始/滤波Vx、Vy、Vw和采样时间。
+        /// </summary>
+        public bool TryGetLatestVelocityDiagnostics(
+            out double detourBodyVxMetersPerSecond,
+            out bool detourVelocityValid,
+            out double rawWheelBodyVxMetersPerSecond,
+            out double filteredWheelBodyVxMetersPerSecond,
+            out double rawWheelBodyVyMetersPerSecond,
+            out double filteredWheelBodyVyMetersPerSecond,
+            out double rawWheelBodyOmegaRadiansPerSecond,
+            out double filteredWheelBodyOmegaRadiansPerSecond,
+            out double wheelSampleTimestampSeconds,
+            out bool wheelVelocityValid)
+        {
+            lock (_syncRoot)
+            {
+                detourBodyVxMetersPerSecond =
+                    _latestDetourBodyVxMetersPerSecond;
+                detourVelocityValid =
+                    _latestDetourVelocityValid;
+                rawWheelBodyVxMetersPerSecond =
+                    _latestRawWheelBodyVxMetersPerSecond;
+                filteredWheelBodyVxMetersPerSecond =
+                    _latestFilteredWheelBodyVxMetersPerSecond;
+                rawWheelBodyVyMetersPerSecond =
+                    _latestRawWheelBodyVyMetersPerSecond;
+                filteredWheelBodyVyMetersPerSecond =
+                    _latestFilteredWheelBodyVyMetersPerSecond;
+                rawWheelBodyOmegaRadiansPerSecond =
+                    _latestRawWheelBodyOmegaRadiansPerSecond;
+                filteredWheelBodyOmegaRadiansPerSecond =
+                    _latestFilteredWheelBodyOmegaRadiansPerSecond;
+                wheelSampleTimestampSeconds =
+                    _latestWheelSampleTimestampSeconds;
+                wheelVelocityValid =
+                    _latestWheelVelocityValid;
+                return _hasVelocityDiagnostics;
+            }
+        }
+
+        /// <summary>
+        /// 读取Detour跳变候选、自动坐标连续化和数据新鲜度诊断。
+        /// </summary>
+        public bool TryGetLatestDetourDiagnostics(
+            out bool jumpCandidateActive,
+            out int jumpCandidateConsistentFrameCount,
+            out double estimatedShiftDistanceMeters,
+            out double estimatedShiftHeadingRadians,
+            out int automaticFrameShiftCount,
+            out string stateStatusReason,
+            out double detourDataAgeMilliseconds)
+        {
+            lock (_syncRoot)
+            {
+                if (_poseProvider is DetourVehicleStateProvider
+                        detourStateProvider)
+                {
+                    var hasDiagnostics = detourStateProvider
+                        .TryGetLatestDiagnostics(
+                            out jumpCandidateActive,
+                            out jumpCandidateConsistentFrameCount,
+                            out estimatedShiftDistanceMeters,
+                            out estimatedShiftHeadingRadians,
+                            out automaticFrameShiftCount,
+                            out stateStatusReason,
+                            out detourDataAgeMilliseconds);
+
+                    if (string.IsNullOrWhiteSpace(
+                            stateStatusReason) &&
+                        !string.IsNullOrWhiteSpace(
+                            LastFailureReason))
+                    {
+                        stateStatusReason = LastFailureReason;
+                    }
+
+                    return hasDiagnostics;
+                }
+
+                jumpCandidateActive = false;
+                jumpCandidateConsistentFrameCount = 0;
+                estimatedShiftDistanceMeters = 0.0;
+                estimatedShiftHeadingRadians = 0.0;
+                automaticFrameShiftCount = 0;
+                stateStatusReason = LastFailureReason;
+                detourDataAgeMilliseconds = 0.0;
+                return false;
+            }
+        }
+
+        /// <summary>
+        /// 读取Detour源帧、轮速预测、创新门限、候选原因和状态诊断。
+        /// </summary>
+        public bool TryGetLatestDetourDiagnostics(
+            out bool jumpCandidateActive,
+            out int jumpCandidateConsistentFrameCount,
+            out double estimatedShiftDistanceMeters,
+            out double estimatedShiftHeadingRadians,
+            out int automaticFrameShiftCount,
+            out double sourceFrameIntervalSeconds,
+            out double motionPredictionTimestampSeconds,
+            out bool hasInnovationDiagnostics,
+            out double positionInnovationMeters,
+            out double allowedPositionInnovationMeters,
+            out double headingInnovationRadians,
+            out double allowedHeadingInnovationRadians,
+            out string jumpCandidateTriggerReason,
+            out string stateStatus,
+            out string stateStatusReason,
+            out double detourDataAgeMilliseconds)
+        {
+            lock (_syncRoot)
+            {
+                if (_poseProvider is DetourVehicleStateProvider
+                        detourStateProvider)
+                {
+                    var hasDiagnostics = detourStateProvider
+                        .TryGetLatestDiagnostics(
+                            out jumpCandidateActive,
+                            out jumpCandidateConsistentFrameCount,
+                            out estimatedShiftDistanceMeters,
+                            out estimatedShiftHeadingRadians,
+                            out automaticFrameShiftCount,
+                            out sourceFrameIntervalSeconds,
+                            out motionPredictionTimestampSeconds,
+                            out hasInnovationDiagnostics,
+                            out positionInnovationMeters,
+                            out allowedPositionInnovationMeters,
+                            out headingInnovationRadians,
+                            out allowedHeadingInnovationRadians,
+                            out jumpCandidateTriggerReason,
+                            out stateStatus,
+                            out stateStatusReason,
+                            out detourDataAgeMilliseconds);
+
+                    if (string.IsNullOrWhiteSpace(
+                            stateStatusReason) &&
+                        !string.IsNullOrWhiteSpace(
+                            LastFailureReason))
+                    {
+                        stateStatus = "Unavailable";
+                        stateStatusReason = LastFailureReason;
+                    }
+
+                    return hasDiagnostics;
+                }
+
+                jumpCandidateActive = false;
+                jumpCandidateConsistentFrameCount = 0;
+                estimatedShiftDistanceMeters = 0.0;
+                estimatedShiftHeadingRadians = 0.0;
+                automaticFrameShiftCount = 0;
+                sourceFrameIntervalSeconds = 0.0;
+                motionPredictionTimestampSeconds = 0.0;
+                hasInnovationDiagnostics = false;
+                positionInnovationMeters = 0.0;
+                allowedPositionInnovationMeters = 0.0;
+                headingInnovationRadians = 0.0;
+                allowedHeadingInnovationRadians = 0.0;
+                jumpCandidateTriggerReason = string.Empty;
+                stateStatus = "Unavailable";
+                stateStatusReason = LastFailureReason;
+                detourDataAgeMilliseconds = 0.0;
+                return false;
+            }
+        }
+
+        /// <summary>
+        /// 清除基础位姿状态、坐标连续化状态以及电机反馈速度滤波历史。
+        /// </summary>
+        public void Reset()
+        {
+            lock (_syncRoot)
+            {
+                if (_poseProvider is DetourVehicleStateProvider
+                        detourStateProvider)
+                {
+                    detourStateProvider.Reset();
+                }
+
+                _longitudinalSpeedFilter.Reset();
+                _lateralSpeedFilter.Reset();
+                _angularSpeedFilter.Reset();
+                _wheelSpeedClock.Restart();
+                _hasPreviousTimestamp = false;
+                _previousTimestampSeconds = 0.0;
+                _hasVelocityDiagnostics = false;
+                _latestDetourBodyVxMetersPerSecond = 0.0;
+                _latestDetourVelocityValid = false;
+                _latestRawWheelBodyVxMetersPerSecond = 0.0;
+                _latestFilteredWheelBodyVxMetersPerSecond = 0.0;
+                _latestRawWheelBodyVyMetersPerSecond = 0.0;
+                _latestFilteredWheelBodyVyMetersPerSecond = 0.0;
+                _latestRawWheelBodyOmegaRadiansPerSecond = 0.0;
+                _latestFilteredWheelBodyOmegaRadiansPerSecond = 0.0;
+                _latestWheelSampleTimestampSeconds = 0.0;
+                _latestWheelVelocityValid = false;
+                _latestWheelFeedbackReadSucceeded = false;
+                LastFailureReason = string.Empty;
+                LastHeadingFailureReason = string.Empty;
+            }
+        }
+
+        /// <summary>
+        /// 从底盘读取一次轮组速度，统一转换为SI单位并更新共用低通滤波状态。
+        /// </summary>
+        private void ReadFilteredWheelTwist(
+            out Twist2D filteredTwistInBody,
+            out double sampleTimestampSeconds,
+            out bool hasValidWheelSpeedEstimate)
+        {
+            var actualCarSpeed =
+                _chassis.GetCarSpeed(true);
+            var rawBodyVxMetersPerSecond =
+                (double)actualCarSpeed.Vx;
+            var rawBodyVyMetersPerSecond =
+                (double)actualCarSpeed.Vy;
+            // CommonUsage.CarSpeed.Vw在旧底盘边界使用deg/s；
+            // 状态估计内部统一转换为rad/s。
+            var rawBodyOmegaRadiansPerSecond =
+                AngleMath.DegreesToRadians(
+                    actualCarSpeed.Vw);
+
+            NumericGuard.EnsureFinite(
+                rawBodyVxMetersPerSecond,
+                "电机反馈车体纵向速度");
+            NumericGuard.EnsureFinite(
+                rawBodyVyMetersPerSecond,
+                "电机反馈车体横向速度");
+            NumericGuard.EnsureFinite(
+                rawBodyOmegaRadiansPerSecond,
+                "电机反馈车体角速度");
+
+            sampleTimestampSeconds =
+                _wheelSpeedClock.Elapsed.TotalSeconds;
+
+            UpdateBodyVelocityFilters(
+                rawBodyVxMetersPerSecond,
+                rawBodyVyMetersPerSecond,
+                rawBodyOmegaRadiansPerSecond,
+                sampleTimestampSeconds,
+                out var filteredBodyVxMetersPerSecond,
+                out var filteredBodyVyMetersPerSecond,
+                out var filteredBodyOmegaRadiansPerSecond,
+                out hasValidWheelSpeedEstimate);
+
+            filteredTwistInBody = new Twist2D(
+                filteredBodyVxMetersPerSecond,
+                filteredBodyVyMetersPerSecond,
+                filteredBodyOmegaRadiansPerSecond);
+
+            _latestRawWheelBodyVxMetersPerSecond =
+                rawBodyVxMetersPerSecond;
+            _latestFilteredWheelBodyVxMetersPerSecond =
+                filteredBodyVxMetersPerSecond;
+            _latestRawWheelBodyVyMetersPerSecond =
+                rawBodyVyMetersPerSecond;
+            _latestFilteredWheelBodyVyMetersPerSecond =
+                filteredBodyVyMetersPerSecond;
+            _latestRawWheelBodyOmegaRadiansPerSecond =
+                rawBodyOmegaRadiansPerSecond;
+            _latestFilteredWheelBodyOmegaRadiansPerSecond =
+                filteredBodyOmegaRadiansPerSecond;
+            _latestWheelSampleTimestampSeconds =
+                sampleTimestampSeconds;
+            _latestWheelVelocityValid =
+                hasValidWheelSpeedEstimate;
+            _latestWheelFeedbackReadSucceeded = true;
+        }
+
+        /// <summary>
+        /// 使用同一个真实采样间隔更新车体Vx、Vy和Omega低通滤波，并在首帧建立共同时间基准。
+        /// </summary>
+        private void UpdateBodyVelocityFilters(
+            double rawBodyVxMetersPerSecond,
+            double rawBodyVyMetersPerSecond,
+            double rawBodyOmegaRadiansPerSecond,
+            double timestampSeconds,
+            out double filteredBodyVxMetersPerSecond,
+            out double filteredBodyVyMetersPerSecond,
+            out double filteredBodyOmegaRadiansPerSecond,
+            out bool hasValidWheelSpeedEstimate)
+        {
+            NumericGuard.EnsureFiniteNonNegative(
+                timestampSeconds,
+                nameof(timestampSeconds));
+
+            if (!_hasPreviousTimestamp)
+            {
+                _longitudinalSpeedFilter.Reset(
+                    rawBodyVxMetersPerSecond);
+                _lateralSpeedFilter.Reset(
+                    rawBodyVyMetersPerSecond);
+                _angularSpeedFilter.Reset(
+                    rawBodyOmegaRadiansPerSecond);
+                _previousTimestampSeconds = timestampSeconds;
+                _hasPreviousTimestamp = true;
+                hasValidWheelSpeedEstimate = false;
+                filteredBodyVxMetersPerSecond =
+                    rawBodyVxMetersPerSecond;
+                filteredBodyVyMetersPerSecond =
+                    rawBodyVyMetersPerSecond;
+                filteredBodyOmegaRadiansPerSecond =
+                    rawBodyOmegaRadiansPerSecond;
+                return;
+            }
+
+            var deltaTimeSeconds =
+                timestampSeconds -
+                _previousTimestampSeconds;
+            _previousTimestampSeconds = timestampSeconds;
+
+            if (deltaTimeSeconds <= 0.0)
+            {
+                _longitudinalSpeedFilter.Reset(
+                    rawBodyVxMetersPerSecond);
+                _lateralSpeedFilter.Reset(
+                    rawBodyVyMetersPerSecond);
+                _angularSpeedFilter.Reset(
+                    rawBodyOmegaRadiansPerSecond);
+                hasValidWheelSpeedEstimate = false;
+                filteredBodyVxMetersPerSecond =
+                    rawBodyVxMetersPerSecond;
+                filteredBodyVyMetersPerSecond =
+                    rawBodyVyMetersPerSecond;
+                filteredBodyOmegaRadiansPerSecond =
+                    rawBodyOmegaRadiansPerSecond;
+                return;
+            }
+
+            hasValidWheelSpeedEstimate = true;
+            filteredBodyVxMetersPerSecond =
+                _longitudinalSpeedFilter.Update(
+                    rawBodyVxMetersPerSecond,
+                    deltaTimeSeconds);
+            filteredBodyVyMetersPerSecond =
+                _lateralSpeedFilter.Update(
+                    rawBodyVyMetersPerSecond,
+                    deltaTimeSeconds);
+            filteredBodyOmegaRadiansPerSecond =
+                _angularSpeedFilter.Update(
+                    rawBodyOmegaRadiansPerSecond,
+                    deltaTimeSeconds);
+        }
+
+    }
+}

MultiWheelC/TrackingExperimentRecorder.cs

@@ -1,415 +0,0 @@
-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;
-using MyParking.Shared;
-
-namespace MultiWheelC
-{
-    // C层实验数据：保存一个采样时刻的定位与控制命令。
-    public sealed class TrackingSample
-    {
-        public double ElapsedSeconds;
-
-        // Detour位置单位为mm，航向单位为deg。
-        public double DetourX;
-        public double DetourY;
-        public double DetourTheta;
-
-        // 车体速度单位为m/s，角速度统一使用rad/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 float _referenceAngularSpeed;
-        private readonly float _referenceMotionFrameYawDegrees;
-        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,
-            float referenceAngularSpeed = 0f,
-            int sampleIntervalMs = 50,
-            float referenceMotionFrameYawDegrees = 0f)
-        {
-            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;
-            _referenceAngularSpeed = referenceAngularSpeed;
-            _referenceMotionFrameYawDegrees =
-                referenceMotionFrameYawDegrees;
-            _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;
-                        // CommonUsage.GetCarSpeed().Vw的单位为deg/s，
-                        // 记录器内部统一转换为rad/s。
-                        commandAngularSpeed =
-                            (float)AngleMath.DegreesToRadians(
-                                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," +
-                    // 保留旧列（deg/s）供历史Python脚本兼容。
-                    "CommandAngularSpeed," +
-                    "CommandAngularSpeedRadPerSecond," +
-                    "CommandVx," +
-                    "CommandVy," +
-                    "ReferenceStartX," +
-                    "ReferenceStartY," +
-                    "ReferenceEndX," +
-                    "ReferenceEndY," +
-                    "ReferenceSpeed," +
-                    "ReferenceAngularSpeedRadPerSecond," +
-                    "ReferenceMotionFrameYawDegrees");
-
-                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(
-                            AngleMath.RadiansToDegrees(
-                                sample.CommandAngularSpeed)),
-                        Format(sample.CommandAngularSpeed),
-                        Format(sample.CommandVx),
-                        Format(sample.CommandVy),
-                        Format(_referenceStart.X),
-                        Format(_referenceStart.Y),
-                        Format(_referenceEnd.X),
-                        Format(_referenceEnd.Y),
-                        Format(_referenceSpeed),
-                        Format(_referenceAngularSpeed),
-                        Format(_referenceMotionFrameYawDegrees)));
-                }
-            }
-        }
-
-        // 将文件名中的非法字符替换为下划线。
-        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("\"", "\"\"") +
-                "\"";
-        }
-    }
-}

MultiWheelC/Trajectory/LegacyTrackAdapter.cs

@@ -0,0 +1 @@
+// 兼容现有 MDCS 的 AbstractTrack

MultiWheelC/Trajectory/Trajectory2D.cs

@@ -0,0 +1,331 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using System.Linq;
+using MyParking.Shared;
+
+namespace MultiWheelC.Trajectory
+{
+    /// <summary>
+    /// 保存一条按预定执行点序排列、以累计弧长参数化的只读二维参考轨迹。
+    /// </summary>
+    public sealed class Trajectory2D
+    {
+        private const double StartArcLengthToleranceMeters = 1e-9;
+        private const double MinimumSegmentLengthMeters = 1e-6;
+        private const double ArcLengthConsistencyAbsoluteToleranceMeters =
+            1e-6;
+        private const double ArcLengthConsistencyRelativeTolerance =
+            0.01;
+
+        private readonly TrajectoryPoint[] _points;
+        private readonly ReadOnlyCollection<TrajectoryPoint> _readOnlyPoints;
+
+        /// <summary>
+        /// 复制并验证按实际执行顺序及累计弧长升序排列的参考轨迹点。
+        /// </summary>
+        public Trajectory2D(
+            IEnumerable<TrajectoryPoint> points)
+        {
+            if (points == null)
+            {
+                throw new ArgumentNullException(
+                    nameof(points));
+            }
+
+            _points = points.ToArray();
+
+            if (_points.Length < 2)
+            {
+                throw new ArgumentException(
+                    "二维轨迹至少需要两个轨迹点。",
+                    nameof(points));
+            }
+
+            if (Math.Abs(_points[0].ArcLengthMeters) >
+                StartArcLengthToleranceMeters)
+            {
+                throw new ArgumentException(
+                    "二维轨迹起点的累计弧长必须为0m。",
+                    nameof(points));
+            }
+
+            for (var index = 1;
+                 index < _points.Length;
+                 index++)
+            {
+                ValidateSegment(
+                    _points[index - 1],
+                    _points[index],
+                    index,
+                    nameof(points));
+            }
+
+            _readOnlyPoints =
+                Array.AsReadOnly(_points);
+        }
+
+        /// <summary>
+        /// 获取轨迹点数量。
+        /// </summary>
+        public int Count => _points.Length;
+
+        /// <summary>
+        /// 获取指定索引处的轨迹点。
+        /// </summary>
+        public TrajectoryPoint this[int index] =>
+            _points[index];
+
+        /// <summary>
+        /// 获取不可修改的有序轨迹点集合。
+        /// </summary>
+        public IReadOnlyList<TrajectoryPoint> Points =>
+            _readOnlyPoints;
+
+        /// <summary>
+        /// 获取轨迹起点。
+        /// </summary>
+        public TrajectoryPoint StartPoint =>
+            _points[0];
+
+        /// <summary>
+        /// 获取轨迹终点。
+        /// </summary>
+        public TrajectoryPoint EndPoint =>
+            _points[_points.Length - 1];
+
+        /// <summary>
+        /// 获取轨迹总弧长，单位为m。
+        /// </summary>
+        public double TotalLengthMeters =>
+            EndPoint.ArcLengthMeters;
+
+        /// <summary>
+        /// 根据当前累计弧长计算到轨迹终点的剩余距离。
+        /// </summary>
+        public double GetRemainingDistanceMeters(
+            double arcLengthMeters)
+        {
+            NumericGuard.EnsureFinite(
+                arcLengthMeters,
+                nameof(arcLengthMeters));
+
+            if (arcLengthMeters <= 0.0)
+                return TotalLengthMeters;
+
+            if (arcLengthMeters >= TotalLengthMeters)
+                return 0.0;
+
+            return TotalLengthMeters - arcLengthMeters;
+        }
+
+        /// <summary>
+        /// 按累计弧长在线性位置、航向、曲率和参考速度之间插值得到轨迹点。
+        /// </summary>
+        public TrajectoryPoint SampleAtArcLength(
+            double arcLengthMeters)
+        {
+            NumericGuard.EnsureFinite(
+                arcLengthMeters,
+                nameof(arcLengthMeters));
+
+            if (arcLengthMeters <= 0.0)
+            {
+                return StartPoint;
+            }
+
+            if (arcLengthMeters >= TotalLengthMeters)
+            {
+                return EndPoint;
+            }
+
+            var segmentStartIndex =
+                FindSegmentStartIndex(
+                    arcLengthMeters);
+            var segmentStart =
+                _points[segmentStartIndex];
+            var segmentEnd =
+                _points[segmentStartIndex + 1];
+            var interpolationRatio =
+                (arcLengthMeters -
+                 segmentStart.ArcLengthMeters) /
+                (segmentEnd.ArcLengthMeters -
+                 segmentStart.ArcLengthMeters);
+
+            return InterpolateSegment(
+                segmentStartIndex,
+                interpolationRatio);
+        }
+
+        /// <summary>
+        /// 在指定线段上统一插值位置、车头航向、曲率和有符号参考速度。
+        /// </summary>
+        internal TrajectoryPoint InterpolateSegment(
+            int segmentStartIndex,
+            double interpolationRatio)
+        {
+            if (segmentStartIndex < 0 ||
+                segmentStartIndex >= _points.Length - 1)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(segmentStartIndex),
+                    "轨迹插值线段索引必须指向一条有效线段的起点。");
+            }
+
+            NumericGuard.EnsureFinite(
+                interpolationRatio,
+                nameof(interpolationRatio));
+
+            if (interpolationRatio < 0.0 ||
+                interpolationRatio > 1.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(interpolationRatio),
+                    "轨迹线段插值比例必须位于[0,1]范围内。");
+            }
+
+            var segmentStart =
+                _points[segmentStartIndex];
+            var segmentEnd =
+                _points[segmentStartIndex + 1];
+            var arcLengthMeters =
+                InterpolationMath.Lerp(
+                    segmentStart.ArcLengthMeters,
+                    segmentEnd.ArcLengthMeters,
+                    interpolationRatio);
+
+            return new TrajectoryPoint(
+                arcLengthMeters,
+                new Pose2D(
+                    InterpolationMath.Lerp(
+                        segmentStart.PoseInWorld.XMeters,
+                        segmentEnd.PoseInWorld.XMeters,
+                        interpolationRatio),
+                    InterpolationMath.Lerp(
+                        segmentStart.PoseInWorld.YMeters,
+                        segmentEnd.PoseInWorld.YMeters,
+                        interpolationRatio),
+                    AngleMath.LerpRadians(
+                        segmentStart.PoseInWorld.YawRadians,
+                        segmentEnd.PoseInWorld.YawRadians,
+                        interpolationRatio)),
+                InterpolationMath.Lerp(
+                    segmentStart.CurvaturePerMeter,
+                    segmentEnd.CurvaturePerMeter,
+                    interpolationRatio),
+                InterpolationMath.Lerp(
+                    segmentStart.ReferenceSpeedMetersPerSecond,
+                    segmentEnd.ReferenceSpeedMetersPerSecond,
+                    interpolationRatio));
+        }
+
+        /// <summary>
+        /// 使用二分查找获取包含指定累计弧长的线段起点索引。
+        /// </summary>
+        internal int FindSegmentStartIndex(
+            double arcLengthMeters)
+        {
+            NumericGuard.EnsureFinite(
+                arcLengthMeters,
+                nameof(arcLengthMeters));
+
+            if (arcLengthMeters <= 0.0)
+            {
+                return 0;
+            }
+
+            if (arcLengthMeters >= TotalLengthMeters)
+            {
+                return _points.Length - 2;
+            }
+
+            var lowerIndex = 0;
+            var upperIndex = _points.Length - 1;
+
+            while (upperIndex - lowerIndex > 1)
+            {
+                var middleIndex =
+                    lowerIndex +
+                    (upperIndex - lowerIndex) / 2;
+
+                if (_points[middleIndex].ArcLengthMeters <=
+                    arcLengthMeters)
+                {
+                    lowerIndex = middleIndex;
+                }
+                else
+                {
+                    upperIndex = middleIndex;
+                }
+            }
+
+            return lowerIndex;
+        }
+
+        /// <summary>
+        /// 检查相邻轨迹点是否构成有效的非零长度有序线段。
+        /// </summary>
+        private static void ValidateSegment(
+            TrajectoryPoint previous,
+            TrajectoryPoint current,
+            int currentIndex,
+            string parameterName)
+        {
+            if (current.ArcLengthMeters <=
+                previous.ArcLengthMeters)
+            {
+                throw new ArgumentException(
+                    $"轨迹点{currentIndex}的累计弧长必须严格大于前一个点。",
+                    parameterName);
+            }
+
+            var deltaX =
+                current.PoseInWorld.XMeters -
+                previous.PoseInWorld.XMeters;
+            var deltaY =
+                current.PoseInWorld.YMeters -
+                previous.PoseInWorld.YMeters;
+            var segmentLengthSquared =
+                deltaX * deltaX +
+                deltaY * deltaY;
+            var minimumLengthSquared =
+                MinimumSegmentLengthMeters *
+                MinimumSegmentLengthMeters;
+
+            if (segmentLengthSquared <
+                minimumLengthSquared)
+            {
+                throw new ArgumentException(
+                    $"轨迹点{currentIndex}与前一个点的位置过近，无法构成有效投影线段。",
+                    parameterName);
+            }
+
+            var segmentLengthMeters =
+                Math.Sqrt(segmentLengthSquared);
+            var arcLengthIncrementMeters =
+                current.ArcLengthMeters -
+                previous.ArcLengthMeters;
+            var maximumAllowedDifferenceMeters =
+                Math.Max(
+                    ArcLengthConsistencyAbsoluteToleranceMeters,
+                    ArcLengthConsistencyRelativeTolerance *
+                    Math.Max(
+                        segmentLengthMeters,
+                        arcLengthIncrementMeters));
+
+            // 当前轨迹在相邻采样点之间按直线段投影，因此累计弧长增量
+            // 必须与该离散线段长度近似一致，防止进度和实际几何脱节。
+            if (Math.Abs(
+                    arcLengthIncrementMeters -
+                    segmentLengthMeters) >
+                maximumAllowedDifferenceMeters)
+            {
+                throw new ArgumentException(
+                    $"轨迹点{currentIndex}的累计弧长增量" +
+                    $"{arcLengthIncrementMeters:F6}m与离散线段长度" +
+                    $"{segmentLengthMeters:F6}m不一致。",
+                    parameterName);
+            }
+        }
+    }
+}

MultiWheelC/Trajectory/TrajectoryPoint.cs

@@ -0,0 +1,64 @@
+using System;
+using MyParking.Shared;
+
+namespace MultiWheelC.Trajectory
+{
+    /// <summary>
+    /// 描述按执行点序和累计弧长参数化的车体中心参考轨迹点，统一使用SI单位。
+    /// </summary>
+    public readonly struct TrajectoryPoint
+    {
+        /// <summary>
+        /// 创建包含中心位姿、曲率和速度信息的参考轨迹点。
+        /// </summary>
+        public TrajectoryPoint(
+            double arcLengthMeters,
+            Pose2D poseInWorld,
+            double curvaturePerMeter,
+            double referenceSpeedMetersPerSecond)
+        {
+            NumericGuard.EnsureFiniteNonNegative(
+                arcLengthMeters,
+                nameof(arcLengthMeters));
+            NumericGuard.EnsureFinite(
+                poseInWorld,
+                nameof(poseInWorld));
+            NumericGuard.EnsureFinite(
+                curvaturePerMeter,
+                nameof(curvaturePerMeter));
+            NumericGuard.EnsureFinite(
+                referenceSpeedMetersPerSecond,
+                nameof(referenceSpeedMetersPerSecond));
+            ArcLengthMeters = arcLengthMeters;
+            PoseInWorld = new Pose2D(
+                poseInWorld.XMeters,
+                poseInWorld.YMeters,
+                AngleMath.NormalizeRadians(
+                    poseInWorld.YawRadians));
+            CurvaturePerMeter = curvaturePerMeter;
+            ReferenceSpeedMetersPerSecond =
+                referenceSpeedMetersPerSecond;
+        }
+
+        /// <summary>
+        /// 获取沿预定执行点序从轨迹起点累计到当前点的弧长，单位为m。
+        /// </summary>
+        public double ArcLengthMeters { get; }
+
+        /// <summary>
+        /// 获取车体中心参考坐标系在世界坐标系中的位姿；航向始终表示车头方向。
+        /// </summary>
+        public Pose2D PoseInWorld { get; }
+
+        /// <summary>
+        /// 获取沿累计弧长增加方向的车体中心参考轨迹曲率，单位为1/m，左弯为正。
+        /// </summary>
+        public double CurvaturePerMeter { get; }
+
+        /// <summary>
+        /// 获取车体纵向有符号参考速度，单位为m/s；正值前进、负值倒车、零值停车。
+        /// </summary>
+        public double ReferenceSpeedMetersPerSecond { get; }
+
+    }
+}

MultiWheelC/Trajectory/TrajectoryProjection.cs

@@ -0,0 +1,91 @@
+using System;
+using MyParking.Shared;
+
+namespace MultiWheelC.Trajectory
+{
+    /// <summary>
+    /// 保存车体中心投影到二维参考轨迹后得到的只读结果。
+    /// </summary>
+    public readonly struct TrajectoryProjection
+    {
+        /// <summary>
+        /// 创建包含轨迹进度、参考状态和跟踪误差的投影结果。
+        /// </summary>
+        public TrajectoryProjection(
+            int segmentStartIndex,
+            TrajectoryPoint referencePoint,
+            double lateralErrorMeters,
+            double headingErrorRadians,
+            double distanceToTrajectoryMeters,
+            double remainingDistanceMeters)
+        {
+            if (segmentStartIndex < 0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(segmentStartIndex),
+                    "投影线段起点索引不能为负数。");
+            }
+
+            NumericGuard.EnsureFinite(
+                lateralErrorMeters,
+                nameof(lateralErrorMeters));
+            NumericGuard.EnsureFinite(
+                headingErrorRadians,
+                nameof(headingErrorRadians));
+            NumericGuard.EnsureFiniteNonNegative(
+                distanceToTrajectoryMeters,
+                nameof(distanceToTrajectoryMeters));
+            NumericGuard.EnsureFiniteNonNegative(
+                remainingDistanceMeters,
+                nameof(remainingDistanceMeters));
+
+            SegmentStartIndex = segmentStartIndex;
+            ReferencePoint = referencePoint;
+            LateralErrorMeters = lateralErrorMeters;
+            HeadingErrorRadians =
+                AngleMath.NormalizeRadians(
+                    headingErrorRadians);
+            DistanceToTrajectoryMeters =
+                distanceToTrajectoryMeters;
+            RemainingDistanceMeters =
+                remainingDistanceMeters;
+        }
+
+        /// <summary>
+        /// 获取投影所在轨迹线段的起点索引，线段终点索引为该值加1。
+        /// </summary>
+        public int SegmentStartIndex { get; }
+
+        /// <summary>
+        /// 获取投影位置插值得到的车体中心参考轨迹点。
+        /// </summary>
+        public TrajectoryPoint ReferencePoint { get; }
+
+        /// <summary>
+        /// 获取相对累计弧长增加方向的有符号横向误差，单位为m，参考轨迹位于该方向左侧时为正。
+        /// </summary>
+        public double LateralErrorMeters { get; }
+
+        /// <summary>
+        /// 获取参考航向减实际车体航向的最短角差，单位为rad，逆时针为正。
+        /// </summary>
+        public double HeadingErrorRadians { get; }
+
+        /// <summary>
+        /// 获取车体中心到投影点的欧氏距离，单位为m。
+        /// </summary>
+        public double DistanceToTrajectoryMeters { get; }
+
+        /// <summary>
+        /// 获取投影位置沿轨迹到终点的剩余弧长，单位为m。
+        /// </summary>
+        public double RemainingDistanceMeters { get; }
+
+        /// <summary>
+        /// 获取投影位置从轨迹起点累计的弧长，单位为m。
+        /// </summary>
+        public double ArcLengthMeters =>
+            ReferencePoint.ArcLengthMeters;
+
+    }
+}

MultiWheelC/Trajectory/TrajectoryProjector.cs

@@ -0,0 +1,286 @@
+using System;
+using MyParking.Shared;
+
+namespace MultiWheelC.Trajectory
+{
+    /// <summary>
+    /// 将实际车体中心位姿投影到二维离散参考轨迹，并支持按上次进度限制搜索范围。
+    /// </summary>
+    public static class TrajectoryProjector
+    {
+        private const double DistanceTieToleranceSquaredMeters =
+            1e-12;
+
+        /// <summary>
+        /// 在整条轨迹上查找距离实际车体中心最近的线段投影结果。
+        /// </summary>
+        public static TrajectoryProjection Project(
+            Trajectory2D trajectory,
+            Pose2D vehiclePoseInWorld)
+        {
+            ValidateProjectionInput(
+                trajectory,
+                vehiclePoseInWorld,
+                nameof(vehiclePoseInWorld));
+
+            return ProjectRange(
+                trajectory,
+                vehiclePoseInWorld,
+                firstSegmentStartIndex: 0,
+                lastSegmentStartIndex:
+                    trajectory.Count - 2,
+                preferredArcLengthMeters: null);
+        }
+
+        /// <summary>
+        /// 以上次投影弧长为中心，仅在指定前后物理距离窗口内查找最近线段。
+        /// </summary>
+        public static TrajectoryProjection Project(
+            Trajectory2D trajectory,
+            Pose2D vehiclePoseInWorld,
+            double previousArcLengthMeters,
+            double maximumBackwardSearchDistanceMeters,
+            double maximumForwardSearchDistanceMeters)
+        {
+            ValidateProjectionInput(
+                trajectory,
+                vehiclePoseInWorld,
+                nameof(vehiclePoseInWorld));
+            NumericGuard.EnsureFiniteNonNegative(
+                previousArcLengthMeters,
+                nameof(previousArcLengthMeters));
+            NumericGuard.EnsureFiniteNonNegative(
+                maximumBackwardSearchDistanceMeters,
+                nameof(maximumBackwardSearchDistanceMeters));
+            NumericGuard.EnsureFinitePositive(
+                maximumForwardSearchDistanceMeters,
+                nameof(maximumForwardSearchDistanceMeters));
+
+            if (previousArcLengthMeters >
+                trajectory.TotalLengthMeters)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(previousArcLengthMeters),
+                    "上次投影弧长不能超过轨迹总长度。");
+            }
+
+            var searchStartArcLengthMeters =
+                Math.Max(
+                    0.0,
+                    previousArcLengthMeters -
+                    maximumBackwardSearchDistanceMeters);
+            var searchEndArcLengthMeters =
+                Math.Min(
+                    trajectory.TotalLengthMeters,
+                    previousArcLengthMeters +
+                    maximumForwardSearchDistanceMeters);
+            var firstSegmentStartIndex =
+                trajectory.FindSegmentStartIndex(
+                    searchStartArcLengthMeters);
+            var lastSegmentStartIndex =
+                trajectory.FindSegmentStartIndex(
+                    searchEndArcLengthMeters);
+
+            return ProjectRange(
+                trajectory,
+                vehiclePoseInWorld,
+                firstSegmentStartIndex,
+                lastSegmentStartIndex,
+                previousArcLengthMeters);
+        }
+
+        /// <summary>
+        /// 在闭区间线段索引范围内查找最近投影，并在距离并列时优先保持原进度。
+        /// </summary>
+        private static TrajectoryProjection ProjectRange(
+            Trajectory2D trajectory,
+            Pose2D vehiclePoseInWorld,
+            int firstSegmentStartIndex,
+            int lastSegmentStartIndex,
+            double? preferredArcLengthMeters)
+        {
+            var bestSegmentStartIndex = 0;
+            var bestInterpolationRatio = 0.0;
+            var bestDistanceSquared =
+                double.PositiveInfinity;
+            var bestProgressDifferenceMeters =
+                double.PositiveInfinity;
+
+            for (var segmentStartIndex =
+                     firstSegmentStartIndex;
+                 segmentStartIndex <=
+                     lastSegmentStartIndex;
+                 segmentStartIndex++)
+            {
+                var segmentStart =
+                    trajectory[segmentStartIndex];
+                var segmentEnd =
+                    trajectory[segmentStartIndex + 1];
+
+                var segmentX =
+                    segmentEnd.PoseInWorld.XMeters -
+                    segmentStart.PoseInWorld.XMeters;
+                var segmentY =
+                    segmentEnd.PoseInWorld.YMeters -
+                    segmentStart.PoseInWorld.YMeters;
+                var segmentLengthSquared =
+                    segmentX * segmentX +
+                    segmentY * segmentY;
+
+                var vehicleFromSegmentStartX =
+                    vehiclePoseInWorld.XMeters -
+                    segmentStart.PoseInWorld.XMeters;
+                var vehicleFromSegmentStartY =
+                    vehiclePoseInWorld.YMeters -
+                    segmentStart.PoseInWorld.YMeters;
+
+                var interpolationRatio =
+                    InterpolationMath.Clamp01(
+                        (vehicleFromSegmentStartX * segmentX +
+                         vehicleFromSegmentStartY * segmentY) /
+                        segmentLengthSquared);
+
+                var projectedX =
+                    InterpolationMath.Lerp(
+                        segmentStart.PoseInWorld.XMeters,
+                        segmentEnd.PoseInWorld.XMeters,
+                        interpolationRatio);
+                var projectedY =
+                    InterpolationMath.Lerp(
+                        segmentStart.PoseInWorld.YMeters,
+                        segmentEnd.PoseInWorld.YMeters,
+                        interpolationRatio);
+
+                var projectionErrorX =
+                    projectedX -
+                    vehiclePoseInWorld.XMeters;
+                var projectionErrorY =
+                    projectedY -
+                    vehiclePoseInWorld.YMeters;
+                var distanceSquared =
+                    projectionErrorX * projectionErrorX +
+                    projectionErrorY * projectionErrorY;
+
+                var progressDifferenceMeters =
+                    preferredArcLengthMeters.HasValue
+                        ? Math.Abs(
+                            InterpolationMath.Lerp(
+                                segmentStart.ArcLengthMeters,
+                                segmentEnd.ArcLengthMeters,
+                                interpolationRatio) -
+                            preferredArcLengthMeters.Value)
+                        : 0.0;
+                var hasMeaningfullyShorterDistance =
+                    distanceSquared <
+                    bestDistanceSquared -
+                    DistanceTieToleranceSquaredMeters;
+                var hasEquivalentDistanceAndCloserProgress =
+                    preferredArcLengthMeters.HasValue &&
+                    Math.Abs(
+                        distanceSquared -
+                        bestDistanceSquared) <=
+                    DistanceTieToleranceSquaredMeters &&
+                    progressDifferenceMeters <
+                    bestProgressDifferenceMeters;
+
+                if (!hasMeaningfullyShorterDistance &&
+                    !hasEquivalentDistanceAndCloserProgress)
+                {
+                    continue;
+                }
+
+                bestSegmentStartIndex =
+                    segmentStartIndex;
+                bestInterpolationRatio =
+                    interpolationRatio;
+                bestDistanceSquared = distanceSquared;
+                bestProgressDifferenceMeters =
+                    progressDifferenceMeters;
+            }
+
+            return BuildProjection(
+                trajectory,
+                vehiclePoseInWorld,
+                bestSegmentStartIndex,
+                bestInterpolationRatio,
+                bestDistanceSquared);
+        }
+
+        /// <summary>
+        /// 根据最近线段和插值比例生成控制器使用的完整投影结果。
+        /// </summary>
+        private static TrajectoryProjection BuildProjection(
+            Trajectory2D trajectory,
+            Pose2D vehiclePoseInWorld,
+            int segmentStartIndex,
+            double interpolationRatio,
+            double distanceSquared)
+        {
+            var segmentStart =
+                trajectory[segmentStartIndex];
+            var segmentEnd =
+                trajectory[segmentStartIndex + 1];
+            var referencePoint =
+                trajectory.InterpolateSegment(
+                    segmentStartIndex,
+                    interpolationRatio);
+
+            var segmentX =
+                segmentEnd.PoseInWorld.XMeters -
+                segmentStart.PoseInWorld.XMeters;
+            var segmentY =
+                segmentEnd.PoseInWorld.YMeters -
+                segmentStart.PoseInWorld.YMeters;
+            var segmentLength =
+                Math.Sqrt(
+                    segmentX * segmentX +
+                    segmentY * segmentY);
+
+            // 以轨迹线段的前进方向判断左右：
+            // 从车辆指向参考轨迹的向量位于轨迹左侧时为正。
+            var vehicleToProjectionX =
+                referencePoint.PoseInWorld.XMeters -
+                vehiclePoseInWorld.XMeters;
+            var vehicleToProjectionY =
+                referencePoint.PoseInWorld.YMeters -
+                vehiclePoseInWorld.YMeters;
+            var lateralErrorMeters =
+                (segmentX * vehicleToProjectionY -
+                 segmentY * vehicleToProjectionX) /
+                segmentLength;
+
+            var headingErrorRadians =
+                AngleMath.ShortestDifferenceRadians(
+                    referencePoint.PoseInWorld.YawRadians,
+                    vehiclePoseInWorld.YawRadians);
+
+            return new TrajectoryProjection(
+                segmentStartIndex,
+                referencePoint,
+                lateralErrorMeters,
+                headingErrorRadians,
+                Math.Sqrt(distanceSquared),
+                trajectory.GetRemainingDistanceMeters(
+                    referencePoint.ArcLengthMeters));
+        }
+
+        /// <summary>
+        /// 检查轨迹对象和用于投影的实际车体中心位姿。
+        /// </summary>
+        private static void ValidateProjectionInput(
+            Trajectory2D trajectory,
+            Pose2D pose,
+            string parameterName)
+        {
+            if (trajectory == null)
+            {
+                throw new ArgumentNullException(
+                    nameof(trajectory));
+            }
+
+            NumericGuard.EnsureFinite(
+                pose,
+                parameterName);
+        }
+    }
+}

ParkingRobot.sln

@@ -0,0 +1,70 @@
+
+Microsoft Visual Studio Solution File, Format Version 12.00
+# Visual Studio Version 17
+VisualStudioVersion = 17.0.31903.59
+MinimumVisualStudioVersion = 10.0.40219.1
+Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "CommonUsage-MultiVehicleSync", "CommonUsage-MultiVehicleSync", "{2291F6EE-0AD9-D010-97BA-674682470F91}"
+EndProject
+Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "commonusage", "commonusage", "{0A043828-FC50-B2F1-03BD-49EF4E42C81C}"
+EndProject
+Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "CommonUsage", "CommonUsage-MultiVehicleSync\commonusage\CommonUsage.csproj", "{DAAF1A42-FA55-4FC0-8083-F5A2FEFE0A41}"
+EndProject
+Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "MedullaAdapter", "MedullaAdapter\MedullaAdapter.csproj", "{95F7BD98-C801-4E4E-B8A6-C0EFE3535C5C}"
+EndProject
+Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "MultiWheelC", "MultiWheelC\MultiWheelC.csproj", "{29520219-FA63-4B59-AA80-91B4FFA0D9B1}"
+EndProject
+Global
+	GlobalSection(SolutionConfigurationPlatforms) = preSolution
+		Debug|Any CPU = Debug|Any CPU
+		Debug|x64 = Debug|x64
+		Debug|x86 = Debug|x86
+		Release|Any CPU = Release|Any CPU
+		Release|x64 = Release|x64
+		Release|x86 = Release|x86
+	EndGlobalSection
+	GlobalSection(ProjectConfigurationPlatforms) = postSolution
+		{DAAF1A42-FA55-4FC0-8083-F5A2FEFE0A41}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
+		{DAAF1A42-FA55-4FC0-8083-F5A2FEFE0A41}.Debug|Any CPU.Build.0 = Debug|Any CPU
+		{DAAF1A42-FA55-4FC0-8083-F5A2FEFE0A41}.Debug|x64.ActiveCfg = Debug|Any CPU
+		{DAAF1A42-FA55-4FC0-8083-F5A2FEFE0A41}.Debug|x64.Build.0 = Debug|Any CPU
+		{DAAF1A42-FA55-4FC0-8083-F5A2FEFE0A41}.Debug|x86.ActiveCfg = Debug|Any CPU
+		{DAAF1A42-FA55-4FC0-8083-F5A2FEFE0A41}.Debug|x86.Build.0 = Debug|Any CPU
+		{DAAF1A42-FA55-4FC0-8083-F5A2FEFE0A41}.Release|Any CPU.ActiveCfg = Release|Any CPU
+		{DAAF1A42-FA55-4FC0-8083-F5A2FEFE0A41}.Release|Any CPU.Build.0 = Release|Any CPU
+		{DAAF1A42-FA55-4FC0-8083-F5A2FEFE0A41}.Release|x64.ActiveCfg = Release|Any CPU
+		{DAAF1A42-FA55-4FC0-8083-F5A2FEFE0A41}.Release|x64.Build.0 = Release|Any CPU
+		{DAAF1A42-FA55-4FC0-8083-F5A2FEFE0A41}.Release|x86.ActiveCfg = Release|Any CPU
+		{DAAF1A42-FA55-4FC0-8083-F5A2FEFE0A41}.Release|x86.Build.0 = Release|Any CPU
+		{95F7BD98-C801-4E4E-B8A6-C0EFE3535C5C}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
+		{95F7BD98-C801-4E4E-B8A6-C0EFE3535C5C}.Debug|Any CPU.Build.0 = Debug|Any CPU
+		{95F7BD98-C801-4E4E-B8A6-C0EFE3535C5C}.Debug|x64.ActiveCfg = Debug|Any CPU
+		{95F7BD98-C801-4E4E-B8A6-C0EFE3535C5C}.Debug|x64.Build.0 = Debug|Any CPU
+		{95F7BD98-C801-4E4E-B8A6-C0EFE3535C5C}.Debug|x86.ActiveCfg = Debug|Any CPU
+		{95F7BD98-C801-4E4E-B8A6-C0EFE3535C5C}.Debug|x86.Build.0 = Debug|Any CPU
+		{95F7BD98-C801-4E4E-B8A6-C0EFE3535C5C}.Release|Any CPU.ActiveCfg = Release|Any CPU
+		{95F7BD98-C801-4E4E-B8A6-C0EFE3535C5C}.Release|Any CPU.Build.0 = Release|Any CPU
+		{95F7BD98-C801-4E4E-B8A6-C0EFE3535C5C}.Release|x64.ActiveCfg = Release|Any CPU
+		{95F7BD98-C801-4E4E-B8A6-C0EFE3535C5C}.Release|x64.Build.0 = Release|Any CPU
+		{95F7BD98-C801-4E4E-B8A6-C0EFE3535C5C}.Release|x86.ActiveCfg = Release|Any CPU
+		{95F7BD98-C801-4E4E-B8A6-C0EFE3535C5C}.Release|x86.Build.0 = Release|Any CPU
+		{29520219-FA63-4B59-AA80-91B4FFA0D9B1}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
+		{29520219-FA63-4B59-AA80-91B4FFA0D9B1}.Debug|Any CPU.Build.0 = Debug|Any CPU
+		{29520219-FA63-4B59-AA80-91B4FFA0D9B1}.Debug|x64.ActiveCfg = Debug|Any CPU
+		{29520219-FA63-4B59-AA80-91B4FFA0D9B1}.Debug|x64.Build.0 = Debug|Any CPU
+		{29520219-FA63-4B59-AA80-91B4FFA0D9B1}.Debug|x86.ActiveCfg = Debug|Any CPU
+		{29520219-FA63-4B59-AA80-91B4FFA0D9B1}.Debug|x86.Build.0 = Debug|Any CPU
+		{29520219-FA63-4B59-AA80-91B4FFA0D9B1}.Release|Any CPU.ActiveCfg = Release|Any CPU
+		{29520219-FA63-4B59-AA80-91B4FFA0D9B1}.Release|Any CPU.Build.0 = Release|Any CPU
+		{29520219-FA63-4B59-AA80-91B4FFA0D9B1}.Release|x64.ActiveCfg = Release|Any CPU
+		{29520219-FA63-4B59-AA80-91B4FFA0D9B1}.Release|x64.Build.0 = Release|Any CPU
+		{29520219-FA63-4B59-AA80-91B4FFA0D9B1}.Release|x86.ActiveCfg = Release|Any CPU
+		{29520219-FA63-4B59-AA80-91B4FFA0D9B1}.Release|x86.Build.0 = Release|Any CPU
+	EndGlobalSection
+	GlobalSection(SolutionProperties) = preSolution
+		HideSolutionNode = FALSE
+	EndGlobalSection
+	GlobalSection(NestedProjects) = preSolution
+		{0A043828-FC50-B2F1-03BD-49EF4E42C81C} = {2291F6EE-0AD9-D010-97BA-674682470F91}
+		{DAAF1A42-FA55-4FC0-8083-F5A2FEFE0A41} = {0A043828-FC50-B2F1-03BD-49EF4E42C81C}
+	EndGlobalSection
+EndGlobal

README.md

@@ -8,32 +8,31 @@
 
 1. 先实现单台停车机器人小车的基本功能；
 2. 在单车闭环稳定后逐步增加停车作业功能；
-3. 基于仿真、台架和实车数据优化轨迹跟踪方法；
+3. 基于台架和实车数据优化轨迹跟踪方法；
 4. 最后再考虑多车通信、编队和协同控制。
 
-当前工作仍以**单车**为主，已经从基础框架搭建进入底盘联调、功能补充和跟踪实验阶段。多车配置仍位于 `PilotConfig.cs` 的 `#if false` 区域，`Shared/FleetKinematics.cs` 仍是占位文件，不能视为多车能力已经实现。
+当前工作仍以**单车**为主，处于底盘联调、功能补充和跟踪实验阶段。多车配置位于 `MultiWheelC/PilotConfig.cs` 的 `#if false` 区域，`Shared/Fleet/FleetKinematics.cs` 仍是占位文件，不能视为多车能力已经实现。
 
 | 阶段 | 当前状态 | 说明 |
 | --- | --- | --- |
 | 1. 单车基本功能 | 联调中 | 已接入运动控制、MCU 通信、轮组反馈、急停 IO、电池、灯光、遥控和诊断代码，仍需持续实车验证 |
-| 2. 增加停车功能 | 部分开展 | 已提供夹臂控制、限位、报警和测试入口；轮胎识别、钻车和完整停车流程尚未实现 |
-| 3. 优化跟踪方法 | 已启动 | 已加入直线、圆弧、S 型、蟹行测试、实验 CSV 记录和 Python 绘图工具 |
+| 2. 增加停车功能 | 部分开展 | 已接入夹臂控制、限位和报警；夹臂动作测试当前已注释，轮胎识别、钻车和完整停车流程尚未实现 |
+| 3. 优化跟踪方法 | 已启动 | 保留旧版 `SendMotion` 测试，并新增 Stanley 横向 + PID 纵向控制、组合运动计划、实验 CSV 和新版绘图工具 |
 | 4. 多车场景 | 暂不实施 | 多车参数和预研内容未参与当前编译，当前版本不提供多车联动 |
 
 ## 项目简介
 
-MyParking 是一个面向多轮停车机器人底盘的 C# 工程，覆盖上层运动动作、共享运动学、底层硬件适配、离线 Web 仿真和实验数据分析。
+MyParking 是一个面向多轮停车机器人底盘的 C# 工程，覆盖上层运动动作、共享运动学、底层硬件适配和实验数据分析。
 
-核心代码分为：
+核心模块：
 
-- `ClumsyPilot`：Clumsy 上层动作、轨迹跟踪和人工测试；
-- `MedullaAdapter`：Medulla 下层 MCU、CAN、串口、轮组、夹臂、遥控和报警适配；
-- `Shared`：统一的二维坐标、底盘命令、坐标变换和多轮底盘适配；
+- `MultiWheelC`：Clumsy 上层（C 层）动作、轨迹跟踪、人工测试和实验记录；
+- `MedullaAdapter`：Medulla 下层（M 层）MCU、CAN、串口、轮组、夹臂、遥控和报警适配；
+- `Shared`：M/C 共享的二维坐标、底盘命令、坐标变换和多轮底盘适配（无独立 `.csproj`，由两端编译引入）；
 - `CommonUsage-MultiVehicleSync/commonusage`：仓库内的 `CommonUsage` 底盘公共库源码；
-- `Simulation`：基于 ASP.NET Core 的单车 Web 仿真器；
-- `data_process`：轨迹实验 CSV 的 Python 分析工具。
+- `data_process`：轨迹实验与舵轮响应的 Python 分析工具。
 
-仓库中没有 ROS/ROS 2 或 Docker 配置。
+仓库中没有 ROS/ROS 2、Docker 或 Web 仿真项目。插件由 Clumsy / Medulla 宿主加载，不能通过 `dotnet run` 独立启动。
 
 ## 当前已接入能力
 
@@ -42,13 +41,13 @@ MyParking 是一个面向多轮停车机器人底盘的 C# 工程，覆盖上层
 | 单车运动 | 直线、圆弧、S 型轨迹，前进、蟹行和原地旋转 |
 | 底盘命令 | `SendMotion`、`SendXYThSpeed` 和虚拟阿克曼测试后端 |
 | 模式切换 | 正常、蟹行、自转模式；切换时先停车、预转舵轮并等待到位 |
-| 跟踪控制 | 终点跟踪、直线跟踪、基于 Detour 的直线跟踪和蟹行运动坐标系跟踪 |
+| 跟踪控制 | 旧版终点/直线/蟹行跟踪；新版 Stanley 横向控制、PID 纵向控制、前后 GCP 分配、轨迹偏离保护和终点状态判定 |
+| 状态估计 | Detour 位姿与差分速度；新版实验可保留 Detour 位姿并用舵轮反馈解算、低通滤波后的车体纵向速度替代其差分纵向速度 |
 | 夹臂 | 左右夹臂速度命令、位置反馈、软限位、驱动报警、实体/虚拟遥控和目标位置动作 |
 | MCU 通信 | 串口桥打开、复位、版本/状态查询、数字 IO、CAN/串口同步收发和异步回调 |
 | 驱动与反馈 | 8 个驱动电机和 4 个舵轮的命令、速度/位置/舵角反馈及远程帧状态 |
 | 车辆状态 | 急停、启停、抱闸、灯光、电池 SOC/SOH 和驱动使能状态 |
 | 诊断 | CAN 轮速事件与周期快照 CSV、轨迹实验 CSV、控制命令和 Detour 位姿记录 |
-| 仿真 | 浏览器二维车辆显示、模式按钮、手动控制、车辆配置、复位和 REST API |
 
 以上表示代码和测试入口已经存在，不等同于所有工况均已完成实车验收。
 
@@ -58,7 +57,7 @@ MyParking 是一个面向多轮停车机器人底盘的 C# 工程，覆盖上层
 Clumsy 宿主
     │
     ▼
-ClumsyPilot ───────────────┐
+MultiWheelC ───────────────┐
     │                      │
     ▼                      │ 实验 CSV
 Shared / CommonUsage       ├──────────► data_process
@@ -74,111 +73,112 @@ mcu_serial_bridge.dll      │
     │                      │
     ▼                      │
 MCU ─► CAN / Serial / IO ──┘
-
-Simulation ─► Shared 数据类型 ─► 浏览器仿真界面
 ```
 
-`ClumsyPilot` 和 `MedullaAdapter` 生成插件类库，需要由对应宿主加载；`Simulation` 是可以独立启动的 ASP.NET Core Web 项目。
+`MultiWheelC` 与 `MedullaAdapter` 生成插件类库，需由对应宿主加载。`CommonUsage` 是独立底盘库，不反向依赖 `Shared`、M 层或 C 层。
+
+## 坐标系与单位
+
+- `Shared` 统一使用 SI 单位：m、m/s、rad、rad/s。
+- 车体坐标系：X 向前、Y 向左、逆时针为正。
+- 旧接口单位只在边界处转换。
+- 角度归一化、最短角差和度弧度转换统一使用 `Shared/Mathematics/AngleMath.cs`。
+- 弧度归一化范围为 `[-π, π)`，度归一化范围为 `[-180°, 180°)`。
+- 车辆航向可用圆周最短角差；受 `[-120°, 120°]` 限制的机械舵角误差必须直接使用目标值减实际值。
 
 ## 目录说明
 
 ```text
 MyParking/
-├── ParkingRobot.sln
-├── ClumsyPilot/                     # 上层动作、跟踪、测试和实验记录
-├── MedullaAdapter/                  # MCU、CAN、轮组、夹臂、遥控和报警
-├── Shared/                          # 共享命令、坐标变换和底盘适配
+├── ParkingRobot.sln                 # 根解决方案（CommonUsage / M / C）
+├── build-and-package.ps1            # 官方构建与 M/C 打包脚本
+├── AGENTS.md                        # 协作与代码规范
+├── MultiWheelC/                     # C 层动作、跟踪、测试和实验记录
+├── MedullaAdapter/                  # M 层 MCU、CAN、轮组、夹臂、遥控和报警
+├── Shared/                          # 共享模型、数学方法和底盘适配
 ├── CommonUsage-MultiVehicleSync/
 │   └── commonusage/                 # CommonUsage 公共底盘库源码
-├── Simulation/                      # .NET 8 Web 仿真器
-│   ├── Commands/                    # 可由特性自动发现的仿真动作
-│   ├── Core/                        # 仿真车辆、舵轮、时钟和世界
-│   ├── Models/                      # Web API DTO
-│   └── wwwroot/                     # 浏览器界面
-├── data_process/                    # Python 实验绘图脚本
-├── ref/                             # 两个插件共同使用的 CommonUsage.dll
-├── 测试方案.txt                     # 单车轨迹实验方案
-├── 记录.txt                         # 项目调试记录
-└── 电机记录.txt                     # 电机调试记录
+├── ref/                             # 构建生成的 CommonUsage.dll（勿手工覆盖）
+├── data_process/
+│   ├── plot_new_controller_experiment.py # 新版控制器实验六子图工具
+│   ├── 新版控制器轨迹测试处理/      # 新版绘图工具的 Python 依赖
+│   ├── 旧版控制器轨迹测试处理/      # 旧版轨迹对比、误差和响应绘图
+│   └── 电机响应处理/                # 舵轮响应快照分析
+├── docs/
+│   ├── SteeringConstraintDesign.md  # 舵轮限位设计讨论
+│   ├── chassis参考.json             # 底盘参数样例
+│   ├── 测试方案.txt                 # 单车轨迹实验方案
+│   └── 记录.txt                     # 项目调试记录
+└── output/                          # 打包输出（gitignore）
+    ├── M/                           # MedullaAdapter.dll + CommonUsage.dll
+    └── C/                           # MultiWheelC.dll + CommonUsage.dll
 ```
 
-根目录的 `ParkingRobot.sln` 当前只包含 `ClumsyPilot` 和 `MedullaAdapter`；`CommonUsage` 与 `Simulation` 需要分别构建。
+根目录 `ParkingRobot.sln` 包含 `CommonUsage`、`MedullaAdapter` 和 `MultiWheelC`，便于在 Visual Studio 中打开整仓。`Shared` 无独立项目，由 M/C 编译引入。部署打包仍以 `build-and-package.ps1` 为准。
 
 ## 开发环境与依赖
 
-- Windows 开发/实机运行环境；
+- Windows 开发 / 实机运行环境；
 - Visual Studio 2022，或支持 .NET 8.0 和 .NET Standard 2.0 的 .NET SDK；
 - Python 环境，用于可选的实验数据绘图；
-- Clumsy/Medulla 内部框架程序集，位于各项目的 `ref` 目录；
+- Clumsy / Medulla 内部框架程序集，位于各项目的 `ref` 目录；
 - 实机所需的 `mcu_serial_bridge.dll`，当前仓库中未包含该文件；
-- 能够加载 `ClumsyPilot.dll` 和 `MedullaAdapter.dll` 的匹配版本宿主程序，当前仓库中未包含宿主。
+- 能够加载 `MultiWheelC.dll` 和 `MedullaAdapter.dll` 的匹配版本宿主程序，当前仓库中未包含宿主。
 
-主要 NuGet/Python 依赖：
+主要依赖：
 
-- `ClumsyPilot`：`Newtonsoft.Json 13.0.3`、`System.Numerics.Vectors 4.6.1`；
-- `CommonUsage`：`MQTTnet 4.3.7.1207`、`Newtonsoft.Json 13.0.3` 等；
-- `data_process`：NumPy、pandas、Matplotlib、SciPy。
+- `MultiWheelC`（`netstandard2.0`）：`Newtonsoft.Json 13.0.3`、`System.Numerics.Vectors 4.6.1`；
+- `MedullaAdapter`（`net8.0`）：无 NuGet PackageReference，依赖本地 `ref` 程序集；
+- `CommonUsage`（`netstandard2.0`）：`MQTTnet 4.3.7.1207`、`Newtonsoft.Json 13.0.3` 等；
+- `data_process`：见各子目录 `requirements.txt`。
 
-## 编译
+## 编译与打包
 
-### 1. 构建 CommonUsage
+在 `MyParking` 目录执行官方脚本（默认 Debug）：
 
-修改公共底盘库后，先执行：
+```powershell
+powershell -NoProfile -ExecutionPolicy Bypass -File .\build-and-package.ps1
+```
+
+Release 构建：
+
+```powershell
+powershell -NoProfile -ExecutionPolicy Bypass -File .\build-and-package.ps1 -Configuration Release
+```
+
+脚本流程：
+
+1. 构建 `CommonUsage`，并将 `CommonUsage.dll` 复制到根目录 `ref/`；
+2. 构建 `MedullaAdapter` 和 `MultiWheelC`；
+3. 将 M/C 产物分别打包到 `output/M` 和 `output/C`，两边使用同一份 `CommonUsage.dll`。
+
+首次克隆或依赖变更后，如遇 `--no-restore` 失败，可先恢复依赖再打包：
 
 ```powershell
 dotnet restore CommonUsage-MultiVehicleSync\commonusage\CommonUsage.csproj
-dotnet build CommonUsage-MultiVehicleSync\commonusage\CommonUsage.csproj -c Debug
+dotnet restore MedullaAdapter\MedullaAdapter.csproj
+dotnet restore MultiWheelC\MultiWheelC.csproj
 ```
 
-该项目的构建目标会把生成的 `CommonUsage.dll` 复制到根目录 `ref`。
-
-### 2. 构建实车插件
-
-```powershell
-dotnet restore ParkingRobot.sln
-dotnet build ParkingRobot.sln -c Debug
-```
-
-主要输出：
+主要中间输出：
 
 ```text
-ClumsyPilot/build/Clumsy/ClumsyPilot.dll
 MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll
+MultiWheelC/build/Clumsy/MultiWheelC.dll
 ```
 
-### 3. 构建 Web 仿真器
-
-```powershell
-dotnet restore Simulation\MyParking.Simulation.csproj
-dotnet build Simulation\MyParking.Simulation.csproj -c Debug
-```
-
-## 启动 Web 仿真
-
-```powershell
-dotnet run --project Simulation\MyParking.Simulation.csproj --launch-profile http
-```
-
-浏览器访问：
-
-```text
-http://localhost:5203
-```
-
-仿真界面提供正常、左蟹行、右蟹行、自转、前进、后退、左转、右转、停止和复位动作，并可修改车辆布局及手动控制输入。主要 API 包括：
-
-- `GET /api/vehicles`
-- `GET /api/actions`
-- `GET/POST /api/configuration`
-- `POST /api/vehicles/{vehicleId}/commands/{command}`
-- `POST /api/vehicles/{vehicleId}/manual-control`
-- `POST /api/reset`
-
-`Simulation/Commands/MySimulationTests.cs` 给出了自定义仿真动作示例；为静态方法添加 `SimulationAction` 特性后，调度器会自动发现并在网页生成对应动作。
+不要直接编辑 `bin`、`obj`、`build`、`output` 中的产物，也不要手工覆盖 `ref/CommonUsage.dll`。
 
 ## 实车运行与 MCU 配置
 
-实车插件不能通过 `dotnet run` 独立启动。需要由匹配版本的 Clumsy/Medulla 宿主加载两个 DLL。宿主版本、部署目录和完整启动步骤尚未随仓库提供，待补充。
+实车插件不能通过 `dotnet run` 独立启动，需要由匹配版本的 Clumsy / Medulla 宿主分别加载：
+
+```text
+output/C/MultiWheelC.dll
+output/M/MedullaAdapter.dll
+```
+
+宿主版本、部署目录和完整启动步骤尚未随仓库提供，待补充。
 
 当前源码中的 MCU 默认参数：
 
@@ -189,29 +189,29 @@ http://localhost:5203
 | CAN | 1 路，`500000 bit/s`，重试时间 `10 ms` |
 | 串口 | 3 路，`9600 bit/s`，接收帧时间 `10 ms` |
 | 电池通信端口索引 | `3` |
-| 自转最大角速度 | `30 deg/s` |
+| 遥控自转最大角速度 | `30 deg/s` |
 | 轮速诊断目录 | `logs\wheel-speed` |
 
-当前工作区存在 `chassis.json` 底盘参数样例，但源码中尚未发现自动加载该文件的入口；实车参数仍应以宿主实际配置为准。
+`docs/chassis参考.json` 是底盘参数样例；源码中尚未发现自动加载该文件的入口，实车参数仍应以宿主实际配置为准。
 
-实机测试前必须确认端口、车号、舵轮零位与限位、速度单位、驱动方向、夹臂限位和急停链路。建议先架空驱动轮或在隔离区域低速测试，并保留独立可靠的物理急停，不能只依赖软件停车。
+实机测试前必须确认端口、车号、舵轮零位与限位、速度单位、驱动方向、夹臂限位和急停链路。建议先架空驱动轮或在隔离区域低速、短距离测试，并保留独立可靠的物理急停，不能只依赖软件停车。
 
 ## 单车测试入口
 
-`ClumsyPilot/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` 中的夹臂测试目前整段注释，不会注册到宿主。
 
 ## 实验数据分析
 
@@ -227,25 +227,38 @@ Medulla 的轮速诊断可通过 `StartWheelSpeedDiagnostic` / `StopWheelSpeedDi
 logs/wheel-speed/
 ```
 
-在自行管理的 Python 环境中安装依赖：
+### 新版控制器轨迹处理
 
 ```powershell
-python -m pip install -r data_process\requirements.txt
+python -m pip install -r data_process\新版控制器轨迹测试处理\requirements.txt
+python data_process\plot_new_controller_experiment.py "路径\实验1.csv" "路径\实验2.csv" --output-dir "路径\plots"
 ```
 
-对一份或多份轨迹 CSV 同时生成轨迹对比、跟踪误差、速度响应和角速度命令图：
+该工具为每份新版控制器 CSV 生成一张六子图总图，包含轨迹、横向/航向误差、速度和前后 GCP/四舵轮转角。省略 CSV 参数时，它只扫描 `data_process` 根目录及其 `data` 子目录。
+
+### 旧版控制器轨迹处理
 
 ```powershell
-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\旧版控制器轨迹测试处理\run_all_plots.py "路径\实验1.csv" "路径\实验2.csv" --output-dir "路径\plots"
 ```
 
-不传 CSV 路径时，脚本会查找 `data_process` 目录中的 CSV。默认重采样频率为 `20 Hz`，滤波窗口为 `0.55 s`，可通过 `--frequency` 和 `--window` 调整。
+旧版工具默认重采样频率为 `20 Hz`，滤波窗口为 `0.55 s`，可通过 `--frequency` 和 `--window` 调整。
+
+### 电机响应处理
+
+```powershell
+python -m pip install -r data_process\电机响应处理\requirements.txt
+python data_process\电机响应处理\plot_steering_response.py
+```
+
+默认读取 `logs\wheel-speed` 中最新的 `*_snapshot.csv`。详见 [`data_process/电机响应处理/README.md`](data_process/电机响应处理/README.md)。
 
 ## 尚未完成或需要继续验证
 
 - 雷达点云、轮胎识别、自动钻车、车辆释放和完整停车作业状态机；
 - 当前运动和夹臂功能的完整实车验收、故障注入及长期稳定性测试；
-- 舵轮软限位预测和自动车身重定向；`SteeringConstraintManager.cs` 当前主要是设计记录；
+- 舵轮软限位预测和自动车身重定向；当前仅有设计文档 [`docs/SteeringConstraintDesign.md`](docs/SteeringConstraintDesign.md)；
 - 自动化单元测试和持续集成；
 - 多车通信、编队、同步和安全降级；`FleetKinematics.cs` 当前仅为占位；
 - 宿主版本、插件部署目录、配置文件位置和发布流程。
@@ -253,12 +266,14 @@ python data_process\run_all_plots.py "路径\实验1.csv" "路径\实验2.csv" -
 ## 参与开发
 
 1. 当前改动优先服务于单车闭环、停车功能和跟踪质量，不提前启用多车代码；
-2. 保持上层动作、共享运动学、底层硬件协议和仿真模块的边界；
+2. 保持 `CommonUsage`、`Shared`、`MedullaAdapter`、`MultiWheelC` 的模块边界；
 3. 新增参数时注明坐标系、单位、默认值、车型和安全范围；
-4. 提交前构建受影响的项目，并记录仿真、台架或实车验证条件；
-5. 修改 `CommonUsage` 后同步更新根目录 `ref/CommonUsage.dll`；
+4. 修改相关项目后运行 `build-and-package.ps1`，并确认 M/C 部署包使用同一份 `CommonUsage.dll`；
+5. 未经明确要求，不改变速度或舵角符号、CAN ID、遥控器映射、机械限位和模式切换策略；
 6. 分支、评审和发布流程待团队补充。
 
+更细的协作约定见 [`AGENTS.md`](AGENTS.md)。
+
 ## 许可证
 
 仓库中暂未提供许可证文件。使用和分发范围请遵循公司内部规定。