diff --git a/.codex/skills/commit/SKILL.md b/.codex/skills/commit/SKILL.md
deleted file mode 100644
index e78e37b..0000000
--- a/.codex/skills/commit/SKILL.md
+++ /dev/null
@@ -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` 强推，交给用户决定
-- 如果改动很大很杂，主动提示用户考虑拆分提交，但不强制
diff --git a/.codex/skills/readme/SKILL.md b/.codex/skills/readme/SKILL.md
deleted file mode 100644
index d4abab7..0000000
--- a/.codex/skills/readme/SKILL.md
+++ /dev/null
@@ -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写成完整设计说明书。
-- 代码块标注正确语言，命令应可复制，并注明必要的工作目录或前置条件。
diff --git a/AGENTS.md b/AGENTS.md
index b2803b0..91481d2 100644
--- a/AGENTS.md
+++ b/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`中的对应小段。
+- 没有值得长期保存的信息时，不为了形式要求强行更新文档。
diff --git a/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll b/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll
index 9ef0dbd..1085c7f 100644
Binary files a/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll and b/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll differ
diff --git a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll
index 54db927..5fee21c 100644
Binary files a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll and b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll differ
diff --git a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb
index f777dfe..b7b301c 100644
Binary files a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb and b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb differ
diff --git a/MultiWheelC.Tests/MultiWheelC.Tests.csproj b/MultiWheelC.Tests/MultiWheelC.Tests.csproj
new file mode 100644
index 0000000..03ff0b4
--- /dev/null
+++ b/MultiWheelC.Tests/MultiWheelC.Tests.csproj
@@ -0,0 +1,14 @@
+
+
+
+ Exe
+ net8.0
+ 10
+ false
+
+
+
+
+
+
+
diff --git a/MultiWheelC.Tests/Program.cs b/MultiWheelC.Tests/Program.cs
new file mode 100644
index 0000000..8eb146e
--- /dev/null
+++ b/MultiWheelC.Tests/Program.cs
@@ -0,0 +1,202 @@
+using System;
+using MultiWheelC.Control.Abstractions;
+using MultiWheelC.Control.Lateral;
+using MultiWheelC.StateEstimation;
+using MultiWheelC.Trajectory;
+using MyParking.Shared;
+
+namespace MultiWheelC.Tests
+{
+ ///
+ /// 验证Stanley控制器在前进和倒车时的横向误差符号及差动转角方向。
+ ///
+ 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;
+
+ ///
+ /// 运行不依赖宿主、Detour或实车底盘的控制器数学测试。
+ ///
+ 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个场景。");
+ }
+
+ ///
+ /// 验证共同转角产生的横向速度始终使轨迹点序横向误差绝对值减小。
+ ///
+ 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。");
+ }
+
+ ///
+ /// 验证航向误差差动转角在倒车时仍按行驶方向反号。
+ ///
+ 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,
+ "航向误差差动转角");
+ }
+
+ ///
+ /// 验证正曲率前馈差动转角在倒车时仍按行驶方向反号。
+ ///
+ 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,
+ "曲率前馈差动转角");
+ }
+
+ ///
+ /// 创建使用固定参数的无状态Stanley控制器。
+ ///
+ private static StanleyLateralController CreateController()
+ {
+ return new StanleyLateralController(
+ controlPointRadiusMeters: 0.5,
+ crossTrackGainPerSecond: 1.0,
+ headingErrorGain: 1.0,
+ minimumSpeedMetersPerSecond: 0.05,
+ useActualSpeedForGain: true);
+ }
+
+ ///
+ /// 创建只包含本次符号测试所需字段的轨迹跟踪上下文。
+ ///
+ private static PathTrackingContext CreateContext(
+ double signedSpeedMetersPerSecond,
+ double lateralErrorMeters,
+ double headingErrorRadians,
+ double feedforwardCurvaturePerMeter)
+ {
+ var vehicleState = new VehicleState(
+ sampleTimestampSeconds: 0.0,
+ poseInWorld: Pose2D.Identity,
+ twistInWorld: new Twist2D(
+ signedSpeedMetersPerSecond,
+ 0.0,
+ 0.0),
+ hasValidVelocityEstimate: true);
+ 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(
+ vehicleState,
+ projection,
+ signedSpeedMetersPerSecond,
+ feedforwardCurvaturePerMeter,
+ deltaTimeSeconds: 0.02);
+ }
+
+ ///
+ /// 验证实际值与预期符号一致。
+ ///
+ private static void AssertSameSign(
+ double actualValue,
+ double expectedSign,
+ string valueName)
+ {
+ AssertTrue(
+ Math.Sign(actualValue) ==
+ Math.Sign(expectedSign),
+ $"{valueName}方向错误：actual={actualValue:F6}，" +
+ $"expectedSign={expectedSign:F0}。");
+ }
+
+ ///
+ /// 条件不成立时抛出异常，使测试进程以非零状态结束。
+ ///
+ private static void AssertTrue(
+ bool condition,
+ string failureMessage)
+ {
+ if (!condition)
+ {
+ throw new InvalidOperationException(
+ failureMessage);
+ }
+ }
+ }
+}
diff --git a/MultiWheelC/Configuration/PilotConfig.ParkingControl.cs b/MultiWheelC/Configuration/PilotConfig.ParkingControl.cs
index a041b4d..367e321 100644
--- a/MultiWheelC/Configuration/PilotConfig.ParkingControl.cs
+++ b/MultiWheelC/Configuration/PilotConfig.ParkingControl.cs
@@ -27,9 +27,28 @@ 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 ParkingDetourLinearVelocityFilterSeconds = 0.15f;
@@ -115,10 +134,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 = 30f;
[FieldMember(desc = "停车控制：原地自转角加速度(deg/s²)")]
- public float InPlaceRotateAcc = 60f;
+ // public float InPlaceRotateAcc = 60f;
+ public float InPlaceRotateAcc = 40f;
[FieldMember(desc = "停车控制：原地自转超时(s)")]
public float InPlaceRotateTimeoutSec = 15f;
diff --git a/MultiWheelC/Control/Abstractions/PathTrackingContext.cs b/MultiWheelC/Control/Abstractions/PathTrackingContext.cs
index 5e2af4d..e685493 100644
--- a/MultiWheelC/Control/Abstractions/PathTrackingContext.cs
+++ b/MultiWheelC/Control/Abstractions/PathTrackingContext.cs
@@ -93,7 +93,7 @@ namespace MultiWheelC.Control.Abstractions
public double FeedforwardCurvaturePerMeter { get; }
///
- /// 获取参考轨迹相对车辆的有符号横向误差，单位为m，轨迹在车辆左侧时为正。
+ /// 获取相对轨迹执行点序的有符号横向误差，单位为m，参考轨迹位于执行方向左侧时为正。
///
public double LateralErrorMeters =>
Projection.LateralErrorMeters;
diff --git a/MultiWheelC/Control/Lateral/StanleyLateralController.cs b/MultiWheelC/Control/Lateral/StanleyLateralController.cs
index c45ba25..c5ac148 100644
--- a/MultiWheelC/Control/Lateral/StanleyLateralController.cs
+++ b/MultiWheelC/Control/Lateral/StanleyLateralController.cs
@@ -123,8 +123,9 @@ namespace MultiWheelC.Control.Lateral
context.HeadingErrorRadians,
MaximumHeadingCorrectionRadians);
+ // 横向误差已经按轨迹执行点序定义；倒车轨迹的点序会自然
+ // 翻转横向轴，因此共同转角不能再按行驶方向重复反号。
var commonAngleRadians =
- travelDirection *
crossTrackCorrectionRadians;
var differentialAngleRadians =
feedforwardAngleRadians +
diff --git a/MultiWheelC/Experiments/CompositeMotionPlanTests.cs b/MultiWheelC/Experiments/CompositeMotionPlanTests.cs
index 047557b..ea6e598 100644
--- a/MultiWheelC/Experiments/CompositeMotionPlanTests.cs
+++ b/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 =
diff --git a/MultiWheelC/Experiments/DetourStaticDiagnosticTest.cs b/MultiWheelC/Experiments/DetourStaticDiagnosticTest.cs
new file mode 100644
index 0000000..9577ab3
--- /dev/null
+++ b/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
+{
+ ///
+ /// 从C层测试界面启动定时的Detour静态定位与轮组反馈诊断记录。
+ ///
+ [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 _samples =
+ new List();
+ 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;
+
+ ///
+ /// 获取或设置自动结束前的记录时长，单位为min。
+ ///
+ public double DurationMinutes = 20.0;
+
+ ///
+ /// 获取或设置本机主动读取Detour的周期，单位为ms。
+ ///
+ public int SampleIntervalMilliseconds = 50;
+
+ ///
+ /// 获取最近一次静态诊断CSV的完整路径。
+ ///
+ public string SavedFilePath { get; private set; } =
+ string.Empty;
+
+ ///
+ /// 停车后开始静态采样，并在到达设定时长时自动保存CSV。
+ ///
+ 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);
+ }
+ }
+
+ ///
+ /// 请求提前停止采样；测试线程随后保存已经采集的数据。
+ ///
+ public override void TestStop()
+ {
+ Interlocked.Exchange(ref _stopRequested, 1);
+ _sampling = false;
+ _chassis?.PredefinedDriveStop();
+ }
+
+ ///
+ /// 清除上一次测试的样本、时间基准和输出路径。
+ ///
+ 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);
+ }
+
+ ///
+ /// 检查测试时长和主动采样周期是否适合执行。
+ ///
+ private void ValidateSettings()
+ {
+ NumericGuard.EnsureFinitePositive(
+ DurationMinutes,
+ nameof(DurationMinutes));
+
+ if (SampleIntervalMilliseconds < 20 ||
+ SampleIntervalMilliseconds > 5000)
+ {
+ throw new ArgumentOutOfRangeException(
+ nameof(SampleIntervalMilliseconds),
+ "Detour主动读取周期必须在20ms到5000ms之间。");
+ }
+ }
+
+ ///
+ /// 按设定周期持续采样，直至测试到时或收到停止请求。
+ ///
+ private void SamplingLoop(int sessionId)
+ {
+ while (_sampling &&
+ sessionId == Volatile.Read(ref _sessionId))
+ {
+ CaptureSample(sessionId);
+ Thread.Sleep(SampleIntervalMilliseconds);
+ }
+ }
+
+ ///
+ /// 采集一帧原始Detour定位、接口耗时和四轮实际反馈。
+ ///
+ 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);
+ }
+ }
+
+ ///
+ /// 读取Detour原始字段并计算与上一成功读取之间的时间和位姿差。
+ ///
+ 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);
+ }
+ }
+
+ ///
+ /// 将Detour原始tick按.NET DateTime ticks解释并记录数据年龄。
+ ///
+ 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;
+ }
+ }
+
+ ///
+ /// 计算Detour帧间差并更新下一帧使用的原始基准。
+ ///
+ 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;
+ }
+
+ ///
+ /// 读取底盘反算速度与按物理安装位置识别的四轮实际反馈。
+ ///
+ 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);
+ }
+ }
+
+ ///
+ /// 根据真实车体X向前、Y向左的物理安装位置查找指定舵轮。
+ ///
+ private static SteerWheel FindWheel(
+ IReadOnlyList 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;
+ }
+
+ ///
+ /// 追加本帧诊断失败原因且保留先前错误信息。
+ ///
+ private static void AppendFailure(
+ DiagnosticSample sample,
+ string reason)
+ {
+ sample.FailureReason =
+ string.IsNullOrWhiteSpace(sample.FailureReason)
+ ? reason
+ : sample.FailureReason + "；" + reason;
+ }
+
+ ///
+ /// 保存采样快照并输出自动结束或手动停止后的摘要提示。
+ ///
+ private void SaveCsvAndReport(
+ bool completedAutomatically,
+ Exception testFailure)
+ {
+ List snapshot;
+ lock (_sampleSyncRoot)
+ {
+ snapshot =
+ new List(_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);
+ }
+
+ ///
+ /// 使用InvariantCulture格式化并转义一行CSV字段。
+ ///
+ 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));
+ }
+
+ ///
+ /// 将单个值转换为区域无关且符合CSV转义规则的文本。
+ ///
+ 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("\"", "\"\"") +
+ "\"";
+ }
+ }
+}
diff --git a/MultiWheelC/Experiments/NewControllerTrackingTests.cs b/MultiWheelC/Experiments/NewControllerTrackingTests.cs
index 2ac7e9f..1adc765 100644
--- a/MultiWheelC/Experiments/NewControllerTrackingTests.cs
+++ b/MultiWheelC/Experiments/NewControllerTrackingTests.cs
@@ -231,7 +231,8 @@ namespace MultiWheelC
(float)AccelerationMetersPerSecondSquared,
referenceDecelerationMetersPerSecondSquared:
(float)DecelerationMetersPerSecondSquared,
- diagnosticChassis: chassis);
+ diagnosticChassis: chassis,
+ diagnosticStateProvider: stateProvider);
_recorder = recorder;
var controlPointRadiusMeters =
@@ -521,7 +522,7 @@ namespace MultiWheelC
/// 从当前Detour位姿开始执行“3m直线—左半圆—3m直线”新版控制器跟踪实验。
///
[MovementTest(name = "新版控制器：直线-左半圆-直线轨迹跟踪")]
- public sealed class NewControllerStraightSemicircleStraightTest
+ public class NewControllerStraightSemicircleStraightTest
: MovementTest
{
private const float MillimetersPerMeter = 1000f;
@@ -579,6 +580,24 @@ namespace MultiWheelC
///
public double PointSpacingMeters = 0.02;
+ ///
+ /// 获取组合轨迹主运动方向相对车头的夹角，单位为rad。
+ ///
+ protected virtual double MotionDirectionInBodyRadians =>
+ 0.0;
+
+ ///
+ /// 获取轨迹完成后是否需要将舵轮主动恢复到车头方向。
+ ///
+ protected virtual bool ReturnWheelsForwardAfterCompletion =>
+ false;
+
+ ///
+ /// 获取实验记录使用的轨迹基础名称。
+ ///
+ protected virtual string ExperimentTrajectoryBaseName =>
+ "ProfiledStraightSmoothLeftTurnStraight";
+
///
/// 读取当前位姿、绘制组合轨迹并启动新版轨迹跟踪动作。
///
@@ -637,7 +656,8 @@ namespace MultiWheelC
SemicircleMaximumSpeedMetersPerSecond,
AccelerationMetersPerSecondSquared,
DecelerationMetersPerSecondSquared,
- PointSpacingMeters);
+ PointSpacingMeters,
+ MotionDirectionInBodyRadians);
DrawTrajectory(trajectory);
@@ -650,7 +670,7 @@ namespace MultiWheelC
controllerName: "NewStanleyPid",
trajectoryName:
TrajectoryExperimentInput.BuildTrajectoryName(
- "ProfiledStraightSmoothLeftTurnStraight",
+ ExperimentTrajectoryBaseName,
lateralOffsetMeters),
trialNumber: TrialNumber,
referenceStart: referenceStart,
@@ -658,11 +678,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 =
@@ -673,6 +697,10 @@ namespace MultiWheelC
{
Trajectory = trajectory,
StateProvider = _stateProvider,
+ MotionDirectionInBodyRadians =
+ MotionDirectionInBodyRadians,
+ ReturnWheelsForwardAfterCompletion =
+ ReturnWheelsForwardAfterCompletion,
CycleObserver = controller =>
RecordControlCycle(
recorder,
@@ -894,4 +922,30 @@ namespace MultiWheelC
MillimetersPerMeter));
}
}
+
+ ///
+ /// 将舵轮准备到车体左前45°，跟踪直线—左半圆—直线轨迹，并在停车后恢复车头方向。
+ ///
+ [MovementTest(name = "新版控制器：45°蟹行直线-左半圆-直线轨迹跟踪")]
+ public sealed class NewControllerCrab45StraightSemicircleStraightTest
+ : NewControllerStraightSemicircleStraightTest
+ {
+ ///
+ /// 使用车体左前45°作为组合轨迹的固定运动方向。
+ ///
+ protected override double MotionDirectionInBodyRadians =>
+ Math.PI / 4.0;
+
+ ///
+ /// 蟹行组合轨迹正常完成后主动将四个舵轮恢复到车头方向。
+ ///
+ protected override bool ReturnWheelsForwardAfterCompletion =>
+ true;
+
+ ///
+ /// 将45°蟹行组合实验与普通组合轨迹实验的CSV名称明确区分。
+ ///
+ protected override string ExperimentTrajectoryBaseName =>
+ "ProfiledCrab45StraightSmoothLeftTurnStraight";
+ }
}
diff --git a/MultiWheelC/Experiments/RotationTests.cs b/MultiWheelC/Experiments/RotationTests.cs
index 41e9a4d..9197b5e 100644
--- a/MultiWheelC/Experiments/RotationTests.cs
+++ b/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
{
@@ -66,6 +68,26 @@ 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 =
@@ -98,7 +120,9 @@ namespace MultiWheelC
referenceSpeed: 0f,
referenceAngularSpeed:
(float)AngleMath.DegreesToRadians(
- config.InPlaceRotateMaxSpeed));
+ config.InPlaceRotateMaxSpeed),
+ diagnosticChassis: chassis,
+ diagnosticStateProvider: stateProvider);
_recorder.Start();
try
@@ -108,6 +132,8 @@ namespace MultiWheelC
{
// MultiWheelRotateInPlace接收世界坐标系绝对航向。
AngleTarget = targetWorldAngle,
+ Chassis = chassis,
+ StateProvider = stateProvider,
CommandAngularSpeedObserver =
commandAngularSpeed =>
_recorder?.UpdateCommand(
@@ -175,6 +201,26 @@ 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;
+ }
+
if (Math.Abs(relativeAngleDegrees) < 1e-3f)
{
Console.WriteLine("旋转角度不能为0，测试已经取消。");
diff --git a/MultiWheelC/Experiments/TestTrajectoryFactory.cs b/MultiWheelC/Experiments/TestTrajectoryFactory.cs
index 06c6f6d..7df722c 100644
--- a/MultiWheelC/Experiments/TestTrajectoryFactory.cs
+++ b/MultiWheelC/Experiments/TestTrajectoryFactory.cs
@@ -125,7 +125,7 @@ namespace MultiWheelC
}
///
- /// 从当前位姿按速度符号生成“3m直线、沿行进方向平滑左弯180°、3m直线”的轨迹。
+ /// 从当前位姿沿指定车体运动方向生成“3m直线、平滑左弯180°、3m直线”的轨迹。
///
public static Trajectory2D CreateStraightLeftSemicircleStraight(
Pose2D startPoseInWorld,
@@ -136,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,
@@ -148,11 +149,12 @@ namespace MultiWheelC
semicircleMaximumSpeedMetersPerSecond,
accelerationMetersPerSecondSquared,
decelerationMetersPerSecondSquared,
- pointSpacingMeters);
+ pointSpacingMeters,
+ motionDirectionInBodyRadians);
}
///
- /// 按共同速度符号生成“直线、沿行进方向平滑左弯、直线”轨迹，并使总转角严格等于指定角度。
+ /// 沿指定车体运动方向生成“直线、平滑左弯、直线”轨迹，并使总转角严格等于指定角度。
///
public static Trajectory2D CreateStraightSmoothLeftTurnStraight(
Pose2D startPoseInWorld,
@@ -164,7 +166,8 @@ namespace MultiWheelC
double turnMaximumSpeedMetersPerSecond,
double accelerationMetersPerSecondSquared,
double decelerationMetersPerSecondSquared,
- double pointSpacingMeters)
+ double pointSpacingMeters,
+ double motionDirectionInBodyRadians = 0.0)
{
NumericGuard.EnsureFinite(
startPoseInWorld,
@@ -195,6 +198,9 @@ namespace MultiWheelC
NumericGuard.EnsureFinitePositive(
pointSpacingMeters,
nameof(pointSpacingMeters));
+ NumericGuard.EnsureFinite(
+ motionDirectionInBodyRadians,
+ nameof(motionDirectionInBodyRadians));
if (turnAngleRadians > 2.0 * Math.PI)
{
@@ -276,10 +282,13 @@ namespace MultiWheelC
var points = new List(
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;
diff --git a/MultiWheelC/Experiments/TrackingExperimentRecorder.cs b/MultiWheelC/Experiments/TrackingExperimentRecorder.cs
index b04bac1..dbd23d3 100644
--- a/MultiWheelC/Experiments/TrackingExperimentRecorder.cs
+++ b/MultiWheelC/Experiments/TrackingExperimentRecorder.cs
@@ -24,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;
@@ -62,6 +83,10 @@ namespace MultiWheelC
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;
@@ -130,6 +155,8 @@ namespace MultiWheelC
private readonly float _referenceDecelerationMetersPerSecondSquared;
private readonly int _sampleIntervalMs;
private readonly MultiWheelChassis _diagnosticChassis;
+ private readonly WheelFeedbackVehicleStateProvider
+ _diagnosticStateProvider;
private readonly List _samples =
new List();
@@ -203,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(
@@ -235,6 +264,7 @@ namespace MultiWheelC
referenceDecelerationMetersPerSecondSquared;
_sampleIntervalMs = sampleIntervalMs;
_diagnosticChassis = diagnosticChassis;
+ _diagnosticStateProvider = diagnosticStateProvider;
}
// 保存成功后的CSV绝对路径；尚未保存时为空。
@@ -534,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;
@@ -556,6 +627,10 @@ namespace MultiWheelC
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;
@@ -655,6 +730,46 @@ namespace MultiWheelC
_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 =
@@ -662,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,
@@ -703,6 +857,14 @@ namespace MultiWheelC
wheelFeedbackFilteredBodyVyMetersPerSecond,
WheelFeedbackVelocityEstimateValid =
wheelFeedbackVelocityEstimateValid,
+ HasWheelFeedbackAngularVelocityDiagnostics =
+ hasWheelFeedbackAngularVelocityDiagnostics,
+ WheelFeedbackRawBodyOmegaRadiansPerSecond =
+ wheelFeedbackRawBodyOmegaRadiansPerSecond,
+ WheelFeedbackFilteredBodyOmegaRadiansPerSecond =
+ wheelFeedbackFilteredBodyOmegaRadiansPerSecond,
+ WheelFeedbackSampleTimestampSeconds =
+ wheelFeedbackSampleTimestampSeconds,
HasGcpCommand = hasGcpCommand,
RequestedFrontGcpAngleRadians =
requestedFrontGcpAngleRadians,
@@ -885,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," +
@@ -928,6 +1109,10 @@ namespace MultiWheelC
"WheelFeedbackRawBodyVyMetersPerSecond," +
"WheelFeedbackFilteredBodyVyMetersPerSecond," +
"WheelFeedbackVelocityEstimateValid," +
+ "HasWheelFeedbackAngularVelocityDiagnostics," +
+ "WheelFeedbackRawBodyOmegaRadiansPerSecond," +
+ "WheelFeedbackFilteredBodyOmegaRadiansPerSecond," +
+ "WheelFeedbackSampleTimestampSeconds," +
"HasSteeringDiagnostics," +
"TargetSteerLeftFrontDegrees," +
"TargetSteerLeftRearDegrees," +
@@ -959,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(
@@ -1067,6 +1319,21 @@ namespace MultiWheelC
? "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",
diff --git a/MultiWheelC/Movements/RotateInPlace.cs b/MultiWheelC/Movements/RotateInPlace.cs
index c9b221c..4386b77 100644
--- a/MultiWheelC/Movements/RotateInPlace.cs
+++ b/MultiWheelC/Movements/RotateInPlace.cs
@@ -200,6 +200,8 @@ namespace MultiWheelC
adapter
.StopXYThDrivePreserveSteeringState();
+ // 到位稳定只依赖已经单独校验的航向。
+ // 位置候选留到停车后处理，避免位置抖动中断航向闭环。
if (thPid.IsArrived())
break;
@@ -253,6 +255,40 @@ namespace MultiWheelC
CommandAngularSpeedObserver?.Invoke(0f);
adapter.StopXYThDrivePreserveSteeringState();
+ if (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();
@@ -356,6 +392,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))
{
@@ -369,6 +435,26 @@ namespace MultiWheelC
state.PoseInWorld.YawRadians);
}
+ ///
+ /// 通知配置化状态源：车辆已经停车，可以重新确认旋转期间的位置候选。
+ ///
+ private static void BeginPostRotationPositionRecovery(
+ IVehicleStateProvider stateProvider)
+ {
+ if (stateProvider is
+ WheelFeedbackVehicleStateProvider wheelProvider)
+ {
+ wheelProvider.BeginPostRotationPositionRecovery();
+ return;
+ }
+
+ if (stateProvider is
+ DetourVehicleStateProvider detourProvider)
+ {
+ detourProvider.BeginPostRotationPositionRecovery();
+ }
+ }
+
///
/// 获取已知停车状态源最近一次失败原因，未知实现返回空字符串。
///
@@ -390,6 +476,31 @@ namespace MultiWheelC
return string.Empty;
}
+ ///
+ /// 判断Detour是否仍在使用轮组预测确认疑似位姿不连续。
+ ///
+ 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;
+ }
+
///
/// 检查原地自转参数是否为正有限值，部分时间和容差参数允许为零。
///
diff --git a/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs b/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs
index eecab94..c1fbcb0 100644
--- a/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs
+++ b/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs
@@ -1,16 +1,34 @@
using System;
using System.Diagnostics;
+using System.Globalization;
using ClumsyCore.Interfaces;
using MyParking.Shared;
namespace MultiWheelC.StateEstimation
{
///
- /// 读取Detour位姿，忽略重复或明显异常的观测，并估算车辆二维速度。
+ /// 读取Detour位姿，并将可能发生重定位的原始世界坐标转换为当前任务使用的连续坐标。
///
public sealed class DetourVehicleStateProvider
: IVehicleStateProvider
{
+ private readonly struct DetourObservation
+ {
+ public DetourObservation(
+ Pose2D poseInDetour,
+ long tickRaw,
+ double localizationStep)
+ {
+ PoseInDetour = poseInDetour;
+ TickRaw = tickRaw;
+ LocalizationStep = localizationStep;
+ }
+
+ public Pose2D PoseInDetour { get; }
+ public long TickRaw { get; }
+ public double LocalizationStep { get; }
+ }
+
public const double DefaultMaximumLinearSpeedMetersPerSecond =
1.20;
public const double DefaultMaximumAngularSpeedRadiansPerSecond =
@@ -25,10 +43,27 @@ namespace MultiWheelC.StateEstimation
5.0 * Math.PI / 180.0;
public const double DefaultStationaryConfirmationSeconds =
0.35;
+ public const int DefaultHeadingOutlierConfirmationFrameCount =
+ 3;
+ public const double
+ DefaultHeadingOutlierPredictionTimeoutSeconds = 0.30;
+ public const int DefaultJumpConfirmationFrameCount = 3;
+ public const double DefaultJumpConfirmationTimeoutSeconds =
+ 0.60;
+ public const double DefaultMaximumAutomaticFrameShiftMeters =
+ 0.15;
+ public const double
+ DefaultMaximumAutomaticHeadingShiftRadians =
+ 5.0 * Math.PI / 180.0;
private const double MillimetersPerMeter = 1000.0;
private const double PositionEqualityToleranceMeters = 1e-9;
private const double HeadingEqualityToleranceRadians = 1e-8;
+ private const double
+ InPlaceRotationLinearSpeedThresholdMetersPerSecond = 0.03;
+ private const double
+ InPlaceRotationAngularSpeedThresholdRadiansPerSecond =
+ Math.PI / 180.0;
private readonly object _syncRoot = new object();
private readonly Stopwatch _clock = Stopwatch.StartNew();
@@ -40,15 +75,59 @@ namespace MultiWheelC.StateEstimation
private readonly double _velocityPositionResidualMeters;
private readonly double _velocityHeadingResidualRadians;
private readonly double _stationaryConfirmationSeconds;
+ private readonly int _headingOutlierConfirmationFrameCount;
+ private readonly double
+ _headingOutlierPredictionTimeoutSeconds;
+ private readonly int _jumpConfirmationFrameCount;
+ private readonly double _jumpConfirmationTimeoutSeconds;
+ private readonly double _maximumAutomaticFrameShiftMeters;
+ private readonly double
+ _maximumAutomaticHeadingShiftRadians;
- private bool _hasAcceptedPose;
- private Pose2D _acceptedPoseInWorld;
+ private Pose2D _acceptedPoseInControl;
private double _acceptedTimestampSeconds;
private VehicleState _latestState;
private bool _stationaryHoldActive;
+ private bool _hasObservedDetourFrame;
+ private Pose2D _lastObservedPoseInDetour;
+ private long _lastObservedTickRaw;
+ private bool _hasDetourSourceFrameInterval;
+ private double _lastDetourSourceFrameIntervalSeconds;
+ private Pose2D _controlFromDetour = Pose2D.Identity;
+
+ private bool _hasMotionPrediction;
+ private Pose2D _predictedPoseInControl;
+ private double _predictionTimestampSeconds;
+ private Twist2D _latestWheelTwistInBody = Twist2D.Zero;
+ private bool _latestWheelVelocityValid;
+
+ private bool _jumpCandidateActive;
+ private Pose2D _candidateControlFromDetour;
+ private int _candidateConsistentFrameCount;
+ private double _candidateStartedTimestampSeconds;
+ private double _candidateShiftDistanceMeters;
+ private double _candidateShiftHeadingRadians;
+ private bool _candidateAutomaticShiftForbidden;
+ private bool _candidateEligibleForAutomaticShift;
+ private bool _postRotationPositionRecoveryActive;
+ private string _lastJumpCandidateTriggerReason = string.Empty;
+
+ private bool _hasInnovationDiagnostics;
+ private double _lastPositionInnovationMeters;
+ private double _lastAllowedPositionInnovationMeters;
+ private double _lastHeadingInnovationRadians;
+ private double _lastAllowedHeadingInnovationRadians;
+
+ private bool _hasReliableHeadingObservation;
+ private double _latestReliableHeadingInDetourRadians;
+ private bool _headingOutlierCandidateActive;
+ private int _headingOutlierConsecutiveFrameCount;
+ private double _headingOutlierStartedTimestampSeconds;
+ private string _headingOutlierCandidateReason = string.Empty;
+
///
- /// 创建使用停车机器人默认物理边界和速度滤波参数的Detour状态源。
+ /// 创建使用停车机器人默认边界和坐标连续化参数的Detour状态源。
///
public DetourVehicleStateProvider()
: this(
@@ -59,12 +138,19 @@ namespace MultiWheelC.StateEstimation
DefaultHeadingJumpMarginRadians,
DefaultVelocityPositionResidualMeters,
DefaultVelocityHeadingResidualRadians,
- DefaultStationaryConfirmationSeconds)
+ DefaultStationaryConfirmationSeconds,
+ DefaultHeadingOutlierConfirmationFrameCount,
+ DefaultHeadingOutlierPredictionTimeoutSeconds,
+ DefaultJumpConfirmationFrameCount,
+ DefaultJumpConfirmationTimeoutSeconds,
+ DefaultMaximumAutomaticFrameShiftMeters,
+ DefaultMaximumAutomaticHeadingShiftRadians)
{
}
///
- /// 创建使用指定物理边界、静止确认时间和速度估计器的Detour状态源。
+ /// 创建使用指定物理边界和既有跳变检测参数的Detour状态源，
+ /// 坐标连续化参数使用停车机器人默认值。
///
public DetourVehicleStateProvider(
VelocityEstimator2D velocityEstimator,
@@ -75,6 +161,42 @@ namespace MultiWheelC.StateEstimation
double velocityPositionResidualMeters,
double velocityHeadingResidualRadians,
double stationaryConfirmationSeconds)
+ : this(
+ velocityEstimator,
+ maximumLinearSpeedMetersPerSecond,
+ maximumAngularSpeedRadiansPerSecond,
+ positionJumpMarginMeters,
+ headingJumpMarginRadians,
+ velocityPositionResidualMeters,
+ velocityHeadingResidualRadians,
+ stationaryConfirmationSeconds,
+ DefaultHeadingOutlierConfirmationFrameCount,
+ DefaultHeadingOutlierPredictionTimeoutSeconds,
+ DefaultJumpConfirmationFrameCount,
+ DefaultJumpConfirmationTimeoutSeconds,
+ DefaultMaximumAutomaticFrameShiftMeters,
+ DefaultMaximumAutomaticHeadingShiftRadians)
+ {
+ }
+
+ ///
+ /// 创建使用指定物理边界、候选确认和自动坐标连续化参数的Detour状态源。
+ ///
+ public DetourVehicleStateProvider(
+ VelocityEstimator2D velocityEstimator,
+ double maximumLinearSpeedMetersPerSecond,
+ double maximumAngularSpeedRadiansPerSecond,
+ double positionJumpMarginMeters,
+ double headingJumpMarginRadians,
+ double velocityPositionResidualMeters,
+ double velocityHeadingResidualRadians,
+ double stationaryConfirmationSeconds,
+ int headingOutlierConfirmationFrameCount,
+ double headingOutlierPredictionTimeoutSeconds,
+ int jumpConfirmationFrameCount,
+ double jumpConfirmationTimeoutSeconds,
+ double maximumAutomaticFrameShiftMeters,
+ double maximumAutomaticHeadingShiftRadians)
{
_velocityEstimator = velocityEstimator ??
throw new ArgumentNullException(
@@ -101,30 +223,203 @@ namespace MultiWheelC.StateEstimation
NumericGuard.EnsureFinitePositive(
stationaryConfirmationSeconds,
nameof(stationaryConfirmationSeconds));
+ NumericGuard.EnsureFinitePositive(
+ headingOutlierPredictionTimeoutSeconds,
+ nameof(headingOutlierPredictionTimeoutSeconds));
+ NumericGuard.EnsureFinitePositive(
+ jumpConfirmationTimeoutSeconds,
+ nameof(jumpConfirmationTimeoutSeconds));
+ NumericGuard.EnsureFinitePositive(
+ maximumAutomaticFrameShiftMeters,
+ nameof(maximumAutomaticFrameShiftMeters));
+ NumericGuard.EnsureFinitePositive(
+ maximumAutomaticHeadingShiftRadians,
+ nameof(maximumAutomaticHeadingShiftRadians));
+
+ if (jumpConfirmationFrameCount < 2)
+ {
+ throw new ArgumentOutOfRangeException(
+ nameof(jumpConfirmationFrameCount),
+ "Detour跳变至少需要两个新帧确认。");
+ }
+
+ if (headingOutlierConfirmationFrameCount < 2)
+ {
+ throw new ArgumentOutOfRangeException(
+ nameof(headingOutlierConfirmationFrameCount),
+ "Detour航向异常至少需要两个新帧确认。");
+ }
_maximumLinearSpeedMetersPerSecond =
maximumLinearSpeedMetersPerSecond;
_maximumAngularSpeedRadiansPerSecond =
maximumAngularSpeedRadiansPerSecond;
- _positionJumpMarginMeters =
- positionJumpMarginMeters;
- _headingJumpMarginRadians =
- headingJumpMarginRadians;
+ _positionJumpMarginMeters = positionJumpMarginMeters;
+ _headingJumpMarginRadians = headingJumpMarginRadians;
_velocityPositionResidualMeters =
velocityPositionResidualMeters;
_velocityHeadingResidualRadians =
velocityHeadingResidualRadians;
_stationaryConfirmationSeconds =
stationaryConfirmationSeconds;
+ _headingOutlierConfirmationFrameCount =
+ headingOutlierConfirmationFrameCount;
+ _headingOutlierPredictionTimeoutSeconds =
+ headingOutlierPredictionTimeoutSeconds;
+ _jumpConfirmationFrameCount =
+ jumpConfirmationFrameCount;
+ _jumpConfirmationTimeoutSeconds =
+ jumpConfirmationTimeoutSeconds;
+ _maximumAutomaticFrameShiftMeters =
+ maximumAutomaticFrameShiftMeters;
+ _maximumAutomaticHeadingShiftRadians =
+ maximumAutomaticHeadingShiftRadians;
}
///
- /// 获取最近一次读取失败或异常观测被忽略的原因，正常时为空字符串。
+ /// 获取最近一次状态不可用或正在使用短时预测的原因。
///
- public string LastFailureReason { get; private set; } = "";
+ public string LastFailureReason { get; private set; } =
+ string.Empty;
///
- /// 尝试读取Detour；重复帧保留最近状态，明显异常帧只忽略本次观测。
+ /// 获取最近一次航向读取失败的原因；位置单独异常时保持为空。
+ ///
+ public string LastHeadingFailureReason { get; private set; } =
+ string.Empty;
+
+ ///
+ /// 获取最近一次读取到的Detour源时间戳原值。
+ ///
+ public long? LastDetourTickRaw { get; private set; }
+
+ ///
+ /// 获取最近一次Detour l_step原值；其精确定义仍由Detour接口文档确认。
+ ///
+ public double? LastDetourLocalizationStep { get; private set; }
+
+ ///
+ /// 获取当前是否正在确认疑似Detour坐标跳变。
+ ///
+ public bool IsJumpCandidateActive => _jumpCandidateActive;
+
+ ///
+ /// 获取本状态源生命周期内已自动连续化的Detour坐标偏移次数。
+ ///
+ public int AutomaticFrameShiftCount { get; private set; }
+
+ ///
+ /// 获取最近一次Detour读取对应的跳变确认和数据新鲜度诊断。
+ ///
+ internal bool TryGetLatestDiagnostics(
+ out bool jumpCandidateActive,
+ out int jumpCandidateConsistentFrameCount,
+ out double estimatedShiftDistanceMeters,
+ out double estimatedShiftHeadingRadians,
+ out int automaticFrameShiftCount,
+ out string stateStatusReason,
+ out double detourDataAgeMilliseconds)
+ {
+ lock (_syncRoot)
+ {
+ jumpCandidateActive = _jumpCandidateActive;
+ jumpCandidateConsistentFrameCount =
+ _candidateConsistentFrameCount;
+ estimatedShiftDistanceMeters =
+ _candidateShiftDistanceMeters;
+ estimatedShiftHeadingRadians =
+ _candidateShiftHeadingRadians;
+ automaticFrameShiftCount =
+ AutomaticFrameShiftCount;
+ stateStatusReason =
+ GetDiagnosticStateStatusReason();
+
+ if (!LastDetourTickRaw.HasValue)
+ {
+ detourDataAgeMilliseconds = 0.0;
+ return false;
+ }
+
+ detourDataAgeMilliseconds =
+ (DateTime.Now.Ticks -
+ LastDetourTickRaw.Value) /
+ (double)TimeSpan.TicksPerMillisecond;
+ return true;
+ }
+ }
+
+ ///
+ /// 获取最近一次Detour源帧、轮速预测、创新门限、候选原因和状态诊断。
+ ///
+ internal bool TryGetLatestDiagnostics(
+ 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)
+ {
+ jumpCandidateActive = _jumpCandidateActive;
+ jumpCandidateConsistentFrameCount =
+ _candidateConsistentFrameCount;
+ estimatedShiftDistanceMeters =
+ _candidateShiftDistanceMeters;
+ estimatedShiftHeadingRadians =
+ _candidateShiftHeadingRadians;
+ automaticFrameShiftCount =
+ AutomaticFrameShiftCount;
+ sourceFrameIntervalSeconds =
+ _hasDetourSourceFrameInterval
+ ? _lastDetourSourceFrameIntervalSeconds
+ : 0.0;
+ motionPredictionTimestampSeconds =
+ _hasMotionPrediction
+ ? _predictionTimestampSeconds
+ : 0.0;
+ hasInnovationDiagnostics =
+ _hasInnovationDiagnostics;
+ positionInnovationMeters =
+ _lastPositionInnovationMeters;
+ allowedPositionInnovationMeters =
+ _lastAllowedPositionInnovationMeters;
+ headingInnovationRadians =
+ _lastHeadingInnovationRadians;
+ allowedHeadingInnovationRadians =
+ _lastAllowedHeadingInnovationRadians;
+ jumpCandidateTriggerReason =
+ _lastJumpCandidateTriggerReason;
+ stateStatus = GetDiagnosticStateStatus();
+ stateStatusReason =
+ GetDiagnosticStateStatusReason();
+
+ if (!LastDetourTickRaw.HasValue)
+ {
+ detourDataAgeMilliseconds = 0.0;
+ return false;
+ }
+
+ detourDataAgeMilliseconds =
+ (DateTime.Now.Ticks -
+ LastDetourTickRaw.Value) /
+ (double)TimeSpan.TicksPerMillisecond;
+ return true;
+ }
+ }
+
+ ///
+ /// 尝试读取Detour，并输出当前任务坐标系中的连续车辆状态。
///
public bool TryGetState(out VehicleState state)
{
@@ -132,78 +427,155 @@ namespace MultiWheelC.StateEstimation
{
try
{
- var poseInWorld =
- ReadDetourPoseInWorld();
+ var observation = ReadDetourObservation();
var timestampSeconds =
_clock.Elapsed.TotalSeconds;
+ AdvanceMotionPrediction(timestampSeconds);
- if (!_hasAcceptedPose)
+ LastDetourTickRaw = observation.TickRaw;
+ LastDetourLocalizationStep =
+ observation.LocalizationStep;
+
+ if (!_hasObservedDetourFrame)
{
+ SetLastObservedDetourFrame(observation);
+ AcceptReliableHeadingObservation(observation);
state = AcceptPoseAfterReset(
- poseInWorld,
+ observation.PoseInDetour,
timestampSeconds);
- LastFailureReason = "";
+ LastFailureReason = string.Empty;
return true;
}
+ if (observation.TickRaw < _lastObservedTickRaw)
+ {
+ InvalidateHeadingObservation(
+ "Detour源时间戳发生倒退，航向暂不可用。");
+ state = default;
+ LastFailureReason =
+ "Detour源时间戳发生倒退，车辆状态暂不可用。";
+ return false;
+ }
+
+ if (observation.TickRaw == _lastObservedTickRaw)
+ {
+ return HandleRepeatedDetourFrame(
+ timestampSeconds,
+ out state);
+ }
+
+ var sourceDeltaTimeSeconds =
+ (observation.TickRaw -
+ _lastObservedTickRaw) /
+ (double)TimeSpan.TicksPerSecond;
+ var previousPoseInDetour =
+ _lastObservedPoseInDetour;
+ SetLastObservedDetourFrame(observation);
+
+ if (!NumericGuard.IsFinite(
+ sourceDeltaTimeSeconds) ||
+ sourceDeltaTimeSeconds <= 0.0)
+ {
+ InvalidateHeadingObservation(
+ "Detour新帧时间间隔无效，航向暂不可用。");
+ state = default;
+ LastFailureReason =
+ "Detour新帧时间间隔无效，车辆状态暂不可用。";
+ return false;
+ }
+
+ _lastDetourSourceFrameIntervalSeconds =
+ sourceDeltaTimeSeconds;
+ _hasDetourSourceFrameInterval = true;
+
+ var poseInControl =
+ FrameTransform2D.TransformPose(
+ _controlFromDetour,
+ observation.PoseInDetour);
+ var predictedPoseInControl =
+ GetPredictedPoseInControl();
+
+ var sourceHeadingPlausible =
+ IsHeadingMotionPlausible(
+ previousPoseInDetour,
+ observation.PoseInDetour,
+ sourceDeltaTimeSeconds);
+
+ if (_jumpCandidateActive)
+ {
+ IsPoseInnovationAbnormal(
+ poseInControl,
+ predictedPoseInControl,
+ sourceDeltaTimeSeconds);
+ UpdateHeadingObservation(
+ observation,
+ sourceHeadingPlausible,
+ timestampSeconds);
+ return HandleJumpCandidate(
+ observation,
+ poseInControl,
+ predictedPoseInControl,
+ sourceDeltaTimeSeconds,
+ timestampSeconds,
+ out state);
+ }
+
+ var sourceMotionPlausible =
+ IsMotionPlausible(
+ previousPoseInDetour,
+ observation.PoseInDetour,
+ sourceDeltaTimeSeconds);
+ var innovationAbnormal =
+ IsPoseInnovationAbnormal(
+ poseInControl,
+ predictedPoseInControl,
+ sourceDeltaTimeSeconds);
+ UpdateHeadingObservation(
+ observation,
+ sourceHeadingPlausible,
+ timestampSeconds);
+
+ if (!sourceMotionPlausible ||
+ innovationAbnormal)
+ {
+ BeginJumpCandidate(
+ observation.PoseInDetour,
+ poseInControl,
+ predictedPoseInControl,
+ timestampSeconds,
+ BuildJumpCandidateTriggerReason(
+ sourceMotionPlausible));
+
+ return ReturnCandidateState(
+ timestampSeconds,
+ out state);
+ }
+
if (ArePosesEquivalent(
- poseInWorld,
- _acceptedPoseInWorld))
+ poseInControl,
+ _acceptedPoseInControl))
{
state = HandleRepeatedPose(
timestampSeconds);
- LastFailureReason = "";
+ LastFailureReason = string.Empty;
return true;
}
- // 静止保持后出现新定位时重新建立差分基准，
- // 避免用很长的静止时间稀释第一次运动速度。
- if (_stationaryHoldActive)
- {
- state = AcceptPoseAfterReset(
- poseInWorld,
+ state = _stationaryHoldActive
+ ? AcceptPoseAfterReset(
+ poseInControl,
+ timestampSeconds)
+ : AcceptContinuousPose(
+ poseInControl,
timestampSeconds);
- LastFailureReason = "";
- return true;
- }
-
- var elapsedSeconds =
- timestampSeconds -
- _acceptedTimestampSeconds;
-
- if (!IsMotionPlausible(
- _acceptedPoseInWorld,
- poseInWorld,
- elapsedSeconds))
- {
- // 单帧异常不进入差分器，也不中断调用方；下一次
- // 正常观测仍相对最近有效位姿和真实时间差计算。
- state = _latestState;
- LastFailureReason =
- "Detour位姿变化超过车辆绝对运动边界，本次观测已忽略。";
- return true;
- }
-
- if (IsVelocityInnovationAbnormal(
- poseInWorld,
- elapsedSeconds))
- {
- state = AcceptPoseAfterReset(
- poseInWorld,
- timestampSeconds);
- LastFailureReason =
- "Detour位姿偏离速度预测，已重新建立速度估计基准。";
- return true;
- }
-
- state = AcceptContinuousPose(
- poseInWorld,
- timestampSeconds);
- LastFailureReason = "";
+ LastFailureReason = string.Empty;
return true;
}
catch (Exception exception)
{
+ InvalidateHeadingObservation(
+ "Detour航向读取失败：" +
+ exception.Message);
state = default;
LastFailureReason =
"Detour车辆状态读取失败：" +
@@ -214,82 +586,497 @@ namespace MultiWheelC.StateEstimation
}
///
- /// 清除Detour位姿历史和速度估计状态。
+ /// 尝试读取经过独立物理边界和创新校验的任务坐标系航向。
+ /// 位置单独异常时仍可成功，供原地自转等只依赖航向的动作使用。
+ ///
+ public bool TryGetHeadingRadians(
+ out double headingRadians)
+ {
+ lock (_syncRoot)
+ {
+ TryGetState(out _);
+ return TryGetLatestReliableHeadingRadians(
+ out headingRadians);
+ }
+ }
+
+ internal bool TryGetLatestReliableHeadingRadians(
+ out double headingRadians)
+ {
+ lock (_syncRoot)
+ {
+ if (_headingOutlierCandidateActive)
+ {
+ var candidateAgeSeconds =
+ _clock.Elapsed.TotalSeconds -
+ _headingOutlierStartedTimestampSeconds;
+ if (candidateAgeSeconds >=
+ _headingOutlierPredictionTimeoutSeconds)
+ {
+ InvalidateHeadingObservation(
+ "Detour航向异常短时预测超过" +
+ _headingOutlierPredictionTimeoutSeconds
+ .ToString("F2", CultureInfo.InvariantCulture) +
+ "s，航向暂不可用。");
+ }
+ else if (_hasReliableHeadingObservation &&
+ _latestWheelVelocityValid &&
+ _hasMotionPrediction)
+ {
+ headingRadians =
+ GetPredictedPoseInControl()
+ .YawRadians;
+ LastHeadingFailureReason = string.Empty;
+ return true;
+ }
+ else
+ {
+ InvalidateHeadingObservation(
+ "Detour航向异常期间缺少有效轮组角速度预测，航向暂不可用。");
+ }
+ }
+
+ if (!_hasReliableHeadingObservation)
+ {
+ headingRadians = 0.0;
+ if (string.IsNullOrWhiteSpace(
+ LastHeadingFailureReason))
+ {
+ LastHeadingFailureReason =
+ string.IsNullOrWhiteSpace(
+ LastFailureReason)
+ ? "Detour当前没有可靠航向观测。"
+ : LastFailureReason;
+ }
+
+ return false;
+ }
+
+ headingRadians = AngleMath.NormalizeRadians(
+ _controlFromDetour.YawRadians +
+ _latestReliableHeadingInDetourRadians);
+ LastHeadingFailureReason = string.Empty;
+ return true;
+ }
+ }
+
+ ///
+ /// 原地自转已经停车后，允许现有候选按普通有限偏移规则重新确认。
+ /// 自转期间不会调用该入口，因此不会在旋转中改写任务坐标系。
+ ///
+ public void BeginPostRotationPositionRecovery()
+ {
+ lock (_syncRoot)
+ {
+ if (!_jumpCandidateActive)
+ {
+ return;
+ }
+
+ _candidateAutomaticShiftForbidden = false;
+ _postRotationPositionRecoveryActive = true;
+ _candidateEligibleForAutomaticShift =
+ _candidateShiftDistanceMeters <=
+ _maximumAutomaticFrameShiftMeters &&
+ _candidateShiftHeadingRadians <=
+ _maximumAutomaticHeadingShiftRadians;
+ _candidateConsistentFrameCount = 0;
+ _candidateStartedTimestampSeconds =
+ _clock.Elapsed.TotalSeconds;
+ _lastJumpCandidateTriggerReason =
+ AppendDiagnosticToken(
+ _lastJumpCandidateTriggerReason,
+ "PostRotationPositionRecovery");
+ }
+ }
+
+ ///
+ /// 由轮组状态源在读取Detour前提供最新滤波车体速度和角速度，用于短时间位姿预测。
+ ///
+ internal void UpdateWheelVelocityEstimate(
+ double bodyVxMetersPerSecond,
+ double bodyVyMetersPerSecond,
+ double bodyOmegaRadiansPerSecond,
+ bool velocityEstimateValid)
+ {
+ NumericGuard.EnsureFinite(
+ bodyVxMetersPerSecond,
+ nameof(bodyVxMetersPerSecond));
+ NumericGuard.EnsureFinite(
+ bodyVyMetersPerSecond,
+ nameof(bodyVyMetersPerSecond));
+ NumericGuard.EnsureFinite(
+ bodyOmegaRadiansPerSecond,
+ nameof(bodyOmegaRadiansPerSecond));
+
+ lock (_syncRoot)
+ {
+ var timestampSeconds =
+ _clock.Elapsed.TotalSeconds;
+ AdvanceMotionPrediction(timestampSeconds);
+
+ _latestWheelTwistInBody = new Twist2D(
+ bodyVxMetersPerSecond,
+ bodyVyMetersPerSecond,
+ bodyOmegaRadiansPerSecond);
+ _latestWheelVelocityValid =
+ velocityEstimateValid;
+ }
+ }
+
+ ///
+ /// 清除Detour历史、任务坐标偏移、候选跳变和速度预测状态。
///
public void Reset()
{
lock (_syncRoot)
{
_velocityEstimator.Reset();
- _hasAcceptedPose = false;
- _acceptedPoseInWorld = Pose2D.Identity;
+ _acceptedPoseInControl = Pose2D.Identity;
_acceptedTimestampSeconds = 0.0;
_latestState = default;
_stationaryHoldActive = false;
- LastFailureReason = "";
+
+ _hasObservedDetourFrame = false;
+ _lastObservedPoseInDetour = Pose2D.Identity;
+ _lastObservedTickRaw = 0L;
+ _hasDetourSourceFrameInterval = false;
+ _lastDetourSourceFrameIntervalSeconds = 0.0;
+ _controlFromDetour = Pose2D.Identity;
+
+ _hasMotionPrediction = false;
+ _predictedPoseInControl = Pose2D.Identity;
+ _predictionTimestampSeconds = 0.0;
+ _latestWheelTwistInBody = Twist2D.Zero;
+ _latestWheelVelocityValid = false;
+
+ _hasInnovationDiagnostics = false;
+ _lastPositionInnovationMeters = 0.0;
+ _lastAllowedPositionInnovationMeters = 0.0;
+ _lastHeadingInnovationRadians = 0.0;
+ _lastAllowedHeadingInnovationRadians = 0.0;
+ _lastJumpCandidateTriggerReason = string.Empty;
+
+ _hasReliableHeadingObservation = false;
+ _latestReliableHeadingInDetourRadians = 0.0;
+ ClearHeadingOutlierCandidate();
+
+ ClearJumpCandidate();
+ AutomaticFrameShiftCount = 0;
+ LastDetourTickRaw = null;
+ LastDetourLocalizationStep = null;
+ LastFailureReason = string.Empty;
+ LastHeadingFailureReason = string.Empty;
}
}
- ///
- /// 读取Detour毫米和角度数据并转换为世界坐标SI位姿。
- ///
- private static Pose2D ReadDetourPoseInWorld()
+ private bool HandleRepeatedDetourFrame(
+ double timestampSeconds,
+ out VehicleState state)
{
- var location =
- DetourInterface.getCartLocation();
+ if (_jumpCandidateActive)
+ {
+ return ReturnCandidateState(
+ timestampSeconds,
+ out state);
+ }
- NumericGuard.EnsureFinite(location.x, "DetourX");
- NumericGuard.EnsureFinite(location.y, "DetourY");
- NumericGuard.EnsureFinite(location.th, "DetourTheta");
+ state = HandleRepeatedPose(timestampSeconds);
+ LastFailureReason = string.Empty;
+ return true;
+ }
- return new Pose2D(
- location.x / MillimetersPerMeter,
- location.y / MillimetersPerMeter,
- AngleMath.NormalizeRadians(
- AngleMath.DegreesToRadians(
- location.th)));
+ private bool HandleJumpCandidate(
+ DetourObservation observation,
+ Pose2D poseUsingCurrentTransform,
+ Pose2D predictedPoseInControl,
+ double sourceDeltaTimeSeconds,
+ double timestampSeconds,
+ out VehicleState state)
+ {
+ if (!IsPoseInnovationAbnormal(
+ poseUsingCurrentTransform,
+ predictedPoseInControl,
+ sourceDeltaTimeSeconds))
+ {
+ ClearJumpCandidate();
+ state = AcceptPoseAfterReset(
+ poseUsingCurrentTransform,
+ timestampSeconds);
+ LastFailureReason = string.Empty;
+ return true;
+ }
+
+ if (timestampSeconds -
+ _candidateStartedTimestampSeconds >
+ _jumpConfirmationTimeoutSeconds)
+ {
+ return ReturnCandidateState(
+ timestampSeconds,
+ out state);
+ }
+
+ var poseUsingCandidateTransform =
+ FrameTransform2D.TransformPose(
+ _candidateControlFromDetour,
+ observation.PoseInDetour);
+ var candidateIsConsistent =
+ !IsPoseInnovationAbnormal(
+ poseUsingCandidateTransform,
+ predictedPoseInControl,
+ sourceDeltaTimeSeconds,
+ recordDiagnostics: false);
+
+ if (candidateIsConsistent)
+ {
+ _candidateConsistentFrameCount++;
+
+ if (_candidateEligibleForAutomaticShift &&
+ _candidateConsistentFrameCount >=
+ _jumpConfirmationFrameCount)
+ {
+ _controlFromDetour =
+ FrameTransform2D.Compose(
+ predictedPoseInControl,
+ FrameTransform2D.Inverse(
+ observation.PoseInDetour));
+ ClearJumpCandidate();
+ AutomaticFrameShiftCount++;
+ state = AcceptPoseAfterReset(
+ predictedPoseInControl,
+ timestampSeconds);
+ LastFailureReason = string.Empty;
+ return true;
+ }
+ }
+ else
+ {
+ // 首个异常帧可能只是离群值。后续异常帧若属于另一稳定分支，
+ // 以新分支重新累计连续帧，但保留总确认超时起点。
+ var candidateStartedTimestampSeconds =
+ _candidateStartedTimestampSeconds;
+ BeginJumpCandidate(
+ observation.PoseInDetour,
+ poseUsingCurrentTransform,
+ predictedPoseInControl,
+ timestampSeconds,
+ AppendDiagnosticToken(
+ BuildJumpCandidateTriggerReason(
+ sourceMotionPlausible: true),
+ "CandidateBranchChanged"),
+ preserveAutomaticShiftRestriction: true);
+ _candidateStartedTimestampSeconds =
+ candidateStartedTimestampSeconds;
+ }
+
+ return ReturnCandidateState(
+ timestampSeconds,
+ out state);
+ }
+
+ private void BeginJumpCandidate(
+ Pose2D poseInDetour,
+ Pose2D poseUsingCurrentTransform,
+ Pose2D predictedPoseInControl,
+ double timestampSeconds,
+ string triggerReason,
+ bool preserveAutomaticShiftRestriction = false)
+ {
+ var automaticShiftAlreadyForbidden =
+ preserveAutomaticShiftRestriction &&
+ _candidateAutomaticShiftForbidden;
+
+ GetPoseDifference(
+ poseUsingCurrentTransform,
+ predictedPoseInControl,
+ out _candidateShiftDistanceMeters,
+ out _candidateShiftHeadingRadians);
+
+ _candidateControlFromDetour =
+ FrameTransform2D.Compose(
+ predictedPoseInControl,
+ FrameTransform2D.Inverse(
+ poseInDetour));
+ _candidateConsistentFrameCount = 1;
+ _candidateStartedTimestampSeconds =
+ timestampSeconds;
+ _candidateAutomaticShiftForbidden =
+ automaticShiftAlreadyForbidden ||
+ (!_postRotationPositionRecoveryActive &&
+ IsInPlaceRotationFromWheelFeedback());
+ _lastJumpCandidateTriggerReason =
+ _candidateAutomaticShiftForbidden
+ ? AppendDiagnosticToken(
+ triggerReason,
+ "AutomaticShiftForbiddenDuringInPlaceRotation")
+ : triggerReason;
+ _candidateEligibleForAutomaticShift =
+ _candidateShiftDistanceMeters <=
+ _maximumAutomaticFrameShiftMeters &&
+ _candidateShiftHeadingRadians <=
+ _maximumAutomaticHeadingShiftRadians &&
+ !_candidateAutomaticShiftForbidden;
+ _jumpCandidateActive = true;
+ }
+
+ private bool ReturnCandidateState(
+ double timestampSeconds,
+ out VehicleState state)
+ {
+ var candidateAgeSeconds =
+ timestampSeconds -
+ _candidateStartedTimestampSeconds;
+
+ if (candidateAgeSeconds >
+ _jumpConfirmationTimeoutSeconds)
+ {
+ state = default;
+ LastFailureReason = _candidateEligibleForAutomaticShift
+ ? "Detour疑似坐标跳变未能在" +
+ $"{_jumpConfirmationTimeoutSeconds:F2}s内确认，" +
+ "车辆状态已置为不可用。"
+ : "Detour位姿偏移在短时确认后仍未恢复，且不能安全地自动连续化：平移" +
+ $"{_candidateShiftDistanceMeters:F3}m，航向" +
+ $"{AngleMath.RadiansToDegrees(_candidateShiftHeadingRadians):F2}°。" +
+ "车辆状态已置为不可用，应停车并重新定位或规划。";
+ return false;
+ }
+
+ state = CreatePredictedState(timestampSeconds);
+ LastFailureReason = _candidateEligibleForAutomaticShift
+ ? "Detour疑似坐标跳变正在确认，当前使用轮组速度短时预测位姿。"
+ : "Detour位姿暂不连续且当前运动状态禁止自动坐标修正，" +
+ "正在使用轮组速度进行短时恢复确认。";
+ return true;
}
///
- /// 接受连续有效定位并更新速度估计和差分基准。
+ /// 判断轮组反馈是否表明车辆正在近似原地自转；此时禁止自动改写任务坐标系。
///
+ private bool IsInPlaceRotationFromWheelFeedback()
+ {
+ if (!_latestWheelVelocityValid)
+ {
+ return false;
+ }
+
+ var linearSpeedMetersPerSecond = Math.Sqrt(
+ _latestWheelTwistInBody.VxMetersPerSecond *
+ _latestWheelTwistInBody.VxMetersPerSecond +
+ _latestWheelTwistInBody.VyMetersPerSecond *
+ _latestWheelTwistInBody.VyMetersPerSecond);
+
+ return linearSpeedMetersPerSecond <=
+ InPlaceRotationLinearSpeedThresholdMetersPerSecond &&
+ Math.Abs(
+ _latestWheelTwistInBody
+ .OmegaRadiansPerSecond) >=
+ InPlaceRotationAngularSpeedThresholdRadiansPerSecond;
+ }
+
+ private void ClearJumpCandidate()
+ {
+ _jumpCandidateActive = false;
+ _candidateControlFromDetour = Pose2D.Identity;
+ _candidateConsistentFrameCount = 0;
+ _candidateStartedTimestampSeconds = 0.0;
+ _candidateShiftDistanceMeters = 0.0;
+ _candidateShiftHeadingRadians = 0.0;
+ _candidateAutomaticShiftForbidden = false;
+ _candidateEligibleForAutomaticShift = false;
+ _postRotationPositionRecoveryActive = false;
+ }
+
+ private static DetourObservation ReadDetourObservation()
+ {
+ var location = DetourInterface.getCartLocation();
+ var xMillimeters = Convert.ToDouble(
+ location.x,
+ CultureInfo.InvariantCulture);
+ var yMillimeters = Convert.ToDouble(
+ location.y,
+ CultureInfo.InvariantCulture);
+ var yawDegrees = Convert.ToDouble(
+ location.th,
+ CultureInfo.InvariantCulture);
+ var tickRaw = Convert.ToInt64(
+ location.tick,
+ CultureInfo.InvariantCulture);
+ var localizationStep = Convert.ToDouble(
+ location.l_step,
+ CultureInfo.InvariantCulture);
+
+ NumericGuard.EnsureFinite(
+ xMillimeters,
+ "DetourX");
+ NumericGuard.EnsureFinite(
+ yMillimeters,
+ "DetourY");
+ NumericGuard.EnsureFinite(
+ yawDegrees,
+ "DetourTheta");
+ NumericGuard.EnsureFinite(
+ localizationStep,
+ "DetourLStep");
+
+ if (tickRaw <= 0L)
+ {
+ throw new InvalidOperationException(
+ "Detour源时间戳必须为正整数。");
+ }
+
+ return new DetourObservation(
+ new Pose2D(
+ xMillimeters / MillimetersPerMeter,
+ yMillimeters / MillimetersPerMeter,
+ AngleMath.NormalizeRadians(
+ AngleMath.DegreesToRadians(
+ yawDegrees))),
+ tickRaw,
+ localizationStep);
+ }
+
+ private void SetLastObservedDetourFrame(
+ DetourObservation observation)
+ {
+ _lastObservedPoseInDetour =
+ observation.PoseInDetour;
+ _lastObservedTickRaw = observation.TickRaw;
+ _hasObservedDetourFrame = true;
+ }
+
private VehicleState AcceptContinuousPose(
- Pose2D poseInWorld,
+ Pose2D poseInControl,
double timestampSeconds)
{
- _latestState =
- _velocityEstimator.Update(
- poseInWorld,
- timestampSeconds);
- _acceptedPoseInWorld = poseInWorld;
- _acceptedTimestampSeconds =
- timestampSeconds;
+ _latestState = _velocityEstimator.Update(
+ poseInControl,
+ timestampSeconds);
+ _acceptedPoseInControl = poseInControl;
+ _acceptedTimestampSeconds = timestampSeconds;
_stationaryHoldActive = false;
+ ResetMotionPrediction(
+ poseInControl,
+ timestampSeconds);
return _latestState;
}
- ///
- /// 接受首帧或静止后的首个新位姿并重新建立零速差分基准。
- ///
private VehicleState AcceptPoseAfterReset(
- Pose2D poseInWorld,
+ Pose2D poseInControl,
double timestampSeconds)
{
- _latestState =
- _velocityEstimator.Reset(
- poseInWorld,
- timestampSeconds);
- _acceptedPoseInWorld = poseInWorld;
- _acceptedTimestampSeconds =
- timestampSeconds;
- _hasAcceptedPose = true;
+ _latestState = _velocityEstimator.Reset(
+ poseInControl,
+ timestampSeconds);
+ _acceptedPoseInControl = poseInControl;
+ _acceptedTimestampSeconds = timestampSeconds;
_stationaryHoldActive = false;
+ ResetMotionPrediction(
+ poseInControl,
+ timestampSeconds);
return _latestState;
}
- ///
- /// 对重复Detour观测保留最近状态，并在持续不变后将速度归零。
- ///
private VehicleState HandleRepeatedPose(
double timestampSeconds)
{
@@ -301,24 +1088,116 @@ namespace MultiWheelC.StateEstimation
unchangedSeconds >=
_stationaryConfirmationSeconds)
{
- _latestState =
- new VehicleState(
- timestampSeconds,
- _acceptedPoseInWorld,
- Twist2D.Zero,
- true);
+ _latestState = new VehicleState(
+ timestampSeconds,
+ _acceptedPoseInControl,
+ Twist2D.Zero,
+ true);
_stationaryHoldActive = true;
+ ResetMotionPrediction(
+ _acceptedPoseInControl,
+ timestampSeconds);
}
return _latestState;
}
- ///
- /// 判断两次有效Detour观测之间的变化是否超过车辆绝对运动能力。
- ///
+ private VehicleState CreatePredictedState(
+ double timestampSeconds)
+ {
+ var predictedPose = GetPredictedPoseInControl();
+ var twistInBody = _latestWheelVelocityValid
+ ? _latestWheelTwistInBody
+ : Twist2D.Zero;
+ var twistInControl =
+ FrameTransform2D.TransformTwistAtSamePoint(
+ predictedPose,
+ twistInBody);
+ _latestState = new VehicleState(
+ timestampSeconds,
+ predictedPose,
+ twistInControl,
+ _latestWheelVelocityValid);
+ return _latestState;
+ }
+
+ private void ResetMotionPrediction(
+ Pose2D poseInControl,
+ double timestampSeconds)
+ {
+ _predictedPoseInControl = poseInControl;
+ _predictionTimestampSeconds = timestampSeconds;
+ _hasMotionPrediction = true;
+ }
+
+ private void AdvanceMotionPrediction(
+ double timestampSeconds)
+ {
+ if (!_hasMotionPrediction)
+ {
+ return;
+ }
+
+ var deltaTimeSeconds =
+ timestampSeconds -
+ _predictionTimestampSeconds;
+ if (!NumericGuard.IsFinite(deltaTimeSeconds) ||
+ deltaTimeSeconds <= 0.0)
+ {
+ _predictionTimestampSeconds = timestampSeconds;
+ return;
+ }
+
+ if (_latestWheelVelocityValid)
+ {
+ _predictedPoseInControl = IntegrateBodyTwist(
+ _predictedPoseInControl,
+ _latestWheelTwistInBody,
+ deltaTimeSeconds);
+ }
+
+ _predictionTimestampSeconds = timestampSeconds;
+ }
+
+ private Pose2D GetPredictedPoseInControl()
+ {
+ return _hasMotionPrediction
+ ? _predictedPoseInControl
+ : _acceptedPoseInControl;
+ }
+
+ private static Pose2D IntegrateBodyTwist(
+ Pose2D startPoseInControl,
+ Twist2D twistInBody,
+ double deltaTimeSeconds)
+ {
+ var middleYawRadians =
+ startPoseInControl.YawRadians +
+ 0.5 * twistInBody.OmegaRadiansPerSecond *
+ deltaTimeSeconds;
+ var cos = Math.Cos(middleYawRadians);
+ var sin = Math.Sin(middleYawRadians);
+ var velocityXInControl =
+ cos * twistInBody.VxMetersPerSecond -
+ sin * twistInBody.VyMetersPerSecond;
+ var velocityYInControl =
+ sin * twistInBody.VxMetersPerSecond +
+ cos * twistInBody.VyMetersPerSecond;
+
+ return new Pose2D(
+ startPoseInControl.XMeters +
+ velocityXInControl * deltaTimeSeconds,
+ startPoseInControl.YMeters +
+ velocityYInControl * deltaTimeSeconds,
+ AngleMath.NormalizeRadians(
+ startPoseInControl.YawRadians +
+ twistInBody.OmegaRadiansPerSecond *
+ deltaTimeSeconds));
+ }
+
private bool IsMotionPlausible(
- Pose2D startPoseInWorld,
- Pose2D endPoseInWorld,
+ Pose2D startPose,
+ Pose2D endPose,
double deltaTimeSeconds)
{
if (!NumericGuard.IsFinite(deltaTimeSeconds) ||
@@ -327,89 +1206,331 @@ namespace MultiWheelC.StateEstimation
return false;
}
- var deltaX =
- endPoseInWorld.XMeters -
- startPoseInWorld.XMeters;
- var deltaY =
- endPoseInWorld.YMeters -
- startPoseInWorld.YMeters;
- var displacementMeters =
- Math.Sqrt(
- deltaX * deltaX +
- deltaY * deltaY);
- var headingChangeRadians =
- Math.Abs(
- AngleMath.ShortestDifferenceRadians(
- endPoseInWorld.YawRadians,
- startPoseInWorld.YawRadians));
+ GetPoseDifference(
+ startPose,
+ endPose,
+ out var displacementMeters,
+ out var headingChangeRadians);
var maximumDisplacementMeters =
_maximumLinearSpeedMetersPerSecond *
deltaTimeSeconds +
_positionJumpMarginMeters;
var maximumHeadingChangeRadians =
- _maximumAngularSpeedRadiansPerSecond *
- deltaTimeSeconds +
- _headingJumpMarginRadians;
+ GetMaximumHeadingChangeRadians(
+ deltaTimeSeconds);
return displacementMeters <=
maximumDisplacementMeters &&
headingChangeRadians <=
- maximumHeadingChangeRadians;
+ maximumHeadingChangeRadians;
}
- ///
- /// 判断新位姿是否明显偏离上一滤波速度给出的恒速预测。
- ///
- private bool IsVelocityInnovationAbnormal(
- Pose2D poseInWorld,
+ private bool IsHeadingMotionPlausible(
+ Pose2D startPose,
+ Pose2D endPose,
double deltaTimeSeconds)
{
- if (!_latestState.HasValidVelocityEstimate)
+ if (!NumericGuard.IsFinite(deltaTimeSeconds) ||
+ deltaTimeSeconds <= 0.0)
{
return false;
}
- var predictedX =
- _acceptedPoseInWorld.XMeters +
- _latestState.TwistInWorld
- .VxMetersPerSecond *
- deltaTimeSeconds;
- var predictedY =
- _acceptedPoseInWorld.YMeters +
- _latestState.TwistInWorld
- .VyMetersPerSecond *
- deltaTimeSeconds;
- var predictedYaw =
- AngleMath.NormalizeRadians(
- _acceptedPoseInWorld.YawRadians +
- _latestState.TwistInWorld
- .OmegaRadiansPerSecond *
- deltaTimeSeconds);
+ var headingChangeRadians = Math.Abs(
+ AngleMath.ShortestDifferenceRadians(
+ endPose.YawRadians,
+ startPose.YawRadians));
+ return headingChangeRadians <=
+ GetMaximumHeadingChangeRadians(
+ deltaTimeSeconds);
+ }
- var positionResidualX =
- poseInWorld.XMeters - predictedX;
- var positionResidualY =
- poseInWorld.YMeters - predictedY;
- var positionResidualMeters =
- Math.Sqrt(
- positionResidualX * positionResidualX +
- positionResidualY * positionResidualY);
- var headingResidualRadians =
- Math.Abs(
- AngleMath.ShortestDifferenceRadians(
- poseInWorld.YawRadians,
- predictedYaw));
+ private double GetMaximumHeadingChangeRadians(
+ double deltaTimeSeconds)
+ {
+ var measuredAngularSpeedRadiansPerSecond =
+ _latestWheelVelocityValid
+ ? Math.Abs(
+ _latestWheelTwistInBody
+ .OmegaRadiansPerSecond)
+ : 0.0;
+ var plausibleAngularSpeedRadiansPerSecond =
+ Math.Max(
+ _maximumAngularSpeedRadiansPerSecond,
+ measuredAngularSpeedRadiansPerSecond);
+ return plausibleAngularSpeedRadiansPerSecond *
+ deltaTimeSeconds +
+ _headingJumpMarginRadians;
+ }
+
+ private bool IsPoseInnovationAbnormal(
+ Pose2D observedPoseInControl,
+ Pose2D predictedPoseInControl,
+ double sourceDeltaTimeSeconds,
+ bool recordDiagnostics = true)
+ {
+ GetPoseDifference(
+ observedPoseInControl,
+ predictedPoseInControl,
+ out var positionResidualMeters,
+ out var headingResidualRadians);
+
+ // 轮速预测与Detour帧的采样时刻可能相差一个源周期。
+ // 航向残差限值随实际角速度和Detour帧间隔增加，避免正常自转
+ // 被固定角度阈值误判；静止和低速时仍保留原来的严格限值。
+ var headingTimingAllowanceRadians =
+ _latestWheelVelocityValid
+ ? Math.Abs(
+ _latestWheelTwistInBody
+ .OmegaRadiansPerSecond) *
+ sourceDeltaTimeSeconds
+ : 0.0;
+ var maximumHeadingResidualRadians =
+ _velocityHeadingResidualRadians +
+ headingTimingAllowanceRadians;
+
+ if (recordDiagnostics)
+ {
+ _hasInnovationDiagnostics = true;
+ _lastPositionInnovationMeters =
+ positionResidualMeters;
+ _lastAllowedPositionInnovationMeters =
+ _velocityPositionResidualMeters;
+ _lastHeadingInnovationRadians =
+ headingResidualRadians;
+ _lastAllowedHeadingInnovationRadians =
+ maximumHeadingResidualRadians;
+ }
return positionResidualMeters >
_velocityPositionResidualMeters ||
headingResidualRadians >
- _velocityHeadingResidualRadians;
+ maximumHeadingResidualRadians;
}
///
- /// 判断两次读取是否为Detour保持输出的同一数值帧。
+ /// 接受健康Detour航向，或将单帧异常转入轮组Vw短时预测确认。
///
+ private void UpdateHeadingObservation(
+ DetourObservation observation,
+ bool sourceHeadingPlausible,
+ double timestampSeconds)
+ {
+ var headingInnovationPlausible =
+ !_hasInnovationDiagnostics ||
+ _lastHeadingInnovationRadians <=
+ _lastAllowedHeadingInnovationRadians;
+
+ if (sourceHeadingPlausible &&
+ headingInnovationPlausible)
+ {
+ AcceptReliableHeadingObservation(observation);
+ return;
+ }
+
+ var reason = !sourceHeadingPlausible
+ ? "Detour单帧航向变化超出物理边界。"
+ : "Detour航向创新超过当前动态允许值。";
+ BeginOrContinueHeadingOutlierCandidate(
+ timestampSeconds,
+ reason);
+ }
+
+ ///
+ /// 接受Detour航向并清除尚未确认的航向异常候选。
+ ///
+ private void AcceptReliableHeadingObservation(
+ DetourObservation observation)
+ {
+ ClearHeadingOutlierCandidate();
+ _latestReliableHeadingInDetourRadians =
+ observation.PoseInDetour.YawRadians;
+ _hasReliableHeadingObservation = true;
+ LastHeadingFailureReason = string.Empty;
+ }
+
+ ///
+ /// 终止短时航向预测并将独立航向标记为不可用。
+ ///
+ private void InvalidateHeadingObservation(
+ string reason)
+ {
+ ClearHeadingOutlierCandidate();
+ _hasReliableHeadingObservation = false;
+ LastHeadingFailureReason = reason;
+ }
+
+ ///
+ /// 对连续异常新帧计数，并在有限时间内保留轮组预测航向。
+ ///
+ private void BeginOrContinueHeadingOutlierCandidate(
+ double timestampSeconds,
+ string reason)
+ {
+ if (!_hasReliableHeadingObservation ||
+ !_latestWheelVelocityValid ||
+ !_hasMotionPrediction)
+ {
+ InvalidateHeadingObservation(
+ reason +
+ " 当前缺少可靠航向或轮组角速度预测，无法短时降级。");
+ return;
+ }
+
+ if (!_headingOutlierCandidateActive)
+ {
+ _headingOutlierCandidateActive = true;
+ _headingOutlierConsecutiveFrameCount = 1;
+ _headingOutlierStartedTimestampSeconds =
+ timestampSeconds;
+ }
+ else
+ {
+ _headingOutlierConsecutiveFrameCount++;
+ }
+
+ _headingOutlierCandidateReason =
+ "Detour航向异常正在确认(" +
+ _headingOutlierConsecutiveFrameCount +
+ "/" +
+ _headingOutlierConfirmationFrameCount +
+ ")，当前使用轮组Vw短时预测。原因：" +
+ reason;
+ LastHeadingFailureReason = string.Empty;
+
+ if (_headingOutlierConsecutiveFrameCount >=
+ _headingOutlierConfirmationFrameCount ||
+ timestampSeconds -
+ _headingOutlierStartedTimestampSeconds >=
+ _headingOutlierPredictionTimeoutSeconds)
+ {
+ InvalidateHeadingObservation(
+ "Detour航向连续异常达到确认条件，航向暂不可用。最后原因：" +
+ reason);
+ }
+ }
+
+ ///
+ /// 清除航向异常候选的帧数、起始时刻和诊断原因。
+ ///
+ private void ClearHeadingOutlierCandidate()
+ {
+ _headingOutlierCandidateActive = false;
+ _headingOutlierConsecutiveFrameCount = 0;
+ _headingOutlierStartedTimestampSeconds = 0.0;
+ _headingOutlierCandidateReason = string.Empty;
+ }
+
+ private string BuildJumpCandidateTriggerReason(
+ bool sourceMotionPlausible)
+ {
+ var reason = sourceMotionPlausible
+ ? string.Empty
+ : "SourceMotionOutsidePhysicalBoundary";
+
+ if (_hasInnovationDiagnostics &&
+ _lastPositionInnovationMeters >
+ _lastAllowedPositionInnovationMeters)
+ {
+ reason = AppendDiagnosticToken(
+ reason,
+ "PositionInnovationExceeded");
+ }
+
+ if (_hasInnovationDiagnostics &&
+ _lastHeadingInnovationRadians >
+ _lastAllowedHeadingInnovationRadians)
+ {
+ reason = AppendDiagnosticToken(
+ reason,
+ "HeadingInnovationExceeded");
+ }
+
+ return string.IsNullOrWhiteSpace(reason)
+ ? "PoseInnovationAbnormal"
+ : reason;
+ }
+
+ private string GetDiagnosticStateStatus()
+ {
+ if (!LastDetourTickRaw.HasValue)
+ {
+ return "Uninitialized";
+ }
+
+ if (_jumpCandidateActive)
+ {
+ var candidateAgeSeconds =
+ _clock.Elapsed.TotalSeconds -
+ _candidateStartedTimestampSeconds;
+ return candidateAgeSeconds >
+ _jumpConfirmationTimeoutSeconds
+ ? _hasReliableHeadingObservation
+ ? "PositionUnavailableHeadingAvailable"
+ : "Unavailable"
+ : "JumpCandidatePrediction";
+ }
+
+ if (_headingOutlierCandidateActive)
+ {
+ return "HeadingOutlierPrediction";
+ }
+
+ if (!_hasReliableHeadingObservation)
+ {
+ return "HeadingUnavailable";
+ }
+
+ return string.IsNullOrWhiteSpace(LastFailureReason)
+ ? "Healthy"
+ : "Unavailable";
+ }
+
+ ///
+ /// 合并完整位姿状态原因和航向短时预测原因供CSV诊断使用。
+ ///
+ private string GetDiagnosticStateStatusReason()
+ {
+ if (!_headingOutlierCandidateActive)
+ {
+ return LastFailureReason;
+ }
+
+ return string.IsNullOrWhiteSpace(LastFailureReason)
+ ? _headingOutlierCandidateReason
+ : LastFailureReason + " " +
+ _headingOutlierCandidateReason;
+ }
+
+ private static string AppendDiagnosticToken(
+ string existing,
+ string token)
+ {
+ return string.IsNullOrWhiteSpace(existing)
+ ? token
+ : existing + "|" + token;
+ }
+
+ private static void GetPoseDifference(
+ Pose2D firstPose,
+ Pose2D secondPose,
+ out double positionDifferenceMeters,
+ out double headingDifferenceRadians)
+ {
+ var deltaX =
+ firstPose.XMeters - secondPose.XMeters;
+ var deltaY =
+ firstPose.YMeters - secondPose.YMeters;
+ positionDifferenceMeters = Math.Sqrt(
+ deltaX * deltaX +
+ deltaY * deltaY);
+ headingDifferenceRadians = Math.Abs(
+ AngleMath.ShortestDifferenceRadians(
+ firstPose.YawRadians,
+ secondPose.YawRadians));
+ }
+
private static bool ArePosesEquivalent(
Pose2D firstPose,
Pose2D secondPose)
@@ -428,6 +1549,5 @@ namespace MultiWheelC.StateEstimation
secondPose.YawRadians)) <=
HeadingEqualityToleranceRadians;
}
-
}
}
diff --git a/MultiWheelC/StateEstimation/ParkingVehicleStateProviderFactory.cs b/MultiWheelC/StateEstimation/ParkingVehicleStateProviderFactory.cs
index 4d1a4e0..2cb4c2b 100644
--- a/MultiWheelC/StateEstimation/ParkingVehicleStateProviderFactory.cs
+++ b/MultiWheelC/StateEstimation/ParkingVehicleStateProviderFactory.cs
@@ -55,7 +55,16 @@ 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));
return new WheelFeedbackVehicleStateProvider(
detourStateProvider,
diff --git a/MultiWheelC/StateEstimation/VehicleState.cs b/MultiWheelC/StateEstimation/VehicleState.cs
index a93cc30..6c86e5b 100644
--- a/MultiWheelC/StateEstimation/VehicleState.cs
+++ b/MultiWheelC/StateEstimation/VehicleState.cs
@@ -57,7 +57,8 @@ namespace MultiWheelC.StateEstimation
public double SampleTimestampSeconds { get; }
///
- /// 获取车体中心在Detour世界坐标系中的位姿，单位为m和rad。
+ /// 获取车体中心在状态源输出世界坐标系中的位姿，单位为m和rad。
+ /// Detour发生经确认的小幅坐标跳变后，该坐标系会保持任务内连续。
///
public Pose2D PoseInWorld { get; }
diff --git a/MultiWheelC/StateEstimation/WheelFeedbackVehicleStateProvider.cs b/MultiWheelC/StateEstimation/WheelFeedbackVehicleStateProvider.cs
index efdb882..6a74ea2 100644
--- a/MultiWheelC/StateEstimation/WheelFeedbackVehicleStateProvider.cs
+++ b/MultiWheelC/StateEstimation/WheelFeedbackVehicleStateProvider.cs
@@ -6,7 +6,7 @@ using System.Diagnostics;
namespace MultiWheelC.StateEstimation
{
///
- /// 保留外部状态源的Detour位姿，并以舵轮电机反馈解算的车体平面速度替换Detour差分线速度。
+ /// 保留外部状态源的Detour位姿，以轮组反馈替换平面线速度，并向短时位姿预测提供角速度。
///
public sealed class WheelFeedbackVehicleStateProvider
: IVehicleStateProvider
@@ -20,6 +20,7 @@ namespace MultiWheelC.StateEstimation
private readonly MultiWheelChassis _chassis;
private readonly FirstOrderLowPassFilter _longitudinalSpeedFilter;
private readonly FirstOrderLowPassFilter _lateralSpeedFilter;
+ private readonly FirstOrderLowPassFilter _angularSpeedFilter;
private bool _hasPreviousTimestamp;
private double _previousTimestampSeconds;
@@ -30,7 +31,11 @@ namespace MultiWheelC.StateEstimation
private double _latestFilteredWheelBodyVxMetersPerSecond;
private double _latestRawWheelBodyVyMetersPerSecond;
private double _latestFilteredWheelBodyVyMetersPerSecond;
+ private double _latestRawWheelBodyOmegaRadiansPerSecond;
+ private double _latestFilteredWheelBodyOmegaRadiansPerSecond;
+ private double _latestWheelSampleTimestampSeconds;
private bool _latestWheelVelocityValid;
+ private bool _latestWheelFeedbackReadSucceeded;
///
/// 创建使用默认0.10s低通时间常数的电机反馈平面速度状态源。
@@ -65,6 +70,9 @@ namespace MultiWheelC.StateEstimation
_lateralSpeedFilter =
new FirstOrderLowPassFilter(
velocityFilterTimeConstantSeconds);
+ _angularSpeedFilter =
+ new FirstOrderLowPassFilter(
+ velocityFilterTimeConstantSeconds);
}
///
@@ -73,6 +81,12 @@ namespace MultiWheelC.StateEstimation
public string LastFailureReason { get; private set; } =
string.Empty;
+ ///
+ /// 获取最近一次航向读取失败的原因；位置单独异常时保持为空。
+ ///
+ public string LastHeadingFailureReason { get; private set; } =
+ string.Empty;
+
///
/// 读取Detour位姿和电机反馈速度，并组合成统一车辆状态。
///
@@ -80,15 +94,6 @@ namespace MultiWheelC.StateEstimation
{
lock (_syncRoot)
{
- if (!_poseProvider.TryGetState(
- out var poseState))
- {
- state = default;
- LastFailureReason =
- "基础位姿状态源暂时不可用。";
- return false;
- }
-
try
{
var actualCarSpeed =
@@ -97,6 +102,11 @@ namespace MultiWheelC.StateEstimation
(double)actualCarSpeed.Vx;
var rawBodyVyMetersPerSecond =
(double)actualCarSpeed.Vy;
+ // CommonUsage.CarSpeed.Vw在旧底盘边界使用deg/s；
+ // 状态估计内部统一转换为rad/s。
+ var rawBodyOmegaRadiansPerSecond =
+ AngleMath.DegreesToRadians(
+ actualCarSpeed.Vw);
NumericGuard.EnsureFinite(
rawBodyVxMetersPerSecond,
@@ -104,6 +114,9 @@ namespace MultiWheelC.StateEstimation
NumericGuard.EnsureFinite(
rawBodyVyMetersPerSecond,
"电机反馈车体横向速度");
+ NumericGuard.EnsureFinite(
+ rawBodyOmegaRadiansPerSecond,
+ "电机反馈车体角速度");
var wheelSpeedTimestampSeconds =
_wheelSpeedClock.Elapsed.TotalSeconds;
@@ -111,15 +124,13 @@ namespace MultiWheelC.StateEstimation
UpdateBodyVelocityFilters(
rawBodyVxMetersPerSecond,
rawBodyVyMetersPerSecond,
+ rawBodyOmegaRadiansPerSecond,
wheelSpeedTimestampSeconds,
out var filteredBodyVxMetersPerSecond,
out var filteredBodyVyMetersPerSecond,
+ out var filteredBodyOmegaRadiansPerSecond,
out var hasValidWheelSpeedEstimate);
- _latestDetourBodyVxMetersPerSecond =
- poseState.TwistInBody.VxMetersPerSecond;
- _latestDetourVelocityValid =
- poseState.HasValidVelocityEstimate;
_latestRawWheelBodyVxMetersPerSecond =
rawBodyVxMetersPerSecond;
_latestFilteredWheelBodyVxMetersPerSecond =
@@ -128,8 +139,40 @@ namespace MultiWheelC.StateEstimation
rawBodyVyMetersPerSecond;
_latestFilteredWheelBodyVyMetersPerSecond =
filteredBodyVyMetersPerSecond;
+ _latestRawWheelBodyOmegaRadiansPerSecond =
+ rawBodyOmegaRadiansPerSecond;
+ _latestFilteredWheelBodyOmegaRadiansPerSecond =
+ filteredBodyOmegaRadiansPerSecond;
+ _latestWheelSampleTimestampSeconds =
+ wheelSpeedTimestampSeconds;
_latestWheelVelocityValid =
hasValidWheelSpeedEstimate;
+ _latestWheelFeedbackReadSucceeded = true;
+
+ // Detour位姿跳变确认期间需要用轮速维持短时运动预测。
+ if (_poseProvider is DetourVehicleStateProvider
+ detourStateProvider)
+ {
+ detourStateProvider.UpdateWheelVelocityEstimate(
+ filteredBodyVxMetersPerSecond,
+ filteredBodyVyMetersPerSecond,
+ filteredBodyOmegaRadiansPerSecond,
+ hasValidWheelSpeedEstimate);
+ }
+
+ if (!_poseProvider.TryGetState(
+ out var poseState))
+ {
+ state = default;
+ LastFailureReason =
+ GetPoseProviderFailureReason();
+ return false;
+ }
+
+ _latestDetourBodyVxMetersPerSecond =
+ poseState.TwistInBody.VxMetersPerSecond;
+ _latestDetourVelocityValid =
+ poseState.HasValidVelocityEstimate;
_hasVelocityDiagnostics = true;
// 车体平面线速度来自四轮电机和舵角反馈；角速度继续使用Detour，
@@ -157,6 +200,7 @@ namespace MultiWheelC.StateEstimation
}
catch (Exception exception)
{
+ _latestWheelFeedbackReadSucceeded = false;
state = default;
LastFailureReason =
"舵轮电机反馈车体速度解算失败：" +
@@ -166,6 +210,85 @@ namespace MultiWheelC.StateEstimation
}
}
+ ///
+ /// 读取Detour独立校验后的航向，同时保持轮组速度预测输入更新。
+ ///
+ 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;
+ }
+ }
+
+ ///
+ /// 原地自转停车后，允许基础Detour状态源重新确认有限位置偏移。
+ ///
+ 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 "基础位姿状态源暂时不可用。";
+ }
+
///
/// 读取最近一帧Detour纵向速度和轮速解算平面速度，供实验记录使用。
///
@@ -199,14 +322,188 @@ namespace MultiWheelC.StateEstimation
}
///
- /// 清除电机反馈速度的时间基准和低通滤波历史。
+ /// 读取最近一帧Detour纵向速度及轮组原始/滤波Vx、Vy、Vw和采样时间。
+ ///
+ 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;
+ }
+ }
+
+ ///
+ /// 读取Detour跳变候选、自动坐标连续化和数据新鲜度诊断。
+ ///
+ 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;
+ }
+ }
+
+ ///
+ /// 读取Detour源帧、轮速预测、创新门限、候选原因和状态诊断。
+ ///
+ 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;
+ }
+ }
+
+ ///
+ /// 清除基础位姿状态、坐标连续化状态以及电机反馈速度滤波历史。
///
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;
@@ -217,20 +514,27 @@ namespace MultiWheelC.StateEstimation
_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;
}
}
///
- /// 使用同一个真实采样间隔更新车体Vx和Vy低通滤波，并在首帧建立共同时间基准。
+ /// 使用同一个真实采样间隔更新车体Vx、Vy和Omega低通滤波，并在首帧建立共同时间基准。
///
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(
@@ -243,6 +547,8 @@ namespace MultiWheelC.StateEstimation
rawBodyVxMetersPerSecond);
_lateralSpeedFilter.Reset(
rawBodyVyMetersPerSecond);
+ _angularSpeedFilter.Reset(
+ rawBodyOmegaRadiansPerSecond);
_previousTimestampSeconds = timestampSeconds;
_hasPreviousTimestamp = true;
hasValidWheelSpeedEstimate = false;
@@ -250,6 +556,8 @@ namespace MultiWheelC.StateEstimation
rawBodyVxMetersPerSecond;
filteredBodyVyMetersPerSecond =
rawBodyVyMetersPerSecond;
+ filteredBodyOmegaRadiansPerSecond =
+ rawBodyOmegaRadiansPerSecond;
return;
}
@@ -264,11 +572,15 @@ namespace MultiWheelC.StateEstimation
rawBodyVxMetersPerSecond);
_lateralSpeedFilter.Reset(
rawBodyVyMetersPerSecond);
+ _angularSpeedFilter.Reset(
+ rawBodyOmegaRadiansPerSecond);
hasValidWheelSpeedEstimate = false;
filteredBodyVxMetersPerSecond =
rawBodyVxMetersPerSecond;
filteredBodyVyMetersPerSecond =
rawBodyVyMetersPerSecond;
+ filteredBodyOmegaRadiansPerSecond =
+ rawBodyOmegaRadiansPerSecond;
return;
}
@@ -281,6 +593,10 @@ namespace MultiWheelC.StateEstimation
_lateralSpeedFilter.Update(
rawBodyVyMetersPerSecond,
deltaTimeSeconds);
+ filteredBodyOmegaRadiansPerSecond =
+ _angularSpeedFilter.Update(
+ rawBodyOmegaRadiansPerSecond,
+ deltaTimeSeconds);
}
}
diff --git a/MultiWheelC/build/Clumsy/CommonUsage.dll b/MultiWheelC/build/Clumsy/CommonUsage.dll
index 9ef0dbd..1085c7f 100644
Binary files a/MultiWheelC/build/Clumsy/CommonUsage.dll and b/MultiWheelC/build/Clumsy/CommonUsage.dll differ
diff --git a/MultiWheelC/build/Clumsy/MultiWheelC.dll b/MultiWheelC/build/Clumsy/MultiWheelC.dll
index ac634e6..60deaaa 100644
Binary files a/MultiWheelC/build/Clumsy/MultiWheelC.dll and b/MultiWheelC/build/Clumsy/MultiWheelC.dll differ
diff --git a/MultiWheelC/build/Clumsy/MultiWheelC.pdb b/MultiWheelC/build/Clumsy/MultiWheelC.pdb
index 0569912..515ae92 100644
Binary files a/MultiWheelC/build/Clumsy/MultiWheelC.pdb and b/MultiWheelC/build/Clumsy/MultiWheelC.pdb differ
diff --git a/Shared/Chassis/MultiWheelChassisAdapter.cs b/Shared/Chassis/MultiWheelChassisAdapter.cs
index 1c17d3b..d721fc6 100644
--- a/Shared/Chassis/MultiWheelChassisAdapter.cs
+++ b/Shared/Chassis/MultiWheelChassisAdapter.cs
@@ -1,33 +1,39 @@
-// 将统一命令转换为原 Chassis API 调用
+// Shared层底盘边界：对外使用SI单位，对内适配旧版MultiWheelChassis的混合单位接口。
using System;
using CommonUsage.Chassis;
namespace MyParking.Shared
{
///
- /// 将统一的单车车体速度命令转换为旧版MultiWheelChassis调用。
- /// 车体坐标系固定为X向前、Y向左、逆时针为正。
+ /// 将真实车体系刚体速度转换为旧版MultiWheelChassis命令，车体系约定为X向前、Y向左、逆时针为正。
///
public sealed class MultiWheelChassisAdapter
{
#region 辅助内容
+
+ // 旧底盘原点偏置使用float角度值，此容差用于判断坐标系是否已经切换到位。
private const float BiasTolerance = 0.001f;
+
+ // 小于该值的线速度或角速度视为零，避免在静止附近进入方向不确定的运动学分支。
private const double MotionDeadband = 1e-6;
+
private readonly MultiWheelChassis _chassis;
+
+ // β：当前运动系X轴相对真实车体X轴的逆时针夹角，单位为rad。
private double _activeMotionDirectionRadians;
+
///
/// 当前适配器对应的车辆编号。
///
public int VehicleId { get; }
///
- /// Maximum distance from the body origin to a wheel center, in metres.
+ /// 车体原点到最远舵轮中心的距离，单位为m，用于描述底盘整体外接半径。
///
public double MaximumWheelRadiusMeters { get; }
///
- /// Maximum longitudinal wheel offset from the body origin, in metres.
- /// For a symmetric four-wheel-steering chassis this is half the wheelbase.
+ /// 车体原点到最前或最后舵轮中心的最大纵向距离，单位为m；对称四舵轮底盘中通常为轴距的一半。
///
public double HalfWheelBaseMeters { get; }
@@ -50,7 +56,7 @@ namespace MyParking.Shared
_activeMotionDirectionRadians;
///
- /// Width of the steering-alignment speed gate, in degrees.
+ /// 舵角误差高斯降速门控的宽度，单位为deg；数值越小，舵轮未对齐时驱动降速越明显。
///
public double SteeringAlignmentSigmaDegrees
{
@@ -78,9 +84,9 @@ namespace MyParking.Shared
}
///
- /// 检查旧底盘当前是否处于指定的运动坐标系。
- /// motionDirectionRadians表示该运动系X轴在真实车体坐标系中的方向。
+ /// 检查旧底盘是否处于指定β运动坐标系，防止准备状态与当前命令使用的坐标系不一致。
///
+ /// 运动系X轴在真实车体系中的方向，单位为rad。
private void EnsureMotionFrameIsActive(
double motionDirectionRadians)
{
@@ -130,6 +136,7 @@ namespace MyParking.Shared
twist.OmegaRadiansPerSecond),
nameof(twist.OmegaRadiansPerSecond));
}
+
///
/// 检查数值是否为有限值且可安全转换为float。
///
@@ -180,10 +187,9 @@ namespace MyParking.Shared
}
///
- /// 激活指定运动方向对应的SendMotion坐标系。
- /// 0表示真实车头，正90度表示将车体左侧作为虚拟车头。
- /// 调用方必须先停车，并确认舵轮已经按该方向完成预对齐。
+ /// 激活指定β对应的SendMotion运动坐标系；调用前必须停车并完成该方向的舵轮预对齐。
///
+ /// 运动系X轴在真实车体系中的方向，单位为rad；0为车头，π/2为车体左侧。
public void ActivateMotionFrame(
double motionDirectionRadians)
{
@@ -195,6 +201,7 @@ namespace MyParking.Shared
AngleMath.NormalizeRadians(
motionDirectionRadians);
+ // 旧底盘以“真实车体系相对运动系”的角度保存偏置，因此符号与β相反。
var biasDegrees =
ConvertRadiansToSingleDegrees(
-normalizedDirectionRadians,
@@ -217,6 +224,8 @@ namespace MyParking.Shared
return;
}
+ // SetOriginBias会把每个真实轮位重新表达在运动系中，并同步设置舵角零方向；
+ // 车辆本体没有发生虚拟旋转，后续SendMotion仍使用这些真实轮位完成四轮解算。
_chassis.SetOriginBias(
x: 0.0f,
y: 0.0f,
@@ -235,6 +244,12 @@ namespace MyParking.Shared
AngleMath.NormalizeRadians(
motionDirectionRadians);
}
+
+ ///
+ /// 创建旧版底盘的SI单位适配器，并从真实轮位提取车辆几何尺寸。
+ ///
+ /// 已经完成舵轮初始化的旧版多舵轮底盘。
+ /// 正整数车辆编号，仅标识该适配器所属车辆。
public MultiWheelChassisAdapter(MultiWheelChassis chassis, int vehicleId)
{
_chassis = chassis ?? throw new ArgumentNullException(nameof(chassis));
@@ -255,8 +270,7 @@ namespace MyParking.Shared
"MultiWheelChassis尚未完成舵轮初始化，" +
"不能创建底盘适配器。");
}
- // 禁用旧版DirectionAngle/ZeroDirection坐标偏置，
- // 保证SendXYThSpeed直接使用真实车体坐标系。
+ // 几何尺寸必须取PhysicalPosition，避免受旧底盘当前原点偏置和运动坐标系影响。
var maximumWheelRadiusMillimeters = 0.0;
var maximumLongitudinalOffsetMillimeters = 0.0;
var maximumLateralOffsetMillimeters = 0.0;
@@ -301,9 +315,11 @@ namespace MyParking.Shared
}
///
- /// 将车体坐标系刚体速度统一转换为滚动SendMotion、原地自转或停车命令。
- /// 非零平移命令使用调用方在运动段开始前已经准备并激活的β运动坐标系。
+ /// 将真实车体系刚体速度分派为滚动SendMotion、真实车体系纯自转或立即停车命令。
///
+ /// 真实车体系速度，线速度单位为m/s，角速度单位为rad/s。
+ /// 与上一条底盘命令的实际时间间隔，用于旧底盘速度和舵角变化率处理。
+ /// 旧底盘是否成功接受并完成运动分解。
public bool SendBodyTwist(
Twist2D bodyTwist,
TimeSpan? interval = null)
@@ -339,7 +355,7 @@ namespace MyParking.Shared
}
///
- /// 将车体刚体速度转换到已准备的运动坐标系，并生成该坐标系中的前后GCP方向。
+ /// 将真实车体系刚体速度转换到已激活的β运动系，并生成该运动系中的前后GCP命令。
///
private bool SendRollingTwistInActiveMotionFrame(
Twist2D bodyTwist,
@@ -354,6 +370,8 @@ namespace MyParking.Shared
0.0,
0.0,
-_activeMotionDirectionRadians);
+
+ // 同一点的速度只需旋转表达坐标系；刚体角速度在二维旋转变换下保持不变。
var motionTwist =
FrameTransform2D.TransformTwistAtSamePoint(
bodyPoseInMotionFrame,
@@ -375,9 +393,13 @@ namespace MyParking.Shared
var travelDirection =
Math.Sign(
motionVxMetersPerSecond);
+
+ // SendMotion用速度符号表达前进/倒车，而GCP角度始终相对当前行驶方向计算。
var signedCenterSpeedMetersPerSecond =
travelDirection *
linearSpeedMetersPerSecond;
+
+ // 刚体速度关系v(point)=v(center)+ω×r；前后GCP位于运动系X轴的±ControlPointRadius处。
var frontVelocityYMetersPerSecond =
motionVyMetersPerSecond +
bodyTwist.OmegaRadiansPerSecond *
@@ -432,6 +454,7 @@ namespace MyParking.Shared
return success;
}
+
///
/// 在当前已激活的运动坐标系中将有符号速度和前后GCP角度发送给旧版SendMotion。
///
@@ -499,9 +522,9 @@ namespace MyParking.Shared
}
///
- /// 停车并将所有舵轮转到指定的车体角度。
- /// 只调整舵轮角度，不产生车辆线速度。
+ /// 停车并将所有舵轮预对齐到真实车体系中的同一机械方向，不产生车辆线速度。
///
+ /// 舵轮相对真实车体X轴的目标方向，单位为rad。
public bool PrepareParallelDirection(
double directionRadians)
{
@@ -544,7 +567,7 @@ namespace MyParking.Shared
}
///
- /// 检查所有舵轮是否已经对准给定方向。
+ /// 检查所有舵轮是否已在给定容差内对准真实车体系中的同一机械方向。
///
public bool AreParallelWheelsAligned(
double directionRadians,
@@ -572,6 +595,7 @@ namespace MyParking.Shared
foreach (var wheel in wheels)
{
+ // 机械舵角受限于非环形区间，此处必须比较直接角差，不能使用圆周最短角差。
var angleErrorDegrees = targetDegrees - wheel.ReadAngle();
if (Math.Abs(angleErrorDegrees) >
@@ -641,12 +665,10 @@ namespace MyParking.Shared
return success;
}
+
///
- /// 所有舵轮是否已对齐到原地自转方向。
+ /// 所有舵轮是否已对齐到最近一次原地自转准备所确定的目标方向。
///
public bool AreSpinWheelsAligned => _chassis.LastRotateAligned;
-
-
-
}
}
diff --git a/data_process/plot_new_controller_experiment.py b/data_process/plot_new_controller_experiment.py
index 4211d71..3629865 100644
--- a/data_process/plot_new_controller_experiment.py
+++ b/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,
@@ -420,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,
@@ -484,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],
@@ -501,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")
@@ -529,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"])
@@ -677,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.append(path)
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 = 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
@@ -701,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。",
diff --git a/data_process/跳变排查/第三次加角速度反馈角速度30加速度40测试/jilu.txt b/data_process/跳变排查/第三次加角速度反馈角速度30加速度40测试/jilu.txt
new file mode 100644
index 0000000..da763fd
--- /dev/null
+++ b/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.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.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.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.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
+
+
+
+
+
+
+
diff --git a/data_process/跳变排查/第二次增加小范围坐标系修正后/其他/jilu.txt b/data_process/跳变排查/第二次增加小范围坐标系修正后/其他/jilu.txt
new file mode 100644
index 0000000..d5c7295
--- /dev/null
+++ b/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.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.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.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
+
+
+
+
+
diff --git a/data_process/跳变排查/第五次/TrackingExperiments/jilu.txt b/data_process/跳变排查/第五次/TrackingExperiments/jilu.txt
new file mode 100644
index 0000000..c6647bf
--- /dev/null
+++ b/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.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.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.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.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
+
+
+
diff --git a/data_process/跳变排查/第六次/TrackingExperiments/记录.txt b/data_process/跳变排查/第六次/TrackingExperiments/记录.txt
new file mode 100644
index 0000000..95db291
--- /dev/null
+++ b/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.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.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.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.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.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
+
+
+
+
diff --git a/data_process/跳变排查/第四次/TrackingExperiments/jilu.txt b/data_process/跳变排查/第四次/TrackingExperiments/jilu.txt
new file mode 100644
index 0000000..2d3ffea
--- /dev/null
+++ b/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.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.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.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.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.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.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.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.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
+
+
diff --git a/data_process/跳变排查/第四次/列表.txt b/data_process/跳变排查/第四次/列表.txt
new file mode 100644
index 0000000..ba2c3a5
--- /dev/null
+++ b/data_process/跳变排查/第四次/列表.txt
@@ -0,0 +1,22 @@
+getCartLocation()返回什么位姿是可能因重定位而跳变的地图位姿？
+还是保证连续的里程计位姿？
+回环或重定位后，x/y/th是否允许突然变化？
+
+这是最重要的问题。
+tick代表什么时间是激光采样时间、定位计算完成时间，还是发布时间？
+是否为 .NET DateTime.Ticks？
+getCartLocation()返回缓存数据时，tick是否保持不变？
+
+这决定MyParking如何进行时间对齐。
+l_step的含义
+只需要问：
+正常、定位退化、定位丢失、重定位中分别是什么范围？
+判断定位恢复的推荐条件是什么？
+自转时 l_step升高是否正常？
+是否存在额外可调用的接口
+重点问有没有：
+连续里程计位姿或速度。
+定位是否有效。
+是否正在重定位。
+定位置信度或匹配分数。
+不需要让对方新增功能，只问现有接口有没有。
\ No newline at end of file
diff --git a/docs/INDEX.md b/docs/INDEX.md
new file mode 100644
index 0000000..de819e8
--- /dev/null
+++ b/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时，再按任务读取工作区参考目录，并明确来源。
diff --git a/docs/architecture.md b/docs/architecture.md
new file mode 100644
index 0000000..f227fe2
--- /dev/null
+++ b/docs/architecture.md
@@ -0,0 +1,129 @@
+# 系统架构
+
+## 解决方案与依赖边界
+
+`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/` | 单周期编排、终点策略、命令执行和耗时诊断 |
+| `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` 是Clumsy车型定义和M/C IO边界。
+- `PilotDefinition.Conf` 是动作读取运行配置的统一入口；`PilotConfig` 字段提供元数据和默认值。
+- `[MovementTest]` 类型由宿主发现并执行，例如 `NewControllerStraight4mTest`、`NewControllerReverseStraight4mTest`、`NewControllerCrab45Straight4mTest`、`TestRotateAngle` 和 `CompositeStopTurnGoTest`。
+- `MovementDefinition.Get()` 以 `IEnumerable` 形式协作执行：`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()
+ → TrajectoryProjector.Project()
+ → ILateralController.Compute()
+ → ILongitudinalController.ComputeSpeedMetersPerSecond()
+ → GcpCommandAllocator.Allocate()
+ → GcpCommandExecutor.Execute()
+ → GcpKinematics.ToBodyTwist()
+ → MultiWheelChassisAdapter.SendBodyTwist()
+ → MultiWheelChassis.SendMotion()
+ → 四个真实舵轮角度和速度
+```
+
+`TrajectoryTrackingMovement` 默认从 `PilotDefinition.Conf` 读取车辆级参数，同时保留少量动作级覆盖字段；横向控制器可通过 `LateralControllerFactory` 替换，纵向控制器当前固定创建为 `PidLongitudinalController`。
+
+## 状态数据流
+
+```text
+DetourInterface.getCartLocation()
+ → DetourVehicleStateProvider
+ ├─ 位姿单位转换、重复帧处理、跳变/预测残差校验
+ └─ VelocityEstimator2D：Detour差分Vx/Vy/Omega
+
+MultiWheelChassis.GetCarSpeed(true)
+ → WheelFeedbackVehicleStateProvider
+ ├─ Vx、Vy分别使用同一时间常数低通滤波
+ ├─ 覆盖Detour线速度
+ └─ 保留Detour位姿和Omega
+
+ → 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`。构建产物目录不作为知识来源，也不直接编辑。
diff --git a/docs/decisions.md b/docs/decisions.md
new file mode 100644
index 0000000..e4f6d86
--- /dev/null
+++ b/docs/decisions.md
@@ -0,0 +1,116 @@
+# 技术决策
+
+只记录长期有效的方案状态；实验临时参数和失败尝试不在此保存。
+
+## 已经实施
+
+### 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位姿与轮组平面速度组合
+
+- 位姿和Omega来自经过校验/滤波的Detour。
+- 车体系Vx、Vy来自 `GetCarSpeed(true)`，分别使用同一时间常数低通滤波。
+- 轮组Vw同样滤波，但仅用于Detour跳变期间的短时位姿预测、运动合理性和动态航向创新阈值，不替换控制输出中的Detour Omega。
+- 原因：位置依赖SLAM，控制平面速度优先使用响应更直接的电机/舵角反馈；轮组Vw对短时运动趋势有用，但动态精度尚不足以直接作为闭环角速度。
+- 依据：`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`。
+
+## 已经确认但尚未实施
+
+- 路线顺序：先完成单车闭环和停车功能验证，再正式实施多车通信、编队和协同控制。来源：`README.md`。
+- 当前代码没有可以确认的完整多车分配方案：`Shared/Fleet/FleetKinematics.cs` 仍只有职责注释，`VehicleLayout` 和 `FleetMotionCommand` 目前只有模型定义、没有业务调用；不得把这些类型当成已经实现的功能。
+- 多车共同搬运不能只闭环车队中心：整体位姿误差与成员相对布局误差必须分开估计和约束，否则成员误差可能相互抵消而使平均中心看似正确。
+- 计划采用分层职责：车队控制器产生参考点 `FleetTwist`，分配层依据成员 `VehicleLayout` 计算每车真实车体系 `BodyTwist`，单车层继续负责β变换、GCP和本车四轮解算。
+- 虚拟车队可使用固定在车队坐标系中的前后GCP作为控制几何，例如位于 `±L_F`；这些点只用于把横向控制结果转换成车队参考点 `FleetTwist`，不是物理轮轴，也不直接参与单车四轮解算。前后GCP方向仍随控制输出动态变化，且横向控制器与GCP到Twist转换必须使用同一 `L_F`。该方案尚未实施。
+- 每辆成员车都应作为反馈来源，但反馈职责必须分层：成员Detour位姿用于融合车队整体位姿和检查相对布局，单车轮速/舵角用于确认命令执行偏差，电机电流、扭矩或力传感信息用于负载与内力监控。相对位姿接近目标并不能证明没有内力，因此不能只依靠刚性连接或位姿误差判断负载均衡。
+- 每车β应由车队分配层基于整个运动段的速度方向、机械舵角和可行性集中选择；只允许在全车停车时准备和激活。所有成员确认舵轮到位后通过车队级同步屏障统一释放非零运动命令。
+- 旧版参考项目采用固定双车布局：各车由 `carWorld ∘ layout⁻¹` 反推车队中心，再对位置和圆周航向求平均；路径控制器以该虚拟中心跟踪轨迹。同时它可按 `fleetTarget ∘ layout_i` 生成每车理想位姿，并叠加Detour布局纠偏和邻车两腿检测纠偏，因此并非只控制平均中心。来源：`原版停车机器人/parkingrobot/ClumsyPilot/PilotDefinition.cs`、`ChassisController.cs`。
+- 旧版 `SetOriginBias(layoutX, layoutY, layoutTh)` 是把各车真实轮子统一表达在车队虚拟坐标系中，属于固定编队布局变换；蟹行通过共同 `frontTh/rearTh` 表达。它与新版“每车按运动段独立选择纯旋转β”的职责不同，虽然两者可产生相同的实际轮子姿态和车辆运动。
+- 旧版自动 `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距离、任意成员布局、加权/异常值鲁棒的车队位姿估计、队形误差闭环，以及 `FleetMotionCommand → 每车Twist2D` 的具体分配与限幅算法。
+- 负载共享和内力监控可用信号、阈值、降级与停车策略；当前项目尚未建立可确认的力/扭矩闭环。
+- 正式轨迹规划层与 `Trajectory2D` 的接入格式；当前 `TestTrajectoryFactory` 仅用于实验。
+- 是否长期保留旧版GCP/`SendMotion`后端，或增加经过充分验证的“车体Twist直接到各轮”的新后端。
+- 蟹行遥控中按半轮距/半轴距缩放舵角的映射是否符合统一曲率语义；这是遥控手感策略，不应与坐标变换混为一谈。
diff --git a/docs/detour-information-checklist.md b/docs/detour-information-checklist.md
new file mode 100644
index 0000000..abd04ac
--- /dev/null
+++ b/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` 中时间同步、跳变连续化、自转短时预测和安全停止规则的最终设置。
+
diff --git a/docs/interfaces.md b/docs/interfaces.md
new file mode 100644
index 0000000..4db5243
--- /dev/null
+++ b/docs/interfaces.md
@@ -0,0 +1,184 @@
+# 关键接口与数据约定
+
+## 坐标系和单位
+
+`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` | 单车车体系在车队系中的位姿；当前仅数据模型 |
+| `FleetMotionCommand` | 车队参考点及该点速度；当前尚无车队分配执行器 |
+
+坐标变换集中在 `FrameTransform2D`；有限值检查集中在 `NumericGuard`；角度处理集中在 `AngleMath`。
+
+## 轨迹契约
+
+文件：`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仍使用Detour估计；轮组 `Vw` 只提供给Detour短时运动预测和动态航向合理性判断。
+
+默认滤波参数来自 `PilotConfig.ParkingControl.cs`：Detour线速度0.15s、Detour角速度0.20s、轮组反馈 `Vx/Vy/Vw` 统一为0.10s。
+
+跳变处理的重要边界：航向创新允许量随 `|Vw| × Detour源帧间隔` 增加；候选若在近似原地自转期间开始，则整个候选确认过程禁止自动改写任务坐标系。默认候选确认窗口为0.60s，窗口内输出轮组预测状态，超时后完整位姿安全返回不可用。
+
+完整轨迹控制仍通过 `TryGetState()` 要求位置和航向均有效。原地自转则由 `WheelFeedbackVehicleStateProvider.TryGetHeadingRadians()` 获取独立可靠航向：仅位置异常时可进入 `PositionUnavailableHeadingAvailable`，不会中断正在进行的航向闭环；航向异常仍会停止动作。到达目标并停止驱动后，`MultiWheelRotateInPlace` 调用 `BeginPostRotationPositionRecovery()`，在原有3帧、0.60s及最大自动平移/航向偏移限制内恢复完整位姿，恢复失败时不释放下一运动段。
+
+`TrackingExperimentRecorder` 已记录原始/滤波轮组 `Vx/Vy/Vw`、轮组采样时刻、Detour `tick/l_step`、数据年龄和源帧间隔、轮速预测时刻、实际/允许的位置与航向创新、候选触发原因、估计器状态及不可用原因。创新和源帧间隔只在收到Detour新帧时更新；若CSV记录频率高于Detour帧率，后续记录行会重复最近一个新帧的诊断值，分析时应按 `DetourTickRaw` 去重或分组。
+
+## 控制接口
+
+### `PathTrackingContext`
+
+一次控制周期的只读快照，包含：`VehicleState`、`TrajectoryProjection`、处理后的控制参考速度、预瞄曲率、真实 `deltaTime` 和运动方向β。
+
+`ActualLongitudinalSpeedMetersPerSecond` 是完整车体平面速度沿β方向的投影：
+
+```text
+Vβ = cos(β)·Vx_body + sin(β)·Vy_body
+```
+
+因此45°/90°蟹行不能只使用车体 `Vx` 判断纵向速度。
+
+当前速度闭环和执行边界的语义并不完全相同：纵向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]` 对齐夹臂命令、驱动使能、位置反馈和车号等字段。
+- `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位和驱动方向属于实车安全边界，未经明确要求不得修改。
diff --git a/docs/overview.md b/docs/overview.md
new file mode 100644
index 0000000..3c85092
--- /dev/null
+++ b/docs/overview.md
@@ -0,0 +1,46 @@
+# 项目概览
+
+## 项目定位
+
+`MyParking` 是停车机器人控制软件的当前正式开发版本，主体为 C# 插件工程。项目目标是让多舵轮停车机器人完成底盘运动、车辆状态获取、轨迹跟踪、原地自转和后续停车作业；当前研发重点仍是单车闭环和实车联调，多车协同尚未形成可运行实现。来源：`README.md`、`MultiWheelC/PilotConfig.cs`、`Shared/Fleet/FleetKinematics.cs`。
+
+项目不是可直接 `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`。
+- 多车模型数据类型已经预留，但多车配置位于 `PilotConfig.cs` 的 `#if false`，`FleetKinematics.cs` 只有占位注释，不能把当前代码视为已支持多车编队。
+
+## 整体运行流程
+
+典型新版轨迹跟踪流程：
+
+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` 在实车上的持久化文件位置、加载时机和发布流程。
+- 真实车辆最终几何参数、舵轮零位/限位和不同车辆配置的权威来源。
+- 完整停车业务的产品流程、安全状态机和验收指标。
diff --git a/docs/problems.md b/docs/problems.md
new file mode 100644
index 0000000..a24b2d0
--- /dev/null
+++ b/docs/problems.md
@@ -0,0 +1,124 @@
+# 问题与验证状态
+
+## 已解决（代码层）
+
+### 轨迹数值检查和插值入口重复
+
+- 现象：多个轨迹文件分别实现有限值检查和插值，语义容易漂移。
+- 处理：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横向误差完成。
+
+## 待解决
+
+### 完整停车作业流程尚未实现
+
+- 缺少或未接入新版流程：轮胎识别、自动钻车、夹抱/释放动作编排、完整安全状态机。
+- 现有 `PilotConfig` 和IO字段不能视为业务流程已经完成。
+
+### 多车能力仍是占位
+
+- `Shared/Fleet/FleetKinematics.cs` 没有实现。
+- `PilotConfig.cs` 的多车区域在 `#if false` 中，不参与当前编译。
+- 当前没有车队状态、通信、命令分配、安全降级和多车测试闭环。
+
+### 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现象不应继续归因于控制算法计算量。
+- 不同速度、载荷下的舵轮物理响应和前馈参数仍需按具体工况验证。
+- 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`。
diff --git a/docs/progress.md b/docs/progress.md
new file mode 100644
index 0000000..fad225f
--- /dev/null
+++ b/docs/progress.md
@@ -0,0 +1,51 @@
+# 当前进展
+
+更新日期：2026-08-19。这里只保存当前状态，不作为完整开发历史。
+
+## 已完成/已接入
+
+- 三项目解决方案与统一构建打包脚本：`CommonUsage`、`MedullaAdapter`、`MultiWheelC`。
+- Shared的SI运动模型、坐标变换、角度工具、数值校验和 `MultiWheelChassisAdapter`。
+- 弧长参数化 `Trajectory2D`、统一插值、轨迹投影窗口和进度连续性。
+- 默认 Stanley 横向 + PID 纵向的新版轨迹控制链，横向控制器可注入替换。
+- 前进4m、倒车4m、45°蟹行4m、直线-左半圆-直线和组合运动宿主测试入口。
+- Detour位姿与轮组反馈组合状态源：Vx/Vy用于控制速度，Vw用于短时位姿预测和动态航向合理性判断，最终控制Omega仍来自Detour。
+- Detour源 `tick/l_step` 诊断、跳变候选确认、有限小偏移任务坐标连续化、自转期间禁止自动吸收偏移，以及超时状态不可用保护。
+- 起步释放、曲率前馈预瞄、GCP角速度限制、终点制动预瞄和单向低速收敛。
+- 原地自转舵轮准备、航向PID、超时保护和完成后回正；自转期间位置/航向有效性已分离，到位停车后才恢复完整位姿并决定是否释放下一段。
+- Detour单帧航向异常已增加连续帧/短时预测确认；第六轮实车数据确认前两帧异常由轮组Vw预测承接，持续到第三个异常新帧时才安全终止。
+- C层轨迹/周期CSV与M层轮速/舵角诊断记录。
+- C层Detour静态诊断入口，记录源时间、`l_step`、位姿帧差和轮组静态反馈；已有约14分41秒实车静态基线。
+- C层轨迹CSV已补充原始/滤波轮组 `Vx/Vy/Vw`、轮组与预测时刻、Detour帧间隔、实际/允许创新、候选原因、估计器状态和不可用原因。
+- 停车控制参数集中到 `Configuration/PilotConfig.ParkingControl.cs`。
+- `MultiWheelC.Tests` 已提供不依赖实车的Stanley前进/倒车横向符号回归，8个场景通过；该项目不进入正式解决方案和打包脚本。
+- 本次建立工作区/项目AGENTS导航和 `docs/` 按需知识库。
+
+以上表示代码入口存在，不表示全部实车工况已经验收。
+
+## 当前进行方向
+
+- 普通正向/倒车直线、曲线和小范围运动中坐标连续化已基本可用；当前单车状态估计的主要未决问题收敛到原地自转时Detour偶发持续退化。
+- 暂时冻结普通运动跳变阈值和30°/s、40°/s²自转参数，等待Detour接口语义后再决定是否实施自转结束后的条件化仅位置连续化或调整航向恢复策略。
+- Detour对接最小问题已整理到 `docs/detour-information-checklist.md`，不要求取得源码。
+- 验证非零β运动系，当前已有45°蟹行直线入口；曲线蟹行仍需设计实验。
+- 在单车和停车流程稳定后研究多车共同搬运的刚体模型与成员命令分配。
+
+## 阻塞/待确认
+
+- Clumsy/Medulla正式宿主、插件部署和配置持久化说明未纳入仓库。
+- 完整停车业务流程和验收指标未确认。
+- 多车通信协议、车队参考点、车辆布局来源和故障降级策略未确认。
+- 最新实车实验数据对控制周期与舵轮滞后的结论尚未沉淀为可复核结果。
+- 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. 若工程上必须先降低位置型自转失败率，再单独设计仅在纯自转结束、轮组无平移且航向有效时启用的位置连续化；不得扩展到普通轨迹运动，也不得吸收航向偏移。
+4. 为轨迹插值/投影、坐标变换、状态跳变候选和终点策略继续补充不依赖宿主的数学回归测试。
+5. 单车稳定后，再设计 `FleetMotionCommand → 每车Twist2D` 的车队运动学，不直接启用 `#if false` 旧多车代码。
diff --git a/output/C/CommonUsage.dll b/output/C/CommonUsage.dll
index 9ef0dbd..1085c7f 100644
Binary files a/output/C/CommonUsage.dll and b/output/C/CommonUsage.dll differ
diff --git a/output/C/MultiWheelC.dll b/output/C/MultiWheelC.dll
index ac634e6..60deaaa 100644
Binary files a/output/C/MultiWheelC.dll and b/output/C/MultiWheelC.dll differ
diff --git a/output/C/MultiWheelC.pdb b/output/C/MultiWheelC.pdb
index 0569912..515ae92 100644
Binary files a/output/C/MultiWheelC.pdb and b/output/C/MultiWheelC.pdb differ
diff --git a/output/M/CommonUsage.dll b/output/M/CommonUsage.dll
index 9ef0dbd..1085c7f 100644
Binary files a/output/M/CommonUsage.dll and b/output/M/CommonUsage.dll differ
diff --git a/output/M/MedullaAdapter.dll b/output/M/MedullaAdapter.dll
index 54db927..5fee21c 100644
Binary files a/output/M/MedullaAdapter.dll and b/output/M/MedullaAdapter.dll differ
diff --git a/output/M/MedullaAdapter.pdb b/output/M/MedullaAdapter.pdb
index f777dfe..b7b301c 100644
Binary files a/output/M/MedullaAdapter.pdb and b/output/M/MedullaAdapter.pdb differ
diff --git a/ref/CommonUsage.dll b/ref/CommonUsage.dll
index 9ef0dbd..1085c7f 100644
Binary files a/ref/CommonUsage.dll and b/ref/CommonUsage.dll differ
diff --git a/docs/SteeringConstraintDesign.md b/参考文档/SteeringConstraintDesign.md
similarity index 100%
rename from docs/SteeringConstraintDesign.md
rename to 参考文档/SteeringConstraintDesign.md
diff --git a/docs/cartparams.json b/参考文档/cartparams.json
similarity index 100%
rename from docs/cartparams.json
rename to 参考文档/cartparams.json
diff --git a/docs/chassis参考.json b/参考文档/chassis参考.json
similarity index 100%
rename from docs/chassis参考.json
rename to 参考文档/chassis参考.json
diff --git a/docs/clumsy参考.json b/参考文档/clumsy参考.json
similarity index 100%
rename from docs/clumsy参考.json
rename to 参考文档/clumsy参考.json
diff --git a/docs/detour数据处理.txt b/参考文档/detour数据处理.txt
similarity index 100%
rename from docs/detour数据处理.txt
rename to 参考文档/detour数据处理.txt
diff --git a/docs/测试方案.txt b/参考文档/测试方案.txt
similarity index 100%
rename from docs/测试方案.txt
rename to 参考文档/测试方案.txt
diff --git a/docs/记录.txt b/参考文档/记录.txt
similarity index 100%
rename from docs/记录.txt
rename to 参考文档/记录.txt