chore: 停止跟踪日报和开发规格文档

.codex/skills/commit/SKILL.md

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

.codex/skills/readme/SKILL.md

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

.cursor/rules/karpathy-guidelines.mdc

@@ -1,70 +0,0 @@
----
-description: Behavioral guidelines to reduce common LLM coding mistakes. Use when writing, reviewing, or refactoring code to avoid overcomplication, make surgical changes, surface assumptions, and define verifiable success criteria.
-alwaysApply: true
----
-
-# Karpathy behavioral guidelines
-
-Behavioral guidelines to reduce common LLM coding mistakes. Merge with project-specific instructions as needed.
-
-**Tradeoff:** These guidelines bias toward caution over speed. For trivial tasks, use judgment.
-
-## 1. Think Before Coding
-
-**Don't assume. Don't hide confusion. Surface tradeoffs.**
-
-Before implementing:
-- State your assumptions explicitly. If uncertain, ask.
-- If multiple interpretations exist, present them - don't pick silently.
-- If a simpler approach exists, say so. Push back when warranted.
-- If something is unclear, stop. Name what's confusing. Ask.
-
-## 2. Simplicity First
-
-**Minimum code that solves the problem. Nothing speculative.**
-
-- No features beyond what was asked.
-- No abstractions for single-use code.
-- No "flexibility" or "configurability" that wasn't requested.
-- No error handling for impossible scenarios.
-- If you write 200 lines and it could be 50, rewrite it.
-
-Ask yourself: "Would a senior engineer say this is overcomplicated?" If yes, simplify.
-
-## 3. Surgical Changes
-
-**Touch only what you must. Clean up only your own mess.**
-
-When editing existing code:
-- Don't "improve" adjacent code, comments, or formatting.
-- Don't refactor things that aren't broken.
-- Match existing style, even if you'd do it differently.
-- If you notice unrelated dead code, mention it - don't delete it.
-
-When your changes create orphans:
-- Remove imports/variables/functions that YOUR changes made unused.
-- Don't remove pre-existing dead code unless asked.
-
-The test: Every changed line should trace directly to the user's request.
-
-## 4. Goal-Driven Execution
-
-**Define success criteria. Loop until verified.**
-
-Transform tasks into verifiable goals:
-- "Add validation" → "Write tests for invalid inputs, then make them pass"
-- "Fix the bug" → "Write a test that reproduces it, then make it pass"
-- "Refactor X" → "Ensure tests pass before and after"
-
-For multi-step tasks, state a brief plan:
-```
-1. [Step] → verify: [check]
-2. [Step] → verify: [check]
-3. [Step] → verify: [check]
-```
-
-Strong success criteria let you loop independently. Weak criteria ("make it work") require constant clarification.
-
----
-
-**These guidelines are working if:** fewer unnecessary changes in diffs, fewer rewrites due to overcomplication, and clarifying questions come before implementation rather than after mistakes.

.gitignore

@@ -1,93 +1,67 @@
-##################################################
-# Visual Studio
-##################################################
-
-# Visual Studio 工作区缓存
+# Build results
+[Bb]in/
+[Oo]bj/
+build/
+artifacts/
+**/bin/
+**/obj/
+# Visual Studio / Rider / VS Code
 .vs/
-**/.vs/
-
-# 用户配置
+.idea/
+.vscode/
 *.user
 *.suo
+*.rsuser
 *.userosscache
 *.sln.docstates
 
-##################################################
-# Build 输出
-##################################################
+# .NET generated files
+project.assets.json
+project.nuget.cache
+*.nuget.g.props
+*.nuget.g.targets
+*.AssemblyInfo.cs
+*.GeneratedMSBuildEditorConfig.editorconfig
+*.GlobalUsings.g.cs
+*.FileListAbsolute.txt
+*.CoreCompileInputs.cache
+*.AssemblyReference.cache
+*.assets.cache
 
-# 编译输出目录
-bin/
-obj/
-**/bin/
-**/obj/
-
-##################################################
-# Rider / VS Code
-##################################################
-
-.idea/
-.vscode/
-
-##################################################
-# NuGet
-##################################################
-
-*.nupkg
-packages/
-
-##################################################
-# 日志
-##################################################
+# Test and coverage output
+TestResults/
+coverage/
+*.coverage
+*.coveragexml
+coverage*.json
 
+# Logs, diagnostics, and temporary files
 *.log
-
-##################################################
-# 临时文件
-##################################################
-
+*.tlog
+*.binlog
+*.pdb
+*.cache
 *.tmp
 *.temp
+*.swp
+*.bak
 
-##################################################
-# 测试结果
-##################################################
-
-TestResults/
-
-##################################################
-# 发布目录
-##################################################
-
-publish/
-
-##################################################
-# Windows
-##################################################
+# Local runtime configuration
+cartparams.json
+appsettings.Development.json
+*.local.json
 
+# OS files
 Thumbs.db
-Desktop.ini
+.DS_Store
 
-##################################################
-# JetBrains
-##################################################
 
-_ReSharper*/
-*.DotSettings.user
-
-##################################################
-# 缓存
-##################################################
-
-*.cache
-
-##################################################
-# 数据库（如果有）
-##################################################
-
-*.db
-*.sqlite
-*.sqlite3
-
-*.csv
-*.png
+# Local / unfinished workspace artifacts
+/TrajPlanner/
+/.agents/
+/.sdd-worktrees/
+/.superpowers/
+/.task8-sweep/
+/dailywork_report/
+/docs/
+/ClumsyPilot/tests/

AGENTS.md

@@ -1,59 +0,0 @@
-# Karpathy原则
-
-## 先理解再修改
-
-- 检查实际实现、调用链和现有约束，不凭名称猜测行为。
-- 明确必要假设；遇到会显著改变结果的歧义时先说明。
-
-## 保持简单
-
-- 使用满足当前需求的最小方案。
-- 不增加推测性功能、无必要抽象或配置。
-
-## 精确修改
-
-- 只触碰与当前任务直接相关的代码，保留既有风格和无关改动。
-- 清理由本次修改产生的废弃代码，不顺便清理原有无关代码。
-
-## 面向验证
-
-- 修改前明确可观察的成功标准，修改后运行相关检查。
-- 如实报告警告、限制和未验证项。
-
-# MyParking项目规则
-
-## 代码边界
-
-- `CommonUsage-MultiVehicleSync`是独立的通用底盘库，不反向依赖`Shared`、M层或C层。
-- `Shared`只包含M/C共享的数据模型、数学方法和底盘适配代码。
-- `MedullaAdapter`负责M层硬件通信、IO和底盘命令。
-- `MultiWheelC`负责C层动作、控制、实验和数据记录。
-
-## 代码规范
-
-- 新增或修改的类、结构体和方法使用一句话的`/// <summary>`说明业务用途。
-- 单位、坐标系或正负方向不明确时补充说明，不复述代码字面内容。
-- Shared统一使用SI单位：m、m/s、rad、rad/s。
-- 车体坐标系为X向前、Y向左、逆时针为正；旧接口单位只在边界处转换。
-- 角度归一化、最短角差和度弧度转换统一使用`Shared/Mathematics/AngleMath.cs`，不重复手写。
-- 弧度归一化范围为`[-π, π)`，度归一化范围为`[-180°, 180°)`。
-- 车辆航向可用圆周最短角差；受`[-120°, 120°]`限制的机械舵角误差必须直接使用目标值减实际值。
-
-## 控制安全
-
-- 未经明确要求，不改变速度或舵角符号、CAN ID、遥控器映射、机械限位和模式切换策略。
-- 修改底盘命令、模式切换或四轮解算时，说明对实际运动的影响。
-- 实车测试采用低速、短距离，并确保可以立即停车。
-
-## 构建验证
-
-- 修改`CommonUsage-MultiVehicleSync`、`Shared`、`MedullaAdapter`或`MultiWheelC`后，在`MyParking`目录运行：
-
-```powershell
-powershell -NoProfile -ExecutionPolicy Bypass -File .\build-and-package.ps1
-```
-
-- Release构建在命令末尾追加`-Configuration Release`。
-- 报告各项目的警告和错误，并确认M/C部署包使用同一份`CommonUsage.dll`。
-- 不直接编辑`bin`、`obj`、`build`和`output`中的产物。
-- 不手工覆盖`ref/CommonUsage.dll`，由构建脚本统一更新。

ClumsyPilot/AGV.cs

@@ -0,0 +1,534 @@
+using ClumsyCore;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Sensors;
+using CommonUsage.Chassis;
+using CommonUsage.Mathematics;
+using FundamentalLib;
+using MDCSToolBox.Clumsy.AgvInterfaces;
+using MDCSToolBox.Clumsy.Calibration;
+using MDCSToolBox.Clumsy.MotionControllers;
+using MDCSToolBox.Clumsy.Tracks;
+using MDCSToolBox.Commons.Controllers;
+using Newtonsoft.Json;
+using System;
+using System.Collections.Generic;
+using System.Net.Http;
+using System.Numerics;
+using System.Security.Cryptography;
+using System.Threading;
+using System.Threading.Tasks;
+using static ClumsyCore.DTools.Painter;
+
+namespace MultiWheelC
+{
+    public class SetLocationRes
+    {
+        public float x, y, th;
+        public int l_step;
+        public long tick;
+        public string error;
+    }
+    public class AGV : MultiWheelInterface
+    {
+        public override AbstractGeometricController GetController()
+        {
+            return new ChassisController().Get();
+        }
+
+        public override MultiWheelMagTracker GetMagController()
+        {
+            return new MultiWheelMagTracker();
+        }
+
+        public override NaiveMagnetController GetNaiveMagnetController()
+        {
+            return new NaiveMagnetController();
+        }
+
+        public void Sleep(float s)
+        {
+            new DriveTask(new Sleep() { Second = s }.Get()).Wait();
+        }
+
+        public void ControlChargePort(bool open)
+        {
+            DLog.Log($"call ControlChargePort({open})");
+            PilotDefinition.Self.OpenChargeByClumsy = open;
+        }
+
+        public void SwitchLidarArea(int area)
+        {
+            DLog.Log($"call SwitchLidarArea({area})");
+            PilotDefinition.Self.AreaChoose = area;
+        }
+
+        public void SwitchIoArea(int area)
+        {
+            if (area != -1)
+            {
+                PilotDefinition.Self.IOObstacleArea = area;
+            }
+        }
+        public void RotateToTarget(float target)
+        {
+            //if (!needrotate) return;
+            var dl = new DriveTask(new MultiWheelRotateInPlace()
+            {
+                AngleTarget = target,
+                PidparamsRead = () => new PIDParams()
+                {
+                    Kp = PilotDefinition.Conf.TireFollowingThkp,
+                    Ki = PilotDefinition.Conf.TireFollowingThki,
+                    Kd = PilotDefinition.Conf.TireFollowingThkd,
+                    DeadZone = PilotDefinition.Conf.TireFollowingThDeadZone,
+                    SpeedAccPerSec = PilotDefinition.Conf.TireFollowingThSpeedAccPerSec,
+                    OutputUpperThreshold = PilotDefinition.Conf.TireFollowingThThresh,
+                    MaxI = PilotDefinition.Conf.TireFollowingThMaxI,
+                }
+            }.Get());
+            dl.Wait();
+        }
+
+        //参数1：tireNum 需要钻过的轮胎对数量
+        //参数2：frontLidarDetect true:前雷达识别  false:后雷达识别
+        public void TireFollowing(int tireNum, bool frontLidarDetect, int srcId, int dstId)
+        {
+            while (!TryLock(dstId))
+            {
+                Thread.Sleep(50);
+            }
+            DLog.Log($"锁点{dstId}完成", "TireFollowing");
+            var lidarName = frontLidarDetect ? "前雷达" : "后雷达";
+            DLog.Log($"开始钻车动作，通过{lidarName}识别结果钻{tireNum}对轮胎", "TireFollowing");
+            if (tireNum != 1 && tireNum != 2)
+            {
+                DLog.Log($"TireNum必须是1或2 (当前输入:{tireNum})", "TireFollowing");
+                return;
+            }
+
+            if (PilotDefinition.Self.GhostMode)
+            {
+                while (!TryLock(dstId))
+                {
+                    Console.WriteLine("等待锁取货点中...");
+                    Thread.Sleep(200);
+                }
+                Console.WriteLine($"锁点{dstId}完成");
+                Thread.Sleep(1000);
+                Console.WriteLine($"开始钻车动作，通过{lidarName}识别结果钻{tireNum}对轮胎");
+                Thread.Sleep(1000);
+                Leave(srcId);
+                Console.WriteLine($"开始第一段盲走，此时释放预取货点{srcId}");
+                Thread.Sleep(2000);
+                //Leave(dstId);
+                //Console.WriteLine($"结束第一段盲走，此时释放取货点{dstId}");
+                Thread.Sleep(2000);
+                Console.WriteLine($"结束钻车动作");
+                return;
+            }
+
+            var detectors = new List<TireFollowing.DetectorDefinition>()
+            {
+                new TireFollowing.DetectorDefinition()
+                {
+                    DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, frontLidarDetect),
+                    StartGuessingX = frontLidarDetect ? PilotDefinition.Conf.TireFollowingStage1GuessX : -PilotDefinition.Conf.TireFollowingStage1GuessX,
+                    StartGuessingY = 0,
+                    SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
+                    FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
+                    PathTransformation = new Tuple<float, float, float>(
+                    frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
+                    frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
+                    0),
+                    LeaveSrcFunction = Leave,
+                    SrcId = srcId,
+                    DstId = dstId,
+                },
+                new TireFollowing.DetectorDefinition()
+                {
+                    DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, frontLidarDetect),
+                    StartGuessingX = frontLidarDetect ? PilotDefinition.Conf.TireFollowingStage2GuessX : -PilotDefinition.Conf.TireFollowingStage2GuessX,
+                    StartGuessingY = 0,
+                    SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
+                    FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
+                    PathTransformation = new Tuple<float, float, float>(
+                    frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
+                    frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
+                    0)
+                },
+            };
+            DLog.Log($"钻胎为{tireNum}", "TireFollowing");
+            var following = new TireFollowing()
+            {
+                GetController = () => new ChassisController().Get(),
+                GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
+                GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
+                detectors = detectors,
+                SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
+                MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
+                TireNum = tireNum,
+                CarDirection = frontLidarDetect ? 0f : 180f,
+                WalkBlindTh = frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarWalkBlindTh : PilotDefinition.Conf.TireFollowingBackLidarWalkBlindTh,
+            };
+            var _dt = new DriveTask(following.Get());
+            _dt.Wait();
+            DLog.Log("钻车动作结束", "TireFollowing");
+        }
+
+        //离车一定是后雷达识别一个轮胎
+        public void LeaveCar(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY)
+        {
+            while (!TryLock(dstId))
+            {
+                Thread.Sleep(50);
+            }
+            DLog.Log($"锁点{dstId}完成", "TireFollowing");
+            DLog.Log($"开始钻车动作，通过后雷达识别结果钻1对轮胎", "TireFollowing");
+            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+            chassis.SetOriginBias(0, 0, 0);
+            var following = new TireFollowing()
+            {
+                GetController = () => new ChassisController().Get(),
+                GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
+                GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
+                detectors = new List<TireFollowing.DetectorDefinition>()
+                {
+                    new TireFollowing.DetectorDefinition()
+                    {
+                        DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, false),
+                        StartGuessingX = -PilotDefinition.Conf.TireFollowingStage2GuessX,
+                        StartGuessingY = 0,
+                        SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingLeaveCarWalkBlindSwitchingDistance,
+                        FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
+                        PathTransformation = new Tuple<float, float, float>(
+                        PilotDefinition.Conf.TireFollowingLeaveCarBackLidarPathTransformationX,
+                        PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
+                        0),
+                        LeaveSrcFunction = Leave,
+                        SrcId = srcId,
+                        DstId = dstId,
+                    },
+                },
+                CarDirection = 180f,
+                SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
+                MaxSpeed = 0.25f,
+                EnableHandover = true,
+                HandoverDistance = 200f,
+                HandoverSpeed = 0.3f,
+                WalkBlindTh = 0,
+                TireNum = 1
+            };
+
+            IEnumerable<bool> LeaveThenFollow()
+            {
+                foreach (var running in following.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+                DLog.Log($"释放锁点{srcId}完成", "TireFollowing");
+                DLog.Log("离车TireFollowing结束，开始DstTracker", "TireFollowing");
+
+                foreach (var running in new DstTracker()
+                {
+                    Src = new Vector2(srcX, srcY),
+                    Dst = new Vector2(dstX, dstY),
+                    CarDirectionBias = 180f,
+                    InitialSendSpeed = 0.3f
+                }.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+
+                yield return false;
+            }
+
+            var _dt = new DriveTask(LeaveThenFollow());
+            _dt.Wait();
+            DLog.Log("离车动作1结束", "TireFollowing");
+        }
+
+        public void LineTracking(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY)
+        {
+            while (!TryLock(dstId))
+            {
+                Thread.Sleep(50);
+            }
+            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+            chassis.SetOriginBias(0, 0, 0);
+            DLog.Log($"锁点{dstId}完成", "TireFollowing");
+            IEnumerable<bool> TrackThenFollow()
+            {
+                foreach (var running in new LineTracking()
+                {
+                    Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
+                    LeaveSrcFunction = Leave,
+                    SrcId = srcId,
+                    EnableHandover = true,
+                    HandoverDistance = 200, 
+                    HandoverSpeed = 0.3f,
+                }.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+                DLog.Log($"释放锁点{srcId}完成", "TireFollowing");
+                DLog.Log("离车LineTracking结束，开始DstTracker", "TireFollowing");
+                while (!TryLock(426))
+                {
+                    Thread.Sleep(20);
+                }
+                Leave(dstId);
+                DLog.Log($"释放锁点{dstId}完成", "TireFollowing");
+                foreach (var running in new DstTracker()
+                {
+                    Src = new Vector2(srcX, srcY),
+                    Dst = new Vector2(dstX, dstY),
+                    InitialSendSpeed = 0.3f
+                }.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+                yield return false;
+            }
+            var _dt = new DriveTask(TrackThenFollow());
+            _dt.Wait();
+            DLog.Log("离车动作2结束", "TireFollowing");
+        }
+
+        //驱动器上使能
+        public void DriverAble()
+        {
+            var dl = new DriveTask(new DriverAble() { }.Get());
+            dl.Wait();
+            DLog.Log("驱动器上使能完成", "TireFollowing");
+        }
+
+        //驱动器下使能
+        public void DriverDisable()
+        {
+            var dl = new DriveTask(new DriverDisable() { }.Get());
+            dl.Wait();
+            DLog.Log("驱动器下使能完成", "TireFollowing");
+        }
+
+        // 夹抱：close 为 true 时关闭夹抱，否则打开夹抱。
+        public void ClamptoTarget(bool close)
+        {
+            if (PilotDefinition.Self.GhostMode)
+            {
+                Thread.Sleep(2000);
+                Console.WriteLine("夹抱完成");
+                return;
+            }
+            new DriveTask(new ClampToTarget()
+            {
+                LeftClampTarget = close ? PilotDefinition.Self.LeftArmUpperPos : PilotDefinition.Self.LeftArmLowerPos,
+                RightClampTarget = close ? PilotDefinition.Self.RightArmUpperPos : PilotDefinition.Self.RightArmLowerPos
+            }.Get()).Wait();
+        }
+        // Fleet crab walk: convert scheduler src/dst into the same relative crab-walk path used by MovementTest.
+        public void FleetCrabWalk(float srcX, float srcY, int srcId, float dstX, float dstY, int dstId,
+            float speed)
+        {
+            var dx = dstX - srcX;
+            var dy = dstY - srcY;
+            var pathLength = (float)Math.Sqrt(dx * dx + dy * dy);
+
+            if (pathLength <= 1f)
+            {
+                DLog.Log("FleetCrabWalk abort: path length is too short.", "FleetCrabDbg");
+                return;
+            }
+
+            var self = PilotDefinition.Self;
+            if (!self.TryGetFleetCenterFromMembers(out var centerX, out var centerY, out var centerTh) &&
+                !self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
+            {
+                DLog.Log("FleetCrabWalk abort: failed to read fleet center.", "FleetCrabDbg");
+                Hedingben.ToastText("FleetCrab requires master localization", "FleetCrab");
+                return;
+            }
+
+            var pathAngle = (float)CommonMath.RoundTh((float)(Math.Atan2(dy, dx) / Math.PI * 180.0));
+            var crabAngle = (float)CommonMath.ThDiff(pathAngle, centerTh);
+            var targetBodyWorldHeading = (float)CommonMath.RoundTh(PilotDefinition.Conf.FleetCrabBodyWorldHeadingDeg);
+            var bodyToPathAngle = (float)CommonMath.ThDiff(pathAngle, targetBodyWorldHeading);
+            DLog.Log(
+                $"call FleetCrabWalk(src=({srcX:0},{srcY:0},id:{srcId}), dst=({dstX:0},{dstY:0},id:{dstId}), " +
+                $"len={pathLength:0.0}, speed={speed:0.000}, pathAngle={pathAngle:0.0}, " +
+                $"center=({centerX:0},{centerY:0},{centerTh:0.0}), crabAngle={crabAngle:0.0}, " +
+                $"targetBodyWorld={targetBodyWorldHeading:0.0}, bodyToPath={bodyToPathAngle:0.0})",
+                "FleetCrabDbg");
+
+            if (dstId != -1)
+            {
+                while (!TryLock(dstId))
+                {
+                    Thread.Sleep(50);
+                }
+                DLog.Log($"锁点{dstId}完成", "FleetCrabDbg");
+            }
+
+            var action = new MultiWheelC.FleetCrabWalk
+            {
+                CrabAngleDeg = crabAngle,
+                BodyToPathAngleDeg = bodyToPathAngle,
+                CrabLengthMm = pathLength,
+                CrabSpeed = speed,
+                FleetCrabAccel = PilotDefinition.Conf.FleetCrabAccel,
+                FleetCrabStartAccel = PilotDefinition.Conf.FleetCrabStartAccel,
+                FleetCrabSlowDistance = PilotDefinition.Conf.FleetCrabSlowDistance,
+                FleetCrabFinishDistance = PilotDefinition.Conf.FleetCrabFinishDistance,
+                FleetCrabFinishSpeed = PilotDefinition.Conf.FleetCrabFinishSpeed,
+                FleetCrabSlowingPow = PilotDefinition.Conf.FleetCrabSlowingPow,
+                GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold
+            };
+
+            try
+            {
+                new DriveTask(action.Get()).Wait();
+            }
+            finally
+            {
+                if (srcId != -1)
+                {
+                    Leave(srcId);
+                    DLog.Log($"释放放车点{srcId}", "FleetCrabDbg");
+                }
+            }
+        }
+
+        public void FleetCurveWalk(float srcX, float srcY, int srcId, float dstX, float dstY, int dstId,
+            float speed, params float[] trackTypeInfo)
+        {
+            if (trackTypeInfo == null || trackTypeInfo.Length < 2)
+            {
+                DLog.Log("FleetCurveWalk abort: invalid trackTypeInfo, expected Bezier type info.", "FleetCurveDbg");
+                Hedingben.ToastText("FleetCurve invalid trackTypeInfo", "FleetCurve");
+                return;
+            }
+
+            var trackType = (int)trackTypeInfo[0];
+            if (trackType != 2)
+            {
+                DLog.Log($"FleetCurveWalk abort: unsupported trackType={trackType}, only Bezier(type=2) is supported.",
+                    "FleetCurveDbg");
+                Hedingben.ToastText("FleetCurve only supports Bezier trackType=2", "FleetCurve");
+                return;
+            }
+
+            var controlPointNum = (int)trackTypeInfo[1];
+            var expectedLength = 2 + controlPointNum * 2;
+            if (controlPointNum < 3 || trackTypeInfo.Length < expectedLength)
+            {
+                DLog.Log(
+                    $"FleetCurveWalk abort: invalid Bezier trackTypeInfo. controlPointNum={controlPointNum}, " +
+                    $"length={trackTypeInfo.Length}, expected>={expectedLength}.",
+                    "FleetCurveDbg");
+                Hedingben.ToastText("FleetCurve invalid Bezier trackTypeInfo", "FleetCurve");
+                return;
+            }
+
+            BezierTrack track;
+            try
+            {
+                track = ProcessTrackTypeInfo(srcX, srcY, dstX, dstY, trackTypeInfo) as BezierTrack;
+            }
+            catch (Exception ex)
+            {
+                DLog.Log($"FleetCurveWalk abort: failed to process trackTypeInfo. {ex.Message}", "FleetCurveDbg");
+                Hedingben.ToastText("FleetCurve failed to process track", "FleetCurve");
+                return;
+            }
+
+            if (track == null)
+            {
+                DLog.Log("FleetCurveWalk abort: ProcessTrackTypeInfo did not return BezierTrack.", "FleetCurveDbg");
+                Hedingben.ToastText("FleetCurve requires BezierTrack", "FleetCurve");
+                return;
+            }
+
+            track.Speed = speed;
+            track.CarDirectionBias = 0f;
+
+            DLog.Log(
+                $"call FleetCurveWalk(src=({srcX:0},{srcY:0},id:{srcId}), dst=({dstX:0},{dstY:0},id:{dstId}), " +
+                $"speed={speed:0.000}, trackType={trackType}, controls={controlPointNum}, track={track.GetType().Name}, " +
+                $"carDirectionBias=0.0)",
+                "FleetCurveDbg");
+
+            if (dstId != -1)
+            {
+                while (!TryLock(dstId))
+                {
+                    Thread.Sleep(50);
+                }
+                DLog.Log($"閿佺偣{dstId}瀹屾垚", "FleetCurveDbg");
+            }
+
+            var action = new MultiWheelC.FleetCurveWalk
+            {
+                Track = track,
+                CurveSpeed = speed,
+                CarDirectionBias = 0f,
+                SlowDistance = PilotDefinition.Conf.FleetCurveSlowDistance,
+                FinishDistance = PilotDefinition.Conf.FleetCurveFinishDistance,
+                FinishSpeed = PilotDefinition.Conf.FleetCurveFinishSpeed,
+                SlowingPow = PilotDefinition.Conf.FleetCurveSlowingPow,
+                GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold,
+                StartSyncTimeoutSec = PilotDefinition.Conf.FleetCrabStartSyncTimeoutSec
+            };
+
+            try
+            {
+                new DriveTask(action.Get()).Wait();
+            }
+            finally
+            {
+                if (srcId != -1)
+                {
+                    Leave(srcId);
+                    DLog.Log($"release srcId={srcId}", "FleetCurveDbg");
+                }
+            }
+        }
+        
+        public void ChangeAvoidanceDistance(float stopDistance, float slowDistance)
+        {
+            DLog.Log($"call ChangeAvoidanceDistance({stopDistance},{slowDistance})");
+            PilotDefinition.Self.SlowDistance = slowDistance;
+            PilotDefinition.Self.StopDistance = stopDistance;
+        }
+        public void ChangeAvoidanceParam(float length = -1, float width = -1)
+        {
+
+            PilotDefinition.Self.CarLength = length;
+            PilotDefinition.Self.CarWidth = width;
+
+        }
+        public void SetLocation(float x, float y, float th)
+        {
+            DLog.Log($"call SetLocation({x},{y},{th})");
+            Console.WriteLine($"call SetLocation({x},{y},{th})");
+            Queue(() =>
+            {
+                while (true)
+                {
+                    var str1 = new HttpClient()
+                        .GetStringAsync(
+                            $"http://127.0.0.1:4321/setLocation?x={x}&y={y}&th={th}")
+                        .Result;
+                    Thread.Sleep(500);
+                    Console.WriteLine($"SetLocation str={str1}");
+                    var setLocationRes = JsonConvert.DeserializeObject<SetLocationRes>(str1);
+                    Console.WriteLine(setLocationRes.l_step);
+                    if (setLocationRes != null && setLocationRes.l_step == 2) break;
+                }
+            });
+        }
+        public float baseSpeed = 0;
+    }
+}

ClumsyPilot/ChassisController.cs

@@ -0,0 +1,100 @@
+using System;
+using System.Numerics;
+using ClumsyCore;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using FundamentalLib;
+using MDCSToolBox.Clumsy.MotionControllers;
+using MDCSToolBox.Clumsy.Movements;
+using MDCSToolBox.Clumsy.Pilot;
+
+namespace MultiWheelC;
+
+public class ChassisController : MovementDefinition<MultiWheelGeometricController>
+{
+    public float BaseSpeed = Configuration.conf.basicSpeed;
+
+    private DateTime _sendMotionDbgLast = DateTime.MinValue;
+
+    public override MultiWheelGeometricController Get()
+    {
+        return new MultiWheelGeometricController
+        {
+            Chassis = BasicPilotBase.Chassis,
+            BaseSpeed = BaseSpeed,
+            SlowDistance = PilotDefinition.Conf.SlowDistance,
+            SlowingPow = PilotDefinition.Conf.SlowingPow,
+            FinishDistance = PilotDefinition.Conf.FinishDistance,
+            FinishSpeed = PilotDefinition.Conf.FinishSpeed,
+            FirstThAccuracy = PilotDefinition.Conf.FirstThAccuracy,
+            FirstRotateSpeedFac = PilotDefinition.Conf.FirstRotateSpeedFac,
+            FirstRotateMaxSpeed = PilotDefinition.Conf.FirstRotateMaxSpeed,
+            NotContinuousAngle = PilotDefinition.Conf.NotContinuousAngle,
+            DebugMode = PilotDefinition.Conf.MotionDebugPrint,
+            DebugCurvature = PilotDefinition.Conf.DebugCurvature,
+            PowerSteeringLookAhead = PilotDefinition.Conf.PowerSteeringLookAhead,
+            SpeedLookAhead = PilotDefinition.Conf.SpeedLookAhead,
+            SpeedLookAheadCurveDiff = PilotDefinition.Conf.SpeedLookAheadCurveDiff,
+            SpeedLookBackCurveDiff = PilotDefinition.Conf.SpeedLookBackCurveDiff,
+            SpeedLimitCurveDiffMin = PilotDefinition.Conf.SpeedLimitCurveDiffMin,
+            SpeedLimitCurveMin = PilotDefinition.Conf.SpeedLimitCurveMin,
+            MaxRotateSpeed = PilotDefinition.Conf.MaxRotateSpeedCurveLimit,
+            MaxRotateAcc = PilotDefinition.Conf.MaxRotateAccCurveLimit,
+            GcpThetaThreshold = PilotDefinition.Conf.GcpThetaThreshold,
+            DthLinearFac = PilotDefinition.Conf.DthLinearFac,
+            DthLinearThreshold = PilotDefinition.Conf.DthLinearThreshold,
+            BiasFac = PilotDefinition.Conf.BiasFac,
+            BiasThreshold = PilotDefinition.Conf.BiasThreshold,
+
+            MultiVehicleSendMotion = (speed, frontTh, rearTh, idealPos, idealAngle) =>
+            {
+                var self = PilotDefinition.Self;
+                self.MultiVehicleAutoEnabled = true;
+                // A: 用固定锁对象（不再锁会被替换的字段引用）。
+                int fleetCnt;
+                lock (self.FleetLock)
+                    fleetCnt = self.MultiVehicleFleet.Count;
+
+                // 诊断（节流 ~300ms）：确认回调被调用、编队是否就绪、是否因数量不符提前 return（导致不下发速度）。
+                if ((DateTime.Now - _sendMotionDbgLast).TotalMilliseconds >= 300)
+                {
+                    _sendMotionDbgLast = DateTime.Now;
+                    DLog.Log(
+                        $"SENDMOTION speed={speed:0.000} fTh={frontTh:0.0} rTh={rearTh:0.0} " +
+                        $"ideal=({idealPos.X:0},{idealPos.Y:0},{idealAngle:0.0}) " +
+                        $"editCnt={fleetCnt}/{PilotDefinition.Conf.MultiVehicleFleetNum} " +
+                        $"earlyReturn={fleetCnt != PilotDefinition.Conf.MultiVehicleFleetNum}",
+                        "FleetCrabDbg");
+                }
+
+                if (fleetCnt != PilotDefinition.Conf.MultiVehicleFleetNum)
+                    return;
+
+                self.MultiVehicleAutoVx = speed;
+                self.MultiVehicleAutoFrontTh = frontTh;
+                self.MultiVehicleAutoRearTh = rearTh;
+                // D: 透传路径控制器算出的理想车队中心位姿（此前被丢弃），供各车按 layout 做前馈。
+                self.MultiVehicleAutoIdealX = idealPos.X;
+                self.MultiVehicleAutoIdealY = idealPos.Y;
+                self.MultiVehicleAutoIdealTh = idealAngle;
+                self.MultiVehicleAutoHasIdeal = true;
+                // B: 标记命令新鲜度。路径结束/早退/卡顿不再刷新此时刻 → 主车超时后清零速度，避免滑行。
+                self.MultiVehicleAutoCmdTime = DateTime.Now;
+            },
+
+            // G: 读取车队中心原子快照，避免跨线程读到撕裂的 x/y/th 组合。
+            MultiVehicleGetFleetPos = () =>
+            {
+                var snap = PilotDefinition.Self.GetFleetCenterSnapshot();
+                return new Location
+                {
+                    x = snap.X,
+                    y = snap.Y,
+                    th = snap.Th,
+                    l_step = 1,
+                    tick = DateTime.Now.Ticks
+                };
+            }
+        };
+    }
+}

ClumsyPilot/ClumsyPilot.csproj

@@ -0,0 +1,84 @@
+<Project Sdk="Microsoft.NET.Sdk">
+
+  <PropertyGroup>
+    <TargetFramework>netstandard2.0</TargetFramework>
+	<LangVersion>10</LangVersion>
+	<AllowUnsafeBlocks>true</AllowUnsafeBlocks>
+  </PropertyGroup>
+
+  <ItemGroup>
+    <PackageReference Include="Newtonsoft.Json" Version="13.0.4" />
+    <PackageReference Include="System.Drawing.Common" Version="10.0.10"
+                      GeneratePathProperty="true" />
+    <PackageReference Include="System.Numerics.Vectors" Version="4.6.1" />
+    <PackageReference Include="StbImageWriteSharp" Version="1.16.7"
+                      GeneratePathProperty="true" />
+  </ItemGroup>
+
+  <ItemGroup>
+    <Compile Remove="TrajectoryPlanningVisualization\**\*.cs" />
+    <Compile Remove="tests\TrajectoryPlanningVisualizationVerificationHost\**\*.cs" />
+    <Compile Remove="tests\PathSmoothingPngVerificationHost\**\*.cs" />
+    <Compile Remove="tests\EMPlannerVerificationHost\**\*.cs" />
+    <Compile Remove="ParkrobTrajplanner\Trajplanner_output\**\*.cs" />
+    <Compile Remove="ParkrobTrajplanner\Trajplanner_guide\**\*.cs" />
+    <Compile Remove="Shared\**\*.cs" />
+    <Compile Remove="ParkrobTrajplanner\auto_avoidance\**\*.cs"
+             Condition="'$(ExcludeLegacyAutoAvoidance)' == 'true'" />
+    <!-- 嵌套验证项目的旧输出不能参与主插件的程序集解析。 -->
+    <None Remove="**\bin\**\*" />
+    <None Remove="**\obj\**\*" />
+  </ItemGroup>
+
+  <ItemGroup>
+    <Compile Include="..\Shared\**\*.cs"
+             Link="Shared\%(RecursiveDir)%(Filename)%(Extension)" />
+  </ItemGroup>
+
+  <ItemGroup>
+    <ProjectReference Include="TrajectoryPlanningVisualization\TrajectoryPlanningVisualization.csproj" />
+  </ItemGroup>
+
+  <ItemGroup>
+    <Reference Include="CommonUsage">
+      <HintPath>ref\CommonUsage.dll</HintPath>
+    </Reference>
+    <Reference Include="LessokajiWeaverUtilities">
+      <HintPath>ref\LessokajiWeaverUtilities.dll</HintPath>
+    </Reference>
+    <Reference Include="MDCSToolBox">
+      <HintPath>ref\MDCSToolBox.dll</HintPath>
+    </Reference>
+    <Reference Include="ClumsyCore">
+      <HintPath>ref\RefClumsyCore.dll</HintPath>
+    </Reference>
+    <Reference Include="ClumsyDance">
+      <HintPath>ref\RefClumsyDance.dll</HintPath>
+    </Reference>
+    <Reference Include="FundamentalLib">
+      <HintPath>ref\RefFundamentalLib.dll</HintPath>
+    </Reference>
+  </ItemGroup>
+
+  <ItemGroup>
+    <None Update="ThirdParty\OSQP\win-x64\osqp.dll"
+          Link="osqp.dll" CopyToOutputDirectory="PreserveNewest" />
+    <None Update="ThirdParty\OSQP\LICENSE"
+          Link="licenses\OSQP-LICENSE.txt" CopyToOutputDirectory="PreserveNewest" />
+    <None Update="ThirdParty\OSQP\NOTICE"
+          Link="licenses\OSQP-NOTICE.txt" CopyToOutputDirectory="PreserveNewest" />
+    <None Update="ThirdParty\OSQP\VERSION"
+          Link="licenses\OSQP-VERSION.txt" CopyToOutputDirectory="PreserveNewest" />
+  </ItemGroup>
+
+  <Target Name="DeployManagedPngRuntime" AfterTargets="Build">
+    <Copy SourceFiles="$(PkgStbImageWriteSharp)\lib\netstandard2.0\StbImageWriteSharp.dll"
+          DestinationFiles="$(TargetDir)StbImageWriteSharp.dll" />
+  </Target>
+
+  <Target Name="DeploySystemDrawingRuntime" AfterTargets="Build">
+    <Copy SourceFiles="$(PkgSystem_Drawing_Common)\lib\netstandard2.0\System.Drawing.Common.dll"
+          DestinationFiles="$(TargetDir)System.Drawing.Common.dll" />
+  </Target>
+
+</Project>

ClumsyPilot/Control/Abstractions/ILateralController.cs

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

ClumsyPilot/Control/Abstractions/ILongitudinalController.cs

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

ClumsyPilot/Control/Abstractions/LateralControlCommand.cs

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

ClumsyPilot/Control/Abstractions/PathTrackingContext.cs

@@ -0,0 +1,126 @@
+using System;
+using MultiWheelC.StateEstimation;
+using MultiWheelC.Trajectory;
+
+namespace MultiWheelC.Control.Abstractions
+{
+    /// <summary>
+    /// 保存一次轨迹跟踪控制周期使用的车辆状态、轨迹投影和真实时间间隔。
+    /// </summary>
+    public readonly struct PathTrackingContext
+    {
+        /// <summary>
+        /// 创建横向和纵向控制器共享的只读控制输入快照。
+        /// </summary>
+        public PathTrackingContext(
+            VehicleState vehicleState,
+            TrajectoryProjection projection,
+            double referenceSpeedMetersPerSecond,
+            double deltaTimeSeconds)
+        {
+            EnsureFinite(
+                referenceSpeedMetersPerSecond,
+                nameof(referenceSpeedMetersPerSecond));
+            EnsureFinitePositive(
+                deltaTimeSeconds,
+                nameof(deltaTimeSeconds));
+
+            VehicleState = vehicleState;
+            Projection = projection;
+            ReferenceSpeedMetersPerSecond =
+                referenceSpeedMetersPerSecond;
+            DeltaTimeSeconds = deltaTimeSeconds;
+        }
+
+        /// <summary>
+        /// 获取本周期经过校验的实际车辆位姿和速度状态。
+        /// </summary>
+        public VehicleState VehicleState { get; }
+
+        /// <summary>
+        /// 获取实际车体中心投影到参考轨迹后得到的参考状态和跟踪误差。
+        /// </summary>
+        public TrajectoryProjection Projection { get; }
+
+        /// <summary>
+        /// 获取本次控制计算距离上次计算的真实时间间隔，单位为s。
+        /// </summary>
+        public double DeltaTimeSeconds { get; }
+
+        /// <summary>
+        /// 获取轨迹投影点要求的有符号参考速度，单位为m/s。
+        /// </summary>
+        public double ReferenceSpeedMetersPerSecond { get; }
+
+        /// <summary>
+        /// 获取车辆在车体X轴方向上的实际纵向速度，单位为m/s。
+        /// </summary>
+        public double ActualLongitudinalSpeedMetersPerSecond =>
+            VehicleState.TwistInBody
+                .VxMetersPerSecond;
+
+        /// <summary>
+        /// 获取轨迹投影点的参考曲率，单位为1/m，左转为正。
+        /// </summary>
+        public double ReferenceCurvaturePerMeter =>
+            Projection.ReferencePoint
+                .CurvaturePerMeter;
+
+        /// <summary>
+        /// 获取参考轨迹相对车辆的有符号横向误差，单位为m，轨迹在车辆左侧时为正。
+        /// </summary>
+        public double LateralErrorMeters =>
+            Projection.LateralErrorMeters;
+
+        /// <summary>
+        /// 获取参考航向减实际车体航向的最短角差，单位为rad，逆时针为正。
+        /// </summary>
+        public double HeadingErrorRadians =>
+            Projection.HeadingErrorRadians;
+
+        /// <summary>
+        /// 获取当前投影位置沿参考轨迹到终点的剩余距离，单位为m。
+        /// </summary>
+        public double RemainingDistanceMeters =>
+            Projection.RemainingDistanceMeters;
+
+        /// <summary>
+        /// 获取实际速度是否已经由至少两个连续有效定位样本估算得到。
+        /// </summary>
+        public bool HasValidVelocityEstimate =>
+            VehicleState.HasValidVelocityEstimate;
+
+        /// <summary>
+        /// 检查控制周期是否为正有限值。
+        /// </summary>
+        private static void EnsureFinitePositive(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value) ||
+                value <= 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "轨迹跟踪控制周期必须是正有限值。");
+            }
+        }
+
+        /// <summary>
+        /// 检查控制参考速度是否为有限值。
+        /// </summary>
+        private static void EnsureFinite(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "轨迹跟踪参考速度必须是有限值。");
+            }
+        }
+    }
+}

ClumsyPilot/Control/Allocation/GcpCommandAllocator.cs

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

ClumsyPilot/Control/Allocation/GcpMotionCommand.cs

@@ -0,0 +1,67 @@
+using System;
+
+namespace MultiWheelC.Control.Allocation
+{
+    /// <summary>
+    /// 表示发送给旧版多舵轮四轮解算前的有符号速度和前后GCP角度命令。
+    /// </summary>
+    public readonly struct GcpMotionCommand
+    {
+        /// <summary>
+        /// 创建统一使用m/s和rad的前后几何控制点运动命令。
+        /// </summary>
+        public GcpMotionCommand(
+            double speedMetersPerSecond,
+            double frontAngleRadians,
+            double rearAngleRadians)
+        {
+            EnsureFinite(
+                speedMetersPerSecond,
+                nameof(speedMetersPerSecond));
+            EnsureFinite(
+                frontAngleRadians,
+                nameof(frontAngleRadians));
+            EnsureFinite(
+                rearAngleRadians,
+                nameof(rearAngleRadians));
+
+            SpeedMetersPerSecond =
+                speedMetersPerSecond;
+            FrontAngleRadians =
+                frontAngleRadians;
+            RearAngleRadians =
+                rearAngleRadians;
+        }
+
+        /// <summary>
+        /// 获取准备交给底盘的有符号纵向速度，单位为m/s，正值表示前进。
+        /// </summary>
+        public double SpeedMetersPerSecond { get; }
+
+        /// <summary>
+        /// 获取前几何控制点相对车体X轴的目标方向，单位为rad，逆时针为正。
+        /// </summary>
+        public double FrontAngleRadians { get; }
+
+        /// <summary>
+        /// 获取后几何控制点相对车体X轴的目标方向，单位为rad，逆时针为正。
+        /// </summary>
+        public double RearAngleRadians { get; }
+
+        /// <summary>
+        /// 检查底盘中间命令是否为有限值。
+        /// </summary>
+        private static void EnsureFinite(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "GCP运动命令必须由有限值组成。");
+            }
+        }
+    }
+}

ClumsyPilot/Control/Common/PidController.cs

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

ClumsyPilot/Control/Execution/GcpCommandExecutor.cs

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

ClumsyPilot/Control/Execution/ParkingGeometricController.cs

@@ -0,0 +1,551 @@
+using System;
+using MultiWheelC.Control.Abstractions;
+using MultiWheelC.Control.Allocation;
+using MultiWheelC.StateEstimation;
+using MultiWheelC.Trajectory;
+using MyParking.Shared;
+
+namespace MultiWheelC.Control.Execution
+{
+    /// <summary>
+    /// 表示新版停车机器人单周期轨迹控制的执行结果。
+    /// </summary>
+    public enum ParkingControlCycleResult
+    {
+        Inactive = 0,
+        CommandSent = 1,
+        Completed = 2,
+        StateUnavailable = 3,
+        Faulted = 4
+    }
+
+    /// <summary>
+    /// 组织状态读取、轨迹投影、横纵向控制、GCP分配和底盘命令执行。
+    /// </summary>
+    public sealed class ParkingGeometricController
+    {
+        private const double ZeroReferenceSpeedToleranceMetersPerSecond =
+            1e-6;
+        private const double StartupRegionMeters = 0.02;
+        private const double StartupPreviewDistanceMeters = 0.05;
+        private const double MaximumStartupSpeedMetersPerSecond = 0.08;
+
+        private readonly IVehicleStateProvider _stateProvider;
+        private readonly ILateralController _lateralController;
+        private readonly ILongitudinalController _longitudinalController;
+        private readonly GcpCommandAllocator _gcpAllocator;
+        private readonly GcpCommandExecutor _commandExecutor;
+
+        private Trajectory2D _trajectory;
+
+        /// <summary>
+        /// 创建具有终点判定和轨迹偏离保护的单车轨迹控制器。
+        /// </summary>
+        public ParkingGeometricController(
+            IVehicleStateProvider stateProvider,
+            ILateralController lateralController,
+            ILongitudinalController longitudinalController,
+            GcpCommandAllocator gcpAllocator,
+            GcpCommandExecutor commandExecutor,
+            double finishDistanceMeters = 0.04,
+            double finishSpeedMetersPerSecond = 0.02,
+            double finishHeadingToleranceRadians =
+                3.0 * Math.PI / 180.0,
+            double maximumDistanceToTrajectoryMeters = 0.30)
+        {
+            _stateProvider = stateProvider ??
+                throw new ArgumentNullException(
+                    nameof(stateProvider));
+            _lateralController = lateralController ??
+                throw new ArgumentNullException(
+                    nameof(lateralController));
+            _longitudinalController = longitudinalController ??
+                throw new ArgumentNullException(
+                    nameof(longitudinalController));
+            _gcpAllocator = gcpAllocator ??
+                throw new ArgumentNullException(
+                    nameof(gcpAllocator));
+            _commandExecutor = commandExecutor ??
+                throw new ArgumentNullException(
+                    nameof(commandExecutor));
+
+            EnsureFinitePositive(
+                finishDistanceMeters,
+                nameof(finishDistanceMeters));
+            EnsureFiniteNonNegative(
+                finishSpeedMetersPerSecond,
+                nameof(finishSpeedMetersPerSecond));
+            EnsureFinitePositive(
+                finishHeadingToleranceRadians,
+                nameof(finishHeadingToleranceRadians));
+            EnsureFinitePositive(
+                maximumDistanceToTrajectoryMeters,
+                nameof(maximumDistanceToTrajectoryMeters));
+
+            FinishDistanceMeters = finishDistanceMeters;
+            FinishSpeedMetersPerSecond =
+                finishSpeedMetersPerSecond;
+            FinishHeadingToleranceRadians =
+                finishHeadingToleranceRadians;
+            MaximumDistanceToTrajectoryMeters =
+                maximumDistanceToTrajectoryMeters;
+        }
+
+        /// <summary>
+        /// 获取终点位置和剩余弧长允许的误差，单位为m。
+        /// </summary>
+        public double FinishDistanceMeters { get; }
+
+        /// <summary>
+        /// 获取判定轨迹执行完成时允许的最大实际线速度，单位为m/s。
+        /// </summary>
+        public double FinishSpeedMetersPerSecond { get; }
+
+        /// <summary>
+        /// 获取判定轨迹完成时允许的最大终点航向误差，单位为rad。
+        /// </summary>
+        public double FinishHeadingToleranceRadians { get; }
+
+        /// <summary>
+        /// 获取允许车辆偏离参考轨迹的最大距离，单位为m。
+        /// </summary>
+        public double MaximumDistanceToTrajectoryMeters { get; }
+
+        /// <summary>
+        /// 获取控制器当前是否持有并正在执行一条轨迹。
+        /// </summary>
+        public bool IsActive { get; private set; }
+
+        /// <summary>
+        /// 获取最近一次轨迹是否已经满足终点完成条件。
+        /// </summary>
+        public bool IsCompleted { get; private set; }
+
+        /// <summary>
+        /// 获取最近一次控制失败原因，正常时为空字符串。
+        /// </summary>
+        public string LastFailureReason { get; private set; } =
+            string.Empty;
+
+        /// <summary>
+        /// 获取最近一次控制异常，正常时为空。
+        /// </summary>
+        public Exception LastException { get; private set; }
+
+        /// <summary>
+        /// 获取最近一次有效车辆状态。
+        /// </summary>
+        public VehicleState? LastVehicleState { get; private set; }
+
+        /// <summary>
+        /// 获取最近一次车体中心到参考轨迹的投影结果。
+        /// </summary>
+        public TrajectoryProjection? LastProjection { get; private set; }
+
+        /// <summary>
+        /// 获取最近一次发送或准备发送的GCP运动命令。
+        /// </summary>
+        public GcpMotionCommand? LastCommand { get; private set; }
+
+        /// <summary>
+        /// 获取最近控制周期实际交给纵向控制器的参考速度，单位为m/s。
+        /// </summary>
+        public double? LastReferenceSpeedMetersPerSecond { get; private set; }
+
+        /// <summary>
+        /// 停止当前底盘并从起点开始执行指定二维轨迹。
+        /// </summary>
+        public void Start(Trajectory2D trajectory)
+        {
+            if (trajectory == null)
+            {
+                throw new ArgumentNullException(
+                    nameof(trajectory));
+            }
+
+            StopAndResetControllers();
+            _trajectory = trajectory;
+            IsActive = true;
+            IsCompleted = false;
+            ClearDiagnostics();
+        }
+
+        /// <summary>
+        /// 读取本周期车辆状态并执行一次完整的轨迹跟踪控制计算。
+        /// </summary>
+        public ParkingControlCycleResult ExecuteCycle(
+            double deltaTimeSeconds)
+        {
+            EnsureFinitePositive(
+                deltaTimeSeconds,
+                nameof(deltaTimeSeconds));
+
+            if (!IsActive || _trajectory == null)
+            {
+                return ParkingControlCycleResult.Inactive;
+            }
+
+            try
+            {
+                if (!_stateProvider.TryGetState(
+                        out var vehicleState))
+                {
+                    StopForUnavailableState();
+                    return ParkingControlCycleResult
+                        .StateUnavailable;
+                }
+
+                LastVehicleState = vehicleState;
+
+                var projection = TrajectoryProjector.Project(
+                    _trajectory,
+                    vehicleState.PoseInWorld);
+                LastProjection = projection;
+
+                if (projection.DistanceToTrajectoryMeters >
+                    MaximumDistanceToTrajectoryMeters)
+                {
+                    return EnterFault(
+                        "车辆距离参考轨迹" +
+                        $"{projection.DistanceToTrajectoryMeters:F3}m，" +
+                        "超过允许值" +
+                        $"{MaximumDistanceToTrajectoryMeters:F3}m。");
+                }
+
+                if (HasReachedEnd(
+                        vehicleState,
+                        projection))
+                {
+                    CompleteTrajectory();
+                    return ParkingControlCycleResult.Completed;
+                }
+
+                if (HasStoppedAtUnsatisfiedTerminal(
+                        vehicleState,
+                        projection,
+                        out var terminalFailureReason))
+                {
+                    return EnterFault(
+                        terminalFailureReason);
+                }
+
+                var referenceSpeedMetersPerSecond =
+                    ResolveReferenceSpeedForControl(
+                        projection);
+                LastReferenceSpeedMetersPerSecond =
+                    referenceSpeedMetersPerSecond;
+                var context = new PathTrackingContext(
+                    vehicleState,
+                    projection,
+                    referenceSpeedMetersPerSecond,
+                    deltaTimeSeconds);
+                var lateralCommand =
+                    _lateralController.Compute(context);
+                var commandSpeedMetersPerSecond =
+                    _longitudinalController
+                        .ComputeSpeedMetersPerSecond(context);
+                var gcpCommand = _gcpAllocator.Allocate(
+                    commandSpeedMetersPerSecond,
+                    lateralCommand);
+
+                if (!_commandExecutor.Execute(
+                        gcpCommand,
+                        deltaTimeSeconds))
+                {
+                    return EnterFault(
+                        string.IsNullOrWhiteSpace(
+                            _commandExecutor.LastFailureReason)
+                            ? "GCP底盘命令执行失败。"
+                            : _commandExecutor.LastFailureReason);
+                }
+
+                LastCommand =
+                    _commandExecutor.LastSentCommand;
+
+                LastFailureReason = string.Empty;
+                LastException = null;
+                return ParkingControlCycleResult.CommandSent;
+            }
+            catch (Exception exception)
+            {
+                return EnterFault(
+                    "停车机器人轨迹控制周期异常：" +
+                    exception.Message,
+                    exception);
+            }
+        }
+
+        /// <summary>
+        /// 主动取消当前轨迹、立即停车并清除全部控制器状态。
+        /// </summary>
+        public void Cancel()
+        {
+            StopAndResetControllers();
+            _trajectory = null;
+            IsActive = false;
+            IsCompleted = false;
+            ClearDiagnostics();
+        }
+
+        /// <summary>
+        /// 在轨迹起点零速固定点处读取前方速度，并限制为低速起步命令。
+        /// </summary>
+        private double ResolveReferenceSpeedForControl(
+            TrajectoryProjection projection)
+        {
+            var currentReferenceSpeed =
+                projection.ReferencePoint
+                    .ReferenceSpeedMetersPerSecond;
+
+            var requiresStartupRelease =
+                projection.ArcLengthMeters <=
+                    StartupRegionMeters &&
+                projection.RemainingDistanceMeters >
+                    FinishDistanceMeters &&
+                Math.Abs(currentReferenceSpeed) <=
+                    ZeroReferenceSpeedToleranceMetersPerSecond;
+
+            if (!requiresStartupRelease)
+            {
+                return currentReferenceSpeed;
+            }
+
+            var previewArcLengthMeters = Math.Min(
+                _trajectory.TotalLengthMeters,
+                projection.ArcLengthMeters +
+                StartupPreviewDistanceMeters);
+            var previewReferenceSpeed =
+                _trajectory
+                    .SampleAtArcLength(
+                        previewArcLengthMeters)
+                    .ReferenceSpeedMetersPerSecond;
+
+            if (Math.Abs(previewReferenceSpeed) <=
+                ZeroReferenceSpeedToleranceMetersPerSecond)
+            {
+                return 0.0;
+            }
+
+            return Math.Sign(previewReferenceSpeed) *
+                   Math.Min(
+                       Math.Abs(previewReferenceSpeed),
+                       MaximumStartupSpeedMetersPerSecond);
+        }
+
+        /// <summary>
+        /// 根据终点距离、剩余弧长和实际线速度判断轨迹是否完成。
+        /// </summary>
+        private bool HasReachedEnd(
+            VehicleState vehicleState,
+            TrajectoryProjection projection)
+        {
+            if (!vehicleState.HasValidVelocityEstimate)
+            {
+                return false;
+            }
+
+            return projection.RemainingDistanceMeters <=
+                       FinishDistanceMeters &&
+                   CalculateDistanceToEndMeters(
+                       vehicleState) <=
+                       FinishDistanceMeters &&
+                   CalculateHeadingErrorToEndRadians(
+                       vehicleState) <=
+                       FinishHeadingToleranceRadians &&
+                   CalculateActualLinearSpeedMetersPerSecond(
+                       vehicleState) <=
+                       FinishSpeedMetersPerSecond;
+        }
+
+        /// <summary>
+        /// 检查车辆是否已在终点零速参考处停稳但最终位置或航向仍不合格。
+        /// </summary>
+        private bool HasStoppedAtUnsatisfiedTerminal(
+            VehicleState vehicleState,
+            TrajectoryProjection projection,
+            out string failureReason)
+        {
+            failureReason = string.Empty;
+
+            var isTerminalZeroSpeedReference =
+                projection.RemainingDistanceMeters <=
+                    FinishDistanceMeters &&
+                Math.Abs(
+                    projection.ReferencePoint
+                        .ReferenceSpeedMetersPerSecond) <=
+                    ZeroReferenceSpeedToleranceMetersPerSecond;
+
+            if (!isTerminalZeroSpeedReference ||
+                !vehicleState.HasValidVelocityEstimate ||
+                CalculateActualLinearSpeedMetersPerSecond(
+                    vehicleState) >
+                FinishSpeedMetersPerSecond)
+            {
+                return false;
+            }
+
+            var positionErrorMeters =
+                CalculateDistanceToEndMeters(
+                    vehicleState);
+            var headingErrorRadians =
+                CalculateHeadingErrorToEndRadians(
+                    vehicleState);
+
+            failureReason =
+                "车辆已在终点零速参考处停稳，但终点精度不满足要求：" +
+                $"位置误差={positionErrorMeters:F3}m，" +
+                "航向误差=" +
+                $"{AngleMath.RadiansToDegrees(headingErrorRadians):F2}°。";
+            return true;
+        }
+
+        /// <summary>
+        /// 计算实际车体中心到轨迹终点的欧氏距离，单位为m。
+        /// </summary>
+        private double CalculateDistanceToEndMeters(
+            VehicleState vehicleState)
+        {
+            var endPoint = _trajectory.EndPoint.PoseInWorld;
+            var deltaX =
+                vehicleState.PoseInWorld.XMeters -
+                endPoint.XMeters;
+            var deltaY =
+                vehicleState.PoseInWorld.YMeters -
+                endPoint.YMeters;
+
+            return Math.Sqrt(
+                deltaX * deltaX +
+                deltaY * deltaY);
+        }
+
+        /// <summary>
+        /// 计算实际车体航向到轨迹终点航向的最短角度误差绝对值，单位为rad。
+        /// </summary>
+        private double CalculateHeadingErrorToEndRadians(
+            VehicleState vehicleState)
+        {
+            return Math.Abs(
+                AngleMath.ShortestDifferenceRadians(
+                    _trajectory.EndPoint
+                        .PoseInWorld.YawRadians,
+                    vehicleState
+                        .PoseInWorld.YawRadians));
+        }
+
+        /// <summary>
+        /// 计算车体坐标系实际线速度的合速度绝对值，单位为m/s。
+        /// </summary>
+        private static double CalculateActualLinearSpeedMetersPerSecond(
+            VehicleState vehicleState)
+        {
+            return Math.Sqrt(
+                vehicleState.TwistInBody.VxMetersPerSecond *
+                vehicleState.TwistInBody.VxMetersPerSecond +
+                vehicleState.TwistInBody.VyMetersPerSecond *
+                vehicleState.TwistInBody.VyMetersPerSecond);
+        }
+
+        /// <summary>
+        /// 在状态暂不可用时停车并重置反馈控制器，同时保留轨迹等待下一周期恢复。
+        /// </summary>
+        private void StopForUnavailableState()
+        {
+            _commandExecutor.Stop();
+            _lateralController.Reset();
+            _longitudinalController.Reset();
+            LastCommand = null;
+            LastFailureReason =
+                "当前无法获得有效车辆状态，底盘已停车并等待定位恢复。";
+            LastException = null;
+        }
+
+        /// <summary>
+        /// 完成当前轨迹并停车，但保留最后状态和投影供实验记录读取。
+        /// </summary>
+        private void CompleteTrajectory()
+        {
+            StopAndResetControllers();
+            IsActive = false;
+            IsCompleted = true;
+            LastCommand = new GcpMotionCommand(
+                0.0,
+                0.0,
+                0.0);
+            LastFailureReason = string.Empty;
+            LastException = null;
+        }
+
+        /// <summary>
+        /// 发生不可继续的控制故障时停车、退出活动状态并保存诊断信息。
+        /// </summary>
+        private ParkingControlCycleResult EnterFault(
+            string reason,
+            Exception exception = null)
+        {
+            StopAndResetControllers();
+            IsActive = false;
+            IsCompleted = false;
+            LastCommand = null;
+            LastFailureReason = reason;
+            LastException = exception;
+            return ParkingControlCycleResult.Faulted;
+        }
+
+        /// <summary>
+        /// 立即停止底盘并清除横向和纵向控制器的跨周期状态。
+        /// </summary>
+        private void StopAndResetControllers()
+        {
+            _commandExecutor.Stop();
+            _lateralController.Reset();
+            _longitudinalController.Reset();
+        }
+
+        /// <summary>
+        /// 清除上一条轨迹留下的状态、命令和故障诊断信息。
+        /// </summary>
+        private void ClearDiagnostics()
+        {
+            LastVehicleState = null;
+            LastProjection = null;
+            LastCommand = null;
+            LastReferenceSpeedMetersPerSecond = null;
+            LastFailureReason = string.Empty;
+            LastException = null;
+        }
+
+        /// <summary>
+        /// 检查控制参数是否为正有限值。
+        /// </summary>
+        private static void EnsureFinitePositive(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value) ||
+                value <= 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "轨迹控制器距离和周期参数必须是正有限值。");
+            }
+        }
+
+        /// <summary>
+        /// 检查控制参数是否为非负有限值。
+        /// </summary>
+        private static void EnsureFiniteNonNegative(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value) ||
+                value < 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "轨迹控制器速度参数必须是非负有限值。");
+            }
+        }
+    }
+}

ClumsyPilot/Control/Lateral/StanleyLateralController.cs

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

ClumsyPilot/Control/Longitudinal/PidLongitudinalController.cs

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

ClumsyPilot/FleetCrabWalk.cs

@@ -0,0 +1,526 @@
+using System;
+using System.Collections.Generic;
+using System.Numerics;
+using ClumsyCore;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using FundamentalLib;
+using CommonUsage.Chassis;
+using CommonUsage.Mathematics;
+using MDCSToolBox.Clumsy.Movements;
+using MDCSToolBox.Clumsy.Pilot;
+
+namespace MultiWheelC;
+
+// ===== 车队联动-自动蟹行动作 =====
+// 以当前车队中心为起点，构造指定方向和长度的直线路径；
+// 执行侧直接写 MultiVehicleAuto...，由 TickMultiVehicle 自动分支统一下发。
+//
+// 控制思路参考 MDCSToolbox 几何控制器，但实现收在 MultiWheelC 内：
+// 1) 读取主车 Detour 反推车队中心，计算沿直线的进度、横向偏差和车身目标朝向偏差；
+// 2) 根据横向偏差给前后 GCP 同向修正，根据车身目标朝向偏差给前后 GCP 反向修正；
+// 3) 根据终点距离减速，并发布 ideal fleet center 给从车做前馈。
+//
+// 前提：在主车(MultiVehicleMasterEndpoint=="/")运行，且主车有 Detour 定位。
+public class FleetCrabWalk : MovementDefinition
+{
+    /// <summary>路径方向相对启动时车队朝向的夹角(deg，逆时针为正)。</summary>
+    public float CrabAngleDeg = 45f;
+
+    /// <summary>路径方向相对车身目标朝向的夹角(deg，逆时针为正)。MovementTest 会设为 CrabAngleDeg，以保持启动时车身朝向。</summary>
+    public float BodyToPathAngleDeg = 45f;
+
+    /// <summary>路径长度(mm)。</summary>
+    public float CrabLengthMm = 2000f;
+
+    /// <summary>行驶速度(m/s)。</summary>
+    public float CrabSpeed = 0.2f;
+
+    /// <summary>速度命令加速度限制(m/s^2)，小于等于 0 表示不限制。</summary>
+    public float FleetCrabAccel = 0.2f;
+
+    /// <summary>预对齐后正式下发速度前 5 秒加速度限制(m/s^2)，小于等于 0 表示不限制。</summary>
+    public float FleetCrabStartAccel = 0.01f;
+
+    /// <summary>末端开始减速距离(mm)。</summary>
+    public float FleetCrabSlowDistance = 2000f;
+
+    /// <summary>完成距离(mm)，低于该剩余距离结束动作。</summary>
+    public float FleetCrabFinishDistance = 20f;
+
+    /// <summary>末端最低速度(m/s)。</summary>
+    public float FleetCrabFinishSpeed = 0.02f;
+
+    /// <summary>末端减速曲线指数。</summary>
+    public float FleetCrabSlowingPow = 0.8f;
+
+    /// <summary>前后 GCP 舵角修正上限(deg)。</summary>
+    public float GcpThetaThreshold = 95f;
+
+    private bool _stopping;
+
+    private void Cleanup()
+    {
+        var self = PilotDefinition.Self;
+        self.MultiVehicleScriptVx = 0;
+        self.MultiVehicleScriptVy = 0;
+        self.MultiVehicleScriptVth = 0;
+        self.MultiVehicleScriptMode = 0;
+        self.MultiVehicleScriptEnabled = false;
+        self.MultiVehicleAutoVx = 0;
+        self.MultiVehicleAutoFrontTh = 0;
+        self.MultiVehicleAutoRearTh = 0;
+        self.MultiVehicleAutoHasIdeal = false;
+        self.MultiVehicleAutoEnabled = false;
+    }
+
+    public void Stop()
+    {
+        _stopping = true;
+        Cleanup();
+    }
+
+    private static float Clamp(float value, float min, float max)
+    {
+        if (value < min) return min;
+        if (value > max) return max;
+        return value;
+    }
+
+    private static float ClampAbs(float value, float limit)
+    {
+        var absLimit = Math.Abs(limit);
+        if (absLimit <= 0) return value;
+        if (value > absLimit) return absLimit;
+        if (value < -absLimit) return -absLimit;
+        return value;
+    }
+
+    private static float Slew(float current, float target, float maxDelta)
+    {
+        if (maxDelta <= 0) return target;
+        if (target > current + maxDelta) return current + maxDelta;
+        if (target < current - maxDelta) return current - maxDelta;
+        return target;
+    }
+
+    private static float AverageAngle(float frontTh, float rearTh)
+    {
+        var diff = (float)CommonMath.ThDiff(frontTh, rearTh);
+        return (float)CommonMath.RoundTh(rearTh + diff / 2f);
+    }
+
+    private static void ResolveCrabDriveEquivalent(float speed, float rawFrontTh, float rawRearTh, float steerLimit,
+        out float driveSpeed, out float frontTh, out float rearTh, out bool reverseEquivalent, out float rawBaseTh)
+    {
+        var limit = Math.Min(179f, Math.Max(1f, Math.Abs(steerLimit)));
+        rawBaseTh = AverageAngle(rawFrontTh, rawRearTh);
+        driveSpeed = speed;
+        frontTh = rawFrontTh;
+        rearTh = rawRearTh;
+        reverseEquivalent = false;
+
+        if (rawBaseTh > limit)
+        {
+            frontTh = (float)CommonMath.RoundTh(frontTh - 180f);
+            rearTh = (float)CommonMath.RoundTh(rearTh - 180f);
+            driveSpeed = -driveSpeed;
+            reverseEquivalent = true;
+        }
+        else if (rawBaseTh < -limit)
+        {
+            frontTh = (float)CommonMath.RoundTh(frontTh + 180f);
+            rearTh = (float)CommonMath.RoundTh(rearTh + 180f);
+            driveSpeed = -driveSpeed;
+            reverseEquivalent = true;
+        }
+
+        frontTh = ClampAbs(frontTh, limit);
+        rearTh = ClampAbs(rearTh, limit);
+    }
+
+    private static float ProbeSpeed(float speed)
+    {
+        return Math.Abs(speed) > 1e-4f ? speed : 1f;
+    }
+
+    private static bool TryGetMotionYawSign(float frontTh, float rearTh, float driveSpeed, float controlRadius,
+        out float yawSign)
+    {
+        yawSign = 0f;
+        if (Math.Abs(CommonMath.ThDiff(frontTh, rearTh)) <= 1e-3f)
+            return false;
+
+        var radius = Math.Max(1f, Math.Abs(controlRadius));
+        Vector2 pFront = new(radius, 0), pRear = new(-radius, 0),
+            normFront = CommonMath.Transform2D(pFront, frontTh + 90f, Vector2.UnitX),
+            normRear = CommonMath.Transform2D(pRear, rearTh + 90f, Vector2.UnitX);
+        var (intersect, center) = CommonMath.TwoLinesIntersection(pFront, normFront, pRear, normRear);
+        if (!intersect)
+            return false;
+
+        // Match MultiWheelChassis.SendMotion: the tangent side is selected by
+        // rotCenter.Y > 1, and reverse-equivalent motion flips the yaw direction.
+        var tangentSign = center.Y > 1f ? 1f : -1f;
+        var speedSign = driveSpeed >= 0f ? 1f : -1f;
+        yawSign = speedSign * tangentSign;
+        return true;
+    }
+
+    private static float GetYawSplitSign(float baseTh, float speed, float steerLimit, float controlRadius)
+    {
+        const float probeDth = 1f;
+        ResolveCrabDriveEquivalent(ProbeSpeed(speed), baseTh + probeDth, baseTh - probeDth, steerLimit,
+            out var probeSpeed, out var probeFrontTh, out var probeRearTh, out _, out _);
+        return TryGetMotionYawSign(probeFrontTh, probeRearTh, probeSpeed, controlRadius, out var yawSign)
+            ? yawSign
+            : 1f;
+    }
+
+    private static float EstimateLateralVelocity(float bodyTh, float frontTh, float rearTh, float driveSpeed,
+        Vector2 pathLeft)
+    {
+        var motionTh = (float)CommonMath.RoundTh(bodyTh + AverageAngle(frontTh, rearTh));
+        var rad = motionTh / 180f * Math.PI;
+        var dir = new Vector2((float)Math.Cos(rad), (float)Math.Sin(rad));
+        if (driveSpeed < 0f)
+            dir = -dir;
+        return Vector2.Dot(dir, pathLeft);
+    }
+
+    private static float ScoreBiasSign(float baseTh, float bodyTh, float speed, float steerLimit, Vector2 pathLeft,
+        float lateral, float biasProbe)
+    {
+        ResolveCrabDriveEquivalent(ProbeSpeed(speed), baseTh + biasProbe, baseTh + biasProbe, steerLimit,
+            out var probeSpeed, out var probeFrontTh, out var probeRearTh, out _, out _);
+        var lateralVelocity = EstimateLateralVelocity(bodyTh, probeFrontTh, probeRearTh, probeSpeed, pathLeft);
+        return -Math.Sign(lateral) * lateralVelocity;
+    }
+
+    private static float GetLateralBiasSign(float baseTh, float bodyTh, float speed, float steerLimit, Vector2 pathLeft,
+        float lateral)
+    {
+        if (Math.Abs(lateral) <= 1e-3f)
+            return 1f;
+
+        const float probeBias = 1f;
+        var positiveScore = ScoreBiasSign(baseTh, bodyTh, speed, steerLimit, pathLeft, lateral, probeBias);
+        var negativeScore = ScoreBiasSign(baseTh, bodyTh, speed, steerLimit, pathLeft, lateral, -probeBias);
+        return positiveScore >= negativeScore ? 1f : -1f;
+    }
+
+    private static bool TryGetControlFleetCenter(PilotDefinition self, out float centerX, out float centerY,
+        out float centerTh, out string source)
+    {
+        if (self.TryGetFleetCenterFromMembers(out centerX, out centerY, out centerTh))
+        {
+            source = "fleet";
+            return true;
+        }
+
+        if (self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
+        {
+            source = "slam";
+            return true;
+        }
+
+        source = "none";
+        return false;
+    }
+
+    public override IEnumerable<bool> Get()
+    {
+        var self = PilotDefinition.Self;
+        var conf = PilotDefinition.Conf;
+        var chassis = BasicPilotBase.Chassis as MultiWheelChassis;
+        if (chassis == null)
+        {
+            DLog.Log("ABORT: FleetCrabWalk requires MultiWheelChassis.", "FleetCrabDbg");
+            yield break;
+        }
+        _stopping = false;
+
+        DLog.Log(
+            $"ENTER master?={conf.MultiVehicleMasterEndpoint == "/"} endpoint={conf.MultiVehicleMasterEndpoint} " +
+            $"fleetNum={conf.MultiVehicleFleetNum} useDetect={conf.MultiVehicleUseDetect} " +
+            $"syncUseDetour={conf.MultiVehicleSyncUseDetour} useIdealCenter={conf.MultiVehicleAutoUseIdealCenter} " +
+            $"autoFields=true pathMode=relative pathAngle={CrabAngleDeg:0.0} " +
+            $"bodyToPath={BodyToPathAngleDeg:0.0} gcpLimit={GcpThetaThreshold:0.0} " +
+            $"biasFac={conf.BiasFac:0.00} fleetCrabDthFac={conf.FleetCrabDthLinearFac:0.00}",
+            "FleetCrabDbg");
+
+        if (conf.MultiVehicleMasterEndpoint != "/")
+        {
+            DLog.Log($"ABORT: 非主车 (endpoint={conf.MultiVehicleMasterEndpoint})", "FleetCrabDbg");
+            Hedingben.ToastText("车队蟹行需在主车(主车端点=\"/\")运行", "FleetCrab");
+            yield break;
+        }
+
+        // 注意：getCartLocation() 在无有效 Detour 定位时会阻塞——若卡在这里且后面看不到 CENTER 日志，即定位未就绪。
+        DLog.Log("主车校验通过，开始读取车队中心 (getCartLocation 无定位会阻塞)…", "FleetCrabDbg");
+        if (!TryGetControlFleetCenter(self, out var x0, out var y0, out var theta, out var initialCenterSource))
+        {
+            DLog.Log("ABORT: TryGetFleetCenterFromSlam 返回 false (无定位)", "FleetCrabDbg");
+            Hedingben.ToastText("车队蟹行需要主车 Detour 定位", "FleetCrab");
+            yield break;
+        }
+        DLog.Log($"CENTER 车队中心=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
+
+        DLog.Log($"CENTER_SOURCE source={initialCenterSource} center=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
+
+        var pathStart = new Vector2(x0, y0);
+        var pathLengthMm = CrabLengthMm;
+        var phi = CommonMath.RoundTh(theta + CrabAngleDeg);
+        var dst = CommonMath.Transform2D(pathStart, phi, new Vector2(pathLengthMm, 0));
+        var targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg);
+        var phiRad = phi / 180.0 * Math.PI;
+        var pathDir = new Vector2((float)Math.Cos(phiRad), (float)Math.Sin(phiRad));
+        var pathLeft = new Vector2(-pathDir.Y, pathDir.X);
+
+        DLog.Log(
+            $"START center=({x0:0},{y0:0},{theta:0.0}) pathMode=relative " +
+            $"src=({pathStart.X:0},{pathStart.Y:0}) pathAngle={CrabAngleDeg:0.0} bodyToPath={BodyToPathAngleDeg:0.0} " +
+            $"phi={phi:0.0} targetBody={targetBodyTh:0.0} " +
+            $"len={pathLengthMm:0} dst=({dst.X:0},{dst.Y:0}) speed={CrabSpeed:0.000} startAccel={FleetCrabStartAccel:0.000} accel={FleetCrabAccel:0.000} " +
+            $"slow={FleetCrabSlowDistance:0} finishDist={FleetCrabFinishDistance:0} " +
+            $"finishSpeed={FleetCrabFinishSpeed:0.000} slowingPow={FleetCrabSlowingPow:0.00}",
+            "FleetCrabDbg");
+
+        var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
+        var controlRadius = Math.Max(1f, Math.Abs(conf.TestCarSyncDistance) / 2f);
+        ResolveCrabDriveEquivalent(0f, (float)CommonMath.ThDiff(phi, theta),
+            (float)CommonMath.ThDiff(phi, theta), gcpLimit, out _, out var holdFrontTh, out var holdRearTh,
+            out _, out _);
+        var warmStart = DateTime.Now;
+        var warmSeqBaseline = self.BeginFleetMotionWarmup();
+
+        self.MultiVehicleScriptEnabled = false;
+        self.MultiVehicleScriptMode = 0;
+        self.MultiVehicleScriptVx = 0;
+        self.MultiVehicleScriptVy = 0;
+        self.MultiVehicleScriptVth = 0;
+        self.MultiVehicleAutoEnabled = true;
+        self.MultiVehicleAutoVx = 0;
+        self.MultiVehicleAutoFrontTh = holdFrontTh;
+        self.MultiVehicleAutoRearTh = holdRearTh;
+        self.MultiVehicleAutoIdealX = pathStart.X;
+        self.MultiVehicleAutoIdealY = pathStart.Y;
+        self.MultiVehicleAutoIdealTh = targetBodyTh;
+        self.MultiVehicleAutoHasIdeal = true;
+        self.MultiVehicleAutoCmdTime = DateTime.Now;
+        self.PrimeMasterAutoFromSlam();
+        DLog.Log(
+            $"WARMUP auto fields enabled, waiting for fleet startup sync seqBase={warmSeqBaseline} " +
+            $"hold=({holdFrontTh:0.00},{holdRearTh:0.00})",
+            "FleetCrabDbg");
+
+        var warmEnd = warmStart.AddSeconds(Math.Max(1.0f, conf.FleetCrabStartSyncTimeoutSec));
+        var warmIter = 0;
+        var warmReady = false;
+        var warmDetail = "";
+        while (!_stopping && DateTime.Now < warmEnd)
+        {
+            warmIter++;
+            self.MultiVehicleScriptEnabled = false;
+            self.MultiVehicleScriptMode = 0;
+            self.MultiVehicleAutoEnabled = true;
+            self.MultiVehicleAutoVx = 0;
+            self.MultiVehicleAutoFrontTh = holdFrontTh;
+            self.MultiVehicleAutoRearTh = holdRearTh;
+            self.MultiVehicleAutoIdealX = pathStart.X;
+            self.MultiVehicleAutoIdealY = pathStart.Y;
+            self.MultiVehicleAutoIdealTh = targetBodyTh;
+            self.MultiVehicleAutoHasIdeal = true;
+            self.MultiVehicleAutoCmdTime = DateTime.Now;
+            self.PrimeMasterAutoFromSlam();
+            var snap = self.GetFleetCenterSnapshot();
+            int cnt;
+            lock (self.FleetLock) cnt = self.MultiVehicleFleet.Count;
+            if (warmIter % 5 == 0)
+                DLog.Log(
+                    $"WARMUP#{warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) tick={snap.Tick} " +
+                    $"autoEn={self.MultiVehicleAutoEnabled} scriptEn={self.MultiVehicleScriptEnabled} cnt={cnt}/{conf.MultiVehicleFleetNum} " +
+                    $"detail={warmDetail}",
+                    "FleetCrabDbg");
+            if (self.IsFleetMotionWarmupReady(warmStart, warmSeqBaseline,
+                    conf.TestCarSyncTh, conf.TestCarSyncDistance, out warmDetail))
+            {
+                warmReady = true;
+                DLog.Log(
+                    $"WARMUP done iter={warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) cnt={cnt} detail={warmDetail}",
+                    "FleetCrabDbg");
+                break;
+            }
+            yield return true;
+        }
+        if (!warmReady)
+        {
+            DLog.Log($"WARMUP timeout: fleet startup sync failed, abort action. detail={warmDetail}",
+                "FleetCrabDbg");
+            Hedingben.ToastText("车队蟹行启动同步超时，已取消", "FleetCrab");
+            Cleanup();
+            yield break;
+        }
+
+        Hedingben.ToastText($"车队蟹行 路径{phi:0.0}° 车身夹角{BodyToPathAngleDeg:0.0}° 长度{pathLengthMm:0}mm", "FleetCrab");
+
+        if (warmReady && self.TryGetFleetCenterFromMembers(out var warmX, out var warmY, out var warmTh))
+        {
+            x0 = warmX;
+            y0 = warmY;
+            theta = warmTh;
+            pathStart = new Vector2(x0, y0);
+            phi = CommonMath.RoundTh(theta + CrabAngleDeg);
+            dst = CommonMath.Transform2D(pathStart, phi, new Vector2(pathLengthMm, 0));
+            targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg);
+            phiRad = phi / 180.0 * Math.PI;
+            pathDir = new Vector2((float)Math.Cos(phiRad), (float)Math.Sin(phiRad));
+            pathLeft = new Vector2(-pathDir.Y, pathDir.X);
+            self.MultiVehicleAutoIdealX = pathStart.X;
+            self.MultiVehicleAutoIdealY = pathStart.Y;
+            self.MultiVehicleAutoIdealTh = targetBodyTh;
+            self.MultiVehicleAutoCmdTime = DateTime.Now;
+            DLog.Log(
+                $"WARMUP_REBASE source=fleet center=({x0:0},{y0:0},{theta:0.0}) phi={phi:0.0} targetBody={targetBodyTh:0.0} dst=({dst.X:0},{dst.Y:0})",
+                "FleetCrabDbg");
+        }
+
+        var iter = 0;
+        var lastLog = DateTime.MinValue;
+        var finishDistance = Math.Max(0f, FleetCrabFinishDistance);
+        var slowDistance = Math.Max(finishDistance + 1f, FleetCrabSlowDistance);
+        var baseSpeed = Math.Abs(CrabSpeed);
+        var finishSpeed = Math.Min(baseSpeed, Math.Abs(FleetCrabFinishSpeed));
+        var slowingPow = Math.Max(0.01f, FleetCrabSlowingPow);
+        var accel = Math.Abs(FleetCrabAccel);
+        var startAccel = Math.Abs(FleetCrabStartAccel);
+        var cmdSpeed = 0f;
+        var lastTick = DateTime.Now;
+        var speedRampStart = DateTime.Now;
+        var stopReason = "done";
+
+        while (!_stopping)
+        {
+            iter++;
+
+            if (!TryGetControlFleetCenter(self, out var cx, out var cy, out var cth, out var centerSource))
+            {
+                stopReason = "fleet center invalid";
+                DLog.Log("ABORT: TryGetControlFleetCenter returned false during auto crab.", "FleetCrabDbg");
+                break;
+            }
+            var delta = new Vector2(cx - pathStart.X, cy - pathStart.Y);
+            var along = Vector2.Dot(delta, pathDir);
+            var lateral = Vector2.Dot(delta, pathLeft);
+            var remain = pathLengthMm - along;
+            if (remain <= finishDistance)
+                break;
+
+            var targetSpeed = baseSpeed;
+            var slowRatio = 1f;
+            if (remain < slowDistance)
+            {
+                slowRatio = (float)Math.Pow(Clamp(Math.Max(0, remain) / slowDistance, 0f, 1f), slowingPow);
+                targetSpeed = slowRatio * (baseSpeed - finishSpeed) + finishSpeed;
+            }
+            var now = DateTime.Now;
+            var dt = Math.Max(0.001f, (float)(now - lastTick).TotalSeconds);
+            lastTick = now;
+            var rampElapsed = (now - speedRampStart).TotalSeconds;
+            var activeAccel = rampElapsed < 5.0 ? startAccel : accel;
+            var speed = activeAccel > 0 ? Slew(cmdSpeed, targetSpeed, activeAccel * dt) : targetSpeed;
+            cmdSpeed = speed;
+
+            var baseCrabTh = (float)CommonMath.ThDiff(phi, cth);
+            var headingErr = (float)CommonMath.ThDiff(targetBodyTh, cth);
+            var headingErrReverse = (float)CommonMath.ThDiff(cth, targetBodyTh);
+            var targetBodyToPath = (float)CommonMath.ThDiff(phi, targetBodyTh);
+            var rawBiasMagnitude = (float)(Math.Atan(conf.BiasFac * Math.Abs(lateral) / 1000f /
+                Math.Max(speed, 0.3f)) / Math.PI * 180.0);
+            var biasSign = GetLateralBiasSign(baseCrabTh, cth, speed, gcpLimit, pathLeft, lateral);
+            var rawBiasItem = rawBiasMagnitude * biasSign;
+            var biasItem = ClampAbs(rawBiasItem, conf.BiasThreshold);
+            var yawSplitSign = GetYawSplitSign(baseCrabTh + biasItem, speed, gcpLimit, controlRadius);
+            var rawDthItem = conf.FleetCrabDthLinearFac * headingErr * yawSplitSign;
+            var dthItem = ClampAbs(rawDthItem, conf.FleetCrabDthLinearThreshold);
+            var rawFrontTh = baseCrabTh + biasItem + dthItem;
+            var rawRearTh = baseCrabTh + biasItem - dthItem;
+            ResolveCrabDriveEquivalent(speed, rawFrontTh, rawRearTh, gcpLimit, out var driveSpeed,
+                out var frontTh, out var rearTh, out var reverseEquivalent, out var rawBaseTh);
+            holdFrontTh = frontTh;
+            holdRearTh = rearTh;
+            var idealAlong = Clamp(along, 0f, pathLengthMm);
+            var ideal = pathStart + pathDir * idealAlong;
+
+            self.MultiVehicleScriptEnabled = false;
+            self.MultiVehicleScriptMode = 0;
+            self.MultiVehicleScriptVx = 0;
+            self.MultiVehicleScriptVy = 0;
+            self.MultiVehicleScriptVth = 0;
+            self.MultiVehicleAutoEnabled = true;
+            self.MultiVehicleAutoVx = driveSpeed;
+            self.MultiVehicleAutoFrontTh = frontTh;
+            self.MultiVehicleAutoRearTh = rearTh;
+            self.MultiVehicleAutoIdealX = ideal.X;
+            self.MultiVehicleAutoIdealY = ideal.Y;
+            self.MultiVehicleAutoIdealTh = targetBodyTh;
+            self.MultiVehicleAutoHasIdeal = true;
+            self.MultiVehicleAutoCmdTime = DateTime.Now;
+
+            if ((DateTime.Now - lastLog).TotalMilliseconds >= 300)
+            {
+                lastLog = DateTime.Now;
+                var snap = self.GetFleetCenterSnapshot();
+                int fleetCnt;
+                lock (self.FleetLock) fleetCnt = self.MultiVehicleFleet.Count;
+                DLog.Log(
+                    $"ITER#{iter} centerSrc={centerSource} center=({cx:0},{cy:0},{cth:0.0}) snap=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) " +
+                    $"along={along:0} lateral={lateral:0} remain={remain:0} headingErr={headingErr:0.0} " +
+                    $"baseTh={baseCrabTh:0.0} bias={biasItem:0.0} dth={dthItem:0.0} " +
+                    $"slowRatio={slowRatio:0.000} targetV={targetSpeed:0.000} rampT={rampElapsed:0.0} accel={activeAccel:0.000} auto=(vx:{driveSpeed:0.000},fTh:{frontTh:0.0},rTh:{rearTh:0.0}) " +
+                    $"ideal=({ideal.X:0},{ideal.Y:0},{targetBodyTh:0.0}) scriptEn={self.MultiVehicleScriptEnabled} " +
+                    $"cnt={fleetCnt}/{conf.MultiVehicleFleetNum}",
+                    "FleetCrabDbg");
+                DLog.Log(
+                    $"CTRL iter={iter} centerSrc:{centerSource} phi:{phi:0.00} targetBody:{targetBodyTh:0.00} startTheta:{theta:0.00} " +
+                    $"cth:{cth:0.00} crabAngle:{CrabAngleDeg:0.00} bodyToPathCfg:{BodyToPathAngleDeg:0.00} " +
+                    $"targetBodyToPath:{targetBodyToPath:0.00} bodyToPathNow:{baseCrabTh:0.00} " +
+                    $"headingErr(target-current):{headingErr:0.00} reverse(current-target):{headingErrReverse:0.00} yawSign:{yawSplitSign:0} " +
+                    $"fleetCrabDthFac:{conf.FleetCrabDthLinearFac:0.000} rawDth:{rawDthItem:0.00} dth:{dthItem:0.00} dthLimit:{conf.FleetCrabDthLinearThreshold:0.00} " +
+                    $"lateral:{lateral:0.0} biasFac:{conf.BiasFac:0.000} biasSign:{biasSign:0} rawBias:{rawBiasItem:0.00} bias:{biasItem:0.00} biasLimit:{conf.BiasThreshold:0.00} " +
+                    $"baseTh:{baseCrabTh:0.00} rawBase:{rawBaseTh:0.00} rawOut(f:{rawFrontTh:0.00},r:{rawRearTh:0.00}) " +
+                    $"out(f:{frontTh:0.00},r:{rearTh:0.00}) gcpLimit:{gcpLimit:0.00} revEq:{reverseEquivalent} " +
+                    $"speedRaw:{speed:0.000} speed:{driveSpeed:0.000} rampT:{rampElapsed:0.0} accel:{activeAccel:0.000} along:{along:0.0} remain:{remain:0.0} ideal=({ideal.X:0.0},{ideal.Y:0.0},{targetBodyTh:0.00})",
+                    "FleetCrabHeadingDbg");
+            }
+            yield return true;
+        }
+
+        if (_stopping)
+            stopReason = "stop";
+
+        self.MultiVehicleAutoVx = 0;
+        self.MultiVehicleAutoFrontTh = holdFrontTh;
+        self.MultiVehicleAutoRearTh = holdRearTh;
+        self.MultiVehicleAutoCmdTime = DateTime.Now;
+        DLog.Log(
+            $"STOP_HOLD iter={iter} reason={stopReason} hold=(fTh:{holdFrontTh:0.0},rTh:{holdRearTh:0.0}) cmdSpeed={cmdSpeed:0.000}",
+            "FleetCrabDbg");
+        var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
+        while (!_stopping && DateTime.Now < settleEnd)
+        {
+            self.MultiVehicleScriptEnabled = false;
+            self.MultiVehicleScriptMode = 0;
+            self.MultiVehicleAutoEnabled = true;
+            self.MultiVehicleAutoVx = 0;
+            self.MultiVehicleAutoFrontTh = holdFrontTh;
+            self.MultiVehicleAutoRearTh = holdRearTh;
+            self.MultiVehicleAutoCmdTime = DateTime.Now;
+            yield return true;
+        }
+
+        Cleanup();
+        Hedingben.ToastText("车队蟹行完成", "FleetCrab");
+        DLog.Log($"DONE iter={iter} reason={stopReason}", "FleetCrabDbg");
+    }
+}

ClumsyPilot/FleetCurveWalk.cs

@@ -0,0 +1,411 @@
+using System;
+using System.Collections.Generic;
+using System.Globalization;
+using System.Numerics;
+using ClumsyCore;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using FundamentalLib;
+using CommonUsage.Chassis;
+using CommonUsage.Mathematics;
+using MDCSToolBox.Clumsy.MotionControllers;
+using MDCSToolBox.Clumsy.Movements;
+using MDCSToolBox.Clumsy.Pilot;
+using MDCSToolBox.Clumsy.Tracks;
+
+namespace MultiWheelC;
+
+public class FleetCurveWalk : MovementDefinition
+{
+    public BezierTrack Track;
+    public List<Vector2> ControlPoints = new();
+    public float CurveSpeed = 0.2f;
+    public float CarDirectionBias = 0f;
+    public int BezierResolution = 100;
+    public float SlowDistance = 2000f;
+    public float FinishDistance = 20f;
+    public float FinishSpeed = 0.02f;
+    public float SlowingPow = 0.8f;
+    public float GcpThetaThreshold = 95f;
+    public float StartSyncTimeoutSec = 8f;
+
+    private bool _stopping;
+    private MultiWheelGeometricController _controller;
+    private MultiWheelChassis _chassis;
+    private bool _savedControlPoints;
+    private float _savedControlRadius;
+    private Vector2 _savedGcp0;
+    private Vector2 _savedGcp1;
+
+    public void Stop()
+    {
+        _stopping = true;
+        if (_controller != null)
+            _controller.BreakAndHold = true;
+        Cleanup();
+    }
+
+    public static bool TryParsePointList(string text, out List<Vector2> points, out string error)
+    {
+        points = new List<Vector2>();
+        error = "";
+        if (string.IsNullOrWhiteSpace(text))
+        {
+            error = "empty control point list";
+            return false;
+        }
+
+        var segments = text.Split(new[] { ';', '|' }, StringSplitOptions.RemoveEmptyEntries);
+        for (var i = 0; i < segments.Length; i++)
+        {
+            var pair = segments[i].Split(new[] { ',', ' ', '\t' }, StringSplitOptions.RemoveEmptyEntries);
+            if (pair.Length != 2)
+            {
+                error = $"invalid point #{i + 1}: {segments[i]}";
+                return false;
+            }
+
+            if (!TryParseFloat(pair[0], out var x) || !TryParseFloat(pair[1], out var y))
+            {
+                error = $"invalid number in point #{i + 1}: {segments[i]}";
+                return false;
+            }
+            points.Add(new Vector2(x, y));
+        }
+
+        if (points.Count < 3)
+        {
+            error = "Bezier curve requires at least 3 control points";
+            return false;
+        }
+
+        return true;
+    }
+
+    public static List<Vector2> BuildRelativeControlPoints(Vector2 start, float startTh, List<Vector2> relativePoints)
+    {
+        var source = relativePoints ?? new List<Vector2>();
+        var normalized = new List<Vector2>();
+        if (source.Count == 0 || Vector2.Distance(source[0], Vector2.Zero) > 1f)
+            normalized.Add(Vector2.Zero);
+        for (var i = 0; i < source.Count; i++)
+            normalized.Add(source[i]);
+        if (normalized.Count < 2)
+            normalized.Add(new Vector2(1000f, 0f));
+        if (normalized.Count < 3)
+            normalized.Add(new Vector2(2000f, 0f));
+
+        var result = new List<Vector2>();
+        for (var i = 0; i < normalized.Count; i++)
+            result.Add(CommonMath.Transform2D(start, startTh, normalized[i]));
+        return result;
+    }
+
+    public static List<Vector2> BuildAgvControlPoints(float srcX, float srcY, float dstX, float dstY,
+        params float[] controlPointCoords)
+    {
+        var src = new Vector2(srcX, srcY);
+        var dst = new Vector2(dstX, dstY);
+        var result = new List<Vector2>();
+        if (controlPointCoords == null || controlPointCoords.Length == 0)
+        {
+            result.Add(src);
+            result.Add((src + dst) / 2f);
+            result.Add(dst);
+            return result;
+        }
+
+        if (controlPointCoords.Length % 2 != 0)
+            throw new ArgumentException("FleetCurve controlPointCoords must contain x,y pairs.");
+
+        var supplied = new List<Vector2>();
+        for (var i = 0; i < controlPointCoords.Length; i += 2)
+            supplied.Add(new Vector2(controlPointCoords[i], controlPointCoords[i + 1]));
+
+        if (supplied.Count >= 3 &&
+            Vector2.Distance(supplied[0], src) <= 10f &&
+            Vector2.Distance(supplied[supplied.Count - 1], dst) <= 10f)
+            return supplied;
+
+        result.Add(src);
+        for (var i = 0; i < supplied.Count; i++)
+            result.Add(supplied[i]);
+        result.Add(dst);
+        if (result.Count < 3)
+            result.Insert(1, (src + dst) / 2f);
+        return result;
+    }
+
+    private static bool TryParseFloat(string text, out float value)
+    {
+        return float.TryParse(text, NumberStyles.Float, CultureInfo.InvariantCulture, out value) ||
+               float.TryParse(text, out value);
+    }
+
+    private static float ClampAbs(float value, float limit)
+    {
+        var absLimit = Math.Abs(limit);
+        if (absLimit <= 0) return value;
+        if (value > absLimit) return absLimit;
+        if (value < -absLimit) return -absLimit;
+        return value;
+    }
+
+    private static bool TryGetControlFleetCenter(PilotDefinition self, out float centerX, out float centerY,
+        out float centerTh, out string source)
+    {
+        if (self.TryGetFleetCenterFromMembers(out centerX, out centerY, out centerTh))
+        {
+            source = "fleet";
+            return true;
+        }
+
+        if (self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
+        {
+            source = "slam";
+            return true;
+        }
+
+        source = "none";
+        return false;
+    }
+
+    private void Cleanup()
+    {
+        var self = PilotDefinition.Self;
+        self.MultiVehicleScriptVx = 0;
+        self.MultiVehicleScriptVy = 0;
+        self.MultiVehicleScriptVth = 0;
+        self.MultiVehicleScriptMode = 0;
+        self.MultiVehicleScriptEnabled = false;
+        self.MultiVehicleAutoVx = 0;
+        self.MultiVehicleAutoFrontTh = 0;
+        self.MultiVehicleAutoRearTh = 0;
+        self.MultiVehicleAutoHasIdeal = false;
+        self.MultiVehicleAutoEnabled = false;
+        RestoreControlPointRadius();
+    }
+
+    private void ApplyFleetControlPointRadius(MultiWheelChassis chassis, float radius)
+    {
+        if (!_savedControlPoints)
+        {
+            _chassis = chassis;
+            _savedControlRadius = chassis.ControlPointRadius;
+            var gcps = chassis.GetGeometricControlPoints();
+            if (gcps.Count >= 2)
+            {
+                _savedGcp0 = gcps[0].Position;
+                _savedGcp1 = gcps[1].Position;
+            }
+            _savedControlPoints = true;
+        }
+
+        chassis.ControlPointRadius = radius;
+        var points = chassis.GetGeometricControlPoints();
+        if (points.Count >= 2)
+        {
+            points[0].Position = new Vector2(radius, 0);
+            points[1].Position = new Vector2(-radius, 0);
+        }
+    }
+
+    private void RestoreControlPointRadius()
+    {
+        if (!_savedControlPoints || _chassis == null)
+            return;
+
+        _chassis.ControlPointRadius = _savedControlRadius;
+        var points = _chassis.GetGeometricControlPoints();
+        if (points.Count >= 2)
+        {
+            points[0].Position = _savedGcp0;
+            points[1].Position = _savedGcp1;
+        }
+        _savedControlPoints = false;
+    }
+
+    private static void WriteWarmupAuto(PilotDefinition self, Vector2 idealPos, float idealTh,
+        float frontTh, float rearTh)
+    {
+        self.MultiVehicleScriptEnabled = false;
+        self.MultiVehicleScriptMode = 0;
+        self.MultiVehicleScriptVx = 0;
+        self.MultiVehicleScriptVy = 0;
+        self.MultiVehicleScriptVth = 0;
+        self.MultiVehicleAutoEnabled = true;
+        self.MultiVehicleAutoVx = 0;
+        self.MultiVehicleAutoFrontTh = frontTh;
+        self.MultiVehicleAutoRearTh = rearTh;
+        self.MultiVehicleAutoIdealX = idealPos.X;
+        self.MultiVehicleAutoIdealY = idealPos.Y;
+        self.MultiVehicleAutoIdealTh = idealTh;
+        self.MultiVehicleAutoHasIdeal = true;
+        self.MultiVehicleAutoCmdTime = DateTime.Now;
+    }
+
+    public override IEnumerable<bool> Get()
+    {
+        var self = PilotDefinition.Self;
+        var conf = PilotDefinition.Conf;
+        var chassis = BasicPilotBase.Chassis as MultiWheelChassis;
+        _stopping = false;
+
+        if (chassis == null)
+        {
+            DLog.Log("ABORT: FleetCurveWalk requires MultiWheelChassis.", "FleetCurveDbg");
+            yield break;
+        }
+
+        if (conf.MultiVehicleMasterEndpoint != "/")
+        {
+            DLog.Log($"ABORT: FleetCurveWalk must run on master endpoint, endpoint={conf.MultiVehicleMasterEndpoint}",
+                "FleetCurveDbg");
+            Hedingben.ToastText("FleetCurve requires master vehicle", "FleetCurve");
+            yield break;
+        }
+
+        if (Track == null && (ControlPoints == null || ControlPoints.Count < 3))
+        {
+            DLog.Log("ABORT: FleetCurveWalk requires a BezierTrack or at least 3 control points.", "FleetCurveDbg");
+            Hedingben.ToastText("FleetCurve requires track or >=3 control points", "FleetCurve");
+            yield break;
+        }
+
+        if (!TryGetControlFleetCenter(self, out var x0, out var y0, out var theta, out var initialCenterSource))
+        {
+            DLog.Log("ABORT: FleetCurveWalk failed to read fleet center.", "FleetCurveDbg");
+            Hedingben.ToastText("FleetCurve requires master localization", "FleetCurve");
+            yield break;
+        }
+
+        var baseSpeed = Math.Abs(CurveSpeed);
+        if (baseSpeed <= 1e-4f)
+        {
+            DLog.Log("ABORT: FleetCurveWalk speed is zero.", "FleetCurveDbg");
+            yield break;
+        }
+
+        var resolution = Math.Max(2, BezierResolution);
+        var speedFinish = Math.Min(baseSpeed, Math.Abs(FinishSpeed));
+        var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
+        var controlRadius = Math.Max(1f, Math.Abs(conf.TestCarSyncDistance) / 2f);
+
+        ApplyFleetControlPointRadius(chassis, controlRadius);
+
+        try
+        {
+            var track = Track;
+            var trackSource = "external";
+            if (track == null)
+            {
+                var points = new List<Vector2>(ControlPoints);
+                track = new BezierTrack(points, resolution);
+                trackSource = "controlPoints";
+            }
+            track.CarDirectionBias = CarDirectionBias;
+            track.Speed = baseSpeed;
+
+            var center = new Vector2(x0, y0);
+            var (idealPos, idealAngle, bias, pd) = track.QueryTangentPoint(center);
+            var carDirection = (float)CommonMath.ThDiff(theta, CarDirectionBias);
+            var holdTh = ClampAbs((float)CommonMath.ThDiff(idealAngle, carDirection), gcpLimit);
+            var targetBodyTh = (float)CommonMath.RoundTh(idealAngle + CarDirectionBias);
+
+            DLog.Log(
+                $"START center=({x0:0},{y0:0},{theta:0.0}) source={initialCenterSource} " +
+                $"track={track.GetType().Name} trackSource={trackSource} controls={ControlPoints?.Count ?? 0} " +
+                $"len={track.Length():0} speed={baseSpeed:0.000} bias={CarDirectionBias:0.0} " +
+                $"query=({idealPos.X:0},{idealPos.Y:0}) tangent={idealAngle:0.0} targetBody={targetBodyTh:0.0} " +
+                $"pathBias={bias:0.0} pd={pd:0.0} hold={holdTh:0.0} radius={controlRadius:0}",
+                "FleetCurveDbg");
+
+            var warmStart = DateTime.Now;
+            var warmSeqBaseline = self.BeginFleetMotionWarmup();
+            WriteWarmupAuto(self, idealPos, targetBodyTh, holdTh, holdTh);
+            self.PrimeMasterAutoFromSlam();
+
+            var warmEnd = warmStart.AddSeconds(Math.Max(1.0f, StartSyncTimeoutSec));
+            var warmIter = 0;
+            var warmReady = false;
+            var warmDetail = "";
+            while (!_stopping && DateTime.Now < warmEnd)
+            {
+                warmIter++;
+                WriteWarmupAuto(self, idealPos, targetBodyTh, holdTh, holdTh);
+                self.PrimeMasterAutoFromSlam();
+
+                if (warmIter % 5 == 0)
+                {
+                    var snap = self.GetFleetCenterSnapshot();
+                    int cnt;
+                    lock (self.FleetLock) cnt = self.MultiVehicleFleet.Count;
+                    DLog.Log(
+                        $"WARMUP#{warmIter} snap=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) " +
+                        $"cnt={cnt}/{conf.MultiVehicleFleetNum} detail={warmDetail}",
+                        "FleetCurveDbg");
+                }
+
+                if (self.IsFleetMotionWarmupReady(warmStart, warmSeqBaseline,
+                        conf.TestCarSyncTh, conf.TestCarSyncDistance, out warmDetail))
+                {
+                    warmReady = true;
+                    DLog.Log($"WARMUP done iter={warmIter} detail={warmDetail}", "FleetCurveDbg");
+                    break;
+                }
+                yield return true;
+            }
+
+            if (!warmReady)
+            {
+                DLog.Log($"WARMUP timeout: fleet startup sync failed, abort curve action. detail={warmDetail}",
+                    "FleetCurveDbg");
+                Hedingben.ToastText("FleetCurve startup sync timeout", "FleetCurve");
+                Cleanup();
+                yield break;
+            }
+
+            _controller = new ChassisController { BaseSpeed = baseSpeed }.Get();
+            _controller.MultiVehicleSync = true;
+            _controller.BaseSpeed = baseSpeed;
+            _controller.SlowDistance = Math.Max(FinishDistance + 1f, SlowDistance);
+            _controller.FinishDistance = Math.Max(0f, FinishDistance);
+            _controller.FinishSpeed = speedFinish;
+            _controller.SlowingPow = Math.Max(0.01f, SlowingPow);
+            _controller.GcpThetaThreshold = gcpLimit;
+            _controller.AddTrack(track, "FleetCurve");
+
+            Hedingben.ToastText($"FleetCurve len {track.Length():0}mm speed {baseSpeed:0.00}", "FleetCurve");
+
+            foreach (var running in _controller.Track())
+            {
+                if (_stopping)
+                    break;
+                if (!running)
+                    break;
+                yield return true;
+            }
+
+            if (!_stopping)
+            {
+                self.MultiVehicleAutoVx = 0;
+                self.MultiVehicleAutoCmdTime = DateTime.Now;
+                var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
+                while (!_stopping && DateTime.Now < settleEnd)
+                {
+                    self.MultiVehicleAutoEnabled = true;
+                    self.MultiVehicleAutoVx = 0;
+                    self.MultiVehicleAutoCmdTime = DateTime.Now;
+                    yield return true;
+                }
+            }
+
+            DLog.Log($"DONE stopping={_stopping}", "FleetCurveDbg");
+        }
+        finally
+        {
+            Cleanup();
+            _controller = null;
+        }
+    }
+}

ClumsyPilot/MovementTests.TireFollowing.cs

@@ -0,0 +1,606 @@
+using ClumsyCore;
+using ClumsyCore.DTools;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using ClumsyCore.Utilities;
+using ClumsyDance.ClumsyDance.Detectors;
+using ClumsyDance.ClumsyWalk.Detectors;
+using CommonUsage.Chassis;
+using CommonUsage.Mathematics;
+using FundamentalLib;
+using MDCSToolBox.Clumsy.Calibration;
+using MDCSToolBox.Clumsy.Tracks;
+using MDCSToolBox.Commons.Controllers;
+using System;
+using System.Collections.Generic;
+using System.Drawing;
+using System.Linq;
+using System.Numerics;
+using System.Security.Cryptography;
+using System.Threading;
+using LineSegment = ClumsyCore.Utilities.LineSegment;
+
+namespace MultiWheelC
+{
+    [MovementTest(name = "轮胎检测")]
+    public class TireDetect : MovementTest
+    {
+        public override void TestStop()
+        {
+            _running = false;
+        }
+
+        public override void Test()
+        {
+            _painter = UI.GetPainter("TwoLegDetectTest", false);
+            _painter.Clear();
+
+            var frontlidar = UI.GetInput("1是用前雷达识别，2是用后雷达识别");
+            var result = int.Parse(frontlidar.ToString());
+            var lastDetectX = result == 1 ? PilotDefinition.Conf.TireFollowingStage1GuessX : -PilotDefinition.Conf.TireFollowingStage1GuessX;
+            var lastDetectY = 0f;
+            while (_running)
+            {
+                var ld = Detect(lastDetectX, SetFilters(lastDetectX, lastDetectY), result == 1 ? true : false);
+                if(ld == null)
+                {
+                    //Console.WriteLine("ld == null");
+                    continue;
+                }
+                _painter.Clear();
+                var center = (ld.Src + ld.Dst) / 2;
+                var distanceToCarOrigin = Vector2.Distance(Vector2.Zero, center);
+                var distanceLabelPos = center / 2;
+                _painter.DrawLine(Color.Cyan, Vector2.Zero, center, width: 2);
+                _painter.DrawText(Color.Yellow, $"{distanceToCarOrigin:F3}", distanceLabelPos.X, distanceLabelPos.Y);
+                lastDetectX = center.X;
+                lastDetectY = center.Y;
+                Thread.Sleep(100);
+            }
+        }
+
+        public static LineSegment Detect(float guessX, List<DetectFilter> filters, bool frontlidar)
+        {
+            return new Lidar2dDetect2LegTray()
+            {
+                BlobDist = frontlidar ? PilotDefinition.Conf.TireFrontTwoLegBlobDist : PilotDefinition.Conf.TireBackTwoLegBlobDist,
+                BlobPtCount = frontlidar ? PilotDefinition.Conf.TireTwoLegBlobPtCount : PilotDefinition.Conf.TireTwoLegBlobPtCount,
+                BlobSize = frontlidar ? PilotDefinition.Conf.TireFrontTwoLegBlobSize : PilotDefinition.Conf.TireBackTwoLegBlobSize,
+                CenterChange = Tuple.Create(frontlidar ? PilotDefinition.Conf.TireFrontTwoLegCenterChangeX : PilotDefinition.Conf.TireBackTwoLegCenterChangeX, 0f, 0f),
+                LegWidth = PilotDefinition.Conf.TireTwoLegWidth,
+                LegWidthErr = frontlidar ? PilotDefinition.Conf.TireTwoLegWidthErr : PilotDefinition.Conf.TireTwoLegWidthErr,
+                Padding = frontlidar ? PilotDefinition.Conf.TireFrontPadding : PilotDefinition.Conf.TireBackPadding,
+                PillarFindingScope = frontlidar ? PilotDefinition.Conf.TireFrontTwoLegPillarFindingScope : PilotDefinition.Conf.TireBackTwoLegPillarFindingScope,
+                SgnDir = PilotDefinition.Conf.TwoLegSgnDir,
+            }.DetectWithGuess(frontlidar ? "frontlidar" : "leftlidar,rightlidar", new LineSegment(new Vector2(guessX, 0), Vector2.Zero),
+                guessCoordinateSystem: CoordinateSystem.Car2D, outCoordinateSystem: CoordinateSystem.Car2D, filters);
+        }
+
+        private List<DetectFilter> SetFilters(float guessCenterX, float guessCenterY)
+        {
+            var painter = UI.GetPainter("GeneralFollowing.SetFilters", false);
+            painter.Clear();
+            painter.Clear(3000);
+
+            var box = new Vector2[]
+            {
+                new (guessCenterX - PilotDefinition.Conf.TireFilterLength / 2, guessCenterY - PilotDefinition.Conf.TireFilterWidth / 2),
+                new (guessCenterX + PilotDefinition.Conf.TireFilterLength / 2, guessCenterY - PilotDefinition.Conf.TireFilterWidth / 2),
+                new (guessCenterX + PilotDefinition.Conf.TireFilterLength / 2, guessCenterY + PilotDefinition.Conf.TireFilterWidth / 2),
+                new (guessCenterX - PilotDefinition.Conf.TireFilterLength / 2, guessCenterY + PilotDefinition.Conf.TireFilterWidth / 2),
+            };
+
+            for (var i = 0; i < box.Length; ++i)
+                painter.DrawLine(Color.DarkOliveGreen, box[i], box[(i + 1) % 4]);
+
+            // PC filter in car coordinate frame
+            return new List<DetectFilter>()
+            {
+                new(CoordinateSystem.Car2D,
+                    p => LessMath.IsPointInPolygon4(
+                        box.Select(v => new PointF(v.X, v.Y)).ToArray(), new PointF(p.X, p.Y))),
+            };
+        }
+
+        private Painter _painter;
+        private bool _running = true;
+    }
+
+    [MovementTest(name = "钻车测试")]
+    public class FollowTire : MovementTest
+    {
+        public override void TestStop()
+        {
+            _dt?.Stop();
+        }
+
+        public override void Test()
+        {
+            var front = UI.GetInput("1是用前雷达识别，2是用后雷达识别");
+            var result = int.Parse(front.ToString());
+            var lidarname = result == 1 ? "前雷达" : "后雷达";
+            DLog.Log($"开始钻车测试，用{lidarname}识别", "TireFollowing");
+
+            var following = new TireFollowing()
+            {
+                GetController = () => new ChassisController().Get(),
+                GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
+                GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
+                detectors = new List<TireFollowing.DetectorDefinition>()
+                {
+                    new TireFollowing.DetectorDefinition()
+                    {
+                        DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, result == 1 ? true : false),
+                        StartGuessingX = result == 1 ? PilotDefinition.Conf.TireFollowingStage1GuessX : -PilotDefinition.Conf.TireFollowingStage1GuessX,
+                        StartGuessingY = 0,
+                        SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
+                        FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
+                        PathTransformation = new Tuple<float, float, float>(
+                        result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
+                        result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
+                        0)
+                    },
+                    new TireFollowing.DetectorDefinition()
+                    {
+                        DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, result == 1 ? true : false),
+                        StartGuessingX = result == 1 ? PilotDefinition.Conf.TireFollowingStage2GuessX : -PilotDefinition.Conf.TireFollowingStage2GuessX,
+                        StartGuessingY = 0,
+                        SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
+                        FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
+                        PathTransformation = new Tuple<float, float, float>(
+                        result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
+                        result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
+                        0)
+                    },
+                },
+                CarDirection = result == 1 ? 0f : 180f,
+                SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
+                MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
+                TireNum = PilotDefinition.Conf.TireFollowingTireNum,
+                WalkBlindTh = result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarWalkBlindTh : PilotDefinition.Conf.TireFollowingBackLidarWalkBlindTh,
+            };
+            _dt = new DriveTask(following.Get());
+            _dt.Wait();
+            DLog.Log($"结束钻车测试", "TireFollowing");
+        }
+        private DriveTask _dt;
+    }
+
+    [MovementTest(name = "离车测试")]
+    public class LeaveCar : MovementTest
+    {
+        public override void TestStop()
+        {
+            _dt?.Stop();
+        }
+
+        public override void Test()
+        {
+            DLog.Log($"开始离车测试，用后雷达识别", "TireFollowing");
+            var following = new TireFollowing()
+            {
+                GetController = () => new ChassisController().Get(),
+                GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
+                GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
+                detectors = new List<TireFollowing.DetectorDefinition>()
+                {
+                    new TireFollowing.DetectorDefinition()
+                    {
+                        DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, false),
+                        StartGuessingX = -PilotDefinition.Conf.TireFollowingStage2GuessX,
+                        StartGuessingY = 0,
+                        SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingLeaveCarWalkBlindSwitchingDistance,
+                        FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
+                        PathTransformation = new Tuple<float, float, float>(
+                        PilotDefinition.Conf.TireFollowingLeaveCarBackLidarPathTransformationX,
+                        PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
+                        0)
+                    },
+                },
+                CarDirection = 180f,
+                SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
+                MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
+                WalkBlindTh = 0,
+                TireNum = 1
+            };
+            _dt = new DriveTask(following.Get());
+            _dt.Wait();
+            DLog.Log($"结束离车测试", "TireFollowing");
+        }
+        private DriveTask _dt;
+    }
+
+    [MovementTest(name = "抱夹关闭")]
+    public class ClampTest1 : MovementTest
+    {
+        public override void TestStop()
+        {
+            _dt?.Stop();
+            PilotDefinition.Self.SpeedLeftArm = 0;
+            PilotDefinition.Self.SpeedRightArm = 0;
+        }
+
+        public override void Test()
+        {
+            _dt = new DriveTask(new ClampToTarget()
+            {
+                LeftClampTarget = PilotDefinition.Self.LeftArmUpperPos,
+                RightClampTarget = PilotDefinition.Self.RightArmUpperPos
+            }.Get());
+            _dt.Wait();
+        }
+
+        private DriveTask _dt;
+    }
+
+    [MovementTest(name = "抱夹打开")]
+    public class ClampTest2 : MovementTest
+    {
+        public override void TestStop()
+        {
+            _dt?.Stop();
+            PilotDefinition.Self.SpeedLeftArm = 0;
+            PilotDefinition.Self.SpeedRightArm = 0;
+        }
+
+        public override void Test()
+        {
+            _dt = new DriveTask(new ClampToTarget()
+            {
+                LeftClampTarget = PilotDefinition.Self.LeftArmLowerPos,
+                RightClampTarget = PilotDefinition.Self.RightArmLowerPos
+            }.Get());
+            _dt.Wait();
+        }
+
+        private DriveTask _dt;
+    }
+
+    [MovementTest(name = "测试前进基于轮里程")]
+    public class LineTrackingTest : MovementTest
+    {
+        public override void TestStop()
+        {
+            _dt?.Stop();
+        }
+        public override void Test()
+        {
+            _dt = new DriveTask(new LineTracking()
+            {
+                Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
+            }.Get());
+            _dt.Wait();
+        }
+        private DriveTask _dt;
+    }
+
+    [MovementTest(name = "测试后退基于轮里程")]
+    public class ReverseLineTrackingTest : MovementTest
+    {
+        public override void TestStop()
+        {
+            _dt?.Stop();
+        }
+        public override void Test()
+        {
+            _dt = new DriveTask(new LineTracking()
+            {
+                Target = -PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
+            }.Get());
+            _dt.Wait();
+        }
+        private DriveTask _dt;
+    }
+
+    [MovementTest(name = "测试终点跟踪动作-前进")]
+    public class DstTrackerForward : MovementTest
+    {
+        public bool UseInteractivePick = true;
+        public float srcX;
+        public float srcY;
+        public float dstX;
+        public float dstY;
+        public float carDirectionBias = 0f;
+        private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
+
+        public override void TestStop()
+        {
+            _dt?.Stop();
+            _painter?.Clear();
+        }
+        public override void Test()
+        {
+            var p1 = UI.GetPoint("point1");
+            var p2 = UI.GetPoint("point2");
+
+            _painter.Clear();
+            _dt = new DriveTask(new DstTracker()
+            {
+                Src = p1,
+                Dst = p2,
+                CarDirectionBias = carDirectionBias,
+            }.Get());
+            _dt.Wait();
+        }
+
+        private DriveTask _dt;
+    }
+
+    [MovementTest(name = "测试终点跟踪动作-后退")]
+    public class DstTrackerhoutui : MovementTest
+    {
+        public bool UseInteractivePick = true;
+        public float srcX;
+        public float srcY;
+        public float dstX;
+        public float dstY;
+        public float carDirectionBias = 180f;
+        private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
+
+        public override void TestStop()
+        {
+            _dt?.Stop();
+            _painter?.Clear();
+        }
+        public override void Test()
+        {
+            var p1 = UI.GetPoint("point1");
+            var p2 = UI.GetPoint("point2");
+
+            _painter.Clear();
+            _dt = new DriveTask(new DstTracker()
+            {
+                Src = p1,
+                Dst = p2,
+                CarDirectionBias = carDirectionBias,
+            }.Get());
+            _dt.Wait();
+        }
+
+        private DriveTask _dt;
+    }
+
+    [MovementTest(name = "测试先直行再终点跟踪")]
+    public class LineTrackThenDstTrackerTest : MovementTest
+    {
+        public float carDirectionBias = 0f;
+        private readonly Painter _painter = UI.GetPainter("LineTrackThenDstTrackerTest");
+        public override void TestStop()
+        {
+            _dt?.Stop();
+            _painter?.Clear();
+        }
+        public override void Test()
+        {
+            var src = UI.GetPoint("请在上位机选择起点(src)");
+            var dst = UI.GetPoint("请在上位机选择终点(dst)");
+            _painter.Clear();
+            _painter.DrawLine(Color.Cyan, src.X, src.Y, dst.X, dst.Y, width: 3);
+            _painter.DrawCircle(Color.LimeGreen, src.X, src.Y, 80f);
+            _painter.DrawCircle(Color.OrangeRed, dst.X, dst.Y, 80f);
+            _painter.DrawText(Color.LimeGreen, "src", src.X + 80f, src.Y + 80f);
+            _painter.DrawText(Color.OrangeRed, "dst", dst.X + 80f, dst.Y + 80f);
+            IEnumerable<bool> TrackThenFollow()
+            {
+                foreach (var running in new LineTracking()
+                {
+                    Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
+                    EnableHandover = true,
+                    HandoverDistance = 200f,
+                    HandoverSpeed = 0.3f,
+                }.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+                foreach (var running in new DstTracker()
+                {
+                    Src = src,
+                    Dst = dst,
+                    CarDirectionBias = carDirectionBias,
+                    InitialSendSpeed = 0.3f
+                }.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+                yield return false;
+            }
+            _dt = new DriveTask(TrackThenFollow());
+            _dt.Wait();
+        }
+        private DriveTask _dt;
+    }
+
+    [MovementTest(name = "测试先离车再终点跟踪")]
+    public class LeaveCarThenDstTrackerTest : MovementTest
+    {
+        private readonly Painter _painter = UI.GetPainter("LeaveCarThenDstTrackerTest");
+
+        public override void TestStop()
+        {
+            _dt?.Stop();
+            _painter?.Clear();
+        }
+
+        public override void Test()
+        {
+            var src = UI.GetPoint("请在上位机选择离车后起点(src)");
+            var dst = UI.GetPoint("请在上位机选择终点(dst)");
+
+            _painter.Clear();
+            _painter.DrawLine(Color.Cyan, src.X, src.Y, dst.X, dst.Y, width: 3);
+            _painter.DrawCircle(Color.LimeGreen, src.X, src.Y, 80f);
+            _painter.DrawCircle(Color.OrangeRed, dst.X, dst.Y, 80f);
+            _painter.DrawText(Color.LimeGreen, "src", src.X + 80f, src.Y + 80f);
+            _painter.DrawText(Color.OrangeRed, "dst", dst.X + 80f, dst.Y + 80f);
+
+            IEnumerable<bool> LeaveThenFollow()
+            {
+                var following = new TireFollowing()
+                {
+                    GetController = () => new ChassisController().Get(),
+                    GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
+                    GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
+                    detectors = new List<TireFollowing.DetectorDefinition>()
+                    {
+                        new TireFollowing.DetectorDefinition()
+                        {
+                            DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, false),
+                            StartGuessingX = -PilotDefinition.Conf.TireFollowingStage2GuessX,
+                            StartGuessingY = 0,
+                            SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
+                            FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
+                            PathTransformation = new Tuple<float, float, float>(
+                                PilotDefinition.Conf.TireFollowingLeaveCarBackLidarPathTransformationX,
+                                PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
+                                0),
+                        },
+                    },
+                    CarDirection = 180f,
+                    SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
+                    MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
+                    WalkBlindTh = 0,
+                    TireNum = 1
+                };
+
+                foreach (var running in following.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+
+                foreach (var running in new DstTracker()
+                {
+                    Src = src,
+                    Dst = dst,
+                    CarDirectionBias = 180f,
+                }.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+
+                yield return false;
+            }
+
+            _dt = new DriveTask(LeaveThenFollow());
+            _dt.Wait();
+        }
+
+        private DriveTask _dt;
+    }
+
+
+    [MovementTest(name = "驱动器下使能测试")]
+    public class DriverDisableTest : MovementTest
+    {
+        public override void TestStop()
+        {
+            throw new NotImplementedException();
+        }
+
+        public override void Test()
+        {
+            new DriveTask(new DriverDisable(){ }.Get()).Wait();
+        }
+    }
+
+    [MovementTest(name = "驱动器复位测试")]
+    public class DriverAbleTest : MovementTest
+    {
+        public override void TestStop()
+        {
+            throw new NotImplementedException();
+        }
+
+        public override void Test()
+        {
+            new DriveTask(new DriverAble(){ }.Get()).Wait();
+        }
+    }
+
+    [MovementTest(name = "底盘旋转测试")]
+    public class RotateToAngleTest : MovementTest
+    {
+        public override void TestStop()
+        {
+            throw new NotImplementedException();
+        }
+
+        public override void Test()
+        {
+            var target = UI.GetInput("输入旋转角度：");
+            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+            new DriveTask(new MultiWheelRotateInPlace()
+            {
+                AngleTarget = float.Parse(target),
+                PidparamsRead = () => new PIDParams()
+                {
+                    Kp = PilotDefinition.Conf.TireFollowingThkp,
+                    Ki = PilotDefinition.Conf.TireFollowingThki,
+                    Kd = PilotDefinition.Conf.TireFollowingThkd,
+                    DeadZone = PilotDefinition.Conf.TireFollowingThDeadZone,
+                    SpeedAccPerSec = PilotDefinition.Conf.TireFollowingThSpeedAccPerSec,
+                    OutputUpperThreshold = PilotDefinition.Conf.TireFollowingThThresh,
+                    MaxI = PilotDefinition.Conf.TireFollowingThMaxI,
+                }
+            }.Get()).Wait();
+        }
+    }
+
+    public class utils
+    {
+        public static List<(float x, float y, float th)> RemoveOutliers(List<(float x, float y, float th)> data, float threshold = 2.0f)
+        {
+            var means = CalculateMean(data);
+            var stdDevs = CalculateStandardDeviation(data, means);
+
+            return data.Where(point =>
+                Math.Abs(point.x - means.x) <= threshold * stdDevs.x &&
+                Math.Abs(point.y - means.y) <= threshold * stdDevs.y &&
+                AngularDistance(point.th, means.th) <= threshold * stdDevs.th
+            ).ToList();
+        }
+
+        public static (float x, float y, float th) CalculateMean(List<(float x, float y, float th)> data)
+        {
+            float meanX = data.Average(point => point.x);
+            float meanY = data.Average(point => point.y);
+            float sinSum = data.Sum(point => (float)Math.Sin(DegreeToRadian(point.th)));
+            float cosSum = data.Sum(point => (float)Math.Cos(DegreeToRadian(point.th)));
+            float meanTh = RadianToDegree((float)Math.Atan2(sinSum, cosSum));
+
+            return (meanX, meanY, meanTh);
+        }
+
+        public static (float x, float y, float th) CalculateStandardDeviation(List<(float x, float y, float th)> data, (float x, float y, float th) means)
+        {
+            float varianceX = data.Average(point => (point.x - means.x) * (point.x - means.x));
+            float varianceY = data.Average(point => (point.y - means.y) * (point.y - means.y));
+
+            // 计算角度的方差
+            float varianceTh = data.Average(point => AngularDistance(point.th, means.th) * AngularDistance(point.th, means.th));
+
+            return ((float)Math.Sqrt(varianceX), (float)Math.Sqrt(varianceY), (float)Math.Sqrt(varianceTh));
+        }
+
+        public static float DegreeToRadian(float degree)
+        {
+            return (float)(degree * Math.PI / 180.0);
+        }
+
+        public static float RadianToDegree(float radian)
+        {
+            return (float)(radian * 180.0 / Math.PI);
+        }
+
+        public static float AngularDistance(float angle1, float angle2)
+        {
+            return CommonMath.ThDiff(angle1, angle2);
+        }
+    }
+
+}
+
+

ClumsyPilot/MovementTests.TwoLegDetect.cs

@@ -0,0 +1,136 @@
+using System;
+using System.Collections.Generic;
+using System.Drawing;
+using System.Linq;
+using System.Numerics;
+using System.Threading;
+using ClumsyCore;
+using ClumsyCore.DTools;
+using ClumsyCore.Pilot;
+using ClumsyCore.Utilities;
+using ClumsyDance.ClumsyDance.Detectors;
+using ClumsyDance.ClumsyWalk.Detectors;
+using FundamentalLib;
+using LineSegment = ClumsyCore.Utilities.LineSegment;
+
+namespace MultiWheelC;
+
+/// <summary>
+/// 2腿检测：用单线激光雷达识别两腿托盘/轮胎，按上一帧结果作为下一帧猜测做闭环检测。
+/// 从 StandardMultiWheelLifter 移植；参数全部走 PilotConfig（Fields 面板），雷达选择改为配置项而非阻塞输入。
+/// </summary>
+[MovementTest(name = "多舵轮-2腿检测")]
+public class TwoLegDetect : MovementTest
+{
+    private Painter _painter;
+    private bool _running = true;
+
+    public override void TestStop() => _running = false;
+
+    public override void Test()
+    {
+        _running = true;
+        _painter = UI.GetPainter("MultiWheelTwoLegDetect", false);
+        _painter.Clear();
+
+        var lidar = PilotDefinition.Conf.TwoLegLidarName;
+        var lastDetectX = PilotDefinition.Conf.TwoLegGuessX;
+        var lastDetectY = 0f;
+
+        while (_running)
+        {
+            var ld = Detect(lidar, lastDetectX, SetFilters(lastDetectX, lastDetectY));
+            if (ld == null)
+            {
+                Thread.Sleep(100);
+                continue;
+            }
+
+            var center = (ld.Src + ld.Dst) / 2;
+            var distanceToCarOrigin = Vector2.Distance(Vector2.Zero, center);
+
+            _painter.Clear();
+            _painter.DrawLine(Color.Cyan, Vector2.Zero, center, width: 2);
+            _painter.DrawText(Color.Yellow, $"{distanceToCarOrigin:F1}", center.X / 2, center.Y / 2);
+
+            Hedingben.ToastText(
+                $"[Test检测] lidar:{lidar} guess x:{lastDetectX:F0} y:{lastDetectY:F0} | " +
+                $"中心 x:{center.X:F0} y:{center.Y:F0} dist:{distanceToCarOrigin:F0}",
+                "MultiWheelTwoLegDetect-test");
+
+            // 用本帧中心作为下一帧猜测，实现闭环跟踪
+            lastDetectX = center.X;
+            lastDetectY = center.Y;
+            Thread.Sleep(100);
+        }
+
+        _painter.Clear();
+    }
+
+    /// <summary>在车体坐标系下，按猜测位置检测两腿，返回连接两腿的线段（车体系）。</summary>
+    public static LineSegment Detect(string lidarName, float guessX, List<DetectFilter> filters)
+    {
+        var conf = PilotDefinition.Conf;
+#pragma warning disable CS0612, CS0618
+        var detector = new Lidar2dDetect2LegTray
+        {
+            BlobDist = conf.TwoLegBlobDist,
+            BlobPtCount = conf.TwoLegBlobPtCount,
+            BlobSize = conf.TwoLegBlobSize,
+            CenterChange = Tuple.Create(conf.TwoLegCenterChangeX, 0f, 0f),
+            LegWidth = conf.TwoLegWidth,
+            LegWidthErr = conf.TwoLegWidthErr,
+            Padding = conf.TwoLegPadding,
+            PillarFindingScope = conf.TwoLegPillarFindingScope,
+            SgnDir = conf.TwoLegSgnDir,
+        };
+#pragma warning restore CS0612, CS0618
+
+        var result = detector.DetectWithGuess(
+            lidarName,
+            new LineSegment(new Vector2(guessX, 0), Vector2.Zero),
+            guessCoordinateSystem: CoordinateSystem.Car2D,
+            outCoordinateSystem: CoordinateSystem.Car2D,
+            filters);
+        return ApplyOutputBias(result);
+    }
+
+    private static LineSegment ApplyOutputBias(LineSegment result)
+    {
+        if (result == null) return null;
+
+        var conf = PilotDefinition.Conf;
+        if (Math.Abs(conf.TwoLegOutputBiasX) < 1e-6f && Math.Abs(conf.TwoLegOutputBiasY) < 1e-6f)
+            return result;
+
+        var bias = new Vector2(conf.TwoLegOutputBiasX, conf.TwoLegOutputBiasY);
+        return new LineSegment(result.Src + bias, result.Dst + bias);
+    }
+
+    /// <summary>在猜测中心周围构造一个矩形 ROI，过滤掉框外点云，降低误识别。</summary>
+    public static List<DetectFilter> SetFilters(float guessCenterX, float guessCenterY)
+    {
+        var conf = PilotDefinition.Conf;
+        var painter = UI.GetPainter("MultiWheelTwoLegDetect.Filter", false);
+        painter.Clear();
+
+        var box = new[]
+        {
+            new Vector2(guessCenterX - conf.TwoLegFilterLength / 2, guessCenterY - conf.TwoLegFilterWidth / 2),
+            new Vector2(guessCenterX + conf.TwoLegFilterLength / 2, guessCenterY - conf.TwoLegFilterWidth / 2),
+            new Vector2(guessCenterX + conf.TwoLegFilterLength / 2, guessCenterY + conf.TwoLegFilterWidth / 2),
+            new Vector2(guessCenterX - conf.TwoLegFilterLength / 2, guessCenterY + conf.TwoLegFilterWidth / 2),
+        };
+
+        for (var i = 0; i < box.Length; ++i)
+            painter.DrawLine(Color.DarkOliveGreen, box[i], box[(i + 1) % 4]);
+
+        // 点云滤波在车体坐标系下进行
+        return new List<DetectFilter>
+        {
+            new(CoordinateSystem.Car2D,
+                p => LessMath.IsPointInPolygon4(
+                    box.Select(v => new PointF(v.X, v.Y)).ToArray(), new PointF(p.X, p.Y))),
+        };
+    }
+}

ClumsyPilot/MovementTests.cs

@@ -0,0 +1,608 @@
+using System;
+using System.Collections.Generic;
+using System.Numerics;
+using ClumsyCore;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using FundamentalLib;
+using CommonUsage.Chassis;
+using CommonUsage.Mathematics;
+using MDCSToolBox.Clumsy.Movements;
+using MDCSToolBox.Clumsy.Pilot;
+using MDCSToolBox.Clumsy.Tracks;
+
+namespace MultiWheelC;
+
+public class MultiForwardTest : MovementDefinition
+{
+    public float Speed = 0.2f;
+    public float DurationSeconds = 2f;
+
+    public override IEnumerable<bool> Get()
+    {
+        var chassis = (MultiWheelChassis)BasicPilotBase.Chassis;
+        chassis.SetOriginBias(0, 0, 0);
+        var end = DateTime.Now.AddSeconds(DurationSeconds);
+        while (DateTime.Now < end)
+        {
+            chassis.SendMotion(Speed, 0, 0);
+            yield return true;
+        }
+
+        chassis.SendMotion(0, 0, 0);
+    }
+}
+
+// 原地旋转到指定世界坐标系朝向：先把舵轮打到旋转所需角度，对齐后再旋转，按目标角度停止（非固定时长）。
+public class MultiRotateToWorldAngle : MovementDefinition
+{
+    /// <summary>目标朝向（世界坐标系，单位 deg）。</summary>
+    public float TargetWorldDeg;
+
+    /// <summary>旋转角速度（deg/s，逆时针为正）。</summary>
+    public float RotSpeed = 30f;
+
+    /// <summary>到位角度精度（deg）。</summary>
+    public float ArriveDeg = 1f;
+
+    /// <summary>起转前舵轮对齐精度（deg）。</summary>
+    public float WheelAlignDeg = 2f;
+
+    public override IEnumerable<bool> Get()
+    {
+        var chassis = (MultiWheelChassis)BasicPilotBase.Chassis;
+        chassis.SetOriginBias(0, 0, 0);
+
+        // 阶段一：仅把舵轮打到原地旋转所需角度（下发 0 速度，只对齐不旋转）。
+        while (true)
+        {
+            chassis.SendRotateMotion(0);
+            if (WheelsAligned(chassis, WheelAlignDeg)) break;
+            yield return true;
+        }
+
+        // 阶段二：旋转到目标世界朝向，到位即停。
+        var target = CommonMath.RoundTh(TargetWorldDeg);
+        while (true)
+        {
+            var cur = CommonMath.RoundTh((float)DetourInterface.getCartLocation().th);
+            var diff = CommonMath.ThDiff(target, cur); // 逆时针为正
+            if (Math.Abs(diff) <= ArriveDeg) break;
+            chassis.SendRotateMotion(Math.Sign(diff) * RotSpeed);
+            yield return true;
+        }
+
+        chassis.PredefinedDriveStop();
+    }
+
+    private static bool WheelsAligned(MultiWheelChassis chassis, float tolDeg)
+    {
+#pragma warning disable CS0612, CS0618
+        var wheels = chassis.GetSteerWheels();
+#pragma warning restore CS0612, CS0618
+        foreach (var sw in wheels)
+            if (Math.Abs(CommonMath.ThDiff(sw.ReadAngle(), sw.GetSendAngle())) > tolDeg)
+                return false;
+        return true;
+    }
+}
+
+[MovementTest(name = "多舵轮-前进2秒")]
+public class MultiForwardMovementTest : MovementTest
+{
+    private DriveTask _task;
+
+    public override void Test()
+    {
+        _task = new DriveTask(new MultiForwardTest().Get());
+        _task.Wait();
+    }
+
+    public override void TestStop() => _task?.Stop();
+}
+
+[MovementTest(name = "多舵轮-原地旋转到目标角度")]
+public class MultiRotateMovementTest : MovementTest
+{
+    private DriveTask _task;
+
+    public override void Test()
+    {
+        _task = new DriveTask(new MultiRotateToWorldAngle
+        {
+            TargetWorldDeg = PilotDefinition.Conf.InPlaceRotateTargetWorldDeg,
+            RotSpeed = PilotDefinition.Conf.InPlaceRotateSpeed,
+            ArriveDeg = PilotDefinition.Conf.InPlaceRotateArriveDeg,
+            WheelAlignDeg = PilotDefinition.Conf.InPlaceRotateWheelAlignDeg
+        }.Get());
+        _task.Wait();
+    }
+
+    public override void TestStop()
+    {
+        _task?.Stop();
+        ((MultiWheelChassis)BasicPilotBase.Chassis).PredefinedDriveStop();
+    }
+}
+
+// ===== 车队联动-原地旋转动作 =====
+// 等价于 FleetRemote 的「原地旋转」模式（已实测可用）：FleetRemote 通过 Medulla 手动 IO
+// (MultiVehicleManualEnabled + Mode=2 + Vth) 驱动 PilotDefinition.TickMultiVehicle 绕车队中心旋转。
+// 手动 IO 是 [AsLowerIO]（Medulla→Clumsy，每周期回写），Clumsy 侧动作直接写会被覆盖；
+// 因此本动作改用 Clumsy 内部脚本字段 MultiVehicleScript*（TickMultiVehicle 已将其作为手动等价输入），
+// 不写一行底盘指令——实际的 SendRotateMotion + PI 纠偏 + 向从车广播均由 TickMultiVehicle 完成。
+//
+// 前提：在「主车」(MultiVehicleMasterEndpoint == "/") 的 Clumsy 上运行，且从车已注册(编队就绪)。
+// 停止条件：主车 SLAM 朝向累计转过 |TargetDeltaDeg|（刚体原地旋转，整车朝向变化量 == 车队转角）；
+//          无定位时退化为按 |TargetDeltaDeg| / |Omega| 估算时长；并带安全超时。
+public class FleetRotateInPlace : MovementDefinition
+{
+    /// <summary>角速度大小(deg/s)；实际方向由 TargetDeltaDeg 的符号决定。</summary>
+    public float Omega = 15f;
+
+    /// <summary>目标相对转角(deg，带符号，+ 为逆时针)。</summary>
+    public float TargetDeltaDeg = 90f;
+
+    /// <summary>到位角度精度(deg)。</summary>
+    public float ArriveDeg = 1.5f;
+
+    /// <summary>减速区宽度(deg)：剩余角度小于此值时，角速度按剩余比例线性降到 MinOmega，抑制惯性超调。</summary>
+    public float SlowDeg = 25f;
+
+    /// <summary>减速区末段最小角速度(deg/s)：避免越接近目标越慢、长尾停不下/到不了位。</summary>
+    public float MinOmega = 3f;
+
+    /// <summary>缓启动角加速度(deg/s²)：起步时角速度从 0 按此斜率爬升到巡航值，抑制起步抖动/队形骤偏。仅作用于起步加速，&lt;=0 关闭缓启动（阶跃起步）。</summary>
+    public float AccelDegPerSec2 = 20f;
+
+    /// <summary>
+    /// 是否用 Detour 主车航向闭环判停（读 getCartLocation().th 累计实际转角，到 |TargetDeltaDeg| 停）。
+    /// 与 MultiVehicleSyncUseDetour 解耦：转到指定角度需要角度反馈，故默认 true。
+    /// false 时退化为按估算时长开环停止（实际转速≠指令时不精确）。注意 true 时若无有效全局定位，
+    /// getCartLocation() 会阻塞（与单车 MultiRotateToWorldAngle 行为一致）。
+    /// </summary>
+    public bool UseDetourHeading = true;
+
+    // 注：不设超时上限——旋转持续到到位（或无定位时按估算时长结束），或被 Stop()/TestStop() 主动中止。
+
+    /// <summary>到位后保持脚本使能、角速度归零的安定时长(s)，让纠偏把队形稳住再撤离。</summary>
+    public float SettleSec = 0.5f;
+
+    private void ClearScript()
+    {
+        var self = PilotDefinition.Self;
+        self.MultiVehicleScriptVx = 0;
+        self.MultiVehicleScriptVy = 0;
+        self.MultiVehicleScriptVth = 0;
+        self.MultiVehicleScriptEnabled = false;
+    }
+
+    public void Stop() => ClearScript();
+
+    public override IEnumerable<bool> Get()
+    {
+        var self = PilotDefinition.Self;
+        var conf = PilotDefinition.Conf;
+
+        if (conf.MultiVehicleMasterEndpoint != "/")
+        {
+            Hedingben.ToastText("车队原地旋转需在主车(主车端点=\"/\")运行", "FleetRotate");
+            yield break;
+        }
+
+        var dir = Math.Sign(TargetDeltaDeg);
+        if (dir == 0) dir = 1;
+        var maxOmega = Math.Abs(Omega);
+        var minOmega = Math.Min(Math.Abs(MinOmega), maxOmega); // 最小不超过最大
+        var slowDeg = Math.Max(1e-3f, SlowDeg);                // 减速区宽度
+        var accel = AccelDegPerSec2;                           // 缓启动角加速度，仅作用于起步，<=0 关闭
+        var targetMag = Math.Abs(TargetDeltaDeg);
+
+        var hasPos = UseDetourHeading;
+        var prevTh = hasPos ? (float)DetourInterface.getCartLocation().th : 0f;
+        var startTh = prevTh;
+        var accumulated = 0f; // 累计带符号转角(deg)
+        var start = DateTime.Now;
+        var lastTime = start;
+        var lastLog = DateTime.MinValue;
+        var lastCenterLog = DateTime.MinValue;
+        var cmdMag = 0f;      // 当前实际下发角速度大小(deg/s)，缓启动从 0 斜坡爬升
+        var centerTracking = false;
+        float centerStartX = 0, centerStartY = 0, centerStartTh = 0;
+        float centerLastX = 0, centerLastY = 0, centerLastTh = 0, centerMaxDrift = 0;
+        // 无定位按时长估算时，补上缓启动斜坡少转的等效时间(≈ maxOmega/(2·accel))，使时长更接近目标角。
+        var estDuration = maxOmega > 1e-3 ? targetMag / maxOmega : 0;
+        if (accel > 1e-3) estDuration += maxOmega / (2 * accel);
+
+        DLog.Log(
+            $"REQUEST target={TargetDeltaDeg:0.0} dir={dir} omega={maxOmega:0.0} accel={accel:0.0} " +
+            $"slowDeg={slowDeg:0.0} minOmega={minOmega:0.0} useDetourHeading={hasPos} startTh={startTh:0.00} " +
+            $"estDuration={estDuration:0.00}s syncUseDetour={conf.MultiVehicleSyncUseDetour}",
+            "FleetRotateDbg");
+
+        // 使能脚本驱动的原地旋转（mode2）。TickMultiVehicle 后台循环据此执行旋转并广播给从车。
+        // 起步从 0 角速度开始，由缓启动斜坡爬升，避免阶跃下发导致队形骤偏/抖动。
+        self.MultiVehicleScriptVx = 0;
+        self.MultiVehicleScriptVy = 0;
+        self.MultiVehicleScriptMode = 2;
+        self.MultiVehicleScriptVth = 0;
+        self.MultiVehicleScriptEnabled = true;
+        self.MultiVehicleRotateWheelsReady = false;
+        self.MultiVehicleRotateFleetReady = false;
+
+        DLog.Log("WAIT_ALIGN fleet rotate wheels", "FleetRotateDbg");
+        while (!self.MultiVehicleRotateFleetReady)
+        {
+            self.MultiVehicleScriptVx = 0;
+            self.MultiVehicleScriptVy = 0;
+            self.MultiVehicleScriptMode = 2;
+            self.MultiVehicleScriptVth = 0;
+            self.MultiVehicleScriptEnabled = true;
+            Hedingben.ToastText("车队原地旋转舵轮预对齐中", "FleetRotate");
+            yield return true;
+        }
+
+        float centerStartCarX = 0, centerStartCarY = 0, centerStartCarTh = 0;
+        if (hasPos)
+        {
+            var startPos = DetourInterface.getCartLocation();
+            centerStartCarX = (float)startPos.x;
+            centerStartCarY = (float)startPos.y;
+            centerStartCarTh = (float)startPos.th;
+            prevTh = centerStartCarTh;
+            startTh = prevTh;
+            if (self.TryGetFleetCenterFromPose(centerStartCarX, centerStartCarY, centerStartCarTh,
+                    out centerStartX, out centerStartY, out centerStartTh))
+            {
+                centerLastX = centerStartX;
+                centerLastY = centerStartY;
+                centerLastTh = centerStartTh;
+                centerMaxDrift = 0;
+                centerTracking = true;
+            }
+        }
+        accumulated = 0f;
+        start = DateTime.Now;
+        lastTime = start;
+        lastLog = DateTime.MinValue;
+        lastCenterLog = DateTime.MinValue;
+        cmdMag = 0f;
+        DLog.Log(
+            $"START target={TargetDeltaDeg:0.0} dir={dir} omega={maxOmega:0.0} startTh={startTh:0.00} " +
+            $"fleetAligned={self.MultiVehicleRotateFleetReady}",
+            "FleetRotateDbg");
+        if (centerTracking)
+        {
+            DLog.Log(
+                $"START center=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
+                $"car=({centerStartCarX:0.0},{centerStartCarY:0.0},{centerStartCarTh:0.00}) " +
+                $"target={TargetDeltaDeg:0.0} omega={maxOmega:0.0}",
+                "FleetRotateCenterDbg");
+        }
+
+        var stopReason = "stop()";
+        while (true)
+        {
+            var now = DateTime.Now;
+            var dt = (float)Math.Min(0.2, Math.Max(0, (now - lastTime).TotalSeconds));
+            lastTime = now;
+            var elapsed = (now - start).TotalSeconds;
+
+            float desiredMag;
+            float curTh = 0f, remaining = 0f, actualRate = 0f;
+            if (hasPos)
+            {
+                var carPos = DetourInterface.getCartLocation();
+                curTh = (float)carPos.th;
+                var step = (float)CommonMath.ThDiff(curTh, prevTh); // 本帧实际转角(逆时针为正)
+                accumulated += step;
+                actualRate = dt > 1e-3 ? step / dt : 0f;            // 实际角速率(deg/s)，用于对比指令
+                prevTh = curTh;
+
+                remaining = targetMag - Math.Abs(accumulated);
+                if (remaining <= ArriveDeg) { stopReason = "arrived"; break; }
+
+                // 减速区：剩余角度 < SlowDeg 时，目标角速度按剩余比例线性降到 MinOmega，
+                // 使切断指令瞬间残余动量足够小，抑制惯性滑行造成的超调。宽度直观、便于现场调试。
+                desiredMag = remaining < slowDeg
+                    ? Math.Max(minOmega, maxOmega * (remaining / slowDeg))
+                    : maxOmega;
+
+                if (centerTracking &&
+                    self.TryGetFleetCenterFromPose((float)carPos.x, (float)carPos.y, (float)carPos.th,
+                        out centerLastX, out centerLastY, out centerLastTh))
+                {
+                    var centerDx = centerLastX - centerStartX;
+                    var centerDy = centerLastY - centerStartY;
+                    var centerDrift = (float)Math.Sqrt(centerDx * centerDx + centerDy * centerDy);
+                    centerMaxDrift = Math.Max(centerMaxDrift, centerDrift);
+                    var centerDth = (float)CommonMath.ThDiff(centerLastTh, centerStartTh);
+                    if ((now - lastCenterLog).TotalMilliseconds >= 250)
+                    {
+                        lastCenterLog = now;
+                        DLog.Log(
+                            $"ACTION t={elapsed:0.00}s center=({centerLastX:0.0},{centerLastY:0.0},{centerLastTh:0.00}) " +
+                            $"start=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
+                            $"drift=({centerDx:0.0},{centerDy:0.0}) dist={centerDrift:0.0} max={centerMaxDrift:0.0} dth={centerDth:0.00} " +
+                            $"cmdW={dir * cmdMag:0.000} actualW={actualRate:0.000} acc={accumulated:0.0} remain={remaining:0.0} " +
+                            $"wheelReady={self.MultiVehicleRotateWheelsReady} fleetReady={self.MultiVehicleRotateFleetReady}",
+                            "FleetRotateCenterDbg");
+                    }
+                }
+            }
+            else
+            {
+                // 无定位：按时长估算，无法测角，目标维持巡航速度到估算时长（仅缓启动整形）。
+                desiredMag = maxOmega;
+                if (elapsed >= estDuration) { stopReason = "estDuration"; break; }
+            }
+
+            // 缓启动：只对“加速(目标>当前)”按角加速度限斜率，让起步平滑爬升；
+            // “减速(目标<当前)”跟随上面的减速曲线立即下调，保证及时刹车不超调。
+            if (accel > 1e-3 && desiredMag > cmdMag)
+                cmdMag = Math.Min(desiredMag, cmdMag + accel * dt);
+            else
+                cmdMag = desiredMag;
+
+            self.MultiVehicleScriptVth = dir * cmdMag;
+
+            // 落盘诊断(节流~150ms)：实际航向/累计转角/实际角速率 vs 指令角速率，定位"开环转速不足"。
+            if ((now - lastLog).TotalMilliseconds >= 150)
+            {
+                lastLog = now;
+                DLog.Log(
+                    hasPos
+                        ? $"t={elapsed:0.00}s curTh={curTh:0.00} acc={accumulated:0.0} remain={remaining:0.0} " +
+                          $"cmdW={dir * cmdMag:0.0} actualW={actualRate:0.0} (实际/指令={(Math.Abs(cmdMag) > 1e-3 ? actualRate / (dir * cmdMag) : 0):0.00})"
+                        : $"t={elapsed:0.00}s/{estDuration:0.00}s (无航向反馈,开环按时长) cmdW={dir * cmdMag:0.0}",
+                    "FleetRotateDbg");
+            }
+
+            Hedingben.ToastText(
+                hasPos
+                    ? $"车队原地旋转 目标{TargetDeltaDeg:0.0}° 已转{accumulated:0.0}° 余{targetMag - Math.Abs(accumulated):0.0}° ω={cmdMag:0.0}"
+                    : $"车队原地旋转(无定位,按时长) {elapsed:0.0}/{estDuration:0.0}s ω={cmdMag:0.0}",
+                "FleetRotate");
+
+            yield return true;
+        }
+
+        // 到位：角速度先归零，保持脚本使能让 TickMultiVehicle 的 PI 把队形稳住一小段时间再撤离。
+        self.MultiVehicleScriptVth = 0;
+        var settleEnd = DateTime.Now.AddSeconds(Math.Max(0, SettleSec));
+        while (DateTime.Now < settleEnd)
+            yield return true;
+
+        ClearScript();
+        DLog.Log(
+            $"DONE reason={stopReason} 累计转角={accumulated:0.0}° 目标={TargetDeltaDeg:0.0}° " +
+            $"用时={(DateTime.Now - start).TotalSeconds:0.00}s useDetourHeading={hasPos}",
+            "FleetRotateDbg");
+        if (centerTracking)
+        {
+            var centerDx = centerLastX - centerStartX;
+            var centerDy = centerLastY - centerStartY;
+            var centerDrift = (float)Math.Sqrt(centerDx * centerDx + centerDy * centerDy);
+            var centerDth = (float)CommonMath.ThDiff(centerLastTh, centerStartTh);
+            DLog.Log(
+                $"DONE reason={stopReason} center=({centerLastX:0.0},{centerLastY:0.0},{centerLastTh:0.00}) " +
+                $"start=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
+                $"drift=({centerDx:0.0},{centerDy:0.0}) dist={centerDrift:0.0} max={centerMaxDrift:0.0} dth={centerDth:0.00} " +
+                $"acc={accumulated:0.0} target={TargetDeltaDeg:0.0}",
+                "FleetRotateCenterDbg");
+        }
+        Hedingben.ToastText($"车队原地旋转完成({stopReason}) 累计{accumulated:0.0}°", "FleetRotate");
+    }
+}
+
+[MovementTest(name = "车队联动-原地旋转")]
+public class FleetRotateInPlaceTest : MovementTest
+{
+    private FleetRotateInPlace _proc;
+    private DriveTask _task;
+
+    public override void Test()
+    {
+        _proc = new FleetRotateInPlace
+        {
+            Omega = PilotDefinition.Conf.FleetRotateOmega,
+            TargetDeltaDeg = PilotDefinition.Conf.FleetRotateTargetDeltaDeg,
+            ArriveDeg = PilotDefinition.Conf.FleetRotateArriveDeg,
+            SlowDeg = PilotDefinition.Conf.FleetRotateSlowDeg,
+            MinOmega = PilotDefinition.Conf.FleetRotateMinOmega,
+            AccelDegPerSec2 = PilotDefinition.Conf.FleetRotateAccel,
+            SettleSec = PilotDefinition.Conf.FleetRotateSettleSec,
+            UseDetourHeading = PilotDefinition.Conf.FleetRotateUseDetourHeading
+        };
+        _task = new DriveTask(_proc.Get());
+        _task.Wait();
+    }
+
+    public override void TestStop()
+    {
+        _proc?.Stop();
+        _task?.Stop();
+    }
+}
+
+[MovementTest(name = "车队联动-曲线行走")]
+public class FleetCurveWalkTest : MovementTest
+{
+    private FleetCurveWalk _proc;
+    private DriveTask _task;
+
+    public override void Test()
+    {
+        var self = PilotDefinition.Self;
+        if (!self.TryGetFleetCenterFromMembers(out var x, out var y, out var th) &&
+            !self.TryGetFleetCenterFromSlam(out x, out y, out th))
+        {
+            DLog.Log("FleetCurveWalkTest abort: failed to read fleet center.", "FleetCurveDbg");
+            Hedingben.ToastText("FleetCurve requires master localization", "FleetCurve");
+            return;
+        }
+
+        var pointCount = Math.Max(3, PilotDefinition.Conf.FleetCurveTestControlPointCount);
+        var controlPoints = new List<Vector2>();
+        for (var i = 0; i < pointCount; i++)
+            controlPoints.Add(UI.GetPoint($"FleetCurve point {i + 1}/{pointCount}"));
+        var fleetCenter = new Vector2(x, y);
+        if (Vector2.Distance(fleetCenter, controlPoints[0]) >
+            Vector2.Distance(fleetCenter, controlPoints[controlPoints.Count - 1]))
+            controlPoints.Reverse();
+        var track = new BezierTrack(controlPoints)
+        {
+            Speed = PilotDefinition.Conf.FleetCurveSpeed,
+            CarDirectionBias = 0f
+        };
+
+        _proc = new FleetCurveWalk
+        {
+            Track = track,
+            CurveSpeed = PilotDefinition.Conf.FleetCurveSpeed,
+            CarDirectionBias = 0f,
+            SlowDistance = PilotDefinition.Conf.FleetCurveSlowDistance,
+            FinishDistance = PilotDefinition.Conf.FleetCurveFinishDistance,
+            FinishSpeed = PilotDefinition.Conf.FleetCurveFinishSpeed,
+            SlowingPow = PilotDefinition.Conf.FleetCurveSlowingPow,
+            GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold,
+            StartSyncTimeoutSec = PilotDefinition.Conf.FleetCrabStartSyncTimeoutSec
+        };
+        _task = new DriveTask(_proc.Get());
+        _task.Wait();
+    }
+
+    public override void TestStop()
+    {
+        _proc?.Stop();
+        _task?.Stop();
+    }
+}
+
+[MovementTest(name = "车队联动-自动蟹行")]
+public class FleetCrabWalkTest : MovementTest
+{
+    private FleetCrabWalk _proc;
+    private DriveTask _task;
+
+    public override void Test()
+    {
+        _proc = new FleetCrabWalk
+        {
+            CrabAngleDeg = PilotDefinition.Conf.FleetCrabAngleDeg,
+            BodyToPathAngleDeg = PilotDefinition.Conf.FleetCrabAngleDeg,
+            CrabLengthMm = PilotDefinition.Conf.FleetCrabLengthMm,
+            CrabSpeed = PilotDefinition.Conf.FleetCrabSpeed,
+            FleetCrabAccel = PilotDefinition.Conf.FleetCrabAccel,
+            FleetCrabStartAccel = PilotDefinition.Conf.FleetCrabStartAccel,
+            FleetCrabSlowDistance = PilotDefinition.Conf.FleetCrabSlowDistance,
+            FleetCrabFinishDistance = PilotDefinition.Conf.FleetCrabFinishDistance,
+            FleetCrabFinishSpeed = PilotDefinition.Conf.FleetCrabFinishSpeed,
+            FleetCrabSlowingPow = PilotDefinition.Conf.FleetCrabSlowingPow,
+            GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold
+        };
+        _task = new DriveTask(_proc.Get());
+        _task.Wait();
+    }
+
+    public override void TestStop()
+    {
+        _proc?.Stop();
+        _task?.Stop();
+    }
+}
+
+// ===== 调用 Playground WebAPI 瞬移小车（前移 / 左移 / 旋转）=====
+// 平移/旋转量在 Fields 面板配置：WebApiTranslateMm（默认100mm）、WebApiRotateDeg（默认5度）。
+
+[MovementTest(name = "多舵轮-WebAPI前移")]
+public class WebApiForwardMoveTest : MovementTest
+{
+    public override void Test()
+    {
+        var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
+        var name = PilotDefinition.Conf.PlaygroundRobotName;
+        var d = PilotDefinition.Conf.WebApiTranslateMm;
+
+        var pose = PlaygroundWebApi.GetPose(url, name);
+        // 车体系前向 (d, 0) 变换到世界系：车头方向即朝向 yaw
+        var dst = CommonMath.Transform2D(new Vector2(pose.X, pose.Y), pose.YawDeg, new Vector2(d, 0));
+        PlaygroundWebApi.Move(url, name, dst.X, dst.Y, pose.YawDeg);
+        Hedingben.ToastText($"前移 {d:f0}mm -> ({dst.X:f0},{dst.Y:f0})", "WebApiForward");
+    }
+
+    public override void TestStop()
+    {
+    }
+}
+
+[MovementTest(name = "多舵轮-WebAPI左移")]
+public class WebApiLeftMoveTest : MovementTest
+{
+    public override void Test()
+    {
+        var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
+        var name = PilotDefinition.Conf.PlaygroundRobotName;
+        var d = PilotDefinition.Conf.WebApiTranslateMm;
+
+        var pose = PlaygroundWebApi.GetPose(url, name);
+        // 车体系左向 (0, d) 变换到世界系（车体 +Y 即左侧）
+        var dst = CommonMath.Transform2D(new Vector2(pose.X, pose.Y), pose.YawDeg, new Vector2(0, d));
+        PlaygroundWebApi.Move(url, name, dst.X, dst.Y, pose.YawDeg);
+        Hedingben.ToastText($"左移 {d:f0}mm -> ({dst.X:f0},{dst.Y:f0})", "WebApiLeft");
+    }
+
+    public override void TestStop()
+    {
+    }
+}
+
+[MovementTest(name = "多舵轮-WebAPI旋转")]
+public class WebApiRotateTest : MovementTest
+{
+    public override void Test()
+    {
+        var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
+        var name = PilotDefinition.Conf.PlaygroundRobotName;
+        var deg = PilotDefinition.Conf.WebApiRotateDeg;
+
+        var pose = PlaygroundWebApi.GetPose(url, name);
+        var ny = pose.YawDeg + deg; // 逆时针为正
+        PlaygroundWebApi.Move(url, name, pose.X, pose.Y, ny);
+        Hedingben.ToastText($"旋转 {deg:f1}° -> {ny:f1}°", "WebApiRotate");
+    }
+
+    public override void TestStop()
+    {
+    }
+}
+
+[MovementTest(name = "多舵轮-WebAPI恢复运动")]
+public class WebApiMotionResumeTest : MovementTest
+{
+    public override void Test()
+    {
+        var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
+        PlaygroundWebApi.ResumeMotion(url);
+        Hedingben.ToastText("已恢复车辆运动", "WebApiMotion");
+    }
+
+    public override void TestStop()
+    {
+    }
+}
+
+[MovementTest(name = "多舵轮-WebAPI暂停运动")]
+public class WebApiMotionPauseTest : MovementTest
+{
+    public override void Test()
+    {
+        var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
+        PlaygroundWebApi.PauseMotion(url); // 默认 zero 模式：反馈归零
+        Hedingben.ToastText("已暂停车辆运动 (zero)", "WebApiMotion");
+    }
+
+    public override void TestStop()
+    {
+    }
+}

ClumsyPilot/Movements.cs

@@ -0,0 +1,326 @@
+using ClumsyCore;
+using ClumsyCore.DTools;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using ClumsyCore.Sensors;
+using ClumsyCore.Utilities;
+using ClumsyDance.ClumsyWalk.Detectors;
+using ClumsyDance.Sensors;
+using CommonUsage.Chassis;
+using FundamentalLib;
+using MDCSToolBox.Clumsy.Calibration;
+using MDCSToolBox.Clumsy.HighLevelSecurity;
+using MDCSToolBox.Clumsy.Movements;
+using MDCSToolBox.Clumsy.Pilot;
+using MDCSToolBox.Clumsy.Tracks;
+using MDCSToolBox.Commons;
+using MDCSToolBox.Commons.Controllers;
+using Newtonsoft.Json;
+using System;
+using System.Collections.Generic;
+using System.Drawing;
+using System.Linq;
+using System.Net.Http;
+using System.Numerics;
+using System.Reflection;
+using System.Text;
+using System.Threading;
+using static ClumsyCore.DTools.Painter;
+
+namespace MultiWheelC
+{
+    public class MultiWheelRotateInPlace : MovementDefinition
+    {
+        /// <summary>
+        /// 旋转目标角度
+        /// </summary>
+        public float AngleTarget;
+
+        public float MaxSpeed;
+
+        public Func<float> ThetaReader = () => (float)DetourInterface.getCartLocation().th;
+
+        public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+
+        public Func<PIDParams> PidparamsRead = () => new PIDParams() { };
+
+        public PIDController thPid;
+
+        private static float RangeAngle(float theta)
+        {
+            return (float)(theta - Math.Round(theta / 360.0f) * 360);
+        }
+
+        public override IEnumerable<bool> Get()
+        {
+            var targetAngle = RangeAngle(AngleTarget);
+            var p = PidparamsRead();
+            thPid = new PIDController(ThetaReader, p.Kp);
+            thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone, p.OutputUpperThreshold, p.SpeedAccPerSec);
+
+            DateTime lastTime = DateTime.Now;
+            while (true)
+            {
+                var s = thPid.GetResponse(targetAngle, true);
+                Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
+                Chassis.SendXYThSpeed(0, 0, s);
+                lastTime = DateTime.Now;
+                if (thPid.IsArrived()) break;
+                yield return true;
+            }
+            Chassis.SendXYThSpeed(0, 0, 0);
+            Console.WriteLine($"final rotate to {targetAngle}");
+        }
+    }
+
+
+    public class ClampToTarget : MovementDefinition
+    {
+        public float LeftClampTarget;
+        public float RightClampTarget;
+
+        public float MaxClampSpeed = PilotDefinition.Conf.MaxClampSpeed;
+        public float ClampKp = PilotDefinition.Conf.ClampControlKp;
+        public float ClampKi = PilotDefinition.Conf.ClampControlKi;
+        public float ClampKd = PilotDefinition.Conf.ClampControlKd;
+        public float ClampMaxI = PilotDefinition.Conf.ClampControlMaxI;
+        public float ClampSpeedAcc = PilotDefinition.Conf.ClampControlSpeedAcc;
+        public float ClampDeadZone = PilotDefinition.Conf.ClampControlDeadZone;
+        private PIDController leftpid, rightpid;
+
+        public override IEnumerable<bool> Get()
+        {
+            leftpid = new PIDController(() => PilotDefinition.Self.ActualPosLeftArm, ClampKp, ClampKi, ClampKd,
+                    ClampMaxI, ClampDeadZone, MaxClampSpeed)
+                { SpeedAccPerSec = ClampSpeedAcc };
+
+            rightpid = new PIDController(() => PilotDefinition.Self.ActualPosRightArm, ClampKp, ClampKi, ClampKd,
+                    ClampMaxI, ClampDeadZone, MaxClampSpeed)
+                { SpeedAccPerSec = ClampSpeedAcc };
+
+            while (true)
+            {
+                var leftspeed = leftpid.GetResponse(LeftClampTarget);
+                var rightspeed = rightpid.GetResponse(RightClampTarget);
+                Console.WriteLine($"left arm speed:{leftspeed}  right arm speed:{rightspeed}");
+                PilotDefinition.Self.SpeedLeftArm = leftspeed;
+                PilotDefinition.Self.SpeedRightArm = rightspeed;
+                if (leftpid.IsArrived()) PilotDefinition.Self.SpeedLeftArm = 0;
+                if (rightpid.IsArrived()) PilotDefinition.Self.SpeedRightArm = 0;
+
+                if (leftpid.IsArrived() && rightpid.IsArrived()) break;
+                yield return true;
+            }
+
+            PilotDefinition.Self.SpeedLeftArm = 0;
+            PilotDefinition.Self.SpeedRightArm = 0;
+            Console.WriteLine($"left clamp to target:{LeftClampTarget}  right clamp to target:{RightClampTarget}");
+        }
+    }
+
+    public class Sleep : MovementDefinition
+    {
+        public float Second = 2;
+        public override IEnumerable<bool> Get()
+        {
+            var start = DateTime.Now;
+            while ((DateTime.Now-start).TotalSeconds<Second)
+            {
+                yield return true;
+                Thread.Sleep(1000);
+                Console.WriteLine("Sleep");
+            }
+
+            yield return false;
+        }
+    }
+
+    //直线行走基于detour
+    public class LineTracking1 : MovementDefinition
+    {
+        public float LineDistance = 1000f;
+        public int SrcId = -1;
+        public int DstId = -1;
+        public Action<int> LeaveSrcFunction = null;
+        public Painter painter = UI.GetPainter("Line", false);
+        public override IEnumerable<bool> Get()
+        {
+            var curpose = DetourInterface.getCartLocation();
+            Console.WriteLine($"curpose.th:{curpose.th}");
+            var src = new Vector2((float)curpose.x, (float)curpose.y);
+            var dst = new Vector2((float)curpose.x + LineDistance * (float)Math.Cos(curpose.th), 
+                                  (float)curpose.y + LineDistance * (float)Math.Sin(curpose.th));
+            Console.WriteLine($"src:{src.X} {src.Y}");
+            Console.WriteLine($"dst:{dst.X} {dst.Y}");
+            painter.DrawLine(Color.Green, src.X, src.Y, dst.X, dst.Y, width: 3);
+
+            var tracker = new ChassisController().Get();
+            var linePath = new LineTrack(src, dst) { CarDirectionBias = LineDistance > 0 ? 0 : 180 };
+            tracker.AddTrack(linePath);
+            var _dt = new DriveTask(tracker.Track());
+            _dt.Wait();
+            if (SrcId != -1 && LeaveSrcFunction != null)
+            {
+                LeaveSrcFunction(SrcId);
+                DLog.Log($"释放放车点{SrcId}", "TireFollowing");
+            }
+            yield return false;
+        }
+    }
+
+    //在世界坐标系下，从路径起点追踪到终点并停车
+    public class DstTracker : MovementDefinition
+    {
+        public Vector2 Src;
+        public Vector2 Dst;
+        public float CarDirectionBias = 0f;
+        public Painter Painter = UI.GetPainter("DstTracker");
+
+        public float InitialSendSpeed = 0;
+
+        public override IEnumerable<bool> Get()
+        {
+            Console.WriteLine($"DstTracker src:({Src.X:F2}, {Src.Y:F2}) dst:({Dst.X:F2}, {Dst.Y:F2})");
+            Painter.DrawLine(Color.Cyan, Src.X, Src.Y, Dst.X, Dst.Y, width: 3);
+
+            var tracker = new ChassisController().Get();
+
+           if (InitialSendSpeed != 0)
+            {
+                tracker.SkipInitialRotate = true;
+                tracker.InitialSendSpeed = InitialSendSpeed;
+            }
+
+            var linePath = new LineTrack(Src, Dst) { CarDirectionBias = CarDirectionBias, Speed = PilotDefinition.Conf.DstTrackerMaxSpeed };
+            tracker.AddTrack(linePath);
+            var task = new DriveTask(tracker.Track());
+            task.Wait();
+
+            // 到点后兜底停车
+            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+            chassis.SendXYThSpeed(0f, 0f, 0f);
+            yield return false;
+        }
+    }
+
+    //直线行走基于轮里程
+    public class LineTracking : MovementDefinition
+    {
+        public float Target;
+        public float MaxSpeed = PilotDefinition.Conf.LineTrackMaxSpeed;
+        public float Kp = PilotDefinition.Conf.LineTrackKp;
+        public float Ki = PilotDefinition.Conf.LineTrackKi;
+        public float Kd = PilotDefinition.Conf.LineTrackKd;
+        public float DeadZone = PilotDefinition.Conf.LineTrackDeadZone;
+        public int SrcId = -1;
+        public int DstId = -1;
+        public Action<int> LeaveSrcFunction = null;
+        private PIDController pid;
+        // 末段衔接：接近目标后不再让 PID 把速度降到 0，保留一个接力速度给后续动作接管
+        public bool EnableHandover = false;
+        public float HandoverDistance = 80f; // mm
+        public float HandoverSpeed = 0.15f;  // m/s
+
+        public override IEnumerable<bool> Get()
+        {
+
+            pid = new PIDController(() =>
+                (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
+                Kp, Ki, Kd, 0, DeadZone, MaxSpeed)
+            { SpeedAccPerSec = MaxSpeed / 2f };
+
+            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+            //chassis.SetOriginBias(0, 0, 0);
+            DLog.Log($"直线行驶距离:{Target}", "TireFollowing");
+            while (true)
+            {
+                var current = (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2;
+                var remain = Target - current;
+                if (EnableHandover && Math.Abs(remain) <= Math.Max(1f, HandoverDistance))
+                {
+                    var handoverSign = Math.Sign(remain);
+                    if (handoverSign == 0) handoverSign = 1;
+                    var handoverSpeed = Math.Abs(HandoverSpeed) * handoverSign;
+                    Console.WriteLine($"handover speed: {handoverSpeed:F3}, remain: {remain:F2}");
+                    chassis.SendXYThSpeed(handoverSpeed, 0, 0);
+                    // 保留一拍接力速度，让后续 DstTracker 无缝接管
+                    yield return true;
+                    break;
+                }
+                var speed = pid.GetResponse(Target);
+                Console.WriteLine($"output: {speed}  current: {(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2}");
+                
+                chassis.SendXYThSpeed(speed, 0, 0);
+
+                if (pid.IsArrived()) break;
+                yield return true;
+            }
+            if (SrcId != -1 && LeaveSrcFunction != null)
+            {
+                LeaveSrcFunction(SrcId);
+                DLog.Log($"释放放车点{SrcId}", "TireFollowing");
+            }
+            yield return false;
+        }
+    }
+
+    public class DriverAble : MovementDefinition
+    {
+        public int WaitTimeoutMs = 2000;
+        public int PollIntervalMs = 50;
+
+        public override IEnumerable<bool> Get()
+        {
+            Console.WriteLine("驱动器上使能");
+            PilotDefinition.Self.ResetFromC = true;
+
+            var start = DateTime.Now;
+            var timeoutMs = Math.Max(0, WaitTimeoutMs);
+            var pollMs = Math.Max(1, PollIntervalMs);
+            var success = PilotDefinition.Self.WheelAbleState;
+            while (!success && (DateTime.Now - start).TotalMilliseconds < timeoutMs)
+            {
+                Thread.Sleep(pollMs);
+                success = PilotDefinition.Self.WheelAbleState;
+                if (!success) yield return true;
+            }
+
+            PilotDefinition.Self.ResetFromC = false;
+            if (success)
+                Console.WriteLine($"驱动器上使能完成，WheelAbleState={PilotDefinition.Self.WheelAbleState}");
+            else
+                Console.WriteLine($"驱动器上使能超时，WheelAbleState={PilotDefinition.Self.WheelAbleState}，等待{timeoutMs}ms");
+            yield return false;
+        }
+    }
+
+    public class DriverDisable : MovementDefinition
+    {
+        public int WaitTimeoutMs = 3000;
+        public int PollIntervalMs = 20;
+
+        public override IEnumerable<bool> Get()
+        {
+            Console.WriteLine("驱动器下使能");
+            PilotDefinition.Self.DisableFromC = true;
+
+            var start = DateTime.Now;
+            var timeoutMs = Math.Max(0, WaitTimeoutMs);
+            var pollMs = Math.Max(1, PollIntervalMs);
+            var success = !PilotDefinition.Self.WheelAbleState;
+            while (!success && (DateTime.Now - start).TotalMilliseconds < timeoutMs)
+            {
+                Thread.Sleep(pollMs);
+                success = !PilotDefinition.Self.WheelAbleState;
+                if (!success) yield return true;
+            }
+
+            PilotDefinition.Self.DisableFromC = false;
+            if (success)
+                Console.WriteLine($"驱动器下使能完成，WheelAbleState={PilotDefinition.Self.WheelAbleState}");
+            else
+                Console.WriteLine($"驱动器下使能超时，WheelAbleState={PilotDefinition.Self.WheelAbleState}，等待{timeoutMs}ms");
+            yield return false;
+        }
+    }
+}

ClumsyPilot/Movements/TrajectoryTrackingMovement.cs

@@ -0,0 +1,306 @@
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using CommonUsage.Chassis;
+using MultiWheelC.Control.Allocation;
+using MultiWheelC.Control.Execution;
+using MultiWheelC.Control.Lateral;
+using MultiWheelC.Control.Longitudinal;
+using MultiWheelC.StateEstimation;
+using MultiWheelC.Trajectory;
+using MyParking.Shared;
+
+namespace MultiWheelC
+{
+    /// <summary>
+    /// 使用新版横纵向控制器持续跟踪一条世界坐标系二维轨迹。
+    /// </summary>
+    public sealed class TrajectoryTrackingMovement
+        : MovementDefinition
+    {
+        /// <summary>
+        /// 获取或设置本次动作需要跟踪的世界坐标系轨迹。
+        /// </summary>
+        public Trajectory2D Trajectory;
+
+        /// <summary>
+        /// 获取或设置本次动作使用的车辆状态源；为空时自动创建Detour状态源。
+        /// </summary>
+        public IVehicleStateProvider StateProvider;
+
+        /// <summary>
+        /// 获取或设置每个有效控制周期结束后的诊断数据观察回调。
+        /// </summary>
+        public Action<ParkingGeometricController> CycleObserver;
+
+        /// <summary>
+        /// Stanley横向误差增益，单位为1/s。
+        /// </summary>
+        public double StanleyCrossTrackGainPerSecond = 0.4;
+
+        /// <summary>
+        /// Stanley航向误差增益。
+        /// </summary>
+        public double StanleyHeadingErrorGain = 1.0;
+
+        /// <summary>
+        /// Stanley低速分母保护速度，单位为m/s。
+        /// </summary>
+        public double StanleyMinimumSpeedMetersPerSecond = 0.15;
+
+        /// <summary>
+        /// 获取或设置Stanley是否优先使用当前状态源提供的实际纵向速度。
+        /// </summary>
+        public bool StanleyUsesActualSpeed = true;
+
+        /// <summary>
+        /// Stanley横向误差共同转角分量的最大绝对值，单位为rad。
+        /// </summary>
+        public double MaximumCrossTrackCorrectionRadians =
+            AngleMath.DegreesToRadians(10.0);
+
+        /// <summary>
+        /// Stanley航向误差差动转角分量的最大绝对值，单位为rad。
+        /// </summary>
+        public double MaximumHeadingCorrectionRadians =
+            AngleMath.DegreesToRadians(10.0);
+
+        /// <summary>
+        /// 纵向速度外环比例增益。
+        /// </summary>
+        public double LongitudinalKp = 0.5;
+
+        /// <summary>
+        /// 纵向速度外环积分增益，单位为1/s。
+        /// </summary>
+        public double LongitudinalKiPerSecond;
+
+        /// <summary>
+        /// 纵向速度外环微分增益，单位为s。
+        /// </summary>
+        public double LongitudinalKdSeconds;
+
+        /// <summary>
+        /// 纵向积分项允许产生的最大速度修正绝对值，单位为m/s。
+        /// </summary>
+        public double MaximumIntegralCorrectionMetersPerSecond = 0.05;
+
+        /// <summary>
+        /// 纵向PID不进行反馈修正的速度误差死区，单位为m/s。
+        /// </summary>
+        public double LongitudinalSpeedErrorDeadbandMetersPerSecond =
+            0.025;
+
+        /// <summary>
+        /// 底盘纵向命令速度绝对值上限，单位为m/s。
+        /// </summary>
+        public double MaximumCommandSpeedMetersPerSecond = 0.50;
+
+        /// <summary>
+        /// 前后GCP允许的最大转角绝对值，单位为rad。
+        /// </summary>
+        public double MaximumGcpAngleRadians =
+            AngleMath.DegreesToRadians(45.0);
+
+        /// <summary>
+        /// 前后GCP目标转角最大变化率，单位为rad/s。
+        /// </summary>
+        public double MaximumGcpAngleRateRadiansPerSecond =
+            AngleMath.DegreesToRadians(15.0);
+
+        /// <summary>
+        /// 终点位置和剩余弧长的完成容差，单位为m。
+        /// </summary>
+        public double FinishDistanceMeters = 0.03;
+
+        /// <summary>
+        /// 终点停稳判定允许的实际线速度，单位为m/s。
+        /// </summary>
+        public double FinishSpeedMetersPerSecond = 0.02;
+
+        /// <summary>
+        /// 终点航向完成容差，单位为rad。
+        /// </summary>
+        public double FinishHeadingToleranceRadians =
+            AngleMath.DegreesToRadians(3.0);
+
+        /// <summary>
+        /// 车辆允许偏离参考轨迹的最大欧氏距离，单位为m。
+        /// </summary>
+        public double MaximumDistanceToTrajectoryMeters = 0.30;
+
+        /// <summary>
+        /// 单次轨迹动作允许的最长执行时间，单位为s。
+        /// </summary>
+        public double ExecutionTimeoutSeconds = 120.0;
+
+        /// <summary>
+        /// 获取本次动作创建的控制器，尚未开始时为空。
+        /// </summary>
+        public ParkingGeometricController Controller { get; private set; }
+
+        /// <summary>
+        /// 创建控制器并持续执行控制周期，直到轨迹完成、失败或动作被取消。
+        /// </summary>
+        public override IEnumerable<bool> Get()
+        {
+            ValidateParameters();
+
+            var chassis =
+                PilotDefinition.Chassis as MultiWheelChassis;
+            if (chassis == null)
+            {
+                throw new InvalidOperationException(
+                    "当前底盘不是MultiWheelChassis，无法执行新版轨迹跟踪动作。");
+            }
+
+            var adapter = new MultiWheelChassisAdapter(
+                chassis,
+                PilotDefinition.Self.CarNum);
+
+            // 新版GCP控制统一以真实车头为车体X正方向，避免继承上一次蟹行偏置。
+            adapter.ResetToBodyFrame();
+
+            var stateProvider =
+                StateProvider ??
+                new DetourVehicleStateProvider();
+            var controlPointRadiusMeters =
+                chassis.ControlPointRadius / 1000.0;
+
+            var lateralController =
+                new StanleyLateralController(
+                    controlPointRadiusMeters,
+                    StanleyCrossTrackGainPerSecond,
+                    StanleyHeadingErrorGain,
+                    StanleyMinimumSpeedMetersPerSecond,
+                    StanleyUsesActualSpeed,
+                    MaximumCrossTrackCorrectionRadians,
+                    MaximumHeadingCorrectionRadians);
+            var longitudinalController =
+                new PidLongitudinalController(
+                    LongitudinalKp,
+                    LongitudinalKiPerSecond,
+                    LongitudinalKdSeconds,
+                    MaximumIntegralCorrectionMetersPerSecond,
+                    MaximumCommandSpeedMetersPerSecond,
+                    LongitudinalSpeedErrorDeadbandMetersPerSecond);
+            var gcpAllocator =
+                new GcpCommandAllocator(
+                    MaximumGcpAngleRadians);
+            var commandExecutor =
+                new GcpCommandExecutor(
+                    adapter,
+                    MaximumGcpAngleRateRadiansPerSecond);
+
+            Controller = new ParkingGeometricController(
+                stateProvider,
+                lateralController,
+                longitudinalController,
+                gcpAllocator,
+                commandExecutor,
+                FinishDistanceMeters,
+                FinishSpeedMetersPerSecond,
+                FinishHeadingToleranceRadians,
+                MaximumDistanceToTrajectoryMeters);
+
+            var clock = Stopwatch.StartNew();
+            var previousCycleSeconds =
+                clock.Elapsed.TotalSeconds;
+            Controller.Start(Trajectory);
+
+            try
+            {
+                while (true)
+                {
+                    if (clock.Elapsed.TotalSeconds >
+                        ExecutionTimeoutSeconds)
+                    {
+                        throw new TimeoutException(
+                            $"新版轨迹跟踪超过{ExecutionTimeoutSeconds:F1}s仍未完成。");
+                    }
+
+                    var currentCycleSeconds =
+                        clock.Elapsed.TotalSeconds;
+                    var deltaTimeSeconds =
+                        currentCycleSeconds -
+                        previousCycleSeconds;
+                    previousCycleSeconds =
+                        currentCycleSeconds;
+
+                    // 极短首周期不参与PID和GCP角速度限制，等待调度器进入下一周期。
+                    if (deltaTimeSeconds <= 1e-6)
+                    {
+                        yield return true;
+                        continue;
+                    }
+
+                    var result =
+                        Controller.ExecuteCycle(
+                            deltaTimeSeconds);
+
+                    if (Controller.LastVehicleState.HasValue)
+                    {
+                        CycleObserver?.Invoke(Controller);
+                    }
+
+                    if (result ==
+                        ParkingControlCycleResult.Completed)
+                    {
+                        break;
+                    }
+
+                    if (result ==
+                        ParkingControlCycleResult.Faulted)
+                    {
+                        throw new InvalidOperationException(
+                            string.IsNullOrWhiteSpace(
+                                Controller.LastFailureReason)
+                                ? "新版轨迹跟踪控制器发生未知故障。"
+                                : Controller.LastFailureReason,
+                            Controller.LastException);
+                    }
+
+                    if (result ==
+                        ParkingControlCycleResult.Inactive)
+                    {
+                        throw new InvalidOperationException(
+                            "新版轨迹跟踪控制器在轨迹完成前意外停止活动。");
+                    }
+
+                    // CommandSent和短暂StateUnavailable均继续下一控制周期；
+                    // 后者已经由控制器主动停车，等待Detour恢复。
+                    yield return true;
+                }
+            }
+            finally
+            {
+                Controller.Cancel();
+            }
+
+            yield return false;
+        }
+
+        /// <summary>
+        /// 在接管实际底盘前检查动作自身无法由子控制器检查的参数。
+        /// </summary>
+        private void ValidateParameters()
+        {
+            if (Trajectory == null)
+            {
+                throw new InvalidOperationException(
+                    "新版轨迹跟踪动作没有设置Trajectory。");
+            }
+
+            if (double.IsNaN(ExecutionTimeoutSeconds) ||
+                double.IsInfinity(ExecutionTimeoutSeconds) ||
+                ExecutionTimeoutSeconds <= 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(ExecutionTimeoutSeconds),
+                    "轨迹跟踪超时时间必须是正有限值。");
+            }
+        }
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/CoarsePathPoint.cs

@@ -0,0 +1,63 @@
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+/// <summary>最终粗路径上的一个稠密连续点；长度、净空和位置使用 m，航向使用 rad，曲率使用 1/m。</summary>
+public sealed class CoarsePathPoint
+{
+    /// <summary>
+    /// 创建粗路径点。
+    /// 参数：xMeters、yMeters 为世界坐标 m；headingRadians 与 unwrappedHeadingRadians 为航向 rad；arcLengthMeters 和 bodyClearanceMeters 为 m；vehicleCurvaturePerMeter 为 1/m。
+    /// </summary>
+    public CoarsePathPoint(
+        double xMeters,
+        double yMeters,
+        double headingRadians,
+        double unwrappedHeadingRadians,
+        double arcLengthMeters,
+        TravelDirection direction,
+        double vehicleCurvaturePerMeter,
+        double bodyClearanceMeters,
+        bool isGearSwitchPoint,
+        CoarsePathPointSource source)
+    {
+        X = xMeters;
+        Y = yMeters;
+        Heading = headingRadians;
+        UnwrappedHeading = unwrappedHeadingRadians;
+        ArcLength = arcLengthMeters;
+        Direction = direction;
+        VehicleCurvature = vehicleCurvaturePerMeter;
+        BodyClearance = bodyClearanceMeters;
+        IsGearSwitchPoint = isGearSwitchPoint;
+        Source = source;
+    }
+
+    /// <summary>世界 X 坐标，单位 m。</summary>
+    public double X { get; }
+
+    /// <summary>世界 Y 坐标，单位 m。</summary>
+    public double Y { get; }
+
+    /// <summary>归一化后可用于几何查询的车头航向，单位 rad。</summary>
+    public double Heading { get; }
+
+    /// <summary>跨越 ±π 后仍连续的车头航向，单位 rad。</summary>
+    public double UnwrappedHeading { get; }
+
+    /// <summary>从路径起点累计的弧长，单位 m；必须非负且不递减。</summary>
+    public double ArcLength { get; }
+
+    /// <summary>从上一点运动到当前点所在方向段的行驶方向。</summary>
+    public TravelDirection Direction { get; }
+
+    /// <summary>车辆在此点采用的恒曲率原语曲率，单位 1/m。</summary>
+    public double VehicleCurvature { get; }
+
+    /// <summary>扩大车体到障碍物的保守净空下界，单位 m。</summary>
+    public double BodyClearance { get; }
+
+    /// <summary>此点是否为新方向段开始的换向点。</summary>
+    public bool IsGearSwitchPoint { get; }
+
+    /// <summary>此点由起点、普通原语或终点截断产生的来源。</summary>
+    public CoarsePathPointSource Source { get; }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/CoarsePathPointSource.cs

@@ -0,0 +1,14 @@
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+/// <summary>粗路径点在搜索与原语重建中的产生来源。</summary>
+public enum CoarsePathPointSource
+{
+    /// <summary>请求提供的起始位姿。</summary>
+    Start,
+
+    /// <summary>未截断恒曲率运动原语的内部积分点。</summary>
+    MotionPrimitive,
+
+    /// <summary>首次满足目标容差而在原语内部截断的终点积分点。</summary>
+    GoalTruncation,
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/GoalDirectionConstraint.cs

@@ -0,0 +1,14 @@
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+/// <summary>终点候选最后一个连续运动段的方向约束。</summary>
+public enum GoalDirectionConstraint
+{
+    /// <summary>不限制进入终点的运动方向。</summary>
+    Any,
+
+    /// <summary>必须以前进方向进入终点。</summary>
+    Forward,
+
+    /// <summary>必须以倒车方向进入终点。</summary>
+    Reverse,
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/HybridAStarConfiguration.cs

@@ -0,0 +1,83 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+/// <summary>
+/// Hybrid A* 粗路径搜索的可调配置。
+/// 长度和距离使用 m，航向使用 rad，曲率使用 1/m；各项有效范围由规划器在执行前校验。
+/// </summary>
+public sealed class HybridAStarConfiguration
+{
+    /// <summary>创建采用 P0 固定安全边界和代价权重的默认配置。</summary>
+    public HybridAStarConfiguration()
+    {
+        PrimitiveLengthMeters = 0.50d;
+        IntegrationStepMeters = 0.05d;
+        MaximumCollisionCheckStepMeters = 0.025d;
+        HeadingResolutionRadians = Math.PI / 36d;
+        CurvatureLevelCount = 5;
+        GoalPositionToleranceMeters = 0.15d;
+        GoalHeadingToleranceRadians = Math.PI / 36d;
+        MaximumExpandedNodes = 200000;
+        SearchTimeout = TimeSpan.FromSeconds(5d);
+        HeuristicWeight = 1d;
+        ReverseCostMultiplier = 1.5d;
+        GearSwitchPenaltyMeters = 1d;
+        CurvatureMagnitudeWeight = 0.10d;
+        CurvatureChangePenaltyMetersPerLevel = 0.05d;
+        ClearanceCostWeight = 0.20d;
+        ClearanceCostDistanceMeters = 0.50d;
+        AllowReverse = true;
+    }
+
+    /// <summary>单个恒曲率原语的最大行驶长度，单位 m。</summary>
+    public double PrimitiveLengthMeters { get; set; }
+
+    /// <summary>原语内部输出积分点之间允许的最大弧长，单位 m。</summary>
+    public double IntegrationStepMeters { get; set; }
+
+    /// <summary>连续碰撞检查允许的最大车辆中心位移，单位 m；实际值还受地图分辨率限制。</summary>
+    public double MaximumCollisionCheckStepMeters { get; set; }
+
+    /// <summary>搜索状态离散所使用的航向格宽，单位 rad。</summary>
+    public double HeadingResolutionRadians { get; set; }
+
+    /// <summary>从最大负曲率到最大正曲率的离散曲率等级数。</summary>
+    public int CurvatureLevelCount { get; set; }
+
+    /// <summary>终点位置允许的欧氏距离误差，单位 m。</summary>
+    public double GoalPositionToleranceMeters { get; set; }
+
+    /// <summary>终点车头航向允许的最小环形角度误差，单位 rad。</summary>
+    public double GoalHeadingToleranceRadians { get; set; }
+
+    /// <summary>单次搜索允许扩展的最大节点数。</summary>
+    public int MaximumExpandedNodes { get; set; }
+
+    /// <summary>单次搜索允许消耗的最长时间；超出后返回 <see cref="PlanningStatus.SearchTimeout"/>。</summary>
+    public TimeSpan SearchTimeout { get; set; }
+
+    /// <summary>二维绕障启发式的权重；1 表示不额外放大。</summary>
+    public double HeuristicWeight { get; set; }
+
+    /// <summary>倒车原语长度代价相对前进原语的倍率。</summary>
+    public double ReverseCostMultiplier { get; set; }
+
+    /// <summary>相邻原语发生换向时增加的等效距离代价，单位 m。</summary>
+    public double GearSwitchPenaltyMeters { get; set; }
+
+    /// <summary>曲率绝对值对应的无量纲代价权重。</summary>
+    public double CurvatureMagnitudeWeight { get; set; }
+
+    /// <summary>相邻曲率等级每变化一级增加的等效距离代价，单位 m。</summary>
+    public double CurvatureChangePenaltyMetersPerLevel { get; set; }
+
+    /// <summary>车体保守净空不足时增加的无量纲代价权重。</summary>
+    public double ClearanceCostWeight { get; set; }
+
+    /// <summary>计算净空代价时视为足够安全的车体保守净空，单位 m。</summary>
+    public double ClearanceCostDistanceMeters { get; set; }
+
+    /// <summary>是否允许生成倒车原语；false 时搜索只生成前进原语。</summary>
+    public bool AllowReverse { get; set; }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/PathSegment.cs

@@ -0,0 +1,37 @@
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+/// <summary>粗路径中方向一致的一段连续点范围；索引两端均包含在段内。</summary>
+public sealed class PathSegment
+{
+    /// <summary>
+    /// 创建方向分段。
+    /// 参数：segmentIndex 为从零开始的段序号；startIndex 与 endIndex 为 <see cref="PlanningResult.Path"/> 的包含式索引；两个换向标记描述段首或段尾是否位于换向对。
+    /// </summary>
+    public PathSegment(int segmentIndex, TravelDirection direction, int startIndex, int endIndex, bool startsAtGearSwitch, bool endsAtGearSwitch)
+    {
+        SegmentIndex = segmentIndex;
+        Direction = direction;
+        StartIndex = startIndex;
+        EndIndex = endIndex;
+        StartsAtGearSwitch = startsAtGearSwitch;
+        EndsAtGearSwitch = endsAtGearSwitch;
+    }
+
+    /// <summary>从零开始的方向段序号。</summary>
+    public int SegmentIndex { get; }
+
+    /// <summary>此段所有连续运动点对应的行驶方向。</summary>
+    public TravelDirection Direction { get; }
+
+    /// <summary>此段在 <see cref="PlanningResult.Path"/> 中的起始索引，包含该点。</summary>
+    public int StartIndex { get; }
+
+    /// <summary>此段在 <see cref="PlanningResult.Path"/> 中的结束索引，包含该点。</summary>
+    public int EndIndex { get; }
+
+    /// <summary>此段首点是否为换向后保留的新方向点。</summary>
+    public bool StartsAtGearSwitch { get; }
+
+    /// <summary>此段尾点是否紧邻下一方向段的换向对。</summary>
+    public bool EndsAtGearSwitch { get; }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/PlanningDiagnostics.cs

@@ -0,0 +1,65 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+/// <summary>一次规划的只读统计与终止说明；长度和净空单位为 m。</summary>
+public sealed class PlanningDiagnostics
+{
+    /// <summary>
+    /// 创建规划统计快照。
+    /// 参数中的节点数量均为非负计数；pathLengthMeters 和 minimumBodyClearanceMeters 单位为 m；elapsed 为总耗时；pathSearchElapsed 为地图和起终点预检通过后的路径产出耗时；terminationReason 为可读终止说明，可为 null。
+    /// </summary>
+    public PlanningDiagnostics(
+        int expandedNodeCount = 0,
+        int generatedNodeCount = 0,
+        int reopenedNodeCount = 0,
+        int staleOpenListEntryCount = 0,
+        int peakOpenListCount = 0,
+        double pathLengthMeters = 0d,
+        double minimumBodyClearanceMeters = 0d,
+        TimeSpan elapsed = default(TimeSpan),
+        string terminationReason = null,
+        TimeSpan pathSearchElapsed = default(TimeSpan))
+    {
+        ExpandedNodeCount = expandedNodeCount;
+        GeneratedNodeCount = generatedNodeCount;
+        ReopenedNodeCount = reopenedNodeCount;
+        StaleOpenListEntryCount = staleOpenListEntryCount;
+        PeakOpenListCount = peakOpenListCount;
+        PathLengthMeters = pathLengthMeters;
+        MinimumBodyClearanceMeters = minimumBodyClearanceMeters;
+        Elapsed = elapsed;
+        PathSearchElapsed = pathSearchElapsed;
+        TerminationReason = terminationReason ?? string.Empty;
+    }
+
+    /// <summary>从 Open List 取出并真正扩展的节点数量。</summary>
+    public int ExpandedNodeCount { get; }
+
+    /// <summary>生成并尝试加入搜索状态的节点数量。</summary>
+    public int GeneratedNodeCount { get; }
+
+    /// <summary>以严格更小代价到达同一离散键而重新打开的节点数量。</summary>
+    public int ReopenedNodeCount { get; }
+
+    /// <summary>从 Open List 取出后因已有更优条目而丢弃的陈旧堆条目数量。</summary>
+    public int StaleOpenListEntryCount { get; }
+
+    /// <summary>搜索期间 Open List 同时容纳的最大有效或待丢弃条目数量。</summary>
+    public int PeakOpenListCount { get; }
+
+    /// <summary>成功路径的累计弧长，单位 m；失败结果通常为 0。</summary>
+    public double PathLengthMeters { get; }
+
+    /// <summary>成功路径所有点中车体保守净空下界的最小值，单位 m；失败结果通常为 0。</summary>
+    public double MinimumBodyClearanceMeters { get; }
+
+    /// <summary>从规划入口到返回结果的总耗时。</summary>
+    public TimeSpan Elapsed { get; }
+
+    /// <summary>地图和起终点预检通过后，二维启发式、Hybrid A*、回溯、装配和最终复核的耗时；不含建图，搜索前失败时为零。</summary>
+    public TimeSpan PathSearchElapsed { get; }
+
+    /// <summary>面向调用方的终止原因；成功时可为空字符串。</summary>
+    public string TerminationReason { get; }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/PlanningRequest.cs

@@ -0,0 +1,41 @@
+using MultiWheelC.TrajectoryPlanning.Mapping;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+/// <summary>
+/// 已建规划地图上的一次 Hybrid A* 请求。
+/// 本对象只接受不可变 <see cref="PlanningGridMap"/> 快照，不包含地图构建器、传感器、UI 或调试对象。
+/// </summary>
+public sealed class PlanningRequest
+{
+    /// <summary>创建空规划请求。调用规划器前必须提供地图、起点、终点、车辆和配置。</summary>
+    public PlanningRequest()
+    {
+        StartVehicleCurvature = 0d;
+        GoalDirection = GoalDirectionConstraint.Any;
+    }
+
+    /// <summary>本次搜索唯一允许查询的不可变规划地图快照；位置查询单位为 m。</summary>
+    public PlanningGridMap Map { get; set; }
+
+    /// <summary>车辆几何中心的起始位姿；位置单位 m，航向单位 rad。</summary>
+    public Pose2D Start { get; set; }
+
+    /// <summary>车辆几何中心的目标位姿；位置单位 m，航向单位 rad。</summary>
+    public Pose2D Goal { get; set; }
+
+    /// <summary>车辆几何、余量和曲率限制；尺寸单位 m，曲率单位 1/m。</summary>
+    public VehicleParameters Vehicle { get; set; }
+
+    /// <summary>搜索步长、离散、终点容差、代价和资源上限配置。</summary>
+    public HybridAStarConfiguration Configuration { get; set; }
+
+    /// <summary>车辆起步时的转向曲率，单位 1/m；默认值为 0。</summary>
+    public double StartVehicleCurvature { get; set; }
+
+    /// <summary>起步方向约束；null 表示可从前进或倒车开始。</summary>
+    public TravelDirection? StartDirection { get; set; }
+
+    /// <summary>目标进入方向约束；默认值为 <see cref="GoalDirectionConstraint.Any"/>。</summary>
+    public GoalDirectionConstraint GoalDirection { get; set; }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/PlanningResult.cs

@@ -0,0 +1,67 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+/// <summary>
+/// 一次粗路径规划的最终不可变结果。
+/// 只有 <see cref="Status"/> 为 <see cref="PlanningStatus.Success"/> 时才携带非空路径与方向分段；其他状态始终返回空只读集合。
+/// </summary>
+public sealed class PlanningResult
+{
+    private static readonly IReadOnlyList<CoarsePathPoint> EmptyPath = new ReadOnlyCollection<CoarsePathPoint>(new List<CoarsePathPoint>());
+    private static readonly IReadOnlyList<PathSegment> EmptySegments = new ReadOnlyCollection<PathSegment>(new List<PathSegment>());
+
+    private PlanningResult(PlanningStatus status, PlanningDiagnostics diagnostics, IReadOnlyList<CoarsePathPoint> path, IReadOnlyList<PathSegment> segments)
+    {
+        Status = status;
+        Diagnostics = diagnostics ?? new PlanningDiagnostics(terminationReason: "未提供诊断信息。");
+        Path = path;
+        Segments = segments;
+    }
+
+    /// <summary>规划最终状态；只有 <see cref="PlanningStatus.Success"/> 可以发布路径。</summary>
+    public PlanningStatus Status { get; }
+
+    /// <summary>节点、耗时、路径长度、净空和终止原因统计；始终非空。</summary>
+    public PlanningDiagnostics Diagnostics { get; }
+
+    /// <summary>成功时的稠密粗路径；失败时为不可修改的空集合。</summary>
+    public IReadOnlyList<CoarsePathPoint> Path { get; }
+
+    /// <summary>成功时覆盖 <see cref="Path"/> 的包含式方向分段；失败时为不可修改的空集合。</summary>
+    public IReadOnlyList<PathSegment> Segments { get; }
+
+    /// <summary>
+    /// 创建成功结果。
+    /// 参数：path 与 segments 必须均为非空；diagnostics 为本次规划的统计快照。参数不符合要求时抛出 <see cref="ArgumentException"/>，防止以成功状态发布不完整路径。
+    /// </summary>
+    public static PlanningResult Success(IReadOnlyList<CoarsePathPoint> path, IReadOnlyList<PathSegment> segments, PlanningDiagnostics diagnostics)
+    {
+        if (path == null || path.Count == 0)
+            throw new ArgumentException("Successful planning results require a non-empty path.", nameof(path));
+        if (segments == null || segments.Count == 0)
+            throw new ArgumentException("Successful planning results require non-empty segments.", nameof(segments));
+        return new PlanningResult(PlanningStatus.Success, diagnostics, CopyReadOnly(path), CopyReadOnly(segments));
+    }
+
+    /// <summary>
+    /// 创建失败、取消或资源受限结果。
+    /// 参数：status 不能为 <see cref="PlanningStatus.Success"/>；diagnostics 会原样保留。返回结果的路径与分段始终为空只读集合。
+    /// </summary>
+    public static PlanningResult Failure(PlanningStatus status, PlanningDiagnostics diagnostics)
+    {
+        if (status == PlanningStatus.Success)
+            throw new ArgumentException("Use Success to create a successful planning result.", nameof(status));
+        return new PlanningResult(status, diagnostics, EmptyPath, EmptySegments);
+    }
+
+    private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
+    {
+        var copy = new List<T>(source.Count);
+        for (int index = 0; index < source.Count; index++)
+            copy.Add(source[index]);
+        return new ReadOnlyCollection<T>(copy);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/PlanningStatus.cs

@@ -0,0 +1,56 @@
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+/// <summary>粗路径规划的最终状态；除 <see cref="Success"/> 外均不发布路径或方向分段。</summary>
+public enum PlanningStatus
+{
+    /// <summary>已得到并通过最终连续碰撞复核的完整粗路径。</summary>
+    Success,
+
+    /// <summary>在搜索扩展检查点收到取消请求。</summary>
+    Cancelled,
+
+    /// <summary>请求对象或其必要成员为空，或包含不符合基本契约的值。</summary>
+    InvalidRequest,
+
+    /// <summary>请求提供的地图对象不满足规划器的结构要求。</summary>
+    InvalidMap,
+
+    /// <summary>地图快照尚未准备好参与规划；应查看地图的阻止原因。</summary>
+    MapNotReady,
+
+    /// <summary>车辆尺寸、安全余量或曲率限制无效。</summary>
+    InvalidVehicleParameters,
+
+    /// <summary>原语、离散、代价、容差或资源限制配置无效。</summary>
+    InvalidCurvatureConfiguration,
+
+    /// <summary>起始车辆几何中心或扩大车体不在地图范围内。</summary>
+    StartOutsideMap,
+
+    /// <summary>起始扩大车体与地图障碍物相交或擦边。</summary>
+    StartInCollision,
+
+    /// <summary>目标车辆几何中心或扩大车体不在地图范围内。</summary>
+    GoalOutsideMap,
+
+    /// <summary>目标扩大车体与地图障碍物相交或擦边。</summary>
+    GoalInCollision,
+
+    /// <summary>搜索达到 <see cref="HybridAStarConfiguration.SearchTimeout"/> 限制。</summary>
+    SearchTimeout,
+
+    /// <summary>搜索达到 <see cref="HybridAStarConfiguration.MaximumExpandedNodes"/> 限制。</summary>
+    SearchNodeLimitExceeded,
+
+    /// <summary>Open List 已耗尽，或二维启发式证明目标不可达。</summary>
+    NoFeasiblePath,
+
+    /// <summary>搜索成功节点无法按父链回溯为完整路径。</summary>
+    BacktrackingFailed,
+
+    /// <summary>回溯路径未通过连续碰撞、终点或输出不变量复核。</summary>
+    FinalValidationFailed,
+
+    /// <summary>规划内部发生未预期错误；不会发布部分路径。</summary>
+    InternalError,
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/Pose2D.cs

@@ -0,0 +1,28 @@
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+/// <summary>
+/// 粗路径规划使用的二维连续位姿。
+/// 位置以世界坐标 m 表示，航向以 rad 表示；此值对象不在构造时归一化航向，调用方可保留展开航向。
+/// </summary>
+public sealed class Pose2D
+{
+    /// <summary>
+    /// 创建二维位姿。
+    /// 参数：xMeters、yMeters 为世界坐标，单位 m；headingRadians 为车头航向，单位 rad。
+    /// </summary>
+    public Pose2D(double xMeters, double yMeters, double headingRadians)
+    {
+        X = xMeters;
+        Y = yMeters;
+        Heading = headingRadians;
+    }
+
+    /// <summary>世界 X 坐标，单位 m。</summary>
+    public double X { get; }
+
+    /// <summary>世界 Y 坐标，单位 m。</summary>
+    public double Y { get; }
+
+    /// <summary>车头航向，单位 rad；可以是已展开的连续航向。</summary>
+    public double Heading { get; }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/TravelDirection.cs

@@ -0,0 +1,11 @@
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+/// <summary>连续运动段的行驶方向。</summary>
+public enum TravelDirection
+{
+    /// <summary>沿车辆车头方向前进。</summary>
+    Forward,
+
+    /// <summary>与车辆车头方向相反地倒车。</summary>
+    Reverse,
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/VehicleParameters.cs

@@ -0,0 +1,28 @@
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+/// <summary>
+/// 车辆几何与运动学参数。
+/// 所有几何尺寸均以车辆几何中心为 <see cref="Pose2D"/> 参考点，单位为 m；曲率单位为 1/m。
+/// </summary>
+public sealed class VehicleParameters
+{
+    /// <summary>创建空车辆参数。调用规划器前必须填写有效的几何尺寸和至少一种曲率限制。</summary>
+    public VehicleParameters()
+    {
+    }
+
+    /// <summary>车辆本体长度，单位 m；不含 <see cref="SafetyMarginMeters"/>。</summary>
+    public double LengthMeters { get; set; }
+
+    /// <summary>车辆本体宽度，单位 m；不含 <see cref="SafetyMarginMeters"/>。</summary>
+    public double WidthMeters { get; set; }
+
+    /// <summary>碰撞检查时加在车体四周的安全余量，单位 m；不会写入地图。</summary>
+    public double SafetyMarginMeters { get; set; }
+
+    /// <summary>车辆允许的最大绝对曲率，单位 1/m；null 表示由 <see cref="MinimumTurningRadiusMeters"/> 提供限制。</summary>
+    public double? MaximumCurvaturePerMeter { get; set; }
+
+    /// <summary>车辆允许的最小转弯半径，单位 m；null 表示由 <see cref="MaximumCurvaturePerMeter"/> 提供限制。</summary>
+    public double? MinimumTurningRadiusMeters { get; set; }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Facade/CoarsePathPlanningJob.cs

@@ -0,0 +1,45 @@
+using MultiWheelC.TrajectoryPlanning.Mapping;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
+
+/// <summary>
+/// 一次从地图输入到 Hybrid A* 粗路径输出的完整业务请求。
+/// 地图边界、分辨率和障碍物几何位于 <see cref="MapRequest"/> 中并使用 mm；位姿和车辆几何使用 m，航向使用 rad，曲率使用 1/m。
+/// </summary>
+public sealed class CoarsePathPlanningJob
+{
+    /// <summary>创建默认目标方向、起步曲率和关闭调试旁路的业务请求。</summary>
+    public CoarsePathPlanningJob()
+    {
+        StartVehicleCurvature = 0d;
+        GoalDirection = GoalDirectionConstraint.Any;
+        DebugOptions = new PlanningDebugOptions();
+    }
+
+    /// <summary>本次唯一建图输入；边界、分辨率和障碍物几何均遵循 Map 模块的 mm 契约。</summary>
+    public PlanningMapRequest MapRequest { get; set; }
+
+    /// <summary>车辆几何中心的起始位姿；位置单位 m，航向单位 rad。</summary>
+    public Pose2D Start { get; set; }
+
+    /// <summary>车辆几何中心的目标位姿；位置单位 m，航向单位 rad。</summary>
+    public Pose2D Goal { get; set; }
+
+    /// <summary>车辆尺寸、安全余量和曲率限制；尺寸单位 m，曲率单位 1/m。</summary>
+    public VehicleParameters Vehicle { get; set; }
+
+    /// <summary>原语、离散、容差、代价和资源上限配置。</summary>
+    public HybridAStarConfiguration Configuration { get; set; }
+
+    /// <summary>车辆起步曲率，单位 1/m；默认值为 0。</summary>
+    public double StartVehicleCurvature { get; set; }
+
+    /// <summary>起步方向约束；null 表示可从前进或倒车开始。</summary>
+    public TravelDirection? StartDirection { get; set; }
+
+    /// <summary>目标进入方向约束；默认值为 <see cref="GoalDirectionConstraint.Any"/>。</summary>
+    public GoalDirectionConstraint GoalDirection { get; set; }
+
+    /// <summary>可选调试旁路配置；默认关闭且使用空接收器，不参与地图或路径计算。</summary>
+    public PlanningDebugOptions DebugOptions { get; set; }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Facade/CoarsePathPlanningJobResult.cs

@@ -0,0 +1,45 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
+
+/// <summary>
+/// 一次粗路径业务编排的不可变结果。
+/// 始终同时保留地图构建结果和规划结果；调试旁路消息只用于诊断，不会修改地图或规划内容。
+/// </summary>
+public sealed class CoarsePathPlanningJobResult
+{
+    /// <summary>
+    /// 创建业务编排结果。
+    /// 参数：mapResult 和 planningResult 均不能为空；debugDiagnostics 可为 null，返回时会转换为只读字符串集合。
+    /// </summary>
+    public CoarsePathPlanningJobResult(PlanningMapBuildResult mapResult, PlanningResult planningResult,
+        IReadOnlyList<string> debugDiagnostics = null)
+    {
+        MapResult = mapResult ?? throw new ArgumentNullException(nameof(mapResult));
+        PlanningResult = planningResult ?? throw new ArgumentNullException(nameof(planningResult));
+        DebugDiagnostics = CopyDiagnostics(debugDiagnostics);
+    }
+
+    /// <summary>本次调用的地图创建结果；失败时读取 <see cref="PlanningMapBuildResult.FailureReason"/>。</summary>
+    public PlanningMapBuildResult MapResult { get; }
+
+    /// <summary>本次调用的粗路径结果；地图创建失败时为无路径的失败状态。</summary>
+    public PlanningResult PlanningResult { get; }
+
+    /// <summary>调试旁路的非致命诊断信息；为空时表示未启用、未发生异常或无额外调试消息。</summary>
+    public IReadOnlyList<string> DebugDiagnostics { get; }
+
+    private static IReadOnlyList<string> CopyDiagnostics(IReadOnlyList<string> source)
+    {
+        if (source == null || source.Count == 0)
+            return new ReadOnlyCollection<string>(new List<string>());
+
+        var copy = new List<string>(source.Count);
+        for (int index = 0; index < source.Count; index++)
+            copy.Add(source[index] ?? string.Empty);
+        return new ReadOnlyCollection<string>(copy);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Facade/CoarsePathPlanningService.cs

@@ -0,0 +1,108 @@
+using System;
+using System.Collections.Generic;
+using System.Threading;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
+
+/// <summary>
+/// 从 <see cref="PlanningMapRequest"/> 到 Hybrid A* 粗路径的一次调用服务。
+/// 服务生命周期内长期持有同一个地图工厂和规划器，以保留地图缓存并避免将 UI、传感器或调试依赖带入规划核心。
+/// </summary>
+public sealed class CoarsePathPlanningService
+{
+    private readonly PlanningMapFactory _mapFactory;
+    private readonly HybridAStarPlanner _planner;
+
+    /// <summary>创建长期复用默认地图工厂与 Hybrid A* 规划器的服务。</summary>
+    public CoarsePathPlanningService()
+        : this(new PlanningMapFactory(), new HybridAStarPlanner())
+    {
+    }
+
+    internal CoarsePathPlanningService(PlanningMapFactory mapFactory, HybridAStarPlanner planner)
+    {
+        _mapFactory = mapFactory ?? throw new ArgumentNullException(nameof(mapFactory));
+        _planner = planner ?? throw new ArgumentNullException(nameof(planner));
+    }
+
+    /// <summary>
+    /// 按固定顺序创建地图并执行一次 Hybrid A* 粗路径规划。
+    /// 参数：job 的地图输入使用 mm，位姿和车辆尺寸使用 m，航向使用 rad，曲率使用 1/m；cancellationToken 会传递给搜索阶段。
+    /// 返回：始终同时保留地图创建结果和规划结果；地图创建失败时不会启动搜索，并返回 <see cref="PlanningStatus.InvalidMap"/> 的空路径结果。
+    /// </summary>
+    public CoarsePathPlanningJobResult Plan(CoarsePathPlanningJob job,
+        CancellationToken cancellationToken = default(CancellationToken))
+    {
+        PlanningOperationBudget budget = CreateBudget(job, cancellationToken);
+        PlanningMapBuildResult mapResult = _mapFactory.Create(job == null ? null : job.MapRequest, budget);
+        if (!mapResult.Succeeded || mapResult.Map == null)
+        {
+            PlanningResult mapFailure = PlanningResult.Failure(MapFailureStatus(mapResult),
+                new PlanningDiagnostics(elapsed: budget.Elapsed, terminationReason: BuildMapFailureReason(mapResult)));
+            return PublishDebug(job, mapResult, mapFailure);
+        }
+
+        var request = new PlanningRequest
+        {
+            Map = mapResult.Map,
+            Start = job.Start,
+            Goal = job.Goal,
+            Vehicle = job.Vehicle,
+            Configuration = job.Configuration,
+            StartVehicleCurvature = job.StartVehicleCurvature,
+            StartDirection = job.StartDirection,
+            GoalDirection = job.GoalDirection,
+        };
+        PlanningResult planningResult = _planner.Plan(request, budget);
+        return PublishDebug(job, mapResult, planningResult);
+    }
+
+    private static PlanningOperationBudget CreateBudget(CoarsePathPlanningJob job, CancellationToken cancellationToken)
+    {
+        HybridAStarConfiguration configuration = job == null ? null : job.Configuration;
+        return configuration != null && configuration.SearchTimeout >= TimeSpan.Zero
+            ? new PlanningOperationBudget(cancellationToken, configuration.SearchTimeout)
+            : PlanningOperationBudget.Unlimited(cancellationToken);
+    }
+
+    private static PlanningStatus MapFailureStatus(PlanningMapBuildResult mapResult)
+    {
+        if (mapResult != null && mapResult.Status == PlanningMapBuildStatus.Cancelled) return PlanningStatus.Cancelled;
+        if (mapResult != null && mapResult.Status == PlanningMapBuildStatus.TimedOut) return PlanningStatus.SearchTimeout;
+        return PlanningStatus.InvalidMap;
+    }
+
+    private static CoarsePathPlanningJobResult PublishDebug(CoarsePathPlanningJob job, PlanningMapBuildResult mapResult,
+        PlanningResult planningResult)
+    {
+        var diagnostics = new List<string>();
+        PlanningDebugOptions options = job == null ? null : job.DebugOptions;
+        if (options != null && options.Enabled)
+        {
+            IPlanningDebugSink sink = options.Sink ?? NullPlanningDebugSink.Instance;
+            try
+            {
+                sink.Publish(mapResult, planningResult);
+            }
+            catch (Exception exception)
+            {
+                diagnostics.Add("调试旁路发布失败：" + exception.GetType().Name + "。" + exception.Message);
+            }
+        }
+
+        return new CoarsePathPlanningJobResult(mapResult, planningResult, diagnostics);
+    }
+
+    private static string BuildMapFailureReason(PlanningMapBuildResult mapResult)
+    {
+        if (mapResult != null && mapResult.Status == PlanningMapBuildStatus.Cancelled)
+            return "规划地图创建已取消，未启动 Hybrid A* 搜索。";
+        if (mapResult != null && mapResult.Status == PlanningMapBuildStatus.TimedOut)
+            return "规划地图创建已超时，未启动 Hybrid A* 搜索。";
+        string reason = mapResult == null ? string.Empty : mapResult.FailureReason;
+        return string.IsNullOrEmpty(reason) ? "规划地图创建失败，未启动 Hybrid A* 搜索。" :
+            "规划地图创建失败，未启动 Hybrid A* 搜索：" + reason;
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Facade/IPlanningDebugSink.cs

@@ -0,0 +1,29 @@
+using MultiWheelC.TrajectoryPlanning.Mapping;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
+
+/// <summary>
+/// 接收一次地图构建和粗路径规划完成后的旁路调试数据。
+/// 实现不得修改输入对象；服务会隔离实现抛出的异常，调试失败不会改变规划结果。
+/// </summary>
+public interface IPlanningDebugSink
+{
+    /// <summary>
+    /// 发布本次编排得到的地图和规划结果。
+    /// 参数：mapResult 为地图创建结果；planningResult 为对应的完整或失败规划结果；二者均不可由接收方修改。
+    /// </summary>
+    void Publish(PlanningMapBuildResult mapResult, PlanningResult planningResult);
+}
+
+internal sealed class NullPlanningDebugSink : IPlanningDebugSink
+{
+    internal static readonly NullPlanningDebugSink Instance = new NullPlanningDebugSink();
+
+    private NullPlanningDebugSink()
+    {
+    }
+
+    public void Publish(PlanningMapBuildResult mapResult, PlanningResult planningResult)
+    {
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Facade/PlanningDebugOptions.cs

@@ -0,0 +1,23 @@
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
+
+/// <summary>
+/// 一次粗路径编排的可选调试旁路配置。
+/// 调试开关和接收器不参与地图输入指纹、缓存键、搜索状态或路径结果。
+/// </summary>
+public sealed class PlanningDebugOptions
+{
+    /// <summary>创建默认关闭并使用空接收器的调试配置。</summary>
+    public PlanningDebugOptions()
+    {
+        Enabled = false;
+        Sink = NullPlanningDebugSink.Instance;
+    }
+
+    /// <summary>是否在本次编排结束后向 <see cref="Sink"/> 发布旁路数据；默认值为 false。</summary>
+    public bool Enabled { get; set; }
+
+    /// <summary>
+    /// 接收地图和规划结果的旁路对象；默认为空实现。赋值为 null 时服务仍会使用空实现，且不会影响主流程。
+    /// </summary>
+    public IPlanningDebugSink Sink { get; set; }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/HybridAStarPlanner.cs

@@ -0,0 +1,264 @@
+using System;
+using System.Diagnostics;
+using System.Globalization;
+using System.Threading;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Output;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+/// <summary>
+/// 已建 <see cref="PlanningGridMap"/> 上 Hybrid A* 粗路径规划的下层门面。
+/// 本类不建图、不读取 UI 或传感器；只有路径经回溯、装配和最终连续复核后才发布成功结果。
+/// </summary>
+public sealed class HybridAStarPlanner
+{
+    private readonly HybridAStarSearch _search;
+    private readonly PathBacktracker _backtracker;
+    private readonly CoarsePathAssembler _assembler;
+    private readonly CoarsePathValidator _validator;
+    private readonly FootprintCollisionChecker _collisionChecker;
+
+    /// <summary>创建使用默认搜索、回溯、装配和最终复核组件的规划器。</summary>
+    public HybridAStarPlanner()
+        : this(new HybridAStarSearch(), new PathBacktracker(), new CoarsePathAssembler(), new CoarsePathValidator(),
+            new FootprintCollisionChecker())
+    {
+    }
+
+    internal HybridAStarPlanner(HybridAStarSearch search, PathBacktracker backtracker, CoarsePathAssembler assembler,
+        CoarsePathValidator validator, FootprintCollisionChecker collisionChecker)
+    {
+        _search = search ?? throw new ArgumentNullException(nameof(search));
+        _backtracker = backtracker ?? throw new ArgumentNullException(nameof(backtracker));
+        _assembler = assembler ?? throw new ArgumentNullException(nameof(assembler));
+        _validator = validator ?? throw new ArgumentNullException(nameof(validator));
+        _collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
+    }
+
+    /// <summary>
+    /// 在请求提供的不可变规划地图上执行一次 Hybrid A* 粗路径规划。
+    /// 参数：request 的位置单位为 m、航向单位为 rad、曲率单位为 1/m；cancellationToken 会在搜索扩展检查点取消。
+    /// 返回：输入、边界、碰撞、搜索、回溯或最终复核失败均返回空路径；只有 <see cref="PlanningStatus.Success"/> 携带完整路径和分段。
+    /// </summary>
+    public PlanningResult Plan(PlanningRequest request, CancellationToken cancellationToken = default(CancellationToken))
+    {
+        PlanningOperationBudget budget = request != null && request.Configuration != null && request.Configuration.SearchTimeout >= TimeSpan.Zero
+            ? new PlanningOperationBudget(cancellationToken, request.Configuration.SearchTimeout)
+            : PlanningOperationBudget.Unlimited(cancellationToken);
+        return Plan(request, budget);
+    }
+
+    /// <summary>使用门面传入的共享预算执行预检、搜索和最终路径复核。</summary>
+    internal PlanningResult Plan(PlanningRequest request, PlanningOperationBudget budget)
+    {
+        Stopwatch pathSearchStopwatch = null;
+        try
+        {
+            if (budget == null) throw new ArgumentNullException(nameof(budget));
+            PlanningOperationStopReason stopReason = budget.GetStopReason();
+            if (stopReason != PlanningOperationStopReason.None)
+                return Failure(ToPlanningStatus(stopReason), budget, "规划在开始前已停止。", null);
+
+            PlanningStatus preflightStatus = ValidatePreflight(request, out string preflightReason);
+            if (preflightStatus != PlanningStatus.Success)
+                return Failure(preflightStatus, budget, preflightReason, null);
+
+            if (!IsFootprintInsideMap(request.Start, request.Map, request.Vehicle))
+                return Failure(PlanningStatus.StartOutsideMap, budget, "起始扩大车体不完全位于地图边界内。", null);
+            if (!_collisionChecker.IsPoseCollisionFree(request.Start, request.Map, request.Vehicle, 0d, out _))
+                return Failure(PlanningStatus.StartInCollision, budget, "起始扩大车体与障碍物相交或擦边。", null);
+            if (!IsFootprintInsideMap(request.Goal, request.Map, request.Vehicle))
+                return Failure(PlanningStatus.GoalOutsideMap, budget, "目标扩大车体不完全位于地图边界内。", null);
+            if (!_collisionChecker.IsPoseCollisionFree(request.Goal, request.Map, request.Vehicle, 0d, out _))
+                return Failure(PlanningStatus.GoalInCollision, budget, "目标扩大车体与障碍物相交或擦边。", null);
+
+            stopReason = budget.GetStopReason();
+            if (stopReason != PlanningOperationStopReason.None)
+                return Failure(ToPlanningStatus(stopReason), budget, "规划在搜索前已停止。", null);
+            pathSearchStopwatch = Stopwatch.StartNew();
+            HybridAStarSearchResult searchResult = _search.Search(request, budget);
+            if (searchResult == null)
+                return Failure(PlanningStatus.InternalError, budget, "搜索器未返回结果。", null, pathSearchStopwatch);
+            if (searchResult.Status != PlanningStatus.Success)
+                return Failure(searchResult.Status, budget,
+                    BuildSearchFailureReason(searchResult, request.Configuration), searchResult, pathSearchStopwatch);
+
+            if (!_backtracker.TryBacktrack(searchResult, request, out BacktrackedPath backtrackedPath, out string backtrackingReason))
+                return Failure(PlanningStatus.BacktrackingFailed, budget, backtrackingReason, searchResult, pathSearchStopwatch);
+            if (!_assembler.TryAssemble(backtrackedPath, request, out var path, out var segments, out string assemblyReason))
+                return Failure(PlanningStatus.FinalValidationFailed, budget, assemblyReason, searchResult, pathSearchStopwatch);
+            if (!_validator.TryValidate(path, segments, request, out double minimumClearanceMeters, out string validationReason))
+                return Failure(PlanningStatus.FinalValidationFailed, budget, validationReason, searchResult, pathSearchStopwatch);
+
+            double pathLengthMeters = path[path.Count - 1].ArcLength;
+            return PlanningResult.Success(path, segments, CreateDiagnostics(searchResult, budget.Elapsed, pathLengthMeters,
+                minimumClearanceMeters, string.Empty, pathSearchStopwatch.Elapsed));
+        }
+        catch (Exception exception)
+        {
+            string reason = "规划内部错误：" + exception.GetType().Name +
+                (string.IsNullOrEmpty(exception.Message) ? "。" : "。" + exception.Message);
+            return Failure(PlanningStatus.InternalError, budget ?? PlanningOperationBudget.Unlimited(CancellationToken.None),
+                reason, null, pathSearchStopwatch);
+        }
+    }
+
+    private static PlanningStatus ValidatePreflight(PlanningRequest request, out string failureReason)
+    {
+        failureReason = string.Empty;
+        if (request == null || request.Map == null || request.Vehicle == null || request.Configuration == null ||
+            !IsFinitePose(request.Start) || !IsFinitePose(request.Goal) || !NumericGuard.IsFinite(request.StartVehicleCurvature) ||
+            !IsGoalDirection(request.GoalDirection) || (request.StartDirection.HasValue && !IsTravelDirection(request.StartDirection.Value)))
+        {
+            failureReason = "规划请求缺少必要对象或包含非法数值。";
+            return PlanningStatus.InvalidRequest;
+        }
+
+        PlanningGridMap map = request.Map;
+        if (map.Bounds == null || map.Rows <= 0 || map.Cols <= 0 || !NumericGuard.IsPositiveFinite(map.ResolutionMeters))
+        {
+            failureReason = "规划地图结构无效。";
+            return PlanningStatus.InvalidMap;
+        }
+        if (!map.PlanningReady)
+        {
+            failureReason = string.IsNullOrEmpty(map.PlanningBlockReason) ? "规划地图尚未就绪。" : map.PlanningBlockReason;
+            return PlanningStatus.MapNotReady;
+        }
+
+        VehicleParameters vehicle = request.Vehicle;
+        if (!NumericGuard.IsPositiveFinite(vehicle.LengthMeters) || !NumericGuard.IsPositiveFinite(vehicle.WidthMeters) ||
+            !NumericGuard.IsFinite(vehicle.SafetyMarginMeters) || vehicle.SafetyMarginMeters < 0d ||
+            !VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
+        {
+            failureReason = "车辆尺寸、安全余量或曲率限制无效。";
+            return PlanningStatus.InvalidVehicleParameters;
+        }
+
+        HybridAStarConfiguration configuration = request.Configuration;
+        if (!IsValidConfiguration(configuration) || Math.Abs(request.StartVehicleCurvature) > maximumCurvaturePerMeter ||
+            (request.StartDirection == TravelDirection.Reverse && !configuration.AllowReverse))
+        {
+            failureReason = "Hybrid A* 曲率、离散、代价或资源配置无效。";
+            return PlanningStatus.InvalidCurvatureConfiguration;
+        }
+
+        return PlanningStatus.Success;
+    }
+
+    private static bool IsValidConfiguration(HybridAStarConfiguration configuration)
+    {
+        return NumericGuard.IsPositiveFinite(configuration.PrimitiveLengthMeters) &&
+            NumericGuard.IsPositiveFinite(configuration.IntegrationStepMeters) &&
+            NumericGuard.IsPositiveFinite(configuration.MaximumCollisionCheckStepMeters) &&
+            NumericGuard.IsPositiveFinite(configuration.HeadingResolutionRadians) &&
+            configuration.HeadingResolutionRadians <= 2d * Math.PI && configuration.CurvatureLevelCount >= 3 &&
+            configuration.CurvatureLevelCount % 2 == 1 && NumericGuard.IsFinite(configuration.GoalPositionToleranceMeters) &&
+            configuration.GoalPositionToleranceMeters >= 0d && NumericGuard.IsFinite(configuration.GoalHeadingToleranceRadians) &&
+            configuration.GoalHeadingToleranceRadians >= 0d && configuration.MaximumExpandedNodes >= 0 &&
+            configuration.SearchTimeout >= TimeSpan.Zero && NumericGuard.IsFinite(configuration.HeuristicWeight) &&
+            configuration.HeuristicWeight >= 0d && NumericGuard.IsPositiveFinite(configuration.ReverseCostMultiplier) &&
+            NumericGuard.IsFinite(configuration.GearSwitchPenaltyMeters) && configuration.GearSwitchPenaltyMeters >= 0d &&
+            NumericGuard.IsFinite(configuration.CurvatureMagnitudeWeight) && configuration.CurvatureMagnitudeWeight >= 0d &&
+            NumericGuard.IsFinite(configuration.CurvatureChangePenaltyMetersPerLevel) &&
+            configuration.CurvatureChangePenaltyMetersPerLevel >= 0d && NumericGuard.IsFinite(configuration.ClearanceCostWeight) &&
+            configuration.ClearanceCostWeight >= 0d && NumericGuard.IsPositiveFinite(configuration.ClearanceCostDistanceMeters);
+    }
+
+    private static bool IsFootprintInsideMap(Pose2D pose, PlanningGridMap map, VehicleParameters vehicle)
+    {
+        if (!map.TryWorldToGrid(pose.X, pose.Y, out _, out _)) return false;
+
+        double halfLengthMeters = vehicle.LengthMeters / 2d + vehicle.SafetyMarginMeters;
+        double halfWidthMeters = vehicle.WidthMeters / 2d + vehicle.SafetyMarginMeters;
+        double longitudinalX = Math.Cos(pose.Heading);
+        double longitudinalY = Math.Sin(pose.Heading);
+        double lateralX = -longitudinalY;
+        double lateralY = longitudinalX;
+        for (int longitudinalSign = -1; longitudinalSign <= 1; longitudinalSign += 2)
+        for (int lateralSign = -1; lateralSign <= 1; lateralSign += 2)
+        {
+            double cornerX = pose.X + longitudinalSign * halfLengthMeters * longitudinalX + lateralSign * halfWidthMeters * lateralX;
+            double cornerY = pose.Y + longitudinalSign * halfLengthMeters * longitudinalY + lateralSign * halfWidthMeters * lateralY;
+            if (!map.TryWorldToGrid(cornerX, cornerY, out _, out _)) return false;
+        }
+
+        return true;
+    }
+
+    private static string BuildSearchFailureReason(HybridAStarSearchResult searchResult,
+        HybridAStarConfiguration configuration)
+    {
+        string reason = string.IsNullOrEmpty(searchResult.TerminationReason)
+            ? "Hybrid A* 搜索以 " + searchResult.Status + " 状态终止。"
+            : searchResult.TerminationReason;
+        string resourceLimit = string.Empty;
+        if (searchResult.Status == PlanningStatus.SearchTimeout)
+        {
+            resourceLimit = "总预算=" + configuration.SearchTimeout.TotalSeconds.ToString(
+                "F3", CultureInfo.InvariantCulture) + "s；";
+        }
+        else if (searchResult.Status == PlanningStatus.SearchNodeLimitExceeded)
+        {
+            resourceLimit = "节点上限=" + configuration.MaximumExpandedNodes.ToString(
+                CultureInfo.InvariantCulture) + "；";
+        }
+
+        return reason + resourceLimit +
+            "扩展=" + searchResult.ExpandedNodeCount.ToString(CultureInfo.InvariantCulture) + "，" +
+            "生成=" + searchResult.GeneratedNodeCount.ToString(CultureInfo.InvariantCulture) + "，" +
+            "重开=" + searchResult.ReopenedNodeCount.ToString(CultureInfo.InvariantCulture) + "，" +
+            "陈旧条目=" + searchResult.StaleOpenListEntryCount.ToString(CultureInfo.InvariantCulture) + "，" +
+            "Open List峰值=" + searchResult.PeakOpenListCount.ToString(CultureInfo.InvariantCulture) + "。";
+    }
+
+    private static PlanningResult Failure(PlanningStatus status, PlanningOperationBudget budget, string reason,
+        HybridAStarSearchResult searchResult, Stopwatch pathSearchStopwatch = null)
+    {
+        TimeSpan pathSearchElapsed = pathSearchStopwatch == null ? TimeSpan.Zero : pathSearchStopwatch.Elapsed;
+        return PlanningResult.Failure(status, CreateDiagnostics(searchResult, budget.Elapsed, 0d, 0d,
+            reason, pathSearchElapsed));
+    }
+
+    private static PlanningDiagnostics CreateDiagnostics(HybridAStarSearchResult searchResult, TimeSpan elapsed,
+        double pathLengthMeters, double minimumClearanceMeters, string reason, TimeSpan pathSearchElapsed)
+    {
+        return new PlanningDiagnostics(
+            searchResult == null ? 0 : searchResult.ExpandedNodeCount,
+            searchResult == null ? 0 : searchResult.GeneratedNodeCount,
+            searchResult == null ? 0 : searchResult.ReopenedNodeCount,
+            searchResult == null ? 0 : searchResult.StaleOpenListEntryCount,
+            searchResult == null ? 0 : searchResult.PeakOpenListCount,
+            pathLengthMeters,
+            minimumClearanceMeters,
+            elapsed,
+            reason,
+            pathSearchElapsed);
+    }
+
+    private static bool IsFinitePose(Pose2D pose)
+    {
+        return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
+    }
+
+    private static bool IsTravelDirection(TravelDirection direction)
+    {
+        return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
+    }
+
+    private static bool IsGoalDirection(GoalDirectionConstraint direction)
+    {
+        return direction == GoalDirectionConstraint.Any || direction == GoalDirectionConstraint.Forward || direction == GoalDirectionConstraint.Reverse;
+    }
+
+    private static PlanningStatus ToPlanningStatus(PlanningOperationStopReason stopReason)
+    {
+        if (stopReason == PlanningOperationStopReason.Cancelled) return PlanningStatus.Cancelled;
+        if (stopReason == PlanningOperationStopReason.TimedOut) return PlanningStatus.SearchTimeout;
+        throw new ArgumentOutOfRangeException(nameof(stopReason));
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Output/CoarsePathAssembler.cs

@@ -0,0 +1,216 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Output;
+
+/// <summary>
+/// 将回溯原语装配为调用方可消费的稠密路径和包含式方向分段。
+/// 装配器保留原语边界的换向双点，其余相邻重复位姿会被删除。
+/// </summary>
+public sealed class CoarsePathAssembler
+{
+    private const double DuplicateTolerance = 1e-8d;
+    private readonly FootprintCollisionChecker _collisionChecker;
+
+    /// <summary>创建使用默认连续车体检查器的路径装配器。</summary>
+    public CoarsePathAssembler()
+        : this(new FootprintCollisionChecker())
+    {
+    }
+
+    /// <summary>创建使用指定连续车体检查器的路径装配器。</summary>
+    public CoarsePathAssembler(FootprintCollisionChecker collisionChecker)
+    {
+        _collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
+    }
+
+    /// <summary>
+    /// 从已回溯的恒曲率原语构造稠密路径。
+    /// 参数：backtrackedPath 提供原语顺序；request 提供地图和车辆；path、segments 为成功时的只读输出。
+    /// 返回：首点可通过连续车体检查、每个原语积分点数据一致且方向分段完整覆盖时为 true；否则返回 false 且输出为空。
+    /// </summary>
+    public bool TryAssemble(BacktrackedPath backtrackedPath, PlanningRequest request,
+        out IReadOnlyList<CoarsePathPoint> path, out IReadOnlyList<PathSegment> segments, out string failureReason)
+    {
+        path = EmptyPath();
+        segments = EmptySegments();
+        failureReason = string.Empty;
+        if (backtrackedPath == null || request == null || request.Map == null || request.Vehicle == null ||
+            !IsFinitePose(backtrackedPath.Start) || !IsTravelDirection(backtrackedPath.StartDirection) ||
+            !NumericGuard.IsFinite(backtrackedPath.StartCurvaturePerMeter))
+        {
+            failureReason = "路径装配输入无效。";
+            return false;
+        }
+
+        if (!_collisionChecker.IsPoseCollisionFree(backtrackedPath.Start, request.Map, request.Vehicle, 0d, out double startClearanceMeters))
+        {
+            failureReason = "回溯路径起点未通过连续车体检查。";
+            return false;
+        }
+
+        var points = new List<CoarsePathPoint>();
+        TravelDirection currentDirection = backtrackedPath.Primitives.Count > 0
+            ? backtrackedPath.Primitives[0].Direction
+            : backtrackedPath.StartDirection;
+        double currentCurvature = request.StartVehicleCurvature;
+        double normalizedStartHeading = AngleMath.NormalizeRadians(backtrackedPath.Start.Heading);
+        if (!NumericGuard.IsFinite(normalizedStartHeading))
+        {
+            failureReason = "回溯路径起点航向无效。";
+            return false;
+        }
+
+        points.Add(new CoarsePathPoint(backtrackedPath.Start.X, backtrackedPath.Start.Y, normalizedStartHeading,
+            backtrackedPath.Start.Heading, 0d, currentDirection, currentCurvature, startClearanceMeters, false,
+            CoarsePathPointSource.Start));
+
+        for (int primitiveIndex = 0; primitiveIndex < backtrackedPath.Primitives.Count; primitiveIndex++)
+        {
+            MotionPrimitive primitive = backtrackedPath.Primitives[primitiveIndex];
+            if (!IsValidPrimitive(primitive))
+            {
+                failureReason = "回溯路径包含无效原语。";
+                return false;
+            }
+
+            CoarsePathPoint lastPoint = points[points.Count - 1];
+            if (primitive.Direction != lastPoint.Direction)
+            {
+                // 换向处的旧方向终点和新方向起点必须共存，二者位置、航向、弧长完全相同。
+                points.Add(new CoarsePathPoint(lastPoint.X, lastPoint.Y, lastPoint.Heading, lastPoint.UnwrappedHeading,
+                    lastPoint.ArcLength, primitive.Direction, primitive.CurvaturePerMeter, lastPoint.BodyClearance,
+                    true, CoarsePathPointSource.MotionPrimitive));
+            }
+
+            Pose2D previousPose = primitive.Start;
+            for (int pointIndex = 0; pointIndex < primitive.Points.Count; pointIndex++)
+            {
+                Pose2D pose = primitive.Points[pointIndex];
+                double bodyClearanceMeters = primitive.BodyClearancesMeters[pointIndex];
+                if (!IsFinitePose(pose) || !IsValidClearance(bodyClearanceMeters))
+                {
+                    failureReason = "原语积分点或净空无效。";
+                    return false;
+                }
+
+                CoarsePathPoint previousPoint = points[points.Count - 1];
+                double arcIncrementMeters = CalculateArcIncrement(previousPose, pose, primitive.CurvaturePerMeter);
+                if (!NumericGuard.IsFinite(arcIncrementMeters) || arcIncrementMeters <= 0d)
+                {
+                    failureReason = "原语积分点未产生正弧长。";
+                    return false;
+                }
+
+                if (IsSamePose(previousPoint, pose))
+                {
+                    // 非换向情况下不允许重复采样点泄露到对外路径。
+                    previousPose = pose;
+                    continue;
+                }
+
+                double normalizedHeading = AngleMath.NormalizeRadians(pose.Heading);
+                double headingDelta = AngleMath.ShortestSignedDifference(previousPoint.Heading, normalizedHeading);
+                if (!NumericGuard.IsFinite(normalizedHeading) || !NumericGuard.IsFinite(headingDelta))
+                {
+                    failureReason = "原语积分点航向无法展开。";
+                    return false;
+                }
+
+                CoarsePathPointSource source = primitive.IsGoalTruncation && pointIndex == primitive.Points.Count - 1
+                    ? CoarsePathPointSource.GoalTruncation
+                    : CoarsePathPointSource.MotionPrimitive;
+                points.Add(new CoarsePathPoint(pose.X, pose.Y, normalizedHeading,
+                    previousPoint.UnwrappedHeading + headingDelta, previousPoint.ArcLength + arcIncrementMeters,
+                    primitive.Direction, primitive.CurvaturePerMeter, bodyClearanceMeters, false, source));
+                previousPose = pose;
+            }
+        }
+
+        if (points.Count == 0)
+        {
+            failureReason = "路径装配未产生起点。";
+            return false;
+        }
+
+        path = new ReadOnlyCollection<CoarsePathPoint>(points);
+        segments = BuildSegments(points);
+        return true;
+    }
+
+    private static IReadOnlyList<PathSegment> BuildSegments(IReadOnlyList<CoarsePathPoint> points)
+    {
+        var segments = new List<PathSegment>();
+        int startIndex = 0;
+        TravelDirection direction = points[0].Direction;
+        for (int index = 1; index < points.Count; index++)
+        {
+            if (points[index].Direction == direction) continue;
+            segments.Add(new PathSegment(segments.Count, direction, startIndex, index - 1,
+                points[startIndex].IsGearSwitchPoint, true));
+            startIndex = index;
+            direction = points[index].Direction;
+        }
+
+        segments.Add(new PathSegment(segments.Count, direction, startIndex, points.Count - 1,
+            points[startIndex].IsGearSwitchPoint, false));
+        return new ReadOnlyCollection<PathSegment>(segments);
+    }
+
+    private static double CalculateArcIncrement(Pose2D from, Pose2D to, double curvaturePerMeter)
+    {
+        if (!IsFinitePose(from) || !IsFinitePose(to) || !NumericGuard.IsFinite(curvaturePerMeter)) return double.NaN;
+        if (Math.Abs(curvaturePerMeter) < 1e-12d)
+        {
+            double deltaX = to.X - from.X;
+            double deltaY = to.Y - from.Y;
+            return Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
+        }
+
+        double headingDelta = AngleMath.ShortestSignedDifference(from.Heading, to.Heading);
+        return Math.Abs(headingDelta / curvaturePerMeter);
+    }
+
+    private static bool IsValidPrimitive(MotionPrimitive primitive)
+    {
+        return primitive != null && primitive.Start != null && primitive.Points != null && primitive.BodyClearancesMeters != null &&
+            primitive.Points.Count == primitive.BodyClearancesMeters.Count && primitive.Points.Count > 0 &&
+            IsTravelDirection(primitive.Direction) && NumericGuard.IsFinite(primitive.CurvaturePerMeter) &&
+            NumericGuard.IsPositiveFinite(primitive.ActualLengthMeters);
+    }
+
+    private static bool IsSamePose(CoarsePathPoint point, Pose2D pose)
+    {
+        return Math.Abs(point.X - pose.X) <= DuplicateTolerance && Math.Abs(point.Y - pose.Y) <= DuplicateTolerance &&
+            Math.Abs(AngleMath.ShortestSignedDifference(point.Heading, pose.Heading)) <= DuplicateTolerance;
+    }
+
+    private static bool IsFinitePose(Pose2D pose)
+    {
+        return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
+    }
+
+    private static bool IsTravelDirection(TravelDirection direction)
+    {
+        return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
+    }
+
+    private static bool IsValidClearance(double clearanceMeters)
+    {
+        return !double.IsNaN(clearanceMeters) && clearanceMeters >= 0d;
+    }
+
+    private static IReadOnlyList<CoarsePathPoint> EmptyPath()
+    {
+        return new ReadOnlyCollection<CoarsePathPoint>(new List<CoarsePathPoint>());
+    }
+
+    private static IReadOnlyList<PathSegment> EmptySegments()
+    {
+        return new ReadOnlyCollection<PathSegment>(new List<PathSegment>());
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Output/CoarsePathValidator.cs

@@ -0,0 +1,245 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Output;
+
+/// <summary>
+/// 对准备发布的粗路径执行独立的连续安全与输出契约复核。
+/// 复核失败的路径不得被包装为 <see cref="PlanningStatus.Success"/>。
+/// </summary>
+public sealed class CoarsePathValidator
+{
+    private const double NumericTolerance = 1e-6d;
+    private readonly FootprintCollisionChecker _collisionChecker;
+
+    /// <summary>创建使用默认连续车体检查器的最终路径复核器。</summary>
+    public CoarsePathValidator()
+        : this(new FootprintCollisionChecker())
+    {
+    }
+
+    /// <summary>创建使用指定连续车体检查器的最终路径复核器。</summary>
+    public CoarsePathValidator(FootprintCollisionChecker collisionChecker)
+    {
+        _collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
+    }
+
+    /// <summary>
+    /// 复核路径数值、曲率、连续扫掠碰撞、终点约束、累计弧长和方向分段。
+    /// 参数：path 与 segments 为待发布输出；request 必须是生成该路径的请求；minimumBodyClearanceMeters 返回沿途的保守净空下界。
+    /// 返回：所有规则通过时为 true；否则返回 false、写入失败原因，调用方必须丢弃 path 和 segments。
+    /// </summary>
+    public bool TryValidate(IReadOnlyList<CoarsePathPoint> path, IReadOnlyList<PathSegment> segments,
+        PlanningRequest request, out double minimumBodyClearanceMeters, out string failureReason)
+    {
+        minimumBodyClearanceMeters = 0d;
+        failureReason = string.Empty;
+        if (path == null || segments == null || request == null || request.Map == null || request.Vehicle == null ||
+            request.Configuration == null || path.Count == 0 || segments.Count == 0)
+        {
+            failureReason = "最终路径或复核请求为空。";
+            return false;
+        }
+
+        if (!VehicleKinematics.TryGetMaximumCurvaturePerMeter(request.Vehicle, out double maximumCurvaturePerMeter))
+        {
+            failureReason = "车辆曲率约束无效。";
+            return false;
+        }
+
+        CoarsePathPoint first = path[0];
+        if (!IsValidPoint(first) || first.Source != CoarsePathPointSource.Start || first.IsGearSwitchPoint ||
+            Math.Abs(first.ArcLength) > NumericTolerance || !IsSamePose(first, request.Start))
+        {
+            failureReason = "最终路径首点不符合起点契约。";
+            return false;
+        }
+
+        double minimumClearance = double.PositiveInfinity;
+        for (int index = 0; index < path.Count; index++)
+        {
+            CoarsePathPoint current = path[index];
+            if (!IsValidPoint(current) || Math.Abs(current.VehicleCurvature) > maximumCurvaturePerMeter + NumericTolerance)
+            {
+                failureReason = "最终路径包含非法数值或超限曲率。";
+                return false;
+            }
+
+            var currentPose = new Pose2D(current.X, current.Y, current.Heading);
+            if (!_collisionChecker.IsPoseCollisionFree(currentPose, request.Map, request.Vehicle, 0d, out double poseClearanceMeters) ||
+                IsClearanceOverclaimed(current.BodyClearance, poseClearanceMeters))
+            {
+                failureReason = "最终路径点未通过连续车体碰撞复核。";
+                return false;
+            }
+
+            minimumClearance = Math.Min(minimumClearance, current.BodyClearance);
+            if (index == 0) continue;
+
+            CoarsePathPoint previous = path[index - 1];
+            if (current.ArcLength + NumericTolerance < previous.ArcLength ||
+                !IsUnwrappedHeadingContinuous(previous, current))
+            {
+                failureReason = "最终路径的弧长或展开航向不连续。";
+                return false;
+            }
+
+            bool isDuplicate = IsDuplicatePoseAndArcLength(previous, current);
+            if (isDuplicate)
+            {
+                if (previous.Direction == current.Direction || !current.IsGearSwitchPoint)
+                {
+                    failureReason = "相邻重复点不是合法换向对。";
+                    return false;
+                }
+            }
+            else
+            {
+                if (current.IsGearSwitchPoint || current.ArcLength <= previous.ArcLength + NumericTolerance ||
+                    !IsArcIncrementConsistent(previous, current))
+                {
+                    failureReason = "非换向路径点的弧长增量不一致。";
+                    return false;
+                }
+            }
+
+            var previousPose = new Pose2D(previous.X, previous.Y, previous.Heading);
+            if (!_collisionChecker.IsSweptMotionCollisionFree(previousPose, currentPose, request.Map, request.Vehicle,
+                request.Configuration.MaximumCollisionCheckStepMeters, out double sweptClearanceMeters))
+            {
+                failureReason = "最终路径相邻点之间的车体扫掠碰撞复核失败。";
+                return false;
+            }
+
+            minimumClearance = Math.Min(minimumClearance, sweptClearanceMeters);
+        }
+
+        CoarsePathPoint last = path[path.Count - 1];
+        if (!GoalToleranceChecker.IsSatisfied(new Pose2D(last.X, last.Y, last.Heading), request.Goal, request.Configuration,
+            last.Direction, request.GoalDirection) || (path.Count > 1 && last.Source != CoarsePathPointSource.GoalTruncation))
+        {
+            failureReason = "最终路径末点未满足终点容差、方向或来源契约。";
+            return false;
+        }
+
+        if (!AreSegmentsValid(path, segments, out failureReason)) return false;
+        minimumBodyClearanceMeters = minimumClearance;
+        return true;
+    }
+
+    private static bool AreSegmentsValid(IReadOnlyList<CoarsePathPoint> path, IReadOnlyList<PathSegment> segments, out string failureReason)
+    {
+        failureReason = string.Empty;
+        int expectedStartIndex = 0;
+        for (int segmentIndex = 0; segmentIndex < segments.Count; segmentIndex++)
+        {
+            PathSegment segment = segments[segmentIndex];
+            if (segment == null || segment.SegmentIndex != segmentIndex || segment.StartIndex != expectedStartIndex ||
+                segment.StartIndex < 0 || segment.EndIndex < segment.StartIndex || segment.EndIndex >= path.Count ||
+                segment.StartsAtGearSwitch != path[segment.StartIndex].IsGearSwitchPoint)
+            {
+                failureReason = "方向分段索引或起始换向标记无效。";
+                return false;
+            }
+
+            for (int pointIndex = segment.StartIndex; pointIndex <= segment.EndIndex; pointIndex++)
+            {
+                if (path[pointIndex].Direction != segment.Direction)
+                {
+                    failureReason = "方向分段包含不同方向的路径点。";
+                    return false;
+                }
+            }
+
+            bool hasNextSegment = segmentIndex + 1 < segments.Count;
+            bool expectedEndsAtGearSwitch = hasNextSegment && segment.EndIndex + 1 < path.Count &&
+                path[segment.EndIndex + 1].IsGearSwitchPoint;
+            if (segment.EndsAtGearSwitch != expectedEndsAtGearSwitch)
+            {
+                failureReason = "方向分段末尾换向标记无效。";
+                return false;
+            }
+
+            expectedStartIndex = segment.EndIndex + 1;
+        }
+
+        if (expectedStartIndex != path.Count)
+        {
+            failureReason = "方向分段未完整覆盖路径点。";
+            return false;
+        }
+
+        return true;
+    }
+
+    private static bool IsValidPoint(CoarsePathPoint point)
+    {
+        if (point == null || !NumericGuard.IsFinite(point.X) || !NumericGuard.IsFinite(point.Y) ||
+            !NumericGuard.IsFinite(point.Heading) || !NumericGuard.IsFinite(point.UnwrappedHeading) ||
+            !NumericGuard.IsFinite(point.ArcLength) || point.ArcLength < 0d ||
+            !NumericGuard.IsFinite(point.VehicleCurvature) || !IsTravelDirection(point.Direction) ||
+            !IsValidClearance(point.BodyClearance))
+            return false;
+
+        return Math.Abs(AngleMath.ShortestSignedDifference(point.Heading, AngleMath.NormalizeRadians(point.Heading))) <= NumericTolerance;
+    }
+
+    private static bool IsSamePose(CoarsePathPoint point, Pose2D pose)
+    {
+        return point != null && pose != null && Math.Abs(point.X - pose.X) <= NumericTolerance &&
+            Math.Abs(point.Y - pose.Y) <= NumericTolerance &&
+            Math.Abs(AngleMath.ShortestSignedDifference(point.Heading, pose.Heading)) <= NumericTolerance;
+    }
+
+    private static bool IsUnwrappedHeadingContinuous(CoarsePathPoint previous, CoarsePathPoint current)
+    {
+        double expectedDelta = AngleMath.ShortestSignedDifference(previous.Heading, current.Heading);
+        return NumericGuard.IsFinite(expectedDelta) &&
+            Math.Abs((current.UnwrappedHeading - previous.UnwrappedHeading) - expectedDelta) <= NumericTolerance;
+    }
+
+    private static bool IsDuplicatePoseAndArcLength(CoarsePathPoint previous, CoarsePathPoint current)
+    {
+        return Math.Abs(previous.X - current.X) <= NumericTolerance && Math.Abs(previous.Y - current.Y) <= NumericTolerance &&
+            Math.Abs(AngleMath.ShortestSignedDifference(previous.Heading, current.Heading)) <= NumericTolerance &&
+            Math.Abs(previous.ArcLength - current.ArcLength) <= NumericTolerance;
+    }
+
+    private static bool IsArcIncrementConsistent(CoarsePathPoint previous, CoarsePathPoint current)
+    {
+        double expectedIncrement;
+        if (Math.Abs(current.VehicleCurvature) < 1e-12d)
+        {
+            double deltaX = current.X - previous.X;
+            double deltaY = current.Y - previous.Y;
+            expectedIncrement = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
+        }
+        else
+        {
+            double headingDelta = AngleMath.ShortestSignedDifference(previous.Heading, current.Heading);
+            expectedIncrement = Math.Abs(headingDelta / current.VehicleCurvature);
+        }
+
+        return NumericGuard.IsFinite(expectedIncrement) && expectedIncrement > 0d &&
+            Math.Abs((current.ArcLength - previous.ArcLength) - expectedIncrement) <= NumericTolerance;
+    }
+
+    private static bool IsClearanceOverclaimed(double reportedClearanceMeters, double checkedClearanceMeters)
+    {
+        if (double.IsPositiveInfinity(reportedClearanceMeters)) return !double.IsPositiveInfinity(checkedClearanceMeters);
+        return reportedClearanceMeters > checkedClearanceMeters + NumericTolerance;
+    }
+
+    private static bool IsValidClearance(double clearanceMeters)
+    {
+        return !double.IsNaN(clearanceMeters) && clearanceMeters >= 0d;
+    }
+
+    private static bool IsTravelDirection(TravelDirection direction)
+    {
+        return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Output/PathBacktracker.cs

@@ -0,0 +1,167 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Output;
+
+/// <summary>
+/// 成功搜索父链重新生成后的原语序列。
+/// 起点位置使用 m/rad，起始曲率使用 1/m；原语集合按从起点到终点的顺序排列。
+/// </summary>
+public sealed class BacktrackedPath
+{
+    internal BacktrackedPath(Pose2D start, TravelDirection startDirection, double startCurvaturePerMeter,
+        IReadOnlyList<MotionPrimitive> primitives)
+    {
+        Start = start;
+        StartDirection = startDirection;
+        StartCurvaturePerMeter = startCurvaturePerMeter;
+        Primitives = primitives;
+    }
+
+    /// <summary>原始请求中的起始车辆中心位姿；位置单位 m，航向单位 rad。</summary>
+    public Pose2D Start { get; }
+
+    /// <summary>成功父链根节点记录的起步方向。</summary>
+    public TravelDirection StartDirection { get; }
+
+    /// <summary>成功父链根节点离散后的起始曲率，单位 1/m。</summary>
+    public double StartCurvaturePerMeter { get; }
+
+    /// <summary>按起点到终点顺序重新积分的恒曲率原语；每项都不包含自身起点。</summary>
+    public IReadOnlyList<MotionPrimitive> Primitives { get; }
+}
+
+/// <summary>
+/// 仅依据成功节点的父索引和原语描述，确定性地重建稠密原语序列。
+/// 不复用搜索期保存的积分点，以保证输出与当前解析积分、扫掠检查规则一致。
+/// </summary>
+public sealed class PathBacktracker
+{
+    private const double PoseComparisonTolerance = 1e-7d;
+    private const double LengthComparisonTolerance = 1e-7d;
+    private readonly MotionPrimitiveGenerator _primitiveGenerator;
+
+    /// <summary>创建使用默认解析积分器的回溯器。</summary>
+    public PathBacktracker()
+        : this(new MotionPrimitiveGenerator())
+    {
+    }
+
+    /// <summary>创建使用指定原语生成器的回溯器。</summary>
+    public PathBacktracker(MotionPrimitiveGenerator primitiveGenerator)
+    {
+        _primitiveGenerator = primitiveGenerator ?? throw new ArgumentNullException(nameof(primitiveGenerator));
+    }
+
+    /// <summary>
+    /// 依据搜索成功节点重建从起点到终点的原语序列。
+    /// 参数：searchResult 必须是携带成功节点索引的搜索结果；request 必须仍指向执行搜索的同一不可变地图和配置。
+    /// 返回：父链无环、索引连续且每条原语按同一解析规则可重建时为 true；失败时 path 为 null 并返回可读原因。
+    /// </summary>
+    public bool TryBacktrack(HybridAStarSearchResult searchResult, PlanningRequest request,
+        out BacktrackedPath path, out string failureReason)
+    {
+        path = null;
+        failureReason = string.Empty;
+        if (searchResult == null || request == null || searchResult.Status != PlanningStatus.Success ||
+            !searchResult.SuccessNodeIndex.HasValue || searchResult.Nodes == null)
+        {
+            failureReason = "搜索结果不含可回溯的成功节点。";
+            return false;
+        }
+
+        int currentIndex = searchResult.SuccessNodeIndex.Value;
+        var reverseNodes = new List<HybridAStarNode>();
+        var visited = new HashSet<int>();
+        while (currentIndex >= 0)
+        {
+            if (currentIndex >= searchResult.Nodes.Count || !visited.Add(currentIndex))
+            {
+                failureReason = "搜索父链索引越界或存在环。";
+                return false;
+            }
+
+            HybridAStarNode current = searchResult.Nodes[currentIndex];
+            if (current == null || current.NodeIndex != currentIndex || current.Pose == null)
+            {
+                failureReason = "搜索节点索引或连续位姿不一致。";
+                return false;
+            }
+
+            reverseNodes.Add(current);
+            currentIndex = current.ParentNodeIndex;
+        }
+
+        if (reverseNodes.Count == 0)
+        {
+            failureReason = "搜索父链为空。";
+            return false;
+        }
+
+        reverseNodes.Reverse();
+        HybridAStarNode root = reverseNodes[0];
+        if (root.ParentNodeIndex != -1 || root.IncomingPrimitive != null || !IsFinitePose(root.Pose))
+        {
+            failureReason = "搜索父链根节点无效。";
+            return false;
+        }
+
+        var primitives = new List<MotionPrimitive>(Math.Max(0, reverseNodes.Count - 1));
+        Pose2D previousPose = root.Pose;
+        for (int index = 1; index < reverseNodes.Count; index++)
+        {
+            HybridAStarNode node = reverseNodes[index];
+            MotionPrimitive descriptor = node.IncomingPrimitive;
+            if (node.ParentNodeIndex != reverseNodes[index - 1].NodeIndex || descriptor == null ||
+                !IsFinitePose(node.Pose) || !NumericGuard.IsFinite(descriptor.CurvaturePerMeter) ||
+                !IsTravelDirection(descriptor.Direction) || descriptor.ActualLengthMeters <= 0d)
+            {
+                failureReason = "搜索父链中的原语描述无效。";
+                return false;
+            }
+
+            // 只把恒曲率、方向和长度描述作为真源，再走一遍相同的解析积分与连续碰撞检查。
+            MotionPrimitive rebuilt = _primitiveGenerator.Generate(previousPose, descriptor.CurvaturePerMeter, descriptor.Direction, request);
+            if (rebuilt == null || !IsEquivalent(descriptor, rebuilt) || !IsSamePose(rebuilt.End, node.Pose))
+            {
+                failureReason = "搜索原语无法按当前解析规则确定性重建。";
+                return false;
+            }
+
+            primitives.Add(rebuilt);
+            previousPose = rebuilt.End;
+        }
+
+        path = new BacktrackedPath(root.Pose, root.Direction, root.CurvaturePerMeter,
+            new ReadOnlyCollection<MotionPrimitive>(primitives));
+        return true;
+    }
+
+    private static bool IsEquivalent(MotionPrimitive expected, MotionPrimitive actual)
+    {
+        return expected.Direction == actual.Direction && expected.IsGoalTruncation == actual.IsGoalTruncation &&
+            Math.Abs(expected.CurvaturePerMeter - actual.CurvaturePerMeter) <= PoseComparisonTolerance &&
+            Math.Abs(expected.ActualLengthMeters - actual.ActualLengthMeters) <= LengthComparisonTolerance;
+    }
+
+    private static bool IsSamePose(Pose2D first, Pose2D second)
+    {
+        return IsFinitePose(first) && IsFinitePose(second) &&
+            Math.Abs(first.X - second.X) <= PoseComparisonTolerance &&
+            Math.Abs(first.Y - second.Y) <= PoseComparisonTolerance &&
+            Math.Abs(AngleMath.ShortestSignedDifference(first.Heading, second.Heading)) <= PoseComparisonTolerance;
+    }
+
+    private static bool IsFinitePose(Pose2D pose)
+    {
+        return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
+    }
+
+    private static bool IsTravelDirection(TravelDirection direction)
+    {
+        return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/README.md

@@ -0,0 +1,387 @@
+# CoarsePath 粗路径规划（P0/P1）
+
+`CoarsePath` 在 `Map` 提供的不可变 `PlanningGridMap` 上执行 Hybrid A*，输出已经过连续碰撞、终点和输出不变量复核的粗路径。它只处理“能否安全地从当前车辆几何中心到达目标”的几何搜索；不读取传感器或定位，不绘制 UI，也不向底盘发送任何命令。
+
+地图障碍物来源、世界坐标栅格化、距离场和缓存细节由 [Map/README.md](../Map/README.md) 说明。本模块唯一建议的业务调用入口是：
+
+```csharp
+CoarsePathPlanningService.Plan(job, cancellationToken)
+```
+
+## 模块说明（Module Overview）
+
+| 模块 | 负责内容 | 不负责内容 |
+| --- | --- | --- |
+| `Map` | 外部障碍物快照、占据栅格、保守障碍距离、地图缓存 | 车辆足迹、运动原语、路径搜索、控制 |
+| `CoarsePath` | 车辆扩大足迹、连续碰撞检查、前进/倒车原语、Hybrid A*、路径复核与方向分段 | 传感器读取、定位读取、速度规划、路径跟踪、底盘命令 |
+| `Facade` | 将建图、搜索、取消、总预算和可选调试旁路编排为一次调用 | 修改地图内容、写入 AMR 占据、执行轨迹 |
+| `Test` | 固定回归场景、P1 手动测试入口与规划结果可视化 | 真实作业地图、实时重规划或车辆控制 |
+
+安全余量只由 `VehicleParameters.SafetyMarginMeters` 扩大车辆矩形。它不会回写到地图障碍物，因此同一个 `PlanningGridMap` 可由不同车辆参数重复使用。
+
+## 文件结构（File Structure）
+
+```text
+CoarsePath/
+├── README.md                                      # 本模块说明：结构、数据流、调用与测试
+├── HybridAStarPlanner.cs                          # 公开规划门面后的核心编排：校验、搜索、回溯和复核
+├── Contracts/
+│   ├── Pose2D.cs                                  # 车辆几何中心位姿：m / rad
+│   ├── PlanningRequest.cs                         # 已有 PlanningGridMap 上的一次内部搜索请求
+│   ├── PlanningResult.cs                          # 不可变规划结果：成功路径或空结果
+│   ├── PlanningStatus.cs                          # 成功、取消、无解、输入和资源限制状态
+│   ├── CoarsePathPoint.cs                         # 稠密路径点、方向、曲率、净空和换向标记
+│   ├── PathSegment.cs                             # 前进或倒车的包含式路径索引段
+│   ├── VehicleParameters.cs                       # 车体尺寸、安全余量和曲率限制
+│   ├── HybridAStarConfiguration.cs                # 原语、离散、代价、容差和资源上限
+│   └── GoalDirectionConstraint.cs                 # 目标进入方向约束
+├── Vehicle/
+│   ├── VehicleKinematics.cs                       # 恒曲率车辆运动学积分
+│   ├── VehicleFootprint.cs                        # 扩大后的车辆矩形几何
+│   ├── OrientedRectangleCellIntersection.cs       # 旋转矩形与占据格相交判定
+│   └── FootprintCollisionChecker.cs               # 连续扫掠的足迹碰撞检查
+├── Search/
+│   ├── BinaryMinHeap.cs                           # 可更新优先级的 Open List
+│   ├── GridDijkstraHeuristic.cs                   # 二维栅格可达性和距离启发式
+│   ├── GoalToleranceChecker.cs                    # 目标位置、航向和方向约束判定
+│   ├── MotionPrimitive.cs                         # 单个前进或倒车恒曲率原语
+│   ├── MotionPrimitiveGenerator.cs                # 原语离散与连续积分点生成
+│   ├── SearchCostCalculator.cs                    # 长度、倒车、换向、曲率和净空代价
+│   ├── HybridAStarNode.cs                         # 搜索节点与父链信息
+│   ├── HybridAStarNodeKey.cs                      # 离散状态键
+│   └── HybridAStarSearch.cs                       # Hybrid A* 主搜索循环
+├── Output/
+│   ├── PathBacktracker.cs                         # 从终点节点安全回溯父链
+│   ├── CoarsePathAssembler.cs                     # 组装稠密路径和方向段
+│   └── CoarsePathValidator.cs                     # 对最终输出重新进行连续复核
+├── Facade/
+│   ├── CoarsePathPlanningJob.cs                   # 一次完整业务输入：地图请求、位姿、车辆、配置
+│   ├── CoarsePathPlanningJobResult.cs             # 同时包含地图结果和规划结果的不可变输出
+│   ├── CoarsePathPlanningService.cs               # 唯一业务调用门面
+│   ├── PlanningDebugOptions.cs                    # 可选调试旁路配置
+│   └── IPlanningDebugSink.cs                      # 调试旁路接收器契约
+└── Test/
+    ├── CoarsePathScenarioFactory.cs               # 六个固定场景和手动目标演示请求工厂
+    └── MovementTest.CoarsePathTest.cs              # 七个 Clumsy 入口、后台取消和 Painter 绘制
+```
+
+## 规划数据流（Planning Data Flow）
+
+```text
+CoarsePathPlanningJob
+        │  MapRequest 使用 mm；Pose2D/车辆使用 m、rad
+        ▼
+CoarsePathPlanningService.Plan(job, cancellationToken)
+        │
+        ├── PlanningMapFactory.Create(job.MapRequest)
+        │       │
+        │       ├── 失败、取消或超时
+        │       │       └── MapResult + 空路径 PlanningResult，搜索不启动
+        │       │
+        │       └── 成功：不可变 PlanningGridMap
+        ▼
+PlanningRequest
+        │
+        ▼
+HybridAStarPlanner
+        ├── 车辆扩大足迹与连续碰撞检查
+        ├── GridDijkstraHeuristic + Hybrid A* 搜索
+        ├── PathBacktracker + CoarsePathAssembler
+        └── CoarsePathValidator 最终复核
+        ▼
+PlanningResult + MapResult
+        ▼
+CoarsePathPlanningJobResult
+```
+
+调用方只创建 `CoarsePathPlanningJob` 并消费 `CoarsePathPlanningJobResult`。`PlanningRequest`、`HybridAStarPlanner`、原语和碰撞检查器属于模块内部协作对象，不应由 UI、传感器或 MovementTest 直接拼接。
+
+## 构建状态与停止（Build Status and Stop）
+
+必须一起处理 `MapResult` 和 `PlanningResult`。`MapResult.Status` 的类型是 `PlanningMapBuildStatus`；地图失败时，门面返回对应的空路径结果，并且不会启动 Hybrid A*。
+
+| 情况 | `MapResult` | `PlanningResult` | 调用方处理 |
+| --- | --- | --- | --- |
+| 地图和搜索成功 | `Success` 且 `Map` 非空 | `Success`，发布完整路径和方向段 | 消费粗路径；后续模块仍需自行进行平滑、速度和控制 |
+| 地图输入/来源失败 | `Failed` | `InvalidMap` | 读取 `FailureReason`，修复地图输入 |
+| 调用被取消 | `Cancelled` | `Cancelled` | 不重试为普通无解；不会发布地图或部分路径 |
+| 总预算耗尽 | `TimedOut` | `SearchTimeout` | 根据上层策略调整预算或稍后重试 |
+| 搜索无解 | 地图成功 | `NoFeasiblePath` | 当前地图、车体和运动约束下无可行路径 |
+| 节点或搜索资源受限 | 地图成功 | `SearchNodeLimitExceeded` 或 `SearchTimeout` | 读取诊断后调整配置或上层策略 |
+
+除 `PlanningStatus.Success` 外，`PlanningResult.Path` 与 `PlanningResult.Segments` 始终为空。取消、超时、无解、输入错误和最终复核失败都不能作为“部分可执行路径”使用。
+
+### 总预算与取消
+
+`HybridAStarConfiguration.SearchTimeout` 是从门面开始计时的一次总预算，依次覆盖建图、距离场、二维启发式和 Hybrid A*。同一个 `CancellationToken` 会沿调用链传递，取消优先于超时。
+
+### 总耗时与路径搜索耗时
+
+`PlanningDiagnostics.Elapsed` 是从 `CoarsePathPlanningService.Plan` 开始的总耗时，包含地图来源、缓存、栅格化、距离场和路径规划。`PathSearchElapsed`（路径搜索耗时）从地图和起终点预检通过后开始，包含二维启发式、Hybrid A*、回溯、装配、方向分段和最终复核；搜索开始前失败时为零。
+
+## 坐标与单位（Coordinates and Units）
+
+| 数据 | 单位 | 说明 |
+| --- | --- | --- |
+| `PlanningMapRequest.Bounds`、分辨率、障碍物几何 | mm | 来自 Map 的世界坐标；边界采用 `[min, max)` |
+| `Pose2D.X`、`Pose2D.Y`、路径位置、车辆尺寸、安全余量、弧长 | m | CoarsePath 的连续世界坐标和长度 |
+| `Pose2D.Heading`、航向容差 | rad | 核心一律使用弧度 |
+| 曲率、起步曲率 | 1/m | 最大曲率或最小转弯半径至少提供一个 |
+| `PlanningGridMap` 世界查询参数 | m | 越界位置按占据处理，净距为 0 |
+| P1 的 AMR/手动目标 X/Y | mm | 仅在 UI 边界读取，进入核心前除以 1000 |
+| P1 的 AMR/手动目标航向 | deg | 仅在 UI 边界转换为 `deg * PI / 180 -> rad` |
+
+起点和终点都表示车辆**几何中心**。若上游定位参考点是雷达、天线或其他安装点，必须先在上游应用安装外参；不要在 CoarsePath 内猜测偏移。车辆外扩由 `VehicleParameters.SafetyMarginMeters` 表达，不要把余量写入 Map 障碍物。
+
+## 最小调用示例（Minimal Call Example）
+
+以下示例明确允许空图，因而只适合算法或单位演示。真实作业必须通过 `IMapObstacleSource` 提供有效障碍物快照；如何构造来源请阅读 [Map/README.md](../Map/README.md)。
+
+```csharp
+using System;
+using System.Threading;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+
+var service = new CoarsePathPlanningService(); // 长期持有，保留地图缓存
+
+var job = new CoarsePathPlanningJob
+{
+    MapRequest = new PlanningMapRequest
+    {
+        Bounds = new MapBoundsMm(0f, 6000f, 0f, 4000f),
+        ResolutionMm = 50f,
+        ObstacleSources = Array.Empty<IMapObstacleSource>(),
+        AllowExplicitEmptyMap = true, // 仅演示时明确允许
+    },
+    Start = new Pose2D(1d, 1d, 0d),
+    Goal = new Pose2D(3d, 1d, 0d),
+    Vehicle = new VehicleParameters
+    {
+        LengthMeters = 0.80d,
+        WidthMeters = 0.60d,
+        SafetyMarginMeters = 0.05d,
+        MaximumCurvaturePerMeter = 1d / 1.20d,
+    },
+    Configuration = new HybridAStarConfiguration(),
+    GoalDirection = GoalDirectionConstraint.Forward,
+};
+
+CoarsePathPlanningJobResult result =
+    service.Plan(job, CancellationToken.None);
+
+if (!result.MapResult.Succeeded)
+    throw new InvalidOperationException(result.MapResult.FailureReason);
+
+if (result.PlanningResult.Status != PlanningStatus.Success)
+    throw new InvalidOperationException(
+        result.PlanningResult.Diagnostics.TerminationReason);
+
+foreach (CoarsePathPoint point in result.PlanningResult.Path)
+    Console.WriteLine(point.X + "," + point.Y + "," + point.Heading);
+```
+
+## 缓存与 SourceVersion（Cache and SourceVersion）
+
+`CoarsePathPlanningService` 在生命周期内长期持有 `PlanningMapFactory`，因此重复调用时能够复用地图缓存。不要每次规划都新建服务，否则会失去缓存收益。
+
+| 缓存层级 | 条件 | 结果 |
+| --- | --- | --- |
+| `Input` | 边界、分辨率、空图策略、来源 ID、`SourceVersion`、必需性和来源结果相同 | 返回同一个不可变 `PlanningGridMap` |
+| `Occupancy` | 输入版本变化，但最终占据栅格相同 | 复用占据/距离数组，生成新的快照元数据 |
+| `None` | 占据内容变化 | 重建规划快照和距离场 |
+
+障碍来源内容改变时，调用方必须递增该来源的 `SourceVersion`。仅修改起终点、车辆、搜索配置、调试开关或调试接收器不会改变地图输入；改变障碍物却不递增版本则可能错误复用旧快照。
+
+## 详细使用指南（Detailed Usage Guide）
+
+本节说明调用方如何从一个地图输入得到可消费的粗路径。所有业务调用都通过 `CoarsePathPlanningService.Plan(job, cancellationToken)` 完成。
+
+### 第 1 步：长期持有服务
+
+服务持有地图工厂和规划器，应该作为规划业务、任务执行器或上层服务的长期字段，而不是在每次调用中创建：
+
+```csharp
+private readonly CoarsePathPlanningService _coarsePathService =
+    new CoarsePathPlanningService();
+```
+
+### 第 2 步：准备地图请求
+
+创建 `PlanningMapRequest`，其边界、分辨率和障碍物仍使用 mm。使用手工圆形/矩形或 TwoLeg 快照时，应先按 [Map/README.md](../Map/README.md) 将它们包装为 `IMapObstacleSource`，并为内容变化递增 `SourceVersion`。
+
+```csharp
+var mapRequest = new PlanningMapRequest
+{
+    Bounds = new MapBoundsMm(0f, 6000f, 0f, 4000f),
+    ResolutionMm = 50f,
+    ObstacleSources = sources,
+    AllowExplicitEmptyMap = false,
+};
+```
+
+`AllowExplicitEmptyMap = true` 只在调用方明确确认空地图安全时使用。未提供有效障碍物且未显式允许空图时，地图不会进入规划。
+
+### 第 3 步：填写起点、终点和方向约束
+
+将车辆几何中心的世界 X/Y 从 mm 转为 m，并将航向转换为 rad 后创建 `Pose2D`。`StartDirection = null` 表示允许从前进或倒车开始；`GoalDirection` 可以限制最终进入目标的方向。
+
+```csharp
+var start = new Pose2D(startXmm / 1000d, startYmm / 1000d,
+    startHeadingDeg * Math.PI / 180d);
+var goal = new Pose2D(goalXmm / 1000d, goalYmm / 1000d,
+    goalHeadingDeg * Math.PI / 180d);
+```
+
+### 第 4 步：填写车辆参数
+
+车辆尺寸和安全余量全部为 m。曲率限制可填写 `MaximumCurvaturePerMeter`，或填写 `MinimumTurningRadiusMeters`；至少必须提供一个有效限制。
+
+```csharp
+var vehicle = new VehicleParameters
+{
+    LengthMeters = 0.80d,
+    WidthMeters = 0.60d,
+    SafetyMarginMeters = 0.05d,
+    MaximumCurvaturePerMeter = 1d / 1.20d,
+};
+```
+
+### 第 5 步：调整搜索配置
+
+默认 `HybridAStarConfiguration` 包含原语长度、积分步长、航向离散、终点容差、代价、节点上限和总超时。若业务需要覆盖默认值，应同时理解安全影响：`MaximumCollisionCheckStepMeters` 不能以牺牲连续碰撞检查精度为代价随意增大。
+
+```csharp
+var configuration = new HybridAStarConfiguration
+{
+    SearchTimeout = TimeSpan.FromSeconds(5d),
+    MaximumExpandedNodes = 200000,
+};
+```
+
+### 第 6 步：调用并消费成功结果
+
+只有 `Success` 可以发布完整路径。`Path` 是稠密点序列；`Segments` 是覆盖整条路径的前进/倒车包含式索引段，可供后续的速度规划或显示模块消费。
+
+```csharp
+var job = new CoarsePathPlanningJob
+{
+    MapRequest = mapRequest,
+    Start = start,
+    Goal = goal,
+    Vehicle = vehicle,
+    Configuration = configuration,
+    GoalDirection = GoalDirectionConstraint.Any,
+};
+
+CoarsePathPlanningJobResult result = _coarsePathService.Plan(job, cancellationToken);
+if (!result.MapResult.Succeeded)
+    ReportMapFailure(result.MapResult.FailureReason);
+else if (result.PlanningResult.Status == PlanningStatus.Success)
+    ConsumeCoarsePath(result.PlanningResult.Path, result.PlanningResult.Segments);
+else
+    ReportPlanningFailure(result.PlanningResult.Diagnostics.TerminationReason);
+```
+
+`CoarsePathPoint.IsGearSwitchPoint` 为 `true` 表示该点是新方向段开始处。换向位置会保留一对位置、航向与弧长相同、方向不同的相邻点；`UnwrappedHeading` 用于跨越 `-pi/pi` 时保持显示连续。
+
+## P1 手动测试与可视化（P1 Manual Tests and Visualization）
+
+P1 在 `Test/MovementTest.CoarsePathTest.cs` 提供只读测试入口。它们共用一个长期存活的 `CoarsePathPlanningService`，只提交规划并绘制结果；不发送底盘、速度或转向命令。
+
+| MovementTest 名称 | 场景 | 预期 |
+| --- | --- | --- |
+| `粗路径规划-显式空图` | 显式允许的空图 | 前进直达成功 |
+| `粗路径规划-单矩形绕行` | 中央矩形阻断直线 | 成功绕障 |
+| `粗路径规划-多来源障碍` | 手工圆形、矩形和 TwoLeg 快照 | 证明多来源经过同一门面 |
+| `粗路径规划-缓存命中` | 重复相同地图输入 | 后续调用显示 `Input` 缓存命中 |
+| `粗路径规划-倒车换向` | 前进起步、倒车到达 | 成功路径含 `IsGearSwitchPoint` |
+| `粗路径规划-无解` | 贯穿地图的障碍带 | 返回 `NoFeasiblePath` 且不发布路径 |
+| `粗路径规划` | 当前 AMR 位姿、人工终点和可选人工障碍物 | 验证手动障碍物、边界、路径与可视化 |
+
+### 固定案例的实时 AMR 锚定
+
+`粗路径规划-显式空图`、`粗路径规划-单矩形绕行`、`粗路径规划-多来源障碍`、`粗路径规划-缓存命中`、`粗路径规划-倒车换向` 和 `粗路径规划-无解` 是六个固定案例。它们不再以写死的世界起点运行：共享运行器在前台仅调用一次 `DetourInterface.getCartLocation()`，校验并冻结本次 AMR 的世界 `X(mm)`、`Y(mm)` 与航向 `deg`，再创建本次规划请求；后台规划期间不会再次读取定位。
+
+`Create(CoarsePathTestScenario scenario, double amrXMillimeters, double amrYMillimeters, double amrHeadingDegrees)`
+
+该入口把基准案例的起点映射为冻结的 AMR 位姿，并以相同的 `ΔX/ΔY` 平移地图边界、目标、圆形/矩形障碍，以及 TwoLeg 的检测原点。因此，固定案例始终在当前 AMR 附近保留原有的相对几何关系。起点航向严格使用冻结的 AMR 航向；终点航向保持基准案例的“终点航向减起点航向”差值，叠加到当前 AMR 航向后规范化到 `[-pi, pi]`。TwoLeg 只平移检测原点，`DetectionHeadingRadians` 不会因 AMR 航向发生旋转。
+
+`粗路径规划-缓存命中` 只有两次运行冻结到相同的 AMR `X/Y`、从而形成相同的平移后地图输入时，才作为缓存命中场景；AMR 位置移动后，地图输入正常未命中并重建快照。仅 AMR 航向变化不会改变固定案例的地图输入，地图缓存仍可命中。若定位读取为空、抛出异常，或 `X`、`Y`、航向含有 `NaN`/无穷值，运行器不会提交后台规划，状态与 Toast 会显示以“AMR 位姿不可用”开头的诊断原因。
+
+手动 `粗路径规划` 的人工终点、障碍物和超时输入流程保持不变；它不套用固定案例的整体平移规则。
+
+### AMR 位姿、手动终点与障碍物
+
+`CoarsePathPlanningTest` 启动时读取一次 `DetourInterface.getCartLocation()`，将当前 AMR 世界位姿冻结为起点；随后依次输入目标世界 `X(mm)`、`Y(mm)`、航向 `deg`，以及障碍物数量 `0-20`。每个障碍物再依次输入类型和几何参数：
+
+| 类型输入 | 形状 | 输入参数（全部为 mm） |
+| --- | --- | --- |
+| `1` | 圆形 | 几何中心 `X`、`Y` 与半径；半径必须大于 0 |
+| `2` | 轴对齐矩形 | 几何中心 `X`、`Y`、X 向长度、Y 向宽度；两个尺寸必须大于 0 |
+
+矩形只支持 `AxisAlignedRectangle`，不提供旋转角；其中心和长宽由 `ManualCoarsePathObstacle.AxisAlignedRectangle` 表达。圆形由 `ManualCoarsePathObstacle.Circle` 表达。所有无穷、NaN、非数字或不合法尺寸都会在输入阶段拒绝。
+
+`CoarsePathScenarioFactory.CreateManualObstacleDemo` 在唯一边界完成转换：
+
+```text
+AMR/目标 X、Y：mm / 1000 -> m
+AMR/目标航向：deg * PI / 180 -> rad
+```
+
+当数量为 0 时，入口使用显式空图；`CreateManualGoalDemo` 保留为同一零障碍物场景的兼容帮助方法。数量大于 0 时，工厂将 `ManualCoarsePathObstacle` 快照封装成来源 ID 为 `manual-user-input` 的地图输入，并为每次手动快照分配新的 `SourceVersion`，避免错误复用地图缓存。规划边界覆盖起点、终点和每个障碍物的完整轮廓，再增加 8000 mm 余量并按 50 mm 对齐。
+
+手动入口还要求输入一次“粗路径规划总超时”，单位为秒，只接受 `TimeSpan` 可表示范围内的有限正数秒；`0`、负数、NaN、Infinity 或溢出值都会在启动规划前拒绝。该值只覆盖本次 `CoarsePathPlanningJob.Configuration.SearchTimeout`，不会改变固定场景或全局默认值。
+
+此入口使用固定演示车辆：长 `0.80 m`、宽 `0.60 m`、四周安全余量 `0.05 m`、最小转弯半径 `1.20 m`。这些值不是从现场 AMR 配置读取的，判断现场可行性前必须确认车辆参数一致。
+
+`getCartLocation` 在无有效定位时可能阻塞，因此应在定位准备完成的测试环境使用。手动障碍物是测试输入，不能替代现场障碍物来源；零障碍物的显式空图也绝不代表现场不存在障碍物。
+
+### 后台执行、停止与图层
+
+每次测试启动时会创建独立的 `CancellationTokenSource`，以 `Task.Run` 调用门面，并先取消旧会话。`Test()` 不等待任务，也不读取 `Task.Result`；`TestStop` 取消当前令牌、使会话失效并清空图层。已取消任务完成后不会覆盖新会话，也不会显示部分路径。
+
+专用世界坐标 Painter 图层为 `CoarsePathPlanningV1`。它直接读取本次 `PlanningGridMap` 的 `Bounds`、`ResolutionMm`、`SnapshotId` 和 `IsOccupied(row, col)`，因此边界、抽稀网格和占据格与实际规划快照一致，而不是重新绘制原始障碍物。
+
+状态图层显示规划状态、总耗时、`PathSearchElapsed`（路径搜索耗时）、扩展/生成节点数、Open List 峰值、失败原因和固定演示车辆参数。Toast 同时显示两种耗时，并在失败时附加 `TerminationReason`，因此超时、节点上限、无解、碰撞和内部错误不会只显示成泛化失败。
+
+| 颜色 | 可视化元素 |
+| --- | --- |
+| 灰白 | 地图边界与栅格网络 |
+| 暗红 | 占据格 |
+| 绿色 | 起点、前进路径和方向箭头 |
+| 橙色 | 终点、航向和位置容差圈 |
+| 天蓝 | 倒车路径和方向箭头 |
+| 紫色 | 换向点 |
+| 金色 | 已纳入安全余量的车辆检查框 |
+
+自动化已检查 UI 入口的后台、取消和数据来源结构。仍需在实际 Clumsy 界面手动运行“粗路径规划-单矩形绕行”和“粗路径规划”，确认图层交互显示与停止按钮效果。
+
+## 常见错误（Common Errors）
+
+| 现象 | 原因 | 处理 |
+| --- | --- | --- |
+| 起点、终点或障碍物位置相差 1000 倍 | 将 mm 直接传给 `Pose2D` 或把 m 传给地图输入 | Map 输入使用 mm；`Pose2D`、车辆和路径使用 m |
+| 路径朝向错误或旋转异常 | 将 P1 的 deg 直接当作核心 rad | 在 UI/上层边界执行 `deg * PI / 180`，核心只保存 rad |
+| 障碍物已经变化却复用旧地图 | 内容变更后没有递增 `SourceVersion` | 每次来源快照内容变化后增加对应版本号 |
+| 地图创建成功但规划被阻止 | 没有有效障碍物且未显式允许空图 | 提供有效来源；仅在确认安全的演示中设置 `AllowExplicitEmptyMap = true` |
+| 无解、取消或超时后仍尝试使用路径 | 没有检查 `PlanningStatus.Success` | 仅成功时消费 `Path` 和 `Segments`；其他状态读取诊断 |
+| 将粗路径直接下发给车辆 | 粗路径不包含速度、时间、执行控制或实时安全闭环 | 在后续阶段增加平滑、时间参数化、跟踪和独立安全控制 |
+| P1 手动终点表现为空场地安全 | 手动入口使用显式空图演示 | 真实作业必须提供真实障碍物快照，不能复用空图语义 |
+
+## 第一版限制（First-Version Limits）
+
+当前 P0/P1 已提供安全、确定性的粗路径核心和手动结果可视化，但不包含：
+
+- 路径平滑或曲率连续优化；
+- Reeds-Shepp 或 Dubins 精确终点连接；
+- 速度、加速度、时间标注、时间轨迹和路径跟踪控制；
+- 底盘命令、避障闭环、现场传感器采集或实时重规划调度；
+- 横移、蟹行或其他非前进/倒车运动原语；
+- 原地旋转；
+- 真实作业地图接入、交互式场景编辑和 Release 性能/资源/确定性基准。
+
+当前只生成汽车式恒曲率前进/倒车原语，并允许在原语边界换向；未实现的 Reeds-Shepp、横移、蟹行和原地旋转是整个粗规划核心的第一版能力边界，不是 MovementTest 单独关闭。
+
+因此，调用方只能把 `Success` 结果视作后续模块的粗路径输入，不能把它当作可直接下发的时间轨迹。

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/BinaryMinHeap.cs

@@ -0,0 +1,146 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
+
+/// <summary>
+/// 为 Hybrid A* Open List 提供确定性优先级的二叉最小堆。
+/// 排序严格依次比较 F、H、较大的 G 和插入序号；F、H、G 必须为有限且非负的等效米代价。
+/// </summary>
+/// <typeparam name="T">与一组搜索代价关联的节点或条目类型。</typeparam>
+public sealed class BinaryMinHeap<T>
+{
+    private readonly List<HeapEntry> _entries = new List<HeapEntry>();
+    private long _nextInsertionSequence;
+
+    /// <summary>创建空的确定性 Open List 堆。</summary>
+    public BinaryMinHeap()
+    {
+    }
+
+    /// <summary>当前堆内尚未出队的条目数量。</summary>
+    public int Count { get { return _entries.Count; } }
+
+    /// <summary>
+    /// 将一个条目和其搜索排序代价压入堆。
+    /// 参数：item 为关联条目；f、h、g 均为有限且非负的等效米代价。
+    /// 失败：任一代价无效、item 为 null（仅引用类型）或插入序号耗尽时抛出异常。
+    /// </summary>
+    public void Push(T item, double f, double h, double g)
+    {
+        if (ReferenceEquals(item, null)) throw new ArgumentNullException(nameof(item));
+        ValidateCost(f, nameof(f));
+        ValidateCost(h, nameof(h));
+        ValidateCost(g, nameof(g));
+        if (_nextInsertionSequence == long.MaxValue)
+            throw new InvalidOperationException("The binary heap insertion sequence has been exhausted.");
+
+        var entry = new HeapEntry(item, f, h, g, _nextInsertionSequence);
+        _nextInsertionSequence++;
+        _entries.Add(entry);
+        SiftUp(_entries.Count - 1);
+    }
+
+    /// <summary>
+    /// 弹出当前排序最优的条目。
+    /// 返回：按 F、H、较大 G 和插入序号排序后的最小条目；空堆时抛出 <see cref="InvalidOperationException"/>。
+    /// </summary>
+    public T Pop()
+    {
+        if (_entries.Count == 0) throw new InvalidOperationException("The binary heap is empty.");
+
+        HeapEntry result = _entries[0];
+        int lastIndex = _entries.Count - 1;
+        if (lastIndex == 0)
+        {
+            _entries.RemoveAt(0);
+            return result.Item;
+        }
+
+        _entries[0] = _entries[lastIndex];
+        _entries.RemoveAt(lastIndex);
+        SiftDown(0);
+        return result.Item;
+    }
+
+    /// <summary>清空尚未出队的条目；后续插入序号继续单调递增以保持整个实例内的确定性。</summary>
+    public void Clear()
+    {
+        _entries.Clear();
+    }
+
+    private void SiftUp(int index)
+    {
+        while (index > 0)
+        {
+            int parentIndex = (index - 1) / 2;
+            if (Compare(_entries[index], _entries[parentIndex]) >= 0) return;
+            Swap(index, parentIndex);
+            index = parentIndex;
+        }
+    }
+
+    private void SiftDown(int index)
+    {
+        while (true)
+        {
+            int leftChildIndex = index * 2 + 1;
+            if (leftChildIndex >= _entries.Count) return;
+
+            int bestChildIndex = leftChildIndex;
+            int rightChildIndex = leftChildIndex + 1;
+            if (rightChildIndex < _entries.Count && Compare(_entries[rightChildIndex], _entries[leftChildIndex]) < 0)
+                bestChildIndex = rightChildIndex;
+
+            if (Compare(_entries[bestChildIndex], _entries[index]) >= 0) return;
+            Swap(index, bestChildIndex);
+            index = bestChildIndex;
+        }
+    }
+
+    private void Swap(int firstIndex, int secondIndex)
+    {
+        HeapEntry temporary = _entries[firstIndex];
+        _entries[firstIndex] = _entries[secondIndex];
+        _entries[secondIndex] = temporary;
+    }
+
+    private static int Compare(HeapEntry left, HeapEntry right)
+    {
+        int comparison = left.F.CompareTo(right.F);
+        if (comparison != 0) return comparison;
+
+        comparison = left.H.CompareTo(right.H);
+        if (comparison != 0) return comparison;
+
+        comparison = right.G.CompareTo(left.G);
+        if (comparison != 0) return comparison;
+
+        return left.InsertionSequence.CompareTo(right.InsertionSequence);
+    }
+
+    private static void ValidateCost(double value, string parameterName)
+    {
+        if (!NumericGuard.IsFinite(value) || value < 0d)
+            throw new ArgumentOutOfRangeException(parameterName, "Search costs must be finite and non-negative.");
+    }
+
+    private sealed class HeapEntry
+    {
+        public HeapEntry(T item, double f, double h, double g, long insertionSequence)
+        {
+            Item = item;
+            F = f;
+            H = h;
+            G = g;
+            InsertionSequence = insertionSequence;
+        }
+
+        public T Item { get; }
+        public double F { get; }
+        public double H { get; }
+        public double G { get; }
+        public long InsertionSequence { get; }
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/GoalToleranceChecker.cs

@@ -0,0 +1,61 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
+
+/// <summary>按位置、航向和进入方向约束判定连续位姿是否可以作为目标候选。</summary>
+public static class GoalToleranceChecker
+{
+    /// <summary>
+    /// 判断一个位姿是否满足目标容差和进入方向约束。
+    /// 参数：pose 与 goal 使用世界 m/rad；configuration 提供位置 m 和航向 rad 容差；direction 为候选末段方向；goalDirection 为目标进入方向约束。
+    /// 返回：输入有限、位置距离和最小环形航向误差均不超过容差且方向匹配时为 true；无效输入保守地返回 false。
+    /// </summary>
+    public static bool IsSatisfied(
+        Pose2D pose,
+        Pose2D goal,
+        HybridAStarConfiguration configuration,
+        TravelDirection direction,
+        GoalDirectionConstraint goalDirection)
+    {
+        if (!IsFinitePose(pose) || !IsFinitePose(goal) || configuration == null ||
+            !NumericGuard.IsFinite(configuration.GoalPositionToleranceMeters) ||
+            !NumericGuard.IsFinite(configuration.GoalHeadingToleranceRadians) ||
+            configuration.GoalPositionToleranceMeters < 0d || configuration.GoalHeadingToleranceRadians < 0d ||
+            !IsTravelDirection(direction) || !IsGoalDirection(goalDirection))
+            return false;
+
+        if (!MatchesDirection(direction, goalDirection)) return false;
+
+        double deltaX = pose.X - goal.X;
+        double deltaY = pose.Y - goal.Y;
+        double positionDistanceMeters = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
+        if (!NumericGuard.IsFinite(positionDistanceMeters) || positionDistanceMeters > configuration.GoalPositionToleranceMeters)
+            return false;
+
+        double headingDifferenceRadians = Math.Abs(AngleMath.ShortestSignedDifference(pose.Heading, goal.Heading));
+        return NumericGuard.IsFinite(headingDifferenceRadians) && headingDifferenceRadians <= configuration.GoalHeadingToleranceRadians;
+    }
+
+    private static bool IsFinitePose(Pose2D pose)
+    {
+        return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
+    }
+
+    private static bool IsTravelDirection(TravelDirection direction)
+    {
+        return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
+    }
+
+    private static bool IsGoalDirection(GoalDirectionConstraint direction)
+    {
+        return direction == GoalDirectionConstraint.Any || direction == GoalDirectionConstraint.Forward || direction == GoalDirectionConstraint.Reverse;
+    }
+
+    private static bool MatchesDirection(TravelDirection direction, GoalDirectionConstraint constraint)
+    {
+        return constraint == GoalDirectionConstraint.Any ||
+            (constraint == GoalDirectionConstraint.Forward && direction == TravelDirection.Forward) ||
+            (constraint == GoalDirectionConstraint.Reverse && direction == TravelDirection.Reverse);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/GridDijkstraHeuristic.cs

@@ -0,0 +1,123 @@
+using System;
+using System.Threading;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
+
+/// <summary>
+/// 基于不可变栅格地图的目标反向八邻域 Dijkstra 启发式。
+/// 距离单位为 m；对角移动仅在两个对应正交邻格都未占据时允许，以避免从障碍夹角穿越。
+/// </summary>
+public sealed class GridDijkstraHeuristic
+{
+    private readonly PlanningGridMap _map;
+    private readonly double[] _costs;
+
+    /// <summary>
+    /// 从目标栅格预计算所有可达自由格到目标的二维最短距离。
+    /// 参数：map 必须是已就绪的不可变地图；goalRow、goalCol 为地图内且未占据的目标格索引。
+    /// 失败：地图为空、未就绪或目标格无效时抛出异常。
+    /// </summary>
+    public GridDijkstraHeuristic(PlanningGridMap map, int goalRow, int goalCol)
+    {
+        if (!TryCreate(map, goalRow, goalCol, PlanningOperationBudget.Unlimited(CancellationToken.None),
+            out GridDijkstraHeuristic heuristic, out _))
+            throw new InvalidOperationException("Unbounded Dijkstra construction unexpectedly stopped.");
+        _map = heuristic._map;
+        _costs = heuristic._costs;
+    }
+
+    private GridDijkstraHeuristic(PlanningGridMap map, double[] costs)
+    {
+        _map = map;
+        _costs = costs;
+    }
+
+    /// <summary>使用共享预算创建完整二维启发式；停止时不返回部分成本数组。</summary>
+    internal static bool TryCreate(PlanningGridMap map, int goalRow, int goalCol, PlanningOperationBudget budget,
+        out GridDijkstraHeuristic heuristic, out PlanningOperationStopReason stopReason)
+    {
+        if (map == null) throw new ArgumentNullException(nameof(map));
+        if (!map.PlanningReady) throw new ArgumentException("The planning map must be ready.", nameof(map));
+        if (goalRow < 0 || goalRow >= map.Rows || goalCol < 0 || goalCol >= map.Cols)
+            throw new ArgumentOutOfRangeException(nameof(goalRow));
+        if (map.IsOccupied(goalRow, goalCol))
+            throw new ArgumentException("The goal grid cell must be free.", nameof(goalRow));
+        if (budget == null) throw new ArgumentNullException(nameof(budget));
+
+        heuristic = null;
+        stopReason = budget.GetStopReason();
+        if (stopReason != PlanningOperationStopReason.None) return false;
+        var costs = new double[checked(map.Rows * map.Cols)];
+        int workItemCount = 0;
+        for (int index = 0; index < costs.Length; index++)
+        {
+            stopReason = budget.CheckEvery(ref workItemCount);
+            if (stopReason != PlanningOperationStopReason.None) return false;
+            costs[index] = double.PositiveInfinity;
+        }
+        if (!TryBuild(map, costs, goalRow, goalCol, budget, ref workItemCount, out stopReason)) return false;
+        heuristic = new GridDijkstraHeuristic(map, costs);
+        stopReason = PlanningOperationStopReason.None;
+        return true;
+    }
+
+    /// <summary>
+    /// 查询指定栅格到构造时目标格的二维最短距离。
+    /// 参数：row、col 为从零开始的栅格索引。
+    /// 返回：单位 m 的有限最短距离；自由格不可达或索引越界时返回正无穷。
+    /// </summary>
+    public double GetCost(int row, int col)
+    {
+        return row < 0 || row >= _map.Rows || col < 0 || col >= _map.Cols
+            ? double.PositiveInfinity
+            : _costs[row * _map.Cols + col];
+    }
+
+    private static bool TryBuild(PlanningGridMap map, double[] costs, int goalRow, int goalCol,
+        PlanningOperationBudget budget, ref int workItemCount, out PlanningOperationStopReason stopReason)
+    {
+        var openList = new BinaryMinHeap<int>();
+        int goalIndex = goalRow * map.Cols + goalCol;
+        costs[goalIndex] = 0d;
+        openList.Push(goalIndex, 0d, 0d, 0d);
+
+        while (openList.Count > 0)
+        {
+            stopReason = budget.CheckEvery(ref workItemCount);
+            if (stopReason != PlanningOperationStopReason.None) return false;
+            int currentIndex = openList.Pop();
+            int currentRow = currentIndex / map.Cols;
+            int currentCol = currentIndex % map.Cols;
+            double currentCost = costs[currentIndex];
+
+            for (int rowOffset = -1; rowOffset <= 1; rowOffset++)
+            for (int colOffset = -1; colOffset <= 1; colOffset++)
+            {
+                stopReason = budget.CheckEvery(ref workItemCount);
+                if (stopReason != PlanningOperationStopReason.None) return false;
+                if (rowOffset == 0 && colOffset == 0) continue;
+
+                int nextRow = currentRow + rowOffset;
+                int nextCol = currentCol + colOffset;
+                if (nextRow < 0 || nextRow >= map.Rows || nextCol < 0 || nextCol >= map.Cols || map.IsOccupied(nextRow, nextCol))
+                    continue;
+
+                bool isDiagonal = rowOffset != 0 && colOffset != 0;
+                if (isDiagonal && (map.IsOccupied(currentRow + rowOffset, currentCol) || map.IsOccupied(currentRow, currentCol + colOffset)))
+                    continue;
+
+                double stepCost = isDiagonal ? Math.Sqrt(2d) * map.ResolutionMeters : map.ResolutionMeters;
+                double candidateCost = currentCost + stepCost;
+                int nextIndex = nextRow * map.Cols + nextCol;
+                if (candidateCost >= costs[nextIndex]) continue;
+
+                costs[nextIndex] = candidateCost;
+                openList.Push(nextIndex, candidateCost, 0d, 0d);
+            }
+        }
+        stopReason = PlanningOperationStopReason.None;
+        return true;
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/HybridAStarNode.cs

@@ -0,0 +1,99 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
+
+/// <summary>
+/// Hybrid A* 运行期节点。
+/// 节点保留连续位姿和其离散闭集键；搜索过程中不会修改已创建节点，改进代价时会追加新节点并使旧 Open List 条目失效。
+/// </summary>
+public sealed class HybridAStarNode
+{
+    /// <summary>
+    /// 创建一个搜索节点。
+    /// 参数：nodeIndex 为本次搜索内稳定索引；key 为离散状态；pose 为连续车辆中心位姿；parentNodeIndex 为父节点索引，根节点使用 -1；
+    /// incomingPrimitive 为父节点到本节点的原语，根节点为 null；curvaturePerMeter、gCostMeters 和 hCostMeters 均使用规划器的标准单位。
+    /// </summary>
+    public HybridAStarNode(
+        int nodeIndex,
+        HybridAStarNodeKey key,
+        Pose2D pose,
+        int parentNodeIndex,
+        MotionPrimitive incomingPrimitive,
+        double curvaturePerMeter,
+        double gCostMeters,
+        double hCostMeters)
+        : this(nodeIndex, key, pose, parentNodeIndex, incomingPrimitive, curvaturePerMeter, gCostMeters, hCostMeters, false)
+    {
+    }
+
+    /// <summary>
+    /// 创建一个搜索节点，并显式指定它是否为原语内部命中的终点候选。
+    /// 参数：isGoalCandidate 为 true 时，本节点不参与普通离散状态的最优 G 值支配；它仍必须在从 Open List 出队后重新通过连续终点和碰撞检查才能成功。
+    /// </summary>
+    public HybridAStarNode(
+        int nodeIndex,
+        HybridAStarNodeKey key,
+        Pose2D pose,
+        int parentNodeIndex,
+        MotionPrimitive incomingPrimitive,
+        double curvaturePerMeter,
+        double gCostMeters,
+        double hCostMeters,
+        bool isGoalCandidate)
+    {
+        if (key == null) throw new ArgumentNullException(nameof(key));
+        if (pose == null) throw new ArgumentNullException(nameof(pose));
+
+        NodeIndex = nodeIndex;
+        Key = key;
+        Pose = pose;
+        ParentNodeIndex = parentNodeIndex;
+        IncomingPrimitive = incomingPrimitive;
+        CurvaturePerMeter = curvaturePerMeter;
+        GCostMeters = gCostMeters;
+        HCostMeters = hCostMeters;
+        IsGoalCandidate = isGoalCandidate;
+    }
+
+    /// <summary>本次搜索节点数组中的稳定索引。</summary>
+    public int NodeIndex { get; }
+
+    /// <summary>本节点用于 Open/Closed 状态管理的离散键。</summary>
+    public HybridAStarNodeKey Key { get; }
+
+    /// <summary>未量化的连续车辆中心位姿。</summary>
+    public Pose2D Pose { get; }
+
+    /// <summary>父节点稳定索引；根节点为 -1。</summary>
+    public int ParentNodeIndex { get; }
+
+    /// <summary>由父节点驶入本节点的已连续碰撞检查原语；根节点为 null。</summary>
+    public MotionPrimitive IncomingPrimitive { get; }
+
+    /// <summary>与 <see cref="IncomingPrimitive"/> 含义相同的原语别名。</summary>
+    public MotionPrimitive Primitive { get { return IncomingPrimitive; } }
+
+    /// <summary>当前末段采用的车辆曲率，单位 1/m。</summary>
+    public double CurvaturePerMeter { get; }
+
+    /// <summary>当前节点的累计等效米代价。</summary>
+    public double GCostMeters { get; }
+
+    /// <summary>当前节点的二维启发式等效米代价。</summary>
+    public double HCostMeters { get; }
+
+    /// <summary>
+    /// 本节点是否由原语内部首次满足终点条件而生成。
+    /// 终点候选必须保留连续位姿，不能因同一离散键的普通低代价节点而被 Open List 准入规则压制。
+    /// </summary>
+    public bool IsGoalCandidate { get; }
+
+    /// <summary>Open List 排序使用的总等效米代价。</summary>
+    public double FCostMeters { get { return GCostMeters + HCostMeters; } }
+
+    /// <summary>本节点末段的行驶方向。</summary>
+    public TravelDirection Direction { get { return Key.Direction; } }
+
+    /// <summary>本节点末段的曲率等级索引。</summary>
+    public int CurvatureLevelIndex { get { return Key.CurvatureLevelIndex; } }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/HybridAStarNodeKey.cs

@@ -0,0 +1,67 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
+
+/// <summary>
+/// Hybrid A* 闭集使用的离散状态键。
+/// 位置使用地图行列索引；航向、行驶方向和曲率等级共同保留车辆运动学状态，避免把同一栅格中的不同可达姿态错误合并。
+/// </summary>
+public sealed class HybridAStarNodeKey : IEquatable<HybridAStarNodeKey>
+{
+    /// <summary>
+    /// 创建一个离散 Hybrid A* 状态键。
+    /// 参数：row、column 为零开始的地图行列；headingIndex 为航向桶；direction 为末段行驶方向；curvatureLevelIndex 为末段曲率等级。
+    /// </summary>
+    public HybridAStarNodeKey(int row, int column, int headingIndex, TravelDirection direction, int curvatureLevelIndex)
+    {
+        Row = row;
+        Column = column;
+        HeadingIndex = headingIndex;
+        Direction = direction;
+        CurvatureLevelIndex = curvatureLevelIndex;
+    }
+
+    /// <summary>车辆中心所在的零开始地图行索引。</summary>
+    public int Row { get; }
+
+    /// <summary>车辆中心所在的零开始地图列索引。</summary>
+    public int Column { get; }
+
+    /// <summary>与 <see cref="Column"/> 含义相同的列索引别名。</summary>
+    public int Col { get { return Column; } }
+
+    /// <summary>根据配置航向分辨率量化后的航向桶索引。</summary>
+    public int HeadingIndex { get; }
+
+    /// <summary>到达当前节点的最后一段行驶方向。</summary>
+    public TravelDirection Direction { get; }
+
+    /// <summary>到达当前节点的最后一段曲率等级索引。</summary>
+    public int CurvatureLevelIndex { get; }
+
+    /// <summary>判断另一个键是否表示完全相同的离散搜索状态。</summary>
+    public bool Equals(HybridAStarNodeKey other)
+    {
+        return other != null && Row == other.Row && Column == other.Column && HeadingIndex == other.HeadingIndex &&
+            Direction == other.Direction && CurvatureLevelIndex == other.CurvatureLevelIndex;
+    }
+
+    /// <summary>判断另一个对象是否表示完全相同的离散搜索状态。</summary>
+    public override bool Equals(object obj)
+    {
+        return Equals(obj as HybridAStarNodeKey);
+    }
+
+    /// <summary>返回用于闭集字典的稳定哈希值。</summary>
+    public override int GetHashCode()
+    {
+        unchecked
+        {
+            int hashCode = Row;
+            hashCode = hashCode * 397 ^ Column;
+            hashCode = hashCode * 397 ^ HeadingIndex;
+            hashCode = hashCode * 397 ^ (int)Direction;
+            return hashCode * 397 ^ CurvatureLevelIndex;
+        }
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/HybridAStarSearch.cs

@@ -0,0 +1,542 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using System.Threading;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
+
+/// <summary>
+/// 单次 Hybrid A* 搜索的只读结果。
+/// 即使失败也会保留已经创建的运行期节点，供上层记录诊断；仅 <see cref="PlanningStatus.Success"/> 时 <see cref="SuccessNodeIndex"/> 有值。
+/// </summary>
+public sealed class HybridAStarSearchResult
+{
+    internal HybridAStarSearchResult(
+        PlanningStatus status,
+        IEnumerable<HybridAStarNode> nodes,
+        int expandedNodeCount,
+        int generatedNodeCount,
+        int reopenedNodeCount,
+        int staleOpenListEntryCount,
+        int peakOpenListCount,
+        int? successNodeIndex,
+        string terminationReason)
+    {
+        Status = status;
+        Nodes = new ReadOnlyCollection<HybridAStarNode>(new List<HybridAStarNode>(nodes ?? Array.Empty<HybridAStarNode>()));
+        ExpandedNodeCount = expandedNodeCount;
+        GeneratedNodeCount = generatedNodeCount;
+        ReopenedNodeCount = reopenedNodeCount;
+        StaleOpenListEntryCount = staleOpenListEntryCount;
+        PeakOpenListCount = peakOpenListCount;
+        SuccessNodeIndex = status == PlanningStatus.Success ? successNodeIndex : null;
+        TerminationReason = status == PlanningStatus.Success ? string.Empty : terminationReason ?? string.Empty;
+    }
+
+    /// <summary>搜索终止状态。</summary>
+    public PlanningStatus Status { get; }
+
+    /// <summary>本次搜索已经创建的全部节点；索引与 <see cref="HybridAStarNode.NodeIndex"/> 一致。</summary>
+    public IReadOnlyList<HybridAStarNode> Nodes { get; }
+
+    /// <summary>实际从 Open List 弹出并扩展的节点数量。</summary>
+    public int ExpandedNodeCount { get; }
+
+    /// <summary>已进入 Open List 的节点数量，包含根节点和因改进代价追加的节点。</summary>
+    public int GeneratedNodeCount { get; }
+
+    /// <summary>更优路径到达已关闭离散状态、并重新放回 Open List 的次数。</summary>
+    public int ReopenedNodeCount { get; }
+
+    /// <summary>从 Open List 弹出后因已有更优普通状态而被丢弃的陈旧条目数量。</summary>
+    public int StaleOpenListEntryCount { get; }
+
+    /// <summary>搜索期间 Open List 持有的最大条目数，包含等待惰性丢弃的旧条目。</summary>
+    public int PeakOpenListCount { get; }
+
+    /// <summary>成功时最后一个从 Open List 弹出且满足终点条件的节点索引；失败时为 null。</summary>
+    public int? SuccessNodeIndex { get; }
+
+    /// <summary>搜索边界记录的原始终止原因；成功时为空字符串，失败时非空。</summary>
+    public string TerminationReason { get; }
+}
+
+/// <summary>
+/// 在不可变 <see cref="PlanningGridMap"/> 上执行前进/倒车恒曲率原语的 Hybrid A* 搜索。
+/// 搜索只消费已准备好的地图快照；所有候选原语均先完成连续扩大车体碰撞检查，再参与 Open List 排序。
+/// </summary>
+public sealed class HybridAStarSearch
+{
+    private const double CostImprovementToleranceMeters = 1e-9d;
+    private readonly MotionPrimitiveGenerator _primitiveGenerator;
+    private readonly SearchCostCalculator _costCalculator;
+    private readonly FootprintCollisionChecker _collisionChecker;
+
+    /// <summary>创建使用默认原语、代价和碰撞检查实现的搜索器。</summary>
+    public HybridAStarSearch()
+        : this(new MotionPrimitiveGenerator(), new SearchCostCalculator(), new FootprintCollisionChecker())
+    {
+    }
+
+    /// <summary>创建使用指定协作对象的搜索器，便于在不引入地图或 UI 依赖的情况下测试搜索过程。</summary>
+    public HybridAStarSearch(
+        MotionPrimitiveGenerator primitiveGenerator,
+        SearchCostCalculator costCalculator,
+        FootprintCollisionChecker collisionChecker)
+    {
+        _primitiveGenerator = primitiveGenerator ?? throw new ArgumentNullException(nameof(primitiveGenerator));
+        _costCalculator = costCalculator ?? throw new ArgumentNullException(nameof(costCalculator));
+        _collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
+    }
+
+    /// <summary>
+    /// 执行一次不可变地图上的 Hybrid A* 搜索。
+    /// 参数：request 提供地图、起终点、车辆和搜索配置；cancellationToken 在每次节点扩展前检查。
+    /// 返回：成功仅在满足目标约束的候选已从 Open List 弹出时报告；任一失败状态均不报告成功节点。
+    /// </summary>
+    public HybridAStarSearchResult Search(PlanningRequest request, CancellationToken cancellationToken)
+    {
+        TimeSpan timeout = request != null && request.Configuration != null && request.Configuration.SearchTimeout >= TimeSpan.Zero
+            ? request.Configuration.SearchTimeout
+            : TimeSpan.Zero;
+        PlanningOperationBudget budget = request != null && request.Configuration != null && request.Configuration.SearchTimeout >= TimeSpan.Zero
+            ? new PlanningOperationBudget(cancellationToken, timeout)
+            : PlanningOperationBudget.Unlimited(cancellationToken);
+        return Search(request, budget);
+    }
+
+    /// <summary>使用门面传入的共享预算执行搜索；预算从整次规划开始计时。</summary>
+    internal HybridAStarSearchResult Search(PlanningRequest request, PlanningOperationBudget budget)
+    {
+        var nodes = new List<HybridAStarNode>();
+        int expandedNodeCount = 0;
+        int generatedNodeCount = 0;
+        int reopenedNodeCount = 0;
+        int staleOpenListEntryCount = 0;
+        int peakOpenListCount = 0;
+
+        try
+        {
+            if (budget == null) throw new ArgumentNullException(nameof(budget));
+            PlanningOperationStopReason stopReason = budget.GetStopReason();
+            if (stopReason != PlanningOperationStopReason.None)
+                return CreateResult(ToPlanningStatus(stopReason), nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                    staleOpenListEntryCount, peakOpenListCount, null);
+
+            PlanningStatus validationStatus = ValidateRequest(request);
+            if (validationStatus != PlanningStatus.Success)
+                return CreateResult(validationStatus, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                    staleOpenListEntryCount, peakOpenListCount, null);
+
+            PlanningGridMap map = request.Map;
+            HybridAStarConfiguration configuration = request.Configuration;
+            VehicleParameters vehicle = request.Vehicle;
+
+            if (!IsFootprintInsideMap(request.Start, map, vehicle))
+                return CreateResult(PlanningStatus.StartOutsideMap, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                    staleOpenListEntryCount, peakOpenListCount, null);
+            if (!_collisionChecker.IsPoseCollisionFree(request.Start, map, vehicle, 0d, out _))
+                return CreateResult(PlanningStatus.StartInCollision, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                    staleOpenListEntryCount, peakOpenListCount, null);
+            if (!IsFootprintInsideMap(request.Goal, map, vehicle))
+                return CreateResult(PlanningStatus.GoalOutsideMap, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                    staleOpenListEntryCount, peakOpenListCount, null);
+            if (!_collisionChecker.IsPoseCollisionFree(request.Goal, map, vehicle, 0d, out _))
+                return CreateResult(PlanningStatus.GoalInCollision, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                    staleOpenListEntryCount, peakOpenListCount, null);
+
+            stopReason = budget.GetStopReason();
+            if (stopReason != PlanningOperationStopReason.None)
+                return CreateResult(ToPlanningStatus(stopReason), nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                    staleOpenListEntryCount, peakOpenListCount, null);
+            if (configuration.MaximumExpandedNodes == 0)
+                return CreateResult(PlanningStatus.SearchNodeLimitExceeded, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                    staleOpenListEntryCount, peakOpenListCount, null);
+
+            if (!map.TryWorldToGrid(request.Goal.X, request.Goal.Y, out int goalRow, out int goalColumn))
+                return CreateResult(PlanningStatus.GoalOutsideMap, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                    staleOpenListEntryCount, peakOpenListCount, null);
+            if (!GridDijkstraHeuristic.TryCreate(map, goalRow, goalColumn, budget, out GridDijkstraHeuristic heuristic, out stopReason))
+                return CreateResult(ToPlanningStatus(stopReason), nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                    staleOpenListEntryCount, peakOpenListCount, null);
+            if (!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
+                return CreateResult(PlanningStatus.InvalidVehicleParameters, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                    staleOpenListEntryCount, peakOpenListCount, null);
+
+            IReadOnlyList<double> curvatureLevels = _primitiveGenerator.GetCurvatureLevels(vehicle, configuration);
+            if (curvatureLevels.Count == 0)
+                return CreateResult(PlanningStatus.InvalidCurvatureConfiguration, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                    staleOpenListEntryCount, peakOpenListCount, null);
+
+            int headingBinCount = GetHeadingBinCount(configuration.HeadingResolutionRadians);
+            int startCurvatureLevelIndex = GetNearestCurvatureLevelIndex(curvatureLevels, request.StartVehicleCurvature);
+            if (startCurvatureLevelIndex < 0)
+                return CreateResult(PlanningStatus.InvalidCurvatureConfiguration, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                    staleOpenListEntryCount, peakOpenListCount, null);
+
+            if (!map.TryWorldToGrid(request.Start.X, request.Start.Y, out int startRow, out int startColumn))
+                return CreateResult(PlanningStatus.StartOutsideMap, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                    staleOpenListEntryCount, peakOpenListCount, null);
+
+            var openList = new BinaryMinHeap<int>();
+            var bestStates = new Dictionary<HybridAStarNodeKey, NodeLabel>();
+            foreach (TravelDirection startDirection in GetStartDirections(request, configuration))
+            {
+                int headingIndex = AngleMath.ToHeadingIndex(request.Start.Heading, configuration.HeadingResolutionRadians, headingBinCount);
+                var key = new HybridAStarNodeKey(startRow, startColumn, headingIndex, startDirection, startCurvatureLevelIndex);
+                if (bestStates.ContainsKey(key)) continue;
+
+                double hCostMeters = GetHeuristicCost(heuristic, startRow, startColumn, configuration);
+                if (double.IsPositiveInfinity(hCostMeters)) continue;
+                var node = new HybridAStarNode(nodes.Count, key, request.Start, -1, null,
+                    curvatureLevels[startCurvatureLevelIndex], 0d, hCostMeters);
+                nodes.Add(node);
+                bestStates.Add(key, new NodeLabel(node.NodeIndex, 0d, false));
+                openList.Push(node.NodeIndex, node.FCostMeters, node.HCostMeters, node.GCostMeters);
+                peakOpenListCount = Math.Max(peakOpenListCount, openList.Count);
+                generatedNodeCount++;
+            }
+
+            if (openList.Count == 0)
+                return CreateResult(PlanningStatus.NoFeasiblePath, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                    staleOpenListEntryCount, peakOpenListCount, null,
+                    "二维启发式标记起点不可达目标，或起始方向无法进入 Open List。");
+
+            while (openList.Count > 0)
+            {
+                stopReason = budget.GetStopReason();
+                if (stopReason != PlanningOperationStopReason.None)
+                    return CreateResult(ToPlanningStatus(stopReason), nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                        staleOpenListEntryCount, peakOpenListCount, null);
+                if (expandedNodeCount >= configuration.MaximumExpandedNodes)
+                    return CreateResult(PlanningStatus.SearchNodeLimitExceeded, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                        staleOpenListEntryCount, peakOpenListCount, null);
+
+                int nodeIndex = openList.Pop();
+                HybridAStarNode current = nodes[nodeIndex];
+                NodeLabel currentLabel = null;
+                if (!current.IsGoalCandidate)
+                {
+                    if (!bestStates.TryGetValue(current.Key, out currentLabel) || currentLabel.NodeIndex != nodeIndex || currentLabel.IsClosed)
+                    {
+                        staleOpenListEntryCount++;
+                        continue;
+                    }
+
+                    currentLabel.IsClosed = true;
+                }
+
+                expandedNodeCount++;
+                if (current.IsGoalCandidate)
+                {
+                    if (!GoalToleranceChecker.IsSatisfied(current.Pose, request.Goal, configuration, current.Direction, request.GoalDirection) ||
+                        !_collisionChecker.IsPoseCollisionFree(current.Pose, map, vehicle, 0d, out _))
+                        continue;
+
+                    return CreateResult(PlanningStatus.Success, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                        staleOpenListEntryCount, peakOpenListCount, current.NodeIndex);
+                }
+
+                if (GoalToleranceChecker.IsSatisfied(current.Pose, request.Goal, configuration, current.Direction, request.GoalDirection))
+                    return CreateResult(PlanningStatus.Success, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                        staleOpenListEntryCount, peakOpenListCount, current.NodeIndex);
+
+                for (int curvatureLevelIndex = 0; curvatureLevelIndex < curvatureLevels.Count; curvatureLevelIndex++)
+                {
+                    stopReason = budget.GetStopReason();
+                    if (stopReason != PlanningOperationStopReason.None)
+                        return CreateResult(ToPlanningStatus(stopReason), nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                            staleOpenListEntryCount, peakOpenListCount, null);
+                    if (!MotionPrimitiveGenerator.AreCurvatureLevelsAdjacent(current.CurvatureLevelIndex, curvatureLevelIndex)) continue;
+
+                    foreach (TravelDirection direction in GetSuccessorDirections(configuration))
+                    {
+                        MotionPrimitive primitive = _primitiveGenerator.Generate(current.Pose, curvatureLevels[curvatureLevelIndex], direction, request);
+                        if (primitive == null || primitive.ActualLengthMeters <= 0d) continue;
+
+                        if (!map.TryWorldToGrid(primitive.End.X, primitive.End.Y, out int row, out int column)) continue;
+                        double hCostMeters = GetHeuristicCost(heuristic, row, column, configuration);
+                        if (double.IsPositiveInfinity(hCostMeters)) continue;
+
+                        double bodyClearanceMeters = GetMinimumBodyClearance(primitive);
+                        bool isGearSwitch = current.ParentNodeIndex >= 0 && current.Direction != direction;
+                        int curvatureLevelDelta = curvatureLevelIndex - current.CurvatureLevelIndex;
+                        double incrementalCostMeters = _costCalculator.Calculate(
+                            primitive.ActualLengthMeters,
+                            direction,
+                            isGearSwitch,
+                            curvatureLevels[curvatureLevelIndex],
+                            maximumCurvaturePerMeter,
+                            curvatureLevelDelta,
+                            bodyClearanceMeters,
+                            configuration);
+                        double gCostMeters = current.GCostMeters + incrementalCostMeters;
+                        if (!NumericGuard.IsFinite(gCostMeters) || gCostMeters < 0d) continue;
+
+                        int headingIndex = AngleMath.ToHeadingIndex(primitive.End.Heading, configuration.HeadingResolutionRadians, headingBinCount);
+                        var key = new HybridAStarNodeKey(row, column, headingIndex, direction, curvatureLevelIndex);
+                        bestStates.TryGetValue(key, out NodeLabel existingLabel);
+                        var successor = new HybridAStarNode(nodes.Count, key, primitive.End, current.NodeIndex, primitive,
+                            curvatureLevels[curvatureLevelIndex], gCostMeters, hCostMeters, primitive.IsGoalTruncation);
+                        if (existingLabel != null && !ShouldEnqueueSuccessor(successor, existingLabel.GCostMeters)) continue;
+
+                        if (!successor.IsGoalCandidate && existingLabel != null && existingLabel.IsClosed) reopenedNodeCount++;
+                        nodes.Add(successor);
+                        if (!successor.IsGoalCandidate)
+                            bestStates[key] = new NodeLabel(successor.NodeIndex, gCostMeters, false);
+                        openList.Push(successor.NodeIndex, successor.FCostMeters, successor.HCostMeters, successor.GCostMeters);
+                        peakOpenListCount = Math.Max(peakOpenListCount, openList.Count);
+                        generatedNodeCount++;
+                    }
+                }
+            }
+
+            return CreateResult(PlanningStatus.NoFeasiblePath, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                staleOpenListEntryCount, peakOpenListCount, null,
+                "Hybrid A* 搜索的 Open List 已耗尽，未找到满足运动和碰撞约束的路径。");
+        }
+        catch (Exception exception)
+        {
+            string reason = "Hybrid A* 搜索内部错误：" + exception.GetType().Name +
+                (string.IsNullOrEmpty(exception.Message) ? "。" : "。" + exception.Message);
+            return CreateResult(PlanningStatus.InternalError, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                staleOpenListEntryCount, peakOpenListCount, null, reason);
+        }
+    }
+
+    private static HybridAStarSearchResult CreateResult(
+        PlanningStatus status,
+        IEnumerable<HybridAStarNode> nodes,
+        int expandedNodeCount,
+        int generatedNodeCount,
+        int reopenedNodeCount,
+        int staleOpenListEntryCount,
+        int peakOpenListCount,
+        int? successNodeIndex,
+        string terminationReason = null)
+    {
+        string reason = status == PlanningStatus.Success
+            ? string.Empty
+            : terminationReason ?? GetDefaultTerminationReason(status);
+        return new HybridAStarSearchResult(status, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+            staleOpenListEntryCount, peakOpenListCount, successNodeIndex, reason);
+    }
+
+    private static string GetDefaultTerminationReason(PlanningStatus status)
+    {
+        switch (status)
+        {
+            case PlanningStatus.Cancelled:
+                return "Hybrid A* 搜索已取消。";
+            case PlanningStatus.InvalidRequest:
+                return "Hybrid A* 搜索请求缺少必要对象或包含非法数值。";
+            case PlanningStatus.InvalidMap:
+                return "Hybrid A* 搜索地图结构无效。";
+            case PlanningStatus.MapNotReady:
+                return "Hybrid A* 搜索地图尚未准备好。";
+            case PlanningStatus.InvalidVehicleParameters:
+                return "Hybrid A* 搜索车辆参数无效。";
+            case PlanningStatus.InvalidCurvatureConfiguration:
+                return "Hybrid A* 搜索曲率、离散、代价或资源配置无效。";
+            case PlanningStatus.StartOutsideMap:
+                return "Hybrid A* 搜索起始扩大车体不完全位于地图内。";
+            case PlanningStatus.StartInCollision:
+                return "Hybrid A* 搜索起始扩大车体与障碍物相交或擦边。";
+            case PlanningStatus.GoalOutsideMap:
+                return "Hybrid A* 搜索目标扩大车体不完全位于地图内。";
+            case PlanningStatus.GoalInCollision:
+                return "Hybrid A* 搜索目标扩大车体与障碍物相交或擦边。";
+            case PlanningStatus.SearchTimeout:
+                return "Hybrid A* 搜索使用的总规划预算已耗尽。";
+            case PlanningStatus.SearchNodeLimitExceeded:
+                return "Hybrid A* 搜索达到扩展节点上限。";
+            case PlanningStatus.NoFeasiblePath:
+                return "Hybrid A* 搜索的 Open List 已耗尽，未找到满足运动和碰撞约束的路径。";
+            case PlanningStatus.BacktrackingFailed:
+                return "Hybrid A* 成功节点无法回溯为完整父链。";
+            case PlanningStatus.FinalValidationFailed:
+                return "Hybrid A* 路径未通过最终复核。";
+            case PlanningStatus.InternalError:
+                return "Hybrid A* 搜索发生未预期内部错误。";
+            default:
+                return "Hybrid A* 搜索以未识别状态终止：" + status + "。";
+        }
+    }
+
+    private static PlanningStatus ValidateRequest(PlanningRequest request)
+    {
+        if (request == null || request.Map == null || request.Vehicle == null || request.Configuration == null ||
+            !IsFinitePose(request.Start) || !IsFinitePose(request.Goal) || !NumericGuard.IsFinite(request.StartVehicleCurvature) ||
+            !IsGoalDirection(request.GoalDirection) || (request.StartDirection.HasValue && !IsTravelDirection(request.StartDirection.Value)))
+            return PlanningStatus.InvalidRequest;
+
+        PlanningGridMap map = request.Map;
+        if (map.Rows <= 0 || map.Cols <= 0 || !NumericGuard.IsPositiveFinite(map.ResolutionMeters) || map.Bounds == null)
+            return PlanningStatus.InvalidMap;
+        if (!map.PlanningReady) return PlanningStatus.MapNotReady;
+
+        VehicleParameters vehicle = request.Vehicle;
+        if (!NumericGuard.IsPositiveFinite(vehicle.LengthMeters) || !NumericGuard.IsPositiveFinite(vehicle.WidthMeters) ||
+            !NumericGuard.IsFinite(vehicle.SafetyMarginMeters) || vehicle.SafetyMarginMeters < 0d ||
+            !VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
+            return PlanningStatus.InvalidVehicleParameters;
+
+        HybridAStarConfiguration configuration = request.Configuration;
+        if (!IsValidConfiguration(configuration) || Math.Abs(request.StartVehicleCurvature) > maximumCurvaturePerMeter ||
+            (request.StartDirection == TravelDirection.Reverse && !configuration.AllowReverse))
+            return PlanningStatus.InvalidCurvatureConfiguration;
+
+        return PlanningStatus.Success;
+    }
+
+    private static bool IsValidConfiguration(HybridAStarConfiguration configuration)
+    {
+        return NumericGuard.IsPositiveFinite(configuration.PrimitiveLengthMeters) &&
+            NumericGuard.IsPositiveFinite(configuration.IntegrationStepMeters) &&
+            NumericGuard.IsPositiveFinite(configuration.MaximumCollisionCheckStepMeters) &&
+            NumericGuard.IsPositiveFinite(configuration.HeadingResolutionRadians) &&
+            configuration.HeadingResolutionRadians <= 2d * Math.PI &&
+            configuration.CurvatureLevelCount >= 3 && configuration.CurvatureLevelCount % 2 == 1 &&
+            NumericGuard.IsFinite(configuration.GoalPositionToleranceMeters) && configuration.GoalPositionToleranceMeters >= 0d &&
+            NumericGuard.IsFinite(configuration.GoalHeadingToleranceRadians) && configuration.GoalHeadingToleranceRadians >= 0d &&
+            configuration.MaximumExpandedNodes >= 0 && configuration.SearchTimeout >= TimeSpan.Zero &&
+            NumericGuard.IsFinite(configuration.HeuristicWeight) && configuration.HeuristicWeight >= 0d &&
+            NumericGuard.IsPositiveFinite(configuration.ReverseCostMultiplier) &&
+            NumericGuard.IsFinite(configuration.GearSwitchPenaltyMeters) && configuration.GearSwitchPenaltyMeters >= 0d &&
+            NumericGuard.IsFinite(configuration.CurvatureMagnitudeWeight) && configuration.CurvatureMagnitudeWeight >= 0d &&
+            NumericGuard.IsFinite(configuration.CurvatureChangePenaltyMetersPerLevel) && configuration.CurvatureChangePenaltyMetersPerLevel >= 0d &&
+            NumericGuard.IsFinite(configuration.ClearanceCostWeight) && configuration.ClearanceCostWeight >= 0d &&
+            NumericGuard.IsPositiveFinite(configuration.ClearanceCostDistanceMeters);
+    }
+
+    private static bool IsFootprintInsideMap(Pose2D pose, PlanningGridMap map, VehicleParameters vehicle)
+    {
+        if (!map.TryWorldToGrid(pose.X, pose.Y, out _, out _)) return false;
+
+        double halfLengthMeters = vehicle.LengthMeters / 2d + vehicle.SafetyMarginMeters;
+        double halfWidthMeters = vehicle.WidthMeters / 2d + vehicle.SafetyMarginMeters;
+        double longitudinalX = Math.Cos(pose.Heading);
+        double longitudinalY = Math.Sin(pose.Heading);
+        double lateralX = -longitudinalY;
+        double lateralY = longitudinalX;
+        for (int longitudinalSign = -1; longitudinalSign <= 1; longitudinalSign += 2)
+        for (int lateralSign = -1; lateralSign <= 1; lateralSign += 2)
+        {
+            double cornerX = pose.X + longitudinalSign * halfLengthMeters * longitudinalX + lateralSign * halfWidthMeters * lateralX;
+            double cornerY = pose.Y + longitudinalSign * halfLengthMeters * longitudinalY + lateralSign * halfWidthMeters * lateralY;
+            if (!map.TryWorldToGrid(cornerX, cornerY, out _, out _)) return false;
+        }
+        return true;
+    }
+
+    private static IEnumerable<TravelDirection> GetStartDirections(PlanningRequest request, HybridAStarConfiguration configuration)
+    {
+        if (request.StartDirection.HasValue)
+        {
+            yield return request.StartDirection.Value;
+            yield break;
+        }
+
+        yield return TravelDirection.Forward;
+        if (configuration.AllowReverse) yield return TravelDirection.Reverse;
+    }
+
+    private static IEnumerable<TravelDirection> GetSuccessorDirections(HybridAStarConfiguration configuration)
+    {
+        yield return TravelDirection.Forward;
+        if (configuration.AllowReverse) yield return TravelDirection.Reverse;
+    }
+
+    private static int GetHeadingBinCount(double headingResolutionRadians)
+    {
+        double rawBinCount = Math.Ceiling(2d * Math.PI / headingResolutionRadians);
+        if (!NumericGuard.IsFinite(rawBinCount) || rawBinCount < 1d || rawBinCount > int.MaxValue)
+            throw new ArgumentOutOfRangeException(nameof(headingResolutionRadians));
+        return (int)rawBinCount;
+    }
+
+    private static int GetNearestCurvatureLevelIndex(IReadOnlyList<double> curvatureLevels, double curvaturePerMeter)
+    {
+        int bestIndex = -1;
+        double smallestDifference = double.PositiveInfinity;
+        for (int index = 0; index < curvatureLevels.Count; index++)
+        {
+            double difference = Math.Abs(curvatureLevels[index] - curvaturePerMeter);
+            if (difference >= smallestDifference) continue;
+            smallestDifference = difference;
+            bestIndex = index;
+        }
+        return bestIndex;
+    }
+
+    private static double GetMinimumBodyClearance(MotionPrimitive primitive)
+    {
+        double minimum = double.PositiveInfinity;
+        foreach (double clearanceMeters in primitive.BodyClearancesMeters)
+        {
+            if (double.IsNaN(clearanceMeters) || clearanceMeters < 0d) return 0d;
+            minimum = Math.Min(minimum, clearanceMeters);
+        }
+        return minimum;
+    }
+
+    private static double GetHeuristicCost(GridDijkstraHeuristic heuristic, int row, int column, HybridAStarConfiguration configuration)
+    {
+        double gridCostMeters = heuristic.GetCost(row, column);
+        if (double.IsPositiveInfinity(gridCostMeters)) return gridCostMeters;
+        double weightedCostMeters = gridCostMeters * configuration.HeuristicWeight;
+        return NumericGuard.IsFinite(weightedCostMeters) && weightedCostMeters >= 0d
+            ? weightedCostMeters
+            : double.PositiveInfinity;
+    }
+
+    /// <summary>
+    /// 判断后继是否应进入 Open List。
+    /// 参数：successor 是已完成连续碰撞检查的后继；bestKnownGCostMeters 是相同离散键普通状态的当前最优 G 值。
+    /// 返回：普通状态仅在严格改善最优 G 值时入队；终点候选始终入队，以保留其未量化的连续终点位姿。
+    /// </summary>
+    private static bool ShouldEnqueueSuccessor(HybridAStarNode successor, double bestKnownGCostMeters)
+    {
+        if (successor == null || !NumericGuard.IsFinite(bestKnownGCostMeters)) return false;
+        return successor.IsGoalCandidate || successor.GCostMeters < bestKnownGCostMeters - CostImprovementToleranceMeters;
+    }
+
+    private static PlanningStatus ToPlanningStatus(PlanningOperationStopReason stopReason)
+    {
+        if (stopReason == PlanningOperationStopReason.Cancelled) return PlanningStatus.Cancelled;
+        if (stopReason == PlanningOperationStopReason.TimedOut) return PlanningStatus.SearchTimeout;
+        throw new ArgumentOutOfRangeException(nameof(stopReason));
+    }
+
+    private static bool IsFinitePose(Pose2D pose)
+    {
+        return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
+    }
+
+    private static bool IsTravelDirection(TravelDirection direction)
+    {
+        return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
+    }
+
+    private static bool IsGoalDirection(GoalDirectionConstraint direction)
+    {
+        return direction == GoalDirectionConstraint.Any || direction == GoalDirectionConstraint.Forward || direction == GoalDirectionConstraint.Reverse;
+    }
+
+    private sealed class NodeLabel
+    {
+        public NodeLabel(int nodeIndex, double gCostMeters, bool isClosed)
+        {
+            NodeIndex = nodeIndex;
+            GCostMeters = gCostMeters;
+            IsClosed = isClosed;
+        }
+
+        public int NodeIndex { get; }
+        public double GCostMeters { get; }
+        public bool IsClosed { get; set; }
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/MotionPrimitive.cs

@@ -0,0 +1,69 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
+
+/// <summary>
+/// 一条经连续碰撞检查的恒曲率运动原语。
+/// 位置和长度单位为 m，航向单位为 rad，曲率单位为 1/m；<see cref="Points"/> 不包含起点，只包含按积分顺序产生的后续点。
+/// </summary>
+public sealed class MotionPrimitive
+{
+    /// <summary>
+    /// 创建不可变运动原语。
+    /// 参数：start 为原语起点；direction 为行驶方向；curvaturePerMeter 为恒定曲率；actualLengthMeters 为已实际行驶长度；
+    /// points 与 bodyClearancesMeters 按同一索引保存内部积分位姿和对应的车体净空；isGoalTruncation 表示末点是否首次命中目标容差。
+    /// </summary>
+    public MotionPrimitive(
+        Pose2D start,
+        TravelDirection direction,
+        double curvaturePerMeter,
+        double actualLengthMeters,
+        IEnumerable<Pose2D> points,
+        IEnumerable<double> bodyClearancesMeters,
+        bool isGoalTruncation)
+    {
+        if (start == null) throw new ArgumentNullException(nameof(start));
+        if (points == null) throw new ArgumentNullException(nameof(points));
+        if (bodyClearancesMeters == null) throw new ArgumentNullException(nameof(bodyClearancesMeters));
+
+        var copiedPoints = new List<Pose2D>(points);
+        var copiedClearances = new List<double>(bodyClearancesMeters);
+        if (copiedPoints.Count != copiedClearances.Count)
+            throw new ArgumentException("The point and clearance counts must match.", nameof(bodyClearancesMeters));
+
+        Start = start;
+        Direction = direction;
+        CurvaturePerMeter = curvaturePerMeter;
+        ActualLengthMeters = actualLengthMeters;
+        Points = new ReadOnlyCollection<Pose2D>(copiedPoints);
+        BodyClearancesMeters = new ReadOnlyCollection<double>(copiedClearances);
+        End = copiedPoints.Count == 0 ? start : copiedPoints[copiedPoints.Count - 1];
+        IsGoalTruncation = isGoalTruncation;
+    }
+
+    /// <summary>原语起始车辆几何中心位姿。</summary>
+    public Pose2D Start { get; }
+
+    /// <summary>原语最后一个积分点；零长度终点候选时等于 <see cref="Start"/>。</summary>
+    public Pose2D End { get; }
+
+    /// <summary>原语对应的行驶方向。</summary>
+    public TravelDirection Direction { get; }
+
+    /// <summary>原语全程采用的恒定车辆曲率，单位 1/m。</summary>
+    public double CurvaturePerMeter { get; }
+
+    /// <summary>从 <see cref="Start"/> 到 <see cref="End"/> 的实际行驶弧长，单位 m。</summary>
+    public double ActualLengthMeters { get; }
+
+    /// <summary>不含起点的连续积分位姿，只读且按行驶顺序排列。</summary>
+    public IReadOnlyList<Pose2D> Points { get; }
+
+    /// <summary>与 <see cref="Points"/> 一一对应的扩大车体保守净空下界，单位 m。</summary>
+    public IReadOnlyList<double> BodyClearancesMeters { get; }
+
+    /// <summary>末点是否因首次满足目标位置、航向和方向约束而截断。</summary>
+    public bool IsGoalTruncation { get; }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/MotionPrimitiveGenerator.cs

@@ -0,0 +1,173 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
+
+/// <summary>
+/// 生成经解析积分和连续碰撞检查的前进或倒车恒曲率原语。
+/// 每个积分点均依次经过有限数值检查、与前一点之间的扫掠碰撞检查和终点容差检查。
+/// </summary>
+public sealed class MotionPrimitiveGenerator
+{
+    private const double StraightCurvatureThreshold = 1e-12d;
+    private readonly FootprintCollisionChecker _collisionChecker;
+
+    /// <summary>创建使用默认车辆连续碰撞检查器的原语生成器。</summary>
+    public MotionPrimitiveGenerator()
+        : this(new FootprintCollisionChecker())
+    {
+    }
+
+    /// <summary>创建使用指定连续车辆碰撞检查器的原语生成器。</summary>
+    public MotionPrimitiveGenerator(FootprintCollisionChecker collisionChecker)
+    {
+        _collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
+    }
+
+    /// <summary>
+    /// 使用完整规划请求生成一条原语。
+    /// 参数：start 为当前连续位姿；curvaturePerMeter 为候选恒定曲率；direction 为前进或倒车；request 提供地图、车辆、配置和目标。
+    /// 返回：输入无效、曲率超限或任一积分点碰撞时为 null；否则返回最大长度不超过配置上限的原语。
+    /// </summary>
+    public MotionPrimitive Generate(Pose2D start, double curvaturePerMeter, TravelDirection direction, PlanningRequest request)
+    {
+        if (request == null) return null;
+        return Generate(start, curvaturePerMeter, direction, request.Map, request.Vehicle, request.Configuration, request.Goal, request.GoalDirection);
+    }
+
+    /// <summary>
+    /// 使用显式地图、车辆、配置和目标生成一条原语。
+    /// 参数：所有位置使用 m/rad，curvaturePerMeter 使用 1/m；goalDirection 限制末段允许的进入方向。
+    /// 返回：输入无效、曲率超限或任一积分点碰撞时为 null；首次命中目标时返回 <see cref="MotionPrimitive.IsGoalTruncation"/> 为 true 的截断原语。
+    /// </summary>
+    public MotionPrimitive Generate(
+        Pose2D start,
+        double curvaturePerMeter,
+        TravelDirection direction,
+        PlanningGridMap map,
+        VehicleParameters vehicle,
+        HybridAStarConfiguration configuration,
+        Pose2D goal,
+        GoalDirectionConstraint goalDirection)
+    {
+        if (!IsValidInput(start, curvaturePerMeter, direction, map, vehicle, configuration, goal, goalDirection)) return null;
+
+        if (GoalToleranceChecker.IsSatisfied(start, goal, configuration, direction, goalDirection))
+            return new MotionPrimitive(start, direction, curvaturePerMeter, 0d, Array.Empty<Pose2D>(), Array.Empty<double>(), true);
+
+        double pointStepMeters = Math.Min(configuration.IntegrationStepMeters,
+            Math.Min(configuration.MaximumCollisionCheckStepMeters, map.ResolutionMeters / 2d));
+        if (!NumericGuard.IsPositiveFinite(pointStepMeters)) return null;
+
+        var points = new List<Pose2D>();
+        var bodyClearancesMeters = new List<double>();
+        Pose2D previous = start;
+        double actualLengthMeters = 0d;
+        while (actualLengthMeters < configuration.PrimitiveLengthMeters)
+        {
+            double remainingLengthMeters = configuration.PrimitiveLengthMeters - actualLengthMeters;
+            double stepMeters = Math.Min(pointStepMeters, remainingLengthMeters);
+            if (!NumericGuard.IsPositiveFinite(stepMeters)) return null;
+
+            Pose2D next = Integrate(previous, curvaturePerMeter, direction, stepMeters);
+            if (!IsFinitePose(next)) return null;
+            if (!_collisionChecker.IsSweptMotionCollisionFree(previous, next, map, vehicle, stepMeters, out double bodyClearanceMeters)) return null;
+
+            actualLengthMeters += stepMeters;
+            points.Add(next);
+            bodyClearancesMeters.Add(bodyClearanceMeters);
+            if (GoalToleranceChecker.IsSatisfied(next, goal, configuration, direction, goalDirection))
+                return new MotionPrimitive(start, direction, curvaturePerMeter, actualLengthMeters, points, bodyClearancesMeters, true);
+
+            previous = next;
+        }
+
+        return new MotionPrimitive(start, direction, curvaturePerMeter, actualLengthMeters, points, bodyClearancesMeters, false);
+    }
+
+    /// <summary>
+    /// 获取从最大负曲率到最大正曲率均匀分布的候选曲率等级。
+    /// 参数：vehicle 提供保守最大曲率；configuration 的曲率等级数必须为不小于 3 的奇数。
+    /// 返回：输入无效时为空只读列表；有效时长度等于配置等级数且中间等级恒为零曲率。
+    /// </summary>
+    public IReadOnlyList<double> GetCurvatureLevels(VehicleParameters vehicle, HybridAStarConfiguration configuration)
+    {
+        if (configuration == null || configuration.CurvatureLevelCount < 3 || configuration.CurvatureLevelCount % 2 == 0 ||
+            !VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
+            return Array.Empty<double>();
+
+        var levels = new double[configuration.CurvatureLevelCount];
+        double increment = 2d * maximumCurvaturePerMeter / (levels.Length - 1d);
+        for (int index = 0; index < levels.Length; index++) levels[index] = -maximumCurvaturePerMeter + increment * index;
+        levels[levels.Length / 2] = 0d;
+        return levels;
+    }
+
+    /// <summary>
+    /// 判断两个曲率等级是否允许在相邻原语间直接切换。
+    /// 参数：previousLevelIndex 与 nextLevelIndex 为从零开始的等级索引。
+    /// 返回：两个索引均非负且最多相差一个等级时为 true。
+    /// </summary>
+    public static bool AreCurvatureLevelsAdjacent(int previousLevelIndex, int nextLevelIndex)
+    {
+        return previousLevelIndex >= 0 && nextLevelIndex >= 0 && Math.Abs(previousLevelIndex - nextLevelIndex) <= 1;
+    }
+
+    private static bool IsValidInput(
+        Pose2D start,
+        double curvaturePerMeter,
+        TravelDirection direction,
+        PlanningGridMap map,
+        VehicleParameters vehicle,
+        HybridAStarConfiguration configuration,
+        Pose2D goal,
+        GoalDirectionConstraint goalDirection)
+    {
+        if (!IsFinitePose(start) || !IsFinitePose(goal) || map == null || vehicle == null || configuration == null ||
+            !NumericGuard.IsFinite(curvaturePerMeter) || !IsTravelDirection(direction) || !IsGoalDirection(goalDirection) ||
+            !NumericGuard.IsPositiveFinite(configuration.PrimitiveLengthMeters) ||
+            !NumericGuard.IsPositiveFinite(configuration.IntegrationStepMeters) ||
+            !NumericGuard.IsPositiveFinite(configuration.MaximumCollisionCheckStepMeters) ||
+            !NumericGuard.IsPositiveFinite(map.ResolutionMeters) ||
+            !VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
+            return false;
+
+        return Math.Abs(curvaturePerMeter) <= maximumCurvaturePerMeter;
+    }
+
+    private static Pose2D Integrate(Pose2D previous, double curvaturePerMeter, TravelDirection direction, double stepMeters)
+    {
+        double signedDistanceMeters = direction == TravelDirection.Forward ? stepMeters : -stepMeters;
+        double nextHeadingRadians = AngleMath.NormalizeRadians(previous.Heading + curvaturePerMeter * signedDistanceMeters);
+        if (Math.Abs(curvaturePerMeter) < StraightCurvatureThreshold)
+        {
+            return new Pose2D(
+                previous.X + signedDistanceMeters * Math.Cos(previous.Heading),
+                previous.Y + signedDistanceMeters * Math.Sin(previous.Heading),
+                nextHeadingRadians);
+        }
+
+        return new Pose2D(
+            previous.X + (Math.Sin(nextHeadingRadians) - Math.Sin(previous.Heading)) / curvaturePerMeter,
+            previous.Y - (Math.Cos(nextHeadingRadians) - Math.Cos(previous.Heading)) / curvaturePerMeter,
+            nextHeadingRadians);
+    }
+
+    private static bool IsFinitePose(Pose2D pose)
+    {
+        return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
+    }
+
+    private static bool IsTravelDirection(TravelDirection direction)
+    {
+        return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
+    }
+
+    private static bool IsGoalDirection(GoalDirectionConstraint direction)
+    {
+        return direction == GoalDirectionConstraint.Any || direction == GoalDirectionConstraint.Forward || direction == GoalDirectionConstraint.Reverse;
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/SearchCostCalculator.cs

@@ -0,0 +1,98 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
+
+/// <summary>
+/// 将运动原语的长度、方向、曲率和净空转换为统一的等效米搜索代价。
+/// 此类型不修改搜索状态；换向标记必须由调用方依据相邻原语的方向关系提供。
+/// </summary>
+public sealed class SearchCostCalculator
+{
+    /// <summary>创建使用调用时配置参数的等效米代价计算器。</summary>
+    public SearchCostCalculator()
+    {
+    }
+
+    /// <summary>
+    /// 计算一条运动原语的等效米增量代价。
+    /// 参数：lengthMeters 为非负弧长 m；direction 为原语方向；isGearSwitch 表示该原语前是否换向；
+    /// curvaturePerMeter 与 maximumCurvaturePerMeter 的单位为 1/m；curvatureLevelDelta 为相邻曲率等级差；
+    /// bodyClearanceMeters 为非负车体保守净空 m，可为正无穷；configuration 提供非负权重和惩罚。
+    /// 返回：有限且非负的等效米代价。
+    /// 失败：任一数值、方向或权重无效时抛出 <see cref="ArgumentOutOfRangeException"/>。
+    /// </summary>
+    public double Calculate(
+        double lengthMeters,
+        TravelDirection direction,
+        bool isGearSwitch,
+        double curvaturePerMeter,
+        double maximumCurvaturePerMeter,
+        int curvatureLevelDelta,
+        double bodyClearanceMeters,
+        HybridAStarConfiguration configuration)
+    {
+        ValidateInput(lengthMeters, direction, curvaturePerMeter, maximumCurvaturePerMeter, bodyClearanceMeters, configuration);
+
+        double directionMultiplier = direction == TravelDirection.Reverse ? configuration.ReverseCostMultiplier : 1d;
+        double normalizedCurvature = Math.Abs(curvaturePerMeter / maximumCurvaturePerMeter);
+        double clearanceDeficit = GetClearanceDeficit(bodyClearanceMeters, configuration.ClearanceCostDistanceMeters);
+        double levelDelta = Math.Abs((double)curvatureLevelDelta);
+        double motionCost = lengthMeters * directionMultiplier *
+            (1d + configuration.CurvatureMagnitudeWeight * normalizedCurvature +
+                configuration.ClearanceCostWeight * clearanceDeficit);
+        double gearSwitchCost = isGearSwitch ? configuration.GearSwitchPenaltyMeters : 0d;
+        double curvatureChangeCost = configuration.CurvatureChangePenaltyMetersPerLevel * levelDelta;
+        double totalCost = motionCost + gearSwitchCost + curvatureChangeCost;
+        if (!NumericGuard.IsFinite(totalCost) || totalCost < 0d)
+            throw new ArgumentOutOfRangeException(nameof(lengthMeters), "The calculated cost must remain finite and non-negative.");
+
+        return totalCost;
+    }
+
+    private static void ValidateInput(
+        double lengthMeters,
+        TravelDirection direction,
+        double curvaturePerMeter,
+        double maximumCurvaturePerMeter,
+        double bodyClearanceMeters,
+        HybridAStarConfiguration configuration)
+    {
+        if (!NumericGuard.IsFinite(lengthMeters) || lengthMeters < 0d)
+            throw new ArgumentOutOfRangeException(nameof(lengthMeters));
+        if (direction != TravelDirection.Forward && direction != TravelDirection.Reverse)
+            throw new ArgumentOutOfRangeException(nameof(direction));
+        if (!NumericGuard.IsFinite(curvaturePerMeter))
+            throw new ArgumentOutOfRangeException(nameof(curvaturePerMeter));
+        if (!NumericGuard.IsPositiveFinite(maximumCurvaturePerMeter))
+            throw new ArgumentOutOfRangeException(nameof(maximumCurvaturePerMeter));
+        if (Math.Abs(curvaturePerMeter) > maximumCurvaturePerMeter)
+            throw new ArgumentOutOfRangeException(nameof(curvaturePerMeter));
+        if (double.IsNaN(bodyClearanceMeters) || bodyClearanceMeters < 0d)
+            throw new ArgumentOutOfRangeException(nameof(bodyClearanceMeters));
+        if (configuration == null) throw new ArgumentNullException(nameof(configuration));
+
+        ValidateNonNegativeFinite(configuration.HeuristicWeight, nameof(configuration.HeuristicWeight));
+        if (!NumericGuard.IsPositiveFinite(configuration.ReverseCostMultiplier))
+            throw new ArgumentOutOfRangeException(nameof(configuration.ReverseCostMultiplier));
+        ValidateNonNegativeFinite(configuration.GearSwitchPenaltyMeters, nameof(configuration.GearSwitchPenaltyMeters));
+        ValidateNonNegativeFinite(configuration.CurvatureMagnitudeWeight, nameof(configuration.CurvatureMagnitudeWeight));
+        ValidateNonNegativeFinite(configuration.CurvatureChangePenaltyMetersPerLevel, nameof(configuration.CurvatureChangePenaltyMetersPerLevel));
+        ValidateNonNegativeFinite(configuration.ClearanceCostWeight, nameof(configuration.ClearanceCostWeight));
+        if (!NumericGuard.IsPositiveFinite(configuration.ClearanceCostDistanceMeters))
+            throw new ArgumentOutOfRangeException(nameof(configuration.ClearanceCostDistanceMeters));
+    }
+
+    private static void ValidateNonNegativeFinite(double value, string parameterName)
+    {
+        if (!NumericGuard.IsFinite(value) || value < 0d)
+            throw new ArgumentOutOfRangeException(parameterName);
+    }
+
+    private static double GetClearanceDeficit(double bodyClearanceMeters, double clearanceCostDistanceMeters)
+    {
+        if (double.IsPositiveInfinity(bodyClearanceMeters)) return 0d;
+        double deficit = 1d - bodyClearanceMeters / clearanceCostDistanceMeters;
+        return deficit > 0d ? deficit : 0d;
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Test/CoarsePathScenarioFactory.cs

@@ -0,0 +1,507 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Test;
+
+/// <summary>Clumsy 粗路径手动测试可选择的固定场景。</summary>
+public enum CoarsePathTestScenario
+{
+    /// <summary>明确允许的空地图直达场景。</summary>
+    ExplicitEmpty,
+
+    /// <summary>由中央矩形阻断直线的绕行场景。</summary>
+    RectangleDetour,
+
+    /// <summary>同时包含手工圆形、矩形与 TwoLeg 快照的多来源场景。</summary>
+    ManualAndTwoLeg,
+
+    /// <summary>与矩形绕行输入完全一致，用于在同一服务中验证输入缓存命中。</summary>
+    CacheHit,
+
+    /// <summary>起步前进、终点倒车进入的换向场景。</summary>
+    ReverseGearSwitch,
+
+    /// <summary>由贯穿边界的障碍带分隔起终点的无解场景。</summary>
+    NoFeasiblePath,
+}
+
+/// <summary>手动障碍物输入支持的世界几何类型。</summary>
+public enum ManualCoarsePathObstacleKind
+{
+    /// <summary>由世界中心和半径定义的圆形障碍物。</summary>
+    Circle,
+
+    /// <summary>由世界中心、X 方向长度和 Y 方向宽度定义的轴对齐矩形障碍物。</summary>
+    AxisAlignedRectangle,
+}
+
+/// <summary>
+/// 手动粗路径测试的不可变障碍物输入。
+/// 所有中心和尺寸均使用世界 mm；矩形始终与世界坐标轴平行，不包含旋转角。
+/// </summary>
+public sealed class ManualCoarsePathObstacle
+{
+    private ManualCoarsePathObstacle(ManualCoarsePathObstacleKind kind, double centerXMillimeters,
+        double centerYMillimeters, double sizeXMillimeters, double sizeYMillimeters)
+    {
+        Kind = kind;
+        CenterXMillimeters = centerXMillimeters;
+        CenterYMillimeters = centerYMillimeters;
+        SizeXMillimeters = sizeXMillimeters;
+        SizeYMillimeters = sizeYMillimeters;
+    }
+
+    /// <summary>障碍物的支持几何类型。</summary>
+    public ManualCoarsePathObstacleKind Kind { get; }
+
+    /// <summary>几何中心世界 X 坐标，单位 mm。</summary>
+    public double CenterXMillimeters { get; }
+
+    /// <summary>几何中心世界 Y 坐标，单位 mm。</summary>
+    public double CenterYMillimeters { get; }
+
+    /// <summary>圆形时为半径，矩形时为 X 方向长度；单位 mm。</summary>
+    public double SizeXMillimeters { get; }
+
+    /// <summary>圆形时为半径，矩形时为 Y 方向宽度；单位 mm。</summary>
+    public double SizeYMillimeters { get; }
+
+    /// <summary>创建圆形障碍物。参数：圆心和半径均使用世界 mm，半径必须为有限正数。</summary>
+    public static ManualCoarsePathObstacle Circle(double centerXMillimeters, double centerYMillimeters,
+        double radiusMillimeters)
+    {
+        EnsureFinite(centerXMillimeters, nameof(centerXMillimeters));
+        EnsureFinite(centerYMillimeters, nameof(centerYMillimeters));
+        EnsurePositiveFinite(radiusMillimeters, nameof(radiusMillimeters));
+        return new ManualCoarsePathObstacle(ManualCoarsePathObstacleKind.Circle, centerXMillimeters,
+            centerYMillimeters, radiusMillimeters, radiusMillimeters);
+    }
+
+    /// <summary>
+    /// 创建轴对齐矩形障碍物。
+    /// 参数：中心、X 方向长度和 Y 方向宽度均使用世界 mm；两个尺寸必须为有限正数。
+    /// </summary>
+    public static ManualCoarsePathObstacle AxisAlignedRectangle(double centerXMillimeters,
+        double centerYMillimeters, double lengthXMillimeters, double widthYMillimeters)
+    {
+        EnsureFinite(centerXMillimeters, nameof(centerXMillimeters));
+        EnsureFinite(centerYMillimeters, nameof(centerYMillimeters));
+        EnsurePositiveFinite(lengthXMillimeters, nameof(lengthXMillimeters));
+        EnsurePositiveFinite(widthYMillimeters, nameof(widthYMillimeters));
+        return new ManualCoarsePathObstacle(ManualCoarsePathObstacleKind.AxisAlignedRectangle,
+            centerXMillimeters, centerYMillimeters, lengthXMillimeters, widthYMillimeters);
+    }
+
+    private static void EnsureFinite(double value, string parameterName)
+    {
+        if (double.IsNaN(value) || double.IsInfinity(value))
+            throw new ArgumentOutOfRangeException(parameterName, "Value must be finite.");
+    }
+
+    private static void EnsurePositiveFinite(double value, string parameterName)
+    {
+        EnsureFinite(value, parameterName);
+        if (value <= 0d)
+            throw new ArgumentOutOfRangeException(parameterName, "Value must be positive.");
+    }
+}
+
+/// <summary>
+/// Clumsy 粗路径测试的纯输入工厂。
+/// 固定场景不读取 UI、传感器、定位或时钟；传入 AMR 位姿的手动入口仅在此处完成世界 mm/deg 到核心 m/rad 的转换。
+/// </summary>
+public static class CoarsePathScenarioFactory
+{
+    private const float MapXMinMillimeters = 0f;
+    private const float MapXMaxMillimeters = 6000f;
+    private const float MapYMinMillimeters = 0f;
+    private const float MapYMaxMillimeters = 4000f;
+    private const float ResolutionMillimeters = 50f;
+    private const double MillimetersPerMeter = 1000d;
+    private const double DegreesToRadians = Math.PI / 180d;
+    private const double ManualMapPaddingMillimeters = 8000d;
+    private const int MaximumManualObstacleCount = 20;
+
+    /// <summary>
+    /// 创建一个新的固定测试业务请求。
+    /// 返回：每次调用都返回独立的可变请求对象，供调用方安全地传入同一个长期存活的规划服务。
+    /// </summary>
+    public static CoarsePathPlanningJob Create(CoarsePathTestScenario scenario)
+    {
+        return CreateCore(scenario, null);
+    }
+
+    /// <summary>
+    /// 创建以当前 AMR 世界位姿为起点的固定测试业务请求。
+    /// 参数：X/Y 使用世界 mm，航向使用 deg；地图、目标和障碍物仅随 AMR 坐标平移，TwoLeg 朝向保持不变。
+    /// </summary>
+    public static CoarsePathPlanningJob Create(CoarsePathTestScenario scenario,
+        double amrXMillimeters, double amrYMillimeters, double amrHeadingDegrees)
+    {
+        return CreateCore(scenario,
+            new FixedScenarioAnchor(amrXMillimeters, amrYMillimeters, amrHeadingDegrees));
+    }
+
+    private static CoarsePathPlanningJob CreateCore(CoarsePathTestScenario scenario, FixedScenarioAnchor anchor)
+    {
+        switch (scenario)
+        {
+            case CoarsePathTestScenario.ExplicitEmpty:
+                return CreateExplicitEmpty(FixedScenarioTransform.From(1000d, 2000d, 0d, anchor));
+            case CoarsePathTestScenario.RectangleDetour:
+            case CoarsePathTestScenario.CacheHit:
+                return CreateRectangleDetour(FixedScenarioTransform.From(1000d, 2000d, 0d, anchor));
+            case CoarsePathTestScenario.ManualAndTwoLeg:
+                return CreateManualAndTwoLeg(FixedScenarioTransform.From(1000d, 1000d, 0d, anchor));
+            case CoarsePathTestScenario.ReverseGearSwitch:
+                return CreateReverseGearSwitch(FixedScenarioTransform.From(1000d, 2000d, 0d, anchor));
+            case CoarsePathTestScenario.NoFeasiblePath:
+                return CreateNoFeasiblePath(FixedScenarioTransform.From(1000d, 2000d, 0d, anchor));
+            default:
+                throw new ArgumentOutOfRangeException(nameof(scenario));
+        }
+    }
+
+    /// <summary>
+    /// 创建传入 AMR 世界位姿和手动世界终点的空图演示请求。
+    /// 参数：X/Y 使用世界 mm，航向使用 deg；返回请求中的 <see cref="Pose2D"/> 使用世界 m/rad。
+    /// 注意：这是坐标、路径和取消流程的演示空图，不能表示现场不存在障碍物。
+    /// </summary>
+    public static CoarsePathPlanningJob CreateManualGoalDemo(
+        double startXMillimeters, double startYMillimeters, double startHeadingDegrees,
+        double goalXMillimeters, double goalYMillimeters, double goalHeadingDegrees)
+    {
+        return CreateManualObstacleDemo(startXMillimeters, startYMillimeters, startHeadingDegrees,
+            goalXMillimeters, goalYMillimeters, goalHeadingDegrees,
+            Array.Empty<ManualCoarsePathObstacle>(), 0L);
+    }
+
+    /// <summary>
+    /// 创建传入 AMR 世界位姿、手动世界终点和手动障碍物快照的测试请求。
+    /// 参数：位姿 X/Y、障碍物中心和尺寸使用世界 mm，航向使用 deg；返回的 <see cref="Pose2D"/> 使用 m/rad。
+    /// 障碍物非空时 obstacleSnapshotVersion 必须为正数，以避免长期服务错误复用旧地图；零障碍物才创建显式空图演示。
+    /// </summary>
+    public static CoarsePathPlanningJob CreateManualObstacleDemo(
+        double startXMillimeters, double startYMillimeters, double startHeadingDegrees,
+        double goalXMillimeters, double goalYMillimeters, double goalHeadingDegrees,
+        IReadOnlyList<ManualCoarsePathObstacle> obstacles, long obstacleSnapshotVersion)
+    {
+        EnsureFinite(startXMillimeters, nameof(startXMillimeters));
+        EnsureFinite(startYMillimeters, nameof(startYMillimeters));
+        EnsureFinite(startHeadingDegrees, nameof(startHeadingDegrees));
+        EnsureFinite(goalXMillimeters, nameof(goalXMillimeters));
+        EnsureFinite(goalYMillimeters, nameof(goalYMillimeters));
+        EnsureFinite(goalHeadingDegrees, nameof(goalHeadingDegrees));
+
+        if (obstacles == null) throw new ArgumentNullException(nameof(obstacles));
+        if (obstacles.Count > MaximumManualObstacleCount)
+            throw new ArgumentOutOfRangeException(nameof(obstacles), "Manual obstacle count exceeds the supported limit.");
+        if (obstacles.Count != 0 && obstacleSnapshotVersion <= 0L)
+            throw new ArgumentOutOfRangeException(nameof(obstacleSnapshotVersion), "Obstacle snapshots require a positive version.");
+
+        return CreateJob(
+            CreateManualDemoMap(startXMillimeters, startYMillimeters, goalXMillimeters, goalYMillimeters,
+                obstacles, obstacleSnapshotVersion),
+            ToPose(startXMillimeters, startYMillimeters, startHeadingDegrees),
+            ToPose(goalXMillimeters, goalYMillimeters, goalHeadingDegrees),
+            null,
+            GoalDirectionConstraint.Any);
+    }
+
+    private static CoarsePathPlanningJob CreateExplicitEmpty(FixedScenarioTransform transform)
+    {
+        return CreateJob(
+            CreateMap(true, Array.Empty<IMapObstacleSource>(), transform),
+            transform.Pose(1d, 2d, 0d),
+            transform.Pose(5d, 2d, 0d),
+            null,
+            GoalDirectionConstraint.Forward);
+    }
+
+    private static CoarsePathPlanningJob CreateRectangleDetour(FixedScenarioTransform transform)
+    {
+        IMapObstacleSource[] sources =
+        {
+            new ManualObstacleSource("manual", 1L, true, new IMapObstacle[]
+            {
+                new AxisAlignedRectangleObstacle(transform.X(2700f), transform.X(3300f),
+                    transform.Y(1200f), transform.Y(2800f)),
+            }),
+        };
+        CoarsePathPlanningJob job = CreateJob(CreateMap(false, sources, transform), transform.Pose(1d, 2d, 0d),
+            transform.Pose(5d, 2d, 0d), null, GoalDirectionConstraint.Forward);
+        // 固定绕行场景保留最优启发式；30 秒覆盖较慢测试环境，UI 仍可随时取消。
+        job.Configuration.SearchTimeout = TimeSpan.FromSeconds(30d);
+        return job;
+    }
+
+    private static CoarsePathPlanningJob CreateManualAndTwoLeg(FixedScenarioTransform transform)
+    {
+        IMapObstacleSource[] sources =
+        {
+            new ManualObstacleSource("manual", 2L, true, new IMapObstacle[]
+            {
+                new CircleObstacle(transform.X(2400f), transform.Y(1300f), 220f),
+                new AxisAlignedRectangleObstacle(transform.X(3000f), transform.X(3600f),
+                    transform.Y(2000f), transform.Y(2600f)),
+            }),
+            new TwoLegObstacleSource("two-leg", 1L, true, new TwoLegProjectionInput(true,
+                transform.X(3900f), transform.Y(2500f), 0d,
+                    -180f, -180f, -180f, 180f, 140f, "P1 fixed TwoLeg snapshot.")),
+        };
+        CoarsePathPlanningJob job = CreateJob(CreateMap(false, sources, transform), transform.Pose(1d, 1d, 0d),
+            transform.Pose(5d, 3d, 0d), null, GoalDirectionConstraint.Forward);
+        // 多来源场景的最优绕行会受机器负载影响；放宽演示总预算但保留全部碰撞与目标判定。
+        job.Configuration.SearchTimeout = TimeSpan.FromSeconds(15d);
+        return job;
+    }
+
+    private static CoarsePathPlanningJob CreateReverseGearSwitch(FixedScenarioTransform transform)
+    {
+        return CreateJob(
+            CreateMap(true, Array.Empty<IMapObstacleSource>(), transform),
+            transform.Pose(1d, 2d, 0d),
+            transform.Pose(4d, 2d, 0d),
+            TravelDirection.Forward,
+            GoalDirectionConstraint.Reverse);
+    }
+
+    private static CoarsePathPlanningJob CreateNoFeasiblePath(FixedScenarioTransform transform)
+    {
+        IMapObstacleSource[] sources =
+        {
+            new ManualObstacleSource("manual", 3L, true, new IMapObstacle[]
+            {
+                new AxisAlignedRectangleObstacle(transform.X(2900f), transform.X(3100f),
+                    transform.Y(0f), transform.Y(4000f)),
+            }),
+        };
+        return CreateJob(CreateMap(false, sources, transform), transform.Pose(1d, 2d, 0d),
+            transform.Pose(5d, 2d, 0d),
+            null, GoalDirectionConstraint.Forward);
+    }
+
+    private static CoarsePathPlanningJob CreateJob(PlanningMapRequest mapRequest, Pose2D start, Pose2D goal,
+        TravelDirection? startDirection, GoalDirectionConstraint goalDirection)
+    {
+        return new CoarsePathPlanningJob
+        {
+            MapRequest = mapRequest,
+            Start = start,
+            Goal = goal,
+            Vehicle = new VehicleParameters
+            {
+                LengthMeters = 0.80d,
+                WidthMeters = 0.60d,
+                SafetyMarginMeters = 0.05d,
+                MaximumCurvaturePerMeter = 1d / 1.20d,
+            },
+            Configuration = new HybridAStarConfiguration(),
+            StartDirection = startDirection,
+            GoalDirection = goalDirection,
+        };
+    }
+
+    private static PlanningMapRequest CreateMap(bool allowExplicitEmptyMap, IReadOnlyList<IMapObstacleSource> sources,
+        FixedScenarioTransform transform)
+    {
+        return new PlanningMapRequest
+        {
+            Bounds = new MapBoundsMm(transform.X(MapXMinMillimeters), transform.X(MapXMaxMillimeters),
+                transform.Y(MapYMinMillimeters), transform.Y(MapYMaxMillimeters)),
+            ResolutionMm = ResolutionMillimeters,
+            ObstacleSources = sources,
+            AllowExplicitEmptyMap = allowExplicitEmptyMap,
+        };
+    }
+
+    private sealed class FixedScenarioAnchor
+    {
+        public FixedScenarioAnchor(double xMillimeters, double yMillimeters, double headingDegrees)
+        {
+            EnsureFinite(xMillimeters, nameof(xMillimeters));
+            EnsureFinite(yMillimeters, nameof(yMillimeters));
+            EnsureFinite(headingDegrees, nameof(headingDegrees));
+            XMillimeters = xMillimeters;
+            YMillimeters = yMillimeters;
+            HeadingRadians = NormalizeRadians((headingDegrees % 360d) * DegreesToRadians);
+        }
+
+        public double XMillimeters { get; }
+        public double YMillimeters { get; }
+        public double HeadingRadians { get; }
+    }
+
+    private sealed class FixedScenarioTransform
+    {
+        private FixedScenarioTransform(double deltaXMillimeters, double deltaYMillimeters, double headingDeltaRadians)
+        {
+            DeltaXMillimeters = deltaXMillimeters;
+            DeltaYMillimeters = deltaYMillimeters;
+            HeadingDeltaRadians = headingDeltaRadians;
+        }
+
+        public double DeltaXMillimeters { get; }
+        public double DeltaYMillimeters { get; }
+        public double HeadingDeltaRadians { get; }
+
+        public static FixedScenarioTransform From(double baselineStartXMillimeters,
+            double baselineStartYMillimeters, double baselineStartHeadingRadians, FixedScenarioAnchor anchor)
+        {
+            if (anchor == null) return new FixedScenarioTransform(0d, 0d, 0d);
+            return new FixedScenarioTransform(anchor.XMillimeters - baselineStartXMillimeters,
+                anchor.YMillimeters - baselineStartYMillimeters,
+                NormalizeRadians(anchor.HeadingRadians - baselineStartHeadingRadians));
+        }
+
+        public float X(float value)
+        {
+            return ToFiniteFloat(value + DeltaXMillimeters, nameof(value));
+        }
+
+        public float Y(float value)
+        {
+            return ToFiniteFloat(value + DeltaYMillimeters, nameof(value));
+        }
+
+        public Pose2D Pose(double xMeters, double yMeters, double headingRadians)
+        {
+            return new Pose2D((xMeters * MillimetersPerMeter + DeltaXMillimeters) / MillimetersPerMeter,
+                (yMeters * MillimetersPerMeter + DeltaYMillimeters) / MillimetersPerMeter,
+                NormalizeRadians(headingRadians + HeadingDeltaRadians));
+        }
+    }
+
+    private static PlanningMapRequest CreateManualDemoMap(double startXMillimeters, double startYMillimeters,
+        double goalXMillimeters, double goalYMillimeters, IReadOnlyList<ManualCoarsePathObstacle> obstacles,
+        long obstacleSnapshotVersion)
+    {
+        double minimumX = Math.Min(startXMillimeters, goalXMillimeters);
+        double maximumX = Math.Max(startXMillimeters, goalXMillimeters);
+        double minimumY = Math.Min(startYMillimeters, goalYMillimeters);
+        double maximumY = Math.Max(startYMillimeters, goalYMillimeters);
+        for (int index = 0; index < obstacles.Count; index++)
+        {
+            ManualCoarsePathObstacle obstacle = obstacles[index] ??
+                throw new ArgumentException("Manual obstacle entries cannot be null.", nameof(obstacles));
+            double halfX;
+            double halfY;
+            switch (obstacle.Kind)
+            {
+                case ManualCoarsePathObstacleKind.Circle:
+                    EnsurePositiveFinite(obstacle.SizeXMillimeters, nameof(obstacles));
+                    halfX = obstacle.SizeXMillimeters;
+                    halfY = obstacle.SizeYMillimeters;
+                    break;
+                case ManualCoarsePathObstacleKind.AxisAlignedRectangle:
+                    EnsurePositiveFinite(obstacle.SizeXMillimeters, nameof(obstacles));
+                    EnsurePositiveFinite(obstacle.SizeYMillimeters, nameof(obstacles));
+                    halfX = obstacle.SizeXMillimeters / 2d;
+                    halfY = obstacle.SizeYMillimeters / 2d;
+                    break;
+                default:
+                    throw new ArgumentOutOfRangeException(nameof(obstacles), "Manual obstacle kind is not supported.");
+            }
+
+            minimumX = Math.Min(minimumX, obstacle.CenterXMillimeters - halfX);
+            maximumX = Math.Max(maximumX, obstacle.CenterXMillimeters + halfX);
+            minimumY = Math.Min(minimumY, obstacle.CenterYMillimeters - halfY);
+            maximumY = Math.Max(maximumY, obstacle.CenterYMillimeters + halfY);
+        }
+
+        float xMin = ToGridLowerBound(minimumX - ManualMapPaddingMillimeters);
+        float xMax = ToGridUpperBound(maximumX + ManualMapPaddingMillimeters);
+        float yMin = ToGridLowerBound(minimumY - ManualMapPaddingMillimeters);
+        float yMax = ToGridUpperBound(maximumY + ManualMapPaddingMillimeters);
+        bool isExplicitEmptyMap = obstacles.Count == 0;
+        return new PlanningMapRequest
+        {
+            Bounds = new MapBoundsMm(xMin, xMax, yMin, yMax),
+            ResolutionMm = ResolutionMillimeters,
+            ObstacleSources = isExplicitEmptyMap ? Array.Empty<IMapObstacleSource>() :
+                new IMapObstacleSource[]
+                {
+                    new ManualObstacleSource("manual-user-input", obstacleSnapshotVersion, true,
+                        ConvertManualObstacles(obstacles)),
+                },
+            AllowExplicitEmptyMap = isExplicitEmptyMap,
+        };
+    }
+
+    private static IMapObstacle[] ConvertManualObstacles(IReadOnlyList<ManualCoarsePathObstacle> obstacles)
+    {
+        var result = new IMapObstacle[obstacles.Count];
+        for (int index = 0; index < obstacles.Count; index++)
+        {
+            ManualCoarsePathObstacle obstacle = obstacles[index] ??
+                throw new ArgumentException("Manual obstacle entries cannot be null.", nameof(obstacles));
+            switch (obstacle.Kind)
+            {
+                case ManualCoarsePathObstacleKind.Circle:
+                    result[index] = new CircleObstacle(ToFiniteFloat(obstacle.CenterXMillimeters, nameof(obstacles)),
+                        ToFiniteFloat(obstacle.CenterYMillimeters, nameof(obstacles)),
+                        ToFiniteFloat(obstacle.SizeXMillimeters, nameof(obstacles)));
+                    break;
+                case ManualCoarsePathObstacleKind.AxisAlignedRectangle:
+                    double halfX = obstacle.SizeXMillimeters / 2d;
+                    double halfY = obstacle.SizeYMillimeters / 2d;
+                    result[index] = new AxisAlignedRectangleObstacle(
+                        ToFiniteFloat(obstacle.CenterXMillimeters - halfX, nameof(obstacles)),
+                        ToFiniteFloat(obstacle.CenterXMillimeters + halfX, nameof(obstacles)),
+                        ToFiniteFloat(obstacle.CenterYMillimeters - halfY, nameof(obstacles)),
+                        ToFiniteFloat(obstacle.CenterYMillimeters + halfY, nameof(obstacles)));
+                    break;
+                default:
+                    throw new ArgumentOutOfRangeException(nameof(obstacles), "Manual obstacle kind is not supported.");
+            }
+        }
+        return result;
+    }
+
+    private static Pose2D ToPose(double xMillimeters, double yMillimeters, double headingDegrees)
+    {
+        return new Pose2D(xMillimeters / MillimetersPerMeter, yMillimeters / MillimetersPerMeter,
+            headingDegrees * DegreesToRadians);
+    }
+
+    private static double NormalizeRadians(double angle)
+    {
+        double normalized = angle % (2d * Math.PI);
+        if (normalized <= -Math.PI) return normalized + 2d * Math.PI;
+        return normalized > Math.PI ? normalized - 2d * Math.PI : normalized;
+    }
+
+    private static float ToGridLowerBound(double millimeters)
+    {
+        double rounded = Math.Floor(millimeters / ResolutionMillimeters) * ResolutionMillimeters;
+        return ToFiniteFloat(rounded, nameof(millimeters));
+    }
+
+    private static float ToGridUpperBound(double millimeters)
+    {
+        double rounded = Math.Ceiling(millimeters / ResolutionMillimeters) * ResolutionMillimeters;
+        return ToFiniteFloat(rounded, nameof(millimeters));
+    }
+
+    private static float ToFiniteFloat(double value, string parameterName)
+    {
+        if (double.IsNaN(value) || double.IsInfinity(value) || value < float.MinValue || value > float.MaxValue)
+            throw new ArgumentOutOfRangeException(parameterName, "Value cannot be represented as a finite millimeter coordinate.");
+        return (float)value;
+    }
+
+    private static void EnsureFinite(double value, string parameterName)
+    {
+        if (double.IsNaN(value) || double.IsInfinity(value))
+            throw new ArgumentOutOfRangeException(parameterName, "Value must be finite.");
+    }
+
+    private static void EnsurePositiveFinite(double value, string parameterName)
+    {
+        EnsureFinite(value, parameterName);
+        if (value <= 0d)
+            throw new ArgumentOutOfRangeException(parameterName, "Value must be positive.");
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Test/MovementTest.CoarsePathTest.cs

@@ -0,0 +1,628 @@
+using System;
+using System.Collections.Generic;
+using System.Drawing;
+using System.Globalization;
+using System.Threading;
+using System.Threading.Tasks;
+using ClumsyCore;
+using ClumsyCore.DTools;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using FundamentalLib;
+using MDCSToolBox.Clumsy.Pilot;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Test;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+
+namespace MultiWheelC;
+
+/// <summary>
+/// 显式空图的粗路径规划测试入口。
+/// 只负责创建纯规划请求；规划、取消与可视化均由共享执行器处理，不会向底盘发送任何命令。
+/// </summary>
+[MovementTest(name = "粗路径规划-显式空图")]
+public sealed class CoarsePathExplicitEmptyTest : MovementTest
+{
+    /// <inheritdoc />
+    public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.ExplicitEmpty, "显式空图");
+
+    /// <inheritdoc />
+    public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
+}
+
+/// <summary>
+/// 单矩形绕行的粗路径规划测试入口。
+/// </summary>
+[MovementTest(name = "粗路径规划-单矩形绕行")]
+// TODO：#在该测试下发生了红色矩形栅格碰撞但依然规划成功，需要进一步核实与确认
+public sealed class CoarsePathRectangleDetourTest : MovementTest
+{
+    /// <inheritdoc />
+    public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.RectangleDetour, "单矩形绕行");
+
+    /// <inheritdoc />
+    public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
+}
+
+/// <summary>
+/// 手工圆形、矩形和 TwoLeg 快照组合的粗路径规划测试入口。
+/// </summary>
+[MovementTest(name = "粗路径规划-多来源障碍")]
+public sealed class CoarsePathManualAndTwoLegTest : MovementTest
+{
+    /// <inheritdoc />
+    public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.ManualAndTwoLeg, "多来源障碍");
+
+    /// <inheritdoc />
+    public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
+}
+
+/// <summary>
+/// 重复输入地图缓存命中的粗路径规划测试入口。
+/// </summary>
+[MovementTest(name = "粗路径规划-缓存命中")]
+public sealed class CoarsePathCacheHitTest : MovementTest
+{
+    /// <inheritdoc />
+    public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.CacheHit, "缓存命中");
+
+    /// <inheritdoc />
+    public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
+}
+
+/// <summary>
+/// 前进起步、倒车到达并显示换向点的粗路径规划测试入口。
+/// </summary>
+[MovementTest(name = "粗路径规划-倒车换向")]
+public sealed class CoarsePathReverseGearSwitchTest : MovementTest
+{
+    /// <inheritdoc />
+    public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.ReverseGearSwitch, "倒车换向");
+
+    /// <inheritdoc />
+    public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
+}
+
+/// <summary>
+/// 障碍带完全隔开起终点的无解粗路径规划测试入口。
+/// </summary>
+[MovementTest(name = "粗路径规划-无解")]
+public sealed class CoarsePathNoFeasiblePathTest : MovementTest
+{
+    /// <inheritdoc />
+    public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.NoFeasiblePath, "无解障碍带");
+
+    /// <inheritdoc />
+    public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
+}
+
+/// <summary>
+/// 使用当前 AMR 车身几何中心位姿和人工终点的粗路径规划演示入口。
+/// 输入的 X/Y 使用世界 mm、航向使用 deg；进入规划核心前由场景工厂一次性转换为 m/rad。
+/// </summary>
+[MovementTest(name = "粗路径规划")]
+public sealed class CoarsePathPlanningTest : MovementTest
+{
+    private const int MaximumManualObstacleCount = 20;
+    private static long _nextManualObstacleSnapshotVersion;
+
+    /// <summary>
+    /// 读取一次 AMR 当前世界位姿、手动终点和障碍物快照后启动规划。
+    /// 注意：getCartLocation 在无定位时可能阻塞；全部输入会在启动后台任务前冻结，不会被规划线程重复读取。
+    /// </summary>
+    public override void Test()
+    {
+        try
+        {
+            var amrPose = DetourInterface.getCartLocation();
+            double goalXmm = ReadFiniteInput("粗路径终点 X（世界 mm）");
+            double goalYmm = ReadFiniteInput("粗路径终点 Y（世界 mm）");
+            double goalHeadingDeg = ReadFiniteInput("粗路径终点航向（世界 deg）");
+            TimeSpan searchTimeout = ReadPositiveTimeoutInput("粗路径规划总超时（秒，必须大于 0）");
+            IReadOnlyList<ManualCoarsePathObstacle> obstacles = ReadManualObstacles();
+            long snapshotVersion = obstacles.Count == 0 ? 0L :
+                Interlocked.Increment(ref _nextManualObstacleSnapshotVersion);
+            CoarsePathPlanningJob job = CoarsePathScenarioFactory.CreateManualObstacleDemo(
+                amrPose.x, amrPose.y, amrPose.th, goalXmm, goalYmm, goalHeadingDeg,
+                obstacles, snapshotVersion);
+            job.Configuration.SearchTimeout = searchTimeout;
+            CoarsePathPlanningTestRunner.Run("AMR 位姿 + 手动终点 + 手动障碍物", job);
+        }
+        catch (Exception exception)
+        {
+            CoarsePathPlanningTestRunner.ShowInputFailure(exception);
+        }
+    }
+
+    /// <inheritdoc />
+    public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
+
+    private static IReadOnlyList<ManualCoarsePathObstacle> ReadManualObstacles()
+    {
+        int count = ReadBoundedIntegerInput("手动障碍物数量（0-20）", 0, MaximumManualObstacleCount);
+        var obstacles = new List<ManualCoarsePathObstacle>(count);
+        for (int index = 0; index < count; index++)
+        {
+            string label = "障碍物 " + (index + 1);
+            int kind = ReadBoundedIntegerInput(label + " 类型（1圆形，2矩形）", 1, 2);
+            double centerXmm = ReadFiniteInput(label + " 中心 X（世界 mm）");
+            double centerYmm = ReadFiniteInput(label + " 中心 Y（世界 mm）");
+            if (kind == 1)
+            {
+                double radiusMm = ReadPositiveFiniteInput(label + " 半径 r（mm）");
+                obstacles.Add(ManualCoarsePathObstacle.Circle(centerXmm, centerYmm, radiusMm));
+            }
+            else
+            {
+                double lengthXmm = ReadPositiveFiniteInput(label + " X方向长度（mm）");
+                double widthYmm = ReadPositiveFiniteInput(label + " Y方向宽度（mm）");
+                obstacles.Add(ManualCoarsePathObstacle.AxisAlignedRectangle(centerXmm, centerYmm,
+                    lengthXmm, widthYmm));
+            }
+        }
+        return obstacles;
+    }
+
+    private static int ReadBoundedIntegerInput(string prompt, int minimum, int maximum)
+    {
+        object raw = UI.GetInput(prompt);
+        string text = Convert.ToString(raw, CultureInfo.CurrentCulture);
+        int value;
+        if (!int.TryParse(text, NumberStyles.Integer, CultureInfo.CurrentCulture, out value) &&
+            !int.TryParse(text, NumberStyles.Integer, CultureInfo.InvariantCulture, out value))
+            throw new ArgumentException("输入必须是整数：" + prompt);
+        if (value < minimum || value > maximum)
+            throw new ArgumentOutOfRangeException(nameof(prompt), "输入超出允许范围：" + prompt);
+        return value;
+    }
+
+    private static double ReadPositiveFiniteInput(string prompt)
+    {
+        double value = ReadFiniteInput(prompt);
+        if (value <= 0d)
+            throw new ArgumentOutOfRangeException(nameof(prompt), "输入必须为正数：" + prompt);
+        return value;
+    }
+
+    private static TimeSpan ReadPositiveTimeoutInput(string prompt)
+    {
+        double timeoutSeconds = ReadFiniteInput(prompt);
+        if (timeoutSeconds <= 0d)
+            throw new ArgumentOutOfRangeException(nameof(prompt), "输入必须为正数：" + prompt);
+        try
+        {
+            return TimeSpan.FromSeconds(timeoutSeconds);
+        }
+        catch (OverflowException)
+        {
+            throw new ArgumentOutOfRangeException(nameof(prompt), "输入超出允许范围：" + prompt);
+        }
+    }
+
+    private static double ReadFiniteInput(string prompt)
+    {
+        object raw = UI.GetInput(prompt);
+        string text = Convert.ToString(raw, CultureInfo.CurrentCulture);
+        double value;
+        if (!double.TryParse(text, NumberStyles.Float, CultureInfo.CurrentCulture, out value) &&
+            !double.TryParse(text, NumberStyles.Float, CultureInfo.InvariantCulture, out value))
+            throw new ArgumentException("输入必须是有限数字：" + prompt);
+        if (double.IsNaN(value) || double.IsInfinity(value))
+            throw new ArgumentException("输入必须是有限数字：" + prompt);
+        return value;
+    }
+}
+
+/// <summary>
+/// 粗路径 MovementTest 的共享后台会话、停止和绘制实现。
+/// 同一时刻只允许一个会话绘制；启动新会话或停止时会取消旧会话，但不会等待旧任务退出。
+/// </summary>
+internal static class CoarsePathPlanningTestRunner
+{
+    private const string PainterLayerName = "CoarsePathPlanningV1";
+    private const float MillimetersPerMeter = 1000f;
+    private const int MaximumVisibleGridLines = 100;
+
+    private static readonly object SessionSync = new object();
+    private static readonly CoarsePathPlanningService PlanningService = new CoarsePathPlanningService();
+    private static readonly Painter Painter = UI.GetPainter(PainterLayerName, true);
+
+    private static CancellationTokenSource _activeCancellation;
+    private static Task<CoarsePathPlanningJobResult> _activeTask;
+    private static long _nextSessionId;
+    private static long _activeSessionId;
+
+    /// <summary>
+    /// 固定场景启动时冻结的 AMR 位姿。规划后台不会重新读取定位，确保输入一致。
+    /// </summary>
+    private sealed class AmrPoseSnapshot
+    {
+        public AmrPoseSnapshot(double xMillimeters, double yMillimeters, double headingDegrees)
+        {
+            EnsureFiniteAmrValue(xMillimeters, "X");
+            EnsureFiniteAmrValue(yMillimeters, "Y");
+            EnsureFiniteAmrValue(headingDegrees, "航向");
+            XMillimeters = xMillimeters;
+            YMillimeters = yMillimeters;
+            HeadingDegrees = headingDegrees;
+        }
+
+        public double XMillimeters { get; }
+        public double YMillimeters { get; }
+        public double HeadingDegrees { get; }
+
+        public string DisplayText
+        {
+            get
+            {
+                return "AMR 起点：X=" + XMillimeters.ToString("F0", CultureInfo.InvariantCulture) +
+                    " mm，Y=" + YMillimeters.ToString("F0", CultureInfo.InvariantCulture) +
+                    " mm，航向=" + HeadingDegrees.ToString("F1", CultureInfo.InvariantCulture) + " deg";
+            }
+        }
+    }
+
+    /// <summary>
+    /// 创建指定固定场景并将其提交给共享后台服务。
+    /// </summary>
+    internal static void RunScenario(CoarsePathTestScenario scenario, string scenarioName)
+    {
+        try
+        {
+            var pose = DetourInterface.getCartLocation();
+            if (ReferenceEquals(pose, null)) throw new ArgumentException("AMR 位姿为空。");
+            var snapshot = new AmrPoseSnapshot(pose.x, pose.y, pose.th);
+            CoarsePathPlanningJob job = CoarsePathScenarioFactory.Create(scenario,
+                snapshot.XMillimeters, snapshot.YMillimeters, snapshot.HeadingDegrees);
+            Run(scenarioName, job, snapshot);
+        }
+        catch (Exception exception)
+        {
+            ShowInputFailure(new ArgumentException("AMR 位姿不可用：" + exception.Message, exception));
+        }
+    }
+
+    /// <summary>
+    /// 提交已经冻结输入的一次规划请求。调用立即返回，结果只会由对应会话的完成回调绘制。
+    /// </summary>
+    internal static void Run(string scenarioName, CoarsePathPlanningJob job)
+    {
+        Run(scenarioName, job, null);
+    }
+
+    private static void Run(string scenarioName, CoarsePathPlanningJob job, AmrPoseSnapshot amrPose)
+    {
+        if (job == null) throw new ArgumentNullException(nameof(job));
+
+        var cancellation = new CancellationTokenSource();
+        CancellationTokenSource previousCancellation;
+        long sessionId;
+        lock (SessionSync)
+        {
+            previousCancellation = _activeCancellation;
+            _activeCancellation = cancellation;
+            _activeTask = null;
+            sessionId = ++_nextSessionId;
+            _activeSessionId = sessionId;
+        }
+
+        // 先发布新会话编号，再取消旧任务，避免旧完成回调覆盖新画面。
+        if (previousCancellation != null) previousCancellation.Cancel();
+        Painter.Clear();
+        DrawPending(scenarioName, job, amrPose);
+
+        Task<CoarsePathPlanningJobResult> task = Task.Run(() => PlanningService.Plan(job, cancellation.Token));
+        lock (SessionSync)
+        {
+            if (_activeSessionId == sessionId) _activeTask = task;
+        }
+
+        _ = task.ContinueWith(completed => Finish(sessionId, scenarioName, job, amrPose, cancellation, completed),
+            TaskScheduler.Default);
+    }
+
+    /// <summary>
+    /// 取消当前会话并清空专用图层；不等待后台任务结束。
+    /// 已取消任务的完成回调只释放资源，不再记录或绘制结果。
+    /// </summary>
+    internal static void Stop()
+    {
+        CancellationTokenSource cancellation;
+        lock (SessionSync)
+        {
+            cancellation = _activeCancellation;
+            _activeCancellation = null;
+            _activeTask = null;
+            _activeSessionId = 0;
+        }
+
+        if (cancellation != null) cancellation.Cancel();
+        Painter.Clear();
+        Hedingben.ToastText("粗路径规划已请求停止。", PainterLayerName);
+    }
+
+    /// <summary>
+    /// 显示输入读取或校验失败，且不改变现有规划任务。
+    /// </summary>
+    internal static void ShowInputFailure(Exception exception)
+    {
+        string message = exception == null ? "未知输入错误。" : exception.Message;
+        Painter.DrawText(Color.LightYellow, "粗路径规划未启动：" + message, 0f, 0f);
+        Hedingben.ToastText("粗路径规划未启动：" + message, PainterLayerName);
+    }
+
+    private static void Finish(long sessionId, string scenarioName, CoarsePathPlanningJob job, AmrPoseSnapshot amrPose,
+        CancellationTokenSource cancellation, Task<CoarsePathPlanningJobResult> completed)
+    {
+        try
+        {
+            CoarsePathPlanningJobResult result = completed.GetAwaiter().GetResult();
+            bool isCurrent;
+            lock (SessionSync)
+            {
+                isCurrent = _activeSessionId == sessionId;
+                if (isCurrent)
+                {
+                    _activeTask = null;
+                    _activeCancellation = null;
+                }
+            }
+
+            if (!isCurrent) return;
+            DrawResult(scenarioName, job, amrPose, result);
+            Hedingben.ToastText(BuildToastMessage(scenarioName, result), PainterLayerName);
+        }
+        catch (Exception exception)
+        {
+            bool isCurrent;
+            lock (SessionSync)
+            {
+                isCurrent = _activeSessionId == sessionId;
+                if (isCurrent)
+                {
+                    _activeTask = null;
+                    _activeCancellation = null;
+                }
+            }
+
+            if (isCurrent)
+                Hedingben.ToastText("粗路径规划任务异常：" + exception.GetType().Name + "。" + exception.Message,
+                    PainterLayerName);
+        }
+        finally
+        {
+            cancellation.Dispose();
+        }
+    }
+
+    private static void DrawPending(string scenarioName, CoarsePathPlanningJob job, AmrPoseSnapshot amrPose)
+    {
+        DrawPose(job.Start, Color.LimeGreen, "起点");
+        DrawPose(job.Goal, Color.Orange, "终点");
+        Painter.DrawText(Color.LightGray, "场景：" + scenarioName + "（规划中）", 0f, 0f);
+        if (amrPose != null) Painter.DrawText(Color.LightGray, amrPose.DisplayText, 0f, -120f);
+    }
+
+    private static void DrawResult(string scenarioName, CoarsePathPlanningJob job, AmrPoseSnapshot amrPose,
+        CoarsePathPlanningJobResult result)
+    {
+        Painter.Clear();
+        PlanningGridMap map = result.MapResult.Map;
+        if (map != null) DrawMap(map);
+
+        DrawPose(job.Start, Color.LimeGreen, "起点");
+        DrawGoal(job.Goal, job.Configuration, Color.Orange);
+        if (result.PlanningResult.Status == PlanningStatus.Success)
+            DrawPath(result.PlanningResult, job.Vehicle);
+
+        DrawLegend(map);
+        DrawStatus(scenarioName, job, result, map, amrPose);
+    }
+
+    private static void DrawMap(PlanningGridMap map)
+    {
+        float xMin = map.Bounds.XMin;
+        float xMax = map.Bounds.XMax;
+        float yMin = map.Bounds.YMin;
+        float yMax = map.Bounds.YMax;
+        float resolution = map.ResolutionMm;
+        int gridStride = Math.Max(1, (int)Math.Ceiling(Math.Max(map.Rows, map.Cols) / (double)MaximumVisibleGridLines));
+
+        // 真实 ResolutionMm 决定网格位置，gridStride 只影响显示抽稀。
+        for (int col = 0; col <= map.Cols; col += gridStride)
+        {
+            float x = Math.Min(xMax, xMin + col * resolution);
+            Painter.DrawLine(Color.FromArgb(80, Color.SlateGray), x, yMin, x, yMax, width: 1);
+        }
+        for (int row = 0; row <= map.Rows; row += gridStride)
+        {
+            float y = Math.Min(yMax, yMin + row * resolution);
+            Painter.DrawLine(Color.FromArgb(80, Color.SlateGray), xMin, y, xMax, y, width: 1);
+        }
+
+        DrawRectangle(Color.Gainsboro, xMin, yMin, xMax, yMax, 3);
+        if (xMin <= 0f && 0f < xMax) Painter.DrawLine(Color.DimGray, 0f, yMin, 0f, yMax, width: 2);
+        if (yMin <= 0f && 0f < yMax) Painter.DrawLine(Color.DimGray, xMin, 0f, xMax, 0f, width: 2);
+
+        for (int row = 0; row < map.Rows; row++)
+        {
+            for (int col = 0; col < map.Cols; col++)
+            {
+                if (!map.IsOccupied(row, col)) continue;
+                float x = xMin + col * resolution + resolution / 2f;
+                float y = yMin + row * resolution;
+                Painter.DrawLine(Color.FromArgb(150, Color.Firebrick), x, y, x, y + resolution,
+                    width: Math.Max(1, (int)Math.Round(resolution)));
+            }
+        }
+    }
+
+    private static void DrawPose(Pose2D pose, Color color, string label)
+    {
+        if (pose == null) return;
+        float x = ToMillimeters(pose.X);
+        float y = ToMillimeters(pose.Y);
+        Painter.DrawCircle(color, x, y, 80f);
+        DrawHeadingArrow(x, y, pose.Heading, color, 260f);
+        Painter.DrawText(color, label, x + 100f, y + 100f);
+    }
+
+    private static void DrawGoal(Pose2D goal, HybridAStarConfiguration configuration, Color color)
+    {
+        DrawPose(goal, color, "终点");
+        if (goal == null || configuration == null) return;
+        Painter.DrawCircle(Color.FromArgb(150, color), ToMillimeters(goal.X), ToMillimeters(goal.Y),
+            ToMillimeters(configuration.GoalPositionToleranceMeters));
+    }
+
+    private static void DrawPath(PlanningResult planningResult, VehicleParameters vehicle)
+    {
+        if (planningResult.Path == null || planningResult.Path.Count == 0) return;
+
+        int frameStride = Math.Max(1, planningResult.Path.Count / 10);
+        for (int index = 1; index < planningResult.Path.Count; index++)
+        {
+            CoarsePathPoint previous = planningResult.Path[index - 1];
+            CoarsePathPoint current = planningResult.Path[index];
+            Color color = current.Direction == TravelDirection.Forward ? Color.LimeGreen : Color.DeepSkyBlue;
+            Painter.DrawLine(color, ToMillimeters(previous.X), ToMillimeters(previous.Y),
+                ToMillimeters(current.X), ToMillimeters(current.Y), width: 4);
+
+            if (index % frameStride == 0 || current.IsGearSwitchPoint || index == planningResult.Path.Count - 1)
+                DrawVehicleFrame(current, vehicle);
+            if (index % Math.Max(1, frameStride / 2) == 0)
+                DrawHeadingArrow(ToMillimeters(current.X), ToMillimeters(current.Y),
+                    current.Heading + (current.Direction == TravelDirection.Reverse ? Math.PI : 0d), color, 140f);
+            if (!current.IsGearSwitchPoint) continue;
+
+            float x = ToMillimeters(current.X);
+            float y = ToMillimeters(current.Y);
+            Painter.DrawCircle(Color.MediumPurple, x, y, 100f);
+            Painter.DrawText(Color.MediumPurple, "换向", x + 110f, y - 110f);
+        }
+
+        DrawVehicleFrame(planningResult.Path[0], vehicle);
+    }
+
+    private static void DrawVehicleFrame(CoarsePathPoint point, VehicleParameters vehicle)
+    {
+        if (point == null || vehicle == null) return;
+        float halfLength = ToMillimeters(vehicle.LengthMeters / 2d + vehicle.SafetyMarginMeters);
+        float halfWidth = ToMillimeters(vehicle.WidthMeters / 2d + vehicle.SafetyMarginMeters);
+        float centerX = ToMillimeters(point.X);
+        float centerY = ToMillimeters(point.Y);
+        double cos = Math.Cos(point.Heading);
+        double sin = Math.Sin(point.Heading);
+
+        TransformVehicleCorner(centerX, centerY, cos, sin, halfLength, halfWidth, out float frontLeftX, out float frontLeftY);
+        TransformVehicleCorner(centerX, centerY, cos, sin, halfLength, -halfWidth, out float frontRightX, out float frontRightY);
+        TransformVehicleCorner(centerX, centerY, cos, sin, -halfLength, -halfWidth, out float rearRightX, out float rearRightY);
+        TransformVehicleCorner(centerX, centerY, cos, sin, -halfLength, halfWidth, out float rearLeftX, out float rearLeftY);
+        Painter.DrawLine(Color.Gold, frontLeftX, frontLeftY, frontRightX, frontRightY, width: 2);
+        Painter.DrawLine(Color.Gold, frontRightX, frontRightY, rearRightX, rearRightY, width: 2);
+        Painter.DrawLine(Color.Gold, rearRightX, rearRightY, rearLeftX, rearLeftY, width: 2);
+        Painter.DrawLine(Color.Gold, rearLeftX, rearLeftY, frontLeftX, frontLeftY, width: 2);
+    }
+
+    private static void TransformVehicleCorner(float centerX, float centerY, double cos, double sin,
+        float longitudinal, float lateral, out float x, out float y)
+    {
+        x = centerX + (float)(cos * longitudinal - sin * lateral);
+        y = centerY + (float)(sin * longitudinal + cos * lateral);
+    }
+
+    private static void DrawHeadingArrow(float x, float y, double headingRadians, Color color, float length)
+    {
+        float endX = x + (float)Math.Cos(headingRadians) * length;
+        float endY = y + (float)Math.Sin(headingRadians) * length;
+        Painter.DrawLine(color, x, y, endX, endY, endArrow: true, width: 3);
+    }
+
+    private static void DrawLegend(PlanningGridMap map)
+    {
+        float x = map == null ? 0f : map.Bounds.XMin + 150f;
+        float y = map == null ? 250f : map.Bounds.YMax - 180f;
+        Painter.DrawText(Color.White, "图例", x, y);
+        DrawLegendItem(x, y - 130f, Color.Gainsboro, "边界 / 栅格");
+        DrawLegendItem(x, y - 260f, Color.Firebrick, "占据格");
+        DrawLegendItem(x, y - 390f, Color.LimeGreen, "起点 / 前进");
+        DrawLegendItem(x, y - 520f, Color.Orange, "终点 / 容差");
+        DrawLegendItem(x, y - 650f, Color.DeepSkyBlue, "倒车");
+        DrawLegendItem(x, y - 780f, Color.MediumPurple, "换向");
+        DrawLegendItem(x, y - 910f, Color.Gold, "扩大车体检查框");
+    }
+
+    private static void DrawLegendItem(float x, float y, Color color, string text)
+    {
+        Painter.DrawLine(color, x, y, x + 90f, y, width: 5);
+        Painter.DrawText(color, text, x + 120f, y - 30f);
+    }
+
+    private static void DrawStatus(string scenarioName, CoarsePathPlanningJob job,
+        CoarsePathPlanningJobResult result, PlanningGridMap map, AmrPoseSnapshot amrPose)
+    {
+        float x = map == null ? 0f : map.Bounds.XMin + 150f;
+        float y = map == null ? -250f : map.Bounds.YMin + 150f;
+        string snapshot = map == null ? "无" : map.SnapshotId.ToString(CultureInfo.InvariantCulture);
+        string resolution = map == null ? "无" : map.ResolutionMm.ToString("F0", CultureInfo.InvariantCulture) + " mm";
+        PlanningDiagnostics diagnostics = result.PlanningResult.Diagnostics;
+        string reason = diagnostics.TerminationReason ?? string.Empty;
+        string turningRadius = "无";
+        if (job != null && VehicleKinematics.TryGetMaximumCurvaturePerMeter(job.Vehicle, out double maximumCurvaturePerMeter))
+            turningRadius = (1d / maximumCurvaturePerMeter).ToString("F2", CultureInfo.InvariantCulture) + " m";
+        Painter.DrawText(Color.White, "场景：" + scenarioName, x, y);
+        float detailOffset = 0f;
+        if (amrPose != null)
+        {
+            Painter.DrawText(Color.White, amrPose.DisplayText, x, y + 120f);
+            detailOffset = 120f;
+        }
+        Painter.DrawText(Color.White, "地图：" + result.MapResult.Status + "，缓存：" + result.MapResult.CacheHit + "，快照：" + snapshot,
+            x, y + 120f + detailOffset);
+        Painter.DrawText(Color.White, "栅格：" + resolution + "，规划：" + result.PlanningResult.Status + "，总耗时：" +
+            diagnostics.Elapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) + " ms，路径搜索：" +
+            diagnostics.PathSearchElapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) + " ms", x, y + 240f + detailOffset);
+        Painter.DrawText(Color.White, "节点：扩展=" + diagnostics.ExpandedNodeCount.ToString(CultureInfo.InvariantCulture) +
+            "，生成=" + diagnostics.GeneratedNodeCount.ToString(CultureInfo.InvariantCulture) + "，Open List峰值=" +
+            diagnostics.PeakOpenListCount.ToString(CultureInfo.InvariantCulture), x, y + 360f + detailOffset);
+        if (job != null && job.Vehicle != null)
+        {
+            Painter.DrawText(Color.White, "演示车辆：长=" + job.Vehicle.LengthMeters.ToString("F2", CultureInfo.InvariantCulture) +
+                " m，宽=" + job.Vehicle.WidthMeters.ToString("F2", CultureInfo.InvariantCulture) + " m，余量=" +
+                job.Vehicle.SafetyMarginMeters.ToString("F2", CultureInfo.InvariantCulture) + " m，最小转弯半径=" +
+                turningRadius, x, y + 480f + detailOffset);
+        }
+        if (!string.IsNullOrEmpty(reason))
+            Painter.DrawText(Color.LightYellow, "原因：" + reason, x, y + 600f + detailOffset);
+    }
+
+    private static string BuildToastMessage(string scenarioName, CoarsePathPlanningJobResult result)
+    {
+        PlanningDiagnostics diagnostics = result.PlanningResult.Diagnostics;
+        string message = "粗路径[" + scenarioName + "]：地图=" + result.MapResult.Status + "，规划=" +
+            result.PlanningResult.Status + "，总耗时=" +
+            diagnostics.Elapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) + "ms，路径搜索=" +
+            diagnostics.PathSearchElapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) + "ms";
+        if (result.PlanningResult.Status != PlanningStatus.Success && !string.IsNullOrEmpty(diagnostics.TerminationReason))
+            message += "，原因=" + diagnostics.TerminationReason;
+        return message + "。";
+    }
+
+    private static void DrawRectangle(Color color, float xMin, float yMin, float xMax, float yMax, int width)
+    {
+        Painter.DrawLine(color, xMin, yMin, xMax, yMin, width: width);
+        Painter.DrawLine(color, xMax, yMin, xMax, yMax, width: width);
+        Painter.DrawLine(color, xMax, yMax, xMin, yMax, width: width);
+        Painter.DrawLine(color, xMin, yMax, xMin, yMin, width: width);
+    }
+
+    private static float ToMillimeters(double meters) => (float)(meters * MillimetersPerMeter);
+
+    private static void EnsureFiniteAmrValue(double value, string name)
+    {
+        if (double.IsNaN(value) || double.IsInfinity(value))
+            throw new ArgumentException(name + " 必须是有限数。");
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Vehicle/FootprintCollisionChecker.cs

@@ -0,0 +1,160 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+
+/// <summary>
+/// 对以车辆几何中心表示的扩大矩形执行连续碰撞检查。
+/// 地图查询使用 m；任何地图外车辆部分、占据格相交或擦边均按碰撞处理。
+/// </summary>
+public sealed class FootprintCollisionChecker
+{
+    /// <summary>创建连续车辆碰撞检查器。</summary>
+    public FootprintCollisionChecker()
+    {
+    }
+
+    /// <summary>
+    /// 判断单个车辆位姿是否无碰撞。
+    /// 参数：pose 为车辆几何中心的世界 m/rad 位姿；map 为不可变规划地图；vehicle 为车辆尺寸；
+    /// additionalMarginMeters 为临时额外安全余量，单位 m；bodyClearanceMeters 输出不含该临时余量的保守车体净空下界，单位 m。
+    /// 返回：扩大车辆矩形完整位于地图内且不与任何占据格相交或擦边时为 true；无效输入保守地返回 false。
+    /// </summary>
+    public bool IsPoseCollisionFree(Pose2D pose, PlanningGridMap map, VehicleParameters vehicle,
+        double additionalMarginMeters, out double bodyClearanceMeters)
+    {
+        bodyClearanceMeters = 0d;
+        if (map == null || !NumericGuard.IsFinite(additionalMarginMeters) || additionalMarginMeters < 0d ||
+            !VehicleFootprint.TryCreate(pose, vehicle, 0d, out VehicleFootprint bodyFootprint) ||
+            !VehicleFootprint.TryCreate(pose, vehicle, additionalMarginMeters, out VehicleFootprint checkedFootprint))
+            return false;
+
+        if (!AreCornersInsideMap(checkedFootprint, map)) return false;
+
+        double centerDistanceMeters = map.GetConservativeObstacleDistanceMeters(pose.X, pose.Y);
+        bodyClearanceMeters = GetBodyClearance(centerDistanceMeters, bodyFootprint.CircumscribedRadiusMeters);
+        if (centerDistanceMeters > checkedFootprint.CircumscribedRadiusMeters) return true;
+
+        return !IntersectsOccupiedCell(checkedFootprint, map);
+    }
+
+    /// <summary>
+    /// 判断两个位姿之间的平移和转向扫掠是否无碰撞。
+    /// 参数：from、to 为世界 m/rad 位姿；maximumCenterStepMeters 为允许的最大中心采样间距，单位 m；
+    /// minimumBodyClearanceMeters 输出沿途不含临时扫掠余量的保守车体净空下界，单位 m。
+    /// 返回：端点和每个分段扫掠均无碰撞时为 true；无效输入、地图外或任一中间碰撞时返回 false。
+    /// </summary>
+    public bool IsSweptMotionCollisionFree(Pose2D from, Pose2D to, PlanningGridMap map, VehicleParameters vehicle,
+        double maximumCenterStepMeters, out double minimumBodyClearanceMeters)
+    {
+        minimumBodyClearanceMeters = 0d;
+        if (map == null || from == null || to == null || !NumericGuard.IsFinite(maximumCenterStepMeters) || maximumCenterStepMeters <= 0d ||
+            !NumericGuard.IsFinite(from.X) || !NumericGuard.IsFinite(from.Y) || !NumericGuard.IsFinite(from.Heading) ||
+            !NumericGuard.IsFinite(to.X) || !NumericGuard.IsFinite(to.Y) || !NumericGuard.IsFinite(to.Heading) ||
+            !VehicleFootprint.TryCreate(from, vehicle, 0d, out VehicleFootprint bodyFootprint))
+            return false;
+
+        double allowedStepMeters = Math.Min(maximumCenterStepMeters, map.ResolutionMeters / 2d);
+        if (!NumericGuard.IsPositiveFinite(allowedStepMeters)) return false;
+
+        if (!IsPoseCollisionFree(from, map, vehicle, 0d, out double fromClearanceMeters)) return false;
+        minimumBodyClearanceMeters = fromClearanceMeters;
+
+        double deltaX = to.X - from.X;
+        double deltaY = to.Y - from.Y;
+        double centerDistanceMeters = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
+        if (!NumericGuard.IsFinite(centerDistanceMeters)) return false;
+        double headingDeltaRadians = AngleMath.ShortestSignedDifference(from.Heading, to.Heading);
+        if (!NumericGuard.IsFinite(headingDeltaRadians)) return false;
+        double rawSegmentCount = Math.Ceiling(centerDistanceMeters / allowedStepMeters);
+        if (!NumericGuard.IsFinite(rawSegmentCount) || rawSegmentCount > int.MaxValue) return false;
+        int segmentCount = Math.Max(1, (int)rawSegmentCount);
+
+        Pose2D previousPose = from;
+        for (int segment = 1; segment <= segmentCount; segment++)
+        {
+            double endFraction = (double)segment / segmentCount;
+            double middleFraction = ((double)segment - 0.5d) / segmentCount;
+            var currentPose = new Pose2D(
+                from.X + deltaX * endFraction,
+                from.Y + deltaY * endFraction,
+                from.Heading + headingDeltaRadians * endFraction);
+            var middlePose = new Pose2D(
+                from.X + deltaX * middleFraction,
+                from.Y + deltaY * middleFraction,
+                from.Heading + headingDeltaRadians * middleFraction);
+            double segmentDeltaX = currentPose.X - previousPose.X;
+            double segmentDeltaY = currentPose.Y - previousPose.Y;
+            double segmentCenterDisplacementMeters = Math.Sqrt(segmentDeltaX * segmentDeltaX + segmentDeltaY * segmentDeltaY);
+            double segmentHeadingDeltaRadians = currentPose.Heading - previousPose.Heading;
+            double temporaryMarginMeters = 0.5d * (segmentCenterDisplacementMeters +
+                bodyFootprint.CircumscribedRadiusMeters * Math.Abs(segmentHeadingDeltaRadians));
+            if (!NumericGuard.IsFinite(temporaryMarginMeters) ||
+                !IsPoseCollisionFree(middlePose, map, vehicle, temporaryMarginMeters, out double middleClearanceMeters))
+                return false;
+            minimumBodyClearanceMeters = Math.Min(minimumBodyClearanceMeters, middleClearanceMeters);
+            previousPose = currentPose;
+        }
+
+        if (!IsPoseCollisionFree(to, map, vehicle, 0d, out double toClearanceMeters)) return false;
+        minimumBodyClearanceMeters = Math.Min(minimumBodyClearanceMeters, toClearanceMeters);
+        return true;
+    }
+
+    private static bool AreCornersInsideMap(VehicleFootprint footprint, PlanningGridMap map)
+    {
+        for (int index = 0; index < 4; index++)
+        {
+            footprint.GetCorner(index, out double cornerX, out double cornerY);
+            if (!map.TryWorldToGrid(cornerX, cornerY, out _, out _)) return false;
+        }
+        return true;
+    }
+
+    private static double GetBodyClearance(double centerDistanceMeters, double bodyRadiusMeters)
+    {
+        if (double.IsPositiveInfinity(centerDistanceMeters)) return double.PositiveInfinity;
+        if (!NumericGuard.IsFinite(centerDistanceMeters) || !NumericGuard.IsFinite(bodyRadiusMeters)) return 0d;
+        return Math.Max(0d, centerDistanceMeters - bodyRadiusMeters);
+    }
+
+    private static bool IntersectsOccupiedCell(VehicleFootprint footprint, PlanningGridMap map)
+    {
+        GetCellRange(map, footprint.MinX, footprint.MaxX, footprint.MinY, footprint.MaxY,
+            out int firstRow, out int lastRow, out int firstCol, out int lastCol);
+        for (int row = firstRow; row <= lastRow; row++)
+        for (int col = firstCol; col <= lastCol; col++)
+        {
+            if (!map.IsOccupied(row, col)) continue;
+            GetCellBoundsMeters(map, row, col, out double cellMinX, out double cellMaxX, out double cellMinY, out double cellMaxY);
+            if (OrientedRectangleCellIntersection.Intersects(footprint, cellMinX, cellMaxX, cellMinY, cellMaxY)) return true;
+        }
+        return false;
+    }
+
+    private static void GetCellRange(PlanningGridMap map, double minX, double maxX, double minY, double maxY,
+        out int firstRow, out int lastRow, out int firstCol, out int lastCol)
+    {
+        double minimumMapX = map.Bounds.XMin / 1000d;
+        double minimumMapY = map.Bounds.YMin / 1000d;
+        firstCol = Clamp((int)Math.Floor((minX - minimumMapX) / map.ResolutionMeters) - 1, 0, map.Cols - 1);
+        lastCol = Clamp((int)Math.Floor((maxX - minimumMapX) / map.ResolutionMeters), 0, map.Cols - 1);
+        firstRow = Clamp((int)Math.Floor((minY - minimumMapY) / map.ResolutionMeters) - 1, 0, map.Rows - 1);
+        lastRow = Clamp((int)Math.Floor((maxY - minimumMapY) / map.ResolutionMeters), 0, map.Rows - 1);
+    }
+
+    private static void GetCellBoundsMeters(PlanningGridMap map, int row, int col,
+        out double cellMinX, out double cellMaxX, out double cellMinY, out double cellMaxY)
+    {
+        cellMinX = map.Bounds.XMin / 1000d + col * map.ResolutionMeters;
+        cellMinY = map.Bounds.YMin / 1000d + row * map.ResolutionMeters;
+        cellMaxX = Math.Min(map.Bounds.XMax / 1000d, cellMinX + map.ResolutionMeters);
+        cellMaxY = Math.Min(map.Bounds.YMax / 1000d, cellMinY + map.ResolutionMeters);
+    }
+
+    private static int Clamp(int value, int minimum, int maximum)
+    {
+        return value < minimum ? minimum : value > maximum ? maximum : value;
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Vehicle/OrientedRectangleCellIntersection.cs

@@ -0,0 +1,33 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+
+/// <summary>旋转矩形与轴对齐栅格的分离轴相交判定。</summary>
+internal static class OrientedRectangleCellIntersection
+{
+    /// <summary>任一投影轴没有严格分离时返回 true；擦边按相交处理。</summary>
+    public static bool Intersects(VehicleFootprint rectangle, double cellMinX, double cellMaxX, double cellMinY, double cellMaxY)
+    {
+        if (rectangle == null || cellMaxX < cellMinX || cellMaxY < cellMinY) return false;
+        double cellCenterX = (cellMinX + cellMaxX) / 2d;
+        double cellCenterY = (cellMinY + cellMaxY) / 2d;
+        double cellHalfX = (cellMaxX - cellMinX) / 2d;
+        double cellHalfY = (cellMaxY - cellMinY) / 2d;
+
+        return !HasStrictSeparation(rectangle, cellCenterX, cellCenterY, cellHalfX, cellHalfY, rectangle.AxisLongitudinalX, rectangle.AxisLongitudinalY) &&
+            !HasStrictSeparation(rectangle, cellCenterX, cellCenterY, cellHalfX, cellHalfY, rectangle.AxisLateralX, rectangle.AxisLateralY) &&
+            !HasStrictSeparation(rectangle, cellCenterX, cellCenterY, cellHalfX, cellHalfY, 1d, 0d) &&
+            !HasStrictSeparation(rectangle, cellCenterX, cellCenterY, cellHalfX, cellHalfY, 0d, 1d);
+    }
+
+    private static bool HasStrictSeparation(VehicleFootprint rectangle, double cellCenterX, double cellCenterY, double cellHalfX, double cellHalfY,
+        double axisX, double axisY)
+    {
+        double rectangleCenter = rectangle.CenterX * axisX + rectangle.CenterY * axisY;
+        double cellCenter = cellCenterX * axisX + cellCenterY * axisY;
+        double rectangleRadius = rectangle.HalfLengthMeters * Math.Abs(rectangle.AxisLongitudinalX * axisX + rectangle.AxisLongitudinalY * axisY) +
+            rectangle.HalfWidthMeters * Math.Abs(rectangle.AxisLateralX * axisX + rectangle.AxisLateralY * axisY);
+        double cellRadius = cellHalfX * Math.Abs(axisX) + cellHalfY * Math.Abs(axisY);
+        return Math.Abs(rectangleCenter - cellCenter) > rectangleRadius + cellRadius;
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Vehicle/VehicleFootprint.cs

@@ -0,0 +1,90 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+
+/// <summary>以车辆几何中心为原点的扩大旋转矩形。</summary>
+internal sealed class VehicleFootprint
+{
+    private VehicleFootprint(Pose2D pose, double halfLengthMeters, double halfWidthMeters)
+    {
+        CenterX = pose.X;
+        CenterY = pose.Y;
+        HalfLengthMeters = halfLengthMeters;
+        HalfWidthMeters = halfWidthMeters;
+        AxisLongitudinalX = Math.Cos(pose.Heading);
+        AxisLongitudinalY = Math.Sin(pose.Heading);
+        AxisLateralX = -AxisLongitudinalY;
+        AxisLateralY = AxisLongitudinalX;
+        CircumscribedRadiusMeters = Math.Sqrt(halfLengthMeters * halfLengthMeters + halfWidthMeters * halfWidthMeters);
+
+        double minX = double.PositiveInfinity;
+        double maxX = double.NegativeInfinity;
+        double minY = double.PositiveInfinity;
+        double maxY = double.NegativeInfinity;
+        for (int index = 0; index < 4; index++)
+        {
+            GetCorner(index, out double x, out double y);
+            minX = Math.Min(minX, x);
+            maxX = Math.Max(maxX, x);
+            minY = Math.Min(minY, y);
+            maxY = Math.Max(maxY, y);
+        }
+        MinX = minX;
+        MaxX = maxX;
+        MinY = minY;
+        MaxY = maxY;
+    }
+
+    public double CenterX { get; }
+    public double CenterY { get; }
+    public double HalfLengthMeters { get; }
+    public double HalfWidthMeters { get; }
+    public double AxisLongitudinalX { get; }
+    public double AxisLongitudinalY { get; }
+    public double AxisLateralX { get; }
+    public double AxisLateralY { get; }
+    public double CircumscribedRadiusMeters { get; }
+    public double MinX { get; }
+    public double MaxX { get; }
+    public double MinY { get; }
+    public double MaxY { get; }
+
+    /// <summary>创建包含车辆安全余量和临时扫掠余量的矩形。</summary>
+    public static bool TryCreate(Pose2D pose, VehicleParameters vehicle, double additionalMarginMeters, out VehicleFootprint footprint)
+    {
+        footprint = null;
+        if (pose == null || vehicle == null || !NumericGuard.IsFinite(pose.X) || !NumericGuard.IsFinite(pose.Y) ||
+            !NumericGuard.IsFinite(pose.Heading) || !NumericGuard.IsPositiveFinite(vehicle.LengthMeters) ||
+            !NumericGuard.IsPositiveFinite(vehicle.WidthMeters) || !NumericGuard.IsFinite(vehicle.SafetyMarginMeters) ||
+            vehicle.SafetyMarginMeters < 0d || !NumericGuard.IsFinite(additionalMarginMeters) || additionalMarginMeters < 0d)
+            return false;
+
+        double totalMarginMeters = vehicle.SafetyMarginMeters + additionalMarginMeters;
+        if (!NumericGuard.IsFinite(totalMarginMeters)) return false;
+        double halfLengthMeters = vehicle.LengthMeters / 2d + totalMarginMeters;
+        double halfWidthMeters = vehicle.WidthMeters / 2d + totalMarginMeters;
+        if (!NumericGuard.IsPositiveFinite(halfLengthMeters) || !NumericGuard.IsPositiveFinite(halfWidthMeters)) return false;
+
+        footprint = new VehicleFootprint(pose, halfLengthMeters, halfWidthMeters);
+        return NumericGuard.IsFinite(footprint.CircumscribedRadiusMeters) && NumericGuard.IsFinite(footprint.MinX) &&
+            NumericGuard.IsFinite(footprint.MaxX) && NumericGuard.IsFinite(footprint.MinY) && NumericGuard.IsFinite(footprint.MaxY);
+    }
+
+    /// <summary>获取指定角点。索引按逆时针顺序为 0 到 3。</summary>
+    public void GetCorner(int index, out double x, out double y)
+    {
+        double longitudinalSign;
+        double lateralSign;
+        switch (index)
+        {
+            case 0: longitudinalSign = 1d; lateralSign = 1d; break;
+            case 1: longitudinalSign = -1d; lateralSign = 1d; break;
+            case 2: longitudinalSign = -1d; lateralSign = -1d; break;
+            case 3: longitudinalSign = 1d; lateralSign = -1d; break;
+            default: throw new ArgumentOutOfRangeException(nameof(index));
+        }
+        x = CenterX + longitudinalSign * HalfLengthMeters * AxisLongitudinalX + lateralSign * HalfWidthMeters * AxisLateralX;
+        y = CenterY + longitudinalSign * HalfLengthMeters * AxisLongitudinalY + lateralSign * HalfWidthMeters * AxisLateralY;
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Vehicle/VehicleKinematics.cs

@@ -0,0 +1,40 @@
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+
+/// <summary>
+/// 从车辆参数提取运动学限制的辅助方法。
+/// 曲率单位为 1/m；当最大曲率与最小转弯半径同时给出时，始终选择更保守的较小曲率。
+/// </summary>
+public static class VehicleKinematics
+{
+    /// <summary>
+    /// 尝试获取车辆允许的最大绝对曲率。
+    /// 参数：vehicle 为车辆参数；maximumCurvaturePerMeter 为输出的正有限曲率，单位 1/m。
+    /// 返回：至少提供一种正有限曲率限制时为 true；任一已提供限制无效或两种限制均未提供时为 false。
+    /// </summary>
+    public static bool TryGetMaximumCurvaturePerMeter(VehicleParameters vehicle, out double maximumCurvaturePerMeter)
+    {
+        maximumCurvaturePerMeter = 0d;
+        if (vehicle == null) return false;
+
+        bool hasMaximumCurvature = vehicle.MaximumCurvaturePerMeter.HasValue;
+        bool hasMinimumRadius = vehicle.MinimumTurningRadiusMeters.HasValue;
+        if (hasMaximumCurvature && !NumericGuard.IsPositiveFinite(vehicle.MaximumCurvaturePerMeter.Value)) return false;
+        if (hasMinimumRadius && !NumericGuard.IsPositiveFinite(vehicle.MinimumTurningRadiusMeters.Value)) return false;
+        if (!hasMaximumCurvature && !hasMinimumRadius) return false;
+
+        if (hasMaximumCurvature && hasMinimumRadius)
+        {
+            maximumCurvaturePerMeter = System.Math.Min(
+                vehicle.MaximumCurvaturePerMeter.Value,
+                1d / vehicle.MinimumTurningRadiusMeters.Value);
+            return true;
+        }
+
+        maximumCurvaturePerMeter = hasMaximumCurvature
+            ? vehicle.MaximumCurvaturePerMeter.Value
+            : 1d / vehicle.MinimumTurningRadiusMeters.Value;
+        return NumericGuard.IsPositiveFinite(maximumCurvaturePerMeter);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/CorridorConfiguration.cs

@@ -0,0 +1,43 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 静态走廊采样、偏移和净空配置；距离单位均为 m。
+/// </summary>
+public sealed class CorridorConfiguration
+{
+    /// <summary>
+    /// 沿参考弧长的走廊采样间距；默认值 0.10 m，必须为正有限值，见 <see cref="EmPlannerConfiguration.CreateDefault"/>。
+    /// </summary>
+    public double LongitudinalSampleSpacingMeters { get; set; }
+    /// <summary>
+    /// 每个采样站横向搜索的间距；默认值 0.025 m，必须为正有限值，见 <see cref="EmPlannerConfiguration.CreateDefault"/>。
+    /// </summary>
+    public double LateralSampleSpacingMeters { get; set; }
+    /// <summary>
+    /// 相对参考线允许搜索的最大横向偏移；单位 m，默认值 0.30，必须为正有限值。
+    /// </summary>
+    public double MaximumLateralOffsetMeters { get; set; }
+    /// <summary>
+    /// 碰撞检测之外预留的附加净空；单位 m，默认值 0.02，必须为非负有限值。
+    /// </summary>
+    public double AdditionalClearanceReserveMeters { get; set; }
+    /// <summary>
+    /// 足迹碰撞检测的最大行进步长；单位 m，默认值 0.025，必须为正有限值。
+    /// </summary>
+    public double MaximumCollisionCheckStepMeters { get; set; }
+
+    /// <summary>
+    /// 复制当前走廊配置；输入为当前五个标量，输出为无共享可变状态的可修改快照，不对数值作校验且不产生失败状态。
+    /// </summary>
+    internal CorridorConfiguration Copy()
+    {
+        return new CorridorConfiguration
+        {
+            LongitudinalSampleSpacingMeters = LongitudinalSampleSpacingMeters,
+            LateralSampleSpacingMeters = LateralSampleSpacingMeters,
+            MaximumLateralOffsetMeters = MaximumLateralOffsetMeters,
+            AdditionalClearanceReserveMeters = AdditionalClearanceReserveMeters,
+            MaximumCollisionCheckStepMeters = MaximumCollisionCheckStepMeters,
+        };
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/EmPlannerConfiguration.cs

@@ -0,0 +1,149 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// EM 规划器的完整可变配置树；所有子配置均须非空，建议通过 <see cref="CreateDefault"/> 创建协调且彼此独立的默认快照。
+/// </summary>
+public sealed partial class EmPlannerConfiguration
+{
+    /// <summary>
+    /// 重规划节奏、时间窗口和资源上限配置；无直接单位，必须为非空的 <see cref="SchedulingConfiguration"/>，默认值为新的调度配置对象。
+    /// </summary>
+    public SchedulingConfiguration Scheduling { get; set; }
+    /// <summary>
+    /// 世界坐标至 Frenet 坐标的投影容差配置；无直接单位，必须为非空的 <see cref="FrenetConfiguration"/>，默认值为新的 Frenet 配置对象。
+    /// </summary>
+    public FrenetConfiguration Frenet { get; set; }
+    /// <summary>
+    /// 静态无碰撞走廊的采样与净空配置；无直接单位，必须为非空的 <see cref="CorridorConfiguration"/>，默认值为新的走廊配置对象。
+    /// </summary>
+    public CorridorConfiguration Corridor { get; set; }
+    /// <summary>
+    /// LS 横向优化的约束与权重配置；无直接单位，必须为非空的 <see cref="LateralConfiguration"/>，默认值为新的横向配置对象。
+    /// </summary>
+    public LateralConfiguration Lateral { get; set; }
+    /// <summary>
+    /// ST 纵向优化的速度、舒适性与权重配置；无直接单位，必须为非空的 <see cref="LongitudinalConfiguration"/>，默认值为新的纵向配置对象。
+    /// </summary>
+    public LongitudinalConfiguration Longitudinal { get; set; }
+    /// <summary>
+    /// QP 求解器的迭代次数、容差和运行选项；无直接单位，必须为非空的 <see cref="SolverConfiguration"/>，默认值为新的求解器配置对象。
+    /// </summary>
+    public SolverConfiguration Solver { get; set; }
+    /// <summary>
+    /// 发布前几何、运动学和终端姿态校验容差；无直接单位，必须为非空的 <see cref="ValidationConfiguration"/>，默认值为新的校验配置对象。
+    /// </summary>
+    public ValidationConfiguration Validation { get; set; }
+
+    /// <summary>
+    /// 创建用于低速泊车的全套默认配置；每次调用均构造新的子配置和权重对象，因此调用方可修改返回值而不影响其他默认快照。
+    /// </summary>
+    public static EmPlannerConfiguration CreateDefault()
+    {
+        return new EmPlannerConfiguration
+        {
+            Scheduling = new SchedulingConfiguration
+            {
+                ReplanPeriodSeconds = 0.20d,
+                TimeHorizonSeconds = 6d,                // 单次规划的时间窗口，单位 s。
+                DistanceHorizonMeters = 500d,           // 单次规划的最大前向探索距离，单位 m。
+                OutputTimeStepSeconds = 0.05d,
+                SolverTimeoutSeconds = 0.10d,
+                HandoffLookaheadSeconds = 0.30d,
+                MaximumVehicleStateAgeSeconds = 0.20d,
+                MaximumOptimizationTimeStepSeconds = 0.20d,
+                MaximumOptimizationSpatialStepMeters = 0.10d,
+                MaximumOptimizationKnotCount = 401,
+                MaximumPublishedSampleCount = 5001,
+            },
+            Corridor = new CorridorConfiguration
+            {
+                LongitudinalSampleSpacingMeters = 0.10d,
+                LateralSampleSpacingMeters = 0.025d,
+                MaximumLateralOffsetMeters = 0.30d,
+                AdditionalClearanceReserveMeters = 0.02d,
+                MaximumCollisionCheckStepMeters = 0.025d,
+            },
+            Frenet = new FrenetConfiguration
+            {
+                MaximumProjectionDistanceMeters = 0.50d,
+                MinimumFrenetDenominator = 0.20d,
+                BoundaryAnchorToleranceMeters = 1e-8d,
+            },
+            Lateral = new LateralConfiguration
+            {
+                MaximumLateralStepPerIterationMeters = 0.05d,
+                MaximumLateralSlope = 0.50d,
+                MaximumLateralSecondDerivativePerMeter = 1d,
+                MaximumLateralThirdDerivativePerSquareMeter = 2d,
+                Weights = new LateralWeights
+                {
+                    ReferenceOffset = 10d,
+                    HeadingDeviation = 1d,
+                    SecondDerivative = 5d,
+                    ThirdDerivative = 10d,
+                    Curvature = 5d,
+                    CurvatureVariation = 20d,
+                    PreviousTrajectory = 5d,
+                    RollingTerminal = 10d,
+                },
+            },
+            Longitudinal = new LongitudinalConfiguration
+            {
+                MaximumForwardSpeedMetersPerSecond = 1d,
+                MaximumReverseSpeedMetersPerSecond = 0.5d,
+                DesiredForwardSpeedMetersPerSecond = 1d,
+                DesiredReverseSpeedMetersPerSecond = 0.5d,
+                MaximumAccelerationMetersPerSecondSquared = 0.20d,
+                MaximumDecelerationMetersPerSecondSquared = 0.30d,
+                MaximumJerkMetersPerSecondCubed = 0.50d,
+                MaximumLateralAccelerationMetersPerSecondSquared = 0.20d,
+                MaximumCurvatureRatePerMeterPerSecond = 0.50d,
+                StopSpeedToleranceMetersPerSecond = 0.01d,
+                ZeroSpeedHoldSeconds = 0.20d,
+                Weights = new LongitudinalWeights
+                {
+                    ReferenceSpeed = 10d,
+                    Acceleration = 1d,
+                    Jerk = 10d,
+                    PreviousTrajectory = 5d,
+                    TerminalAcceleration = 1d,
+                },
+            },
+            Solver = new SolverConfiguration
+            {
+                MaximumOuterIterations = 5,
+                MaximumOsqpIterations = 4000,
+                AbsoluteTolerance = 1e-5d,
+                RelativeTolerance = 1e-5d,
+                StrictResidualTolerance = 1e-5d,
+                WarmStart = true,
+                Polish = true,
+                NativeVerbose = false,
+            },
+            Validation = new ValidationConfiguration
+            {
+                SpatialToleranceMeters = 1e-8d,
+                KinematicTolerance = 1e-5d,
+                TerminalPositionToleranceMeters = 0d,
+                TerminalYawToleranceRadians = 0d,
+            },
+        };
+    }
+
+    /// <summary>
+    /// 深复制当前配置树；输入为当前实例状态，输出与当前实例不共享非空嵌套对象的可修改快照，原本为 <c>null</c> 的子配置保持为 <c>null</c> 且不产生失败状态。
+    /// </summary>
+    internal EmPlannerConfiguration Copy()
+    {
+        return new EmPlannerConfiguration
+        {
+            Scheduling = Scheduling == null ? null : Scheduling.Copy(),
+            Frenet = Frenet == null ? null : Frenet.Copy(),
+            Corridor = Corridor == null ? null : Corridor.Copy(),
+            Lateral = Lateral == null ? null : Lateral.Copy(),
+            Longitudinal = Longitudinal == null ? null : Longitudinal.Copy(),
+            Solver = Solver == null ? null : Solver.Copy(),
+            Validation = Validation == null ? null : Validation.Copy(),
+        };
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/FrenetConfiguration.cs

@@ -0,0 +1,33 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 世界坐标与 Frenet 参考之间投影和边界判定的容差配置。
+/// </summary>
+public sealed class FrenetConfiguration
+{
+    /// <summary>
+    /// 世界姿态投影到方向段所允许的最大欧氏距离；单位 m，默认值 0.50，必须为正有限值。
+    /// </summary>
+    public double MaximumProjectionDistanceMeters { get; set; }
+    /// <summary>
+    /// Frenet 重建中 <c>1-kappa*l</c> 的最小绝对安全余量；无量纲，默认值 0.20，必须为大于 0 且小于 1 的有限值。
+    /// </summary>
+    public double MinimumFrenetDenominator { get; set; }
+    /// <summary>
+    /// 判断投影是否锚定在段边界的距离容差；单位 m，默认值 1e-8，必须为正有限值。
+    /// </summary>
+    public double BoundaryAnchorToleranceMeters { get; set; }
+
+    /// <summary>
+    /// 复制当前 Frenet 配置；输入为当前三个标量，输出为无共享可变状态的标量快照，不对数值作校验且不产生失败状态。
+    /// </summary>
+    internal FrenetConfiguration Copy()
+    {
+        return new FrenetConfiguration
+        {
+            MaximumProjectionDistanceMeters = MaximumProjectionDistanceMeters,
+            MinimumFrenetDenominator = MinimumFrenetDenominator,
+            BoundaryAnchorToleranceMeters = BoundaryAnchorToleranceMeters,
+        };
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/LateralConfiguration.cs

@@ -0,0 +1,43 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// LS 横向优化的离散、收敛、曲率和边界限制配置。
+/// </summary>
+public sealed class LateralConfiguration
+{
+    /// <summary>
+    /// 相邻外层迭代横向解之间允许的最大偏移量；单位 m，默认值 0.05，必须为正有限值。
+    /// </summary>
+    public double MaximumLateralStepPerIterationMeters { get; set; }
+    /// <summary>
+    /// 横向偏移对弧长的一阶导数上限；无量纲，默认值 0.50，必须为正有限值。
+    /// </summary>
+    public double MaximumLateralSlope { get; set; }
+    /// <summary>
+    /// 横向偏移对弧长的二阶导数上限；单位 1/m，默认值 1，必须为正有限值。
+    /// </summary>
+    public double MaximumLateralSecondDerivativePerMeter { get; set; }
+    /// <summary>
+    /// 横向偏移对弧长的三阶导数上限；单位 1/m²，默认值 2，必须为正有限值。
+    /// </summary>
+    public double MaximumLateralThirdDerivativePerSquareMeter { get; set; }
+    /// <summary>
+    /// 横向 QP 目标函数的权重集合；无直接单位，必须非空且其每项为非负有限值，默认值为新的 <see cref="LateralWeights"/> 对象。
+    /// </summary>
+    public LateralWeights Weights { get; set; }
+
+    /// <summary>
+    /// 复制当前横向配置；输入为当前标量和权重引用，输出为不共享非空权重对象的可修改快照，权重为 <c>null</c> 时保持为空且不产生失败状态。
+    /// </summary>
+    internal LateralConfiguration Copy()
+    {
+        return new LateralConfiguration
+        {
+            MaximumLateralStepPerIterationMeters = MaximumLateralStepPerIterationMeters,
+            MaximumLateralSlope = MaximumLateralSlope,
+            MaximumLateralSecondDerivativePerMeter = MaximumLateralSecondDerivativePerMeter,
+            MaximumLateralThirdDerivativePerSquareMeter = MaximumLateralThirdDerivativePerSquareMeter,
+            Weights = Weights == null ? null : Weights.Copy(),
+        };
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/LateralWeights.cs

@@ -0,0 +1,58 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 横向 QP 目标函数的权重快照；权重只影响偏好，不放宽安全约束。
+/// </summary>
+public sealed class LateralWeights
+{
+    /// <summary>
+    /// 惩罚偏离参考线横向偏移的权重；无量纲，默认值 10，必须为非负有限值。
+    /// </summary>
+    public double ReferenceOffset { get; set; }
+    /// <summary>
+    /// 惩罚横向偏移一阶导数的权重；无量纲，默认值 1，必须为非负有限值。
+    /// </summary>
+    public double HeadingDeviation { get; set; }
+    /// <summary>
+    /// 惩罚横向偏移二阶导数的权重；无量纲，默认值 5，必须为非负有限值。
+    /// </summary>
+    public double SecondDerivative { get; set; }
+    /// <summary>
+    /// 惩罚横向偏移三阶导数的权重；无量纲，默认值 10，必须为非负有限值。
+    /// </summary>
+    public double ThirdDerivative { get; set; }
+    /// <summary>
+    /// 惩罚由横向解导出的车辆曲率的权重；无量纲，默认值 5，必须为非负有限值。
+    /// </summary>
+    public double Curvature { get; set; }
+    /// <summary>
+    /// 惩罚相邻站曲率变化的权重；无量纲，默认值 20，必须为非负有限值。
+    /// </summary>
+    public double CurvatureVariation { get; set; }
+    /// <summary>
+    /// 惩罚偏离上一轮横向轨迹的权重；无量纲，默认值 5，必须为非负有限值。
+    /// </summary>
+    public double PreviousTrajectory { get; set; }
+    /// <summary>
+    /// 滚动规划末端横向状态的稳定权重；无量纲，默认值 10，必须为非负有限值。
+    /// </summary>
+    public double RollingTerminal { get; set; }
+
+    /// <summary>
+    /// 复制当前横向权重；输入为当前八个标量，输出为无共享可变状态的标量快照，不对数值作校验且不产生失败状态。
+    /// </summary>
+    internal LateralWeights Copy()
+    {
+        return new LateralWeights
+        {
+            ReferenceOffset = ReferenceOffset,
+            HeadingDeviation = HeadingDeviation,
+            SecondDerivative = SecondDerivative,
+            ThirdDerivative = ThirdDerivative,
+            Curvature = Curvature,
+            CurvatureVariation = CurvatureVariation,
+            PreviousTrajectory = PreviousTrajectory,
+            RollingTerminal = RollingTerminal,
+        };
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/LongitudinalConfiguration.cs

@@ -0,0 +1,78 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// ST 纵向优化的速度、舒适性和终端保持限制配置；所有数值由 <see cref="EmPlannerConfiguration.CreateDefault"/> 提供低速泊车默认值。
+/// </summary>
+public sealed class LongitudinalConfiguration
+{
+    /// <summary>
+    /// 前进方向允许的最大速度；单位 m/s，默认值 1，必须为正有限值。
+    /// </summary>
+    public double MaximumForwardSpeedMetersPerSecond { get; set; }
+    /// <summary>
+    /// 倒车方向允许的最大速度；单位 m/s，默认值 0.5，必须为正有限值。
+    /// </summary>
+    public double MaximumReverseSpeedMetersPerSecond { get; set; }
+    /// <summary>
+    /// 前进方向的目标巡航速度；单位 m/s，默认值 1，必须为正有限值且不得超过 <see cref="MaximumForwardSpeedMetersPerSecond"/>。
+    /// </summary>
+    public double DesiredForwardSpeedMetersPerSecond { get; set; }
+    /// <summary>
+    /// 倒车方向的目标巡航速度；单位 m/s，默认值 0.5，必须为正有限值且不得超过 <see cref="MaximumReverseSpeedMetersPerSecond"/>。
+    /// </summary>
+    public double DesiredReverseSpeedMetersPerSecond { get; set; }
+    /// <summary>
+    /// 允许的最大纵向加速度；单位 m/s²，默认值 0.20，必须为正有限值。
+    /// </summary>
+    public double MaximumAccelerationMetersPerSecondSquared { get; set; }
+    /// <summary>
+    /// 允许的最大纵向减速度幅值；单位 m/s²，默认值 0.30，必须为正有限值。
+    /// </summary>
+    public double MaximumDecelerationMetersPerSecondSquared { get; set; }
+    /// <summary>
+    /// 允许的最大纵向加加速度幅值；单位 m/s³，默认值 0.50，必须为正有限值。
+    /// </summary>
+    public double MaximumJerkMetersPerSecondCubed { get; set; }
+    /// <summary>
+    /// 由速度和曲率共同施加的最大横向加速度；单位 m/s²，默认值 0.20，必须为正有限值。
+    /// </summary>
+    public double MaximumLateralAccelerationMetersPerSecondSquared { get; set; }
+    /// <summary>
+    /// 车辆曲率随时间变化的上限；单位 1/(m·s)，默认值 0.50，必须为正有限值。
+    /// </summary>
+    public double MaximumCurvatureRatePerMeterPerSecond { get; set; }
+    /// <summary>
+    /// 将进度速度视为停止的容差；单位 m/s，默认值 0.01，必须为非负有限值。
+    /// </summary>
+    public double StopSpeedToleranceMetersPerSecond { get; set; }
+    /// <summary>
+    /// 到达零速度后需保持的时长；单位 s，默认值 0.20，必须为正有限值。
+    /// </summary>
+    public double ZeroSpeedHoldSeconds { get; set; }
+    /// <summary>
+    /// 纵向 QP 目标函数的权重集合；无直接单位，必须非空且其每项为非负有限值，默认值为新的 <see cref="LongitudinalWeights"/> 对象。
+    /// </summary>
+    public LongitudinalWeights Weights { get; set; }
+
+    /// <summary>
+    /// 复制当前纵向配置；输入为当前标量和权重引用，输出为不共享非空权重对象的可修改快照，权重为 <c>null</c> 时保持为空且不产生失败状态。
+    /// </summary>
+    internal LongitudinalConfiguration Copy()
+    {
+        return new LongitudinalConfiguration
+        {
+            MaximumForwardSpeedMetersPerSecond = MaximumForwardSpeedMetersPerSecond,
+            MaximumReverseSpeedMetersPerSecond = MaximumReverseSpeedMetersPerSecond,
+            DesiredForwardSpeedMetersPerSecond = DesiredForwardSpeedMetersPerSecond,
+            DesiredReverseSpeedMetersPerSecond = DesiredReverseSpeedMetersPerSecond,
+            MaximumAccelerationMetersPerSecondSquared = MaximumAccelerationMetersPerSecondSquared,
+            MaximumDecelerationMetersPerSecondSquared = MaximumDecelerationMetersPerSecondSquared,
+            MaximumJerkMetersPerSecondCubed = MaximumJerkMetersPerSecondCubed,
+            MaximumLateralAccelerationMetersPerSecondSquared = MaximumLateralAccelerationMetersPerSecondSquared,
+            MaximumCurvatureRatePerMeterPerSecond = MaximumCurvatureRatePerMeterPerSecond,
+            StopSpeedToleranceMetersPerSecond = StopSpeedToleranceMetersPerSecond,
+            ZeroSpeedHoldSeconds = ZeroSpeedHoldSeconds,
+            Weights = Weights == null ? null : Weights.Copy(),
+        };
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/LongitudinalWeights.cs

@@ -0,0 +1,43 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 纵向 QP 目标函数的权重快照；权重只改变偏好而不放宽硬约束。
+/// </summary>
+public sealed class LongitudinalWeights
+{
+    /// <summary>
+    /// 惩罚偏离参考速度的权重；无量纲，默认值 10，必须为非负有限值。
+    /// </summary>
+    public double ReferenceSpeed { get; set; }
+    /// <summary>
+    /// 惩罚纵向加速度的权重；无量纲，默认值 1，必须为非负有限值。
+    /// </summary>
+    public double Acceleration { get; set; }
+    /// <summary>
+    /// 惩罚纵向加加速度的权重；无量纲，默认值 10，必须为非负有限值。
+    /// </summary>
+    public double Jerk { get; set; }
+    /// <summary>
+    /// 惩罚偏离上一条轨迹速度解的权重；无量纲，默认值 5，必须为非负有限值。
+    /// </summary>
+    public double PreviousTrajectory { get; set; }
+    /// <summary>
+    /// 惩罚末端加速度的权重；无量纲，默认值 1，必须为非负有限值。
+    /// </summary>
+    public double TerminalAcceleration { get; set; }
+
+    /// <summary>
+    /// 复制当前纵向权重；输入为当前五个标量，输出为无共享可变状态的标量快照，不对数值作校验且不产生失败状态。
+    /// </summary>
+    internal LongitudinalWeights Copy()
+    {
+        return new LongitudinalWeights
+        {
+            ReferenceSpeed = ReferenceSpeed,
+            Acceleration = Acceleration,
+            Jerk = Jerk,
+            PreviousTrajectory = PreviousTrajectory,
+            TerminalAcceleration = TerminalAcceleration,
+        };
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/SchedulingConfiguration.cs

@@ -0,0 +1,73 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// EM 规划的触发节奏、预测窗口、离散上限和发布样本上限配置；默认值由 <see cref="EmPlannerConfiguration.CreateDefault"/> 为低速泊车协调。
+/// </summary>
+public sealed class SchedulingConfiguration
+{
+    /// <summary>
+    /// 两次规划触发之间的周期；单位 s，默认值 0.20，必须为正有限值。
+    /// </summary>
+    public double ReplanPeriodSeconds { get; set; }
+    /// <summary>
+    /// 单次优化覆盖的预测时间窗口；单位 s，默认值 6，必须为正有限值。
+    /// </summary>
+    public double TimeHorizonSeconds { get; set; }
+    /// <summary>
+    /// 单次优化覆盖的最大参考线距离；单位 m，默认值 500，必须为正有限值；滚动规划时还须覆盖制动距离及一个重规划周期内的行程。
+    /// </summary>
+    public double DistanceHorizonMeters { get; set; }
+    /// <summary>
+    /// 发布轨迹相邻样本的时间间隔；单位 s，默认值 0.05，必须为正有限值。
+    /// </summary>
+    public double OutputTimeStepSeconds { get; set; }
+    /// <summary>
+    /// 单次求解允许使用的超时预算；单位 s，默认值 0.10，必须为正有限值。
+    /// </summary>
+    public double SolverTimeoutSeconds { get; set; }
+    /// <summary>
+    /// 与已发布轨迹交接时向前查看的时长；单位 s，默认值 0.30，必须为正有限值。
+    /// </summary>
+    public double HandoffLookaheadSeconds { get; set; }
+    /// <summary>
+    /// 接受车辆状态输入的最大时效；单位 s，默认值 0.20，必须为正有限值。
+    /// </summary>
+    public double MaximumVehicleStateAgeSeconds { get; set; }
+    /// <summary>
+    /// 纵向优化结点允许的最大时间间距；单位 s，默认值 0.20，必须为正有限值。
+    /// </summary>
+    public double MaximumOptimizationTimeStepSeconds { get; set; }
+    /// <summary>
+    /// 横向或走廊优化结点允许的最大空间间距；单位 m，默认值 0.10，必须为正有限值。
+    /// </summary>
+    public double MaximumOptimizationSpatialStepMeters { get; set; }
+    /// <summary>
+    /// 一次优化允许的最大结点数量；单位为结点数，默认值 401，必须为不小于 3 的整数。
+    /// </summary>
+    public int MaximumOptimizationKnotCount { get; set; }
+    /// <summary>
+    /// 一条发布轨迹允许的最大样本数量；单位为样本数，默认值 5001，必须为不小于 2 的整数。
+    /// </summary>
+    public int MaximumPublishedSampleCount { get; set; }
+
+    /// <summary>
+    /// 复制当前调度配置；输入为当前标量限制，输出为无共享可变状态的标量快照，不对数值作校验且不产生失败状态。
+    /// </summary>
+    internal SchedulingConfiguration Copy()
+    {
+        return new SchedulingConfiguration
+        {
+            ReplanPeriodSeconds = ReplanPeriodSeconds,
+            TimeHorizonSeconds = TimeHorizonSeconds,
+            DistanceHorizonMeters = DistanceHorizonMeters,
+            OutputTimeStepSeconds = OutputTimeStepSeconds,
+            SolverTimeoutSeconds = SolverTimeoutSeconds,
+            HandoffLookaheadSeconds = HandoffLookaheadSeconds,
+            MaximumVehicleStateAgeSeconds = MaximumVehicleStateAgeSeconds,
+            MaximumOptimizationTimeStepSeconds = MaximumOptimizationTimeStepSeconds,
+            MaximumOptimizationSpatialStepMeters = MaximumOptimizationSpatialStepMeters,
+            MaximumOptimizationKnotCount = MaximumOptimizationKnotCount,
+            MaximumPublishedSampleCount = MaximumPublishedSampleCount,
+        };
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/SolverConfiguration.cs

@@ -0,0 +1,58 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// LS/ST QP 求解器的迭代预算、收敛容差和运行选项。
+/// </summary>
+public sealed class SolverConfiguration
+{
+    /// <summary>
+    /// 外层顺序凸化最大迭代次数；默认值 5，必须为正整数。
+    /// </summary>
+    public int MaximumOuterIterations { get; set; }
+    /// <summary>
+    /// 单次 OSQP 求解的最大迭代次数；默认值 4000，必须为正整数。
+    /// </summary>
+    public int MaximumOsqpIterations { get; set; }
+    /// <summary>
+    /// 求解器绝对残差容差；默认值 1e-5，必须为正有限值。
+    /// </summary>
+    public double AbsoluteTolerance { get; set; }
+    /// <summary>
+    /// 求解器相对残差容差；默认值 1e-5，必须为正有限值。
+    /// </summary>
+    public double RelativeTolerance { get; set; }
+    /// <summary>
+    /// 发布前接受解的严格残差容差；默认值 1e-5，必须为正有限值。
+    /// </summary>
+    public double StrictResidualTolerance { get; set; }
+    /// <summary>
+    /// 是否将上一轮可用解作为初值；无单位，默认值 <c>true</c>，仅接受布尔值。
+    /// </summary>
+    public bool WarmStart { get; set; }
+    /// <summary>
+    /// 是否请求 OSQP 进行结果修正；无单位，默认值 <c>true</c>，仅接受布尔值。
+    /// </summary>
+    public bool Polish { get; set; }
+    /// <summary>
+    /// 是否启用原生求解器诊断输出；无单位，默认值 <c>false</c>，仅接受布尔值。
+    /// </summary>
+    public bool NativeVerbose { get; set; }
+
+    /// <summary>
+    /// 复制当前求解器配置；输入为当前迭代预算、容差和布尔选项，输出为无共享可变状态的标量快照，不对数值作校验且不产生失败状态。
+    /// </summary>
+    internal SolverConfiguration Copy()
+    {
+        return new SolverConfiguration
+        {
+            MaximumOuterIterations = MaximumOuterIterations,
+            MaximumOsqpIterations = MaximumOsqpIterations,
+            AbsoluteTolerance = AbsoluteTolerance,
+            RelativeTolerance = RelativeTolerance,
+            StrictResidualTolerance = StrictResidualTolerance,
+            WarmStart = WarmStart,
+            Polish = Polish,
+            NativeVerbose = NativeVerbose,
+        };
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/ValidationConfiguration.cs

@@ -0,0 +1,38 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 发布前轨迹几何、运动学和终端误差的验收容差配置；默认值由 <see cref="EmPlannerConfiguration.CreateDefault"/> 提供。
+/// </summary>
+public sealed class ValidationConfiguration
+{
+    /// <summary>
+    /// 几何位置、弧长和采样比较使用的空间容差；单位 m，默认值 1e-8，必须为正有限值。
+    /// </summary>
+    public double SpatialToleranceMeters { get; set; }
+    /// <summary>
+    /// 速度、加速度和曲率等无量纲相对比较的容差；无量纲，默认值 1e-5，必须为正有限值。
+    /// </summary>
+    public double KinematicTolerance { get; set; }
+    /// <summary>
+    /// 精确终止模式下终端位置允许的误差；单位 m，默认值 0，必须为非负有限值。
+    /// </summary>
+    public double TerminalPositionToleranceMeters { get; set; }
+    /// <summary>
+    /// 精确终止模式下终端航向允许的误差；单位 rad，默认值 0，必须为 [0, π] 内的有限值。
+    /// </summary>
+    public double TerminalYawToleranceRadians { get; set; }
+
+    /// <summary>
+    /// 复制当前校验配置；输入为当前四个容差，输出为无共享可变状态的标量快照，不对数值作校验且不产生失败状态。
+    /// </summary>
+    internal ValidationConfiguration Copy()
+    {
+        return new ValidationConfiguration
+        {
+            SpatialToleranceMeters = SpatialToleranceMeters,
+            KinematicTolerance = KinematicTolerance,
+            TerminalPositionToleranceMeters = TerminalPositionToleranceMeters,
+            TerminalYawToleranceRadians = TerminalYawToleranceRadians,
+        };
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/ContractNumeric.cs

@@ -0,0 +1,18 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 验证规划契约中必须为有限数的标量输入。
+/// </summary>
+internal static class ContractNumeric
+{
+    /// <summary>
+    /// 拒绝 NaN 或无穷数值。<paramref name="value"/> 的单位由调用方语境决定；<paramref name="parameterName"/> 原样传给异常构造器，方法本身不验证它。
+    /// </summary>
+    public static void RequireFinite(double value, string parameterName)
+    {
+        if (double.IsNaN(value) || double.IsInfinity(value))
+            throw new ArgumentOutOfRangeException(parameterName, "A finite value is required.");
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmBoundaryType.cs

@@ -0,0 +1,28 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 表示 EM 规划中参考段终端或滚动窗口终端的边界类型。
+/// </summary>
+public enum EmBoundaryType
+{
+    /// <summary>
+    /// 非边界采样点；不携带终端或换挡边界语义。
+    /// </summary>
+    None,
+    /// <summary>
+    /// 滚动窗口因安全约束形成的停车终点，而非方向段的真实终点。
+    /// </summary>
+    RollingSafetyStop,
+    /// <summary>
+    /// 到达换挡位置前的真实停车边界。
+    /// </summary>
+    GearSwitchApproach,
+    /// <summary>
+    /// 换挡后新方向段开始时的离开边界。
+    /// </summary>
+    GearSwitchDeparture,
+    /// <summary>
+    /// 整条参考路径或当前任务目标的真实终点边界。
+    /// </summary>
+    Goal,
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmLongitudinalMode.cs

@@ -0,0 +1,20 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 指定纵向规划面对当前窗口末端时采用的速度终端语义。
+/// </summary>
+public enum EmLongitudinalMode
+{
+    /// <summary>
+    /// 普通滚动窗口末端；保持连续行驶，不要求在本窗口内停止。
+    /// </summary>
+    RollingContinuation,
+    /// <summary>
+    /// 将真实停车边界纳入窗口，但在可达性不足时仅以可停车方式接近。
+    /// </summary>
+    ApproachStopBoundary,
+    /// <summary>
+    /// 要求在当前窗口内到达真实边界并保持零速。
+    /// </summary>
+    ExactStopAtBoundary,
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmMotionModel.cs

@@ -0,0 +1,20 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 请求允许的车辆运动模型；服务依据此值选择或拒绝相应规划分支。
+/// </summary>
+public enum EmMotionModel
+{
+    /// <summary>
+    /// 常规非完整约束车辆，可前进或倒车但不能侧移。
+    /// </summary>
+    NonholonomicForwardReverse,
+    /// <summary>
+    /// 蟹行平移模型；当前 EM 管线不支持时返回明确状态。
+    /// </summary>
+    CrabTranslation,
+    /// <summary>
+    /// 原地旋转模型；当前 EM 管线不支持时返回明确状态。
+    /// </summary>
+    InPlaceRotation,
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningRequest.cs

@@ -0,0 +1,329 @@
+using System;
+using System.Diagnostics;
+using System.Threading;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+internal interface IEmPlanningPublicationAuthorization
+{
+    TimeSpan Remaining { get; }
+
+    bool IsExpired { get; }
+
+    EmPlanningPublicationDecision TryPublish(CancellationToken requestCallerCancellationToken,
+        CancellationToken coordinatorCallerCancellationToken, Action publish);
+}
+
+internal enum EmPlanningPublicationDecision
+{
+    Published = 0,
+    CallerCancelled = 1,
+    DeadlineExpired = 2,
+}
+
+internal sealed class EmPlanningRequestPublicationAuthorization : IEmPlanningPublicationAuthorization
+{
+    private readonly object gate = new object();
+    private readonly TimeSpan? initialRemaining;
+    private readonly Stopwatch stopwatch;
+    private readonly CancellationToken deadlineToken;
+    private bool callerCancelled;
+    private bool deadlineExpired;
+
+    public EmPlanningRequestPublicationAuthorization(TimeSpan? remaining, CancellationToken deadlineToken)
+    {
+        initialRemaining = remaining;
+        this.deadlineToken = deadlineToken;
+        stopwatch = remaining.HasValue ? Stopwatch.StartNew() : null;
+        deadlineExpired = remaining.HasValue && remaining.Value <= TimeSpan.Zero ||
+            deadlineToken.IsCancellationRequested;
+    }
+
+    public TimeSpan Remaining
+    {
+        get
+        {
+            lock (gate)
+            {
+                ObserveDeadlineInsideGate();
+                return ComputeRemainingInsideGate();
+            }
+        }
+    }
+
+    public bool IsExpired
+    {
+        get
+        {
+            lock (gate)
+            {
+                ObserveDeadlineInsideGate();
+                return deadlineExpired;
+            }
+        }
+    }
+
+    public EmPlanningPublicationDecision TryPublish(CancellationToken requestCallerCancellationToken,
+        CancellationToken coordinatorCallerCancellationToken, Action publish)
+    {
+        if (publish == null)
+            throw new ArgumentNullException(nameof(publish));
+
+        using CancellationTokenRegistration requestCallerRegistration =
+            requestCallerCancellationToken.Register(ObserveCallerCancellation);
+        using CancellationTokenRegistration coordinatorCallerRegistration =
+            coordinatorCallerCancellationToken.Register(ObserveCallerCancellation);
+        using CancellationTokenRegistration deadlineRegistration =
+            deadlineToken.Register(ObserveDeadlineCancellation);
+        lock (gate)
+        {
+            if (requestCallerCancellationToken.IsCancellationRequested ||
+                coordinatorCallerCancellationToken.IsCancellationRequested)
+            {
+                callerCancelled = true;
+            }
+            ObserveDeadlineInsideGate();
+            if (callerCancelled)
+                return EmPlanningPublicationDecision.CallerCancelled;
+            if (deadlineExpired)
+                return EmPlanningPublicationDecision.DeadlineExpired;
+            publish();
+            return EmPlanningPublicationDecision.Published;
+        }
+    }
+
+    private void ObserveCallerCancellation()
+    {
+        lock (gate)
+            callerCancelled = true;
+    }
+
+    private void ObserveDeadlineCancellation()
+    {
+        lock (gate)
+            deadlineExpired = true;
+    }
+
+    private void ObserveDeadlineInsideGate()
+    {
+        if (deadlineToken.IsCancellationRequested || ComputeRemainingInsideGate() <= TimeSpan.Zero)
+            deadlineExpired = true;
+    }
+
+    private TimeSpan ComputeRemainingInsideGate()
+    {
+        if (!initialRemaining.HasValue)
+            return deadlineExpired ? TimeSpan.Zero : TimeSpan.MaxValue;
+        TimeSpan remaining = initialRemaining.Value - stopwatch.Elapsed;
+        return remaining > TimeSpan.Zero && !deadlineExpired ? remaining : TimeSpan.Zero;
+    }
+}
+
+/// <summary>
+/// EM 单次规划的只读输入引用快照，绑定平滑参考路径、地图、车辆状态、目标方向段和输出身份。
+/// 坐标使用 m，航向使用 rad，时间使用 UTC；调用方必须在进入服务前保证这些输入属于同一业务版本。
+/// </summary>
+public sealed class EmPlanningRequest
+{
+    /// <summary>
+    /// 保存一次规划所需的引用快照和发布身份。构造器只赋值而不校验、复制或取得对象所有权；调用方仍拥有传入引用，服务随后负责验证并复制配置。
+    /// </summary>
+    public EmPlanningRequest(
+        PathSmoothingResult referencePath,
+        PlanningGridMap map,
+        VehicleParameters vehicle,
+        VehicleMotionState vehicleState,
+        EmPlannerConfiguration configuration,
+        int segmentIndex,
+        EmTrajectory previousTrajectory,
+        DateTimeOffset requestedAtUtc,
+        DateTimeOffset effectiveAtUtc,
+        string outputTrajectoryId,
+        string referencePathId,
+        string previousTrajectoryId,
+        EmMotionModel motionModel,
+        EmPlanningScope planningScope,
+        TimeSpan? cycleDeadlineRemaining = null,
+        CancellationToken callerCancellationToken = default,
+        CancellationToken cycleDeadlineToken = default)
+    {
+        ReferencePath = referencePath;
+        Map = map;
+        Vehicle = vehicle;
+        VehicleState = vehicleState;
+        Configuration = configuration;
+        SegmentIndex = segmentIndex;
+        PreviousTrajectory = previousTrajectory;
+        RequestedAtUtc = requestedAtUtc;
+        EffectiveAtUtc = effectiveAtUtc;
+        OutputTrajectoryId = outputTrajectoryId;
+        ReferencePathId = referencePathId;
+        PreviousTrajectoryId = previousTrajectoryId;
+        MotionModel = motionModel;
+        PlanningScope = planningScope;
+        CycleDeadlineRemaining = cycleDeadlineRemaining;
+        CallerCancellationToken = callerCancellationToken;
+        CycleDeadlineToken = cycleDeadlineToken;
+        PublicationAuthorization = new EmPlanningRequestPublicationAuthorization(
+            cycleDeadlineRemaining, cycleDeadlineToken);
+    }
+
+    internal EmPlanningRequest(
+        PathSmoothingResult referencePath,
+        PlanningGridMap map,
+        VehicleParameters vehicle,
+        VehicleMotionState vehicleState,
+        EmPlannerConfiguration configuration,
+        int segmentIndex,
+        EmTrajectory previousTrajectory,
+        DateTimeOffset requestedAtUtc,
+        DateTimeOffset effectiveAtUtc,
+        string outputTrajectoryId,
+        string referencePathId,
+        string previousTrajectoryId,
+        EmMotionModel motionModel,
+        EmPlanningScope planningScope,
+        TimeSpan? cycleDeadlineRemaining,
+        CancellationToken callerCancellationToken,
+        CancellationToken cycleDeadlineToken,
+        IEmPlanningPublicationAuthorization publicationAuthorization)
+        : this(referencePath, map, vehicle, vehicleState, configuration, segmentIndex, previousTrajectory,
+            requestedAtUtc, effectiveAtUtc, outputTrajectoryId, referencePathId, previousTrajectoryId,
+            motionModel, planningScope, cycleDeadlineRemaining, callerCancellationToken, cycleDeadlineToken)
+    {
+        PublicationAuthorization = publicationAuthorization ??
+            new EmPlanningRequestPublicationAuthorization(cycleDeadlineRemaining, cycleDeadlineToken);
+    }
+
+    /// <summary>
+    /// 平滑后的参考路径结果引用；服务要求其为可消费的成功结果，且调用方负责保持其与其他输入版本一致。
+    /// </summary>
+    public PathSmoothingResult ReferencePath { get; }
+
+    /// <summary>
+    /// 规划使用的栅格地图快照引用；地图坐标单位和快照身份由地图契约定义，构造器不检查 null 或就绪状态。
+    /// </summary>
+    public PlanningGridMap Map { get; }
+
+    /// <summary>
+    /// 车辆几何与运动限制引用；调用方拥有该对象，服务在请求验证后使用其约束。
+    /// </summary>
+    public VehicleParameters Vehicle { get; }
+
+    /// <summary>
+    /// 一次性捕获的车辆运动状态；其位置为世界坐标 m、航向为 rad，时效和数值有效性由服务验证。
+    /// </summary>
+    public VehicleMotionState VehicleState { get; }
+
+    /// <summary>
+    /// 本次规划的配置引用；请求不深拷贝它，验证通过后服务取得配置副本供本次规划使用。
+    /// </summary>
+    public EmPlannerConfiguration Configuration { get; }
+
+    /// <summary>
+    /// 参考路径中待规划的方向段索引；必须由服务验证为有效的非负范围。
+    /// </summary>
+    public int SegmentIndex { get; }
+
+    /// <summary>
+    /// 可选的上一条已发布轨迹引用，用于衔接或诊断；构造器允许为 null，调用方保有其所有权。
+    /// </summary>
+    public EmTrajectory PreviousTrajectory { get; }
+
+    /// <summary>
+    /// 本次规划请求发起的 UTC 时刻；用于车辆状态时效判断和生成轨迹元数据，构造器不验证其时间关系。
+    /// </summary>
+    public DateTimeOffset RequestedAtUtc { get; }
+
+    /// <summary>
+    /// 成功发布轨迹开始生效的 UTC 时刻；由下游消费者依据其调度语义使用。
+    /// </summary>
+    public DateTimeOffset EffectiveAtUtc { get; }
+
+    /// <summary>
+    /// 成功轨迹的发布标识；构造器不校验，轨迹元数据构造时要求为非空白字符串。
+    /// </summary>
+    public string OutputTrajectoryId { get; }
+
+    /// <summary>
+    /// 输入参考路径的业务标识，用于成功轨迹的来源追踪；构造器不校验其 null 或空白值。
+    /// </summary>
+    public string ReferencePathId { get; }
+
+    /// <summary>
+    /// 上一条轨迹的业务标识；允许为 null，成功轨迹元数据会将 null 规范化为空字符串。
+    /// </summary>
+    public string PreviousTrajectoryId { get; }
+
+    /// <summary>
+    /// 请求的车辆运动模型；不支持或无效的枚举值由服务转换为失败状态。
+    /// </summary>
+    public EmMotionModel MotionModel { get; }
+
+    /// <summary>
+    /// 本次请求覆盖滚动窗口或整个方向段的范围；服务负责验证其枚举值和资源约束。
+    /// </summary>
+    public EmPlanningScope PlanningScope { get; }
+
+    /// <summary>Optional remaining wall-clock budget for the complete outer planning cycle.</summary>
+    public TimeSpan? CycleDeadlineRemaining { get; }
+
+    /// <summary>Dynamic caller-cancellation origin used to preserve terminal-status precedence.</summary>
+    public CancellationToken CallerCancellationToken { get; }
+
+    /// <summary>Dynamic shared-cycle deadline origin checked again at atomic publication.</summary>
+    public CancellationToken CycleDeadlineToken { get; }
+
+    internal IEmPlanningPublicationAuthorization PublicationAuthorization { get; }
+
+    internal bool IsCycleDeadlineExpired()
+    {
+        try
+        {
+            return PublicationAuthorization.IsExpired;
+        }
+        catch
+        {
+            return true;
+        }
+    }
+
+    internal TimeSpan? DynamicCycleDeadlineRemaining()
+    {
+        try
+        {
+            TimeSpan remaining = PublicationAuthorization.Remaining;
+            return remaining > TimeSpan.Zero ? remaining : TimeSpan.Zero;
+        }
+        catch
+        {
+            return TimeSpan.Zero;
+        }
+    }
+
+    internal EmPlanningPublicationDecision TryAuthorizePublication(
+        CancellationToken coordinatorCallerCancellationToken, Action publish)
+    {
+        if (publish == null)
+            throw new ArgumentNullException(nameof(publish));
+        try
+        {
+            return PublicationAuthorization.TryPublish(CallerCancellationToken,
+                coordinatorCallerCancellationToken, publish);
+        }
+        catch
+        {
+            return EmPlanningPublicationDecision.DeadlineExpired;
+        }
+    }
+}
+
+/// <summary>
+/// 为请求契约保留的 <see cref="EmPlannerConfiguration"/> 部分声明；实际配置成员由其他同名分部提供。
+/// </summary>
+public sealed partial class EmPlannerConfiguration
+{
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningResult.cs

@@ -0,0 +1,43 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 一次 EM 规划的不可变结果。只有成功状态才携带可发布的完整轨迹；其余状态仅提供诊断，不能作为部分执行轨迹消费。
+/// </summary>
+public sealed class EmPlanningResult
+{
+    /// <summary>
+    /// 创建规划结果并强制成功状态与轨迹发布的一致性：<see cref="EmPlanningStatus.Success"/> 和 <see cref="EmPlanningStatus.SuccessWithFallback"/> 必须提供非 null 轨迹，其他状态必须提供 null 轨迹；null 失败原因规范化为空字符串。
+    /// </summary>
+    public EmPlanningResult(EmPlanningStatus status, EmTrajectory trajectory, string failureReason)
+    {
+        if (!Enum.IsDefined(typeof(EmPlanningStatus), status))
+            throw new ArgumentOutOfRangeException(nameof(status));
+
+        bool isSuccess = status == EmPlanningStatus.Success || status == EmPlanningStatus.SuccessWithFallback;
+        if (isSuccess && trajectory == null)
+            throw new ArgumentException("Successful results require a trajectory.", nameof(trajectory));
+        if (!isSuccess && trajectory != null)
+            throw new ArgumentException("Only successful results may contain a trajectory.", nameof(trajectory));
+
+        Status = status;
+        Trajectory = trajectory;
+        FailureReason = failureReason ?? string.Empty;
+    }
+
+    /// <summary>
+    /// 本次规划的最终状态；消费者必须先判断其是否为成功状态，再访问可发布轨迹。
+    /// </summary>
+    public EmPlanningStatus Status { get; }
+
+    /// <summary>
+    /// 仅在成功或成功降级状态下存在的完整可发布轨迹；失败、取消和无效输入结果始终为 null，消费者不得把失败结果当作部分轨迹执行。
+    /// </summary>
+    public EmTrajectory Trajectory { get; }
+
+    /// <summary>
+    /// 面向诊断的失败或降级原因；null 输入已规范化为空字符串，不替代 <see cref="Status"/> 的机器可读状态。
+    /// </summary>
+    public string FailureReason { get; }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningScope.cs

@@ -0,0 +1,16 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 定义一次 EM 规划覆盖当前滚动窗口还是完整方向段。
+/// </summary>
+public enum EmPlanningScope
+{
+    /// <summary>
+    /// 仅覆盖有限的滚动规划窗口；段末之外保留给后续重规划。
+    /// </summary>
+    RollingHorizon,
+    /// <summary>
+    /// 覆盖选定方向段直至真实停车边界，并受完整段资源上限保护。
+    /// </summary>
+    FullDirectionSegment,
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningStatus.cs

@@ -0,0 +1,96 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 表示 EM 规划请求的最终处理状态；仅两个成功状态允许发布轨迹，其余状态要求消费者保留现有安全策略并读取诊断。
+/// </summary>
+public enum EmPlanningStatus
+{
+    /// <summary>
+    /// 所有规划和验证阶段成功，结果可按其生效时间发布。
+    /// </summary>
+    Success,
+    /// <summary>
+    /// 已发布经过验证的降级或回退轨迹；消费者仍可使用轨迹，并应记录诊断原因。
+    /// </summary>
+    SuccessWithFallback,
+    /// <summary>
+    /// 请求对象或其成员未满足服务输入要求。
+    /// </summary>
+    InvalidInput,
+    /// <summary>
+    /// 请求的车辆运动模型未被当前 EM 管线支持。
+    /// </summary>
+    UnsupportedMotionMode,
+    /// <summary>
+    /// 车辆状态采集时刻相对于请求时刻过旧。
+    /// </summary>
+    StaleVehicleState,
+    /// <summary>
+    /// 车辆状态的行驶方向与选定参考路径方向段不一致。
+    /// </summary>
+    StateDirectionMismatch,
+    /// <summary>
+    /// 参考路径结果、方向段或其状态不能用于规划。
+    /// </summary>
+    InvalidReferencePath,
+    /// <summary>
+    /// 无法将车辆状态投影到选定参考路径或方向段。
+    /// </summary>
+    ProjectionFailed,
+    /// <summary>
+    /// 地图和车辆约束下未能构造可行的行驶走廊。
+    /// </summary>
+    CorridorInfeasible,
+    /// <summary>
+    /// 横向优化未得到满足约束的候选解。
+    /// </summary>
+    LateralInfeasible,
+    /// <summary>
+    /// 纵向优化未得到满足时空与动力学约束的候选解。
+    /// </summary>
+    LongitudinalInfeasible,
+    /// <summary>
+    /// 当前速度、距离或限制不足以在所需边界前完成停车。
+    /// </summary>
+    StoppingDistanceInsufficient,
+    /// <summary>
+    /// 所需二次规划求解器不可用。
+    /// </summary>
+    SolverUnavailable,
+    /// <summary>
+    /// 求解在共享时间预算或迭代限制内未完成。
+    /// </summary>
+    SolverTimedOut,
+    /// <summary>
+    /// 调用方在可发布结果生成前取消了请求。
+    /// </summary>
+    Cancelled,
+    /// <summary>
+    /// 候选轨迹未通过最终世界空间、动力学或终端语义验证。
+    /// </summary>
+    ValidationFailed,
+    /// <summary>
+    /// 请求在处理期间被更新版本的规划工作替代。
+    /// </summary>
+    Superseded,
+    /// <summary>
+    /// 候选轨迹相对于当前状态未形成足够的有效进展。
+    /// </summary>
+    NoProgress,
+    /// <summary>
+    /// 轨迹末端位姿未满足要求的真实终端位姿。
+    /// </summary>
+    TerminalPoseMismatch,
+    /// <summary>
+    /// 完整方向段规划超过配置的时间、采样或求解资源上限。
+    /// </summary>
+    FullSegmentResourceLimitExceeded,
+    /// <summary>
+    /// 未被其他状态细分的规划失败。
+    /// </summary>
+    Failed,
+    /// <summary>
+    /// The outer planning cycle exhausted its shared bootstrap-to-publication deadline.
+    /// </summary>
+    CycleDeadlineExpired,
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTerminalType.cs

@@ -0,0 +1,20 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 表示已发布轨迹末端所对应的规划终端语义。
+/// </summary>
+public enum EmTerminalType
+{
+    /// <summary>
+    /// 因滚动窗口安全约束形成的停车终端，不代表方向段完成。
+    /// </summary>
+    RollingSafetyStop,
+    /// <summary>
+    /// 用于在方向改变前后执行换挡的终端。
+    /// </summary>
+    GearSwitch,
+    /// <summary>
+    /// 最终任务目标的终端。
+    /// </summary>
+    Goal,
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectory.cs

@@ -0,0 +1,45 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 供成功规划结果发布的时间参数化轨迹；本类型只施加结构约束，世界空间复核由上游服务完成，点序列、元数据和终端语义在发布后保持不可变。
+/// </summary>
+public sealed class EmTrajectory
+{
+    /// <summary>
+    /// 创建可发布的轨迹并复制点列表容器。元数据和每个点对象按引用保存、调用方仍拥有它们；传入列表可随后修改而不影响 <see cref="Points"/>，且 null 或空列表会抛出异常。
+    /// </summary>
+    public EmTrajectory(EmTrajectoryMetadata metadata, IReadOnlyList<EmTrajectoryPoint> points)
+    {
+        if (metadata == null)
+            throw new ArgumentNullException(nameof(metadata));
+        if (points == null)
+            throw new ArgumentNullException(nameof(points));
+        if (points.Count == 0)
+            throw new ArgumentException("A published trajectory requires at least one point.", nameof(points));
+
+        var copy = new List<EmTrajectoryPoint>(points.Count);
+        for (int index = 0; index < points.Count; index++)
+        {
+            if (points[index] == null)
+                throw new ArgumentException("Trajectory points cannot contain null values.", nameof(points));
+            copy.Add(points[index]);
+        }
+
+        Metadata = metadata;
+        Points = new ReadOnlyCollection<EmTrajectoryPoint>(copy);
+    }
+
+    /// <summary>
+    /// 轨迹的不可变发布元数据引用；构造时必须非 null，未在本类中深拷贝。
+    /// </summary>
+    public EmTrajectoryMetadata Metadata { get; }
+
+    /// <summary>
+    /// 按调用方提供顺序保存的只读点列表；列表容器为构造时复制的快照，至少包含一个非 null 点，时间单调性由上游验证保证而非本构造器检查。
+    /// </summary>
+    public IReadOnlyList<EmTrajectoryPoint> Points { get; }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectoryMetadata.cs

@@ -0,0 +1,120 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 轨迹发布身份、来源版本、有效时间、方向段和终端边界的不可变元数据。
+/// </summary>
+public sealed class EmTrajectoryMetadata
+{
+    /// <summary>
+    /// 创建轨迹的发布和来源元数据。验证标识、序列号、索引和枚举值；<paramref name="previousTrajectoryId"/> 可为 null 并会规范化为空字符串，两个 UTC 时刻的先后关系不在此处验证。
+    /// </summary>
+    public EmTrajectoryMetadata(
+        string trajectoryId,
+        DateTimeOffset generatedAtUtc,
+        DateTimeOffset effectiveAtUtc,
+        long mapSnapshotId,
+        string referencePathId,
+        long vehicleStateSequenceId,
+        string previousTrajectoryId,
+        int segmentIndex,
+        TravelDirection direction,
+        EmTerminalType terminalType,
+        EmLongitudinalMode longitudinalMode,
+        EmPlanningScope planningScope)
+    {
+        if (string.IsNullOrWhiteSpace(trajectoryId))
+            throw new ArgumentException("A trajectory ID is required.", nameof(trajectoryId));
+        if (mapSnapshotId < 0)
+            throw new ArgumentOutOfRangeException(nameof(mapSnapshotId));
+        if (string.IsNullOrWhiteSpace(referencePathId))
+            throw new ArgumentException("A reference path ID is required.", nameof(referencePathId));
+        if (vehicleStateSequenceId < 0)
+            throw new ArgumentOutOfRangeException(nameof(vehicleStateSequenceId));
+        if (segmentIndex < 0)
+            throw new ArgumentOutOfRangeException(nameof(segmentIndex));
+        if (!Enum.IsDefined(typeof(TravelDirection), direction))
+            throw new ArgumentOutOfRangeException(nameof(direction));
+        if (!Enum.IsDefined(typeof(EmTerminalType), terminalType))
+            throw new ArgumentOutOfRangeException(nameof(terminalType));
+        if (!Enum.IsDefined(typeof(EmLongitudinalMode), longitudinalMode))
+            throw new ArgumentOutOfRangeException(nameof(longitudinalMode));
+        if (!Enum.IsDefined(typeof(EmPlanningScope), planningScope))
+            throw new ArgumentOutOfRangeException(nameof(planningScope));
+
+        TrajectoryId = trajectoryId;
+        GeneratedAtUtc = generatedAtUtc;
+        EffectiveAtUtc = effectiveAtUtc;
+        MapSnapshotId = mapSnapshotId;
+        ReferencePathId = referencePathId;
+        VehicleStateSequenceId = vehicleStateSequenceId;
+        PreviousTrajectoryId = previousTrajectoryId ?? string.Empty;
+        SegmentIndex = segmentIndex;
+        Direction = direction;
+        TerminalType = terminalType;
+        LongitudinalMode = longitudinalMode;
+        PlanningScope = planningScope;
+    }
+
+    /// <summary>
+    /// 非空白的本次发布轨迹标识，由消费者用于去重、替换和追踪。
+    /// </summary>
+    public string TrajectoryId { get; }
+
+    /// <summary>
+    /// 生成此元数据的 UTC 时刻；值原样保存，构造器不与生效时刻比较。
+    /// </summary>
+    public DateTimeOffset GeneratedAtUtc { get; }
+
+    /// <summary>
+    /// 轨迹计划开始生效的 UTC 时刻；执行消费者负责按其调度策略解释。
+    /// </summary>
+    public DateTimeOffset EffectiveAtUtc { get; }
+
+    /// <summary>
+    /// 地图快照的非负版本标识；用于确认轨迹依赖的环境版本。
+    /// </summary>
+    public long MapSnapshotId { get; }
+
+    /// <summary>
+    /// 非空白的输入参考路径标识；用于关联轨迹的几何来源。
+    /// </summary>
+    public string ReferencePathId { get; }
+
+    /// <summary>
+    /// 车辆状态的非负序列版本；用于判断轨迹是否基于当前状态快照。
+    /// </summary>
+    public long VehicleStateSequenceId { get; }
+
+    /// <summary>
+    /// 前一轨迹的可选标识；null 输入已规范化为空字符串，空字符串表示没有可关联的前轨迹标识。
+    /// </summary>
+    public string PreviousTrajectoryId { get; }
+
+    /// <summary>
+    /// 参考路径中此轨迹所属的非负方向段索引。
+    /// </summary>
+    public int SegmentIndex { get; }
+
+    /// <summary>
+    /// 所属方向段的已验证行驶方向。
+    /// </summary>
+    public TravelDirection Direction { get; }
+
+    /// <summary>
+    /// 轨迹最后一个终端的已验证业务类型。
+    /// </summary>
+    public EmTerminalType TerminalType { get; }
+
+    /// <summary>
+    /// 生成轨迹时采用的已验证纵向终端速度语义。
+    /// </summary>
+    public EmLongitudinalMode LongitudinalMode { get; }
+
+    /// <summary>
+    /// 生成轨迹时采用的已验证规划覆盖范围。
+    /// </summary>
+    public EmPlanningScope PlanningScope { get; }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectoryPoint.cs

@@ -0,0 +1,155 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 轨迹中的单个时间样本。世界位置单位 m、航向单位 rad、速度单位 m/s、曲率单位 1/m。
+/// </summary>
+public sealed class EmTrajectoryPoint
+{
+    /// <summary>
+    /// 创建一个轨迹时间样本。所有浮点输入必须有限；时间、段内弧长和路径弧长必须非负，段索引和枚举值必须有效。派生的绝对速度、世界速度分量和偏航角速度由有符号纵向速度、航向和曲率计算。
+    /// </summary>
+    public EmTrajectoryPoint(
+        double x,
+        double y,
+        double yaw,
+        double signedLongitudinalVelocity,
+        double timeFromStart,
+        double vehicleCurvature,
+        int segmentIndex,
+        double segmentLocalS,
+        double pathS,
+        TravelDirection direction,
+        EmBoundaryType boundaryType,
+        double longitudinalAcceleration,
+        double longitudinalJerk)
+    {
+        ContractNumeric.RequireFinite(x, nameof(x));
+        ContractNumeric.RequireFinite(y, nameof(y));
+        ContractNumeric.RequireFinite(yaw, nameof(yaw));
+        ContractNumeric.RequireFinite(signedLongitudinalVelocity, nameof(signedLongitudinalVelocity));
+        ContractNumeric.RequireFinite(timeFromStart, nameof(timeFromStart));
+        ContractNumeric.RequireFinite(vehicleCurvature, nameof(vehicleCurvature));
+        ContractNumeric.RequireFinite(segmentLocalS, nameof(segmentLocalS));
+        ContractNumeric.RequireFinite(pathS, nameof(pathS));
+        ContractNumeric.RequireFinite(longitudinalAcceleration, nameof(longitudinalAcceleration));
+        ContractNumeric.RequireFinite(longitudinalJerk, nameof(longitudinalJerk));
+        if (segmentIndex < 0)
+            throw new ArgumentOutOfRangeException(nameof(segmentIndex));
+        if (timeFromStart < 0d)
+            throw new ArgumentOutOfRangeException(nameof(timeFromStart));
+        if (segmentLocalS < 0d)
+            throw new ArgumentOutOfRangeException(nameof(segmentLocalS));
+        if (pathS < 0d)
+            throw new ArgumentOutOfRangeException(nameof(pathS));
+        if (!Enum.IsDefined(typeof(TravelDirection), direction))
+            throw new ArgumentOutOfRangeException(nameof(direction));
+        if (!Enum.IsDefined(typeof(EmBoundaryType), boundaryType))
+            throw new ArgumentOutOfRangeException(nameof(boundaryType));
+
+        X = x;
+        Y = y;
+        Yaw = yaw;
+        SignedLongitudinalVelocity = signedLongitudinalVelocity;
+        Speed = Math.Abs(signedLongitudinalVelocity);
+        VelocityX = signedLongitudinalVelocity * Math.Cos(yaw);
+        VelocityY = signedLongitudinalVelocity * Math.Sin(yaw);
+        YawRate = signedLongitudinalVelocity * vehicleCurvature;
+        TimeFromStart = timeFromStart;
+        VehicleCurvature = vehicleCurvature;
+        SegmentIndex = segmentIndex;
+        SegmentLocalS = segmentLocalS;
+        PathS = pathS;
+        Direction = direction;
+        BoundaryType = boundaryType;
+        LongitudinalAcceleration = longitudinalAcceleration;
+        LongitudinalJerk = longitudinalJerk;
+    }
+
+    /// <summary>
+    /// 世界坐标系 X 位置，单位 m。
+    /// </summary>
+    public double X { get; }
+
+    /// <summary>
+    /// 世界坐标系 Y 位置，单位 m。
+    /// </summary>
+    public double Y { get; }
+
+    /// <summary>
+    /// 车辆在世界坐标系中的航向，单位 rad；构造器仅要求有限，不归一化角度。
+    /// </summary>
+    public double Yaw { get; }
+
+    /// <summary>
+    /// 沿车辆前向轴的有符号纵向速度，单位 m/s；符号表示前进或倒车。
+    /// </summary>
+    public double SignedLongitudinalVelocity { get; }
+
+    /// <summary>
+    /// 有符号纵向速度的绝对值，单位 m/s。
+    /// </summary>
+    public double Speed { get; }
+
+    /// <summary>
+    /// 由有符号纵向速度和航向导出的世界坐标 X 速度分量，单位 m/s。
+    /// </summary>
+    public double VelocityX { get; }
+
+    /// <summary>
+    /// 由有符号纵向速度和航向导出的世界坐标 Y 速度分量，单位 m/s。
+    /// </summary>
+    public double VelocityY { get; }
+
+    /// <summary>
+    /// 由有符号纵向速度乘车辆曲率导出的偏航角速度，单位 rad/s。
+    /// </summary>
+    public double YawRate { get; }
+
+    /// <summary>
+    /// 从本条轨迹开始执行起累计的非负时间，单位 s。
+    /// </summary>
+    public double TimeFromStart { get; }
+
+    /// <summary>
+    /// 车辆路径曲率，单位 1/m；符号约定由上游几何计算定义。
+    /// </summary>
+    public double VehicleCurvature { get; }
+
+    /// <summary>
+    /// 此点所属的非负方向段索引。
+    /// </summary>
+    public int SegmentIndex { get; }
+
+    /// <summary>
+    /// 从该方向段开始沿参考路径累计的非负弧长，单位 m。
+    /// </summary>
+    public double SegmentLocalS { get; }
+
+    /// <summary>
+    /// 从整条参考路径开始累计的非负弧长，单位 m。
+    /// </summary>
+    public double PathS { get; }
+
+    /// <summary>
+    /// 此点的已验证行驶方向。
+    /// </summary>
+    public TravelDirection Direction { get; }
+
+    /// <summary>
+    /// 此点的已验证边界语义；普通内部点使用 <see cref="EmBoundaryType.None"/>。
+    /// </summary>
+    public EmBoundaryType BoundaryType { get; }
+
+    /// <summary>
+    /// 沿车辆前向轴的纵向加速度，单位 m/s²；仅供同程序集的轨迹验证和执行逻辑读取。
+    /// </summary>
+    internal double LongitudinalAcceleration { get; }
+
+    /// <summary>
+    /// 沿车辆前向轴的纵向加加速度，单位 m/s³；仅供同程序集的轨迹验证和执行逻辑读取。
+    /// </summary>
+    internal double LongitudinalJerk { get; }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/VehicleMotionState.cs

@@ -0,0 +1,52 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 调用方一次性捕获的车辆运动状态；EMPlanner 只消费此快照，不直接读取定位、轮速或系统时钟。
+/// </summary>
+public sealed class VehicleMotionState
+{
+    /// <summary>
+    /// 保存调用方捕获的车辆状态。构造器不校验 null、数值有限性、UTC 时效或序列号；服务在接受请求前负责验证，传入 <paramref name="pose"/> 的所有权仍归调用方。
+    /// </summary>
+    public VehicleMotionState(
+        Pose2D pose,
+        double signedLongitudinalSpeedMetersPerSecond,
+        double? longitudinalAccelerationMetersPerSecondSquared,
+        DateTimeOffset capturedAtUtc,
+        long sequenceId)
+    {
+        Pose = pose;
+        SignedLongitudinalSpeedMetersPerSecond = signedLongitudinalSpeedMetersPerSecond;
+        LongitudinalAccelerationMetersPerSecondSquared = longitudinalAccelerationMetersPerSecondSquared;
+        CapturedAtUtc = capturedAtUtc;
+        SequenceId = sequenceId;
+    }
+
+    /// <summary>
+    /// 车辆世界位姿引用，其中位置单位为 m、航向单位为 rad；构造器允许 null，但服务要求有效位姿。
+    /// </summary>
+    public Pose2D Pose { get; }
+
+    /// <summary>
+    /// 沿车辆前向轴的有符号纵向速度，单位 m/s；正负方向约定由上游状态生产者负责，服务要求其为有限数。
+    /// </summary>
+    public double SignedLongitudinalSpeedMetersPerSecond { get; }
+
+    /// <summary>
+    /// 可选的沿车辆前向轴纵向加速度，单位 m/s²；null 表示采集方未提供该量，非 null 值必须由服务验证为有限数。
+    /// </summary>
+    public double? LongitudinalAccelerationMetersPerSecondSquared { get; }
+
+    /// <summary>
+    /// 采集此状态的 UTC 时刻；服务用它相对请求发起时刻判断状态是否过期。
+    /// </summary>
+    public DateTimeOffset CapturedAtUtc { get; }
+
+    /// <summary>
+    /// 调用方提供的状态快照序列版本；服务要求其非负，成功轨迹将其写入元数据供消费者关联。
+    /// </summary>
+    public long SequenceId { get; }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Corridor/LateralInterval.cs

@@ -0,0 +1,56 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 一个精确参考站 S 处与种子横向位置连通的自由横向区间。
+/// 参考站和横向偏移均以 m 计，横向正负号遵循该方向段的行驶坐标系；构造时拒绝非有限值、反向边界，或超出边界 1e-12 m 的种子。
+/// </summary>
+public sealed class LateralInterval
+{
+    /// <summary>
+    /// 创建一个固定参考站上的闭合自由横向区间。
+    /// 参数：referenceS、minimumL、maximumL 与 seedL 均为 m；seedL 必须在闭区间内（容许 1e-12 m 数值误差）。
+    /// 返回：保存已验证边界的不可变区间；非法数值或不连通种子会引发 <see cref="ArgumentOutOfRangeException"/>。
+    /// </summary>
+    public LateralInterval(double referenceS, double minimumL, double maximumL, double seedL)
+    {
+        if (!IsFinite(referenceS) || !IsFinite(minimumL) || !IsFinite(maximumL) || !IsFinite(seedL) ||
+            minimumL > maximumL || seedL < minimumL - 1e-12d || seedL > maximumL + 1e-12d)
+            throw new ArgumentOutOfRangeException(nameof(referenceS));
+
+        ReferenceS = referenceS;
+        MinimumL = minimumL;
+        MaximumL = maximumL;
+        SeedL = seedL;
+    }
+
+    /// <summary>
+    /// 本区间所属方向段局部参考弧长 S，单位 m。
+    /// </summary>
+    public double ReferenceS { get; }
+
+    /// <summary>
+    /// 可通行横向偏移闭区间的下界，单位 m。
+    /// </summary>
+    public double MinimumL { get; }
+
+    /// <summary>
+    /// 可通行横向偏移闭区间的上界，单位 m。
+    /// </summary>
+    public double MaximumL { get; }
+
+    /// <summary>
+    /// 用于保持拓扑连通性的种子横向偏移，单位 m。
+    /// </summary>
+    public double SeedL { get; }
+
+    /// <summary>
+    /// 判定标量能否参与走廊边界计算。
+    /// 参数：value 为无单位或 m 制实数；返回：仅非 NaN 且非无穷大时为 true。
+    /// </summary>
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Corridor/StaticCorridor.cs

@@ -0,0 +1,39 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 一个已选择拓扑走廊在离散参考站上的不可变横向硬边界集合。
+/// 站点按方向段局部 S（m）非递减保存，调用方只能沿每个站点的种子连通自由区间优化。
+/// </summary>
+public sealed class StaticCorridor
+{
+    /// <summary>
+    /// 从站点序列创建走廊的防御性只读副本。
+    /// 参数：stations 不能为空且至少含一个按 ReferenceS（m）非递减的非空区间；违反这些约束会被拒绝。
+    /// </summary>
+    public StaticCorridor(IReadOnlyList<LateralInterval> stations)
+    {
+        if (stations == null || stations.Count == 0)
+            throw new ArgumentException("A static corridor requires stations.", nameof(stations));
+
+        var copy = new List<LateralInterval>(stations.Count);
+        double previousS = double.NegativeInfinity;
+        for (int index = 0; index < stations.Count; index++)
+        {
+            LateralInterval station = stations[index];
+            if (station == null || station.ReferenceS < previousS)
+                throw new ArgumentException("Static corridor stations must be non-null and sorted.", nameof(stations));
+            copy.Add(station);
+            previousS = station.ReferenceS;
+        }
+        Stations = new ReadOnlyCollection<LateralInterval>(copy);
+    }
+
+    /// <summary>
+    /// 按方向段局部参考弧长 S（m）排序的横向硬边界只读列表。
+    /// </summary>
+    public IReadOnlyList<LateralInterval> Stations { get; }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Corridor/StaticCorridorBuilder.cs

@@ -0,0 +1,373 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 为单一方向段构建仅与种子轨迹横向连通的静态自由空间走廊。
+/// 采样站和横向偏移使用 Frenet S/L（m），碰撞在世界 X/Y（m）车体几何中验证；任何无效、碰撞或断连站都会拒绝整个走廊。
+/// </summary>
+public sealed class StaticCorridorBuilder
+{
+    /// <summary>
+    /// 横向边界、采样去重和相邻区间重叠判定使用的数值容差，单位 m。
+    /// </summary>
+    private const double Epsilon = 1e-12d;
+
+    /// <summary>
+    /// Frenet 重建时拒绝 1-κL 接近零的最小正分母，无量纲。
+    /// </summary>
+    private const double ReconstructionDenominator = 1e-12d;
+
+    /// <summary>
+    /// 对未被保守净空快速放行的位置执行精确车体碰撞复核的依赖项。
+    /// </summary>
+    private readonly FootprintCollisionChecker _collisionChecker;
+
+    /// <summary>
+    /// 创建使用默认连续车体碰撞检查器的静态走廊构建器。
+    /// </summary>
+    public StaticCorridorBuilder()
+        : this(new FootprintCollisionChecker())
+    {
+    }
+
+    /// <summary>
+    /// 创建使用指定车体碰撞检查器的静态走廊构建器。
+    /// 参数：collisionChecker 不可为空；该检查器在世界坐标中按车辆尺寸和安全裕度验证采样位姿。
+    /// </summary>
+    public StaticCorridorBuilder(FootprintCollisionChecker collisionChecker)
+    {
+        _collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
+    }
+
+    /// <summary>
+    /// 从 S 起止锚点、种子轨迹和栅格地图构建种子连通的横向硬边界。
+    /// 参数：startReferenceS/endReferenceS、种子 S/L 和配置采样尺度均为 m，map 使用世界 X/Y 栅格，vehicle 提供 m 制尺寸；区间限定在一个方向段内。
+    /// 返回：每站均存在含种子的自由区间且相邻区间在 1e-12 m 内重叠时返回 true；否则 corridor 为 null 并写入拒绝原因。
+    /// </summary>
+    public bool TryBuild(DirectionSegmentView segment, double startReferenceS, double endReferenceS,
+        IReadOnlyList<FrenetProjection> seed, PlanningGridMap map, VehicleParameters vehicle,
+        CorridorConfiguration configuration, out StaticCorridor corridor, out string failureReason)
+    {
+        corridor = null;
+        failureReason = string.Empty;
+        if (!TryValidateInput(segment, startReferenceS, endReferenceS, map, vehicle, configuration, out failureReason))
+            return false;
+        if (!TryReadSeeds(seed, segment, out List<SeedSample> seeds, out failureReason))
+            return false;
+
+        var stations = new List<LateralInterval>();
+        LateralInterval previous = null;
+        foreach (double referenceS in CreateStations(startReferenceS, endReferenceS,
+                     configuration.LongitudinalSampleSpacingMeters))
+        {
+            FrenetReferencePoint reference;
+            try
+            {
+                reference = ReferencePathInterpolator.Interpolate(segment, referenceS);
+            }
+            catch (ArgumentException)
+            {
+                failureReason = "Reference interpolation failed at S=" + referenceS + ".";
+                return false;
+            }
+
+            double seedL = GetSeedL(seeds, referenceS);
+            if (seedL < -configuration.MaximumLateralOffsetMeters - Epsilon ||
+                seedL > configuration.MaximumLateralOffsetMeters + Epsilon)
+            {
+                failureReason = "Seed lateral offset is outside configured bounds at S=" + referenceS + ".";
+                return false;
+            }
+
+            if (!TrySelectSeedConnectedInterval(reference, seedL, map, vehicle, configuration,
+                    out double minimumL, out double maximumL))
+            {
+                failureReason = "Seed-connected free interval disappeared at S=" + referenceS + ".";
+                return false;
+            }
+
+            var selected = new LateralInterval(referenceS, minimumL, maximumL, seedL);
+            if (previous != null && Math.Max(previous.MinimumL, selected.MinimumL) >
+                Math.Min(previous.MaximumL, selected.MaximumL) + Epsilon)
+            {
+                failureReason = "Seed-connected corridor loses overlap at S=" + referenceS + ".";
+                return false;
+            }
+
+            stations.Add(selected);
+            previous = selected;
+        }
+
+        corridor = new StaticCorridor(stations);
+        return true;
+    }
+
+    /// <summary>
+    /// 验证构建走廊所需的段、S 范围、地图、车辆和采样配置。
+    /// 参数：S 锚点和配置距离均为 m，车辆尺寸为 m；返回：地图未就绪、非有限量、越段锚点或非正采样尺度时为 false 并说明原因。
+    /// </summary>
+    private static bool TryValidateInput(DirectionSegmentView segment, double startReferenceS, double endReferenceS,
+        PlanningGridMap map, VehicleParameters vehicle, CorridorConfiguration configuration, out string failureReason)
+    {
+        failureReason = string.Empty;
+        if (segment == null || map == null || vehicle == null || configuration == null || !map.PlanningReady)
+        {
+            failureReason = "A planning-ready map, vehicle, segment, and corridor configuration are required.";
+            return false;
+        }
+        if (!IsFinite(startReferenceS) || !IsFinite(endReferenceS) || startReferenceS < 0d ||
+            endReferenceS < startReferenceS || endReferenceS > segment.LengthMeters + Epsilon)
+        {
+            failureReason = "Requested corridor S anchors are invalid.";
+            return false;
+        }
+        if (!IsPositiveFinite(configuration.LongitudinalSampleSpacingMeters) ||
+            !IsPositiveFinite(configuration.LateralSampleSpacingMeters) ||
+            !IsFinite(configuration.MaximumLateralOffsetMeters) || configuration.MaximumLateralOffsetMeters < 0d ||
+            !IsFinite(configuration.AdditionalClearanceReserveMeters) || configuration.AdditionalClearanceReserveMeters < 0d ||
+            !IsPositiveFinite(configuration.MaximumCollisionCheckStepMeters))
+        {
+            failureReason = "Corridor configuration is invalid.";
+            return false;
+        }
+        if (!IsPositiveFinite(vehicle.LengthMeters) || !IsPositiveFinite(vehicle.WidthMeters) ||
+            !IsFinite(vehicle.SafetyMarginMeters) || vehicle.SafetyMarginMeters < 0d)
+        {
+            failureReason = "Vehicle footprint dimensions are invalid.";
+            return false;
+        }
+        return true;
+    }
+
+    /// <summary>
+    /// 验证并按局部参考 S 排序输入种子投影。
+    /// 参数：input 可为 null（表示中心线 L=0 种子），各投影 S/L 为 m；返回：种子属于当前段且横向量有限时为 true，否则为 false。
+    /// </summary>
+    private static bool TryReadSeeds(IReadOnlyList<FrenetProjection> input, DirectionSegmentView segment,
+        out List<SeedSample> seeds, out string failureReason)
+    {
+        seeds = new List<SeedSample>();
+        failureReason = string.Empty;
+        if (input == null)
+            return true;
+
+        for (int index = 0; index < input.Count; index++)
+        {
+            FrenetProjection projection = input[index];
+            if (projection == null || projection.ReferencePoint == null ||
+                projection.ReferenceS < -Epsilon || projection.ReferenceS > segment.LengthMeters + Epsilon ||
+                !IsFinite(projection.LateralOffset))
+            {
+                failureReason = "Corridor seeds must belong to the selected direction segment.";
+                return false;
+            }
+            seeds.Add(new SeedSample(projection.ReferenceS, projection.LateralOffset));
+        }
+        seeds.Sort((left, right) => left.ReferenceS.CompareTo(right.ReferenceS));
+        return true;
+    }
+
+    /// <summary>
+    /// 生成包含起止锚点的纵向采样站。
+    /// 参数：起止 S 和 spacing 单位均为 m；返回：起点、严格内部等距站以及不同于起点超过 1e-12 m 的终点。
+    /// </summary>
+    private static IEnumerable<double> CreateStations(double startReferenceS, double endReferenceS, double spacing)
+    {
+        yield return startReferenceS;
+        for (double candidate = startReferenceS + spacing; candidate < endReferenceS - Epsilon; candidate += spacing)
+            yield return candidate;
+        if (endReferenceS > startReferenceS + Epsilon)
+            yield return endReferenceS;
+    }
+
+    /// <summary>
+    /// 找出横向离散样本中包含种子的连续无碰撞区间。
+    /// 参数：reference 为世界几何，seedL 及配置横向距离为 m；返回：种子样本自由时给出 [minimumL, maximumL]，碰撞或断连时返回 false。
+    /// </summary>
+    private bool TrySelectSeedConnectedInterval(FrenetReferencePoint reference, double seedL, PlanningGridMap map,
+        VehicleParameters vehicle, CorridorConfiguration configuration, out double minimumL, out double maximumL)
+    {
+        minimumL = 0d;
+        maximumL = 0d;
+        List<double> samples = CreateLateralSamples(seedL, configuration.MaximumLateralOffsetMeters,
+            configuration.LateralSampleSpacingMeters);
+        int index = 0;
+        while (index < samples.Count)
+        {
+            if (!IsCollisionFree(reference, samples[index], map, vehicle, configuration))
+            {
+                index++;
+                continue;
+            }
+
+            double groupMinimum = samples[index];
+            double groupMaximum = samples[index];
+            bool containsSeed = Math.Abs(samples[index] - seedL) <= Epsilon;
+            index++;
+            while (index < samples.Count && samples[index] - groupMaximum <= configuration.LateralSampleSpacingMeters + Epsilon)
+            {
+                if (!IsCollisionFree(reference, samples[index], map, vehicle, configuration))
+                {
+                    index++;
+                    break;
+                }
+                groupMaximum = samples[index];
+                containsSeed |= Math.Abs(samples[index] - seedL) <= Epsilon;
+                index++;
+            }
+
+            if (containsSeed)
+            {
+                minimumL = groupMinimum;
+                maximumL = groupMaximum;
+                return true;
+            }
+        }
+        return false;
+    }
+
+    /// <summary>
+    /// 在配置横向范围内生成有序且包含种子的唯一 L 采样值。
+    /// 参数：seedL、maximumOffset 和 spacing 为 m；返回：包含两端和种子、以 1e-12 m 去重的升序列表。
+    /// </summary>
+    private static List<double> CreateLateralSamples(double seedL, double maximumOffset, double spacing)
+    {
+        var samples = new List<double>();
+        for (double candidate = -maximumOffset; candidate < maximumOffset - Epsilon; candidate += spacing)
+            AddSortedUnique(samples, candidate);
+        AddSortedUnique(samples, maximumOffset);
+        AddSortedUnique(samples, -maximumOffset);
+        AddSortedUnique(samples, seedL);
+        samples.Sort();
+        return samples;
+    }
+
+    /// <summary>
+    /// 重建给定 L 的世界车体位姿并检查其是否无碰撞。
+    /// 参数：lateralOffset 为 m，reference 使用世界 X/Y（m）和 rad 航向，车辆与储备净空为 m；返回：重建奇异、越图或碰撞均为 false。
+    /// </summary>
+    private bool IsCollisionFree(FrenetReferencePoint reference, double lateralOffset, PlanningGridMap map,
+        VehicleParameters vehicle, CorridorConfiguration configuration)
+    {
+        if (!FrenetTransform.TryReconstruct(reference, lateralOffset, 0d, ReconstructionDenominator, out Pose2D pose))
+            return false;
+
+        if (IsObviouslyClear(pose, map, vehicle, configuration.AdditionalClearanceReserveMeters))
+            return true;
+        return _collisionChecker.IsPoseCollisionFree(pose, map, vehicle,
+            configuration.AdditionalClearanceReserveMeters, out _);
+    }
+
+    /// <summary>
+    /// 用保守障碍距离和四个扩张车体角点快速确认明显净空。
+    /// 参数：pose 为世界 X/Y（m）和 rad，车辆尺寸及额外储备为 m；返回：圆形下界安全且四角均在图内时为 true，否则交由精确检查器。
+    /// </summary>
+    private static bool IsObviouslyClear(Pose2D pose, PlanningGridMap map, VehicleParameters vehicle,
+        double additionalClearanceReserveMeters)
+    {
+        double totalMargin = vehicle.SafetyMarginMeters + additionalClearanceReserveMeters;
+        double halfLength = vehicle.LengthMeters / 2d + totalMargin;
+        double halfWidth = vehicle.WidthMeters / 2d + totalMargin;
+        double radius = Math.Sqrt(halfLength * halfLength + halfWidth * halfWidth);
+        if (!IsFinite(radius) || map.GetConservativeObstacleDistanceMeters(pose.X, pose.Y) <= radius)
+            return false;
+
+        double cosine = Math.Cos(pose.Heading);
+        double sine = Math.Sin(pose.Heading);
+        for (int longitudinalSign = -1; longitudinalSign <= 1; longitudinalSign += 2)
+        for (int lateralSign = -1; lateralSign <= 1; lateralSign += 2)
+        {
+            double x = pose.X + longitudinalSign * halfLength * cosine - lateralSign * halfWidth * sine;
+            double y = pose.Y + longitudinalSign * halfLength * sine + lateralSign * halfWidth * cosine;
+            if (!map.TryWorldToGrid(x, y, out _, out _))
+                return false;
+        }
+        return true;
+    }
+
+    /// <summary>
+    /// 以相邻种子 S 线性插值得到采样站的横向种子。
+    /// 参数：seeds 已按局部 S（m）升序，referenceS 为 m；返回：区间外保持端点 L，重合 S 间隔不超过 1e-12 m 时取上端 L。
+    /// </summary>
+    private static double GetSeedL(IReadOnlyList<SeedSample> seeds, double referenceS)
+    {
+        if (seeds.Count == 0)
+            return 0d;
+        if (referenceS <= seeds[0].ReferenceS)
+            return seeds[0].LateralOffset;
+        for (int index = 1; index < seeds.Count; index++)
+        {
+            SeedSample upper = seeds[index];
+            if (referenceS <= upper.ReferenceS)
+            {
+                SeedSample lower = seeds[index - 1];
+                double span = upper.ReferenceS - lower.ReferenceS;
+                return span <= Epsilon ? upper.LateralOffset : lower.LateralOffset +
+                    (upper.LateralOffset - lower.LateralOffset) * (referenceS - lower.ReferenceS) / span;
+            }
+        }
+        return seeds[seeds.Count - 1].LateralOffset;
+    }
+
+    /// <summary>
+    /// 向样本列表加入未在 1e-12 m 容差内出现过的横向值。
+    /// 参数：samples 保存 m 制 L 值，value 为待加入 L（m）；返回：重复近似值被拒绝，唯一值追加后由调用方排序。
+    /// </summary>
+    private static void AddSortedUnique(List<double> samples, double value)
+    {
+        for (int index = 0; index < samples.Count; index++)
+            if (Math.Abs(samples[index] - value) <= Epsilon)
+                return;
+        samples.Add(value);
+    }
+
+    /// <summary>
+    /// 判定采样步长或车辆尺寸是否为正的有限值。
+    /// 参数：value 为对应单位的标量；返回：仅有限且严格大于零时为 true。
+    /// </summary>
+    private static bool IsPositiveFinite(double value)
+    {
+        return IsFinite(value) && value > 0d;
+    }
+
+    /// <summary>
+    /// 判定走廊计算输入是否为有限实数。
+    /// 参数：value 为任意标量；返回：NaN 和无穷时为 false。
+    /// </summary>
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+
+    /// <summary>
+    /// 走廊种子在一个局部参考站上的轻量不可变记录。
+    /// ReferenceS 与 LateralOffset 均以 m 计，仅用于站间种子插值。
+    /// </summary>
+    private sealed class SeedSample
+    {
+        /// <summary>
+        /// 创建一个种子记录。
+        /// 参数：referenceS 为段局部 S（m），lateralOffset 为行驶坐标系中的 L（m）。
+        /// </summary>
+        public SeedSample(double referenceS, double lateralOffset)
+        {
+            ReferenceS = referenceS;
+            LateralOffset = lateralOffset;
+        }
+
+        /// <summary>
+        /// 种子所在方向段局部参考弧长 S，单位 m。
+        /// </summary>
+        public double ReferenceS { get; }
+
+        /// <summary>
+        /// 种子相对中心线的横向偏移 L，单位 m。
+        /// </summary>
+        public double LateralOffset { get; }
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Diagnostics/EmPlannerDebugOptions.cs

@@ -0,0 +1,13 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class EmPlannerDebugOptions
+{
+    public bool EnableSummary { get; set; }
+    public bool EnableProjectionTrace { get; set; }
+    public bool EnableCorridorTrace { get; set; }
+    public bool EnableLateralSolverTrace { get; set; }
+    public bool EnableLongitudinalSolverTrace { get; set; }
+    public bool EnableTrajectoryDump { get; set; }
+    public bool EnableVisualization { get; set; }
+    public IEmPlannerDebugSink Sink { get; set; }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Diagnostics/EmPlanningDiagnostics.cs

@@ -0,0 +1,30 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class EmPlanningDiagnostics
+{
+    private readonly List<string> _debugSinkFailures = new List<string>();
+
+    public IReadOnlyList<string> DebugSinkFailures
+    {
+        get { return new ReadOnlyCollection<string>(new List<string>(_debugSinkFailures)); }
+    }
+
+    public void WriteDebug(EmPlannerDebugOptions options, string message)
+    {
+        if (options == null || options.Sink == null)
+            return;
+
+        try
+        {
+            options.Sink.Write(message ?? string.Empty);
+        }
+        catch (Exception exception)
+        {
+            _debugSinkFailures.Add(exception.GetType().FullName + ": " + exception.Message);
+        }
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Diagnostics/IEmPlannerDebugSink.cs

@@ -0,0 +1,6 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public interface IEmPlannerDebugSink
+{
+    void Write(string message);
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs

@@ -0,0 +1,455 @@
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.Globalization;
+using System.Threading;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Runs one deterministic EM LS/ST planning pipeline and publishes only independently validated trajectories.</summary>
+public sealed class EmPlanningService : IEmPlanningService
+{
+    private static readonly TimeSpan MaximumCycleDeadlineRemaining =
+        TimeSpan.FromMilliseconds(int.MaxValue);
+    private readonly IQpSolver qpSolver;
+    private readonly IEmPlannerDebugSink defaultDebugSink;
+    private readonly Func<TimeSpan> cycleElapsedForTesting;
+
+    public EmPlanningService(IQpSolver qpSolver, IEmPlannerDebugSink defaultDebugSink = null)
+        : this(qpSolver, defaultDebugSink, null)
+    {
+    }
+
+    internal EmPlanningService(IQpSolver qpSolver, IEmPlannerDebugSink defaultDebugSink,
+        Func<TimeSpan> cycleElapsedForTesting)
+    {
+        this.qpSolver = qpSolver ?? throw new ArgumentNullException(nameof(qpSolver));
+        this.defaultDebugSink = defaultDebugSink;
+        this.cycleElapsedForTesting = cycleElapsedForTesting;
+    }
+
+    public EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken)
+    {
+        Stopwatch cycleStopwatch = cycleElapsedForTesting == null ? Stopwatch.StartNew() : null;
+        Func<TimeSpan> cycleElapsed = cycleElapsedForTesting ?? (() => cycleStopwatch.Elapsed);
+        if (CallerCancellationRequested(request, cancellationToken))
+            return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled before request validation.");
+        if (request?.CycleDeadlineRemaining is TimeSpan suppliedRemaining &&
+            (suppliedRemaining < TimeSpan.Zero || suppliedRemaining > MaximumCycleDeadlineRemaining))
+        {
+            return Failure(EmPlanningStatus.InvalidInput, request,
+                "CycleDeadlineRemaining must be between zero and " +
+                MaximumCycleDeadlineRemaining.TotalMilliseconds + " milliseconds.");
+        }
+        if (DeadlineExpired(request, cycleElapsed))
+            return CycleDeadlineFailure(request, "request", CycleRemaining(request, cycleElapsed));
+
+        EmPlanningRequestValidationResult requestValidation = EmPlanningRequestValidator.Validate(request);
+        if (!requestValidation.IsValid)
+            return Failure(requestValidation.Status, request, requestValidation.FailureReason);
+        EmPlannerConfiguration configuration = requestValidation.Snapshot.Configuration;
+        EmitDebug(request, "request/config validation succeeded");
+        if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
+                "request-validation", out EmPlanningResult validationFailure))
+        {
+            return validationFailure;
+        }
+        TimeSpan expirationRemaining = CycleRemaining(request, cycleElapsed);
+        using var cycleExpiration = request.CycleDeadlineRemaining.HasValue
+            ? new CancellationTokenSource(expirationRemaining)
+            : null;
+        using var linkedCancellation = CancellationTokenSource.CreateLinkedTokenSource(
+            cancellationToken, request.CallerCancellationToken, request.CycleDeadlineToken,
+            cycleExpiration?.Token ?? CancellationToken.None);
+        CancellationToken planningCancellationToken = linkedCancellation.Token;
+
+        try
+        {
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
+                    "projection", out EmPlanningResult deadlineFailure))
+                return deadlineFailure;
+            IReadOnlyList<DirectionSegmentView> segments = ReferencePathSegmenter.Create(request.ReferencePath);
+            if (request.SegmentIndex < 0 || request.SegmentIndex >= segments.Count)
+                return Failure(EmPlanningStatus.InvalidReferencePath, request, "The requested direction segment is unavailable.");
+            DirectionSegmentView segment = segments[request.SegmentIndex];
+            if (!HasCompatibleStateDirection(request.VehicleState, segment.Direction,
+                    configuration.Longitudinal.StopSpeedToleranceMetersPerSecond))
+            {
+                return Failure(EmPlanningStatus.StateDirectionMismatch, request,
+                    "Vehicle signed speed contradicts the selected direction segment.");
+            }
+            EmitDebug(request, "direction-segment selection succeeded");
+
+            var projector = new FrenetProjector();
+            double startProjectionUpperS = request.PlanningScope == EmPlanningScope.FullDirectionSegment
+                ? Math.Min(segment.LengthMeters, configuration.Frenet.MaximumProjectionDistanceMeters)
+                : segment.LengthMeters;
+            if (!projector.TryProject(request.VehicleState.Pose, segment, 0d, startProjectionUpperS,
+                    configuration.Frenet.MaximumProjectionDistanceMeters, 0d, out FrenetProjection startProjection))
+            {
+                return Failure(EmPlanningStatus.ProjectionFailed, request,
+                    "Vehicle pose could not be projected at an admissible start of the selected direction segment.");
+            }
+            if (Math.Abs(startProjection.HeadingError) >= Math.PI / 2d)
+            {
+                return Failure(EmPlanningStatus.ProjectionFailed, request,
+                    "Vehicle travel heading differs by at least 90 degrees from the selected direction segment.");
+            }
+            EmitDebug(request, "bounded ego projection succeeded");
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "projection", out deadlineFailure))
+                return deadlineFailure;
+
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "corridor", out deadlineFailure))
+                return deadlineFailure;
+            double initialProgressSpeed = Math.Abs(request.VehicleState.SignedLongitudinalSpeedMetersPerSecond);
+            double initialAcceleration = request.VehicleState.LongitudinalAccelerationMetersPerSecondSquared ?? 0d;
+            var horizonSelector = new PlanningHorizonSelector();
+            EmPlanningStatus horizonStatus = horizonSelector.Select(segment, startProjection.ReferenceS, initialProgressSpeed,
+                initialAcceleration, request.PlanningScope, configuration, out PlanningHorizonSelection horizon,
+                out string horizonReason);
+            if (horizonStatus != EmPlanningStatus.Success)
+                return Failure(horizonStatus, request, horizonReason);
+            ReferenceHorizonSlice slice = ReferenceHorizonSlicer.Slice(segment, horizon.WindowEndReferenceS);
+            EmitDebug(request, "exact horizon and terminal selection succeeded");
+
+            IReadOnlyList<FrenetProjection> previousSeed = ProjectPreviousTrajectorySeed(request.PreviousTrajectory, segment,
+                startProjection.ReferenceS, horizon.WindowEndReferenceS, configuration.Frenet.MaximumProjectionDistanceMeters);
+            var corridorSeed = new List<FrenetProjection>(previousSeed.Count + 1) { startProjection };
+            for (int index = 0; index < previousSeed.Count; index++) corridorSeed.Add(previousSeed[index]);
+            EmitDebug(request, "previous-trajectory seed projection completed");
+
+            var corridorBuilder = new StaticCorridorBuilder();
+            if (!corridorBuilder.TryBuild(segment, startProjection.ReferenceS, slice.TerminalBoundary.SegmentLocalS, corridorSeed,
+                    request.Map, request.Vehicle, configuration.Corridor, out StaticCorridor corridor, out string corridorReason))
+            {
+                return Failure(EmPlanningStatus.CorridorInfeasible, request, corridorReason);
+            }
+            EmitDebug(request, "static connected corridor succeeded");
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "corridor", out deadlineFailure))
+                return deadlineFailure;
+
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "lateral", out deadlineFailure))
+                return deadlineFailure;
+            TimeSpan lateralBudget = SmallerBudget(
+                TimeSpan.FromSeconds(configuration.Scheduling.SolverTimeoutSeconds),
+                CycleRemaining(request, cycleElapsed));
+            EmPlannerConfiguration lateralConfiguration = configuration.Copy();
+            lateralConfiguration.Scheduling.SolverTimeoutSeconds = lateralBudget.TotalSeconds;
+            var lateralInput = new LateralPlanningInput(segment, corridor, startProjection, horizon.TerminalType,
+                request.Vehicle, lateralConfiguration, previousSeed);
+            TimeSpan totalSolveBudget = lateralBudget;
+            var solveBudgetStopwatch = Stopwatch.StartNew();
+            LateralPlanningResult lateral = new LateralPlanner(qpSolver).Plan(lateralInput, planningCancellationToken);
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "lateral", out deadlineFailure))
+                return deadlineFailure;
+            if (!IsSuccess(lateral.Status))
+                return Failure(lateral.Status, request, lateral.FailureReason);
+            if (cancellationToken.IsCancellationRequested)
+                return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled after LS optimization.");
+            EmitDebug(request, "LS optimization and validation succeeded");
+
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "envelope", out deadlineFailure))
+                return deadlineFailure;
+            EmPlanningStatus envelopeStatus = new PathSpeedLimitBuilder().Build(lateral.Path, segment.Direction,
+                initialProgressSpeed, initialAcceleration, horizon.TerminalType, configuration, out PathSpeedLimit speedLimit,
+                out string envelopeReason);
+            if (envelopeStatus != EmPlanningStatus.Success)
+                return Failure(envelopeStatus, request, envelopeReason);
+            LongitudinalKnotSchedule knotSchedule;
+            if (request.PlanningScope == EmPlanningScope.FullDirectionSegment)
+            {
+                EmPlanningStatus scheduleStatus = new FullDirectionSegmentScheduleBuilder().TryBuild(lateral.Path, speedLimit,
+                    initialProgressSpeed, initialAcceleration, DesiredSpeed(configuration, segment.Direction), configuration,
+                    out knotSchedule, out string scheduleReason);
+                if (scheduleStatus != EmPlanningStatus.Success)
+                    return Failure(scheduleStatus, request, scheduleReason);
+            }
+            else
+            {
+                knotSchedule = LongitudinalKnotSchedule.CreateRolling(configuration.Scheduling.TimeHorizonSeconds,
+                    configuration.Scheduling.OutputTimeStepSeconds);
+            }
+            LongitudinalPreviousTrajectorySeed previousLongitudinalSeed =
+                new LongitudinalPreviousTrajectorySeedBuilder().Build(
+                    request.PreviousTrajectory, lateral.Path, request.EffectiveAtUtc, knotSchedule,
+                    segment.SegmentIndex, segment.Direction);
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "envelope", out deadlineFailure))
+                return deadlineFailure;
+            TimeSpan remainingSolveBudget = totalSolveBudget - solveBudgetStopwatch.Elapsed;
+            remainingSolveBudget = SmallerBudget(remainingSolveBudget, CycleRemaining(request, cycleElapsed));
+            if (remainingSolveBudget <= TimeSpan.Zero)
+            {
+                if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
+                        "longitudinal", out deadlineFailure))
+                    return deadlineFailure;
+                return Failure(EmPlanningStatus.SolverTimedOut, request,
+                    "LS/ST optimization exhausted the shared solve budget before ST optimization.");
+            }
+            EmPlannerConfiguration longitudinalConfiguration = configuration.Copy();
+            longitudinalConfiguration.Scheduling.SolverTimeoutSeconds = remainingSolveBudget.TotalSeconds;
+            var longitudinalInput = new LongitudinalPlanningInput(lateral.Path, segment.Direction, initialProgressSpeed,
+                initialAcceleration, horizon.TerminalType, horizon.LongitudinalMode, longitudinalConfiguration,
+                request.PlanningScope, knotSchedule,
+                previousLongitudinalSeed.PathS, previousLongitudinalSeed.ProgressSpeedMetersPerSecond);
+            envelopeStatus = new PathSpeedLimitBuilder().Build(longitudinalInput, out _, out envelopeReason);
+            if (envelopeStatus != EmPlanningStatus.Success)
+                return Failure(envelopeStatus, request, envelopeReason);
+            EmitDebug(request, "PathS speed envelope succeeded");
+
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
+                    "longitudinal", out deadlineFailure))
+                return deadlineFailure;
+            LongitudinalPlanningResult longitudinal = new LongitudinalPlanner(qpSolver).Plan(
+                longitudinalInput, planningCancellationToken);
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
+                    "longitudinal", out deadlineFailure))
+                return deadlineFailure;
+            if (!IsSuccess(longitudinal.Status))
+                return Failure(longitudinal.Status, request, longitudinal.FailureReason);
+            if (cancellationToken.IsCancellationRequested)
+                return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled after ST optimization.");
+            EmitDebug(request, "ST optimization and validation succeeded");
+
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "assembly", out deadlineFailure))
+                return deadlineFailure;
+            var metadata = new EmTrajectoryMetadata(request.OutputTrajectoryId, request.RequestedAtUtc, request.EffectiveAtUtc,
+                request.Map.SnapshotId, request.ReferencePathId, request.VehicleState.SequenceId, request.PreviousTrajectoryId,
+                segment.SegmentIndex, segment.Direction, horizon.TerminalType, horizon.LongitudinalMode,
+                request.PlanningScope);
+            EmPlanningStatus assemblyStatus = new EmTrajectoryAssembler(configuration).TryAssemble(lateral.Path,
+                longitudinal, metadata, out EmTrajectory trajectory, out string assemblyFailure);
+            if (assemblyStatus != EmPlanningStatus.Success)
+                return Failure(assemblyStatus, request, assemblyFailure);
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "assembly", out deadlineFailure))
+                return deadlineFailure;
+            if (cancellationToken.IsCancellationRequested)
+                return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled after trajectory assembly.");
+            EmitDebug(request, "trajectory assembly succeeded");
+
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
+                    "world-validation", out deadlineFailure))
+                return deadlineFailure;
+            EmBoundaryType terminalBoundary = slice.TerminalBoundary.BoundaryType;
+            Pose2D terminalPose = IsRealTerminalBoundary(terminalBoundary)
+                ? TerminalPose(lateral.Path)
+                : null;
+            EmTrajectoryValidationResult publication = new EmTrajectoryValidator().Validate(trajectory, request.Map,
+                request.Vehicle, configuration, segment.SegmentIndex, segment.LengthMeters, longitudinalInput.PathUpperBoundS,
+                terminalPose, terminalBoundary);
+            if (!publication.IsValid)
+            {
+                return Failure(MapPublicationFailure(publication.Failure), request,
+                    publication.Failure + " at point " + publication.PointIndex + ": " + publication.Message);
+            }
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
+                    "world-validation", out deadlineFailure))
+                return deadlineFailure;
+            EmitDebug(request, "world-space publication validation succeeded");
+
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "publication", out deadlineFailure))
+                return deadlineFailure;
+            if (cancellationToken.IsCancellationRequested)
+                return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled before trajectory publication.");
+
+            EmPlanningStatus finalStatus = lateral.Status == EmPlanningStatus.SuccessWithFallback ||
+                longitudinal.Status == EmPlanningStatus.SuccessWithFallback
+                ? EmPlanningStatus.SuccessWithFallback
+                : EmPlanningStatus.Success;
+            string publicationDiagnostic = DiagnosticsPrefix(request) + ";terminal=" + horizon.TerminalType +
+                ";publication=validated";
+            if (lateral.Status == EmPlanningStatus.SuccessWithFallback)
+                publicationDiagnostic += ";lateralFallback=" + lateral.FailureReason;
+            if (!string.IsNullOrWhiteSpace(longitudinal.FailureReason))
+            {
+                publicationDiagnostic += longitudinal.Status == EmPlanningStatus.SuccessWithFallback
+                    ? ";longitudinalFallback=" + longitudinal.FailureReason
+                    : ";longitudinal=" + longitudinal.FailureReason;
+            }
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "publication", out deadlineFailure))
+                return deadlineFailure;
+            return new EmPlanningResult(finalStatus, trajectory, publicationDiagnostic);
+        }
+        catch (OperationCanceledException)
+        {
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
+                    "operation", out EmPlanningResult deadlineFailure))
+                return deadlineFailure;
+            return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled.");
+        }
+        catch (ArgumentException exception)
+        {
+            return Failure(EmPlanningStatus.Failed, request, exception.Message);
+        }
+        catch (InvalidOperationException exception)
+        {
+            return Failure(EmPlanningStatus.Failed, request, exception.Message);
+        }
+    }
+
+    private static bool TryTerminalFailure(EmPlanningRequest request, CancellationToken cancellationToken,
+        Func<TimeSpan> elapsed, string phase, out EmPlanningResult failure)
+    {
+        if (CallerCancellationRequested(request, cancellationToken))
+        {
+            failure = Failure(EmPlanningStatus.Cancelled, request,
+                "callerCancellation=true;phase=" + phase);
+            return true;
+        }
+        TimeSpan remaining = CycleRemaining(request, elapsed);
+        if (!DeadlineExpired(request, elapsed))
+        {
+            failure = null;
+            return false;
+        }
+        failure = CycleDeadlineFailure(request, phase, remaining);
+        return true;
+    }
+
+    private static bool CallerCancellationRequested(EmPlanningRequest request,
+        CancellationToken cancellationToken)
+    {
+        if (request?.CallerCancellationToken.IsCancellationRequested == true)
+            return true;
+        if (!cancellationToken.IsCancellationRequested)
+            return false;
+        return request == null || !request.CycleDeadlineToken.IsCancellationRequested ||
+            !request.CallerCancellationToken.CanBeCanceled;
+    }
+
+    private static bool DeadlineExpired(EmPlanningRequest request, Func<TimeSpan> elapsed)
+    {
+        return request != null && (request.CycleDeadlineToken.IsCancellationRequested ||
+            request.IsCycleDeadlineExpired() ||
+            request.CycleDeadlineRemaining.HasValue && CycleRemaining(request, elapsed) <= TimeSpan.Zero);
+    }
+
+    private static TimeSpan CycleRemaining(EmPlanningRequest request, Func<TimeSpan> elapsed)
+    {
+        TimeSpan snapshotRemaining = TimeSpan.MaxValue;
+        if (request.CycleDeadlineRemaining.HasValue)
+        {
+            snapshotRemaining = request.CycleDeadlineRemaining.Value - elapsed();
+            if (snapshotRemaining < TimeSpan.Zero)
+                snapshotRemaining = TimeSpan.Zero;
+        }
+        TimeSpan? dynamicRemaining = request.DynamicCycleDeadlineRemaining();
+        if (!dynamicRemaining.HasValue)
+            return snapshotRemaining;
+        return dynamicRemaining.Value < snapshotRemaining ? dynamicRemaining.Value : snapshotRemaining;
+    }
+
+    private static TimeSpan SmallerBudget(TimeSpan configured, TimeSpan cycleRemaining)
+    {
+        return configured < cycleRemaining ? configured : cycleRemaining;
+    }
+
+    private static EmPlanningResult CycleDeadlineFailure(EmPlanningRequest request, string phase,
+        TimeSpan remaining)
+    {
+        return Failure(EmPlanningStatus.CycleDeadlineExpired, request,
+            "cycleDeadlineExpired=true;phase=" + phase + ";remainingMs=" +
+            remaining.TotalMilliseconds.ToString("F3", CultureInfo.InvariantCulture));
+    }
+
+    private void EmitDebug(EmPlanningRequest request, string message)
+    {
+        if (defaultDebugSink == null || request == null || request.Configuration == null || request.Configuration.Solver == null ||
+            !request.Configuration.Solver.NativeVerbose)
+        {
+            return;
+        }
+        try
+        {
+            defaultDebugSink.Write(message ?? string.Empty);
+        }
+        catch
+        {
+            // Debug output is deliberately isolated from pure planning results.
+        }
+    }
+
+    private static IReadOnlyList<FrenetProjection> ProjectPreviousTrajectorySeed(EmTrajectory previousTrajectory,
+        DirectionSegmentView segment, double minimumReferenceS, double maximumReferenceS, double maximumDistanceMeters)
+    {
+        var projected = new List<FrenetProjection>();
+        if (previousTrajectory == null || previousTrajectory.Metadata.SegmentIndex != segment.SegmentIndex ||
+            previousTrajectory.Metadata.Direction != segment.Direction)
+        {
+            return projected;
+        }
+
+        var projector = new FrenetProjector();
+        double seedReferenceS = minimumReferenceS;
+        for (int index = 0; index < previousTrajectory.Points.Count; index++)
+        {
+            EmTrajectoryPoint point = previousTrajectory.Points[index];
+            if (point == null || point.TimeFromStart <= 0d || point.Direction != segment.Direction)
+                continue;
+            if (projector.TryProject(new Pose2D(point.X, point.Y, point.Yaw), segment, minimumReferenceS,
+                    maximumReferenceS, maximumDistanceMeters, seedReferenceS, out FrenetProjection projection))
+            {
+                projected.Add(projection);
+                seedReferenceS = projection.ReferenceS;
+            }
+        }
+        return projected;
+    }
+
+    private static bool HasCompatibleStateDirection(VehicleMotionState state, TravelDirection direction, double stopTolerance)
+    {
+        if (state == null || double.IsNaN(stopTolerance) || double.IsInfinity(stopTolerance) || stopTolerance < 0d)
+            return false;
+        if (Math.Abs(state.SignedLongitudinalSpeedMetersPerSecond) <= stopTolerance)
+            return true;
+        return direction == TravelDirection.Forward
+            ? state.SignedLongitudinalSpeedMetersPerSecond > 0d
+            : state.SignedLongitudinalSpeedMetersPerSecond < 0d;
+    }
+
+    private static double DesiredSpeed(EmPlannerConfiguration configuration, TravelDirection direction)
+    {
+        return direction == TravelDirection.Forward
+            ? configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond
+            : configuration.Longitudinal.DesiredReverseSpeedMetersPerSecond;
+    }
+
+    private static bool IsSuccess(EmPlanningStatus status)
+    {
+        return status == EmPlanningStatus.Success || status == EmPlanningStatus.SuccessWithFallback;
+    }
+
+    internal static EmPlanningStatus MapPublicationFailure(EmTrajectoryValidationFailure failure)
+    {
+        return failure == EmTrajectoryValidationFailure.TerminalPoseMismatch
+            ? EmPlanningStatus.TerminalPoseMismatch
+            : EmPlanningStatus.ValidationFailed;
+    }
+
+    private static bool IsRealTerminalBoundary(EmBoundaryType boundaryType)
+    {
+        return boundaryType == EmBoundaryType.Goal || boundaryType == EmBoundaryType.GearSwitchApproach;
+    }
+
+    private static Pose2D TerminalPose(LateralPath path)
+    {
+        LateralPathPoint terminal = path.Points[path.Points.Count - 1];
+        return new Pose2D(terminal.X, terminal.Y, terminal.VehicleYaw);
+    }
+
+    private static EmPlanningResult Failure(EmPlanningStatus status, EmPlanningRequest request, string reason)
+    {
+        return new EmPlanningResult(status, null, DiagnosticsPrefix(request) + ";reason=" + (reason ?? string.Empty));
+    }
+
+    private static string DiagnosticsPrefix(EmPlanningRequest request)
+    {
+        if (request == null)
+            return "map=;reference=;state=;previous=;segment=";
+        return "map=" + (request.Map == null ? string.Empty : request.Map.SnapshotId.ToString()) +
+            ";reference=" + (request.ReferencePathId ?? string.Empty) +
+            ";state=" + (request.VehicleState == null ? string.Empty : request.VehicleState.SequenceId.ToString()) +
+            ";previous=" + (request.PreviousTrajectoryId ?? string.Empty) +
+            ";segment=" + request.SegmentIndex + ";scope=" + request.PlanningScope;
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/IEmPlanningService.cs

@@ -0,0 +1,9 @@
+using System.Threading;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Pure, one-shot EM planning boundary with no scheduler, UI, hardware, or clock dependency.</summary>
+public interface IEmPlanningService
+{
+    EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken);
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/FrenetProjection.cs

@@ -0,0 +1,64 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 世界车辆位姿投影到单一受限方向段后的 Frenet 描述。
+/// S 与 L 单位均为 m，航向误差为 rad，距离以 m² 保存；对象不代表跨越换向边界的投影。
+/// </summary>
+public sealed class FrenetProjection
+{
+    /// <summary>
+    /// 从参考点和相对量创建不可变投影结果。
+    /// 参数：lateralOffset 为沿行驶方向左法线的 L（m），headingError 为行驶航向差（rad），squaredDistanceMeters 为非负 m²。
+    /// 返回：携带参考 S 的投影；空参考点、非有限值或负平方距离会被拒绝。
+    /// </summary>
+    public FrenetProjection(FrenetReferencePoint referencePoint, double lateralOffset, double headingError,
+        double squaredDistanceMeters)
+    {
+        if (referencePoint == null)
+            throw new ArgumentNullException(nameof(referencePoint));
+        if (!IsFinite(lateralOffset) || !IsFinite(headingError) || !IsFinite(squaredDistanceMeters) || squaredDistanceMeters < 0d)
+            throw new ArgumentOutOfRangeException(nameof(lateralOffset));
+
+        ReferencePoint = referencePoint;
+        ReferenceS = referencePoint.ReferenceS;
+        LateralOffset = lateralOffset;
+        HeadingError = headingError;
+        SquaredDistanceMeters = squaredDistanceMeters;
+    }
+
+    /// <summary>
+    /// 投影命中的插值参考点，其坐标为世界 X/Y（m）。
+    /// </summary>
+    public FrenetReferencePoint ReferencePoint { get; }
+
+    /// <summary>
+    /// 命中点在当前方向段的局部参考弧长 S，单位 m。
+    /// </summary>
+    public double ReferenceS { get; }
+
+    /// <summary>
+    /// 世界位姿相对参考行驶方向左法线的横向偏移 L，单位 m。
+    /// </summary>
+    public double LateralOffset { get; }
+
+    /// <summary>
+    /// 车辆行驶航向相对参考行驶航向的归一化误差，单位 rad。
+    /// </summary>
+    public double HeadingError { get; }
+
+    /// <summary>
+    /// 世界位置与命中参考位置的欧氏距离平方，单位 m²。
+    /// </summary>
+    public double SquaredDistanceMeters { get; }
+
+    /// <summary>
+    /// 判定投影标量是否有限。
+    /// 参数：value 为任意实数；返回：NaN 和正负无穷均返回 false。
+    /// </summary>
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/FrenetProjector.cs

@@ -0,0 +1,188 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 在指定方向段及局部 S 窗口内确定性地投影世界位姿。
+/// 投影不跨越换向边界；世界 X/Y 与距离为 m，航向为 rad，并以种子 S 消除等距候选的拓扑歧义。
+/// </summary>
+public sealed class FrenetProjector
+{
+    /// <summary>
+    /// 等距候选比较和零长度线段识别使用的 S/距离平方数值容差 1e-14。
+    /// </summary>
+    private const double TieTolerance = 1e-14d;
+
+    /// <summary>
+    /// 在窗口内投影世界位姿，并以窗口起点作为等距候选的种子 S。
+    /// 参数：窗口与 maximumDistanceMeters 均为当前段局部 m 制距离；返回：命中距离不超过阈值时返回 true，否则 projection 为 null。
+    /// </summary>
+    public bool TryProject(Pose2D worldPose, DirectionSegmentView segment, double minimumReferenceS,
+        double maximumReferenceS, double maximumDistanceMeters, out FrenetProjection projection)
+    {
+        return TryProject(worldPose, segment, minimumReferenceS, maximumReferenceS, maximumDistanceMeters,
+            minimumReferenceS, out projection);
+    }
+
+    /// <summary>
+    /// 在窗口内投影世界位姿，按距离、种子 S 距离及较小 S 的顺序稳定选择候选。
+    /// 参数：worldPose 使用世界 X/Y（m）和车身航向（rad），S 窗口、距离阈值和种子均为 m；返回：窗口非法、无候选或候选超阈值时返回 false。
+    /// </summary>
+    public bool TryProject(Pose2D worldPose, DirectionSegmentView segment, double minimumReferenceS,
+        double maximumReferenceS, double maximumDistanceMeters, double seedReferenceS, out FrenetProjection projection)
+    {
+        projection = null;
+        if (worldPose == null || segment == null || !IsFinite(worldPose.X) || !IsFinite(worldPose.Y) ||
+            !IsFinite(worldPose.Heading) || !IsFinite(minimumReferenceS) || !IsFinite(maximumReferenceS) ||
+            !IsFinite(maximumDistanceMeters) || !IsFinite(seedReferenceS) || maximumDistanceMeters < 0d)
+            return false;
+
+        double lowerBound = Math.Max(0d, minimumReferenceS);
+        double upperBound = Math.Min(segment.LengthMeters, maximumReferenceS);
+        if (lowerBound > upperBound)
+            return false;
+
+        Candidate best = null;
+        for (int index = 0; index + 1 < segment.Points.Count; index++)
+        {
+            double startS = Math.Max(lowerBound, segment.Points[index].ArcLength);
+            double endS = Math.Min(upperBound, segment.Points[index + 1].ArcLength);
+            if (startS > endS)
+                continue;
+
+            FrenetReferencePoint start = ReferencePathInterpolator.Interpolate(segment, startS);
+            FrenetReferencePoint end = ReferencePathInterpolator.Interpolate(segment, endS);
+            ConsiderLine(worldPose, start, end, seedReferenceS, ref best);
+        }
+
+        if (best == null && Math.Abs(lowerBound - upperBound) <= TieTolerance)
+        {
+            FrenetReferencePoint point = ReferencePathInterpolator.Interpolate(segment, lowerBound);
+            ConsiderPoint(worldPose, point, seedReferenceS, ref best);
+        }
+
+        if (best == null || best.SquaredDistanceMeters > maximumDistanceMeters * maximumDistanceMeters)
+            return false;
+
+        FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(segment, best.ReferenceS);
+        double dx = worldPose.X - reference.X;
+        double dy = worldPose.Y - reference.Y;
+        double travelYaw = reference.TravelYaw;
+        double lateralOffset = -dx * Math.Sin(travelYaw) + dy * Math.Cos(travelYaw);
+        double egoTravelYaw = FrenetTransform.GetTravelYaw(worldPose.Heading, segment.Direction);
+        double headingError = AngleMath.NormalizeRadians(egoTravelYaw - travelYaw);
+        projection = new FrenetProjection(reference, lateralOffset, headingError, best.SquaredDistanceMeters);
+        return true;
+    }
+
+    /// <summary>
+    /// 考察被 S 窗口裁剪后的参考折线边，并将世界点的正交投影加入最优候选比较。
+    /// 参数：端点为世界 X/Y（m）和局部 S（m），seedReferenceS 为 m；零长度（不超过 1e-14）边退化为两个端点比较。
+    /// </summary>
+    private static void ConsiderLine(Pose2D worldPose, FrenetReferencePoint start, FrenetReferencePoint end,
+        double seedReferenceS, ref Candidate best)
+    {
+        double dx = end.X - start.X;
+        double dy = end.Y - start.Y;
+        double lengthSquared = dx * dx + dy * dy;
+        if (lengthSquared <= TieTolerance)
+        {
+            ConsiderPoint(worldPose, start, seedReferenceS, ref best);
+            ConsiderPoint(worldPose, end, seedReferenceS, ref best);
+            return;
+        }
+
+        double fraction = ((worldPose.X - start.X) * dx + (worldPose.Y - start.Y) * dy) / lengthSquared;
+        fraction = Math.Max(0d, Math.Min(1d, fraction));
+        double referenceS = start.ReferenceS + (end.ReferenceS - start.ReferenceS) * fraction;
+        double x = start.X + dx * fraction;
+        double y = start.Y + dy * fraction;
+        Consider(worldPose, referenceS, x, y, seedReferenceS, ref best);
+    }
+
+    /// <summary>
+    /// 将一个离散参考点作为投影候选参与比较。
+    /// 参数：point 的位置为世界 m 制坐标，seedReferenceS 为局部 m 制种子；结果通过 best 原位更新。
+    /// </summary>
+    private static void ConsiderPoint(Pose2D worldPose, FrenetReferencePoint point, double seedReferenceS,
+        ref Candidate best)
+    {
+        Consider(worldPose, point.ReferenceS, point.X, point.Y, seedReferenceS, ref best);
+    }
+
+    /// <summary>
+    /// 依据世界平面平方距离登记候选，并按确定性优先级替换当前最佳值。
+    /// 参数：referenceS、x、y 与 seedReferenceS 分别为局部 S（m）和世界坐标（m）；平方距离由内部计算，单位 m²。
+    /// </summary>
+    private static void Consider(Pose2D worldPose, double referenceS, double x, double y, double seedReferenceS,
+        ref Candidate best)
+    {
+        double dx = worldPose.X - x;
+        double dy = worldPose.Y - y;
+        double squaredDistance = dx * dx + dy * dy;
+        var candidate = new Candidate(referenceS, squaredDistance, Math.Abs(referenceS - seedReferenceS));
+        if (best == null || candidate.IsPreferredTo(best))
+            best = candidate;
+    }
+
+    /// <summary>
+    /// 判定投影计算的标量是否有限。
+    /// 参数：value 为任意实数；返回：NaN 与无穷均返回 false。
+    /// </summary>
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+
+    /// <summary>
+    /// 保存一个待比较的折线投影候选。
+    /// ReferenceS 和 SeedDistance 使用 m，SquaredDistanceMeters 使用 m²，优先级由距离、种子距离及较小 S 依次决定。
+    /// </summary>
+    private sealed class Candidate
+    {
+        /// <summary>
+        /// 创建投影候选。
+        /// 参数：referenceS 与 seedDistance 为 m，squaredDistanceMeters 为 m²；调用方仅传入有限的已计算值。
+        /// </summary>
+        public Candidate(double referenceS, double squaredDistanceMeters, double seedDistance)
+        {
+            ReferenceS = referenceS;
+            SquaredDistanceMeters = squaredDistanceMeters;
+            SeedDistance = seedDistance;
+        }
+
+        /// <summary>
+        /// 候选在当前方向段的局部参考弧长 S，单位 m。
+        /// </summary>
+        public double ReferenceS { get; }
+
+        /// <summary>
+        /// 候选世界位置到车辆位置的距离平方，单位 m²。
+        /// </summary>
+        public double SquaredDistanceMeters { get; }
+
+        /// <summary>
+        /// 候选 S 与调用方种子 S 的绝对距离，单位 m。
+        /// </summary>
+        public double SeedDistance { get; }
+
+        /// <summary>
+        /// 比较两个候选的稳定优先级。
+        /// 参数：other 为非空候选；返回：平方距离差超过 1e-14 时取较小者，随后取较近种子，仍相等时取较小 S。
+        /// </summary>
+        public bool IsPreferredTo(Candidate other)
+        {
+            if (SquaredDistanceMeters < other.SquaredDistanceMeters - TieTolerance) return true;
+            if (SquaredDistanceMeters > other.SquaredDistanceMeters + TieTolerance) return false;
+            if (SeedDistance < other.SeedDistance - TieTolerance) return true;
+            if (SeedDistance > other.SeedDistance + TieTolerance) return false;
+            return ReferenceS < other.ReferenceS;
+        }
+    }
+}
+    /// <summary>
+    /// 在窗口内投影世界位姿，按距离、种子 S 距离及较小 S 的顺序稳定选择候选。
+    /// 参数：worldPose 使用世界 X/Y（m）和车身航向（rad），S 窗口、距离阈值和种子均为 m；返回：窗口非法、无候选或候选超阈值时返回 false。
+    /// </summary>

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/FrenetReferencePoint.cs

@@ -0,0 +1,112 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 单一方向段内的不可变插值参考样本，连接世界几何与 Frenet 坐标。
+/// X/Y 与参考 S 为 m，航向为 rad，曲率为 1/m、曲率导数为 1/m²；倒车段仍以车辆航向保存几何。
+/// </summary>
+public sealed class FrenetReferencePoint
+{
+    /// <summary>
+    /// 创建已验证的方向段参考样本。
+    /// 参数：位置采用世界 X/Y（m），referenceS 为段局部弧长（m），航向为 rad，曲率量遵循 1/m 与 1/m²；所有数值必须有限。
+    /// 返回：车辆航向会规范到 [-π, π]；任一非有限输入会引发 <see cref="ArgumentOutOfRangeException"/>。
+    /// </summary>
+    public FrenetReferencePoint(double referenceS, double x, double y, double vehicleYaw, double unwrappedVehicleYaw,
+        TravelDirection direction, double geometricCurvature, double vehicleCurvature,
+        double vehicleCurvatureDerivative, double bodyClearance)
+    {
+        RequireFinite(referenceS, nameof(referenceS));
+        RequireFinite(x, nameof(x));
+        RequireFinite(y, nameof(y));
+        RequireFinite(vehicleYaw, nameof(vehicleYaw));
+        RequireFinite(unwrappedVehicleYaw, nameof(unwrappedVehicleYaw));
+        RequireFinite(geometricCurvature, nameof(geometricCurvature));
+        RequireFinite(vehicleCurvature, nameof(vehicleCurvature));
+        RequireFinite(vehicleCurvatureDerivative, nameof(vehicleCurvatureDerivative));
+        RequireFinite(bodyClearance, nameof(bodyClearance));
+
+        ReferenceS = referenceS;
+        X = x;
+        Y = y;
+        VehicleYaw = AngleMath.NormalizeRadians(vehicleYaw);
+        UnwrappedVehicleYaw = unwrappedVehicleYaw;
+        Direction = direction;
+        GeometricCurvature = geometricCurvature;
+        VehicleCurvature = vehicleCurvature;
+        VehicleCurvatureDerivative = vehicleCurvatureDerivative;
+        BodyClearance = bodyClearance;
+    }
+
+    /// <summary>
+    /// 样本在当前方向段的局部参考弧长 S，单位 m。
+    /// </summary>
+    public double ReferenceS { get; }
+
+    /// <summary>
+    /// 参考中心线点的世界 X 坐标，单位 m。
+    /// </summary>
+    public double X { get; }
+
+    /// <summary>
+    /// 参考中心线点的世界 Y 坐标，单位 m。
+    /// </summary>
+    public double Y { get; }
+
+    /// <summary>
+    /// 归一化后的车辆车身航向，单位 rad。
+    /// </summary>
+    public double VehicleYaw { get; }
+
+    /// <summary>
+    /// 连续展开的车辆车身航向，单位 rad，供跨点几何插值使用。
+    /// </summary>
+    public double UnwrappedVehicleYaw { get; }
+
+    /// <summary>
+    /// 该参考样本的行驶方向，决定 Frenet 横向正负号和行驶航向。
+    /// </summary>
+    public TravelDirection Direction { get; }
+
+    /// <summary>
+    /// 中心线几何曲率，单位 1/m。
+    /// </summary>
+    public double GeometricCurvature { get; }
+
+    /// <summary>
+    /// 满足车辆模型后的车辆曲率，单位 1/m。
+    /// </summary>
+    public double VehicleCurvature { get; }
+
+    /// <summary>
+    /// 车辆曲率相对弧长的导数，单位 1/m²。
+    /// </summary>
+    public double VehicleCurvatureDerivative { get; }
+
+    /// <summary>
+    /// 参考点记录的车体净空或可用裕度，单位 m。
+    /// </summary>
+    public double BodyClearance { get; }
+
+    /// <summary>
+    /// 获取与当前行驶方向一致的连续航向。
+    /// 返回：前进时为车辆展开航向，倒车时加 π；单位 rad，仅供 Frenet 几何计算，不重新归一化。
+    /// </summary>
+    public double TravelYaw
+    {
+        get { return Direction == TravelDirection.Forward ? UnwrappedVehicleYaw : UnwrappedVehicleYaw + Math.PI; }
+    }
+
+    /// <summary>
+    /// 拒绝不能安全保存为参考几何的数值。
+    /// 参数：value 是待验证的任意单位标量，name 是异常参数名；NaN 或无穷会引发 <see cref="ArgumentOutOfRangeException"/>。
+    /// </summary>
+    private static void RequireFinite(double value, string name)
+    {
+        if (double.IsNaN(value) || double.IsInfinity(value))
+            throw new ArgumentOutOfRangeException(name);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/FrenetTransform.cs

@@ -0,0 +1,61 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 在世界坐标与 Frenet 横向约定间转换，并始终以实际行驶方向确定 L 的正负。
+/// 世界位置使用 X/Y（m）、航向使用 rad；倒车时车辆车身航向与行驶航向相差 π。
+/// </summary>
+public static class FrenetTransform
+{
+    /// <summary>
+    /// 将参考点及其横向状态重建为世界车辆中心位姿。
+    /// 参数：lateralOffset 为 L（m），lateralDerivative 为 dL/dS（无量纲），minimumFrenetDenominator 为正的奇异性下界；参考点采用世界 X/Y（m）与 rad 航向。
+    /// 返回：当 1-κL 有限且不小于下界、重建位姿有限时返回 true；空参考点、非法输入或接近 Frenet 奇异点时返回 false。
+    /// </summary>
+    public static bool TryReconstruct(FrenetReferencePoint referencePoint, double lateralOffset, double lateralDerivative,
+        double minimumFrenetDenominator, out Pose2D pose)
+    {
+        pose = null;
+        if (referencePoint == null || !IsFinite(lateralOffset) || !IsFinite(lateralDerivative) ||
+            !IsFinite(minimumFrenetDenominator) || minimumFrenetDenominator <= 0d)
+            return false;
+
+        double denominator = 1d - referencePoint.GeometricCurvature * lateralOffset;
+        if (!IsFinite(denominator) || denominator < minimumFrenetDenominator)
+            return false;
+
+        double travelYaw = referencePoint.TravelYaw;
+        double x = referencePoint.X - lateralOffset * Math.Sin(travelYaw);
+        double y = referencePoint.Y + lateralOffset * Math.Cos(travelYaw);
+        double optimizedTravelYaw = travelYaw + Math.Atan2(lateralDerivative, denominator);
+        double vehicleYaw = referencePoint.Direction == TravelDirection.Forward
+            ? AngleMath.NormalizeRadians(optimizedTravelYaw)
+            : AngleMath.NormalizeRadians(optimizedTravelYaw + Math.PI);
+        if (!IsFinite(x) || !IsFinite(y) || !IsFinite(vehicleYaw))
+            return false;
+
+        pose = new Pose2D(x, y, vehicleYaw);
+        return true;
+    }
+
+    /// <summary>
+    /// 从车辆车身航向取得对应实际行驶的连续航向。
+    /// 参数：vehicleYaw 为 rad，direction 指定前进或倒车；返回：前进原样、倒车加 π，结果不归一化。
+    /// </summary>
+    internal static double GetTravelYaw(double vehicleYaw, TravelDirection direction)
+    {
+        return direction == TravelDirection.Forward ? vehicleYaw : vehicleYaw + Math.PI;
+    }
+
+    /// <summary>
+    /// 判定重建中间量是否为有限实数。
+    /// 参数：value 为任意标量；返回：NaN 或正负无穷时为 false。
+    /// </summary>
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/ReferencePathInterpolator.cs

@@ -0,0 +1,87 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 在恰好一个方向段内按局部参考弧长插值参考几何。
+/// 输入和输出 S、X/Y 与净空均为 m，航向为 rad，曲率为 1/m；插值不会越过方向段边界。
+/// </summary>
+public static class ReferencePathInterpolator
+{
+    /// <summary>
+    /// 接受端点轻微 S 超差并判断精确节点的数值容差，单位 m。
+    /// </summary>
+    private const double Epsilon = 1e-12d;
+
+    /// <summary>
+    /// 为给定局部 S 生成方向一致的 Frenet 参考样本。
+    /// 参数：segment 不可为空，referenceS 为 m；[-1e-12, Length+1e-12] 内的端点超差会夹紧到边界。
+    /// 返回：精确点直接转换，区间内对位置、展开航向和几何量线性插值；超界或退化跨度会引发异常。
+    /// </summary>
+    public static FrenetReferencePoint Interpolate(DirectionSegmentView segment, double referenceS)
+    {
+        if (segment == null)
+            throw new ArgumentNullException(nameof(segment));
+        if (!IsFinite(referenceS) || referenceS < -Epsilon || referenceS > segment.LengthMeters + Epsilon)
+            throw new ArgumentOutOfRangeException(nameof(referenceS));
+
+        double clampedS = Math.Max(0d, Math.Min(segment.LengthMeters, referenceS));
+        for (int index = 0; index < segment.Points.Count; index++)
+        {
+            SmoothedPathPoint upper = segment.Points[index];
+            if (Math.Abs(upper.ArcLength - clampedS) <= Epsilon)
+                return FromPoint(upper);
+            if (upper.ArcLength > clampedS)
+            {
+                SmoothedPathPoint lower = segment.Points[index - 1];
+                double span = upper.ArcLength - lower.ArcLength;
+                if (span <= Epsilon)
+                    throw new ArgumentException("Reference points must have positive interpolation spans.", nameof(segment));
+                double fraction = (clampedS - lower.ArcLength) / span;
+                return new FrenetReferencePoint(
+                    clampedS,
+                    Lerp(lower.X, upper.X, fraction),
+                    Lerp(lower.Y, upper.Y, fraction),
+                    AngleMath.NormalizeRadians(Lerp(lower.UnwrappedHeading, upper.UnwrappedHeading, fraction)),
+                    Lerp(lower.UnwrappedHeading, upper.UnwrappedHeading, fraction),
+                    segment.Direction,
+                    Lerp(lower.GeometricCurvature, upper.GeometricCurvature, fraction),
+                    Lerp(lower.VehicleCurvature, upper.VehicleCurvature, fraction),
+                    Lerp(lower.VehicleCurvatureDerivative, upper.VehicleCurvatureDerivative, fraction),
+                    Lerp(lower.BodyClearance, upper.BodyClearance, fraction));
+            }
+        }
+        return FromPoint(segment.Points[segment.Points.Count - 1]);
+    }
+
+    /// <summary>
+    /// 将原始平滑路径点转换为同一局部 S 的 Frenet 参考样本。
+    /// 参数：point 已携带世界 X/Y（m）、航向（rad）和曲率量；返回：保持其方向及所有几何量的不可变副本。
+    /// </summary>
+    private static FrenetReferencePoint FromPoint(SmoothedPathPoint point)
+    {
+        return new FrenetReferencePoint(point.ArcLength, point.X, point.Y, point.Heading, point.UnwrappedHeading,
+            point.Direction, point.GeometricCurvature, point.VehicleCurvature, point.VehicleCurvatureDerivative,
+            point.BodyClearance);
+    }
+
+    /// <summary>
+    /// 在线性标量区间内计算插值值。
+    /// 参数：lower、upper 为同单位端点，fraction 为无单位比例；返回：同单位的未夹紧线性结果。
+    /// </summary>
+    private static double Lerp(double lower, double upper, double fraction)
+    {
+        return lower + (upper - lower) * fraction;
+    }
+
+    /// <summary>
+    /// 判定插值输入是否为有限实数。
+    /// 参数：value 为任意标量；返回：NaN 或无穷时为 false。
+    /// </summary>
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}