219 lines
18 KiB
Markdown
219 lines
18 KiB
Markdown
# 关键接口与数据约定
|
||
|
||
## 坐标系和单位
|
||
|
||
`Shared/` 的统一约定：
|
||
|
||
- 位置：m；线速度：m/s；角度：rad；角速度：rad/s。
|
||
- 真实车体坐标系：X向前、Y向左、逆时针为正。
|
||
- 原始世界坐标来自Detour；位姿在 `DetourVehicleStateProvider.ReadDetourObservation()` 边界由mm/deg转换为m/rad，再通过任务坐标变换生成连续控制位姿。
|
||
- `AngleMath` 的弧度归一化范围是 `[-π, π)`，角度范围是 `[-180°, 180°)`。
|
||
- `Pose2D` 表示局部坐标系在父坐标系中的位姿；变量名使用 `XxxInYyy` 说明关系。
|
||
- `Twist2D` 不携带坐标系标签，必须由变量名、外层类型或接口契约说明。
|
||
|
||
旧版 `CommonUsage` 底盘接口使用混合单位：轮子位置和 `ControlPointRadius` 为mm，`SendMotion` 舵角与 `CarSpeed.Vw` 为deg/deg/s，线速度为m/s。单位转换应只出现在Shared适配边界。
|
||
|
||
## Shared 数据模型
|
||
|
||
文件：`Shared/Models/MotionModels.cs`。
|
||
|
||
| 类型 | 语义 |
|
||
| --- | --- |
|
||
| `Point2D` | 二维位置或向量，单位m |
|
||
| `Pose2D` | 二维位置和朝向，单位m/rad |
|
||
| `Twist2D` | 同一点处的 `Vx`、`Vy`、`Omega`，单位m/s、rad/s |
|
||
| `VehicleLayout` | 单车车体系在车队系中的固定目标位姿 |
|
||
| `FleetLayout` | 不可变的成员布局快照，构造时校验成员数量、车号唯一性和位姿有限值 |
|
||
| `FleetMotionCommand` | 车队参考点及该点在车队系中表达的刚体速度 |
|
||
| `FleetMemberCommand` | 指定车辆及其真实车体系中表达的成员中心速度 |
|
||
| `FleetState` | 同一采样时刻的车队虚拟中心位姿和速度快照 |
|
||
|
||
坐标变换集中在 `FrameTransform2D`；有限值检查集中在 `NumericGuard`；角度处理集中在 `AngleMath`。
|
||
|
||
`FleetKinematics.Decompose()` 依据 `v_i = v_ref + omega × (r_i-r_ref)` 生成每车命令，再按 `VehicleLayout.PoseInFleet` 的朝向把线速度从车队系转换到成员车体系；所有刚性连接成员的 `Omega` 保持相同。
|
||
|
||
`MultiWheelC/Fleet/FleetState.cs` 中，`FleetPoseInWorld` 表示车队虚拟中心位姿，其 `Yaw` 同时定义车队坐标系 `+X` 在世界系中的方向；车队系采用 `+X` 前、`+Y` 左、逆时针为正。`TwistAtFleetOriginInWorld` 与 `TwistAtFleetOriginInFleet` 是同一参考点、同一物理速度在两个坐标系中的表达，后者由前者和 `FleetPoseInWorld` 推导，不是第二份独立测量。`HasValidVelocityEstimate=false` 时速度按零保存，用于区分尚未形成可靠速度估计与真实零速。
|
||
|
||
`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/`。
|
||
|
||
### `TrajectoryPoint`
|
||
|
||
- `ArcLengthMeters`：按预定执行点序从起点累计的弧长，首点必须为0。
|
||
- `PoseInWorld`：车体中心参考位姿；Yaw始终表示车头方向，不因倒车改为车尾方向。
|
||
- `CurvaturePerMeter`：沿弧长增加/执行点序定义，左弯为正。
|
||
- `ReferenceSpeedMetersPerSecond`：轨迹切线方向的有符号车体中心参考平移速度，绝对值是速度模长，正值前进、负值倒车、0停车。
|
||
|
||
### `Trajectory2D`
|
||
|
||
- 至少两个点，弧长严格递增，相邻位置不能重合。
|
||
- 弧长增量必须与离散线段长度在容差内一致。
|
||
- 当前是空间轨迹，以弧长插值位置、Yaw、曲率和参考速度；没有时间戳，也不是时间参数化轨迹。
|
||
- `SampleAtArcLength()` 和 `TrajectoryProjector` 共用 `InterpolateSegment()`，避免两套插值语义。
|
||
|
||
### `TrajectoryProjector`
|
||
|
||
- 首周期可全轨迹搜索；后续控制器使用上次弧长附近窗口，当前常量为后退0.10m、前进1.00m。
|
||
- 距离并列时优先接近上次进度，降低交叉或平行轨迹跳段风险。
|
||
- `LateralErrorMeters` 相对轨迹点序判断左右，轨迹位于车辆左侧时为正。
|
||
- `HeadingErrorRadians` 是参考车头航向减实际车头航向的最短角差。
|
||
|
||
测试轨迹由 `Experiments/TestTrajectoryFactory.cs` 生成，不是正式规划层。直线和曲线速度同号；负速度表示倒车，测试工厂不支持在同一条曲线中直接切换前进/倒车方向。
|
||
|
||
## 状态接口
|
||
|
||
### `DetourInterface.getCartLocation()`
|
||
|
||
当前引用接口返回的定位对象包含：
|
||
|
||
- `x`、`y`：Detour世界坐标，单位mm。
|
||
- `th`：车体航向，单位deg。
|
||
- `tick`：定位源时间；实车静态记录确认可按`.NET DateTime.Ticks`转换，同时必须保留原始整数用于诊断。
|
||
- `l_step`：Detour定位过程的质量/步骤相关指标，精确定义和正式阈值待Detour文档确认；实车正常静态基线包含2和周期性单帧3，不能将3直接判为异常。
|
||
|
||
`DetourVehicleStateProvider` 使用 `x/y/th` 生成控制位姿，使用 `tick` 区分重复、新到和倒退帧，并保留 `l_step` 供诊断。`l_step` 当前不作为单一硬门限：实车数据中既出现过高 `l_step` 后恢复，也出现过低 `l_step` 但位姿创新异常的情况。状态时间仍使用本机单调 `Stopwatch`；`Experiments/DetourStaticDiagnosticTest.cs` 可只读记录原始字段、接口耗时、帧间差和轮组反馈。
|
||
|
||
当前继续设计状态层所需的Detour侧最小信息是：`getCartLocation()` 返回的是可能重定位跳变的全局/map位姿还是连续里程计位姿；`tick` 对应采集、解算还是发布时刻及缓存语义；`l_step` 的状态含义；现有接口是否另有连续里程计位姿/速度或定位有效、重定位状态。无需以取得Detour源码为前提，部署版本、里程计和单线激光SLAM配置截图可用于核对实际运行配置。以上信息尚待Detour侧确认。
|
||
|
||
### `IVehicleStateProvider`
|
||
|
||
```text
|
||
bool TryGetState(out VehicleState state)
|
||
```
|
||
|
||
返回 `false` 表示当前状态不可用；`ParkingGeometricController` 会主动停车、重置反馈控制器并等待下一周期恢复。
|
||
|
||
### `VehicleState`
|
||
|
||
- `SampleTimestampSeconds`：状态源单调时钟时间。
|
||
- `PoseInWorld`：当前任务控制世界系中的车体中心位姿；初始化时与Detour世界系对齐，确认有限坐标阶跃后可通过内部变换保持任务连续，不保证始终等于原始Detour坐标。
|
||
- `TwistInWorld`、`TwistInBody`：同一刚体速度的两种表达。
|
||
- `HasValidVelocityEstimate`：速度反馈是否已建立有效时间基准；无效时构造器将速度置零。
|
||
|
||
### 默认组合状态源
|
||
|
||
`ParkingVehicleStateProviderFactory.Create()` 创建：
|
||
|
||
- `DetourVehicleStateProvider`：读取Detour位姿，处理源时间、重复帧、运动合理性、预测创新、静止确认和跳变候选；候选确认期间使用轮组速度短时预测控制位姿。确认后的有限小坐标偏移可更新 `controlFromDetour` 以保持当前任务坐标连续，超限或未恢复时将状态置为不可用。
|
||
- `WheelFeedbackVehicleStateProvider`：调用 `MultiWheelChassis.GetCarSpeed(true)`，低通滤波车体 `Vx`、`Vy` 和由deg/s转换为rad/s的 `Vw`。轮组估计有效后，最终 `VehicleState` 的 `Vx/Vy/Omega` 均使用滤波后的轮组反馈；首帧速度标记为无效，并由 `VehicleState` 对外置零。轮组 `Vw` 同时提供给Detour短时运动预测和动态航向合理性判断。
|
||
|
||
默认滤波参数来自 `PilotConfig.ParkingControl.cs`：Detour线速度0.15s、Detour角速度0.20s、轮组反馈 `Vx/Vy/Vw` 统一为0.10s。
|
||
|
||
跳变处理的重要边界：航向创新允许量随 `|Vw| × Detour源帧间隔` 增加；候选若在近似原地自转期间开始，则整个候选确认过程禁止自动改写任务坐标系。默认候选确认窗口为0.60s，窗口内输出轮组预测状态，超时后完整位姿安全返回不可用。
|
||
|
||
完整轨迹控制仍通过 `TryGetState()` 要求位置和航向均有效。原地自转提供两种反馈模式：
|
||
|
||
- `DetourAbsoluteHeading` 使用 `TryGetHeadingRadians()` 做世界航向闭环；仅位置异常时可继续，航向异常仍会停止。到达目标并停车后调用 `BeginPostRotationPositionRecovery()` 恢复完整位姿，恢复失败时不释放下一运动段。
|
||
- `RelativeWheelOdometry` 使用 `TryGetWheelTwist(out Twist2D, out timestamp)` 直接读取并滤波轮组 `Vx/Vy/Vw`，按轮组采样时间梯形积分相对转角；活动自转期间不调用Detour，也不执行Detour停车后恢复。该模式用于允许约±5°误差的相对角动作，不能提供绝对世界航向校正，轮胎打滑和轮径误差会累积到角度结果中。
|
||
|
||
`TryGetWheelTwist()` 是不依赖Detour的窄接口；首次尚未形成滤波样本时返回 `false`，其时间戳来自该状态源使用的本机单调时钟。
|
||
|
||
`TrackingExperimentRecorder` 已记录原始/滤波轮组 `Vx/Vy/Vw`、轮组采样时刻、Detour `tick/l_step`、数据年龄和源帧间隔、轮速预测时刻、实际/允许的位置与航向创新、候选触发原因、估计器状态及不可用原因。创新和源帧间隔只在收到Detour新帧时更新；若CSV记录频率高于Detour帧率，后续记录行会重复最近一个新帧的诊断值，分析时应按 `DetourTickRaw` 去重或分组。
|
||
|
||
## 控制接口
|
||
|
||
### `PathTrackingContext`
|
||
|
||
一次控制周期的只读快照，包含：受控刚体的车体系 `Twist2D`、速度有效标志、`TrajectoryProjection`、处理后的控制参考速度、预瞄曲率、真实 `deltaTime` 和运动方向β。该类型不依赖单车 `VehicleState` 或车队 `FleetState`。
|
||
|
||
`ActualLongitudinalSpeedMetersPerSecond` 是完整车体平面速度沿β方向的投影：
|
||
|
||
```text
|
||
Vβ = cos(β)·Vx_body + sin(β)·Vy_body
|
||
```
|
||
|
||
因此45°/90°蟹行不能只使用车体 `Vx` 判断纵向速度。
|
||
|
||
### `PathTrackingCore`
|
||
|
||
`PathTrackingCore.Compute(Pose2D, Twist2D, bool, double)` 是单车与车队共用的纯轨迹跟踪周期：负责连续投影、距离保护、起步释放、终点制动/完成判断、曲率预瞄、横纵向控制和GCP分配，返回 `PathTrackingCycleOutput`。输出只包含周期结果、可选 `GcpMotionCommand`、投影和计算耗时；核心不读取状态源、不发送底盘命令，也不处理通信。
|
||
|
||
状态暂时不可用时，外层调用 `PauseForUnavailableState()` 保留当前轨迹与投影连续性；明确失败或取消分别使用 `Fail()`、`Cancel()`。
|
||
|
||
`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β` 才相差余弦因子。当前最大命令速度仍限制最终发送的模长。
|
||
|
||
### `ILateralController`
|
||
|
||
- `Compute(PathTrackingContext)` 返回 `LateralControlCommand`。
|
||
- `Reset()` 清除跨周期状态。
|
||
- 默认实现 `StanleyLateralController` 输出前后GCP角度：横向误差形成共同转角，航向误差和曲率前馈形成差动转角；倒车时按行驶方向修正符号。
|
||
- `TrajectoryTrackingMovement.LateralControllerFactory` 是替换Stanley的动作级扩展点。
|
||
|
||
### `ILongitudinalController`
|
||
|
||
- `ComputeSpeedMetersPerSecond(PathTrackingContext)` 返回有符号中心命令速度。
|
||
- 默认 `PidLongitudinalController` 使用轨迹参考速度前馈叠加实际速度PID反馈，带死区、积分限制和最大命令速度；参考明确为0时禁止反向速度纠偏。
|
||
|
||
### GCP命令
|
||
|
||
- `LateralControlCommand`：前后GCP目标角度，以及共同/差动分量。
|
||
- `GcpCommandAllocator`：分别限制前后GCP角度并与纵向速度组合。
|
||
- `GcpMotionCommand`：进入底盘执行前的有符号速度和前后GCP角度，SI单位。
|
||
- `GcpCommandExecutor`：限制GCP角速度，保存 `LastRequestedCommand`/`LastSentCommand`，转换为 `Twist2D` 后执行。
|
||
|
||
对称前后GCP位于当前运动系的 `(±R, 0)`。依据刚体速度关系 `v(point)=v(center)+ω×r`：
|
||
|
||
```text
|
||
Vfront = (Vx, Vy + ωR)
|
||
Vrear = (Vx, Vy - ωR)
|
||
```
|
||
|
||
因此 `Vy` 形成前后同向的共同转角，`ωR` 形成前后反向的差动转角。正常圆弧的理想车体速度为 `(v, 0, v·κ)`，曲率通过差动转角实现，并不要求车体系存在 `Vy`。
|
||
|
||
## 底盘命令接口
|
||
|
||
### `MultiWheelChassisAdapter.SendBodyTwist`
|
||
|
||
输入始终是真实车体系 `Twist2D`：
|
||
|
||
- 平移和角速度均近零：立即停车。
|
||
- 平移近零、角速度非零：要求真实车体系已激活，调用 `SendXYThSpeed` 做纯自转。
|
||
- 平移非零：使用动作开始前已经准备并激活的固定β运动系，转换为前后GCP方向并调用旧版 `SendMotion`。
|
||
|
||
运动中不通过 `Vx/Vy` 猜测模式；模式由“停车→舵轮预对齐→`ActivateMotionFrame(β)`”显式确定。命令若几乎垂直于当前运动系X轴会停车并拒绝执行。
|
||
|
||
`MultiWheelChassisAdapter` 构造时读取旧底盘 `GetOriginBias().Z`，按 `β=-biasZ` 同步内部缓存；这不是SLAM Yaw，也不会切换坐标系或转动舵轮。真正切换由 `ActivateMotionFrame(β)` 调用 `SetOriginBias(0, 0, -β)` 完成，旧底盘随后把真实轮位重新表达在β运动系中。
|
||
|
||
滚动命令在 `SendRollingTwistInActiveMotionFrame()` 中按 `R(-β)` 将真实车体系平面速度转换到已激活运动系；角速度在二维旋转变换下不变。传给 `SendMotion` 的前后GCP角度均相对该运动系X轴，速度绝对值为 `sqrt(Vx²+Vy²)`，正负号取运动系 `Vx` 的方向。倒车方向由速度符号表达，GCP角度保持为相对行驶方向的等效机械方向，避免无意义旋转180°。
|
||
|
||
### `MultiWheelChassis.SendMotion`
|
||
|
||
输入是车体中心有符号平移速度模长、当前运动系中的虚拟前后GCP方向。前后GCP法线交点确定ICR；每个真实轮子使用其运动系位置计算切线舵角和半径速度比例。机械限位通过等效舵角/反向轮速解析，无法满足时返回失败原因。
|
||
|
||
## 配置接口
|
||
|
||
- `PilotConfig` 是Clumsy运行配置模型；`[FieldMember]` 字段提供默认值和宿主显示/持久化元数据。
|
||
- `PilotDefinition.Conf` 是C层动作实际读取的运行配置对象。
|
||
- `MultiWheelC/Configuration/PilotConfig.ParkingControl.cs` 集中停车状态估计、Stanley、纵向PID、原地自转、舵轮准备、GCP与完成条件参数。
|
||
- 动作中的 nullable 覆盖字段用于特定动作段或测试；为空时使用车辆配置。车辆级限速和通用控制参数不应在普通实验中随意覆盖。
|
||
- `参考文档/*.json` 是样例/实车复制资料；当前源码未发现这些JSON被 `PilotDefinition.Conf` 自动读取的入口。
|
||
|
||
运行时配置文件格式、位置和覆盖优先级由外部Clumsy宿主决定，当前仓库内待确认。
|
||
|
||
## 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`。
|
||
- `MCUSerialBridgeCLR.cs` 是 `mcu_serial_bridge.dll` 的P/Invoke封装；本仓库源码目录未包含该本机库。
|
||
|
||
CAN ID、串口参数、IO位和驱动方向属于实车安全边界，未经明确要求不得修改。
|