模式：本页「
运动模拟」承载全部运动学能力；「三维模型」为纯静态模型视图（关节锁定在设计零位、无目标点与求解控件），「装配图」为二维图纸。运动模拟下左栏只保留 IK 结果与关节角度，零件树/打印清单自动收走。
目标：使 TCP（末端工具法兰 P09 安装面中心，可加“工具长”）到达给定点，并在一切满足限位的可行构型中取
Σ|θᵢ| 最小者（交互连续求解时叠加参考位姿连续项，见第 6 条）。开启上方“
姿态控制”后，
约束升级为
位置 3 + 姿态 3（工具轴指向 + 绕轴滚转），见第 5 条。
姿态操控：姿态开启后目标球处出现方向箭头与粉色侧球——
箭头指向即工具朝外方向（法兰面法向朝外，也就是工具伸出的方向）。
拖箭头头部用弧球法（指针射线与以目标为球心、半径＝箭长的球求交，方向＝交点−球心）连续调整方位/俯仰；
拖箭杆或粉色侧球按指针绕轴的转角增量改滚转；右侧数字框随时同步，可精确微调，
「取当前」把真实工具姿态读回、「垂直向下」一键设为爪取常用姿态。
约定：求解器内部工具坐标系 z 轴取法兰面法向（朝臂内，与 TCP 外伸反向）；界面上的方位/仰角/滚转、
箭头与绿色当前轴线均以朝外法向描述，入栈时绕工具 x 轴翻 180° 换算，两者看到的物理姿态完全一致。
- 先消去一维（仅位置任务）：TCP 恰好落在 J6 轴线上，绕 J6 转动不改变 TCP 位置 ⇒ 对“到点”任务取
θ6 = 0 即已最优。
- 识别结构：J2 / J4 / J5 三轴严格平行（轴向 Y，过 x = 18.5），J1 轴为 X，J3 轴沿臂长方向 Z。
记
A = X−X0、R = |(Y−Y0, Z−Z1)|、φ = atan2(Z−Z1, Y−Y0)，
则 X = X0 + a，而 (Y, Z) 由 (b, c) 绕 J1 轴旋转 φ 得到：
a = Vx·cosθ3·cosθ2 + Vz·sinθ2，b = −Vx·sinθ3，
c = C0 + (−Vx·cosθ3·sinθ2 + Vz·cosθ2)，
其中 Vx, Vz 只是 (θ4, θ5) 的函数。
- 两式联立消元（关键一步）：由
a = A 得恒等式
A² + n² = Vx² + Vz²（n = −Vx·cosθ3·sinθ2 + Vz·cosθ2）；
代入 b² + c² = R² 后 θ3 的项全部抵消，于是
n = (R² + A² − Vx² − Vz² − C0²) / (2·C0) —— 唯一确定；
再由 n² + A² = Vx²·cos²θ3 + Vz² 得
cos²θ3 = (n² + A² − Vz²) / Vx² ⇒ θ3 共四支 ±σ, ±(π−σ)；
最后 θ2 与 θ1 各由一个 2×2 线性方程组唯一确定：
cosθ2 = (κA + Vz·n)/(κ² + Vz²)，sinθ2 = (Vz·A − κ·n)/(κ² + Vz²)，
κ = Vx·cosθ3；θ1 = φ − atan2(c, b)。
- 遍历冗余：(θ4, θ5) 就是二维冗余。在 (θ4, θ5) 的限位盒内全域扫描（区间端点必采，含上下限位），
每个网格点闭式求出全部支 ⇒ 覆盖整个可行解族；再对最优的若干互异候选做 h/8 局部加密 + 模式搜索，
得到 全局最小 Σ|θ|，误差量级 ≤ 扫描步长。
- 姿态任务（6 自由度）：工具坐标系姿态
R = R₁·R₂·…·R₆（与场景图 6 级矩阵链一致）。
结构上 J5 轴与 J6 轴在腕心 (18.5, 128, 222) 垂直相交，因此 θ6 只提供绕工具自身轴的滚转：
工具轴指向 = (R₁…R₅)·ẑ，与 θ6 无关，TCP 位置也与 θ6 无关。每条位置闭式支都附带确定的 R₅
（= R₁…R₅），滚转角随之闭式解出：N = R₅ᵀ·R_des（需为 Rz(−θ6)）⇒
θ6 = −atan2(N₁₀, N₀₀)；姿态偏差 e = ∠(R₅·Rz(−θ6), R_des) ≤ 2° 的支才收入。
- 姿态达标优先：达标解在 (θ4,θ5) 平面上是一条斜窄谷（奇异邻域灵敏度可达 ~100°/°），
按 Σ|θ| 取种子会整体错过它。达标带（e ≤ 1°）按 e 升序取种子、局部窗口加密、必要时 escalating
（姿态惩罚 ×100 重扫），最后做 二维牛顿收口：对“工具轴在目标轴法平面上的切向偏差”迭代，
每步在新 (θ4,θ5) 上重解闭式位置支（按 θ1..θ3 最近匹配取同支，位置天然保持精确），把 e 压到 ~10⁻³° 量级。
最终只在 e ≤ 0.05° 的达标候选里按含连续项代价选优——不为省几度关节角牺牲姿态精度；
确有 ~5% 的均匀随机姿型落在奇异“刀尖”上（姿态仅在奇异点精确可达），此时按最近位姿返回并如实报告残差。
- 连续优先（防倒摆）：等价多分支（θ3 的 ±σ/±(π−σ)、肘上/肘下）之间只按 Σ|θ| 选优时，目标微动就会在分支间
跳来跳去，某轴大幅倒摆（实测 4mm 步长过可达边界最大跳 81.9°）。交互求解以当前手臂姿态为参考位姿，
代价加入
λ·Σwrap(θᵢ−refᵢ)²（λ=0.02）：同族分支被锁定，仅当近支越限或误差超差才换支。
A/B 实测（4 条扫描路径）：>10° 跳变 14→5 次，最大跳变 163.5°→16.8°，平均 Σ|θ| 不升反降。
- 代表角：J1 / J3 / J6 为 ±180° 整周轴，取 |θ| 最小的等价角；J2 / J4 / J5 有限位，越限即丢弃。
- 自检：每次求解都用闭式 FK 与场景图真实 6 级矩阵链各算一次 TCP 和工具姿态并与目标比对（见上表残差）。
验证数据（本机实测，可复现）
| 检查项 | 结果 |
|---|
| 闭式 FK vs 6 级矩阵链（1500 组随机位形） | 4.6 × 10⁻¹³ mm |
| 逆解分支重构（200 组随机可行位形） | 200 / 200 命中 |
| θ6 不变性（0° → 137°，TCP 位移） | 0.00 mm |
| 姿态参数 ↔ 旋转矩阵往返（2000 组随机） | 2.4 × 10⁻⁶° |
| 姿态重构达标率（200 组随机可行位形，0.5° 步长） | 185–191 / 200（4–7% 奇异刀尖构型残差 ≤ 2°，如实报告） |
| 滚转角交互 roll 0→90（参考位姿连续） | Δθ6 = 89.99°，θ1..θ5 变化 < 0.01° |
| 60 个可达目标：失败次数 / 最大残差 | 0 次 / 3.5 × 10⁻¹³ mm |
| 相对 0.05° 极细网格参考的最大劣化 | 0.02° |
| vs 4000 次随机多起点数值解 | 0 / 20 次劣于数值解 |
| 等价分支跳变 · 4 条扫描路径（修复前 → 后） | >10° 跳变 14 → 5 次；最大单步 163.5° → 16.8° |
| 单次求解耗时（默认 0.25°） | 约 0.1 s（1° ≈ 30 ms，0.05° ≈ 1.1 s；开姿态后同级或更快） |
限位：J2 ∈ [−106, 9]，J4 ∈ [−111, 16]，J5 ∈ [−95, 34]，J1 / J3 / J6 为 ±180°（可整周）。
局限：本算法为纯运动学求解，只受各关节行程限位约束，
不含跨关节自碰撞校核。
原模型限位来自全对全动态扫描；若需要更舒展的姿态，可从上方“互异构型”列表改选次优解。
不可达：若目标超出工作空间，自动求最近可达位姿——阻尼最小二乘
从当前姿态锚定起步（抑制退化方向的无用漂移）、固定种子的确定性重启（同一点重复解算逐位一致）、再经零空间抛光取最小总关节角的代表值；报告的距离为抛光后真实值。
姿态不可达：姿态任务超可达空间时，阻尼最小二乘误差向量升级为 6 维（位置 mm + 姿态 deg，按 20 mm/rad 折算），返回位置与姿态联合最近的位姿，两个残差分别显示。
API：控制台可用
IK.solver.solve([x,y,z],{step:0.25,tool:0,pose:{R:ArmIK.poseMat(az,el,roll)}})；
IK.setPose(az,el,roll) 可直接设定目标姿态。