完善状态估计、车队协调与舵轮辨识日志
This commit is contained in:
+13
-1
@@ -36,6 +36,17 @@
|
||||
|
||||
`FleetState.SampleTimestampSeconds` 表示主车/协调器生成该车队状态快照时的本机单调时间，不是Detour全局时间。各成员电脑的本机时钟和Detour `tick` 当前不能直接互相比较；未来接收层应另行保存来源时间并完成新鲜度和时间对齐。
|
||||
|
||||
### `FleetLayoutCapture`
|
||||
|
||||
文件：`MultiWheelC/Fleet/FleetLayoutCapture.cs`。
|
||||
|
||||
- `FleetMemberPose`：用于布局采集的车号和成员世界位姿输入。
|
||||
- `FleetLayoutCaptureResult`：同时返回采集时刻的 `FleetPoseInWorld` 和固定的 `FleetLayout`；车队当前世界位姿不存入布局。
|
||||
- `Capture(members, leaderVehicleId)`：车队原点X/Y取成员车体中心的算术平均，Yaw取主车Yaw，并通过 `inverse(FleetPoseInWorld) ∘ VehiclePoseInWorld` 得到每车 `PoseInFleet`。
|
||||
- 空成员、非法或重复车号、非有限位姿、主车不存在均拒绝采集；主车缺失时不使用其他车辆降级代替。
|
||||
|
||||
该接口是纯几何计算，调用者必须在外部保证成员位姿处于同一世界坐标系，并完成夹紧、静止、数据新鲜度和时间对齐检查；布局的存储与原子激活也不属于该类。
|
||||
|
||||
## 轨迹契约
|
||||
|
||||
文件：`MultiWheelC/Trajectory/`。
|
||||
@@ -133,7 +144,7 @@ Vβ = cos(β)·Vx_body + sin(β)·Vy_body
|
||||
|
||||
状态暂时不可用时，外层调用 `PauseForUnavailableState()` 保留当前轨迹与投影连续性；明确失败或取消分别使用 `Fail()`、`Cancel()`。
|
||||
|
||||
`ParkingGeometricController` 是单车适配层，负责读取 `IVehicleStateProvider` 并通过 `GcpCommandExecutor` 发送实体底盘命令。`FleetController` 是车队虚拟中心适配层，使用 `FleetState.FleetPoseInWorld` 和 `TwistAtFleetOriginInFleet` 调用同一核心，再将GCP结果转换为车队原点处的 `FleetMotionCommand`。
|
||||
`ParkingGeometricController` 是单车适配层，负责读取 `IVehicleStateProvider` 并通过 `GcpCommandExecutor` 发送实体底盘命令。`FleetController` 是车队虚拟中心适配层，使用 `FleetState.FleetPoseInWorld` 和 `TwistAtFleetOriginInFleet` 调用同一核心。构造参数 `motionDirectionInFleetRadians` 定义固定的 `β_fleet`：核心按该方向投影实际纵向速度，GCP结果先解释为运动坐标系Twist，再通过 `R(β_fleet)` 转换成车队坐标系下、车队原点处的 `FleetMotionCommand`。`β_fleet=0` 保持常规前向语义；当前控制器不负责运行中切换β。
|
||||
|
||||
当前速度闭环和执行边界的语义并不完全相同：纵向PID与Stanley实际速度分母使用 `Vβ`（Stanley也可按配置改用参考速度），而 `GcpMotionCommand.SpeedMetersPerSecond` 和旧版 `SendMotion.speed` 表示带行驶方向符号的车体中心平移速度模长。正常圆弧理想跟踪时 `Vy=0`，两者相等；只有横向误差共同转角产生非零 `Vy` 时，模长与 `Vβ` 才相差余弦因子。当前最大命令速度仍限制最终发送的模长。
|
||||
|
||||
@@ -198,6 +209,7 @@ Vrear = (Vx, Vy - ωR)
|
||||
## M/C IO与MCU边界
|
||||
|
||||
- C层 `PilotDefinition` 和M层 `DiverCartDefinition` 使用 `[AsUpperIO]`/`[AsLowerIO]` 对齐夹臂命令、驱动使能、位置反馈和车号等字段。
|
||||
- M层 `WheelSpeedDiagnosticLogger` 每次记录生成 `_can.csv` 和 `_snapshot.csv`：前者按CAN回调时刻保存八电机速度/位置与四舵角事件，后者按最多50Hz保存目标/实际舵角、目标角速度、PID/前馈/合成差速、限幅前后电机命令、速度/位置/电流反馈及各事件的本机单调时间、序号和数据年龄。MATLAB辨识时可使用 `TargetTh` 作为参考、`TotalDiff` 或 `Sent*` 作为执行输入、`ActualTh` 作为输出；左右轮公共/差速通道由方向统一后的左右命令和反馈在分析侧组合。
|
||||
- `DiverCartDefinition.CommunicationInit()` 默认通过Windows `COM4`、`1,000,000 baud` 打开MCU桥；`MCUPort` 是可配置初始化参数。
|
||||
- MCU内部配置为逻辑端口0：CAN `500,000 bit/s`；逻辑端口1～3：串口 `9,600 bit/s`。`MCURoutine.BatteryPortIndex = 3` 指MCU桥逻辑端口，不等同于Windows `COM3`。
|
||||
- CAN命令/反馈范围集中在 `MCURoutine.cs`：驱动命令 `0x201～0x20A`，速度/位置反馈 `0x281～0x28A`，状态 `0x181～0x18A`，舵角 `0x18B～0x18E`，远程帧 `0x701～0x70A`。
|
||||
|
||||
Reference in New Issue
Block a user