增加车队轨迹控制核心与轮组自转模式

2026-08-21 17:33:29 +08:00
parent fea2265e2d
commit 0ab409cd2a
33 changed files with 1949 additions and 989 deletions
@@ -37,7 +37,7 @@ Medulla宿主
 | `MultiWheelC/Control/Lateral/` | 当前默认 `StanleyLateralController` |
 | `MultiWheelC/Control/Longitudinal/` | 当前默认 `PidLongitudinalController` |
 | `MultiWheelC/Control/Allocation/` | 横纵结果组合、GCP限幅及GCP与刚体速度的转换 |
-| `MultiWheelC/Control/Execution/` | 单周期编排、终点策略、命令执行和耗时诊断 |
+| `MultiWheelC/Control/Execution/` | `PathTrackingCore` 共享纯控制周期、单车命令执行、终点策略和耗时诊断 |
 | `MultiWheelC/Movements/` | 舵轮准备、轨迹跟踪、原地自转和组合动作计划 |
 | `MultiWheelC/Experiments/` | Clumsy宿主人工测试、测试轨迹工厂和CSV记录 |
 | `MultiWheelC/Old/` | 保留的旧实现；不能仅因仍参与编译就视为新版流程依赖 |
@@ -73,10 +73,11 @@ MovementTest / MotionPlanExecutor
  → ParkingVehicleStateProviderFactory.Create()
  → ParkingGeometricController.Start()/ExecuteCycle()
      → IVehicleStateProvider.TryGetState()
-      → TrajectoryProjector.Project()
-      → ILateralController.Compute()
-      → ILongitudinalController.ComputeSpeedMetersPerSecond()
-      → GcpCommandAllocator.Allocate()
+      → PathTrackingCore.Compute()
+          → TrajectoryProjector.Project()
+          → ILateralController.Compute()
+          → ILongitudinalController.ComputeSpeedMetersPerSecond()
+          → GcpCommandAllocator.Allocate()
      → GcpCommandExecutor.Execute()
      → GcpKinematics.ToBodyTwist()
      → MultiWheelChassisAdapter.SendBodyTwist()
@@ -84,8 +85,24 @@ MovementTest / MotionPlanExecutor
      → 四个真实舵轮角度和速度
 ```

+`PathTrackingCore` 只依赖受控刚体的 `Pose2D`、车体系 `Twist2D`、速度有效标志和控制周期，不依赖 `VehicleState`、`FleetState`、底盘或通信。`ParkingGeometricController` 负责把单车状态源和实体底盘接到该核心；横向、纵向控制算法仍通过接口组合注入，没有采用控制器继承层次。
+
 `TrajectoryTrackingMovement` 默认从 `PilotDefinition.Conf` 读取车辆级参数，同时保留少量动作级覆盖字段；横向控制器可通过 `LateralControllerFactory` 替换，纵向控制器当前固定创建为 `PidLongitudinalController`。

+## 车队纯计算调用链
+
+```text
+FleetState
+  → FleetController
+      → PathTrackingCore.Compute()
+      → GcpKinematics.ToBodyTwist()
+      → FleetMotionCommand（参考点为车队原点）
+  → FleetKinematics.Decompose(FleetLayout, command)
+  → FleetMemberCommand[]（各成员真实车体系Twist）
+```
+
+这条链已经覆盖虚拟中心轨迹闭环和确定性刚体速度分解，但尚未接入车队状态估计、通信/时间对齐、相对布局纠偏、共同能力限幅、安全降级和成员底盘发送。
+
 ## 状态数据流

 ```text
@@ -97,8 +114,8 @@ DetourInterface.getCartLocation()
 MultiWheelChassis.GetCarSpeed(true)
  → WheelFeedbackVehicleStateProvider
      ├─ Vx、Vy分别使用同一时间常数低通滤波
-      ├─ 覆盖Detour线速度
-      └─ 保留Detour位姿和Omega
+      ├─ 轮组估计有效后覆盖Detour的Vx、Vy和Omega
+      └─ 保留Detour位姿

  → VehicleState（世界位姿、世界Twist、车体Twist）
  → ParkingGeometricController
@@ -50,10 +50,10 @@

 ### 7. 默认状态采用Detour位姿与轮组平面速度组合

- 位姿和Omega来自经过校验/滤波的Detour。
- 车体系Vx、Vy来自 `GetCarSpeed(true)`，分别使用同一时间常数低通滤波。
- 轮组Vw同样滤波，但仅用于Detour跳变期间的短时位姿预测、运动合理性和动态航向创新阈值，不替换控制输出中的Detour Omega。
- 原因：位置依赖SLAM，控制平面速度优先使用响应更直接的电机/舵角反馈；轮组Vw对短时运动趋势有用，但动态精度尚不足以直接作为闭环角速度。
+- 位姿来自经过校验和任务坐标连续化处理的Detour。
+- 车体系 `Vx/Vy/Vw` 来自 `GetCarSpeed(true)`，使用同一时间常数低通滤波；轮组估计有效后，控制状态的 `Omega` 也使用轮组 `Vw`。首帧尚未建立采样时间基准时速度状态无效并对外置零。
+- 轮组 `Vw` 同时用于Detour跳变期间的短时位姿预测、运动合理性和动态航向创新阈值。
+- 原因：位置仍依赖SLAM，控制速度优先使用响应更直接且不受Detour自转退化影响的电机/舵角反馈。
 - 依据：`ParkingVehicleStateProviderFactory`、`WheelFeedbackVehicleStateProvider`。

 ### 8. 横向控制可替换，纵向控制暂保持PID
@@ -90,22 +90,39 @@
 - `l_step` 当前只作为诊断和后续健康分级依据，不单独决定状态有效性。
 - 依据：`DetourVehicleStateProvider`、`WheelFeedbackVehicleStateProvider`、`MultiWheelRotateInPlace`。

-### 13. 车队布局采用不可变快照，刚体速度采用确定性分解
+### 13. 车队布局与状态采用不可变快照，刚体速度采用确定性分解

 - `Shared/Fleet/FleetLayout.cs` 保存经车号唯一性和有限值校验的 `VehicleLayout` 快照，不提供搬运过程中的逐车修改入口。
 - `FleetKinematics.Decompose()` 已按平面刚体关系把 `FleetMotionCommand` 分解为各成员中心速度，并转换到各车真实车体系；不依赖通信、Detour、β、底盘限幅或QP。
 - `MultiWheelC.Tests/FleetKinematicsTests.cs` 已覆盖整体平移、绕车队中心旋转、绕成员车旋转和停止四个数学场景。
+- `MultiWheelC/Fleet/FleetState.cs` 保存车队虚拟中心的世界位姿、同一点的世界系/车队系速度、状态采样时间和速度有效性；`FleetPoseInWorld.Yaw` 定义车队 `+X` 方向，两份速度只是同一物理速度的不同坐标表达。
+- `FleetState.SampleTimestampSeconds` 采用主车/协调器生成聚合快照时的本机单调时间。成员本机时钟和Detour `tick` 的同步属于未来接收/状态估计层职责，不阻塞使用人工构造 `FleetState` 开发纯车队控制器。
+
+### 14. 原地自转保留绝对航向与轮组相对角两种反馈模式
+
+- `DetourAbsoluteHeading` 保留世界航向闭环和停车后的Detour位姿恢复，适用于必须对准绝对方向的任务。
+- `RelativeWheelOdometry` 通过 `TryGetWheelTwist()` 获取与Detour无关的滤波轮组 `Vw`，按轮组采样时间积分相对角度；活动自转不因Detour位置或航向退化中断。
+- 轮组相对模式是工程降级/测试模式，不等同于绝对定位：允许约±5°误差，不能消除轮胎打滑、轮径误差和积分漂移，也不能用于长期世界航向基准。
+- 依据：`WheelFeedbackVehicleStateProvider`、`MultiWheelRotateInPlace`、`RotationTests`。
+
+### 15. 单车与车队轨迹跟踪通过组合共享纯控制核心
+
+- `PathTrackingContext` 只携带受控刚体的车体系速度、速度有效性、轨迹投影和周期控制量，不依赖 `VehicleState` 或 `FleetState`。
+- `PathTrackingCore` 集中实现投影连续性、保护/终点策略、曲率预瞄、横纵向控制和GCP分配；`ParkingGeometricController` 与 `FleetController` 分别组合该核心，负责各自的状态适配和输出边界，不通过继承复制控制流程。
+- `FleetController` 第一版只闭环车队虚拟中心，并将GCP结果转换成车队原点处的 `FleetMotionCommand`；成员速度继续由 `FleetKinematics.Decompose()` 确定性分解。
+- 控制算法的可替换性继续由 `ILateralController`、`ILongitudinalController` 组合注入；状态源、通信、底盘发送和成员协调不进入纯核心。
+- `MultiWheelC.Tests` 已覆盖8个Stanley前进/倒车符号场景、6个车队控制周期场景和4个刚体分解场景；统一构建与打包通过。

 ## 已经确认但尚未实施

 - 路线顺序：先完成单车闭环和停车功能验证，再正式实施多车通信、编队和协同控制。来源：`README.md`。
- 当前只完成车队布局模型和纯运动学分解，尚未形成可运行的多车链路：布局采集/激活、车队状态估计、轨迹控制、通信和安全协调仍未接入业务流程。
+- 当前已完成车队布局模型、虚拟中心状态模型、虚拟中心轨迹控制和纯运动学分解，尚未形成可运行的多车链路：布局采集/激活、车队状态估计、通信、成员纠偏和安全协调仍未接入业务流程。
 - 布局生命周期区分夹紧前后的语义：夹紧前的预设布局只用于引导车辆就位；车辆夹紧且静止后，应同步读取成员位姿，选择车队参考系并创建新的不可变 `FleetLayout`，再由上层协调器原子激活。共同搬运期间的相对位姿变化属于状态误差，不能通过修改 `FleetLayout` 吸收；松开车辆后清除激活布局。具体采集和激活接口尚未实施。
 - 多车共同搬运不能只闭环车队中心：整体位姿误差与成员相对布局误差必须分开估计和约束，否则成员误差可能相互抵消而使平均中心看似正确。
 - 计划采用分层职责：车队控制器产生参考点 `FleetTwist`，分配层依据成员 `VehicleLayout` 计算每车真实车体系 `BodyTwist`，单车层继续负责β变换、GCP和本车四轮解算。
 - 第一版采用确定性的虚拟刚体速度分配，不先引入QP/HQP：若成员在车队系中的固定布局为位置 `(x_i,y_i)`、朝向 `theta_i`，则成员中心在车队系中的速度为 `(Vx-omega*y_i, Vy+omega*x_i, omega)`，再通过 `R(-theta_i)` 转到本车体系后交给 `SendBodyTwist()`。QP/HQP只在需要同时调整车队参考速度、处理成员能力差异、松弛约束或严格任务优先级时再引入。
 - “按状态最差车辆协调速度”采用车队共同可行性和统一缩放表达：普通能力受限时由所有成员约束确定共同速度比例；任一成员报警、通信超时、状态不可用或刚体误差越界时整队停车。时间戳、心跳、命令有效期和本地超时停车属于第一版安全契约，延迟预测补偿可以后续增加。
- 虚拟车队可使用固定在车队坐标系中的前后GCP作为控制几何，例如位于 `±L_F`；这些点只用于把横向控制结果转换成车队参考点 `FleetTwist`，不是物理轮轴，也不直接参与单车四轮解算。前后GCP方向仍随控制输出动态变化，且横向控制器与GCP到Twist转换必须使用同一 `L_F`。该方案尚未实施。
+- 虚拟车队使用固定在车队坐标系中的对称前后GCP，把横向控制结果转换成车队原点 `FleetTwist`；这些点不是物理轮轴，也不直接参与单车四轮解算。横向控制器与GCP到Twist转换必须使用同一控制点半径；具体车辆级配置值和实车验证仍待完成。
 - 每辆成员车都应作为反馈来源，但反馈职责必须分层：成员Detour位姿用于融合车队整体位姿和检查相对布局，单车轮速/舵角用于确认命令执行偏差，电机电流、扭矩或力传感信息用于负载与内力监控。相对位姿接近目标并不能证明没有内力，因此不能只依靠刚性连接或位姿误差判断负载均衡。
 - 第一版不把每车β作为复杂优化变量：车队动作先明确主要滚动方向 `beta_fleet`（如正常0°、斜行45°、横移90°），成员按固定布局朝向换算 `beta_i = beta_fleet - theta_i`，并利用180°等效和有符号速度选择方便的本地表示。β是单车执行坐标系，不改变刚体分配得到的真实车体系 `BodyTwist`，也不会让各车命令数值相同；仅允许在全车停车时准备和激活，全部成员舵轮到位后通过同步屏障释放非零命令。只有出现复杂布局、整段方向变化、限位余量或频繁反号问题时，才增加轨迹级β候选搜索。
 - 旧版参考项目采用固定双车布局：各车由 `carWorld ∘ layout⁻¹` 反推车队中心，再对位置和圆周航向求平均；路径控制器以该虚拟中心跟踪轨迹。同时它可按 `fleetTarget ∘ layout_i` 生成每车理想位姿，并叠加Detour布局纠偏和邻车两腿检测纠偏，因此并非只控制平均中心。来源：`原版停车机器人/parkingrobot/ClumsyPilot/PilotDefinition.cs`、`ChassisController.cs`。
@@ -26,11 +26,16 @@
 | `FleetLayout` | 不可变的成员布局快照，构造时校验成员数量、车号唯一性和位姿有限值 |
 | `FleetMotionCommand` | 车队参考点及该点在车队系中表达的刚体速度 |
 | `FleetMemberCommand` | 指定车辆及其真实车体系中表达的成员中心速度 |
+| `FleetState` | 同一采样时刻的车队虚拟中心位姿和速度快照 |

 坐标变换集中在 `FrameTransform2D`；有限值检查集中在 `NumericGuard`；角度处理集中在 `AngleMath`。

 `FleetKinematics.Decompose()` 依据 `v_i = v_ref + omega × (r_i-r_ref)` 生成每车命令，再按 `VehicleLayout.PoseInFleet` 的朝向把线速度从车队系转换到成员车体系；所有刚性连接成员的 `Omega` 保持相同。

+`MultiWheelC/Fleet/FleetState.cs` 中，`FleetPoseInWorld` 表示车队虚拟中心位姿，其 `Yaw` 同时定义车队坐标系 `+X` 在世界系中的方向；车队系采用 `+X` 前、`+Y` 左、逆时针为正。`TwistAtFleetOriginInWorld` 与 `TwistAtFleetOriginInFleet` 是同一参考点、同一物理速度在两个坐标系中的表达，后者由前者和 `FleetPoseInWorld` 推导，不是第二份独立测量。`HasValidVelocityEstimate=false` 时速度按零保存，用于区分尚未形成可靠速度估计与真实零速。
+
+`FleetState.SampleTimestampSeconds` 表示主车/协调器生成该车队状态快照时的本机单调时间，不是Detour全局时间。各成员电脑的本机时钟和Detour `tick` 当前不能直接互相比较；未来接收层应另行保存来源时间并完成新鲜度和时间对齐。
+
 ## 轨迹契约

 文件：`MultiWheelC/Trajectory/`。
@@ -93,13 +98,18 @@ bool TryGetState(out VehicleState state)
 `ParkingVehicleStateProviderFactory.Create()` 创建：

 - `DetourVehicleStateProvider`：读取Detour位姿，处理源时间、重复帧、运动合理性、预测创新、静止确认和跳变候选；候选确认期间使用轮组速度短时预测控制位姿。确认后的有限小坐标偏移可更新 `controlFromDetour` 以保持当前任务坐标连续，超限或未恢复时将状态置为不可用。
- `WheelFeedbackVehicleStateProvider`：调用 `MultiWheelChassis.GetCarSpeed(true)`，低通滤波车体 `Vx`、`Vy` 和由deg/s转换为rad/s的 `Vw`。最终 `VehicleState` 的平面速度使用轮组 `Vx/Vy`，Omega仍使用Detour估计；轮组 `Vw` 只提供给Detour短时运动预测和动态航向合理性判断。
+- `WheelFeedbackVehicleStateProvider`：调用 `MultiWheelChassis.GetCarSpeed(true)`，低通滤波车体 `Vx`、`Vy` 和由deg/s转换为rad/s的 `Vw`。轮组估计有效后，最终 `VehicleState` 的 `Vx/Vy/Omega` 均使用滤波后的轮组反馈；首帧速度标记为无效，并由 `VehicleState` 对外置零。轮组 `Vw` 同时提供给Detour短时运动预测和动态航向合理性判断。

 默认滤波参数来自 `PilotConfig.ParkingControl.cs`：Detour线速度0.15s、Detour角速度0.20s、轮组反馈 `Vx/Vy/Vw` 统一为0.10s。

 跳变处理的重要边界：航向创新允许量随 `|Vw| × Detour源帧间隔` 增加；候选若在近似原地自转期间开始，则整个候选确认过程禁止自动改写任务坐标系。默认候选确认窗口为0.60s，窗口内输出轮组预测状态，超时后完整位姿安全返回不可用。

-完整轨迹控制仍通过 `TryGetState()` 要求位置和航向均有效。原地自转则由 `WheelFeedbackVehicleStateProvider.TryGetHeadingRadians()` 获取独立可靠航向：仅位置异常时可进入 `PositionUnavailableHeadingAvailable`，不会中断正在进行的航向闭环；航向异常仍会停止动作。到达目标并停止驱动后，`MultiWheelRotateInPlace` 调用 `BeginPostRotationPositionRecovery()`，在原有3帧、0.60s及最大自动平移/航向偏移限制内恢复完整位姿，恢复失败时不释放下一运动段。
+完整轨迹控制仍通过 `TryGetState()` 要求位置和航向均有效。原地自转提供两种反馈模式：
+
+- `DetourAbsoluteHeading` 使用 `TryGetHeadingRadians()` 做世界航向闭环；仅位置异常时可继续，航向异常仍会停止。到达目标并停车后调用 `BeginPostRotationPositionRecovery()` 恢复完整位姿，恢复失败时不释放下一运动段。
+- `RelativeWheelOdometry` 使用 `TryGetWheelTwist(out Twist2D, out timestamp)` 直接读取并滤波轮组 `Vx/Vy/Vw`，按轮组采样时间梯形积分相对转角；活动自转期间不调用Detour，也不执行Detour停车后恢复。该模式用于允许约±5°误差的相对角动作，不能提供绝对世界航向校正，轮胎打滑和轮径误差会累积到角度结果中。
+
+`TryGetWheelTwist()` 是不依赖Detour的窄接口；首次尚未形成滤波样本时返回 `false`，其时间戳来自该状态源使用的本机单调时钟。

 `TrackingExperimentRecorder` 已记录原始/滤波轮组 `Vx/Vy/Vw`、轮组采样时刻、Detour `tick/l_step`、数据年龄和源帧间隔、轮速预测时刻、实际/允许的位置与航向创新、候选触发原因、估计器状态及不可用原因。创新和源帧间隔只在收到Detour新帧时更新；若CSV记录频率高于Detour帧率，后续记录行会重复最近一个新帧的诊断值，分析时应按 `DetourTickRaw` 去重或分组。

@@ -107,7 +117,7 @@ bool TryGetState(out VehicleState state)

 ### `PathTrackingContext`

-一次控制周期的只读快照，包含：`VehicleState`、`TrajectoryProjection`、处理后的控制参考速度、预瞄曲率、真实 `deltaTime` 和运动方向β。
+一次控制周期的只读快照，包含：受控刚体的车体系 `Twist2D`、速度有效标志、`TrajectoryProjection`、处理后的控制参考速度、预瞄曲率、真实 `deltaTime` 和运动方向β。该类型不依赖单车 `VehicleState` 或车队 `FleetState`。

 `ActualLongitudinalSpeedMetersPerSecond` 是完整车体平面速度沿β方向的投影：

@@ -117,6 +127,14 @@ Vβ = cos(β)·Vx_body + sin(β)·Vy_body

 因此45°/90°蟹行不能只使用车体 `Vx` 判断纵向速度。

+### `PathTrackingCore`
+
+`PathTrackingCore.Compute(Pose2D, Twist2D, bool, double)` 是单车与车队共用的纯轨迹跟踪周期：负责连续投影、距离保护、起步释放、终点制动/完成判断、曲率预瞄、横纵向控制和GCP分配，返回 `PathTrackingCycleOutput`。输出只包含周期结果、可选 `GcpMotionCommand`、投影和计算耗时；核心不读取状态源、不发送底盘命令，也不处理通信。
+
+状态暂时不可用时，外层调用 `PauseForUnavailableState()` 保留当前轨迹与投影连续性；明确失败或取消分别使用 `Fail()`、`Cancel()`。
+
+`ParkingGeometricController` 是单车适配层，负责读取 `IVehicleStateProvider` 并通过 `GcpCommandExecutor` 发送实体底盘命令。`FleetController` 是车队虚拟中心适配层，使用 `FleetState.FleetPoseInWorld` 和 `TwistAtFleetOriginInFleet` 调用同一核心，再将GCP结果转换为车队原点处的 `FleetMotionCommand`。
+
 当前速度闭环和执行边界的语义并不完全相同：纵向PID与Stanley实际速度分母使用 `Vβ`（Stanley也可按配置改用参考速度），而 `GcpMotionCommand.SpeedMetersPerSecond` 和旧版 `SendMotion.speed` 表示带行驶方向符号的车体中心平移速度模长。正常圆弧理想跟踪时 `Vy=0`，两者相等；只有横向误差共同转角产生非零 `Vy` 时，模长与 `Vβ` 才相差余弦因子。当前最大命令速度仍限制最终发送的模长。

 ### `ILateralController`
@@ -2,7 +2,7 @@

 ## 项目定位

-`MyParking` 是停车机器人控制软件的当前正式开发版本，主体为 C# 插件工程。项目目标是让多舵轮停车机器人完成底盘运动、车辆状态获取、轨迹跟踪、原地自转和后续停车作业；当前研发重点仍是单车闭环和实车联调，多车协同尚未形成可运行实现。来源：`README.md`、`MultiWheelC/PilotConfig.cs`、`Shared/Fleet/FleetKinematics.cs`。
+`MyParking` 是停车机器人控制软件的当前正式开发版本，主体为 C# 插件工程。项目目标是让多舵轮停车机器人完成底盘运动、车辆状态获取、轨迹跟踪、原地自转和后续停车作业；当前研发重点仍是单车闭环和实车联调，多车协同已有纯计算骨架但尚未形成可运行实现。来源：`README.md`、`MultiWheelC/PilotConfig.cs`、`MultiWheelC/Fleet/`、`Shared/Fleet/`。

 项目不是可直接 `dotnet run` 的独立应用：

@@ -17,7 +17,7 @@
 - 代码接口面向四个可转向轮组，并暴露8个驱动电机的位置/速度反馈和4个舵角反馈。M/C IO定义见 `PilotDefinition`、`DiverCartDefinition`。
 - 车体支持正常前后行驶、倒车轨迹、任意固定运动方向β下的滚动运动、蟹行和停车后原地自转。主要入口见 `MultiWheelC/Movements/` 和 `MultiWheelC/Experiments/`。
 - 夹臂速度、位置、限位和报警IO已经接入，但轮胎识别、自动钻车、释放车辆和完整停车作业状态机尚未在当前新版流程中完成。来源：`PilotConfig.cs`、`PilotDefinition.cs`、`README.md`。
- 多车模型数据类型已经预留，但多车配置位于 `PilotConfig.cs` 的 `#if false`，`FleetKinematics.cs` 只有占位注释，不能把当前代码视为已支持多车编队。
+- 多车已具备 `FleetLayout`、`FleetState`、`FleetController` 和 `FleetKinematics` 组成的纯计算链；多车配置仍位于 `PilotConfig.cs` 的 `#if false`，且状态聚合、通信、成员纠偏、安全协调和实体发送尚未接入，因此不能把当前代码视为已支持可运行的多车编队。

 ## 整体运行流程

@@ -45,6 +45,12 @@
 - 位置：`DetourVehicleStateProvider`、`WheelFeedbackVehicleStateProvider`、`MultiWheelRotateInPlace`、`PilotConfig.ParkingControl.cs`。
 - 验证：第三轮实车组合动作中发生一次约47mm/3.44°的原始坐标变化，自动连续化计数增加后控制轨迹保持连续，动作最终以约24.7mm剩余距离、2.8mm横向误差完成。

+### Detour自转退化会中断允许小角度误差的相对自转
+
+- 处理：`MultiWheelRotateInPlace` 增加 `RelativeWheelOdometry` 反馈模式，直接读取并积分滤波轮组 `Vw`；活动自转期间不调用Detour。原有 `DetourAbsoluteHeading` 模式继续用于需要世界绝对航向的任务。
+- 验证：2026-08-21第七轮包含19份轮组里程计自转记录，其中17份表现为正常停车并完成舵轮回正；这17份中轮组积分转角与Detour原始航向变化的绝对差中位数约0.89°、最大约3.07°。同批Detour原始位置端点变化中位数约56mm、最大约298mm，说明轮组模式确实绕开了自转期间的Detour退化依赖。
+- 限制：CSV尚未记录请求角度、内部积分角和明确完成原因，因此上述数据不能作为目标角精度的正式验收；另有2份记录未完成回正，属于手动停止、小角度超时或其他原因仍待确认。轮组积分也不能替代绝对世界航向。
+
 ## 待解决

 ### 完整停车作业流程尚未实现
@@ -52,11 +58,16 @@
 - 缺少或未接入新版流程：轮胎识别、自动钻车、夹抱/释放动作编排、完整安全状态机。
 - 现有 `PilotConfig` 和IO字段不能视为业务流程已经完成。

-### 多车能力仍是占位
+### 多车执行链仍未完成

- `Shared/Fleet/FleetKinematics.cs` 没有实现。
+- 纯计算链 `FleetState → FleetController → FleetMotionCommand → FleetKinematics.Decompose()` 已实现，并有6个车队控制周期场景和4个刚体分解场景通过。
 - `PilotConfig.cs` 的多车区域在 `#if false` 中，不参与当前编译。
- 当前没有车队状态、通信、命令分配、安全降级和多车测试闭环。
+- 当前仍没有布局采集/激活、车队状态估计、通信接收与时间对齐、相对布局闭环、能力限幅、安全降级、成员底盘发送和多车实车测试闭环，因此不能把纯计算层视为可运行的多车功能。
+
+### 轮组速度初始化回退语义不一致
+
+- `WheelFeedbackVehicleStateProvider` 在轮组首帧无效时局部选择Detour `Omega`，但随后以 `hasValidVelocityEstimate=false` 构造 `VehicleState`；`VehicleState` 会把整份速度对外置零，因此该Detour角速度回退实际不可见。
+- 影响仅限轮组滤波尚未形成有效采样的初始化阶段；应确认期望是保持整份速度无效并置零，还是允许单独提供有效的Detour角速度，再决定是否修改代码和注释。

 ### Detour跳变判据和动作段衔接仍需收敛

@@ -68,7 +79,7 @@
 - 健康 `l_step` 下仅凭预测创新不应轻易自动改写坐标系；在完成同一时刻比较前，不继续通过反复放宽阈值堆叠补丁。
 - 不建议通过全局放宽0.60s确认窗口、40mm残差或允许自转期间完整坐标修正来掩盖问题。
 - 2026-08-19第六轮记录确认普通正向/倒车直线、曲线和小范围运动中坐标连续化总体可用，主要剩余失败集中在原地自转：有样本在 `l_step=94` 时连续3个Detour新帧航向创新超限，状态层先短时使用轮组Vw预测，第三帧才安全终止，说明单帧航向容错已按设计生效；另有约163mm、207mm的位置不连续或候选不稳定导致停车后恢复失败。
- 当前决定：保持普通运动150mm自动连续化上限、自转最大角速度30°/s和角加速度40°/s²，不以全局放宽阈值或提高自转速度掩盖Detour退化。原地自转后的条件化“仅位置连续化”仍是可评估方案，但尚未实施；持续航向异常需要Detour质量/重定位语义或额外可信航向来源才能进一步收敛。
+- 当前决定：保持普通运动150mm自动连续化上限、自转最大角速度30°/s和角加速度40°/s²，不以全局放宽阈值或提高自转速度掩盖Detour退化。允许相对角误差的动作可选已实现的轮组里程计模式；必须闭环到绝对世界航向时，持续航向异常仍需要Detour质量/重定位语义或额外可信绝对航向来源才能进一步收敛。

 ### 自动化测试和CI覆盖有限

@@ -1,6 +1,6 @@
 # 当前进展

-更新日期：2026-08-20。这里只保存当前状态，不作为完整开发历史。
+更新日期：2026-08-21。这里只保存当前状态，不作为完整开发历史。

 ## 已完成/已接入

@@ -9,28 +9,30 @@
 - 弧长参数化 `Trajectory2D`、统一插值、轨迹投影窗口和进度连续性。
 - 默认 Stanley 横向 + PID 纵向的新版轨迹控制链，横向控制器可注入替换。
 - 前进4m、倒车4m、45°蟹行4m、直线-左半圆-直线和组合运动宿主测试入口。
- Detour位姿与轮组反馈组合状态源：Vx/Vy用于控制速度，Vw用于短时位姿预测和动态航向合理性判断，最终控制Omega仍来自Detour。
+- Detour位姿与轮组反馈组合状态源：位姿来自Detour；轮组估计有效后，控制状态的Vx/Vy/Omega均来自滤波后的轮组反馈，Vw同时用于短时位姿预测和动态航向合理性判断。
 - Detour源 `tick/l_step` 诊断、跳变候选确认、有限小偏移任务坐标连续化、自转期间禁止自动吸收偏移，以及超时状态不可用保护。
 - 起步释放、曲率前馈预瞄、GCP角速度限制、终点制动预瞄和单向低速收敛。
 - 原地自转舵轮准备、航向PID、超时保护和完成后回正；自转期间位置/航向有效性已分离，到位停车后才恢复完整位姿并决定是否释放下一段。
 - Detour单帧航向异常已增加连续帧/短时预测确认；第六轮实车数据确认前两帧异常由轮组Vw预测承接，持续到第三个异常新帧时才安全终止。
+- 原地自转已增加 `RelativeWheelOdometry` 模式和不依赖Detour的 `TryGetWheelTwist()` 接口；第七轮19份记录中17份表现为正常停车回正，轮组积分与Detour原始航向变化绝对差中位数约0.89°、最大约3.07°，但目标角精度尚缺专用字段正式验收。
 - C层轨迹/周期CSV与M层轮速/舵角诊断记录。
 - C层Detour静态诊断入口，记录源时间、`l_step`、位姿帧差和轮组静态反馈；已有约14分41秒实车静态基线。
 - C层轨迹CSV已补充原始/滤波轮组 `Vx/Vy/Vw`、轮组与预测时刻、Detour帧间隔、实际/允许创新、候选原因、估计器状态和不可用原因。
 - 停车控制参数集中到 `Configuration/PilotConfig.ParkingControl.cs`。
+- `PathTrackingContext` 已解除对单车 `VehicleState` 的依赖；`PathTrackingCore` 统一单车与车队的投影、保护/终点策略、横纵向控制和GCP分配，`ParkingGeometricController` 保留单车状态读取与实体命令执行职责。
 - `MultiWheelC.Tests` 已提供不依赖实车的Stanley前进/倒车横向符号回归，8个场景通过；该项目不进入正式解决方案和打包脚本。
- `Shared/Fleet` 已加入不可变 `FleetLayout`、成员/车队命令模型和确定性 `FleetKinematics`；不依赖实车的4个刚体分解场景已通过。
+- `Shared/Fleet` 已加入不可变 `FleetLayout`、成员/车队命令模型和确定性 `FleetKinematics`；`MultiWheelC/Fleet` 已加入虚拟中心 `FleetState` 和复用 `PathTrackingCore` 的纯计算 `FleetController`。6个车队控制周期场景和4个刚体分解场景通过。
 - 本次建立工作区/项目AGENTS导航和 `docs/` 按需知识库。

 以上表示代码入口存在，不表示全部实车工况已经验收。

 ## 当前进行方向

- 普通正向/倒车直线、曲线和小范围运动中坐标连续化已基本可用；当前单车状态估计的主要未决问题收敛到原地自转时Detour偶发持续退化。
+- 普通正向/倒车直线、曲线和小范围运动中坐标连续化已基本可用；允许小角度误差的相对自转已有轮组里程计模式，必须对准绝对世界航向时仍受Detour偶发持续退化限制。
 - 暂时冻结普通运动跳变阈值和30°/s、40°/s²自转参数，等待Detour接口语义后再决定是否实施自转结束后的条件化仅位置连续化或调整航向恢复策略。
 - Detour对接最小问题已整理到 `docs/detour-information-checklist.md`，不要求取得源码。
 - 验证非零β运动系，当前已有45°蟹行直线入口；曲线蟹行仍需设计实验。
- 多车共同搬运已完成布局模型和纯刚体速度分解；下一阶段是夹紧后布局采集/原子激活、车队状态与控制、通信和安全协调，再把成员命令接入各车 `SendBodyTwist()`。QP/HQP不作为第一版前置条件。
+- 多车共同搬运已完成布局模型、虚拟中心状态、虚拟中心轨迹闭环和纯刚体速度分解；下一步处理夹紧后布局采集/原子激活、车队状态估计、通信和安全协调，并把成员命令接入各车 `SendBodyTwist()`。QP/HQP不作为第一版前置条件。
 - β在多车中定位为单车执行坐标系而非车队核心优化量：常规、斜行和横移动作先确定车队主要滚动方向，各车按布局朝向换算本地β，停车预对齐并经车队同步屏障统一释放。轨迹级β搜索只作为机械余量或复杂方向变化下的后续增强。

 ## 阻塞/待确认
@@ -48,8 +50,8 @@

 1. 按 `docs/detour-information-checklist.md` 确认 `getCartLocation()` 字段、`tick`、`l_step`、定位延迟、重定位行为和质量状态接口。
 2. 在Detour信息返回前不继续全局放宽状态估计边界，也不提高自转角速度/角加速度；现有偶发定位不可用保持安全停车。
-3. 若工程上必须先降低位置型自转失败率，再单独设计仅在纯自转结束、轮组无平移且航向有效时启用的位置连续化；不得扩展到普通轨迹运动，也不得吸收航向偏移。
+3. 为轮组里程计自转CSV补充请求角度、内部积分角、完成/超时/手动停止原因，按固定目标角重复验证误差和重复性；绝对航向任务继续保留Detour模式，不用轮组积分冒充世界航向。
 4. 为轨迹插值/投影、坐标变换、状态跳变候选和终点策略继续补充不依赖宿主的数学回归测试。
 5. 设计车队建立生命周期：预设布局引导就位，夹紧静止后同步采集成员位姿，创建并原子激活新的 `FleetLayout`，松开后清除；车队参考系和采样同步方法需先明确。
-6. 继续补充 `FleetKinematics` 的圆弧、任意布局和倒车数学场景，再接入车队状态与控制流程，不直接启用 `#if false` 旧多车代码。
+6. 建立车队状态与执行边界：将时间对齐后的成员状态聚合为 `FleetState`，把 `FleetKinematics` 的成员命令接入各车 `SendBodyTwist()`；通信、成员纠偏和底盘发送不塞回 `PathTrackingCore`，也不直接启用 `#if false` 旧多车代码。
 7. 在接入通信前定义车队状态时间对齐、心跳/命令有效期、共同速度缩放、成员故障整队停车和舵轮准备同步屏障。