Changes

fix: SSEP: 解决终点区域被加入close set导致无法搜到终点的问题

feat: SSEP: IMV新增基于pass point倒车点的多段搜索

feat: 添加车辆最大曲率计算函数，优化运动学处理

fix: 修复 TrajectoryPoints 三处按弧长插值的参数误用

MatchPtByt/MatchPtByS/MatchMultiPtsByMultiS 都把 [0,1] 的比例传给
InterpPtByLinearS, 而后者第三参是绝对弧长, 于是恒命中其 "p0.s() >= s - 0.01"
早退分支 —— 轨迹中段 s 是几十米量级、恒大于比例值, 插值从未真正生效, 取点一直
退化成按离散点向下取整。

- MatchPtByt: 把时间比例换算成对应弧长再传
- MatchPtByS / MatchMultiPtsByMultiS: 本就按 s 查, 直接传 s
- MatchMultiPtsByMultiS: 顺带修解引用 end() 的 UB 与 l/u 写反(该函数无调用方)
- InterpPtByLinearS: z_ 是被重载的字段(存挂车横摆角, 另有 10000 终止哨兵),
  插值真正生效后不能再盲目 lerp —— 任一端越界或两端相差超过 pi 时取下界点

影响面较大: obstacle 预测采样、DP 动态代价、走廊 s<->t 反查、lattice 停车轨迹、
cloud_trajectory 都从"取整"变为"真插值", 数值会有变化, 目前只验证过变道场景。

fix: 修复 PJPO 变道横移走不完与跨两车道路径扭曲

一、变道横移走不完(车挪 1~3m 就停住, 路径起点段变直)
车的横移速率 ≈ init_dl × v(每帧只执行路径最前约 0.5m), 所以起点斜率一旦被压到
0, 路径形状再对车也不动。两处把它压到 0:
- 起点 dl 在变道场景恢复硬约束(新增 soften_init_dl)。dcf2390c1 为治 side-pass
  的起点 kappa kink 把 init_dl 软化, 但变道恰恰需要起点持续保持朝目标的横移
  斜率, 软化后 QP 每帧把它拉回 0(kSoftInitPenalty 相对 l_ref 权重极低, 拉回几乎
  无代价), 实测逐帧 0.113 -> 0.0137 -> 0.00065。
- kInitDlOutwardMax 的"向外"判据改为相对 path_reference_l。原按 init_l 符号判,
  会把 PREPARE 段"正朝目标车道靠拢"误判成向外漂移(实测 ref_l=1.57/init_l=0.82/
  init_dl=0.06), 起点斜率被夹死在 0.05。
另新增 BoostLaneChangeInitDl: QP 每帧的解从起点起 dl 就单调递减, 而车只执行起点
段, 构成一阶惯性环节(指数收敛、尾巴无限长), 只能在入口给定横移速率下限打破。

二、跨两车道路径扭曲(变一道正常, 变两道路径扭、控制跟不住而触发曲率限速)
- 变道 dl 上限放宽到 0.8。单车道 S 曲线峰值仅 0.26, 跨两车道需 ~0.5, 恰好顶死
  ±0.5 上限, QP 在约束边界上滑动导致解非光滑。用同一帧 dump 离线扫描, dl 拐点
  数与触限程度严格对应: 0.3->7, 0.5->3, 0.8->1; 放宽后实际只用到 0.505。
- config_local 的 lane_change_weight.dl_weight 20->5000(不在本仓库, 需单独同步)
  压 dl 峰值治"急": 峰值 0.42->0.30, 拐点 2->0, 收敛距离 18m->44.5m。

三、连带修复
- init_dl 为硬约束时补上夹进 dl 边界。piecewise_jerk_problem 把 dx[0] 上下界同设
  为 init 值(硬等式), 与 dx 的 box 约束矛盾时 OSQP 直接 primal infeasible, 而变道
  关闭了 all_soft 兜底, 会一路掉到 last_path fallback。
- 起点 dl/ddl 增加 Frenet 直传(参考线几何未变时从上一帧解按 s 取), 绕开 Cartesian
  往返; 失败自动回退反算。
- PjpoConstraintOptions 序列化补 enable_all_soft_fallback(版本 2->3)。生产置 false
  而结构体默认 true, 之前从不入档, 离线回放会多挂 all_soft 档致复现不可信。
- 新增离线工具 AnalyzeDlBoundEffect(扫 dl 上限/权重, 打印 opt_dl 曲线与拐点数)与
  SidePassOutwardClampRegression(比对新旧向外判据并校验解是否退化)。

验证: 实车变道正常、曲率连续; side-pass 回归 72 例 0 退化, 其中 1 例判据差异是对
旧判据误夹的纠正(车朝走廊中点走却被当成向外漂移)。

feat: 云端轨迹下发各 lane 的剩余长度与总长度

Traj.msg 的 length_s/total_length_s 与 lane_ids 逐项对齐, 取值来自
第一条引导线的 line_infos: 自车所在 lane 只算未走完的部分, 其后的
lane 取引导线覆盖的全长; 变道备用引导线不下发长度。

feat: HMI 显示 road boundary 的 shift type

在 road boundary 左边界点旁标注每段的 shift type, 类型跳变处各标一个,
同类型连续断面不重复标。

文字画在 cv::flip 之后(flip 前画会上下颠倒), 沿用 DrawReverseApproach
的两段式: Draw 阶段只缓存 UTM 锚点, DrawText 做坐标变换并出字。

fix:修复合并的bug

chore: KEB: 集卡lane follow优化轨迹末状态使用软约束、调整参数

feat: 增加堆高机侧向吊具高度防护

按堆高机与车身的横纵向位置及垂直朝向识别侧向作业场景。

场景内采用可配置的 5.5 米吊具高度阈值，并追加角度、横向距离和纵向容错配置及回放序列化。

feat: 显示障碍物朝向与速度方向

修正速度方向访问器，并为外部聚类障碍物初始化速度方向。

HMI 使用较浅、较短的箭头区分 heading 和速度方向，同时过滤场桥虚拟障碍物及聚类障碍物。

feat: 补充 SL 边界语义访问器

新增左右边界、中心位置及横纵向跨度访问接口，统一屏蔽 start/end 顺序差异。

补充数学函数直接依赖，并整理头文件格式。