feat(ik): 避障路径改为多目标 Lazy RRT-Connect + 规划提速 32%
- 避障路径改为多目标 Lazy RRT-Connect + 捷径平滑;碰撞窄相位预测提速。 - 规划提速 32%:(2,4) 零件对类离线间隙表、撞点前移、判定记忆、routeFrom 惰性回退、平滑预算收紧。
This commit is contained in:
+4
-2
File diff suppressed because one or more lines are too long
@@ -0,0 +1,68 @@
|
||||
# 避障路径规划优化报告(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%。
|
||||
Reference in New Issue
Block a user