ParkingRobot/docs/interfaces.md

关键接口与数据约定

坐标系和单位

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 当前不能直接互相比较；未来接收层应另行保存来源时间并完成新鲜度和时间对齐。

轨迹契约

文件：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

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 是完整车体平面速度沿β方向的投影：

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 调用同一核心，再将GCP结果转换为车队原点处的 FleetMotionCommand。

当前速度闭环和执行边界的语义并不完全相同：纵向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：

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] 对齐夹臂命令、驱动使能、位置反馈和车号等字段。
• 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位和驱动方向属于实车安全边界，未经明确要求不得修改。