diff --git a/docs/superpowers/specs/2026-08-05-em-longitudinal-rolling-planning-design.md b/docs/superpowers/specs/2026-08-05-em-longitudinal-rolling-planning-design.md index aca9c36..f3198a5 100644 --- a/docs/superpowers/specs/2026-08-05-em-longitudinal-rolling-planning-design.md +++ b/docs/superpowers/specs/2026-08-05-em-longitudinal-rolling-planning-design.md @@ -30,7 +30,7 @@ t=2.0s: U=0, A=-0.279m/s^2 t=2.1s: U=0, A=0 ``` -在 `OutputTimeStepSeconds=0.1s` 时,发布验证器根据速度有限差分得到约 `2.79m/s^3` 的 jerk,超过 `0.5m/s^3` 限制。只增加 `A[N]=0` 可以消除这一个突变,但仍会让车辆在每个滚动窗口停车,不能满足连续轨迹规划要求。 +在 `OutputTimeStepSeconds=0.1s` 时,发布验证器根据速度有限差分得到约 `2.79m/s^3` 的 jerk,超过 `0.5m/s^3` 限制。只增加 `A[N]=0` 仍不能直接保证最后一个运动区间的速度有限差分加速度为零;准确停车还必须在 ST 时间范围末尾保留一个完整零速稳定区间。即使补上该稳定区间,若仍把每个局部窗口当作停车点,也不能满足连续轨迹规划要求。 ## 方案选择 @@ -86,7 +86,7 @@ public enum EmLongitudinalMode - `RollingContinuation`:真实方向段末端不在空间窗口内;本次轨迹连续行驶,末点允许非零速度和加速度。 - `ApproachStopBoundary`:真实边界已进入空间窗口,但无法在当前时间范围内完成准确停车;本次轨迹继续前进,不追加保持点。 -- `ExactStopAtBoundary`:在当前时间范围及运动学限制内能够到达真实边界;本次轨迹必须在该边界满足 `S=S_end, U=0, A=0`。 +- `ExactStopAtBoundary`:在当前时间范围及运动学限制内能够到达真实边界;本次轨迹必须在时间范围结束前于该边界满足 `S=S_end, U=0, A=0`,并在剩余时间内保持至少一个完整输出时间步的静止状态。 边界身份继续由 `EmBoundaryType` 表达。纵向行为和边界身份是两个维度,不能再用一个 `EmTerminalType` 同时表达。 @@ -112,7 +112,7 @@ public bool HasStopBoundary { get; } 6. 若真实边界在空间窗口内,但当前时间范围内无法到达并以 `U=0,A=0` 停止,选择 `ApproachStopBoundary`。 7. 若真实边界在当前时间范围内可准确停车,选择 `ExactStopAtBoundary`。 -时间可停车判定使用与 ST 相同的速度、加速度、jerk 和恒 jerk 离散积分模型,避免选择器判断“可停”而 QP 实际不可行。`ZeroSpeedHoldSeconds` 是优化时间范围之外的发布保持段,不从 `TimeHorizonSeconds` 中扣除。 +时间可停车判定使用与 ST 相同的速度、加速度、jerk 和恒 jerk 离散积分模型,避免选择器判断“可停”而 QP 实际不可行。准确停车可使用的运动时间为 `TimeHorizonSeconds - TerminalStabilizationDuration`,其中稳定时间至少为一个完整 `OutputTimeStepSeconds`。`ZeroSpeedHoldSeconds` 是优化时间范围之后额外追加的发布保持段,不从 `TimeHorizonSeconds` 中扣除。 ## jerk 受限停车模型 @@ -184,15 +184,15 @@ exact constant-jerk dynamics `RollingContinuation` 和 `ApproachStopBoundary` 不增加末点等式。现有参考速度、加速度、jerk 和上一轨迹权重继续推动平顺前进;不增加强制 `S[N]=PathUpperBoundS` 的进度条件。 -`ExactStopAtBoundary` 增加三个硬约束: +`ExactStopAtBoundary` 先确定稳定尾的第一个 knot `H`。`H` 到最后一个 knot `N` 的累计时间不得小于一个完整 `OutputTimeStepSeconds`;若最后一个 knot interval 因无法整除时间范围而缩短,则 `H` 相应前移。对每个 `k=H..N` 增加三个硬约束: ```text -S[N] = StopBoundaryPathS -U[N] = 0 -A[N] = 0 +S[k] = StopBoundaryPathS +U[k] = 0 +A[k] = 0 ``` -当前约束行数必须改为动态计算。没有停车终端时为 `8*N-2`;准确停车增加三个终端行后为 `8*N+1`。 +因此真实边界锚点位于 `H`,`H+1..N` 是 QP 内部稳定尾。设 knot 总数为 `K`、稳定尾 knot 数为 `M=K-H`,约束行数为:滚动/接近模式 `8*K-2`;准确停车模式 `8*K-2+3*M`。常规等间隔情况下 `M=2`,约束行数为 `8*K+4`。 ## 初始迭代与顺序凸化 @@ -200,7 +200,7 @@ A[N] = 0 - `RollingContinuation`:从实车 `U0,A0` 出发,按路径速度参考和 jerk/加速度限制向前积分;不强制末点到达路径上界或零速。 - `ApproachStopBoundary`:使用真实边界相关的减速速度参考构造连续前进解;不强制当前周期停车。 -- `ExactStopAtBoundary`:构造巡航加完整 S 曲线停车种子,末点严格为 `S_end,U=0,A=0`。 +- `ExactStopAtBoundary`:构造巡航加完整 S 曲线停车种子,在 `H` 严格到达 `S_end,U=0,A=0`,并将 `H+1..N` 填充为同一位置的零速稳定尾。 `TryCreateEnvelopeIterate`、候选裁剪和末点规范化中所有无条件设置最后一个 `S` 为路径上界的代码,都改为只在 `ExactStopAtBoundary` 执行。滚动模式的末点由动力学和优化结果决定。 @@ -230,12 +230,12 @@ candidatePathS[k] + stoppingDistance(U[k], A[k]) 该检查不信任构建 QP 时使用的顺序凸化采样点。违反可停车域的候选不能发布,并作为下一次外层迭代更新速度包络的依据。 -仅 `ExactStopAtBoundary` 检查并规范化: +仅 `ExactStopAtBoundary` 检查并规范化 `H..N`: ```text -S[N] == StopBoundaryPathS -U[N] == 0 -A[N] == 0 +S[k] == StopBoundaryPathS +U[k] == 0 +A[k] == 0 ``` 滚动和接近模式只要求末点位于 `[0, PathUpperBoundS]` 且满足普通运动学限制,允许非零末点速度、加速度和由曲率产生的 yaw rate。 @@ -245,13 +245,13 @@ A[N] == 0 `TrajectorySampleSchedule` 接收 `EmLongitudinalMode`: - 滚动和接近模式只输出 `0..TimeHorizonSeconds` 的 ST 样本,不要求零速,不追加保持点。 -- 准确停车模式在追加保持点前同时检查末点速度和加速度近零,然后按 `ZeroSpeedHoldSeconds` 追加同一 PathS 上 `U=0,A=0,J=0` 的样本。 +- 准确停车模式在追加保持点前检查候选包含至少一个完整输出时间步的 `S=S_end,U=0,A=0` 内部稳定尾,然后按 `ZeroSpeedHoldSeconds` 继续追加同一 PathS 上 `U=0,A=0,J=0` 的外部保持样本。 -当 `TimeHorizonSeconds=2.0s`、`OutputTimeStepSeconds=0.1s` 时,滚动轨迹包含 21 个样本。若准确停车并配置 `ZeroSpeedHoldSeconds=0.2s`,则再追加 `t=2.1s` 和 `t=2.2s` 两个保持样本。由于优化末点已满足 `A=0`,末点到第一个保持点的有限差分 jerk 为零。 +当 `TimeHorizonSeconds=2.0s`、`OutputTimeStepSeconds=0.1s` 时,滚动轨迹包含 21 个样本。准确停车在常规等间隔情况下最迟于 `t=1.9s` 到达真实边界,`t=2.0s` 是 QP 内部稳定点;若配置 `ZeroSpeedHoldSeconds=0.2s`,再追加 `t=2.1s` 和 `t=2.2s` 两个外部保持样本。由于 `t=1.9..2.2s` 均保持位置、速度和状态加速度不变,速度有限差分加速度和 jerk 在进入外部保持段时都为零。 ## 轨迹组装和发布验证 -`EmTrajectoryMetadata` 记录纵向模式和真实边界身份。`EmTrajectoryAssembler` 只有在准确停车样本到达真实边界时才标记 `Goal` 或 `GearSwitchApproach` 锚点;滚动时间末点不伪装为路径边界。 +`EmTrajectoryMetadata` 记录纵向模式和真实边界身份。`EmTrajectoryAssembler` 将准确停车稳定尾的第一个 knot `H` 标记为 `Goal` 或 `GearSwitchApproach` 锚点;后续内部稳定点和外部保持点不重复创建锚点。滚动时间末点不伪装为路径边界。 `EmTrajectoryValidator` 始终检查有限值、时间递增、速度、有限差分加速度、有限差分 jerk、曲率、曲率变化率、yaw rate、地图边界和车辆轮廓碰撞。 @@ -285,7 +285,7 @@ A[N] == 0 配置验证额外确认: 1. 空间窗口不小于配置最大速度、最大正加速度下的完整 jerk 受限停车距离,再加一个 `MaximumSpeed * ReplanPeriodSeconds` 的重规划行驶余量; -2. 时间范围至少包含两个正时间间隔;若当前完整停车持续时间大于时间范围,则选择 `ApproachStopBoundary` 并发布制动轨迹前缀,而不是拒绝配置; +2. 时间范围在预留一个完整输出时间步的内部稳定尾后仍至少包含一个正运动时间间隔;若当前完整停车持续时间大于剩余运动时间,则选择 `ApproachStopBoundary` 并发布制动轨迹前缀,而不是拒绝配置; 3. knot 生成器始终准确包含 `t=0` 和 `t=TimeHorizonSeconds`,无法整除时最后一个时间间隔缩短但保持为正;保持段采用相同规则准确结束于 `ZeroSpeedHoldSeconds`; 4. 诊断信息打印计算得到的最大停车距离、停车时间和所需空间余量,便于解释配置不足。 @@ -328,17 +328,17 @@ speed=..., acceleration=..., decelerationLimit=..., jerkLimit=... 4. Goal 或 GearSwitch 在本周期可准确停车时选择 `ExactStopAtBoundary`。 5. jerk 受限停车计算最终同时满足 `U=0,A=0`。 6. 滚动 QP 不包含末点 `S/U/A` 等式。 -7. 准确停车 QP 包含末点 `S/U/A` 三个等式。 +7. 准确停车 QP 对真实边界锚点及其后的内部稳定尾都包含 `S/U/A` 三个等式。 8. 滚动初始迭代不强制末点到达路径上界或零速。 9. `TryCreateEnvelopeIterate` 只在准确停车模式固定末点。 10. 滚动采样不追加保持点并允许非零末点速度。 -11. 准确停车采样要求末点 `U/A` 为零并追加保持点。 +11. 准确停车采样要求至少一个完整输出时间步的内部零速稳定尾,然后追加外部保持点。 12. 发布验证器对滚动轨迹不要求零速边界锚点。 13. 发布验证器对准确停车轨迹要求真实边界和 `U/A/yawRate=0`。 ### 回归与集成测试 -1. 复现当前 point 21 场景,确认末个运动点到第一个保持点的 jerk 不超过限制。 +1. 复现当前 point 21 场景,确认真实边界锚点、QP内部稳定点和第一个外部保持点之间的有限差分 jerk 均不超过限制。 2. 在无障碍直线路径上连续执行多次滚动规划,每次都发布有效非零末速轨迹。 3. 使用 `DistanceHorizonMeters=5m`、`TimeHorizonSeconds=2s` 和 `MaximumSpeed=0.2m/s`,确认 ST 不被强制在两秒内走完五米。 4. 多周期接近 Goal,模式按 `RollingContinuation -> ApproachStopBoundary -> ExactStopAtBoundary` 演进,并最终以 `U=0,A=0` 停车。