Robot_Arm/docs/路径规划优化报告.md

避障路径规划优化报告（2026-10-15，基于 1dfdd48）

现状问题（旧 routeOrVia 阶梯）

1. 穿模：routeAroundPlan 每段只查 ≤8 点（长段采样间隔 15°+），coordinated 二分松弛只查 6 点。 60 个"直线被挡"随机场景里 18 条路径在 1° 细验下实际穿模（13 条为 coordinated）。
2. 怪动作：CRUISE 巡航位形把 J3/J5/J6 强制拉到固定值（0/-60/0），与障碍无关的腕部和大臂自转也被 "归零再转回"；回退绕行（retract_pass）先收臂再伸展，路径绕远。
3. 单目标：绕行只认一个目标解 qT，换个肘/腕构型就能直走的情况被忽略。
4. 动画顿挫：animTo 每个中间位形各占固定 ms 并各自 smoothstep —— 每过一个 via 都减速到 0； 5° 小段与 150° 大段用同样时间。

改动

• ik-solver.js 新增 6c 节：
  • gatherGoals：候选表里端点安全的 IK 解按加权关节距离取前 6 个（同盆地去重）作为目标集；
  • 先试多目标直线（1.5° 细验）→ 再试弦中点 J2/J4 微调单中间点 → 再上 Lazy RRT-Connect （起点树 ↔ 多根目标树；生长时只查节点 + 边中点，连通后整条路径细验，坏边判死子树）；
  • shortcut：随机捷径 + 贪心最远可达 + 逐轴拉直（去掉与障碍无关轴的来回摆）；
  • 加权度量 PW=[1.6,1.4,1,1,0.7,0.5]（近端轴代价高），定种子 LCG，结果可复现；
  • 连续轴段内变化 <179°，保证与查看器短弧插值走的是同一条线；
  • 规划失败时仍回落旧阶梯与 routeVia，最坏行为不劣于旧版。
• 碰撞窄相位前加胶囊级包围球预筛（保守、零漏判）：collideFree 2.9 → 1.8 ms/次， 4000 个随机位形与旧实现判定 0 差异。
• ik-glue.js animTo：各段时长按"最慢轴所需时间"（轴速比 VREL）分配，整条折线一次全局缓入缓出， 中间位形不停顿；总时长按路径长度伸缩。

基准（node tests/route_bench.cjs [solver] 60，同一批 60 个直线被挡场景）

指标	旧	新
1° 细验无穿模的路径	42/60	58/60（剩 2 条最大侵入 0.28/0.08 mm，在 0.3 mm 安全裕度内）
到达且无穿模	37	53
两版都成功的 37 例：平均关节总行程	417°	388°（−7%）
同上：估算执行时间	2.56 s	2.46 s
平均规划耗时（Node 单线程）	0.72 s	1.23 s（全集）/ 0.94 s（37 例）

到不了的 5 例两版相同，属于目标超出安全可达区（IK 残差 4~235 mm），不是规划问题。

已知取舍 / 后续

• 规划耗时上升约 0.3~0.5 s（Worker 里跑，不卡界面）；拖拽实时模式预算 150 ms，超时回落旧阶梯。
• 执行层仍是关节空间折线；若要上真机，下一步应做带加速度约束的时间最优参数化（TOPP）+ 拐角抛物线过渡， 过渡段需重新做碰撞校验。

二期：规划提速（2026-10-15）

逐段计时（20 个被挡场景）：时间主要花在"单中间点微调"（~350 ms/例）、RRT（~280 ms）和平滑（~250 ms 截止）； 每次碰撞判定约 1.5 ms，其中 80% 的线段距离计算来自 (2,4) 类零件对（P05/M3 × P07/M5，长期相距 ~5 mm）。

改动：

1. 两关节零件对类间隙表：(2,4) 类的相对位姿只取决于 J3、J4，离线按 1° 网格生成最小间隙表 （webapp/tools/gen_pair2d.cjs → docs/collide.json 的 pair2d）。运行时取最近网格点，按杠杆界 （J3 3.24 mm/°、J4 2.54 mm/°）给出上下界，能定论就跳过/判撞，否则回退精算。2 万随机姿态与精算判定 0 差异； 单次判定 1.60 → 1.25 ms。指纹校验：重新拟合胶囊后未重生成表会自动弃表（只变慢不出错）。
2. 前移启发：撞上的零件对/簇对移到表头，规划中反复撞同一处时首查即短路。
3. 单次求解内判定记忆：routeFrom / gatherGoals / 规划器对同一批 IK 候选不再重复判定。
4. routeFrom 惰性回退：被挡时先不算"贴边停"位形（findSafeTarget + 二分），规划失败才补算。
5. 微调只试前 2 个目标；平滑 50 次随机捷径 / 160 ms 截止，逐轴拉直 1 轮。

同一批 60 例（node tests/route_bench.cjs <solver> 60）：

指标	一期	二期
平均规划耗时	1239 ms	837 ms（−32%）
到达目标	55	58
1° 细验不穿模	58	58
关节总行程均值	438°	436°
往返次数均值	2.08	1.98

尝试过但未采用：基于间隙 + 杠杆界的保守连续检测。(2,4) 类常年仅 ~5 mm 间隙把步长压到 <1°， 单次间隙查询又比布尔判定贵，实测比 1.5° 离散检测慢 25%。

第三轮：实时版（2026-10-16）

思路

立柱和自身零件都是静态障碍，关节空间的自由区永远不变，所以把"安全路网"离线算好、随碰撞模型发布；运行时只做"接入 + 查图"。

1. 离线路网（PRM）：webapp/tools/gen_roadmap.cjs 均匀采样 2500 个无干涉位形，连 10 近邻，每条边按 1.5° 细验（双线程约 9 分钟）。结果：15103 条有效边、单一连通分量。写入 docs/collide.json 的 roadmap 字段（约 130 KB），按碰撞模型 + 限位指纹校验，不符自动弃用、退回在线 RRT。
   • 重新拟合胶囊或改限位后，按顺序重跑：node webapp/tools/gen_pair2d.cjs，再跑 node webapp/tools/gen_roadmap.cjs（pair2d 生成会截掉其后的 roadmap 字段）。
2. 在线 Lazy A*：起点和各 IK 目标接到 12 近邻，只细验路径上的"接入边"，失败删边重搜；路网内部边零成本。
3. 会话内终身增广：每条细验通过的接入边连同起点/目标并入内存路网；超时也保留已验进度，下一帧接着用（跨帧随时可停）。
4. 拖拽热启动：重规划起点就在上一条已验路径上，"起点 → 段终点"免检；新目标只需验 1~2 条短边。
5. 实时管线 routeRT（拖拽 quick 模式自动启用）：直连 → 热启动 → 路网 A*（40 ms）→ 沿旧路径继续走 → 都不行就停在直线上最后一个安全采样点。不再进入旧的启发式阶梯（仿真里它的段只抽查少量点，会穿模）；松手口径的旧阶梯 / 抬高 via 结果执行前也补了整条 1.5° 细验。
6. 热跟随解撞了时补一次 solveDyn 全量解（同一目标缓存；拖动中 40 mm 内复用附近目标的安全解，目标停下再精解）。
7. 碰撞判定：立柱组（组 0）的胶囊/簇/零件中心变换只算一次永久缓存（4000 个随机位形与上一版判定差异 0）。
8. 前端 ik-glue.js：动画每帧对显示位形做一次碰撞判定（实时碰撞守护，约 1.2 ms/帧，命中即停并亮红标，window.__RT_GUARD() 看统计）；被新规划打断时从运动中接续（只缓出），拖拽跟随不再一顿一顿。

实时拖拽仿真（tests/rt_drag_bench.cjs，12 个横穿立柱场景 × 480 帧 @60 fps）

指标	原版 1dfdd48	上一版 919e704	实时版
重规划耗时 P50	14.9 ms	2.6 ms	5.3 ms
重规划耗时 P90	43.7 ms	14.5 ms	18.2 ms
重规划耗时 P99	464 ms	171 ms	169 ms
穿模帧（1° 细验）	15	430	0
拖完 6 s 内到达	9/12	10/12	10/12

上一版 P50 更低是因为它在拖拽中几乎不绕行（直接停/直走），代价是 430 帧穿模。

松手全量规划（tests/route_bench.cjs，60 场景）

有效路径 60/60（上一版 58），到达 58，平均关节行程 458°（上一版 436°），往返 1.86 次，执行 2.60 s。平均规划耗时 662 ms（与拖拽仿真并行跑测得，偏高）。

剩余问题

• 尾部尖峰（最大约 2 s）来自目标刚进入"热跟随解会撞"区域时的首次全量 IK，以及偶发长接入边细验；下一步可把全量 IK 改为分帧增量。
• 未做真实浏览器交互与实体电机测试。