完善状态估计、车队协调与舵轮辨识日志

This commit is contained in:
2026-08-24 18:39:41 +08:00
parent 0ab409cd2a
commit 95c0b19a26
40 changed files with 5983 additions and 95 deletions
+23 -9
View File
@@ -89,19 +89,33 @@ MovementTest / MotionPlanExecutor
`TrajectoryTrackingMovement` 默认从 `PilotDefinition.Conf` 读取车辆级参数,同时保留少量动作级覆盖字段;横向控制器可通过 `LateralControllerFactory` 替换,纵向控制器当前固定创建为 `PidLongitudinalController`
## 车队纯计算调用
## 车队组件与尚未贯通的执行
```text
FleetState
→ FleetController
→ PathTrackingCore.Compute()
→ GcpKinematics.ToBodyTwist()
→ FleetMotionCommand(参考点为车队原点)
→ FleetKinematics.Decompose(FleetLayout, command)
→ FleetMemberCommand[](各成员真实车体系Twist
夹紧且静止时的成员世界位姿快照
→ FleetLayoutCapture.Capture()
→ 初始FleetPoseInWorld + 不可变FleetLayout
成员状态样本 FleetMemberStateSample[]
→ FleetStateEstimator.Estimate()
→ FleetState + FleetMemberLayoutError[]
→ FleetCoordinator.ExecuteCycle()
→ FleetController(虚拟中心轨迹闭环和固定β_fleet)
→ 相对布局误差统一速度缩放
→ FleetKinematics.Decompose()
→ FleetMemberCommandCorrector
→ FleetMemberCommand[]
车队动作主要滚动方向β_fleet
→ FleetPreparationCoordinator(换算每车β_i并等待全员Ready)
→ FleetMemberAgent(本车停车准备、舵轮到位、激活、Execute/Stop
```
这条链已经覆盖虚拟中心轨迹闭环和确定性刚体速度分解,但尚未接入车队状态估计、通信/时间对齐、相对布局纠偏、共同能力限幅、安全降级和成员底盘发送
`FleetLayoutCapture` 只负责固定布局的几何计算:车队原点X/Y取成员车体中心的算术平均,车队Yaw取主车Yaw,再把各成员世界位姿反变换为 `VehicleLayout.PoseInFleet`。它不读取通信或Detour,也不负责静止/夹紧确认、时间对齐和布局激活
上述类目前是可以独立构造和测试的组件,并没有正式的车队任务运行入口把两条链串起来。缺少的外层需要负责布局原子激活、成员状态实际采集与时间对齐、准备/激活状态机、每周期协调、安全门控、成员命令分发、任务完成与取消。`FleetCoordinator` 生成零速或故障结果不等于实车已经停车;只有运行层把结果送到各车 `FleetMemberAgent.Execute()``Stop()` 后才会影响底盘。
实际部署为每车独立电脑,因此主车还需要状态/命令通信,从车需要本地命令超时看门狗。无线串口初始化可以后接,但消息契约、任务号、心跳/有效期和本地失联停车语义必须在运行层接入前明确。
## 状态数据流