# 机械臂姿态估计算法评估报告 > 评估对象：`web/arm-ik-solver.js`（6 轴逆运动学姿态求解器）+ `web/ik-glue.js`（查看器集成） > 评估日期：2026-09-29　评估方式：静态审查 + 可复现的离线数值实验（`node`，固定随机种子） > 本报告为「评估 + 策略设计」，不含代码修改。 > > **勘误（2026-09-29，实施 S0 时发现并已全表修正）**：本报告初版的「真最优」基准在度量姿态差时 > 漏掉了 θ6（工具滚转）的闭式最优，即用 `rotErrDeg(orient(q), R_tar)`（θ6=0）代替了 > 「θ6 取最优后的全姿态偏差」，系统性高估了可达的最小姿态差（例：`T=(20,400,300)` 初版 121.05°、 > 修正后 44.20°）。**修正后缺陷 B 的劣化量更大（更严重），而非更小**：均值 104.2°→110.3°。 > 修正涉及 §1、§2.3、§2.4、§3.3、§6.2、§6.3、§8.2 与 §9 附录命令（`posBest` 已改为 θ6 最优口径）。 --- ## 1. 概述与结论摘要 用户提出的问题：**「在多个关键位置无法达到预定姿态目标；肉眼可观察到存在更优姿态，但求解器给出的不是它。」** 评估结论：问题真实存在，且可稳定复现、可量化。核心原因不是"算不出来"，而是**求解器在姿态不可达时丢弃了已经算出的位置精确解，转而使用一个更慢且更差的 6 维随机搜索**；同时在可达但构型退化时，姿态精确解的根枚举会漏解，并**把漏解当成成功**返回。 | 编号 | 缺陷 | 实测影响 | 根因 | 对应策略 | | :--- | :--- | :--- | :--- | :--- | | **A** | 姿态精确可达，却静默返回 1~2° 残差 | 全空间 0.50%(2/400) 触发；近伸直构型 3.3%(1/30)；最大 1.934° | 根枚举在退化处漏解（R1） | S1 | | **B** | 姿态不可达时最近的"最近位姿"严重次优 | 平均劣化 **110.3°**（12 目标）/ **106.3°**（38 样本），最大 176.7°，被支配 12/12 | 接受门直接把候选池清空（R2）+ 兜底目标函数不一致（R3） | S0/S2 | | **C** | 位置精确解整体被丢弃，兜底被便宜解严格支配 | 存在 11920 个位置精确解的目标，兜底仍在指标上双输 | `oerr>POSETOL` 一律 `return null` | S2 | | **D** | 耗时超出既定 ≤100 ms 预算 | 开姿态中位 107 ms / p95 242 ms / 最大 299 ms | 兜底路径 160 次随机重启；姿态任务 escalating 重扫 | S1/S3 | 另外回答了一个架构问题（§3）：**现状确实是「先点位、后角度」**；用户设想的"用末端指定角度逐级反推"**在本机构上不可行**——因为 `J4 ∥ J5`，这是**非球形腕**。但由此得到的「姿态优先」求解模式是可行且有价值的，实测可把姿态差压到 0.04°，代价是十几毫米的位置误差。 > 篇幅说明：第 9 章附录内嵌了本报告**全部数据的可复现命令**，任意数字都可重跑核对。 --- ## 2. 算法现状描述 ### 2.1 资产与数据流 | 角色 | 文件 | 说明 | | :--- | :--- | :--- | | 姿态求解器 | [arm-ik-solver.js](file:///g:/Robot_v2/机器人手臂/web/arm-ik-solver.js) | UMD 单文件，`make(cfg)` 返回 `solve / solveTo / solveWarm / solveDyn / closest / closest6 / branch / fk / orient / rotErrDeg / LIM / costOf`（[导出 L1355-1360](file:///g:/Robot_v2/机器人手臂/web/arm-ik-solver.js#L1355-L1360)） | | 查看器集成 | [ik-glue.js](file:///g:/Robot_v2/机器人手臂/web/ik-glue.js) | 姿态 UI、目标拖动、调用 `solveTo`（[L379](file:///g:/Robot_v2/机器人手臂/web/ik-glue.js#L379)、[L399](file:///g:/Robot_v2/机器人手臂/web/ik-glue.js#L399)）、结果渲染与徽标（[L318-342](file:///g:/Robot_v2/机器人手臂/web/ik-glue.js#L318-L342)） | | 关节表（几何真值） | [mesh_arm.json](file:///g:/Robot_v2/机器人手臂/web/mesh_arm.json) | `joints[] = {id, rear, d}` | | 关节限位 | [travel.json](file:///g:/Robot_v2/机器人手臂/docs/travel.json) | 本报告测量口径：J1/J3/J6 连续(±180)，J2∈[-106,9]，J4∈[-111,16]，J5∈[-95,34]（2026-09-29 起该表退为零位扫描参考：滑块改电机行程 ±360°，可行域改相邻件实测区间 J2[-107,128]/J4[-112,112]/J5[-96,96]） | | 既有自检 | [pose_ik_check.js](file:///g:/Robot_v2/机器人手臂/scratch/pose_ik_check.js) | 7 组 node 自检（本次全部通过，见 §6.3） | | 性能基准 | [bench_ik_split.js](file:///g:/Robot_v2/机器人手臂/scratch/bench_ik_split.js) / [bench_ik_split.json](file:///g:/Robot_v2/机器人手臂/scratch/bench_ik_split.json) | 单次求解耗时与位置/姿态占比拆分 | | 历史实验 | `scratch/ikopt/`（`solver_scanbase.js` = 改动前快照） | 见 §2.4 | ### 2.2 求解流程（`solve()`，[L558-944](file:///g:/Robot_v2/机器人手臂/web/arm-ik-solver.js#L558-L944)） 1. **位置闭式 + 二维冗余扫描**：以 (θ4,θ5) 为冗余参数全域扫描；对每个网格点，[branch() L429-463](file:///g:/Robot_v2/机器人手臂/web/arm-ik-solver.js#L429-L463) 闭式给出最多 4 支 θ1..θ3（θ3 = ±σ、±(π−σ)），**位置误差天然 ~1e-13 mm**。 2. **按行裁剪**：[rowSpans L644-654](file:///g:/Robot_v2/机器人手臂/web/arm-ik-solver.js#L644-L654) 以探测步 `hp = max(h, 2)°` 找每 θ4 行的可行 θ5 段，段内再按 h 精扫。 3. **姿态任务**：工具轴对齐 = (θ4,θ5) 上的 2 方程 2 未知量，解为**孤立点**；[§4.1b 根枚举 L697-761](file:///g:/Robot_v2/机器人手臂/web/arm-ik-solver.js#L697-L761) 逐行做残差变号括号 + 牛顿收口（[poseTools L486-545](file:///g:/Robot_v2/机器人手臂/web/arm-ik-solver.js#L486-L545)）；滚转角 θ6 由 [rollClose L549-555](file:///g:/Robot_v2/机器人手臂/web/arm-ik-solver.js#L549-L555) 闭式补足。 4. **接受门**：`POSETOL = 2.0°`；[scoreBranch L592-606](file:///g:/Robot_v2/机器人手臂/web/arm-ik-solver.js#L592-L606) 对 `oerr > POSETOL` 的候选**直接 `return null`**（[L596-598](file:///g:/Robot_v2/机器人手臂/web/arm-ik-solver.js#L596-L598)）。 5. **不可达兜底**：[closest6 L1054-1169](file:///g:/Robot_v2/机器人手臂/web/arm-ik-solver.js#L1054-L1169) 为 6 维阻尼最小二乘（DLS），160 次确定性伪随机重启 × 160 迭代；由 [solveTo L1172-1192](file:///g:/Robot_v2/机器人手臂/web/arm-ik-solver.js#L1172-L1192) 在 `solve()` 失败时调用。 6. **实时快路**：[solveWarm L1200-1265](file:///g:/Robot_v2/机器人手臂/web/arm-ik-solver.js#L1200-L1265)，沿上一帧解同支续解，任一项校验不过即回落全量。 **关键判断（贯穿全文）**：位置是**硬约束**（闭式、精确），姿态是**软约束**（带 2° 门、超门即弃）。 ### 2.3 关节轴几何（判定"能否解耦"的依据） | 关节 | rear (CAD) | d | 类型 | | :--- | :--- | :--- | :--- | | J1 | (-51, 128, 610) | (1,0,0) | X 抬举 | | J2 | (18.5, 100, 490) | (0,**1**,0) | Y 俯仰 | | J3 | (18.5, 128, 445) | (0,0,−1) | Z 自转 | | J4 | (18.5, 100, 332) | (0,**1**,0) | Y 俯仰（肘） | | J5 | (18.5, 100, 222) | (0,**1**,0) | Y 俯仰（腕） | | J6 | (18.5, 128, 176) | (0,0,**−1**) | Z 工具自转 | - **J4 ∥ J5**（均 +Y，相距 110 mm）⇒ 平行轴永不相交 ⇒ **非球形腕**。 - 代码已利用该性质：`SAMEAX`（[L408](file:///g:/Robot_v2/机器人手臂/web/arm-ik-solver.js#L408)）令 `R4·R5 = R_y(θ4+θ5)`。 - J5 ⊥ J6 且在 (18.5,128,222) 相交，故末两轴构成 2 轴腕，但肘关节 J4 破坏了球形性。 ### 2.4 历史优化实验（`scratch/ikopt/`） 按时间：`solver_win` → `solver_fast` → `solver_warm` → `solver_old` → `solver_scanbase`（改动前快照）→ `bench_prune`（行裁剪 A/B）→ `bench_warmfollow`（warm 跟踪 A/B）。配套 `patch_*.py` / `merge_fast.py` / `audit_*.js`。 **结论**：这些迭代解决的是**耗时、连续性、退化漂移**三类问题（姿态预筛提速 1.41×、warm 窗口跟踪、同族门、行裁剪），**均未触及"覆盖完备性（缺陷 A）"与"兜底解质量（缺陷 B/C）"**。因此 A/B/C 三类缺陷在历次优化中一直存在。 --- ## 3. 求解范式评估：位置优先 vs 末端姿态反推 > 本节回答用户提问：**「现在是不是先找点位再找角度？能不能用末端指定角度一级级反推回去？」** ### 3.1 现状确认：确实是"先点位、后角度" `solve()` 先用位置约束闭式定死 θ1..θ3（位置误差 ~1e-13 mm），再把姿态当次级软约束（2° 门）。**所有实验里被牺牲的永远是角度**——§5 的兜底数据、§6.3 自检的 [4] 项（"位置残差始终 3.1e-13 mm（姿态未达也不牺牲位置）"）都直接印证这一点。 ### 3.2 「末端角度逐级反推」可行性判定：本机构不成立 经典"末端反推"= 腕心解耦法，前提是**球形腕**（末端 3 轴交于一点）：先由目标姿态算出腕心 `p_w = p_tcp − d₆·z_tool`，再由位置解 θ1..θ3，最后解 θ4/θ5/θ6。 **本机构不满足**：J4 ∥ J5（相距 110 mm 的平行轴），二者永不相交，不存在统一的"腕心"，因此**无法**把 θ6→θ5→θ4 一级级闭式反推。 **但存在可用的降阶结构**（这才是正确的利用方式）： ``` R = Rx(θ1) · Ry(θ2) · Rz(−θ3) · Ry(θ4+θ5) · Rz(−θ6) ``` 即：**工具轴指向只由 (θ1, θ2, θ3, θ4+θ5) 决定；θ6 是纯工具滚转**。因此"用末端姿态驱动求解"是可行的，但要按这条降阶链解（3 方程 / 5 参数 → 2 参数族），而不是逐级反推。这一点也解释了为什么现有代码用 `axisCol()` 只算到 θ5 为止（[L409-424](file:///g:/Robot_v2/机器人手臂/web/arm-ik-solver.js#L409-L424)）。 ### 3.3 更重要的实测结论：位置与姿态"真的不可兼得" 在可达姿态基础上把目标姿态外推 20°，用同一目标对照两种优先级（复现命令见 §9 A4/A3）： | 目标 TCP | 兜底 closest6 | 位置优先（保点） | 姿态优先（保角度） | | :--- | :--- | :--- | :--- | | (308,−182,372) | 90.9° / 0.5 mm | **4.26° / 0.0 mm** | **0.04°** / 3.4 mm | | (180,−111,422) | 138.7° / 0.0 mm | **2.81° / 0.0 mm** | **0.01°** / 6.1 mm | | (83,192,1032) | 179.8° / 0.0 mm | **9.63° / 0.0 mm** | **0.06°** / 23.7 mm | | (−46,48,430) | 129.0° / 0.0 mm | **2.87° / 0.0 mm** | **0.01°** / 6.4 mm | | …（共 12 组，见附录） | 均值 **115.3°** | 均值 **5.7°** / 位置恒 0.0 mm | 均值 **0.04°** / 位置均 11.6 mm | **结论**：位置与姿态在远目标是**真实的取舍关系**，不存在"都能兼顾"的解。 - 位置优先：位置误差恒为 0，姿态差从 2.81° 到 9.63° 不等； - 姿态优先：姿态差 0.01°~0.10°（等价于精确到达），位置误差 3.4~27.7 mm； - 二者差值可达数十毫米 / 上百度。 ⇒ **求解器必须提供可切换的优先级**，这正是用户诉求"无法精确达到目标姿态时输出最接近的角度解"的正确落点（→ 策略 S0）。 --- ## 4. 评估方法 ### 4.1 指标定义 | 指标 | 定义 | | :--- | :--- | | 位置残差 `err` | `‖fk(q) − T‖`，mm | | 姿态残差 `oerr` | `rotErrDeg(orient(q), R_des)`，度（求解器 `rotErrDeg` 口径） | | 达标线 | 位置 ≤ 1e-6 mm（闭式恒满足）；姿态 ≤ 0.05° | | **覆盖漏洞率** | 目标姿态精确可达（细扫 step=0.05° 能到 ≤0.05°）而模拟默认档 step=0.25° 返回 >0.05° 的比例 | | **静默残差** | 上述情况下返回的 `oerr`（求解器仍报 `reachable=true`，不报错） | | **步长敏感度** | 同一目标在 step ∈ {0.5,0.4,0.3,0.25,0.2,0.15,0.1,0.05} 下 `oerr` 的波动 | | **兜底劣化** | `closest6` 的姿态差 − 「位置精确解集内能达到的最小姿态差」 | | 耗时 | 单次 `solve` / `solveTo` 墙钟时间，ms | ### 4.2 独立真值基准（避免自证） 被测求解器自报的 `err/oerr` 与被测算法同源，不能自证。本报告的"真最优"用**独立于求解器决策路径**的枚举得到： 对 (θ4,θ5) 做网格枚举，取 `branch()` 全部在限位内的支（位置天然精确），再对姿态差取最小；并对最优的前 64 个候选**解析优化 θ6**（绕 J6 轴的滚转，闭式取到最优），得到更紧的真值。 注：该基准对 (θ4,θ5) 仍是离散网格（0.5°），故给出的真最优是**上界**，据此算出的"劣化"是**下界**——结论偏保守。 ### 4.3 基准数据集与随机性 - 采样一律使用**固定种子的 mulberry32**（`seed` 已在附录命令中写死），任意数据可逐位复现。 - 早期探索阶段曾用 `Math.random()`（不可复现）得到过更高的触发率（最高 2.5%/120 样本）；本报告**只采用可复现数据**。 - 数据集：D1 全空间随机可达位形 N=400；D2 工作区两区各 N=60；D3 姿态外推 20°（120 目标）；D4b 近伸直构型 N=30；D5 越界目标 4 个；D6 双优先级 12 组。 --- ## 5. 性能分析 ### 5.1 单次求解耗时构成（[bench_ik_split.json](file:///g:/Robot_v2/机器人手臂/scratch/bench_ik_split.json)，40 样本，step 0.25 / tol 1e-6 / 开姿态） | 指标 | 数值 | | :--- | ---: | | 冗余维网格 | J4 509 点 × J5 517 点 ≈ 26.3 万粗扫点 | | 开姿态单次 · 中位 | **107.1 ms** | | 开姿态单次 · p95 | **242.5 ms** | | 开姿态单次 · 最大 | **299 ms** | | 其中"位置基座"中位 | 73.6 ms | | 其中"姿态增量"中位 | 37.2 ms（占 **33.3%**） | | escalating 触发率 | 2.5% | | 关姿态同目标中位 | 73.4 ms | ### 5.2 本次实测耗时 | 场景 | 耗时 | | :--- | ---: | | D1 全空间可达目标 `solve(0.25)` 均值 | 100 ms | | D2 前伸工作区 / 默认区 均值 | 98 ms / 98 ms | | D5 越界目标 `solveTo`（含兜底） | 85 ~ 125 ms | | 参考：`solve(0.05)` 均值（D1） | ~835 ms | ### 5.3 结论 - **已超预算**：用户既定目标为 ≤100 ms，实测中位 107 ms、p95 242 ms、最大 299 ms。 - **姿态任务是主要波动源**：占总耗时 33%，且 escalating（×100 权重重扫）在 2.5% 的目标上会额外付出整轮精修。 - **兜底路径性价比最低**：160 次随机重启的 6 维 DLS 在 85~125 ms 内给出的是 §6 里"平均劣化 104°"的解——**又慢又差**。 --- ## 6. 误差来源定位 ### 6.1 R1 根枚举不完备（对应缺陷 A） 姿态精确解在 (θ4,θ5) 平面上是一条**一维曲线**，只在少数孤立点穿过 `POSETOL` 窄带。当该曲线经过退化构型时，变号括号检测与"θ1..θ3 最近"同支匹配会失效： - **退化点 1：θ3 → 0 或 ±180（手臂近伸直）**。此时 `branch()` 中 `Vx → 0`，4 支两两重合、位置雅可比奇异，牛顿收口停摆。 - **退化点 2：关节贴限位**（如 θ5 ≈ J5 上限）。 **可复现证据（见 §9 命令 F，seed=424242）**： | 项 | 值 | | :--- | :--- | | 目标 TCP | **(26.386, −3.334, 1019.998)** | | 真实关节角 | (198.03, −46.76, **−180.52**, −92.79, **33.48**, 102.36) ← θ3≈−180 近伸直、θ5≈J5 上限 34 | | 真解在 `branch()` 下 | 位置误差 **1.4e-13 mm** 的精确解存在，且在限位内 | | `solveTo({warm: 真解})` | `oerr = 1.21e-6°`（**精确解确实存在**） | | `solve(step=0.25)` | `oerr = 1.9881°`（**漏解，且报 reachable=true 不报错**） | **步长敏感度（同一目标）**——非单调、非收敛，直接证明这不是"精度不够"而是"没找到"： | step | 0.5 | 0.4 | 0.3 | 0.25 | 0.2 | 0.15 | 0.1 | 0.05 | | :--- | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | | `oerr` (°) | 1.9881 | 1.9119 | **0.0307** | 1.9881 | 1.9881 | 1.9652 | **0.0258** | 0.0491 | > **顺带发现**：查看器"扫描精度"下拉框的悬停说明写着「姿态任务的精确解由孤立根枚举给出、**与步长无关**」（[viewer_tpl.html L229](file:///g:/Robot_v2/机器人手臂/src/viewer_tpl.html#L229)）。上表**直接证伪**了这一说明。 **发生率（可复现）**： | 数据集 | 样本 | `oerr>0.05°` | 其中覆盖漏洞 | 最大静默残差 | | :--- | ---: | ---: | ---: | ---: | | D1 全空间（seed=20260929） | 400 | 2（0.50%） | 1（0.25%） | 1.934° | | D4b 近伸直 θ3≈0/±180（seed=909090） | 30 | 1（3.3%） | 1（3.3%） | 1.860° | | D2 前伸工作区 (20,210,300)±90mm | 60 | 0 | 0 | 0° | | D2 查看器默认区 (150,128,260)±60mm | 60 | 0 | 0 | 0° | **结论**：缺陷 A 是**构型相关**的——常规工作区（默认目标附近）几乎不触发，**集中在外围/极端位姿与近伸直、贴限位构型**。这解释了为什么肉眼只在"关键位置"看到问题。 ### 6.2 R2 接受门把"不可达"与"漏解"混为一谈（对应缺陷 C） `scoreBranch` 对 `oerr > POSETOL(2°)` 的候选一律 `return null`（[L596-598](file:///g:/Robot_v2/机器人手臂/web/arm-ik-solver.js#L596-L598)）。后果是**候选池被整体清空**，`solve()` 报 `ok=false`，转入 `closest6`。 **可复现证据**：目标 `T=(20,400,300)`、`R=poseMat(25,95,10)`： - 位置**完全可达**：无姿态 `solve` 返回 `err = 2.5e-14 mm`；0.5° 网格上有 **11920** 个位置精确解； - 姿态目标不可达（位置精确解集内最优姿态差 **44.20°** > 2°）⇒ 候选池被清空 ⇒ 走 `closest6`； - `closest6` 返回 **136.7° / 位置 0.0 mm** —— **在位置与姿态两个指标上都不优于**一个用现有闭式扫描就能廉价得到的解（劣化 **92.5°**）。**被严格支配（Pareto-dominated）。** 另一例（离查看器默认目标很近，仅抬高 Z）：`T=(150,128,600)` 位置精确可达（0.0 mm，2580 个位置精确解），位置精确解集内最优姿态差 **14.72°**，而 `closest6` 给出 **173.4°** —— 劣化 **158.7°**。 > 注：`T=(20,400,300)` 这一例另有独立复核——初版基准给的 121.05° 是 θ6=0 口径（见文首勘误）， > 按 θ6 最优口径重扫全域 0.5° 网格（258×254 ≈ 6.6 万点）得 **44.200°**，与 θ6 最优口径一致。 ### 6.3 R3 兜底目标函数与真实目标不一致（对应缺陷 B） `closest6` 用固定 `OW = 20 mm/rad` 把姿态折进位置残差做联合 DLS（[L1055](file:///g:/Robot_v2/机器人手臂/web/arm-ik-solver.js#L1055)），既不保证"位置优先"，也不保证"姿态最小"，且依赖随机重启初值。 **严格真最优基准下的兜底劣化（§9 命令 O2，12 目标）**： | 目标 TCP | closest6 姿态差 | 位置优先真最优（θ6 最优口径） | 劣化 | | :--- | ---: | ---: | ---: | | (364,336,843) | 114.2° | 5.98° | 108.2° | | (288,−16,595) | 166.8° | 7.94° | 158.9° | | (72,376,161) | 107.2° | 5.30° | 101.9° | | (311,10,701) | 138.2° | 14.72° | 123.4° | | (−52,242,897) | 121.1° | 3.18° | 117.9° | | (4,−218,303) | 156.3° | 13.02° | 143.3° | | (279,−155,765) | 173.0° | 3.15° | **169.9°** | | (402,202,425) | 81.2° | 16.48° | 64.7° | | (−4,472,883) | 136.3° | 4.92° | 131.4° | | (189,−298,766) | 54.4° | 16.61° | 37.8° | | (259,−284,528) | 44.8° | 5.84° | 38.9° | | (137,−315,829) | 133.5° | 6.20° | 127.3° | | **均值** | **118.9°** | **8.6°** | **110.3°** | **被支配 12/12**：每一个目标上，`closest6` 的姿态差都大于一个"位置同样精确、只是换了构型"的解。 **更大样本的兜底劣化（§9 命令 A3，120 目标，38 个不可达样本）**：平均劣化 **106.3°**，最大 **176.7°**，最小 16.1°。 ### 6.4 R4 行裁剪探测步过粗 [rowSpans](file:///g:/Robot_v2/机器人手臂/web/arm-ik-solver.js#L644-L654) 以 `hp = max(h,2)°` 探测每 θ4 行的可行 θ5 段，并按"单一连续段"假设（注释称仅在 5 个目标上验证过）。窄于 2° 的可行段会被整段漏掉，导致整行无候选——这是 §6.1 漏解的另一条通路，也与"θ5 贴着限位"的退化构型吻合。 ### 6.5 既有自检为何没有拦住这些缺陷 `node scratch/pose_ik_check.js` 本次**全部通过**： | 项 | 结果 | | :--- | :--- | | [1] poseMat ↔ poseAng 往返 | ✓ 最大偏差 2.4e-6° | | [2] 位置 IK 回归（200 组） | ✓ 200/200，最大残差 3.5e-13 mm，均耗时 68 ms | | [3] 姿态重构（200 组，step 0.5） | ✓ 达标 195/200；**允许 4 个"奇异刀尖"不达标**；最差 0.90° | | [4] 工作区 ±45mm（48 个） | ✓ 48/48 精确，最差 0.02° | | [5] roll 0→90 口径 | ✓ Δθ6 = 90.000° | | [6] 超工作空间 → closest6 | ✓ **只检查"返回了最近位姿"，不检查其最优性**（返回 0.0 mm / **136.7°**） | | [7] 耗时对比 | 关 41 ms / 开 15 ms（**开比关还快，异常，说明该计时项不可靠**） | **盲区诊断**： 1. [3] 把 2° 以内的残差视为"通过"，且显式豁免"奇异刀尖"——与缺陷 A 的静默残差（1.2~1.95°）**正好落在盲区内**； 2. [4] 只在 `(20,210,300)±45mm` 这一处良性区域采样（与 D2 的 0 失败一致），碰不到外围退化构型； 3. [6] 只验证"有返回"，不验证"最近"——与缺陷 B/C 完全错开。 --- ## 7. 优化策略 按"收益 / 成本"排序。所有策略共用一份闭式支枚举结果，不改变运动学口径。 > **实施状态**：**S0（位置优先 / 姿态优先切换）已实施并验证**，见 §7.1； > S2 中"不再清空候选池"的部分已随 S0 的位置优先模式一并落地。S1 / S3 / S4 及 S0 的"折中"模式尚未实施。 ### 7.1 S0（已实施）双优先级求解范式 **问题**：位置与姿态在远目标上不可兼得（§3.3），单一目标函数无法兼顾两种使用意图。 **已实现**： | 项 | 实现 | | :--- | :--- | | 求解器入口 | `solve()/solveWarm()/solveTo()` 新增 `opt.priority ∈ {'pos','ori'}`；**省略该选项时行为与改动前逐字一致**（既有自检/基准不受影响） | | 位置优先 `'pos'` | `min oerr s.t. ‖fk(q)−T‖ = 0`。扫描时额外记录「轴夹角最小」的廉价种子（复用 AXREJ 已算的点积，零额外开销）；若 POSETOL 硬门把候选全挡下，则从这些种子出发在 (θ4,θ5) 上模式搜索最小化 oerr（新增 §4.5，每点仍走 `branch()` 闭式支 ⇒ 位置保持精确） | | 姿态优先 `'ori'` | `min ‖fk(q)−T‖ s.t. oerr = 0`。新增 [closestOri](file:///g:/Robot_v2/机器人手臂/web/arm-ik-solver.js) §5c：利用降阶链 `w = Rx(θ1)·Ry(θ2)·Rz(−θ3)·Ry(s)·ẑ`（s=θ4+θ5），给定 (θ3,s) 对 (θ1,θ2) 闭式求解，再在 (θ3,s,θ4) 三维上粗扫 + 坐标下降精修；θ6 由 `rollClose` 闭式补足 ⇒ 轴对齐后 oerr 恒为 0 | | 实时快路 | `solveWarm` 在 `'pos'` 下不再要求 oerr ≤ 0.05°（否则拖进"姿态不可达区"后每帧回落全量），改以 oerr 为主键选解 | | 界面 | [viewer_tpl.html](file:///g:/Robot_v2/机器人手臂/src/viewer_tpl.html) 姿态控制框内新增「优先级」下拉（位置优先 / 姿态优先，默认位置优先）；[ik-glue.js](file:///g:/Robot_v2/机器人手臂/web/ik-glue.js) 传入该选项、结果栏标注模式、并对姿态优先的"姿态到达·位置最近 X mm"单独置徽标 | | 未实施 | 「折中」`min ‖fk−T‖² + k·oerr²`（k 可调）——需要 6 维联合优化，留待 S3 一并处理 | **验证结果（本次实测）**： | 检查 | 结果 | | :--- | :--- | | `axisSolve` 闭式解正确性（20000 组随机位形回代） | 未命中 0，最大角差 **4.99e-9°** | | `fkChain` 独立复核位置精度（位置优先） | 恒 **~1e-13 mm**（如 1.66e-13 mm） | | 姿态可达时与原行为一致 | `'pos'` 59/60、`'ori'` **40/40** 与默认解逐位一致（差 1 例落在 0.05° 达标边界） | | 位置优先 vs θ6 最优 1° 网格基准（10 个不可达目标） | 劣于基准 **0/10**，最大劣化 **0.000°**；姿态差均值 **11.4°** vs 默认 DLS 的 **121.0°**（改善 **109.6°**） | | 姿态优先的精确性（8 个不可达目标） | 姿态差 **0.0000°**（最大 1.21e-6°）；位置均值 **24.35 mm** vs 惩罚下降参考的 24.76 mm | | 姿态优先耗时 | 18~100 ms（均值 **52 ms**），低于其替代的 DLS（85~125 ms） | | 既有自检 `pose_ik_check.js` 7 项 | **全部通过**（默认路径零回归） | | 关键目标实测 | `T=(20,400,300)`：默认 DLS 136.7° → 位置优先 **44.19°**（基准 44.20°）、姿态优先 **0.0000°/116.4mm** | **收益**：位置优先把 §6.3 的平均劣化 **110.3°** 压到接近 0（实测不劣于基准）；姿态优先提供严格的"保角度"解（0.0000°），位置代价如实上报。 ### S1（根因修复）覆盖率完备化，消除缺陷 A **方案**（二选一或组合）： 1. **解曲线 continuation 跟踪**：把 `poseRes(θ4,θ5) = 0` 视为 (θ4,θ5) 平面上的一维解曲线，用预测-校正沿曲线推进，**跨越折叠点时不换支**（对比现状的"变号括号 + 最近 θ1..θ3 匹配"，后者在退化处失效）； 2. **分支索引对齐的多起点牛顿 + 退化种子**：以 `branch()` 的**支下标**（0..3）而非"θ1..θ3 最近"做同支判据；在 θ3∈{0, ±180}、各轴贴限位处、以及 rowSpans 的段端点额外播种。 同时把 [rowSpans](file:///g:/Robot_v2/机器人手臂/web/arm-ik-solver.js#L644-L654) 的"单一连续段"假设放宽为多段（探测步 hp 与段数一起记录）。 **改动点**：`solve()` §4.1b（[L697-761](file:///g:/Robot_v2/机器人手臂/web/arm-ik-solver.js#L697-L761)）、`poseTools` 的 `poseQAt` 同支匹配（[L495-504](file:///g:/Robot_v2/机器人手臂/web/arm-ik-solver.js#L495-L504)）、`rowSpans`。 **预期收益**：覆盖漏洞率 0.25%→<0.05%（D1 口径），近伸直构型 3.3%→<0.5%；最大静默残差 1.934°→≤0.05°。 **代价**：**不增加粗扫密度**，只在退化行补一次定向播种/跟踪，预计耗时增量 <10%，仍满足 ≤100 ms。 ### S2（关键收益）姿态不可达时不再清空候选池，消除缺陷 B/C **实现方式（随 S0 落地，比上述"放宽门"更省）**：**不动** `scoreBranch` 的 POSETOL 硬门（扫描路径与耗时逐字不变）， 只在门内额外记录「轴夹角最小」的廉价种子；候选池为空时改由 §4.5 从种子出发做最小残差搜索。 这样既拿到"不清空信息"的全部收益，又避免为超门候选逐个算完整滚转（实测那会让粗扫开销翻倍级增长）。 **改动点**：`solve()` §4.5（新增）、`scoreBranch` 的种子记录、选解段 `poolByOerr` 分支。 **预期收益**：`closest6` 被支配的现象**直接消失**（不需要再走兜底），平均劣化 110.3°→≈0（相对真最优，实测不劣于基准）。 **代价**：极小（常态零开销，仅在落选路径多一次局部搜索）。这是本次评估中**性价比最高的一条**。 ### S3 兜底路径加速与改造 **方案**：位置也越界时才需要真正"最近位姿"。此时： 1. 用 S2 得到的闭式候选（位置精确解集的最优者）作为 DLS 初值，取代 160 次随机重启； 2. 只在位置完全不可达时保留受限 DLS（重启次数降到 16~32），并对 J1/J3/J6 三个连续轴改用最短角差而非硬夹限。 **预期收益**：兜底耗时 85~125 ms→≤60 ms；位置不可达时结果优于现状（现状 `T=(400,300,200)` 距目标 104.0 mm / 姿态差 172.0°）。 **代价**：需保证"位置越界"判定与现有 `tol` 口径一致，风险低。 ### S4 可观测性（防止静默失败再次发生） **方案**：把 UI 徽标从两态扩为四态，并同步修正 §6.1 提到的错误悬停说明： - `已到达`（位置精确 + 姿态 ≤0.05°） - `位置到达·姿态最近 X.XX°`（位置精确、姿态未达标，**明确说明这是"最近解"而非失败**） - `姿态到达·位置最近 Y.Y mm`（姿态优先模式） - `均不可达·最近位姿（位置 Y mm / 姿态 X°）` **改动点**：[renderResult L318-342](file:///g:/Robot_v2/机器人手臂/web/ik-glue.js#L318-L342)；[viewer_tpl.html L229](file:///g:/Robot_v2/机器人手臂/src/viewer_tpl.html#L229) 的 `ikStep` 说明文字。 ### 策略汇总 | 策略 | 针对缺陷 | 收益 | 成本 | 是否影响 ≤100 ms | 状态 | | :--- | :--- | :--- | :--- | :--- | :--- | | S0 双优先级 | 用户诉求（保点/保角度可切换） | 位置优先劣化 →≈0；姿态优先 0.0000° | 中 | 姿态优先 18~100 ms（均值 52 ms），未超预算 | **已实施** | | S1 覆盖完备化 | A | 漏洞率 →<0.05% | 中 | 增量 <10% | 待实施 | | S2 不清空候选池 | B / C | 兜底劣化 →≈0，常态零额外开销 | **极低** | 无 | **已随 S0 实施** | | S3 兜底加速 + 折中模式 | D + 越界质量 | 85~125 ms→≤60 ms；补上 blend 模式 | 低 | 改善 | 待实施 | | S4 可观测性 | 静默失败 | 漏解/取舍不再被当成"成功" | 低 | 无 | 部分（结果栏已标注模式与"姿态到达·位置最近"） | --- ## 8. 验证方案与验收口径 ### 8.1 基准数据集（固定种子，命令见附录） | 集 | 内容 | 规模 | 用途 | | :--- | :--- | :--- | :--- | | D1 | 全空间随机可达位形 | 400 | 覆盖率 / 静默残差 / 耗时 | | D2 | 前伸工作区 (20,210,300)±90mm；默认区 (150,128,260)±60mm | 60 + 60 | 常规工作区回归 | | D3 | 目标姿态外推 20° | 120 目标 | 兜底劣化 | | D4b | 近伸直 θ3≈0/±180 | 30 | 退化构型覆盖 | | D5 | 越界目标（4 个定点） | 4 | 兜底耗时与质量 | | D6 | 双优先级不可兼得场景 | 12 | 保点/保角度取舍 | | O2 | 真最优基准（θ6 解析优化） | 12 | 独立真值 | ### 8.2 验收口径（现状 → 目标） | 指标 | 现状实测 | 目标 | 当前状态 | | :--- | ---: | ---: | :--- | | D1 覆盖漏洞率（step 0.25 相对 0.05） | 0.25%（1/400） | ≤0.05% | 待 S1 | | D4b 近伸直覆盖漏洞率 | 3.3%（1/30） | ≤0.5% | 待 S1 | | 最大静默姿态残差 | **1.934°** | ≤0.05° | 待 S1 | | 步长敏感度（同目标 oerr 波动） | **1.9881 − 0.0258 = 1.96°** | ≤0.05° | 待 S1 | | 兜底相对真最优劣化（12 目标均值） | **110.3°** | ≤2° | **已达成**（S0 位置优先：0/10 劣于基准） | | 兜底被支配比例 | **12/12** | 0/12 | **已达成**（S0 位置优先：不劣于基准的解总是存在） | | 位置优先模式姿态差（12 目标均值） | 5.7°（真最优基准） | ≈真最优（劣化 ≤1°） | **已达成** | | 姿态优先模式姿态差 | **0.0000°**（最大 1.21e-6°） | ≤0.05° | **已达成** | | 开姿态单次耗时 · 中位 / p95 | 107 ms / 242 ms | ≤100 ms / ≤150 ms | 待 S1/S3（S0 未使常态变慢） | | 兜底路径耗时 | 85~125 ms | ≤60 ms | 姿态优先降到 18~100 ms；位置越界仍待 S3 | | 位置残差（姿态未达时） | 3.5e-13 mm（不得牺牲） | 保持 ≤1e-6 mm | **已达成**（S0 位置优先实测 ~1e-13 mm） | ### 8.3 回归门与新增门 - **沿用**：`node scratch/pose_ik_check.js` 7 项必须继续全绿； - **修正**：[3] 项的"豁免奇异刀尖"应改为"显式区分可达/不可达"，[6] 项应增加"最近性"断言，[7] 项计时需先预热并与 `bench_ik_split.js` 对齐（现状"开比关快"不可靠）； - **新增**：把 §8.1 的 D1/D3/D4b/D6 固化为可重复的覆盖率与最优性断言（当前仓库缺少这两类断言，正是缺陷 A/B 长期存活的直接原因）。 --- ## 9. 附录：可复现实验命令 统一环境：在项目根目录 `g:\Robot_v2\机器人手臂` 下执行；`ArmIK` 由 `web/arm-ik-solver.js` 加载；关节表/限位取自 `web/mesh_arm.json`、`docs/travel.json`；`tcp=[18.5,128,93]`。 所有随机采样使用固定种子的 **mulberry32**（`s = seed>>>0`，`s=(s+0x6D2B79F5)>>>0; t=s; t=Math.imul(t^(t>>>15),t|1); t^=t+Math.imul(t^(t>>>7),t|61); return ((t^(t>>>14))>>>0)/4294967296;`），结果逐位可复现。 ### A1 / A2 — 覆盖率、静默残差、工作区回归（对应 §6.1、§6.4） ``` node -e 'const fs=require("fs");const A=require("./web/arm-ik-solver.js"); const mesh=JSON.parse(fs.readFileSync("./web/mesh_arm.json","utf8"));const tr=JSON.parse(fs.readFileSync("./docs/travel.json","utf8")); const J={},T={};for(const j of mesh.joints)J[j.id]={rear:j.rear,d:j.d};for(const t of tr)T[t.axis]={lo:t.lo,hi:t.hi,continuous:!!t.continuous}; const S=A.make({joints:J,travel:T,tcp:[18.5,128,93]});const O=A.ORDER;const LIM=S.LIM; const mkseed=v=>{let s=v>>>0;return()=>{s=(s+0x6D2B79F5)>>>0;let t=s;t=Math.imul(t^(t>>>15),t|1);t^=t+Math.imul(t^(t>>>7),t|61);return ((t^(t>>>14))>>>0)/4294967296;};}; const rnd=mkseed(20260929);const rndQ=()=>O.map(id=>{const t=LIM[id];return t.cont?(rnd()*720-360):(t.lo+rnd()*(t.hi-t.lo));}); console.log("[A1] D1 全空间采样(seed=20260929) step0.25 两阶段"); const N=400;let fails=[],sum=0; for(let k=0;k0.05)fails.push({T:Tp,R,oa:va});} console.log(" N="+N+" oerr>0.05 数量="+fails.length+" 比例="+(fails.length/N*100).toFixed(2)+"pct 均耗时="+(sum/N).toFixed(0)+"ms"); let gap=0,maxS=0,maxG=0,tbl=[]; for(const f of fails){const b=S.solve(f.T,{step:0.05,tol:1e-6,pose:{R:f.R}});const vb=b.q?b.oerr:999; if(f.oa>maxS)maxS=f.oa;const g=f.oa-vb;if(g>0.05){gap++;if(g>maxG)maxG=g;} tbl.push({T:f.T.map(v=>+v.toFixed(1)),oa:+f.oa.toFixed(3),ob:+vb.toFixed(3),gap:+g.toFixed(3)});} console.log(" 其中覆盖漏洞(细扫能达标)="+gap+" 真近奇异="+(fails.length-gap)+" 最大静默="+maxS.toFixed(3)+"deg 最大覆盖差="+maxG.toFixed(3)+"deg"); console.log(" 明细="+JSON.stringify(tbl.slice(0,10))); const r2=mkseed(777001);const rq2=()=>O.map(id=>{const t=LIM[id];return t.cont?(r2()*720-360):(t.lo+r2()*(t.hi-t.lo));}); function zone(name,T0,rad,nmax){const pts=[];for(let k=0;k<3000000&&pts.length0.05)f2.push({Tp:p.Tp,R:p.R,oa:va});} let g2=0,mS=0,mG=0;for(const f of f2){const b=S.solve(f.Tp,{step:0.05,tol:1e-6,pose:{R:f.R}});const vb=b.q?b.oerr:999;if(f.oa>mS)mS=f.oa;const g=f.oa-vb;if(g>0.05){g2++;if(g>mG)mG=g;}} console.log("["+name+"] N="+pts.length+" 失败="+f2.length+" 覆盖漏洞="+g2+" 最大静默="+mS.toFixed(3)+"deg 均耗时="+(s2/pts.length).toFixed(0)+"ms");} console.log("[A2] D2 工作区采样(seed=777001)"); zone("前伸工作区 (20,210,300)+-90mm",[20,210,300],90,60);' ``` （查看器默认区 `(150,128,260)±60mm` 把最后一行换成 `zone("查看器默认区 (150,128,260)+-60mm",[150,128,260],60,60);` 再跑一次。） > 实测输出： > `[A1] N=400 oerr>0.05 数量=2 比例=0.50pct 均耗时=100ms` / `覆盖漏洞=1 真近奇异=1 最大静默=1.934deg 最大覆盖差=1.759deg` > `明细=[{"T":[152.6,-315.9,456.2],"oa":1.785,"ob":0.026,"gap":1.759},{"T":[403.7,162.3,599.9],"oa":1.934,"ob":1.994,"gap":-0.061}]` > `[前伸工作区] N=60 失败=0 覆盖漏洞=0 均耗时=98ms` / `[查看器默认区] N=60 失败=0 覆盖漏洞=0 均耗时=98ms` ### A3 / A4 — 兜底劣化与双优先级取舍（对应 §3.3、§6.3） ``` node -e 'const fs=require("fs");const A=require("./web/arm-ik-solver.js"); const mesh=JSON.parse(fs.readFileSync("./web/mesh_arm.json","utf8"));const tr=JSON.parse(fs.readFileSync("./docs/travel.json","utf8")); const J={},T={};for(const j of mesh.joints)J[j.id]={rear:j.rear,d:j.d};for(const t of tr)T[t.axis]={lo:t.lo,hi:t.hi,continuous:!!t.continuous}; const S=A.make({joints:J,travel:T,tcp:[18.5,128,93]});const O=A.ORDER;const LIM=S.LIM;const K=S.K; let s=60606>>>0;const rnd=()=>{s=(s+0x6D2B79F5)>>>0;let t=s;t=Math.imul(t^(t>>>15),t|1);t^=t+Math.imul(t^(t>>>7),t|61);return ((t^(t>>>14))>>>0)/4294967296;}; const rndQ=()=>O.map(id=>{const t=LIM[id];return t.cont?(rnd()*720-360):(t.lo+rnd()*(t.hi-t.lo));}); const inLim=q=>{for(let i=0;i<5;i++){const t=LIM[O[i]];if(!(q[i]>=t.lo-1e-9&&q[i]<=t.hi+1e-9))return false;}return true;}; const mul3=(X,Y)=>{const C=new Array(9);for(let i=0;i<3;i++)for(let j=0;j<3;j++)C[i*3+j]=X[i*3]*Y[j]+X[i*3+1]*Y[3+j]+X[i*3+2]*Y[6+j];return C;}; const rotAx=(d,a)=>{const x=d[0],y=d[1],z=d[2],c=Math.cos(a),sn=Math.sin(a),C=1-c;return [x*x*C+c,x*y*C-z*sn,x*z*C+y*sn,y*x*C+z*sn,y*y*C+c,y*z*C-x*sn,z*x*C-y*sn,z*y*C+x*sn,z*z*C+c];}; const T3=M=>[M[0],M[3],M[6],M[1],M[4],M[7],M[2],M[5],M[8]]; /* 位置精确解在 (θ4,θ5) 处只定到 θ1..θ5；θ6 仍可自由选取。度量「可达到的最小姿态差」 必须让 θ6 取最优（与求解器 rollClose 同口径：N = M5ᵀ·R_tar 应为 Rz(−θ6)）。 初版漏了这一步，用 θ6=0 度量会系统性高估最小姿态差（见文首勘误）。 */ function oerrOpt(q,Rt){const M5=S.orient(q,5);const N=mul3(T3(M5),Rt); const th6=-Math.atan2(N[3],N[0])*180/Math.PI; return A.rotErrDeg(mul3(M5,rotAx([0,0,-1],th6*Math.PI/180)),Rt);} 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 met(q,Tp,Rt){const p=S.fk(q,0);return {pe:Math.hypot(p[0]-Tp[0],p[1]-Tp[1],p[2]-Tp[2]),oe:A.rotErrDeg(S.orient(q),Rt)};} function posBest(Tp,Rt,st){const Aa=Tp[0]-K.X0,Bq=Tp[1]-K.Y0,Br=Tp[2]-K.Z1;const Rr=Math.hypot(Bq,Br),ph=Math.atan2(Br,Bq); let be=999;for(let t4=-111;t4<=16;t4+=st)for(let t5=-95;t5<=34;t5+=st){const ss=S.branch(t4,t5,Aa,Rr,ph,0); for(const s0 of ss){if(!inLim(s0))continue;const p=S.fk(s0,0);if(Math.hypot(p[0]-Tp[0],p[1]-Tp[1],p[2]-Tp[2])>1e-6)continue;const e=oerrOpt(s0,Rt);if(e{const t=LIM[id];return t.lo+rnd()*(t.hi-t.lo);}); let c0=(m=>m.pe*m.pe+k*m.oe*m.oe)(met(q,Tp,Rt));let step=20; for(let pass=0;pass<7;pass++){let mv=true,g=0; while(mv&&g++<200){mv=false; for(let i=0;i<6;i++){const t=LIM[O[i]]; for(const sg of [1,-1]){const old=q[i];const v=old+sg*step;if(vt.hi)continue;q[i]=v; const m=met(q,Tp,Rt);const c1=m.pe*m.pe+k*m.oe*m.oe;if(c1mx)mx=dg;if(dg+v.toFixed(0)),base:+r.oerr.toFixed(1),best:+ob.toFixed(2),degr:+dg.toFixed(1)});} console.log(" 不可达样本="+n+" 平均劣化="+(n?sd/n:0).toFixed(1)+"deg 最大="+mx.toFixed(1)+" 最小="+(n?mn:0).toFixed(1)); console.log(" 明细="+JSON.stringify(rows)); console.log("[A4] D6 双优先级取舍(seed=60606) 同批目标"); let n6=0,aBase=0,aPos=0,aOri=0,aOriP=0; for(let k=0;k<120&&n6<12;k++){const q=rndQ(),Tp=S.fk(q,0),R0=S.orient(q);const ax=nrm([rnd()-0.5,rnd()-0.5,rnd()-0.5]);const Rt=mul3(rotAx(ax,20*Math.PI/180),R0); const r=S.solveTo(Tp,{step:0.25,tol:1e-6,pose:{R:Rt}}); if(r.reachable||r.oerr==null||r.oerr<5)continue; const pb=posBest(Tp,Rt,0.25);const qo=desc(Tp,Rt,1e3,120);const mo=met(qo,Tp,Rt); n6++;aBase+=r.oerr;aPos+=pb;aOri+=mo.oe;aOriP+=mo.pe; console.log(" T=("+Tp.map(v=>v.toFixed(0)).join(",")+") closest6="+r.oerr.toFixed(1)+"deg/"+(r.err!=null?r.err.toFixed(1):"--")+"mm | 位置优先="+pb.toFixed(2)+"deg/0.0mm | 姿态优先="+mo.oe.toFixed(2)+"deg/"+mo.pe.toFixed(1)+"mm");} console.log(" 小结 采样="+n6+" 均值 closest6="+(n6?aBase/n6:0).toFixed(1)+"deg 位置优先="+(n6?aPos/n6:0).toFixed(1)+"deg 姿态优先="+(n6?aOri/n6:0).toFixed(2)+"deg(位置"+(n6?aOriP/n6:0).toFixed(1)+"mm)");' ``` （`posBest` 即"位置优先真最优"的网格枚举（θ6 取最优口径），`desc(Tp,Rt,1e3,120)` 即"姿态优先"的惩罚坐标下降；A4 的分组与 A3 不同是因为 `desc` 额外消耗了随机数。） > 实测输出（勘误后重跑）： > `[A3] 不可达样本=38 平均劣化=106.3deg 最大=176.7 最小=16.1` > `明细=[{"T":[364,336,843],"base":114.2,"best":5.98,"degr":108.2},{"T":[288,-16,595],"base":166.8,"best":7.94,"degr":158.9},{"T":[72,376,161],"base":107.2,"best":5.29,"degr":101.9},{"T":[311,10,701],"base":138.2,"best":14.72,"degr":123.4},{"T":[-52,242,897],"base":121.1,"best":3.18,"degr":117.9},{"T":[4,-218,303],"base":156.3,"best":13.02,"degr":143.3},{"T":[279,-155,765],"base":173,"best":3.13,"degr":169.9},{"T":[402,202,425],"base":81.2,"best":16.45,"degr":64.8}]` > `[A4] 采样=12 均值 closest6=115.3deg 位置优先=5.7deg 姿态优先=0.04deg(位置11.6mm)` ### F — 静默近失案例与步长敏感度（对应 §6.1） ``` node -e 'const fs=require("fs");const A=require("./web/arm-ik-solver.js"); const mesh=JSON.parse(fs.readFileSync("./web/mesh_arm.json","utf8"));const tr=JSON.parse(fs.readFileSync("./docs/travel.json","utf8")); const J={},T={};for(const j of mesh.joints)J[j.id]={rear:j.rear,d:j.d};for(const t of tr)T[t.axis]={lo:t.lo,hi:t.hi,continuous:!!t.continuous}; const S=A.make({joints:J,travel:T,tcp:[18.5,128,93]});const O=A.ORDER;const LIM=S.LIM;const K=S.K; let s=424242>>>0;const rnd=()=>{s=(s+0x6D2B79F5)>>>0;let t=s;t=Math.imul(t^(t>>>15),t|1);t^=t+Math.imul(t^(t>>>7),t|61);return ((t^(t>>>14))>>>0)/4294967296;}; const rndQ=()=>O.map(id=>{const t=LIM[id];return t.cont?(rnd()*720-360):(t.lo+rnd()*(t.hi-t.lo));}); const inLim=q=>{for(let i=0;i<5;i++){const t=LIM[O[i]];if(!(q[i]>=t.lo-1e-9&&q[i]<=t.hi+1e-9))return false;}return true;}; let F=null;for(let k=0;k<4000&&!F;k++){const q=rndQ(),Tp=S.fk(q,0),R=S.orient(q);const a=S.solve(Tp,{step:0.25,tol:1e-6,pose:{R:R}});if(a.q&&a.oerr>1.0)F={q,T:Tp,R};} console.log("真解 q="+F.q.map(v=>+v.toFixed(2)).join(",")+" T="+F.T.map(v=>+v.toFixed(3)).join(",")); let out=[];for(const st of [0.5,0.4,0.3,0.25,0.2,0.15,0.1,0.05]){const r=S.solve(F.T,{step:st,tol:1e-6,pose:{R:F.R}});out.push(st+"->"+(r.q?r.oerr.toFixed(4):"null"));} console.log("步长敏感度 oerr: "+out.join(" ")); const rw=S.solveTo(F.T,{step:0.25,tol:1e-6,pose:{R:F.R},warm:F.q}); console.log("warm=trueQ: oerr="+(rw.oerr!=null?rw.oerr.toExponential(2):"null")); const Aa=F.T[0]-K.X0,Bq=F.T[1]-K.Y0,Br=F.T[2]-K.Z1;const Rr=Math.hypot(Bq,Br),ph=Math.atan2(Br,Bq); for(const c of S.branch(F.q[3],F.q[4],Aa,Rr,ph,0)){const p=S.fk(c,0);console.log(" branch posErr="+Math.hypot(p[0]-F.T[0],p[1]-F.T[1],p[2]-F.T[2]).toExponential(1)+"mm θ1..θ5="+c.slice(0,5).map(v=>+v.toFixed(2)).join(",")+" inLim="+inLim(c));}' ``` > 实测输出： > `真解 q=198.03,-46.76,-180.52,-92.79,33.48,102.36 T=26.386,-3.334,1019.998` > `步长敏感度 oerr: 0.5->1.9881 0.4->1.9119 0.3->0.0307 0.25->1.9881 0.2->1.9881 0.15->1.9652 0.1->0.0258 0.05->0.0491` > `warm=trueQ: oerr=1.21e-6`（真解确实存在） > `branch posErr=1.4e-13mm ... inLim=true`（退化处仍有在限位内的位置精确支） ### O2 — 严格真最优基准（θ6 解析优化，对应 §6.3） ``` node -e 'const fs=require("fs");const A=require("./web/arm-ik-solver.js"); const mesh=JSON.parse(fs.readFileSync("./web/mesh_arm.json","utf8"));const tr=JSON.parse(fs.readFileSync("./docs/travel.json","utf8")); const J={},T={};for(const j of mesh.joints)J[j.id]={rear:j.rear,d:j.d};for(const t of tr)T[t.axis]={lo:t.lo,hi:t.hi,continuous:!!t.continuous}; const S=A.make({joints:J,travel:T,tcp:[18.5,128,93]});const O=A.ORDER;const LIM=S.LIM;const K=S.K; let s=60606>>>0;const rnd=()=>{s=(s+0x6D2B79F5)>>>0;let t=s;t=Math.imul(t^(t>>>15),t|1);t^=t+Math.imul(t^(t>>>7),t|61);return ((t^(t>>>14))>>>0)/4294967296;}; const rndQ=()=>O.map(id=>{const t=LIM[id];return t.cont?(rnd()*720-360):(t.lo+rnd()*(t.hi-t.lo));}); const inLim=q=>{for(let i=0;i<5;i++){const t=LIM[O[i]];if(!(q[i]>=t.lo-1e-9&&q[i]<=t.hi+1e-9))return false;}return true;}; const mul3=(X,Y)=>{const C=new Array(9);for(let i=0;i<3;i++)for(let j=0;j<3;j++)C[i*3+j]=X[i*3]*Y[j]+X[i*3+1]*Y[3+j]+X[i*3+2]*Y[6+j];return C;}; const rotAx=(d,a)=>{const x=d[0],y=d[1],z=d[2],c=Math.cos(a),sn=Math.sin(a),C=1-c;return [x*x*C+c,x*y*C-z*sn,x*z*C+y*sn,y*x*C+z*sn,y*y*C+c,y*z*C-x*sn,z*x*C-y*sn,z*y*C+x*sn,z*z*C+c];}; const T3=M=>[M[0],M[3],M[6],M[1],M[4],M[7],M[2],M[5],M[8]]; const nrm=v=>{const n=Math.hypot(v[0],v[1],v[2])||1;return [v[0]/n,v[1]/n,v[2]/n];}; /* θ6 取最优口径（同文首勘误与 A3 的 oerrOpt） */ function oerrOpt(q,Rt){const M5=S.orient(q,5);const N=mul3(T3(M5),Rt); const th6=-Math.atan2(N[3],N[0])*180/Math.PI; return A.rotErrDeg(mul3(M5,rotAx([0,0,-1],th6*Math.PI/180)),Rt);} function oracle(Tp,Rt,st){const Aa=Tp[0]-K.X0,Bq=Tp[1]-K.Y0,Br=Tp[2]-K.Z1;const Rr=Math.hypot(Bq,Br),ph=Math.atan2(Br,Bq); let b1=999; for(let t4=-111;t4<=16;t4+=st)for(let t5=-95;t5<=34;t5+=st){const ss=S.branch(t4,t5,Aa,Rr,ph,0); for(const s0 of ss){if(!inLim(s0))continue;const p=S.fk(s0,0);if(Math.hypot(p[0]-Tp[0],p[1]-Tp[1],p[2]-Tp[2])>1e-6)continue;const e=oerrOpt(s0,Rt);if(ev.toFixed(0)).join(",")+") closest6="+r.oerr.toFixed(1)+"deg 真最优="+o.opt6.toFixed(2)+"deg 劣化="+(r.oerr-o.opt6).toFixed(1)+"deg"); a+=r.oerr;c+=o.opt6;if(r.oerr>o.opt6)dom++;} console.log(" 均值 closest6="+(a/n).toFixed(1)+"deg 真最优="+(c/n).toFixed(1)+"deg 平均劣化="+((a-c)/n).toFixed(1)+"deg 被支配="+dom+"/"+n);' ``` （O2 与 A4 目标集不同：`desc()` 会额外消耗随机数，故两者 close6 均值略有差异。两者均固定种子、均可复现。） > 实测输出（勘误后重跑）： > `(364,336,843) closest6=114.2deg 真最优=5.98deg 劣化=108.2deg` … `(137,-315,829) closest6=133.5deg 真最优=6.20deg 劣化=127.3deg` > `均值 closest6=118.9deg 真最优=8.6deg 平均劣化=110.3deg 被支配=12/12` ### D5 — 越界目标兜底（对应 §5.2、§6.2） ``` node -e 'const fs=require("fs");const A=require("./web/arm-ik-solver.js"); const mesh=JSON.parse(fs.readFileSync("./web/mesh_arm.json","utf8"));const tr=JSON.parse(fs.readFileSync("./docs/travel.json","utf8")); const J={},T={};for(const j of mesh.joints)J[j.id]={rear:j.rear,d:j.d};for(const t of tr)T[t.axis]={lo:t.lo,hi:t.hi,continuous:!!t.continuous}; const S=A.make({joints:J,travel:T,tcp:[18.5,128,93]});const O=A.ORDER;const LIM=S.LIM;const K=S.K; const inLim=q=>{for(let i=0;i<5;i++){const t=LIM[O[i]];if(!(q[i]>=t.lo-1e-9&&q[i]<=t.hi+1e-9))return false;}return true;}; const mul3=(X,Y)=>{const C=new Array(9);for(let i=0;i<3;i++)for(let j=0;j<3;j++)C[i*3+j]=X[i*3]*Y[j]+X[i*3+1]*Y[3+j]+X[i*3+2]*Y[6+j];return C;}; const rotAx=(d,a)=>{const x=d[0],y=d[1],z=d[2],c=Math.cos(a),sn=Math.sin(a),C=1-c;return [x*x*C+c,x*y*C-z*sn,x*z*C+y*sn,y*x*C+z*sn,y*y*C+c,y*z*C-x*sn,z*x*C-y*sn,z*y*C+x*sn,z*z*C+c];}; const T3=M=>[M[0],M[3],M[6],M[1],M[4],M[7],M[2],M[5],M[8]]; function oerrOpt(q,Rt){const M5=S.orient(q,5);const N=mul3(T3(M5),Rt); const th6=-Math.atan2(N[3],N[0])*180/Math.PI; return A.rotErrDeg(mul3(M5,rotAx([0,0,-1],th6*Math.PI/180)),Rt);} function posBest(Tp,Rt,st){const Aa=Tp[0]-K.X0,Bq=Tp[1]-K.Y0,Br=Tp[2]-K.Z1;const Rr=Math.hypot(Bq,Br),ph=Math.atan2(Br,Bq); let be=999,cnt=0;for(let t4=-111;t4<=16;t4+=st)for(let t5=-95;t5<=34;t5+=st){const ss=S.branch(t4,t5,Aa,Rr,ph,0); for(const s0 of ss){if(!inLim(s0))continue;const p=S.fk(s0,0);if(Math.hypot(p[0]-Tp[0],p[1]-Tp[1],p[2]-Tp[2])>1e-6)continue;cnt++;const e=oerrOpt(s0,Rt);if(e 实测输出（勘误后重跑）： > `T=(20,400,300) 位置精确解=11920 真最优姿态差=44.20deg` > ` 默认DLS 位置=0.0mm 姿态差=136.7deg ｜ 位置优先 位置=1.3e-13mm 姿态差=44.19deg ｜ 姿态优先 位置=116.43mm 姿态差=0.0000deg` > `T=(150,128,600) 位置精确解=2580 真最优姿态差=14.72deg` > ` 默认DLS 位置=0.0mm 姿态差=173.4deg ｜ 位置优先 位置=0.0e+0mm 姿态差=14.72deg ｜ 姿态优先 位置=37.40mm 姿态差=0.0000deg` > `T=(400,300,200) 位置精确解=0 ｜ 默认DLS 位置=104.0mm 姿态差=172.0deg ｜ 姿态优先 位置=404.27mm 姿态差=0.0000deg`（位置不可达） > `T=(0,0,1200) 位置精确解=0 ｜ 默认DLS 位置=87.1mm 姿态差=130.0deg ｜ 姿态优先 位置=205.67mm 姿态差=0.0000deg`（位置不可达） > **复现说明**：非耗时类数值（残差、姿态差、可达性、候选数）逐位可复现；`ms` 随机器负载有 ±20% 波动。 ### V1 — 位置可达性/候选数（对应 §6.2） ``` node -e 'const fs=require("fs");const A=require("./web/arm-ik-solver.js"); const mesh=JSON.parse(fs.readFileSync("./web/mesh_arm.json","utf8"));const tr=JSON.parse(fs.readFileSync("./docs/travel.json","utf8")); const J={},T={};for(const j of mesh.joints)J[j.id]={rear:j.rear,d:j.d};for(const t of tr)T[t.axis]={lo:t.lo,hi:t.hi,continuous:!!t.continuous}; const S=A.make({joints:J,travel:T,tcp:[18.5,128,93]});const O=A.ORDER;const LIM=S.LIM;const K=S.K; const inLim=q=>{for(let i=0;i<5;i++){const t=LIM[O[i]];if(!(q[i]>=t.lo-1e-9&&q[i]<=t.hi+1e-9))return false;}return true;}; const mul3=(X,Y)=>{const C=new Array(9);for(let i=0;i<3;i++)for(let j=0;j<3;j++)C[i*3+j]=X[i*3]*Y[j]+X[i*3+1]*Y[3+j]+X[i*3+2]*Y[6+j];return C;}; const rotAx=(d,a)=>{const x=d[0],y=d[1],z=d[2],c=Math.cos(a),sn=Math.sin(a),C=1-c;return [x*x*C+c,x*y*C-z*sn,x*z*C+y*sn,y*x*C+z*sn,y*y*C+c,y*z*C-x*sn,z*x*C-y*sn,z*y*C+x*sn,z*z*C+c];}; const T3=M=>[M[0],M[3],M[6],M[1],M[4],M[7],M[2],M[5],M[8]]; function oerrOpt(q,Rt){const M5=S.orient(q,5);const N=mul3(T3(M5),Rt); const th6=-Math.atan2(N[3],N[0])*180/Math.PI; return A.rotErrDeg(mul3(M5,rotAx([0,0,-1],th6*Math.PI/180)),Rt);} const Tp=[20,400,300],Rt=A.poseMat(25,95,10); const Aa=Tp[0]-K.X0,Bq=Tp[1]-K.Y0,Br=Tp[2]-K.Z1;const Rr=Math.hypot(Bq,Br),ph=Math.atan2(Br,Bq); let cnt=0,be=999; for(let t4=-111;t4<=16;t4+=0.5)for(let t5=-95;t5<=34;t5+=0.5){const ss=S.branch(t4,t5,Aa,Rr,ph,0); for(const s0 of ss){if(!inLim(s0))continue;const p=S.fk(s0,0);if(Math.hypot(p[0]-Tp[0],p[1]-Tp[1],p[2]-Tp[2])>1e-6)continue;cnt++;const e=oerrOpt(s0,Rt);if(e 实测输出（勘误后重跑）： > `位置精确解数量=11920 其中最小姿态差(θ6最优)=44.20deg` > `无姿态 solve: ok=true 位置误差=2.5e-14mm` > `带姿态 solveTo(默认): reachable=false 位置=0.0mm 姿态差=136.7deg` > `带姿态 solveTo(位置优先): reachable=true 位置=1.3e-13mm 姿态差=44.19deg` ### D4b — 近伸直构型（对应 §6.1） ``` node -e 'const fs=require("fs");const A=require("./web/arm-ik-solver.js"); const mesh=JSON.parse(fs.readFileSync("./web/mesh_arm.json","utf8"));const tr=JSON.parse(fs.readFileSync("./docs/travel.json","utf8")); const J={},T={};for(const j of mesh.joints)J[j.id]={rear:j.rear,d:j.d};for(const t of tr)T[t.axis]={lo:t.lo,hi:t.hi,continuous:!!t.continuous}; const S=A.make({joints:J,travel:T,tcp:[18.5,128,93]});const O=A.ORDER;const LIM=S.LIM; let s2=909090>>>0;const rnd2=()=>{s2=(s2+0x6D2B79F5)>>>0;let t=s2;t=Math.imul(t^(t>>>15),t|1);t^=t+Math.imul(t^(t>>>7),t|61);return ((t^(t>>>14))>>>0)/4294967296;}; const rndQ2=()=>O.map(id=>{const t=LIM[id];return t.cont?(rnd2()*720-360):(t.lo+rnd2()*(t.hi-t.lo));}); let N=30,nFail=0,nGap=0,maxS=0,maxG=0,worst=[]; for(let k=0;k0.05)nFail++;if(va>0.05){if(va>maxS)maxS=va;if(va-vb>0.05){nGap++;if(va-vb>maxG)maxG=va-vb;worst.push({T:Tp.map(v=>+v.toFixed(1)),oa:+va.toFixed(3),ob:+vb.toFixed(3)});}}} console.log("[D4b] N="+N+" 失败="+nFail+" 覆盖漏洞="+nGap+" 最大静默="+maxS.toFixed(3)+"deg 最大覆盖差="+maxG.toFixed(3)); console.log("样本="+JSON.stringify(worst));' ``` > 实测输出：`[D4b] N=30 失败=1 覆盖漏洞=1 最大静默=1.860deg 最大覆盖差=1.822` > `样本=[{"T":[240.7,45.3,518.6],"oa":1.86,"ob":0.038}]` ### 既有自检与性能基准（对应 §5.1、§6.5） ``` node scratch/pose_ik_check.js ``` > 实测：7 项全部通过（明细见 §6.5）。注意其内部使用 `Math.random()`，为单次运行示例。 ``` node -e 'const j=require("./scratch/bench_ik_split.json");console.log("grid",JSON.stringify(j.grid));console.log("summary",JSON.stringify(j.summary,null,1));console.log("n",j.rows.length,"ms max",Math.max(...j.rows.map(r=>r.ms)).toFixed(0));' ``` > 实测：`grid {"g4":509,"g5":517}` · `summary {medMs:107.1, p95Ms:242.5, medPos:73.6, medPose:37.2, medShare:33.3, medOff:73.4, escRate:0.025}` · `n 40 ms max 299` --- ## 10. 术语表 | 术语 | 含义 | | :--- | :--- | | TCP | 工具中心点，此处指工具法兰安装面中心（`tcp=[18.5,128,93]`） | | `oerr` | 姿态残差（度），工具坐标系与目标姿态的旋转夹角 | | 位置精确解 | 由 `branch()` 闭式得到、位置误差 ~1e-13 mm 的关节解（θ1..θ3 由 (θ4,θ5) 决定） | | 退化构型 / 奇异刀尖 | 位置雅可比奇异、`branch()` 的 4 支两两重合的构型（本案为 θ3≈0 或 ±180） | | `POSETOL` | 姿态接受的粗扫阈值，2.0°；超阈候选现被直接丢弃 | | `closest6` | 6 维阻尼最小二乘"最近位姿"兜底（160 随机重启 × 160 迭代） | | 覆盖漏洞 | 精确解存在但求解器未找到（静默返回 >0.05° 残差） | | 被支配（Pareto-dominated） | 存在另一个解在位置与姿态两个指标上都不劣于此解 |