增加车队布局与刚体速度分配基础

docs/decisions.md

@@ -90,17 +90,26 @@
 - `l_step` 当前只作为诊断和后续健康分级依据，不单独决定状态有效性。
 - 依据：`DetourVehicleStateProvider`、`WheelFeedbackVehicleStateProvider`、`MultiWheelRotateInPlace`。
 
+### 13. 车队布局采用不可变快照，刚体速度采用确定性分解
+
+- `Shared/Fleet/FleetLayout.cs` 保存经车号唯一性和有限值校验的 `VehicleLayout` 快照，不提供搬运过程中的逐车修改入口。
+- `FleetKinematics.Decompose()` 已按平面刚体关系把 `FleetMotionCommand` 分解为各成员中心速度，并转换到各车真实车体系；不依赖通信、Detour、β、底盘限幅或QP。
+- `MultiWheelC.Tests/FleetKinematicsTests.cs` 已覆盖整体平移、绕车队中心旋转、绕成员车旋转和停止四个数学场景。
+
 ## 已经确认但尚未实施
 
 - 路线顺序：先完成单车闭环和停车功能验证，再正式实施多车通信、编队和协同控制。来源：`README.md`。
-- 当前代码没有可以确认的完整多车分配方案：`Shared/Fleet/FleetKinematics.cs` 仍只有职责注释，`VehicleLayout` 和 `FleetMotionCommand` 目前只有模型定义、没有业务调用；不得把这些类型当成已经实现的功能。
+- 当前只完成车队布局模型和纯运动学分解，尚未形成可运行的多车链路：布局采集/激活、车队状态估计、轨迹控制、通信和安全协调仍未接入业务流程。
+- 布局生命周期区分夹紧前后的语义：夹紧前的预设布局只用于引导车辆就位；车辆夹紧且静止后，应同步读取成员位姿，选择车队参考系并创建新的不可变 `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`。该方案尚未实施。
 - 每辆成员车都应作为反馈来源，但反馈职责必须分层：成员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`。
-- 旧版 `SetOriginBias(layoutX, layoutY, layoutTh)` 是把各车真实轮子统一表达在车队虚拟坐标系中，属于固定编队布局变换；蟹行通过共同 `frontTh/rearTh` 表达。它与新版“每车按运动段独立选择纯旋转β”的职责不同，虽然两者可产生相同的实际轮子姿态和车辆运动。
+- 旧版 `SetOriginBias(layoutX, layoutY, layoutTh)` 是把各车真实轮子统一表达在车队虚拟坐标系中，属于固定编队布局变换。旧版联动显式区分常规、蟹行和绕车队中心旋转三类模式；蟹行角可由动作或遥控给出任意值（`FleetCrabWalk` 默认45°），并在运动前以零速度对齐舵轮、运行时使用180°等效和轮速反号，但没有根据整段轨迹和每车约束自主求解β的统一规划过程。给定简单蟹行动作时，它与新版固定β可能产生相同的实际轮子姿态和车辆运动。
 - 旧版自动 `FleetCurveWalk`、`FleetCrabWalk` 会先以零速度下发初始GCP角，等待成员新鲜、布局正确、命令可行、舵轮到位和从车应用新序列后才开始运动；原地旋转通过 `RotateWheelsAligned` 和 `FleetMotionReleased` 做整队释放。普通手动入口仍有 `SendMotion` 本车舵轮未对齐时速度置零的门控，但不保证与自动动作相同的车队级同步屏障。
 - 旧版的全局与局部定位用途不同：自动模式的车队中心估计仍依赖Detour，与局部POS纠偏开关无关；邻车两腿检测只能提供相对间距和姿态信息，不能单独确定世界坐标中的车队中心。
 - 旧版单车几何控制器和原地自转直接读取 `DetourInterface.getCartLocation()`，未见当前新版的源时间对齐、跳变候选或任务坐标连续化；旧版宿主参考代码另有基于 `l_step` 和数据新鲜度减速/暂停并触发重定位的 `RelocalizationManager`。这只能说明旧版通过宿主安全状态机管理明显失效，不能据此认定Detour位姿天然连续；实车部署是否使用相同宿主版本和配置仍待确认。来源：`mdcstoolbox/Clumsy/MotionControllers/AbstractGeometricController.cs`、`原版停车机器人/parkingrobot/ClumsyPilot/Movements.cs`、`mdcstoolbox/Clumsy/HighLevelSecurity/RelocalizationManager.cs`。
@@ -110,6 +119,7 @@
 ## 待评估
 
 - 多车共同搬运时的车队参考点与固定GCP距离、任意成员布局、加权/异常值鲁棒的车队位姿估计、队形误差闭环，以及 `FleetMotionCommand → 每车Twist2D` 的具体分配与限幅算法。
+- 确定性刚体分配验证完成后，再评估集中式单步QP/HQP：安全和刚体可行性应作为硬约束或更高层级，相对布局、中心跟踪、平滑与能耗依次降低优先级；严格HQP不能仅靠单个加权QP的大权重近似。短时域MPC及舵轮动态延迟预测属于更后续阶段。
 - 负载共享和内力监控可用信号、阈值、降级与停车策略；当前项目尚未建立可确认的力/扭矩闭环。
 - 正式轨迹规划层与 `Trajectory2D` 的接入格式；当前 `TestTrajectoryFactory` 仅用于实验。
 - 是否长期保留旧版GCP/`SendMotion`后端，或增加经过充分验证的“车体Twist直接到各轮”的新后端。