705 lines
55 KiB
Markdown
705 lines
55 KiB
Markdown
# 机械臂姿态估计算法评估报告
|
||
|
||
> 评估对象：`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;k<N;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}});sum+=a.ms;
|
||
const va=a.q?a.oerr:999;if(va>0.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.length<nmax;k++){const q=rq2();const p=S.fk(q,0);if(Math.hypot(p[0]-T0[0],p[1]-T0[1],p[2]-T0[2])<=rad)pts.push({Tp:p,R:S.orient(q)});}
|
||
let f2=[],s2=0;for(const p of pts){const a=S.solve(p.Tp,{step:0.25,tol:1e-6,pose:{R:p.R}});s2+=a.ms;const va=a.q?a.oerr:999;if(va>0.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<be)be=e;}}return be;}
|
||
function desc(Tp,Rt,k,starts){let best=null,bc=1e18;
|
||
for(let st=0;st<starts;st++){let q=O.map((id,i)=>{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(v<t.lo||v>t.hi)continue;q[i]=v;
|
||
const m=met(q,Tp,Rt);const c1=m.pe*m.pe+k*m.oe*m.oe;if(c1<c0-1e-12){c0=c1;mv=true;}else q[i]=old;}}}
|
||
step*=0.3;}
|
||
if(c0<bc){bc=c0;best=q.slice();}}
|
||
return best;}
|
||
console.log("[A3] D3 姿态外推20deg 兜底劣化(seed=60606) 采样120个目标");
|
||
let n=0,sd=0,mx=0,mn=1e9;const rows=[];
|
||
for(let k=0;k<120;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)continue;const ob=posBest(Tp,Rt,0.25);const dg=r.oerr-ob;
|
||
n++;sd+=dg;if(dg>mx)mx=dg;if(dg<mn)mn=dg;if(rows.length<8)rows.push({T:Tp.map(v=>+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(e<b1)b1=e;}}
|
||
return {opt6:b1};}
|
||
let n=0,a=0,c=0,dom=0;
|
||
for(let k=0;k<120&&n<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;n++;
|
||
const o=oracle(Tp,Rt,0.5);
|
||
console.log(" ("+Tp.map(v=>v.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<be)be=e;}}
|
||
return {e:be,cnt:cnt};}
|
||
const Rs=A.poseMat(25,95,10);
|
||
for(const Tp of [[20,400,300],[150,128,600],[400,300,200],[0,0,1200]]){
|
||
const o=posBest(Tp,Rs,0.5);const t0=Date.now();const r=S.solveTo(Tp,{step:0.25,tol:1e-6,pose:{R:Rs}});const ms=Date.now()-t0;
|
||
const rp=S.solveTo(Tp,{step:0.25,tol:1e-6,pose:{R:Rs},priority:"pos"});
|
||
const ro=S.solveTo(Tp,{step:0.25,tol:1e-6,pose:{R:Rs},priority:"ori"});
|
||
console.log("T=("+Tp.join(",")+") 位置精确解="+o.cnt+" 真最优姿态差="+(o.cnt?o.e.toFixed(2)+"deg":"n/a"));
|
||
console.log(" 默认DLS "+ms+"ms 位置="+(r.err!=null?r.err.toFixed(1):"--")+"mm 姿态差="+(r.oerr!=null?r.oerr.toFixed(1):"--")+"deg");
|
||
console.log(" 位置优先 位置="+(rp.err!=null?rp.err.toExponential(1):"--")+"mm 姿态差="+(rp.oerr!=null?rp.oerr.toFixed(2):"--")+"deg");
|
||
console.log(" 姿态优先 位置="+(ro.err!=null?ro.err.toFixed(2):"--")+"mm 姿态差="+(ro.oerr!=null?ro.oerr.toFixed(4):"--")+"deg");}'
|
||
```
|
||
|
||
> 实测输出（勘误后重跑）：
|
||
> `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<be)be=e;}}
|
||
console.log("位置精确解数量="+cnt+" 其中最小姿态差(θ6最优)="+be.toFixed(2)+"deg");
|
||
const rp=S.solve(Tp,{step:0.25,tol:1e-6});
|
||
console.log("无姿态 solve: ok="+rp.ok+" 位置误差="+(rp.err!=null?rp.err.toExponential(1):"--")+"mm");
|
||
const r6=S.solveTo(Tp,{step:0.25,tol:1e-6,pose:{R:Rt}});
|
||
console.log("带姿态 solveTo(默认): reachable="+r6.reachable+" 位置="+(r6.err!=null?r6.err.toFixed(1):"--")+"mm 姿态差="+(r6.oerr!=null?r6.oerr.toFixed(1):"--")+"deg");
|
||
const r5=S.solveTo(Tp,{step:0.25,tol:1e-6,pose:{R:Rt},priority:"pos"});
|
||
console.log("带姿态 solveTo(位置优先): reachable="+r5.reachable+" 位置="+(r5.err!=null?r5.err.toExponential(1):"--")+"mm 姿态差="+(r5.oerr!=null?r5.oerr.toFixed(2):"--")+"deg");'
|
||
```
|
||
|
||
> 实测输出（勘误后重跑）：
|
||
> `位置精确解数量=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;k<N;k++){const q=rndQ2();const m=rnd2();
|
||
if(m<0.5)q[2]=rnd2()*6-3; else q[2]=(rnd2()<0.5?180:-180)+(rnd2()*6-3);
|
||
const Tp=S.fk(q,0),R=S.orient(q);
|
||
const a=S.solve(Tp,{step:0.25,tol:1e-6,pose:{R:R}}),b=S.solve(Tp,{step:0.05,tol:1e-6,pose:{R:R}});
|
||
const va=a.q?a.oerr:999,vb=b.q?b.oerr:999;
|
||
if(va>0.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） | 存在另一个解在位置与姿态两个指标上都不劣于此解 |
|