8.9 KiB
EM 闭环测试设计
目标
在 ClumsyPilot 中新增名为“EM闭环测试”的 MovementTest。测试从车辆当前位姿出发完成一次 EM 轨迹规划，冻结首次成功生成的单方向段轨迹，将其转换为现有几何控制器可消费的 Trajectory2D，并通过车辆状态反馈执行到该方向段终点。
本测试用于验证“轨迹规划结果 → 控制轨迹适配 → Stanley/PID/GCP 控制 → 底盘状态反馈”的首条实车闭环链路。
范围
包含
- 复用现有
TrajectoryObservationMovementTest的当前定位、目标位姿、手动障碍物和规划参数约定。 - 完成粗路径、路径平滑和当前方向段的一次 EM 规划。
- 将首次成功发布的
EmTrajectory冻结，执行期间不重规划、不替换轨迹。 - 将 EM 轨迹转换为控制模块的
Trajectory2D。 - 使用参考工程已有的 Stanley 横向控制、PID 纵向控制、GCP 命令分配和底盘执行逻辑进行闭环跟踪。
- 在当前方向段终点停车并结束测试。
- 支持用户随时调用
TestStop()取消规划或停止运动。
不包含
- 行驶中的周期重规划或轨迹热切换。
- 到达换向点后规划或执行下一方向段。
- 修改控制器的速度符号、舵角符号、CAN ID、机械限位或模式切换策略。
- 对参考控制器进行无关重构。
- 自动代替现场实车安全验证。
现有代码基础
- 现有观察入口：
ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs。 - EM 输出：
EmTrajectory和EmTrajectoryPoint，使用世界坐标米、航向弧度、速度米每秒和曲率每米。 - 参考控制器输入：
MultiWheelC.Trajectory.Trajectory2D，要求至少两个轨迹点、首点弧长为零、弧长严格递增且相邻位置不重复。 ClumsyPilot/Control和ClumsyPilot/Trajectory中已有一批与参考目录内容一致的未跟踪文件。本功能保留并复用这些内容，不覆盖用户的其他工作树修改。- 控制器的运行依赖尚未完整接入，需要补齐其使用到的共享数学/底盘边界、车辆状态源和
TrajectoryTrackingMovement。
方案选择
采用“完整接入最小控制依赖链，并在规划与控制模型之间增加专用适配器”的方案。
不让通用控制器直接依赖 EmTrajectory，以保持规划层和控制层边界；也不引用工作区外的绝对工程路径，避免构建和部署依赖本机目录结构。
组件设计
1. EM 闭环 MovementTest
在 ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest 新增独立测试文件，注册名必须精确为：
EM闭环测试
测试公开参数沿用现有观察测试中与单次规划直接相关的参数，并新增一个保守的控制速度上限。默认值必须适合低速、短距离首轮联调，不得高于参考控制器已有的 0.50 m/s 默认上限。
Test() 的顺序为：
- 防止同一测试重复启动。
- 读取并校验目标位姿与手动障碍物。
- 检查实时定位、实时底盘读接口和
MultiWheelChassis。 - 检查车轮已处于控制器要求的车体前向准备状态。
- 创建可取消的后台任务，执行一次规划。
- 仅在规划成功且控制轨迹适配成功后创建
DriveTask。 - 等待控制器将当前方向段执行至终点。
- 无论成功、失败或取消，都释放任务状态并走停车清理路径。
TestStop() 必须同时请求规划取消并停止活动的 DriveTask。多次调用必须安全，不得因任务已结束而抛出异常。
2. 单次规划流程
单次规划复用观察测试已经验证的建图、粗路径和平滑路径装配流程，不复制规划算法。
规划只生成当前方向段的 EM 轨迹：
- 起点状态来自启动时读取的实时车辆位姿和有符号纵向速度。
- 终点与障碍物在启动时冻结。
- EM 使用完整方向段规划模式。
- 接受的结果必须为成功状态、非空轨迹且至少能转换成两个不同位置的控制点。
- 一旦获得首次成功轨迹，规划对象不再运行，车辆运动期间不发起新周期。
- 轨迹终端若为换向点，本次测试只跟踪到该点并停车；它不是启动下一段的信号。
3. EM 到控制轨迹适配器
新增专用适配器，输入为不可变 EmTrajectory，输出为不可变 Trajectory2D。
字段映射如下：
| EM 字段 | 控制轨迹字段 | 规则 |
|---|---|---|
X, Y |
PoseInWorld.XMeters, YMeters |
原值复制，单位保持 m |
Yaw |
PoseInWorld.YawRadians |
交由共享 AngleMath 按控制模型约定归一化 |
VehicleCurvature |
CurvaturePerMeter |
原值复制，单位保持 1/m |
SignedLongitudinalVelocity |
ReferenceSpeedMetersPerSecond |
保留符号，前进为正、倒车为负 |
| 相邻世界位置 | ArcLengthMeters |
从零开始累计欧氏距离，严格递增 |
EM 终点停车保持可能产生相同位置、不同时间的重复样本。适配器必须过滤与上一个已接收点位置距离小于控制轨迹最小有效线段要求的样本。过滤不得改变剩余点的坐标、航向、曲率或有符号速度。
若输入为空、最终不足两个有效点，或任一必要数值非法，适配失败且不得启动底盘控制。
4. 控制器最小依赖链
从参考工程接入控制闭环所需的最小代码，保持原命名空间、SI 单位与坐标系约定：
Control：横向、纵向、GCP 分配和执行组件。Trajectory：二维轨迹、投影和采样组件。Shared中这些组件实际引用的数学类型、数据模型和MultiWheelChassisAdapter。StateEstimation中TrajectoryTrackingMovement实际使用的实时状态接口和 Detour 状态实现。TrajectoryTrackingMovement及车轮前向准备检查所需的最小运动封装。
不接入实验记录器、复合动作、原地转向或与本测试无关的参考实验。
生命周期与并发
测试对象在任一时刻只允许一个活动会话。会话拥有独立的取消令牌和控制任务引用。
规划发生在后台，避免阻塞测试入口。规划完成与 TestStop() 可能并发，因此只有仍为当前会话且未取消的规划结果才允许启动控制任务。过期会话的结果必须丢弃。
会话结束时按以下顺序清理：请求规划取消、停止活动控制任务、清空活动引用。清理逻辑需要幂等。
安全行为
- 在规划和适配全部成功之前不发送任何底盘运动命令。
- 控制启动前必须确认底盘类型、定位和车轮准备状态。
- 控制器状态不可用时沿用参考实现的主动停车行为。
- 超出最大轨迹偏差、执行超时或控制命令失败时立即停止并报告原因。
TestStop()必须可作为软件侧立即停车入口；现场测试仍须保证硬件急停可用。- 首次实车验证采用低速、短距离、空旷环境，不得直接使用复杂障碍物或换向场景。
诊断与可观察性
状态输出至少包含：
- 会话启动和输入终点。
- 粗路径/平滑/EM 规划成功或失败原因。
- 原始 EM 点数、过滤后的控制点数、轨迹长度和方向。
- 控制启动、到达方向段终点、用户停止、超时和控制故障。
可复用现有绘图能力显示冻结轨迹，但绘图失败不得阻止安全停车，也不得改变规划或控制结果。
测试策略
自动测试
按照测试驱动开发顺序覆盖：
- EM 字段正确映射到控制轨迹。
- 弧长从零开始并按相邻几何距离累计。
- 终点停车保持产生的重复位置样本被过滤。
- 正向和倒向的速度符号均被保留。
- 空轨迹或不足两个有效几何点时拒绝转换。
- MovementTest 注册名精确为“EM闭环测试”。
- 代码只启动一次规划，且不包含周期重规划入口。
- 换向终端只结束当前方向段，不创建下一方向段。
- 停止和异常路径会取消规划并停止控制任务。
构建验证
- 构建新增适配器的验证宿主。
- 构建整个
ClumsyPilot项目，报告全部警告和错误。 - 检查工作树差异，确认未覆盖既有无关修改。
现场验证
自动测试不声明实车闭环已经通过。现场首次运行应选择无需换向的短直线或小曲率单方向段，设置低速上限，确认定位、速度符号、转向方向和软件/硬件停车均正常后再扩大测试范围。
完成标准
ClumsyPilot中出现可选择的“EM闭环测试”。- 测试仅规划一次并冻结一个方向段。
- EM 轨迹经验证适配为
Trajectory2D后才交给参考控制器。 - 当前方向段完成、用户停止或任何失败路径均能停车并结束会话。
- 适配器自动测试通过，
ClumsyPilot完整构建通过。 - 实车执行结果与自动验证结果分开报告，不以构建成功替代现场闭环结论。