# 姿态估计算法评估与优化 —— 实施计划

> 交付物（用户已确认）：**仅一份中文评估报告**，不改动求解器代码。
> 报告落盘位置：`docs/姿态估计算法评估报告.md`

---

## 1. 摘要（Summary）

本任务对机械臂的「姿态估计算法」——即 [arm-ik-solver.js](file:///g:/Robot_v2/机器人手臂/web/arm-ik-solver.js) 的 6 轴逆运动学（IK）姿态求解器及其查看器集成 [ik-glue.js](file:///g:/Robot_v2/机器人手臂/web/ik-glue.js)（用户已确认该文件即「姿态估计算法」）——做一次全面评估。

需要解决的核心问题：**在多个关键位姿（近工作空间边界、折叠/奇异构型、贴近关节限位处）无法达到预定姿态目标；且当目标姿态确实不可达时，返回的「最近位姿」明显劣于肉眼可见的更优姿态。**

评估已在本阶段（Plan 模式，只读）实测复现并定位到 4 类缺陷（见 §2.3），并额外回答了两个关键设计问题：**现状是「先点位、后角度」的位置优先架构**（§2.4），以及**位置与姿态在远目标上真正不可兼得、必须提供可切换优先级**（§2.4 实测取舍表）。本计划定义报告的**结构、数据来源、可复现实验命令、优化策略与验证口径**。执行阶段只产出一份报告 md，不新增任何脚本文件（实验以 `node -e` 内联执行，命令内嵌报告附录）。

---

## 2. 现状分析（Current State Analysis）

### 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/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)）、结果渲染与徽标判定（[renderResult 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}`，FK/IK 运动学常量来源 |
| 关节限位 | [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°，可行域改相邻件实测区间） |
| 既有自检 | [pose_ik_check.js](file:///g:/Robot_v2/机器人手臂/scratch/pose_ik_check.js) | 7 组 node 自检（往返、位置回归、姿态重构、交互口径、兜底、耗时） |
| 既有性能基准 | [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.2 现有算法骨架（报告需准确描述）

1. **位置闭式 + 二维冗余扫描**：以 (θ4,θ5) 为冗余参数全域扫描，θ1..θ3 由 [branch() L429-463](file:///g:/Robot_v2/机器人手臂/web/arm-ik-solver.js#L429-L463) 闭式给出（最多 4 支：θ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 未知量，解为**孤立点**；[poseTools L486-545](file:///g:/Robot_v2/机器人手臂/web/arm-ik-solver.js#L486-L545) 提供残差/牛顿原语；[§4.1b 根枚举 L697-761](file:///g:/Robot_v2/机器人手臂/web/arm-ik-solver.js#L697-L761) 逐行做变号括号 + 牛顿收口；滚转由 [rollClose L549-555](file:///g:/Robot_v2/机器人手臂/web/arm-ik-solver.js#L549-L555) 闭式补 θ6。
4. **接受门**：`POSETOL=2.0°`（[L581](file:///g:/Robot_v2/机器人手臂/web/arm-ik-solver.js#L581)）——`scoreBranch` 对 `oerr>POSETOL` 的候选**直接返回 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.3 已实测缺陷（本阶段只读复现，数据可直接写入报告）

**缺陷 A — 覆盖漏洞：目标姿态精确可达，求解器却静默返回 ~1–2° 残差**
- 全空间随机可达位形，`solve(step=0.25)` 返回 `oerr>0.05°` 的比例：**2.5%（3/120）**，量级 `oerr∈[1.24, 1.95]°`（真值应为 0）。
- 其中可明确归因于「覆盖漏洞」（细扫 step=0.05 能到 ≤0.05°、即确有大步长漏解）的部分，小样本估计 **0.3%–1.3%**（300 组样本：1/300；80 组样本：1/80）；其余为真·近奇异构型（细扫同样到不了 ≤0.05°，属物理不可达的刀尖构型）。**执行阶段用更大样本（D1/D4）收敛该比例。**
- 典型案例 `T=[329.3,253.4,750.0]`：真解 (θ4,θ5)=(-16.45,-51.97)，`branch()` 在该点给出 **位置误差 5.7e-14 mm、轴误差 0.000°** 的精确解；但 `solve(step=0.25)` 返回 oerr=1.909°。
- **步长敏感（非分辨率问题）**：同一目标 `oerr` 随 `step` 非单调跳变——`0.25°→1.60`、`0.15°→0.045`、`0.10°→1.14`、`0.05°→0.013`；用 `solveTo({warm:真解})` 则得到 **oerr=0.0000**。证明确切解存在，是**根枚举/牛顿收口的完备性缺口**。
- 工作区实测：前伸工作区 (20,210,300)±90mm 失败率 **1.7%（1/60）**；查看器默认区 (150,128,260)±60mm 为 **0%（0/60）**——缺口集中在工作空间外围/极端构型。

**缺陷 B — 不可达兜底 closest6 严重次优（「肉眼可见更优姿态」的直接来源）**
以「位置精确解中姿态误差的全局最小值」为真最优（对 (θ4,θ5) 网格 0.05° 枚举 `branch()` 全部在限位内的支），目标姿态在可达姿态基础上外推 20°：

| 案例 | 目标 TCP | 兜底返回 | 真最优姿态差 | 劣化 |
| :--- | :--- | ---: | ---: | ---: |
| 1 | (40,455,216) | 位置 0.4mm / 姿态 **74.0°** | **11.04°** | **63.0°** |
| 2 | (196,122,136) | 位置 0.5mm / 姿态 **130.0°** | **17.26°** | **112.8°** |
| 3 | (-141,-18,1030) | 位置 0.0mm / 姿态 **167.2°** | **89.09°** | **78.1°** |

**缺陷 C — 姿态不可达时把「位置精确解候选池」整体丢弃，导致兜底被便宜解严格支配**
- `T=[20,400,300]`、`R=poseMat(25,95,10)`：位置完全可达（无姿态 `solve` 返回 err=2.5e-14，0.5° 格上有 **11920** 个位置精确解），但姿态目标不可达时 `scoreBranch` 因 `oerr>POSETOL` 把全部候选置 null，`solve()` 返回 `ok=false`、候选池为空，被迫走 [closest6](file:///g:/Robot_v2/机器人手臂/web/arm-ik-solver.js#L1054-L1169) 的 6 维随机 DLS。
- 同例对照（实测）：位置精确解集内最小姿态差 = **121.05°**；`closest6` 返回 **136.7° / 位置 0.0mm**。即 `closest6` 在**位置与姿态两个指标上同时劣于**一个用现有闭式扫描就能廉价得到的解 —— **被严格支配（Pareto-dominated）**。
- 后果：`位置精确 + 姿态尽力` 这条最自然的路径没有实现；兜底既不最优、又更慢（159–224ms vs 扫描 ~100ms）。

**缺陷 D — 性能超出既定预算**
- [bench_ik_split.json](file:///g:/Robot_v2/机器人手臂/scratch/bench_ik_split.json) 摘要：开姿态单次 **中位 107ms / p95 242ms / 最大 299ms**（位置基座中位 73.6ms，姿态增量中位 37.2ms 占 33%，escalating 触发率 2.5%）。
- 本阶段在工作区实测均耗时 112–157ms；不可达兜底 159–224ms。
- 均高于用户设定的 **≤100ms** 目标，且兜底路径（random-restart DLS）是主要浪费点。

### 2.4 求解范式评估：位置优先 vs 末端姿态反推（用户提出，报告中作为备选简要说明）

**用户提问**：能否用已指定的末端角度，从末端逐级反推回各关节？

**几何事实（以 [mesh_arm.json](file:///g:/Robot_v2/机器人手臂/web/mesh_arm.json) 为准）**：

| 关节 | rear | 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，相距 110mm）⇒ 平行轴永不相交 ⇒ **非球形腕**，经典"腕心解耦 + θ6→θ5→θ4 逐级闭式反推"不成立。（J5⊥J6 在 (18.5,128,222) 相交，仅构成 2 轴腕。）
- 但存在可用的**降阶结构**：因 J4∥J5，`R4·R5 = R_y(θ4+θ5)`（代码 [L406-411](file:///g:/Robot_v2/机器人手臂/web/arm-ik-solver.js#L406-L411) 已用 `SAMEAX` 利用该性质），故

  ```
  R = Rx(θ1)·Ry(θ2)·Rz(−θ3)·Ry(θ4+θ5)·Rz(−θ6)
  ```

  ⇒ **工具轴只由 (θ1,θ2,θ3,θ4+θ5) 决定，θ6 是纯工具滚转**；"用末端姿态驱动求解"可行，但需按此降阶链求解（3 方程 / 5 参数 → 2 参数族），而非逐级反推。

**位置与姿态在远目标上"真的不可兼得"（实测，报告须收录）**：

| 目标 | 优先级 | 位置误差 | 姿态差 |
| :--- | :--- | ---: | ---: |
| T=(20,400,300), R=poseMat(25,95,10) | 位置优先（保点） | **0.0 mm** | 121.05° |
| 同上 | 姿态优先（保角度） | 127.6 mm | **0.22°** |
| T=(196,122,136), R=poseMat(25,95,10) | 位置优先 | **0.0 mm** | 116.39° |
| 同上 | 姿态优先 | 233.6 mm | **0.45°** |

⇒ 结论：二者是**真实的取舍**，不存在"都能兼顾"的解；算法必须提供**可切换的优先级**。

### 2.5 误差来源定位（报告核心结论）

- **R1 根枚举不完备**：姿态精确解在 (θ4,θ5) 平面是一条一维曲线，只在少数孤立点穿过 `POSETOL` 窄带；当该曲线经过 θ3≈±90° 的**折叠点**（`branch()` 的 4 支两两重合、位置雅可比奇异）、或贴着关节限位/可行段边缘时，变号括号检测失效、`match()` 的「θ1..θ3 最近」同支判据在折叠处换支，牛顿收口失败。
- **R2 接受门把「不可达」与「漏解」混为一谈**：`oerr>POSETOL` 一律丢弃 ⇒ 无法区分「确实不可达」与「可达但没找到」，导致 A 类静默近失 + C 类候选池清空。
- **R3 兜底目标函数与真实目标不一致**：`closest6` 用固定的 20mm/rad 把姿态折进位置残差做联合 DLS，既不保证「位置优先、姿态最小」，也依赖随机重启初值，落点常是劣质局部极值。
- **R4 行裁剪探测步过粗**：`hp=max(h,2)°` 且按单一连续段假设（注释称仅在 5 个目标上验证过），窄于 2° 的可行段会被整段漏掉 ⇒ 整行/整区无候选。

---

## 3. 交付物与报告大纲（Proposed Changes）

**唯一变更**：新建 `docs/姿态估计算法评估报告.md`（中文）。

报告章节（每章明确数据来源，均为本阶段或执行阶段 `node -e` 实测可得）：

1. **概述与结论摘要**（TL;DR 表格：缺陷 / 影响 / 根因 / 建议 / 预期收益）
2. **算法现状描述**——§2.2 的骨架，附关键函数行号链接与数据流图
3. **求解范式评估（位置优先 vs 末端姿态反推）**——§2.4：
   - 确认现状为「先点位、后角度」（位置被强制精确、姿态是带 2° 门的软约束）
   - 关节轴几何表 + `J4∥J5 ⇒ 非球形腕` 的判定；说明「逐级闭式反推」不成立的**原因**（备选简要说明，约半页，含降阶链 `Rx·Ry·Rz·Ry(θ4+θ5)·Rz` 的结论式）
   - 位置/姿态**不可兼得**的实测取舍表（§2.4），给出「必须提供可切换优先级」的结论
4. **评估方法**——指标定义：
   - 姿态残差 `oerr`（`rotErrDeg`，度）
   - 位置残差 `err`（mm）
   - 覆盖漏洞率 = 「真解 oerr≤0.05° 而解算 oerr>0.05°」占比
   - 步长敏感度 = 同目标在 step∈{0.05,0.1,0.15,0.25,0.5} 的 oerr 波动
   - 兜底劣化 = `closest6` 姿态差 −「位置精确解集内最优姿态差」
   - 单次耗时中位/p95（开/关姿态、可达/兜底）
5. **性能分析**——引用 bench_ik_split.json 的耗时构成 + 工作区实测；给出「位置/姿态/兜底」三段占比
6. **误差来源定位**——§2.5 的 R1–R4，逐条给出**可复现证据**（目标坐标、真解、实测值）
7. **优化策略（设计方案，不在本轮落地）**——按收益/成本排序给出可实施建议：
   - **S0（架构）双优先级求解范式**：仿 [ik-glue.js](file:///g:/Robot_v2/机器人手臂/web/ik-glue.js) 现有姿态模式的 UI 开关，新增「优先级」选择——**位置优先 / 姿态优先 / 折中**；三种模式下共用同一闭式支枚举结果，只切换目标函数与选解规则。给出各模式的数学表述：
     - 位置优先：`min oerr  s.t. ‖fk(q)−T‖=0`（在位置精确解集上最小化姿态误差）
     - 姿态优先：`min ‖fk(q)−T‖  s.t. oerr=0`（在姿态精确解集上最小化位置误差；利用降阶链参数化）
     - 折中：`min ‖fk(q)−T‖² + k·oerr²`，k 可调
     - 预期：位置优先模式把 B 类劣化从 63–113° 压到接近真最优；姿态优先模式提供 oerr≤0.05° 的「保角度」解（代价是位置误差，如实上报）
   - **S1（根因修复）覆盖率完备化**：把 (θ4,θ5) 上的姿态方程看作一维解曲线，用**预测-校正 continuation 跟踪**（以解析/数值切向推进、跨越折叠点不换支），或等价地做**分支索引对齐的多起点牛顿 + 折叠点/限位角种子**；预期把 A 类覆盖漏洞降到 <0.2%，且不加粗扫密度 ⇒ 耗时基本不变（≤100ms）。
   - **S2（关键收益）姿态不可达时不再清空候选池**：去掉 `POSETOL` 硬门对候选池的**清空**语义，改为保留并按 `oerr` 排序入池（门只用于「达标」判定，不用于「丢弃」）；预期消除 C 类 Pareto 支配，且兜底不再需要。
   - **S3 兜底加速**：以 S0/S2 的闭式候选为种子替代 160 次随机重启，仅在位置也越界时才退回受限 DLS；预期兜底耗时从 ~160–220ms 降到 ≤100ms。
   - **S4 可观测性**：UI 明确区分「已到达 / 位置到达·姿态最近 X° / 姿态到达·位置最近 Y mm / 均不可达」四种语义，避免把漏解或取舍结果当成功。
   - 每条策略给出：改动点（函数+行号）、原理、预期指标、代价与风险、与 ≤100ms 预算的关系。
8. **验证方案与验收口径**——见 §5。
9. **附录**：可复现实验命令（`node -e` 全文内联，不落脚本文件）+ 基准数据表 + 术语表。

> 说明：报告只做「评估 + 策略设计」，不含代码修改。若后续要落地 S0–S3，需另起一次实施任务。

---

## 4. 假设与决策（Assumptions & Decisions）

| 项 | 决策 | 依据 |
| :--- | :--- | :--- |
| 交付物 | 仅评估报告 md，**不改代码** | 用户选择「仅评估报告」 |
| 「姿态估计算法」定义 | = `web/arm-ik-solver.js` 的 IK 姿态求解 + `ik-glue.js` 集成 | 全仓检索无 IMU/卡尔曼/姿态估计类算法；仓库内「姿态」相关实现仅此一处 |
| 性能预算 | 以 **≤100ms** 为优化目标约束（报告据此评估各策略） | 用户选择「保持 ≤100ms」 |
| 失败场景 | A（可达却漏解）与 B/C（不可达最近位姿）**都覆盖** | 用户选择「两者都做」 |
| 实验脚本 | 不新增文件，全部 `node -e` 内联并在报告附录给出原文 | 遵守「仅评估报告」与「不新建文件」 |
| **求解优先级** | 报告须设计**双优先级机制（位置优先 / 姿态优先 / 折中），界面可切换**；不可兼得时两种解都给出 | 用户选择「两者都做、界面可切换」 |
| **末端姿态反推** | 报告仅作**备选简要说明**（说明非球形腕故不能逐级闭式反推 + 降阶链结论），不展开公式级设计 | 用户选择「只作备选简要说明」 |
| 关节表/限位 | 以 `web/mesh_arm.json` + `docs/travel.json` 为准，tcp=[18.5,128,93] | 与 `pose_ik_check.js`、`bench_ik_split.js` 一致 |
| 真最优基准 | 「位置精确解集内姿态误差最小」的网格枚举（0.05° 分辨率，θ6 另行闭式最优） | 独立于被测求解器，避免自证 |
| 随机性 | 固定 LCG 种子的确定性采样，报告数据可复现 | 与求解器内部自检策略一致 |

---

## 5. 验证方案（报告第 8 章内容）

**基准数据集**（执行阶段固定种子生成，规模与命令写入附录）：
- D1 全空间随机可达位形 ≥200 组（覆盖极端/边界构型）
- D2 前伸工作区 (20,210,300)±90mm ≥60 组、查看器默认区 (150,128,260)±60mm ≥60 组
- D3 姿态外推集（外推 3/8/20°）≥30 组 → 测兜底劣化
- D4 退化构型集（θ3≈±90°、临近限位）≥30 组 → 测覆盖漏洞
- D5 越界目标集 → 测兜底耗时与质量
- D6 **双优先级取舍集**：位置与姿态不可兼得的远目标 ≥20 组 → 测「位置优先」与「姿态优先」两解各自的误差与真实性

**指标与验收口径（现状基线 → 目标）**：

| 指标 | 现状实测 | 目标 |
| :--- | :--- | :--- |
| D1/D4 覆盖漏洞率（真解≤0.05° 却返回>0.05°） | 2.5% | ≤0.2% |
| D1 最大静默姿态残差 | 1.95° | ≤0.05° |
| 步长敏感度（同目标 oerr 波动） | 0→1.86° | ≤0.05° |
| D3/D5 兜底相对真最优劣化（姿态优先口径） | 63–113° | ≤2° |
| D6 位置优先解：位置误差 / 姿态差 | 0.0mm / 121.05° | 位置=0；姿态=真最优（劣化 ≤0.5°） |
| D6 姿态优先解：姿态差 / 位置误差 | 0.22° / 127.6mm | 姿态 ≤0.05°；位置如实上报 |
| 开姿态单次耗时 p95 | 242ms | ≤100ms |
| 兜底耗时 | 159–224ms | ≤100ms |
| 姿态残差 P95（可达集） | ≤2.05°（接受限） | ≤0.05° |

**沿用既有自检**：`node scratch/pose_ik_check.js` 的 7 项（含往返一致性、位置回归、姿态重构、roll→θ6 口径、6D 兜底、耗时对比）作为回归门，报告中给出其现状通过情况与目标。

**报告自检**：报告内每张数据表须与附录命令重跑结果一致；所有结论必须能由附录命令复现。

---

## 6. 执行步骤（待批准后按序执行）

1. 复跑并固化基线数据（D1–D6 + bench + 自检），生成报告用数据表。
2. 撰写 `docs/姿态估计算法评估报告.md` 正文（§3 大纲 1–9 章）。
3. 内嵌可复现 `node -e` 命令与原始输出，形成附录。
4. 复核报告数据与结论一致性（重跑附录命令抽查），交付。