1059 lines
31 KiB
Markdown
1059 lines
31 KiB
Markdown
# ParkrobTrajplanner EM Planner（LS/ST）设计
|
||
|
||
## 1. 设计目标
|
||
|
||
在现有 `Map → Hybrid A* CoarsePath → Local G2 PathSmoothing` 链路之后新增 EM Planner，将已经通过完整车体碰撞和曲率复核的平滑空间路径转换为可供轨迹执行器消费的带时间轨迹。
|
||
|
||
第一版完成以下能力：
|
||
|
||
- 按前进/倒车方向段进行独立规划；
|
||
- 在 Frenet `l-s` 坐标中构建局部静态安全走廊并优化横向路径；
|
||
- 在最终路径 `s-t` 坐标中规划速度、加速度、jerk 和停车过程；
|
||
- 在换向点和任务终点精确停车；
|
||
- 支持固定周期滚动重规划、上一轨迹拼接和安全停车尾段；
|
||
- 通过可配置参数、可关闭调试旁路、明确状态和独立复核保证模块可标定、可诊断；
|
||
- 输出包含 `x、y、yaw、signedLongitudinalVelocity、speed、vx、vy、yawRate、t、k` 的不可变轨迹。
|
||
|
||
第一版车辆模型仅支持普通非完整运动的前进和倒车。蟹行、横移和原地旋转不进入该模型。
|
||
|
||
## 2. 范围边界
|
||
|
||
### 2.1 模块职责
|
||
|
||
```text
|
||
Map
|
||
→ Hybrid A* CoarsePath
|
||
决定静态绕障拓扑、前进/倒车结构和换向位置
|
||
→ Local G2 PathSmoothing
|
||
保持拓扑，输出经过复核的连续空间参考路径
|
||
→ EM Planner
|
||
在既有通道内优化横向形状并完成时间参数化
|
||
→ TrajectoryExecution
|
||
轨迹跟踪、停车确认、换向状态机和底盘适配
|
||
```
|
||
|
||
EM Planner 不得：
|
||
|
||
- 改变 Hybrid A* 已确定的全局绕障侧或拓扑；
|
||
- 新增、删除或移动换向事件；
|
||
- 直接读取传感器、定位、轮速或底盘对象；
|
||
- 直接下发舵角、轮速或现有 `MultiVehicleScriptVx/Vy/Vth` 字段；
|
||
- 把横向偏移 `l` 解释为 AMR 的横移自由度；
|
||
- 发布未经过独立世界坐标复核的求解器结果。
|
||
|
||
### 2.2 明确延期的能力
|
||
|
||
以下能力作为后续独立设计范围，不在第一版实现中：
|
||
|
||
- 动态障碍物轨迹预测；
|
||
- 动态障碍物的时空占用边界；
|
||
- 跟随、让行、超越、动态停车和动态绕行决策；
|
||
- 由 SL lattice DP 重新选择静态障碍物绕障侧；
|
||
- 蟹行、横移、原地旋转和横向车体速度；
|
||
- 同一优化问题跨越前进/倒车换向点；
|
||
- 直接输出四轮舵角或四轮轮速。
|
||
|
||
## 3. 上游输入资格
|
||
|
||
EM Planner 只消费已经发布路径的 `PathSmoothingResult`。以下状态可作为输入：
|
||
|
||
```text
|
||
Complete
|
||
PartialImprovement
|
||
NotNeeded
|
||
Unchanged
|
||
```
|
||
|
||
以下状态不得进入 EM Planner：
|
||
|
||
```text
|
||
InvalidInput
|
||
Infeasible
|
||
Failed
|
||
Cancelled
|
||
```
|
||
|
||
每次请求还必须携带：
|
||
|
||
- 与平滑路径一致的 `PlanningGridMap`；
|
||
- 与上游规划一致的 `VehicleParameters`；
|
||
- 当前车辆状态快照；
|
||
- 本次配置的不可变快照；
|
||
- 可选的上一条已发布轨迹；
|
||
- 地图、参考路径、车辆状态和上一轨迹的版本信息。
|
||
|
||
## 4. 核心数据契约
|
||
|
||
### 4.1 车辆状态快照
|
||
|
||
规划核心不主动读取硬件。调用方通过不可变快照传入状态：
|
||
|
||
```csharp
|
||
public sealed class VehicleMotionState
|
||
{
|
||
public Pose2D Pose { get; }
|
||
public double SignedLongitudinalSpeedMetersPerSecond { get; }
|
||
public double? LongitudinalAccelerationMetersPerSecondSquared { get; }
|
||
public DateTimeOffset CapturedAtUtc { get; }
|
||
public long SequenceId { get; }
|
||
}
|
||
```
|
||
|
||
速度符号固定为：
|
||
|
||
```text
|
||
前进：SignedLongitudinalSpeed > 0
|
||
倒车：SignedLongitudinalSpeed < 0
|
||
停车：SignedLongitudinalSpeed = 0
|
||
```
|
||
|
||
若绝对速度小于 `StopSpeedToleranceMetersPerSecond`，统一按零处理。车辆状态方向与当前方向段明显矛盾时返回状态不一致，不启动正常优化。
|
||
|
||
运行时可以通过 `IVehicleStateProvider.Capture()` 创建快照，但该接口属于执行适配层，不属于 EM Planner 核心。
|
||
|
||
### 4.2 规划请求
|
||
|
||
```csharp
|
||
public sealed class EmPlanningRequest
|
||
{
|
||
public PathSmoothingResult ReferencePath { get; set; }
|
||
public PlanningGridMap Map { get; set; }
|
||
public VehicleParameters Vehicle { get; set; }
|
||
public VehicleMotionState CurrentState { get; set; }
|
||
public EmPlannerConfiguration Configuration { get; set; }
|
||
public EmPlanningResult PreviousTrajectory { get; set; }
|
||
public EmPlannerDebugOptions DebugOptions { get; set; }
|
||
public long MapSnapshotId { get; set; }
|
||
public string ReferencePathId { get; set; }
|
||
}
|
||
```
|
||
|
||
公开门面保持单次、同步、可取消：
|
||
|
||
```csharp
|
||
EmPlanningResult EmPlanningService.Plan(
|
||
EmPlanningRequest request,
|
||
CancellationToken cancellationToken);
|
||
```
|
||
|
||
周期调度、并发淘汰和轨迹执行不进入该方法。
|
||
|
||
### 4.3 轨迹点
|
||
|
||
公开轨迹点包含：
|
||
|
||
```text
|
||
X m，世界坐标
|
||
Y m，世界坐标
|
||
Yaw rad，车辆车头世界航向
|
||
SignedLongitudinalVelocity m/s，车体纵向带符号速度，前进为正、倒车为负
|
||
Speed m/s，无符号合成速率，始终非负
|
||
VelocityX m/s，世界坐标速度分量
|
||
VelocityY m/s，世界坐标速度分量
|
||
YawRate rad/s，车辆航向角速度，逆时针为正
|
||
TimeFromStart s，从本条轨迹生效时刻起
|
||
VehicleCurvature 1/m，最终公开字段 k
|
||
```
|
||
|
||
为保证诊断和后续扩展，内部契约另外保留：
|
||
|
||
```text
|
||
SegmentIndex
|
||
SegmentLocalS
|
||
PathS
|
||
Direction
|
||
LongitudinalAcceleration
|
||
LongitudinalJerk
|
||
GeometricCurvature
|
||
VehicleCurvatureDerivative
|
||
BoundaryType
|
||
```
|
||
|
||
上述公开速度字段允许冗余，但只有一套权威关系：
|
||
|
||
```text
|
||
signedV = SignedLongitudinalVelocity
|
||
speed = abs(signedV)
|
||
vx = signedV*cos(Yaw)
|
||
vy = signedV*sin(Yaw)
|
||
yawRate = signedV*VehicleCurvature
|
||
```
|
||
|
||
`SignedLongitudinalVelocity` 是 ST 结果结合方向段得到的权威速度标量。`Speed`、`VelocityX`、`VelocityY` 和 `YawRate` 均由它生成，不允许调用者分别赋值。独立验证器必须复核这些等式；任一字段超过数值容差即返回 `ValidationFailed`，不得发布自相矛盾的轨迹。
|
||
|
||
每次 EM 结果只覆盖当前一个方向段，因此其中的 `SegmentLocalS` 与 `PathS` 都从本次结果起点开始严格递增；全局任务进度必须与 `SegmentIndex` 组合使用，不能仅靠单个 S 值定位换向前后的状态。
|
||
|
||
`TimeFromStart` 从零开始且严格递增。`EmPlanningResult` 另行保存 `GeneratedAtUtc`、`EffectiveAtUtc` 和输入状态时间戳。
|
||
|
||
### 4.4 终端事件
|
||
|
||
轨迹终端类型固定为：
|
||
|
||
```text
|
||
RollingSafetyStop
|
||
GearSwitch
|
||
Goal
|
||
```
|
||
|
||
终端元数据包含：
|
||
|
||
```text
|
||
SegmentIndex
|
||
SegmentLocalS
|
||
RequiresStop
|
||
BoundaryEventId
|
||
```
|
||
|
||
所有第一版已发布轨迹都以零速度终止。普通滚动规划的 `RollingSafetyStop` 是失去更新时的兜底停车尾段；换向和目标终端则是必须在指定空间位置满足的精确停车事件。
|
||
|
||
## 5. 坐标系与符号契约
|
||
|
||
### 5.1 世界系和车体系
|
||
|
||
沿用现有二维右手约定：
|
||
|
||
- 世界和核心规划长度统一为 m；
|
||
- 航向统一为 rad；
|
||
- 曲率统一为 1/m；
|
||
- 车辆坐标 `+x` 指向车头，`+y` 指向车体左侧；
|
||
- `yaw=0` 时车头指向世界 `+X`；
|
||
- yaw 逆时针为正；
|
||
- 对外 yaw 归一化到 `[-π, π)`；
|
||
- 数值微分使用连续展开角，不对归一化角直接差分。
|
||
|
||
### 5.2 VehicleYaw 与 TravelYaw
|
||
|
||
必须区分车辆车头角和实际运动切向角：
|
||
|
||
```text
|
||
前进：TravelYaw = VehicleYaw
|
||
倒车：TravelYaw = Normalize(VehicleYaw + π)
|
||
```
|
||
|
||
Frenet 基底只使用 `TravelYaw`：
|
||
|
||
```text
|
||
t = ( cos(TravelYaw), sin(TravelYaw))
|
||
n = (-sin(TravelYaw), cos(TravelYaw))
|
||
```
|
||
|
||
`s` 始终沿实际运动方向递增；`l>0` 始终表示运动方向左侧。倒车时，运动方向左侧对应车体右侧，这是预期符号，不得在倒车分支额外翻转 `l`。
|
||
|
||
### 5.3 世界坐标投影到 Frenet
|
||
|
||
对当前方向段参考点 `r=(xr,yr)` 和世界点 `p=(x,y)`：
|
||
|
||
```text
|
||
dx = x - xr
|
||
dy = y - yr
|
||
|
||
deltaS = dx*cos(TravelYaw) + dy*sin(TravelYaw)
|
||
l = -dx*sin(TravelYaw) + dy*cos(TravelYaw)
|
||
ReferenceS = referencePointS + deltaS
|
||
```
|
||
|
||
投影只能在指定 `SegmentIndex` 内进行，并携带前一投影索引或 `ReferenceS` 提示，在有限索引窗口搜索。禁止跨完整路径寻找全局最近点，因为回环和换向重复位姿会造成分支歧义。
|
||
|
||
车辆航向误差使用运动方向角：
|
||
|
||
```text
|
||
egoTravelYaw = direction == Forward
|
||
? egoVehicleYaw
|
||
: Normalize(egoVehicleYaw + π)
|
||
|
||
headingError = Normalize(egoTravelYaw - referenceTravelYaw)
|
||
lPrime = (1 - referenceGeometricCurvature*l) * tan(headingError)
|
||
```
|
||
|
||
### 5.4 Frenet 转回世界坐标
|
||
|
||
```text
|
||
x = xr - l*sin(referenceTravelYaw)
|
||
y = yr + l*cos(referenceTravelYaw)
|
||
|
||
A = 1 - referenceGeometricCurvature*l
|
||
optimizedTravelYaw = referenceTravelYaw + atan2(lPrime, A)
|
||
|
||
VehicleYaw = direction == Forward
|
||
? Normalize(optimizedTravelYaw)
|
||
: Normalize(optimizedTravelYaw + π)
|
||
```
|
||
|
||
必须满足 Frenet 非奇异条件：
|
||
|
||
```text
|
||
1 - referenceGeometricCurvature*l >= MinimumFrenetDenominator
|
||
```
|
||
|
||
### 5.5 曲率和速度符号
|
||
|
||
沿运动方向定义几何曲率：
|
||
|
||
```text
|
||
GeometricCurvature = d(TravelYaw) / d(PathS)
|
||
directionSign = Forward ? +1 : -1
|
||
VehicleCurvature = directionSign * GeometricCurvature
|
||
```
|
||
|
||
ST 内部使用非负进度速度 `u=d(PathS)/dt`。发布时：
|
||
|
||
```text
|
||
signedV = directionSign * u
|
||
vx = signedV*cos(VehicleYaw)
|
||
vy = signedV*sin(VehicleYaw)
|
||
yawRate = signedV*VehicleCurvature
|
||
```
|
||
|
||
公开 `k` 必须是 `VehicleCurvature`，从而前进和倒车均满足同一 `yawRate=v*k` 关系。
|
||
|
||
### 5.6 ReferenceS 与 PathS
|
||
|
||
LS 的自变量是平滑参考线弧长 `ReferenceS`。横向偏移后真实路径弧长不再等于参考线弧长：
|
||
|
||
```text
|
||
dPathS = sqrt(
|
||
(1-referenceGeometricCurvature*l)^2 + lPrime^2
|
||
) * dReferenceS
|
||
```
|
||
|
||
LS 完成后必须按以下顺序处理：
|
||
|
||
```text
|
||
Frenet 优化结果
|
||
→ 世界 x/y/yaw
|
||
→ 按实际几何重新累计弧长
|
||
→ 生成严格递增 PathS
|
||
→ 交给 ST
|
||
```
|
||
|
||
ST 禁止使用 `ReferenceS` 作为实际运动距离。
|
||
|
||
## 6. 分段、换向和边界锚点
|
||
|
||
### 6.1 分段单位
|
||
|
||
现有 `SmoothedPathSegment` 是 EM Planner 的唯一方向段来源。每个方向段建立独立局部坐标：
|
||
|
||
```text
|
||
SegmentLocalS = 0 ... SegmentLength
|
||
```
|
||
|
||
一次 LS 或 ST 问题不得跨方向段。
|
||
|
||
### 6.2 换向重复点
|
||
|
||
换向前后存在相同 `x、y、yaw` 和全局弧长的两个点，但方向和边界语义不同。不得仅以世界位姿或全局 S 去重。边界唯一键为：
|
||
|
||
```text
|
||
SegmentIndex + SegmentLocalS + BoundaryType
|
||
```
|
||
|
||
前段终点表示 `GearSwitchApproach`，后段起点表示 `GearSwitchDeparture`。
|
||
|
||
### 6.3 Horizon 截取
|
||
|
||
普通过滤表达式不得用于截取路径。`SegmentHorizonSlicer` 必须显式插入起点、请求终点和强制事件：
|
||
|
||
```text
|
||
distanceLimitedEnd = currentSegmentS + PlanningDistanceHorizon
|
||
timeReachableEnd = 当前状态在 PlanningTimeHorizon 内、满足速度/加减速度/jerk
|
||
且能以零速终止时可到达的最远位置
|
||
requestedEnd = min(distanceLimitedEnd, timeReachableEnd)
|
||
|
||
if requestedEnd < segmentEnd - BoundaryEpsilon:
|
||
planningEnd = requestedEnd
|
||
terminal = RollingSafetyStop
|
||
else:
|
||
planningEnd = segmentEnd
|
||
terminal = GearSwitch 或 Goal
|
||
```
|
||
|
||
`timeReachableEnd` 由独立的一维保守可达性计算得到。不能简单使用 `MaximumSpeed*PlanningTimeHorizon`，因为规划末端还必须预留满足减速度和 jerk 限制的停车尾段。若当前速度在配置的时间窗口内本身无法安全降到零，则返回 `StoppingDistanceInsufficient`，而不是构造一个必然无解的 ST 问题。
|
||
|
||
切片结果按以下集合生成：
|
||
|
||
```text
|
||
精确插值起点
|
||
+ 区间内普通参考点
|
||
+ 精确插值 planningEnd
|
||
+ 必须保留的边界事件
|
||
```
|
||
|
||
强制事件点不得被等距重采样或数值去重删除。
|
||
|
||
### 6.4 时间采样事件
|
||
|
||
若固定时间采样未命中换向或目标时刻，必须插入精确事件点。例如停车发生在 `2.137 s`，轨迹可包含 `2.10 s` 和 `2.137 s`，但不得直接越到 `2.15 s` 的下一方向运动点。
|
||
|
||
### 6.5 换向执行所有权
|
||
|
||
EM Planner 输出零速终端和 `GearSwitch` 事件。独立 `GearSwitchStateMachine` 负责确认：
|
||
|
||
- 车辆进入位置容差；
|
||
- 实际绝对速度不大于停车阈值；
|
||
- 停车状态连续保持指定时间；
|
||
- 前一方向段已经终止；
|
||
- 下一方向段从 `SegmentLocalS=0`、初始速度零开始。
|
||
|
||
## 7. 静态障碍物与横向安全走廊
|
||
|
||
### 7.1 绕障侧所有权
|
||
|
||
Hybrid A* 已经在含 `VehicleParameters.SafetyMarginMeters` 的完整车体模型下选择静态绕障拓扑。第一版 LS 不重新比较障碍物左右侧，而是在参考路径所在的连通自由通道中优化。
|
||
|
||
障碍物投影是约束生成，不是重新决策。
|
||
|
||
### 7.2 走廊构建
|
||
|
||
对当前方向段规划窗口：
|
||
|
||
1. 按 `LongitudinalSampleSpacingMeters` 采样多个 `ReferenceS`；
|
||
2. 在每个站点从种子 `l` 向正负方向按 `LateralSampleSpacingMeters` 摆放虚拟 AMR；
|
||
3. 将候选 Frenet 位姿转换到世界坐标；
|
||
4. 使用距离场作为快速宽相筛选；
|
||
5. 使用完整旋转矩形和现有连续碰撞检查语义确认安全性；
|
||
6. 得到该站点的多个自由横向区间；
|
||
7. 只选择包含种子轨迹的自由区间；
|
||
8. 沿 `s` 传播相互连通的区间，形成 `l_min(s)`、`l_max(s)`。
|
||
|
||
首轮种子优先级为：
|
||
|
||
```text
|
||
有效上一周期轨迹
|
||
→ 当前参考路径 l=0
|
||
```
|
||
|
||
`l=0` 应当位于首轮走廊内。若不在，说明地图或参考路径版本不一致、地图已经变化、坐标投影错误或上游路径失效，不能通过放宽安全约束继续发布。
|
||
|
||
### 7.3 SQP 中的走廊更新
|
||
|
||
首轮按参考姿态建立走廊。LS 产生候选后，使用候选 `l、l'` 恢复的新 yaw 和完整车体重新检查。必要时收紧走廊并再次求解，但不得跳到另一个不连通自由区间。
|
||
|
||
如果种子通道消失：
|
||
|
||
```text
|
||
缩短规划窗口
|
||
→ 尝试上一周期仍有效的安全轨迹
|
||
→ 执行安全停车尾段
|
||
→ 请求 Hybrid A* 重新规划拓扑
|
||
```
|
||
|
||
### 7.4 安全约束不软化
|
||
|
||
走廊边界属于硬约束。若内部诊断模型为了定位问题引入 slack，则任何超过数值容差的安全 slack 都禁止发布。
|
||
|
||
## 8. LS 横向优化
|
||
|
||
### 8.1 离散变量
|
||
|
||
对 `ReferenceS` 等距离散：
|
||
|
||
```text
|
||
l_i 横向偏移
|
||
dl_i dl/ds
|
||
ddl_i d²l/ds²
|
||
dddl_i d³l/ds³
|
||
```
|
||
|
||
采用三阶积分关系作为线性等式约束：
|
||
|
||
```text
|
||
l[i+1] = l[i] + dl[i]*Δs + 0.5*ddl[i]*Δs² + (1/6)*dddl[i]*Δs³
|
||
dl[i+1] = dl[i] + ddl[i]*Δs + 0.5*dddl[i]*Δs²
|
||
ddl[i+1] = ddl[i] + dddl[i]*Δs
|
||
```
|
||
|
||
### 8.2 硬约束
|
||
|
||
LS 硬约束包括：
|
||
|
||
```text
|
||
corridorMin_i <= l_i <= corridorMax_i
|
||
|l_i| <= MaximumLateralOffsetMeters
|
||
|l_i-lLinearization_i| <= MaximumLateralStepPerIterationMeters
|
||
1-referenceK_i*l_i >= MinimumFrenetDenominator
|
||
|dl_i| <= MaximumLateralSlope
|
||
|ddl_i| <= MaximumLateralSecondDerivative
|
||
|dddl_i| <= MaximumLateralThirdDerivative
|
||
|VehicleCurvature_i| <= MaximumVehicleCurvature
|
||
```
|
||
|
||
起点约束：
|
||
|
||
```text
|
||
l_0 = 当前车辆投影偏差
|
||
dl_0 = 根据运动方向航向误差计算
|
||
```
|
||
|
||
测量曲率或初始二阶导数易受噪声影响，第一版不把 `ddl_0` 设置为无条件硬等式。
|
||
|
||
换向或目标终端：
|
||
|
||
```text
|
||
l_end = 0
|
||
dl_end = 0
|
||
```
|
||
|
||
普通滚动终端通过软代价回归参考线，不在中途强制 `l=0`。
|
||
|
||
### 8.3 非线性曲率
|
||
|
||
设：
|
||
|
||
```text
|
||
A = 1-referenceK*l
|
||
B = dl
|
||
```
|
||
|
||
优化路径几何曲率为：
|
||
|
||
```text
|
||
optimizedGeometricK =
|
||
(A²*referenceK
|
||
+ A*ddl
|
||
+ referenceDK*l*dl
|
||
+ 2*referenceK*dl²)
|
||
/ (A²+B²)^(3/2)
|
||
```
|
||
|
||
车辆曲率为 `directionSign*optimizedGeometricK`。每轮顺序凸化在当前候选附近建立仿射近似和信赖域；最终验收使用原始非线性公式及世界坐标几何复核。
|
||
|
||
### 8.4 归一化代价
|
||
|
||
```text
|
||
J_LS =
|
||
w_l * Σ(l_i/scale_l)²
|
||
+ w_dl * Σ(dl_i/scale_dl)²
|
||
+ w_ddl * Σ(ddl_i/scale_ddl)²
|
||
+ w_dddl * Σ(dddl_i/scale_dddl)²
|
||
+ w_k * Σ(vehicleK_i/scale_k)²
|
||
+ w_dk * Σ(ΔvehicleK_i/scale_dk)²
|
||
+ w_previous * Σ((l_i-lPrevious_i)/scale_l)²
|
||
+ J_terminal
|
||
```
|
||
|
||
各项分别控制参考线偏离、航向偏差、空间平顺性、曲率、曲率变化和相邻规划周期稳定性。障碍物安全不进入代价函数，因为它已经是硬约束。
|
||
|
||
## 9. ST 纵向优化
|
||
|
||
### 9.1 输入路径
|
||
|
||
LS 输出必须先转回世界坐标、通过完整车体复核并重新计算严格递增的 `PathS`。ST 对该最终路径进行时间参数化。
|
||
|
||
### 9.2 离散变量
|
||
|
||
按固定 `Δt` 离散：
|
||
|
||
```text
|
||
s_i 当前方向段最终路径进度
|
||
u_i ds/dt，非负进度速度
|
||
a_i d²s/dt²
|
||
j_i d³s/dt³
|
||
```
|
||
|
||
线性动力学：
|
||
|
||
```text
|
||
s[i+1] = s[i] + u[i]*Δt + 0.5*a[i]*Δt² + (1/6)*j[i]*Δt³
|
||
u[i+1] = u[i] + a[i]*Δt + 0.5*j[i]*Δt²
|
||
a[i+1] = a[i] + j[i]*Δt
|
||
```
|
||
|
||
### 9.3 空间速度上限
|
||
|
||
先沿最终路径生成空间速度包络：
|
||
|
||
```text
|
||
speedLimit(s) = min(
|
||
directionMaximumSpeed,
|
||
curvatureSpeedLimit(s),
|
||
curvatureRateSpeedLimit(s),
|
||
stoppingSpeedLimit(s))
|
||
```
|
||
|
||
其中：
|
||
|
||
```text
|
||
curvatureSpeedLimit = sqrt(
|
||
MaximumLateralAcceleration / max(|k|, epsilon))
|
||
|
||
curvatureRateSpeedLimit =
|
||
MaximumCurvatureRatePerSecond / max(|dk/ds|, epsilon)
|
||
|
||
stoppingSpeedLimit = sqrt(
|
||
2*MaximumDeceleration*(terminalS-s))
|
||
```
|
||
|
||
时间域 QP 按当前 `s(t)` 候选读取或保守插值速度包络；迭代后重新核对。动态障碍物不产生第一版速度边界。
|
||
|
||
### 9.4 硬约束
|
||
|
||
```text
|
||
0 <= s_i <= terminalS
|
||
s[i+1] >= s[i]
|
||
0 <= u_i <= speedLimit(s_i)
|
||
-MaximumDeceleration <= a_i <= MaximumAcceleration
|
||
|j_i| <= MaximumJerk
|
||
```
|
||
|
||
初始状态：
|
||
|
||
```text
|
||
s_0 = 当前车辆在最终路径上的进度
|
||
u_0 = abs(SignedLongitudinalSpeed)
|
||
a_0 = 已提供的纵向加速度；未提供时使用零并记录诊断
|
||
```
|
||
|
||
所有终端均满足：
|
||
|
||
```text
|
||
s_N = terminalS
|
||
u_N = 0
|
||
```
|
||
|
||
终端 `a_N=0` 作为软目标，避免短窗口无解。
|
||
|
||
### 9.5 归一化代价
|
||
|
||
```text
|
||
J_ST =
|
||
w_speed * Σ((u_i-uReference_i)/scale_u)²
|
||
+ w_acc * Σ(a_i/scale_a)²
|
||
+ w_jerk * Σ(j_i/scale_jerk)²
|
||
+ w_previous * Σ((s_i-sPrevious_i)/scale_s)²
|
||
+ J_terminal
|
||
```
|
||
|
||
由于所有第一版问题都硬约束 `s_N=terminalS`，进度项会成为常数，因此不得加入目标函数。正常前进由参考速度项驱动，空间终端由 horizon 可达性计算确定。
|
||
|
||
停车位置、速度上限、加速度上限和 jerk 上限不得由大权重代替硬约束。
|
||
|
||
## 10. 滚动规划、拼接和安全停车尾段
|
||
|
||
### 10.1 规划与调度分离
|
||
|
||
`EmPlanningService` 只处理一次请求。`EmPlanningCoordinator` 负责：
|
||
|
||
- 按 `ReplanPeriod` 捕获车辆状态；
|
||
- 绑定地图和参考路径版本；
|
||
- 取消已过期求解；
|
||
- 提供上一轨迹和未来拼接点；
|
||
- 原子发布完整结果；
|
||
- 拒绝版本已经过期的结果。
|
||
|
||
### 10.2 基本轨迹拼接
|
||
|
||
上一轨迹仍有效且跟踪误差在允许范围内时，从上一轨迹未来 `TrajectoryHandoffLeadTime` 对应状态开始新规划，以补偿计算和发布延迟。
|
||
|
||
跟踪误差过大、上一轨迹过期或接近换向事件时，放弃预测拼接，从最新实测状态建立新的安全规划。禁止跨换向边界拼接。
|
||
|
||
### 10.3 完整发布与安全尾段
|
||
|
||
每次发布完整规划轨迹，而不是只发布执行前缀。正常情况下，新轨迹会在 `ReplanPeriod` 后接力；若后续求解失败，执行器继续使用上一条完整轨迹，并进入其安全停车尾段。
|
||
|
||
执行器不得在轨迹终点以后外推。若没有新的有效轨迹，车辆必须在旧轨迹的零速终端停下。
|
||
|
||
### 10.4 版本一致性
|
||
|
||
每次求解绑定：
|
||
|
||
```text
|
||
MapSnapshotId
|
||
ReferencePathId
|
||
VehicleState.SequenceId
|
||
PreviousTrajectoryId
|
||
SegmentIndex
|
||
```
|
||
|
||
地图、参考路径或当前方向段在求解期间改变时，旧结果不得发布。
|
||
|
||
## 11. 求解器架构
|
||
|
||
### 11.1 选择
|
||
|
||
第一版采用：
|
||
|
||
```text
|
||
C# SequentialConvexOptimizer
|
||
→ IQpSolver
|
||
→ OsqpNativeSolver
|
||
→ OSQP C API
|
||
```
|
||
|
||
OSQP 只负责标准凸 QP：
|
||
|
||
```text
|
||
minimize 0.5*xᵀPx + qᵀx
|
||
subject to lower <= A*x <= upper
|
||
```
|
||
|
||
LS/ST 不得直接依赖 P/Invoke 类型。
|
||
|
||
### 11.2 顺序凸化验收
|
||
|
||
每轮保存最近一个经过约束检查的可行候选。停止条件同时考虑：
|
||
|
||
- 变量无穷范数变化；
|
||
- 目标函数相对变化；
|
||
- 最大约束残差；
|
||
- 最大迭代次数；
|
||
- 规划总预算和取消信号。
|
||
|
||
求解器返回的最后一次迭代不自动等于可发布结果。只有最后可行候选通过世界坐标独立复核后才可发布。
|
||
|
||
`SolvedInaccurate` 只有在残差满足 EM Planner 自身更严格的验收阈值并通过完整复核时才可接受。超时可以使用本次调用中已经得到的最后可行候选；若没有可行候选则进入回退流程。
|
||
|
||
### 11.3 原生插件加载
|
||
|
||
源码中保存固定版本的 Windows x64 OSQP 运行库和许可证：
|
||
|
||
```text
|
||
ClumsyPilot/ThirdParty/OSQP/
|
||
├── win-x64/osqp.dll
|
||
├── LICENSE
|
||
├── NOTICE
|
||
└── VERSION
|
||
```
|
||
|
||
第一版插件发布目标明确固定为 Windows x64；如果实际宿主为 x86，必须生成独立的 win-x86 发布包，不能在同一插件目录中混放并依赖运行时猜测。
|
||
|
||
最终插件目录：
|
||
|
||
```text
|
||
plugins/
|
||
├── ClumsyPilot.dll
|
||
├── osqp.dll
|
||
└── licenses/
|
||
├── OSQP-LICENSE.txt
|
||
└── OSQP-NOTICE.txt
|
||
```
|
||
|
||
`OsqpNativeLoader` 使用 `ClumsyPilot.dll` 的 `Assembly.Location` 得到插件绝对目录，并用绝对路径预加载同级 `osqp.dll`。禁止依赖当前工作目录或要求用户修改系统 `PATH`。
|
||
|
||
原生约束：
|
||
|
||
- OSQP C API 版本固定，包装声明与对应头文件一致；
|
||
- 宿主进程和 `osqp.dll` 位数一致；
|
||
- 使用 Cdecl；
|
||
- 使用内置 QDLDL，避免 MKL/CUDA 运行库；
|
||
- 初始化并发安全，模块句柄保持到插件生命周期结束；
|
||
- 所有 workspace 先释放，再释放模块；
|
||
- 加载失败返回 `SolverUnavailable`，不得使宿主进程崩溃；
|
||
- 发布前使用依赖检查工具确认没有遗漏原生运行库。
|
||
|
||
## 12. 配置结构与初始值
|
||
|
||
配置按职责分组，并在请求创建时复制为不可变快照：
|
||
|
||
```text
|
||
EmPlannerConfiguration
|
||
├── Scheduling
|
||
├── Frenet
|
||
├── Corridor
|
||
├── Lateral
|
||
├── Longitudinal
|
||
├── Solver
|
||
└── Validation
|
||
```
|
||
|
||
第一版初始工程值：
|
||
|
||
```text
|
||
Scheduling:
|
||
ReplanPeriodSeconds 0.20
|
||
PlanningTimeHorizonSeconds 6.00
|
||
PlanningDistanceHorizonMeters 5.00
|
||
OutputTimeStepSeconds 0.05
|
||
SolverTimeoutSeconds 0.10
|
||
TrajectoryHandoffLeadTimeSeconds 0.30
|
||
MaximumVehicleStateAgeSeconds 0.20
|
||
|
||
Corridor:
|
||
LongitudinalSampleSpacingMeters 0.10
|
||
LateralSampleSpacingMeters 0.025
|
||
MaximumLateralOffsetMeters 0.30
|
||
AdditionalClearanceReserveMeters 0.02
|
||
MaximumCollisionCheckStepMeters 0.025
|
||
|
||
Frenet/Lateral:
|
||
MaximumProjectionDistanceMeters 0.50
|
||
BoundaryEpsilonMeters 0.00000001
|
||
MinimumFrenetDenominator 0.20
|
||
MaximumLateralStepPerIterationMeters 0.05
|
||
MaximumLateralSlope 0.50
|
||
MaximumLateralSecondDerivative 1.00 1/m
|
||
MaximumLateralThirdDerivative 2.00 1/m²
|
||
|
||
Longitudinal:
|
||
MaximumForwardSpeedMetersPerSecond 0.20
|
||
MaximumReverseSpeedMetersPerSecond 0.20
|
||
MaximumAccelerationMetersPerSecond2 0.20
|
||
MaximumDecelerationMetersPerSecond2 0.30
|
||
MaximumJerkMetersPerSecond3 0.50
|
||
MaximumLateralAccelerationMetersPerSecond2 0.20
|
||
MaximumCurvatureRatePerMeterPerSecond 0.50
|
||
StopSpeedToleranceMetersPerSecond 0.01
|
||
StopHoldDurationSeconds 0.20
|
||
|
||
Solver:
|
||
MaximumSequentialIterations 5
|
||
MaximumQpIterations 4000
|
||
AbsoluteTolerance 0.00001
|
||
RelativeTolerance 0.00001
|
||
MaximumAcceptedConstraintResidual 0.00001
|
||
EnableWarmStart true
|
||
EnablePolishing true
|
||
EnableNativeVerboseOutput false
|
||
```
|
||
|
||
这些值是结合现有工程 `0.2～0.3 m/s` 运行范围设定的安全初值，不是车辆最终标定值。所有数值必须通过配置覆盖，且配置校验必须拒绝非有限值、非正采样间隔、冲突上下界和无法满足停车预算的组合。
|
||
|
||
代价权重使用无量纲归一化项，配置同时保存权重和尺度。禁止使用散落在算法文件中的魔法常量。
|
||
|
||
归一化后的初始权重为：
|
||
|
||
```text
|
||
Lateral:
|
||
ReferenceOffset 10
|
||
HeadingDeviation 1
|
||
SecondDerivative 5
|
||
ThirdDerivative 10
|
||
Curvature 5
|
||
CurvatureVariation 20
|
||
PreviousTrajectory 5
|
||
RollingTerminal 10
|
||
|
||
Longitudinal:
|
||
ReferenceSpeed 10
|
||
Acceleration 1
|
||
Jerk 10
|
||
PreviousTrajectory 5
|
||
TerminalAcceleration 1
|
||
```
|
||
|
||
这些权重只在对应物理量除以配置尺度后生效。标定时修改配置，不修改问题构造代码。
|
||
|
||
第一版归一化尺度取对应硬限制：
|
||
|
||
```text
|
||
scale_l = MaximumLateralOffset
|
||
scale_dl = MaximumLateralSlope
|
||
scale_ddl = MaximumLateralSecondDerivative
|
||
scale_dddl = MaximumLateralThirdDerivative
|
||
scale_k = Vehicle.MaximumCurvature
|
||
scale_dk = max(1, reference path maximum |dk/ds|)
|
||
scale_u = 当前方向 MaximumSpeed
|
||
scale_a = max(MaximumAcceleration, MaximumDeceleration)
|
||
scale_jerk = MaximumJerk
|
||
scale_s = terminalS
|
||
```
|
||
|
||
配置校验保证所有尺度为正有限值；零长度方向段不进入优化。
|
||
|
||
## 13. 调试与诊断
|
||
|
||
沿用现有旁路模式：
|
||
|
||
```text
|
||
EmPlannerDebugOptions
|
||
Enabled
|
||
EnableSummaryLog
|
||
EnableProjectionTrace
|
||
EnableCorridorTrace
|
||
EnableLateralSolverTrace
|
||
EnableLongitudinalSolverTrace
|
||
EnableTrajectoryDump
|
||
EnableVisualization
|
||
Sink
|
||
```
|
||
|
||
核心算法禁止直接 `Console.WriteLine`。默认使用空 sink。调试接收器异常必须隔离，不得改变规划状态或轨迹内容。
|
||
|
||
无论调试是否开启，结果都保留紧凑诊断：
|
||
|
||
- 总耗时及各阶段耗时；
|
||
- 输入版本；
|
||
- 当前方向段和终端类型；
|
||
- 投影结果和起始状态；
|
||
- 走廊最窄宽度；
|
||
- LS/ST 迭代次数和求解状态；
|
||
- 最大约束残差；
|
||
- 最小完整车体净空；
|
||
- 最大曲率、曲率变化、速度、加速度和 jerk；
|
||
- 回退原因或终止原因。
|
||
|
||
## 14. 状态与回退
|
||
|
||
结果状态至少区分：
|
||
|
||
```text
|
||
Success
|
||
SuccessWithFallback
|
||
InvalidInput
|
||
UnsupportedMotionMode
|
||
StaleVehicleState
|
||
StateDirectionMismatch
|
||
InvalidReferencePath
|
||
ProjectionFailed
|
||
CorridorInfeasible
|
||
LateralInfeasible
|
||
LongitudinalInfeasible
|
||
StoppingDistanceInsufficient
|
||
SolverUnavailable
|
||
SolverTimedOut
|
||
Cancelled
|
||
ValidationFailed
|
||
Superseded
|
||
Failed
|
||
```
|
||
|
||
回退规则：
|
||
|
||
```text
|
||
LS 不可行
|
||
→ 收紧/缩小单轮步长并重试
|
||
→ 使用 l=0 的已验证参考路径
|
||
→ 世界坐标复核
|
||
→ 仍失败则停车并请求粗路径重规划
|
||
|
||
ST 不可行
|
||
→ 降低参考速度
|
||
→ 使用保守停车速度包络
|
||
→ 仍无法在剩余距离停车则返回 StoppingDistanceInsufficient
|
||
|
||
周期求解失败
|
||
→ 继续上一条仍有效的完整轨迹
|
||
→ 进入其安全停车尾段
|
||
```
|
||
|
||
只有 `Success` 和 `SuccessWithFallback` 可以携带非空轨迹。使用 `l=0` 参考路径、上一轮可行 SQP 候选、保守速度包络或已验证上一轨迹安全尾段时，返回 `SuccessWithFallback` 并在诊断中记录具体来源。
|
||
|
||
任何失败结果不得包含可被误认为可执行轨迹的部分输出。
|
||
|
||
## 15. 文件结构
|
||
|
||
```text
|
||
ClumsyPilot/ParkrobTrajplanner/
|
||
├── EMPlanner/
|
||
│ ├── README.md
|
||
│ ├── Contracts/
|
||
│ ├── Configuration/
|
||
│ ├── Segmentation/
|
||
│ ├── Frenet/
|
||
│ ├── Corridor/
|
||
│ ├── Lateral/
|
||
│ ├── Longitudinal/
|
||
│ ├── Optimization/
|
||
│ │ └── Osqp/
|
||
│ ├── Trajectory/
|
||
│ ├── Validation/
|
||
│ ├── Diagnostics/
|
||
│ ├── Facade/
|
||
│ └── Test/
|
||
├── TrajectoryExecution/
|
||
└── Utils/
|
||
|
||
ClumsyPilot/ThirdParty/OSQP/
|
||
├── win-x64/osqp.dll
|
||
├── LICENSE
|
||
├── NOTICE
|
||
└── VERSION
|
||
```
|
||
|
||
依赖方向：
|
||
|
||
```text
|
||
Contracts + Configuration
|
||
→ Segmentation + Frenet
|
||
→ Corridor
|
||
→ Lateral + Longitudinal
|
||
→ Trajectory
|
||
→ Validation
|
||
→ Facade
|
||
→ TrajectoryExecution
|
||
```
|
||
|
||
`Optimization` 只提供数学求解能力。`TrajectoryExecution` 消费不可变结果，不反向修改规划对象。
|
||
|
||
## 16. 验证策略
|
||
|
||
### 16.1 坐标与倒车
|
||
|
||
- `yaw=0、±π/2、π` 的世界/Frenet 往返；
|
||
- 前进与倒车的 `TravelYaw`；
|
||
- 倒车时 `l>0` 位于运动方向左侧；
|
||
- `±π` 附近连续展开；
|
||
- `yawRate=signedV*VehicleCurvature`；
|
||
- `speed=abs(signedV)` 且 `vx/vy` 与 `Yaw`、`signedV` 一致；
|
||
- 回环路径不投影到错误分支；
|
||
- 换向重复位姿不跨段投影；
|
||
- `ReferenceS` 与 `PathS` 不混用。
|
||
|
||
### 16.2 分段和事件
|
||
|
||
- horizon 正好命中、略早于和略晚于段末；
|
||
- 段末位于普通采样点之间；
|
||
- 换向前后语义点不被去重；
|
||
- 时间采样插入精确停车事件；
|
||
- 输出不包含超过 `SegmentLocalS` 终端的点；
|
||
- 轨迹终点速度为零且执行器不外推。
|
||
|
||
### 16.3 走廊和 LS
|
||
|
||
- 空地图得到对称走廊；
|
||
- 单侧障碍收紧正确方向的边界；
|
||
- 只选择包含种子的连通自由区间；
|
||
- 上一周期种子保持通道稳定；
|
||
- 矩形角点、旋转姿态和地图边界均参与检查；
|
||
- LS 所有点满足走廊、Frenet 和曲率约束；
|
||
- 非线性复核拒绝线性化残差过大的结果。
|
||
|
||
### 16.4 ST
|
||
|
||
- 前进和倒车内部 `u>=0`，发布速度符号正确；
|
||
- 最大速度、加减速度、jerk 和曲率限速；
|
||
- 换向、目标和滚动安全尾段均零速终止；
|
||
- 停车距离不足返回明确状态；
|
||
- 固定输入产生确定性输出；
|
||
- 上一轨迹拼接不跨换向点。
|
||
|
||
### 16.5 OSQP 和插件
|
||
|
||
- 同目录绝对路径加载；
|
||
- 缺失 DLL、错误位数和错误 API 版本的诊断；
|
||
- 固定微型 QP 冒烟测试；
|
||
- warm start 和超时；
|
||
- 多线程首次初始化；
|
||
- 干净 Windows 插件目录验证。
|
||
|
||
### 16.6 集成验收
|
||
|
||
- 现有可消费平滑状态均可进入 EM；
|
||
- 不可消费平滑状态被拒绝；
|
||
- 最终所有点数值有限，时间严格递增；
|
||
- 公开的 `SignedLongitudinalVelocity`、`Speed`、`VelocityX`、`VelocityY` 和 `YawRate` 满足统一派生关系；
|
||
- 完整车体逐点和连续扫掠无碰撞；
|
||
- 曲率、速度、加速度和 jerk 均在配置限制内；
|
||
- 换向和目标边界精确保留；
|
||
- 调试开关不改变轨迹数值；
|
||
- 取消、超时和版本淘汰不发布部分轨迹。
|
||
|
||
## 17. 当前工程基线说明
|
||
|
||
当前工作区包含大量未提交的 CoarsePath、Map 和 PathSmoothing 变更，EM 实施必须避免覆盖这些用户改动。
|
||
|
||
完整 `ClumsyPilot.csproj` 构建目前会被现有 `auto_avoidance/MultiWheelAutoAvoidance.cs` 缺少 `NetTopologySuite` 和 `OpenCvSharp` 引用阻断。该问题不是 EM Planner 引入，但实施阶段必须通过补齐现有依赖或隔离旧模块建立可重复的基线验证；不得把该既有失败误判为 EM 回归。
|
||
|
||
## 18. 设计完成标准
|
||
|
||
第一版设计落地后应满足：
|
||
|
||
1. 核心规划完全由纯请求驱动，不读取硬件和 UI；
|
||
2. 前进/倒车坐标、速度和曲率符号只有一套定义；
|
||
3. 任何优化都不跨换向点；
|
||
4. 静态障碍只生成已选拓扑内的硬走廊；
|
||
5. 所有轨迹均以安全零速终端结束；
|
||
6. OSQP 通过隔离接口和绝对路径原生加载；
|
||
7. 所有安全属性由独立世界坐标验证器复核；
|
||
8. 动态障碍、横移和底盘逆解保持在明确的后续边界之外。
|