ParkingRobot/ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md

EM Planner Foundation

当前范围

EM Planner 位于既有的空间路径之后：

PlanningGridMap -> Hybrid A* CoarsePath -> Local G2 PathSmoothing -> EM Planner -> TrajectoryExecution

本 Foundation 只消费一个已验证的 PathSmoothingResult 方向段、同版本的 PlanningGridMap、车辆几何和请求快照。它不读取硬件、UI、时钟或当前工作目录。 每次计算只处理一个 DirectionSegmentView，因此不会跨越换向边界。

此门禁已提供 solver-neutral IQpSolver 契约和固定 OSQP 后端；尚未实现 LS 横向优化、 ST 纵向优化、滚动协调、轨迹发布或动态障碍物。

坐标与符号

世界长度使用 m，时间使用 s，航向使用 rad，曲率使用 1/m。车体 +x 指向车头， 车体 +y 指向车体左侧，正航向为逆时针。Frenet 的基向量永远使用实际行驶方向：

travelYaw = Forward ? vehicleYaw : Normalize(vehicleYaw + PI)
t = (cos(travelYaw), sin(travelYaw))
n = (-sin(travelYaw), cos(travelYaw))

deltaS = dx*cos(travelYaw) + dy*sin(travelYaw)
l      = -dx*sin(travelYaw) + dy*cos(travelYaw)

x = referenceX - l*sin(travelYaw)
y = referenceY + l*cos(travelYaw)
optimizedTravelYaw = travelYaw + atan2(dl, 1 - referenceK*l)
vehicleYaw = Reverse ? Normalize(optimizedTravelYaw + PI) : Normalize(optimizedTravelYaw)

ReferenceS 始终沿实际运动方向递增；l > 0 永远代表运动方向左侧。 例如倒车时车头朝 -PI + 0.01，travelYaw 为 0.01。此时 l > 0 在 运动方向左侧、也就是车体右侧；不得在倒车分支额外翻转 l。重建要求 1 - referenceK*l >= MinimumFrenetDenominator，否则拒绝奇异 Frenet 几何。

参考航向的插值采用未归一化航向；只有公开的 vehicle yaw 被归一化到 [-PI, PI)。 投影只检查请求的同一方向段和 S 区间：它对线段候选做夹紧投影，按距离、种子 S 距离和较小 S 的顺序确定性地破平局，绝不搜索另一方向段或另一条回环分支。

精确边界与静态走廊

换向配对点可拥有相同的世界位姿和源弧长，但仍有不同身份：前一段末端是 GearSwitchApproach，下一段起点是 GearSwitchDeparture。边界身份是 (SegmentIndex, SegmentLocalS, BoundaryType)，不按世界坐标去重。

静态走廊以精确请求的起止 ReferenceS 为首末站，并在中间按配置采样。每站从 上一条有效轨迹的 Frenet 种子开始；没有种子时才使用 l=0。候选横向位置包含精确 种子和偏移限值。距离场仅用于明显安全的快速接受；其余候选使用旋转矩形 FootprintCollisionChecker 和额外净空复核。仅选择包含种子的自由区间，并要求相邻 站的选择区间重叠。若种子连通区消失，构建失败，绝不跳到障碍物另一侧。

Foundation 配置单位

配置组	关键单位和初值
Corridor	纵向间距 0.10 m、横向间距 0.025 m、最大偏移 0.30 m、额外净空 0.02 m
Frenet	最大投影距离 0.50 m、最小分母 0.20、边界锚点容差 1e-8 m
Scheduling	重规划周期 0.20 s、时间窗 6.0 s、距离窗 5.0 m
Longitudinal	前进/倒车最大速度各 0.20 m/s；加速度 0.20 m/s²；减速度 0.30 m/s²；jerk 0.50 m/s³

请求验证会复制配置快照并拒绝非有限值、非法采样间距、不可用地图、不可消费的平滑结果 和不支持的运动模型。

可重复 Foundation 验证

在仓库根目录运行：

dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- all-foundation

成功时输出严格为：

PASS foundation
PASS segmentation
PASS frenet
PASS corridor

OSQP 后端部署与诊断

横向与纵向规划器只能依赖 IQpSolver、QuadraticProgram、QpSolverSettings 和 QpSolveResult；它们不得依赖 OSQP 的 P/Invoke 类型。当前后端固定为 OSQP v1.0.0， Windows x64、double precision、int32 索引、未打包 settings 和内置 QDLDL algebra。源包布局为：

ClumsyPilot/ThirdParty/OSQP/
├── build-win-x64.ps1
├── LICENSE
├── NOTICE
├── VERSION
├── SHA256SUMS
└── win-x64/osqp.dll

VERSION 记录且构建脚本强制使用以下开关：

OSQP_ALGEBRA_BACKEND=builtin
OSQP_BUILD_SHARED_LIB=ON
OSQP_BUILD_STATIC_LIB=OFF
OSQP_BUILD_DEMO_EXE=OFF
OSQP_BUILD_UNITTESTS=OFF
OSQP_USE_FLOAT=OFF
OSQP_USE_LONG=OFF
OSQP_PACK_SETTINGS=OFF
OSQP_ENABLE_PRINTING=OFF
OSQP_CODEGEN=OFF
OSQP_ENABLE_DERIVATIVES=OFF

最终插件发布布局（由后续插件打包阶段负责复制）必须为：

plugins/
├── ClumsyPilot.dll
├── osqp.dll
└── licenses/
    ├── OSQP-LICENSE.txt
    └── OSQP-NOTICE.txt

OsqpNativeLoader 只从 typeof(OsqpNativeLoader).Assembly.Location 所在目录以绝对路径 预加载同级 osqp.dll，不读取当前工作目录，也不要求用户修改 PATH。它拒绝非 x64 进程、缺失或无法加载的 DLL、缺少导出及非 1.0.0 版本，并以结构化 SolverUnavailable/NativeError 结果返回；不会让宿主进程因加载失败而崩溃。所有 C API 委托均采用 Cdecl，原生 verbose 强制关闭。

每次 OsqpNativeSolver.Solve 独立拥有 P/Q/A/l/u 和可选 warm-start 的固定数组、CSC 包装、settings 和 OSQP workspace。它在 finally 中按反向顺序释放：先 osqp_cleanup workspace，再释放 settings/CSC 块，最后释放托管数组 pin；加载后的模块句柄保留至进程生命周期结束。

原生状态映射固定如下：

OSQP status value	QpSolveStatus
1	Solved
2	SolvedInaccurate
3, 4	PrimalInfeasible
5, 6	DualInfeasible
7	MaximumIterations
8	TimeLimit
9, 10, 11 或未知值	NativeError

SolvedInaccurate 只在后续规划器完成更严格的独立残差和领域复核后才可发布。调用前取消映射 为 Cancelled；加载失败映射为 SolverUnavailable；OSQP setup、warm-start 或 solve API 返回错误时映射为 NativeError。

在仓库根目录运行 OSQP 回归门：

dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- optimization
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- osqp

osqp 会在临时的干净插件目录中验证缺失 DLL、损坏 DLL、绝对路径加载、并发首次加载，以及 有界最优、等式最优、不可行和极短时限 QP。成功时包含：

PASS osqp-solve
PASS osqp-loader

LS 横向 SQP

LS 只对单个 DirectionSegmentView 的 ReferenceS 站点求解；l > 0 在前进和倒车时 都表示行驶方向左侧。每个有 N 个站点的问题使用连续区间的变量布局：

l[0..N-1], dl[0..N-1], ddl[0..N-1], dddl[0..N-2]

相邻站点之间按实际 ds = ReferenceS[i+1]-ReferenceS[i] 精确满足三阶积分关系：

ddl[i+1] = ddl[i] + ds*dddl[i]
dl[i+1]  = dl[i] + ds*ddl[i] + 0.5*ds^2*dddl[i]
l[i+1]   = l[i] + ds*dl[i] + 0.5*ds^2*ddl[i] + ds^3*dddl[i]/6

每轮 QP 将以下项目作为硬约束：静态走廊、最大横向偏移、以当前迭代为中心且半径不超过 0.05 m 的 l 信赖域、1-referenceK*l >= 0.20、dl/ddl/dddl 上限、起始 l 与 dl，以及线性化的车辆曲率约束。Goal 和 GearSwitch 末端额外强制 l_end=0、dl_end=0；RollingSafetyStop 不添加这两个等式，而是使用软终端回归代价。

目标函数采用 OSQP 的 0.5*x'P*x + q'x 形式。所有平方残差先除以相应物理尺度的平方， 再乘权重：l 使用最大横向偏移，dl、ddl、dddl 分别使用对应导数上限，曲率使用 车辆最大曲率，曲率变化使用 max(1, max |dk/ds|)。代价覆盖参考线、航向、二阶导、三阶导、 线性化曲率、曲率变化、上一轨迹种子及滚动终端；走廊安全绝不软化为代价。

SequentialConvexOptimizer 最多运行五轮，通过 IQpSolver 取得完整 primal 向量并将它用作 下一轮 warm start。每个解都先由完整 Frenet 公式重建为世界坐标，再由独立验证器复核走廊、 起点/终端、导数、分母、曲率、有限值和严格递增弧长。只有该复核通过的深拷贝候选才能保留。 后续超时、取消或失败不会发布未验证的最后求解器向量：若已有候选则返回 SuccessWithFallback，否则返回最具体的失败状态。

ReferenceS 是 LS 的独立变量，不能被当作行驶距离。重建后以世界坐标相邻弦长重新累计 PathS，因此输出 PathS[0]=0 且严格递增；这个实际几何 PathS 才是后续纵向规划可消费 的距离契约。

可重复横向验证：

dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- lateral-all

该门禁依次验证 LS 模型、脚本化 SQP 状态机，以及在干净复制 plugin bundle 中运行的真实 OSQP 固定场景：前进/倒车直线、缓弯、静态障碍收窄的种子连通走廊、换向终端和滚动终端。每个真实 场景运行两次，状态、点数和全部数值输出必须在 1e-10 内一致。

ST、完整轨迹与单次服务

ST 只消费 LS 已复核的实际 PathS，而不把 ReferenceS 当作行驶距离。它在固定时间 knot 上 求解非负进度速度 u、加速度和 jerk，并在所有终端硬约束 PathS=terminalPathS、u=0。 速度包络取方向限速、横向加速度、曲率率和停车包络中的最小值。成功的轨迹始终包含精确零速终端， 随后按 0.05 s 间隔提供 0.20 s 的同姿态、零速度 hold tail。

公开门面仅提供单次、同步且可取消的调用：

EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken);

EmPlanningService 不负责周期调度、版本淘汰、轨迹执行、换向状态机、控制适配、UI 或硬件读取。 它只使用请求提供的 RequestedAtUtc / EffectiveAtUtc，绝不读取系统时钟或当前工作目录。处理顺序固定为：

request/config validation
-> direction-segment selection
-> bounded ego projection
-> PlanningHorizonSelector exact terminal
-> previous-trajectory seed projection
-> static connected corridor
-> LS optimization and validation
-> PathS speed envelope
-> ST optimization and validation
-> immutable trajectory assembly
-> world-space publication validation
-> immutable result publication

请求快照

EmPlanningRequest 的构造参数依次为：已发布的 PathSmoothingResult、同版本的 PlanningGridMap、VehicleParameters、不可变 VehicleMotionState、EmPlannerConfiguration、 当前 SegmentIndex、可选 PreviousTrajectory、RequestedAtUtc、EffectiveAtUtc、输出轨迹 ID、 参考路径 ID、上一轨迹 ID，以及 EmMotionModel.NonholonomicForwardReverse。状态快照中的正带符号 速度表示前进，负值表示倒车；绝对值不大于 StopSpeedToleranceMetersPerSecond 时按停车处理。

服务会复制配置和各规划输入，不修改请求所属对象或列表。结果诊断（FailureReason）总是以以下稳定 标识开始，方便调用方审计版本绑定：

map=<MapSnapshotId>;reference=<ReferencePathId>;state=<VehicleState.SequenceId>;
previous=<PreviousTrajectoryId>;segment=<SegmentIndex>

结果、字段和单位

EmPlanningResult 只有 Success 或 SuccessWithFallback 时才携带不可变 EmTrajectory；所有失败状态 都携带空轨迹。EmTrajectory.Metadata 包含轨迹 ID、生成/生效时间、地图 ID、参考路径 ID、状态序列、 上一轨迹 ID、方向段、方向和终端类型。每个公开 EmTrajectoryPoint 字段如下：

字段	单位 / 符号
X, Y	世界坐标 m
Yaw	车辆车头世界航向 rad，归一化到 [-PI, PI)
SignedLongitudinalVelocity	车体纵向 m/s；前进为正，倒车为负；权威速度字段
Speed	abs(SignedLongitudinalVelocity)，m/s，非负
VelocityX, VelocityY	世界速度 m/s；分别等于 signedV*cos(Yaw)、signedV*sin(Yaw)
YawRate	rad/s，逆时针为正；等于 signedV*VehicleCurvature
TimeFromStart	从本条轨迹生效时刻起的 s，严格递增
VehicleCurvature	车辆曲率 1/m；倒车时已按车头 yaw 符号转换
SegmentIndex, SegmentLocalS, PathS	当前方向段标识与局部实际进度 m；PathS 不递减
Direction, BoundaryType	Forward / Reverse 与 None、RollingSafetyStop、GearSwitchApproach 或 Goal

冗余速度字段不可独立赋值；组装器从权威 signedV 和 VehicleCurvature 派生它们，发布前 EmTrajectoryValidator 再独立复算。验证器还会重算有限差分加速度、jerk、曲率率，逐点调用完整 车体姿态检查，并以最大 0.025 m 中心步长检查每个相邻点的扫掠运动。

终端类型固定为：

EmTerminalType	含义
RollingSafetyStop	当前规划窗口未到分段边界时的安全停车终端
GearSwitch	当前方向段末端的精确停车；执行层随后拥有换向状态机
Goal	最后方向段末端的精确停车

状态为 Success、SuccessWithFallback、InvalidInput、UnsupportedMotionMode、StaleVehicleState、 StateDirectionMismatch、InvalidReferencePath、ProjectionFailed、CorridorInfeasible、 LateralInfeasible、LongitudinalInfeasible、StoppingDistanceInsufficient、SolverUnavailable、 SolverTimedOut、Cancelled、ValidationFailed、Superseded 和 Failed。除前两项外，全部状态 均发布空轨迹和确定性诊断。

最小调用示例

调用方先在规划边界外获取地图、平滑路径和车辆状态快照；下面的对象均为该步骤已经准备好的不可变输入：

var service = new EmPlanningService(qpSolver);
var forwardRequest = new EmPlanningRequest(
    publishedSmoothingResult, planningMap, vehicle, forwardState, configuration,
    segmentIndex: 0, previousTrajectory: null,
    requestedAtUtc: capturedRequestTime, effectiveAtUtc: effectiveTime,
    outputTrajectoryId: "traj-100", referencePathId: "path-17", previousTrajectoryId: "",
    motionModel: EmMotionModel.NonholonomicForwardReverse);

EmPlanningResult forward = service.Plan(forwardRequest, cancellationToken);

倒车不需要额外翻转横向坐标或世界速度；选择倒车方向段并把状态带符号速度设为负即可：

var reverseState = new VehicleMotionState(reversePose, -0.05d, null, capturedRequestTime, sequenceId: 44);
var reverseRequest = new EmPlanningRequest(
    publishedSmoothingResult, planningMap, vehicle, reverseState, configuration,
    segmentIndex: 1, previousTrajectory: forward.Trajectory,
    requestedAtUtc: capturedRequestTime, effectiveAtUtc: effectiveTime,
    outputTrajectoryId: "traj-101", referencePathId: "path-17", previousTrajectoryId: "traj-100",
    motionModel: EmMotionModel.NonholonomicForwardReverse);

EmPlanningResult reverse = service.Plan(reverseRequest, cancellationToken);

在仓库根目录运行完整单次服务门禁：

dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-core-all

成功输出依次为：

PASS longitudinal-model
PASS longitudinal-integration
PASS trajectory
PASS em-planning-service

Rolling execution ownership and deployment

The first-version boundary has three independently testable layers:

caller snapshots -> EmPlanningCoordinator -> immutable published EmTrajectory
                                                -> TrajectoryExecutor -> TrajectoryControlCommand

• EmPlanningService remains a pure, synchronous, one-shot planner. It consumes only the request snapshot and never reads a clock, current directory, UI, localization, wheel speed, or hardware object.
• The caller owns state capture, map/reference-path version selection, the replan clock, and any hardware-specific action after it receives a generic command. IVehicleStateProvider.Capture() belongs to this execution boundary and returns a VehicleMotionState snapshot; it is not a planner dependency.
• EmPlanningCoordinator owns latest-wins cycle cancellation and atomic publication. A cycle binds MapSnapshotId, ReferencePathId, vehicle-state SequenceId, PreviousTrajectoryId, and SegmentIndex; a result publishes only when that complete identity and its version are still current. The default update cadence is 0.20 s (with the configured 6.0 s / 5.0 m planning horizons).
• TrajectoryExecutor only samples the immutable published trajectory with caller-provided time and measured state. It never extrapolates beyond the final point. A failed replan leaves the last complete published trajectory in service through its exact zero-speed safety tail.

Handoff and gear changes

A normal replan may use a future sample from the prior trajectory only when it is within the configured age and position/yaw/speed tracking tolerances, remains in the same segment and direction, and does not cross a gear boundary. Unsafe tracking, stale data, a terminal boundary, or any segment/direction mismatch causes a deterministic reset to the caller-supplied measured state with no trajectory seed.

At an exact GearSwitchApproach boundary the executor follows this sequence:

Following -> ApproachingGearSwitch -> HoldingZero
-> RequestingDirectionChange (one request) -> AwaitingDirectionConfirmation -> Following

Measured absolute speed must remain below 0.01 m/s continuously for 0.20 s before the single direction-change request. Every holding, direction-confirmation, rolling-stop, and goal-completion command is zero speed and zero yaw rate. Goal and RollingSafetyStop leave the executor completed while braking is held.

Trajectory telemetry and generic command

Every EmTrajectoryPoint retains these fields for execution telemetry and independent validation:

X, Y, Yaw,
SignedLongitudinalVelocity, Speed, VelocityX, VelocityY, YawRate, VehicleCurvature,
TimeFromStart,
SegmentIndex, SegmentLocalS, PathS, Direction, BoundaryType,
LongitudinalAcceleration, LongitudinalJerk

SignedLongitudinalVelocity is authoritative: Speed = abs(signedV), world VelocityX/Y are derived from vehicle yaw, and YawRate = signedV * VehicleCurvature. Pose, world velocity, speed, and curvature stay available in TrajectoryExecutionState.SelectedPoint; they are monitoring telemetry, not controller inputs.

TrajectoryControlAdapter produces only the controller-neutral immutable command below:

SignedLongitudinalVelocity
YawRate
Direction
RequestDirectionChange
HoldBrake
IsTrajectoryComplete

There is intentionally no body-lateral-velocity, crab-motion, in-place-rotation, UI, or hardware field. A later hardware adapter may map this command only after that controller's field semantics are independently confirmed.

Windows x64 plugin package

The build output carries the pinned OSQP runtime and notices. Create the deployable plugin tree from a built managed assembly with an explicit destination that is neither a drive root nor the repository root:

& .\ClumsyPilot\scripts\Publish-ClumsyPilotPlugin.ps1 `
  -ManagedDll .\ClumsyPilot\bin\Debug\netstandard2.0\ClumsyPilot.dll `
  -OutputDirectory C:\deploy\ParkingRobot

The transactional publisher verifies a 64-bit PowerShell host, the OSQP PE machine type, and the pinned ThirdParty/OSQP/SHA256SUMS hash before staging and renaming exactly:

plugins/ClumsyPilot.dll
plugins/osqp.dll
plugins/licenses/OSQP-LICENSE.txt
plugins/licenses/OSQP-NOTICE.txt
plugins/licenses/OSQP-VERSION.txt

Run the complete first-version gate from the repository root:

dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-all

Dynamic-obstacle prediction, time-space occupancy, following/yielding/overtaking behavior, dynamic rerouting, body-lateral motion, in-place rotation, UI integration, and hardware integration are explicitly deferred and are not implemented by this first version.