diff --git a/docs/路径规划优化报告.md b/docs/路径规划优化报告.md new file mode 100644 index 0000000..3afd1e5 --- /dev/null +++ b/docs/路径规划优化报告.md @@ -0,0 +1,40 @@ +# 避障路径规划优化报告（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）+ 拐角抛物线过渡， + 过渡段需重新做碰撞校验。 diff --git a/tests/route_bench.cjs b/tests/route_bench.cjs new file mode 100644 index 0000000..c3f5941 --- /dev/null +++ b/tests/route_bench.cjs @@ -0,0 +1,43 @@ +const setup=require('./route_bench_setup.cjs'); +const solverPath=process.argv[2];const N=+process.argv[3]||40;const live=process.argv[4]==='live'; +const {S}=setup(solverPath); +const L=S.LIM,O=S.ORDER,w=S.wrap180; +let s=12345;const rnd=()=>{s=(s*1664525+1013904223)>>>0;return s/4294967296}; +const rq=()=>O.map(id=>{const t=L[id];return t.lo+rnd()*(t.hi-t.lo)}); +const d=(a,b)=>a.map((v,i)=>L[O[i]].cont?w(b[i]-v):b[i]-v); +function segOK(a,b){const dd=d(a,b);const m=Math.max(...dd.map(Math.abs));const n=Math.max(1,Math.ceil(m/1));for(let i=1;i<=n;i++){if(!S.collideFree(a.map((v,k)=>v+dd[k]*i/n)))return false}return true} +// build cases: free start, free goal config, straight blocked +const cases=[];let tries=0; +while(cases.lengthMath.abs(x)/VMAX[k]))+ (pts.length>2&&iL[O[k]].cont?v+w(pts[i][k]-v):pts[i][k]); + let ok=true;for(let i=1;ia+Math.abs(x),0); + const direct=d(c.from,r.q).reduce((a,x)=>a+Math.abs(x),0); + // reversals + let rev=0;for(let k=0;k<6;k++){let sg=0;for(let i=1;i a + b, 0); + /* 总时长：ms 视为"标准 90° 移动"的时长，按路径实际长度伸缩，夹在 [0.6ms, 3.5ms] */ + const total = Math.max(ms * 0.6, Math.min(ms * 3.5, ms * sum / 150)); + const cum = [0]; for (const d of dur) cum.push(cum[cum.length - 1] + d / sum); anim = { from: from, to: segs[segs.length - 1], t0: performance.now(), ms: total }; const tick = () => { if (!anim) return; const el = performance.now() - anim.t0; - const si = Math.min(segs.length - 1, Math.floor(el / ms)); - const k = Math.min(1, (el - si * ms) / ms); - const e = k * k * (3 - 2 * k); - const a0 = si === 0 ? from : segs[si - 1], a1 = segs[si]; + const u = Math.min(1, el / total); + const k = u; + const e = u * u * (3 - 2 * u); // 全局一次缓入缓出 + let si = 0; while (si < dur.length - 1 && e > cum[si + 1]) si++; + const f = Math.min(1, Math.max(0, (e - cum[si]) / (cum[si + 1] - cum[si]))); + const a0 = pts[si], a1 = pts[si + 1]; for (let i = 0; i < 6; i++) { - const id = ORDER[i], a = a0[i] + (a1[i] - a0[i]) * e; + const id = ORDER[i], a = a0[i] + (a1[i] - a0[i]) * f; ang[id] = a; const sl = $('s' + id), v = $('v' + id); if (sl) sl.value = a; @@ -492,7 +509,8 @@ function renderResult(res, src) { : res.blocked ? '路径被立柱挡住 · 已停在边界（目标差 ' + dsc.toFixed(1) + ' mm）' : res.detour - ? (res.detour === 'retract_pass' ? '避障到达 · 模拟人类回退绕过立柱' + ? (res.detour === 'planned' ? '避障到达 · 规划绕行（' + ((res.via || []).length) + ' 个中间位形）' + : res.detour === 'retract_pass' ? '避障到达 · 模拟人类回退绕过立柱' : (res.detour === 'gentle' ? '避障到达 · 微调协同避开立柱' : (res.detour === 'coordinated' ? '避障到达 · 平滑协同跨越立柱' : '绕行立柱后到达'))) diff --git a/webapp/js/ik-solver.js b/webapp/js/ik-solver.js index 9d04d28..b6ef48f 100644 --- a/webapp/js/ik-solver.js +++ b/webapp/js/ik-solver.js @@ -175,6 +175,8 @@ function make(cfg) { const HARD = (COL && COL.hard) || {}; const ADJ = (COL && COL.adj) || {}; // 相邻打印件对的 1D 自由角区间（度） const PART = [], PARTPAIR = []; // 零件级（宽相位）与零件对 + /* 胶囊半长（窄相位前的胶囊级包围球预筛用，2026-10-15） */ + const HLEN = CAPG ? CAPG.map(c => 0.5 * Math.hypot(c.b[0] - c.a[0], c.b[1] - c.a[1], c.b[2] - c.a[2])) : null; if (COL) { const pidx = {}; for (const nm in COL.parts) { @@ -260,7 +262,7 @@ function make(cfg) { const ca = A.clus[ia], cb = B.clus[ib]; const pr = []; for (const i of ca.caps) for (const j of cb.caps) - pr.push([i, j, CAPG[i].r + CAPG[j].r + CAPM]); + pr.push([i, j, CAPG[i].r + CAPG[j].r + CAPM, HLEN[i] + HLEN[j] + CAPG[i].r + CAPG[j].r + CAPM]); if (pr.length) cl.push([ia, ib, pr, ca.rad + cb.rad, ca.rmax + cb.rmax]); } if (cl.length) PARTPAIR.push([A, B, cl, A.rad + B.rad, A.rmax + B.rmax]); @@ -353,6 +355,11 @@ function make(cfg) { for (let n = 0; n < pr.length; n++) { const e1 = xf(pr[n][0]), e2 = xf(pr[n][1]); const need = pr[n][2]; + /* 胶囊级包围球预筛：中点距 > 半长和 + 半径和 + margin 时线段距离必然 ≥ need（保守、无漏判）， + 实测 segSegDistRaw 占碰撞判定 ~45% 耗时，绝大多数胶囊对在这一步就能跳过 */ + const mx = 0.5 * (e1[0] + e1[3] - e2[0] - e2[3]), my = 0.5 * (e1[1] + e1[4] - e2[1] - e2[4]), + mz = 0.5 * (e1[2] + e1[5] - e2[2] - e2[5]), bl = pr[n][3]; + if (mx * mx + my * my + mz * mz > bl * bl) continue; const d = segSegDistRaw(e1[0], e1[1], e1[2], e1[3], e1[4], e1[5], e2[0], e2[1], e2[2], e2[3], e2[4], e2[5]); if (d < need) { @@ -2193,11 +2200,230 @@ function make(cfg) { } return null; } + /* ---------- 6c. 采样式避障规划：多目标 RRT-Connect + 捷径平滑（2026-10-15） ---------- + 取代原先"微调偏移 → 固定巡航位形 → 回退绕行 → 收臂转 J1"的手工阶梯： + · 旧阶梯的 CRUISE 位形把 J3/J5/J6 强制拉回固定值（0/-60/0），哪怕它们和障碍无关， + 所以每次绕行都会出现"腕部无故甩一圈、大臂自转归零再转回去"的怪动作； + · 旧阶梯每段只查 ≤8 个点（长段间隔可达 15°+），且只认一个目标解； + · 找不到就贴边停（blocked），用户看到的是"卡住/抖动"。 + 新做法： + 1) 目标集 = 候选表里所有端点安全的 IK 解（按加权关节距离取前 GOAL_K 个）， + 而不是单一 qT —— 换一个肘/腕构型往往就有直线可走； + 2) 双向 RRT-Connect（起点树 ↔ 多根目标树），关节空间按加权距离扩展， + 连续轴（J1/J3/J6）在起点 ±180° 的展开坐标里采样，天然走短弧； + 3) 边检测按 EDGE_RES 度二分顺序（先中点再细分，撞点通常 1~3 次就被否决）； + 4) 捷径平滑：随机两点直连 + 贪心最远可达，去掉 RRT 的锯齿， + 最终路径通常只剩 1~2 个中间位形，且只动"必须动"的轴； + 5) 定种子 LCG：同一起点/目标结果可复现，不会每次松手走不同的路。 */ + const PW = [1.6, 1.4, 1.0, 1.0, 0.7, 0.5]; // 加权：近端轴惯量大、扫掠体积大，动它代价更高 + const EDGE_RES = 1.5, RRT_STEP = 20, GOAL_K = 6; + function dq(a, b) { // 展开坐标下的差（连续轴取短弧） + const o = new Array(6); + for (let i = 0; i < 6; i++) o[i] = LIM[ORDER[i]].cont ? wrap180(b[i] - a[i]) : b[i] - a[i]; + return o; + } + function wdist(a, b) { // 加权 L2（最近邻 / 代价） + let s = 0; + for (let i = 0; i < 6; i++) { const x = (a[i] - b[i]) * PW[i]; s += x * x; } + return Math.sqrt(s); + } + function pathCost(pts) { let c = 0; for (let i = 1; i < pts.length; i++) c += wdist(pts[i - 1], pts[i]); return c; } + /* 二分顺序的边检测：端点由调用侧保证；先查中点、再 1/4、3/4……撞点早否决 */ + function edgeFree(a, b, res) { + res = res || EDGE_RES; + let m = 0; + for (let i = 0; i < 6; i++) { + const d = Math.abs(b[i] - a[i]); + if (d >= 179 && LIM[ORDER[i]].cont) return false; // 动画按短弧插值：段内连续轴不得 ≥180° + m = Math.max(m, d); + } + const n = Math.max(1, Math.ceil(m / res)); + if (n <= 1) return true; + const q = new Array(6), Q = [[0, n]]; + for (let h = 0; h < Q.length; h++) { // 广度优先二分：中点 → 四分点 → … + const [lo, hi] = Q[h]; + if (hi - lo < 2) continue; + const k = (lo + hi) >> 1, t = k / n; + for (let i = 0; i < 6; i++) q[i] = a[i] + (b[i] - a[i]) * t; + if (!collideFree(q)) return false; + Q.push([lo, k], [k, hi]); + } + return true; + } + /* 把 q 展开到离 ref 最近的等价角（连续轴），供规划在连续坐标里工作 */ + function unwrapTo(ref, q) { return q.map((v, i) => LIM[ORDER[i]].cont ? ref[i] + wrap180(v - ref[i]) : v); } + function inBox(q) { + for (let i = 0; i < 6; i++) { const t = LIM[ORDER[i]]; if (!t.cont && (q[i] < t.lo - 1e-6 || q[i] > t.hi + 1e-6)) return false; } + return true; + } + function gatherGoals(cur, from) { + const seen = new Set(), out = []; + const add = q => { + if (!q || q.length !== 6) return; + const u = unwrapTo(from, q); + const key = u.map(v => Math.round(v)).join(','); + if (seen.has(key)) return; + seen.add(key); out.push(u); + }; + add(cur.q); + const L = cur.list || []; + for (let i = 0; i < Math.min(L.length, 160); i++) add(L[i] && L[i].q); + out.sort((a, b) => wdist(from, a) - wdist(from, b)); + const goals = []; + for (const g of out) { + if (goals.length >= GOAL_K) break; + if (goals.some(h => wdist(g, h) < 8)) continue; // 同一盆地只留一个 + if (collideFree(g)) goals.push(g); + } + return goals; + } + function shortcut(pts, deadline, rnd) { + pts = pts.map(p => p.slice()); + /* 随机捷径：在两段上各取一点直连 */ + for (let it = 0; it < 80 && pts.length > 2 && nowMs() < deadline; it++) { + const i = Math.floor(rnd() * (pts.length - 1)), j = Math.floor(rnd() * (pts.length - 1)); + if (Math.abs(i - j) < 1) continue; + const a = Math.min(i, j), b = Math.max(i, j); + const ta = rnd(), tb = rnd(); + const pa = pts[a].map((v, k) => v + (pts[a + 1][k] - v) * ta); + const pb = pts[b].map((v, k) => v + (pts[b + 1][k] - v) * tb); + if (wdist(pa, pb) >= pathCost(pts.slice(a, b + 2).concat()) - 1e-6) continue; + if (!edgeFree(pa, pb)) continue; + pts = pts.slice(0, a + 1).concat([pa, pb], pts.slice(b + 1)); + } + /* 贪心：从当前点直连最远可达点，删掉多余中间位形 */ + const out = [pts[0]]; + let i = 0; + while (i < pts.length - 1) { + let j = pts.length - 1; + while (j > i + 1 && !edgeFree(pts[i], pts[j])) j--; + out.push(pts[j]); i = j; + } + /* 逐轴拉直：中间位形的每个轴若能取"前后两点按段长插值"的值且两侧边仍无碰撞，就拉直它。 + 去掉 RRT 留下的"某轴来回摆"（与障碍无关的轴不该动），这正是视觉上"怪动作"的来源。 */ + for (let pass = 0; pass < 2 && out.length > 2; pass++) { + for (let i = 1; i < out.length - 1 && nowMs() < deadline; i++) { + const a = out[i - 1], b = out[i + 1]; + const la = wdist(a, out[i]), lb = wdist(out[i], b), t = la / (la + lb || 1); + for (let k = 0; k < 6; k++) { + const v = a[k] + (b[k] - a[k]) * t; + if (Math.abs(v - out[i][k]) < 0.5) continue; + const q = out[i].slice(); q[k] = v; + if (collideFree(q) && edgeFree(a, q) && edgeFree(q, b)) out[i] = q; + } + } + } + return out; + } + /* Lazy RRT-Connect：生长时只查新节点本身 + 边中点（2 次判定），连通后再对整条路径 + 逐边 2° 细验；不通的边把子节点判死（连同其子树），继续生长。碰撞判定是唯一的大头 + （~3 ms/次），惰性验证把每次迭代从 ~25 ms 降到 ~6 ms。 */ + function rrtConnect(from, goals, budgetMs, seed) { + const t0 = nowMs(), deadline = t0 + budgetMs; + let s = seed >>> 0 || 1; + const rnd = () => { s = (s * 1664525 + 1013904223) >>> 0; return s / 4294967296; }; + const A = [{ q: from.slice(), p: -1, ok: true, dead: false }]; + const B = goals.map(g => ({ q: g.slice(), p: -1, ok: true, dead: false })); + const alive = (T, i) => { while (i >= 0) { if (T[i].dead) return false; i = T[i].p; } return true; }; + const nearest = (T, q) => { + let bi = -1, bd = Infinity; + for (let i = 0; i < T.length; i++) { const d = wdist(T[i].q, q); if (d < bd && alive(T, i)) { bd = d; bi = i; } } + return bi; + }; + const steer = (a, b) => { + let m = 0; for (let i = 0; i < 6; i++) m = Math.max(m, Math.abs(b[i] - a[i])); + if (m <= RRT_STEP) return b.slice(); + const k = RRT_STEP / m; return a.map((v, i) => v + (b[i] - v) * k); + }; + const extend = (T, q) => { // 0=trapped 1=advanced 2=reached + const ni = nearest(T, q); + if (ni < 0) return [0, -1]; + const qn = steer(T[ni].q, q); + if (!inBox(qn) || !collideFree(qn)) return [0, -1]; + const mid = T[ni].q.map((v, i) => 0.5 * (v + qn[i])); + if (!collideFree(mid)) return [0, -1]; + T.push({ q: qn, p: ni, ok: false, dead: false }); // ok=false：边尚未细验 + return [wdist(qn, q) < 1e-6 ? 2 : 1, T.length - 1]; + }; + /* 细验一条树枝（叶→根），返回 true=全通；失败则判死第一条坏边的子端 */ + const verify = (T, i) => { + while (i >= 0 && T[i].p >= 0) { + const n = T[i]; + if (!n.ok) { + if (!edgeFree(T[n.p].q, n.q)) { n.dead = true; return false; } + n.ok = true; + } + i = n.p; + } + return true; + }; + const trace = (T, i) => { const o = []; while (i >= 0) { o.push(T[i].q); i = T[i].p; } return o; }; + let Ta = A, Tb = B, swapped = false, it = 0; + while (nowMs() < deadline) { + it++; + let qr; + if (rnd() < 0.1) qr = Tb[Math.floor(rnd() * Tb.length)].q.slice(); + else qr = ORDER.map((id, i) => { const t = LIM[id]; return t.cont ? from[i] - 180 + 360 * rnd() : t.lo + (t.hi - t.lo) * rnd(); }); + const [st, ia] = extend(Ta, qr); + if (st) { + const qa = Ta[ia].q; + let r = [1, -1]; + while (r[0] === 1) r = extend(Tb, qa); + if (r[0] === 2) { + /* 连接点在两棵树里是同一位形：两侧树枝都细验通过才算路径 */ + const okA = verify(Ta, ia), okB = okA && verify(Tb, r[1]); + if (okA && okB) { + const pa = trace(Ta, ia), pb = trace(Tb, r[1]); + let path = pa.reverse().concat(pb.slice(1)); + if (swapped) path = path.reverse(); + return { path: path, iters: it, ms: nowMs() - t0, rnd: rnd }; + } + } + } + const t = Ta; Ta = Tb; Tb = t; swapped = !swapped; + if (Ta.length + Tb.length > 6000) break; + } + return null; + } + function planAround(from, cur, quick) { + const goals = gatherGoals(cur, from); + if (!goals.length) return null; + const budget = quick ? 150 : 1500; + /* 先试直线（多目标、2° 细验）：换个构型能直走就不绕 */ + for (const g of goals) if (edgeFree(from, g)) return { goal: g, via: [], detour: null }; + /* 再试"单中间点微调"：弦中点抬肩 / 收肘（只动 J2/J4，其余轴保持直线插值），2° 细验。 + 贴边擦碰的大多数场景这里就能解决，动作最接近直线、最像人。 */ + for (const g of goals.slice(0, 3)) { + const mid = from.map((v, i) => 0.5 * (v + g[i])); + for (const [d2, d4] of GENTLE_OFFSETS) { + const m = mid.slice(); m[1] += d2; m[3] += d4; + if (!inBox(m) || !collideFree(m)) continue; + if (edgeFree(from, m) && edgeFree(m, g)) return { goal: g, via: [m], detour: 'planned' }; + } + } + let seed = 2166136261; + for (const v of from.concat(goals[0])) seed = Math.imul(seed ^ Math.round(v * 10), 16777619); + const r = rrtConnect(from, goals, budget, seed); + if (!r) return null; + const sm = shortcut(r.path, nowMs() + (quick ? 40 : 250), r.rnd); + const goal = sm[sm.length - 1]; + return { goal: goal, via: sm.slice(1, -1), detour: 'planned', iters: r.iters, ms: r.ms }; + } function routeOrVia(from, target, cur, opt) { const r = routeFrom(from, cur); if (!r.blocked || !from) return r; - const around = opt.noAround ? null : routeAroundPlan(from, cur, opt.quick, r.safeT); - if (around) return Object.assign({}, around.res, { via: around.via, routed: true, blocked: false, detour: around.detour || 'around' }); + /* 首选：采样规划（多目标 + 平滑），给出的路径已按 EDGE_RES 细验 */ + if (!opt.noAround) { + const pl = planAround(from, cur, opt.quick); + if (pl) { + const res = Object.assign({}, cur, { q: pl.goal.map((v, i) => LIM[ORDER[i]].cont ? wrap180(v) : v), routed: true, blocked: false, kept: false }); + if (pl.via.length) Object.assign(res, { via: pl.via, detour: pl.detour, planIters: pl.iters, planMs: pl.ms }); + return res; + } + /* 规划预算内未连通（极少）：退回旧的启发式阶梯 */ + const around = routeAroundPlan(from, cur, opt.quick, r.safeT); + if (around) return Object.assign({}, around.res, { via: around.via, routed: true, blocked: false, detour: around.detour || 'around' }); + } if (opt.noVia) return r; return routeVia(from, target, opt) || r; } @@ -2313,7 +2539,7 @@ function make(cfg) { } return { fk: fk, fkChain: fkChain, solve: solve, solveTo: solveTo, solveWarm: solveWarm, solveDyn: solveDyn, - solveRoute: solveRoute, routeFrom: routeFrom, transitPt: transitPt, + solveRoute: solveRoute, routeFrom: routeFrom, transitPt: transitPt, planAround: planAround, edgeFree: edgeFree, mixSolve: mixSolve, closest: closest, closest6: closest6, closestOri: closestOri, orient: orient, branch: branch, selfTest: selfTest, poseSelfTest: poseSelfTest, collideFree: collideFree, collideDetail: collideDetail,