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

This commit is contained in:
2026-08-21 17:33:29 +08:00
parent fea2265e2d
commit 0ab409cd2a
33 changed files with 1949 additions and 989 deletions
+24 -7
View File
@@ -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