371 lines
18 KiB
Markdown
371 lines
18 KiB
Markdown
# EM 纵向滚动轨迹规划修复设计
|
||
|
||
日期：2026-08-05
|
||
|
||
## 目标
|
||
|
||
修复当前 EM 纵向 ST 规划把每个局部路径窗口末端都当作零速停车点的问题，使规划器能够持续输出滚动轨迹，并且只在当前方向段的真实 `Goal` 或 `GearSwitchApproach` 边界前完成 jerk 受限停车。
|
||
|
||
本设计不通过放宽 jerk、加速度或求解器收敛阈值规避问题。发布轨迹仍须通过完整的独立运动学、碰撞和边界验证。
|
||
|
||
## 当前问题
|
||
|
||
当前实现把三个不同概念合并为同一个“终端”：
|
||
|
||
1. LS 使用的参考路径空间窗口末端；
|
||
2. ST 在固定时间范围内实际能够到达的位置；
|
||
3. 车辆必须停车的真实目标或换挡边界。
|
||
|
||
`PlanningHorizonSelector` 使用剩余段长度、`DistanceHorizonMeters` 和时间可达距离的最小值生成局部末端，并在没有到达真实方向段边界时将其标为 `RollingSafetyStop`。后续纵向链路仍对该局部末端统一施加：
|
||
|
||
```text
|
||
S[N] = TerminalPathS
|
||
U[N] = 0
|
||
```
|
||
|
||
`TrajectorySampleSchedule` 随后追加 `U=0, A=0, J=0` 的保持点，但 QP 没有硬约束 `A[N]=0`。因此当前报告中的典型序列为：
|
||
|
||
```text
|
||
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` 仍不能直接保证最后一个运动区间的速度有限差分加速度为零；准确停车还必须在 ST 时间范围末尾保留一个完整零速稳定区间。即使补上该稳定区间，若仍把每个局部窗口当作停车点，也不能满足连续轨迹规划要求。
|
||
|
||
## 方案选择
|
||
|
||
### 方案一：只增加终点加速度硬约束
|
||
|
||
为现有停车终端增加 `A[N]=0`。改动最少，但局部窗口仍是停车点，不满足目标。
|
||
|
||
### 方案二：只取消滚动窗口的零速约束
|
||
|
||
仅在 `LongitudinalConstraintBuilder` 中取消 `U[N]=0`。这会与当前停车速度包络、停车初始解、末点规范化、零速保持采样和发布校验冲突，不能形成一致的数据链路。
|
||
|
||
### 方案三：拆分空间窗口、纵向模式和真实停车边界
|
||
|
||
这是采用的方案。空间窗口只定义 LS 能查看的路径范围；ST 时间范围只定义一次输出覆盖的未来时间；真实 `Goal/GearSwitchApproach` 才定义停车位置。纵向链路根据明确模式选择约束、初始解、采样和校验规则。
|
||
|
||
## 核心语义
|
||
|
||
### 三个独立范围
|
||
|
||
空间窗口：
|
||
|
||
```text
|
||
windowEndReferenceS = min(currentReferenceS + DistanceHorizonMeters,
|
||
currentSegmentEndS)
|
||
```
|
||
|
||
时间范围：
|
||
|
||
```text
|
||
0 <= t <= TimeHorizonSeconds
|
||
```
|
||
|
||
真实停车边界：
|
||
|
||
```text
|
||
current direction segment end == Goal or GearSwitchApproach
|
||
```
|
||
|
||
例如，`DistanceHorizonMeters=5m`、`TimeHorizonSeconds=2s`、最大速度 `0.2m/s` 时，LS 可以查看前方 5m，但 ST 两秒内通常只前进约 0.4m。ST 不得被强制在两秒内走完 5m，也不得在 5m 窗口截断位置停车。
|
||
|
||
### 纵向模式
|
||
|
||
新增内部纵向行为枚举：
|
||
|
||
```csharp
|
||
public enum EmLongitudinalMode
|
||
{
|
||
RollingContinuation,
|
||
ApproachStopBoundary,
|
||
ExactStopAtBoundary,
|
||
}
|
||
```
|
||
|
||
- `RollingContinuation`：真实方向段末端不在空间窗口内；本次轨迹连续行驶，末点允许非零速度和加速度。
|
||
- `ApproachStopBoundary`：真实边界已进入空间窗口，但无法在当前时间范围内完成准确停车；本次轨迹继续前进，不追加保持点。
|
||
- `ExactStopAtBoundary`：在当前时间范围及运动学限制内能够到达真实边界；本次轨迹必须在时间范围结束前于该边界满足 `S=S_end, U=0, A=0`，并在剩余时间内保持至少一个完整输出时间步的静止状态。
|
||
|
||
边界身份继续由 `EmBoundaryType` 表达。纵向行为和边界身份是两个维度，不能再用一个 `EmTerminalType` 同时表达。
|
||
|
||
## 规划窗口选择
|
||
|
||
`PlanningHorizonSelection` 改为显式保存：
|
||
|
||
```csharp
|
||
public double WindowEndReferenceS { get; }
|
||
public EmBoundaryType WindowEndBoundaryType { get; }
|
||
public EmLongitudinalMode LongitudinalMode { get; }
|
||
public double StopBoundaryReferenceS { get; }
|
||
public bool HasStopBoundary { get; }
|
||
```
|
||
|
||
`PlanningHorizonSelector` 执行以下顺序：
|
||
|
||
1. 使用当前方向段的真实末端计算 `remainingToBoundary`。
|
||
2. 使用完整 jerk 受限停车模型计算当前状态的最小停车距离和时间。
|
||
3. 如果最小停车距离超过真实剩余距离，返回 `StoppingDistanceInsufficient`，并报告所需距离、实际距离及差值。
|
||
4. 仅使用 `DistanceHorizonMeters` 截取 LS 空间窗口，不再使用“必须在当前时间范围内停车”的时间可达距离缩短空间窗口。
|
||
5. 若真实边界不在空间窗口内，选择 `RollingContinuation`。
|
||
6. 若真实边界在空间窗口内，但当前时间范围内无法到达并以 `U=0,A=0` 停止，选择 `ApproachStopBoundary`。
|
||
7. 若真实边界在当前时间范围内可准确停车，选择 `ExactStopAtBoundary`。
|
||
|
||
时间可停车判定使用与 ST 相同的速度、加速度、jerk 和恒 jerk 离散积分模型，避免选择器判断“可停”而 QP 实际不可行。准确停车可使用的运动时间为 `TimeHorizonSeconds - TerminalStabilizationDuration`，其中稳定时间至少为一个完整 `OutputTimeStepSeconds`。`ZeroSpeedHoldSeconds` 是优化时间范围之后额外追加的发布保持段，不从 `TimeHorizonSeconds` 中扣除。
|
||
|
||
## jerk 受限停车模型
|
||
|
||
现有 `LongitudinalStoppingMath` 被替换为完整 S 曲线停车计算。停车过程包括：
|
||
|
||
1. 通过负 jerk 将加速度向允许的负减速度移动；
|
||
2. 必要时保持负减速度；
|
||
3. 通过正 jerk 将加速度恢复到零；
|
||
4. 最终同时满足 `U=0` 和 `A=0`。
|
||
|
||
每一段使用与 QP 一致的恒 jerk 积分：
|
||
|
||
```text
|
||
A1 = A0 + J * dt
|
||
U1 = U0 + A0 * dt + 0.5 * J * dt^2
|
||
S1 = S0 + U0 * dt + 0.5 * A0 * dt^2 + J * dt^3 / 6
|
||
```
|
||
|
||
停车结果至少包含：
|
||
|
||
```csharp
|
||
public double DistanceMeters { get; }
|
||
public double DurationSeconds { get; }
|
||
public double FinalSpeedMetersPerSecond { get; }
|
||
public double FinalAccelerationMetersPerSecondSquared { get; }
|
||
```
|
||
|
||
构造结果必须在运动学容差内满足最终速度和加速度为零。最大速度、最大允许正加速度下的停车距离与时间用于验证空间前视和时间范围是否足够；当前实车状态下的停车结果用于每周期模式选择和失败诊断。
|
||
|
||
## 纵向输入模型
|
||
|
||
`LongitudinalPlanningInput` 中含义模糊的 `TerminalPathS` 拆分为：
|
||
|
||
```csharp
|
||
public double PathUpperBoundS { get; }
|
||
public bool HasStopBoundary { get; }
|
||
public double StopBoundaryPathS { get; }
|
||
public EmBoundaryType StopBoundaryType { get; }
|
||
public EmLongitudinalMode Mode { get; }
|
||
```
|
||
|
||
`PathUpperBoundS` 是当前 LS 结果允许 ST 使用的最大 PathS，不代表必须到达。`StopBoundaryPathS` 只在真实停车边界包含于当前 LS 路径时有效。
|
||
|
||
## 速度包络
|
||
|
||
`PathSpeedLimitBuilder` 不再无条件在 `PathUpperBoundS` 设置零速：
|
||
|
||
- `RollingContinuation`：速度上限只由方向最大速度、横向加速度和曲率变化率决定。
|
||
- `ApproachStopBoundary`：真实停车边界可见，速度参考按照真实剩余停车距离降低，但当前时间末点不强制到达边界。参考轨迹由“巡航 + 完整 jerk 受限停车”曲线生成，并截取当前时间范围内的前缀，因此跨多个重规划周期时能够连续进入制动过程。
|
||
- `ExactStopAtBoundary`：真实边界处速度上限为零，并与 ST 终端硬约束共同保证停车。
|
||
|
||
现有 `sqrt(2*a*d)` 只能表达恒减速度停车，不能代表 jerk 受限可停车性。对 `ApproachStopBoundary` 和 `ExactStopAtBoundary`，每个路径采样位置的停止速度上限由完整停车模型通过二分反求：在 `[0, directionMaximumSpeed]` 内寻找能够在剩余距离中以 `A=0` 停止的最大速度。反求时使用允许的最大正加速度作为保守初始加速度。最终准确停车再由 ST 的动力学、jerk 约束和 `S/U/A` 终端硬约束保证。
|
||
|
||
## ST QP 约束
|
||
|
||
所有模式都保留：
|
||
|
||
```text
|
||
S[0] = 0
|
||
U[0] = measured progress speed
|
||
A[0] = measured/estimated longitudinal acceleration
|
||
0 <= S[k] <= PathUpperBoundS
|
||
0 <= U[k] <= path speed limit
|
||
-maximumDeceleration <= A[k] <= maximumAcceleration
|
||
-maximumJerk <= J[k] <= maximumJerk
|
||
S[k+1] >= S[k]
|
||
exact constant-jerk dynamics
|
||
```
|
||
|
||
`RollingContinuation` 和 `ApproachStopBoundary` 不增加末点等式。现有参考速度、加速度、jerk 和上一轨迹权重继续推动平顺前进；不增加强制 `S[N]=PathUpperBoundS` 的进度条件。
|
||
|
||
`ExactStopAtBoundary` 先确定稳定尾的第一个 knot `H`。`H` 到最后一个 knot `N` 的累计时间不得小于一个完整 `OutputTimeStepSeconds`；若最后一个 knot interval 因无法整除时间范围而缩短，则 `H` 相应前移。对每个 `k=H..N` 增加三个硬约束：
|
||
|
||
```text
|
||
S[k] = StopBoundaryPathS
|
||
U[k] = 0
|
||
A[k] = 0
|
||
```
|
||
|
||
因此真实边界锚点位于 `H`，`H+1..N` 是 QP 内部稳定尾。设 knot 总数为 `K`、稳定尾 knot 数为 `M=K-H`，约束行数为：滚动/接近模式 `8*K-2`；准确停车模式 `8*K-2+3*M`。常规等间隔情况下 `M=2`，约束行数为 `8*K+4`。
|
||
|
||
## 初始迭代与顺序凸化
|
||
|
||
`SequentialLongitudinalOptimizer.CreateInitialIterate` 按模式构造：
|
||
|
||
- `RollingContinuation`：从实车 `U0,A0` 出发，按路径速度参考和 jerk/加速度限制向前积分；不强制末点到达路径上界或零速。
|
||
- `ApproachStopBoundary`：使用真实边界相关的减速速度参考构造连续前进解；不强制当前周期停车。
|
||
- `ExactStopAtBoundary`：构造巡航加完整 S 曲线停车种子，在 `H` 严格到达 `S_end,U=0,A=0`，并将 `H+1..N` 填充为同一位置的零速稳定尾。
|
||
|
||
`TryCreateEnvelopeIterate`、候选裁剪和末点规范化中所有无条件设置最后一个 `S` 为路径上界的代码，都改为只在 `ExactStopAtBoundary` 执行。滚动模式的末点由动力学和优化结果决定。
|
||
|
||
## 上一轨迹纵向连续性
|
||
|
||
`EmPlanningService` 当前接收 `PreviousTrajectory`，但创建 `LongitudinalPlanningInput` 时传入两个空数组，导致现有 `PreviousTrajectory` 权重未用于纵向规划。
|
||
|
||
修复后，同方向、同方向段且时间有效的上一轨迹按新 ST knot 时间重采样，生成：
|
||
|
||
```text
|
||
PreviousPathS[k]
|
||
PreviousProgressSpeedMetersPerSecond[k]
|
||
```
|
||
|
||
它们作为软参考传给纵向目标函数。新规划第一个点仍以最新实车状态为硬约束，上一轨迹不得覆盖实车测量状态。上一轨迹无效、过期、方向不符或方向段不符时，安全地退化为无纵向热启动。
|
||
|
||
## 纵向解验证
|
||
|
||
`LongitudinalSolutionValidator` 对所有模式检查时间 knot、变量数量、有限值、PathS 单调性、速度/加速度/jerk 限制和恒 jerk 动力学。
|
||
|
||
在 `ApproachStopBoundary` 和 `ExactStopAtBoundary` 中，验证器还使用候选点自身的 `S/U/A` 重新计算完整停车距离，并逐点检查：
|
||
|
||
```text
|
||
candidatePathS[k] + stoppingDistance(U[k], A[k])
|
||
<= StopBoundaryPathS + spatialTolerance
|
||
```
|
||
|
||
该检查不信任构建 QP 时使用的顺序凸化采样点。违反可停车域的候选不能发布，并作为下一次外层迭代更新速度包络的依据。
|
||
|
||
仅 `ExactStopAtBoundary` 检查并规范化 `H..N`：
|
||
|
||
```text
|
||
S[k] == StopBoundaryPathS
|
||
U[k] == 0
|
||
A[k] == 0
|
||
```
|
||
|
||
滚动和接近模式只要求末点位于 `[0, PathUpperBoundS]` 且满足普通运动学限制，允许非零末点速度、加速度和由曲率产生的 yaw rate。
|
||
|
||
## 轨迹采样和零速保持
|
||
|
||
`TrajectorySampleSchedule` 接收 `EmLongitudinalMode`：
|
||
|
||
- 滚动和接近模式只输出 `0..TimeHorizonSeconds` 的 ST 样本，不要求零速，不追加保持点。
|
||
- 准确停车模式在追加保持点前检查候选包含至少一个完整输出时间步的 `S=S_end,U=0,A=0` 内部稳定尾，然后按 `ZeroSpeedHoldSeconds` 继续追加同一 PathS 上 `U=0,A=0,J=0` 的外部保持样本。
|
||
|
||
当 `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` 将准确停车稳定尾的第一个 knot `H` 标记为 `Goal` 或 `GearSwitchApproach` 锚点；后续内部稳定点和外部保持点不重复创建锚点。滚动时间末点不伪装为路径边界。
|
||
|
||
`EmTrajectoryValidator` 始终检查有限值、时间递增、速度、有限差分加速度、有限差分 jerk、曲率、曲率变化率、yaw rate、地图边界和车辆轮廓碰撞。
|
||
|
||
仅 `ExactStopAtBoundary` 额外检查：
|
||
|
||
- 存在准确的真实边界锚点；
|
||
- 终点速度为零；
|
||
- 终点加速度为零；
|
||
- 终点 yaw rate 为零；
|
||
- 零速保持段有效。
|
||
|
||
滚动和接近模式不要求边界锚点，也不要求末点速度、加速度或 yaw rate 为零。
|
||
|
||
## Goal 与 GearSwitch
|
||
|
||
`Goal` 是当前参考路径最终方向段的真实停止边界。进入 `ExactStopAtBoundary` 后，轨迹在 Goal 满足 `S/U/A` 准确终端条件。
|
||
|
||
`GearSwitchApproach` 是当前方向段的真实停止边界。当前轨迹必须先在换挡点满足 `U=0,A=0`；外部调度器确认车辆停止后，下一次请求才切换到下一方向段。下一方向段从 `GearSwitchDeparture` 以 `U=0,A=0` 开始。同一条发布轨迹不跨越前进/倒车切换。
|
||
|
||
## 配置语义
|
||
|
||
以下现有配置继续生效：
|
||
|
||
- `DistanceHorizonMeters`：LS 前方空间查看范围，不是一次 ST 必须行驶的距离。
|
||
- `TimeHorizonSeconds`：一次 ST 优化输出覆盖的未来时间，不包含额外零速保持时间。
|
||
- `OutputTimeStepSeconds`：相邻轨迹时间戳间隔，也是离散动力学的 knot 间隔。
|
||
- `ReplanPeriodSeconds`：滚动重规划周期，不等于输出时间戳间隔。
|
||
- 最大正向/反向速度、加速度、减速度和 jerk：硬运动学限制。
|
||
- `ZeroSpeedHoldSeconds`：仅准确停车轨迹使用。
|
||
|
||
配置验证额外确认：
|
||
|
||
1. 空间窗口不小于配置最大速度、最大正加速度下的完整 jerk 受限停车距离，再加一个 `MaximumSpeed * ReplanPeriodSeconds` 的重规划行驶余量；
|
||
2. 时间范围在预留一个完整输出时间步的内部稳定尾后仍至少包含一个正运动时间间隔；若当前完整停车持续时间大于剩余运动时间，则选择 `ApproachStopBoundary` 并发布制动轨迹前缀，而不是拒绝配置；
|
||
3. knot 生成器始终准确包含 `t=0` 和 `t=TimeHorizonSeconds`，无法整除时最后一个时间间隔缩短但保持为正；保持段采用相同规则准确结束于 `ZeroSpeedHoldSeconds`；
|
||
4. 诊断信息打印计算得到的最大停车距离、停车时间和所需空间余量，便于解释配置不足。
|
||
|
||
本设计不新增求解容差、最大迭代次数或 solver timeout 的特殊放宽。
|
||
|
||
## 诊断
|
||
|
||
每个规划周期的成功或失败诊断增加：
|
||
|
||
```text
|
||
longitudinalMode
|
||
currentReferenceS
|
||
windowEndReferenceS
|
||
actualBoundaryReferenceS
|
||
remainingToBoundary
|
||
minimumStoppingDistance
|
||
minimumStoppingDuration
|
||
maximumStoppedReachableDistance
|
||
terminal S/U/A when applicable
|
||
```
|
||
|
||
停车距离不足时返回：
|
||
|
||
```text
|
||
StoppingDistanceInsufficient: remaining=0.080, required=0.127, shortage=0.047,
|
||
speed=0.200, acceleration=0.000, decelerationLimit=0.300, jerkLimit=0.500
|
||
```
|
||
|
||
发布成功摘要继续报告点数、持续时间、实际 PathS 长度和最大速度/加速度/jerk。滚动轨迹的实际 PathS 长度不得被错误报告为 `DistanceHorizonMeters`。
|
||
|
||
## 测试策略
|
||
|
||
所有行为修改先写失败测试，再修改生产代码。
|
||
|
||
### 单元测试
|
||
|
||
1. 空间窗口按 `DistanceHorizonMeters` 截取，不再被当前时间可停车距离替代。
|
||
2. 真实边界不在窗口时选择 `RollingContinuation`。
|
||
3. 边界在窗口但本周期不可准确停车时选择 `ApproachStopBoundary`。
|
||
4. Goal 或 GearSwitch 在本周期可准确停车时选择 `ExactStopAtBoundary`。
|
||
5. jerk 受限停车计算最终同时满足 `U=0,A=0`。
|
||
6. 滚动 QP 不包含末点 `S/U/A` 等式。
|
||
7. 准确停车 QP 对真实边界锚点及其后的内部稳定尾都包含 `S/U/A` 三个等式。
|
||
8. 滚动初始迭代不强制末点到达路径上界或零速。
|
||
9. `TryCreateEnvelopeIterate` 只在准确停车模式固定末点。
|
||
10. 滚动采样不追加保持点并允许非零末点速度。
|
||
11. 准确停车采样要求至少一个完整输出时间步的内部零速稳定尾，然后追加外部保持点。
|
||
12. 发布验证器对滚动轨迹不要求零速边界锚点。
|
||
13. 发布验证器对准确停车轨迹要求真实边界和 `U/A/yawRate=0`。
|
||
|
||
### 回归与集成测试
|
||
|
||
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` 停车。
|
||
5. 多周期接近 GearSwitch，当前方向段停车后才允许下一方向段从零速出发。
|
||
6. 上一条有效轨迹参与下一周期纵向软参考，首点仍与最新实车状态一致。
|
||
7. 停车距离不足、时间范围不足和空间前视不足分别返回可区分的失败诊断。
|
||
8. 现有横向、碰撞、求解器和 observation-only 验证保持通过。
|
||
|
||
## 实施边界
|
||
|
||
本次包含：
|
||
|
||
- 纵向滚动/接近/准确停车模式；
|
||
- 空间窗口和时间范围解耦；
|
||
- 完整 jerk 受限停车模型；
|
||
- ST 约束、初始解、速度包络、验证和采样的条件化；
|
||
- 上一轨迹纵向软参考；
|
||
- 对应诊断和测试。
|
||
|
||
本次不包含：
|
||
|
||
- 底盘命令下发；
|
||
- 轨迹执行器超时看门狗；
|
||
- 控制器跟踪算法；
|
||
- 障碍物动态预测；
|
||
- 修改横向 LS 的基本优化模型；
|
||
- 放宽运动学或求解器验收阈值。
|
||
|
||
在没有独立下层安全停车机制的情况下，带非零末速的滚动轨迹仅用于当前 `OBSERVE_ONLY` 测试，不在本次工作中启用实车底盘下发。
|