19 KiB
19 KiB
姿态估计算法评估与优化 —— 实施计划
交付物（用户已确认）：仅一份中文评估报告，不改动求解器代码。 报告落盘位置：
docs/姿态估计算法评估报告.md
1. 摘要（Summary）
本任务对机械臂的「姿态估计算法」——即 arm-ik-solver.js 的 6 轴逆运动学（IK）姿态求解器及其查看器集成 ik-glue.js（用户已确认该文件即「姿态估计算法」）——做一次全面评估。
需要解决的核心问题：在多个关键位姿（近工作空间边界、折叠/奇异构型、贴近关节限位处）无法达到预定姿态目标；且当目标姿态确实不可达时，返回的「最近位姿」明显劣于肉眼可见的更优姿态。
评估已在本阶段（Plan 模式，只读）实测复现并定位到 4 类缺陷（见 §2.3），并额外回答了两个关键设计问题：现状是「先点位、后角度」的位置优先架构（§2.4），以及位置与姿态在远目标上真正不可兼得、必须提供可切换优先级（§2.4 实测取舍表）。本计划定义报告的结构、数据来源、可复现实验命令、优化策略与验证口径。执行阶段只产出一份报告 md，不新增任何脚本文件（实验以 node -e 内联执行，命令内嵌报告附录）。
2. 现状分析（Current State Analysis）
2.1 代码与数据资产
| 角色 | 文件 | 说明 |
|---|---|---|
| 姿态求解器（主） | arm-ik-solver.js | UMD，make(cfg) 返回 solve/solveTo/solveWarm/solveDyn/closest/closest6/branch/fk/orient/costOf 等（导出清单 L1355-1360） |
| 查看器集成 | ik-glue.js | 姿态 UI、目标拖动、solveTo 调用（L379、L399）、结果渲染与徽标判定（renderResult L318-342） |
| 关节表（几何真值） | mesh_arm.json | joints[] = {id, rear, d}，FK/IK 运动学常量来源 |
| 关节限位 | travel.json | 本报告测量口径：J1/J3/J6 连续（±180），J2∈[-106,9]，J4∈[-111,16]，J5∈[-95,34]（2026-09-29 起该表退为零位扫描参考：滑块改电机行程 ±360°，可行域改相邻件实测区间） |
| 既有自检 | pose_ik_check.js | 7 组 node 自检（往返、位置回归、姿态重构、交互口径、兜底、耗时） |
| 既有性能基准 | bench_ik_split.js / bench_ik_split.json | 单次求解耗时与「位置/姿态」占比拆分 |
| 历史优化实验 | scratch/ikopt/（solver_scanbase.js 等） |
根枚举/窗口/同族门等过往迭代产物，报告需说明其结论 |
2.2 现有算法骨架（报告需准确描述）
- 位置闭式 + 二维冗余扫描：以 (θ4,θ5) 为冗余参数全域扫描，θ1..θ3 由 branch() L429-463 闭式给出（最多 4 支：θ3=±σ、±(π−σ)），位置天然精确到 1e-13 mm。
- 按行裁剪：rowSpans L644-654 用探测步
hp=max(h,2)°找每 θ4 行的可行 θ5 段，段内再按 h 精扫。 - 姿态任务：工具轴指向 = 目标轴，是 (θ4,θ5) 上的 2 方程 2 未知量，解为孤立点；poseTools L486-545 提供残差/牛顿原语；§4.1b 根枚举 L697-761 逐行做变号括号 + 牛顿收口；滚转由 rollClose L549-555 闭式补 θ6。
- 接受门：
POSETOL=2.0°（L581）——scoreBranch对oerr>POSETOL的候选直接返回 null 丢弃（L596-598）。 - 不可达兜底：closest6 L1054-1169 为 6 维阻尼最小二乘（DLS），160 次确定性拟随机重启 × 160 迭代；solveTo L1172-1192 在
solve()失败时调用它。 - 实时快路：solveWarm L1200-1265，沿上一帧解同支续解，失败回落全量。
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 的 6 维随机 DLS。- 同例对照（实测）：位置精确解集内最小姿态差 = 121.05°；
closest6返回 136.7° / 位置 0.0mm。即closest6在位置与姿态两个指标上同时劣于一个用现有闭式扫描就能廉价得到的解 —— 被严格支配（Pareto-dominated）。 - 后果：
位置精确 + 姿态尽力这条最自然的路径没有实现；兜底既不最优、又更慢（159–224ms vs 扫描 ~100ms）。
缺陷 D — 性能超出既定预算
- 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 为准）：
| 关节 | 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 已用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 实测可得）：
- 概述与结论摘要（TL;DR 表格：缺陷 / 影响 / 根因 / 建议 / 预期收益）
- 算法现状描述——§2.2 的骨架，附关键函数行号链接与数据流图
- 求解范式评估（位置优先 vs 末端姿态反推）——§2.4：
- 确认现状为「先点位、后角度」（位置被强制精确、姿态是带 2° 门的软约束）
- 关节轴几何表 +
J4∥J5 ⇒ 非球形腕的判定；说明「逐级闭式反推」不成立的原因（备选简要说明，约半页，含降阶链Rx·Ry·Rz·Ry(θ4+θ5)·Rz的结论式） - 位置/姿态不可兼得的实测取舍表（§2.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（开/关姿态、可达/兜底）
- 姿态残差
- 性能分析——引用 bench_ik_split.json 的耗时构成 + 工作区实测；给出「位置/姿态/兜底」三段占比
- 误差来源定位——§2.5 的 R1–R4，逐条给出可复现证据（目标坐标、真解、实测值）
- 优化策略（设计方案，不在本轮落地）——按收益/成本排序给出可实施建议：
- S0（架构）双优先级求解范式：仿 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 预算的关系。
- S0（架构）双优先级求解范式：仿 ik-glue.js 现有姿态模式的 UI 开关，新增「优先级」选择——位置优先 / 姿态优先 / 折中；三种模式下共用同一闭式支枚举结果，只切换目标函数与选解规则。给出各模式的数学表述：
- 验证方案与验收口径——见 §5。
- 附录：可复现实验命令（
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. 执行步骤（待批准后按序执行）
- 复跑并固化基线数据（D1–D6 + bench + 自检），生成报告用数据表。
- 撰写
docs/姿态估计算法评估报告.md正文（§3 大纲 1–9 章）。 - 内嵌可复现
node -e命令与原始输出，形成附录。 - 复核报告数据与结论一致性（重跑附录命令抽查），交付。