386 lines
16 KiB
JavaScript
386 lines
16 KiB
JavaScript
/* ============================================================================
|
||
* arm-ik-solver.js — 六轴手臂逆运动学求解器（最小总关节角）
|
||
*
|
||
* 正运动学：World = A1(θ1)·A2(θ2)·A3(θ3)·A4(θ4)·A5(θ5)·A6(θ6)·TCP，
|
||
* Ai(θ) = T(Pi)·R(axis_i, θ)·T(−Pi)，与查看器场景图完全一致。
|
||
* 逆运动学：以 (θ4,θ5) 为二维冗余参数全域扫描，其余三轴闭式求解。
|
||
*
|
||
* MIT / 无依赖。可在浏览器（window.ArmIK）或 Node（module.exports）中加载。
|
||
* ==========================================================================*/
|
||
(function (root, factory) {
|
||
if (typeof module === 'object' && module.exports) module.exports = factory();
|
||
else root.ArmIK = factory();
|
||
}(typeof self !== 'undefined' ? self : this, function () {
|
||
'use strict';
|
||
|
||
const D2R = Math.PI / 180, R2D = 180 / Math.PI;
|
||
const ORDER = ['J1', 'J2', 'J3', 'J4', 'J5', 'J6'];
|
||
|
||
/* ---------- 小工具 ---------- */
|
||
const wrap180 = a => { a = (a + 180) % 360; if (a < 0) a += 360; return a - 180; };
|
||
const nrm = v => { const n = Math.hypot(v[0], v[1], v[2]) || 1; return [v[0] / n, v[1] / n, v[2] / n]; };
|
||
function rotAxis(d, a) { // 3x3 旋转：绕单位轴 d 转 a 弧度（右手法）
|
||
const x = d[0], y = d[1], z = d[2], c = Math.cos(a), s = Math.sin(a), C = 1 - c;
|
||
return [
|
||
x * x * C + c, x * y * C - z * s, x * z * C + y * s,
|
||
y * x * C + z * s, y * y * C + c, y * z * C - x * s,
|
||
z * x * C - y * s, z * y * C + x * s, z * z * C + c
|
||
];
|
||
}
|
||
const mul3 = (A, B) => {
|
||
const C = new Array(9);
|
||
for (let i = 0; i < 3; i++) for (let j = 0; j < 3; j++)
|
||
C[i * 3 + j] = A[i * 3] * B[j] + A[i * 3 + 1] * B[3 + j] + A[i * 3 + 2] * B[6 + j];
|
||
return C;
|
||
};
|
||
const mv3 = (A, v) => [
|
||
A[0] * v[0] + A[1] * v[1] + A[2] * v[2],
|
||
A[3] * v[0] + A[4] * v[1] + A[5] * v[2],
|
||
A[6] * v[0] + A[7] * v[1] + A[8] * v[2]
|
||
];
|
||
|
||
/* ==========================================================================*/
|
||
function make(cfg) {
|
||
/* ---------- 0. 关节表 → 运动学常量 ---------- */
|
||
const JJ = cfg.joints, TR = cfg.travel || {};
|
||
const P = {}, D = {};
|
||
for (const id of ORDER) {
|
||
if (!JJ[id]) throw new Error('缺少关节 ' + id);
|
||
P[id] = JJ[id].rear.slice();
|
||
D[id] = nrm(JJ[id].d);
|
||
}
|
||
const LIM = {};
|
||
for (const id of ORDER) {
|
||
const t = TR[id] || { lo: -180, hi: 180, continuous: true };
|
||
LIM[id] = { lo: +t.lo, hi: +t.hi, cont: !!t.continuous };
|
||
}
|
||
const TCP0 = (cfg.tcp || [18.5, 128, 93]).slice();
|
||
const ax = D.J6; // 工具沿 J6 轴向外伸
|
||
const K = {
|
||
X0: P.J3[0], Y0: P.J6[1],
|
||
Z1: P.J1[2], Z2: P.J2[2], Z3: P.J3[2], Z4: P.J4[2], Z5: P.J5[2],
|
||
TCP0: TCP0, ax: ax
|
||
};
|
||
K.C0 = K.Z2 - K.Z1;
|
||
K.A43 = K.Z4 - K.Z3;
|
||
K.A32 = K.Z3 - K.Z2;
|
||
K.A54 = K.Z5 - K.Z4;
|
||
K.L5 = K.Z5 - TCP0[2]; // J5 轴 → TCP（沿 −Z）
|
||
/* 结构前提断言：J2/J4/J5 同向平行；J1 轴为 X；J3/J6 轴为 Z；TCP 落在 J6 轴线上 */
|
||
const AXP = v => Math.abs(Math.abs(v[2]) - 1) < 1e-9;
|
||
const WARN = [];
|
||
if (Math.abs(D.J1[0]) < 1e-9 || Math.abs(Math.abs(D.J1[0]) - 1) > 1e-9) WARN.push('J1 轴不是 X');
|
||
for (const id of ['J2', 'J4', 'J5']) if (Math.abs(Math.abs(D[id][1]) - 1) > 1e-9) WARN.push(id + ' 轴不是 Y');
|
||
if (!AXP(D.J3)) WARN.push('J3 轴不是 Z');
|
||
if (!AXP(D.J6)) WARN.push('J6 轴不是 Z');
|
||
if (Math.abs(P.J3[0] - P.J2[0]) > 1e-9 || Math.abs(P.J4[0] - P.J2[0]) > 1e-9 || Math.abs(P.J5[0] - P.J2[0]) > 1e-9)
|
||
WARN.push('J2/J4/J5 不在同一 x 平面');
|
||
if (Math.abs(TCP0[0] - P.J6[0]) > 1e-9 || Math.abs(TCP0[1] - P.J6[1]) > 1e-9)
|
||
WARN.push('TCP 不在 J6 轴线上（θ6 将影响位置）');
|
||
|
||
/* ---------- 1. 严格矩阵链正运动学（对照基准） ---------- */
|
||
function mul4(A, B) {
|
||
const C = new Array(16);
|
||
for (let i = 0; i < 4; i++) for (let j = 0; j < 4; j++) {
|
||
let s = 0;
|
||
for (let k = 0; k < 4; k++) s += A[i * 4 + k] * B[k * 4 + j];
|
||
C[i * 4 + j] = s;
|
||
}
|
||
return C;
|
||
}
|
||
function mv4(M, v) {
|
||
return [0, 1, 2, 3].map(i => M[i * 4] * v[0] + M[i * 4 + 1] * v[1] + M[i * 4 + 2] * v[2] + M[i * 4 + 3] * v[3]);
|
||
}
|
||
|
||
/* 标准 4x4 同质链 World = A1·A2·…·A6·TCP，Ai = T(Pi)·R(轴,θ)·T(−Pi)
|
||
—— 与查看器场景图（逐级左乘的 matrix 局部变换）逐位等价 */
|
||
function fkChain(q, tool) {
|
||
const tcp = [TCP0[0], TCP0[1], TCP0[2] + (+tool || 0) * ax[2]];
|
||
let M = [1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1];
|
||
for (let i = 0; i < 6; i++) {
|
||
const id = ORDER[i], p = P[id], R = rotAxis(D[id], q[i] * D2R);
|
||
const rp = mv3(R, p); // Ai 的旋转=R，平移=p−R·p
|
||
const A = [R[0], R[1], R[2], p[0] - rp[0],
|
||
R[3], R[4], R[5], p[1] - rp[1],
|
||
R[6], R[7], R[8], p[2] - rp[2], 0, 0, 0, 1];
|
||
M = mul4(M, A);
|
||
}
|
||
const w = mv4(M, [tcp[0], tcp[1], tcp[2], 1]);
|
||
return [w[0], w[1], w[2]];
|
||
}
|
||
|
||
/* ---------- 2. 闭式正运动学（快速，用于求解内部） ---------- */
|
||
function fk(q, tool) {
|
||
const L = K.L5 + (+tool || 0);
|
||
const s1 = Math.sin(q[0] * D2R), c1 = Math.cos(q[0] * D2R);
|
||
const s2 = Math.sin(q[1] * D2R), c2 = Math.cos(q[1] * D2R);
|
||
const s3 = Math.sin(q[2] * D2R), c3 = Math.cos(q[2] * D2R);
|
||
const s4 = Math.sin(q[3] * D2R), c4 = Math.cos(q[3] * D2R);
|
||
const s5 = Math.sin(q[4] * D2R), c5 = Math.cos(q[4] * D2R);
|
||
const u = -L * s5, w = K.A54 - L * c5;
|
||
const dx = u * c4 + w * s4;
|
||
const dz = K.A43 - u * s4 + w * c4;
|
||
const Vx = dx, Vz = dz + K.A32;
|
||
const b = -Vx * s3;
|
||
const c = K.C0 + (-Vx * c3 * s2 + Vz * c2);
|
||
const a = Vx * c3 * c2 + Vz * s2;
|
||
return [K.X0 + a, K.Y0 + b * c1 - c * s1, K.Z1 + b * s1 + c * c1];
|
||
}
|
||
|
||
/* ---------- 3. 逆解：给定 (θ4,θ5) 的闭式分支 ---------- */
|
||
function branch(th4, th5, A, R, ph, tool) {
|
||
const L = K.L5 + (+tool || 0);
|
||
const s5 = Math.sin(th5 * D2R), c5 = Math.cos(th5 * D2R);
|
||
const s4 = Math.sin(th4 * D2R), c4 = Math.cos(th4 * D2R);
|
||
const u = -L * s5, w = K.A54 - L * c5;
|
||
const dx = u * c4 + w * s4;
|
||
const dz = K.A43 - u * s4 + w * c4;
|
||
const Vx = dx, Vz = dz + K.A32;
|
||
const out = [];
|
||
const n = (R * R + A * A - Vx * Vx - Vz * Vz - K.C0 * K.C0) / (2 * K.C0);
|
||
const cc = K.C0 + n;
|
||
if (Math.abs(Vx) < 1e-7) { // 退化：θ3 不影响 TCP
|
||
if (Math.abs(Vz) < 1e-9) return out;
|
||
const s2 = A / Vz, c2 = n / Vz;
|
||
if (Math.abs(s2 * s2 + c2 * c2 - 1) > 1e-7) return out;
|
||
const th1 = (Math.abs(cc) > 1e-12) ? (ph - Math.atan2(cc, 0)) * R2D : 0;
|
||
out.push([wrap180(th1), wrap180(Math.atan2(s2, c2) * R2D), 0, th4, th5, 0]);
|
||
return out;
|
||
}
|
||
const m2 = (n * n + A * A - Vz * Vz) / (Vx * Vx);
|
||
if (m2 < -1e-9 || m2 > 1 + 1e-9) return out;
|
||
const sig = Math.acos(Math.sqrt(Math.max(0, Math.min(1, m2))));
|
||
const TH = [sig, -sig, Math.PI - sig, -(Math.PI - sig)];
|
||
for (let k = 0; k < 4; k++) {
|
||
const th3 = TH[k];
|
||
const kap = Vx * Math.cos(th3), lam = Vz;
|
||
const det = kap * kap + lam * lam;
|
||
if (det < 1e-12) continue;
|
||
const th2 = Math.atan2((lam * A - kap * n) / det, (kap * A + lam * n) / det) * R2D;
|
||
const b = -Vx * Math.sin(th3);
|
||
const th1 = (Math.abs(b) + Math.abs(cc) > 1e-12) ? (ph - Math.atan2(cc, b)) * R2D : 0;
|
||
out.push([wrap180(th1), wrap180(th2), wrap180(th3 * R2D), th4, th5, 0]);
|
||
}
|
||
return out;
|
||
}
|
||
|
||
const inLimit = q => {
|
||
for (let i = 0; i < 5; i++) {
|
||
const t = LIM[ORDER[i]];
|
||
if (q[i] < t.lo - 1e-9 || q[i] > t.hi + 1e-9) return false;
|
||
}
|
||
return true;
|
||
};
|
||
const costOf = (q, W) => {
|
||
let c = 0; for (let i = 0; i < 6; i++) c += Math.abs(q[i]) * (W ? W[i] : 1);
|
||
return c;
|
||
};
|
||
const nowMs = () => (typeof performance !== 'undefined' ? performance.now() : Date.now());
|
||
|
||
/* ---------- 4. 主求解入口 ---------- */
|
||
function solve(target, opt) {
|
||
opt = opt || {};
|
||
const t0 = nowMs();
|
||
const tool = +opt.tool || 0;
|
||
const W = opt.weights || [1, 1, 1, 1, 1, 1];
|
||
const h = Math.max(0.02, +(opt.step || 0.25));
|
||
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 KEEP = 4000;
|
||
|
||
const evalAt = (t4, t5) => {
|
||
let best = null;
|
||
const sols = branch(t4, t5, A, R, ph, tool);
|
||
for (let k = 0; k < sols.length; k++) {
|
||
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 = costOf(q, W);
|
||
if (!best || c < best.cost) best = { q: q, cost: c, err: e };
|
||
}
|
||
return best;
|
||
};
|
||
|
||
/* 采样轴：含上下限位端点，避免恰在限位上的最优解被漏掉 */
|
||
const axis = (lo, hi, step) => {
|
||
const v = [];
|
||
const n = Math.floor((hi - lo) / step + 1e-9);
|
||
for (let i = 0; i <= n; i++) v.push(lo + i * step);
|
||
if (v[v.length - 1] < hi - 1e-9) v.push(hi);
|
||
return v;
|
||
};
|
||
const L4 = LIM.J4, L5 = LIM.J5;
|
||
const g4 = axis(L4.lo, L4.hi, h), g5 = axis(L5.lo, L5.hi, h);
|
||
|
||
/* --- 4.1 全域粗扫：覆盖整个二维冗余 (J4,J5) --- */
|
||
let all = [], samples = 0, nSol = 0, trimmed = false;
|
||
for (let i = 0; i < g4.length; i++) {
|
||
for (let j = 0; j < g5.length; j++) {
|
||
samples++;
|
||
const s = evalAt(g4[i], g5[j]);
|
||
if (s) { all.push(s); nSol++; }
|
||
}
|
||
if (all.length > KEEP * 12) { // 有界保留：只留代价最优者
|
||
all.sort((x, y) => x.cost - y.cost);
|
||
all.length = KEEP;
|
||
trimmed = true;
|
||
}
|
||
}
|
||
all.sort((x, y) => x.cost - y.cost);
|
||
|
||
/* --- 4.2 多起点局部加密 + 模式搜索 --- */
|
||
const seeds = [];
|
||
for (let i = 0; i < all.length && seeds.length < 32; i++) {
|
||
const s = all[i];
|
||
let dup = false;
|
||
for (const t of seeds) if (Math.hypot(t.q[3] - s.q[3], t.q[4] - s.q[4]) <= h * 1.5) { dup = true; break; }
|
||
if (!dup) seeds.push(s);
|
||
}
|
||
if (opt.refine !== false) {
|
||
const cl = (v, a, b) => v < a ? a : (v > b ? b : v);
|
||
for (const s of seeds) { // 局部加密网格（h/8，±2h 窗口）
|
||
const hh = h / 8;
|
||
const w4 = axis(Math.max(L4.lo, s.q[3] - 2 * h), Math.min(L4.hi, s.q[3] + 2 * h), hh);
|
||
const w5 = axis(Math.max(L5.lo, s.q[4] - 2 * h), Math.min(L5.hi, s.q[4] + 2 * h), hh);
|
||
let best = s;
|
||
for (const a of w4) for (const b of w5) {
|
||
samples++;
|
||
const c = evalAt(a, b);
|
||
if (c) { nSol++; if (c.cost < best.cost) best = c; }
|
||
}
|
||
all.push(best);
|
||
}
|
||
all.sort((x, y) => x.cost - y.cost);
|
||
const NB = [[1,0],[-1,0],[0,1],[0,-1],[1,1],[1,-1],[-1,1],[-1,-1]];
|
||
for (const s of all.slice(0, 24)) { // 模式搜索
|
||
let c4 = s.q[3], c5 = s.q[4], best = s, step = h / 4, guard = 0;
|
||
while (step > 2e-4 && guard++ < 400) {
|
||
let moved = false;
|
||
for (const d of NB) {
|
||
const t4 = cl(c4 + d[0] * step, L4.lo, L4.hi), t5 = cl(c5 + d[1] * step, L5.lo, L5.hi);
|
||
if (t4 === c4 && t5 === c5) continue;
|
||
samples++;
|
||
const c = evalAt(t4, t5);
|
||
if (c && c.cost < best.cost - 1e-10) { best = c; c4 = t4; c5 = t5; moved = true; }
|
||
}
|
||
if (!moved) step *= 0.5;
|
||
}
|
||
all.push(best);
|
||
}
|
||
}
|
||
all.sort((x, y) => x.cost - y.cost);
|
||
const best = all.length ? all[0] : null;
|
||
return {
|
||
ok: !!best, reachable: !!best, target: target.slice(), tool: tool,
|
||
q: best ? best.q.slice() : null, cost: best ? best.cost : null,
|
||
err: best ? best.err : null, nSol: nSol, truncated: trimmed,
|
||
samples: samples, ms: nowMs() - t0, list: all.slice(0, KEEP)
|
||
};
|
||
}
|
||
|
||
/* ---------- 5. 最近可达位姿（阻尼最小二乘，兜底用） ---------- */
|
||
function closest(target, tool, tries) {
|
||
const dp = 0.05, lam = 4.0;
|
||
let bestQ = null, bestD = Infinity;
|
||
const run = q => {
|
||
const J = new Float64Array(18);
|
||
for (let it = 0; it < 160; it++) {
|
||
const p = fk(q, tool);
|
||
const d = Math.hypot(p[0] - target[0], p[1] - target[1], p[2] - target[2]);
|
||
if (d < bestD) { bestD = d; bestQ = q.slice(); }
|
||
if (d < 1e-9) break;
|
||
for (let c = 0; c < 6; c++) {
|
||
const s = q.slice(); s[c] += dp;
|
||
const pp = fk(s, tool);
|
||
J[c * 3] = (pp[0] - p[0]) / dp; J[c * 3 + 1] = (pp[1] - p[1]) / dp; J[c * 3 + 2] = (pp[2] - p[2]) / dp;
|
||
}
|
||
let m00 = lam, m01 = 0, m02 = 0, m11 = lam, m12 = 0, m22 = lam;
|
||
for (let c = 0; c < 6; c++) {
|
||
const ax2 = J[c * 3], ay = J[c * 3 + 1], az2 = J[c * 3 + 2];
|
||
m00 += ax2 * ax2; m01 += ax2 * ay; m02 += ax2 * az2;
|
||
m11 += ay * ay; m12 += ay * az2; m22 += az2 * az2;
|
||
}
|
||
const det = m00 * (m11 * m22 - m12 * m12) - m01 * (m01 * m22 - m12 * m02) + m02 * (m01 * m12 - m11 * m02);
|
||
if (Math.abs(det) < 1e-12) break;
|
||
const i00 = (m11 * m22 - m12 * m12) / det, i01 = -(m01 * m22 - m12 * m02) / det,
|
||
i02 = (m01 * m12 - m11 * m02) / det, i11 = (m00 * m22 - m02 * m02) / det,
|
||
i12 = -(m00 * m12 - m01 * m02) / det, i22 = (m00 * m11 - m01 * m01) / det;
|
||
const ex = p[0] - target[0], ey = p[1] - target[1], ez = p[2] - target[2];
|
||
const y0 = -(i00 * ex + i01 * ey + i02 * ez);
|
||
const y1 = -(i01 * ex + i11 * ey + i12 * ez);
|
||
const y2 = -(i02 * ex + i12 * ey + i22 * ez);
|
||
const dq = [0, 0, 0, 0, 0, 0]; let mx = 0;
|
||
for (let c = 0; c < 6; c++) {
|
||
dq[c] = J[c * 3] * y0 + J[c * 3 + 1] * y1 + J[c * 3 + 2] * y2;
|
||
mx = Math.max(mx, Math.abs(dq[c]));
|
||
}
|
||
const gain = mx > 12 ? 12 / mx : 1;
|
||
for (let c = 0; c < 6; c++) {
|
||
const t = LIM[ORDER[c]];
|
||
q[c] = Math.max(t.lo, Math.min(t.hi, q[c] + dq[c] * gain));
|
||
}
|
||
}
|
||
};
|
||
run([0, 0, 0, 0, 0, 0]);
|
||
const N = tries == null ? 160 : tries;
|
||
for (let k = 0; k < N; k++) {
|
||
const q = [0, 0, 0, 0, 0, 0];
|
||
for (let c = 0; c < 5; c++) { const t = LIM[ORDER[c]]; q[c] = t.lo + Math.random() * (t.hi - t.lo); }
|
||
run(q);
|
||
}
|
||
return { q: bestQ, dist: bestD };
|
||
}
|
||
|
||
/* ---------- 6. 一键求解（不可达时返回最近可达位姿） ---------- */
|
||
function solveTo(target, opt) {
|
||
opt = opt || {};
|
||
const res = solve(target, opt);
|
||
if (res.ok) return res;
|
||
const cp = closest(target, +opt.tool || 0, opt.tries);
|
||
if (!cp.q) return res;
|
||
return {
|
||
ok: false, reachable: false, target: target.slice(), tool: +opt.tool || 0,
|
||
q: cp.q.slice(), cost: costOf(cp.q, opt.weights), err: cp.dist, resid: cp.dist,
|
||
nSol: res.nSol, samples: res.samples, ms: res.ms, list: [],
|
||
note: '目标超出可达工作空间：返回最近可达位姿（阻尼最小二乘）'
|
||
};
|
||
}
|
||
|
||
/* ---------- 7. 自检：闭式 FK ↔ 矩阵链；逆解重构 ---------- */
|
||
function selfTest(n) {
|
||
n = n || 400;
|
||
let maxFk = 0;
|
||
for (let k = 0; k < n; k++) {
|
||
const q = [];
|
||
for (let i = 0; i < 6; i++) { const t = LIM[ORDER[i]]; q.push(t.cont ? (Math.random() * 720 - 360) : t.lo + Math.random() * (t.hi - t.lo)); }
|
||
const a = fk(q, 0), b = fkChain(q, 0);
|
||
maxFk = Math.max(maxFk, Math.hypot(a[0] - b[0], a[1] - b[1], a[2] - b[2]));
|
||
}
|
||
let maxIk = 0, hit = 0, m = 0;
|
||
for (let k = 0; k < Math.min(n, 200); k++) {
|
||
const q = [];
|
||
for (let i = 0; i < 6; i++) { const t = LIM[ORDER[i]]; q.push(t.lo + Math.random() * (t.hi - t.lo)); }
|
||
const T = fk(q, 0);
|
||
const A = T[0] - K.X0, BY = T[1] - K.Y0, BZ = T[2] - K.Z1;
|
||
const sols = branch(q[3], q[4], A, Math.hypot(BY, BZ), Math.atan2(BZ, BY), 0);
|
||
if (!sols.length) continue;
|
||
m++;
|
||
let best = Infinity;
|
||
for (const s of sols) best = Math.min(best, Math.hypot.apply(null, fk(s, 0).map((v, i) => v - T[i])));
|
||
maxIk = Math.max(maxIk, best);
|
||
if (best < 1e-6) hit++;
|
||
}
|
||
return { maxFkErr: maxFk, maxIkErr: maxIk, ikHit: hit, ikTried: m, structWarn: WARN, K: K, LIM: LIM };
|
||
}
|
||
|
||
return { fk: fk, fkChain: fkChain, solve: solve, solveTo: solveTo,
|
||
closest: closest, branch: branch, selfTest: selfTest,
|
||
K: K, LIM: LIM, P: P, D: D, ORDER: ORDER, wrap180: wrap180, costOf: costOf };
|
||
}
|
||
|
||
return { make: make, ORDER: ORDER };
|
||
})); |