增加车队布局与刚体速度分配基础
This commit is contained in:
+13
-3
@@ -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直接到各轮”的新后端。
|
||||
|
||||
+6
-2
@@ -22,11 +22,15 @@
|
||||
| `Point2D` | 二维位置或向量，单位m |
|
||||
| `Pose2D` | 二维位置和朝向，单位m/rad |
|
||||
| `Twist2D` | 同一点处的 `Vx`、`Vy`、`Omega`，单位m/s、rad/s |
|
||||
| `VehicleLayout` | 单车车体系在车队系中的位姿；当前仅数据模型 |
|
||||
| `FleetMotionCommand` | 车队参考点及该点速度；当前尚无车队分配执行器 |
|
||||
| `VehicleLayout` | 单车车体系在车队系中的固定目标位姿 |
|
||||
| `FleetLayout` | 不可变的成员布局快照，构造时校验成员数量、车号唯一性和位姿有限值 |
|
||||
| `FleetMotionCommand` | 车队参考点及该点在车队系中表达的刚体速度 |
|
||||
| `FleetMemberCommand` | 指定车辆及其真实车体系中表达的成员中心速度 |
|
||||
|
||||
坐标变换集中在 `FrameTransform2D`；有限值检查集中在 `NumericGuard`；角度处理集中在 `AngleMath`。
|
||||
|
||||
`FleetKinematics.Decompose()` 依据 `v_i = v_ref + omega × (r_i-r_ref)` 生成每车命令，再按 `VehicleLayout.PoseInFleet` 的朝向把线速度从车队系转换到成员车体系；所有刚性连接成员的 `Omega` 保持相同。
|
||||
|
||||
## 轨迹契约
|
||||
|
||||
文件：`MultiWheelC/Trajectory/`。
|
||||
|
||||
+8
-4
@@ -1,6 +1,6 @@
|
||||
# 当前进展
|
||||
|
||||
更新日期：2026-08-19。这里只保存当前状态，不作为完整开发历史。
|
||||
更新日期：2026-08-20。这里只保存当前状态，不作为完整开发历史。
|
||||
|
||||
## 已完成/已接入
|
||||
|
||||
@@ -19,6 +19,7 @@
|
||||
- C层轨迹CSV已补充原始/滤波轮组 `Vx/Vy/Vw`、轮组与预测时刻、Detour帧间隔、实际/允许创新、候选原因、估计器状态和不可用原因。
|
||||
- 停车控制参数集中到 `Configuration/PilotConfig.ParkingControl.cs`。
|
||||
- `MultiWheelC.Tests` 已提供不依赖实车的Stanley前进/倒车横向符号回归，8个场景通过；该项目不进入正式解决方案和打包脚本。
|
||||
- `Shared/Fleet` 已加入不可变 `FleetLayout`、成员/车队命令模型和确定性 `FleetKinematics`；不依赖实车的4个刚体分解场景已通过。
|
||||
- 本次建立工作区/项目AGENTS导航和 `docs/` 按需知识库。
|
||||
|
||||
以上表示代码入口存在，不表示全部实车工况已经验收。
|
||||
@@ -29,13 +30,14 @@
|
||||
- 暂时冻结普通运动跳变阈值和30°/s、40°/s²自转参数，等待Detour接口语义后再决定是否实施自转结束后的条件化仅位置连续化或调整航向恢复策略。
|
||||
- Detour对接最小问题已整理到 `docs/detour-information-checklist.md`，不要求取得源码。
|
||||
- 验证非零β运动系，当前已有45°蟹行直线入口；曲线蟹行仍需设计实验。
|
||||
- 在单车和停车流程稳定后研究多车共同搬运的刚体模型与成员命令分配。
|
||||
- 多车共同搬运已完成布局模型和纯刚体速度分解；下一阶段是夹紧后布局采集/原子激活、车队状态与控制、通信和安全协调，再把成员命令接入各车 `SendBodyTwist()`。QP/HQP不作为第一版前置条件。
|
||||
- β在多车中定位为单车执行坐标系而非车队核心优化量：常规、斜行和横移动作先确定车队主要滚动方向，各车按布局朝向换算本地β，停车预对齐并经车队同步屏障统一释放。轨迹级β搜索只作为机械余量或复杂方向变化下的后续增强。
|
||||
|
||||
## 阻塞/待确认
|
||||
|
||||
- Clumsy/Medulla正式宿主、插件部署和配置持久化说明未纳入仓库。
|
||||
- 完整停车业务流程和验收指标未确认。
|
||||
- 多车通信协议、车队参考点、车辆布局来源和故障降级策略未确认。
|
||||
- 多车通信协议、统一时间轴、车队参考点选取、夹紧后布局采集方法、刚体误差阈值和故障降级策略未确认；已确认运行布局应在夹紧后生成不可变快照并由上层整体激活。
|
||||
- 最新实车实验数据对控制周期与舵轮滞后的结论尚未沉淀为可复核结果。
|
||||
- Detour `l_step` 精确定义、显式重定位/坐标重置状态和部署端MDCS恢复策略待确认；当前不能把 `l_step<4` 当作唯一有效性条件。
|
||||
- Detour `getCartLocation()` 的位姿坐标系、`tick` 采样/解算/发布语义、是否已有独立连续里程计或定位状态接口，以及部署版本和实际里程计/SLAM配置待确认。
|
||||
@@ -48,4 +50,6 @@
|
||||
2. 在Detour信息返回前不继续全局放宽状态估计边界，也不提高自转角速度/角加速度；现有偶发定位不可用保持安全停车。
|
||||
3. 若工程上必须先降低位置型自转失败率，再单独设计仅在纯自转结束、轮组无平移且航向有效时启用的位置连续化；不得扩展到普通轨迹运动，也不得吸收航向偏移。
|
||||
4. 为轨迹插值/投影、坐标变换、状态跳变候选和终点策略继续补充不依赖宿主的数学回归测试。
|
||||
5. 单车稳定后，再设计 `FleetMotionCommand → 每车Twist2D` 的车队运动学，不直接启用 `#if false` 旧多车代码。
|
||||
5. 设计车队建立生命周期：预设布局引导就位，夹紧静止后同步采集成员位姿，创建并原子激活新的 `FleetLayout`，松开后清除；车队参考系和采样同步方法需先明确。
|
||||
6. 继续补充 `FleetKinematics` 的圆弧、任意布局和倒车数学场景，再接入车队状态与控制流程，不直接启用 `#if false` 旧多车代码。
|
||||
7. 在接入通信前定义车队状态时间对齐、心跳/命令有效期、共同速度缩放、成员故障整队停车和舵轮准备同步屏障。
|
||||
|
||||
Reference in New Issue
Block a user