319 lines
24 KiB
Markdown
319 lines
24 KiB
Markdown
# 机器人手臂：运动学模拟与碰撞算法详解
|
||
|
||
> 本文基于 2026-10-03 的代码现状逐文件核对写成，所有数字（关节轴坐标、行程、胶囊数、自检误差）均取自当前仓库里的真实代码与数据文件，不是设计目标值。
|
||
> 配图：`docs/algo_svg/*.svg` 为算法示意图，`docs/shots/*.png` 为查看器实机渲染截图。
|
||
|
||
---
|
||
|
||
## 目录
|
||
|
||
- [第一篇 系统总览：一个事实源、两个碰撞模型](#第一篇-系统总览一个事实源两个碰撞模型)
|
||
- [第二篇 运动学建模与正运动学（FK）](#第二篇-运动学建模与正运动学fk)
|
||
- [第三篇 逆运动学：闭式支 + 二维冗余扫描 + 牛顿收口](#第三篇-逆运动学闭式支--二维冗余扫描--牛顿收口)
|
||
- [第四篇 交互层：把求解器接进 30fps](#第四篇-交互层把求解器接进-30fps)
|
||
- [第五篇 碰撞检测 I：离线权威扫描（sweep_v2.py）](#第五篇-碰撞检测-i离线权威扫描sweep_v2py)
|
||
- [第六篇 碰撞检测 II：实时胶囊模型与动态限位](#第六篇-碰撞检测-ii实时胶囊模型与动态限位)
|
||
- [第七篇 力矩与强度校核（简述）](#第七篇-力矩与强度校核简述)
|
||
- [第八篇 自检体系与复现命令](#第八篇-自检体系与复现命令)
|
||
|
||
---
|
||
|
||
## 第一篇 系统总览：一个事实源、两个碰撞模型
|
||
|
||
整台手臂的"运动学模拟"分布在两个运行环境里，它们共享同一份关节几何，靠数据文件（而不是人工抄写）保持一致：
|
||
|
||
| 环境 | 载体 | 算法 |
|
||
|---|---|---|
|
||
| 离线（Python/CadQuery） | `src/human_layout.py` → `arm_model.py` → `sweep_v2.py` | 参数化 CAD 建模、OCCT 精确布尔碰撞、行程扫描 |
|
||
| 在线（浏览器） | `web/arm-ik-solver.js` + `ik-glue.js` + three.js 场景图 | 闭式 IK、实时胶囊碰撞、动态限位 |
|
||
|
||
|
||
|
||
关键纪律是**单一事实源**：6 根关节轴的位置 `rear` 与方向 `d` 只在 `human_layout.py` 里定义一次（`human_layout.py:110-115`），CAD 装配、离线碰撞扫描、网页求解器三处都从这里取值——网页端由 `build_viewer.py` 打包时把同一组常量注入 HTML，因此"CAD 里看到的"和"网页里算的"逐位一致，不存在第二套会漂移的关节表。
|
||
|
||
两个碰撞模型各司其职：
|
||
|
||
- **离线精确模型**（`sweep_v2.py`）：对真实 CAD 实体做布尔求交，慢（每姿态秒级）但精确，负责产出权威行程数据；
|
||
- **在线近似模型**（`collide.json` 胶囊体）：把每件零件拟合成一组胶囊，快（每次 ~5ms）但近似，负责求解过程中的实时避碰。
|
||
|
||
几何变更时的同步链（缺一步网页就是旧的、行程就是错的）：
|
||
|
||
```
|
||
arm_model.py 重建 → AB.export 写 mesh_arm.json → build_viewer.py 重打包
|
||
→ 渲染出图确认 → sweep_v2.py 重扫（travel.json）→ fit_capsules.py 重拟合（collide.json）
|
||
```
|
||
|
||
实机全貌（查看器渲染，零位姿态）：
|
||
|
||
|
||
|
||
---
|
||
|
||
## 第二篇 运动学建模与正运动学（FK）
|
||
|
||
### 2.1 关节拓扑
|
||
|
||
对标智元灵犀 X1 的人形构型：肩部两轴（抬举+俯仰）→ 大臂自转 → 肘俯仰 → 腕俯仰 → 腕自转。所有关节轴都是坐标轴方向的（±X/±Y/±Z），这是后面一切降阶解法的前提：
|
||
|
||
| 关节 | 名称 | 轴方向 | 轴位置（CAD 系） | 驱动 |
|
||
|---|---|---|---|---|
|
||
| J1 | 肩抬举 | +X（滚转） | x=−51, z=610 | M1 → P04 肩叉 |
|
||
| J2 | 肩俯仰 | +Y（俯仰） | x=19.5, z=490 | M2 → P05 大臂旋转座 |
|
||
| J3 | 大臂自转 | −Z（滚转） | x=19.5, z=445 | M3 → P06 大臂骨架 |
|
||
| J4 | 肘部俯仰 | +Y（俯仰） | x=19.5, z=332 | M4 → P07 小臂骨架 |
|
||
| J5 | 腕部俯仰 | +Y（俯仰） | x=19.5, z=222 | M5 → P08 腕偏转座 |
|
||
| J6 | 腕自转 | −Z（滚转） | x=19.5, z=176 | M6 → P09 工具法兰 |
|
||
|
||
|
||
|
||
|
||
|
||
求解器启动时会对五条**结构前提**做断言（`arm-ik-solver.js` 的 `WARN` 列表）：J1 轴是 X；J2/J4/J5 是平行同向的 Y 轴且落在同一 x 平面；J3/J6 是 Z 轴；TCP 落在 J6 轴线上。任何一条被将来的改型破坏，页面会显式报警并回落到通用（慢）路径，而不是悄悄算错。
|
||
|
||
### 2.2 正运动学：枢轴链
|
||
|
||
每个关节是"绕一根不经过原点的轴"的旋转，写成枢轴变换：
|
||
|
||
```
|
||
Ai(θi) = T(Pi) · R(di, θi) · T(−Pi) 对任意点：x' = R·(x−Pi) + Pi
|
||
```
|
||
|
||
整条链就是六次左乘：
|
||
|
||
```
|
||
World = A1(θ1)·A2(θ2)·A3(θ3)·A4(θ4)·A5(θ5)·A6(θ6)·TCP
|
||
```
|
||
|
||
|
||
|
||
代码里有两套实现互为对账：
|
||
|
||
- `fkChain()`：严格 4×4 同质矩阵链，直白但慢，作为对照基准；
|
||
- `fk()`：把结构常量（`K.C0=Z2−Z1`、`K.A32=Z3−Z2`、`K.A43=Z4−Z3`、`K.A54=Z5−Z4`、`K.L5=Z5−TCP_z` 等，即各关节轴的竖直间距链）代入后展开三角函数的闭式快路径，是求解器热循环的专用实现。
|
||
|
||
预置自检：5000 个随机位形下两实现的 TCP 差 ≤ **5.7e-13 mm**（数值见 `ik-glue.js` 的 `PRECOMPUTED_CHK`，绑定代码指纹，模型一变即失效提示重算）。
|
||
|
||
三个对算法影响深远的结构性质：
|
||
|
||
1. **J4 ∥ J5 同向** ⇒ `R4·R5 = R_y(θ4+θ5)`，腕部两个俯仰在"工具轴指向"这件事上可以合并成一个未知数 `s = θ4+θ5`；
|
||
2. **TCP 在 J6 轴线上** ⇒ θ6 只提供绕工具轴的滚转，**完全不改变 TCP 位置**——位置任务与姿态任务由此解耦；
|
||
3. **工具轴（法兰 +Z）= (R1…R5)·ẑ**，与 θ6 无关——姿态任务只剩 2 个方程。
|
||
|
||
---
|
||
|
||
## 第三篇 逆运动学：闭式支 + 二维冗余扫描 + 牛顿收口
|
||
|
||
### 3.1 问题形式与降阶
|
||
|
||
6 自由度机械臂，任务是给定 TCP 目标点（位置任务，3 个方程）或"目标点 + 工具姿态 {方位角 az, 仰角 el, 滚转 roll}"（位姿任务，再加 3 个方程）。臂本身有 6 个未知数，位置任务下存在 **2 维冗余**。
|
||
|
||
核心思路（`branch()`，arm-ik-solver.js:528）：**把 (θ4, θ5) 当作冗余参数**。一旦给定这两个角，腕心相对量 `V` 完全确定（u=−L·sinθ5, w=A54−L·cosθ5, dx=u·c4+w·s4, dz=A43−u·s4+w·c4），剩下的 θ1..θ3 可以用平面三角学**一步闭式解出**（每格至多 4 支：θ3 的 ±σ 与 ±(π−σ)），位置精确到浮点误差，不需要任何迭代。
|
||
|
||
|
||
|
||
于是"求 IK"变成了"在 (θ4,θ5) 二维平面上挑最好的格点"——**扫描代替数值搜索**。每个格点的解天然精确，扫描的全部意义在于挑出代价（关节行程）最小的那支构型。
|
||
|
||
### 3.2 姿态参数化：方位角 / 仰角 / 滚转
|
||
|
||
姿态任务用三个角描述工具姿态（`poseMat()` 构造工具系旋转矩阵，`poseAng()` 逆分解）：**az** 绕 CAD Z 的方位角、**el** 自 +Z 的仰角（0=朝上，180=朝下）、**roll** 绕工具轴自身的滚转。三个关键性质撑起了姿态算法：
|
||
|
||
- θ6 只提供绕工具轴的滚转，**工具轴 = (R1…R5)·ẑ 与 θ6 无关**——姿态任务因此只剩 2 个方程；
|
||
- 滚转由 `rollClose()` 闭式补出：θ6 = −atan2(N[1,0], N[0,0])，其中 N = R5ᵀ·R_target；
|
||
- 推论：**全姿态偏差 oerr ≥ 工具轴夹角**（20 万随机样本验证零违反）——粗扫用一次点积预筛（1−cos2°）就挡掉所有必被拒的候选，免掉矩阵链 + acos。
|
||
|
||
|
||
|
||
### 3.3 求解流水线
|
||
|
||
|
||
|
||
1. **逐行可行段探测**：位置闭式支只在平面三角有解时存在，限位矩形里只有 7~27% 的格点能过判据。先用探测步 `hp` 找每行的连续可行段（位置任务 2°、姿态任务 0.5°——姿态任务的可行带实测最窄 **0.98°**，2° 探测会整条漏掉造成"误报不可达"），段端外扩 PAD=2°，段内仍按原步长 h（默认 0.25°）精扫：密度不降、成本大降。
|
||
2. **全域粗扫**：每格 `branch()` 出 ≤4 支 → 限位筛（NaN 候选用否定式写法保证落选）→ 打分，候选有界保留 4000。
|
||
3. **孤立根枚举**（姿态任务）：不再靠网格密度去撞窄带——逐行对姿态残差两个分量找**变号括号**，从括号中点沿该支跑阻尼高斯-牛顿收根；切向相切的根不变号，按每支 ‖e‖ 最小的节点补种。
|
||
4. **局部加密 + 模式搜索**：32 个种子各开 ±2h 窗口（步长 h/8，小步长档 h/4），再对最优的 24 个候选做 8 邻域模式搜索（步长逐轮减半至 2e-4°）。
|
||
5. **姿态收口**：姿态达标带是 (θ4,θ5) 平面上的斜窄谷（局部灵敏度可达 ~10°/°），模式搜索只能停在谷壁（实测最好 0.106°）。直接对"工具轴在目标轴法平面上的切向偏差"做 **(θ4,θ5) 二维牛顿迭代**（每步重解闭式位置支、按 θ1..θ3 最近匹配保持同支），轴残差压到 **1e-12** 量级、位置全程保持精确；尚无 oerr≤0.05° 的解时姿态权重 ×100 重扫（最多 3 轮）。
|
||
6. **选解**：`cost = Σ w·|wrap180(θ−ref)| + 1e-6·Σ|θ|`。ref 是当前姿态——判据是用户定的「**所有电机需要转过的角度之和最小**」（这一步行程最短），连续轴取等价角防绕远。姿态任务先取 oerr≤0.05° 的达标集、集内再比代价，"省几度关节角、丢 0.2° 姿态"的交易被显式禁止。
|
||
7. **solveDyn 碰撞后处理**（第六篇）。
|
||
|
||
### 3.4 兜底：最近可达位姿
|
||
|
||
目标不可达时不报错了事，而是返回最近可达位姿，并且按任务语义分三种：
|
||
|
||
- `closest()`：位置 DLS（阻尼最小二乘）——160 迭代 × 160 次 **LCG 确定性重启**（固定种子，同一目标逐位可复现，消灭"随机倒摆"）+ anchor 回拉 0.03（压准退化构型下 J3 的无用漂移，实测 82°）+ **零空间抛光**（到目标距离不增的前提下把 Σ|θ| 滑到最小）；
|
||
- `closest6()`：位置 + 姿态 6 维联合 DLS（姿态旋转矢量按 20 mm/rad 折算进误差向量）；
|
||
- `closestOri()`：姿态优先——利用 J4∥J5 的降阶结构（工具轴 = Rx(θ1)·Ry(θ2)·Rz(−θ3)·Ry(s)·ẑ），(θ3, s, θ4) 三维粗扫 + 坐标下降，**姿态精确、位置取最近**。
|
||
|
||
### 3.5 混合优先（位置/姿态滑杆）
|
||
|
||
位置与姿态在远目标上不可兼得（实测同一目标：位置 0mm 时姿态差 44°，姿态 0° 时位置差 116mm）。`mix` 模式用两端锚点（位置优先解 / 姿态优先解）的误差各自归一，按
|
||
|
||
```
|
||
J(q) = w·(e/e_ref)² + (1−w)·(o/o_ref)²
|
||
```
|
||
|
||
加权折中，种子取两锚点 + 关节空间插值 + ref，6 维阻尼高斯-牛顿 + 单轴抛光；J 同量级带内取行程更小者防横跳。滑杆 0%/100% 直接走两条专用算法，结果逐位一致。
|
||
|
||
### 3.6 实时续解（warm 路径）
|
||
|
||
拖拽时目标每帧只挪几毫米，全域粗扫（0.25° 下 6~30 万点，40~430ms）是 100ms 级延迟的唯一来源。`solveWarm` 只在上一帧解附近的 (θ4,θ5) 递进窗口（±4°→14°→40°）里走**同一闭式支**：位置残差 ≤1e-6、关节跳 ≤25°（防换支/倒摆）、姿态牛顿收口——任一校验不过就返回 null 交回全量。快路只负责"能快的时候快"，松手后的全量求解才是最终结果。
|
||
|
||
### 3.7 工作空间剖面
|
||
|
||
工作空间由纯 FK 采样刻画（`scratch/gen_workspace_svg.cjs`，与查看器「可达空间」同口径：travel.json + adj 机械区间内随机位形、J6=0、未计自碰撞）。J1 整周连续，可达集对 J1 轴旋转对称，所以 (x, r) 二维剖面（r = 离 J1 轴的距离）就是完整工作空间：rmax = 517mm 恰好是**零位臂竖直下垂的 TCP**，前伸边界 x≈+416mm。
|
||
|
||
|
||
|
||
自检成绩（预置值，指纹校验）：
|
||
|
||
| 项目 | 样本 | 结果 |
|
||
|---|---|---|
|
||
| FK 双实现对账 | 5000 | 最大差 5.7e-13 mm |
|
||
| 位置 IK 重构 | 200/200 命中 | 最大残差 9.9e-13 mm |
|
||
| 姿态 IK 重构 | 400 组 | 398 命中，最大残差 1.79°，平均 77.8 ms/次 |
|
||
| 运动学可达空间 | 400 000 位形 | x −378…415 / y −388…642 / z 94…1125 mm |
|
||
|
||
（姿态 IK 的 2 组未达 0.05° 达标线，以近似解如实上报 `poseTier=approx` 并给出真实残差，不虚报精确。）
|
||
|
||
|
||
|
||
---
|
||
|
||
## 第四篇 交互层：把求解器接进 30fps
|
||
|
||
`ik-glue.js` 负责三件事：
|
||
|
||
**调度**。全量求解（40~430ms）放进 Blob Worker 后台跑，主线程只收结果渲染。每个请求按 id 记账，"是否最新一帧"用第二参交给回调——被插队作废的请求也会被放行，保证实时追踪的门闩（liveBusy/livePending）不因一次竞态永久停摆。Worker 不可用时自动回落主线程同步求解，行为逐位一致。
|
||
|
||
**拖拽语义**。黄球 = 移动目标（Shift 只上下）；姿态控制开启后出现工具箭头：拖箭头头部 = 弧球式瞄准（指针射线与"以目标为球心、箭长为半径"的球求交，方向即工具轴指向），拖箭杆/捏环/粉色侧球 = 绕轴滚转（指针在垂直于工具轴的平面上的转角）。"实时追踪"模式下拖动即算即动，松手全量精修。
|
||
|
||
**动画与对账**。解算结果经 `animTo` 动画写入：连续轴（J1/J3/J6）按 `wrap180` 走**短弧**（+151.9°→−167° 若按数值插值会反向扫 318.9°，走短弧只有 41.1°），末帧落回求解器给的代表角——与插值终点恰差 360° 整数倍，同一物理角、视觉零跳变。每帧渲染后还用**场景图真实矩阵链**复算一次 TCP 位置与姿态偏差（`sceneOrientErr`），和求解器的闭式残差双通道对账——任何"求解器以为对了、场景画错了"的不同步都会立刻暴露在界面上。
|
||
|
||
---
|
||
|
||
## 第五篇 碰撞检测 I：离线权威扫描（sweep_v2.py）
|
||
|
||
离线扫描回答的问题是："**每个关节到底能转到多少度？**"答案直接写进 `docs/travel.json` 成为查看器滑块的硬限位，因此判定必须精确——它直接对真实 CAD 实体做 OCCT 布尔求交，一切预筛都设计成**保守证书**：只能"确证分离"才允许跳过精确布尔。
|
||
|
||
|
||
|
||
逐层拆解（每姿态的判定路径 `test_posture()`）：
|
||
|
||
1. **全对全**：19 件（9 打印件 + 6 电机 + 台架 P01/P02）≈ 171 对。台架默认**参与**碰撞——臂撞立柱就是臂撞躯干，是真约束（`--no-stand` 仅用于诊断某个限位是臂自身折叠还是躯干挡的）。
|
||
2. **跳过刚性安装对**：12 对电机↔被驱动件由螺钉锁死（`MOUNT_PAIRS`），无相对运动。
|
||
3. **组跨度规则**：某姿态下没有活动关节跨越的零件对相对静止，跳过；但**零位（无活动关节）时必须全查**——相对位姿不变不代表不干涉，零位单实体检查就是这么兜住的。
|
||
4. **轴区间预剪枝**（单关节扫描专用）：按关节轴算每对零件的轴向/径向区间，永不相交的对在该关节任何角度都不可能碰撞——对单关节扫描是**精确安全**的。
|
||
5. **AABB**：8 角点变换后取包围盒，0.1mm 余量。
|
||
6. **包围球**：球心随组刚体变换、半径旋转不变，仍精确保守。
|
||
7. **GJK 凸包预筛**：零件按 0.5mm 容差网格化取顶点云 → scipy 凸包精简（电机 15 万顶点 → 千余点），GJK 带 0.8mm 安全外扩（> 网格内缩 0.5mm，保证"确证分离"判据保守）。单纯形用 Muratori 形式手写，"可能干涉/数值退化/迭代超限"一律回落精确布尔——**结果与纯布尔完全一致**，`--no-gjk` 可关闭做对比校验。
|
||
8. **顶点侵入实锤**（反方向捷径）：一方的网格顶点（≤240 采样点）落入对方实体内部 >1mm（`BRepClass3d_SolidClassifier`，容差与 `VOL_MIN=2mm³` 语义对齐）即实锤干涉，免掉 4~12s 的 NURBS 布尔。注意方向性：它能证"撞"，不能证"自由"——无命中则照旧走布尔。
|
||
9. **OCCT 精确布尔** `BRepAlgoAPI_Common`：交体积 > 2mm³ 记干涉并附干涉质心；布尔失败记入 errors 不装作没看见。
|
||
|
||
**扫描策略**：
|
||
|
||
- **单关节硬限位**：2° 粗扫（小批量并行、首个卡死角即停，少算深度卡死姿态）+ 二分逼近到 0.2°，正反两向。这是 travel.json 的来源。
|
||
- **组合姿态收敛**：一个关节保持在极限、扫其余关节（6° 步长），两轮迭代收敛出"组合折叠约束"。这些约束**只写 travel_detail.json、不压缩滑块**——否则手臂会被静态限位锁死（真实机器人由控制器避碰）；查看器改用动态模型按当前姿态实时处理（第六篇）。
|
||
- **并行**：mp.Pool 每个工作进程独立建模（杜绝 STEP/网格过期），`imap_unordered` 保序回传，workers = min(12, CPU−2, 可用内存/2.5GB)。
|
||
|
||
当前权威数据（`travel.json` / `travel_detail.json`，2026-10-03 扫描；`collide.json` 胶囊拟合 2026-09-29）：
|
||
|
||
| 轴 | 单关节硬限位 | 滑块范围 | 备注 |
|
||
|---|---|---|---|
|
||
| J1 | −180…+180 | ±180 连续 | 整周无干涉 |
|
||
| J2 | −106.25…+10.38 | **−105…+9** | 含 1° 安全余量 |
|
||
| J3 | −180…+180 | ±180 连续 | 整周无干涉 |
|
||
| J4 | −112.62…+17.88 | **−111…+16** | |
|
||
| J5 | −94.75…+35.75 | **−93…+34** | |
|
||
| J6 | −180…+180 | ±180 连续 | 电机行程 ±360° |
|
||
|
||
组合折叠明细共 99 条（典型：工具法兰 P09 撞立柱 P01_A 限制了 J3/J4/J5 的组合极限）。
|
||
|
||
|
||
|
||
---
|
||
|
||
## 第六篇 碰撞检测 II：实时胶囊模型与动态限位
|
||
|
||
### 6.1 模型：CAD → 胶囊
|
||
|
||
离线布尔每姿态秒级，放进 30fps 的交互里不可能。`scratch/fit_capsules.py` 把每件零件的 CAD 网格拟合为一组**胶囊体**（线段端点 a,b + 半径 r），写进 `docs/collide.json`：共 **1995 枚胶囊**，归属 7 个运动组（g0=台架固定 … g6=J6 组），端点是零位 CAD 世界坐标，组 g 的胶囊随 A1…Ag 嵌套旋转——与 FK 链同一累积变换，与场景图逐位一致。
|
||
|
||
|
||
|
||
胶囊是外凸近似，所以它判"干涉"略偏保守（含 margin=0.3mm 外扩），判"自由"需离线布尔背书；换来的是每次 `collideFree()` 只需 **~5ms**（零位最坏情形）。
|
||
|
||
### 6.2 宽相位与热路径
|
||
|
||
|
||
|
||
- **只检 |Δg|≥2 的零件对**：同组相对静止（零位单实体检查已保证不干涉）；相邻组的对（P04×P05 / P06×P07 / P07×P08）同心跨在同一关节轴上，胶囊轴距≈0，塞进模型会在零位误报（实测 P04×P03 超报 15mm）——它们改用 **adj 一维自由区间表**（CAD 实测、与姿态无关，由 `scratch/probe_adj.py` 扫描生成；collide.json 缺 adj 时查看器会显式告警并退回静态限位）：J2 [−107°,+128°]、J4 [−112°,+112°]、J5 [−96°,+96°]。
|
||
- **两级宽相位**：零件包围球（19 球）→ 簇球（每零件的胶囊按主轴投影分桶、每簇 ≤4 枚，主轴由幂迭代 5 轮定向）→ 只有邻近簇才逐对变换胶囊端点做**线段-线段距离**（Ericson《Real-Time Collision Detection》标准式，手写标量直传版，免建数组——碰撞热循环每次调用上万次，分配开销实测占一半）。宽相位阈值带 `rmax+margin` 是有推导的：早期版本漏了 rmax，把 P01_C×P06 在 J1=20°/J2=49° 的 1618mm³ 深撞整对筛掉。
|
||
- **性能工程**：预分配缓冲 + 代际戳缓存（一次判定 ~2500 个小数组的 GC 开销占 13% → 归零）、轴对齐特化 `rotAxis`（六个关节轴都是 ±X/±Y/±Z，Rodrigues 通用式退化为每轴 4 个非零元素）、簇球再细分实测得不偿失（62 万次球对测试 27ms/check）。
|
||
|
||
### 6.3 动态限位：静态值只是零位快照
|
||
|
||
travel.json 的单关节限位是"其它轴全零"时的一次性扫描，对"撞固定立柱"这类跨 ≥2 组的约束**系统性过紧**：实测 J2 正向静态 +9.75°（垂臂零位），J1=+90° 时真边界放到 +108.75°。查看器的滑块边界因此分三层：
|
||
|
||
|
||
|
||
1. **机械区间** = adj 表（相邻件实测，必然无自碰撞）；
|
||
2. **动态边界** = `dynLimit()` 从当前角向限位方向**二分**"collideFree"翻转点（tol 0.25°），上限受电机行程 ±360° 与 adj 封顶；"硬边"（lo 侧相邻件自碰撞，配置无关）直接取静态值不放宽；
|
||
3. **滑块夹取** `clampAxis()` 带边界缓存：`[轴, 其余轴 1° 量化姿态] → {lo,hi}`，拖同一轴直接取缓存，拖过边界才重算一次二分（~40ms）；当前姿已干涉时退回机械区间——否则 dynLimit 会把界外值当边界，滑块再也回不来。
|
||
|
||
求解侧对应的是 **solveDyn**：静态解先过全姿态碰撞校验（不安全→按代价升序在候选表里找 ≤1.5×cost+20° 的安全解）；解贴在限位边界（0.5° 内）时对非连续、非硬边的轴二分动态边界，放宽 ≥5° 才值得重解（粗档步长 ×2，省 3~4 倍），新解 cost 不升才采用——最坏情况就是静态行为，无回退风险。这就是"动态限位放宽重解"徽章的来源。
|
||
|
||
|
||
|
||
---
|
||
|
||
## 第七篇 力矩与强度校核（简述）
|
||
|
||
运动学之外，同一张关节表还驱动两份物理校核（`torque_balance.py` / `strength_check.py`），保证"能转到位"之外还"转得动、扛得住"：
|
||
|
||
- **力矩**（`docs/torque_balance.json` + `torque_report.md`）：零件质量与质心全部取自 CAD（`bom.json`），在可达姿态网格上搜索每个关节的**最大重力力矩**与 Σm·d² 惯量——不做任何硬编码常数。电机参数为工程假定（连续 2.5 / 峰值 8.0 / 热衰减许用 2.0 N·m，可用环境变量覆盖）。当前结论：J1 重力力矩 5.97 N·m、峰值需求 7.36 N·m（峰值裕度 1.09×），连续负载率超许用（J1 238.8%/J2 171.6%），即**持臂静态工况超出热衰减后的连续额定**——这正是后续减速比/选型迭代的输入。
|
||
- **强度**（`docs/strength.json`）：对关键截面（吊臂根座、吊臂颊板、肩叉腹板等）按梁理论算弯曲应力与挠度，输出安全系数 SF（当前最小截面 SF≈9.1）。
|
||
|
||
---
|
||
|
||
## 第八篇 自检体系与复现命令
|
||
|
||
这套系统最重要的工程习惯是**每个算法都带数值自检**，且自检值绑定代码指纹（FNV-1a over 限位表+运动学常量+求解器源码），改模型或改求解器后指纹失配、页面自动标红要求重算：
|
||
|
||
| 自检 | 工具 | 通过线 |
|
||
|---|---|---|
|
||
| FK 双实现对账 | `ArmIK.selfTest()` | ≤1e-9 mm（实测 5.7e-13） |
|
||
| 位置 IK 重构 | 同上 | 逐支精确命中（实测 9.9e-13） |
|
||
| 姿态 IK 重构 | `poseSelfTest()` | oerr<0.05° 且位置<1e-3 |
|
||
| 线段距离解析式 | 暴力密集采样对比（scratch 自检脚本） | 逐位一致 |
|
||
| 螺钉孔对位 | `scratch/fit_check.py` 布尔干涉 | 0 干涉（轴线取样法会误报，弃用） |
|
||
| 全行程零干涉 | `sweep_v2.py` 全对全扫描 | 单关节推满 0 碰撞 |
|
||
|
||
复现命令：
|
||
|
||
```bash
|
||
python src/arm_model.py # 重建模型 + mesh_arm.json + STEP/STL + bom
|
||
python src/build_viewer.py # 重打包单文件网页查看器
|
||
python src/sweep_v2.py # 离线行程/碰撞权威扫描（几何变更后必跑）
|
||
node scratch/compute_precomputed.cjs # 重算预置自检值与指纹
|
||
python src/torque_balance.py # 力矩校核报告
|
||
python src/strength_check.py # 强度校核
|
||
```
|
||
|
||
### 文件索引
|
||
|
||
| 文件 | 角色 |
|
||
|---|---|
|
||
| `src/human_layout.py` | 关节轴/拓扑单一事实源 |
|
||
| `src/arm_model.py` · `arm_build.py` | 参数化 CAD 与导出 |
|
||
| `src/sweep_v2.py` | 离线碰撞与行程权威工具 |
|
||
| `src/shot.py` | 渲染出图（docs/shots/） |
|
||
| `web/arm-ik-solver.js` | 六轴 IK 求解器（纯 JS，浏览器/Node 双端） |
|
||
| `web/ik-glue.js` | 交互层：拖拽/Worker/动态限位/动画 |
|
||
| `src/finalize_travel.py` | 从 sweep 明细重建 travel.json 的维护工具 |
|
||
| `scratch/fit_capsules.py` | CAD 网格 → 胶囊模型拟合（collide.json） |
|
||
| `scratch/probe_adj.py` | 相邻打印件 1D 自由区间实测（adj 表） |
|
||
| `scratch/compute_precomputed.cjs` | 重算预置自检值与代码指纹 |
|
||
| `scratch/gen_workspace_svg.cjs` | 工作空间剖面图生成（本文图 11） |
|
||
| `docs/travel.json` · `travel_detail.json` | 滑块硬限位 / 组合折叠明细 |
|
||
| `docs/collide.json` | 1995 胶囊 + adj 机械区间 + 硬边表 |
|
||
| `docs/algo_svg/` | 本文全部算法示意图源文件 |
|