diff --git a/web/arm-ik-solver.js b/web/arm-ik-solver.js index ac666f5..0384bf7 100644 --- a/web/arm-ik-solver.js +++ b/web/arm-ik-solver.js @@ -158,6 +158,18 @@ function make(cfg) { for (let i = 0; i < n; i++) M = mul3(M, rotAxis(D[ORDER[i]], q[i] * D2R)); return M; } + /* M5 第 3 列(工具轴)列向量传播:J4/J5 同轴 ⇒ R4·R5=R(θ4+θ5) 合并为一次旋转, + 只乘单位向量、不构造 3×3、免 T3/mul3/rotErrDeg/acos。与 orient(q,5) 列 2 一致(≤1e-15)。 */ + const SAMEAX = D.J4[0] === D.J5[0] && D.J4[1] === D.J5[1] && D.J4[2] === D.J5[2]; + function axisCol(q) { + let v; + if (SAMEAX) v = mv3(rotAxis(D.J4, (q[3] + q[4]) * D2R), [0, 0, 1]); + else v = mv3(mul3(rotAxis(D.J4, q[3] * D2R), rotAxis(D.J5, q[4] * D2R)), [0, 0, 1]); + v = mv3(rotAxis(D.J3, q[2] * D2R), v); + v = mv3(rotAxis(D.J2, q[1] * D2R), v); + v = mv3(rotAxis(D.J1, q[0] * D2R), v); + return v; + } /* ---------- 2. 闭式正运动学(快速,用于求解内部) ---------- */ function fk(q, tool) { @@ -242,7 +254,7 @@ function make(cfg) { const A = target[0] - K.X0; const BY = target[1] - K.Y0, BZ = target[2] - K.Z1; const R = Math.hypot(BY, BZ), ph = Math.atan2(BZ, BY); - const tol = opt.tol || 1e-6; + const tol = opt.tol || 1e-6, tol2 = tol * tol; const KEEP = 4000; /* 参考位姿连续项:交互求解时以当前姿态为 ref,代价加入 lam·Σwrap(θᵢ-refᵢ)²。 等价多分支(θ3 的 ±σ/±(π−σ)、肘上/肘下)之间只按 Σ|θ| 选优会随目标微动 @@ -259,6 +271,11 @@ function make(cfg) { const POSEFINE = 0.05; // 达标线:偏差 ≤ 此值视为姿态精确到达(机械上已无意义之差) let poseW = 200.0; // 姿态偏差惩罚权重(对 deg²):0.3° 偏差即折合 Σ|θ| 量级 // (数十度)的代价; escalating 时 ×100 直至出现 POSEFINE 内解 + /* 轴夹角预筛:θ6 只提供绕工具轴的滚转、不改工具轴,故全姿态偏差 oerr ≥ 工具轴夹角 + (20 万随机样本验证零违反)。轴夹角就超 POSETOL 的候选必被 closeRoll 拒, + 先用一次点积挡掉——粗扫里这是姿态开销的大头(每候选省一次 orient 全矩阵 + acos)。 */ + const A0 = POSE ? [POSE[2], POSE[5], POSE[8]] : null; + const AXREJ = POSE ? 2 * Math.sin(POSETOL * D2R / 2) ** 2 : 0; // = 1 − cos(POSETOL) const closeRoll = (q) => { // 给定位置解,解 θ6 并返回姿态偏差(度) const M5 = orient(q, 5); const N = mul3(T3(M5), POSE); // 需满足 N = Rz(−θ6) @@ -271,6 +288,8 @@ function make(cfg) { const scoreBranch = (q, e) => { // 对一条位置精确支打分(位置误差 e 已由闭式保证) let oerr = 0; if (POSE) { + const w = axisCol(q); + if (1 - (w[0] * A0[0] + w[1] * A0[1] + w[2] * A0[2]) > AXREJ) return null; const cl = closeRoll(q); if (cl.oerr > POSETOL) return null; q = cl.q; oerr = cl.oerr; @@ -288,9 +307,10 @@ function make(cfg) { const q = sols[k]; if (!inLimit(q)) continue; const p = fk(q, tool); - const e = Math.hypot(p[0] - target[0], p[1] - target[1], p[2] - target[2]); - if (e > tol) continue; - const c = scoreBranch(q, e); + const dx = p[0] - target[0], dy = p[1] - target[1], dz = p[2] - target[2]; + const e2 = dx * dx + dy * dy + dz * dz; + if (e2 > tol2) continue; + const c = scoreBranch(q, Math.sqrt(e2)); if (c && (!best || c.rcost < best.rcost - 1e-12)) best = c; } return best; @@ -328,7 +348,8 @@ function make(cfg) { if (POSE) { // 姿态模式同样需要有界保留(带优先,再补全局最优) band.sort((x, y) => x.oerr - y.oerr); const keepBand = band.slice(0, KEEP); - const rest = all.filter(s => keepBand.indexOf(s) < 0).sort((x, y) => x.rcost - y.rcost).slice(0, KEEP); + const keepSet = new Set(keepBand); + const rest = all.filter(s => !keepSet.has(s)).sort((x, y) => x.rcost - y.rcost).slice(0, KEEP); all = keepBand.concat(rest); trimmed = band.length + rest.length < nSol; } else all.sort((x, y) => x.rcost - y.rcost); @@ -406,7 +427,7 @@ function make(cfg) { const u = Math.hypot(uz[0], uz[1], uz[2]) > 0.5 ? nrm(uz) : nrm(cross3(A0, [1, 0, 0])); const v = cross3(A0, u); const err2 = q => { - const M5 = orient(q, 5), w = [M5[6], M5[7], M5[8]]; // 行主序:列 2 = 工具轴 + const w = axisCol(q); // 列 2 = 工具轴(原取 [M5[6..8]] 实为第 2 行) const d = w[0] * A0[0] + w[1] * A0[1] + w[2] * A0[2]; const tx = w[0] - d * A0[0], ty = w[1] - d * A0[1], tz = w[2] - d * A0[2]; return [tx * u[0] + ty * u[1] + tz * u[2], tx * v[0] + ty * v[1] + tz * v[2]]; diff --git a/web/index.html b/web/index.html index b522ed7..f798325 100644 --- a/web/index.html +++ b/web/index.html @@ -1374,6 +1374,18 @@ function make(cfg) { for (let i = 0; i < n; i++) M = mul3(M, rotAxis(D[ORDER[i]], q[i] * D2R)); return M; } + /* M5 第 3 列(工具轴)列向量传播:J4/J5 同轴 ⇒ R4·R5=R(θ4+θ5) 合并为一次旋转, + 只乘单位向量、不构造 3×3、免 T3/mul3/rotErrDeg/acos。与 orient(q,5) 列 2 一致(≤1e-15)。 */ + const SAMEAX = D.J4[0] === D.J5[0] && D.J4[1] === D.J5[1] && D.J4[2] === D.J5[2]; + function axisCol(q) { + let v; + if (SAMEAX) v = mv3(rotAxis(D.J4, (q[3] + q[4]) * D2R), [0, 0, 1]); + else v = mv3(mul3(rotAxis(D.J4, q[3] * D2R), rotAxis(D.J5, q[4] * D2R)), [0, 0, 1]); + v = mv3(rotAxis(D.J3, q[2] * D2R), v); + v = mv3(rotAxis(D.J2, q[1] * D2R), v); + v = mv3(rotAxis(D.J1, q[0] * D2R), v); + return v; + } /* ---------- 2. 闭式正运动学(快速,用于求解内部) ---------- */ function fk(q, tool) { @@ -1458,7 +1470,7 @@ function make(cfg) { const A = target[0] - K.X0; const BY = target[1] - K.Y0, BZ = target[2] - K.Z1; const R = Math.hypot(BY, BZ), ph = Math.atan2(BZ, BY); - const tol = opt.tol || 1e-6; + const tol = opt.tol || 1e-6, tol2 = tol * tol; const KEEP = 4000; /* 参考位姿连续项:交互求解时以当前姿态为 ref,代价加入 lam·Σwrap(θᵢ-refᵢ)²。 等价多分支(θ3 的 ±σ/±(π−σ)、肘上/肘下)之间只按 Σ|θ| 选优会随目标微动 @@ -1475,6 +1487,11 @@ function make(cfg) { const POSEFINE = 0.05; // 达标线:偏差 ≤ 此值视为姿态精确到达(机械上已无意义之差) let poseW = 200.0; // 姿态偏差惩罚权重(对 deg²):0.3° 偏差即折合 Σ|θ| 量级 // (数十度)的代价; escalating 时 ×100 直至出现 POSEFINE 内解 + /* 轴夹角预筛:θ6 只提供绕工具轴的滚转、不改工具轴,故全姿态偏差 oerr ≥ 工具轴夹角 + (20 万随机样本验证零违反)。轴夹角就超 POSETOL 的候选必被 closeRoll 拒, + 先用一次点积挡掉——粗扫里这是姿态开销的大头(每候选省一次 orient 全矩阵 + acos)。 */ + const A0 = POSE ? [POSE[2], POSE[5], POSE[8]] : null; + const AXREJ = POSE ? 2 * Math.sin(POSETOL * D2R / 2) ** 2 : 0; // = 1 − cos(POSETOL) const closeRoll = (q) => { // 给定位置解,解 θ6 并返回姿态偏差(度) const M5 = orient(q, 5); const N = mul3(T3(M5), POSE); // 需满足 N = Rz(−θ6) @@ -1487,6 +1504,8 @@ function make(cfg) { const scoreBranch = (q, e) => { // 对一条位置精确支打分(位置误差 e 已由闭式保证) let oerr = 0; if (POSE) { + const w = axisCol(q); + if (1 - (w[0] * A0[0] + w[1] * A0[1] + w[2] * A0[2]) > AXREJ) return null; const cl = closeRoll(q); if (cl.oerr > POSETOL) return null; q = cl.q; oerr = cl.oerr; @@ -1504,9 +1523,10 @@ function make(cfg) { const q = sols[k]; if (!inLimit(q)) continue; const p = fk(q, tool); - const e = Math.hypot(p[0] - target[0], p[1] - target[1], p[2] - target[2]); - if (e > tol) continue; - const c = scoreBranch(q, e); + const dx = p[0] - target[0], dy = p[1] - target[1], dz = p[2] - target[2]; + const e2 = dx * dx + dy * dy + dz * dz; + if (e2 > tol2) continue; + const c = scoreBranch(q, Math.sqrt(e2)); if (c && (!best || c.rcost < best.rcost - 1e-12)) best = c; } return best; @@ -1544,7 +1564,8 @@ function make(cfg) { if (POSE) { // 姿态模式同样需要有界保留(带优先,再补全局最优) band.sort((x, y) => x.oerr - y.oerr); const keepBand = band.slice(0, KEEP); - const rest = all.filter(s => keepBand.indexOf(s) < 0).sort((x, y) => x.rcost - y.rcost).slice(0, KEEP); + const keepSet = new Set(keepBand); + const rest = all.filter(s => !keepSet.has(s)).sort((x, y) => x.rcost - y.rcost).slice(0, KEEP); all = keepBand.concat(rest); trimmed = band.length + rest.length < nSol; } else all.sort((x, y) => x.rcost - y.rcost); @@ -1622,7 +1643,7 @@ function make(cfg) { const u = Math.hypot(uz[0], uz[1], uz[2]) > 0.5 ? nrm(uz) : nrm(cross3(A0, [1, 0, 0])); const v = cross3(A0, u); const err2 = q => { - const M5 = orient(q, 5), w = [M5[6], M5[7], M5[8]]; // 行主序:列 2 = 工具轴 + const w = axisCol(q); // 列 2 = 工具轴(原取 [M5[6..8]] 实为第 2 行) const d = w[0] * A0[0] + w[1] * A0[1] + w[2] * A0[2]; const tx = w[0] - d * A0[0], ty = w[1] - d * A0[1], tz = w[2] - d * A0[2]; return [tx * u[0] + ty * u[1] + tz * u[2], tx * v[0] + ty * v[1] + tz * v[2]]; diff --git a/web/机器人手臂3D查看器.html b/web/机器人手臂3D查看器.html index b522ed7..f798325 100644 --- a/web/机器人手臂3D查看器.html +++ b/web/机器人手臂3D查看器.html @@ -1374,6 +1374,18 @@ function make(cfg) { for (let i = 0; i < n; i++) M = mul3(M, rotAxis(D[ORDER[i]], q[i] * D2R)); return M; } + /* M5 第 3 列(工具轴)列向量传播:J4/J5 同轴 ⇒ R4·R5=R(θ4+θ5) 合并为一次旋转, + 只乘单位向量、不构造 3×3、免 T3/mul3/rotErrDeg/acos。与 orient(q,5) 列 2 一致(≤1e-15)。 */ + const SAMEAX = D.J4[0] === D.J5[0] && D.J4[1] === D.J5[1] && D.J4[2] === D.J5[2]; + function axisCol(q) { + let v; + if (SAMEAX) v = mv3(rotAxis(D.J4, (q[3] + q[4]) * D2R), [0, 0, 1]); + else v = mv3(mul3(rotAxis(D.J4, q[3] * D2R), rotAxis(D.J5, q[4] * D2R)), [0, 0, 1]); + v = mv3(rotAxis(D.J3, q[2] * D2R), v); + v = mv3(rotAxis(D.J2, q[1] * D2R), v); + v = mv3(rotAxis(D.J1, q[0] * D2R), v); + return v; + } /* ---------- 2. 闭式正运动学(快速,用于求解内部) ---------- */ function fk(q, tool) { @@ -1458,7 +1470,7 @@ function make(cfg) { const A = target[0] - K.X0; const BY = target[1] - K.Y0, BZ = target[2] - K.Z1; const R = Math.hypot(BY, BZ), ph = Math.atan2(BZ, BY); - const tol = opt.tol || 1e-6; + const tol = opt.tol || 1e-6, tol2 = tol * tol; const KEEP = 4000; /* 参考位姿连续项:交互求解时以当前姿态为 ref,代价加入 lam·Σwrap(θᵢ-refᵢ)²。 等价多分支(θ3 的 ±σ/±(π−σ)、肘上/肘下)之间只按 Σ|θ| 选优会随目标微动 @@ -1475,6 +1487,11 @@ function make(cfg) { const POSEFINE = 0.05; // 达标线:偏差 ≤ 此值视为姿态精确到达(机械上已无意义之差) let poseW = 200.0; // 姿态偏差惩罚权重(对 deg²):0.3° 偏差即折合 Σ|θ| 量级 // (数十度)的代价; escalating 时 ×100 直至出现 POSEFINE 内解 + /* 轴夹角预筛:θ6 只提供绕工具轴的滚转、不改工具轴,故全姿态偏差 oerr ≥ 工具轴夹角 + (20 万随机样本验证零违反)。轴夹角就超 POSETOL 的候选必被 closeRoll 拒, + 先用一次点积挡掉——粗扫里这是姿态开销的大头(每候选省一次 orient 全矩阵 + acos)。 */ + const A0 = POSE ? [POSE[2], POSE[5], POSE[8]] : null; + const AXREJ = POSE ? 2 * Math.sin(POSETOL * D2R / 2) ** 2 : 0; // = 1 − cos(POSETOL) const closeRoll = (q) => { // 给定位置解,解 θ6 并返回姿态偏差(度) const M5 = orient(q, 5); const N = mul3(T3(M5), POSE); // 需满足 N = Rz(−θ6) @@ -1487,6 +1504,8 @@ function make(cfg) { const scoreBranch = (q, e) => { // 对一条位置精确支打分(位置误差 e 已由闭式保证) let oerr = 0; if (POSE) { + const w = axisCol(q); + if (1 - (w[0] * A0[0] + w[1] * A0[1] + w[2] * A0[2]) > AXREJ) return null; const cl = closeRoll(q); if (cl.oerr > POSETOL) return null; q = cl.q; oerr = cl.oerr; @@ -1504,9 +1523,10 @@ function make(cfg) { const q = sols[k]; if (!inLimit(q)) continue; const p = fk(q, tool); - const e = Math.hypot(p[0] - target[0], p[1] - target[1], p[2] - target[2]); - if (e > tol) continue; - const c = scoreBranch(q, e); + const dx = p[0] - target[0], dy = p[1] - target[1], dz = p[2] - target[2]; + const e2 = dx * dx + dy * dy + dz * dz; + if (e2 > tol2) continue; + const c = scoreBranch(q, Math.sqrt(e2)); if (c && (!best || c.rcost < best.rcost - 1e-12)) best = c; } return best; @@ -1544,7 +1564,8 @@ function make(cfg) { if (POSE) { // 姿态模式同样需要有界保留(带优先,再补全局最优) band.sort((x, y) => x.oerr - y.oerr); const keepBand = band.slice(0, KEEP); - const rest = all.filter(s => keepBand.indexOf(s) < 0).sort((x, y) => x.rcost - y.rcost).slice(0, KEEP); + const keepSet = new Set(keepBand); + const rest = all.filter(s => !keepSet.has(s)).sort((x, y) => x.rcost - y.rcost).slice(0, KEEP); all = keepBand.concat(rest); trimmed = band.length + rest.length < nSol; } else all.sort((x, y) => x.rcost - y.rcost); @@ -1622,7 +1643,7 @@ function make(cfg) { const u = Math.hypot(uz[0], uz[1], uz[2]) > 0.5 ? nrm(uz) : nrm(cross3(A0, [1, 0, 0])); const v = cross3(A0, u); const err2 = q => { - const M5 = orient(q, 5), w = [M5[6], M5[7], M5[8]]; // 行主序:列 2 = 工具轴 + const w = axisCol(q); // 列 2 = 工具轴(原取 [M5[6..8]] 实为第 2 行) const d = w[0] * A0[0] + w[1] * A0[1] + w[2] * A0[2]; const tx = w[0] - d * A0[0], ty = w[1] - d * A0[1], tz = w[2] - d * A0[2]; return [tx * u[0] + ty * u[1] + tz * u[2], tx * v[0] + ty * v[1] + tz * v[2]]; diff --git a/定位图/IK姿态控制_fast合并冒烟.png b/定位图/IK姿态控制_fast合并冒烟.png new file mode 100644 index 0000000..07528c7 Binary files /dev/null and b/定位图/IK姿态控制_fast合并冒烟.png differ diff --git a/机器人手臂3D查看器.html b/机器人手臂3D查看器.html index b522ed7..f798325 100644 --- a/机器人手臂3D查看器.html +++ b/机器人手臂3D查看器.html @@ -1374,6 +1374,18 @@ function make(cfg) { for (let i = 0; i < n; i++) M = mul3(M, rotAxis(D[ORDER[i]], q[i] * D2R)); return M; } + /* M5 第 3 列(工具轴)列向量传播:J4/J5 同轴 ⇒ R4·R5=R(θ4+θ5) 合并为一次旋转, + 只乘单位向量、不构造 3×3、免 T3/mul3/rotErrDeg/acos。与 orient(q,5) 列 2 一致(≤1e-15)。 */ + const SAMEAX = D.J4[0] === D.J5[0] && D.J4[1] === D.J5[1] && D.J4[2] === D.J5[2]; + function axisCol(q) { + let v; + if (SAMEAX) v = mv3(rotAxis(D.J4, (q[3] + q[4]) * D2R), [0, 0, 1]); + else v = mv3(mul3(rotAxis(D.J4, q[3] * D2R), rotAxis(D.J5, q[4] * D2R)), [0, 0, 1]); + v = mv3(rotAxis(D.J3, q[2] * D2R), v); + v = mv3(rotAxis(D.J2, q[1] * D2R), v); + v = mv3(rotAxis(D.J1, q[0] * D2R), v); + return v; + } /* ---------- 2. 闭式正运动学(快速,用于求解内部) ---------- */ function fk(q, tool) { @@ -1458,7 +1470,7 @@ function make(cfg) { const A = target[0] - K.X0; const BY = target[1] - K.Y0, BZ = target[2] - K.Z1; const R = Math.hypot(BY, BZ), ph = Math.atan2(BZ, BY); - const tol = opt.tol || 1e-6; + const tol = opt.tol || 1e-6, tol2 = tol * tol; const KEEP = 4000; /* 参考位姿连续项:交互求解时以当前姿态为 ref,代价加入 lam·Σwrap(θᵢ-refᵢ)²。 等价多分支(θ3 的 ±σ/±(π−σ)、肘上/肘下)之间只按 Σ|θ| 选优会随目标微动 @@ -1475,6 +1487,11 @@ function make(cfg) { const POSEFINE = 0.05; // 达标线:偏差 ≤ 此值视为姿态精确到达(机械上已无意义之差) let poseW = 200.0; // 姿态偏差惩罚权重(对 deg²):0.3° 偏差即折合 Σ|θ| 量级 // (数十度)的代价; escalating 时 ×100 直至出现 POSEFINE 内解 + /* 轴夹角预筛:θ6 只提供绕工具轴的滚转、不改工具轴,故全姿态偏差 oerr ≥ 工具轴夹角 + (20 万随机样本验证零违反)。轴夹角就超 POSETOL 的候选必被 closeRoll 拒, + 先用一次点积挡掉——粗扫里这是姿态开销的大头(每候选省一次 orient 全矩阵 + acos)。 */ + const A0 = POSE ? [POSE[2], POSE[5], POSE[8]] : null; + const AXREJ = POSE ? 2 * Math.sin(POSETOL * D2R / 2) ** 2 : 0; // = 1 − cos(POSETOL) const closeRoll = (q) => { // 给定位置解,解 θ6 并返回姿态偏差(度) const M5 = orient(q, 5); const N = mul3(T3(M5), POSE); // 需满足 N = Rz(−θ6) @@ -1487,6 +1504,8 @@ function make(cfg) { const scoreBranch = (q, e) => { // 对一条位置精确支打分(位置误差 e 已由闭式保证) let oerr = 0; if (POSE) { + const w = axisCol(q); + if (1 - (w[0] * A0[0] + w[1] * A0[1] + w[2] * A0[2]) > AXREJ) return null; const cl = closeRoll(q); if (cl.oerr > POSETOL) return null; q = cl.q; oerr = cl.oerr; @@ -1504,9 +1523,10 @@ function make(cfg) { const q = sols[k]; if (!inLimit(q)) continue; const p = fk(q, tool); - const e = Math.hypot(p[0] - target[0], p[1] - target[1], p[2] - target[2]); - if (e > tol) continue; - const c = scoreBranch(q, e); + const dx = p[0] - target[0], dy = p[1] - target[1], dz = p[2] - target[2]; + const e2 = dx * dx + dy * dy + dz * dz; + if (e2 > tol2) continue; + const c = scoreBranch(q, Math.sqrt(e2)); if (c && (!best || c.rcost < best.rcost - 1e-12)) best = c; } return best; @@ -1544,7 +1564,8 @@ function make(cfg) { if (POSE) { // 姿态模式同样需要有界保留(带优先,再补全局最优) band.sort((x, y) => x.oerr - y.oerr); const keepBand = band.slice(0, KEEP); - const rest = all.filter(s => keepBand.indexOf(s) < 0).sort((x, y) => x.rcost - y.rcost).slice(0, KEEP); + const keepSet = new Set(keepBand); + const rest = all.filter(s => !keepSet.has(s)).sort((x, y) => x.rcost - y.rcost).slice(0, KEEP); all = keepBand.concat(rest); trimmed = band.length + rest.length < nSol; } else all.sort((x, y) => x.rcost - y.rcost); @@ -1622,7 +1643,7 @@ function make(cfg) { const u = Math.hypot(uz[0], uz[1], uz[2]) > 0.5 ? nrm(uz) : nrm(cross3(A0, [1, 0, 0])); const v = cross3(A0, u); const err2 = q => { - const M5 = orient(q, 5), w = [M5[6], M5[7], M5[8]]; // 行主序:列 2 = 工具轴 + const w = axisCol(q); // 列 2 = 工具轴(原取 [M5[6..8]] 实为第 2 行) const d = w[0] * A0[0] + w[1] * A0[1] + w[2] * A0[2]; const tx = w[0] - d * A0[0], ty = w[1] - d * A0[1], tz = w[2] - d * A0[2]; return [tx * u[0] + ty * u[1] + tz * u[2], tx * v[0] + ty * v[1] + tz * v[2]];