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

.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

@@ -66,7 +66,8 @@ namespace MedullaAdapter
         #region  初始参数
 
         [AsInitParam(desc = "差速转舵目标角速度前馈增益")]
-        public float DiffSteerRateFeedforwardGain = 0.9f;
+        // public float DiffSteerRateFeedforwardGain = 0.9f;
+        public float DiffSteerRateFeedforwardGain = 0f;
 
         [AsInitParam(desc = "差速舵轮左右轮间距，单位mm")]
         public float DiffSteerWheelDistanceMillimeters = 85f;
@@ -124,6 +125,47 @@ namespace MedullaAdapter
         public bool WheelSpeedDiagnosticEnabled;
         [IOObjectMonitor(desc = "轮速诊断记录状态")]
         public string WheelSpeedDiagnosticStatus = "未启动";
+
+        // M层辨识日志：以下字段只保存控制和反馈事件的单调时钟快照，
+        // 不参与底盘控制、限幅或模式切换。
+        internal long DiffSteerControlTimestamp;
+        internal long DiffSteerControlSequence;
+        internal long WheelCommandTimestamp;
+        internal long WheelCommandSequence;
+        internal float SentSpeedLFL;
+        internal float SentSpeedLFR;
+        internal float SentSpeedLRL;
+        internal float SentSpeedLRR;
+        internal float SentSpeedRFL;
+        internal float SentSpeedRFR;
+        internal float SentSpeedRRL;
+        internal float SentSpeedRRR;
+        internal bool WheelCommandLimitedLFL;
+        internal bool WheelCommandLimitedLFR;
+        internal bool WheelCommandLimitedLRL;
+        internal bool WheelCommandLimitedLRR;
+        internal bool WheelCommandLimitedRFL;
+        internal bool WheelCommandLimitedRFR;
+        internal bool WheelCommandLimitedRRL;
+        internal bool WheelCommandLimitedRRR;
+        internal bool WheelCommandSuppressed;
+        internal float DiffSteerTargetRateLeftFrontDegreesPerSecond;
+        internal float DiffSteerTargetRateLeftRearDegreesPerSecond;
+        internal float DiffSteerTargetRateRightFrontDegreesPerSecond;
+        internal float DiffSteerTargetRateRightRearDegreesPerSecond;
+        internal float DiffSteerFeedforwardDeltaTimeMilliseconds;
+        internal bool DiffSteerFeedforwardLimitedLeftFront;
+        internal bool DiffSteerFeedforwardLimitedLeftRear;
+        internal bool DiffSteerFeedforwardLimitedRightFront;
+        internal bool DiffSteerFeedforwardLimitedRightRear;
+        internal long ActualThLeftFrontTimestamp;
+        internal long ActualThLeftFrontSequence;
+        internal long ActualThLeftRearTimestamp;
+        internal long ActualThLeftRearSequence;
+        internal long ActualThRightFrontTimestamp;
+        internal long ActualThRightFrontSequence;
+        internal long ActualThRightRearTimestamp;
+        internal long ActualThRightRearSequence;
         [IOObjectMonitor(desc = "左前左驱动器远程帧701")] public byte LFLRemoteCode = 0;
         [IOObjectMonitor(desc = "左前右驱动器远程帧702")] public byte LFRRemoteCode = 0;
         [IOObjectMonitor(desc = "右前左驱动器远程帧703")] public byte RFLRemoteCode = 0;
@@ -283,8 +325,6 @@ namespace MedullaAdapter
                 Math.Sign(x);
             var steeringDegrees =
                 -normalizedSteering * MaxManualTheta;
-            var frontTh = steeringDegrees;
-            var rearTh = -steeringDegrees;
             ManualMode = (int)mode;
 
             switch (mode)
@@ -292,16 +332,24 @@ namespace MedullaAdapter
                 case ManualControlMode.Normal:
                     // 普通模式统一使用车体速度命令：
                     // X向前，行驶中连续改变角速度时舵轮边转、车辆边走。
-                    // SendBodyCommand(
-                    //     vx: speed,
-                    //     vy: 0.0,
-                    //     omegaRadiansPerSecond: omega,
-                    //     interval);
-                    Chassis.SendMotion(
+                    var normalOmegaRadiansPerSecond =
+                        speed *
+                        Math.Tan(
+                            AngleMath.DegreesToRadians(
+                                steeringDegrees)) /
+                        adapter.ControlPointRadiusMeters;
+                    if (!adapter.SendBodyTwist(
+                            new Twist2D(
                                 speed,
-                                frontTh,
-                                rearTh,
-                                interval);
+                                0.0,
+                                normalOmegaRadiansPerSecond),
+                            interval))
+                    {
+                        adapter.StopImmediately();
+                        Console.WriteLine(
+                            "Normal SendMotion decomposition failed: " +
+                            adapter.LastFailureReason);
+                    }
                     break;
                 case ManualControlMode.Crab:
                     // 舵轮机械范围为[-120°,120°]。
@@ -332,13 +380,15 @@ namespace MedullaAdapter
 
                     // 将车体左侧作为虚拟阿克曼车头，并在该运动坐标系中
                     // 复用与普通模式相同的SendMotion前后控制点解算。
-                    if (!adapter.SendVirtualAckermannMotion(
-                            motionDirectionRadians:
-                                Math.PI / 2.0,
-                            speedMetersPerSecond:
+                    var crabOmegaRadiansPerSecond =
+                        speed *
+                        Math.Tan(crabSteeringRadians) /
+                        adapter.ControlPointRadiusMeters;
+                    if (!adapter.SendBodyTwist(
+                            new Twist2D(
+                                0.0,
                                 speed,
-                            steeringRadians:
-                                crabSteeringRadians,
+                                crabOmegaRadiansPerSecond),
                             interval))
                     {
                         adapter.StopImmediately();
@@ -380,13 +430,11 @@ namespace MedullaAdapter
 
                     // 普通安全版SendXYThSpeed只下发角速度，
                     // 四轮实际舵角未到位时不会开放驱动速度。
-                    if (!adapter.Send(
-                            new ChassisCommand(
-                                CarNum,
+                    if (!adapter.SendBodyTwist(
                             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);
-                    cart.LFLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
+                        speed,
+                        positionMillimeters);
                 },
                 [0x282] = (msg) =>
                 {
@@ -700,13 +758,18 @@ namespace MedullaAdapter
                     if (payload == null || payload.Length < 8) return;
                     var rpm = DecodeRpmFromPayload(payload);
                     var speed = -ConvertRpm2Mps(rpm);
+                    var positionMillimeters =
+                        ConvertR2MM(
+                            -BitConverter.ToInt32(payload, 0) /
+                            10000f);
                     cart.ActualSpeedLeftFrontRight = speed;
+                    cart.LFRActualPos = positionMillimeters;
                     _wheelSpeedLogger.RecordCanFeedback(
                         0x282,
                         "LFR",
                         rpm,
-                        speed);
-                    cart.LFRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
+                        speed,
+                        positionMillimeters);
                 },
                 [0x283] = (msg) =>
                 {
@@ -715,13 +778,18 @@ namespace MedullaAdapter
                     if (payload == null || payload.Length < 8) return;
                     var rpm = DecodeRpmFromPayload(payload);
                     var speed = ConvertRpm2Mps(rpm);
+                    var positionMillimeters =
+                        ConvertR2MM(
+                            BitConverter.ToInt32(payload, 0) /
+                            10000f);
                     cart.ActualSpeedRightFrontLeft = speed;
+                    cart.RFLActualPos = positionMillimeters;
                     _wheelSpeedLogger.RecordCanFeedback(
                         0x283,
                         "RFL",
                         rpm,
-                        speed);
-                    cart.RFLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
+                        speed,
+                        positionMillimeters);
                 },
                 [0x284] = (msg) =>
                 {
@@ -730,13 +798,18 @@ namespace MedullaAdapter
                     if (payload == null || payload.Length < 8) return;
                     var rpm = DecodeRpmFromPayload(payload);
                     var speed = -ConvertRpm2Mps(rpm);
+                    var positionMillimeters =
+                        ConvertR2MM(
+                            -BitConverter.ToInt32(payload, 0) /
+                            10000f);
                     cart.ActualSpeedRightFrontRight = speed;
+                    cart.RFRActualPos = positionMillimeters;
                     _wheelSpeedLogger.RecordCanFeedback(
                         0x284,
                         "RFR",
                         rpm,
-                        speed);
-                    cart.RFRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
+                        speed,
+                        positionMillimeters);
                 },
                 [0x285] = (msg) =>
                 {
@@ -745,13 +818,18 @@ namespace MedullaAdapter
                     if (payload == null || payload.Length < 8) return;
                     var rpm = DecodeRpmFromPayload(payload);
                     var speed = ConvertRpm2Mps(rpm);
+                    var positionMillimeters =
+                        ConvertR2MM(
+                            BitConverter.ToInt32(payload, 0) /
+                            10000f);
                     cart.ActualSpeedLeftRearLeft = speed;
+                    cart.LRLActualPos = positionMillimeters;
                     _wheelSpeedLogger.RecordCanFeedback(
                         0x285,
                         "LRL",
                         rpm,
-                        speed);
-                    cart.LRLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
+                        speed,
+                        positionMillimeters);
                 },
                 [0x286] = (msg) =>
                 {
@@ -760,13 +838,18 @@ namespace MedullaAdapter
                     if (payload == null || payload.Length < 8) return;
                     var rpm = DecodeRpmFromPayload(payload);
                     var speed = -ConvertRpm2Mps(rpm);
+                    var positionMillimeters =
+                        ConvertR2MM(
+                            -BitConverter.ToInt32(payload, 0) /
+                            10000f);
                     cart.ActualSpeedLeftRearRight = speed;
+                    cart.LRRActualPos = positionMillimeters;
                     _wheelSpeedLogger.RecordCanFeedback(
                         0x286,
                         "LRR",
                         rpm,
-                        speed);
-                    cart.LRRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
+                        speed,
+                        positionMillimeters);
                 },
                 [0x287] = (msg) =>
                 {
@@ -775,13 +858,18 @@ namespace MedullaAdapter
                     if (payload == null || payload.Length < 8) return;
                     var rpm = DecodeRpmFromPayload(payload);
                     var speed = ConvertRpm2Mps(rpm);
+                    var positionMillimeters =
+                        ConvertR2MM(
+                            BitConverter.ToInt32(payload, 0) /
+                            10000f);
                     cart.ActualSpeedRightRearLeft = speed;
+                    cart.RRLActualPos = positionMillimeters;
                     _wheelSpeedLogger.RecordCanFeedback(
                         0x287,
                         "RRL",
                         rpm,
-                        speed);
-                    cart.RRLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
+                        speed,
+                        positionMillimeters);
                 },
                 [0x288] = (msg) =>
                 {
@@ -790,13 +878,18 @@ namespace MedullaAdapter
                     if (payload == null || payload.Length < 8) return;
                     var rpm = DecodeRpmFromPayload(payload);
                     var speed = -ConvertRpm2Mps(rpm);
+                    var positionMillimeters =
+                        ConvertR2MM(
+                            -BitConverter.ToInt32(payload, 0) /
+                            10000f);
                     cart.ActualSpeedRightRearRight = speed;
+                    cart.RRRActualPos = positionMillimeters;
                     _wheelSpeedLogger.RecordCanFeedback(
                         0x288,
                         "RRR",
                         rpm,
-                        speed);
-                    cart.RRRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
+                        speed,
+                        positionMillimeters);
                 },
                 [0x289] = (msg) =>
                 {
@@ -914,10 +1007,18 @@ namespace MedullaAdapter
                     //Console.WriteLine("Received 0x18B CAN Message");
                     var payload = msg.Payload;
                     if (payload == null || payload.Length < 4) return;
-                    cart.ActualThLeftFront = BitConverter.ToInt32(payload, 0);
-                    cart.ActualThLeftFront = cart.ActualThLeftFront >= 16384
-                    ? (cart.ActualThLeftFront - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftFront
-                    : cart.ActualThLeftFront / 4096f / 5f * 360 - cart.ThBiasLeftFront;
+                    var raw = BitConverter.ToInt32(payload, 0);
+                    cart.ActualThLeftFront = raw >= 16384
+                    ? (raw - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftFront
+                    : raw / 4096f / 5f * 360 - cart.ThBiasLeftFront;
+                    MarkFeedbackReceived(
+                        ref cart.ActualThLeftFrontTimestamp,
+                        ref cart.ActualThLeftFrontSequence);
+                    _wheelSpeedLogger.RecordSteeringAngleFeedback(
+                        0x18B,
+                        "LF",
+                        raw,
+                        cart.ActualThLeftFront);
                 },
                 [0x18C] = (msg) =>
                 {
@@ -927,6 +1028,14 @@ namespace MedullaAdapter
                     cart.ActualThRightFront = raw >= 16384
                         ? (raw - 98303) / 4096f / 5f * 360 - cart.ThBiasRightFront
                         : raw / 4096f / 5f * 360 - cart.ThBiasRightFront;
+                    MarkFeedbackReceived(
+                        ref cart.ActualThRightFrontTimestamp,
+                        ref cart.ActualThRightFrontSequence);
+                    _wheelSpeedLogger.RecordSteeringAngleFeedback(
+                        0x18C,
+                        "RF",
+                        raw,
+                        cart.ActualThRightFront);
                     //DLog.Log($"RF raw=0x{raw:X8}({raw}) angle={cart.ActualThRightFront:F2}", "0x18C");
                 },
                 [0x18D] = (msg) =>
@@ -934,20 +1043,36 @@ namespace MedullaAdapter
                     //Console.WriteLine($"Received 0x18D CAN Message {DateTime.Now:yyyy-MM-dd HH:mm:ss.ffffff}"); 
                     var payload = msg.Payload;
                     if (payload == null || payload.Length < 4) return;
-                    cart.ActualThLeftRear = BitConverter.ToInt32(payload, 0);
-                    cart.ActualThLeftRear = cart.ActualThLeftRear >= 16384
-                        ? (cart.ActualThLeftRear - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftRear
-                        : cart.ActualThLeftRear / 4096f / 5f * 360 - cart.ThBiasLeftRear;
+                    var raw = BitConverter.ToInt32(payload, 0);
+                    cart.ActualThLeftRear = raw >= 16384
+                        ? (raw - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftRear
+                        : raw / 4096f / 5f * 360 - cart.ThBiasLeftRear;
+                    MarkFeedbackReceived(
+                        ref cart.ActualThLeftRearTimestamp,
+                        ref cart.ActualThLeftRearSequence);
+                    _wheelSpeedLogger.RecordSteeringAngleFeedback(
+                        0x18D,
+                        "LR",
+                        raw,
+                        cart.ActualThLeftRear);
                 },
                 [0x18E] = (msg) =>
                 {
                     //Console.WriteLine("Received 0x18E CAN Message");
                     var payload = msg.Payload;
                     if (payload == null || payload.Length < 4) return;
-                    cart.ActualThRightRear = BitConverter.ToInt32(payload, 0);
-                    cart.ActualThRightRear = cart.ActualThRightRear >= 16384
-                        ? (cart.ActualThRightRear - 98303) / 4096f / 5f * 360 - cart.ThBiasRightRear
-                        : cart.ActualThRightRear / 4096f / 5f * 360 - cart.ThBiasRightRear;
+                    var raw = BitConverter.ToInt32(payload, 0);
+                    cart.ActualThRightRear = raw >= 16384
+                        ? (raw - 98303) / 4096f / 5f * 360 - cart.ThBiasRightRear
+                        : raw / 4096f / 5f * 360 - cart.ThBiasRightRear;
+                    MarkFeedbackReceived(
+                        ref cart.ActualThRightRearTimestamp,
+                        ref cart.ActualThRightRearSequence);
+                    _wheelSpeedLogger.RecordSteeringAngleFeedback(
+                        0x18E,
+                        "RR",
+                        raw,
+                        cart.ActualThRightRear);
                 },
 
                 // 远程帧

MedullaAdapter/MedullaAdapter.csproj

@@ -42,8 +42,8 @@
   </ItemGroup>
 
   <ItemGroup>
-  <Compile Include="..\Shared\Models\ChassisCommand.cs"
-           Link="Shared\Models\ChassisCommand.cs" />
+  <Compile Include="..\Shared\Models\MotionModels.cs"
+           Link="Shared\Models\MotionModels.cs" />
 
   <Compile Include="..\Shared\Mathematics\FrameTransform2D.cs"
            Link="Shared\Mathematics\FrameTransform2D.cs" />

MedullaAdapter/MotorRoutine.cs

@@ -4,6 +4,7 @@ using FundamentalLib;
 using MDCSToolBox.Commons;
 using System;
 using System.Diagnostics;
+using System.Threading;
 using static MDCSToolBox.Medulla.Chassis.BasicCartDefinition;
 
 namespace MedullaAdapter
@@ -75,6 +76,11 @@ namespace MedullaAdapter
             cart.ClumsyControl = CartDefinition.currentPriority == 0;
             // 计算四个舵轮PID和8个驱动电机最终速度。
             UpdateDiffSteerWheelSpeeds();
+            Interlocked.Exchange(
+                ref cart.DiffSteerControlTimestamp,
+                Stopwatch.GetTimestamp());
+            Interlocked.Increment(
+                ref cart.DiffSteerControlSequence);
             // 平滑更新硬件速度限制。
             UpdateSendSpeedLimit();
             // 更新红黄绿灯状态。
@@ -351,6 +357,15 @@ namespace MedullaAdapter
             leftRear = 0f;
             rightFront = 0f;
             rightRear = 0f;
+            cart.DiffSteerTargetRateLeftFrontDegreesPerSecond = 0f;
+            cart.DiffSteerTargetRateLeftRearDegreesPerSecond = 0f;
+            cart.DiffSteerTargetRateRightFrontDegreesPerSecond = 0f;
+            cart.DiffSteerTargetRateRightRearDegreesPerSecond = 0f;
+            cart.DiffSteerFeedforwardDeltaTimeMilliseconds = 0f;
+            cart.DiffSteerFeedforwardLimitedLeftFront = false;
+            cart.DiffSteerFeedforwardLimitedLeftRear = false;
+            cart.DiffSteerFeedforwardLimitedRightFront = false;
+            cart.DiffSteerFeedforwardLimitedRightRear = false;
 
             var currentTimestamp = Stopwatch.GetTimestamp();
 
@@ -365,22 +380,45 @@ namespace MedullaAdapter
                     deltaTimeSeconds <=
                     MaximumFeedforwardIntervalSeconds)
                 {
+                    cart.DiffSteerFeedforwardDeltaTimeMilliseconds =
+                        (float)(deltaTimeSeconds * 1000.0);
                     leftFront = CalculateDiffSteerRateFeedforward(
                         cart.ThLeftFront,
                         _previousThLeftFront,
-                        deltaTimeSeconds);
+                        deltaTimeSeconds,
+                        out var targetRateLf,
+                        out var limitedLf);
                     leftRear = CalculateDiffSteerRateFeedforward(
                         cart.ThLeftRear,
                         _previousThLeftRear,
-                        deltaTimeSeconds);
+                        deltaTimeSeconds,
+                        out var targetRateLr,
+                        out var limitedLr);
                     rightFront = CalculateDiffSteerRateFeedforward(
                         cart.ThRightFront,
                         _previousThRightFront,
-                        deltaTimeSeconds);
+                        deltaTimeSeconds,
+                        out var targetRateRf,
+                        out var limitedRf);
                     rightRear = CalculateDiffSteerRateFeedforward(
                         cart.ThRightRear,
                         _previousThRightRear,
-                        deltaTimeSeconds);
+                        deltaTimeSeconds,
+                        out var targetRateRr,
+                        out var limitedRr);
+
+                    cart.DiffSteerTargetRateLeftFrontDegreesPerSecond =
+                        targetRateLf;
+                    cart.DiffSteerTargetRateLeftRearDegreesPerSecond =
+                        targetRateLr;
+                    cart.DiffSteerTargetRateRightFrontDegreesPerSecond =
+                        targetRateRf;
+                    cart.DiffSteerTargetRateRightRearDegreesPerSecond =
+                        targetRateRr;
+                    cart.DiffSteerFeedforwardLimitedLeftFront = limitedLf;
+                    cart.DiffSteerFeedforwardLimitedLeftRear = limitedLr;
+                    cart.DiffSteerFeedforwardLimitedRightFront = limitedRf;
+                    cart.DiffSteerFeedforwardLimitedRightRear = limitedRr;
                 }
             }
 
@@ -398,29 +436,44 @@ namespace MedullaAdapter
         private float CalculateDiffSteerRateFeedforward(
             float targetAngleDegrees,
             float previousTargetAngleDegrees,
-            double deltaTimeSeconds)
+            double deltaTimeSeconds,
+            out float targetRateDegreesPerSecond,
+            out bool limited)
         {
+            targetRateDegreesPerSecond = 0f;
+            limited = false;
+
             var gain = cart.DiffSteerRateFeedforwardGain;
             var wheelDistanceMillimeters =
                 cart.DiffSteerWheelDistanceMillimeters;
             var maximumSpeed =
                 cart.DiffSteerRateFeedforwardMaximumSpeed;
 
-            if (!IsFinite(gain) || gain <= 0f ||
-                !IsFinite(wheelDistanceMillimeters) ||
-                wheelDistanceMillimeters <= 0f ||
-                !IsFinite(maximumSpeed) || maximumSpeed <= 0f ||
-                !IsFinite(targetAngleDegrees) ||
-                !IsFinite(previousTargetAngleDegrees))
+            if (!IsFinite(targetAngleDegrees) ||
+                !IsFinite(previousTargetAngleDegrees) ||
+                !double.IsFinite(deltaTimeSeconds) ||
+                deltaTimeSeconds <= 0.0)
             {
                 return 0f;
             }
 
             // 机械舵角受限，必须使用直接差值而不是圆周最短角差。
+            targetRateDegreesPerSecond =
+                (float)((targetAngleDegrees -
+                         previousTargetAngleDegrees) /
+                        deltaTimeSeconds);
+
+            if (!IsFinite(gain) || gain <= 0f ||
+                !IsFinite(wheelDistanceMillimeters) ||
+                wheelDistanceMillimeters <= 0f ||
+                !IsFinite(maximumSpeed) || maximumSpeed <= 0f)
+            {
+                return 0f;
+            }
+
             var targetRateRadiansPerSecond =
-                (targetAngleDegrees - previousTargetAngleDegrees) *
-                Math.PI / 180.0 /
-                deltaTimeSeconds;
+                targetRateDegreesPerSecond *
+                Math.PI / 180.0;
             var wheelDistanceMeters =
                 wheelDistanceMillimeters / 1000.0;
             var feedforwardSpeed =
@@ -429,10 +482,13 @@ namespace MedullaAdapter
                 targetRateRadiansPerSecond *
                 gain;
 
-            return (float)Math.Clamp(
+            var limitedSpeed = (float)Math.Clamp(
                 feedforwardSpeed,
                 -maximumSpeed,
                 maximumSpeed);
+            limited = Math.Abs(
+                feedforwardSpeed - limitedSpeed) > 1e-9;
+            return limitedSpeed;
         }
 
         /// <summary>
@@ -446,6 +502,15 @@ namespace MedullaAdapter
             cart.DiffSteerRateFeedforwardLeftRear = 0f;
             cart.DiffSteerRateFeedforwardRightFront = 0f;
             cart.DiffSteerRateFeedforwardRightRear = 0f;
+            cart.DiffSteerTargetRateLeftFrontDegreesPerSecond = 0f;
+            cart.DiffSteerTargetRateLeftRearDegreesPerSecond = 0f;
+            cart.DiffSteerTargetRateRightFrontDegreesPerSecond = 0f;
+            cart.DiffSteerTargetRateRightRearDegreesPerSecond = 0f;
+            cart.DiffSteerFeedforwardDeltaTimeMilliseconds = 0f;
+            cart.DiffSteerFeedforwardLimitedLeftFront = false;
+            cart.DiffSteerFeedforwardLimitedLeftRear = false;
+            cart.DiffSteerFeedforwardLimitedRightFront = false;
+            cart.DiffSteerFeedforwardLimitedRightRear = false;
             cart.DiffSteerTotalOutputLeftFront = 0f;
             cart.DiffSteerTotalOutputLeftRear = 0f;
             cart.DiffSteerTotalOutputRightFront = 0f;

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

MedullaAdapter/WirelessFleetTransport.cs

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

MultiWheelC.Tests/FleetControllerTests.cs

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

MultiWheelC.Tests/FleetCoordinatorTests.cs

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

MultiWheelC.Tests/FleetKinematicsTests.cs

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

MultiWheelC.Tests/FleetLayoutCaptureTests.cs

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

MultiWheelC.Tests/FleetMemberAgentTests.cs

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

MultiWheelC.Tests/FleetMemberCommandCorrectorTests.cs

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

MultiWheelC.Tests/FleetPreparationCoordinatorTests.cs

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

MultiWheelC.Tests/FleetRuntimeTests.cs

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

MultiWheelC.Tests/FleetSafetySupervisorTests.cs

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

MultiWheelC.Tests/FleetStateEstimatorTests.cs

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

MultiWheelC.Tests/InMemoryFleetTransport.cs

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

MultiWheelC.Tests/InMemoryFleetTransportTests.cs

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

MultiWheelC.Tests/MultiWheelC.Tests.csproj

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

MultiWheelC.Tests/Program.cs

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

MultiWheelC/Configuration/PilotConfig.ParkingControl.cs

@@ -27,9 +27,34 @@ public partial class PilotConfig
     [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;
 
@@ -94,7 +119,7 @@ public partial class PilotConfig
     #region 停车控制-原地自转
 
     [FieldMember(desc = "停车控制：原地自转Kp")]
-    public float InPlaceRotateKp = 1.05f;
+    public float InPlaceRotateKp = 1.0f;
 
     [FieldMember(desc = "停车控制：原地自转Ki")]
     public float InPlaceRotateKi = 0f;
@@ -115,10 +140,12 @@ public partial class PilotConfig
     public float InPlaceRotateMinimumSpeed = 1f;
 
     [FieldMember(desc = "停车控制：原地自转最大角速度(deg/s)")]
-    public float InPlaceRotateMaxSpeed = 47.5f;
+    // public float InPlaceRotateMaxSpeed = 47.5f;
+    public float InPlaceRotateMaxSpeed = 45f;
 
     [FieldMember(desc = "停车控制：原地自转角加速度(deg/s²)")]
-    public float InPlaceRotateAcc = 60f;
+    // public float InPlaceRotateAcc = 60f;
+    public float InPlaceRotateAcc = 45f;
 
     [FieldMember(desc = "停车控制：原地自转超时(s)")]
     public float InPlaceRotateTimeoutSec = 15f;

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,11 +13,13 @@ namespace MultiWheelC.Control.Abstractions
         /// 创建横向和纵向控制器共享的只读控制输入快照。
         /// </summary>
         public PathTrackingContext(
-            VehicleState vehicleState,
+            Twist2D actualTwistInBody,
+            bool hasValidVelocityEstimate,
             TrajectoryProjection projection,
             double controlReferenceSpeedMetersPerSecond,
             double feedforwardCurvaturePerMeter,
-            double deltaTimeSeconds)
+            double deltaTimeSeconds,
+            double motionDirectionInBodyRadians = 0.0)
         {
             EnsureFinite(
                 controlReferenceSpeedMetersPerSecond,
@@ -28,20 +30,31 @@ namespace MultiWheelC.Control.Abstractions
             EnsureFinitePositive(
                 deltaTimeSeconds,
                 nameof(deltaTimeSeconds));
+            EnsureFinite(
+                motionDirectionInBodyRadians,
+                nameof(motionDirectionInBodyRadians));
+            NumericGuard.EnsureFinite(
+                actualTwistInBody,
+                nameof(actualTwistInBody));
 
-            VehicleState = vehicleState;
+            ActualTwistInBody = actualTwistInBody;
+            HasValidVelocityEstimate =
+                hasValidVelocityEstimate;
             Projection = projection;
             ControlReferenceSpeedMetersPerSecond =
                 controlReferenceSpeedMetersPerSecond;
             FeedforwardCurvaturePerMeter =
                 feedforwardCurvaturePerMeter;
             DeltaTimeSeconds = deltaTimeSeconds;
+            MotionDirectionInBodyRadians =
+                AngleMath.NormalizeRadians(
+                    motionDirectionInBodyRadians);
         }
 
         /// <summary>
-        /// 获取本周期经过校验的实际车辆位姿和速度状态。
+        /// 获取受控刚体坐标系下的实际速度。
         /// </summary>
-        public VehicleState VehicleState { get; }
+        public Twist2D ActualTwistInBody { get; }
 
         /// <summary>
         /// 获取实际车体中心投影到参考轨迹后得到的参考状态和跟踪误差。
@@ -59,11 +72,18 @@ namespace MultiWheelC.Control.Abstractions
         public double ControlReferenceSpeedMetersPerSecond { get; }
 
         /// <summary>
-        /// 获取车辆在车体X轴方向上的实际纵向速度，单位为m/s。
+        /// 获取车辆沿当前运动坐标系X轴方向的实际纵向速度，单位为m/s。
         /// </summary>
         public double ActualLongitudinalSpeedMetersPerSecond =>
-            VehicleState.TwistInBody
-                .VxMetersPerSecond;
+            Math.Cos(MotionDirectionInBodyRadians) *
+                ActualTwistInBody.VxMetersPerSecond +
+            Math.Sin(MotionDirectionInBodyRadians) *
+                ActualTwistInBody.VyMetersPerSecond;
+
+        /// <summary>
+        /// 获取当前运动坐标系X轴在车体系中的方向，单位为rad。
+        /// </summary>
+        public double MotionDirectionInBodyRadians { get; }
 
         /// <summary>
         /// 获取沿轨迹执行点序定义的参考曲率，单位为1/m，左弯为正。
@@ -78,7 +98,7 @@ namespace MultiWheelC.Control.Abstractions
         public double FeedforwardCurvaturePerMeter { get; }
 
         /// <summary>
-        /// 获取参考轨迹相对车辆的有符号横向误差，单位为m，轨迹在车辆左侧时为正。
+        /// 获取相对轨迹执行点序的有符号横向误差，单位为m，参考轨迹位于执行方向左侧时为正。
         /// </summary>
         public double LateralErrorMeters =>
             Projection.LateralErrorMeters;
@@ -96,10 +116,9 @@ namespace MultiWheelC.Control.Abstractions
             Projection.RemainingDistanceMeters;
 
         /// <summary>
-        /// 获取实际速度是否已经由至少两个连续有效定位样本估算得到。
+        /// 获取本周期实际速度估计是否可供闭环控制使用。
         /// </summary>
-        public bool HasValidVelocityEstimate =>
-            VehicleState.HasValidVelocityEstimate;
+        public bool HasValidVelocityEstimate { get; }
 
         /// <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(
-                limitedCommand.SpeedMetersPerSecond,
-                limitedCommand.FrontAngleRadians,
-                limitedCommand.RearAngleRadians,
+            var motionFrameTwist =
+                GcpKinematics.ToBodyTwist(
+                    limitedCommand,
+                    _chassisAdapter.ControlPointRadiusMeters);
+            var bodyTwist =
+                FrameTransform2D.TransformTwistAtSamePoint(
+                    new Pose2D(
+                        0.0,
+                        0.0,
+                        _motionDirectionInBodyRadians),
+                    motionFrameTwist);
+            var success = _chassisAdapter.SendBodyTwist(
+                bodyTwist,
                 TimeSpan.FromSeconds(deltaTimeSeconds));
 
             LastFailureReason = success

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

@@ -123,8 +123,9 @@ namespace MultiWheelC.Control.Lateral
                     context.HeadingErrorRadians,
                     MaximumHeadingCorrectionRadians);
 
+            // 横向误差已经按轨迹执行点序定义；倒车轨迹的点序会自然
+            // 翻转横向轴，因此共同转角不能再按行驶方向重复反号。
             var commonAngleRadians =
-                travelDirection *
                 crossTrackCorrectionRadians;
             var differentialAngleRadians =
                 feedforwardAngleRadians +

MultiWheelC/Experiments/CompositeMotionPlanTests.cs

@@ -157,7 +157,8 @@ namespace MultiWheelC
                     (float)AccelerationMetersPerSecondSquared,
                 referenceDecelerationMetersPerSecondSquared:
                     (float)DecelerationMetersPerSecondSquared,
-                diagnosticChassis: chassis);
+                diagnosticChassis: chassis,
+                diagnosticStateProvider: stateProvider);
             _recorder.Start();
 
             var controlPointRadiusMeters =
@@ -291,7 +292,9 @@ namespace MultiWheelC
             {
                 _recorder?.RecordControlCycleTiming(
                     controller.LastCycleTiming.Value,
-                    motionSegmentIndex);
+                    motionSegmentIndex,
+                    controller.LastRequestedCommand,
+                    controller.LastCommand);
             }
 
             if (controller.LastVehicleState.HasValue)
@@ -305,6 +308,8 @@ namespace MultiWheelC
                     out var detourVelocityValid,
                     out var rawWheelBodyVx,
                     out var filteredWheelBodyVx,
+                    out var rawWheelBodyVy,
+                    out var filteredWheelBodyVy,
                     out var wheelVelocityValid))
             {
                 _recorder?.UpdateVelocityDiagnostics(
@@ -312,6 +317,8 @@ namespace MultiWheelC
                     detourVelocityValid,
                     rawWheelBodyVx,
                     filteredWheelBodyVx,
+                    rawWheelBodyVy,
+                    filteredWheelBodyVy,
                     wheelVelocityValid);
             }
 
@@ -338,8 +345,13 @@ namespace MultiWheelC
                     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(

MultiWheelC/Experiments/DetourStaticDiagnosticTest.cs

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

MultiWheelC/Experiments/NewControllerTrackingTests.cs

@@ -94,7 +94,7 @@ namespace MultiWheelC
     /// 从当前Detour位姿开始执行新版控制器4m直线跟踪并保存实验数据。
     /// </summary>
     [MovementTest(name = "新版控制器：4m直线轨迹跟踪")]
-    public sealed class NewControllerStraight4mTest
+    public class NewControllerStraight4mTest
         : MovementTest
     {
         private const float MillimetersPerMeter = 1000f;
@@ -131,6 +131,30 @@ namespace MultiWheelC
         /// </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>
@@ -184,7 +208,8 @@ namespace MultiWheelC
                     CruiseSpeedMetersPerSecond,
                     AccelerationMetersPerSecondSquared,
                     DecelerationMetersPerSecondSquared,
-                    PointSpacingMeters);
+                    PointSpacingMeters,
+                    MotionDirectionInBodyRadians);
 
             DrawTrajectory(trajectory);
 
@@ -197,7 +222,7 @@ namespace MultiWheelC
                     controllerName: "NewStanleyPid",
                     trajectoryName:
                         TrajectoryExperimentInput.BuildTrajectoryName(
-                            "ProfiledStraight4m",
+                            ExperimentTrajectoryBaseName,
                             lateralOffsetMeters),
                     trialNumber: TrialNumber,
                     referenceStart: referenceStart,
@@ -205,11 +230,15 @@ namespace MultiWheelC
                     referenceSpeed:
                         (float)CruiseSpeedMetersPerSecond,
                     sampleIntervalMs: 50,
+                    referenceMotionFrameYawDegrees:
+                        (float)AngleMath.RadiansToDegrees(
+                            MotionDirectionInBodyRadians),
                     referenceAccelerationMetersPerSecondSquared:
                         (float)AccelerationMetersPerSecondSquared,
                     referenceDecelerationMetersPerSecondSquared:
                         (float)DecelerationMetersPerSecondSquared,
-                    diagnosticChassis: chassis);
+                    diagnosticChassis: chassis,
+                    diagnosticStateProvider: stateProvider);
             _recorder = recorder;
 
             var controlPointRadiusMeters =
@@ -220,6 +249,12 @@ namespace MultiWheelC
                 {
                     Trajectory = trajectory,
                     StateProvider = _stateProvider,
+                    MotionDirectionInBodyRadians =
+                        ResolveMotionDirectionFromTrajectory
+                            ? (double?)null
+                            : MotionDirectionInBodyRadians,
+                    ReturnWheelsForwardAfterCompletion =
+                        ReturnWheelsForwardAfterCompletion,
                     CycleObserver = controller =>
                         RecordControlCycle(
                             recorder,
@@ -336,7 +371,11 @@ namespace MultiWheelC
             if (controller.LastCycleTiming.HasValue)
             {
                 recorder.RecordControlCycleTiming(
-                    controller.LastCycleTiming.Value);
+                    controller.LastCycleTiming.Value,
+                    requestedCommand:
+                        controller.LastRequestedCommand,
+                    sentCommand:
+                        controller.LastCommand);
             }
 
             if (controller.LastVehicleState.HasValue)
@@ -371,8 +410,13 @@ namespace MultiWheelC
                     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(
@@ -400,6 +444,8 @@ namespace MultiWheelC
                     out var detourVelocityValid,
                     out var rawWheelBodyVx,
                     out var filteredWheelBodyVx,
+                    out var rawWheelBodyVy,
+                    out var filteredWheelBodyVy,
                     out var wheelVelocityValid))
             {
                 return;
@@ -410,6 +456,8 @@ namespace MultiWheelC
                 detourVelocityValid,
                 rawWheelBodyVx,
                 filteredWheelBodyVx,
+                rawWheelBodyVy,
+                filteredWheelBodyVy,
                 wheelVelocityValid);
         }
 
@@ -430,11 +478,65 @@ namespace MultiWheelC
 
     }
 
+    /// <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;
@@ -492,6 +594,30 @@ namespace MultiWheelC
         /// </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>
@@ -550,7 +676,8 @@ namespace MultiWheelC
                         SemicircleMaximumSpeedMetersPerSecond,
                         AccelerationMetersPerSecondSquared,
                         DecelerationMetersPerSecondSquared,
-                        PointSpacingMeters);
+                        PointSpacingMeters,
+                        MotionDirectionInBodyRadians);
 
             DrawTrajectory(trajectory);
 
@@ -563,7 +690,7 @@ namespace MultiWheelC
                     controllerName: "NewStanleyPid",
                     trajectoryName:
                         TrajectoryExperimentInput.BuildTrajectoryName(
-                            "ProfiledStraightSmoothLeftTurnStraight",
+                            ExperimentTrajectoryBaseName,
                             lateralOffsetMeters),
                     trialNumber: TrialNumber,
                     referenceStart: referenceStart,
@@ -571,11 +698,15 @@ namespace MultiWheelC
                     referenceSpeed:
                         (float)StraightMaximumSpeedMetersPerSecond,
                     sampleIntervalMs: 50,
+                    referenceMotionFrameYawDegrees:
+                        (float)AngleMath.RadiansToDegrees(
+                            MotionDirectionInBodyRadians),
                     referenceAccelerationMetersPerSecondSquared:
                         (float)AccelerationMetersPerSecondSquared,
                     referenceDecelerationMetersPerSecondSquared:
                         (float)DecelerationMetersPerSecondSquared,
-                    diagnosticChassis: chassis);
+                    diagnosticChassis: chassis,
+                    diagnosticStateProvider: stateProvider);
             _recorder = recorder;
 
             var controlPointRadiusMeters =
@@ -586,6 +717,12 @@ namespace MultiWheelC
                 {
                     Trajectory = trajectory,
                     StateProvider = _stateProvider,
+                    MotionDirectionInBodyRadians =
+                        ResolveMotionDirectionFromTrajectory
+                            ? (double?)null
+                            : MotionDirectionInBodyRadians,
+                    ReturnWheelsForwardAfterCompletion =
+                        ReturnWheelsForwardAfterCompletion,
                     CycleObserver = controller =>
                         RecordControlCycle(
                             recorder,
@@ -702,7 +839,11 @@ namespace MultiWheelC
             if (controller.LastCycleTiming.HasValue)
             {
                 recorder.RecordControlCycleTiming(
-                    controller.LastCycleTiming.Value);
+                    controller.LastCycleTiming.Value,
+                    requestedCommand:
+                        controller.LastRequestedCommand,
+                    sentCommand:
+                        controller.LastCommand);
             }
 
             if (controller.LastVehicleState.HasValue)
@@ -737,8 +878,13 @@ namespace MultiWheelC
                     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(
@@ -766,6 +912,8 @@ namespace MultiWheelC
                     out var detourVelocityValid,
                     out var rawWheelBodyVx,
                     out var filteredWheelBodyVx,
+                    out var rawWheelBodyVy,
+                    out var filteredWheelBodyVy,
                     out var wheelVelocityValid))
             {
                 return;
@@ -776,6 +924,8 @@ namespace MultiWheelC
                 detourVelocityValid,
                 rawWheelBodyVx,
                 filteredWheelBodyVx,
+                rawWheelBodyVy,
+                filteredWheelBodyVy,
                 wheelVelocityValid);
         }
 
@@ -794,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/RotationTests.cs

@@ -7,10 +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 MyParking.Shared;
+using MultiWheelC.StateEstimation;
 
 namespace MultiWheelC
 {
@@ -18,6 +20,8 @@ namespace MultiWheelC
     {
         public float RelativeAngleDegrees; // 相对当前航向的旋转角度，逆时针为正。
         public int TrialNumber = 1; // 重复实验编号。
+        public InPlaceRotationFeedbackMode FeedbackMode =
+            InPlaceRotationFeedbackMode.DetourAbsoluteHeading;
 
         private DriveTask _task;
         private TrackingExperimentRecorder _recorder;
@@ -66,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(
                 "原地自转实际参数：" +
@@ -84,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,
@@ -98,7 +134,9 @@ namespace MultiWheelC
                 referenceSpeed: 0f,
                 referenceAngularSpeed:
                     (float)AngleMath.DegreesToRadians(
-                        config.InPlaceRotateMaxSpeed));
+                        config.InPlaceRotateMaxSpeed),
+                diagnosticChassis: chassis,
+                diagnosticStateProvider: stateProvider);
             _recorder.Start();
 
             try
@@ -106,8 +144,10 @@ namespace MultiWheelC
                 _task = new DriveTask(
                     new MultiWheelRotateInPlace
                     {
-                        // MultiWheelRotateInPlace接收世界坐标系绝对航向。
-                        AngleTarget = targetWorldAngle,
+                        AngleTarget = movementAngleTarget,
+                        FeedbackMode = FeedbackMode,
+                        Chassis = chassis,
+                        StateProvider = stateProvider,
                         CommandAngularSpeedObserver =
                             commandAngularSpeed =>
                                 _recorder?.UpdateCommand(
@@ -139,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：输入角度原地自转")]
@@ -155,38 +235,37 @@ namespace MultiWheelC
         /// </summary>
         public override void Test()
         {
-            var input = UI.GetInput(
-                "输入相对旋转角度（deg，正数逆时针，负数顺时针，范围-180到180之间）：");
-
-            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))
+            if (!TryReadRelativeAngleDegrees(
+                    out var relativeAngleDegrees))
             {
-                Console.WriteLine("旋转角度输入无效，测试已经取消。");
                 return;
             }
 
-            if (Math.Abs(relativeAngleDegrees) < 1e-3f)
-            {
-                Console.WriteLine("旋转角度不能为0，测试已经取消。");
-                return;
+            RelativeAngleDegrees = relativeAngleDegrees;
+            base.Test();
+        }
     }
 
-            // 当前控制器按照圆周最短角旋转；精确±180°的方向存在二义性。
-            if (Math.Abs(relativeAngleDegrees) >= 180f)
+    [MovementTest(name = "轮组里程计：输入角度原地相对自转")]
+    public sealed class TestWheelOdometryRotateAngle :
+        InPlaceRotateTestBase
+    {
+        public TestWheelOdometryRotateAngle()
+            : base(0f, "RotateWheelOdometryCustomAngle")
+        {
+            FeedbackMode =
+                InPlaceRotationFeedbackMode
+                    .RelativeWheelOdometry;
+        }
+
+        /// <summary>
+        /// 读取相对角度并仅用滤波后的轮组角速度积分完成自转。
+        /// </summary>
+        public override void Test()
+        {
+            if (!TryReadRelativeAngleDegrees(
+                    out var relativeAngleDegrees))
             {
-                Console.WriteLine(
-                    "输入角度必须满足-180° < angle < 180°；" +
-                    "当前最短角控制不支持指定精确±180°的旋转方向。");
                 return;
             }
 

MultiWheelC/Experiments/TestTrajectoryFactory.cs

@@ -20,7 +20,8 @@ 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)
         {
             return CreateStraight(
                 startPoseInWorld,
@@ -28,7 +29,8 @@ namespace MultiWheelC
                 cruiseSpeedMetersPerSecond,
                 accelerationMetersPerSecondSquared,
                 decelerationMetersPerSecondSquared,
-                pointSpacingMeters);
+                pointSpacingMeters,
+                motionDirectionInBodyRadians);
         }
 
         /// <summary>
@@ -40,7 +42,8 @@ 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)
         {
             NumericGuard.EnsureFinite(
                 startPoseInWorld,
@@ -60,6 +63,9 @@ namespace MultiWheelC
             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 worldMotionYawRadians =
+                startPoseInWorld.YawRadians +
+                motionDirectionInBodyRadians;
             var directionX = travelDirection *
-                Math.Cos(startPoseInWorld.YawRadians);
+                Math.Cos(worldMotionYawRadians);
             var directionY = travelDirection *
-                Math.Sin(startPoseInWorld.YawRadians);
+                Math.Sin(worldMotionYawRadians);
 
             for (var index = 0;
                  index <= segmentCount;
@@ -116,7 +125,7 @@ namespace MultiWheelC
         }
 
         /// <summary>
-        /// 从当前位姿按速度符号生成“3m直线、沿行进方向平滑左弯180°、3m直线”的轨迹。
+        /// 从当前位姿沿指定车体运动方向生成“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,7 +166,8 @@ namespace MultiWheelC
             double turnMaximumSpeedMetersPerSecond,
             double accelerationMetersPerSecondSquared,
             double decelerationMetersPerSecondSquared,
-            double pointSpacingMeters)
+            double pointSpacingMeters,
+            double motionDirectionInBodyRadians = 0.0)
         {
             NumericGuard.EnsureFinite(
                 startPoseInWorld,
@@ -186,6 +198,9 @@ namespace MultiWheelC
             NumericGuard.EnsureFinitePositive(
                 pointSpacingMeters,
                 nameof(pointSpacingMeters));
+            NumericGuard.EnsureFinite(
+                motionDirectionInBodyRadians,
+                nameof(motionDirectionInBodyRadians));
 
             if (turnAngleRadians > 2.0 * Math.PI)
             {
@@ -267,10 +282,13 @@ namespace MultiWheelC
 
             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;

MultiWheelC/Experiments/TrackingExperimentRecorder.cs

@@ -9,6 +9,7 @@ using System.Text;
 using System.Threading;
 using CommonUsage.Chassis;
 using MyParking.Shared;
+using MultiWheelC.Control.Allocation;
 using MultiWheelC.Control.Execution;
 using MultiWheelC.StateEstimation;
 
@@ -23,6 +24,27 @@ namespace MultiWheelC
         public double DetourX;
         public double DetourY;
         public double DetourTheta;
+        public long DetourTickRaw;
+        public double DetourLStep;
+
+        // Detour状态估计内部诊断；偏移量仅在跳变候选有效时有意义。
+        public bool HasDetourStateDiagnostics;
+        public bool DetourJumpCandidateActive;
+        public int DetourJumpCandidateConsistentFrameCount;
+        public double DetourEstimatedShiftDistanceMeters;
+        public double DetourEstimatedShiftHeadingRadians;
+        public int DetourAutomaticFrameShiftCount;
+        public string DetourStateStatusReason;
+        public double DetourDataAgeMilliseconds;
+        public double DetourSourceFrameIntervalMilliseconds;
+        public double DetourMotionPredictionTimestampSeconds;
+        public bool HasDetourInnovationDiagnostics;
+        public double DetourPositionInnovationMeters;
+        public double DetourAllowedPositionInnovationMeters;
+        public double DetourHeadingInnovationRadians;
+        public double DetourAllowedHeadingInnovationRadians;
+        public string DetourLastJumpTriggerReason;
+        public string DetourStateStatus;
 
         // 车体速度单位为m/s，角速度统一使用rad/s。
         public float CommandSpeed;
@@ -52,13 +74,19 @@ namespace MultiWheelC
         public double CurvaturePreviewDistanceMeters;
         public double FeedforwardCurvaturePerMeter;
 
-        // 并列保存Detour速度与轮速解算速度，避免StateBodyVx的数据来源产生歧义。
+        // 并列保存Detour速度与轮速解算速度，避免StateBodyVx/Vy的数据来源产生歧义。
         public bool HasVelocityDiagnostics;
         public double DetourEstimatedBodyVxMetersPerSecond;
         public bool DetourVelocityEstimateValid;
         public double WheelFeedbackRawBodyVxMetersPerSecond;
         public double WheelFeedbackFilteredBodyVxMetersPerSecond;
+        public double WheelFeedbackRawBodyVyMetersPerSecond;
+        public double WheelFeedbackFilteredBodyVyMetersPerSecond;
         public bool WheelFeedbackVelocityEstimateValid;
+        public bool HasWheelFeedbackAngularVelocityDiagnostics;
+        public double WheelFeedbackRawBodyOmegaRadiansPerSecond;
+        public double WheelFeedbackFilteredBodyOmegaRadiansPerSecond;
+        public double WheelFeedbackSampleTimestampSeconds;
 
         // 四舵轮机械角使用deg，前后虚拟GCP命令角使用rad。
         public bool HasSteeringDiagnostics;
@@ -71,6 +99,13 @@ namespace MultiWheelC
         public double ActualSteerRightFrontDegrees;
         public double ActualSteerRightRearDegrees;
         public bool HasGcpCommand;
+        public double RequestedFrontGcpAngleRadians;
+        public double RequestedRearGcpAngleRadians;
+        public double SentFrontGcpAngleRadians;
+        public double SentRearGcpAngleRadians;
+        public bool FrontGcpRateLimitActive;
+        public bool RearGcpRateLimitActive;
+        // 兼容既有分析脚本：Command角仍表示限速后实际发送角。
         public double CommandFrontGcpAngleRadians;
         public double CommandRearGcpAngleRadians;
     }
@@ -86,19 +121,28 @@ namespace MultiWheelC
             public ControlCycleTimingRecord(
                 double recorderElapsedSeconds,
                 int motionSegmentIndex,
-                ParkingControlCycleTiming timing)
+                ParkingControlCycleTiming timing,
+                GcpMotionCommand? requestedCommand,
+                GcpMotionCommand? sentCommand)
             {
                 RecorderElapsedSeconds =
                     recorderElapsedSeconds;
                 MotionSegmentIndex = motionSegmentIndex;
                 Timing = timing;
+                RequestedCommand = requestedCommand;
+                SentCommand = sentCommand;
             }
 
             public double RecorderElapsedSeconds { get; }
             public int MotionSegmentIndex { get; }
             public ParkingControlCycleTiming Timing { get; }
+            public GcpMotionCommand? RequestedCommand { get; }
+            public GcpMotionCommand? SentCommand { get; }
         }
 
+        private const double GcpAngleComparisonToleranceRadians =
+            1e-9;
+
         private readonly string _controllerName;
         private readonly string _trajectoryName;
         private readonly int _trialNumber;
@@ -111,6 +155,8 @@ namespace MultiWheelC
         private readonly float _referenceDecelerationMetersPerSecondSquared;
         private readonly int _sampleIntervalMs;
         private readonly MultiWheelChassis _diagnosticChassis;
+        private readonly WheelFeedbackVehicleStateProvider
+            _diagnosticStateProvider;
 
         private readonly List<TrackingSample> _samples =
             new List<TrackingSample>();
@@ -159,8 +205,16 @@ namespace MultiWheelC
         private bool _detourVelocityEstimateValid;
         private double _wheelFeedbackRawBodyVxMetersPerSecond;
         private double _wheelFeedbackFilteredBodyVxMetersPerSecond;
+        private double _wheelFeedbackRawBodyVyMetersPerSecond;
+        private double _wheelFeedbackFilteredBodyVyMetersPerSecond;
         private bool _wheelFeedbackVelocityEstimateValid;
         private bool _hasGcpCommand;
+        private double _requestedFrontGcpAngleRadians;
+        private double _requestedRearGcpAngleRadians;
+        private double _sentFrontGcpAngleRadians;
+        private double _sentRearGcpAngleRadians;
+        private bool _frontGcpRateLimitActive;
+        private bool _rearGcpRateLimitActive;
         private double _commandFrontGcpAngleRadians;
         private double _commandRearGcpAngleRadians;
 
@@ -176,7 +230,9 @@ namespace MultiWheelC
             float referenceMotionFrameYawDegrees = 0f,
             float referenceAccelerationMetersPerSecondSquared = 0f,
             float referenceDecelerationMetersPerSecondSquared = 0f,
-            MultiWheelChassis diagnosticChassis = null)
+            MultiWheelChassis diagnosticChassis = null,
+            WheelFeedbackVehicleStateProvider
+                diagnosticStateProvider = null)
         {
             if (string.IsNullOrWhiteSpace(controllerName))
                 throw new ArgumentException(
@@ -208,6 +264,7 @@ namespace MultiWheelC
                 referenceDecelerationMetersPerSecondSquared;
             _sampleIntervalMs = sampleIntervalMs;
             _diagnosticChassis = diagnosticChassis;
+            _diagnosticStateProvider = diagnosticStateProvider;
         }
 
         // 保存成功后的CSV绝对路径；尚未保存时为空。
@@ -288,12 +345,16 @@ namespace MultiWheelC
         /// </summary>
         public void RecordControlCycleTiming(
             ParkingControlCycleTiming timing,
-            int motionSegmentIndex = -1)
+            int motionSegmentIndex = -1,
+            GcpMotionCommand? requestedCommand = null,
+            GcpMotionCommand? sentCommand = null)
         {
             var record = new ControlCycleTimingRecord(
                 _stopwatch.Elapsed.TotalSeconds,
                 motionSegmentIndex,
-                timing);
+                timing,
+                requestedCommand,
+                sentCommand);
 
             lock (_controlCycleTimingSyncRoot)
             {
@@ -313,13 +374,15 @@ namespace MultiWheelC
         }
 
         /// <summary>
-        /// 保存同一控制周期的Detour纵向速度以及轮速解算的原始和滤波纵向速度。
+        /// 保存同一控制周期的Detour纵向速度以及轮速解算的原始和滤波平面速度。
         /// </summary>
         public void UpdateVelocityDiagnostics(
             double detourEstimatedBodyVxMetersPerSecond,
             bool detourVelocityEstimateValid,
             double wheelFeedbackRawBodyVxMetersPerSecond,
             double wheelFeedbackFilteredBodyVxMetersPerSecond,
+            double wheelFeedbackRawBodyVyMetersPerSecond,
+            double wheelFeedbackFilteredBodyVyMetersPerSecond,
             bool wheelFeedbackVelocityEstimateValid)
         {
             lock (_stateSyncRoot)
@@ -332,6 +395,10 @@ namespace MultiWheelC
                     wheelFeedbackRawBodyVxMetersPerSecond;
                 _wheelFeedbackFilteredBodyVxMetersPerSecond =
                     wheelFeedbackFilteredBodyVxMetersPerSecond;
+                _wheelFeedbackRawBodyVyMetersPerSecond =
+                    wheelFeedbackRawBodyVyMetersPerSecond;
+                _wheelFeedbackFilteredBodyVyMetersPerSecond =
+                    wheelFeedbackFilteredBodyVyMetersPerSecond;
                 _wheelFeedbackVelocityEstimateValid =
                     wheelFeedbackVelocityEstimateValid;
                 _hasVelocityDiagnostics = true;
@@ -339,18 +406,37 @@ namespace MultiWheelC
         }
 
         /// <summary>
-        /// 保存经过GCP角速度限制后实际交给底盘的前后虚拟控制点转角。
+        /// 保存本周期经过GCP角速度限制前后的前后虚拟控制点转角。
         /// </summary>
         public void UpdateGcpCommand(
-            double frontGcpAngleRadians,
-            double rearGcpAngleRadians)
+            double requestedFrontGcpAngleRadians,
+            double requestedRearGcpAngleRadians,
+            double sentFrontGcpAngleRadians,
+            double sentRearGcpAngleRadians)
         {
             lock (_stateSyncRoot)
             {
+                _requestedFrontGcpAngleRadians =
+                    requestedFrontGcpAngleRadians;
+                _requestedRearGcpAngleRadians =
+                    requestedRearGcpAngleRadians;
+                _sentFrontGcpAngleRadians =
+                    sentFrontGcpAngleRadians;
+                _sentRearGcpAngleRadians =
+                    sentRearGcpAngleRadians;
+                _frontGcpRateLimitActive =
+                    IsGcpRateLimitActive(
+                        requestedFrontGcpAngleRadians,
+                        sentFrontGcpAngleRadians);
+                _rearGcpRateLimitActive =
+                    IsGcpRateLimitActive(
+                        requestedRearGcpAngleRadians,
+                        sentRearGcpAngleRadians);
+                // 保留原字段语义，避免既有绘图脚本失效。
                 _commandFrontGcpAngleRadians =
-                    frontGcpAngleRadians;
+                    sentFrontGcpAngleRadians;
                 _commandRearGcpAngleRadians =
-                    rearGcpAngleRadians;
+                    sentRearGcpAngleRadians;
                 _hasGcpCommand = true;
             }
         }
@@ -478,6 +564,47 @@ namespace MultiWheelC
                 var location =
                     DetourInterface.getCartLocation();
 
+                var hasDetourStateDiagnostics = false;
+                var detourJumpCandidateActive = false;
+                var detourJumpCandidateConsistentFrameCount = 0;
+                var detourEstimatedShiftDistanceMeters = 0.0;
+                var detourEstimatedShiftHeadingRadians = 0.0;
+                var detourAutomaticFrameShiftCount = 0;
+                var detourStateStatusReason = string.Empty;
+                var detourDataAgeMilliseconds = 0.0;
+                var detourSourceFrameIntervalSeconds = 0.0;
+                var detourMotionPredictionTimestampSeconds = 0.0;
+                var hasDetourInnovationDiagnostics = false;
+                var detourPositionInnovationMeters = 0.0;
+                var detourAllowedPositionInnovationMeters = 0.0;
+                var detourHeadingInnovationRadians = 0.0;
+                var detourAllowedHeadingInnovationRadians = 0.0;
+                var detourLastJumpTriggerReason = string.Empty;
+                var detourStateStatus = string.Empty;
+
+                if (_diagnosticStateProvider != null)
+                {
+                    hasDetourStateDiagnostics =
+                        _diagnosticStateProvider
+                            .TryGetLatestDetourDiagnostics(
+                                out detourJumpCandidateActive,
+                                out detourJumpCandidateConsistentFrameCount,
+                                out detourEstimatedShiftDistanceMeters,
+                                out detourEstimatedShiftHeadingRadians,
+                                out detourAutomaticFrameShiftCount,
+                                out detourSourceFrameIntervalSeconds,
+                                out detourMotionPredictionTimestampSeconds,
+                                out hasDetourInnovationDiagnostics,
+                                out detourPositionInnovationMeters,
+                                out detourAllowedPositionInnovationMeters,
+                                out detourHeadingInnovationRadians,
+                                out detourAllowedHeadingInnovationRadians,
+                                out detourLastJumpTriggerReason,
+                                out detourStateStatus,
+                                out detourStateStatusReason,
+                                out detourDataAgeMilliseconds);
+                }
+
                 float commandSpeed;
                 float commandVx;
                 float commandVy;
@@ -497,8 +624,20 @@ namespace MultiWheelC
                 bool detourVelocityEstimateValid;
                 double wheelFeedbackRawBodyVxMetersPerSecond;
                 double wheelFeedbackFilteredBodyVxMetersPerSecond;
+                double wheelFeedbackRawBodyVyMetersPerSecond;
+                double wheelFeedbackFilteredBodyVyMetersPerSecond;
                 bool wheelFeedbackVelocityEstimateValid;
+                var hasWheelFeedbackAngularVelocityDiagnostics = false;
+                var wheelFeedbackRawBodyOmegaRadiansPerSecond = 0.0;
+                var wheelFeedbackFilteredBodyOmegaRadiansPerSecond = 0.0;
+                var wheelFeedbackSampleTimestampSeconds = 0.0;
                 bool hasGcpCommand;
+                double requestedFrontGcpAngleRadians;
+                double requestedRearGcpAngleRadians;
+                double sentFrontGcpAngleRadians;
+                double sentRearGcpAngleRadians;
+                bool frontGcpRateLimitActive;
+                bool rearGcpRateLimitActive;
                 double commandFrontGcpAngleRadians;
                 double commandRearGcpAngleRadians;
 
@@ -566,15 +705,71 @@ namespace MultiWheelC
                         _wheelFeedbackRawBodyVxMetersPerSecond;
                     wheelFeedbackFilteredBodyVxMetersPerSecond =
                         _wheelFeedbackFilteredBodyVxMetersPerSecond;
+                    wheelFeedbackRawBodyVyMetersPerSecond =
+                        _wheelFeedbackRawBodyVyMetersPerSecond;
+                    wheelFeedbackFilteredBodyVyMetersPerSecond =
+                        _wheelFeedbackFilteredBodyVyMetersPerSecond;
                     wheelFeedbackVelocityEstimateValid =
                         _wheelFeedbackVelocityEstimateValid;
                     hasGcpCommand = _hasGcpCommand;
+                    requestedFrontGcpAngleRadians =
+                        _requestedFrontGcpAngleRadians;
+                    requestedRearGcpAngleRadians =
+                        _requestedRearGcpAngleRadians;
+                    sentFrontGcpAngleRadians =
+                        _sentFrontGcpAngleRadians;
+                    sentRearGcpAngleRadians =
+                        _sentRearGcpAngleRadians;
+                    frontGcpRateLimitActive =
+                        _frontGcpRateLimitActive;
+                    rearGcpRateLimitActive =
+                        _rearGcpRateLimitActive;
                     commandFrontGcpAngleRadians =
                         _commandFrontGcpAngleRadians;
                     commandRearGcpAngleRadians =
                         _commandRearGcpAngleRadians;
                 }
 
+                // 直接从状态源读取最新完整轮组诊断，使原地自转等没有
+                // 控制周期回调的实验也能记录原始/滤波Vw及其采样时间。
+                if (_diagnosticStateProvider != null &&
+                    _diagnosticStateProvider
+                        .TryGetLatestVelocityDiagnostics(
+                            out var directDetourBodyVx,
+                            out var directDetourVelocityValid,
+                            out var directRawWheelBodyVx,
+                            out var directFilteredWheelBodyVx,
+                            out var directRawWheelBodyVy,
+                            out var directFilteredWheelBodyVy,
+                            out var directRawWheelBodyOmega,
+                            out var directFilteredWheelBodyOmega,
+                            out var directWheelSampleTimestampSeconds,
+                            out var directWheelVelocityValid))
+                {
+                    hasVelocityDiagnostics = true;
+                    detourEstimatedBodyVxMetersPerSecond =
+                        directDetourBodyVx;
+                    detourVelocityEstimateValid =
+                        directDetourVelocityValid;
+                    wheelFeedbackRawBodyVxMetersPerSecond =
+                        directRawWheelBodyVx;
+                    wheelFeedbackFilteredBodyVxMetersPerSecond =
+                        directFilteredWheelBodyVx;
+                    wheelFeedbackRawBodyVyMetersPerSecond =
+                        directRawWheelBodyVy;
+                    wheelFeedbackFilteredBodyVyMetersPerSecond =
+                        directFilteredWheelBodyVy;
+                    wheelFeedbackRawBodyOmegaRadiansPerSecond =
+                        directRawWheelBodyOmega;
+                    wheelFeedbackFilteredBodyOmegaRadiansPerSecond =
+                        directFilteredWheelBodyOmega;
+                    wheelFeedbackSampleTimestampSeconds =
+                        directWheelSampleTimestampSeconds;
+                    wheelFeedbackVelocityEstimateValid =
+                        directWheelVelocityValid;
+                    hasWheelFeedbackAngularVelocityDiagnostics = true;
+                }
+
                 var sample = new TrackingSample
                 {
                     ElapsedSeconds =
@@ -582,6 +777,45 @@ namespace MultiWheelC
                     DetourX = location.x,
                     DetourY = location.y,
                     DetourTheta = location.th,
+                    DetourTickRaw = Convert.ToInt64(
+                        location.tick,
+                        CultureInfo.InvariantCulture),
+                    DetourLStep = Convert.ToDouble(
+                        location.l_step,
+                        CultureInfo.InvariantCulture),
+                    HasDetourStateDiagnostics =
+                        hasDetourStateDiagnostics,
+                    DetourJumpCandidateActive =
+                        detourJumpCandidateActive,
+                    DetourJumpCandidateConsistentFrameCount =
+                        detourJumpCandidateConsistentFrameCount,
+                    DetourEstimatedShiftDistanceMeters =
+                        detourEstimatedShiftDistanceMeters,
+                    DetourEstimatedShiftHeadingRadians =
+                        detourEstimatedShiftHeadingRadians,
+                    DetourAutomaticFrameShiftCount =
+                        detourAutomaticFrameShiftCount,
+                    DetourStateStatusReason =
+                        detourStateStatusReason,
+                    DetourDataAgeMilliseconds =
+                        detourDataAgeMilliseconds,
+                    DetourSourceFrameIntervalMilliseconds =
+                        detourSourceFrameIntervalSeconds * 1000.0,
+                    DetourMotionPredictionTimestampSeconds =
+                        detourMotionPredictionTimestampSeconds,
+                    HasDetourInnovationDiagnostics =
+                        hasDetourInnovationDiagnostics,
+                    DetourPositionInnovationMeters =
+                        detourPositionInnovationMeters,
+                    DetourAllowedPositionInnovationMeters =
+                        detourAllowedPositionInnovationMeters,
+                    DetourHeadingInnovationRadians =
+                        detourHeadingInnovationRadians,
+                    DetourAllowedHeadingInnovationRadians =
+                        detourAllowedHeadingInnovationRadians,
+                    DetourLastJumpTriggerReason =
+                        detourLastJumpTriggerReason,
+                    DetourStateStatus = detourStateStatus,
                     CommandSpeed = commandSpeed,
                     CommandVx = commandVx,
                     CommandVy = commandVy,
@@ -617,9 +851,33 @@ namespace MultiWheelC
                         wheelFeedbackRawBodyVxMetersPerSecond,
                     WheelFeedbackFilteredBodyVxMetersPerSecond =
                         wheelFeedbackFilteredBodyVxMetersPerSecond,
+                    WheelFeedbackRawBodyVyMetersPerSecond =
+                        wheelFeedbackRawBodyVyMetersPerSecond,
+                    WheelFeedbackFilteredBodyVyMetersPerSecond =
+                        wheelFeedbackFilteredBodyVyMetersPerSecond,
                     WheelFeedbackVelocityEstimateValid =
                         wheelFeedbackVelocityEstimateValid,
+                    HasWheelFeedbackAngularVelocityDiagnostics =
+                        hasWheelFeedbackAngularVelocityDiagnostics,
+                    WheelFeedbackRawBodyOmegaRadiansPerSecond =
+                        wheelFeedbackRawBodyOmegaRadiansPerSecond,
+                    WheelFeedbackFilteredBodyOmegaRadiansPerSecond =
+                        wheelFeedbackFilteredBodyOmegaRadiansPerSecond,
+                    WheelFeedbackSampleTimestampSeconds =
+                        wheelFeedbackSampleTimestampSeconds,
                     HasGcpCommand = hasGcpCommand,
+                    RequestedFrontGcpAngleRadians =
+                        requestedFrontGcpAngleRadians,
+                    RequestedRearGcpAngleRadians =
+                        requestedRearGcpAngleRadians,
+                    SentFrontGcpAngleRadians =
+                        sentFrontGcpAngleRadians,
+                    SentRearGcpAngleRadians =
+                        sentRearGcpAngleRadians,
+                    FrontGcpRateLimitActive =
+                        frontGcpRateLimitActive,
+                    RearGcpRateLimitActive =
+                        rearGcpRateLimitActive,
                     CommandFrontGcpAngleRadians =
                         commandFrontGcpAngleRadians,
                     CommandRearGcpAngleRadians =
@@ -789,6 +1047,25 @@ namespace MultiWheelC
                     "DetourX," +
                     "DetourY," +
                     "DetourTheta," +
+                    "DetourTickRaw," +
+                    "DetourLStep," +
+                    "HasDetourStateDiagnostics," +
+                    "DetourJumpCandidateActive," +
+                    "DetourJumpCandidateConsistentFrameCount," +
+                    "DetourEstimatedShiftDistanceMeters," +
+                    "DetourEstimatedShiftHeadingRadians," +
+                    "DetourAutomaticFrameShiftCount," +
+                    "DetourStateStatusReason," +
+                    "DetourDataAgeMilliseconds," +
+                    "DetourSourceFrameIntervalMilliseconds," +
+                    "DetourMotionPredictionTimestampSeconds," +
+                    "HasDetourInnovationDiagnostics," +
+                    "DetourPositionInnovationMeters," +
+                    "DetourAllowedPositionInnovationMeters," +
+                    "DetourHeadingInnovationRadians," +
+                    "DetourAllowedHeadingInnovationRadians," +
+                    "DetourLastJumpTriggerReason," +
+                    "DetourStateStatus," +
                     "CommandSpeed," +
                     // 保留旧列（deg/s）供历史Python脚本兼容。
                     "CommandAngularSpeed," +
@@ -829,7 +1106,13 @@ namespace MultiWheelC
                     "DetourVelocityEstimateValid," +
                     "WheelFeedbackRawBodyVxMetersPerSecond," +
                     "WheelFeedbackFilteredBodyVxMetersPerSecond," +
+                    "WheelFeedbackRawBodyVyMetersPerSecond," +
+                    "WheelFeedbackFilteredBodyVyMetersPerSecond," +
                     "WheelFeedbackVelocityEstimateValid," +
+                    "HasWheelFeedbackAngularVelocityDiagnostics," +
+                    "WheelFeedbackRawBodyOmegaRadiansPerSecond," +
+                    "WheelFeedbackFilteredBodyOmegaRadiansPerSecond," +
+                    "WheelFeedbackSampleTimestampSeconds," +
                     "HasSteeringDiagnostics," +
                     "TargetSteerLeftFrontDegrees," +
                     "TargetSteerLeftRearDegrees," +
@@ -841,7 +1124,13 @@ namespace MultiWheelC
                     "ActualSteerRightRearDegrees," +
                     "HasGcpCommand," +
                     "CommandFrontGcpAngleRadians," +
-                    "CommandRearGcpAngleRadians");
+                    "CommandRearGcpAngleRadians," +
+                    "RequestedFrontGcpAngleRadians," +
+                    "RequestedRearGcpAngleRadians," +
+                    "SentFrontGcpAngleRadians," +
+                    "SentRearGcpAngleRadians," +
+                    "FrontGcpRateLimitActive," +
+                    "RearGcpRateLimitActive");
 
                 foreach (var sample in snapshot)
                 {
@@ -855,6 +1144,73 @@ namespace MultiWheelC
                         Format(sample.DetourX),
                         Format(sample.DetourY),
                         Format(sample.DetourTheta),
+                        sample.DetourTickRaw.ToString(
+                            CultureInfo.InvariantCulture),
+                        Format(sample.DetourLStep),
+                        sample.HasDetourStateDiagnostics
+                            ? "1"
+                            : "0",
+                        FormatOptionalBoolean(
+                            sample.HasDetourStateDiagnostics,
+                            sample.DetourJumpCandidateActive),
+                        sample.HasDetourStateDiagnostics
+                            ? sample
+                                .DetourJumpCandidateConsistentFrameCount
+                                .ToString(
+                                    CultureInfo.InvariantCulture)
+                            : string.Empty,
+                        FormatOptional(
+                            sample.HasDetourStateDiagnostics &&
+                            sample.DetourJumpCandidateActive,
+                            sample.DetourEstimatedShiftDistanceMeters),
+                        FormatOptional(
+                            sample.HasDetourStateDiagnostics &&
+                            sample.DetourJumpCandidateActive,
+                            sample.DetourEstimatedShiftHeadingRadians),
+                        sample.HasDetourStateDiagnostics
+                            ? sample.DetourAutomaticFrameShiftCount
+                                .ToString(
+                                    CultureInfo.InvariantCulture)
+                            : string.Empty,
+                        sample.HasDetourStateDiagnostics
+                            ? EscapeCsv(
+                                sample.DetourStateStatusReason)
+                            : string.Empty,
+                        FormatOptional(
+                            sample.HasDetourStateDiagnostics,
+                            sample.DetourDataAgeMilliseconds),
+                        FormatOptional(
+                            sample.HasDetourStateDiagnostics,
+                            sample.DetourSourceFrameIntervalMilliseconds),
+                        FormatOptional(
+                            sample.HasDetourStateDiagnostics,
+                            sample.DetourMotionPredictionTimestampSeconds),
+                        FormatOptionalBoolean(
+                            sample.HasDetourStateDiagnostics,
+                            sample.HasDetourInnovationDiagnostics),
+                        FormatOptional(
+                            sample.HasDetourStateDiagnostics &&
+                            sample.HasDetourInnovationDiagnostics,
+                            sample.DetourPositionInnovationMeters),
+                        FormatOptional(
+                            sample.HasDetourStateDiagnostics &&
+                            sample.HasDetourInnovationDiagnostics,
+                            sample.DetourAllowedPositionInnovationMeters),
+                        FormatOptional(
+                            sample.HasDetourStateDiagnostics &&
+                            sample.HasDetourInnovationDiagnostics,
+                            sample.DetourHeadingInnovationRadians),
+                        FormatOptional(
+                            sample.HasDetourStateDiagnostics &&
+                            sample.HasDetourInnovationDiagnostics,
+                            sample.DetourAllowedHeadingInnovationRadians),
+                        sample.HasDetourStateDiagnostics
+                            ? EscapeCsv(
+                                sample.DetourLastJumpTriggerReason)
+                            : string.Empty,
+                        sample.HasDetourStateDiagnostics
+                            ? EscapeCsv(sample.DetourStateStatus)
+                            : string.Empty,
                         Format(sample.CommandSpeed),
                         Format(
                             AngleMath.RadiansToDegrees(
@@ -952,11 +1308,32 @@ namespace MultiWheelC
                         FormatOptional(
                             sample.HasVelocityDiagnostics,
                             sample.WheelFeedbackFilteredBodyVxMetersPerSecond),
+                        FormatOptional(
+                            sample.HasVelocityDiagnostics,
+                            sample.WheelFeedbackRawBodyVyMetersPerSecond),
+                        FormatOptional(
+                            sample.HasVelocityDiagnostics,
+                            sample.WheelFeedbackFilteredBodyVyMetersPerSecond),
                         sample.HasVelocityDiagnostics
                             ? sample.WheelFeedbackVelocityEstimateValid
                                 ? "1"
                                 : "0"
                             : string.Empty,
+                        FormatOptionalBoolean(
+                            sample.HasVelocityDiagnostics,
+                            sample.HasWheelFeedbackAngularVelocityDiagnostics),
+                        FormatOptional(
+                            sample.HasVelocityDiagnostics &&
+                            sample.HasWheelFeedbackAngularVelocityDiagnostics,
+                            sample.WheelFeedbackRawBodyOmegaRadiansPerSecond),
+                        FormatOptional(
+                            sample.HasVelocityDiagnostics &&
+                            sample.HasWheelFeedbackAngularVelocityDiagnostics,
+                            sample.WheelFeedbackFilteredBodyOmegaRadiansPerSecond),
+                        FormatOptional(
+                            sample.HasVelocityDiagnostics &&
+                            sample.HasWheelFeedbackAngularVelocityDiagnostics,
+                            sample.WheelFeedbackSampleTimestampSeconds),
                         sample.HasSteeringDiagnostics
                             ? "1"
                             : "0",
@@ -992,7 +1369,25 @@ namespace MultiWheelC
                             sample.CommandFrontGcpAngleRadians),
                         FormatOptional(
                             sample.HasGcpCommand,
-                            sample.CommandRearGcpAngleRadians)));
+                            sample.CommandRearGcpAngleRadians),
+                        FormatOptional(
+                            sample.HasGcpCommand,
+                            sample.RequestedFrontGcpAngleRadians),
+                        FormatOptional(
+                            sample.HasGcpCommand,
+                            sample.RequestedRearGcpAngleRadians),
+                        FormatOptional(
+                            sample.HasGcpCommand,
+                            sample.SentFrontGcpAngleRadians),
+                        FormatOptional(
+                            sample.HasGcpCommand,
+                            sample.SentRearGcpAngleRadians),
+                        FormatOptionalBoolean(
+                            sample.HasGcpCommand,
+                            sample.FrontGcpRateLimitActive),
+                        FormatOptionalBoolean(
+                            sample.HasGcpCommand,
+                            sample.RearGcpRateLimitActive)));
                 }
             }
         }
@@ -1046,7 +1441,14 @@ namespace MultiWheelC
                     "HasStateTimestamp," +
                     "StateTimestampSeconds," +
                     "StateTimestampChanged," +
-                    "CycleResult");
+                    "CycleResult," +
+                    "HasGcpCommand," +
+                    "RequestedFrontGcpAngleRadians," +
+                    "RequestedRearGcpAngleRadians," +
+                    "SentFrontGcpAngleRadians," +
+                    "SentRearGcpAngleRadians," +
+                    "FrontGcpRateLimitActive," +
+                    "RearGcpRateLimitActive");
 
                 foreach (var record in snapshot)
                 {
@@ -1060,6 +1462,23 @@ namespace MultiWheelC
                         0.0,
                         timing.TotalCycleMilliseconds -
                         measuredStageMilliseconds);
+                    var hasGcpCommand =
+                        record.RequestedCommand.HasValue &&
+                        record.SentCommand.HasValue;
+                    var requestedCommand =
+                        record.RequestedCommand.GetValueOrDefault();
+                    var sentCommand =
+                        record.SentCommand.GetValueOrDefault();
+                    var frontRateLimitActive =
+                        hasGcpCommand &&
+                        IsGcpRateLimitActive(
+                            requestedCommand.FrontAngleRadians,
+                            sentCommand.FrontAngleRadians);
+                    var rearRateLimitActive =
+                        hasGcpCommand &&
+                        IsGcpRateLimitActive(
+                            requestedCommand.RearAngleRadians,
+                            sentCommand.RearAngleRadians);
 
                     writer.WriteLine(string.Join(
                         ",",
@@ -1092,7 +1511,28 @@ namespace MultiWheelC
                                 ? "1"
                                 : "0"
                             : string.Empty,
-                        EscapeCsv(timing.Result.ToString())));
+                        EscapeCsv(timing.Result.ToString()),
+                        hasGcpCommand
+                            ? "1"
+                            : "0",
+                        FormatOptional(
+                            hasGcpCommand,
+                            requestedCommand.FrontAngleRadians),
+                        FormatOptional(
+                            hasGcpCommand,
+                            requestedCommand.RearAngleRadians),
+                        FormatOptional(
+                            hasGcpCommand,
+                            sentCommand.FrontAngleRadians),
+                        FormatOptional(
+                            hasGcpCommand,
+                            sentCommand.RearAngleRadians),
+                        FormatOptionalBoolean(
+                            hasGcpCommand,
+                            frontRateLimitActive),
+                        FormatOptionalBoolean(
+                            hasGcpCommand,
+                            rearRateLimitActive)));
                 }
             }
         }
@@ -1133,6 +1573,33 @@ namespace MultiWheelC
                 : string.Empty;
         }
 
+        /// <summary>
+        /// 在GCP命令有效时将布尔诊断输出为0或1，否则保持CSV空值。
+        /// </summary>
+        private static string FormatOptionalBoolean(
+            bool hasValue,
+            bool value)
+        {
+            return hasValue
+                ? value
+                    ? "1"
+                    : "0"
+                : string.Empty;
+        }
+
+        /// <summary>
+        /// 判断GCP角速度限制是否实质改变了控制器请求角度。
+        /// </summary>
+        private static bool IsGcpRateLimitActive(
+            double requestedAngleRadians,
+            double sentAngleRadians)
+        {
+            return Math.Abs(
+                       requestedAngleRadians -
+                       sentAngleRadians) >
+                   GcpAngleComparisonToleranceRadians;
+        }
+
         // 对CSV文本字段进行引号和逗号转义。
         private static string EscapeCsv(string value)
         {

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

@@ -14,6 +14,11 @@ namespace MultiWheelC
     /// </summary>
     public class PrepareWheelsForward : MovementDefinition
     {
+        /// <summary>
+        /// 获取或设置舵轮需要对准的车体方向，单位为rad；0表示车头方向。
+        /// </summary>
+        public double DirectionRadians;
+
         /// <summary>
         /// 获取或设置本次动作的回正到位容差覆盖值，单位为deg；为空时读取车辆配置。
         /// </summary>
@@ -36,6 +41,10 @@ namespace MultiWheelC
         /// </summary>
         public override IEnumerable<bool> Get()
         {
+            NumericGuard.EnsureFinite(
+                DirectionRadians,
+                nameof(DirectionRadians));
+
             var config = PilotDefinition.Conf;
             var toleranceDegrees =
                 ToleranceDegrees ??
@@ -75,10 +84,11 @@ namespace MultiWheelC
             DateTime? alignedSince = null;
 
             Completed = false;
-            if (!adapter.PrepareParallelDirection(0.0))
+            if (!adapter.PrepareParallelDirection(
+                    DirectionRadians))
             {
                 throw new InvalidOperationException(
-                    "无法将所有舵轮下发到车体前向0°。");
+                    "无法将所有舵轮下发到指定运动方向。");
             }
 
             try
@@ -87,7 +97,7 @@ namespace MultiWheelC
                 {
                     var aligned =
                         adapter.AreParallelWheelsAligned(
-                            0.0,
+                            DirectionRadians,
                             toleranceRadians);
 
                     if (aligned)
@@ -122,7 +132,7 @@ namespace MultiWheelC
             }
             finally
             {
-                // 只清零驱动速度，保留已经下发的0°舵角。
+                // 只清零驱动速度，保留已经下发的目标舵角。
                 adapter.StopImmediately();
             }
         }

MultiWheelC/Movements/RotateInPlace.cs

@@ -12,15 +12,30 @@ using MultiWheelC.StateEstimation;
 namespace MultiWheelC
 {
     /// <summary>
-    /// 将四个舵轮准备到自转姿态并按世界航向闭环旋转，正常完成后等待舵轮回正。
+    /// 指定原地自转使用Detour绝对航向或轮组相对角度反馈。
+    /// </summary>
+    public enum InPlaceRotationFeedbackMode
+    {
+        DetourAbsoluteHeading,
+        RelativeWheelOdometry
+    }
+
+    /// <summary>
+    /// 将四个舵轮准备到自转姿态，按所选反馈旋转并在完成后等待舵轮回正。
     /// </summary>
     public class MultiWheelRotateInPlace : MovementDefinition
     {
         /// <summary>
-        /// 旋转目标角度
+        /// Detour模式表示世界目标航向，轮组模式表示有符号相对旋转角度，单位deg。
         /// </summary>
         public float AngleTarget;
 
+        /// <summary>
+        /// 获取或设置原地自转反馈模式；默认保持现有Detour绝对航向闭环。
+        /// </summary>
+        public InPlaceRotationFeedbackMode FeedbackMode =
+            InPlaceRotationFeedbackMode.DetourAbsoluteHeading;
+
         // 留作标定或单元测试时显式替换；为空时使用配置化Detour与电机反馈组合状态源。
         public Func<float> ThetaReader;
 
@@ -152,10 +167,32 @@ 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
+                        ? AngleTarget
+                        : (float)AngleMath.NormalizeDegrees(
+                            AngleTarget);
                 var currentAngle =
-                    ReadCurrentAngleDegrees(stateProvider);
+                    useRelativeWheelOdometry
+                        ? 0f
+                        : ReadCurrentAngleDegrees(stateProvider);
                 var cachedCurrentAngle = currentAngle;
                 thPid = new PIDController(
                     () => cachedCurrentAngle,
@@ -170,6 +207,10 @@ namespace MultiWheelC
                     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)
                 {
@@ -181,12 +222,71 @@ namespace MultiWheelC
                             $"原地自转超过{rotationTimeoutSeconds:F1}s仍未到位。");
                     }
 
+                    if (useRelativeWheelOdometry)
+                    {
+                        if (!wheelStateProvider.TryGetWheelTwist(
+                                out var wheelTwist,
+                                out var wheelTimestampSeconds))
+                        {
+                            CommandAngularSpeedObserver?.Invoke(0f);
+                            adapter
+                                .StopXYThDrivePreserveSteeringState();
+
+                            if (hasPreviousWheelSample)
+                            {
+                                throw new InvalidOperationException(
+                                    "原地自转期间轮组角速度不可用：" +
+                                    wheelStateProvider.LastFailureReason);
+                            }
+
+                            yield return true;
+                            continue;
+                        }
+
+                        if (hasPreviousWheelSample)
+                        {
+                            var wheelDeltaTimeSeconds =
+                                wheelTimestampSeconds -
+                                previousWheelTimestampSeconds;
+                            if (!NumericGuard.IsFinite(
+                                    wheelDeltaTimeSeconds) ||
+                                wheelDeltaTimeSeconds <= 0.0)
+                            {
+                                throw new InvalidOperationException(
+                                    "轮组角速度采样时间没有单调递增。");
+                            }
+
+                            accumulatedWheelAngleRadians +=
+                                0.5 *
+                                (previousWheelOmegaRadiansPerSecond +
+                                 wheelTwist
+                                     .OmegaRadiansPerSecond) *
+                                wheelDeltaTimeSeconds;
+                        }
+
+                        previousWheelOmegaRadiansPerSecond =
+                            wheelTwist.OmegaRadiansPerSecond;
+                        previousWheelTimestampSeconds =
+                            wheelTimestampSeconds;
+                        hasPreviousWheelSample = true;
+                        currentAngle =
+                            (float)AngleMath.RadiansToDegrees(
+                                accumulatedWheelAngleRadians);
+                    }
+                    else
+                    {
                         currentAngle =
                             ReadCurrentAngleDegrees(stateProvider);
+                    }
+
                     cachedCurrentAngle = currentAngle;
-                    var s = thPid.GetResponse(targetAngle, true);
+                    var s = thPid.GetResponse(
+                        targetAngle,
+                        !useRelativeWheelOdometry);
                     var angleErrorDegrees =
-                        (float)AngleMath
+                        useRelativeWheelOdometry
+                            ? targetAngle - currentAngle
+                            : (float)AngleMath
                                 .ShortestDifferenceDegrees(
                                     targetAngle,
                                     currentAngle);
@@ -200,6 +300,8 @@ namespace MultiWheelC
                         adapter
                             .StopXYThDrivePreserveSteeringState();
 
+                        // 到位稳定只依赖已经单独校验的航向。
+                        // 位置候选留到停车后处理，避免位置抖动中断航向闭环。
                         if (thPid.IsArrived())
                             break;
 
@@ -233,17 +335,14 @@ namespace MultiWheelC
                     var interval = now - lastCommandTime;
                     lastCommandTime = now;
 
-                    // PID输出s为deg/s，Shared命令统一使用rad/s。
-                    // adapter.Send最终调用普通安全版SendXYThSpeed。
+                    // PID输出s为deg/s，Shared统一使用车体坐标系Twist2D和rad/s。
                     var omegaRadiansPerSecond =
                         (float)AngleMath.DegreesToRadians(s);
-                    if (!adapter.Send(
-                            new ChassisCommand(
-                                PilotDefinition.Self.CarNum,
+                    if (!adapter.SendBodyTwist(
                             new Twist2D(
                                 0.0,
                                 0.0,
-                                    omegaRadiansPerSecond)),
+                                omegaRadiansPerSecond),
                             interval))
                     {
                         throw new InvalidOperationException(
@@ -256,6 +355,41 @@ namespace MultiWheelC
                 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();
@@ -276,7 +410,10 @@ namespace MultiWheelC
                 }
 
                 Console.WriteLine(
-                    $"final rotate to {targetAngle}, wheels forward");
+                    useRelativeWheelOdometry
+                        ? "final relative wheel rotate to " +
+                          $"{currentAngle:F2}deg, wheels forward"
+                        : $"final rotate to {targetAngle}, wheels forward");
             }
             finally
             {
@@ -312,6 +449,33 @@ namespace MultiWheelC
                 rotationTimeoutSeconds,
                 nameof(RotationTimeoutSeconds));
 
+            if (float.IsNaN(AngleTarget) ||
+                float.IsInfinity(AngleTarget))
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(AngleTarget),
+                    "原地自转目标角度必须是有限值。");
+            }
+
+            if (!Enum.IsDefined(
+                    typeof(InPlaceRotationFeedbackMode),
+                    FeedbackMode))
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(FeedbackMode),
+                    "原地自转反馈模式无效。");
+            }
+
+            if (FeedbackMode ==
+                    InPlaceRotationFeedbackMode
+                        .RelativeWheelOdometry &&
+                Math.Abs(AngleTarget) >= 180f)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(AngleTarget),
+                    "轮组相对自转角度必须满足-180° < angle < 180°。");
+            }
+
             if (pidParameters == null)
             {
                 throw new InvalidOperationException(
@@ -359,6 +523,36 @@ namespace MultiWheelC
                     angleDegrees);
             }
 
+            if (stateProvider is
+                WheelFeedbackVehicleStateProvider wheelProvider)
+            {
+                if (!wheelProvider.TryGetHeadingRadians(
+                        out var wheelHeadingRadians))
+                {
+                    throw new InvalidOperationException(
+                        "无法从Detour状态源读取有效车辆航向。" +
+                        wheelProvider.LastHeadingFailureReason);
+                }
+
+                return (float)AngleMath.RadiansToDegrees(
+                    wheelHeadingRadians);
+            }
+
+            if (stateProvider is
+                DetourVehicleStateProvider detourProvider)
+            {
+                if (!detourProvider.TryGetHeadingRadians(
+                        out var detourHeadingRadians))
+                {
+                    throw new InvalidOperationException(
+                        "无法从Detour状态源读取有效车辆航向。" +
+                        detourProvider.LastHeadingFailureReason);
+                }
+
+                return (float)AngleMath.RadiansToDegrees(
+                    detourHeadingRadians);
+            }
+
             if (stateProvider == null ||
                 !stateProvider.TryGetState(out var state))
             {
@@ -372,6 +566,26 @@ namespace MultiWheelC
                 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>
@@ -393,6 +607,31 @@ namespace MultiWheelC
             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

@@ -21,6 +21,11 @@ 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>
@@ -42,6 +47,25 @@ namespace MultiWheelC
         /// </summary>
         public Action<ParkingGeometricController> CycleObserver;
 
+        /// <summary>
+        /// 获取或设置本动作运动坐标系X轴在车体系中的方向，单位为rad；为空时从轨迹自动推导。
+        /// </summary>
+        public double? MotionDirectionInBodyRadians = 0.0;
+
+        /// <summary>
+        /// 获取本次执行最终采用的运动坐标系方向，动作尚未开始时为空。
+        /// </summary>
+        public double? ResolvedMotionDirectionInBodyRadians
+        {
+            get;
+            private set;
+        }
+
+        /// <summary>
+        /// 获取或设置轨迹正常完成后是否停车并将舵轮主动恢复到车头方向。
+        /// </summary>
+        public bool ReturnWheelsForwardAfterCompletion;
+
         /// <summary>
         /// 获取或设置本次动作的Stanley横向误差增益覆盖值，单位为1/s；为空时读取车辆配置。
         /// </summary>
@@ -260,6 +284,12 @@ namespace MultiWheelC
                 config.ParkingExecutionTimeoutSeconds;
 
             ValidateParameters(executionTimeoutSeconds);
+            var motionDirectionInBodyRadians =
+                MotionDirectionInBodyRadians ??
+                ResolveFixedMotionDirectionInBodyRadians(
+                    Trajectory);
+            ResolvedMotionDirectionInBodyRadians =
+                motionDirectionInBodyRadians;
 
             var chassis =
                 PilotDefinition.Chassis as MultiWheelChassis;
@@ -270,7 +300,11 @@ namespace MultiWheelC
             }
 
             var wheelPreparation =
-                new PrepareWheelsForward();
+                new PrepareWheelsForward
+                {
+                    DirectionRadians =
+                        motionDirectionInBodyRadians
+                };
             foreach (var keepRunning in wheelPreparation.Get())
             {
                 if (!keepRunning)
@@ -284,15 +318,15 @@ namespace MultiWheelC
             if (!wheelPreparation.Completed)
             {
                 throw new InvalidOperationException(
-                    "轨迹跟踪开始前舵轮未能稳定回到车头方向。");
+                    "轨迹跟踪开始前舵轮未能稳定到达目标运动方向。");
             }
 
             var adapter = new MultiWheelChassisAdapter(
                 chassis,
                 PilotDefinition.Self.CarNum);
 
-            // 新版GCP控制统一以真实车头为车体X正方向，避免继承上一次蟹行偏置。
-            adapter.ResetToBodyFrame();
+            adapter.ActivateMotionFrame(
+                motionDirectionInBodyRadians);
 
             var stateProvider =
                 StateProvider ??
@@ -333,7 +367,8 @@ namespace MultiWheelC
             var commandExecutor =
                 new GcpCommandExecutor(
                     adapter,
-                    maximumGcpAngleRateRadiansPerSecond);
+                    maximumGcpAngleRateRadiansPerSecond,
+                    motionDirectionInBodyRadians);
 
             Controller = new ParkingGeometricController(
                 stateProvider,
@@ -350,7 +385,8 @@ namespace MultiWheelC
                 terminalApproachGainPerSecond,
                 maximumTerminalApproachSpeedMetersPerSecond,
                 stanleyCurvaturePreviewSeconds,
-                stanleyMaximumCurvaturePreviewMeters);
+                stanleyMaximumCurvaturePreviewMeters,
+                motionDirectionInBodyRadians);
 
             var clock = Stopwatch.StartNew();
             var previousCycleSeconds =
@@ -425,6 +461,28 @@ 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;
         }
 
@@ -440,6 +498,13 @@ namespace MultiWheelC
                     "新版轨迹跟踪动作没有设置Trajectory。");
             }
 
+            if (MotionDirectionInBodyRadians.HasValue)
+            {
+                NumericGuard.EnsureFinite(
+                    MotionDirectionInBodyRadians.Value,
+                    nameof(MotionDirectionInBodyRadians));
+            }
+
             if (double.IsNaN(executionTimeoutSeconds) ||
                 double.IsInfinity(executionTimeoutSeconds) ||
                 executionTimeoutSeconds <= 0.0)
@@ -449,5 +514,114 @@ namespace MultiWheelC
                     "轨迹跟踪超时时间必须是正有限值。");
             }
         }
+
+        /// <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/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

@@ -5,7 +5,7 @@ using MyParking.Shared;
 namespace MultiWheelC.StateEstimation
 {
     /// <summary>
-    /// 根据车载配置创建Detour位姿过滤与电机反馈纵向速度组合的停车状态源。
+    /// 根据车载配置创建Detour位姿过滤与电机反馈平面速度组合的停车状态源。
     /// </summary>
     public static class ParkingVehicleStateProviderFactory
     {
@@ -55,7 +55,19 @@ namespace MultiWheelC.StateEstimation
                     AngleMath.DegreesToRadians(
                         config
                             .ParkingDetourVelocityHeadingResidualDegrees),
-                    config.ParkingDetourStationaryConfirmationSeconds);
+                    config.ParkingDetourStationaryConfirmationSeconds,
+                    config.ParkingDetourHeadingOutlierConfirmationFrames,
+                    config
+                        .ParkingDetourHeadingOutlierPredictionTimeoutSeconds,
+                    config.ParkingDetourJumpConfirmationFrames,
+                    config.ParkingDetourJumpConfirmationTimeoutSeconds,
+                    config.ParkingDetourMaximumAutomaticFrameShift,
+                    AngleMath.DegreesToRadians(
+                        config
+                            .ParkingDetourMaximumAutomaticHeadingShiftDegrees),
+                    config.ParkingDetourMaximumCachedFrameAgeSeconds,
+                    config
+                        .ParkingDetourLocalizationQualityConfirmationFrames);
 
             return new WheelFeedbackVehicleStateProvider(
                 detourStateProvider,

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

@@ -6,7 +6,7 @@ using System.Diagnostics;
 namespace MultiWheelC.StateEstimation
 {
     /// <summary>
-    /// 保留外部状态源的Detour位姿，并以舵轮电机反馈解算的车体纵向速度替换Detour差分纵向速度。
+    /// 保留外部状态源的Detour位姿，以轮组反馈替换平面线速度，并向短时位姿预测提供角速度。
     /// </summary>
     public sealed class WheelFeedbackVehicleStateProvider
         : IVehicleStateProvider
@@ -19,6 +19,8 @@ namespace MultiWheelC.StateEstimation
         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;
@@ -27,10 +29,16 @@ namespace MultiWheelC.StateEstimation
         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低通时间常数的电机反馈纵向速度状态源。
+        /// 创建使用默认0.10s低通时间常数的电机反馈平面速度状态源。
         /// </summary>
         public WheelFeedbackVehicleStateProvider(
             IVehicleStateProvider poseProvider,
@@ -43,7 +51,7 @@ namespace MultiWheelC.StateEstimation
         }
 
         /// <summary>
-        /// 创建使用指定低通时间常数的电机反馈纵向速度状态源。
+        /// 创建使用指定低通时间常数的电机反馈平面速度状态源。
         /// </summary>
         public WheelFeedbackVehicleStateProvider(
             IVehicleStateProvider poseProvider,
@@ -59,6 +67,12 @@ namespace MultiWheelC.StateEstimation
             _longitudinalSpeedFilter =
                 new FirstOrderLowPassFilter(
                     velocityFilterTimeConstantSeconds);
+            _lateralSpeedFilter =
+                new FirstOrderLowPassFilter(
+                    velocityFilterTimeConstantSeconds);
+            _angularSpeedFilter =
+                new FirstOrderLowPassFilter(
+                    velocityFilterTimeConstantSeconds);
         }
 
         /// <summary>
@@ -67,6 +81,12 @@ namespace MultiWheelC.StateEstimation
         public string LastFailureReason { get; private set; } =
             string.Empty;
 
+        /// <summary>
+        /// 获取最近一次航向读取失败的原因；位置单独异常时保持为空。
+        /// </summary>
+        public string LastHeadingFailureReason { get; private set; } =
+            string.Empty;
+
         /// <summary>
         /// 读取Detour位姿和电机反馈速度，并组合成统一车辆状态。
         /// </summary>
@@ -74,55 +94,51 @@ namespace MultiWheelC.StateEstimation
         {
             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;
                     }
 
-                try
-                {
-                    var actualCarSpeed =
-                        _chassis.GetCarSpeed(true);
-                    var rawLongitudinalSpeedMetersPerSecond =
-                        (double)actualCarSpeed.Vx;
-
-                    EnsureFinite(
-                        rawLongitudinalSpeedMetersPerSecond,
-                        "电机反馈车体纵向速度");
-
-                    var wheelSpeedTimestampSeconds =
-                        _wheelSpeedClock.Elapsed.TotalSeconds;
-
-                    var filteredLongitudinalSpeedMetersPerSecond =
-                        UpdateLongitudinalSpeedFilter(
-                            rawLongitudinalSpeedMetersPerSecond,
-                            wheelSpeedTimestampSeconds,
-                            out var hasValidWheelSpeedEstimate);
-
                     _latestDetourBodyVxMetersPerSecond =
                         poseState.TwistInBody.VxMetersPerSecond;
                     _latestDetourVelocityValid =
                         poseState.HasValidVelocityEstimate;
-                    _latestRawWheelBodyVxMetersPerSecond =
-                        rawLongitudinalSpeedMetersPerSecond;
-                    _latestFilteredWheelBodyVxMetersPerSecond =
-                        filteredLongitudinalSpeedMetersPerSecond;
-                    _latestWheelVelocityValid =
-                        hasValidWheelSpeedEstimate;
                     _hasVelocityDiagnostics = true;
 
-                    // 第一阶段只替换控制器使用的车体纵向速度；横向速度和角速度
-                    // 继续使用Detour估计，避免轮速差和舵角误差放大Vy与Omega噪声。
+                    // 轮组反馈有效后统一使用滤波后的平面速度；初始化期间
+                    // 暂时保留Detour角速度作为回退值。
+                    var omegaRadiansPerSecond =
+                        hasValidWheelSpeedEstimate
+                            ? filteredWheelTwist
+                                .OmegaRadiansPerSecond
+                            : poseState.TwistInBody
+                                .OmegaRadiansPerSecond;
                     var twistInBody = new Twist2D(
-                        filteredLongitudinalSpeedMetersPerSecond,
-                        poseState.TwistInBody
-                            .VyMetersPerSecond,
-                        poseState.TwistInBody
-                            .OmegaRadiansPerSecond);
+                        filteredWheelTwist.VxMetersPerSecond,
+                        filteredWheelTwist.VyMetersPerSecond,
+                        omegaRadiansPerSecond);
 
                     var twistInWorld =
                         FrameTransform2D
@@ -141,6 +157,7 @@ namespace MultiWheelC.StateEstimation
                 }
                 catch (Exception exception)
                 {
+                    _latestWheelFeedbackReadSucceeded = false;
                     state = default;
                     LastFailureReason =
                         "舵轮电机反馈车体速度解算失败：" +
@@ -151,13 +168,135 @@ namespace MultiWheelC.StateEstimation
         }
 
         /// <summary>
-        /// 读取最近一帧Detour纵向速度和轮速解算纵向速度，供实验记录使用。
+        /// 读取并滤波轮组反馈速度，不访问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)
@@ -170,6 +309,10 @@ namespace MultiWheelC.StateEstimation
                     _latestRawWheelBodyVxMetersPerSecond;
                 filteredWheelBodyVxMetersPerSecond =
                     _latestFilteredWheelBodyVxMetersPerSecond;
+                rawWheelBodyVyMetersPerSecond =
+                    _latestRawWheelBodyVyMetersPerSecond;
+                filteredWheelBodyVyMetersPerSecond =
+                    _latestFilteredWheelBodyVyMetersPerSecond;
                 wheelVelocityValid =
                     _latestWheelVelocityValid;
                 return _hasVelocityDiagnostics;
@@ -177,13 +320,188 @@ namespace MultiWheelC.StateEstimation
         }
 
         /// <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;
@@ -192,31 +510,120 @@ namespace MultiWheelC.StateEstimation
                 _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 double UpdateLongitudinalSpeedFilter(
-            double rawLongitudinalSpeedMetersPerSecond,
-            double timestampSeconds,
+        private void ReadFilteredWheelTwist(
+            out Twist2D filteredTwistInBody,
+            out double sampleTimestampSeconds,
             out bool hasValidWheelSpeedEstimate)
         {
-            EnsureFiniteNonNegative(
+            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(
-                    rawLongitudinalSpeedMetersPerSecond);
+                    rawBodyVxMetersPerSecond);
+                _lateralSpeedFilter.Reset(
+                    rawBodyVyMetersPerSecond);
+                _angularSpeedFilter.Reset(
+                    rawBodyOmegaRadiansPerSecond);
                 _previousTimestampSeconds = timestampSeconds;
                 _hasPreviousTimestamp = true;
                 hasValidWheelSpeedEstimate = false;
-                return rawLongitudinalSpeedMetersPerSecond;
+                filteredBodyVxMetersPerSecond =
+                    rawBodyVxMetersPerSecond;
+                filteredBodyVyMetersPerSecond =
+                    rawBodyVyMetersPerSecond;
+                filteredBodyOmegaRadiansPerSecond =
+                    rawBodyOmegaRadiansPerSecond;
+                return;
             }
 
             var deltaTimeSeconds =
@@ -227,48 +634,35 @@ namespace MultiWheelC.StateEstimation
             if (deltaTimeSeconds <= 0.0)
             {
                 _longitudinalSpeedFilter.Reset(
-                    rawLongitudinalSpeedMetersPerSecond);
+                    rawBodyVxMetersPerSecond);
+                _lateralSpeedFilter.Reset(
+                    rawBodyVyMetersPerSecond);
+                _angularSpeedFilter.Reset(
+                    rawBodyOmegaRadiansPerSecond);
                 hasValidWheelSpeedEstimate = false;
-                return rawLongitudinalSpeedMetersPerSecond;
+                filteredBodyVxMetersPerSecond =
+                    rawBodyVxMetersPerSecond;
+                filteredBodyVyMetersPerSecond =
+                    rawBodyVyMetersPerSecond;
+                filteredBodyOmegaRadiansPerSecond =
+                    rawBodyOmegaRadiansPerSecond;
+                return;
             }
 
             hasValidWheelSpeedEstimate = true;
-            return _longitudinalSpeedFilter.Update(
-                rawLongitudinalSpeedMetersPerSecond,
+            filteredBodyVxMetersPerSecond =
+                _longitudinalSpeedFilter.Update(
+                    rawBodyVxMetersPerSecond,
+                    deltaTimeSeconds);
+            filteredBodyVyMetersPerSecond =
+                _lateralSpeedFilter.Update(
+                    rawBodyVyMetersPerSecond,
+                    deltaTimeSeconds);
+            filteredBodyOmegaRadiansPerSecond =
+                _angularSpeedFilter.Update(
+                    rawBodyOmegaRadiansPerSecond,
                     deltaTimeSeconds);
         }
 
-        /// <summary>
-        /// 检查采样时刻是否为非负有限值。
-        /// </summary>
-        private static void EnsureFiniteNonNegative(
-            double value,
-            string parameterName)
-        {
-            EnsureFinite(value, parameterName);
-
-            if (value < 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "采样时刻必须是非负有限值。");
-            }
-        }
-
-        /// <summary>
-        /// 检查状态输入是否为有限值。
-        /// </summary>
-        private static void EnsureFinite(
-            double value,
-            string parameterName)
-        {
-            if (double.IsNaN(value) ||
-                double.IsInfinity(value))
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "车辆状态输入必须是有限值。");
-            }
-        }
     }
 }

Shared/Chassis/MultiWheelChassisAdapter.cs

@@ -1,58 +1,74 @@
-// 将统一命令转换为原 Chassis API 调用
+// Shared层底盘边界：对外使用SI单位，对内适配旧版MultiWheelChassis的混合单位接口。
 using System;
 using CommonUsage.Chassis;
 
 namespace MyParking.Shared
 {
     /// <summary>
-    /// 将统一的单车车体速度命令转换为旧版MultiWheelChassis调用。
-    /// 车体坐标系固定为X向前、Y向左、逆时针为正。
+    /// 将真实车体系刚体速度转换为旧版MultiWheelChassis命令，车体系约定为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.
-        /// For a symmetric four-wheel-steering chassis this is half the wheelbase.
+        /// 车体原点到最前或最后舵轮中心的最大纵向距离，单位为m；对称四舵轮底盘中通常为轴距的一半。
         /// </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) ||
-                    double.IsInfinity(value) ||
-                    value <= 0.0 ||
-                    value > float.MaxValue)
-                {
-                    throw new ArgumentOutOfRangeException(
-                        nameof(value),
-                        "Steering alignment sigma must be a positive finite value.");
-                }
+                NumericGuard.EnsureFinitePositive(
+                    value,
+                    nameof(value));
+                EnsureRepresentableAsSingle(
+                    value,
+                    nameof(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)(
-                    -FrameTransform2D.NormalizeAngle(
-                        motionDirectionRadians) *
-                    RadiansToDegrees);
+                ConvertRadiansToSingleDegrees(
+                    -AngleMath.NormalizeRadians(
+                        motionDirectionRadians),
+                    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(
-                twist.OmegaRadiansPerSecond,
+            EnsureRepresentableAsSingle(
+                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) ||
-                double.IsInfinity(value))
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "底盘速度命令不能是NaN或无穷大。");
-            }
+            NumericGuard.EnsureFinite(value, parameterName);
 
             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坐标系。
-        /// 0表示真实车头，正90度表示将车体左侧作为虚拟车头。
+        /// 激活指定β对应的SendMotion运动坐标系；调用前必须停车并完成该方向的舵轮预对齐。
         /// </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)(
-                    -FrameTransform2D.NormalizeAngle(
-                        motionDirectionRadians) *
-                    RadiansToDegrees);
+                ConvertRadiansToSingleDegrees(
+                    -normalizedDirectionRadians,
+                    nameof(motionDirectionRadians));
             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坐标偏置，
-            // 保证SendXYThSpeed直接使用真实车体坐标系。
+            // 几何尺寸必须取PhysicalPosition，避免受旧底盘当前原点偏置和运动坐标系影响。
             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)
-        {
-            if (command.VehicleId != VehicleId)
-            {
-                throw new InvalidOperationException(
-                    $"命令车辆编号{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,
+        /// <param name="bodyTwist">真实车体系速度，线速度单位为m/s，角速度单位为rad/s。</param>
+        /// <param name="interval">与上一条底盘命令的实际时间间隔，用于旧底盘速度和舵角变化率处理。</param>
+        /// <returns>旧底盘是否成功接受并完成运动分解。</returns>
+        public bool SendBodyTwist(
+            Twist2D bodyTwist,
             TimeSpan? interval = null)
         {
-            ValidateFinite(
-                motionDirectionRadians,
-                nameof(motionDirectionRadians));
-            ValidateFinite(
-                speedMetersPerSecond,
-                nameof(speedMetersPerSecond));
-            ValidateFinite(
-                steeringRadians,
-                nameof(steeringRadians));
-            EnsureMotionFrameIsActive(
-                motionDirectionRadians);
+            ValidateTwist(bodyTwist);
 
-            if (Math.Abs(steeringRadians) >=
-                Math.PI / 2.0)
+            var linearSpeedMetersPerSecond =
+                Math.Sqrt(
+                    bodyTwist.VxMetersPerSecond *
+                    bodyTwist.VxMetersPerSecond +
+                    bodyTwist.VyMetersPerSecond *
+                    bodyTwist.VyMetersPerSecond);
+
+            if (linearSpeedMetersPerSecond <= MotionDeadband)
             {
-                throw new ArgumentOutOfRangeException(
-                    nameof(steeringRadians),
-                    "虚拟阿克曼转向角必须位于正负90度以内。");
+                if (Math.Abs(
+                        bodyTwist.OmegaRadiansPerSecond) <=
+                    MotionDeadband)
+                {
+                    StopImmediately();
+                    return true;
                 }
 
-            var steeringDegrees =
-                (float)(
-                    steeringRadians *
-                    RadiansToDegrees);
-            var success =
-                _chassis.SendMotion(
-                    (float)speedMetersPerSecond,
-                    steeringDegrees,
-                    -steeringDegrees,
+                return SendPureRotation(
+                    bodyTwist.OmegaRadiansPerSecond,
                     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 *
-                        RadiansToDegrees),
-                (float)(rearAngleRadians *
-                        RadiansToDegrees),
+                ConvertRadiansToSingleDegrees(
+                    frontAngleRadians,
+                    nameof(frontAngleRadians)),
+                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) *
-                                    RadiansToDegrees);
+            var targetDegrees =
+                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) ||
-        double.IsInfinity(toleranceRadians) ||
-        toleranceRadians < 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    nameof(toleranceRadians),
-                    "舵轮到位容差必须是非负有限值。");
-            }
+            NumericGuard.EnsureFiniteNonNegative(
+                toleranceRadians,
+                nameof(toleranceRadians));
 
             EnsureBodyFrameIsActive();
-            var targetDegrees = (float)(
-                FrameTransform2D.NormalizeAngle(directionRadians) *
-                180.0 / Math.PI);
+            var targetDegrees =
+                ConvertRadiansToSingleDegrees(
+                    AngleMath.NormalizeRadians(
+                        directionRadians),
+                    nameof(directionRadians));
 
-            var toleranceDegrees = (float)(
-                Math.Abs(toleranceRadians) *
-                180.0 / Math.PI);
+            var toleranceDegrees =
+                ConvertRadiansToSingleDegrees(
+                    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) ||
-                double.IsInfinity(toleranceRadians) ||
-                toleranceRadians < 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    nameof(toleranceRadians),
-                    "自转状态交接容差必须是非负有限值。");
-            }
+            NumericGuard.EnsureFiniteNonNegative(
+                toleranceRadians,
+                nameof(toleranceRadians));
 
             EnsureBodyFrameIsActive();
 
             var success =
                 _chassis
                     .AdoptPreparedRotateWheelsForXYTh(
-                        (float)(
-                            toleranceRadians *
-                            RadiansToDegrees));
+                        ConvertRadiansToSingleDegrees(
+                            toleranceRadians,
+                            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);
+    }
+}

Shared/Validation/NumericGuard.cs

@@ -2,20 +2,22 @@ using System;
 
 namespace MyParking.Shared
 {
-    /// <summary>
-    /// 统一检查跨层数值和二维位姿是否由满足基本范围要求的有限值组成。
-    /// </summary>
+    // 统一拒绝跨层数据中的NaN、无穷大和不符合正负范围的数值。
     public static class NumericGuard
     {
-        /// <summary>
-        /// 确保指定浮点数不是NaN或无穷大。
-        /// </summary>
+        // 判断浮点数是否为有限值。
+        public static bool IsFinite(double value)
+        {
+            return !double.IsNaN(value) &&
+                   !double.IsInfinity(value);
+        }
+
+        // 拒绝NaN和无穷大。
         public static void EnsureFinite(
             double value,
             string parameterName)
         {
-            if (double.IsNaN(value) ||
-                double.IsInfinity(value))
+            if (!IsFinite(value))
             {
                 throw new ArgumentOutOfRangeException(
                     parameterName,
@@ -23,9 +25,7 @@ namespace MyParking.Shared
             }
         }
 
-        /// <summary>
-        /// 确保指定浮点数是非负有限值。
-        /// </summary>
+        // 拒绝负数、NaN和无穷大。
         public static void EnsureFiniteNonNegative(
             double value,
             string parameterName)
@@ -40,9 +40,7 @@ namespace MyParking.Shared
             }
         }
 
-        /// <summary>
-        /// 确保指定浮点数是正有限值。
-        /// </summary>
+        // 拒绝零、负数、NaN和无穷大。
         public static void EnsureFinitePositive(
             double value,
             string parameterName)
@@ -57,24 +55,34 @@ namespace MyParking.Shared
             }
         }
 
-        /// <summary>
-        /// 确保二维位姿的位置和航向均为有限值。
-        /// </summary>
+        // 同时检查二维位姿的位置和航向。
         public static void EnsureFinite(
             Pose2D pose,
             string parameterName)
         {
-            if (double.IsNaN(pose.XMeters) ||
-                double.IsInfinity(pose.XMeters) ||
-                double.IsNaN(pose.YMeters) ||
-                double.IsInfinity(pose.YMeters) ||
-                double.IsNaN(pose.YawRadians) ||
-                double.IsInfinity(pose.YawRadians))
+            if (!IsFinite(pose.XMeters) ||
+                !IsFinite(pose.YMeters) ||
+                !IsFinite(pose.YawRadians))
             {
                 throw new ArgumentOutOfRangeException(
                     parameterName,
                     "二维位姿必须由有限值组成。");
             }
         }
+
+        // 同时检查二维刚体速度的两个线速度分量和角速度。
+        public static void EnsureFinite(
+            Twist2D twist,
+            string parameterName)
+        {
+            if (!IsFinite(twist.VxMetersPerSecond) ||
+                !IsFinite(twist.VyMetersPerSecond) ||
+                !IsFinite(twist.OmegaRadiansPerSecond))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "二维刚体速度必须由有限值组成。");
+            }
+        }
     }
 }

data_process/plot_new_controller_experiment.py

@@ -14,6 +14,10 @@ import pandas as pd
 
 
 SCRIPT_DIR = Path(__file__).resolve().parent
+LATERAL_JUMP_THRESHOLD_METERS = 0.03
+JUMP_INSET_CONTEXT_SAMPLES = 6
+MAXIMUM_PLAUSIBLE_LINEAR_SPEED_METERS_PER_SECOND = 1.20
+POSITION_JUMP_MARGIN_METERS = 0.03
 
 
 def configure_matplotlib() -> None:
@@ -57,6 +61,16 @@ def first_text(frame: pd.DataFrame, name: str, default: str) -> str:
     return values.iloc[0] if not values.empty else default
 
 
+def text_column(frame: pd.DataFrame, name: str) -> np.ndarray:
+    """读取用于诊断标注的原始文本列，缺失值转换为空字符串。"""
+    if name not in frame.columns:
+        return np.full(len(frame), "", dtype=object)
+    return frame[name].fillna("").astype(str).to_numpy(
+        dtype=object,
+        copy=True,
+    )
+
+
 def fill_reference_series(
     values: np.ndarray,
     fallback: np.ndarray,
@@ -169,6 +183,11 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
 
     raw_x_meters = numeric_column(frame, "DetourX") / 1000.0
     raw_y_meters = numeric_column(frame, "DetourY") / 1000.0
+    valid_raw_position = (
+        np.isfinite(raw_x_meters) & np.isfinite(raw_y_meters)
+    )
+    detour_tick_raw = text_column(frame, "DetourTickRaw")
+    detour_l_step = numeric_column(frame, "DetourLStep")
     actual_x = np.where(processed_valid, state_x, raw_x_meters)
     actual_y = np.where(processed_valid, state_y, raw_y_meters)
     valid_position = np.isfinite(actual_x) & np.isfinite(actual_y)
@@ -257,6 +276,67 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
         & np.isfinite(reference_y)
     )
 
+    # 同时检查Detour是否超出车辆物理运动边界，以及控制状态的横向误差
+    # 是否发生离散突变。后者能覆盖状态层延迟接受持续定位偏移的情况。
+    raw_delta_x = np.full(len(frame), np.nan, dtype=float)
+    raw_delta_y = np.full(len(frame), np.nan, dtype=float)
+    sample_delta_time = np.full(len(frame), np.nan, dtype=float)
+    raw_delta_x[1:] = np.diff(raw_x_meters)
+    raw_delta_y[1:] = np.diff(raw_y_meters)
+    sample_delta_time[1:] = np.diff(time_seconds)
+    detour_position_step = np.hypot(raw_delta_x, raw_delta_y)
+
+    detour_tick_numeric = numeric_column(frame, "DetourTickRaw")
+    detour_tick_delta_seconds = np.full(len(frame), np.nan, dtype=float)
+    detour_tick_delta_seconds[1:] = (
+        np.diff(detour_tick_numeric) / 10_000_000.0
+    )
+    source_delta_time = sample_delta_time.copy()
+    valid_tick_delta = (
+        np.isfinite(detour_tick_delta_seconds)
+        & (detour_tick_delta_seconds > 0.0)
+        & (detour_tick_delta_seconds <= 0.5)
+    )
+    source_delta_time[valid_tick_delta] = (
+        detour_tick_delta_seconds[valid_tick_delta]
+    )
+
+    consecutive_raw_position_valid = np.zeros(len(frame), dtype=bool)
+    consecutive_raw_position_valid[1:] = (
+        valid_raw_position[1:] & valid_raw_position[:-1]
+    )
+    maximum_plausible_position_step = (
+        MAXIMUM_PLAUSIBLE_LINEAR_SPEED_METERS_PER_SECOND
+        * source_delta_time
+        + POSITION_JUMP_MARGIN_METERS
+    )
+    raw_detour_jump = (
+        consecutive_raw_position_valid
+        & np.isfinite(detour_position_step)
+        & np.isfinite(source_delta_time)
+        & (source_delta_time > 0.0)
+        & (source_delta_time <= 0.5)
+        & (detour_position_step > maximum_plausible_position_step)
+    )
+
+    state_lateral_step = np.full(len(frame), np.nan, dtype=float)
+    state_lateral_step[1:] = np.diff(lateral_error)
+    state_lateral_jump = (
+        np.isfinite(state_lateral_step)
+        & np.isfinite(sample_delta_time)
+        & (sample_delta_time > 0.0)
+        & (sample_delta_time <= 0.5)
+        & (np.abs(state_lateral_step) >= LATERAL_JUMP_THRESHOLD_METERS)
+    )
+    suspected_jump = raw_detour_jump | state_lateral_jump
+    jump_indices = np.flatnonzero(suspected_jump)
+    jump_magnitude = np.zeros(len(frame), dtype=float)
+    jump_magnitude[raw_detour_jump] = detour_position_step[raw_detour_jump]
+    jump_magnitude[state_lateral_jump] = np.maximum(
+        jump_magnitude[state_lateral_jump],
+        np.abs(state_lateral_step[state_lateral_jump]),
+    )
+
     cruise_speed = first_finite(
         numeric_column(frame, "ReferenceSpeed"),
         0.30,
@@ -293,11 +373,22 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
     )
     if np.any(has_control_reference):
         reference_speed[~has_control_reference] = np.nan
+    motion_frame_yaw_radians = np.deg2rad(
+        numeric_column(frame, "ReferenceMotionFrameYawDegrees", 0.0)
+    )
+    motion_direction_cosine = np.cos(motion_frame_yaw_radians)
+    motion_direction_sine = np.sin(motion_frame_yaw_radians)
     state_body_vx = numeric_column(frame, "StateBodyVxMetersPerSecond")
+    state_body_vy = numeric_column(frame, "StateBodyVyMetersPerSecond")
     velocity_valid = (
         numeric_column(frame, "StateVelocityEstimateValid", 0.0) > 0.5
     )
     state_body_vx[~velocity_valid] = np.nan
+    state_body_vy[~velocity_valid] = np.nan
+    state_motion_speed = (
+        state_body_vx * motion_direction_cosine
+        + state_body_vy * motion_direction_sine
+    )
     has_velocity_diagnostics = (
         numeric_column(frame, "HasVelocityDiagnostics", 0.0) > 0.5
     )
@@ -317,14 +408,22 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
         )
     )
     detour_speed[~detour_speed_valid] = np.nan
-    wheel_raw_speed = numeric_column(
+    wheel_raw_body_vx = numeric_column(
         frame,
         "WheelFeedbackRawBodyVxMetersPerSecond",
     )
-    wheel_filtered_speed = numeric_column(
+    wheel_filtered_body_vx = numeric_column(
         frame,
         "WheelFeedbackFilteredBodyVxMetersPerSecond",
     )
+    wheel_raw_body_vy = numeric_column(
+        frame,
+        "WheelFeedbackRawBodyVyMetersPerSecond",
+    )
+    wheel_filtered_body_vy = numeric_column(
+        frame,
+        "WheelFeedbackFilteredBodyVyMetersPerSecond",
+    )
     wheel_speed_valid = (
         has_velocity_diagnostics
         & (
@@ -336,12 +435,33 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
             > 0.5
         )
     )
+    wheel_raw_speed = (
+        wheel_raw_body_vx * motion_direction_cosine
+        + wheel_raw_body_vy * motion_direction_sine
+    )
+    wheel_filtered_speed = (
+        wheel_filtered_body_vx * motion_direction_cosine
+        + wheel_filtered_body_vy * motion_direction_sine
+    )
+
+    # 兼容尚未记录轮速Vy的旧版β=0实验；非零β缺少Vy时不能伪造投影速度。
+    body_x_motion = np.abs(motion_direction_sine) <= 1e-12
+    missing_raw_projection = ~np.isfinite(wheel_raw_speed)
+    missing_filtered_projection = ~np.isfinite(wheel_filtered_speed)
+    wheel_raw_speed[body_x_motion & missing_raw_projection] = (
+        wheel_raw_body_vx[body_x_motion & missing_raw_projection]
+    )
+    wheel_filtered_speed[
+        body_x_motion & missing_filtered_projection
+    ] = wheel_filtered_body_vx[
+        body_x_motion & missing_filtered_projection
+    ]
     wheel_raw_speed[~wheel_speed_valid] = np.nan
     wheel_filtered_speed[~wheel_speed_valid] = np.nan
     actual_speed = np.where(
         np.isfinite(wheel_filtered_speed),
         wheel_filtered_speed,
-        state_body_vx,
+        state_motion_speed,
     )
     command_speed = numeric_column(frame, "CommandSpeed")
 
@@ -380,6 +500,17 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
         "actual_x": actual_x,
         "actual_y": actual_y,
         "valid_position": valid_position,
+        "raw_x": raw_x_meters,
+        "raw_y": raw_y_meters,
+        "valid_raw_position": valid_raw_position,
+        "detour_tick_raw": detour_tick_raw,
+        "detour_l_step": detour_l_step,
+        "detour_position_step": detour_position_step,
+        "state_lateral_step": state_lateral_step,
+        "raw_detour_jump": raw_detour_jump,
+        "state_lateral_jump": state_lateral_jump,
+        "jump_magnitude": jump_magnitude,
+        "jump_indices": jump_indices,
         "reference_x": reference_x,
         "reference_y": reference_y,
         "valid_reference_position": valid_reference_position,
@@ -444,7 +575,9 @@ def plot_experiment(
     # 1. 期望轨迹与实际轨迹。
     axis = axes[0, 0]
     valid_position = data["valid_position"]
+    valid_raw_position = data["valid_raw_position"]
     valid_reference_position = data["valid_reference_position"]
+    jump_indices = data["jump_indices"]
     if np.count_nonzero(valid_reference_position) >= 2:
         axis.plot(
             data["reference_x"][valid_reference_position],
@@ -461,26 +594,166 @@ def plot_experiment(
             linewidth=2.0,
             label="参考起终点连线",
         )
+    axis.plot(
+        data["raw_x"][valid_raw_position],
+        data["raw_y"][valid_raw_position],
+        ":",
+        color="tab:gray",
+        linewidth=1.2,
+        alpha=0.85,
+        label="Detour原始轨迹",
+    )
     axis.plot(
         data["actual_x"][valid_position],
         data["actual_y"][valid_position],
+        color="tab:orange",
         linewidth=1.5,
-        label="状态估计后的实际轨迹",
+        label="控制使用的状态轨迹",
+    )
+    if jump_indices.size:
+        axis.scatter(
+            data["raw_x"][jump_indices],
+            data["raw_y"][jump_indices],
+            color="red",
+            marker="x",
+            s=65,
+            linewidths=1.8,
+            zorder=8,
+            label="疑似定位/状态突变",
         )
     axis.scatter(*data["start"], color="green", s=45, label="起点")
     axis.scatter(*data["end"], color="red", s=45, label="终点")
-    axis.set_aspect("equal", adjustable="box")
+    # 诊断图优先展示厘米级横向变化；横纵轴独立缩放，避免4m行程
+    # 将数厘米的定位阶跃压缩成几乎不可见的一条细线。
+    axis.set_aspect("auto")
     axis.set_xlabel("世界坐标X / m")
     axis.set_ylabel("世界坐标Y / m")
-    axis.set_title("期望轨迹与实际轨迹对比")
+    axis.set_title("期望轨迹与状态轨迹对比（横纵轴独立缩放）")
     axis.grid(True, alpha=0.3)
-    axis.legend(fontsize=8)
+    axis.legend(fontsize=7, loc="upper left")
+
+    if jump_indices.size:
+        strongest_jump_index = int(
+            jump_indices[
+                np.argmax(
+                    np.abs(
+                        data["jump_magnitude"][jump_indices]
+                    )
+                )
+            ]
+        )
+        context_start = max(
+            0,
+            strongest_jump_index - JUMP_INSET_CONTEXT_SAMPLES,
+        )
+        context_end = min(
+            len(data["time"]),
+            strongest_jump_index + JUMP_INSET_CONTEXT_SAMPLES + 1,
+        )
+        context = np.arange(context_start, context_end)
+        inset = axis.inset_axes([0.54, 0.08, 0.43, 0.43])
+        inset.set_zorder(10)
+        inset.set_facecolor("white")
+        context_reference_valid = (
+            data["valid_reference_position"][context]
+        )
+        if np.count_nonzero(context_reference_valid) >= 2:
+            reference_context = context[context_reference_valid]
+            inset.plot(
+                data["reference_x"][reference_context],
+                data["reference_y"][reference_context],
+                "--",
+                linewidth=1.2,
+                color="tab:blue",
+            )
+        context_raw_valid = data["valid_raw_position"][context]
+        raw_context = context[context_raw_valid]
+        inset.plot(
+            data["raw_x"][raw_context],
+            data["raw_y"][raw_context],
+            ":",
+            linewidth=1.0,
+            color="tab:gray",
+        )
+        context_state_valid = data["valid_position"][context]
+        state_context = context[context_state_valid]
+        inset.plot(
+            data["actual_x"][state_context],
+            data["actual_y"][state_context],
+            linewidth=1.2,
+            color="tab:orange",
+        )
+        inset.scatter(
+            data["raw_x"][strongest_jump_index],
+            data["raw_y"][strongest_jump_index],
+            color="red",
+            marker="x",
+            s=45,
+            linewidths=1.5,
+            zorder=8,
+        )
+        jump_descriptions = []
+        if data["raw_detour_jump"][strongest_jump_index]:
+            jump_descriptions.append(
+                "Detour位移="
+                f"{data['detour_position_step'][strongest_jump_index] * 1000.0:.1f}mm"
+            )
+        if data["state_lateral_jump"][strongest_jump_index]:
+            jump_descriptions.append(
+                "状态横向Δ="
+                f"{data['state_lateral_step'][strongest_jump_index] * 1000.0:+.1f}mm"
+            )
+        diagnostic_parts = []
+        detour_tick = data["detour_tick_raw"][strongest_jump_index]
+        if detour_tick:
+            diagnostic_parts.append(f"tick={detour_tick}")
+        detour_l_step = data["detour_l_step"][strongest_jump_index]
+        if np.isfinite(detour_l_step):
+            diagnostic_parts.append(f"l_step={detour_l_step:g}")
+        diagnostic_suffix = (
+            "\n" + "  ".join(diagnostic_parts)
+            if diagnostic_parts
+            else ""
+        )
+        inset.set_title(
+            f"最大疑似突变：t={data['time'][strongest_jump_index]:.3f}s\n"
+            f"{'，'.join(jump_descriptions)}"
+            f"{diagnostic_suffix}",
+            fontsize=7,
+        )
+        inset.set_aspect("auto")
+        inset.tick_params(labelsize=6)
+        inset.grid(True, alpha=0.25)
 
     # 2. 横向误差。
     lateral_mm = data["lateral_error"] * 1000.0
     lateral_rmse_mm = finite_rmse(lateral_mm)
     axis = axes[0, 1]
     axis.plot(data["time"], lateral_mm, linewidth=1.5)
+    if jump_indices.size:
+        for jump_index in jump_indices:
+            axis.axvline(
+                data["time"][jump_index],
+                color="red",
+                linewidth=0.8,
+                alpha=0.35,
+            )
+        valid_jump_error = (
+            data["state_lateral_jump"][jump_indices]
+            & np.isfinite(lateral_mm[jump_indices])
+        )
+        visible_jump_indices = jump_indices[valid_jump_error]
+        if visible_jump_indices.size:
+            axis.scatter(
+                data["time"][visible_jump_indices],
+                lateral_mm[visible_jump_indices],
+                color="red",
+                marker="x",
+                s=45,
+                linewidths=1.5,
+                zorder=7,
+                label="控制状态横向突变",
+            )
     axis.axhline(0.0, color="black", linewidth=0.8)
     axis.set_xlabel("时间 / s")
     axis.set_ylabel("横向误差 / mm")
@@ -489,6 +762,11 @@ def plot_experiment(
         f"RMSE={lateral_rmse_mm:.2f}mm"
     )
     axis.grid(True, alpha=0.3)
+    if jump_indices.size and np.any(
+        data["state_lateral_jump"][jump_indices]
+        & np.isfinite(lateral_mm[jump_indices])
+    ):
+        axis.legend(fontsize=8)
 
     # 3. 航向误差。
     heading_degrees = np.rad2deg(data["heading_error"])
@@ -504,7 +782,7 @@ def plot_experiment(
     )
     axis.grid(True, alpha=0.3)
 
-    # 4. 参考、命令、Detour估计和轮速解算速度。
+    # 4. 参考、命令、Detour车头分量和沿β投影的轮速解算速度。
     speed_error = data["actual_speed"] - data["reference_speed"]
     speed_rmse = finite_rmse(speed_error)
     axis = axes[1, 1]
@@ -527,7 +805,7 @@ def plot_experiment(
             data["detour_speed"],
             ":",
             linewidth=1.2,
-            label="Detour估计Vx",
+            label="Detour估计Vx（车头分量）",
         )
     if np.any(np.isfinite(data["wheel_filtered_speed"])):
         wheel_filtered_valid = np.isfinite(
@@ -540,7 +818,7 @@ def plot_experiment(
             s=14,
             marker="o",
             zorder=5,
-            label="轮速解算滤波Vx（控制使用）",
+            label="轮速解算β方向速度（控制使用）",
         )
     else:
         axis.plot(
@@ -553,7 +831,7 @@ def plot_experiment(
     axis.set_ylabel("速度 / (m/s)")
     axis.set_title(
         "参考速度、控制命令与观测速度\n"
-        f"轮速Vx相对参考速度RMSE={speed_rmse:.4f}m/s"
+        f"轮速β方向速度相对参考速度RMSE={speed_rmse:.4f}m/s"
     )
     axis.grid(True, alpha=0.3)
     axis.legend(fontsize=8)
@@ -637,21 +915,58 @@ def plot_experiment(
         f"航向RMSE={heading_rmse_degrees:.4f}°, "
         f"速度RMSE={speed_rmse:.5f}m/s"
     )
+    if jump_indices.size:
+        strongest_jump_index = int(
+            jump_indices[
+                np.argmax(
+                    np.abs(
+                        data["jump_magnitude"][jump_indices]
+                    )
+                )
+            ]
+        )
+        print(
+            f"  检出{jump_indices.size}个疑似定位/状态突变，"
+            f"最大幅值={data['jump_magnitude'][strongest_jump_index] * 1000.0:.2f}mm，"
+            f"时刻={data['time'][strongest_jump_index]:.3f}s"
+        )
     print(f"已生成六子图总图：{destination}")
     return [destination]
 
 
 def discover_csv_files(arguments: list[str]) -> list[Path]:
-    """读取命令行文件；未指定时扫描脚本目录及data子目录中的CSV。"""
+    """读取命令行文件或目录；目录中只选取非计时CSV。"""
     if arguments:
-        files = [Path(item).expanduser().resolve() for item in arguments]
+        files = []
+        for item in arguments:
+            path = Path(item).expanduser().resolve()
+            if path.is_dir():
+                files.extend(
+                    sorted(
+                        candidate
+                        for candidate in path.glob("*.csv")
+                        if not candidate.stem.endswith("_timing")
+                    )
+                )
             else:
-        files = sorted(SCRIPT_DIR.glob("*.csv"))
-        files.extend(sorted((SCRIPT_DIR / "data").glob("*.csv")))
-    files = [path for path in files if path.is_file()]
+                files.append(path)
+    else:
+        files = sorted(
+            path
+            for path in SCRIPT_DIR.glob("*.csv")
+            if not path.stem.endswith("_timing")
+        )
+        files.extend(
+            sorted(
+                path
+                for path in (SCRIPT_DIR / "data").glob("*.csv")
+                if not path.stem.endswith("_timing")
+            )
+        )
+    files = list(dict.fromkeys(path for path in files if path.is_file()))
     if not files:
         raise FileNotFoundError(
-            "没有找到CSV；请传入文件路径，或将文件放到脚本目录/data中。"
+            "没有找到轨迹CSV；请传入文件、目录，或将文件放到脚本目录/data中。"
         )
     return files
 
@@ -661,7 +976,11 @@ def main() -> None:
     parser = argparse.ArgumentParser(
         description="绘制新版控制器轨迹实验的六子图总图。"
     )
-    parser.add_argument("csv", nargs="*", help="需要处理的CSV文件路径。")
+    parser.add_argument(
+        "csv",
+        nargs="*",
+        help="需要处理的轨迹CSV文件或包含轨迹CSV的目录。",
+    )
     parser.add_argument(
         "--output-dir",
         help="图片输出目录；默认使用脚本目录/plots。",

data_process/跳变排查/第三次加角速度反馈角速度30加速度40测试/jilu.txt

@@ -0,0 +1,53 @@
+: * (Exception):DriveTask failed, msg=无法从Detour状态源读取有效车辆航向。Detour位姿偏移在短时确认后仍未恢复，且不能安全地自动连续化：平移0.052m，航向2.82°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.InPlaceRotateTestBase.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 145
+   at MultiWheelC.TestRotateAngle.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 240
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):无法从Detour状态源读取有效车辆航向。Detour位姿偏移在短时确认后仍未恢复，且不能安全地自动连续化：平移0.052m，航向2.82°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at MultiWheelC.MultiWheelRotateInPlace.ReadCurrentAngleDegrees(IVehicleStateProvider stateProvider) in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 367
+   at MultiWheelC.MultiWheelRotateInPlace.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 184
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+: * (Exception):DriveTask failed, msg=无法从Detour状态源读取有效车辆航向。Detour位姿偏移在短时确认后仍未恢复，且不能安全地自动连续化：平移0.068m，航向0.30°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.InPlaceRotateTestBase.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 145
+   at MultiWheelC.TestRotateAngle.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 240
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):无法从Detour状态源读取有效车辆航向。Detour位姿偏移在短时确认后仍未恢复，且不能安全地自动连续化：平移0.068m，航向0.30°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at MultiWheelC.MultiWheelRotateInPlace.ReadCurrentAngleDegrees(IVehicleStateProvider stateProvider) in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 367
+   at MultiWheelC.MultiWheelRotateInPlace.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 184
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+: * (Exception):DriveTask failed, msg=无法从Detour状态源读取有效车辆航向。Detour位姿偏移在短时确认后仍未恢复，且不能安全地自动连续化：平移0.050m，航向4.45°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.InPlaceRotateTestBase.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 145
+   at MultiWheelC.TestRotateAngle.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 240
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):无法从Detour状态源读取有效车辆航向。Detour位姿偏移在短时确认后仍未恢复，且不能安全地自动连续化：平移0.050m，航向4.45°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at MultiWheelC.MultiWheelRotateInPlace.ReadCurrentAngleDegrees(IVehicleStateProvider stateProvider) in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 367
+   at MultiWheelC.MultiWheelRotateInPlace.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 184
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+: * (Exception):DriveTask failed, msg=无法从Detour状态源读取有效车辆航向。Detour位姿偏移在短时确认后仍未恢复，且不能安全地自动连续化：平移0.084m，航向1.08°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.InPlaceRotateTestBase.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 145
+   at MultiWheelC.TestRotateAngle.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 240
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):无法从Detour状态源读取有效车辆航向。Detour位姿偏移在短时确认后仍未恢复，且不能安全地自动连续化：平移0.084m，航向1.08°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at MultiWheelC.MultiWheelRotateInPlace.ReadCurrentAngleDegrees(IVehicleStateProvider stateProvider) in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 367
+   at MultiWheelC.MultiWheelRotateInPlace.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 184
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+
+
+
+
+

data_process/跳变排查/第二次增加小范围坐标系修正后/其他/jilu.txt

@@ -0,0 +1,38 @@
+: * (Exception):DriveTask failed, msg=无法从Detour状态源读取有效车辆航向。Detour位姿偏移超过自动连续化范围：平移0.034m，航向5.66°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.InPlaceRotateTestBase.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 145
+   at MultiWheelC.TestRotateAngle.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 240
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):无法从Detour状态源读取有效车辆航向。Detour位姿偏移超过自动连续化范围：平移0.034m，航向5.66°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at MultiWheelC.MultiWheelRotateInPlace.ReadCurrentAngleDegrees(IVehicleStateProvider stateProvider) in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 362
+   at MultiWheelC.MultiWheelRotateInPlace.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 184
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+: * (Exception):DriveTask failed, msg=无法从Detour状态源读取有效车辆航向。Detour位姿偏移超过自动连续化范围：平移0.073m，航向5.61°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.InPlaceRotateTestBase.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 145
+   at MultiWheelC.TestRotateAngle.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 240
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):无法从Detour状态源读取有效车辆航向。Detour位姿偏移超过自动连续化范围：平移0.073m，航向5.61°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at MultiWheelC.MultiWheelRotateInPlace.ReadCurrentAngleDegrees(IVehicleStateProvider stateProvider) in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 362
+   at MultiWheelC.MultiWheelRotateInPlace.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 184
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+: * (Exception):DriveTask failed, msg=无法从Detour状态源读取有效车辆航向。Detour位姿偏移超过自动连续化范围：平移0.037m，航向6.82°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.InPlaceRotateTestBase.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 145
+   at MultiWheelC.TestRotateAngle.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 240
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):无法从Detour状态源读取有效车辆航向。Detour位姿偏移超过自动连续化范围：平移0.037m，航向6.82°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at MultiWheelC.MultiWheelRotateInPlace.ReadCurrentAngleDegrees(IVehicleStateProvider stateProvider) in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 362
+   at MultiWheelC.MultiWheelRotateInPlace.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 184
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+
+
+

data_process/跳变排查/第五次/TrackingExperiments/jilu.txt

@@ -0,0 +1,43 @@
+: * (Exception):DriveTask failed, msg=无法从Detour状态源读取有效车辆航向。Detour单帧航向变化超出物理边界。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.InPlaceRotateTestBase.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 145
+   at MultiWheelC.TestRotateAngle.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 240
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):无法从Detour状态源读取有效车辆航向。Detour单帧航向变化超出物理边界。, stack:
+   at MultiWheelC.MultiWheelRotateInPlace.ReadCurrentAngleDegrees(IVehicleStateProvider stateProvider) in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 401
+   at MultiWheelC.MultiWheelRotateInPlace.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 184
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.130m，航向误差=0.08°。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.NewControllerStraight4mTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\NewControllerTrackingTests.cs:line 264
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.130m，航向误差=0.08°。, stack:
+   at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 419
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+: * (Exception):DriveTask failed, msg=原地自转完成后Detour位置在限定时间内未恢复。Detour位姿偏移在短时确认后仍未恢复，且不能安全地自动连续化：平移0.187m，航向1.34°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.InPlaceRotateTestBase.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 145
+   at MultiWheelC.TestRotateAngle.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 240
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):原地自转完成后Detour位置在限定时间内未恢复。Detour位姿偏移在短时确认后仍未恢复，且不能安全地自动连续化：平移0.187m，航向1.34°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at MultiWheelC.MultiWheelRotateInPlace.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 282
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.240m，航向误差=0.24°。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.NewControllerStraight4mTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\NewControllerTrackingTests.cs:line 264
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.240m，航向误差=0.24°。, stack:
+   at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 419
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+

data_process/跳变排查/第六次/TrackingExperiments/记录.txt

@@ -0,0 +1,70 @@
+: * (Exception):DriveTask failed, msg=无法从Detour状态源读取有效车辆航向。Detour航向连续异常达到确认条件，航向暂不可用。最后原因：Detour航向创新超过当前动态允许值。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.InPlaceRotateTestBase.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 145
+   at MultiWheelC.TestRotateAngle.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 240
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):无法从Detour状态源读取有效车辆航向。Detour航向连续异常达到确认条件，航向暂不可用。最后原因：Detour航向创新超过当前动态允许值。, stack:
+   at MultiWheelC.MultiWheelRotateInPlace.ReadCurrentAngleDegrees(IVehicleStateProvider stateProvider) in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 401
+   at MultiWheelC.MultiWheelRotateInPlace.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 184
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+: * (Exception):DriveTask failed, msg=停车机器人轨迹控制周期异常：MultiWheelChassis当前运动坐标系与命令不一致。当前偏置为X=0, Y=0, Th=-0°，期望Th=-45°。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.NewControllerStraight4mTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\NewControllerTrackingTests.cs:line 264
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):停车机器人轨迹控制周期异常：MultiWheelChassis当前运动坐标系与命令不一致。当前偏置为X=0, Y=0, Th=-0°，期望Th=-45°。, stack:
+   at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 419
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+  *p.InnerException * (InvalidOperationException):MultiWheelChassis当前运动坐标系与命令不一致。当前偏置为X=0, Y=0, Th=-0°，期望Th=-45°。, stack:
+   at MyParking.Shared.MultiWheelChassisAdapter.EnsureMotionFrameIsActive(Double motionDirectionRadians) in D:\Users\Desktop\入职培训\停车机器人\MyParking\Shared\Chassis\MultiWheelChassisAdapter.cs:line 115
+   at MyParking.Shared.MultiWheelChassisAdapter.SendRollingTwistInActiveMotionFrame(Twist2D bodyTwist, Double linearSpeedMetersPerSecond, Nullable`1 interval) in D:\Users\Desktop\入职培训\停车机器人\MyParking\Shared\Chassis\MultiWheelChassisAdapter.cs:line 365
+   at MyParking.Shared.MultiWheelChassisAdapter.SendBodyTwist(Twist2D bodyTwist, Nullable`1 interval) in D:\Users\Desktop\入职培训\停车机器人\MyParking\Shared\Chassis\MultiWheelChassisAdapter.cs:line 351
+   at MultiWheelC.Control.Execution.GcpCommandExecutor.Execute(GcpMotionCommand command, Double deltaTimeSeconds) in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Control\Execution\GcpCommandExecutor.cs:line 125
+   at MultiWheelC.Control.Execution.ParkingGeometricController.ExecuteCycle(Double deltaTimeSeconds) in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Control\Execution\ParkingGeometricController.cs:line 517
+
+: * (Exception):DriveTask failed, msg=停车机器人轨迹控制周期异常：MultiWheelChassis当前运动坐标系与命令不一致。当前偏置为X=0, Y=0, Th=-0°，期望Th=-45°。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.NewControllerStraight4mTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\NewControllerTrackingTests.cs:line 264
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):停车机器人轨迹控制周期异常：MultiWheelChassis当前运动坐标系与命令不一致。当前偏置为X=0, Y=0, Th=-0°，期望Th=-45°。, stack:
+   at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 419
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+  *p.InnerException * (InvalidOperationException):MultiWheelChassis当前运动坐标系与命令不一致。当前偏置为X=0, Y=0, Th=-0°，期望Th=-45°。, stack:
+   at MyParking.Shared.MultiWheelChassisAdapter.EnsureMotionFrameIsActive(Double motionDirectionRadians) in D:\Users\Desktop\入职培训\停车机器人\MyParking\Shared\Chassis\MultiWheelChassisAdapter.cs:line 115
+   at MyParking.Shared.MultiWheelChassisAdapter.SendRollingTwistInActiveMotionFrame(Twist2D bodyTwist, Double linearSpeedMetersPerSecond, Nullable`1 interval) in D:\Users\Desktop\入职培训\停车机器人\MyParking\Shared\Chassis\MultiWheelChassisAdapter.cs:line 365
+   at MyParking.Shared.MultiWheelChassisAdapter.SendBodyTwist(Twist2D bodyTwist, Nullable`1 interval) in D:\Users\Desktop\入职培训\停车机器人\MyParking\Shared\Chassis\MultiWheelChassisAdapter.cs:line 351
+   at MultiWheelC.Control.Execution.GcpCommandExecutor.Execute(GcpMotionCommand command, Double deltaTimeSeconds) in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Control\Execution\GcpCommandExecutor.cs:line 125
+   at MultiWheelC.Control.Execution.ParkingGeometricController.ExecuteCycle(Double deltaTimeSeconds) in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Control\Execution\ParkingGeometricController.cs:line 517
+
+
+
+: * (Exception):DriveTask failed, msg=原地自转完成后Detour位置在限定时间内未恢复。Detour疑似坐标跳变未能在0.60s内确认，车辆状态已置为不可用。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.InPlaceRotateTestBase.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 145
+   at MultiWheelC.TestRotateAngle.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 240
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):原地自转完成后Detour位置在限定时间内未恢复。Detour疑似坐标跳变未能在0.60s内确认，车辆状态已置为不可用。, stack:
+   at MultiWheelC.MultiWheelRotateInPlace.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 282
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+: * (Exception):DriveTask failed, msg=原地自转完成后Detour位置在限定时间内未恢复。Detour位姿偏移在短时确认后仍未恢复，且不能安全地自动连续化：平移0.163m，航向2.26°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.InPlaceRotateTestBase.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 145
+   at MultiWheelC.TestRotateAngle.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 240
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):原地自转完成后Detour位置在限定时间内未恢复。Detour位姿偏移在短时确认后仍未恢复，且不能安全地自动连续化：平移0.163m，航向2.26°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at MultiWheelC.MultiWheelRotateInPlace.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 282
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+
+

data_process/跳变排查/第四次/TrackingExperiments/jilu.txt

@@ -0,0 +1,94 @@
+: * (Exception):DriveTask failed, msg=无法从Detour状态源读取有效车辆航向。Detour位姿偏移在短时确认后仍未恢复，且不能安全地自动连续化：平移0.045m，航向4.53°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.InPlaceRotateTestBase.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 145
+   at MultiWheelC.TestRotateAngle.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 240
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):无法从Detour状态源读取有效车辆航向。Detour位姿偏移在短时确认后仍未恢复，且不能安全地自动连续化：平移0.045m，航向4.53°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at MultiWheelC.MultiWheelRotateInPlace.ReadCurrentAngleDegrees(IVehicleStateProvider stateProvider) in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 367
+   at MultiWheelC.MultiWheelRotateInPlace.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 184
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+: * (Exception):DriveTask failed, msg=无法从Detour状态源读取有效车辆航向。Detour位姿偏移在短时确认后仍未恢复，且不能安全地自动连续化：平移0.102m，航向0.39°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.InPlaceRotateTestBase.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 145
+   at MultiWheelC.TestRotateAngle.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 240
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):无法从Detour状态源读取有效车辆航向。Detour位姿偏移在短时确认后仍未恢复，且不能安全地自动连续化：平移0.102m，航向0.39°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at MultiWheelC.MultiWheelRotateInPlace.ReadCurrentAngleDegrees(IVehicleStateProvider stateProvider) in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 367
+   at MultiWheelC.MultiWheelRotateInPlace.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 184
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+: * (Exception):DriveTask failed, msg=无法从Detour状态源读取有效车辆航向。Detour位姿偏移在短时确认后仍未恢复，且不能安全地自动连续化：平移0.162m，航向0.78°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.InPlaceRotateTestBase.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 145
+   at MultiWheelC.TestRotateAngle.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 240
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):无法从Detour状态源读取有效车辆航向。Detour位姿偏移在短时确认后仍未恢复，且不能安全地自动连续化：平移0.162m，航向0.78°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at MultiWheelC.MultiWheelRotateInPlace.ReadCurrentAngleDegrees(IVehicleStateProvider stateProvider) in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 367
+   at MultiWheelC.MultiWheelRotateInPlace.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 184
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+: * (Exception):DriveTask failed, msg=无法从Detour状态源读取有效车辆航向。Detour位姿偏移在短时确认后仍未恢复，且不能安全地自动连续化：平移0.070m，航向1.89°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.InPlaceRotateTestBase.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 145
+   at MultiWheelC.TestRotateAngle.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 240
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):无法从Detour状态源读取有效车辆航向。Detour位姿偏移在短时确认后仍未恢复，且不能安全地自动连续化：平移0.070m，航向1.89°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at MultiWheelC.MultiWheelRotateInPlace.ReadCurrentAngleDegrees(IVehicleStateProvider stateProvider) in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 367
+   at MultiWheelC.MultiWheelRotateInPlace.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 184
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+: * (Exception):DriveTask failed, msg=无法从Detour状态源读取有效车辆航向。Detour位姿偏移在短时确认后仍未恢复，且不能安全地自动连续化：平移0.102m，航向1.72°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.InPlaceRotateTestBase.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 145
+   at MultiWheelC.TestRotateAngle.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 240
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):无法从Detour状态源读取有效车辆航向。Detour位姿偏移在短时确认后仍未恢复，且不能安全地自动连续化：平移0.102m，航向1.72°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at MultiWheelC.MultiWheelRotateInPlace.ReadCurrentAngleDegrees(IVehicleStateProvider stateProvider) in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 367
+   at MultiWheelC.MultiWheelRotateInPlace.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 184
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+: * (Exception):DriveTask failed, msg=无法从Detour状态源读取有效车辆航向。Detour位姿偏移在短时确认后仍未恢复，且不能安全地自动连续化：平移0.112m，航向0.79°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.InPlaceRotateTestBase.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 145
+   at MultiWheelC.TestRotateAngle.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 240
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):无法从Detour状态源读取有效车辆航向。Detour位姿偏移在短时确认后仍未恢复，且不能安全地自动连续化：平移0.112m，航向0.79°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at MultiWheelC.MultiWheelRotateInPlace.ReadCurrentAngleDegrees(IVehicleStateProvider stateProvider) in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 367
+   at MultiWheelC.MultiWheelRotateInPlace.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 184
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+: * (Exception):DriveTask failed, msg=无法从Detour状态源读取有效车辆航向。Detour位姿偏移在短时确认后仍未恢复，且不能安全地自动连续化：平移0.048m，航向0.29°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.InPlaceRotateTestBase.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 145
+   at MultiWheelC.TestRotateAngle.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 240
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):无法从Detour状态源读取有效车辆航向。Detour位姿偏移在短时确认后仍未恢复，且不能安全地自动连续化：平移0.048m，航向0.29°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at MultiWheelC.MultiWheelRotateInPlace.ReadCurrentAngleDegrees(IVehicleStateProvider stateProvider) in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 367
+   at MultiWheelC.MultiWheelRotateInPlace.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 184
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+: * (Exception):DriveTask failed, msg=无法从Detour状态源读取有效车辆航向。Detour位姿偏移在短时确认后仍未恢复，且不能安全地自动连续化：平移0.102m，航向2.53°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.InPlaceRotateTestBase.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 145
+   at MultiWheelC.TestRotateAngle.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\RotationTests.cs:line 240
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):无法从Detour状态源读取有效车辆航向。Detour位姿偏移在短时确认后仍未恢复，且不能安全地自动连续化：平移0.102m，航向2.53°。车辆状态已置为不可用，应停车并重新定位或规划。, stack:
+   at MultiWheelC.MultiWheelRotateInPlace.ReadCurrentAngleDegrees(IVehicleStateProvider stateProvider) in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 367
+   at MultiWheelC.MultiWheelRotateInPlace.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\RotateInPlace.cs:line 184
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+

data_process/跳变排查/第四次/列表.txt

@@ -0,0 +1,22 @@
+getCartLocation()返回什么位姿是可能因重定位而跳变的地图位姿？
+还是保证连续的里程计位姿？
+回环或重定位后，x/y/th是否允许突然变化？
+
+这是最重要的问题。
+tick代表什么时间是激光采样时间、定位计算完成时间，还是发布时间？
+是否为 .NET DateTime.Ticks？
+getCartLocation()返回缓存数据时，tick是否保持不变？
+
+这决定MyParking如何进行时间对齐。
+l_step的含义
+只需要问：
+正常、定位退化、定位丢失、重定位中分别是什么范围？
+判断定位恢复的推荐条件是什么？
+自转时 l_step升高是否正常？
+是否存在额外可调用的接口
+重点问有没有：
+连续里程计位姿或速度。
+定位是否有效。
+是否正在重定位。
+定位置信度或匹配分数。
+不需要让对方新增功能，只问现有接口有没有。

detour-information-checklist.md

@@ -0,0 +1,388 @@
+# Detour 定位接口信息确认（源码答复）
+
+用途：答复控制程序（`MultiWheelC` / `StateEstimation`）使用定位结果所必需的接口语义。
+依据：本仓库 `DetourCore` 源码（`Location.cs`、`TightCoupler.cs`、`Frame.cs`、`LidarOdometry.cs`、`LidarMap.cs`、`WebAPI.cs`、`G.cs`）。
+仓库内**没有**名为 `getCartLocation()` 的函数；控制侧该调用对应 Detour 的两类对外位姿出口。下文按字段对齐后统一说明。
+
+---
+
+## 对外位姿出口（先对齐接口）
+
+控制程序看到的 `x / y / theta / tick / l_step`，来自下面之一（或对它们的封装）。字段名在源码里是 `th`，不是 `theta`。
+
+| 出口 | 入口 | 字段 | `tick` 的真实来源 |
+|---|---|---|---|
+| HTTP `GET /getPos` | `CartLocation.FormatPosition()` | JSON：`x, y, th, l_step, tick`，异常时多 `error` | `tick = CartLocation.st_time`（观测时刻） |
+| 共享对象 `DetourPos{shareObjectTag}` | `TightCoupler.CommitLocation` 每次提交后 `Post` | 二进制：`x, y, th, tick, l_step, error` | `tick = DateTime.Now.Ticks`（提交/推送时刻） |
+
+默认 HTTP 端口：`Configuration.conf.guru.DetourPort`，默认 `4321`。
+`shareObjectTag` 必须与控制侧 Clumsy 的 soTag 一致。
+
+两种出口的 `x/y` 都已经过：
+
+```
+x_out = 内部x / guru.inputScale + guru.biasX
+y_out = 内部y / guru.inputScale + guru.biasY
+```
+
+默认 `inputScale = 1`、`biasX = biasY = 0`，因此默认单位就是内部单位（毫米）。
+`th` / `l_step` 不做缩放。
+
+---
+
+## 1. `getCartLocation()` 字段定义
+
+### 1.1 各字段含义
+
+| 字段 | 源码名 | 含义 |
+|---|---|---|
+| `x` | `CartLocation.x` | 车体原点在**当前地图坐标系**中的 X。已从传感器坐标系经安装外参反变换到车体中心（`TightCoupler.CommitLocation` 里 `ReverseTransform(传感器位姿, 组件安装位姿)`）。 |
+| `y` | `CartLocation.y` | 同上，Y。 |
+| `theta` | `CartLocation.th` | 车体航向。`0°` 朝向地图 `+X`，逆时针为正（内部用 `cos(th)` / `sin(th)` 画朝向）。 |
+| `tick` | 见上表 | **不是**统一的一种时间。HTTP 是观测时间 `st_time`；DObject 是提交瞬间的 `DateTime.Now.Ticks`。控制侧必须先确认自己走的是哪条通道。 |
+| `l_step` | `Frame.l_step` | **到已标注（labeled）关键帧的图上步数**，用来表达“离可信锚点有多远”，**不是**优化迭代次数，也不是匹配分数。注释原文：`steps to labeled keyframe`。 |
+
+### 1.2 单位、正方向、坐标系
+
+- **内部单位**：`x`、`y` 为毫米。UI 直接按 `mm` 显示。
+- **对外单位**：默认仍是毫米；只有改了 `guru.inputScale`（例如设为 `1000` 输出米）才会变。
+- **角度单位**：度。
+- **坐标系**：当前加载的 SLAM 地图坐标系。原点由建图/标注关键帧决定，不是 GNSS 或车体启动点。
+- **车体姿态**：输出的是**车体原点**，不是雷达原点。雷达/相机安装位姿在 `layout.components` 的 `x,y,th` 里。
+- **右手平面**：`+X` 为航向 0，`+Y` 为航向 +90°。
+
+### 1.3 `theta` 取值范围
+
+**不是严格的 `[-180°, 180°]`。**
+
+`LessMath.normalizeTh` 只在绝对值 ≥ 360 时按 360° 回绕，`(-360, 360)` 内原样返回。因此对外可能看到 `200`、`-270` 这类值。控制程序若假设 `[-180, 180]` 或 `[0, 360)`，应自己归一化。内部比较角度用 `LessMath.thDiff`，会按最短弧处理。
+
+### 1.4 一次调用的字段是否同一帧
+
+**是。** 一次读取对应同一个 `CartLocation` 对象上的 `x/y/th/l_step/st_time`。
+`getPos` 先用当前 `latest` 判断 Timeout/Unstable，再序列化；若这两步之间恰好发生新提交，状态字和位姿可能差一帧，但单次 JSON 里的数值字段仍来自同一个对象。
+
+---
+
+## 2. `tick` 的准确含义（优先）
+
+### 2.1 它是什么时刻
+
+分通道：
+
+**HTTP `/getPos` 的 `tick` = `st_time`**
+
+- `st_time` 在 `Frame` 上的注释是：传感器数据**到达 Detour 时**由 `G.watch` 打的时间，不是激光头硬件曝光时刻，也不是 HTTP 返回时刻。
+- 激光采集线程会按扫描周期做轻微平滑，并加上 `time_bias_ms`。
+- 里程计把 `frame.st_time` 原样交给 `TightCoupler.CommitLocation`，再写入 `CartLocation.st_time`。
+- 因此：`tick` ≈ **该位姿所对应的那帧观测到达时刻**。SLAM 计算发生在这之后，接口返回更晚。
+
+**DObject `DetourPos` 的 `tick` = `DateTime.Now.Ticks`**
+
+- 这是 **TightCoupler 提交并推送的本地时刻**，100 纳秒为单位，从公元 1 年 1 月 1 日起算（.NET `DateTime.Ticks`）。
+- 它**不是**传感器采集时刻，也**不是** Unix epoch。
+- 受本机本地时钟、时区、校时影响，必要时可能回跳。
+
+### 2.2 单位、频率、单调、回绕
+
+| 项目 | HTTP `tick`（`st_time`） | DObject `tick`（`DateTime.Now.Ticks`） |
+|---|---|---|
+| 单位 | 毫秒 | 100 ns（1 ms = 10000） |
+| 时钟 | 进程启动时锚定一次 UTC，之后用 `Stopwatch` 单调累加：`ElapsedTicks*1000/Frequency + stMillis` | 本机本地 `DateTime.Now` |
+| 更新频率 | 随传感器/融合提交，通常接近雷达帧率（常见 10–20 Hz，取决于设备 `timeBudget`） | 每次 `CommitLocation` 成功推一次 |
+| 单调 | 进程内单调递增（不受事后改系统时间影响） | 一般递增；改系统时间或 DST 可能回跳 |
+| 回绕 | `long` 毫秒，实际不会回绕 | `long` ticks，实际不会回绕 |
+| 进程重启 | 重新锚定当前 UTC，**不会从 0 开始**，但与重启前不保证连续 | 继续跟本机时钟，与进程无关 |
+
+`G.watch.TimeStampMillis` **不是** Unix 毫秒（1970），而是从公元 1 年起算的 UTC 毫秒，再加上启动后的单调流逝。
+
+### 2.3 多车能否用 `tick` 对齐
+
+**不能当作已同步的多车时钟。**
+
+- 各车各自启动 `G.watch`，没有 PTP/NTP 协议，也没有跨车时间服务。
+- HTTP `tick`：若各车系统 UTC 在启动时大致同步，数值会接近“同一套绝对时间”，但仍有启动锚定误差和各车处理延迟，**不能直接当多车位姿对齐的主时钟**。
+- DObject `tick`：本地时间 ticks，跨时区会直接错开，更不适合多车对齐。
+- 进程重启、暂停（`G.paused`）、提交失败都会造成时间空洞。
+
+控制侧建议：单车内部用 `tick` 做延迟估计和短时预测；多车对齐应使用外部统一时钟，或只把 Detour `tick` 当相对时间。
+
+---
+
+## 3. 定位延迟与更新方式
+
+### 3.1 输出位姿比真实运动滞后多少
+
+源码**没有**标定“官方延迟 xx ms”。能确定的是延迟结构：
+
+```
+真实运动 → 雷达扫描/到达(st_time) → 里程计配准 → TightCoupler 融合 → 写入 CartLocation.latest → 接口读到缓存
+```
+
+经验上由源码阈值约束：
+
+- 激光一圈通常几十毫秒（`Lidar2DStat.timeBudget`）。
+- TightCoupler 常用窗口 `TCtimeWndSz = 150 ms`，上限 `TCtimeWndLimit = 700 ms`。
+- `LidarOdometry` 把 `reg_ms > 200` 记为 `bad perf`。
+- `/getPos` 若 `now - st_time > 500 ms` 打 `Timeout`。
+
+因此正常运行时，**从观测到可读位姿大约是 1 帧雷达周期 + 配准/融合，常见几十到两百毫秒；超过 500 ms 会被 HTTP 接口判超时。** 这是实现上的门槛，不是出厂标定值。
+
+### 3.2 `getCartLocation()` 是否返回最近一次缓存
+
+**是。**
+
+- `CartLocation.latest` 是全局缓存。
+- 只有 `TightCoupler.CommitLocation` 成功后才会替换（取 `history` 中 `st_time` 最新的一帧）。
+- `/getPos` 只读缓存，不触发新的 SLAM 计算。
+- 配准失败、`CommitLocation` 返回 `null`（`bad variance` / `bad trace`）、或 `G.paused` 时，缓存停在上一帧。
+
+### 3.3 原地自转时频率/延迟是否明显变化
+
+**轮询频率不变，有效更新和延迟会变差。**
+
+- 接口仍按调用方频率读同一缓存。
+- 原地旋转时二维扫描重叠变差，帧间分、相位锁分容易掉，`allowCommit` 更常失败，缓存更容易停住。
+- 配准变慢时单帧 `reg_ms` 上升；失败后 `l_step` 被加大（见第 4 节），看起来像“转的时候定位变钝”。
+- 源码没有“自转专用降频”。
+
+控制侧：自转短时预测应假设**有效位姿更新可能变稀、变老**，不要假设 `tick` 仍按雷达周期前进。
+
+---
+
+## 4. `l_step` 的含义（优先）
+
+### 4.1 它是什么
+
+**定位质量的图距离，不是优化迭代次数，也不是匹配状态枚举。**
+
+定义：当前位姿/关键帧沿约束图走到**已标注关键帧**还要几步。
+
+- `0`：本身就是标注帧（`labeledXY` / `labeledTh`），或刚被标成锚点。
+- `1`：刚和地图/标注帧配准成功（`LidarMap` 成功后会把 `compared.l_step = 1`）。
+- 正常递推：新关键帧 `l_step = 旧关键帧.l_step + lstepInc`。
+- TightCoupler 提交时：有参考关键帧则 `reference.l_step + 1`；没有则 `latest.l_step + 3`。
+- `9999`：未定位、手动设位、或与标注图断开。这是 `Frame` / 初始 `CartLocation` 的默认值。
+
+**越小越好、越可信。**
+`AIImplementationNote.md` 里“越小越不确定”与源码相反，不要采用。
+
+内部用途包括：
+
+- TightCoupler 边权：`l_step > 10 → 1`，`> 5 → 2`，`> 2 → 3`，否则 `4`；标注帧权 1000。
+- 图优化用 `1/(l_step+0.01)` 当权。
+- `l_step` 大时 `LidarMap` 更容易走全局配准（`forceGRegStep`、`GregThresK * l_step`）。
+- `l_step > 1000` 且孤立的关键帧可被删掉。
+
+源码 TODO 也写过：希望将来“去掉 `l_step`，改用误差圆”。当前对外接口仍是这个整数。
+
+### 4.2 为什么原地自转会从 2～4 升到几十、上百
+
+自转本身不会改公式，但会让 **`lstepInc` 连续被惩罚**，再在切关键帧时一次性加到 `l_step` 上。
+
+`LidarOdometry` 里常见加项：
+
+| 条件 | `lstepInc` 增量 |
+|---|---|
+| 帧间隔过大 | `+3`～`+20` |
+| 时间落后 > 1 s | 可到 `+1000` 量级并重启局部图 |
+| 有效点太少 | `+3` |
+| 掩膜后点过少 | `+10` |
+| 帧间序贯配准失败 | `+5` |
+| 局部里程计分过低 | `+15` |
+| 历史分偏低 | `+1`～`+15` |
+| 局部图重启 / 相位锁差 | `+2` |
+| 上一帧能提交、这一帧不能 | `+100` |
+| 离开 hard 区域 | 默认再 `+20` |
+
+原地自转时典型连锁是：旋转导致重叠变差 → 分数掉 → `lstepInc` 累加 → 因“新点变多 / 超时 / restart”切关键帧 → `新 l_step = 旧值 + lstepInc`。
+若此时地图匹配也失败，TightCoupler 按 `latest.l_step + 3` 继续推高。
+所以 2～4 很快可以变成几十，失败恢复前看到上百是符合实现的。
+
+匹配一旦重新成功，关键帧会被改回 `l_step = 1`，输出也会掉下来。
+
+### 4.3 有没有官方阈值
+
+**没有写给控制程序的官方阈值表。** 下面是源码内部实际在用的分档，可作控制侧初值，现场仍要按地图和雷达标定。
+
+| `l_step` | 源码含义 | 给控制程序的建议 |
+|---|---|---|
+| `0` | 标注锚点 | 最可信 |
+| `1`～`2` | 刚贴上地图 / 离锚点很近 | **正常，接受** |
+| `3`～`5` | TightCoupler 已降权 | 可用，开始警惕跳变 |
+| `6`～`10` | 权更低；地图更倾向全局配准 | **退化，降低信任 / 收紧预测** |
+| `11`～`98` | 最低权；曾从不稳定区回到 `2` 时会打工作区快照 | **明显退化，不宜做精细控制** |
+| `≥ 99` | TightCoupler 允许关键帧被推得更狠 | 按不可靠处理 |
+| `≥ 1000` | 孤立帧可删 | 基本断开 |
+| `9999` | 未定位 / 手动设位 / 断开 | **不可用，应安全停止或等待重定位** |
+
+Detour 自己判断“车是否静止可做全局重定位”时用：`l_step > 2` 且 TightCoupler 速度估计很小。这是内部策略，不是对外状态机。
+
+---
+
+## 5. 坐标跳变与重定位行为（优先）
+
+### 5.1 重定位、回环、失败恢复后，`x/y/theta` 会不会永久跳变
+
+**会。跳变是设计行为，不是毛刺。**
+
+会改当前输出的情况：
+
+1. **地图配准成功**：`LidarMap` 把当前关键帧改到匹配位姿，并 `l_step = 1`，再交给 TightCoupler。下一帧 `CartLocation` 跟着新参考走，表现为一次台阶。
+2. **全局重定位**（`/relocalize` 或 UI Relocalize）：定位器进入 `relocalizing`，按全图关键帧搜索（`source = 9`）。第一次更好的匹配会 `TightCoupler.Reset`，位姿被拉到地图上，通常是大幅度永久跳变。
+3. **回环 + `GraphOptimizer`**：关键帧 `x/y/th` 被就地改写。当前参考帧若被挪动，后续融合位姿跟着变。优化有动量平滑，但仍可能出现肉眼可见的台阶。
+4. **手动 `/setLocation`**：直接改 `CartLocation.latest` 并重置融合窗口。
+5. **匹配长期失败后突然恢复**：从里程计漂过的位置一下子贴回地图，跳变幅度等于累计漂移。
+
+没有“只在内部跳、对外插值抹平”的保证。控制程序必须自己做跳变连续化或拒绝。
+
+### 5.2 跳变后还在原来的地图坐标系吗
+
+**同一张已加载地图内：是。**
+跳的是车在这张图里的估计，不是换了一套轴。
+
+会换坐标系的只有：换图（`/loadMap`）、`Remapper`（GNSS/外参映射）、或手动把车标到另一个锚点。这些不是普通重定位。
+
+### 5.3 有没有“正在重定位 / 定位丢失 / 地图坐标调整”标志
+
+**定位结果包里没有这些标志。**
+
+内部有、但**不随 `getPos` / `DetourPos` 下发**：
+
+| 内部量 | 作用 | 是否对外 |
+|---|---|---|
+| `Locator.relocalizing` / `relocalized` | 地图层正在/已经全局搜 | 否 |
+| `G.IsSettingPosition` | 正在手动设位 | 否（`/getStat` 的 `globalStat` 里能看到） |
+| `G.paused` | 定位暂停 | 否（同上，或调 `/pause` `/resume`） |
+| `CartLocation.unstable` | 本帧掩膜过狠、场景不稳定 | **仅 HTTP**：`error = "Unstable"`。DObject 当前推送的 `error` 被写成空串 |
+| `l_step` 升到很大 / `9999` | 实际的“丢了” | 是，但这是间接指标 |
+| 图优化改关键帧 | “地图坐标调整” | 无单独标志 |
+
+HTTP `/getPos` 仅有的显式错误：
+
+- `"Timeout"`：`st_time` 已超过 500 ms
+- `"Unstable"`：`latest.unstable == true`
+
+没有 `"Relocalizing"`、`"Lost"`、`"MapAdjusted"`。
+`/getStat` 可拉到各模块 `StatusMember`（雷达间隔、里程计状态、TC 状态等），但不是每帧位姿附属字段，也不适合当硬实时互锁。
+
+控制侧应自己构造状态：
+
+- **疑似丢失**：`error` 为 Timeout/Unstable，或 `l_step ≥ 99`，或 `tick` 长期不涨。
+- **疑似重定位/回环跳变**：相邻两帧 `x/y/th` 突变，同时 `l_step` 突然掉回 1～2。
+- **地图在拧**：跳变较缓、持续多帧，且 `l_step` 并不爆掉。
+
+---
+
+## 6. 可用的定位质量接口（优先）
+
+### 6.1 除 `l_step` 外还能拿到什么
+
+**位姿包几乎只有 `l_step` + HTTP 的 `error`。**
+
+| 信息 | 有没有 | 说明 |
+|---|---|---|
+| 匹配得分 | 对控制程序：无 | 在 `LidarOdometry` / `LidarMap` 内部（`score`、`phaselocker_score`），不下发 |
+| 协方差 | 对控制程序：无 | `Frame.errXX/errXY/errYY` 已预留，**未填进 `/getPos` 或 `DetourLocation`** |
+| 置信度 | 间接 | 就是 `l_step`；源码 TODO 想换成误差圆，尚未做 |
+| 定位状态 | 很弱 | HTTP：`Timeout` / `Unstable`；DObject：`error` 目前恒为空 |
+| 错误码 | 无枚举 | 只有上述字符串 |
+| 系统状态 | 有，但是慢接口 | `GET /getStat` → 布局/里程计/定位器/TC/GO/`G.paused`/`G.IsSettingPosition` |
+
+因此控制程序**不能**指望每帧拿到匹配分或协方差。
+
+### 6.2 Detour 实际用什么条件接受一帧（可当作官方内部规则）
+
+源码里“接受并提交”的条件是分层的，没有单独的对外规范文档：
+
+1. **里程计层**（`LidarOdometry.updateLocation`）
+   - 分数过低会把 `strength` 压下去，甚至不把 `reference` 挂上。
+   - 掩膜过狠 → `unstable = true`。
+   - 点太少、序贯/局部配准失败 → 不切健康关键帧，并加大 `lstepInc`。
+2. **融合层**（`TightCoupler.CommitLocation`）
+   - 传感器处于 `bad variance` / `bad trace` → 直接丢弃，返回 `null`。
+   - 丢弃后该源会被禁一段时间（约 1～2 s）。
+3. **地图层**（`LidarMap`）
+   - `result.score < ScoreThres` 丢弃。
+   - 相对已有位姿的 `xy/th` 偏差超过按 `l_step` 放大的门限则丢弃（防止乱跳）。
+   - 落到无效区域丢弃。
+4. **HTTP 出口**
+   - 缓存超过 500 ms → `Timeout`。
+   - `unstable` → `Unstable`。
+
+### 6.3 建议控制程序如何接受/拒绝一帧
+
+源码没有写给 `MultiWheelC` 的官方判据。按上面的内部规则，建议：
+
+**接受（正常闭环）**
+
+- HTTP：无 `error`（DObject：至少 `tick` 在前进）。
+- `l_step ≤ 5`。
+- `tick` 新鲜（HTTP：`now - tick < 300 ms` 较稳妥；DObject：换算成 ms 后同样看提交间隔）。
+- 相对上一接受帧：位移/转角不超过本底盘短周期能达到的上限。
+
+**降级（短时预测、降低增益、禁止精细对位）**
+
+- `6 ≤ l_step ≤ 10`，或偶发 Timeout 后立刻恢复。
+- 原地自转期间 `l_step` 爬升但 `tick` 仍在更新。
+
+**拒绝并安全停止 / 等待**
+
+- `error == "Timeout"` 持续，或 `error == "Unstable"`。
+- `l_step ≥ 99`（含 `9999`）。
+- 单帧出现与运动学不符的永久台阶（尤其伴随 `l_step` 从很大突然回到 1）：先当重定位跳变，做连续化或刹停，不要直接当编码器。
+
+**不要做的事**
+
+- 不要用 `l_step` 当 ICP 迭代次数或“正在计算中”。
+- 不要假设 `theta ∈ [-180, 180]`。
+- 不要用 `tick` 做多车时间同步主时钟。
+- 不要假设 DObject 的 `error` 会带 Timeout/Unstable（当前实现是空的）。
+
+---
+
+## 对 `StateEstimation` 的直接含义
+
+清单里最优先的 2 / 4 / 5 / 6，对应控制侧应这样定：
+
+1. **时间同步**
+   - 先确认 `getCartLocation` 走 HTTP 还是 `DetourPos`。两条通道的 `tick` **单位和语义都不同**。
+   - 单车：用 `tick` 估延迟、做自转短时预测。
+   - 多车：另选同步时钟。
+
+2. **自转短时预测**
+   - 自转时有效更新可能变稀，`l_step` 会从个位数爬到几十上百。
+   - 这是质量变差，不是接口卡死。预测窗口应随 `tick` 变老、`l_step` 变大而缩短。
+
+3. **跳变连续化**
+   - 重定位、回环、失败恢复都会造成**同地图下的永久台阶**。
+   - 没有“正在改图”标志；用位姿差分 + `l_step` 回落来识别。
+
+4. **安全停止**
+   - 硬条件：`Timeout` / `Unstable` / `l_step ≥ 99` / `tick` 停更。
+   - `l_step` 建议按 `≤5` 正常、`6–10` 退化、`>10` 不可用于精细控制。
+
+---
+
+## 源码锚点
+
+| 主题 | 位置 |
+|---|---|
+| 对外 JSON / Timeout / Unstable | `DetourCore/Location.cs`：`ConstructRet`、`FormatPosition` |
+| HTTP 路由 | `DetourCore/WebAPI.cs`：`/getPos`、`/setLocation`、`/relocalize`、`/getStat` |
+| DObject 推送与 `tick=DateTime.Now.Ticks` | `DetourCore/Algorithms/TightCoupler.cs`：`DetourLocation`、`CommitLocation` |
+| `l_step` 定义 | `DetourCore/Types/Frame.cs` |
+| 车体中心变换、提交缓存 | `TightCoupler.CommitLocation` |
+| 时钟 | `DetourCore/G.cs`：`DetourWatch.TimeStampMillis` |
+| 角度回绕 | `DetourCore/LessMath.cs`：`normalizeTh` |
+| 自转时 `l_step` 被加大 | `DetourCore/Algorithms/LidarOdometry.cs`（`lstepInc`） |
+| 匹配成功后的位姿跳变 | `DetourCore/LocatorTypes/LidarMap.cs`（loop / relocalize） |
+| 回环拧图 | `DetourCore/Algorithms/GraphOptimizer.cs` |
+| 全局重定位入口 | `DetourCore/DetourLib.cs`：`Relocalize()` |
+| 单位缩放 | `DetourCore/Configuration.cs`：`GuruOptions.inputScale / biasX / biasY` |
+
+---
+
+## 修订记录
+
+- 2026-08-24：按当前 Detour 源码逐条答复原《Detour 信息确认清单》。

docs/INDEX.md

@@ -0,0 +1,15 @@
+# MyParking 知识库导航
+
+默认只读本页，再按任务选择1～2份文档；不要一次加载全部知识库。
+
+| 文档 | 内容 | 适合任务 |
+| --- | --- | --- |
+| `overview.md` | 项目背景、车辆和运行目标 | 初次了解项目、业务范围判断 |
+| `architecture.md` | 模块职责、入口、调用链和数据流 | 定位代码、评估结构调整 |
+| `interfaces.md` | 关键接口、数据模型、坐标、单位和通信边界 | 修改控制、轨迹、状态、底盘或MCU接口 |
+| `decisions.md` | 已实施方案、未实施决定和待评估方向 | 方案选择、避免推翻既有约束 |
+| `problems.md` | 已解决、待解决和待验证问题 | 排障、实验设计、回归检查 |
+| `progress.md` | 当前能力、进行中工作、阻塞和下一步 | 恢复近期开发上下文 |
+| `detour-information-checklist.md` | 需要向 Detour 负责人确认的最小信息清单 | 对接定位接口、确认时间戳和定位质量语义 |
+
+读取建议：先用类名或路径定位代码；只有涉及长期背景时才读对应文档。涉及旧版或MDCS时，再按任务读取工作区参考目录，并明确来源。

docs/architecture.md

@@ -0,0 +1,174 @@
+# 系统架构
+
+## 解决方案与依赖边界
+
+`ParkingRobot.sln` 包含三个项目：
+
+| 项目 | 目标框架 | 职责 |
+| --- | --- | --- |
+| `CommonUsage-MultiVehicleSync/commonusage/CommonUsage.csproj` | `netstandard2.0` | 通用底盘、轮子模型、`SendMotion`/`SendXYThSpeed`和四轮几何解算 |
+| `MedullaAdapter/MedullaAdapter.csproj` | `net8.0` | Medulla M层插件、MCU/CAN/串口/IO、遥控、报警和硬件反馈 |
+| `MultiWheelC/MultiWheelC.csproj` | `netstandard2.0` | Clumsy C层插件、动作、控制器、轨迹、状态估计、实验记录 |
+
+`Shared/` 没有独立项目：`MultiWheelC` 链接全部 `Shared/**/*.cs`，`MedullaAdapter` 只链接其需要的模型、数学、校验和底盘适配文件。`CommonUsage` 是独立底盘库，不反向依赖 `Shared`、M层或C层。来源：三个 `.csproj`。
+
+```text
+Clumsy宿主
+  └─ MultiWheelC
+      ├─ Trajectory / StateEstimation / Control / Movements
+      ├─ Shared
+      └─ CommonUsage.dll
+
+Medulla宿主
+  └─ MedullaAdapter
+      ├─ Shared（链接的必要文件）
+      ├─ CommonUsage.dll
+      └─ mcu_serial_bridge.dll → MCU → CAN / Serial / IO
+```
+
+## 目录职责
+
+| 路径 | 当前职责 |
+| --- | --- |
+| `MultiWheelC/Configuration/` | 停车控制、状态估计、原地自转和完成条件的车辆级运行参数 |
+| `MultiWheelC/Trajectory/` | 弧长参数化轨迹、插值、投影和进度窗口；`LegacyTrackAdapter.cs` 目前仅占位 |
+| `MultiWheelC/StateEstimation/` | Detour位姿校验/差分速度、轮组反馈速度组合与低通滤波 |
+| `MultiWheelC/Control/Abstractions/` | 横向、纵向控制器接口与周期输入/输出模型 |
+| `MultiWheelC/Control/Lateral/` | 当前默认 `StanleyLateralController` |
+| `MultiWheelC/Control/Longitudinal/` | 当前默认 `PidLongitudinalController` |
+| `MultiWheelC/Control/Allocation/` | 横纵结果组合、GCP限幅及GCP与刚体速度的转换 |
+| `MultiWheelC/Control/Execution/` | `PathTrackingCore` 共享纯控制周期、单车命令执行、终点策略和耗时诊断 |
+| `MultiWheelC/Movements/` | 舵轮准备、轨迹跟踪、原地自转和组合动作计划 |
+| `MultiWheelC/Experiments/` | Clumsy宿主人工测试、测试轨迹工厂和CSV记录 |
+| `MultiWheelC/Old/` | 保留的旧实现；不能仅因仍参与编译就视为新版流程依赖 |
+| `Shared/` | M/C共享的SI数据模型、坐标变换、数值校验和底盘适配 |
+| `MedullaAdapter/` | 车型定义、LadderLogic、MCU桥、CAN/串口、遥控、诊断 |
+| `CommonUsage-MultiVehicleSync/commonusage/Chassis/` | 实际轮子模型、GCP/ICR求解、舵角/轮速分配和机械约束 |
+| `data_process/` | 离线实验数据处理；日常代码任务不扫描其中的实验日志 |
+| `参考文档/` | 参数样例和历史设计资料；不等同于运行时配置 |
+
+## 入口与宿主生命周期
+
+### C层
+
+- `PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefinition>` 是Clumsy车型定义和M/C IO边界。
+- `PilotDefinition.Conf` 是动作读取运行配置的统一入口；`PilotConfig` 字段提供元数据和默认值。
+- `[MovementTest]` 类型由宿主发现并执行，例如 `NewControllerStraight4mTest`、`NewControllerReverseStraight4mTest`、`NewControllerCrab45Straight4mTest`、`TestRotateAngle` 和 `CompositeStopTurnGoTest`。
+- `MovementDefinition.Get()` 以 `IEnumerable<bool>` 形式协作执行：`true` 表示继续，结束动作时返回/产生 `false` 或退出枚举。具体宿主调度细节来自外部程序集，仓库内不可完全确认。
+
+### M层
+
+- `DiverCartDefinition : MultiWheelCartDefinition` 是Medulla车型定义。
+- `CommunicationInit()` 打开并配置MCU桥。
+- `[UseLadderLogic]` 注册 `AlarmRoutine`（50ms）、`MotorRoutine`（50ms）和 `MCURoutine`（20ms）；这些是声明的扫描间隔，不能直接等同于实测稳定周期。
+- `[UseManualController]` 注册 `Remote`。
+
+## 新版轨迹跟踪调用链
+
+```text
+MovementTest / MotionPlanExecutor
+  → TrajectoryTrackingMovement.Get()
+  → PrepareWheelsForward(DirectionRadians=β)
+  → MultiWheelChassisAdapter.ActivateMotionFrame(β)
+  → ParkingVehicleStateProviderFactory.Create()
+  → ParkingGeometricController.Start()/ExecuteCycle()
+      → IVehicleStateProvider.TryGetState()
+      → PathTrackingCore.Compute()
+          → TrajectoryProjector.Project()
+          → ILateralController.Compute()
+          → ILongitudinalController.ComputeSpeedMetersPerSecond()
+          → GcpCommandAllocator.Allocate()
+      → GcpCommandExecutor.Execute()
+      → GcpKinematics.ToBodyTwist()
+      → MultiWheelChassisAdapter.SendBodyTwist()
+      → MultiWheelChassis.SendMotion()
+      → 四个真实舵轮角度和速度
+```
+
+`PathTrackingCore` 只依赖受控刚体的 `Pose2D`、车体系 `Twist2D`、速度有效标志和控制周期，不依赖 `VehicleState`、`FleetState`、底盘或通信。`ParkingGeometricController` 负责把单车状态源和实体底盘接到该核心；横向、纵向控制算法仍通过接口组合注入，没有采用控制器继承层次。
+
+`TrajectoryTrackingMovement` 默认从 `PilotDefinition.Conf` 读取车辆级参数，同时保留少量动作级覆盖字段；横向控制器可通过 `LateralControllerFactory` 替换，纵向控制器当前固定创建为 `PidLongitudinalController`。
+
+## 车队组件与首版运行链
+
+```text
+夹紧且静止时的成员世界位姿快照
+  → FleetLayoutCapture.Capture()
+  → 初始FleetPoseInWorld + 不可变FleetLayout
+
+成员状态样本 FleetMemberStateSample[]
+  → FleetStateEstimator.Estimate()
+  → FleetState + FleetMemberLayoutError[]
+  → FleetCoordinator.ExecuteCycle()
+      → FleetController（虚拟中心轨迹闭环和固定β_fleet）
+      → 相对布局误差统一速度缩放
+      → FleetKinematics.Decompose()
+      → FleetMemberCommandCorrector
+      → FleetMemberCommand[]
+
+车队动作主要滚动方向β_fleet
+  → FleetPreparationCoordinator（换算每车β_i并等待全员Ready）
+  → FleetMemberAgent（本车停车准备、舵轮到位、激活、Execute/Stop）
+
+主车FleetCommand
+  → IFleetTransport
+  → 从车FleetMemberAgent
+
+从车FleetMemberReport
+  → IFleetTransport
+  → 主车状态缓存 / FleetSafetySupervisor
+
+FleetRuntime
+  ├─ 主车：准备屏障 → 状态/安全 → FleetCoordinator → 本车执行/远端分发
+  └─ 从车：接收命令 → FleetMemberAgent → 本地看门狗 → 状态上报
+```
+
+`FleetLayoutCapture` 只负责固定布局的几何计算：车队原点X/Y取成员车体中心的算术平均，车队Yaw取主车Yaw，再把各成员世界位姿反变换为 `VehicleLayout.PoseInFleet`。它不读取通信或Detour，也不负责静止/夹紧确认、时间对齐和布局激活。
+
+`Shared/Fleet/FleetProtocol.cs` 已定义最小任务命令和成员报告值类型，`IFleetTransport` 已把运行逻辑与无线串口或内存队列隔离；测试项目中的 `InMemoryFleetTransport` 只用于无通信硬件验证，不进入正式部署。`FleetSafetySupervisor` 在主车侧按成员报告接收时间、状态和故障锁存整队停车决定，`FleetMemberAgent.UpdateCommandWatchdog()` 在每辆车本地按命令有效期独立停车，避免通信中断时只依赖主车广播停止。
+
+`MultiWheelC/Fleet/FleetRuntime.cs` 已贯通首版固定布局滚动任务：同一类型按 `selfVehicleId`/`leaderVehicleId` 区分角色；主车执行β准备、全员Ready、统一激活、状态缓存、安全判定、周期协调、本车直接执行和远端分发；从车执行任务/序列校验、舵轮准备、激活、速度命令、本地看门狗和状态上报。完成、通信/状态/成员故障和取消都会由运行层转换为本车停车及可达成员停止命令，失联成员最终由本地命令有效期兜底。
+
+当前端到端链路只通过测试项目的内存传输验证，尚未接入C层正式 `Movement/Experiment`、实际成员状态采集、无线串口和真实跨机时间换算。实际部署仍是每车独立电脑；主车也是普通成员，其本车命令不经传输层回环。无线实现不能改变已经定义的任务号、序列号、命令有效期和本地失联停车语义。
+
+## 状态数据流
+
+```text
+DetourInterface.getCartLocation()
+  → DetourVehicleStateProvider
+      ├─ 位姿单位转换、重复帧处理、跳变/预测残差校验
+      └─ VelocityEstimator2D：Detour差分Vx/Vy/Omega
+
+MultiWheelChassis.GetCarSpeed(true)
+  → WheelFeedbackVehicleStateProvider
+      ├─ Vx、Vy分别使用同一时间常数低通滤波
+      ├─ 轮组估计有效后覆盖Detour的Vx、Vy和Omega
+      └─ 保留Detour位姿
+
+  → VehicleState（世界位姿、世界Twist、车体Twist）
+  → ParkingGeometricController
+```
+
+## 运动坐标系与四轮解算数据流
+
+`MultiWheelChassisAdapter` 对外只接收真实车体系 `Twist2D`。滚动动作开始前先固定运动方向β：
+
+1. `ActivateMotionFrame(β)` 调用 `MultiWheelChassis.SetOriginBias(0,0,-β)`。
+2. 车体线速度通过 `R(-β)` 表达到运动坐标系。
+3. 运动系中以 `±ControlPointRadius` 作为虚拟前后GCP，计算GCP方向。
+4. `MultiWheelChassis.SendMotion` 由两个GCP方向求瞬时旋转中心ICR。
+5. 每个真实轮子的 `PhysicalPosition` 被旋转成运动系 `Position`，再由ICR分别求切线角和速度半径比例。
+6. `sTh - sw.ZeroDirection` 把运动系方向转成真实舵轮机械命令；距离和速度大小不因坐标旋转改变。
+
+关键位置：`Shared/Chassis/MultiWheelChassisAdapter.cs`、`MultiWheelChassis.cs::SetOriginBias`、`SendMotion`、`CalculateAxes`。
+
+## 动作组织
+
+- `PrepareWheelsForward`：停车、下发任意固定方向β并等待稳定；名称保留“Forward”，但功能已支持非零方向。
+- `TrajectoryTrackingMovement`：准备运动系、创建控制器、周期执行、失败停车，可选完成后回正。
+- `MultiWheelRotateInPlace`：准备自转舵角、交接到XYTh解算、世界航向PID、完成后回正。
+- `MotionPlanExecutor`：开始运动前预检全部段，顺序执行 `TrackMotionPlanSegment` 和 `RotateInPlaceMotionPlanSegment`，共享状态源。
+
+## 构建与打包
+
+`build-and-package.ps1` 顺序构建 `CommonUsage`、M层和C层，将新 `CommonUsage.dll` 复制到 `ref/`，最后生成 `output/M` 和 `output/C`。两个部署包必须使用同一份 `CommonUsage.dll`。构建产物目录不作为知识来源，也不直接编辑。

docs/decisions.md

@@ -0,0 +1,158 @@
+# 技术决策
+
+只记录长期有效的方案状态；实验临时参数和失败尝试不在此保存。
+
+## 已经实施
+
+### 1. 分离 CommonUsage、Shared、M层和C层
+
+- `CommonUsage` 保持独立通用底盘库，不反向依赖项目控制层。
+- `Shared` 只保存M/C共享模型、数学、校验和底盘适配，不建立独立程序集。
+- `MedullaAdapter` 管硬件，`MultiWheelC` 管动作和控制。
+- 原因：避免硬件通信、控制算法和旧底盘库相互反向耦合。
+- 依据：三个 `.csproj`、`AGENTS.md`。
+
+### 2. Shared统一SI单位和真实车体系命令
+
+- Shared使用m、m/s、rad、rad/s，车体系X前Y左、逆时针为正。
+- 对外底盘命令统一为 `Twist2D`；旧版mm/deg接口只在 `MultiWheelChassisAdapter` 边界转换。
+- 原因：减少不同层之间的单位和符号歧义，为后续单车/车队刚体速度分配保留统一模型。
+
+### 3. 显式β运动坐标系，不根据速度分量猜测模式
+
+- 切换流程固定为停车、预对齐舵轮、`ActivateMotionFrame(β)`，运行阶段统一调用 `SendBodyTwist()`。
+- β=0表示车头方向，β=90°表示车体左侧作为虚拟前向；支持任意固定β。
+- 实际四轮始终使用真实布局参与解算，β只改变表达坐标系。
+- β不是改变目标刚体运动的额外自由度；它用于为同一 `BodyTwist` 选择本运动段更易满足舵角、最小转舵和非奇异条件的主要滚动方向。
+- β不增加车辆物理运动能力，也不改变同一刚体速度最终要求的轮子滚动轴线；当前主要价值是让真实车体系 `BodyTwist` 能稳定接入方向型旧 `SendMotion`，并使虚拟GCP角远离±90°表达奇异区。若以后改为 `BodyTwist` 直接到四轮的完整逆解，β可以退化为底盘内部的等效解选择，不应成为车队控制器的核心概念。
+- β可参与运动前的候选方向规划，但不能代替真实舵轮限位检查：±120°保护必须对完整四轮解算结果逐轮检查，并同时考虑“舵角±180°、轮速反向”的等效解。
+- 限制：运动中不切换β；纯自转要求回到真实车体系并采用专用舵轮准备。
+- 依据：`PrepareWheelsForward`、`TrajectoryTrackingMovement`、`MultiWheelChassisAdapter`。
+
+### 4. 保留前后GCP作为旧底盘几何边界
+
+- 控制层输出前后虚拟GCP方向；`MultiWheelChassis.SendMotion` 由两条法线求ICR，再计算每个真实轮子的切线方向和速度比例。
+- GCP是刚体控制点，不是物理轮轴中心；`ControlPointRadius` 必须在前馈、运动学和旧解算中保持一致。
+- 原因：复用已经存在的机械限位、等效舵角、速度斜坡和差速舵轮分配逻辑。
+
+### 5. 轨迹采用弧长参数化而非时间参数化
+
+- 轨迹保存点序、累计弧长、车体中心位姿、曲率和有符号参考速度。
+- 实际周期 `deltaTime` 只进入控制器、滤波器和命令变化率，不作为轨迹索引。
+- 原因：执行进度由车辆空间位置决定，调度周期波动不会直接跳过时间采样点。
+
+### 6. 正负参考速度表达前进和倒车
+
+- 轨迹Yaw始终是车头方向；正速度前进、负速度倒车。
+- Stanley曲率前馈、横向/航向修正和终点行驶方向显式考虑速度符号。
+- 同一测试曲线要求速度同号，不在滚动中直接切换前进/倒车。
+- 依据：`TrajectoryPoint`、`TestTrajectoryFactory`、`StanleyLateralController`。
+
+### 7. 默认状态采用Detour位姿与轮组平面速度组合
+
+- 位姿来自经过校验和任务坐标连续化处理的Detour。
+- 车体系 `Vx/Vy/Vw` 来自 `GetCarSpeed(true)`，使用同一时间常数低通滤波；轮组估计有效后，控制状态的 `Omega` 也使用轮组 `Vw`。首帧尚未建立采样时间基准时速度状态无效并对外置零。
+- 轮组 `Vw` 同时用于Detour跳变期间的短时位姿预测、运动合理性和动态航向创新阈值。
+- 原因：位置仍依赖SLAM，控制速度优先使用响应更直接且不受Detour自转退化影响的电机/舵角反馈。
+- 依据：`ParkingVehicleStateProviderFactory`、`WheelFeedbackVehicleStateProvider`。
+
+### 8. 横向控制可替换，纵向控制暂保持PID
+
+- `ILateralController` 是Stanley/LQR/MPC等的稳定扩展点；`TrajectoryTrackingMovement.LateralControllerFactory` 可注入实现。
+- 默认横向为Stanley，默认纵向为 `PidLongitudinalController`。
+- 当前没有单独的控制器工厂文件，避免为手动替换增加不必要结构。
+
+### 9. 低速和终点采用单向收敛
+
+- 起步区域使用小幅释放速度；终点前读取更低速度并进入单向低速逼近。
+- 到达或越过终点后只停车，不生成反向修正速度。
+- 完成同时检查剩余弧长、终点欧氏距离、车头航向和β方向实际速度。
+- 原因：停车机器人终点附近反复前后修正风险高，优先保证运动方向稳定和可预测。
+- 依据：`ParkingGeometricController.ResolveTerminalApproachSpeed()`、`HasReachedEnd()`。
+
+### 10. 对已观察的舵轮响应加入有限前馈与预瞄
+
+- M层差速转舵角速度前馈已接入，当前默认增益0.9、速度上限0.03m/s；实现位于 `MotorRoutine.CalculateDiffSteerRateFeedforward()`。
+- Stanley曲率前馈预瞄已接入，当前车辆默认时间0.15s、最大距离0.12m。
+- 两者均是可配置补偿，不替代底层PID、真实周期和机械响应验证。
+
+### 11. 车辆级参数集中到 PilotConfig，动作只做必要覆盖
+
+- 停车控制参数集中在 `Configuration/PilotConfig.ParkingControl.cs`，动作默认读取 `PilotDefinition.Conf`。
+- 组合运动段可以覆盖自身完成条件；实验轨迹速度描述实验本身，车辆级最大命令速度仍负责最终限制。
+
+### 12. Detour阶跃采用任务坐标连续化与安全停车分级处理
+
+- Detour原始世界观测与当前任务控制位姿通过 `controlFromDetour` 隔离；疑似阶跃先进入候选确认，确认期间使用轮组 `Vx/Vy/Vw` 短时预测。
+- 仅对幅值受限且连续确认的小坐标偏移自动更新变换；大幅、不一致或超过确认窗口的变化使状态不可用，由动作安全停车。
+- 完整轨迹状态继续要求位置和航向均可靠；原地自转只依赖独立验证的航向，位置单独异常不再终止正在进行的自转，航向异常仍触发安全停止。
+- 近似原地自转期间禁止自动吸收坐标偏移，避免把旋转定位劣化写入任务坐标系；目标航向到达并停止驱动后，才在既有幅值和确认窗口内处理位置恢复，未恢复时禁止进入下一运动段。
+- `l_step` 当前只作为诊断和后续健康分级依据，不单独决定状态有效性。
+- 依据：`DetourVehicleStateProvider`、`WheelFeedbackVehicleStateProvider`、`MultiWheelRotateInPlace`。
+
+### 13. 车队布局与状态采用不可变快照，刚体速度采用确定性分解
+
+- `Shared/Fleet/FleetLayout.cs` 保存经车号唯一性和有限值校验的 `VehicleLayout` 快照，不提供搬运过程中的逐车修改入口。
+- `FleetKinematics.Decompose()` 已按平面刚体关系把 `FleetMotionCommand` 分解为各成员中心速度，并转换到各车真实车体系；不依赖通信、Detour、β、底盘限幅或QP。
+- `MultiWheelC.Tests/FleetKinematicsTests.cs` 已覆盖整体平移、绕车队中心旋转、绕成员车旋转和停止四个数学场景。
+- `MultiWheelC/Fleet/FleetState.cs` 保存车队虚拟中心的世界位姿、同一点的世界系/车队系速度、状态采样时间和速度有效性；`FleetPoseInWorld.Yaw` 定义车队 `+X` 方向，两份速度只是同一物理速度的不同坐标表达。
+- `FleetState.SampleTimestampSeconds` 采用主车/协调器生成聚合快照时的本机单调时间。成员本机时钟和Detour `tick` 的同步属于未来接收/状态估计层职责，不阻塞使用人工构造 `FleetState` 开发纯车队控制器。
+- 当前无法直接获得被搬运车辆中心，因此 `FleetLayoutCapture.Capture()` 已确定用成员车体中心X/Y的算术平均定义车队原点，用指定主车Yaw定义车队朝向；主车缺失时拒绝采集，不做隐式降级。
+- 布局采集只返回采集时刻的 `FleetPoseInWorld` 与不可变 `FleetLayout`，不读取Detour/通信，也不负责夹紧、静止、时间对齐或原子激活。6个布局采集数学场景已通过。
+
+### 14. 原地自转保留绝对航向与轮组相对角两种反馈模式
+
+- `DetourAbsoluteHeading` 保留世界航向闭环和停车后的Detour位姿恢复，适用于必须对准绝对方向的任务。
+- `RelativeWheelOdometry` 通过 `TryGetWheelTwist()` 获取与Detour无关的滤波轮组 `Vw`，按轮组采样时间积分相对角度；活动自转不因Detour位置或航向退化中断。
+- 轮组相对模式是工程降级/测试模式，不等同于绝对定位：允许约±5°误差，不能消除轮胎打滑、轮径误差和积分漂移，也不能用于长期世界航向基准。
+- 依据：`WheelFeedbackVehicleStateProvider`、`MultiWheelRotateInPlace`、`RotationTests`。
+
+### 15. 单车与车队轨迹跟踪通过组合共享纯控制核心
+
+- `PathTrackingContext` 只携带受控刚体的车体系速度、速度有效性、轨迹投影和周期控制量，不依赖 `VehicleState` 或 `FleetState`。
+- `PathTrackingCore` 集中实现投影连续性、保护/终点策略、曲率预瞄、横纵向控制和GCP分配；`ParkingGeometricController` 与 `FleetController` 分别组合该核心，负责各自的状态适配和输出边界，不通过继承复制控制流程。
+- `FleetController` 第一版只闭环车队虚拟中心，并支持固定 `β_fleet`：实际纵向速度沿β投影，GCP结果从运动坐标系旋转到车队坐标系后形成车队原点处的 `FleetMotionCommand`；成员速度继续由 `FleetKinematics.Decompose()` 确定性分解。
+- `FleetStateEstimator` 已根据固定 `FleetLayout` 和时间对齐目标时刻反算、融合车队中心，并输出每车 `FleetMemberLayoutError`；当前第一版先做候选中心两两一致性检查，再对X/Y等权平均、对Yaw做圆周平均。正负候选偏差的公共分量进入车队中心，相对分量仍保留在成员布局误差中，不会因平均而消失。成员样本的实际采集和跨电脑时间处理仍属于外层接收链。
+- `FleetCoordinator` 已串联状态估计、虚拟中心控制、布局误差警告区间内的统一速度缩放、刚体分解和 `FleetMemberCommandCorrector` 小范围纠偏；`FleetRuntime` 已把周期结果转换为本车直接执行和远端成员命令。
+- `FleetPreparationCoordinator` 已按 `beta_i = beta_fleet - theta_i` 生成成员准备目标，并通过任务号和全员Ready形成统一激活屏障；`FleetMemberAgent` 负责本车舵轮准备、激活、命令校验和底盘执行；`FleetRuntime` 将二者接入同一固定布局滚动任务生命周期。
+- 控制算法的可替换性继续由 `ILateralController`、`ILongitudinalController` 组合注入；状态源、通信、底盘发送和成员协调不进入纯核心。
+- `MultiWheelC.Tests` 已覆盖8个Stanley前进/倒车符号场景、8个车队控制周期场景、6个布局采集场景和4个刚体分解场景；统一构建与打包通过。
+
+### 16. 车队通信保持最小值契约，失联停车采用主车与本车双层保护
+
+- `Shared/Fleet/FleetProtocol.cs` 只保存 `FleetCommand`、`FleetMemberReport`、枚举和协议常量，不绑定停车机器人控制器、串口实现或复杂消息类层次；无线字节帧和校验留在具体传输实现。
+- `IFleetTransport` 是运行层唯一传输边界；当前 `InMemoryFleetTransport` 只在测试项目中模拟定向命令、广播和成员报告，后续无线实现替换该接口而不改车队算法。
+- 主车 `FleetSafetySupervisor` 依据主车本地接收时间、成员状态和故障锁存整队停车决定；每辆车的 `FleetMemberAgent` 依据本机单调时间和命令有效期独立看门狗停车。`FleetRuntime` 已执行本车停止并向可达成员下发停止，两层保护仍不能互相替代。
+- 同一 `FleetRuntime` 部署到所有车辆，角色由显式 `selfVehicleId` 与 `leaderVehicleId` 决定，不把固定车号硬编码为主车；主车也是成员，本车命令直接执行，不要求传输层回环。
+
+## 已经确认但尚未实施
+
+- 路线顺序：先完成单车闭环和停车功能验证，再正式实施多车通信、编队和协同控制。来源：`README.md`。
+- 转舵系统辨识应对四个轮组分别使用相同激励，输入采用实际差速转舵命令（`TotalDiff`或左右轮实际发送命令之差），输出采用实际舵角；若只用目标角到实际角，会把当前PID包含在闭环模型中，更换PID后该模型不能继续代表转舵机构。控制阶段优先保留四个独立PID实例和内部状态、共用一套参数，并依据四个健康轮组中最不利的动态设计稳定裕量；只有硬件健康且公共参数仍无法兼顾时，才评估配置化的逐轮小幅校准，不在代码中硬编码某个轮位特例。
+- 当前多车的固定布局滚动任务已形成内存通信可运行链路。仍缺少C层正式动作入口、布局原子激活、成员状态实际采集、跨机时间换算、公共世界坐标验证和无线传输实现。
+- 单车 `MultiWheelC/StateEstimation` 继续负责Detour重复帧、跳变候选、轮速短时预测和任务坐标连续化；车队层不复制这套原始定位处理，只消费经过本车校验的成员状态，并负责跨车时间对齐、固定布局反算、成员一致性检查和中心融合。成员状态进入融合前仍必须确认处于同一公共坐标系；各车独立的 `_controlFromDetour` 连续化变换是否会造成跨车基准差异，属于通信/状态接收契约必须验证的事项。
+- 第一版保留当前保守的等权车队中心融合；任务生命周期和内存零速运行链已经贯通。公共坐标系与主车时间轴语义确认并取得静止/低速双车日志后，再按数据增加车队历史预测、逐成员创新门控、健康降级和Huber等鲁棒加权；不在缺少Detour协方差时提前实现协方差加权或Covariance Intersection。
+- 布局生命周期区分夹紧前后的语义：夹紧前的预设布局只用于引导车辆就位；车辆夹紧且静止后，应同步取得同一世界坐标系下的成员位姿，调用 `FleetLayoutCapture` 创建新的不可变布局，再由上层协调器原子激活。共同搬运期间的相对位姿变化属于状态误差，不能通过修改 `FleetLayout` 吸收；松开车辆后清除激活布局。实际数据采集和激活接口尚未实施。
+- 多车共同搬运不能只闭环车队中心：整体位姿误差与成员相对布局误差必须分开估计和约束，否则成员误差可能相互抵消而使平均中心看似正确。
+- 计划采用分层职责：车队控制器产生参考点 `FleetTwist`，分配层依据成员 `VehicleLayout` 计算每车真实车体系 `BodyTwist`，单车层继续负责β变换、GCP和本车四轮解算。
+- 第一版采用确定性的虚拟刚体速度分配，不先引入QP/HQP：若成员在车队系中的固定布局为位置 `(x_i,y_i)`、朝向 `theta_i`，则成员中心在车队系中的速度为 `(Vx-omega*y_i, Vy+omega*x_i, omega)`，再通过 `R(-theta_i)` 转到本车体系后交给 `SendBodyTwist()`。QP/HQP只在需要同时调整车队参考速度、处理成员能力差异、松弛约束或严格任务优先级时再引入。
+- “按状态最差车辆协调速度”第一步已经对成员相对布局误差实现警告阈值至停止阈值之间的统一速度缩放。通信超时、状态不可用、成员故障和命令有效期已经进入运行链；成员报警/夹紧信号来源、可恢复降级策略和实车阈值仍待接入，延迟预测补偿可以后续增加。
+- 虚拟车队使用固定在车队坐标系中的对称前后GCP，把横向控制结果转换成车队原点 `FleetTwist`；这些点不是物理轮轴，也不直接参与单车四轮解算。横向控制器与GCP到Twist转换必须使用同一控制点半径；具体车辆级配置值和实车验证仍待完成。
+- 每辆成员车都应作为反馈来源，但反馈职责必须分层：成员Detour位姿用于融合车队整体位姿和检查相对布局，单车轮速/舵角用于确认命令执行偏差，电机电流、扭矩或力传感信息用于负载与内力监控。相对位姿接近目标并不能证明没有内力，因此不能只依靠刚性连接或位姿误差判断负载均衡。
+- 第一版不把每车β作为复杂优化变量：`FleetController` 支持固定车队主要滚动方向 `beta_fleet`，`FleetPreparationCoordinator` 按布局换算成员 `beta_i` 并采用180°等效轴，`FleetRuntime` 已将停车预对齐、全员Ready和统一激活接入运行链。β是单车执行坐标系，不改变刚体分配得到的真实车体系 `BodyTwist`；只有出现复杂布局、整段方向变化、限位余量或频繁反号问题时，才增加轨迹级β候选搜索。
+- 旧版参考项目采用固定双车布局：各车由 `carWorld ∘ layout⁻¹` 反推车队中心，再对位置和圆周航向求平均；路径控制器以该虚拟中心跟踪轨迹。同时它可按 `fleetTarget ∘ layout_i` 生成每车理想位姿，并叠加Detour布局纠偏和邻车两腿检测纠偏，因此并非只控制平均中心。来源：`原版停车机器人/parkingrobot/ClumsyPilot/PilotDefinition.cs`、`ChassisController.cs`。
+- 旧版 `SetOriginBias(layoutX, layoutY, layoutTh)` 是把各车真实轮子统一表达在车队虚拟坐标系中，属于固定编队布局变换。旧版联动显式区分常规、蟹行和绕车队中心旋转三类模式；蟹行角可由动作或遥控给出任意值（`FleetCrabWalk` 默认45°），并在运动前以零速度对齐舵轮、运行时使用180°等效和轮速反号，但没有根据整段轨迹和每车约束自主求解β的统一规划过程。给定简单蟹行动作时，它与新版固定β可能产生相同的实际轮子姿态和车辆运动。
+- 旧版自动 `FleetCurveWalk`、`FleetCrabWalk` 会先以零速度下发初始GCP角，等待成员新鲜、布局正确、命令可行、舵轮到位和从车应用新序列后才开始运动；原地旋转通过 `RotateWheelsAligned` 和 `FleetMotionReleased` 做整队释放。普通手动入口仍有 `SendMotion` 本车舵轮未对齐时速度置零的门控，但不保证与自动动作相同的车队级同步屏障。
+- 旧版的全局与局部定位用途不同：自动模式的车队中心估计仍依赖Detour，与局部POS纠偏开关无关；邻车两腿检测只能提供相对间距和姿态信息，不能单独确定世界坐标中的车队中心。
+- 旧版单车几何控制器和原地自转直接读取 `DetourInterface.getCartLocation()`，未见当前新版的源时间对齐、跳变候选或任务坐标连续化；旧版宿主参考代码另有基于 `l_step` 和数据新鲜度减速/暂停并触发重定位的 `RelocalizationManager`。这只能说明旧版通过宿主安全状态机管理明显失效，不能据此认定Detour位姿天然连续；实车部署是否使用相同宿主版本和配置仍待确认。来源：`mdcstoolbox/Clumsy/MotionControllers/AbstractGeometricController.cs`、`原版停车机器人/parkingrobot/ClumsyPilot/Movements.cs`、`mdcstoolbox/Clumsy/HighLevelSecurity/RelocalizationManager.cs`。
+- 旧版的 `MultiVehicleUseDetect` 与 `MultiVehicleSyncUseDetour` 是彼此独立且默认关闭的开关，并非强制二选一。两者同时开启时，常规/蟹行的 `SendMotion()` 会直接将检测补偿与POS补偿逐分量相加，没有互斥或状态级融合；这不等于每次运行必然冲突，但存在重复修正、相互对抗和放大噪声的结构风险。来源：`原版停车机器人/parkingrobot/ClumsyPilot/PilotConfig.cs`、`PilotDefinition.cs`。
+- 旧版方案只作为设计参考，不直接移植：`GetLayoutPoseForCar()` 将成员写死为两侧镜像布局，车队几何通过 `SetOriginBias(layout)` 和 `ControlPointRadius=distance/2` 进入各车底盘解算；当两类局部纠偏均关闭时，成员一致性仍可能依赖共同开环命令和被搬运物的机械约束。
+
+## 待评估
+
+- 多车共同搬运时的车队参考点与固定GCP距离、逐成员能力/命令可行性限幅，以及带时间预测和异常成员隔离的鲁棒车队位姿融合；当前任意成员布局、确定性刚体分解和小范围队形误差闭环已有第一版实现。
+- 确定性刚体分配验证完成后，再评估集中式单步QP/HQP：安全和刚体可行性应作为硬约束或更高层级，相对布局、中心跟踪、平滑与能耗依次降低优先级；严格HQP不能仅靠单个加权QP的大权重近似。短时域MPC及舵轮动态延迟预测属于更后续阶段。
+- 负载共享和内力监控可用信号、阈值、降级与停车策略；当前项目尚未建立可确认的力/扭矩闭环。
+- 正式轨迹规划层与 `Trajectory2D` 的接入格式；当前 `TestTrajectoryFactory` 仅用于实验。
+- 是否长期保留旧版GCP/`SendMotion`后端，或增加经过充分验证的“车体Twist直接到各轮”的新后端。
+- 蟹行遥控中按半轮距/半轴距缩放舵角的映射是否符合统一曲率语义；这是遥控手感策略，不应与坐标变换混为一谈。

docs/detour-information-checklist.md

@@ -0,0 +1,44 @@
+# Detour 信息确认清单
+
+用途：向 Detour 负责人确认控制程序使用定位结果所必需的信息。当前不需要 Detour 源码，接口说明或书面答复即可。
+
+## 1. `getCartLocation()` 字段定义
+
+- `x`、`y`、`theta`、`tick`、`l_step` 分别表示什么。
+- 各字段的单位、正方向和坐标系。
+- `theta` 的取值范围，例如 `[-180°, 180°]`。
+- 一次调用返回的字段是否属于同一个定位帧。
+
+## 2. `tick` 的准确含义
+
+- 是传感器采集时刻、SLAM 计算时刻，还是接口返回时刻。
+- 单位、更新频率、是否单调递增，以及是否可能重置或回绕。
+- 多台车的 `tick` 是否来自同步时钟，能否用于多车位姿时间对齐。
+
+## 3. 定位延迟与更新方式
+
+- 输出位姿通常比真实运动滞后多少毫秒。
+- `getCartLocation()` 是否返回最近一次缓存结果。
+- 原地自转时更新频率或延迟是否会明显变化。
+
+## 4. `l_step` 的含义
+
+- 它表示优化迭代次数、定位质量、匹配状态，还是其他指标。
+- 为什么原地自转时可能从 2～4 上升到几十甚至上百。
+- 是否有官方推荐的正常、退化和不可用阈值。
+
+## 5. 坐标跳变与重定位行为
+
+- 重定位、回环优化或匹配失败恢复后，`x/y/theta` 是否可能永久跳变。
+- 跳变后是否仍处于原来的地图坐标系。
+- 是否存在“正在重定位”“定位丢失”“地图坐标调整”等状态标志。
+
+## 6. 可用的定位质量接口
+
+- 除 `l_step` 外，能否获得匹配得分、协方差、置信度、定位状态或错误码。
+- Detour 官方建议控制程序依据哪些条件接受或拒绝一帧位姿。
+
+## 优先级
+
+最优先确认第 2、4、5、6 项。拿到这些信息后，可确定 `MultiWheelC/StateEstimation` 中时间同步、跳变连续化、自转短时预测和安全停止规则的最终设置。
+

docs/interfaces.md

@@ -0,0 +1,240 @@
+# 关键接口与数据约定
+
+## 坐标系和单位
+
+`Shared/` 的统一约定：
+
+- 位置：m；线速度：m/s；角度：rad；角速度：rad/s。
+- 真实车体坐标系：X向前、Y向左、逆时针为正。
+- 原始世界坐标来自Detour；位姿在 `DetourVehicleStateProvider.ReadDetourObservation()` 边界由mm/deg转换为m/rad，再通过任务坐标变换生成连续控制位姿。
+- `AngleMath` 的弧度归一化范围是 `[-π, π)`，角度范围是 `[-180°, 180°)`。
+- `Pose2D` 表示局部坐标系在父坐标系中的位姿；变量名使用 `XxxInYyy` 说明关系。
+- `Twist2D` 不携带坐标系标签，必须由变量名、外层类型或接口契约说明。
+
+旧版 `CommonUsage` 底盘接口使用混合单位：轮子位置和 `ControlPointRadius` 为mm，`SendMotion` 舵角与 `CarSpeed.Vw` 为deg/deg/s，线速度为m/s。单位转换应只出现在Shared适配边界。
+
+## Shared 数据模型
+
+文件：`Shared/Models/MotionModels.cs`。
+
+| 类型 | 语义 |
+| --- | --- |
+| `Point2D` | 二维位置或向量，单位m |
+| `Pose2D` | 二维位置和朝向，单位m/rad |
+| `Twist2D` | 同一点处的 `Vx`、`Vy`、`Omega`，单位m/s、rad/s |
+| `VehicleLayout` | 单车车体系在车队系中的固定目标位姿 |
+| `FleetLayout` | 不可变的成员布局快照，构造时校验成员数量、车号唯一性和位姿有限值 |
+| `FleetMotionCommand` | 车队参考点及该点在车队系中表达的刚体速度 |
+| `FleetMemberCommand` | 指定车辆及其真实车体系中表达的成员中心速度 |
+| `FleetState` | 同一采样时刻的车队虚拟中心位姿和速度快照 |
+
+坐标变换集中在 `FrameTransform2D`；有限值检查集中在 `NumericGuard`；角度处理集中在 `AngleMath`。
+
+`FleetKinematics.Decompose()` 依据 `v_i = v_ref + omega × (r_i-r_ref)` 生成每车命令，再按 `VehicleLayout.PoseInFleet` 的朝向把线速度从车队系转换到成员车体系；所有刚性连接成员的 `Omega` 保持相同。
+
+`MultiWheelC/Fleet/FleetState.cs` 中，`FleetPoseInWorld` 表示车队虚拟中心位姿，其 `Yaw` 同时定义车队坐标系 `+X` 在世界系中的方向；车队系采用 `+X` 前、`+Y` 左、逆时针为正。`TwistAtFleetOriginInWorld` 与 `TwistAtFleetOriginInFleet` 是同一参考点、同一物理速度在两个坐标系中的表达，后者由前者和 `FleetPoseInWorld` 推导，不是第二份独立测量。`HasValidVelocityEstimate=false` 时速度按零保存，用于区分尚未形成可靠速度估计与真实零速。
+
+`FleetState.SampleTimestampSeconds` 表示主车/协调器生成该车队状态快照时的本机单调时间，不是Detour全局时间。各成员电脑的本机时钟和Detour `tick` 当前不能直接互相比较；未来接收层应另行保存来源时间并完成新鲜度和时间对齐。
+
+### `FleetLayoutCapture`
+
+文件：`MultiWheelC/Fleet/FleetLayoutCapture.cs`。
+
+- `FleetMemberPose`：用于布局采集的车号和成员世界位姿输入。
+- `FleetLayoutCaptureResult`：同时返回采集时刻的 `FleetPoseInWorld` 和固定的 `FleetLayout`；车队当前世界位姿不存入布局。
+- `Capture(members, leaderVehicleId)`：车队原点X/Y取成员车体中心的算术平均，Yaw取主车Yaw，并通过 `inverse(FleetPoseInWorld) ∘ VehiclePoseInWorld` 得到每车 `PoseInFleet`。
+- 空成员、非法或重复车号、非有限位姿、主车不存在均拒绝采集；主车缺失时不使用其他车辆降级代替。
+
+该接口是纯几何计算，调用者必须在外部保证成员位姿处于同一世界坐标系，并完成夹紧、静止、数据新鲜度和时间对齐检查；布局的存储与原子激活也不属于该类。
+
+## 车队通信与安全接口
+
+`Shared/Fleet/FleetProtocol.cs` 只定义与具体无线模块无关的值类型：
+
+- `FleetCommand`：任务号、主车命令序号、目标车号、命令类型、成员β、成员车体系速度、有效期和原因码。目标车号0表示广播。
+- `FleetMemberReport`：车号、任务号、本车报告序号、来源采样时间、公共世界系位姿/速度、状态有效性、本地任务阶段、最近执行命令序号和故障码；它同时承担心跳和命令确认。
+- `FleetCommandKind` 当前覆盖滚动准备、自转准备、激活、运动和停止；`FleetMemberState` 覆盖空闲、准备、就绪、活动和故障。
+
+`MultiWheelC/Fleet/IFleetTransport.cs` 仅提供 `SendCommand`、`SendReport`、`TryReceiveCommand` 和 `TryReceiveReport`。它不负责串口初始化、字节序列化、任务/序列校验、安全判定或时钟同步；测试项目的 `InMemoryFleetTransport` 按车号路由FIFO消息，广播只送从车，主车本地命令由 `FleetRuntime` 直接执行。
+
+时间语义分层：报告的 `SampleTimestampSeconds` 是来源车的采样时刻，真实跨机接收层必须换算到主车时间轴后才能用于 `FleetMemberStateSample` 对齐；`FleetMemberSafetyStatus.LastAcceptedReportTimeSeconds` 必须使用主车本地单调接收时间；`FleetMemberAgent` 的命令接收时间和截止时间必须使用本车本地单调时钟。Detour `tick` 不能直接替代这三类时间。
+
+`FleetSafetySupervisor` 只在主车侧检查成员缺失、任务不一致、通信超时、状态不可用和故障，并锁存首个 `ShouldStop` 原因；它不直接操作底盘。`FleetMemberAgent.UpdateCommandWatchdog()` 是每车本地最后一道失联保护，活动状态下命令过期或本地时钟倒退会立即 `StopImmediately()` 并进入 `Faulted`。运行循环即使没有收到新命令也必须周期调用该方法。
+
+### `FleetRuntime`
+
+- 构造时传入本车ID、主车ID、`IFleetTransport`、本车 `FleetMemberAgent` 和 `IVehicleStateProvider`；本车ID等于主车ID时，还必须传入 `FleetPreparationCoordinator`、`FleetCoordinator` 和 `FleetSafetySupervisor`。
+- `StartRollingPlan(planId, layout, trajectory)` 只允许主车调用；成员准备β直接取自 `FleetCoordinator.MotionDirectionInFleetRadians`，避免准备方向与控制方向出现两份配置。
+- `Update(currentTimeSeconds, deltaTimeSeconds)` 使用本机单调时间驱动准备超时、主车报告新鲜度和本地命令看门狗。主车缓存的远端报告采样时间必须已经换算到主车时间轴，当前运行层不会自行估计跨机时钟偏移。
+- `Stop(reason)` 正常取消本车任务；主车同时广播当前任务停止。运行状态覆盖 `Idle`、`Preparing`、`Ready`、`Active`、`Completed` 和锁存的 `Faulted`。
+- 第一版公开启动入口只支持固定布局滚动轨迹；协议和从车执行器虽已保留 `PrepareSpin`，但整队自转尚无对应主车任务入口。
+
+## 轨迹契约
+
+文件：`MultiWheelC/Trajectory/`。
+
+### `TrajectoryPoint`
+
+- `ArcLengthMeters`：按预定执行点序从起点累计的弧长，首点必须为0。
+- `PoseInWorld`：车体中心参考位姿；Yaw始终表示车头方向，不因倒车改为车尾方向。
+- `CurvaturePerMeter`：沿弧长增加/执行点序定义，左弯为正。
+- `ReferenceSpeedMetersPerSecond`：轨迹切线方向的有符号车体中心参考平移速度，绝对值是速度模长，正值前进、负值倒车、0停车。
+
+### `Trajectory2D`
+
+- 至少两个点，弧长严格递增，相邻位置不能重合。
+- 弧长增量必须与离散线段长度在容差内一致。
+- 当前是空间轨迹，以弧长插值位置、Yaw、曲率和参考速度；没有时间戳，也不是时间参数化轨迹。
+- `SampleAtArcLength()` 和 `TrajectoryProjector` 共用 `InterpolateSegment()`，避免两套插值语义。
+
+### `TrajectoryProjector`
+
+- 首周期可全轨迹搜索；后续控制器使用上次弧长附近窗口，当前常量为后退0.10m、前进1.00m。
+- 距离并列时优先接近上次进度，降低交叉或平行轨迹跳段风险。
+- `LateralErrorMeters` 相对轨迹点序判断左右，轨迹位于车辆左侧时为正。
+- `HeadingErrorRadians` 是参考车头航向减实际车头航向的最短角差。
+
+测试轨迹由 `Experiments/TestTrajectoryFactory.cs` 生成，不是正式规划层。直线和曲线速度同号；负速度表示倒车，测试工厂不支持在同一条曲线中直接切换前进/倒车方向。
+
+## 状态接口
+
+### `DetourInterface.getCartLocation()`
+
+当前引用接口返回的定位对象包含：
+
+- `x`、`y`：Detour世界坐标，单位mm。
+- `th`：车体航向，单位deg。
+- `tick`：定位源时间；实车静态记录确认可按`.NET DateTime.Ticks`转换，同时必须保留原始整数用于诊断。
+- `l_step`：Detour定位过程的质量/步骤相关指标，精确定义和正式阈值待Detour文档确认；实车正常静态基线包含2和周期性单帧3，不能将3直接判为异常。
+
+`DetourVehicleStateProvider` 使用 `x/y/th` 生成控制位姿，使用 `tick` 区分重复、新到和倒退帧，并保留 `l_step` 供诊断。`l_step` 当前不作为单一硬门限：实车数据中既出现过高 `l_step` 后恢复，也出现过低 `l_step` 但位姿创新异常的情况。状态时间仍使用本机单调 `Stopwatch`；`Experiments/DetourStaticDiagnosticTest.cs` 可只读记录原始字段、接口耗时、帧间差和轮组反馈。
+
+当前继续设计状态层所需的Detour侧最小信息是：`getCartLocation()` 返回的是可能重定位跳变的全局/map位姿还是连续里程计位姿；`tick` 对应采集、解算还是发布时刻及缓存语义；`l_step` 的状态含义；现有接口是否另有连续里程计位姿/速度或定位有效、重定位状态。无需以取得Detour源码为前提，部署版本、里程计和单线激光SLAM配置截图可用于核对实际运行配置。以上信息尚待Detour侧确认。
+
+### `IVehicleStateProvider`
+
+```text
+bool TryGetState(out VehicleState state)
+```
+
+返回 `false` 表示当前状态不可用；`ParkingGeometricController` 会主动停车、重置反馈控制器并等待下一周期恢复。
+
+### `VehicleState`
+
+- `SampleTimestampSeconds`：状态源单调时钟时间。
+- `PoseInWorld`：当前任务控制世界系中的车体中心位姿；初始化时与Detour世界系对齐，确认有限坐标阶跃后可通过内部变换保持任务连续，不保证始终等于原始Detour坐标。
+- `TwistInWorld`、`TwistInBody`：同一刚体速度的两种表达。
+- `HasValidVelocityEstimate`：速度反馈是否已建立有效时间基准；无效时构造器将速度置零。
+
+### 默认组合状态源
+
+`ParkingVehicleStateProviderFactory.Create()` 创建：
+
+- `DetourVehicleStateProvider`：读取Detour位姿，处理源时间、重复帧、运动合理性、预测创新、静止确认和跳变候选；候选确认期间使用轮组速度短时预测控制位姿。确认后的有限小坐标偏移可更新 `controlFromDetour` 以保持当前任务坐标连续，超限或未恢复时将状态置为不可用。
+- `WheelFeedbackVehicleStateProvider`：调用 `MultiWheelChassis.GetCarSpeed(true)`，低通滤波车体 `Vx`、`Vy` 和由deg/s转换为rad/s的 `Vw`。轮组估计有效后，最终 `VehicleState` 的 `Vx/Vy/Omega` 均使用滤波后的轮组反馈；首帧速度标记为无效，并由 `VehicleState` 对外置零。轮组 `Vw` 同时提供给Detour短时运动预测和动态航向合理性判断。
+
+默认滤波参数来自 `PilotConfig.ParkingControl.cs`：Detour线速度0.15s、Detour角速度0.20s、轮组反馈 `Vx/Vy/Vw` 统一为0.10s。
+
+跳变处理的重要边界：航向创新允许量随 `|Vw| × Detour源帧间隔` 增加；候选若在近似原地自转期间开始，则整个候选确认过程禁止自动改写任务坐标系。默认候选确认窗口为0.60s，窗口内输出轮组预测状态，超时后完整位姿安全返回不可用。
+
+完整轨迹控制仍通过 `TryGetState()` 要求位置和航向均有效。原地自转提供两种反馈模式：
+
+- `DetourAbsoluteHeading` 使用 `TryGetHeadingRadians()` 做世界航向闭环；仅位置异常时可继续，航向异常仍会停止。到达目标并停车后调用 `BeginPostRotationPositionRecovery()` 恢复完整位姿，恢复失败时不释放下一运动段。
+- `RelativeWheelOdometry` 使用 `TryGetWheelTwist(out Twist2D, out timestamp)` 直接读取并滤波轮组 `Vx/Vy/Vw`，按轮组采样时间梯形积分相对转角；活动自转期间不调用Detour，也不执行Detour停车后恢复。该模式用于允许约±5°误差的相对角动作，不能提供绝对世界航向校正，轮胎打滑和轮径误差会累积到角度结果中。
+
+`TryGetWheelTwist()` 是不依赖Detour的窄接口；首次尚未形成滤波样本时返回 `false`，其时间戳来自该状态源使用的本机单调时钟。
+
+`TrackingExperimentRecorder` 已记录原始/滤波轮组 `Vx/Vy/Vw`、轮组采样时刻、Detour `tick/l_step`、数据年龄和源帧间隔、轮速预测时刻、实际/允许的位置与航向创新、候选触发原因、估计器状态及不可用原因。创新和源帧间隔只在收到Detour新帧时更新；若CSV记录频率高于Detour帧率，后续记录行会重复最近一个新帧的诊断值，分析时应按 `DetourTickRaw` 去重或分组。
+
+## 控制接口
+
+### `PathTrackingContext`
+
+一次控制周期的只读快照，包含：受控刚体的车体系 `Twist2D`、速度有效标志、`TrajectoryProjection`、处理后的控制参考速度、预瞄曲率、真实 `deltaTime` 和运动方向β。该类型不依赖单车 `VehicleState` 或车队 `FleetState`。
+
+`ActualLongitudinalSpeedMetersPerSecond` 是完整车体平面速度沿β方向的投影：
+
+```text
+Vβ = cos(β)·Vx_body + sin(β)·Vy_body
+```
+
+因此45°/90°蟹行不能只使用车体 `Vx` 判断纵向速度。
+
+### `PathTrackingCore`
+
+`PathTrackingCore.Compute(Pose2D, Twist2D, bool, double)` 是单车与车队共用的纯轨迹跟踪周期：负责连续投影、距离保护、起步释放、终点制动/完成判断、曲率预瞄、横纵向控制和GCP分配，返回 `PathTrackingCycleOutput`。输出只包含周期结果、可选 `GcpMotionCommand`、投影和计算耗时；核心不读取状态源、不发送底盘命令，也不处理通信。
+
+状态暂时不可用时，外层调用 `PauseForUnavailableState()` 保留当前轨迹与投影连续性；明确失败或取消分别使用 `Fail()`、`Cancel()`。
+
+`ParkingGeometricController` 是单车适配层，负责读取 `IVehicleStateProvider` 并通过 `GcpCommandExecutor` 发送实体底盘命令。`FleetController` 是车队虚拟中心适配层，使用 `FleetState.FleetPoseInWorld` 和 `TwistAtFleetOriginInFleet` 调用同一核心。构造参数 `motionDirectionInFleetRadians` 定义固定的 `β_fleet`：核心按该方向投影实际纵向速度，GCP结果先解释为运动坐标系Twist，再通过 `R(β_fleet)` 转换成车队坐标系下、车队原点处的 `FleetMotionCommand`。`β_fleet=0` 保持常规前向语义；当前控制器不负责运行中切换β。
+
+当前速度闭环和执行边界的语义并不完全相同：纵向PID与Stanley实际速度分母使用 `Vβ`（Stanley也可按配置改用参考速度），而 `GcpMotionCommand.SpeedMetersPerSecond` 和旧版 `SendMotion.speed` 表示带行驶方向符号的车体中心平移速度模长。正常圆弧理想跟踪时 `Vy=0`，两者相等；只有横向误差共同转角产生非零 `Vy` 时，模长与 `Vβ` 才相差余弦因子。当前最大命令速度仍限制最终发送的模长。
+
+### `ILateralController`
+
+- `Compute(PathTrackingContext)` 返回 `LateralControlCommand`。
+- `Reset()` 清除跨周期状态。
+- 默认实现 `StanleyLateralController` 输出前后GCP角度：横向误差形成共同转角，航向误差和曲率前馈形成差动转角；倒车时按行驶方向修正符号。
+- `TrajectoryTrackingMovement.LateralControllerFactory` 是替换Stanley的动作级扩展点。
+
+### `ILongitudinalController`
+
+- `ComputeSpeedMetersPerSecond(PathTrackingContext)` 返回有符号中心命令速度。
+- 默认 `PidLongitudinalController` 使用轨迹参考速度前馈叠加实际速度PID反馈，带死区、积分限制和最大命令速度；参考明确为0时禁止反向速度纠偏。
+
+### GCP命令
+
+- `LateralControlCommand`：前后GCP目标角度，以及共同/差动分量。
+- `GcpCommandAllocator`：分别限制前后GCP角度并与纵向速度组合。
+- `GcpMotionCommand`：进入底盘执行前的有符号速度和前后GCP角度，SI单位。
+- `GcpCommandExecutor`：限制GCP角速度，保存 `LastRequestedCommand`/`LastSentCommand`，转换为 `Twist2D` 后执行。
+
+对称前后GCP位于当前运动系的 `(±R, 0)`。依据刚体速度关系 `v(point)=v(center)+ω×r`：
+
+```text
+Vfront = (Vx, Vy + ωR)
+Vrear  = (Vx, Vy - ωR)
+```
+
+因此 `Vy` 形成前后同向的共同转角，`ωR` 形成前后反向的差动转角。正常圆弧的理想车体速度为 `(v, 0, v·κ)`，曲率通过差动转角实现，并不要求车体系存在 `Vy`。
+
+## 底盘命令接口
+
+### `MultiWheelChassisAdapter.SendBodyTwist`
+
+输入始终是真实车体系 `Twist2D`：
+
+- 平移和角速度均近零：立即停车。
+- 平移近零、角速度非零：要求真实车体系已激活，调用 `SendXYThSpeed` 做纯自转。
+- 平移非零：使用动作开始前已经准备并激活的固定β运动系，转换为前后GCP方向并调用旧版 `SendMotion`。
+
+运动中不通过 `Vx/Vy` 猜测模式；模式由“停车→舵轮预对齐→`ActivateMotionFrame(β)`”显式确定。命令若几乎垂直于当前运动系X轴会停车并拒绝执行。
+
+`MultiWheelChassisAdapter` 构造时读取旧底盘 `GetOriginBias().Z`，按 `β=-biasZ` 同步内部缓存；这不是SLAM Yaw，也不会切换坐标系或转动舵轮。真正切换由 `ActivateMotionFrame(β)` 调用 `SetOriginBias(0, 0, -β)` 完成，旧底盘随后把真实轮位重新表达在β运动系中。
+
+滚动命令在 `SendRollingTwistInActiveMotionFrame()` 中按 `R(-β)` 将真实车体系平面速度转换到已激活运动系；角速度在二维旋转变换下不变。传给 `SendMotion` 的前后GCP角度均相对该运动系X轴，速度绝对值为 `sqrt(Vx²+Vy²)`，正负号取运动系 `Vx` 的方向。倒车方向由速度符号表达，GCP角度保持为相对行驶方向的等效机械方向，避免无意义旋转180°。
+
+### `MultiWheelChassis.SendMotion`
+
+输入是车体中心有符号平移速度模长、当前运动系中的虚拟前后GCP方向。前后GCP法线交点确定ICR；每个真实轮子使用其运动系位置计算切线舵角和半径速度比例。机械限位通过等效舵角/反向轮速解析，无法满足时返回失败原因。
+
+## 配置接口
+
+- `PilotConfig` 是Clumsy运行配置模型；`[FieldMember]` 字段提供默认值和宿主显示/持久化元数据。
+- `PilotDefinition.Conf` 是C层动作实际读取的运行配置对象。
+- `MultiWheelC/Configuration/PilotConfig.ParkingControl.cs` 集中停车状态估计、Stanley、纵向PID、原地自转、舵轮准备、GCP与完成条件参数。
+- 动作中的 nullable 覆盖字段用于特定动作段或测试；为空时使用车辆配置。车辆级限速和通用控制参数不应在普通实验中随意覆盖。
+- `参考文档/*.json` 是样例/实车复制资料；当前源码未发现这些JSON被 `PilotDefinition.Conf` 自动读取的入口。
+
+运行时配置文件格式、位置和覆盖优先级由外部Clumsy宿主决定，当前仓库内待确认。
+
+## M/C IO与MCU边界
+
+- C层 `PilotDefinition` 和M层 `DiverCartDefinition` 使用 `[AsUpperIO]`/`[AsLowerIO]` 对齐夹臂命令、驱动使能、位置反馈和车号等字段。
+- M层 `WheelSpeedDiagnosticLogger` 每次记录生成 `_can.csv` 和 `_snapshot.csv`：前者按CAN回调时刻保存八电机速度/位置与四舵角事件，后者按最多50Hz保存目标/实际舵角、目标角速度、PID/前馈/合成差速、限幅前后电机命令、速度/位置/电流反馈及各事件的本机单调时间、序号和数据年龄。MATLAB辨识时可使用 `TargetTh` 作为参考、`TotalDiff` 或 `Sent*` 作为执行输入、`ActualTh` 作为输出；左右轮公共/差速通道由方向统一后的左右命令和反馈在分析侧组合。
+- `DiverCartDefinition.CommunicationInit()` 默认通过Windows `COM4`、`1,000,000 baud` 打开MCU桥；`MCUPort` 是可配置初始化参数。
+- MCU内部配置为逻辑端口0：CAN `500,000 bit/s`；逻辑端口1～3：串口 `9,600 bit/s`。`MCURoutine.BatteryPortIndex = 3` 指MCU桥逻辑端口，不等同于Windows `COM3`。
+- CAN命令/反馈范围集中在 `MCURoutine.cs`：驱动命令 `0x201～0x20A`，速度/位置反馈 `0x281～0x28A`，状态 `0x181～0x18A`，舵角 `0x18B～0x18E`，远程帧 `0x701～0x70A`。
+- `MCUSerialBridgeCLR.cs` 是 `mcu_serial_bridge.dll` 的P/Invoke封装；本仓库源码目录未包含该本机库。
+
+CAN ID、串口参数、IO位和驱动方向属于实车安全边界，未经明确要求不得修改。

docs/overview.md

@@ -0,0 +1,46 @@
+# 项目概览
+
+## 项目定位
+
+`MyParking` 是停车机器人控制软件的当前正式开发版本，主体为 C# 插件工程。项目目标是让多舵轮停车机器人完成底盘运动、车辆状态获取、轨迹跟踪、原地自转和后续停车作业；当前研发重点仍是单车闭环和实车联调，多车协同已有纯计算骨架但尚未形成可运行实现。来源：`README.md`、`MultiWheelC/PilotConfig.cs`、`MultiWheelC/Fleet/`、`Shared/Fleet/`。
+
+项目不是可直接 `dotnet run` 的独立应用：
+
+- `MultiWheelC.dll` 由 Clumsy 宿主加载，提供 C 层动作、控制和 `MovementTest` 入口。
+- `MedullaAdapter.dll` 由 Medulla 宿主加载，负责 M 层硬件通信和周期逻辑。
+- `CommonUsage.dll` 提供通用底盘模型和四轮几何解算。
+
+对应入口类型为 `MultiWheelC/PilotDefinition.cs::PilotDefinition` 和 `MedullaAdapter/DiverCartDefinition.cs::DiverCartDefinition`；仓库中没有 `Program.Main`。
+
+## 车辆与业务场景
+
+- 代码接口面向四个可转向轮组，并暴露8个驱动电机的位置/速度反馈和4个舵角反馈。M/C IO定义见 `PilotDefinition`、`DiverCartDefinition`。
+- 车体支持正常前后行驶、倒车轨迹、任意固定运动方向β下的滚动运动、蟹行和停车后原地自转。主要入口见 `MultiWheelC/Movements/` 和 `MultiWheelC/Experiments/`。
+- 夹臂速度、位置、限位和报警IO已经接入，但轮胎识别、自动钻车、释放车辆和完整停车作业状态机尚未在当前新版流程中完成。来源：`PilotConfig.cs`、`PilotDefinition.cs`、`README.md`。
+- 多车已具备 `FleetLayout`、`FleetState`、`FleetController` 和 `FleetKinematics` 组成的纯计算链；多车配置仍位于 `PilotConfig.cs` 的 `#if false`，且状态聚合、通信、成员纠偏、安全协调和实体发送尚未接入，因此不能把当前代码视为已支持可运行的多车编队。
+
+## 整体运行流程
+
+典型新版轨迹跟踪流程：
+
+1. Clumsy 宿主通过 `MovementTest` 或动作计划启动 `TrajectoryTrackingMovement`。
+2. 动作停车并通过 `PrepareWheelsForward` 将舵轮预对齐到本段运动方向β。
+3. `ParkingVehicleStateProviderFactory` 组合 Detour 位姿和轮组反馈速度。
+4. `ParkingGeometricController` 完成轨迹投影、横纵向控制、GCP分配和完成条件判断。
+5. `GcpCommandExecutor`、`MultiWheelChassisAdapter` 将SI单位命令转换到当前运动坐标系，再调用 `CommonUsage.Chassis.MultiWheelChassis`。
+6. M层周期逻辑将底盘和夹臂命令发送到MCU/CAN，并把反馈通过宿主IO返回C层。
+
+详细调用链见 `architecture.md`，接口和坐标语义见 `interfaces.md`。
+
+## 当前阶段
+
+- 单车基本运动、新版轨迹跟踪、倒车直线、45°蟹行直线、原地自转和组合动作均有宿主测试入口。
+- 新版控制默认采用 Stanley 横向控制和 PID 纵向控制；轨迹实验和控制周期诊断可输出CSV。
+- 当前代码能力不等于完整实车验收，特别是高速、曲线倒车、蟹行曲线、长期稳定性和异常工况仍需单独验证。
+
+## 待确认
+
+- Clumsy/Medulla 宿主的正式版本、插件部署目录和启动顺序。
+- `PilotDefinition.Conf` 在实车上的持久化文件位置、加载时机和发布流程。
+- 真实车辆最终几何参数、舵轮零位/限位和不同车辆配置的权威来源。
+- 完整停车业务的产品流程、安全状态机和验收指标。

docs/problems.md

@@ -0,0 +1,136 @@
+# 问题与验证状态
+
+## 已解决（代码层）
+
+### 轨迹数值检查和插值入口重复
+
+- 现象：多个轨迹文件分别实现有限值检查和插值，语义容易漂移。
+- 处理：Shared增加 `NumericGuard`；轨迹采样和投影共用 `Trajectory2D.InterpolateSegment()`。
+- 位置：`Shared/Validation/NumericGuard.cs`、`Trajectory2D.cs`、`TrajectoryProjector.cs`。
+- 验证范围：代码结构已统一；无自动化单元测试记录。
+
+### 交叉/邻近轨迹可能发生投影进度跳变
+
+- 原因：每周期在全轨迹搜索最近线段可能跳到空间上接近但进度不连续的线段。
+- 处理：首周期全局搜索，后续按上次弧长使用后退0.10m、前进1.00m窗口，并在距离并列时优先原进度。
+- 位置：`TrajectoryProjector`、`ParkingGeometricController`。
+
+### 倒车曲率和反馈符号不完整
+
+- 处理：轨迹使用有符号参考速度，Yaw保持车头方向；横向误差继续相对轨迹执行点序定义，共同转角不再乘行驶方向，航向修正和曲率前馈保留倒车反号。
+- 现有入口：`NewControllerReverseStraight4mTest`。
+- 验证：`MultiWheelC.Tests` 的8个无实车符号场景通过；2026-08-19第六轮实车记录中，倒车零偏置终点误差约15.9mm和12.8mm，+150mm偏置横向误差由约-150.4mm收敛到-5.0mm、终点误差约18.1mm，已与正向直线精度接近。
+- 限制：直线倒车有测试入口，曲线倒车仍归入待验证。
+
+### 终点零速参考可能提前停车且无法收敛
+
+- 处理：增加终点制动预瞄和单向低速逼近；越过终点不反向修正。
+- 完成条件：剩余弧长、终点距离、终点航向和β方向轮组实际速度全部满足。
+- 位置：`ParkingGeometricController`、`PilotConfig.ParkingControl.cs`。
+
+### 状态配置和终点速度来源不一致
+
+- 处理：统一通过 `ParkingVehicleStateProviderFactory` 创建状态链；终点速度使用Vx/Vy沿β投影，不再让Detour横向速度单独影响普通前进判停。
+- 后续补充：`WheelFeedbackVehicleStateProvider` 已保留并滤波轮组反馈Vy，支持蟹行纵向速度投影。
+
+### 动作开始/结束时舵轮模式不明确
+
+- 处理：轨迹跟踪开始前自动准备β方向；原地自转先准备自转姿态，完成后回正；组合计划在运动前预检全部段。
+- 位置：`PrepareWheelsForward`、`TrajectoryTrackingMovement`、`MultiWheelRotateInPlace`、`MotionPlanExecutor`。
+
+### Detour持续小阶跃会直接扰动控制轨迹
+
+- 处理：状态层已分离Detour原始坐标与任务控制坐标；疑似阶跃期间使用滤波后的轮组 `Vx/Vy/Vw` 短时预测，有限小偏移经连续帧确认后更新坐标变换，大幅或超时未恢复的变化返回状态不可用。
+- 自转保护：动态航向阈值考虑实际轮组Vw和Detour源帧间隔；近似原地自转期间禁止自动吸收坐标偏移；位置单独异常时航向闭环可继续，到位停车后再恢复完整位姿，未恢复时阻止下一运动段。
+- 位置：`DetourVehicleStateProvider`、`WheelFeedbackVehicleStateProvider`、`MultiWheelRotateInPlace`、`PilotConfig.ParkingControl.cs`。
+- 验证：第三轮实车组合动作中发生一次约47mm/3.44°的原始坐标变化，自动连续化计数增加后控制轨迹保持连续，动作最终以约24.7mm剩余距离、2.8mm横向误差完成。
+
+### Detour自转退化会中断允许小角度误差的相对自转
+
+- 处理：`MultiWheelRotateInPlace` 增加 `RelativeWheelOdometry` 反馈模式，直接读取并积分滤波轮组 `Vw`；活动自转期间不调用Detour。原有 `DetourAbsoluteHeading` 模式继续用于需要世界绝对航向的任务。
+- 验证：2026-08-21第七轮包含19份轮组里程计自转记录，其中17份表现为正常停车并完成舵轮回正；这17份中轮组积分转角与Detour原始航向变化的绝对差中位数约0.89°、最大约3.07°。同批Detour原始位置端点变化中位数约56mm、最大约298mm，说明轮组模式确实绕开了自转期间的Detour退化依赖。
+- 限制：CSV尚未记录请求角度、内部积分角和明确完成原因，因此上述数据不能作为目标角精度的正式验收；另有2份记录未完成回正，属于手动停止、小角度超时或其他原因仍待确认。轮组积分也不能替代绝对世界航向。
+
+## 待解决
+
+### 完整停车作业流程尚未实现
+
+- 缺少或未接入新版流程：轮胎识别、自动钻车、夹抱/释放动作编排、完整安全状态机。
+- 现有 `PilotConfig` 和IO字段不能视为业务流程已经完成。
+
+### 多车执行链仍未完成
+
+- 纯计算链 `FleetState → FleetController → FleetMotionCommand → FleetKinematics.Decompose()` 已实现，并有6个车队控制周期场景和4个刚体分解场景通过。
+- `PilotConfig.cs` 的多车区域在 `#if false` 中，不参与当前编译。
+- 当前仍没有布局采集/激活、车队状态估计、通信接收与时间对齐、相对布局闭环、能力限幅、安全降级、成员底盘发送和多车实车测试闭环，因此不能把纯计算层视为可运行的多车功能。
+
+### 轮组速度初始化回退语义不一致
+
+- `WheelFeedbackVehicleStateProvider` 在轮组首帧无效时局部选择Detour `Omega`，但随后以 `hasValidVelocityEstimate=false` 构造 `VehicleState`；`VehicleState` 会把整份速度对外置零，因此该Detour角速度回退实际不可见。
+- 影响仅限轮组滤波尚未形成有效采样的初始化阶段；应确认期望是保持整份速度无效并置零，还是允许单独提供有效的Detour角速度，再决定是否修改代码和注释。
+
+### Detour跳变判据和动作段衔接仍需收敛
+
+- 2026-08-19第四轮记录包含16次独立原地自转：4次未出现候选、3次候选后恢复、9次因候选超时变为不可用，共25个候选片段。全部由 `PositionInnovationExceeded` 触发，没有 `HeadingInnovationExceeded`；航向创新峰值约7.1°，动态允许量约7.5～9.3°，说明当轮航向动态判据不是主要失败点。
+- 候选开始时Detour单帧平移中位数约53mm、最大约162.3mm，轮组 `Vx/Vy` 接近零；Detour新帧间隔约98.2～134.4ms，中位数约114.4ms。候选开始时 `l_step` 为2～83，其中9/25发生在 `l_step<=3`，另有 `l_step=61` 的自转未触发候选，继续证明 `l_step` 不能单独作为有效性门限。
+- 同轮7次直线中6次完成，其中2次完成有限坐标连续化；1次在约190mm原始阶跃后安全失败。组合动作完成，仅出现约47.7mm的短候选并自行清除。小范围连续化已有正面证据，大幅阶跃仍应保持安全边界。
+- 已实施“自转期间位置/航向分离、停车后再恢复位置”的动作衔接，编译和统一打包通过，但尚待实车验证。预期它解决位置单独异常导致的自转中断，不代表已经消除Detour原始位置阶跃。
+- 当前实现先读取Detour观测，再把轮速预测推进到本机当前 `Stopwatch` 时刻并比较两者；Detour观测对应的源时刻通常更早，因此比较时刻尚未严格对齐。0.4m/s且Detour约0.11s一帧时，这种时序差可产生约44mm表观位置创新，是固定40mm阈值可能过严的重要原因。短期可评估有界的“基础余量 + 平面轮速 × 源帧间隔”，但成熟修正应保存短时轮速/里程计历史，将观测与同一源时刻的预测比较，再把校正状态预测到当前时刻；该方案尚未实施。
+- 健康 `l_step` 下仅凭预测创新不应轻易自动改写坐标系；在完成同一时刻比较前，不继续通过反复放宽阈值堆叠补丁。
+- 不建议通过全局放宽0.60s确认窗口、40mm残差或允许自转期间完整坐标修正来掩盖问题。
+- 2026-08-19第六轮记录确认普通正向/倒车直线、曲线和小范围运动中坐标连续化总体可用，主要剩余失败集中在原地自转：有样本在 `l_step=94` 时连续3个Detour新帧航向创新超限，状态层先短时使用轮组Vw预测，第三帧才安全终止，说明单帧航向容错已按设计生效；另有约163mm、207mm的位置不连续或候选不稳定导致停车后恢复失败。
+- 当前决定：保持普通运动150mm自动连续化上限、自转最大角速度30°/s和角加速度40°/s²，不以全局放宽阈值或提高自转速度掩盖Detour退化。允许相对角误差的动作可选已实现的轮组里程计模式；必须闭环到绝对世界航向时，持续航向异常仍需要Detour质量/重定位语义或额外可信绝对航向来源才能进一步收敛。
+
+### 自动化测试和CI覆盖有限
+
+- 现有 `[MovementTest]` 是Clumsy宿主人工/实车入口，不是 `dotnet test`。
+- `MultiWheelC.Tests` 是不进入正式打包的独立可执行回归项目，现已覆盖Stanley前进/倒车横向符号的8个场景；应长期保留其源码，`bin/obj`仅为可删除编译产物。
+- 轨迹数学、坐标变换、状态跳变过滤和终点策略仍缺少标准测试框架下的可重复单元测试，当前也没有CI。
+
+### 运行和发布信息不完整
+
+- 宿主版本、正式插件目录、启动顺序、运行时配置文件位置和发布审批流程待补充。
+- `mcu_serial_bridge.dll` 不在当前源码目录，实车运行依赖外部部署。
+
+## 待验证
+
+### Detour静态基线与运动跳变条件
+
+- `Experiments/DetourStaticDiagnosticTest.cs` 已提供只读宿主测试，记录 `x/y/th/tick/l_step`、数据年龄、接口耗时、帧间差、四轮反算 `Vx/Vy/Vw` 及单轮反馈。
+- 2026-08-18约881s静态记录中没有复现5～7cm阶跃：Detour新帧最大位移9.21mm、2s前后中值最大持续偏移6.11mm，前后30s位置中值变化约15.5mm；该结果是当前环境的正常静态基线，不证明运动中阶跃已经消失。
+- 同次记录确认接口无失败或乱序，Detour实际新帧间隔中位数约109.94ms（约9Hz），数据年龄中位数57.19ms、最大134.49ms；`getCartLocation()`调用本身中位耗时仅0.0139ms，说明接口主要返回缓存的最新定位。
+- 静止时直接差分Detour位姿仍产生表观速度：线速度中位数约0.0132m/s、P95约0.0320m/s，角速度绝对值P95约0.680°/s；继续使用轮组反馈作为速度闭环来源是合理的。轮组反算在本次静态记录中基本为零，但仍需运动实验验证。
+- `l_step` 正常基线主要为2，并约每30s出现一个新帧值3，且未与明显位姿变化相关；精确定义、异常阈值以及Detour是否提供显式重定位/坐标重置状态仍待确认。
+- 运动记录进一步表明 `l_step` 与位姿创新并非一一对应。建议仅将其作为分级健康信号：低值稳定可支持恢复判断，高值持续且同时存在异常创新时才支持停车；实际分级阈值仍需结合部署端Detour/MDCS版本确认。
+- C层轨迹CSV现已同时记录原始/滤波轮组 `Vx/Vy/Vw`、轮组采样时刻、Detour源帧间隔和预测时刻、实际/允许的位置与航向创新、候选触发原因、估计器状态及不可用原因。该记录解决了字段缺口，但Detour源 `tick` 与本机单调时钟/轮组采样时钟之间尚未建立统一时间轴，仍需通过新实验和Detour `tick` 语义确认后才能定量评价预测误差。
+
+### 控制周期和舵轮响应
+
+- 代码已经记录控制周期分段耗时、请求/限速后GCP命令和四舵角；M层已有轮速/舵角诊断CSV。
+- 差速转舵角速度前馈默认增益0.9，曲率预瞄默认0.15s/0.12m。
+- 2026-08-19第六轮共9131个轨迹控制周期：周期中位数约31.24ms、P95约32.68ms、最大约59.44ms；控制计算总耗时中位数约0.12ms、P95约0.22ms。当前C层计算不是主要周期瓶颈，历史约110ms现象不应继续归因于控制算法计算量。
+- 2026-08-25静止诊断确认：目标角和角速度前馈均为零时，右后轮仍可形成约1s量级的持续差速转舵往复；死区由0.1°增至0.5°后，另外三轮均停止输出，但一次自转模式切回正常模式后的右后轮仍在约-1.9°至+2.9°间振荡。约2°的瞬态偏差本身不能证明硬件故障，待定位对象是仅该轮不收敛的闭环动态差异；应先用降低公共比例增益的重复切换实验区分控制稳定裕量，再对四轮分别辨识延迟、增益、摩擦和方向不对称，明显离群轮组先排查编码器、机械间隙和低速驱动响应。
+- 不同速度、载荷下的舵轮物理响应和前馈参数仍需按具体工况验证。
+- CAN/MCU正常运行逻辑风险较高；除诊断外不应在没有明确方案和实车回退措施时修改。
+
+### 尚未覆盖的运动工况
+
+- 曲线倒车。
+- 45°/90°蟹行曲线及不同β下的机械限位。
+- 非零β曲线中 `MultiWheelChassis.GetCarSpeed(true)` 的坐标一致性：`SetOriginBias` 会旋转 `sw.Position`，需要确认 `ReadAngle()` 返回值在该解算中的参考系与之匹配；当前不能仅由蟹行直线推断曲线速度反馈正确。
+- 0～1.2m/s范围内曲率预瞄、速度滤波和GCP角速度限制的参数适用性。
+- 高速、低附着、载车后质量/惯量变化、定位丢失和急停恢复。
+
+### 参数与实车配置一致性
+
+- `参考文档/*.json` 是否与当前实车配置一致待确认。
+- `PilotDefinition.CarLength/CarWidth`、宿主底盘轮子布局、`ControlPointRadius`、舵轮零偏和机械限位需要按车辆编号核验。
+
+## 排障入口
+
+- C层轨迹CSV：`TrackingExperimentRecorder`，默认宿主目录 `TrackingExperiments/`。
+- C层Detour静态诊断：`DetourStaticDiagnosticTest`，默认宿主目录 `DetourStaticDiagnostics/`。
+- C层周期耗时：`ParkingGeometricController.LastCycleTiming` 和 `*_timing.csv`。
+- M层轮速/舵角：`StartWheelSpeedDiagnostic()` / `StopWheelSpeedDiagnostic()`，默认 `logs/wheel-speed/`。
+- 底盘分解失败：`MultiWheelChassisAdapter.LastFailureReason`、`GcpCommandExecutor.LastFailureReason`。
+- 状态失败：`DetourVehicleStateProvider.LastFailureReason`、`WheelFeedbackVehicleStateProvider.LastFailureReason`。

docs/progress.md

@@ -0,0 +1,62 @@
+# 当前进展
+
+更新日期：2026-08-25。这里只保存当前状态，不作为完整开发历史。
+
+## 已完成/已接入
+
+- 三项目解决方案与统一构建打包脚本：`CommonUsage`、`MedullaAdapter`、`MultiWheelC`。
+- Shared的SI运动模型、坐标变换、角度工具、数值校验和 `MultiWheelChassisAdapter`。
+- 弧长参数化 `Trajectory2D`、统一插值、轨迹投影窗口和进度连续性。
+- 默认 Stanley 横向 + PID 纵向的新版轨迹控制链，横向控制器可注入替换。
+- 前进4m、倒车4m、45°蟹行4m、直线-左半圆-直线和组合运动宿主测试入口。
+- Detour位姿与轮组反馈组合状态源：位姿来自Detour；轮组估计有效后，控制状态的Vx/Vy/Omega均来自滤波后的轮组反馈，Vw同时用于短时位姿预测和动态航向合理性判断。
+- Detour源 `tick/l_step` 诊断、跳变候选确认、有限小偏移任务坐标连续化、自转期间禁止自动吸收偏移，以及超时状态不可用保护。
+- 起步释放、曲率前馈预瞄、GCP角速度限制、终点制动预瞄和单向低速收敛。
+- 原地自转舵轮准备、航向PID、超时保护和完成后回正；自转期间位置/航向有效性已分离，到位停车后才恢复完整位姿并决定是否释放下一段。
+- Detour单帧航向异常已增加连续帧/短时预测确认；第六轮实车数据确认前两帧异常由轮组Vw预测承接，持续到第三个异常新帧时才安全终止。
+- 原地自转已增加 `RelativeWheelOdometry` 模式和不依赖Detour的 `TryGetWheelTwist()` 接口；第七轮19份记录中17份表现为正常停车回正，轮组积分与Detour原始航向变化绝对差中位数约0.89°、最大约3.07°，但目标角精度尚缺专用字段正式验收。
+- C层轨迹/周期CSV与M层轮速/舵角诊断记录。
+- C层Detour静态诊断入口，记录源时间、`l_step`、位姿帧差和轮组静态反馈；已有约14分41秒实车静态基线。
+- C层轨迹CSV已补充原始/滤波轮组 `Vx/Vy/Vw`、轮组与预测时刻、Detour帧间隔、实际/允许创新、候选原因、估计器状态和不可用原因。
+- 停车控制参数集中到 `Configuration/PilotConfig.ParkingControl.cs`。
+- `PathTrackingContext` 已解除对单车 `VehicleState` 的依赖；`PathTrackingCore` 统一单车与车队的投影、保护/终点策略、横纵向控制和GCP分配，`ParkingGeometricController` 保留单车状态读取与实体命令执行职责。
+- `MultiWheelC.Tests` 已提供不依赖实车的Stanley前进/倒车横向符号回归，8个场景通过；该项目不进入正式解决方案和打包脚本。
+- `Shared/Fleet` 已加入不可变 `FleetLayout`、成员/车队命令模型、确定性 `FleetKinematics` 及最小 `FleetCommand`/`FleetMemberReport` 契约；`MultiWheelC/Fleet` 已具备布局采集、车队状态估计、固定 `β_fleet` 中心控制、统一速度缩放、刚体分解、小范围成员纠偏、成员β准备屏障和本车执行边界。
+- 车队安全、通信边界和首版运行链已经贯通：`FleetRuntime` 按显式本车/主车ID执行主从分支，串联β准备屏障、全员激活、状态缓存、主车安全判定、周期协调、本车直接执行、远端分发、完成/取消和从车本地命令看门狗；`IFleetTransport` 隔离具体传输，测试项目提供不回环主车命令的内存实现。Fleet相关自动化测试共70个场景通过。
+- 本次建立工作区/项目AGENTS导航和 `docs/` 按需知识库。
+
+以上表示代码入口存在，不表示全部实车工况已经验收。
+
+## 当前进行方向
+
+- 普通正向/倒车直线、曲线和小范围运动中坐标连续化已基本可用；允许小角度误差的相对自转已有轮组里程计模式，必须对准绝对世界航向时仍受Detour偶发持续退化限制。
+- 暂时冻结普通运动跳变阈值和30°/s、40°/s²自转参数，等待Detour接口语义后再决定是否实施自转结束后的条件化仅位置连续化或调整航向恢复策略。
+- Detour对接最小问题已整理到 `docs/detour-information-checklist.md`，不要求取得源码。
+- 验证非零β运动系，当前已有45°蟹行直线入口；曲线蟹行仍需设计实验。
+- 多车共同搬运的固定布局滚动任务已在内存传输下完成端到端贯通。当前重点转为真实无线链路和工程接入：确认无线参数与帧格式，实现串口 `IFleetTransport`，接入公共坐标系成员状态和主车时间轴，再通过静止状态机与低速双车逐级验证。QP/HQP不作为第一版前置条件。
+- 车队状态估计第一版保留“成员状态先经单车状态估计处理、车队层做时间对齐与刚体一致性检查、候选中心等权/圆周平均”的保守方案；暂不重复实现单车Detour跳变逻辑，也不在运行链和实车数据建立前加入复杂鲁棒优化。后续内部融合升级应尽量保持 `FleetStateEstimateResult`、`FleetState` 和 `MemberErrors` 外部接口不变。
+- β在多车中定位为单车执行坐标系而非车队核心优化量：常规、斜行和横移动作先确定车队主要滚动方向，各车按布局朝向换算本地β，停车预对齐并经车队同步屏障统一释放。轨迹级β搜索只作为机械余量或复杂方向变化下的后续增强。
+
+## 阻塞/待确认
+
+- Clumsy/Medulla正式宿主、插件部署和配置持久化说明未纳入仓库。
+- 完整停车业务流程和验收指标未确认。
+- 最小内存消息契约已经确定，但真实无线串口的端口参数、帧格式、序列化/校验和跨机时钟换算尚未实现；成员状态实际采集方式、布局原子激活接口、刚体误差实车阈值、夹紧/报警状态来源和故障降级策略仍待确认。
+- 各车经过独立 `_controlFromDetour` 任务坐标连续化后的位姿如何统一到同一公共车队坐标系仍待验证。车队参考点和纯几何采集方法已经确定，运行布局应在夹紧后生成不可变快照并由上层整体激活。
+- 最新实车实验数据对控制周期与舵轮滞后的结论尚未沉淀为可复核结果。
+- Detour `l_step` 精确定义、显式重定位/坐标重置状态和部署端MDCS恢复策略待确认；当前不能把 `l_step<4` 当作唯一有效性条件。
+- Detour `getCartLocation()` 的位姿坐标系、`tick` 采样/解算/发布语义、是否已有独立连续里程计或定位状态接口，以及部署版本和实际里程计/SLAM配置待确认。
+- Detour原地自转时 `l_step` 大幅上升、100～200mm级位置不连续和连续航向创新异常的内部原因待Detour负责人说明；仅凭当前位姿接口无法保证持续航向异常时仍可靠完成绝对角度闭环。
+- Detour源 `tick` 与本机单调时钟、轮组采样时刻尚无统一时间轴；当前创新比较可能包含数据年龄造成的表观误差。
+
+## 建议下一步
+
+1. 按 `docs/detour-information-checklist.md` 确认 `getCartLocation()` 字段、`tick`、`l_step`、定位延迟、重定位行为和质量状态接口。
+2. 在Detour信息返回前不继续全局放宽状态估计边界，也不提高自转角速度/角加速度；现有偶发定位不可用保持安全停车。
+3. 为轮组里程计自转CSV补充请求角度、内部积分角、完成/超时/手动停止原因，按固定目标角重复验证误差和重复性；绝对航向任务继续保留Detour模式，不用轮组积分冒充世界航向。
+4. 为轨迹插值/投影、坐标变换、状态跳变候选和终点策略继续补充不依赖宿主的数学回归测试。
+5. 确认无线模块的Windows COM端口、波特率、数据位、停止位、校验方式、收发模式和模块配置；定义带版本、长度、类型、任务/序列号及CRC的最小字节帧。
+6. 实现串口版 `IFleetTransport`，先只做主车心跳/命令和从车状态报告的静态通信测试，测量丢包、乱序、延迟和断线；不得绕过现有命令有效期与本车看门狗。
+7. 接入C层正式车队动作入口、成员状态实际来源和主车接收时间轴，保证 `FleetMemberReport` 位姿处于同一公共世界坐标系，远端采样时间已换算为主车时间后再形成 `FleetMemberStateSample[]`。
+8. 先验证静止布局建立、β准备、全员Ready、统一激活和停止，再进行空载低速双车直线与圆弧；共同搬运前补齐夹紧/报警输入和可立即停车措施。
+9. 记录各成员候选中心、数据年龄、状态来源、布局误差和减速/停车原因。只有数据证明当前等权融合频繁被单成员异常拖累时，再增加车队预测、逐成员门控和鲁棒加权。

docs/数据融合.txt

@@ -1,94 +0,0 @@
-是的，强烈建议做系统辨识，尤其是你这种要把电机反馈和 SLAM 融合的场景。
-为什么需要系统辨识？
-卡尔曼滤波（或 EKF）的效果很大程度上取决于过程模型有多准。模型不准的话，会出现：
-
-预测步持续往错误方向跑
-滤波器过度依赖测量（SLAM），或者反过来过度信任错误的模型
-速度估计系统性偏大/偏小
-原地自转时航向纠正效果变差
-
-你现在已经知道电机反馈“偏大”，这本身就是典型的模型参数问题（可能是轮胎半径、减速比、编码器标定、打滑补偿等）。
-建议辨识的主要参数
-针对四轮差速车，优先考虑这些：
-
-运动学参数
-有效轮半径（或左右轮半径）
-轮距（左右轮中心距离）
-可能的轴距相关项（如果不是纯差速）
-
-速度相关
-电机速度到实际车速的缩放系数（你提到的“偏大”问题）
-左右轮速度的不对称性（如果存在）
-低速段的非线性（静摩擦、死区）
-
-可选但有用的
-速度偏置 / 零点漂移
-简单的打滑系数（如果经常在特定地面跑）
-
-
-怎么做比较实际？
-不需要一开始就上很复杂的在线辨识，可以分两步：
-离线标定（推荐先做）
-
-让车在平坦地面以不同速度直线行驶、原地旋转，记录：
-四个电机反馈速度
-SLAM 给出的位置和航向变化
-
-用最小二乘或简单优化，拟合出轮半径、轮距、速度缩放系数等
-原地自转特别适合标定轮距和左右轮差异
-
-在线微调（可选）
-
-把关键参数（比如速度缩放系数、轮半径）也放进状态向量里一起估计（Augmented EKF）
-或者用简单的自适应方法，定期用 SLAM 和电机速度的一致性去微调缩放因子
-
-实际建议顺序
-
-先建立基本运动学模型 + EKF 框架
-做一轮离线系统辨识，把明显偏差（尤其是速度偏大）修正掉
-再跑融合，看残差（innovation）是否变小、速度估计是否更合理
-如果还有残余误差，再考虑把个别参数做成在线估计
-
-总结：建完模型后做系统辨识，是让卡尔曼滤波真正好用的关键一步。不做的话，即使融合了电机和 SLAM，效果也容易打折扣。需要的话，我可以给你一个针对差速车的简单离线标定思路（需要记录哪些数据、怎么拟合）。
-
-有没有必要做卡尔曼/传感器融合？
-分两层看：
-
-如果只是为了速度闭环，且当前绿色线已经能让车跑得比较稳
-→ 性价比一般。
-你现在的处理已经解决了最严重的问题，继续花时间做完整的 EKF + 电机融合，短期内收益可能没有那么大。可以先把绿色线再优化一下（比如调整低通截止频率、加一点自适应滤波，或者用更温和的滑动平均），看能不能把残余抖动再压低一点。
-如果你对速度精度、平滑度要求比较高，或者后面还要做更精细的控制（轨迹跟踪、力控、高精度定位等）
-→ 值得做，性价比会变高。
-原因是：
-绿色线本质还是“从位置差分出来的速度”，天然会有延迟和残余噪声。
-电机反馈是更直接的速度源，频率高、相位更好。
-把电机速度和经过处理的 SLAM 差分速度做融合（哪怕是简单的互补滤波或一阶卡尔曼），通常能同时得到：
-更低的噪声
-更小的滞后
-对电机“偏大”问题的一定校正能力
-
-
-
-实际建议（按性价比排序）
-
-先低成本优化现有绿色线（最快）
-检查低通滤波的截止频率是否合适（太低会滞后，太高会留噪声）。
-可以尝试一阶低通 + 中值滤波组合，或者简单的互补滤波（位置差分 + 电机速度）。
-看看闭环时车的实际表现，如果抖动已经可接受，就先用着。
-
-中等成本：做简单融合（推荐优先考虑）
-不用完整 EKF，先做一个互补滤波或一维卡尔曼只融速度：
-高频信任电机反馈速度
-低频用处理后的 SLAM 差分速度去校正偏差
-
-实现简单，效果通常比单纯低通好一截，滞后也更小。
-
-完整 EKF + 系统辨识
-只有在你需要同时提升位置、航向、速度的整体一致性，或者后面要上更复杂的控制时，再上这个。
-工作量明显更大，但长期收益最高。
-
-
-总结
-从你这张图来看，目前绿色线已经能用，不是“必须立刻上卡尔曼”的紧急情况。
-但如果你觉得绿色线的残余抖动还是影响闭环效果，或者想要更干净、滞后更小的速度，那么把四个电机反馈融进去（哪怕先做简单融合）是性价比不错的下一步，比继续死磕低通滤波更有前途。
-你现在速度闭环时，车实际表现如何？是觉得绿色线的抖动已经导致控制不稳，还是只是看着不舒服？这能帮助判断要不要马上上融合。