feat(ik): 避障路径改为多目标 Lazy RRT-Connect + 规划提速 32%

- 避障路径改为多目标 Lazy RRT-Connect + 捷径平滑;碰撞窄相位预测提速。
- 规划提速 32%:(2,4) 零件对类离线间隙表、撞点前移、判定记忆、routeFrom 惰性回退、平滑预算收紧。
This commit is contained in:
Hark
2026-10-18 06:40:22 +08:00
committed by cbb
parent 1dfdd48792
commit 318ad0e264
7 changed files with 541 additions and 21 deletions
+26 -8
View File
@@ -340,7 +340,7 @@ function animTo(q, ms, via) {
end 保存求解器给的代表角,末帧落回它——与插值终点恰好相差 360° 的整数倍,
同一物理角,视觉零跳变,同时保证滑块/关节表数值与求解结果一致。
via:绕行中间位形(solveRoute 判直线被挡时给的两段式路径),逐段同样按短弧折算,
每段各 ms 毫秒——渲染走的段与碰撞校验走的段是同一批,不存在"校验过了但动画穿模"。 */
渲染走的段与碰撞校验走的段是同一批,不存在"校验过了但动画穿模"。 */
const segs = [];
let p = from;
for (const raw of (via || []).concat([q])) {
@@ -352,17 +352,34 @@ function animTo(q, ms, via) {
segs.push(t); p = t;
}
const end = q.slice();
const total = ms * segs.length;
/* 时间参数化(2026-10-15):旧版每段固定 ms 且每段各自 smoothstep —— 每个中间位形都
减速到 0 再起步(顿挫),且 5° 的小段和 150° 的大段用同样时间(小段慢吞吞、大段猛甩)。
现在:各段时长 ∝ 该段"最慢轴所需时间"(按轴速比 VMAX 折算,近端轴更慢),
整条折线只做一次全局 smoothstep:只在起点/终点加减速,途经中间位形不停顿。
走的仍是校验过的那条折线(中间位形经捷径平滑后通常只剩 1~2 个,折角很少)。 */
const VREL = [0.6, 0.6, 0.9, 0.9, 1.2, 1.8];
const pts = [from].concat(segs), dur = [];
for (let i = 1; i < pts.length; i++) {
let t = 0;
for (let j = 0; j < 6; j++) t = Math.max(t, Math.abs(pts[i][j] - pts[i - 1][j]) / VREL[j]);
dur.push(Math.max(t, 1e-3));
}
const sum = dur.reduce((a, b) => 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' ? '避障到达 · 平滑协同跨越立柱'
: '绕行立柱后到达')))