优化差速转舵并增加纵向辨识测试

.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,63 +0,0 @@
----
-name: readme
-description: Create, update, audit, or synchronize repository README documentation from evidence in the codebase. Use when the user asks to write or improve a README, document setup/build/run/test workflows, explain project structure or architecture, fix stale README content, or maintain multilingual README files for any software project.
----
-
-# README维护
-
-生成或更新准确、简洁、可执行的项目README，不预设托管平台、技术栈、运行环境或文档语言。
-
-## 工作流程
-
-### 1. 调研仓库
-
-- 读取适用的`AGENTS.md`、现有README和主要设计文档。
-- 检查源码目录、项目清单、依赖文件、入口、配置、构建脚本、测试和CI配置。
-- 使用`rg --files`和针对性搜索；排除`bin`、`obj`、`build`、依赖缓存及其他生成目录。
-- 从代码和配置确认项目名称、用途、模块边界、环境要求及实际命令，不根据目录名猜测。
-
-### 2. 确定范围
-
-- 优先更新现有README，保留仍然准确的内容和仓库既有风格。
-- 默认沿用现有文件名和主要语言。
-- 只有用户明确要求或仓库已有约定时，才创建双语或多份README，并添加相对链接切换语言。
-- 删除或修正已改名、已删除、不存在或无法验证的内容。
-
-### 3. 组织内容
-
-根据项目实际情况选择必要章节，不强制套用完整模板。常用顺序为：
-
-1. 项目名称与一句话说明
-2. 当前能力与适用范围
-3. 目录或架构概览
-4. 环境与依赖
-5. 构建、运行和测试
-6. 配置与部署
-7. 已知限制或故障排查
-8. 贡献方式与许可证（仅在仓库有依据时）
-
-- 把最常用的成功路径放在前面。
-- 仅在能显著解释模块关系或执行流程时使用表格、目录树或Mermaid图。
-- 使用相对路径链接仓库内文件，避免复制大段源码或生成完整文件清单。
-
-### 4. 保证事实准确
-
-- 命令必须来自项目文件、脚本或已验证的工具链；不要编造安装、启动、部署或硬件步骤。
-- 区分“已验证可用”“根据配置推断”和“尚未验证”，不要把编译成功描述为运行或实机验证成功。
-- 不编造版本、性能指标、兼容平台、许可证、维护状态或安全保证。
-- 不在README中写入密码、令牌、内网地址、个人路径或其他敏感信息。
-- 信息不足时优先省略非必要章节；必要信息缺失时明确标注待确认内容。
-
-### 5. 验证结果
-
-- 检查README中的名称、路径、文件和命令仍真实存在。
-- 检查中英文或多语言版本的关键事实、命令和链接保持一致。
-- 对能够安全执行的核心命令进行适度验证；未执行时明确说明。
-- 查看最终差异，避免无关重写、重复章节和过度宣传。
-
-## 写作要求
-
-- 面向首次接触仓库的开发者，使用直接、具体、可操作的语言。
-- 说明“是什么、怎么用、如何验证”，避免空泛的优势描述。
-- 保持章节简短；复杂设计链接到专门文档，不把README写成完整设计说明书。
-- 代码块标注正确语言，命令应可复制，并注明必要的工作目录或前置条件。

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`中的对应小段。
+- 没有值得长期保存的信息时，不为了形式要求强行更新文档。

MedullaAdapter/DiverCartDefinition.cs

@@ -16,6 +16,14 @@ namespace MedullaAdapter
     [UseManualController(manualController = typeof(Remote))]
     public class DiverCartDefinition : MultiWheelCartDefinition
     {
+        public DiverCartDefinition()
+        {
+            // 实车配置可覆盖这些回退值；四个舵轮在MotorRoutine中统一读取它们。
+            DiffSteerKp = 0.0042f;
+            DiffSteerKi = 0f;
+            DiffSteerKd = 0f;
+        }
+
         #region 基本成员
         public MCUSerialBridge Bridge;
 
@@ -64,6 +72,38 @@ namespace MedullaAdapter
         #endregion
 
         #region  初始参数
+
+        // 旧参数仅用于兼容已有配置和历史日志，新模型逆前馈不读取它。
+        [AsInitParam(desc = "旧差速转舵目标角速度前馈增益（已停用）")]
+        public float DiffSteerRateFeedforwardGain = 0f;
+
+        [AsInitParam(desc = "旧前馈差速舵轮左右轮间距，单位mm（已停用）")]
+        public float DiffSteerWheelDistanceMillimeters = 85f;
+
+        [AsInitParam(desc = "差速转舵前馈最大速度，单位m/s")]
+        public float DiffSteerRateFeedforwardMaximumSpeed = 0.03f;
+
+        [AsInitParam(desc = "启用差速舵轮到位迟滞")]
+        public bool EnableDiffSteerSettlingHysteresis = false;
+
+        [AsInitParam(desc = "差速舵轮停止调整误差，单位deg")]
+        public float DiffSteerStopErrorDegrees = 0.5f;
+
+        [AsInitParam(desc = "差速舵轮重新启动误差，单位deg")]
+        public float DiffSteerRestartErrorDegrees = 0.8f;
+
+        [AsInitParam(desc = "差速舵轮到位确认周期数")]
+        public int DiffSteerSettlingCycles = 3;
+
+        [AsInitParam(desc = "启用差速转舵模型逆前馈")]
+        public bool EnableDiffSteerInverseFeedforward = false;
+
+        [AsInitParam(desc = "静止舵轮模型增益")]
+        public float DiffSteerPlantGain = 687.06f;
+
+        [AsInitParam(desc = "模型逆前馈滤波时间常数，单位s")]
+        public float DiffSteerInverseFeedforwardTimeConstantSeconds = 0.15f;
+
         [AsInitParam(desc = "MCU端口号")] public string MCUPort = "COM4";
         [AsInitParam(desc = "遥控器速度上限")] public float TransmitterSpeedUpperLimit = 1.0f;
         [AsInitParam(desc = "遥控器速度下限")] public float TransmitterSpeedLowerLimit = 0.0f;
@@ -100,12 +140,81 @@ 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 bool DiffSteerInStopZoneLeftFront;
+        [IOObjectMonitor(desc = "左后舵轮进入停止调整区")] public bool DiffSteerInStopZoneLeftRear;
+        [IOObjectMonitor(desc = "右前舵轮进入停止调整区")] public bool DiffSteerInStopZoneRightFront;
+        [IOObjectMonitor(desc = "右后舵轮进入停止调整区")] public bool DiffSteerInStopZoneRightRear;
+        [IOObjectMonitor(desc = "左前舵轮连续到位周期数")] public int DiffSteerSettlingCountLeftFront;
+        [IOObjectMonitor(desc = "左后舵轮连续到位周期数")] public int DiffSteerSettlingCountLeftRear;
+        [IOObjectMonitor(desc = "右前舵轮连续到位周期数")] public int DiffSteerSettlingCountRightFront;
+        [IOObjectMonitor(desc = "右后舵轮连续到位周期数")] public int DiffSteerSettlingCountRightRear;
+        [IOObjectMonitor(desc = "左前舵轮已到位")] public bool DiffSteerSettledLeftFront;
+        [IOObjectMonitor(desc = "左后舵轮已到位")] public bool DiffSteerSettledLeftRear;
+        [IOObjectMonitor(desc = "右前舵轮已到位")] public bool DiffSteerSettledRightFront;
+        [IOObjectMonitor(desc = "右后舵轮已到位")] public bool DiffSteerSettledRightRear;
         [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 float DiffSteerInverseFeedforwardRawLeftFront;
+        internal float DiffSteerInverseFeedforwardRawLeftRear;
+        internal float DiffSteerInverseFeedforwardRawRightFront;
+        internal float DiffSteerInverseFeedforwardRawRightRear;
+        internal float DiffSteerInverseFeedforwardFilteredLeftFront;
+        internal float DiffSteerInverseFeedforwardFilteredLeftRear;
+        internal float DiffSteerInverseFeedforwardFilteredRightFront;
+        internal float DiffSteerInverseFeedforwardFilteredRightRear;
+        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;
@@ -265,8 +374,6 @@ namespace MedullaAdapter
                 Math.Sign(x);
             var steeringDegrees =
                 -normalizedSteering * MaxManualTheta;
-            var frontTh = steeringDegrees;
-            var rearTh = -steeringDegrees;
             ManualMode = (int)mode;
 
             switch (mode)
@@ -274,16 +381,24 @@ namespace MedullaAdapter
                 case ManualControlMode.Normal:
                     // 普通模式统一使用车体速度命令：
                     // X向前，行驶中连续改变角速度时舵轮边转、车辆边走。
-                    // SendBodyCommand(
+                    var normalOmegaRadiansPerSecond =
-                    //     vx: speed,
+                        speed *
-                    //     vy: 0.0,
+                        Math.Tan(
-                    //     omegaRadiansPerSecond: omega,
+                            AngleMath.DegreesToRadians(
-                    //     interval);
+                                steeringDegrees)) /
-                    Chassis.SendMotion(
+                        adapter.ControlPointRadiusMeters;
+                    if (!adapter.SendBodyTwist(
+                            new Twist2D(
                                 speed,
-                                frontTh,
+                                0.0,
-                                rearTh,
+                                normalOmegaRadiansPerSecond),
-                                interval);
+                            interval))
+                    {
+                        adapter.StopImmediately();
+                        Console.WriteLine(
+                            "Normal SendMotion decomposition failed: " +
+                            adapter.LastFailureReason);
+                    }
                     break;
                 case ManualControlMode.Crab:
                     // 舵轮机械范围为[-120°,120°]。
@@ -314,13 +429,15 @@ namespace MedullaAdapter
 
                     // 将车体左侧作为虚拟阿克曼车头，并在该运动坐标系中
                     // 复用与普通模式相同的SendMotion前后控制点解算。
-                    if (!adapter.SendVirtualAckermannMotion(
+                    var crabOmegaRadiansPerSecond =
-                            motionDirectionRadians:
+                        speed *
-                                Math.PI / 2.0,
+                        Math.Tan(crabSteeringRadians) /
-                            speedMetersPerSecond:
+                        adapter.ControlPointRadiusMeters;
+                    if (!adapter.SendBodyTwist(
+                            new Twist2D(
+                                0.0,
                                 speed,
-                            steeringRadians:
+                                crabOmegaRadiansPerSecond),
-                                crabSteeringRadians,
                             interval))
                     {
                         adapter.StopImmediately();
@@ -362,13 +479,11 @@ namespace MedullaAdapter
 
                     // 普通安全版SendXYThSpeed只下发角速度，
                     // 四轮实际舵角未到位时不会开放驱动速度。
-                    if (!adapter.Send(
+                    if (!adapter.SendBodyTwist(
-                            new ChassisCommand(
-                                CarNum,
                             new Twist2D(
                                 0.0,
                                 0.0,
-                                    spinOmegaRadiansPerSecond)),
+                                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);
+                        speed,
-                    cart.LFLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
+                        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);
+                        speed,
-                    cart.LFRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
+                        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);
+                        speed,
-                    cart.RFLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
+                        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);
+                        speed,
-                    cart.RFRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
+                        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);
+                        speed,
-                    cart.LRLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
+                        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);
+                        speed,
-                    cart.LRRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
+                        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);
+                        speed,
-                    cart.RRLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
+                        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);
+                        speed,
-                    cart.RRRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
+                        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);
+                    var raw = BitConverter.ToInt32(payload, 0);
-                    cart.ActualThLeftFront = cart.ActualThLeftFront >= 16384
+                    cart.ActualThLeftFront = raw >= 16384
-                    ? (cart.ActualThLeftFront - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftFront
+                    ? (raw - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftFront
-                    : cart.ActualThLeftFront / 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);
+                    var raw = BitConverter.ToInt32(payload, 0);
-                    cart.ActualThLeftRear = cart.ActualThLeftRear >= 16384
+                    cart.ActualThLeftRear = raw >= 16384
-                        ? (cart.ActualThLeftRear - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftRear
+                        ? (raw - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftRear
-                        : cart.ActualThLeftRear / 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);
+                    var raw = BitConverter.ToInt32(payload, 0);
-                    cart.ActualThRightRear = cart.ActualThRightRear >= 16384
+                    cart.ActualThRightRear = raw >= 16384
-                        ? (cart.ActualThRightRear - 98303) / 4096f / 5f * 360 - cart.ThBiasRightRear
+                        ? (raw - 98303) / 4096f / 5f * 360 - cart.ThBiasRightRear
-                        : cart.ActualThRightRear / 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"
+  <Compile Include="..\Shared\Models\MotionModels.cs"
-           Link="Shared\Models\ChassisCommand.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,37 @@ 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 sealed class DiffSteerWheelControlState
+        {
+            public float PreviousTargetAngleDegrees;
+            public double FilteredInverseFeedforwardMetersPerSecond;
+            public bool IsInStopZone;
+            public int SettlingCycleCount;
+            public bool IsSettled;
+        }
+
         private bool _wasTransmitterControlling;
         private DateTime _lastMoveTime = DateTime.Now;
+        private bool _diffSteerFeedforwardInitialized;
+        private long _lastDiffSteerFeedforwardTimestamp;
+        private readonly DiffSteerWheelControlState _leftFrontSteerState =
+            new DiffSteerWheelControlState();
+        private readonly DiffSteerWheelControlState _leftRearSteerState =
+            new DiffSteerWheelControlState();
+        private readonly DiffSteerWheelControlState _rightFrontSteerState =
+            new DiffSteerWheelControlState();
+        private readonly DiffSteerWheelControlState _rightRearSteerState =
+            new DiffSteerWheelControlState();
         private DiverCartDefinition.ManualControlMode?
             _pendingTransmitterControlMode;
         private DateTime _pendingTransmitterControlModeSince =
@@ -67,6 +90,11 @@ namespace MedullaAdapter
             cart.ClumsyControl = CartDefinition.currentPriority == 0;
             // 计算四个舵轮PID和8个驱动电机最终速度。
             UpdateDiffSteerWheelSpeeds();
+            Interlocked.Exchange(
+                ref cart.DiffSteerControlTimestamp,
+                Stopwatch.GetTimestamp());
+            Interlocked.Increment(
+                ref cart.DiffSteerControlSequence);
             // 平滑更新硬件速度限制。
             UpdateSendSpeedLimit();
             // 更新红黄绿灯状态。
@@ -213,7 +241,9 @@ namespace MedullaAdapter
             cart.SpeedRightArm = 0;
         }
 
-        // M层单车底盘：根据四个舵轮的目标角度和实际角度修正8个驱动电机速度。
+        /// <summary>
+        /// 根据四个舵轮的目标角速度前馈和实际角度反馈修正八个驱动电机速度。
+        /// </summary>
         private void UpdateDiffSteerWheelSpeeds()
         {
             if (cart.LeftFrontPid == null ||
@@ -229,6 +259,7 @@ namespace MedullaAdapter
                 cart.SpeedLRR = 0;
                 cart.SpeedRRL = 0;
                 cart.SpeedRRR = 0;
+                ResetDiffSteerRateFeedforward();
                 return;
             }
 
@@ -272,26 +303,54 @@ namespace MedullaAdapter
                 cart.DiffSteerThresh,
                 cart.DiffSteerSpeedAcc);
 
-            // 根据实际舵角计算四条腿的差速修正量。
+            // 根据实际舵角计算四条腿的PID反馈修正量。
-            var diffLf = cart.LeftFrontPid.GetResponse(
+            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记录使用。
+            CalculateDiffSteerRateFeedforward(
-            cart.DiffSteerOutputLeftFront = diffLf;
+                out var feedforwardLf,
-            cart.DiffSteerOutputLeftRear = diffLr;
+                out var feedforwardLr,
-            cart.DiffSteerOutputRightFront = diffRf;
+                out var feedforwardRf,
-            cart.DiffSteerOutputRightRear = diffRr;
+                out var feedforwardRr,
+                out var suppressLf,
+                out var suppressLr,
+                out var suppressRf,
+                out var suppressRr);
 
-            // 左前腿：左右电机施加方向相反的PID修正量。
+            if (suppressLf) feedbackLf = 0f;
+            if (suppressLr) feedbackLr = 0f;
+            if (suppressRf) feedbackRf = 0f;
+            if (suppressRr) feedbackRr = 0f;
+
+            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;
 
@@ -308,6 +367,444 @@ namespace MedullaAdapter
             cart.SpeedRRR = cart.SpeedRightRearRight + diffRr;
         }
 
+        /// <summary>
+        /// 更新四轮独立的到位迟滞状态，并计算模型逆前馈。
+        /// </summary>
+        private void CalculateDiffSteerRateFeedforward(
+            out float leftFront,
+            out float leftRear,
+            out float rightFront,
+            out float rightRear,
+            out bool suppressLeftFront,
+            out bool suppressLeftRear,
+            out bool suppressRightFront,
+            out bool suppressRightRear)
+        {
+            leftFront = leftRear = rightFront = rightRear = 0f;
+            suppressLeftFront = suppressLeftRear = false;
+            suppressRightFront = suppressRightRear = false;
+            ResetDiffSteerCycleDiagnostics();
+
+            var currentTimestamp = Stopwatch.GetTimestamp();
+            var deltaTimeSeconds = 0.0;
+            var hasValidControlPeriod = false;
+
+            if (_diffSteerFeedforwardInitialized)
+            {
+                deltaTimeSeconds =
+                    (currentTimestamp -
+                     _lastDiffSteerFeedforwardTimestamp) /
+                    (double)Stopwatch.Frequency;
+
+                if (double.IsFinite(deltaTimeSeconds) &&
+                    deltaTimeSeconds > 0.0)
+                {
+                    cart.DiffSteerFeedforwardDeltaTimeMilliseconds =
+                        (float)(deltaTimeSeconds * 1000.0);
+                    hasValidControlPeriod =
+                        deltaTimeSeconds <=
+                        MaximumFeedforwardIntervalSeconds;
+                }
+            }
+
+            suppressLeftFront = UpdateDiffSteerSettlingState(
+                _leftFrontSteerState,
+                cart.ThLeftFront,
+                cart.ActualThLeftFront,
+                hasValidControlPeriod);
+            suppressLeftRear = UpdateDiffSteerSettlingState(
+                _leftRearSteerState,
+                cart.ThLeftRear,
+                cart.ActualThLeftRear,
+                hasValidControlPeriod);
+            suppressRightFront = UpdateDiffSteerSettlingState(
+                _rightFrontSteerState,
+                cart.ThRightFront,
+                cart.ActualThRightFront,
+                hasValidControlPeriod);
+            suppressRightRear = UpdateDiffSteerSettlingState(
+                _rightRearSteerState,
+                cart.ThRightRear,
+                cart.ActualThRightRear,
+                hasValidControlPeriod);
+
+            leftFront = CalculateDiffSteerRateFeedforward(
+                _leftFrontSteerState,
+                cart.ThLeftFront,
+                deltaTimeSeconds,
+                hasValidControlPeriod,
+                suppressLeftFront,
+                out var targetRateLf,
+                out var rawLf,
+                out var limitedLf);
+            leftRear = CalculateDiffSteerRateFeedforward(
+                _leftRearSteerState,
+                cart.ThLeftRear,
+                deltaTimeSeconds,
+                hasValidControlPeriod,
+                suppressLeftRear,
+                out var targetRateLr,
+                out var rawLr,
+                out var limitedLr);
+            rightFront = CalculateDiffSteerRateFeedforward(
+                _rightFrontSteerState,
+                cart.ThRightFront,
+                deltaTimeSeconds,
+                hasValidControlPeriod,
+                suppressRightFront,
+                out var targetRateRf,
+                out var rawRf,
+                out var limitedRf);
+            rightRear = CalculateDiffSteerRateFeedforward(
+                _rightRearSteerState,
+                cart.ThRightRear,
+                deltaTimeSeconds,
+                hasValidControlPeriod,
+                suppressRightRear,
+                out var targetRateRr,
+                out var rawRr,
+                out var limitedRr);
+
+            cart.DiffSteerTargetRateLeftFrontDegreesPerSecond = targetRateLf;
+            cart.DiffSteerTargetRateLeftRearDegreesPerSecond = targetRateLr;
+            cart.DiffSteerTargetRateRightFrontDegreesPerSecond = targetRateRf;
+            cart.DiffSteerTargetRateRightRearDegreesPerSecond = targetRateRr;
+            cart.DiffSteerInverseFeedforwardRawLeftFront = rawLf;
+            cart.DiffSteerInverseFeedforwardRawLeftRear = rawLr;
+            cart.DiffSteerInverseFeedforwardRawRightFront = rawRf;
+            cart.DiffSteerInverseFeedforwardRawRightRear = rawRr;
+            cart.DiffSteerFeedforwardLimitedLeftFront = limitedLf;
+            cart.DiffSteerFeedforwardLimitedLeftRear = limitedLr;
+            cart.DiffSteerFeedforwardLimitedRightFront = limitedRf;
+            cart.DiffSteerFeedforwardLimitedRightRear = limitedRr;
+
+            // 无论周期是否有效，都保存本周期目标，避免补算过期阶跃。
+            _leftFrontSteerState.PreviousTargetAngleDegrees =
+                cart.ThLeftFront;
+            _leftRearSteerState.PreviousTargetAngleDegrees =
+                cart.ThLeftRear;
+            _rightFrontSteerState.PreviousTargetAngleDegrees =
+                cart.ThRightFront;
+            _rightRearSteerState.PreviousTargetAngleDegrees =
+                cart.ThRightRear;
+            _lastDiffSteerFeedforwardTimestamp = currentTimestamp;
+            _diffSteerFeedforwardInitialized = true;
+            UpdateDiffSteerStateDiagnostics();
+        }
+
+        /// <summary>
+        /// 更新单个舵轮的停止区、连续确认和重新启动状态。
+        /// </summary>
+        private bool UpdateDiffSteerSettlingState(
+            DiffSteerWheelControlState state,
+            float targetAngleDegrees,
+            float actualAngleDegrees,
+            bool hasValidControlPeriod)
+        {
+            var stopErrorDegrees = cart.DiffSteerStopErrorDegrees;
+            var restartErrorDegrees = cart.DiffSteerRestartErrorDegrees;
+            var settlingCycles = cart.DiffSteerSettlingCycles;
+            var hasValidAngles =
+                IsFinite(targetAngleDegrees) &&
+                IsFinite(actualAngleDegrees);
+            var hasValidParameters =
+                IsFinite(stopErrorDegrees) &&
+                stopErrorDegrees >= 0f &&
+                IsFinite(restartErrorDegrees) &&
+                restartErrorDegrees > stopErrorDegrees &&
+                settlingCycles > 0;
+
+            if (!hasValidAngles)
+            {
+                state.IsInStopZone = false;
+                state.SettlingCycleCount = 0;
+                state.IsSettled = false;
+                ClearDiffSteerFeedforwardState(state);
+                return true;
+            }
+
+            var absoluteErrorDegrees = Math.Abs(
+                (double)targetAngleDegrees - actualAngleDegrees);
+            state.IsInStopZone =
+                hasValidParameters &&
+                absoluteErrorDegrees <= stopErrorDegrees;
+
+            if (!cart.EnableDiffSteerSettlingHysteresis ||
+                !hasValidParameters)
+            {
+                state.SettlingCycleCount = 0;
+                state.IsSettled = false;
+                return false;
+            }
+
+            if (state.IsSettled)
+            {
+                if (absoluteErrorDegrees <= restartErrorDegrees)
+                {
+                    ClearDiffSteerFeedforwardState(state);
+                    return true;
+                }
+
+                state.IsSettled = false;
+                state.SettlingCycleCount = 0;
+            }
+
+            if (!state.IsInStopZone)
+            {
+                state.SettlingCycleCount = 0;
+                return false;
+            }
+
+            ClearDiffSteerFeedforwardState(state);
+            if (hasValidControlPeriod)
+            {
+                state.SettlingCycleCount = Math.Min(
+                    state.SettlingCycleCount + 1,
+                    settlingCycles);
+                state.IsSettled =
+                    state.SettlingCycleCount >= settlingCycles;
+            }
+            else
+            {
+                state.SettlingCycleCount = 0;
+            }
+
+            return true;
+        }
+
+        /// <summary>
+        /// 将目标机械舵角变化率转换为带一阶滤波的对象逆前馈。
+        /// </summary>
+        private float CalculateDiffSteerRateFeedforward(
+            DiffSteerWheelControlState state,
+            float targetAngleDegrees,
+            double deltaTimeSeconds,
+            bool hasValidControlPeriod,
+            bool suppressOutput,
+            out float targetRateDegreesPerSecond,
+            out float rawInverseFeedforward,
+            out bool limited)
+        {
+            targetRateDegreesPerSecond = 0f;
+            rawInverseFeedforward = 0f;
+            limited = false;
+
+            if (!hasValidControlPeriod ||
+                !IsFinite(targetAngleDegrees) ||
+                !IsFinite(state.PreviousTargetAngleDegrees) ||
+                !double.IsFinite(deltaTimeSeconds) ||
+                deltaTimeSeconds <= 0.0)
+            {
+                ClearDiffSteerFeedforwardState(state);
+                return 0f;
+            }
+
+            // 机械舵角受限，必须使用直接差值而不是圆周最短角差。
+            var targetRate =
+                ((double)targetAngleDegrees -
+                 state.PreviousTargetAngleDegrees) /
+                deltaTimeSeconds;
+            if (!double.IsFinite(targetRate))
+            {
+                ClearDiffSteerFeedforwardState(state);
+                return 0f;
+            }
+
+            targetRateDegreesPerSecond = (float)targetRate;
+            if (!IsFinite(targetRateDegreesPerSecond))
+            {
+                targetRateDegreesPerSecond = 0f;
+                ClearDiffSteerFeedforwardState(state);
+                return 0f;
+            }
+
+            var plantGain = cart.DiffSteerPlantGain;
+            var timeConstantSeconds =
+                cart.DiffSteerInverseFeedforwardTimeConstantSeconds;
+            var maximumSpeed =
+                cart.DiffSteerRateFeedforwardMaximumSpeed;
+
+            if (!IsFinite(plantGain) || plantGain <= 0f ||
+                !IsFinite(timeConstantSeconds) ||
+                timeConstantSeconds <= 0f ||
+                !IsFinite(maximumSpeed) || maximumSpeed <= 0f)
+            {
+                ClearDiffSteerFeedforwardState(state);
+                return 0f;
+            }
+
+            var inverseFeedforward = targetRate / plantGain;
+            if (!double.IsFinite(inverseFeedforward))
+            {
+                ClearDiffSteerFeedforwardState(state);
+                return 0f;
+            }
+
+            rawInverseFeedforward = (float)inverseFeedforward;
+            if (!IsFinite(rawInverseFeedforward))
+            {
+                rawInverseFeedforward = 0f;
+                ClearDiffSteerFeedforwardState(state);
+                return 0f;
+            }
+
+            if (!cart.EnableDiffSteerInverseFeedforward ||
+                suppressOutput)
+            {
+                ClearDiffSteerFeedforwardState(state);
+                return 0f;
+            }
+
+            var alpha = 1.0 - Math.Exp(
+                -deltaTimeSeconds / timeConstantSeconds);
+            var filteredFeedforward =
+                state.FilteredInverseFeedforwardMetersPerSecond +
+                alpha *
+                (inverseFeedforward -
+                 state.FilteredInverseFeedforwardMetersPerSecond);
+            if (!double.IsFinite(alpha) ||
+                !double.IsFinite(filteredFeedforward))
+            {
+                ClearDiffSteerFeedforwardState(state);
+                return 0f;
+            }
+
+            state.FilteredInverseFeedforwardMetersPerSecond =
+                filteredFeedforward;
+
+            var limitedSpeed = (float)Math.Clamp(
+                filteredFeedforward,
+                -maximumSpeed,
+                maximumSpeed);
+            limited = Math.Abs(
+                filteredFeedforward - limitedSpeed) > 1e-9;
+            return limitedSpeed;
+        }
+
+        private void ResetDiffSteerCycleDiagnostics()
+        {
+            cart.DiffSteerTargetRateLeftFrontDegreesPerSecond = 0f;
+            cart.DiffSteerTargetRateLeftRearDegreesPerSecond = 0f;
+            cart.DiffSteerTargetRateRightFrontDegreesPerSecond = 0f;
+            cart.DiffSteerTargetRateRightRearDegreesPerSecond = 0f;
+            cart.DiffSteerFeedforwardDeltaTimeMilliseconds = 0f;
+            cart.DiffSteerInverseFeedforwardRawLeftFront = 0f;
+            cart.DiffSteerInverseFeedforwardRawLeftRear = 0f;
+            cart.DiffSteerInverseFeedforwardRawRightFront = 0f;
+            cart.DiffSteerInverseFeedforwardRawRightRear = 0f;
+            cart.DiffSteerFeedforwardLimitedLeftFront = false;
+            cart.DiffSteerFeedforwardLimitedLeftRear = false;
+            cart.DiffSteerFeedforwardLimitedRightFront = false;
+            cart.DiffSteerFeedforwardLimitedRightRear = false;
+        }
+
+        /// <summary>
+        /// 将四轮独立控制状态复制到M层监控和CSV数据源。
+        /// </summary>
+        private void UpdateDiffSteerStateDiagnostics()
+        {
+            cart.DiffSteerInStopZoneLeftFront =
+                _leftFrontSteerState.IsInStopZone;
+            cart.DiffSteerInStopZoneLeftRear =
+                _leftRearSteerState.IsInStopZone;
+            cart.DiffSteerInStopZoneRightFront =
+                _rightFrontSteerState.IsInStopZone;
+            cart.DiffSteerInStopZoneRightRear =
+                _rightRearSteerState.IsInStopZone;
+            cart.DiffSteerSettlingCountLeftFront =
+                _leftFrontSteerState.SettlingCycleCount;
+            cart.DiffSteerSettlingCountLeftRear =
+                _leftRearSteerState.SettlingCycleCount;
+            cart.DiffSteerSettlingCountRightFront =
+                _rightFrontSteerState.SettlingCycleCount;
+            cart.DiffSteerSettlingCountRightRear =
+                _rightRearSteerState.SettlingCycleCount;
+            cart.DiffSteerSettledLeftFront =
+                _leftFrontSteerState.IsSettled;
+            cart.DiffSteerSettledLeftRear =
+                _leftRearSteerState.IsSettled;
+            cart.DiffSteerSettledRightFront =
+                _rightFrontSteerState.IsSettled;
+            cart.DiffSteerSettledRightRear =
+                _rightRearSteerState.IsSettled;
+            cart.DiffSteerInverseFeedforwardFilteredLeftFront =
+                (float)_leftFrontSteerState
+                    .FilteredInverseFeedforwardMetersPerSecond;
+            cart.DiffSteerInverseFeedforwardFilteredLeftRear =
+                (float)_leftRearSteerState
+                    .FilteredInverseFeedforwardMetersPerSecond;
+            cart.DiffSteerInverseFeedforwardFilteredRightFront =
+                (float)_rightFrontSteerState
+                    .FilteredInverseFeedforwardMetersPerSecond;
+            cart.DiffSteerInverseFeedforwardFilteredRightRear =
+                (float)_rightRearSteerState
+                    .FilteredInverseFeedforwardMetersPerSecond;
+        }
+
+        private static void ClearDiffSteerFeedforwardState(
+            DiffSteerWheelControlState state)
+        {
+            state.FilteredInverseFeedforwardMetersPerSecond = 0.0;
+        }
+
+        /// <summary>
+        /// 清除差速转舵前馈历史和监控输出，避免恢复控制时使用过期目标角。
+        /// </summary>
+        private void ResetDiffSteerRateFeedforward()
+        {
+            _diffSteerFeedforwardInitialized = false;
+            _lastDiffSteerFeedforwardTimestamp = 0;
+            ResetDiffSteerWheelState(_leftFrontSteerState);
+            ResetDiffSteerWheelState(_leftRearSteerState);
+            ResetDiffSteerWheelState(_rightFrontSteerState);
+            ResetDiffSteerWheelState(_rightRearSteerState);
+            cart.DiffSteerOutputLeftFront = 0f;
+            cart.DiffSteerOutputLeftRear = 0f;
+            cart.DiffSteerOutputRightFront = 0f;
+            cart.DiffSteerOutputRightRear = 0f;
+            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.DiffSteerInverseFeedforwardRawLeftFront = 0f;
+            cart.DiffSteerInverseFeedforwardRawLeftRear = 0f;
+            cart.DiffSteerInverseFeedforwardRawRightFront = 0f;
+            cart.DiffSteerInverseFeedforwardRawRightRear = 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;
+            UpdateDiffSteerStateDiagnostics();
+        }
+
+        private static void ResetDiffSteerWheelState(
+            DiffSteerWheelControlState state)
+        {
+            state.PreviousTargetAngleDegrees = 0f;
+            state.FilteredInverseFeedforwardMetersPerSecond = 0.0;
+            state.IsInStopZone = false;
+            state.SettlingCycleCount = 0;
+            state.IsSettled = false;
+        }
+
+        /// <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,50 @@ 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," +
+                    "EnableDiffSteerSettlingHysteresis,DiffSteerStopErrorDegrees,DiffSteerRestartErrorDegrees,DiffSteerSettlingCycles," +
+                    "EnableDiffSteerInverseFeedforward,DiffSteerPlantGain,DiffSteerInverseFeedforwardTimeConstantSeconds," +
+                    "FeedforwardDeltaTimeMs,DiffSteerDeltaTimeSeconds," +
+                    "TargetRateThLeftFrontDegreesPerSecond,TargetRateThLeftRearDegreesPerSecond," +
+                    "TargetRateThRightFrontDegreesPerSecond,TargetRateThRightRearDegreesPerSecond," +
+                    "InverseFeedforwardRawLeftFront,InverseFeedforwardRawLeftRear,InverseFeedforwardRawRightFront,InverseFeedforwardRawRightRear," +
+                    "InverseFeedforwardFilteredLeftFront,InverseFeedforwardFilteredLeftRear,InverseFeedforwardFilteredRightFront,InverseFeedforwardFilteredRightRear," +
+                    "FeedforwardLimitedLeftFront,FeedforwardLimitedLeftRear,FeedforwardLimitedRightFront,FeedforwardLimitedRightRear," +
+                    "InStopZoneLeftFront,InStopZoneLeftRear,InStopZoneRightFront,InStopZoneRightRear," +
+                    "SettlingCountLeftFront,SettlingCountLeftRear,SettlingCountRightFront,SettlingCountRightRear," +
+                    "SettledLeftFront,SettledLeftRear,SettledRightFront,SettledRightRear," +
                     "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 +132,11 @@ namespace MedullaAdapter
 
                 _queuedRecordCount = 0;
                 _receiveSequence = 0;
+                _snapshotSequence = 0;
                 _droppedRecordCount = 0;
                 _lastSnapshotMilliseconds =
                     double.NegativeInfinity;
+                _startTimestamp = Stopwatch.GetTimestamp();
                 _stopwatch = Stopwatch.StartNew();
                 _isRunning = true;
 
@@ -154,13 +189,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 +208,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 +269,9 @@ namespace MedullaAdapter
             if (!_isRunning || cart == null)
                 return;
 
+            var snapshotTimestamp = Stopwatch.GetTimestamp();
             var elapsedMilliseconds =
-                _stopwatch.Elapsed.TotalMilliseconds;
+                GetElapsedMilliseconds(snapshotTimestamp);
 
             if (elapsedMilliseconds -
                 _lastSnapshotMilliseconds <
@@ -202,14 +283,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 +356,61 @@ namespace MedullaAdapter
                 Format(cart.DiffSteerDeadZone),
                 Format(cart.DiffSteerThresh),
                 Format(cart.DiffSteerSpeedAcc),
+                Format(cart.DiffSteerRateFeedforwardGain),
+                Format(cart.DiffSteerWheelDistanceMillimeters),
+                Format(cart.DiffSteerRateFeedforwardMaximumSpeed),
+                FormatBoolean(cart.EnableDiffSteerSettlingHysteresis),
+                Format(cart.DiffSteerStopErrorDegrees),
+                Format(cart.DiffSteerRestartErrorDegrees),
+                Format(cart.DiffSteerSettlingCycles),
+                FormatBoolean(cart.EnableDiffSteerInverseFeedforward),
+                Format(cart.DiffSteerPlantGain),
+                Format(
+                    cart.DiffSteerInverseFeedforwardTimeConstantSeconds),
+                Format(cart.DiffSteerFeedforwardDeltaTimeMilliseconds),
+                Format(
+                    cart.DiffSteerFeedforwardDeltaTimeMilliseconds /
+                    1000f),
+                Format(cart.DiffSteerTargetRateLeftFrontDegreesPerSecond),
+                Format(cart.DiffSteerTargetRateLeftRearDegreesPerSecond),
+                Format(cart.DiffSteerTargetRateRightFrontDegreesPerSecond),
+                Format(cart.DiffSteerTargetRateRightRearDegreesPerSecond),
+                Format(cart.DiffSteerInverseFeedforwardRawLeftFront),
+                Format(cart.DiffSteerInverseFeedforwardRawLeftRear),
+                Format(cart.DiffSteerInverseFeedforwardRawRightFront),
+                Format(cart.DiffSteerInverseFeedforwardRawRightRear),
+                Format(cart.DiffSteerInverseFeedforwardFilteredLeftFront),
+                Format(cart.DiffSteerInverseFeedforwardFilteredLeftRear),
+                Format(cart.DiffSteerInverseFeedforwardFilteredRightFront),
+                Format(cart.DiffSteerInverseFeedforwardFilteredRightRear),
+                FormatBoolean(cart.DiffSteerFeedforwardLimitedLeftFront),
+                FormatBoolean(cart.DiffSteerFeedforwardLimitedLeftRear),
+                FormatBoolean(cart.DiffSteerFeedforwardLimitedRightFront),
+                FormatBoolean(cart.DiffSteerFeedforwardLimitedRightRear),
+                FormatBoolean(cart.DiffSteerInStopZoneLeftFront),
+                FormatBoolean(cart.DiffSteerInStopZoneLeftRear),
+                FormatBoolean(cart.DiffSteerInStopZoneRightFront),
+                FormatBoolean(cart.DiffSteerInStopZoneRightRear),
+                Format(cart.DiffSteerSettlingCountLeftFront),
+                Format(cart.DiffSteerSettlingCountLeftRear),
+                Format(cart.DiffSteerSettlingCountRightFront),
+                Format(cart.DiffSteerSettlingCountRightRear),
+                FormatBoolean(cart.DiffSteerSettledLeftFront),
+                FormatBoolean(cart.DiffSteerSettledLeftRear),
+                FormatBoolean(cart.DiffSteerSettledRightFront),
+                FormatBoolean(cart.DiffSteerSettledRightRear),
                 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 +427,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 +468,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 +495,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 +635,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/PathTrackingContext.cs

@@ -1,11 +1,11 @@
 using System;
-using MultiWheelC.StateEstimation;
 using MultiWheelC.Trajectory;
+using MyParking.Shared;
 
 namespace MultiWheelC.Control.Abstractions
 {
     /// <summary>
-    /// 保存一次轨迹跟踪控制周期使用的车辆状态、轨迹投影和真实时间间隔。
+    /// 保存一次轨迹跟踪控制周期使用的刚体速度、轨迹投影和真实时间间隔。
     /// </summary>
     public readonly struct PathTrackingContext
     {
@@ -13,29 +13,48 @@ namespace MultiWheelC.Control.Abstractions
         /// 创建横向和纵向控制器共享的只读控制输入快照。
         /// </summary>
         public PathTrackingContext(
-            VehicleState vehicleState,
+            Twist2D actualTwistInBody,
+            bool hasValidVelocityEstimate,
             TrajectoryProjection projection,
-            double referenceSpeedMetersPerSecond,
+            double controlReferenceSpeedMetersPerSecond,
-            double deltaTimeSeconds)
+            double feedforwardCurvaturePerMeter,
+            double deltaTimeSeconds,
+            double motionDirectionInBodyRadians = 0.0)
         {
             EnsureFinite(
-                referenceSpeedMetersPerSecond,
+                controlReferenceSpeedMetersPerSecond,
-                nameof(referenceSpeedMetersPerSecond));
+                nameof(controlReferenceSpeedMetersPerSecond));
+            EnsureFinite(
+                feedforwardCurvaturePerMeter,
+                nameof(feedforwardCurvaturePerMeter));
             EnsureFinitePositive(
                 deltaTimeSeconds,
                 nameof(deltaTimeSeconds));
+            EnsureFinite(
+                motionDirectionInBodyRadians,
+                nameof(motionDirectionInBodyRadians));
+            NumericGuard.EnsureFinite(
+                actualTwistInBody,
+                nameof(actualTwistInBody));
 
-            VehicleState = vehicleState;
+            ActualTwistInBody = actualTwistInBody;
+            HasValidVelocityEstimate =
+                hasValidVelocityEstimate;
             Projection = projection;
-            ReferenceSpeedMetersPerSecond =
+            ControlReferenceSpeedMetersPerSecond =
-                referenceSpeedMetersPerSecond;
+                controlReferenceSpeedMetersPerSecond;
+            FeedforwardCurvaturePerMeter =
+                feedforwardCurvaturePerMeter;
             DeltaTimeSeconds = deltaTimeSeconds;
+            MotionDirectionInBodyRadians =
+                AngleMath.NormalizeRadians(
+                    motionDirectionInBodyRadians);
         }
 
         /// <summary>
-        /// 获取本周期经过校验的实际车辆位姿和速度状态。
+        /// 获取受控刚体坐标系下的实际速度。
         /// </summary>
-        public VehicleState VehicleState { get; }
+        public Twist2D ActualTwistInBody { get; }
 
         /// <summary>
         /// 获取实际车体中心投影到参考轨迹后得到的参考状态和跟踪误差。
@@ -48,26 +67,38 @@ namespace MultiWheelC.Control.Abstractions
         public double DeltaTimeSeconds { get; }
 
         /// <summary>
-        /// 获取轨迹投影点要求的有符号参考速度，单位为m/s。
+        /// 获取轨迹原始速度经过起步释放和制动预瞄处理后，本控制周期实际使用的有符号参考速度，单位为m/s。
         /// </summary>
-        public double ReferenceSpeedMetersPerSecond { get; }
+        public double ControlReferenceSpeedMetersPerSecond { get; }
 
         /// <summary>
-        /// 获取车辆在车体X轴方向上的实际纵向速度，单位为m/s。
+        /// 获取车辆沿当前运动坐标系X轴方向的实际纵向速度，单位为m/s。
         /// </summary>
         public double ActualLongitudinalSpeedMetersPerSecond =>
-            VehicleState.TwistInBody
+            Math.Cos(MotionDirectionInBodyRadians) *
-                .VxMetersPerSecond;
+                ActualTwistInBody.VxMetersPerSecond +
+            Math.Sin(MotionDirectionInBodyRadians) *
+                ActualTwistInBody.VyMetersPerSecond;
 
         /// <summary>
-        /// 获取轨迹投影点的参考曲率，单位为1/m，左转为正。
+        /// 获取当前运动坐标系X轴在车体系中的方向，单位为rad。
+        /// </summary>
+        public double MotionDirectionInBodyRadians { get; }
+
+        /// <summary>
+        /// 获取沿轨迹执行点序定义的参考曲率，单位为1/m，左弯为正。
         /// </summary>
         public double ReferenceCurvaturePerMeter =>
             Projection.ReferencePoint
                 .CurvaturePerMeter;
 
         /// <summary>
-        /// 获取参考轨迹相对车辆的有符号横向误差，单位为m，轨迹在车辆左侧时为正。
+        /// 获取沿轨迹点序适量预瞄后专供几何前馈使用的参考曲率，单位为1/m，左弯为正。
+        /// </summary>
+        public double FeedforwardCurvaturePerMeter { get; }
+
+        /// <summary>
+        /// 获取相对轨迹执行点序的有符号横向误差，单位为m，参考轨迹位于执行方向左侧时为正。
         /// </summary>
         public double LateralErrorMeters =>
             Projection.LateralErrorMeters;
@@ -85,10 +116,9 @@ namespace MultiWheelC.Control.Abstractions
             Projection.RemainingDistanceMeters;
 
         /// <summary>
-        /// 获取实际速度是否已经由至少两个连续有效定位样本估算得到。
+        /// 获取本周期实际速度估计是否可供闭环控制使用。
         /// </summary>
-        public bool HasValidVelocityEstimate =>
+        public bool HasValidVelocityEstimate { get; }
-            VehicleState.HasValidVelocityEstimate;
 
         /// <summary>
         /// 检查控制周期是否为正有限值。

MultiWheelC/Control/Allocation/GcpCommandAllocator.cs

@@ -1,6 +1,7 @@
 using System;
 using MultiWheelC.Control.Abstractions;
-
+using MyParking.Shared;
+// 限制目标角度的最大绝对值，例如不能超过60°。
 namespace MultiWheelC.Control.Allocation
 {
     /// <summary>
@@ -13,7 +14,7 @@ namespace MultiWheelC.Control.Allocation
         /// </summary>
         public GcpCommandAllocator(double maximumGcpAngleRadians)
         {
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 maximumGcpAngleRadians,
                 nameof(maximumGcpAngleRadians));
 
@@ -39,7 +40,7 @@ namespace MultiWheelC.Control.Allocation
             double speedMetersPerSecond,
             LateralControlCommand lateralCommand)
         {
-            EnsureFinite(
+            NumericGuard.EnsureFinite(
                 speedMetersPerSecond,
                 nameof(speedMetersPerSecond));
 
@@ -68,37 +69,5 @@ namespace MultiWheelC.Control.Allocation
                 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,
-                    "GCP分配参数必须是正有限值。");
-            }
-        }
-
-        /// <summary>
-        /// 检查参数或命令是否为有限值。
-        /// </summary>
-        private static void EnsureFinite(
-            double value,
-            string parameterName)
-        {
-            if (double.IsNaN(value) ||
-                double.IsInfinity(value))
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "GCP分配参数和命令必须是有限值。");
-            }
-        }
     }
 }

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

@@ -1,4 +1,4 @@
-using System;
+using MyParking.Shared;
 
 namespace MultiWheelC.Control.Allocation
 {
@@ -15,13 +15,13 @@ namespace MultiWheelC.Control.Allocation
             double frontAngleRadians,
             double rearAngleRadians)
         {
-            EnsureFinite(
+            NumericGuard.EnsureFinite(
                 speedMetersPerSecond,
                 nameof(speedMetersPerSecond));
-            EnsureFinite(
+            NumericGuard.EnsureFinite(
                 frontAngleRadians,
                 nameof(frontAngleRadians));
-            EnsureFinite(
+            NumericGuard.EnsureFinite(
                 rearAngleRadians,
                 nameof(rearAngleRadians));
 
@@ -48,20 +48,5 @@ namespace MultiWheelC.Control.Allocation
         /// </summary>
         public double RearAngleRadians { get; }
 
-        /// <summary>
-        /// 检查底盘中间命令是否为有限值。
-        /// </summary>
-        private static void EnsureFinite(
-            double value,
-            string parameterName)
-        {
-            if (double.IsNaN(value) ||
-                double.IsInfinity(value))
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "GCP运动命令必须由有限值组成。");
-            }
-        }
     }
 }

MultiWheelC/Control/Execution/GcpCommandExecutor.cs

@@ -13,6 +13,7 @@ namespace MultiWheelC.Control.Execution
             1e-6;
 
         private readonly MultiWheelChassisAdapter _chassisAdapter;
+        private readonly double _motionDirectionInBodyRadians;
         private double _lastFrontAngleRadians;
         private double _lastRearAngleRadians;
 
@@ -22,7 +23,8 @@ namespace MultiWheelC.Control.Execution
         public GcpCommandExecutor(
             MultiWheelChassisAdapter chassisAdapter,
             double maximumGcpAngleRateRadiansPerSecond =
-                10.0 * Math.PI / 180.0)
+                10.0 * Math.PI / 180.0,
+            double motionDirectionInBodyRadians = 0.0)
         {
             _chassisAdapter = chassisAdapter ??
                 throw new ArgumentNullException(
@@ -30,9 +32,15 @@ namespace MultiWheelC.Control.Execution
             EnsureFinitePositive(
                 maximumGcpAngleRateRadiansPerSecond,
                 nameof(maximumGcpAngleRateRadiansPerSecond));
+            NumericGuard.EnsureFinite(
+                motionDirectionInBodyRadians,
+                nameof(motionDirectionInBodyRadians));
 
             MaximumGcpAngleRateRadiansPerSecond =
                 maximumGcpAngleRateRadiansPerSecond;
+            _motionDirectionInBodyRadians =
+                AngleMath.NormalizeRadians(
+                    motionDirectionInBodyRadians);
         }
 
         /// <summary>
@@ -103,10 +111,19 @@ namespace MultiWheelC.Control.Execution
                 _lastRearAngleRadians);
             LastSentCommand = limitedCommand;
 
-            var success = _chassisAdapter.SendGcpMotion(
+            var motionFrameTwist =
-                limitedCommand.SpeedMetersPerSecond,
+                GcpKinematics.ToBodyTwist(
-                limitedCommand.FrontAngleRadians,
+                    limitedCommand,
-                limitedCommand.RearAngleRadians,
+                    _chassisAdapter.ControlPointRadiusMeters);
+            var bodyTwist =
+                FrameTransform2D.TransformTwistAtSamePoint(
+                    new Pose2D(
+                        0.0,
+                        0.0,
+                        _motionDirectionInBodyRadians),
+                    motionFrameTwist);
+            var success = _chassisAdapter.SendBodyTwist(
+                bodyTwist,
                 TimeSpan.FromSeconds(deltaTimeSeconds));
 
             LastFailureReason = success

MultiWheelC/Control/Execution/ParkingGeometricController.cs

@@ -1,4 +1,5 @@
 using System;
+using System.Diagnostics;
 using MultiWheelC.Control.Abstractions;
 using MultiWheelC.Control.Allocation;
 using MultiWheelC.StateEstimation;
@@ -7,9 +8,7 @@ using MyParking.Shared;
 
 namespace MultiWheelC.Control.Execution
 {
-    /// <summary>
+    // 单周期轨迹控制结果。
-    /// 表示新版停车机器人单周期轨迹控制的执行结果。
-    /// </summary>
     public enum ParkingControlCycleResult
     {
         Inactive = 0,
@@ -19,533 +18,418 @@ namespace MultiWheelC.Control.Execution
         Faulted = 4
     }
 
-    /// <summary>
+    // 单周期各阶段耗时及定位帧更新状态，用于区分控制计算与底层通信延迟。
-    /// 组织状态读取、轨迹投影、横纵向控制、GCP分配和底盘命令执行。
+    public readonly struct ParkingControlCycleTiming
-    /// </summary>
+    {
+        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 const double ZeroReferenceSpeedToleranceMetersPerSecond =
-            1e-6;
-        private const double StartupRegionMeters = 0.02;
-        private const double StartupPreviewDistanceMeters = 0.05;
-        private const double MaximumStartupSpeedMetersPerSecond = 0.08;
-
         private readonly IVehicleStateProvider _stateProvider;
-        private readonly ILateralController _lateralController;
-        private readonly ILongitudinalController _longitudinalController;
-        private readonly GcpCommandAllocator _gcpAllocator;
         private readonly GcpCommandExecutor _commandExecutor;
+        private readonly PathTrackingCore _trackingCore;
 
-        private Trajectory2D _trajectory;
+        private long _cycleIndex;
+        private bool _hasPreviousStateTimestamp;
+        private double _previousStateTimestampSeconds;
 
-        /// <summary>
+        // 创建具有终点判定、轨迹偏离保护和曲率前馈预瞄的单车轨迹控制器。
-        /// 创建具有终点判定和轨迹偏离保护的单车轨迹控制器。
-        /// </summary>
         public ParkingGeometricController(
             IVehicleStateProvider stateProvider,
             ILateralController lateralController,
             ILongitudinalController longitudinalController,
             GcpCommandAllocator gcpAllocator,
             GcpCommandExecutor commandExecutor,
-            double finishDistanceMeters = 0.03,
+            double finishDistanceMeters = 0.04,
             double finishSpeedMetersPerSecond = 0.02,
             double finishHeadingToleranceRadians =
                 3.0 * Math.PI / 180.0,
-            double maximumDistanceToTrajectoryMeters = 0.50)
+            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));
-            _lateralController = lateralController ??
-                throw new ArgumentNullException(
-                    nameof(lateralController));
-            _longitudinalController = longitudinalController ??
-                throw new ArgumentNullException(
-                    nameof(longitudinalController));
-            _gcpAllocator = gcpAllocator ??
-                throw new ArgumentNullException(
-                    nameof(gcpAllocator));
             _commandExecutor = commandExecutor ??
                 throw new ArgumentNullException(
                     nameof(commandExecutor));
-
+            _trackingCore = new PathTrackingCore(
-            EnsureFinitePositive(
+                lateralController,
+                longitudinalController,
+                gcpAllocator,
                 finishDistanceMeters,
-                nameof(finishDistanceMeters));
-            EnsureFiniteNonNegative(
                 finishSpeedMetersPerSecond,
-                nameof(finishSpeedMetersPerSecond));
-            EnsureFinitePositive(
                 finishHeadingToleranceRadians,
-                nameof(finishHeadingToleranceRadians));
-            EnsureFinitePositive(
                 maximumDistanceToTrajectoryMeters,
-                nameof(maximumDistanceToTrajectoryMeters));
+                terminalBrakingPreviewMeters,
-
+                terminalApproachDistanceMeters,
-            FinishDistanceMeters = finishDistanceMeters;
+                terminalApproachGainPerSecond,
-            FinishSpeedMetersPerSecond =
+                maximumTerminalApproachSpeedMetersPerSecond,
-                finishSpeedMetersPerSecond;
+                curvaturePreviewSeconds,
-            FinishHeadingToleranceRadians =
+                maximumCurvaturePreviewMeters,
-                finishHeadingToleranceRadians;
+                motionDirectionInBodyRadians);
-            MaximumDistanceToTrajectoryMeters =
-                maximumDistanceToTrajectoryMeters;
         }
 
-        /// <summary>
+        // 以下控制参数由公共轨迹核心统一持有。
-        /// 获取终点位置和剩余弧长允许的误差，单位为m。
+        public double FinishDistanceMeters =>
-        /// </summary>
+            _trackingCore.FinishDistanceMeters;
-        public double FinishDistanceMeters { get; }
 
-        /// <summary>
+        public double FinishSpeedMetersPerSecond =>
-        /// 获取判定轨迹执行完成时允许的最大实际线速度，单位为m/s。
+            _trackingCore.FinishSpeedMetersPerSecond;
-        /// </summary>
-        public double FinishSpeedMetersPerSecond { get; }
 
-        /// <summary>
+        public double FinishHeadingToleranceRadians =>
-        /// 获取判定轨迹完成时允许的最大终点航向误差，单位为rad。
+            _trackingCore.FinishHeadingToleranceRadians;
-        /// </summary>
-        public double FinishHeadingToleranceRadians { get; }
 
-        /// <summary>
+        public double MaximumDistanceToTrajectoryMeters =>
-        /// 获取允许车辆偏离参考轨迹的最大距离，单位为m。
+            _trackingCore.MaximumDistanceToTrajectoryMeters;
-        /// </summary>
-        public double MaximumDistanceToTrajectoryMeters { get; }
 
-        /// <summary>
+        public double TerminalBrakingPreviewMeters =>
-        /// 获取控制器当前是否持有并正在执行一条轨迹。
+            _trackingCore.TerminalBrakingPreviewMeters;
-        /// </summary>
-        public bool IsActive { get; private set; }
 
-        /// <summary>
+        public double TerminalApproachDistanceMeters =>
-        /// 获取最近一次轨迹是否已经满足终点完成条件。
+            _trackingCore.TerminalApproachDistanceMeters;
-        /// </summary>
-        public bool IsCompleted { get; private set; }
 
-        /// <summary>
+        public double TerminalApproachGainPerSecond =>
-        /// 获取最近一次控制失败原因，正常时为空字符串。
+            _trackingCore.TerminalApproachGainPerSecond;
-        /// </summary>
-        public string LastFailureReason { get; private set; } =
-            string.Empty;
 
-        /// <summary>
+        public double MaximumTerminalApproachSpeedMetersPerSecond =>
-        /// 获取最近一次控制异常，正常时为空。
+            _trackingCore.MaximumTerminalApproachSpeedMetersPerSecond;
-        /// </summary>
-        public Exception LastException { get; private set; }
 
-        /// <summary>
+        public double CurvaturePreviewSeconds =>
-        /// 获取最近一次有效车辆状态。
+            _trackingCore.CurvaturePreviewSeconds;
-        /// </summary>
+
+        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; }
 
-        /// <summary>
+        public TrajectoryProjection? LastProjection =>
-        /// 获取最近一次车体中心到参考轨迹的投影结果。
+            _trackingCore.LastProjection;
-        /// </summary>
-        public TrajectoryProjection? LastProjection { get; private set; }
 
-        /// <summary>
+        public GcpMotionCommand? LastRequestedCommand =>
-        /// 获取最近一次发送或准备发送的GCP运动命令。
+            _trackingCore.LastRequestedCommand;
-        /// </summary>
+
+        // 经过GCP角速度限制后实际发送给底盘的最近一次命令。
         public GcpMotionCommand? LastCommand { get; private set; }
 
-        /// <summary>
+        public double? LastControlReferenceSpeedMetersPerSecond =>
-        /// 获取最近控制周期实际交给纵向控制器的参考速度，单位为m/s。
+            _trackingCore.LastControlReferenceSpeedMetersPerSecond;
-        /// </summary>
-        public double? LastReferenceSpeedMetersPerSecond { get; private set; }
 
-        /// <summary>
+        public double? LastCurvaturePreviewDistanceMeters =>
-        /// 停止当前底盘并从起点开始执行指定二维轨迹。
+            _trackingCore.LastCurvaturePreviewDistanceMeters;
-        /// </summary>
+
+        public double? LastFeedforwardCurvaturePerMeter =>
+            _trackingCore.LastFeedforwardCurvaturePerMeter;
+
+        public ParkingControlCycleTiming? LastCycleTiming { get; private set; }
+
+        // 先停车，再从轨迹起点重置公共控制核心和单车执行诊断。
         public void Start(Trajectory2D trajectory)
         {
-            if (trajectory == null)
+            _commandExecutor.Stop();
-            {
+            _trackingCore.Start(trajectory);
-                throw new ArgumentNullException(
+            ClearExecutionDiagnostics();
-                    nameof(trajectory));
         }
 
-            StopAndResetControllers();
+        // 读取车辆状态、调用公共核心并发送一次真实底盘命令。
-            _trajectory = trajectory;
-            IsActive = true;
-            IsCompleted = false;
-            ClearDiagnostics();
-        }
-
-        /// <summary>
-        /// 读取本周期车辆状态并执行一次完整的轨迹跟踪控制计算。
-        /// </summary>
         public ParkingControlCycleResult ExecuteCycle(
             double deltaTimeSeconds)
         {
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 deltaTimeSeconds,
                 nameof(deltaTimeSeconds));
 
-            if (!IsActive || _trajectory == null)
+            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
             {
-                if (!_stateProvider.TryGetState(
+                var stateReadStartTimestamp =
-                        out var vehicleState))
+                    Stopwatch.GetTimestamp();
+                bool stateAvailable;
+                VehicleState vehicleState;
+                try
+                {
+                    stateAvailable =
+                        _stateProvider.TryGetState(
+                            out vehicleState);
+                }
+                finally
+                {
+                    stateReadMilliseconds =
+                        GetElapsedMilliseconds(
+                            stateReadStartTimestamp);
+                }
+
+                if (!stateAvailable)
+                {
+                    var stopStartTimestamp =
+                        Stopwatch.GetTimestamp();
+                    try
                     {
                         StopForUnavailableState();
-                    return ParkingControlCycleResult
+                    }
+                    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 projection = TrajectoryProjector.Project(
+                var output = _trackingCore.Compute(
-                    _trajectory,
+                    vehicleState.PoseInWorld,
-                    vehicleState.PoseInWorld);
+                    vehicleState.TwistInBody,
-                LastProjection = projection;
+                    vehicleState.HasValidVelocityEstimate,
-
-                if (projection.DistanceToTrajectoryMeters >
-                    MaximumDistanceToTrajectoryMeters)
-                {
-                    return EnterFault(
-                        "车辆距离参考轨迹" +
-                        $"{projection.DistanceToTrajectoryMeters:F3}m，" +
-                        "超过允许值" +
-                        $"{MaximumDistanceToTrajectoryMeters:F3}m。");
-                }
-
-                if (HasReachedEnd(
-                        vehicleState,
-                        projection))
-                {
-                    CompleteTrajectory();
-                    return ParkingControlCycleResult.Completed;
-                }
-
-                if (HasStoppedAtUnsatisfiedTerminal(
-                        vehicleState,
-                        projection,
-                        out var terminalFailureReason))
-                {
-                    return EnterFault(
-                        terminalFailureReason);
-                }
-
-                var referenceSpeedMetersPerSecond =
-                    ResolveReferenceSpeedForControl(
-                        projection);
-                LastReferenceSpeedMetersPerSecond =
-                    referenceSpeedMetersPerSecond;
-                var context = new PathTrackingContext(
-                    vehicleState,
-                    projection,
-                    referenceSpeedMetersPerSecond,
                     deltaTimeSeconds);
-                var lateralCommand =
+                projectionMilliseconds =
-                    _lateralController.Compute(context);
+                    output.ProjectionMilliseconds;
-                var commandSpeedMetersPerSecond =
+                controllerComputeMilliseconds =
-                    _longitudinalController
+                    output.ControllerComputeMilliseconds;
-                        .ComputeSpeedMetersPerSecond(context);
-                var gcpCommand = _gcpAllocator.Allocate(
-                    commandSpeedMetersPerSecond,
-                    lateralCommand);
 
-                if (!_commandExecutor.Execute(
+                if (output.Result ==
-                        gcpCommand,
+                    PathTrackingCycleResult.Completed)
-                        deltaTimeSeconds))
                 {
-                    return EnterFault(
+                    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 =
-                LastFailureReason = string.Empty;
+                    ParkingControlCycleResult.CommandSent;
-                LastException = null;
+                return cycleResult;
-                return ParkingControlCycleResult.CommandSent;
             }
             catch (Exception exception)
             {
-                return EnterFault(
+                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);
             }
         }
 
-        /// <summary>
+        // 主动取消当前轨迹、立即停车并清除全部控制状态。
-        /// 主动取消当前轨迹、立即停车并清除全部控制器状态。
-        /// </summary>
         public void Cancel()
         {
-            StopAndResetControllers();
+            _commandExecutor.Stop();
-            _trajectory = null;
+            _trackingCore.Cancel();
-            IsActive = false;
+            ClearExecutionDiagnostics();
-            IsCompleted = false;
-            ClearDiagnostics();
         }
 
-        /// <summary>
+        // 状态不可用时停车并重置反馈历史，同时保留轨迹等待恢复。
-        /// 在轨迹起点零速固定点处读取前方速度，并限制为低速起步命令。
-        /// </summary>
-        private double ResolveReferenceSpeedForControl(
-            TrajectoryProjection projection)
-        {
-            var currentReferenceSpeed =
-                projection.ReferencePoint
-                    .ReferenceSpeedMetersPerSecond;
-
-            var requiresStartupRelease =
-                projection.ArcLengthMeters <=
-                    StartupRegionMeters &&
-                projection.RemainingDistanceMeters >
-                    FinishDistanceMeters &&
-                Math.Abs(currentReferenceSpeed) <=
-                    ZeroReferenceSpeedToleranceMetersPerSecond;
-
-            if (!requiresStartupRelease)
-            {
-                return currentReferenceSpeed;
-            }
-
-            var previewArcLengthMeters = Math.Min(
-                _trajectory.TotalLengthMeters,
-                projection.ArcLengthMeters +
-                StartupPreviewDistanceMeters);
-            var previewReferenceSpeed =
-                _trajectory
-                    .SampleAtArcLength(
-                        previewArcLengthMeters)
-                    .ReferenceSpeedMetersPerSecond;
-
-            if (Math.Abs(previewReferenceSpeed) <=
-                ZeroReferenceSpeedToleranceMetersPerSecond)
-            {
-                return 0.0;
-            }
-
-            return Math.Sign(previewReferenceSpeed) *
-                   Math.Min(
-                       Math.Abs(previewReferenceSpeed),
-                       MaximumStartupSpeedMetersPerSecond);
-        }
-
-        /// <summary>
-        /// 根据终点距离、剩余弧长和实际线速度判断轨迹是否完成。
-        /// </summary>
-        private bool HasReachedEnd(
-            VehicleState vehicleState,
-            TrajectoryProjection projection)
-        {
-            if (!vehicleState.HasValidVelocityEstimate)
-            {
-                return false;
-            }
-
-            return projection.RemainingDistanceMeters <=
-                       FinishDistanceMeters &&
-                   CalculateDistanceToEndMeters(
-                       vehicleState) <=
-                       FinishDistanceMeters &&
-                   CalculateHeadingErrorToEndRadians(
-                       vehicleState) <=
-                       FinishHeadingToleranceRadians &&
-                   CalculateActualLinearSpeedMetersPerSecond(
-                       vehicleState) <=
-                       FinishSpeedMetersPerSecond;
-        }
-
-        /// <summary>
-        /// 检查车辆是否已在终点零速参考处停稳但最终位置或航向仍不合格。
-        /// </summary>
-        private bool HasStoppedAtUnsatisfiedTerminal(
-            VehicleState vehicleState,
-            TrajectoryProjection projection,
-            out string failureReason)
-        {
-            failureReason = string.Empty;
-
-            var isTerminalZeroSpeedReference =
-                projection.RemainingDistanceMeters <=
-                    FinishDistanceMeters &&
-                Math.Abs(
-                    projection.ReferencePoint
-                        .ReferenceSpeedMetersPerSecond) <=
-                    ZeroReferenceSpeedToleranceMetersPerSecond;
-
-            if (!isTerminalZeroSpeedReference ||
-                !vehicleState.HasValidVelocityEstimate ||
-                CalculateActualLinearSpeedMetersPerSecond(
-                    vehicleState) >
-                FinishSpeedMetersPerSecond)
-            {
-                return false;
-            }
-
-            var positionErrorMeters =
-                CalculateDistanceToEndMeters(
-                    vehicleState);
-            var headingErrorRadians =
-                CalculateHeadingErrorToEndRadians(
-                    vehicleState);
-
-            failureReason =
-                "车辆已在终点零速参考处停稳，但终点精度不满足要求：" +
-                $"位置误差={positionErrorMeters:F3}m，" +
-                "航向误差=" +
-                $"{AngleMath.RadiansToDegrees(headingErrorRadians):F2}°。";
-            return true;
-        }
-
-        /// <summary>
-        /// 计算实际车体中心到轨迹终点的欧氏距离，单位为m。
-        /// </summary>
-        private double CalculateDistanceToEndMeters(
-            VehicleState vehicleState)
-        {
-            var endPoint = _trajectory.EndPoint.PoseInWorld;
-            var deltaX =
-                vehicleState.PoseInWorld.XMeters -
-                endPoint.XMeters;
-            var deltaY =
-                vehicleState.PoseInWorld.YMeters -
-                endPoint.YMeters;
-
-            return Math.Sqrt(
-                deltaX * deltaX +
-                deltaY * deltaY);
-        }
-
-        /// <summary>
-        /// 计算实际车体航向到轨迹终点航向的最短角度误差绝对值，单位为rad。
-        /// </summary>
-        private double CalculateHeadingErrorToEndRadians(
-            VehicleState vehicleState)
-        {
-            return Math.Abs(
-                AngleMath.ShortestDifferenceRadians(
-                    _trajectory.EndPoint
-                        .PoseInWorld.YawRadians,
-                    vehicleState
-                        .PoseInWorld.YawRadians));
-        }
-
-        /// <summary>
-        /// 计算车体坐标系实际线速度的合速度绝对值，单位为m/s。
-        /// </summary>
-        private static double CalculateActualLinearSpeedMetersPerSecond(
-            VehicleState vehicleState)
-        {
-            return Math.Sqrt(
-                vehicleState.TwistInBody.VxMetersPerSecond *
-                vehicleState.TwistInBody.VxMetersPerSecond +
-                vehicleState.TwistInBody.VyMetersPerSecond *
-                vehicleState.TwistInBody.VyMetersPerSecond);
-        }
-
-        /// <summary>
-        /// 在状态暂不可用时停车并重置反馈控制器，同时保留轨迹等待下一周期恢复。
-        /// </summary>
         private void StopForUnavailableState()
         {
             _commandExecutor.Stop();
-            _lateralController.Reset();
+            _trackingCore.PauseForUnavailableState(
-            _longitudinalController.Reset();
+                "当前无法获得有效车辆状态，底盘已停车并等待定位恢复。");
             LastCommand = null;
-            LastFailureReason =
-                "当前无法获得有效车辆状态，底盘已停车并等待定位恢复。";
-            LastException = null;
         }
 
-        /// <summary>
+        // 公共核心完成轨迹后发送停车，并保留零命令供实验记录。
-        /// 完成当前轨迹并停车，但保留最后状态和投影供实验记录读取。
+        private double StopForCompletedTrajectory()
-        /// </summary>
-        private void CompleteTrajectory()
         {
-            StopAndResetControllers();
+            var startTimestamp = Stopwatch.GetTimestamp();
-            IsActive = false;
+            _commandExecutor.Stop();
-            IsCompleted = true;
             LastCommand = new GcpMotionCommand(
                 0.0,
                 0.0,
                 0.0);
-            LastFailureReason = string.Empty;
+            return GetElapsedMilliseconds(startTimestamp);
-            LastException = null;
         }
 
-        /// <summary>
+        // 公共核心故障后只负责真实底盘停车，不覆盖核心保存的失败原因。
-        /// 发生不可继续的控制故障时停车、退出活动状态并保存诊断信息。
+        private double StopForTrackingFault()
-        /// </summary>
+        {
+            var startTimestamp = Stopwatch.GetTimestamp();
+            _commandExecutor.Stop();
+            LastCommand = null;
+            return GetElapsedMilliseconds(startTimestamp);
+        }
+
+        // 将底盘执行或外层异常同步到公共核心，并立即停车。
         private ParkingControlCycleResult EnterFault(
             string reason,
             Exception exception = null)
         {
-            StopAndResetControllers();
+            _commandExecutor.Stop();
-            IsActive = false;
+            _trackingCore.Fail(reason, exception);
-            IsCompleted = false;
             LastCommand = null;
-            LastFailureReason = reason;
-            LastException = exception;
             return ParkingControlCycleResult.Faulted;
         }
 
-        /// <summary>
+        // 清除只属于单车状态读取、发送和周期计时的诊断。
-        /// 立即停止底盘并清除横向和纵向控制器的跨周期状态。
+        private void ClearExecutionDiagnostics()
-        /// </summary>
-        private void StopAndResetControllers()
-        {
-            _commandExecutor.Stop();
-            _lateralController.Reset();
-            _longitudinalController.Reset();
-        }
-
-        /// <summary>
-        /// 清除上一条轨迹留下的状态、命令和故障诊断信息。
-        /// </summary>
-        private void ClearDiagnostics()
         {
+            _cycleIndex = 0;
+            _hasPreviousStateTimestamp = false;
+            _previousStateTimestampSeconds = 0.0;
             LastVehicleState = null;
-            LastProjection = null;
             LastCommand = null;
-            LastReferenceSpeedMetersPerSecond = null;
+            LastCycleTiming = null;
-            LastFailureReason = string.Empty;
-            LastException = null;
         }
 
-        /// <summary>
+        private static double GetElapsedMilliseconds(
-        /// 检查控制参数是否为正有限值。
+            long startTimestamp)
-        /// </summary>
-        private static void EnsureFinitePositive(
-            double value,
-            string parameterName)
         {
-            if (double.IsNaN(value) ||
+            return (Stopwatch.GetTimestamp() - startTimestamp) *
-                double.IsInfinity(value) ||
+                   1000.0 /
-                value <= 0.0)
+                   Stopwatch.Frequency;
-            {
-                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/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

@@ -73,7 +73,7 @@ namespace MultiWheelC.Control.Lateral
         public double MinimumSpeedMetersPerSecond { get; }
 
         /// <summary>
-        /// 获取是否优先使用Detour估算的实际纵向速度计算横向修正。
+        /// 获取是否优先使用当前状态源提供的实际纵向速度计算横向修正。
         /// </summary>
         public bool UseActualSpeedForGain { get; }
 
@@ -99,9 +99,12 @@ namespace MultiWheelC.Control.Lateral
                 MinimumSpeedMetersPerSecond);
             var travelDirection = SelectTravelDirection(context);
 
-            // 参考曲率决定前后反向的差动转角，使无跟踪误差时也能沿曲线行驶。
+            // 参考曲率按轨迹点序的实际行进方向定义；倒车时底盘有符号
-            var feedforwardAngleRadians = Math.Atan(
+            // 纵向速度反向，因此GCP曲率前馈也必须反向才能保持相同几何曲率。
-                context.ReferenceCurvaturePerMeter *
+            var feedforwardAngleRadians =
+                travelDirection *
+                Math.Atan(
+                    context.FeedforwardCurvaturePerMeter *
                     ControlPointRadiusMeters);
 
             // 横向误差生成前后同向的共同转角，使四舵轮车辆平稳靠近轨迹。
@@ -120,8 +123,9 @@ namespace MultiWheelC.Control.Lateral
                     context.HeadingErrorRadians,
                     MaximumHeadingCorrectionRadians);
 
+            // 横向误差已经按轨迹执行点序定义；倒车轨迹的点序会自然
+            // 翻转横向轴，因此共同转角不能再按行驶方向重复反号。
             var commonAngleRadians =
-                travelDirection *
                 crossTrackCorrectionRadians;
             var differentialAngleRadians =
                 feedforwardAngleRadians +
@@ -155,7 +159,7 @@ namespace MultiWheelC.Control.Lateral
                     .ActualLongitudinalSpeedMetersPerSecond;
             }
 
-            return context.ReferenceSpeedMetersPerSecond;
+            return context.ControlReferenceSpeedMetersPerSecond;
         }
 
         /// <summary>
@@ -166,11 +170,11 @@ namespace MultiWheelC.Control.Lateral
         {
             const double directionDeadbandMetersPerSecond = 1e-6;
 
-            if (Math.Abs(context.ReferenceSpeedMetersPerSecond) >
+            if (Math.Abs(context.ControlReferenceSpeedMetersPerSecond) >
                 directionDeadbandMetersPerSecond)
             {
                 return Math.Sign(
-                    context.ReferenceSpeedMetersPerSecond);
+                    context.ControlReferenceSpeedMetersPerSecond);
             }
 
             if (context.HasValidVelocityEstimate &&

MultiWheelC/Control/Longitudinal/PidLongitudinalController.cs

@@ -85,16 +85,16 @@ namespace MultiWheelC.Control.Longitudinal
             _feedbackPid.LastDerivativeOutput;
 
         /// <summary>
-        /// 根据轨迹参考速度和Detour实际纵向速度计算底盘命令速度。
+        /// 根据本周期控制参考速度和实际纵向速度计算底盘命令速度。
         /// </summary>
         public double ComputeSpeedMetersPerSecond(
             PathTrackingContext context)
         {
-            var referenceSpeedMetersPerSecond =
+            var controlReferenceSpeedMetersPerSecond =
-                context.ReferenceSpeedMetersPerSecond;
+                context.ControlReferenceSpeedMetersPerSecond;
 
             // 轨迹明确要求停车时直接输出零，防止速度反馈使车辆在终点反向纠偏。
-            if (Math.Abs(referenceSpeedMetersPerSecond) <=
+            if (Math.Abs(controlReferenceSpeedMetersPerSecond) <=
                 ReferenceStopDeadbandMetersPerSecond)
             {
                 Reset();
@@ -106,11 +106,11 @@ namespace MultiWheelC.Control.Longitudinal
             {
                 Reset();
                 return LimitReferenceSpeed(
-                    referenceSpeedMetersPerSecond);
+                    controlReferenceSpeedMetersPerSecond);
             }
 
             var speedErrorMetersPerSecond =
-                referenceSpeedMetersPerSecond -
+                controlReferenceSpeedMetersPerSecond -
                 context.ActualLongitudinalSpeedMetersPerSecond;
 
             // Detour差分速度在参考速度附近会有小幅波动；死区内只使用速度前馈，
@@ -120,24 +120,24 @@ namespace MultiWheelC.Control.Longitudinal
             {
                 Reset();
                 return LimitReferenceSpeed(
-                    referenceSpeedMetersPerSecond);
+                    controlReferenceSpeedMetersPerSecond);
             }
 
             GetCorrectionOutputRange(
-                referenceSpeedMetersPerSecond,
+                controlReferenceSpeedMetersPerSecond,
                 out var minimumCorrectionMetersPerSecond,
                 out var maximumCorrectionMetersPerSecond);
 
             var correctionMetersPerSecond =
                 _feedbackPid.Update(
-                    referenceSpeedMetersPerSecond,
+                    controlReferenceSpeedMetersPerSecond,
                     context
                         .ActualLongitudinalSpeedMetersPerSecond,
                     context.DeltaTimeSeconds,
                     minimumCorrectionMetersPerSecond,
                     maximumCorrectionMetersPerSecond);
 
-            return referenceSpeedMetersPerSecond +
+            return controlReferenceSpeedMetersPerSecond +
                    correctionMetersPerSecond;
         }
 

MultiWheelC/Experiments/ClampTests.cs

@@ -1,90 +0,0 @@
-
-
-// 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/Experiments/CompositeMotionPlanTests.cs

@@ -16,7 +16,7 @@ namespace MultiWheelC
     /// <summary>
     /// 测试平滑左转轨迹、停车原地左转90°和再次直行的组合运动执行过程。
     /// </summary>
-    [MovementTest(name = "新版控制器：曲线-停车自转-直线组合测试")]
+    [MovementTest(name = "新版控制器：直线-圆弧-折线组合测试")]
     public sealed class CompositeStopTurnGoTest : MovementTest
     {
         private const float MillimetersPerMeter = 1000f;
@@ -37,7 +37,7 @@ namespace MultiWheelC
         public double StraightMaximumSpeedMetersPerSecond = 0.40; // 直线限速。
         public double CurveMaximumSpeedMetersPerSecond = 0.30; // 转弯和过渡段限速。
         public double AccelerationMetersPerSecondSquared = 0.20; // 参考加速度。
-        public double DecelerationMetersPerSecondSquared = 0.12; // 参考减速度。
+        public double DecelerationMetersPerSecondSquared = 0.08; // 参考减速度。
         public double PointSpacingMeters = 0.02; // 离散轨迹点间距。
 
         /// <summary>
@@ -52,7 +52,9 @@ namespace MultiWheelC
                 return;
             }
 
-            if (!MovementTestPreparation.AreWheelsForward())
+            if (!TrajectoryExperimentInput
+                    .TryReadLateralOffsetMeters(
+                        out var lateralOffsetMeters))
             {
                 return;
             }
@@ -65,19 +67,26 @@ namespace MultiWheelC
                 return;
             }
 
-            var stateProvider = new DetourVehicleStateProvider();
+            var stateProvider =
-            if (!stateProvider.TryGetState(out var initialState))
+                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(
-                        initialState.PoseInWorld,
+                        planStartPose,
                         StraightLengthMeters,
                         TurnRadiusMeters,
                         AngleMath.DegreesToRadians(
@@ -133,7 +142,9 @@ namespace MultiWheelC
             _recorder = new TrackingExperimentRecorder(
                 controllerName: "NewStanleyPidComposite",
                 trajectoryName:
+                    TrajectoryExperimentInput.BuildTrajectoryName(
                         "SmoothTurnStopRotateStraight",
+                        lateralOffsetMeters),
                 trialNumber: TrialNumber,
                 referenceStart: ToMillimeterVector(
                     firstTrajectory.StartPoint.PoseInWorld),
@@ -145,7 +156,9 @@ namespace MultiWheelC
                 referenceAccelerationMetersPerSecondSquared:
                     (float)AccelerationMetersPerSecondSquared,
                 referenceDecelerationMetersPerSecondSquared:
-                    (float)DecelerationMetersPerSecondSquared);
+                    (float)DecelerationMetersPerSecondSquared,
+                diagnosticChassis: chassis,
+                diagnosticStateProvider: stateProvider);
             _recorder.Start();
 
             var controlPointRadiusMeters =
@@ -155,17 +168,10 @@ namespace MultiWheelC
             {
                 Segments = plan,
                 StateProvider = stateProvider,
-                ConfigureTrackingMovement = tracking =>
-                {
-                    tracking.MaximumCommandSpeedMetersPerSecond =
-                        StraightMaximumSpeedMetersPerSecond;
-                    tracking
-                        .LongitudinalSpeedErrorDeadbandMetersPerSecond =
-                        0.025;
-                },
                 SegmentStarted = (index, segment) =>
                 {
                     _recorder?.ClearControlReference();
+                    _recorder?.ClearGcpCommand();
                     _recorder?.UpdateCommand(0f, 0f);
                     Console.WriteLine(
                         $"组合运动开始第{index + 1}段：" +
@@ -173,8 +179,10 @@ namespace MultiWheelC
                 },
                 TrackingCycleObserver = (index, controller) =>
                     RecordTrackingCycle(
+                        index,
                         controller,
-                        controlPointRadiusMeters),
+                        controlPointRadiusMeters,
+                        stateProvider),
                 RotationCommandObserver = (index, omega) =>
                     _recorder?.UpdateCommand(
                         0f,
@@ -275,36 +283,77 @@ namespace MultiWheelC
         /// 将轨迹控制周期使用的状态、参考误差和最终GCP命令写入记录器。
         /// </summary>
         private void RecordTrackingCycle(
+            int motionSegmentIndex,
             ParkingGeometricController controller,
-            double controlPointRadiusMeters)
+            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.LastReferenceSpeedMetersPerSecond.HasValue)
+                controller.LastControlReferenceSpeedMetersPerSecond.HasValue)
             {
                 var projection =
                     controller.LastProjection.Value;
                 _recorder?.UpdateControlReference(
                     projection.ArcLengthMeters,
                     controller
-                        .LastReferenceSpeedMetersPerSecond.Value,
+                        .LastControlReferenceSpeedMetersPerSecond.Value,
                     projection.LateralErrorMeters,
                     projection.HeadingErrorRadians,
                     projection.DistanceToTrajectoryMeters,
-                    projection.RemainingDistanceMeters);
+                    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;

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/LongitudinalIdentificationTests.cs

@@ -0,0 +1,562 @@
+using System;
+using System.Diagnostics;
+using System.Globalization;
+using System.Numerics;
+using System.Threading;
+using ClumsyCore;
+using ClumsyCore.Pilot;
+using CommonUsage.Chassis;
+using FundamentalLib;
+using MultiWheelC.StateEstimation;
+using MyParking.Shared;
+
+namespace MultiWheelC
+{
+    /// <summary>
+    /// 提供纵向开环辨识测试共用的舵轮准备、速度下发、采样和安全停车流程。
+    /// </summary>
+    public abstract class LongitudinalIdentificationTestBase
+        : MovementTest
+    {
+        private const double MaximumTargetSpeedMetersPerSecond =
+            0.70;
+        private const double MinimumTargetSpeedMetersPerSecond =
+            0.02;
+        private const float MillimetersPerMeter = 1000f;
+
+        private DriveTask _preparationTask;
+        private MultiWheelChassis _chassis;
+        private MultiWheelChassisAdapter _adapter;
+        private WheelFeedbackVehicleStateProvider _stateProvider;
+        private TrackingExperimentRecorder _recorder;
+        private int _testRunning;
+        private int _stopRequested;
+
+        /// <summary>
+        /// 获取或设置本次重复实验编号。
+        /// </summary>
+        public int TrialNumber = 1;
+
+        /// <summary>
+        /// 获取或设置速度阶跃前的静止记录时间，单位为s。
+        /// </summary>
+        public double BaselineSeconds = 1.0;
+
+        /// <summary>
+        /// 获取或设置目标速度保持时间，单位为s。
+        /// </summary>
+        public double CommandHoldSeconds = 5.0;
+
+        /// <summary>
+        /// 获取或设置停车后的继续记录时间，单位为s。
+        /// </summary>
+        public double PostStopSeconds = 2.0;
+
+        /// <summary>
+        /// 获取或设置速度命令循环周期，单位为ms。
+        /// </summary>
+        public int ControlIntervalMilliseconds = 50;
+
+        /// <summary>
+        /// 派生测试选择是否使用底盘DeAccPerSecond完成正常减速。
+        /// </summary>
+        protected abstract bool UseConfiguredDeceleration { get; }
+
+        /// <summary>
+        /// 获取用于CSV文件名区分停车方式的标识。
+        /// </summary>
+        protected abstract string StopModeName { get; }
+
+        /// <summary>
+        /// 读取目标速度，完成舵轮回正后执行纵向开环命令并保存CSV。
+        /// </summary>
+        public override void Test()
+        {
+            if (Interlocked.CompareExchange(
+                    ref _testRunning,
+                    1,
+                    0) != 0)
+            {
+                Console.WriteLine(
+                    "纵向辨识测试已经在运行，请先停止当前测试。");
+                return;
+            }
+
+            Interlocked.Exchange(ref _stopRequested, 0);
+
+            try
+            {
+                ValidateSettings();
+                if (!TryReadTargetSpeed(
+                        out var targetSpeedMetersPerSecond))
+                {
+                    return;
+                }
+
+                _chassis =
+                    PilotDefinition.Chassis as MultiWheelChassis;
+                if (_chassis == null)
+                {
+                    throw new InvalidOperationException(
+                        "当前底盘不是MultiWheelChassis，无法执行纵向辨识。");
+                }
+
+                ValidateChassisAndTarget(
+                    targetSpeedMetersPerSecond);
+
+                Console.WriteLine(
+                    "纵向辨识开始前请确认M层轮速诊断记录已经开启。");
+                Hedingben.ToastText(
+                    "请确认M层轮速诊断记录已开启。",
+                    "LongitudinalIdentification");
+
+                if (!MovementTestPreparation.AlignWheelsForward(
+                        ref _preparationTask))
+                {
+                    throw new InvalidOperationException(
+                        "四个舵轮未能稳定回正，纵向辨识已经取消。");
+                }
+
+                ThrowIfStopRequested();
+
+                _adapter = new MultiWheelChassisAdapter(
+                    _chassis,
+                    PilotDefinition.Self.CarNum);
+                _adapter.ResetToBodyFrame();
+                _adapter.StopImmediately();
+
+                _stateProvider =
+                    ParkingVehicleStateProviderFactory.Create(
+                        _chassis);
+                if (!_stateProvider.TryGetState(
+                        out var initialState))
+                {
+                    throw new InvalidOperationException(
+                        "无法读取纵向辨识起点位姿：" +
+                        _stateProvider.LastFailureReason);
+                }
+
+                _recorder = CreateRecorder(
+                    targetSpeedMetersPerSecond,
+                    initialState.PoseInWorld);
+                _recorder.Start();
+
+                Console.WriteLine(
+                    $"纵向辨识参数：目标速度={targetSpeedMetersPerSecond:F3}m/s，" +
+                    $"AccPerSecond={_chassis.AccPerSecond:F3}m/s²，" +
+                    $"DeAccPerSecond={_chassis.DeAccPerSecond:F3}m/s²，" +
+                    $"保持={CommandHoldSeconds:F1}s，停车方式={StopModeName}。");
+
+                RunStationaryPhase(
+                    BaselineSeconds,
+                    "静止基线");
+                RunTargetSpeedPhase(
+                    targetSpeedMetersPerSecond);
+
+                if (UseConfiguredDeceleration)
+                {
+                    RunConfiguredDecelerationPhase(
+                        targetSpeedMetersPerSecond);
+                }
+                else
+                {
+                    _adapter.StopImmediately();
+                    RunStationaryPhase(
+                        PostStopSeconds,
+                        "立即停车后记录");
+                }
+
+                Console.WriteLine(
+                    "纵向辨识测试完成。请同时保存对应的M层轮速诊断CSV。");
+                Hedingben.ToastText(
+                    "纵向辨识完成，请停止并保存M层轮速诊断记录。",
+                    "LongitudinalIdentification");
+            }
+            catch (OperationCanceledException)
+            {
+                Console.WriteLine("纵向辨识已由用户停止。");
+            }
+            catch (Exception exception)
+            {
+                Console.WriteLine(
+                    "纵向辨识失败：" +
+                    exception.Message);
+                Hedingben.ToastText(
+                    "纵向辨识失败：" +
+                    exception.Message,
+                    "LongitudinalIdentification");
+            }
+            finally
+            {
+                _adapter?.StopImmediately();
+                _chassis?.PredefinedDriveStop();
+                _recorder?.UpdateBodyCommand(0f, 0f, 0f);
+                _recorder?.StopAndSave();
+
+                _preparationTask?.Stop();
+                _preparationTask = null;
+                _recorder = null;
+                _stateProvider = null;
+                _adapter = null;
+                _chassis = null;
+                Interlocked.Exchange(ref _testRunning, 0);
+            }
+        }
+
+        /// <summary>
+        /// 请求停止当前辨识并立即清零底盘驱动速度。
+        /// </summary>
+        public override void TestStop()
+        {
+            Interlocked.Exchange(ref _stopRequested, 1);
+            _preparationTask?.Stop();
+            _adapter?.StopImmediately();
+            _chassis?.PredefinedDriveStop();
+        }
+
+        /// <summary>
+        /// 在固定周期内保持零命令并记录静止数据。
+        /// </summary>
+        private void RunStationaryPhase(
+            double durationSeconds,
+            string phaseName)
+        {
+            _recorder.UpdateBodyCommand(0f, 0f, 0f);
+            RunPeriodicPhase(
+                durationSeconds,
+                phaseName,
+                _ => UpdateWheelDiagnostics());
+        }
+
+        /// <summary>
+        /// 通过标准车体速度适配器持续下发直线目标速度。
+        /// </summary>
+        private void RunTargetSpeedPhase(
+            double targetSpeedMetersPerSecond)
+        {
+            _recorder.UpdateBodyCommand(
+                (float)targetSpeedMetersPerSecond,
+                0f,
+                0f);
+
+            RunPeriodicPhase(
+                CommandHoldSeconds,
+                "目标速度保持",
+                interval =>
+                {
+                    var accepted = _adapter.SendBodyTwist(
+                        new Twist2D(
+                            targetSpeedMetersPerSecond,
+                            0.0,
+                            0.0),
+                        interval);
+                    if (!accepted)
+                    {
+                        throw new InvalidOperationException(
+                            "底盘拒绝纵向速度命令：" +
+                            _adapter.LastFailureReason);
+                    }
+
+                    UpdateWheelDiagnostics();
+                });
+        }
+
+        /// <summary>
+        /// 重复发送零速SendMotion，使底盘内部DeAccPerSecond斜坡真实参与减速。
+        /// </summary>
+        private void RunConfiguredDecelerationPhase(
+            double targetSpeedMetersPerSecond)
+        {
+            _recorder.UpdateBodyCommand(0f, 0f, 0f);
+
+            var rampDurationSeconds =
+                Math.Abs(targetSpeedMetersPerSecond) /
+                _chassis.DeAccPerSecond;
+            var totalDurationSeconds =
+                rampDurationSeconds +
+                PostStopSeconds;
+
+            RunPeriodicPhase(
+                totalDurationSeconds,
+                "配置化正常减速",
+                interval =>
+                {
+                    // SendBodyTwist(Zero)会立即停车；辨识DeAccPerSecond时必须
+                    // 直接保持SendMotion零目标，让底盘内部发送速度逐周期下降。
+                    var accepted = _chassis.SendMotion(
+                        0f,
+                        0f,
+                        0f,
+                        interval);
+                    if (!accepted)
+                    {
+                        throw new InvalidOperationException(
+                            "底盘拒绝正常减速命令：" +
+                            _chassis.LastMotionDecomposeFailureReason);
+                    }
+
+                    UpdateWheelDiagnostics();
+                });
+        }
+
+        /// <summary>
+        /// 按实际循环间隔运行一个阶段，并响应测试界面的停止请求。
+        /// </summary>
+        private void RunPeriodicPhase(
+            double durationSeconds,
+            string phaseName,
+            Action<TimeSpan> cycleAction)
+        {
+            Console.WriteLine(
+                $"纵向辨识阶段：{phaseName}，预计{durationSeconds:F2}s。");
+
+            var periodSeconds =
+                ControlIntervalMilliseconds / 1000.0;
+            var phaseClock = Stopwatch.StartNew();
+            var previousCycleSeconds = -periodSeconds;
+
+            while (phaseClock.Elapsed.TotalSeconds <
+                   durationSeconds)
+            {
+                ThrowIfStopRequested();
+
+                var cycleStartSeconds =
+                    phaseClock.Elapsed.TotalSeconds;
+                var deltaTimeSeconds =
+                    cycleStartSeconds -
+                    previousCycleSeconds;
+                previousCycleSeconds = cycleStartSeconds;
+
+                cycleAction(
+                    TimeSpan.FromSeconds(
+                        deltaTimeSeconds));
+
+                var elapsedMilliseconds =
+                    (phaseClock.Elapsed.TotalSeconds -
+                     cycleStartSeconds) * 1000.0;
+                var remainingMilliseconds =
+                    ControlIntervalMilliseconds -
+                    elapsedMilliseconds;
+                if (remainingMilliseconds > 1.0)
+                {
+                    Thread.Sleep(
+                        (int)Math.Floor(
+                            remainingMilliseconds));
+                }
+            }
+        }
+
+        /// <summary>
+        /// 刷新轮组原始与滤波速度，供后台CSV记录器读取最新诊断值。
+        /// </summary>
+        private void UpdateWheelDiagnostics()
+        {
+            _stateProvider.TryGetWheelTwist(
+                out _,
+                out _);
+        }
+
+        /// <summary>
+        /// 创建复用现有字段格式的纵向辨识CSV记录器。
+        /// </summary>
+        private TrackingExperimentRecorder CreateRecorder(
+            double targetSpeedMetersPerSecond,
+            Pose2D initialPoseInWorld)
+        {
+            var expectedTravelMeters =
+                targetSpeedMetersPerSecond *
+                CommandHoldSeconds;
+            var referenceStart = new Vector2(
+                (float)(
+                    initialPoseInWorld.XMeters *
+                    MillimetersPerMeter),
+                (float)(
+                    initialPoseInWorld.YMeters *
+                    MillimetersPerMeter));
+            var referenceEnd = new Vector2(
+                referenceStart.X +
+                (float)(
+                    Math.Cos(initialPoseInWorld.YawRadians) *
+                    expectedTravelMeters *
+                    MillimetersPerMeter),
+                referenceStart.Y +
+                (float)(
+                    Math.Sin(initialPoseInWorld.YawRadians) *
+                    expectedTravelMeters *
+                    MillimetersPerMeter));
+            var speedMillimetersPerSecond =
+                targetSpeedMetersPerSecond *
+                MillimetersPerMeter;
+            var trajectoryName =
+                "LongitudinalStep_" +
+                speedMillimetersPerSecond.ToString(
+                    "+0;-0;0",
+                    CultureInfo.InvariantCulture) +
+                "mmps_" +
+                StopModeName;
+
+            return new TrackingExperimentRecorder(
+                controllerName:
+                    "OpenLoopLongitudinalIdentification",
+                trajectoryName: trajectoryName,
+                trialNumber: TrialNumber,
+                referenceStart: referenceStart,
+                referenceEnd: referenceEnd,
+                referenceSpeed:
+                    (float)targetSpeedMetersPerSecond,
+                sampleIntervalMs:
+                    ControlIntervalMilliseconds,
+                referenceMotionFrameYawDegrees: 0f,
+                referenceAccelerationMetersPerSecondSquared:
+                    _chassis.AccPerSecond,
+                referenceDecelerationMetersPerSecondSquared:
+                    _chassis.DeAccPerSecond,
+                diagnosticChassis: _chassis,
+                diagnosticStateProvider: _stateProvider);
+        }
+
+        /// <summary>
+        /// 从测试界面读取带方向的纵向目标速度，正值前进、负值倒车。
+        /// </summary>
+        private static bool TryReadTargetSpeed(
+            out double targetSpeedMetersPerSecond)
+        {
+            targetSpeedMetersPerSecond = 0.0;
+            var input = UI.GetInput(
+                "输入纵向目标速度（m/s，正数前进、负数倒车，" +
+                $"范围-{MaximumTargetSpeedMetersPerSecond:F1}～" +
+                $"{MaximumTargetSpeedMetersPerSecond:F1}且不能为0）：");
+            var parsed = double.TryParse(
+                input,
+                NumberStyles.Float,
+                CultureInfo.CurrentCulture,
+                out targetSpeedMetersPerSecond) ||
+                double.TryParse(
+                    input,
+                    NumberStyles.Float,
+                    CultureInfo.InvariantCulture,
+                    out targetSpeedMetersPerSecond);
+
+            if (!parsed ||
+                double.IsNaN(targetSpeedMetersPerSecond) ||
+                double.IsInfinity(targetSpeedMetersPerSecond) ||
+                Math.Abs(targetSpeedMetersPerSecond) <
+                    MinimumTargetSpeedMetersPerSecond ||
+                Math.Abs(targetSpeedMetersPerSecond) >
+                    MaximumTargetSpeedMetersPerSecond)
+            {
+                Console.WriteLine(
+                    "目标速度必须是绝对值位于" +
+                    $"{MinimumTargetSpeedMetersPerSecond:F2}～" +
+                    $"{MaximumTargetSpeedMetersPerSecond:F2}m/s之间的有限数值。");
+                return false;
+            }
+
+            return true;
+        }
+
+        /// <summary>
+        /// 验证实验时间、循环周期和编号配置。
+        /// </summary>
+        private void ValidateSettings()
+        {
+            NumericGuard.EnsureFiniteNonNegative(
+                BaselineSeconds,
+                nameof(BaselineSeconds));
+            NumericGuard.EnsureFinitePositive(
+                CommandHoldSeconds,
+                nameof(CommandHoldSeconds));
+            NumericGuard.EnsureFiniteNonNegative(
+                PostStopSeconds,
+                nameof(PostStopSeconds));
+
+            if (CommandHoldSeconds > 30.0 ||
+                BaselineSeconds > 10.0 ||
+                PostStopSeconds > 10.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(CommandHoldSeconds),
+                    "辨识阶段时间超出测试允许范围。");
+            }
+
+            if (ControlIntervalMilliseconds < 20 ||
+                ControlIntervalMilliseconds > 200)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(ControlIntervalMilliseconds),
+                    "纵向辨识命令周期必须在20～200ms之间。");
+            }
+
+            if (TrialNumber <= 0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(TrialNumber),
+                    "测试编号必须大于零。");
+            }
+        }
+
+        /// <summary>
+        /// 验证底盘运行时加载的速度、加速度和减速度配置。
+        /// </summary>
+        private void ValidateChassisAndTarget(
+            double targetSpeedMetersPerSecond)
+        {
+            NumericGuard.EnsureFinitePositive(
+                _chassis.MaxSpeed,
+                nameof(_chassis.MaxSpeed));
+            NumericGuard.EnsureFinitePositive(
+                _chassis.AccPerSecond,
+                nameof(_chassis.AccPerSecond));
+            NumericGuard.EnsureFinitePositive(
+                _chassis.DeAccPerSecond,
+                nameof(_chassis.DeAccPerSecond));
+
+            if (Math.Abs(targetSpeedMetersPerSecond) >
+                _chassis.MaxSpeed)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(targetSpeedMetersPerSecond),
+                    "目标速度超过底盘当前MaxSpeed配置。");
+            }
+        }
+
+        /// <summary>
+        /// 在收到停止请求时中断当前阶段并转入finally安全停车。
+        /// </summary>
+        private void ThrowIfStopRequested()
+        {
+            if (Volatile.Read(ref _stopRequested) != 0)
+            {
+                throw new OperationCanceledException();
+            }
+        }
+    }
+
+    /// <summary>
+    /// 测量速度阶跃、加速与稳态，并在保持结束后立即清零驱动速度。
+    /// </summary>
+    [MovementTest(name = "纵向辨识：速度阶跃与立即停车")]
+    public sealed class LongitudinalStepImmediateStopTest
+        : LongitudinalIdentificationTestBase
+    {
+        protected override bool UseConfiguredDeceleration =>
+            false;
+
+        protected override string StopModeName =>
+            "ImmediateStop";
+    }
+
+    /// <summary>
+    /// 测量速度阶跃、稳态以及由底盘DeAccPerSecond形成的正常减速过程。
+    /// </summary>
+    [MovementTest(name = "纵向辨识：速度阶跃与正常减速")]
+    public sealed class LongitudinalStepConfiguredDecelerationTest
+        : LongitudinalIdentificationTestBase
+    {
+        protected override bool UseConfiguredDeceleration =>
+            true;
+
+        protected override string StopModeName =>
+            "ConfiguredDeceleration";
+    }
+}

MultiWheelC/Experiments/NewControllerTrackingTests.cs

@@ -1,5 +1,6 @@
 using System;
 using System.Drawing;
+using System.Globalization;
 using System.Numerics;
 using System.Threading;
 using ClumsyCore;
@@ -13,11 +14,87 @@ 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 sealed class NewControllerStraight4mTest
+    public class NewControllerStraight4mTest
         : MovementTest
     {
         private const float MillimetersPerMeter = 1000f;
@@ -27,7 +104,7 @@ namespace MultiWheelC
 
         private DriveTask _task;
         private TrackingExperimentRecorder _recorder;
-        private DetourVehicleStateProvider _stateProvider;
+        private IVehicleStateProvider _stateProvider;
 
         /// <summary>
         /// 获取或设置本次测试编号，用于区分重复实验CSV。
@@ -37,7 +114,7 @@ namespace MultiWheelC
         /// <summary>
         /// 获取或设置4m直线的巡航参考速度，单位为m/s。
         /// </summary>
-        public double CruiseSpeedMetersPerSecond = 0.30;
+        public double CruiseSpeedMetersPerSecond = 0.40;
 
         /// <summary>
         /// 获取或设置参考速度加速度，单位为m/s²。
@@ -47,13 +124,37 @@ namespace MultiWheelC
         /// <summary>
         /// 获取或设置参考速度减速度，单位为m/s²。
         /// </summary>
-        public double DecelerationMetersPerSecondSquared = 0.10;
+        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>
@@ -66,7 +167,9 @@ namespace MultiWheelC
                 return;
             }
 
-            if (!MovementTestPreparation.AreWheelsForward())
+            if (!TrajectoryExperimentInput
+                    .TryReadLateralOffsetMeters(
+                        out var lateralOffsetMeters))
             {
                 return;
             }
@@ -80,25 +183,33 @@ namespace MultiWheelC
                 return;
             }
 
-            _stateProvider =
+            var stateProvider =
-                new DetourVehicleStateProvider();
+                ParkingVehicleStateProviderFactory.Create(
-            if (!_stateProvider.TryGetState(
+                    chassis);
+            if (!stateProvider.TryGetState(
                     out var initialState))
             {
                 Console.WriteLine(
-                    "无法读取有效Detour起点位姿：" +
+                    "无法读取有效停车状态起点位姿：" +
-                    _stateProvider.LastFailureReason);
+                    stateProvider.LastFailureReason);
                 _stateProvider = null;
                 return;
             }
 
+            _stateProvider = stateProvider;
+
+            var trajectoryStartPose =
+                TrajectoryExperimentInput.OffsetPoseLaterally(
+                    initialState.PoseInWorld,
+                    lateralOffsetMeters);
             var trajectory =
                 TestTrajectoryFactory.CreateStraight4Meters(
-                    initialState.PoseInWorld,
+                    trajectoryStartPose,
                     CruiseSpeedMetersPerSecond,
                     AccelerationMetersPerSecondSquared,
                     DecelerationMetersPerSecondSquared,
-                    PointSpacingMeters);
+                    PointSpacingMeters,
+                    MotionDirectionInBodyRadians);
 
             DrawTrajectory(trajectory);
 
@@ -109,17 +220,25 @@ namespace MultiWheelC
             var recorder =
                 new TrackingExperimentRecorder(
                     controllerName: "NewStanleyPid",
-                    trajectoryName: "ProfiledStraight4m",
+                    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);
+                        (float)DecelerationMetersPerSecondSquared,
+                    diagnosticChassis: chassis,
+                    diagnosticStateProvider: stateProvider);
             _recorder = recorder;
 
             var controlPointRadiusMeters =
@@ -130,12 +249,19 @@ namespace MultiWheelC
                 {
                     Trajectory = trajectory,
                     StateProvider = _stateProvider,
-                    MaximumCommandSpeedMetersPerSecond = 0.50,
+                    MotionDirectionInBodyRadians =
+                        ResolveMotionDirectionFromTrajectory
+                            ? (double?)null
+                            : MotionDirectionInBodyRadians,
+                    ReturnWheelsForwardAfterCompletion =
+                        ReturnWheelsForwardAfterCompletion,
                     CycleObserver = controller =>
                         RecordControlCycle(
                             recorder,
                             controller,
-                            controlPointRadiusMeters)
+                            controlPointRadiusMeters,
+                            _stateProvider as
+                                WheelFeedbackVehicleStateProvider)
                 };
 
             recorder.Start();
@@ -239,34 +365,60 @@ namespace MultiWheelC
         private static void RecordControlCycle(
             TrackingExperimentRecorder recorder,
             ParkingGeometricController controller,
-            double controlPointRadiusMeters)
+            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.LastReferenceSpeedMetersPerSecond.HasValue)
+                controller.LastControlReferenceSpeedMetersPerSecond.HasValue)
             {
                 var projection =
                     controller.LastProjection.Value;
                 recorder.UpdateControlReference(
                     projection.ArcLengthMeters,
-                    controller.LastReferenceSpeedMetersPerSecond.Value,
+                    controller.LastControlReferenceSpeedMetersPerSecond.Value,
                     projection.LateralErrorMeters,
                     projection.HeadingErrorRadians,
                     projection.DistanceToTrajectoryMeters,
-                    projection.RemainingDistanceMeters);
+                    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;
@@ -279,6 +431,36 @@ namespace MultiWheelC
                 (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>
@@ -293,13 +475,68 @@ namespace MultiWheelC
                     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 sealed class NewControllerStraightSemicircleStraightTest
+    public class NewControllerStraightSemicircleStraightTest
         : MovementTest
     {
         private const float MillimetersPerMeter = 1000f;
@@ -310,7 +547,7 @@ namespace MultiWheelC
 
         private DriveTask _task;
         private TrackingExperimentRecorder _recorder;
-        private DetourVehicleStateProvider _stateProvider;
+        private IVehicleStateProvider _stateProvider;
 
         /// <summary>
         /// 获取或设置本次测试编号，用于区分重复实验CSV。
@@ -350,13 +587,37 @@ namespace MultiWheelC
         /// <summary>
         /// 获取或设置参考速度减速度，单位为m/s²。
         /// </summary>
-        public double DecelerationMetersPerSecondSquared = 0.12;
+        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>
@@ -369,7 +630,9 @@ namespace MultiWheelC
                 return;
             }
 
-            if (!MovementTestPreparation.AreWheelsForward())
+            if (!TrajectoryExperimentInput
+                    .TryReadLateralOffsetMeters(
+                        out var lateralOffsetMeters))
             {
                 return;
             }
@@ -383,22 +646,29 @@ namespace MultiWheelC
                 return;
             }
 
-            _stateProvider =
+            var stateProvider =
-                new DetourVehicleStateProvider();
+                ParkingVehicleStateProviderFactory.Create(
-            if (!_stateProvider.TryGetState(
+                    chassis);
+            if (!stateProvider.TryGetState(
                     out var initialState))
             {
                 Console.WriteLine(
-                    "无法读取有效Detour起点位姿：" +
+                    "无法读取有效停车状态起点位姿：" +
-                    _stateProvider.LastFailureReason);
+                    stateProvider.LastFailureReason);
                 _stateProvider = null;
                 return;
             }
 
+            _stateProvider = stateProvider;
+
+            var trajectoryStartPose =
+                TrajectoryExperimentInput.OffsetPoseLaterally(
+                    initialState.PoseInWorld,
+                    lateralOffsetMeters);
             var trajectory =
                 TestTrajectoryFactory
                     .CreateStraightLeftSemicircleStraight(
-                        initialState.PoseInWorld,
+                        trajectoryStartPose,
                         StraightLengthMeters,
                         TurnRadiusMeters,
                         CurvatureTransitionLengthMeters,
@@ -406,7 +676,8 @@ namespace MultiWheelC
                         SemicircleMaximumSpeedMetersPerSecond,
                         AccelerationMetersPerSecondSquared,
                         DecelerationMetersPerSecondSquared,
-                        PointSpacingMeters);
+                        PointSpacingMeters,
+                        MotionDirectionInBodyRadians);
 
             DrawTrajectory(trajectory);
 
@@ -418,17 +689,24 @@ namespace MultiWheelC
                 new TrackingExperimentRecorder(
                     controllerName: "NewStanleyPid",
                     trajectoryName:
-                        "ProfiledStraightSmoothLeftTurnStraight",
+                        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);
+                        (float)DecelerationMetersPerSecondSquared,
+                    diagnosticChassis: chassis,
+                    diagnosticStateProvider: stateProvider);
             _recorder = recorder;
 
             var controlPointRadiusMeters =
@@ -439,13 +717,19 @@ namespace MultiWheelC
                 {
                     Trajectory = trajectory,
                     StateProvider = _stateProvider,
-                    MaximumCommandSpeedMetersPerSecond =
+                    MotionDirectionInBodyRadians =
-                        StraightMaximumSpeedMetersPerSecond,
+                        ResolveMotionDirectionFromTrajectory
+                            ? (double?)null
+                            : MotionDirectionInBodyRadians,
+                    ReturnWheelsForwardAfterCompletion =
+                        ReturnWheelsForwardAfterCompletion,
                     CycleObserver = controller =>
                         RecordControlCycle(
                             recorder,
                             controller,
-                            controlPointRadiusMeters)
+                            controlPointRadiusMeters,
+                            _stateProvider as
+                                WheelFeedbackVehicleStateProvider)
                 };
 
             recorder.Start();
@@ -549,34 +833,60 @@ namespace MultiWheelC
         private static void RecordControlCycle(
             TrackingExperimentRecorder recorder,
             ParkingGeometricController controller,
-            double controlPointRadiusMeters)
+            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.LastReferenceSpeedMetersPerSecond.HasValue)
+                controller.LastControlReferenceSpeedMetersPerSecond.HasValue)
             {
                 var projection =
                     controller.LastProjection.Value;
                 recorder.UpdateControlReference(
                     projection.ArcLengthMeters,
-                    controller.LastReferenceSpeedMetersPerSecond.Value,
+                    controller.LastControlReferenceSpeedMetersPerSecond.Value,
                     projection.LateralErrorMeters,
                     projection.HeadingErrorRadians,
                     projection.DistanceToTrajectoryMeters,
-                    projection.RemainingDistanceMeters);
+                    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;
@@ -589,6 +899,36 @@ namespace MultiWheelC
                 (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>
@@ -604,4 +944,36 @@ namespace MultiWheelC
                     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/PathTrackingTests.cs

@@ -1,635 +0,0 @@
-
-
-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/Experiments/RotationTests.cs

@@ -7,11 +7,12 @@ 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 MDCSToolBox.Commons.Controllers;
 using MyParking.Shared;
+using MultiWheelC.StateEstimation;
 
 namespace MultiWheelC
 {
@@ -19,6 +20,8 @@ namespace MultiWheelC
     {
         public float RelativeAngleDegrees; // 相对当前航向的旋转角度，逆时针为正。
         public int TrialNumber = 1; // 重复实验编号。
+        public InPlaceRotationFeedbackMode FeedbackMode =
+            InPlaceRotationFeedbackMode.DetourAbsoluteHeading;
 
         private DriveTask _task;
         private TrackingExperimentRecorder _recorder;
@@ -67,11 +70,37 @@ namespace MultiWheelC
                 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(
                 "原地自转实际参数：" +
@@ -85,13 +114,19 @@ namespace MultiWheelC
                 $"舵轮到位误差={config.InPlaceRotateWheelAlignDeg:F2}°，" +
                 $"旋转超时={config.InPlaceRotateTimeoutSec:F1}s；" +
                 $"起点航向={location.th:F2}°，" +
-                $"目标航向={targetWorldAngle:F2}°。");
+                $"目标航向={targetWorldAngle:F2}°，" +
+                $"反馈模式={FeedbackMode}。");
             Console.WriteLine(
                 "原地自转CSV保存目录：" +
                 TrackingExperimentRecorder.DefaultOutputDirectory);
 
             _recorder = new TrackingExperimentRecorder(
-                controllerName: "InPlaceRotateFilteredPID",
+                controllerName:
+                    FeedbackMode ==
+                        InPlaceRotationFeedbackMode
+                            .RelativeWheelOdometry
+                        ? "InPlaceRotateWheelOdometry"
+                        : "InPlaceRotateFilteredPID",
                 trajectoryName: _trajectoryName,
                 trialNumber: TrialNumber,
                 referenceStart: rotationCenter,
@@ -99,7 +134,9 @@ namespace MultiWheelC
                 referenceSpeed: 0f,
                 referenceAngularSpeed:
                     (float)AngleMath.DegreesToRadians(
-                        config.InPlaceRotateMaxSpeed));
+                        config.InPlaceRotateMaxSpeed),
+                diagnosticChassis: chassis,
+                diagnosticStateProvider: stateProvider);
             _recorder.Start();
 
             try
@@ -107,31 +144,10 @@ namespace MultiWheelC
                 _task = new DriveTask(
                     new MultiWheelRotateInPlace
                     {
-                        // MultiWheelRotateInPlace接收世界坐标系绝对航向。
+                        AngleTarget = movementAngleTarget,
-                        AngleTarget = targetWorldAngle,
+                        FeedbackMode = FeedbackMode,
-                        PidparamsRead = () => new PIDParams
+                        Chassis = chassis,
-                        {
+                        StateProvider = stateProvider,
-                            Kp =
-                                config.InPlaceRotateKp,
-                            Ki =
-                                config.InPlaceRotateKi,
-                            Kd =
-                                config.InPlaceRotateKd,
-                            DeadZone =
-                                config.InPlaceRotateArriveDeg,
-                            SpeedAccPerSec =
-                                config.InPlaceRotateAcc,
-                            OutputUpperThreshold =
-                                config.InPlaceRotateMaxSpeed,
-                            MaxI =
-                                config.InPlaceRotateMaxI
-                        },
-                        MinimumAngularSpeedDegreesPerSecond =
-                            config.InPlaceRotateMinimumSpeed,
-                        WheelAlignmentToleranceDegrees =
-                            config.InPlaceRotateWheelAlignDeg,
-                        RotationTimeoutSeconds =
-                            config.InPlaceRotateTimeoutSec,
                         CommandAngularSpeedObserver =
                             commandAngularSpeed =>
                                 _recorder?.UpdateCommand(
@@ -163,6 +179,46 @@ namespace MultiWheelC
             _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：输入角度原地自转")]
@@ -179,38 +235,37 @@ namespace MultiWheelC
         /// </summary>
         public override void Test()
         {
-            var input = UI.GetInput(
+            if (!TryReadRelativeAngleDegrees(
-                "输入相对旋转角度（deg，正数逆时针，负数顺时针，范围-180到180之间）：");
+                    out var relativeAngleDegrees))
-
-            if ((!float.TryParse(
-                     input,
-                     NumberStyles.Float,
-                     CultureInfo.CurrentCulture,
-                     out var relativeAngleDegrees) &&
-                 !float.TryParse(
-                     input,
-                     NumberStyles.Float,
-                     CultureInfo.InvariantCulture,
-                     out relativeAngleDegrees)) ||
-                float.IsNaN(relativeAngleDegrees) ||
-                float.IsInfinity(relativeAngleDegrees))
             {
-                Console.WriteLine("旋转角度输入无效，测试已经取消。");
                 return;
             }
 
-            if (Math.Abs(relativeAngleDegrees) < 1e-3f)
+            RelativeAngleDegrees = relativeAngleDegrees;
-            {
+            base.Test();
-                Console.WriteLine("旋转角度不能为0，测试已经取消。");
+        }
-                return;
     }
 
-            // 当前控制器按照圆周最短角旋转；精确±180°的方向存在二义性。
+    [MovementTest(name = "轮组里程计：输入角度原地相对自转")]
-            if (Math.Abs(relativeAngleDegrees) >= 180f)
+    public sealed class TestWheelOdometryRotateAngle :
+        InPlaceRotateTestBase
+    {
+        public TestWheelOdometryRotateAngle()
+            : base(0f, "RotateWheelOdometryCustomAngle")
+        {
+            FeedbackMode =
+                InPlaceRotationFeedbackMode
+                    .RelativeWheelOdometry;
+        }
+
+        /// <summary>
+        /// 读取相对角度并仅用滤波后的轮组角速度积分完成自转。
+        /// </summary>
+        public override void Test()
+        {
+            if (!TryReadRelativeAngleDegrees(
+                    out var relativeAngleDegrees))
             {
-                Console.WriteLine(
-                    "输入角度必须满足-180° < angle < 180°；" +
-                    "当前最短角控制不支持指定精确±180°的旋转方向。");
                 return;
             }
 

MultiWheelC/Experiments/TestTrajectoryFactory.cs

@@ -13,14 +13,15 @@ namespace MultiWheelC
         private const double StraightLengthMeters = 4.0;
 
         /// <summary>
-        /// 从给定车体中心位姿沿当前航向生成带梯形速度规划的4m直线轨迹。
+        /// 从给定车体中心位姿按速度符号沿车头或车尾方向生成带梯形速度规划的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 pointSpacingMeters = 0.02,
+            double motionDirectionInBodyRadians = 0.0)
         {
             return CreateStraight(
                 startPoseInWorld,
@@ -28,11 +29,12 @@ namespace MultiWheelC
                 cruiseSpeedMetersPerSecond,
                 accelerationMetersPerSecondSquared,
                 decelerationMetersPerSecondSquared,
-                pointSpacingMeters);
+                pointSpacingMeters,
+                motionDirectionInBodyRadians);
         }
 
         /// <summary>
-        /// 从给定车体中心位姿沿当前航向生成指定长度并在终点停车的直线轨迹。
+        /// 从给定车体中心位姿按速度符号沿车头或车尾方向生成指定长度并在终点停车的直线轨迹。
         /// </summary>
         public static Trajectory2D CreateStraight(
             Pose2D startPoseInWorld,
@@ -40,26 +42,30 @@ namespace MultiWheelC
             double cruiseSpeedMetersPerSecond = 0.30,
             double accelerationMetersPerSecondSquared = 0.20,
             double decelerationMetersPerSecondSquared = 0.20,
-            double pointSpacingMeters = 0.02)
+            double pointSpacingMeters = 0.02,
+            double motionDirectionInBodyRadians = 0.0)
         {
-            EnsureFinitePose(
+            NumericGuard.EnsureFinite(
                 startPoseInWorld,
                 nameof(startPoseInWorld));
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 lengthMeters,
                 nameof(lengthMeters));
-            EnsureFinitePositive(
+            var travelDirection = GetTravelDirection(
                 cruiseSpeedMetersPerSecond,
                 nameof(cruiseSpeedMetersPerSecond));
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 accelerationMetersPerSecondSquared,
                 nameof(accelerationMetersPerSecondSquared));
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 decelerationMetersPerSecondSquared,
                 nameof(decelerationMetersPerSecondSquared));
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 pointSpacingMeters,
                 nameof(pointSpacingMeters));
+            NumericGuard.EnsureFinite(
+                motionDirectionInBodyRadians,
+                nameof(motionDirectionInBodyRadians));
 
             if (pointSpacingMeters > lengthMeters)
             {
@@ -73,10 +79,13 @@ namespace MultiWheelC
                 pointSpacingMeters);
             var points = new List<TrajectoryPoint>(
                 segmentCount + 1);
-            var directionX = Math.Cos(
+            var worldMotionYawRadians =
-                startPoseInWorld.YawRadians);
+                startPoseInWorld.YawRadians +
-            var directionY = Math.Sin(
+                motionDirectionInBodyRadians;
-                startPoseInWorld.YawRadians);
+            var directionX = travelDirection *
+                Math.Cos(worldMotionYawRadians);
+            var directionY = travelDirection *
+                Math.Sin(worldMotionYawRadians);
 
             for (var index = 0;
                  index <= segmentCount;
@@ -116,7 +125,7 @@ namespace MultiWheelC
         }
 
         /// <summary>
-        /// 从当前位姿生成“3m直线、平滑进入半径2m左转、平滑退出、3m直线”的180°转弯轨迹。
+        /// 从当前位姿沿指定车体运动方向生成“3m直线、平滑左弯180°、3m直线”的轨迹。
         /// </summary>
         public static Trajectory2D CreateStraightLeftSemicircleStraight(
             Pose2D startPoseInWorld,
@@ -127,7 +136,8 @@ namespace MultiWheelC
             double semicircleMaximumSpeedMetersPerSecond = 0.25,
             double accelerationMetersPerSecondSquared = 0.20,
             double decelerationMetersPerSecondSquared = 0.12,
-            double pointSpacingMeters = 0.02)
+            double pointSpacingMeters = 0.02,
+            double motionDirectionInBodyRadians = 0.0)
         {
             return CreateStraightSmoothLeftTurnStraight(
                 startPoseInWorld,
@@ -139,11 +149,12 @@ namespace MultiWheelC
                 semicircleMaximumSpeedMetersPerSecond,
                 accelerationMetersPerSecondSquared,
                 decelerationMetersPerSecondSquared,
-                pointSpacingMeters);
+                pointSpacingMeters,
+                motionDirectionInBodyRadians);
         }
 
         /// <summary>
-        /// 生成“直线、平滑左转、直线”轨迹，并使总转向角严格等于指定角度。
+        /// 沿指定车体运动方向生成“直线、平滑左弯、直线”轨迹，并使总转角严格等于指定角度。
         /// </summary>
         public static Trajectory2D CreateStraightSmoothLeftTurnStraight(
             Pose2D startPoseInWorld,
@@ -155,38 +166,41 @@ namespace MultiWheelC
             double turnMaximumSpeedMetersPerSecond,
             double accelerationMetersPerSecondSquared,
             double decelerationMetersPerSecondSquared,
-            double pointSpacingMeters)
+            double pointSpacingMeters,
+            double motionDirectionInBodyRadians = 0.0)
         {
-            EnsureFinitePose(
+            NumericGuard.EnsureFinite(
                 startPoseInWorld,
                 nameof(startPoseInWorld));
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 straightLengthMeters,
                 nameof(straightLengthMeters));
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 turnRadiusMeters,
                 nameof(turnRadiusMeters));
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 turnAngleRadians,
                 nameof(turnAngleRadians));
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 curvatureTransitionLengthMeters,
                 nameof(curvatureTransitionLengthMeters));
-            EnsureFinitePositive(
+            var travelDirection = GetCommonTravelDirection(
                 straightMaximumSpeedMetersPerSecond,
-                nameof(straightMaximumSpeedMetersPerSecond));
+                nameof(straightMaximumSpeedMetersPerSecond),
-            EnsureFinitePositive(
                 turnMaximumSpeedMetersPerSecond,
                 nameof(turnMaximumSpeedMetersPerSecond));
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 accelerationMetersPerSecondSquared,
                 nameof(accelerationMetersPerSecondSquared));
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 decelerationMetersPerSecondSquared,
                 nameof(decelerationMetersPerSecondSquared));
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 pointSpacingMeters,
                 nameof(pointSpacingMeters));
+            NumericGuard.EnsureFinite(
+                motionDirectionInBodyRadians,
+                nameof(motionDirectionInBodyRadians));
 
             if (turnAngleRadians > 2.0 * Math.PI)
             {
@@ -245,8 +259,10 @@ namespace MultiWheelC
                         turnStartArcLengthMeters &&
                     arcLengthMeters <=
                         turnEndArcLengthMeters
-                        ? turnMaximumSpeedMetersPerSecond
+                        ? Math.Abs(
-                        : straightMaximumSpeedMetersPerSecond;
+                            turnMaximumSpeedMetersPerSecond)
+                        : Math.Abs(
+                            straightMaximumSpeedMetersPerSecond);
             }
 
             ApplyAccelerationAndBrakingLimits(
@@ -256,12 +272,23 @@ namespace MultiWheelC
                 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(
-                startPoseInWorld.YawRadians);
+                worldMotionStartYawRadians);
             var startSin = Math.Sin(
-                startPoseInWorld.YawRadians);
+                worldMotionStartYawRadians);
             var localX = 0.0;
             var localY = 0.0;
             var localYawRadians = 0.0;
@@ -296,10 +323,10 @@ namespace MultiWheelC
                     if (Math.Abs(segmentCurvaturePerMeter) <=
                         1e-12)
                     {
-                        localX +=
+                        localX += travelDirection *
                             Math.Cos(localYawRadians) *
                             segmentLengthMeters;
-                        localY +=
+                        localY += travelDirection *
                             Math.Sin(localYawRadians) *
                             segmentLengthMeters;
                     }
@@ -308,11 +335,11 @@ namespace MultiWheelC
                         var nextYawRadians =
                             localYawRadians +
                             segmentYawChangeRadians;
-                        localX +=
+                        localX += travelDirection *
                             (Math.Sin(nextYawRadians) -
                              Math.Sin(localYawRadians)) /
                             segmentCurvaturePerMeter;
-                        localY +=
+                        localY += travelDirection *
                             (Math.Cos(localYawRadians) -
                              Math.Cos(nextYawRadians)) /
                             segmentCurvaturePerMeter;
@@ -490,7 +517,7 @@ namespace MultiWheelC
         }
 
         /// <summary>
-        /// 对逐点速度上限执行前向加速约束和反向制动约束，生成连续可执行的空间速度曲线。
+        /// 对逐点速度幅值上限执行前向加速约束和反向制动约束，生成连续可执行的空间速度曲线。
         /// </summary>
         private static void ApplyAccelerationAndBrakingLimits(
             IReadOnlyList<double> arcLengthsMeters,
@@ -547,7 +574,7 @@ namespace MultiWheelC
         }
 
         /// <summary>
-        /// 根据起步、巡航和制动能力计算指定弧长位置允许的参考速度。
+        /// 根据起步、巡航和制动能力计算指定弧长位置允许的有符号参考速度。
         /// </summary>
         private static double CalculateReferenceSpeed(
             double arcLengthMeters,
@@ -566,52 +593,64 @@ namespace MultiWheelC
                 decelerationMetersPerSecondSquared *
                 Math.Max(0.0, remainingDistanceMeters));
 
-            return Math.Min(
+            var travelDirection = GetTravelDirection(
                 cruiseSpeedMetersPerSecond,
+                nameof(cruiseSpeedMetersPerSecond));
+            var speedMagnitude = Math.Min(
+                Math.Abs(cruiseSpeedMetersPerSecond),
                 Math.Min(
                     accelerationLimitedSpeed,
                     brakingLimitedSpeed));
+
+            return travelDirection * speedMagnitude;
         }
 
         /// <summary>
-        /// 检查世界坐标系起点位姿是否全部为有限值。
+        /// 获取非零有符号速度表示的前进或倒车方向。
         /// </summary>
-        private static void EnsureFinitePose(
+        private static double GetTravelDirection(
-            Pose2D pose,
+            double signedSpeedMetersPerSecond,
             string parameterName)
         {
-            if (!IsFinite(pose.XMeters) ||
+            NumericGuard.EnsureFinite(
-                !IsFinite(pose.YMeters) ||
+                signedSpeedMetersPerSecond,
-                !IsFinite(pose.YawRadians))
+                parameterName);
+
+            if (signedSpeedMetersPerSecond == 0.0)
             {
                 throw new ArgumentOutOfRangeException(
                     parameterName,
-                    "直线测试轨迹的起点位姿必须由有限值组成。");
+                    "测试轨迹的最大速度不能为零；正值表示前进，负值表示倒车。");
             }
+
+            return Math.Sign(
+                signedSpeedMetersPerSecond);
         }
 
         /// <summary>
-        /// 检查测试轨迹参数是否为正有限值。
+        /// 确保直线段和转弯段速度使用相同的前进或倒车方向。
         /// </summary>
-        private static void EnsureFinitePositive(
+        private static double GetCommonTravelDirection(
-            double value,
+            double firstSpeedMetersPerSecond,
-            string parameterName)
+            string firstParameterName,
+            double secondSpeedMetersPerSecond,
+            string secondParameterName)
         {
-            if (!IsFinite(value) || value <= 0.0)
+            var firstDirection = GetTravelDirection(
+                firstSpeedMetersPerSecond,
+                firstParameterName);
+            var secondDirection = GetTravelDirection(
+                secondSpeedMetersPerSecond,
+                secondParameterName);
+
+            if (firstDirection != secondDirection)
             {
-                throw new ArgumentOutOfRangeException(
+                throw new ArgumentException(
-                    parameterName,
+                    "同一条测试轨迹的直线段和转弯段速度必须同号，不能在运动中直接切换前进与倒车方向。",
-                    "直线测试轨迹的速度、加速度和点间距必须是正有限值。");
+                    firstParameterName);
-            }
             }
 
-        /// <summary>
+            return firstDirection;
-        /// 判断数值是否可用于轨迹计算。
-        /// </summary>
-        private static bool IsFinite(double value)
-        {
-            return !double.IsNaN(value) &&
-                   !double.IsInfinity(value);
         }
     }
 }

MultiWheelC/Experiments/WheelPreparationTests.cs

@@ -3,17 +3,20 @@
 using System;
 using ClumsyCore;
 using ClumsyCore.Pilot;
-using CommonUsage.Chassis;
 using FundamentalLib;
 using MDCSToolBox.Clumsy.Movements;
 using MDCSToolBox.Clumsy.Pilot;
-using MyParking.Shared;
 
 namespace MultiWheelC
 {
+    /// <summary>
+    /// 为需要显式执行舵轮回正的测试管理准备动作及其DriveTask生命周期。
+    /// </summary>
     internal static class MovementTestPreparation
     {
-        // 在测试正式开始前，将四个舵轮稳定回正到车体前向。
+        /// <summary>
+        /// 执行舵轮回正动作并返回四轮是否已经稳定朝向车体前方。
+        /// </summary>
         public static bool AlignWheelsForward(
             ref DriveTask activeTask)
         {
@@ -40,47 +43,11 @@ namespace MultiWheelC
             }
         }
 
-        // 只读取实际舵角，检查四个舵轮是否已与车头方向一致。
-        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;
-            }
-        }
     }
 
+    /// <summary>
+    /// 提供可从测试界面单独触发的四舵轮回正动作。
+    /// </summary>
     [MovementTest(name = "准备：四个舵轮与车头方向一致")]
     public class AlignWheelsForwardTest : MovementTest
     {

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/Movements/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/Movements/DriverMovements.cs

@@ -1,123 +0,0 @@
-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/Movements/DstTracker.cs

@@ -1,55 +0,0 @@
-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/Movements/LineTracking.cs

@@ -1,178 +0,0 @@
-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/Movements/MotionPlanExecutor.cs

@@ -2,7 +2,7 @@ using System;
 using System.Collections.Generic;
 using ClumsyCore.Interfaces;
 using ClumsyCore.Pilot;
-using MDCSToolBox.Commons.Controllers;
+using CommonUsage.Chassis;
 using MultiWheelC.Control.Execution;
 using MultiWheelC.StateEstimation;
 using MultiWheelC.Trajectory;
@@ -81,7 +81,7 @@ namespace MultiWheelC
         public IReadOnlyList<MotionPlanSegment> Segments;
 
         /// <summary>
-        /// 获取或设置所有动作段共享的车辆状态源；为空时使用Detour状态源。
+        /// 获取或设置所有动作段共享的车辆状态源；为空时组合Detour位姿与电机反馈速度。
         /// </summary>
         public IVehicleStateProvider StateProvider;
 
@@ -118,23 +118,26 @@ namespace MultiWheelC
         /// </summary>
         public override IEnumerable<bool> Get()
         {
-            if (Segments == null || Segments.Count == 0)
+            var segments = ValidateAndSnapshotSegments();
+
+            var chassis =
+                PilotDefinition.Chassis as MultiWheelChassis;
+            if (chassis == null)
             {
                 throw new InvalidOperationException(
-                    "组合运动计划至少需要包含一个动作段。");
+                    "当前底盘不是MultiWheelChassis，无法执行组合运动计划。");
             }
 
             var stateProvider =
                 StateProvider ??
-                new DetourVehicleStateProvider();
+                ParkingVehicleStateProviderFactory.Create(
+                    chassis);
 
             for (var index = 0;
-                 index < Segments.Count;
+                 index < segments.Count;
                  index++)
             {
-                var segment = Segments[index] ??
+                var segment = segments[index];
-                    throw new InvalidOperationException(
-                        $"组合运动计划第{index}段为空。");
 
                 SegmentStarted?.Invoke(index, segment);
 
@@ -186,7 +189,6 @@ namespace MultiWheelC
 
                 if (segment is RotateInPlaceMotionPlanSegment rotate)
                 {
-                    var config = PilotDefinition.Conf;
                     var movement =
                         new MultiWheelRotateInPlace
                         {
@@ -194,25 +196,6 @@ namespace MultiWheelC
                                 (float)AngleMath.RadiansToDegrees(
                                     rotate.TargetYawRadians),
                             StateProvider = stateProvider,
-                            PidparamsRead = () => new PIDParams
-                            {
-                                Kp = config.InPlaceRotateKp,
-                                Ki = config.InPlaceRotateKi,
-                                Kd = config.InPlaceRotateKd,
-                                DeadZone =
-                                    config.InPlaceRotateArriveDeg,
-                                SpeedAccPerSec =
-                                    config.InPlaceRotateAcc,
-                                OutputUpperThreshold =
-                                    config.InPlaceRotateMaxSpeed,
-                                MaxI = config.InPlaceRotateMaxI
-                            },
-                            MinimumAngularSpeedDegreesPerSecond =
-                                config.InPlaceRotateMinimumSpeed,
-                            WheelAlignmentToleranceDegrees =
-                                config.InPlaceRotateWheelAlignDeg,
-                            RotationTimeoutSeconds =
-                                config.InPlaceRotateTimeoutSec,
                             CommandAngularSpeedObserver =
                                 commandDegreesPerSecond =>
                                     RotationCommandObserver?.Invoke(
@@ -242,5 +225,42 @@ namespace MultiWheelC
             // 所有子动作均已完成后，才向外层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

@@ -9,16 +9,63 @@ using MyParking.Shared;
 
 namespace MultiWheelC
 {
-    // C层测试准备：停车并等待四个舵轮稳定回到车体前向0°。
+    /// <summary>
+    /// 停车并等待四个舵轮稳定回到车体前向0°。
+    /// </summary>
     public class PrepareWheelsForward : MovementDefinition
     {
-        public float ToleranceDegrees = 2f;
+        /// <summary>
-        public float StableSeconds = 0.3f;
+        /// 获取或设置舵轮需要对准的车体方向，单位为rad；0表示车头方向。
-        public float TimeoutSeconds = 10f;
+        /// </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)
@@ -32,15 +79,16 @@ namespace MultiWheelC
                 PilotDefinition.Self.CarNum);
             adapter.ResetToBodyFrame();
             var toleranceRadians =
-                AngleMath.DegreesToRadians(ToleranceDegrees);
+                AngleMath.DegreesToRadians(toleranceDegrees);
             var startTime = DateTime.UtcNow;
             DateTime? alignedSince = null;
 
             Completed = false;
-            if (!adapter.PrepareParallelDirection(0.0))
+            if (!adapter.PrepareParallelDirection(
+                    DirectionRadians))
             {
                 throw new InvalidOperationException(
-                    "无法将所有舵轮下发到车体前向0°。");
+                    "无法将所有舵轮下发到指定运动方向。");
             }
 
             try
@@ -49,7 +97,7 @@ namespace MultiWheelC
                 {
                     var aligned =
                         adapter.AreParallelWheelsAligned(
-                            0.0,
+                            DirectionRadians,
                             toleranceRadians);
 
                     if (aligned)
@@ -59,7 +107,7 @@ namespace MultiWheelC
 
                         if ((DateTime.UtcNow -
                                 alignedSince.Value).TotalSeconds >=
-                            StableSeconds)
+                            stableSeconds)
                         {
                             Completed = true;
                             yield break;
@@ -70,12 +118,12 @@ namespace MultiWheelC
                         alignedSince = null;
                     }
 
-                    if (TimeoutSeconds > 0f &&
+                    if (timeoutSeconds > 0f &&
                         (DateTime.UtcNow - startTime).TotalSeconds >
-                        TimeoutSeconds)
+                        timeoutSeconds)
                     {
                         throw new TimeoutException(
-                            $"舵轮回正超过{TimeoutSeconds:F1}s，" +
+                            $"舵轮回正超过{timeoutSeconds:F1}s，" +
                             "测试已经取消。");
                     }
 
@@ -84,7 +132,7 @@ namespace MultiWheelC
             }
             finally
             {
-                // 只清零驱动速度，保留已经下发的0°舵角。
+                // 只清零驱动速度，保留已经下发的目标舵角。
                 adapter.StopImmediately();
             }
         }

MultiWheelC/Movements/RotateInPlace.cs

@@ -11,30 +11,51 @@ 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;
 
-        // 留作标定或单元测试时显式替换；为空时使用经过校验的Detour状态源。
+        /// <summary>
+        /// 获取或设置原地自转反馈模式；默认保持现有Detour绝对航向闭环。
+        /// </summary>
+        public InPlaceRotationFeedbackMode FeedbackMode =
+            InPlaceRotationFeedbackMode.DetourAbsoluteHeading;
+
+        // 留作标定或单元测试时显式替换；为空时使用配置化Detour与电机反馈组合状态源。
         public Func<float> ThetaReader;
 
-        public IVehicleStateProvider StateProvider =
+        public IVehicleStateProvider StateProvider;
-            new DetourVehicleStateProvider();
 
-        public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+        public MultiWheelChassis Chassis =
+            PilotDefinition.Chassis as MultiWheelChassis;
 
-        public Func<PIDParams> PidparamsRead = () => new PIDParams() { };
+        /// <summary>
+        /// 获取或设置本次动作的PID参数读取覆盖；为空时读取车辆配置。
+        /// </summary>
+        public Func<PIDParams> PidparamsRead;
 
         public PIDController thPid;
 
         // 将本周期PID角速度输出提供给实验记录器，单位deg/s。
         public Action<float> CommandAngularSpeedObserver;
 
-        // 自转前舵轮实际角度允许误差，单位deg。
+        // 自转前舵轮实际角度允许误差覆盖值，单位deg；为空时读取车辆配置。
-        public float WheelAlignmentToleranceDegrees = 2f;
+        public float? WheelAlignmentToleranceDegrees;
 
         // 自转舵轮连续保持到位的时间，单位s。
         public float WheelAlignmentStableSeconds = 0.3f;
@@ -42,20 +63,56 @@ namespace MultiWheelC
         // 自转舵轮准备超时时间，单位s。
         public float WheelAlignmentTimeoutSeconds = 10f;
 
-        // 航向尚未到位时允许下发的最小有效角速度，单位deg/s。
+        // 航向尚未到位时允许下发的最小有效角速度覆盖值，单位deg/s；为空时读取车辆配置。
-        public float MinimumAngularSpeedDegreesPerSecond = 1f;
+        public float? MinimumAngularSpeedDegreesPerSecond;
 
-        // 舵轮到位后执行航向闭环允许的最长时间，单位s。
+        // 舵轮到位后执行航向闭环允许的最长时间覆盖值，单位s；为空时读取车辆配置。
-        public float RotationTimeoutSeconds = 15f;
+        public float? RotationTimeoutSeconds;
 
-        // 先准备自转舵角，再通过安全版SendXYThSpeed闭环旋转到目标角度。
+        /// <summary>
+        /// 读取一次有效配置，闭环旋转到目标航向并在正常完成后等待舵轮稳定回正。
+        /// </summary>
         public override IEnumerable<bool> Get()
         {
             if (Chassis == null)
                 throw new InvalidOperationException(
                     "当前底盘不是MultiWheelChassis，无法执行原地自转。");
 
-            ValidateParameters();
+            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,
@@ -70,7 +127,7 @@ namespace MultiWheelC
                 {
                     if (!adapter.PrepareSpin(
                             alignmentToleranceDegrees:
-                                WheelAlignmentToleranceDegrees))
+                                wheelAlignmentToleranceDegrees))
                         throw new InvalidOperationException(
                             "无法生成原地自转舵轮目标：" +
                             adapter.LastFailureReason);
@@ -101,7 +158,7 @@ namespace MultiWheelC
 
                 var alignmentToleranceRadians =
                     AngleMath.DegreesToRadians(
-                        WheelAlignmentToleranceDegrees);
+                        wheelAlignmentToleranceDegrees);
                 if (!adapter.AdoptPreparedSpinForXYTh(
                         alignmentToleranceRadians))
                 {
@@ -110,34 +167,126 @@ namespace MultiWheelC
                         adapter.LastFailureReason);
                 }
 
+                var useRelativeWheelOdometry =
+                    FeedbackMode ==
+                    InPlaceRotationFeedbackMode
+                        .RelativeWheelOdometry;
+                var wheelStateProvider =
+                    useRelativeWheelOdometry
+                        ? stateProvider as
+                            WheelFeedbackVehicleStateProvider
+                        : null;
+                if (useRelativeWheelOdometry &&
+                    wheelStateProvider == null)
+                {
+                    throw new InvalidOperationException(
+                        "轮组相对角度自转需要" +
+                        "WheelFeedbackVehicleStateProvider。");
+                }
+
                 var targetAngle =
-                    (float)AngleMath.NormalizeDegrees(AngleTarget);
+                    useRelativeWheelOdometry
-                var p = PidparamsRead();
+                        ? AngleTarget
-                var currentAngle = ReadCurrentAngleDegrees();
+                        : (float)AngleMath.NormalizeDegrees(
+                            AngleTarget);
+                var currentAngle =
+                    useRelativeWheelOdometry
+                        ? 0f
+                        : ReadCurrentAngleDegrees(stateProvider);
                 var cachedCurrentAngle = currentAngle;
                 thPid = new PIDController(
                     () => cachedCurrentAngle,
-                    p.Kp);
+                    pidParameters.Kp);
-                thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone,
+                thPid.ChangeParameters(
-                    p.OutputUpperThreshold, p.SpeedAccPerSec);
+                    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)
+                        rotationTimeoutSeconds)
                     {
                         throw new TimeoutException(
-                            $"原地自转超过{RotationTimeoutSeconds:F1}s仍未到位。");
+                            $"原地自转超过{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);
                     }
 
-                    currentAngle = ReadCurrentAngleDegrees();
                     cachedCurrentAngle = currentAngle;
-                    var s = thPid.GetResponse(targetAngle, true);
+                    var s = thPid.GetResponse(
+                        targetAngle,
+                        !useRelativeWheelOdometry);
                     var angleErrorDegrees =
-                        (float)AngleMath
+                        useRelativeWheelOdometry
+                            ? targetAngle - currentAngle
+                            : (float)AngleMath
                                 .ShortestDifferenceDegrees(
                                     targetAngle,
                                     currentAngle);
@@ -145,12 +294,14 @@ namespace MultiWheelC
                     // PID进入到位死区后等待其0.3s稳定确认；等待期间
                     // 只清零驱动速度，不清除已经准备好的自转舵角状态。
                     if (Math.Abs(angleErrorDegrees) <=
-                        p.DeadZone)
+                        pidParameters.DeadZone)
                     {
                         CommandAngularSpeedObserver?.Invoke(0f);
                         adapter
                             .StopXYThDrivePreserveSteeringState();
 
+                        // 到位稳定只依赖已经单独校验的航向。
+                        // 位置候选留到停车后处理，避免位置抖动中断航向闭环。
                         if (thPid.IsArrived())
                             break;
 
@@ -162,10 +313,10 @@ namespace MultiWheelC
                     // 避免接近目标时反复出现微小命令但车辆实际不动。
                     if (Math.Abs(s) > 1e-6f &&
                         Math.Abs(s) <
-                            MinimumAngularSpeedDegreesPerSecond)
+                            minimumAngularSpeedDegreesPerSecond)
                     {
                         s = Math.Sign(angleErrorDegrees) *
-                            MinimumAngularSpeedDegreesPerSecond;
+                            minimumAngularSpeedDegreesPerSecond;
                     }
 
                     // PID加速限制在首周期可能暂时输出零；此时保留
@@ -184,17 +335,14 @@ namespace MultiWheelC
                     var interval = now - lastCommandTime;
                     lastCommandTime = now;
 
-                    // PID输出s为deg/s，Shared命令统一使用rad/s。
+                    // PID输出s为deg/s，Shared统一使用车体坐标系Twist2D和rad/s。
-                    // adapter.Send最终调用普通安全版SendXYThSpeed。
                     var omegaRadiansPerSecond =
                         (float)AngleMath.DegreesToRadians(s);
-                    if (!adapter.Send(
+                    if (!adapter.SendBodyTwist(
-                            new ChassisCommand(
-                                PilotDefinition.Self.CarNum,
                             new Twist2D(
                                 0.0,
                                 0.0,
-                                    omegaRadiansPerSecond)),
+                                omegaRadiansPerSecond),
                             interval))
                     {
                         throw new InvalidOperationException(
@@ -204,7 +352,68 @@ namespace MultiWheelC
                     yield return true;
                 }
 
-                Console.WriteLine($"final rotate to {targetAngle}");
+                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
             {
@@ -216,10 +425,14 @@ namespace MultiWheelC
         /// <summary>
         /// 检查原地自转的舵轮准备、最小速度和超时参数是否可执行。
         /// </summary>
-        private void ValidateParameters()
+        private void ValidateParameters(
+            PIDParams pidParameters,
+            float wheelAlignmentToleranceDegrees,
+            float minimumAngularSpeedDegreesPerSecond,
+            float rotationTimeoutSeconds)
         {
             EnsureFinitePositive(
-                WheelAlignmentToleranceDegrees,
+                wheelAlignmentToleranceDegrees,
                 nameof(WheelAlignmentToleranceDegrees),
                 allowZero: true);
             EnsureFinitePositive(
@@ -230,13 +443,39 @@ namespace MultiWheelC
                 WheelAlignmentTimeoutSeconds,
                 nameof(WheelAlignmentTimeoutSeconds));
             EnsureFinitePositive(
-                MinimumAngularSpeedDegreesPerSecond,
+                minimumAngularSpeedDegreesPerSecond,
                 nameof(MinimumAngularSpeedDegreesPerSecond));
             EnsureFinitePositive(
-                RotationTimeoutSeconds,
+                rotationTimeoutSeconds,
                 nameof(RotationTimeoutSeconds));
 
-            var pidParameters = PidparamsRead();
+            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(
@@ -256,7 +495,7 @@ namespace MultiWheelC
                 pidParameters.Kp,
                 "PidparamsRead.Kp");
 
-            if (MinimumAngularSpeedDegreesPerSecond >
+            if (minimumAngularSpeedDegreesPerSecond >
                 pidParameters.OutputUpperThreshold)
             {
                 throw new InvalidOperationException(
@@ -267,7 +506,8 @@ namespace MultiWheelC
         /// <summary>
         /// 读取经过状态源校验的世界航向，显式设置ThetaReader时优先使用替代读数。
         /// </summary>
-        private float ReadCurrentAngleDegrees()
+        private float ReadCurrentAngleDegrees(
+            IVehicleStateProvider stateProvider)
         {
             if (ThetaReader != null)
             {
@@ -283,20 +523,115 @@ namespace MultiWheelC
                     angleDegrees);
             }
 
-            if (StateProvider == null ||
+            if (stateProvider is
-                !StateProvider.TryGetState(out var state))
+                WheelFeedbackVehicleStateProvider wheelProvider)
+            {
+                if (!wheelProvider.TryGetHeadingRadians(
+                        out var wheelHeadingRadians))
                 {
                     throw new InvalidOperationException(
                         "无法从Detour状态源读取有效车辆航向。" +
-                    (StateProvider is DetourVehicleStateProvider provider
+                        wheelProvider.LastHeadingFailureReason);
-                        ? provider.LastFailureReason
+                }
-                        : ""));
+
+                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>

MultiWheelC/Movements/TrajectoryTrackingMovement.cs

@@ -4,6 +4,7 @@ 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;
@@ -20,121 +21,175 @@ namespace MultiWheelC
     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状态源。
+        /// 获取或设置本次动作使用的车辆状态源；为空时组合Detour位姿与电机反馈速度。
         /// </summary>
         public IVehicleStateProvider StateProvider;
 
+        /// <summary>
+        /// 获取或设置横向控制器创建委托；参数为车辆控制点半径（m），为空时使用配置化Stanley控制器。
+        /// </summary>
+        public Func<double, ILateralController>
+            LateralControllerFactory;
+
         /// <summary>
         /// 获取或设置每个有效控制周期结束后的诊断数据观察回调。
         /// </summary>
         public Action<ParkingGeometricController> CycleObserver;
 
         /// <summary>
-        /// Stanley横向误差增益，单位为1/s。
+        /// 获取或设置本动作运动坐标系X轴在车体系中的方向，单位为rad；为空时从轨迹自动推导。
         /// </summary>
-        public double StanleyCrossTrackGainPerSecond = 0.4;
+        public double? MotionDirectionInBodyRadians = 0.0;
 
         /// <summary>
-        /// Stanley航向误差增益。
+        /// 获取本次执行最终采用的运动坐标系方向，动作尚未开始时为空。
         /// </summary>
-        public double StanleyHeadingErrorGain = 1.0;
+        public double? ResolvedMotionDirectionInBodyRadians
+        {
+            get;
+            private set;
+        }
 
         /// <summary>
-        /// Stanley低速分母保护速度，单位为m/s。
+        /// 获取或设置轨迹正常完成后是否停车并将舵轮主动恢复到车头方向。
         /// </summary>
-        public double StanleyMinimumSpeedMetersPerSecond = 0.15;
+        public bool ReturnWheelsForwardAfterCompletion;
 
         /// <summary>
-        /// 获取或设置Stanley是否优先使用Detour估算的实际速度。
+        /// 获取或设置本次动作的Stanley横向误差增益覆盖值，单位为1/s；为空时读取车辆配置。
         /// </summary>
-        public bool StanleyUsesActualSpeed = true;
+        public double? StanleyCrossTrackGainPerSecond;
 
         /// <summary>
-        /// Stanley横向误差共同转角分量的最大绝对值，单位为rad。
+        /// 获取或设置本次动作的Stanley航向误差增益覆盖值；为空时读取车辆配置。
         /// </summary>
-        public double MaximumCrossTrackCorrectionRadians =
+        public double? StanleyHeadingErrorGain;
-            AngleMath.DegreesToRadians(10.0);
 
         /// <summary>
-        /// Stanley航向误差差动转角分量的最大绝对值，单位为rad。
+        /// 获取或设置本次动作的Stanley低速分母保护速度覆盖值，单位为m/s；为空时读取车辆配置。
         /// </summary>
-        public double MaximumHeadingCorrectionRadians =
+        public double? StanleyMinimumSpeedMetersPerSecond;
-            AngleMath.DegreesToRadians(10.0);
 
         /// <summary>
-        /// 纵向速度外环比例增益。
+        /// 获取或设置本次动作是否使用实际纵向速度的覆盖值；为空时读取车辆配置。
         /// </summary>
-        public double LongitudinalKp = 0.5;
+        public bool? StanleyUsesActualSpeed;
 
         /// <summary>
-        /// 纵向速度外环积分增益，单位为1/s。
+        /// 获取或设置本次动作的Stanley曲率前馈预瞄时间覆盖值，单位为s，0为关闭；为空时读取车辆配置。
         /// </summary>
-        public double LongitudinalKiPerSecond;
+        public double? StanleyCurvaturePreviewSeconds;
 
         /// <summary>
-        /// 纵向速度外环微分增益，单位为s。
+        /// 获取或设置本次动作的Stanley曲率前馈最大预瞄距离覆盖值，单位为m；为空时读取车辆配置。
         /// </summary>
-        public double LongitudinalKdSeconds;
+        public double? StanleyMaximumCurvaturePreviewMeters;
 
         /// <summary>
-        /// 纵向积分项允许产生的最大速度修正绝对值，单位为m/s。
+        /// 获取或设置本次动作的Stanley横向修正上限覆盖值，单位为rad；为空时读取车辆配置。
         /// </summary>
-        public double MaximumIntegralCorrectionMetersPerSecond = 0.05;
+        public double? MaximumCrossTrackCorrectionRadians;
 
         /// <summary>
-        /// 纵向PID不进行反馈修正的速度误差死区，单位为m/s。
+        /// 获取或设置本次动作的Stanley航向修正上限覆盖值，单位为rad；为空时读取车辆配置。
         /// </summary>
-        public double LongitudinalSpeedErrorDeadbandMetersPerSecond =
+        public double? MaximumHeadingCorrectionRadians;
-            0.025;
 
         /// <summary>
-        /// 底盘纵向命令速度绝对值上限，单位为m/s。
+        /// 获取或设置本次动作的纵向速度比例增益覆盖值；为空时读取车辆配置。
         /// </summary>
-        public double MaximumCommandSpeedMetersPerSecond = 0.50;
+        public double? LongitudinalKp;
 
         /// <summary>
-        /// 前后GCP允许的最大转角绝对值，单位为rad。
+        /// 获取或设置本次动作的纵向速度积分增益覆盖值，单位为1/s；为空时读取车辆配置。
         /// </summary>
-        public double MaximumGcpAngleRadians =
+        public double? LongitudinalKiPerSecond;
-            AngleMath.DegreesToRadians(45.0);
 
         /// <summary>
-        /// 前后GCP目标转角最大变化率，单位为rad/s。
+        /// 获取或设置本次动作的纵向速度微分增益覆盖值，单位为s；为空时读取车辆配置。
         /// </summary>
-        public double MaximumGcpAngleRateRadiansPerSecond =
+        public double? LongitudinalKdSeconds;
-            AngleMath.DegreesToRadians(15.0);
 
         /// <summary>
-        /// 终点位置和剩余弧长的完成容差，单位为m。
+        /// 获取或设置本次动作的纵向积分修正上限覆盖值，单位为m/s；为空时读取车辆配置。
         /// </summary>
-        public double FinishDistanceMeters = 0.03;
+        public double? MaximumIntegralCorrectionMetersPerSecond;
 
         /// <summary>
-        /// 终点停稳判定允许的实际线速度，单位为m/s。
+        /// 获取或设置本次动作的纵向速度误差死区覆盖值，单位为m/s；为空时读取车辆配置。
         /// </summary>
-        public double FinishSpeedMetersPerSecond = 0.02;
+        public double? LongitudinalSpeedErrorDeadbandMetersPerSecond;
 
         /// <summary>
-        /// 终点航向完成容差，单位为rad。
+        /// 获取或设置本次动作的底盘纵向命令速度上限覆盖值，单位为m/s；为空时读取车辆配置。
         /// </summary>
-        public double FinishHeadingToleranceRadians =
+        public double? MaximumCommandSpeedMetersPerSecond;
-            AngleMath.DegreesToRadians(3.0);
 
         /// <summary>
-        /// 车辆允许偏离参考轨迹的最大欧氏距离，单位为m。
+        /// 获取或设置本次动作的GCP转角上限覆盖值，单位为rad；为空时读取车辆配置。
         /// </summary>
-        public double MaximumDistanceToTrajectoryMeters = 0.50;
+        public double? MaximumGcpAngleRadians;
 
         /// <summary>
-        /// 单次轨迹动作允许的最长执行时间，单位为s。
+        /// 获取或设置本次动作的GCP转角变化率上限覆盖值，单位为rad/s；为空时读取车辆配置。
         /// </summary>
-        public double ExecutionTimeoutSeconds = 120.0;
+        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>
         /// 获取本次动作创建的控制器，尚未开始时为空。
@@ -142,11 +197,99 @@ namespace MultiWheelC
         public ParkingGeometricController Controller { get; private set; }
 
         /// <summary>
-        /// 创建控制器并持续执行控制周期，直到轨迹完成、失败或动作被取消。
+        /// 等待舵轮稳定回正后创建控制器并持续执行，直到轨迹完成、失败或动作被取消。
         /// </summary>
         public override IEnumerable<bool> Get()
         {
-            ValidateParameters();
+            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;
@@ -156,43 +299,76 @@ namespace MultiWheelC
                     "当前底盘不是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);
 
-            // 新版GCP控制统一以真实车头为车体X正方向，避免继承上一次蟹行偏置。
+            adapter.ActivateMotionFrame(
-            adapter.ResetToBodyFrame();
+                motionDirectionInBodyRadians);
 
             var stateProvider =
                 StateProvider ??
-                new DetourVehicleStateProvider();
+                ParkingVehicleStateProviderFactory.Create(
+                    chassis,
+                    config);
             var controlPointRadiusMeters =
                 chassis.ControlPointRadius / 1000.0;
 
             var lateralController =
-                new StanleyLateralController(
+                LateralControllerFactory == null
+                    ? new StanleyLateralController(
                         controlPointRadiusMeters,
-                    StanleyCrossTrackGainPerSecond,
+                        stanleyCrossTrackGainPerSecond,
-                    StanleyHeadingErrorGain,
+                        stanleyHeadingErrorGain,
-                    StanleyMinimumSpeedMetersPerSecond,
+                        stanleyMinimumSpeedMetersPerSecond,
-                    StanleyUsesActualSpeed,
+                        stanleyUsesActualSpeed,
-                    MaximumCrossTrackCorrectionRadians,
+                        maximumCrossTrackCorrectionRadians,
-                    MaximumHeadingCorrectionRadians);
+                        maximumHeadingCorrectionRadians)
+                    : LateralControllerFactory(
+                        controlPointRadiusMeters);
+            if (lateralController == null)
+            {
+                throw new InvalidOperationException(
+                    "横向控制器创建委托不能返回空值。");
+            }
             var longitudinalController =
                 new PidLongitudinalController(
-                    LongitudinalKp,
+                    longitudinalKp,
-                    LongitudinalKiPerSecond,
+                    longitudinalKiPerSecond,
-                    LongitudinalKdSeconds,
+                    longitudinalKdSeconds,
-                    MaximumIntegralCorrectionMetersPerSecond,
+                    maximumIntegralCorrectionMetersPerSecond,
-                    MaximumCommandSpeedMetersPerSecond,
+                    maximumCommandSpeedMetersPerSecond,
-                    LongitudinalSpeedErrorDeadbandMetersPerSecond);
+                    longitudinalSpeedErrorDeadbandMetersPerSecond);
             var gcpAllocator =
                 new GcpCommandAllocator(
-                    MaximumGcpAngleRadians);
+                    maximumGcpAngleRadians);
             var commandExecutor =
                 new GcpCommandExecutor(
                     adapter,
-                    MaximumGcpAngleRateRadiansPerSecond);
+                    maximumGcpAngleRateRadiansPerSecond,
+                    motionDirectionInBodyRadians);
 
             Controller = new ParkingGeometricController(
                 stateProvider,
@@ -200,10 +376,17 @@ namespace MultiWheelC
                 longitudinalController,
                 gcpAllocator,
                 commandExecutor,
-                FinishDistanceMeters,
+                finishDistanceMeters,
-                FinishSpeedMetersPerSecond,
+                finishSpeedMetersPerSecond,
-                FinishHeadingToleranceRadians,
+                finishHeadingToleranceRadians,
-                MaximumDistanceToTrajectoryMeters);
+                maximumDistanceToTrajectoryMeters,
+                terminalBrakingPreviewMeters,
+                terminalApproachDistanceMeters,
+                terminalApproachGainPerSecond,
+                maximumTerminalApproachSpeedMetersPerSecond,
+                stanleyCurvaturePreviewSeconds,
+                stanleyMaximumCurvaturePreviewMeters,
+                motionDirectionInBodyRadians);
 
             var clock = Stopwatch.StartNew();
             var previousCycleSeconds =
@@ -215,10 +398,10 @@ namespace MultiWheelC
                 while (true)
                 {
                     if (clock.Elapsed.TotalSeconds >
-                        ExecutionTimeoutSeconds)
+                        executionTimeoutSeconds)
                     {
                         throw new TimeoutException(
-                            $"新版轨迹跟踪超过{ExecutionTimeoutSeconds:F1}s仍未完成。");
+                            $"新版轨迹跟踪超过{executionTimeoutSeconds:F1}s仍未完成。");
                     }
 
                     var currentCycleSeconds =
@@ -240,10 +423,9 @@ namespace MultiWheelC
                         Controller.ExecuteCycle(
                             deltaTimeSeconds);
 
-                    if (Controller.LastVehicleState.HasValue)
+                    // 诊断观察器按真实控制周期触发，即使本周期状态不可用，
-                    {
+                    // 也允许记录状态读取和主动停车所消耗的时间。
                     CycleObserver?.Invoke(Controller);
-                    }
 
                     if (result ==
                         ParkingControlCycleResult.Completed)
@@ -279,13 +461,36 @@ namespace MultiWheelC
                 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()
+        private void ValidateParameters(
+            double executionTimeoutSeconds)
         {
             if (Trajectory == null)
             {
@@ -293,14 +498,130 @@ namespace MultiWheelC
                     "新版轨迹跟踪动作没有设置Trajectory。");
             }
 
-            if (double.IsNaN(ExecutionTimeoutSeconds) ||
+            if (MotionDirectionInBodyRadians.HasValue)
-                double.IsInfinity(ExecutionTimeoutSeconds) ||
+            {
-                ExecutionTimeoutSeconds <= 0.0)
+                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/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,53 +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 = 1.5f;
-
-    [FieldMember(desc = "原地旋转：起转前舵轮对齐精度(deg)")]
-    public float InPlaceRotateWheelAlignDeg = 2f;
-
-    [FieldMember(desc = "原地旋转：旋转过程中舵轮偏差重对齐阈值(deg)")]
-    public float InPlaceRotateActiveWheelAlignDeg = 10f;
-
-    #endregion
-
-    #region 单车-临时
-    [FieldMember(desc = "原地旋转Kp")]
-    public float InPlaceRotateKp = 1.1f;
-
-    [FieldMember(desc = "原地旋转Ki")]
-    public float InPlaceRotateKi = 0f;
-
-    [FieldMember(desc = "原地旋转Kd")]
-    public float InPlaceRotateKd = 0f;
-
-    [FieldMember(desc = "原地旋转积分限幅")]
-    public float InPlaceRotateMaxI = 0f;
-
-    [FieldMember(desc = "原地旋转最小有效角速度(deg/s)")]
-    public float InPlaceRotateMinimumSpeed = 1f;
-
-    [FieldMember(desc = "原地旋转最大角速度(deg/s)")]
-    public float InPlaceRotateMaxSpeed = 47.5f;
-
-    [FieldMember(desc = "原地旋转角加速度(deg/s²)")]
-    public float InPlaceRotateAcc = 60f;
-
-    [FieldMember(desc = "原地旋转超时(s)")]
-    public float InPlaceRotateTimeoutSec = 15f;
-    #endregion
-
-
-
-
     #region 单车-钻车与夹抱
     [FieldMember(desc = "2腿检测：雷达名（逗号分隔可多个）")]
     public string TwoLegLidarName = "rear_left_lidar_1,rear_right_lidar_1";

MultiWheelC/StateEstimation/FirstOrderLowPassFilter.cs

@@ -1,4 +1,4 @@
-using System;
+using MyParking.Shared;
 
 namespace MultiWheelC.StateEstimation
 {
@@ -17,7 +17,7 @@ namespace MultiWheelC.StateEstimation
         public FirstOrderLowPassFilter(
             double timeConstantSeconds)
         {
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 timeConstantSeconds,
                 nameof(timeConstantSeconds));
 
@@ -25,35 +25,6 @@ namespace MultiWheelC.StateEstimation
                 timeConstantSeconds;
         }
 
-        /// <summary>
-        /// 获取滤波时间常数，单位为s；数值越大，滤波越强但响应越慢。
-        /// </summary>
-        public double TimeConstantSeconds =>
-            _timeConstantSeconds;
-
-        /// <summary>
-        /// 获取滤波器是否已经接收过有效初值。
-        /// </summary>
-        public bool IsInitialized =>
-            _isInitialized;
-
-        /// <summary>
-        /// 获取当前滤波输出；尚未初始化时读取会抛出异常。
-        /// </summary>
-        public double Value
-        {
-            get
-            {
-                if (!_isInitialized)
-                {
-                    throw new InvalidOperationException(
-                        "一阶低通滤波器尚未初始化。");
-                }
-
-                return _value;
-            }
-        }
-
         /// <summary>
         /// 使用当前输入和真实采样间隔更新滤波结果。
         /// </summary>
@@ -61,10 +32,10 @@ namespace MultiWheelC.StateEstimation
             double input,
             double deltaTimeSeconds)
         {
-            EnsureFinite(
+            NumericGuard.EnsureFinite(
                 input,
                 nameof(input));
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 deltaTimeSeconds,
                 nameof(deltaTimeSeconds));
 
@@ -98,7 +69,7 @@ namespace MultiWheelC.StateEstimation
         /// </summary>
         public void Reset(double initialValue)
         {
-            EnsureFinite(
+            NumericGuard.EnsureFinite(
                 initialValue,
                 nameof(initialValue));
 
@@ -106,37 +77,5 @@ namespace MultiWheelC.StateEstimation
             _isInitialized = true;
         }
 
-        /// <summary>
-        /// 检查数值是否为正有限值。
-        /// </summary>
-        private static void EnsureFinitePositive(
-            double value,
-            string parameterName)
-        {
-            EnsureFinite(value, parameterName);
-
-            if (value <= 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "滤波时间常数和采样间隔必须是正有限值。");
-            }
-        }
-
-        /// <summary>
-        /// 检查数值是否为有限值。
-        /// </summary>
-        private static void EnsureFinite(
-            double value,
-            string parameterName)
-        {
-            if (double.IsNaN(value) ||
-                double.IsInfinity(value))
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "滤波输入必须是有限值。");
-            }
-        }
     }
 }

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

@@ -1,4 +1,3 @@
-using System;
 using MyParking.Shared;
 
 namespace MultiWheelC.StateEstimation
@@ -17,13 +16,13 @@ namespace MultiWheelC.StateEstimation
             Twist2D twistInWorld,
             bool hasValidVelocityEstimate)
         {
-            EnsureFiniteNonNegative(
+            NumericGuard.EnsureFiniteNonNegative(
                 sampleTimestampSeconds,
                 nameof(sampleTimestampSeconds));
-            EnsureFinitePose(
+            NumericGuard.EnsureFinite(
                 poseInWorld,
                 nameof(poseInWorld));
-            EnsureFiniteTwist(
+            NumericGuard.EnsureFinite(
                 twistInWorld,
                 nameof(twistInWorld));
 
@@ -58,7 +57,8 @@ namespace MultiWheelC.StateEstimation
         public double SampleTimestampSeconds { get; }
 
         /// <summary>
-        /// 获取车体中心在Detour世界坐标系中的位姿，单位为m和rad。
+        /// 获取车体中心在状态源输出世界坐标系中的位姿，单位为m和rad。
+        /// Detour发生经确认的小幅坐标跳变后，该坐标系会保持任务内连续。
         /// </summary>
         public Pose2D PoseInWorld { get; }
 
@@ -73,66 +73,9 @@ namespace MultiWheelC.StateEstimation
         public Twist2D TwistInBody { get; }
 
         /// <summary>
-        /// 获取当前速度是否已由至少两个连续有效定位样本估算得到。
+        /// 获取当前速度估计是否已经初始化并可用于闭环控制。
         /// </summary>
         public bool HasValidVelocityEstimate { get; }
 
-        /// <summary>
-        /// 检查位姿是否由有限数值组成。
-        /// </summary>
-        private static void EnsureFinitePose(
-            Pose2D pose,
-            string parameterName)
-        {
-            if (!IsFinite(pose.XMeters) ||
-                !IsFinite(pose.YMeters) ||
-                !IsFinite(pose.YawRadians))
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "车辆位姿必须由有限数值组成。");
-            }
-        }
-
-        /// <summary>
-        /// 检查速度是否由有限数值组成。
-        /// </summary>
-        private static void EnsureFiniteTwist(
-            Twist2D twist,
-            string parameterName)
-        {
-            if (!IsFinite(twist.VxMetersPerSecond) ||
-                !IsFinite(twist.VyMetersPerSecond) ||
-                !IsFinite(twist.OmegaRadiansPerSecond))
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "车辆速度必须由有限数值组成。");
-            }
-        }
-
-        /// <summary>
-        /// 检查数值是否为非负有限值。
-        /// </summary>
-        private static void EnsureFiniteNonNegative(
-            double value,
-            string parameterName)
-        {
-            if (!IsFinite(value) || value < 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "采样时刻必须是非负有限值。");
-            }
-        }
-
-        /// <summary>
-        /// 判断数值是否可用于车辆状态计算。
-        /// </summary>
-        private static bool IsFinite(double value)
-        {
-            return !double.IsNaN(value) &&
-                   !double.IsInfinity(value);
-        }
     }
 }

MultiWheelC/StateEstimation/VelocityEstimator2D.cs

@@ -52,12 +52,6 @@ namespace MultiWheelC.StateEstimation
                     angularFilterTimeConstantSeconds);
         }
 
-        /// <summary>
-        /// 获取是否已经保存了可用于下一次差分的位姿基准。
-        /// </summary>
-        public bool HasPreviousSample =>
-            _hasPreviousSample;
-
         /// <summary>
         /// 使用一个新的有效定位样本更新并返回车辆状态。
         /// </summary>
@@ -65,10 +59,10 @@ namespace MultiWheelC.StateEstimation
             Pose2D poseInWorld,
             double sampleTimestampSeconds)
         {
-            EnsureFinitePose(
+            NumericGuard.EnsureFinite(
                 poseInWorld,
                 nameof(poseInWorld));
-            EnsureFiniteNonNegative(
+            NumericGuard.EnsureFiniteNonNegative(
                 sampleTimestampSeconds,
                 nameof(sampleTimestampSeconds));
 
@@ -135,53 +129,6 @@ namespace MultiWheelC.StateEstimation
                 true);
         }
 
-        /// <summary>
-        /// 更新位姿差分基准但保留当前滤波速度，避免定位跳变形成虚假速度尖峰。
-        /// </summary>
-        public VehicleState RebasePreservingVelocity(
-            Pose2D poseInWorld,
-            double sampleTimestampSeconds)
-        {
-            EnsureFinitePose(
-                poseInWorld,
-                nameof(poseInWorld));
-            EnsureFiniteNonNegative(
-                sampleTimestampSeconds,
-                nameof(sampleTimestampSeconds));
-
-            var normalizedPoseInWorld =
-                new Pose2D(
-                    poseInWorld.XMeters,
-                    poseInWorld.YMeters,
-                    AngleMath.NormalizeRadians(
-                        poseInWorld.YawRadians));
-
-            _previousPoseInWorld =
-                normalizedPoseInWorld;
-            _previousTimestampSeconds =
-                sampleTimestampSeconds;
-            _hasPreviousSample = true;
-
-            var hasValidVelocityEstimate =
-                _worldVelocityXFilter.IsInitialized &&
-                _worldVelocityYFilter.IsInitialized &&
-                _angularVelocityFilter.IsInitialized;
-
-            var retainedTwistInWorld =
-                hasValidVelocityEstimate
-                    ? new Twist2D(
-                        _worldVelocityXFilter.Value,
-                        _worldVelocityYFilter.Value,
-                        _angularVelocityFilter.Value)
-                    : Twist2D.Zero;
-
-            return new VehicleState(
-                sampleTimestampSeconds,
-                normalizedPoseInWorld,
-                retainedTwistInWorld,
-                hasValidVelocityEstimate);
-        }
-
         /// <summary>
         /// 使用当前定位重新建立差分基准，并返回速度无效的零速状态。
         /// </summary>
@@ -189,10 +136,10 @@ namespace MultiWheelC.StateEstimation
             Pose2D poseInWorld,
             double sampleTimestampSeconds)
         {
-            EnsureFinitePose(
+            NumericGuard.EnsureFinite(
                 poseInWorld,
                 nameof(poseInWorld));
-            EnsureFiniteNonNegative(
+            NumericGuard.EnsureFiniteNonNegative(
                 sampleTimestampSeconds,
                 nameof(sampleTimestampSeconds));
 
@@ -231,45 +178,5 @@ namespace MultiWheelC.StateEstimation
             _angularVelocityFilter.Reset();
         }
 
-        /// <summary>
-        /// 检查位姿是否由有限数值组成。
-        /// </summary>
-        private static void EnsureFinitePose(
-            Pose2D pose,
-            string parameterName)
-        {
-            if (!IsFinite(pose.XMeters) ||
-                !IsFinite(pose.YMeters) ||
-                !IsFinite(pose.YawRadians))
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "速度估计使用的车辆位姿必须由有限数值组成。");
-            }
-        }
-
-        /// <summary>
-        /// 检查数值是否为非负有限值。
-        /// </summary>
-        private static void EnsureFiniteNonNegative(
-            double value,
-            string parameterName)
-        {
-            if (!IsFinite(value) || value < 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "速度估计使用的采样时刻必须是非负有限值。");
-            }
-        }
-
-        /// <summary>
-        /// 判断数值是否可用于速度估计。
-        /// </summary>
-        private static bool IsFinite(double value)
-        {
-            return !double.IsNaN(value) &&
-                   !double.IsInfinity(value);
-        }
     }
 }

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/Trajectory/Trajectory2D.cs

@@ -7,18 +7,22 @@ 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)
@@ -102,7 +106,7 @@ namespace MultiWheelC.Trajectory
         public double GetRemainingDistanceMeters(
             double arcLengthMeters)
         {
-            EnsureFinite(
+            NumericGuard.EnsureFinite(
                 arcLengthMeters,
                 nameof(arcLengthMeters));
 
@@ -121,7 +125,7 @@ namespace MultiWheelC.Trajectory
         public TrajectoryPoint SampleAtArcLength(
             double arcLengthMeters)
         {
-            EnsureFinite(
+            NumericGuard.EnsureFinite(
                 arcLengthMeters,
                 nameof(arcLengthMeters));
 
@@ -148,6 +152,48 @@ namespace MultiWheelC.Trajectory
                 (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(
@@ -176,9 +222,23 @@ namespace MultiWheelC.Trajectory
         /// <summary>
         /// 使用二分查找获取包含指定累计弧长的线段起点索引。
         /// </summary>
-        private int FindSegmentStartIndex(
+        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;
 
@@ -239,21 +299,32 @@ namespace MultiWheelC.Trajectory
                     $"轨迹点{currentIndex}与前一个点的位置过近，无法构成有效投影线段。",
                     parameterName);
             }
-        }
 
-        /// <summary>
+            var segmentLengthMeters =
-        /// 检查数值是否为有限值。
+                Math.Sqrt(segmentLengthSquared);
-        /// </summary>
+            var arcLengthIncrementMeters =
-        private static void EnsureFinite(
+                current.ArcLengthMeters -
-            double value,
+                previous.ArcLengthMeters;
-            string parameterName)
+            var maximumAllowedDifferenceMeters =
+                Math.Max(
+                    ArcLengthConsistencyAbsoluteToleranceMeters,
+                    ArcLengthConsistencyRelativeTolerance *
+                    Math.Max(
+                        segmentLengthMeters,
+                        arcLengthIncrementMeters));
+
+            // 当前轨迹在相邻采样点之间按直线段投影，因此累计弧长增量
+            // 必须与该离散线段长度近似一致，防止进度和实际几何脱节。
+            if (Math.Abs(
+                    arcLengthIncrementMeters -
+                    segmentLengthMeters) >
+                maximumAllowedDifferenceMeters)
             {
-            if (double.IsNaN(value) ||
+                throw new ArgumentException(
-                double.IsInfinity(value))
+                    $"轨迹点{currentIndex}的累计弧长增量" +
-            {
+                    $"{arcLengthIncrementMeters:F6}m与离散线段长度" +
-                throw new ArgumentOutOfRangeException(
+                    $"{segmentLengthMeters:F6}m不一致。",
-                    parameterName,
+                    parameterName);
-                    "轨迹弧长必须是有限值。");
             }
         }
     }

MultiWheelC/Trajectory/TrajectoryPoint.cs

@@ -4,7 +4,7 @@ using MyParking.Shared;
 namespace MultiWheelC.Trajectory
 {
     /// <summary>
-    /// 描述按弧长参数化的车体中心参考轨迹点，统一使用SI单位。
+    /// 描述按执行点序和累计弧长参数化的车体中心参考轨迹点，统一使用SI单位。
     /// </summary>
     public readonly struct TrajectoryPoint
     {
@@ -17,22 +17,16 @@ namespace MultiWheelC.Trajectory
             double curvaturePerMeter,
             double referenceSpeedMetersPerSecond)
         {
-            EnsureFiniteNonNegative(
+            NumericGuard.EnsureFiniteNonNegative(
                 arcLengthMeters,
                 nameof(arcLengthMeters));
-            EnsureFinite(
+            NumericGuard.EnsureFinite(
-                poseInWorld.XMeters,
+                poseInWorld,
                 nameof(poseInWorld));
-            EnsureFinite(
+            NumericGuard.EnsureFinite(
-                poseInWorld.YMeters,
-                nameof(poseInWorld));
-            EnsureFinite(
-                poseInWorld.YawRadians,
-                nameof(poseInWorld));
-            EnsureFinite(
                 curvaturePerMeter,
                 nameof(curvaturePerMeter));
-            EnsureFinite(
+            NumericGuard.EnsureFinite(
                 referenceSpeedMetersPerSecond,
                 nameof(referenceSpeedMetersPerSecond));
             ArcLengthMeters = arcLengthMeters;
@@ -47,56 +41,24 @@ namespace MultiWheelC.Trajectory
         }
 
         /// <summary>
-        /// 获取从轨迹起点累计到当前点的弧长，单位为m。
+        /// 获取沿预定执行点序从轨迹起点累计到当前点的弧长，单位为m。
         /// </summary>
         public double ArcLengthMeters { get; }
 
         /// <summary>
-        /// 获取车体中心参考坐标系在世界坐标系中的位姿。
+        /// 获取车体中心参考坐标系在世界坐标系中的位姿；航向始终表示车头方向。
         /// </summary>
         public Pose2D PoseInWorld { get; }
 
         /// <summary>
-        /// 获取车体中心参考轨迹曲率，单位为1/m，左转为正。
+        /// 获取沿累计弧长增加方向的车体中心参考轨迹曲率，单位为1/m，左弯为正。
         /// </summary>
         public double CurvaturePerMeter { get; }
 
         /// <summary>
-        /// 获取沿轨迹切线方向的有符号参考速度，单位为m/s。
+        /// 获取车体纵向有符号参考速度，单位为m/s；正值前进、负值倒车、零值停车。
         /// </summary>
         public double ReferenceSpeedMetersPerSecond { get; }
 
-        /// <summary>
-        /// 检查数值是否为有限值。
-        /// </summary>
-        private static void EnsureFinite(
-            double value,
-            string parameterName)
-        {
-            if (double.IsNaN(value) ||
-                double.IsInfinity(value))
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "轨迹点参数必须是有限值。");
-            }
-        }
-
-        /// <summary>
-        /// 检查数值是否为非负有限值。
-        /// </summary>
-        private static void EnsureFiniteNonNegative(
-            double value,
-            string parameterName)
-        {
-            EnsureFinite(value, parameterName);
-
-            if (value < 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "轨迹累计弧长不能为负数。");
-            }
-        }
     }
 }

MultiWheelC/Trajectory/TrajectoryProjection.cs

@@ -26,16 +26,16 @@ namespace MultiWheelC.Trajectory
                     "投影线段起点索引不能为负数。");
             }
 
-            EnsureFinite(
+            NumericGuard.EnsureFinite(
                 lateralErrorMeters,
                 nameof(lateralErrorMeters));
-            EnsureFinite(
+            NumericGuard.EnsureFinite(
                 headingErrorRadians,
                 nameof(headingErrorRadians));
-            EnsureFiniteNonNegative(
+            NumericGuard.EnsureFiniteNonNegative(
                 distanceToTrajectoryMeters,
                 nameof(distanceToTrajectoryMeters));
-            EnsureFiniteNonNegative(
+            NumericGuard.EnsureFiniteNonNegative(
                 remainingDistanceMeters,
                 nameof(remainingDistanceMeters));
 
@@ -62,7 +62,7 @@ namespace MultiWheelC.Trajectory
         public TrajectoryPoint ReferencePoint { get; }
 
         /// <summary>
-        /// 获取有符号横向误差，单位为m，参考轨迹位于车辆左侧时为正。
+        /// 获取相对累计弧长增加方向的有符号横向误差，单位为m，参考轨迹位于该方向左侧时为正。
         /// </summary>
         public double LateralErrorMeters { get; }
 
@@ -87,37 +87,5 @@ namespace MultiWheelC.Trajectory
         public double ArcLengthMeters =>
             ReferencePoint.ArcLengthMeters;
 
-        /// <summary>
-        /// 检查数值是否为有限值。
-        /// </summary>
-        private static void EnsureFinite(
-            double value,
-            string parameterName)
-        {
-            if (double.IsNaN(value) ||
-                double.IsInfinity(value))
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "轨迹投影参数必须是有限值。");
-            }
-        }
-
-        /// <summary>
-        /// 检查数值是否为非负有限值。
-        /// </summary>
-        private static void EnsureFiniteNonNegative(
-            double value,
-            string parameterName)
-        {
-            EnsureFinite(value, parameterName);
-
-            if (value < 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "轨迹投影距离不能为负数。");
-            }
-        }
     }
 }

MultiWheelC/Trajectory/TrajectoryProjector.cs

@@ -4,10 +4,13 @@ using MyParking.Shared;
 namespace MultiWheelC.Trajectory
 {
     /// <summary>
-    /// 将Detour给出的实际车体中心位姿投影到二维离散参考轨迹。
+    /// 将实际车体中心位姿投影到二维离散参考轨迹，并支持按上次进度限制搜索范围。
     /// </summary>
     public static class TrajectoryProjector
     {
+        private const double DistanceTieToleranceSquaredMeters =
+            1e-12;
+
         /// <summary>
         /// 在整条轨迹上查找距离实际车体中心最近的线段投影结果。
         /// </summary>
@@ -15,25 +18,98 @@ namespace MultiWheelC.Trajectory
             Trajectory2D trajectory,
             Pose2D vehiclePoseInWorld)
         {
-            if (trajectory == null)
+            ValidateProjectionInput(
-            {
+                trajectory,
-                throw new ArgumentNullException(
-                    nameof(trajectory));
-            }
-
-            EnsureFinitePose(
                 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 bestProjectedX = 0.0;
-            var bestProjectedY = 0.0;
             var bestDistanceSquared =
                 double.PositiveInfinity;
+            var bestProgressDifferenceMeters =
+                double.PositiveInfinity;
 
-            for (var segmentStartIndex = 0;
+            for (var segmentStartIndex =
-                 segmentStartIndex < trajectory.Count - 1;
+                     firstSegmentStartIndex;
+                 segmentStartIndex <=
+                     lastSegmentStartIndex;
                  segmentStartIndex++)
             {
                 var segmentStart =
@@ -85,7 +161,30 @@ namespace MultiWheelC.Trajectory
                     projectionErrorX * projectionErrorX +
                     projectionErrorY * projectionErrorY;
 
-                if (distanceSquared >= bestDistanceSquared)
+                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;
                 }
@@ -94,9 +193,9 @@ namespace MultiWheelC.Trajectory
                     segmentStartIndex;
                 bestInterpolationRatio =
                     interpolationRatio;
-                bestProjectedX = projectedX;
-                bestProjectedY = projectedY;
                 bestDistanceSquared = distanceSquared;
+                bestProgressDifferenceMeters =
+                    progressDifferenceMeters;
             }
 
             return BuildProjection(
@@ -104,8 +203,6 @@ namespace MultiWheelC.Trajectory
                 vehiclePoseInWorld,
                 bestSegmentStartIndex,
                 bestInterpolationRatio,
-                bestProjectedX,
-                bestProjectedY,
                 bestDistanceSquared);
         }
 
@@ -117,45 +214,16 @@ namespace MultiWheelC.Trajectory
             Pose2D vehiclePoseInWorld,
             int segmentStartIndex,
             double interpolationRatio,
-            double projectedX,
-            double projectedY,
             double distanceSquared)
         {
             var segmentStart =
                 trajectory[segmentStartIndex];
             var segmentEnd =
                 trajectory[segmentStartIndex + 1];
-
-            var referenceYawRadians =
-                AngleMath.LerpRadians(
-                    segmentStart.PoseInWorld.YawRadians,
-                    segmentEnd.PoseInWorld.YawRadians,
-                    interpolationRatio);
-            var referenceArcLengthMeters =
-                InterpolationMath.Lerp(
-                    segmentStart.ArcLengthMeters,
-                    segmentEnd.ArcLengthMeters,
-                    interpolationRatio);
-            var referenceCurvaturePerMeter =
-                InterpolationMath.Lerp(
-                    segmentStart.CurvaturePerMeter,
-                    segmentEnd.CurvaturePerMeter,
-                    interpolationRatio);
-            var referenceSpeedMetersPerSecond =
-                InterpolationMath.Lerp(
-                    segmentStart.ReferenceSpeedMetersPerSecond,
-                    segmentEnd.ReferenceSpeedMetersPerSecond,
-                    interpolationRatio);
-
             var referencePoint =
-                new TrajectoryPoint(
+                trajectory.InterpolateSegment(
-                    referenceArcLengthMeters,
+                    segmentStartIndex,
-                    new Pose2D(
+                    interpolationRatio);
-                        projectedX,
-                        projectedY,
-                        referenceYawRadians),
-                    referenceCurvaturePerMeter,
-                    referenceSpeedMetersPerSecond);
 
             var segmentX =
                 segmentEnd.PoseInWorld.XMeters -
@@ -171,10 +239,10 @@ namespace MultiWheelC.Trajectory
             // 以轨迹线段的前进方向判断左右：
             // 从车辆指向参考轨迹的向量位于轨迹左侧时为正。
             var vehicleToProjectionX =
-                projectedX -
+                referencePoint.PoseInWorld.XMeters -
                 vehiclePoseInWorld.XMeters;
             var vehicleToProjectionY =
-                projectedY -
+                referencePoint.PoseInWorld.YMeters -
                 vehiclePoseInWorld.YMeters;
             var lateralErrorMeters =
                 (segmentX * vehicleToProjectionY -
@@ -183,7 +251,7 @@ namespace MultiWheelC.Trajectory
 
             var headingErrorRadians =
                 AngleMath.ShortestDifferenceRadians(
-                    referenceYawRadians,
+                    referencePoint.PoseInWorld.YawRadians,
                     vehiclePoseInWorld.YawRadians);
 
             return new TrajectoryProjection(
@@ -193,27 +261,26 @@ namespace MultiWheelC.Trajectory
                 headingErrorRadians,
                 Math.Sqrt(distanceSquared),
                 trajectory.GetRemainingDistanceMeters(
-                    referenceArcLengthMeters));
+                    referencePoint.ArcLengthMeters));
         }
 
         /// <summary>
-        /// 检查用于投影的实际车体中心位姿是否包含有限数值。
+        /// 检查轨迹对象和用于投影的实际车体中心位姿。
         /// </summary>
-        private static void EnsureFinitePose(
+        private static void ValidateProjectionInput(
+            Trajectory2D trajectory,
             Pose2D pose,
             string parameterName)
         {
-            if (double.IsNaN(pose.XMeters) ||
+            if (trajectory == null)
-                double.IsInfinity(pose.XMeters) ||
-                double.IsNaN(pose.YMeters) ||
-                double.IsInfinity(pose.YMeters) ||
-                double.IsNaN(pose.YawRadians) ||
-                double.IsInfinity(pose.YawRadians))
             {
-                throw new ArgumentOutOfRangeException(
+                throw new ArgumentNullException(
-                    parameterName,
+                    nameof(trajectory));
-                    "用于轨迹投影的车体位姿必须是有限值。");
+            }
-            }
+
+            NumericGuard.EnsureFinite(
+                pose,
+                parameterName);
         }
     }
 }

README.md

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

README_en.md

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

Shared/Chassis/MultiWheelChassisAdapter.cs

@@ -1,58 +1,74 @@
-// 将统一命令转换为原 Chassis API 调用
+// Shared层底盘边界：对外使用SI单位，对内适配旧版MultiWheelChassis的混合单位接口。
 using System;
 using CommonUsage.Chassis;
 
 namespace MyParking.Shared
 {
     /// <summary>
-    /// 将统一的单车车体速度命令转换为旧版MultiWheelChassis调用。
+    /// 将真实车体系刚体速度转换为旧版MultiWheelChassis命令，车体系约定为X向前、Y向左、逆时针为正。
-    /// 车体坐标系固定为X向前、Y向左、逆时针为正。
     /// </summary>
     public sealed class MultiWheelChassisAdapter
     {
         #region 辅助内容
-        private const double RadiansToDegrees = 180.0 / Math.PI;
+
+        // 旧底盘原点偏置使用float角度值，此容差用于判断坐标系是否已经切换到位。
         private const float BiasTolerance = 0.001f;
+
+        // 小于该值的线速度或角速度视为零，避免在静止附近进入方向不确定的运动学分支。
+        private const double MotionDeadband = 1e-6;
+
         private readonly MultiWheelChassis _chassis;
+
+        // β：当前运动系X轴相对真实车体X轴的逆时针夹角，单位为rad。
+        private double _activeMotionDirectionRadians;
+
         /// <summary>
         /// 当前适配器对应的车辆编号。
         /// </summary>
         public int VehicleId { get; }
 
         /// <summary>
-        /// Maximum distance from the body origin to a wheel center, in metres.
+        /// 车体原点到最远舵轮中心的距离，单位为m，用于描述底盘整体外接半径。
         /// </summary>
         public double MaximumWheelRadiusMeters { get; }
 
         /// <summary>
-        /// Maximum longitudinal wheel offset from the body origin, in metres.
+        /// 车体原点到最前或最后舵轮中心的最大纵向距离，单位为m；对称四舵轮底盘中通常为轴距的一半。
-        /// For a symmetric four-wheel-steering chassis this is half the wheelbase.
         /// </summary>
         public double HalfWheelBaseMeters { get; }
 
         /// <summary>
         /// 车体原点到最外侧舵轮中心的最大横向距离，单位为米。
-        /// 对称四舵轮底盘中，它也是蟹行虚拟阿克曼模型的半轴距。
+        /// 对称四舵轮底盘中，它通常等于物理轮距的一半。
         /// </summary>
         public double HalfTrackWidthMeters { get; }
 
         /// <summary>
-        /// Width of the steering-alignment speed gate, in degrees.
+        /// 获取旧版SendMotion使用的对称前后GCP半径，单位为m。
+        /// </summary>
+        public double ControlPointRadiusMeters =>
+            _chassis.ControlPointRadius / 1000.0;
+
+        /// <summary>
+        /// 获取当前已经准备并激活的滚动运动系X轴在真实车体系中的方向，单位为rad。
+        /// </summary>
+        public double ActiveMotionDirectionRadians =>
+            _activeMotionDirectionRadians;
+
+        /// <summary>
+        /// 舵角误差高斯降速门控的宽度，单位为deg；数值越小，舵轮未对齐时驱动降速越明显。
         /// </summary>
         public double SteeringAlignmentSigmaDegrees
         {
             get => _chassis.SteeringAlignmentSigmaDegrees;
             set
             {
-                if (double.IsNaN(value) ||
+                NumericGuard.EnsureFinitePositive(
-                    double.IsInfinity(value) ||
+                    value,
-                    value <= 0.0 ||
+                    nameof(value));
-                    value > float.MaxValue)
+                EnsureRepresentableAsSingle(
-                {
+                    value,
-                    throw new ArgumentOutOfRangeException(
+                    nameof(value));
-                        nameof(value),
-                        "Steering alignment sigma must be a positive finite value.");
-                }
 
                 _chassis.SteeringAlignmentSigmaDegrees =
                     (float)value;
@@ -68,24 +84,24 @@ namespace MyParking.Shared
         }
 
         /// <summary>
-        /// 检查旧底盘当前是否处于指定的运动坐标系。
+        /// 检查旧底盘是否处于指定β运动坐标系，防止准备状态与当前命令使用的坐标系不一致。
-        /// motionDirectionRadians表示该运动系X轴在真实车体坐标系中的方向。
         /// </summary>
+        /// <param name="motionDirectionRadians">运动系X轴在真实车体系中的方向，单位为rad。</param>
         private void EnsureMotionFrameIsActive(
             double motionDirectionRadians)
         {
-            ValidateFinite(
+            NumericGuard.EnsureFinite(
                 motionDirectionRadians,
                 nameof(motionDirectionRadians));
 
             var expectedBiasDegrees =
-                (float)(
+                ConvertRadiansToSingleDegrees(
-                    -FrameTransform2D.NormalizeAngle(
+                    -AngleMath.NormalizeRadians(
-                        motionDirectionRadians) *
+                        motionDirectionRadians),
-                    RadiansToDegrees);
+                    nameof(motionDirectionRadians));
             var bias = _chassis.GetOriginBias();
             var angleErrorDegrees =
-                NormalizeDegrees(
+                AngleMath.NormalizeDegrees(
                     bias.Z - expectedBiasDegrees);
 
             if (Math.Abs(bias.X) <= BiasTolerance &&
@@ -102,46 +118,33 @@ namespace MyParking.Shared
                 $"期望Th={expectedBiasDegrees}°。");
         }
 
-        /// <summary>
-        /// 将角度归一化到[-180°,180°]附近。
-        /// </summary>
-        private static float NormalizeDegrees(float degrees)
-        {
-            return (float)(
-                degrees -
-                Math.Round(degrees / 360.0) * 360.0);
-        }
         /// <summary>
         /// 检查底盘命令是否包含无效数值。
         /// </summary>
         private static void ValidateTwist(Twist2D twist)
         {
-            ValidateFinite(
+            EnsureRepresentableAsSingle(
                 twist.VxMetersPerSecond,
                 nameof(twist.VxMetersPerSecond));
 
-            ValidateFinite(
+            EnsureRepresentableAsSingle(
                 twist.VyMetersPerSecond,
                 nameof(twist.VyMetersPerSecond));
 
-            ValidateFinite(
+            EnsureRepresentableAsSingle(
-                twist.OmegaRadiansPerSecond,
+                AngleMath.RadiansToDegrees(
+                    twist.OmegaRadiansPerSecond),
                 nameof(twist.OmegaRadiansPerSecond));
         }
+
         /// <summary>
-        /// 检查数值是否为有限值。
+        /// 检查数值是否为有限值且可安全转换为float。
         /// </summary>
-        private static void ValidateFinite(
+        private static void EnsureRepresentableAsSingle(
             double value,
             string parameterName)
         {
-            if (double.IsNaN(value) ||
+            NumericGuard.EnsureFinite(value, parameterName);
-                double.IsInfinity(value))
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "底盘速度命令不能是NaN或无穷大。");
-            }
 
             if (value > float.MaxValue ||
                 value < -float.MaxValue)
@@ -152,6 +155,21 @@ namespace MyParking.Shared
             }
         }
 
+        /// <summary>
+        /// 将有限弧度值转换为float可表示的角度值。
+        /// </summary>
+        private static float ConvertRadiansToSingleDegrees(
+            double angleRadians,
+            string parameterName)
+        {
+            var angleDegrees =
+                AngleMath.RadiansToDegrees(angleRadians);
+            EnsureRepresentableAsSingle(
+                angleDegrees,
+                parameterName);
+            return (float)angleDegrees;
+        }
+
         /// <summary>
         /// 获取最近一次底盘运动分解失败原因。
         /// </summary>
@@ -169,21 +187,25 @@ namespace MyParking.Shared
         }
 
         /// <summary>
-        /// 激活指定运动方向对应的SendMotion坐标系。
+        /// 激活指定β对应的SendMotion运动坐标系；调用前必须停车并完成该方向的舵轮预对齐。
-        /// 0表示真实车头，正90度表示将车体左侧作为虚拟车头。
         /// </summary>
+        /// <param name="motionDirectionRadians">运动系X轴在真实车体系中的方向，单位为rad；0为车头，π/2为车体左侧。</param>
         public void ActivateMotionFrame(
             double motionDirectionRadians)
         {
-            ValidateFinite(
+            NumericGuard.EnsureFinite(
                 motionDirectionRadians,
                 nameof(motionDirectionRadians));
 
+            var normalizedDirectionRadians =
+                AngleMath.NormalizeRadians(
+                    motionDirectionRadians);
+
+            // 旧底盘以“真实车体系相对运动系”的角度保存偏置，因此符号与β相反。
             var biasDegrees =
-                (float)(
+                ConvertRadiansToSingleDegrees(
-                    -FrameTransform2D.NormalizeAngle(
+                    -normalizedDirectionRadians,
-                        motionDirectionRadians) *
+                    nameof(motionDirectionRadians));
-                    RadiansToDegrees);
             var currentBias =
                 _chassis.GetOriginBias();
 
@@ -192,19 +214,42 @@ namespace MyParking.Shared
                 Math.Abs(currentBias.Y) <=
                     BiasTolerance &&
                 Math.Abs(
-                    NormalizeDegrees(
+                    AngleMath.NormalizeDegrees(
                         currentBias.Z -
                         biasDegrees)) <=
                     BiasTolerance)
             {
+                SetActiveMotionDirection(
+                    normalizedDirectionRadians);
                 return;
             }
 
+            // SetOriginBias会把每个真实轮位重新表达在运动系中，并同步设置舵角零方向；
+            // 车辆本体没有发生虚拟旋转，后续SendMotion仍使用这些真实轮位完成四轮解算。
             _chassis.SetOriginBias(
                 x: 0.0f,
                 y: 0.0f,
                 th: biasDegrees);
+            SetActiveMotionDirection(
+                normalizedDirectionRadians);
         }
+
+        /// <summary>
+        /// 缓存当前运动坐标系方向，供控制周期内转换车体速度。
+        /// </summary>
+        private void SetActiveMotionDirection(
+            double motionDirectionRadians)
+        {
+            _activeMotionDirectionRadians =
+                AngleMath.NormalizeRadians(
+                    motionDirectionRadians);
+        }
+
+        /// <summary>
+        /// 创建旧版底盘的SI单位适配器，并从真实轮位提取车辆几何尺寸。
+        /// </summary>
+        /// <param name="chassis">已经完成舵轮初始化的旧版多舵轮底盘。</param>
+        /// <param name="vehicleId">正整数车辆编号，仅标识该适配器所属车辆。</param>
         public MultiWheelChassisAdapter(MultiWheelChassis chassis, int vehicleId)
         {
             _chassis = chassis ?? throw new ArgumentNullException(nameof(chassis));
@@ -225,8 +270,7 @@ namespace MyParking.Shared
                     "MultiWheelChassis尚未完成舵轮初始化，" +
                     "不能创建底盘适配器。");
             }
-            // 禁用旧版DirectionAngle/ZeroDirection坐标偏置，
+            // 几何尺寸必须取PhysicalPosition，避免受旧底盘当前原点偏置和运动坐标系影响。
-            // 保证SendXYThSpeed直接使用真实车体坐标系。
             var maximumWheelRadiusMillimeters = 0.0;
             var maximumLongitudinalOffsetMillimeters = 0.0;
             var maximumLateralOffsetMillimeters = 0.0;
@@ -254,94 +298,154 @@ namespace MyParking.Shared
 
             if (MaximumWheelRadiusMeters <= 0.0 ||
                 HalfWheelBaseMeters <= 0.0 ||
-                HalfTrackWidthMeters <= 0.0)
+                HalfTrackWidthMeters <= 0.0 ||
+                ControlPointRadiusMeters <= 0.0)
             {
                 throw new InvalidOperationException(
-                    "Wheel positions cannot produce valid chassis dimensions.");
+                    "Wheel positions and ControlPointRadius must produce valid chassis dimensions.");
             }
 
+            var initialBias = _chassis.GetOriginBias();
+            SetActiveMotionDirection(
+                -AngleMath.DegreesToRadians(
+                    initialBias.Z));
+
             // 通过反转轮速表达反向运动，避免蟹行正反切换时舵轮无意义地旋转180°。
             _chassis.PreferMinimumSteeringTravel = true;
         }
 
         /// <summary>
-        /// 将车体坐标系速度命令发送给多舵轮底盘。
+        /// 将真实车体系刚体速度分派为滚动SendMotion、真实车体系纯自转或立即停车命令。
         /// </summary>
-        public bool Send(ChassisCommand command, TimeSpan? interval = null)
+        /// <param name="bodyTwist">真实车体系速度，线速度单位为m/s，角速度单位为rad/s。</param>
-        {
+        /// <param name="interval">与上一条底盘命令的实际时间间隔，用于旧底盘速度和舵角变化率处理。</param>
-            if (command.VehicleId != VehicleId)
+        /// <returns>旧底盘是否成功接受并完成运动分解。</returns>
-            {
+        public bool SendBodyTwist(
-                throw new InvalidOperationException(
+            Twist2D bodyTwist,
-                    $"命令车辆编号{command.VehicleId}与适配器车辆编号" +
-                    $"{VehicleId}不一致。");
-            }
-            ValidateTwist(command.BodyTwist);
-            // 防止其他旧逻辑再次调用DirectionAngle或
-            // SetOriginBias改变底盘坐标语义。
-            EnsureBodyFrameIsActive();
-            var vxMetersPerSecond =
-                            (float)command.BodyTwist.VxMetersPerSecond;
-            var vyMetersPerSecond =
-                (float)command.BodyTwist.VyMetersPerSecond;
-            var omegaDegreesPerSecond =
-                (float)(
-                    command.BodyTwist.OmegaRadiansPerSecond *
-                    RadiansToDegrees);
-            var success = _chassis.SendXYThSpeed(
-                                vxMetersPerSecond,
-                                vyMetersPerSecond,
-                                omegaDegreesPerSecond,
-                                interval,
-                                enableDifferentialSteerFeedforward: true);
-            if (!success)
-            {
-                // 防止分解失败后继续执行上一条运动命令。
-                _chassis.PredefinedDriveStop();
-            }
-            return success;
-        }
-
-
-        /// <summary>
-        /// 在已经激活的运动坐标系中使用SendMotion执行虚拟阿克曼运动。
-        /// 转向角均相对该运动坐标系表达；正90度运动系对应车体左侧蟹行。
-        /// </summary>
-        public bool SendVirtualAckermannMotion(
-            double motionDirectionRadians,
-            double speedMetersPerSecond,
-            double steeringRadians,
             TimeSpan? interval = null)
         {
-            ValidateFinite(
+            ValidateTwist(bodyTwist);
-                motionDirectionRadians,
-                nameof(motionDirectionRadians));
-            ValidateFinite(
-                speedMetersPerSecond,
-                nameof(speedMetersPerSecond));
-            ValidateFinite(
-                steeringRadians,
-                nameof(steeringRadians));
-            EnsureMotionFrameIsActive(
-                motionDirectionRadians);
 
-            if (Math.Abs(steeringRadians) >=
+            var linearSpeedMetersPerSecond =
-                Math.PI / 2.0)
+                Math.Sqrt(
+                    bodyTwist.VxMetersPerSecond *
+                    bodyTwist.VxMetersPerSecond +
+                    bodyTwist.VyMetersPerSecond *
+                    bodyTwist.VyMetersPerSecond);
+
+            if (linearSpeedMetersPerSecond <= MotionDeadband)
             {
-                throw new ArgumentOutOfRangeException(
+                if (Math.Abs(
-                    nameof(steeringRadians),
+                        bodyTwist.OmegaRadiansPerSecond) <=
-                    "虚拟阿克曼转向角必须位于正负90度以内。");
+                    MotionDeadband)
+                {
+                    StopImmediately();
+                    return true;
                 }
 
-            var steeringDegrees =
+                return SendPureRotation(
-                (float)(
+                    bodyTwist.OmegaRadiansPerSecond,
-                    steeringRadians *
-                    RadiansToDegrees);
-            var success =
-                _chassis.SendMotion(
-                    (float)speedMetersPerSecond,
-                    steeringDegrees,
-                    -steeringDegrees,
                     interval);
+            }
+
+            return SendRollingTwistInActiveMotionFrame(
+                bodyTwist,
+                linearSpeedMetersPerSecond,
+                interval);
+        }
+
+        /// <summary>
+        /// 将真实车体系刚体速度转换到已激活的β运动系，并生成该运动系中的前后GCP命令。
+        /// </summary>
+        private bool SendRollingTwistInActiveMotionFrame(
+            Twist2D bodyTwist,
+            double linearSpeedMetersPerSecond,
+            TimeSpan? interval)
+        {
+            EnsureMotionFrameIsActive(
+                _activeMotionDirectionRadians);
+
+            var bodyPoseInMotionFrame =
+                new Pose2D(
+                    0.0,
+                    0.0,
+                    -_activeMotionDirectionRadians);
+
+            // 同一点的速度只需旋转表达坐标系；刚体角速度在二维旋转变换下保持不变。
+            var motionTwist =
+                FrameTransform2D.TransformTwistAtSamePoint(
+                    bodyPoseInMotionFrame,
+                    bodyTwist);
+            var motionVxMetersPerSecond =
+                motionTwist.VxMetersPerSecond;
+            var motionVyMetersPerSecond =
+                motionTwist.VyMetersPerSecond;
+
+            if (Math.Abs(motionVxMetersPerSecond) <=
+                MotionDeadband)
+            {
+                StopImmediately();
+                throw new InvalidOperationException(
+                    "当前车体速度几乎垂直于已经准备的运动坐标系，" +
+                    "无法由方向型前后GCP稳定表示。请停车后按目标主运动方向重新准备并激活β。");
+            }
+
+            var travelDirection =
+                Math.Sign(
+                    motionVxMetersPerSecond);
+
+            // SendMotion用速度符号表达前进/倒车，而GCP角度始终相对当前行驶方向计算。
+            var signedCenterSpeedMetersPerSecond =
+                travelDirection *
+                linearSpeedMetersPerSecond;
+
+            // 刚体速度关系v(point)=v(center)+ω×r；前后GCP位于运动系X轴的±ControlPointRadius处。
+            var frontVelocityYMetersPerSecond =
+                motionVyMetersPerSecond +
+                bodyTwist.OmegaRadiansPerSecond *
+                ControlPointRadiusMeters;
+            var rearVelocityYMetersPerSecond =
+                motionVyMetersPerSecond -
+                bodyTwist.OmegaRadiansPerSecond *
+                ControlPointRadiusMeters;
+            var directedVxMetersPerSecond =
+                travelDirection *
+                motionVxMetersPerSecond;
+            var frontAngleRadians =
+                Math.Atan2(
+                    travelDirection *
+                    frontVelocityYMetersPerSecond,
+                    directedVxMetersPerSecond);
+            var rearAngleRadians =
+                Math.Atan2(
+                    travelDirection *
+                    rearVelocityYMetersPerSecond,
+                    directedVxMetersPerSecond);
+
+            return SendGcpMotionInActiveFrame(
+                signedCenterSpeedMetersPerSecond,
+                frontAngleRadians,
+                rearAngleRadians,
+                interval);
+        }
+
+        /// <summary>
+        /// 将已经完成自转舵轮准备的纯角速度命令交给XYTh底盘解算。
+        /// </summary>
+        private bool SendPureRotation(
+            double omegaRadiansPerSecond,
+            TimeSpan? interval)
+        {
+            EnsureBodyFrameIsActive();
+
+            var success = _chassis.SendXYThSpeed(
+                0.0f,
+                0.0f,
+                ConvertRadiansToSingleDegrees(
+                    omegaRadiansPerSecond,
+                    nameof(omegaRadiansPerSecond)),
+                interval,
+                enableDifferentialSteerFeedforward: true);
 
             if (!success)
             {
@@ -352,24 +456,25 @@ namespace MyParking.Shared
         }
 
         /// <summary>
-        /// 在真实车体坐标系中将有符号速度和独立前后GCP角度发送给旧版SendMotion。
+        /// 在当前已激活的运动坐标系中将有符号速度和前后GCP角度发送给旧版SendMotion。
         /// </summary>
-        public bool SendGcpMotion(
+        private bool SendGcpMotionInActiveFrame(
             double speedMetersPerSecond,
             double frontAngleRadians,
             double rearAngleRadians,
             TimeSpan? interval = null)
         {
-            ValidateFinite(
+            EnsureRepresentableAsSingle(
                 speedMetersPerSecond,
                 nameof(speedMetersPerSecond));
-            ValidateFinite(
+            NumericGuard.EnsureFinite(
                 frontAngleRadians,
                 nameof(frontAngleRadians));
-            ValidateFinite(
+            NumericGuard.EnsureFinite(
                 rearAngleRadians,
                 nameof(rearAngleRadians));
-            EnsureBodyFrameIsActive();
+            EnsureMotionFrameIsActive(
+                _activeMotionDirectionRadians);
 
             if (Math.Abs(frontAngleRadians) >=
                     Math.PI / 2.0 ||
@@ -383,10 +488,12 @@ namespace MyParking.Shared
 
             var success = _chassis.SendMotion(
                 (float)speedMetersPerSecond,
-                (float)(frontAngleRadians *
+                ConvertRadiansToSingleDegrees(
-                        RadiansToDegrees),
+                    frontAngleRadians,
-                (float)(rearAngleRadians *
+                    nameof(frontAngleRadians)),
-                        RadiansToDegrees),
+                ConvertRadiansToSingleDegrees(
+                    rearAngleRadians,
+                    nameof(rearAngleRadians)),
                 interval);
 
             if (!success)
@@ -415,15 +522,18 @@ namespace MyParking.Shared
         }
 
         /// <summary>
-        /// 停车并将所有舵轮转到指定的车体角度。
+        /// 停车并将所有舵轮预对齐到真实车体系中的同一机械方向，不产生车辆线速度。
-        /// 只调整舵轮角度，不产生车辆线速度。
         /// </summary>
+        /// <param name="directionRadians">舵轮相对真实车体X轴的目标方向，单位为rad。</param>
         public bool PrepareParallelDirection(
             double directionRadians)
         {
             EnsureBodyFrameIsActive();
-            var targetDegrees = (float)(FrameTransform2D.NormalizeAngle(directionRadians) *
+            var targetDegrees =
-                                    RadiansToDegrees);
+                ConvertRadiansToSingleDegrees(
+                    AngleMath.NormalizeRadians(
+                        directionRadians),
+                    nameof(directionRadians));
 
 #pragma warning disable CS0612, CS0618
             var wheels = _chassis.GetSteerWheels();
@@ -457,29 +567,27 @@ namespace MyParking.Shared
         }
 
         /// <summary>
-        /// 检查所有舵轮是否已经对准给定方向。
+        /// 检查所有舵轮是否已在给定容差内对准真实车体系中的同一机械方向。
         /// </summary>
         public bool AreParallelWheelsAligned(
             double directionRadians,
             double toleranceRadians)
         {
-            if (double.IsNaN(toleranceRadians) ||
+            NumericGuard.EnsureFiniteNonNegative(
-        double.IsInfinity(toleranceRadians) ||
+                toleranceRadians,
-        toleranceRadians < 0.0)
+                nameof(toleranceRadians));
-            {
-                throw new ArgumentOutOfRangeException(
-                    nameof(toleranceRadians),
-                    "舵轮到位容差必须是非负有限值。");
-            }
 
             EnsureBodyFrameIsActive();
-            var targetDegrees = (float)(
+            var targetDegrees =
-                FrameTransform2D.NormalizeAngle(directionRadians) *
+                ConvertRadiansToSingleDegrees(
-                180.0 / Math.PI);
+                    AngleMath.NormalizeRadians(
+                        directionRadians),
+                    nameof(directionRadians));
 
-            var toleranceDegrees = (float)(
+            var toleranceDegrees =
-                Math.Abs(toleranceRadians) *
+                ConvertRadiansToSingleDegrees(
-                180.0 / Math.PI);
+                    toleranceRadians,
+                    nameof(toleranceRadians));
 
 #pragma warning disable CS0612, CS0618
             var wheels = _chassis.GetSteerWheels();
@@ -487,6 +595,7 @@ namespace MyParking.Shared
 
             foreach (var wheel in wheels)
             {
+                // 机械舵角受限于非环形区间，此处必须比较直接角差，不能使用圆周最短角差。
                 var angleErrorDegrees = targetDegrees - wheel.ReadAngle();
 
                 if (Math.Abs(angleErrorDegrees) >
@@ -507,16 +616,12 @@ namespace MyParking.Shared
             TimeSpan? interval = null,
             double alignmentToleranceDegrees = 2.0)
         {
-            ValidateFinite(
+            NumericGuard.EnsureFiniteNonNegative(
+                alignmentToleranceDegrees,
+                nameof(alignmentToleranceDegrees));
+            EnsureRepresentableAsSingle(
                 alignmentToleranceDegrees,
                 nameof(alignmentToleranceDegrees));
-
-            if (alignmentToleranceDegrees < 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    nameof(alignmentToleranceDegrees),
-                    "自转舵轮到位容差必须是非负有限值。");
-            }
 
             EnsureBodyFrameIsActive();
 
@@ -540,23 +645,18 @@ namespace MyParking.Shared
             double toleranceRadians =
                 2.0 * Math.PI / 180.0)
         {
-            if (double.IsNaN(toleranceRadians) ||
+            NumericGuard.EnsureFiniteNonNegative(
-                double.IsInfinity(toleranceRadians) ||
+                toleranceRadians,
-                toleranceRadians < 0.0)
+                nameof(toleranceRadians));
-            {
-                throw new ArgumentOutOfRangeException(
-                    nameof(toleranceRadians),
-                    "自转状态交接容差必须是非负有限值。");
-            }
 
             EnsureBodyFrameIsActive();
 
             var success =
                 _chassis
                     .AdoptPreparedRotateWheelsForXYTh(
-                        (float)(
+                        ConvertRadiansToSingleDegrees(
-                            toleranceRadians *
+                            toleranceRadians,
-                            RadiansToDegrees));
+                            nameof(toleranceRadians)));
 
             if (!success)
             {
@@ -565,12 +665,10 @@ namespace MyParking.Shared
 
             return success;
         }
+
         /// <summary>
-        /// 所有舵轮是否已对齐到原地自转方向。
+        /// 所有舵轮是否已对齐到最近一次原地自转准备所确定的目标方向。
         /// </summary>
         public bool AreSpinWheelsAligned => _chassis.LastRotateAligned;
-
-
-
     }
 }

Shared/Fleet/FleetKinematics.cs

@@ -1 +1,74 @@
-// 把车队整体速度分解为每辆车的局部速度
+using System;
+using System.Collections.Generic;
+
+namespace MyParking.Shared
+{
+    // 纯数学地把车队参考点速度分解成各成员车体系中的刚体速度。
+    public static class FleetKinematics
+    {
+        public static IReadOnlyList<FleetMemberCommand> Decompose(
+            FleetLayout layout,
+            FleetMotionCommand command)
+        {
+            if (layout == null)
+            {
+                throw new ArgumentNullException(nameof(layout));
+            }
+
+            EnsureCommandIsFinite(command);
+
+            var commands =
+                new FleetMemberCommand[layout.VehicleCount];
+            var referencePoint = command.ReferencePointInFleet;
+            var referenceTwist = command.TwistAtReferencePoint;
+
+            for (var index = 0; index < layout.VehicleCount; index++)
+            {
+                var vehicle = layout.Vehicles[index];
+                var offsetXMeters =
+                    vehicle.PoseInFleet.XMeters -
+                    referencePoint.XMeters;
+                var offsetYMeters =
+                    vehicle.PoseInFleet.YMeters -
+                    referencePoint.YMeters;
+
+                var twistAtVehicleInFleet = new Twist2D(
+                    referenceTwist.VxMetersPerSecond -
+                    referenceTwist.OmegaRadiansPerSecond *
+                    offsetYMeters,
+                    referenceTwist.VyMetersPerSecond +
+                    referenceTwist.OmegaRadiansPerSecond *
+                    offsetXMeters,
+                    referenceTwist.OmegaRadiansPerSecond);
+
+                var fleetPoseInVehicle =
+                    FrameTransform2D.Inverse(
+                        vehicle.PoseInFleet);
+                var twistInVehicleBody =
+                    FrameTransform2D.TransformTwistAtSamePoint(
+                        fleetPoseInVehicle,
+                        twistAtVehicleInFleet);
+
+                commands[index] = new FleetMemberCommand(
+                    vehicle.VehicleId,
+                    twistInVehicleBody);
+            }
+
+            return Array.AsReadOnly(commands);
+        }
+
+        private static void EnsureCommandIsFinite(
+            FleetMotionCommand command)
+        {
+            NumericGuard.EnsureFinite(
+                command.ReferencePointInFleet.XMeters,
+                nameof(command.ReferencePointInFleet));
+            NumericGuard.EnsureFinite(
+                command.ReferencePointInFleet.YMeters,
+                nameof(command.ReferencePointInFleet));
+            NumericGuard.EnsureFinite(
+                command.TwistAtReferencePoint,
+                nameof(command.TwistAtReferencePoint));
+        }
+    }
+}

Shared/Fleet/FleetLayout.cs

@@ -0,0 +1,76 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MyParking.Shared
+{
+    // 编队固定布局快照，车队坐标系原点就是编队参考中心。
+    public sealed class FleetLayout
+    {
+        private readonly ReadOnlyCollection<VehicleLayout> _vehicles;
+
+        public FleetLayout(IReadOnlyList<VehicleLayout> vehicles)
+        {
+            if (vehicles == null)
+            {
+                throw new ArgumentNullException(nameof(vehicles));
+            }
+
+            if (vehicles.Count == 0)
+            {
+                throw new ArgumentException(
+                    "编队布局至少需要包含一辆车。",
+                    nameof(vehicles));
+            }
+
+            var snapshot = new VehicleLayout[vehicles.Count];
+            var vehicleIds = new HashSet<int>();
+
+            for (var index = 0; index < vehicles.Count; index++)
+            {
+                var vehicle = vehicles[index];
+                if (vehicle.VehicleId <= 0)
+                {
+                    throw new ArgumentOutOfRangeException(
+                        nameof(vehicles),
+                        $"第{index}辆车的编号必须大于零。");
+                }
+
+                if (!vehicleIds.Add(vehicle.VehicleId))
+                {
+                    throw new ArgumentException(
+                        $"编队布局包含重复车号{vehicle.VehicleId}。",
+                        nameof(vehicles));
+                }
+
+                NumericGuard.EnsureFinite(
+                    vehicle.PoseInFleet,
+                    $"{nameof(vehicles)}[{index}].{nameof(VehicleLayout.PoseInFleet)}");
+                snapshot[index] = vehicle;
+            }
+
+            _vehicles = Array.AsReadOnly(snapshot);
+        }
+
+        public IReadOnlyList<VehicleLayout> Vehicles => _vehicles;
+
+        public int VehicleCount => _vehicles.Count;
+
+        public bool TryGetVehicle(
+            int vehicleId,
+            out VehicleLayout vehicle)
+        {
+            for (var index = 0; index < _vehicles.Count; index++)
+            {
+                if (_vehicles[index].VehicleId == vehicleId)
+                {
+                    vehicle = _vehicles[index];
+                    return true;
+                }
+            }
+
+            vehicle = default;
+            return false;
+        }
+    }
+}

Shared/Fleet/FleetModels.cs

@@ -0,0 +1,67 @@
+namespace MyParking.Shared
+{
+    // 单车车体系在车队坐标系中的固定位姿。
+    public readonly struct VehicleLayout
+    {
+        public VehicleLayout(int vehicleId, Pose2D poseInFleet)
+        {
+            VehicleId = vehicleId;
+            PoseInFleet = poseInFleet;
+        }
+
+        public int VehicleId { get; }
+
+        public Pose2D PoseInFleet { get; }
+    }
+
+    // 车队参考点及该点在车队坐标系中表达的刚体速度。
+    public readonly struct FleetMotionCommand
+    {
+        public FleetMotionCommand(
+            Point2D referencePointInFleet,
+            Twist2D twistAtReferencePoint)
+        {
+            ReferencePointInFleet = referencePointInFleet;
+            TwistAtReferencePoint = twistAtReferencePoint;
+        }
+
+        public Point2D ReferencePointInFleet { get; }
+
+        public Twist2D TwistAtReferencePoint { get; }
+
+        public static FleetMotionCommand RotateAround(
+            Point2D rotationCenterInFleet,
+            double omegaRadiansPerSecond)
+        {
+            return new FleetMotionCommand(
+                rotationCenterInFleet,
+                new Twist2D(
+                    0.0,
+                    0.0,
+                    omegaRadiansPerSecond));
+        }
+
+        public static FleetMotionCommand Stop()
+        {
+            return new FleetMotionCommand(
+                Point2D.Zero,
+                Twist2D.Zero);
+        }
+    }
+
+    // 分配给指定车辆、在该车车体系中表达的刚体速度。
+    public readonly struct FleetMemberCommand
+    {
+        public FleetMemberCommand(
+            int vehicleId,
+            Twist2D twistInVehicleBody)
+        {
+            VehicleId = vehicleId;
+            TwistInVehicleBody = twistInVehicleBody;
+        }
+
+        public int VehicleId { get; }
+
+        public Twist2D TwistInVehicleBody { get; }
+    }
+}

Shared/Fleet/FleetProtocol.cs

@@ -0,0 +1,141 @@
+// 四舵轮共同搬运协议
+
+namespace MyParking.Shared
+{
+    /// <summary>定义车队无线协议的公共常量。</summary>
+    public static class FleetProtocol
+    {
+        public const int CurrentVersion = 1;
+        public const int BroadcastVehicleId = 0;
+        public const long NoActivePlanId = 0;
+        public const long NoAppliedCommandSequence = 0;
+    }
+
+    /// <summary>表示成员车本地的车队任务执行阶段。</summary>
+    public enum FleetMemberState
+    {
+        Idle = 0,
+        Preparing = 1,
+        Ready = 2,
+        Active = 3,
+        Faulted = 4
+    }
+
+    /// <summary>表示主车要求成员执行的动作。</summary>
+    public enum FleetCommandKind
+    {
+        PrepareRolling = 1,
+        PrepareSpin = 2,
+        Activate = 3,
+        Motion = 4,
+        Stop = 5
+    }
+
+    /// <summary>成员车周期上报给主车的状态快照，同时承担心跳和命令确认。</summary>
+    public readonly struct FleetMemberReport
+    {
+        public FleetMemberReport(
+            int vehicleId,
+            long planId,
+            long sequenceNumber,
+            double sampleTimestampSeconds,
+            Pose2D poseInCommonWorld,
+            Twist2D twistAtVehicleOriginInCommonWorld,
+            bool isStateAvailable,
+            bool hasValidVelocityEstimate,
+            FleetMemberState state,
+            long lastAppliedCommandSequence,
+            int failureCode = 0)
+        {
+            VehicleId = vehicleId;
+            PlanId = planId;
+            SequenceNumber = sequenceNumber;
+            SampleTimestampSeconds = sampleTimestampSeconds;
+            PoseInCommonWorld = poseInCommonWorld;
+            TwistAtVehicleOriginInCommonWorld =
+                twistAtVehicleOriginInCommonWorld;
+            IsStateAvailable = isStateAvailable;
+            HasValidVelocityEstimate =
+                hasValidVelocityEstimate;
+            State = state;
+            LastAppliedCommandSequence =
+                lastAppliedCommandSequence;
+            FailureCode = failureCode;
+        }
+
+        public int VehicleId { get; }
+
+        // 零表示车辆当前不属于活动任务。
+        public long PlanId { get; }
+
+        // 本车上报流中单调递增，用于丢弃乱序旧报文。
+        public long SequenceNumber { get; }
+
+        // 本车单调时钟的采样时刻，通信层负责换算到主车时间轴。
+        public double SampleTimestampSeconds { get; }
+
+        // 位姿必须已经转换到所有成员约定一致的公共世界坐标系。
+        public Pose2D PoseInCommonWorld { get; }
+
+        public Twist2D TwistAtVehicleOriginInCommonWorld { get; }
+
+        public bool IsStateAvailable { get; }
+
+        public bool HasValidVelocityEstimate { get; }
+
+        public FleetMemberState State { get; }
+
+        // 零表示尚未执行任何主车命令。
+        public long LastAppliedCommandSequence { get; }
+
+        // 零表示没有结构化故障码。
+        public int FailureCode { get; }
+    }
+
+    /// <summary>主车向指定成员或全队下发的一条车队任务命令。</summary>
+    public readonly struct FleetCommand
+    {
+        public FleetCommand(
+            long planId,
+            long sequenceNumber,
+            int targetVehicleId,
+            FleetCommandKind kind,
+            double motionDirectionInBodyRadians,
+            Twist2D twistInVehicleBody,
+            double validForSeconds,
+            int reasonCode = 0)
+        {
+            PlanId = planId;
+            SequenceNumber = sequenceNumber;
+            TargetVehicleId = targetVehicleId;
+            Kind = kind;
+            MotionDirectionInBodyRadians =
+                motionDirectionInBodyRadians;
+            TwistInVehicleBody = twistInVehicleBody;
+            ValidForSeconds = validForSeconds;
+            ReasonCode = reasonCode;
+        }
+
+        public long PlanId { get; }
+
+        // 主车命令流中单调递增，成员据此拒绝乱序旧命令。
+        public long SequenceNumber { get; }
+
+        // 零表示广播，正数表示指定成员车。
+        public int TargetVehicleId { get; }
+
+        public FleetCommandKind Kind { get; }
+
+        // 仅PrepareRolling使用，单位rad，车体系X轴到运动X轴逆时针为正。
+        public double MotionDirectionInBodyRadians { get; }
+
+        // 仅Motion使用，采用目标成员车体系。
+        public Twist2D TwistInVehicleBody { get; }
+
+        // 从成员本机收到消息时开始计时，超时后必须停车。
+        public double ValidForSeconds { get; }
+
+        // 零表示没有结构化停止或故障原因。
+        public int ReasonCode { get; }
+    }
+}

Shared/Mathematics/FrameTransform2D.cs

@@ -9,27 +9,6 @@ namespace MyParking.Shared
     /// </summary>
     public static class FrameTransform2D
     {
-        /// <summary>
-        /// 将角度归一化到[-π, π)范围。
-        /// </summary>
-        public static double NormalizeAngle(double angleRadians)
-        {
-            return AngleMath.NormalizeRadians(angleRadians);
-        }
-
-        /// <summary>
-        /// 计算从current到target的最短角度差。
-        /// 返回正值表示逆时针旋转。
-        /// </summary>
-        public static double ShortestAngleDifference(
-            double targetRadians,
-            double currentRadians)
-        {
-            return AngleMath.ShortestDifferenceRadians(
-                targetRadians,
-                currentRadians);
-        }
-
         /// <summary>
         /// 将源坐标系中的点变换到目标坐标系。
         /// sourcePoseInTarget表示源坐标系在目标坐标系中的位姿。
@@ -106,7 +85,7 @@ namespace MyParking.Shared
             return new Pose2D(
                 childPositionInParent.XMeters,
                 childPositionInParent.YMeters,
-                NormalizeAngle(
+                AngleMath.NormalizeRadians(
                     parentFromMiddle.YawRadians +
                     middleFromChild.YawRadians));
         }
@@ -127,7 +106,7 @@ namespace MyParking.Shared
                 sin * childPoseInParent.XMeters -
                 cos * childPoseInParent.YMeters,
 
-                NormalizeAngle(
+                AngleMath.NormalizeRadians(
                     -childPoseInParent.YawRadians));
         }
 

Shared/Models/ChassisCommand.cs

@@ -1,183 +0,0 @@
-// 纯数据层：只描述坐标、速度和命令
-// 定义二维坐标、位姿、速度、车队布局和单车底盘命令。
-// Shared层统一使用SI单位：位置m、线速度m/s、角度rad、角速度rad/s。
-// 车体坐标系采用右手系：X向前、Y向左、逆时针角度和角速度为正。
-// 命名约定：XxxInYyy表示Xxx在Yyy坐标系中的表达。
-
-namespace MyParking.Shared
-{
-    /// <summary>
-    /// 二维坐标点，X、Y单位均为米。
-    /// </summary>
-    public readonly struct Point2D
-    {
-        public Point2D(double xMeters, double yMeters)
-        {
-            XMeters = xMeters;
-            YMeters = yMeters;
-        }
-
-        public double XMeters { get; }
-
-        public double YMeters { get; }
-
-        public static Point2D Zero => new Point2D(0.0, 0.0);
-    }
-
-    /// <summary>
-    /// 二维局部坐标系在父坐标系中的位姿。
-    /// 位置单位为米，朝向单位为弧度，逆时针为正。
-    /// 具体父子关系由变量名称说明，例如RadarPoseInBody。
-    /// </summary>
-    public readonly struct Pose2D
-    {
-        public Pose2D(
-            double xMeters,
-            double yMeters,
-            double yawRadians)
-        {
-            XMeters = xMeters;
-            YMeters = yMeters;
-            YawRadians = yawRadians;
-        }
-
-        public double XMeters { get; }
-
-        public double YMeters { get; }
-
-        public double YawRadians { get; }
-
-        public Point2D Position =>
-            new Point2D(XMeters, YMeters);
-
-        public static Pose2D Identity =>
-            new Pose2D(0.0, 0.0, 0.0);
-    }
-
-    /// <summary>
-    /// 二维刚体速度。
-    /// 线速度单位为m/s，角速度单位为rad/s。
-    /// 速度所属坐标系由持有该Twist2D的外层类型或变量名称确定。
-    /// </summary>
-    public readonly struct Twist2D
-    {
-        public Twist2D(
-            double vxMetersPerSecond,
-            double vyMetersPerSecond,
-            double omegaRadiansPerSecond)
-        {
-            VxMetersPerSecond = vxMetersPerSecond;
-            VyMetersPerSecond = vyMetersPerSecond;
-            OmegaRadiansPerSecond = omegaRadiansPerSecond;
-        }
-
-        public double VxMetersPerSecond { get; }
-
-        public double VyMetersPerSecond { get; }
-
-        public double OmegaRadiansPerSecond { get; }
-
-        public static Twist2D Zero =>
-            new Twist2D(0.0, 0.0, 0.0);
-    }
-
-    /// <summary>
-    /// 发送给单辆车的车体坐标系速度命令。
-    /// </summary>
-    public readonly struct ChassisCommand
-    {
-        public ChassisCommand(
-            int vehicleId,
-            Twist2D bodyTwist)
-        {
-            VehicleId = vehicleId;
-            BodyTwist = bodyTwist;
-        }
-
-        public int VehicleId { get; }
-
-        /// <summary>
-        /// 单车车体坐标系速度：X向前、Y向左、逆时针旋转为正。
-        /// </summary>
-        public Twist2D BodyTwist { get; }
-
-        /// <summary>
-        /// 创建指定车辆的停止命令。
-        /// </summary>
-        public static ChassisCommand Stop(int vehicleId)
-        {
-            return new ChassisCommand(
-                vehicleId,
-                Twist2D.Zero);
-        }
-    }
-
-    /// <summary>
-    /// 单辆车的车体坐标系在车队坐标系中的位姿。
-    /// </summary>
-    public readonly struct VehicleLayout
-    {
-        public VehicleLayout(
-            int vehicleId,
-            Pose2D poseInFleet)
-        {
-            VehicleId = vehicleId;
-            PoseInFleet = poseInFleet;
-        }
-
-        public int VehicleId { get; }
-
-        public Pose2D PoseInFleet { get; }
-    }
-
-    /// <summary>
-    /// 车队整体运动命令，速度分量均在车队坐标系中表达。
-    /// </summary>
-    public readonly struct FleetMotionCommand
-    {
-        public FleetMotionCommand(
-            Point2D referencePointInFleet,
-            Twist2D twistAtReferencePoint)
-        {
-            ReferencePointInFleet = referencePointInFleet;
-            TwistAtReferencePoint = twistAtReferencePoint;
-        }
-
-        /// <summary>
-        /// 速度命令对应的参考点，也可作为自定义旋转中心。
-        /// </summary>
-        public Point2D ReferencePointInFleet { get; }
-
-        /// <summary>
-        /// 参考点处的车队速度。
-        /// </summary>
-        public Twist2D TwistAtReferencePoint { get; }
-
-        /// <summary>
-        /// 创建绕指定中心原地旋转的车队命令。
-        /// </summary>
-        public static FleetMotionCommand RotateAround(
-            Point2D rotationCenterInFleet,
-            double omegaRadiansPerSecond)
-        {
-            return new FleetMotionCommand(
-                rotationCenterInFleet,
-                new Twist2D(
-                    0.0,
-                    0.0,
-                    omegaRadiansPerSecond));
-        }
-
-        /// <summary>
-        /// 创建车队停止命令。
-        /// </summary>
-        public static FleetMotionCommand Stop()
-        {
-            return new FleetMotionCommand(
-                Point2D.Zero,
-                Twist2D.Zero);
-        }
-    }
-
-
-}

Shared/Models/MotionModels.cs

@@ -0,0 +1,68 @@
+// 使用SI单位；车体系X向前、Y向左、逆时针为正，XxxInYyy表示Xxx在Yyy坐标系中。
+namespace MyParking.Shared
+{
+    // 二维坐标点，单位为m。
+    public readonly struct Point2D
+    {
+        public Point2D(double xMeters, double yMeters)
+        {
+            XMeters = xMeters;
+            YMeters = yMeters;
+        }
+
+        public double XMeters { get; }
+
+        public double YMeters { get; }
+
+        public static Point2D Zero => new Point2D(0.0, 0.0);
+    }
+
+    // 二维位姿，位置单位为m，航向单位为rad。
+    public readonly struct Pose2D
+    {
+        public Pose2D(
+            double xMeters,
+            double yMeters,
+            double yawRadians)
+        {
+            XMeters = xMeters;
+            YMeters = yMeters;
+            YawRadians = yawRadians;
+        }
+
+        public double XMeters { get; }
+
+        public double YMeters { get; }
+
+        public double YawRadians { get; }
+
+        public Point2D Position =>
+            new Point2D(XMeters, YMeters);
+
+        public static Pose2D Identity =>
+            new Pose2D(0.0, 0.0, 0.0);
+    }
+
+    // 二维刚体速度，线速度单位为m/s，角速度单位为rad/s。
+    public readonly struct Twist2D
+    {
+        public Twist2D(
+            double vxMetersPerSecond,
+            double vyMetersPerSecond,
+            double omegaRadiansPerSecond)
+        {
+            VxMetersPerSecond = vxMetersPerSecond;
+            VyMetersPerSecond = vyMetersPerSecond;
+            OmegaRadiansPerSecond = omegaRadiansPerSecond;
+        }
+
+        public double VxMetersPerSecond { get; }
+
+        public double VyMetersPerSecond { get; }
+
+        public double OmegaRadiansPerSecond { get; }
+
+        public static Twist2D Zero =>
+            new Twist2D(0.0, 0.0, 0.0);
+    }
+}