🤖 机器人手臂：运动学模拟与碰撞算法详解

第一篇系统总览：一个事实源、两个碰撞模型

整台手臂的"运动学模拟"分布在两个运行环境里，靠数据文件（而不是人工抄写）保持一致。

关键纪律是单一事实源：6 根关节轴的位置 rear 与方向 d 只在 human_layout.py 里定义一次，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），这是后面一切降阶解法的前提：

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

三个对算法影响深远的结构性质：

• J4 ∥ J5 同向 ⇒ R4·R5 = R_y(θ4+θ5)，腕部两个俯仰在"工具轴指向"上合并成一个未知数 s = θ4+θ5；
• TCP 在 J6 轴线上 ⇒ θ6 只提供绕工具轴的滚转，完全不改变 TCP 位置——位置任务与姿态任务由此解耦；
• 工具轴（法兰 +Z）= (R1…R5)·ẑ，与 θ6 无关——姿态任务只剩 2 个方程。

第三篇逆运动学：闭式支 + 二维冗余扫描 + 牛顿收口

3.1问题形式与降阶

6 自由度机械臂，任务是给定 TCP 目标点（位置任务，3 个方程）或"目标点 + 工具姿态 {方位角 az, 仰角 el, 滚转 roll}"（位姿任务，再加 3 个方程）。臂本身有 6 个未知数，位置任务下存在 2 维冗余。

核心思路（branch()）：把 (θ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() 逆分解）。三个关键性质撑起了姿态算法：

• θ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% 直接走两条专用算法，结果逐位一致；拖动中走 mixWarm 快路，位移超限（疑换盆地）回落全量。

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。

自检成绩（预置值，指纹校验）：

第四篇交互层：把求解器接进 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）

离线扫描回答的问题是："每个关节到底能转到多少度？"判定必须精确——它直接对真实 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，保证"确证分离"判据保守）。"可能干涉/数值退化/迭代超限"一律回落精确布尔——结果与纯布尔完全一致，--no-gjk 可关闭做对比校验。
8. 顶点侵入实锤（反方向捷径）：一方的网格顶点（≤240 采样点）落入对方实体内部 >1mm 即实锤干涉，免掉 4~12s 的 NURBS 布尔。注意方向性：它能证"撞"，不能证"自由"——无命中则照旧走布尔。
9. OCCT 精确布尔 BRepAlgoAPI_Common：交体积 > 2mm³ 记干涉并附干涉质心；布尔失败记入 errors 不装作没看见。

扫描策略：

• 单关节硬限位：2° 粗扫（小批量并行、首个卡死角即停）+ 二分逼近到 0.2°，正反两向——travel.json 的来源。
• 组合姿态收敛：一个关节保持在极限、扫其余关节（6° 步长），两轮迭代收敛出"组合折叠约束"。这些约束只写 travel_detail.json、不压缩滑块——否则手臂会被静态限位锁死；查看器改用动态模型按当前姿态实时处理（第六篇）。
• 并行：mp.Pool 每个工作进程独立建模（杜绝 STEP/网格过期），workers = min(12, CPU−2, 可用内存/2.5GB)。

当前权威数据（travel.json / travel_detail.json，2026-10-03 扫描；collide.json 胶囊拟合 2026-09-29）：

组合折叠明细共 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 一维自由区间表：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 不升才采用——最坏情况就是静态行为，无回退风险。这就是"动态限位放宽重解"徽章的来源。

第七篇力矩与强度校核（简述）

运动学之外，同一张关节表还驱动两份物理校核，保证"能转到位"之外还"转得动、扛得住"。

• 力矩（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）：对 15 个关键截面（吊臂根座、吊臂颊板、肩叉腹板等）按梁理论算弯曲应力与挠度，输出安全系数 SF（当前最小截面：吊臂根座 @z=480，SF≈9.1）。

第八篇自检体系与复现命令

这套系统最重要的工程习惯是：每个算法都带数值自检，且自检值绑定代码指纹（FNV-1a over 限位表+运动学常量+求解器源码），改模型或改求解器后指纹失配、页面自动标红要求重算。

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   # 重算预置自检值与指纹
node scratch/gen_workspace_svg.cjs     # 重新生成工作空间剖面图（本文图 10）
python src/torque_balance.py   # 力矩校核报告
python src/strength_check.py   # 强度校核
python scratch/build_algo_doc_html.py  # 重新生成本页（SVG 变更后）

附文件索引