Files
Robot_Arm/docs/运动学与碰撞算法详解.html
T

1295 lines
755 KiB
HTML
Raw Normal View History

<!DOCTYPE html>
<html lang="zh-CN">
<head>
<meta charset="utf-8">
<meta name="viewport" content="width=device-width, initial-scale=1">
<title>机器人手臂:运动学模拟与碰撞算法详解</title>
<style>
:root{
--ink:#1c2b3a; --sub:#5c7185; --line:#dfe7ee; --bg:#f6f8fa;
--ac:#1f6f43; --ab:#1f4f8f; --aw:#8a5a00; --ap:#6b46a8;
--code-bg:#eef2f6;
}
*{box-sizing:border-box}
html{scroll-behavior:smooth}
body{margin:0;background:var(--bg);color:var(--ink);
font-family:"Segoe UI","PingFang SC","Microsoft YaHei",sans-serif;
font-size:15px;line-height:1.75}
.wrap{max-width:1240px;margin:0 auto;padding:0 20px;display:flex;gap:28px;align-items:flex-start}
/* ---------- 顶部 ---------- */
header.top{background:linear-gradient(135deg,#12283c 0%,#1b3a5c 60%,#1f4f8f 100%);color:#eaf2fa;
padding:34px 20px 26px;margin-bottom:26px}
header.top .inner{max-width:1240px;margin:0 auto;padding:0 20px}
header.top h1{margin:0 0 10px;font-size:26px;letter-spacing:.5px}
header.top p{margin:4px 0;font-size:13.5px;color:#b9cfe4}
header.top .tags{margin-top:12px}
header.top .tag{display:inline-block;background:rgba(255,255,255,.12);border:1px solid rgba(255,255,255,.22);
border-radius:999px;padding:2px 12px;font-size:12px;margin-right:8px}
/* ---------- 目录 ---------- */
nav.toc{position:sticky;top:16px;width:250px;flex:none;background:#fff;border:1px solid var(--line);
border-radius:12px;padding:14px 14px 12px;max-height:calc(100vh - 32px);overflow:auto}
nav.toc h3{margin:2px 0 8px;font-size:13px;color:var(--sub);letter-spacing:2px}
nav.toc a{display:block;color:var(--ink);text-decoration:none;font-size:13px;padding:4px 8px;
border-radius:6px;border-left:3px solid transparent}
nav.toc a:hover{background:#eef4fa}
nav.toc a.sub{padding-left:20px;color:var(--sub);font-size:12.5px}
nav.toc a.active{background:#e8f1fa;color:var(--ab);border-left-color:var(--ab);font-weight:600}
main{flex:1;min-width:0;background:#fff;border:1px solid var(--line);border-radius:14px;
padding:30px 36px 40px;margin-bottom:40px}
@media (max-width:1100px){
.wrap{flex-direction:column}
nav.toc{position:static;width:100%;max-height:none;order:2}
main{order:1;width:100%}
}
/* ---------- 章节 ---------- */
section{margin-bottom:44px}
h2{font-size:21px;margin:0 0 6px;padding-left:12px;border-left:5px solid var(--ab);line-height:1.4}
h2 .no{color:var(--ab);font-family:Consolas,monospace;margin-right:8px}
section p.lead{color:var(--sub);font-size:13.5px;margin:0 0 14px;padding-left:17px}
h3{font-size:16.5px;margin:26px 0 8px;color:#123a5e}
h3 .no{color:var(--ab);font-family:Consolas,monospace;margin-right:6px;font-size:14px}
p{margin:9px 0}
ul,ol{margin:8px 0;padding-left:26px}
li{margin:4px 0}
b{color:#10314f}
.note{background:#f2f8f4;border:1px solid #d5e8dc;border-left:4px solid var(--ac);
border-radius:8px;padding:10px 16px;margin:14px 0;font-size:13.5px}
.warn{background:#fdf6ec;border:1px solid #f0e2c0;border-left:4px solid #c99418;
border-radius:8px;padding:10px 16px;margin:14px 0;font-size:13.5px}
code{font-family:Consolas,"Courier New",monospace;background:var(--code-bg);
border-radius:4px;padding:1px 6px;font-size:.88em;color:#0f2f4d}
pre{background:#f4f6f8;border:1px solid var(--line);border-radius:10px;padding:14px 18px;
overflow-x:auto;font-size:13px;line-height:1.6}
pre code{background:none;padding:0}
table{border-collapse:collapse;width:100%;margin:14px 0;font-size:13.5px}
th{background:#eef4fa;color:#123a5e;text-align:left}
th,td{border:1px solid var(--line);padding:7px 12px;vertical-align:top}
tbody tr:nth-child(even){background:#fafcfe}
/* ---------- 插图 ---------- */
figure{margin:20px 0;border:1px solid var(--line);border-radius:12px;
padding:16px 16px 10px;background:#ffffff;text-align:center}
figure svg{width:100%;max-width:980px;height:auto;display:block;margin:0 auto}
figure img{width:100%;max-width:980px;height:auto;display:block;margin:0 auto;border-radius:8px}
figcaption{font-size:12.5px;color:var(--sub);margin-top:10px;padding-bottom:4px}
figcaption b{color:#33475b}
/* ---------- 页脚 ---------- */
footer{color:var(--sub);font-size:12.5px;border-top:1px solid var(--line);
margin-top:34px;padding-top:16px}
@media print{
body{background:#fff}
nav.toc{display:none}
.wrap{display:block;max-width:none}
main{border:none;padding:0}
figure{page-break-inside:avoid}
section{page-break-inside:auto}
}
</style>
</head>
<body>
<header class="top"><div class="inner">
<h1>🤖 机器人手臂:运动学模拟与碰撞算法详解</h1>
<p>基于 2026-10-03 的代码现状逐文件核对写成——所有数字(关节轴坐标、行程、胶囊数、自检误差)均取自当前仓库的真实代码与数据文件,不是设计目标值。</p>
<div class="tags">
<span class="tag">正运动学:枢轴链</span><span class="tag">IK:闭式支 + 冗余扫描 + 牛顿收口</span>
<span class="tag">碰撞 I:OCCT 精确布尔(离线)</span><span class="tag">碰撞 II:实时胶囊 + 动态限位</span>
<span class="tag">11 张算法图 · 5 张实机渲染</span>
</div>
<p style="margin-top:10px">算法图(SVG)已内联本页,可单文件分享;实机渲染图引用 docs/shots/ 相对路径。文字版同步于 <code>docs/运动学与碰撞算法详解.md</code>,由 <code>scratch/build_algo_doc_html.py</code> 生成。</p>
</div></header>
<div class="wrap">
<nav class="toc" id="toc">
<h3>目 录</h3>
<a href="#ch1" data-spy>第一篇 · 系统总览</a>
<a href="#ch2" data-spy>第二篇 · 运动学建模与 FK</a>
<a href="#ch3" data-spy>第三篇 · 逆运动学</a>
<a href="#s32" class="sub">3.2 姿态参数化 az/el/roll</a>
<a href="#s33" class="sub">3.3 求解流水线</a>
<a href="#s37" class="sub">3.7 工作空间剖面</a>
<a href="#ch4" data-spy>第四篇 · 交互层</a>
<a href="#ch5" data-spy>第五篇 · 碰撞检测 I(离线)</a>
<a href="#ch6" data-spy>第六篇 · 碰撞检测 II(实时)</a>
<a href="#ch7" data-spy>第七篇 · 力矩与强度校核</a>
<a href="#ch8" data-spy>第八篇 · 自检体系与复现</a>
</nav>
<main>
<!-- ================= 第一篇 ================= -->
<section id="ch1">
<h2><span class="no">第一篇</span>系统总览:一个事实源、两个碰撞模型</h2>
<p class="lead">整台手臂的"运动学模拟"分布在两个运行环境里,靠数据文件(而不是人工抄写)保持一致。</p>
<table>
<tr><th style="width:150px">环境</th><th style="width:330px">载体</th><th>算法</th></tr>
<tr><td>离线(Python/CadQuery)</td><td>human_layout.py → arm_model.py → sweep_v2.py</td><td>参数化 CAD 建模、OCCT 精确布尔碰撞、行程扫描</td></tr>
<tr><td>在线(浏览器)</td><td>arm-ik-solver.js + ik-glue.js + three.js 场景图</td><td>闭式 IK、实时胶囊碰撞、动态限位</td></tr>
</table>
<figure><svg xmlns="http://www.w3.org/2000/svg" viewBox="0 0 980 700" font-family="'Segoe UI','PingFang SC','Microsoft YaHei',sans-serif">
<rect x="0" y="0" width="980" height="700" fill="#ffffff"/>
<defs>
<marker id="ar" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="7" markerHeight="7" orient="auto-start-reverse">
<path d="M0,0 L10,5 L0,10 z" fill="#5a7d9a"/>
</marker>
<marker id="arb" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="7" markerHeight="7" orient="auto-start-reverse">
<path d="M0,0 L10,5 L0,10 z" fill="#7c5cd6"/>
</marker>
</defs>
<text x="490" y="34" text-anchor="middle" font-size="21" font-weight="bold" fill="#16324f">系统总览:一个事实源、两个碰撞模型、一条数据链</text>
<!-- 列标题 -->
<text x="175" y="70" text-anchor="middle" font-size="15" font-weight="bold" fill="#1f6f43">① 建模侧(Python · CadQuery)</text>
<text x="490" y="70" text-anchor="middle" font-size="15" font-weight="bold" fill="#8a5a00">② 数据契约(docs/*.json)</text>
<text x="805" y="70" text-anchor="middle" font-size="15" font-weight="bold" fill="#1f4f8f">③ 网页查看器(单文件 HTML)</text>
<!-- ① 建模侧 -->
<rect x="40" y="88" width="270" height="86" rx="8" fill="#eef8f1" stroke="#1f6f43" stroke-width="1.6"/>
<text x="175" y="112" text-anchor="middle" font-size="15" font-weight="bold" fill="#14472c">human_layout.py</text>
<text x="175" y="133" text-anchor="middle" font-size="12.5" fill="#2c5c44">唯一事实源:6 根关节轴 rear/d、</text>
<text x="175" y="151" text-anchor="middle" font-size="12.5" fill="#2c5c44">连杆高度链、台架/机身界面</text>
<rect x="40" y="206" width="270" height="86" rx="8" fill="#eef8f1" stroke="#1f6f43" stroke-width="1.6"/>
<text x="175" y="230" text-anchor="middle" font-size="15" font-weight="bold" fill="#14472c">arm_model.py · arm_build.py</text>
<text x="175" y="251" text-anchor="middle" font-size="12.5" fill="#2c5c44">参数化 CAD:9 打印件 + 6 电机模组</text>
<text x="175" y="269" text-anchor="middle" font-size="12.5" fill="#2c5c44">装配体 / STEP / STL / 质量属性</text>
<rect x="40" y="324" width="270" height="70" rx="8" fill="#f6f8fa" stroke="#8aa2b5" stroke-width="1.2"/>
<text x="175" y="348" text-anchor="middle" font-size="13" font-weight="bold" fill="#3d5164">strength_check.py · torque_balance.py</text>
<text x="175" y="368" text-anchor="middle" font-size="12" fill="#5c7185">同一关节表:截面刚度 / 重力力矩网格搜索</text>
<rect x="40" y="426" width="270" height="70" rx="8" fill="#fdf2ee" stroke="#b0562f" stroke-width="1.4"/>
<text x="175" y="450" text-anchor="middle" font-size="13.5" font-weight="bold" fill="#7c3a1c">sweep_v2.py(离线权威碰撞)</text>
<text x="175" y="470" text-anchor="middle" font-size="12" fill="#8a5433">多进程 + GJK 预筛 + OCCT 精确布尔</text>
<rect x="40" y="528" width="270" height="62" rx="8" fill="#fdf2ee" stroke="#b0562f" stroke-width="1.2" stroke-dasharray="5 3"/>
<text x="175" y="552" text-anchor="middle" font-size="13" font-weight="bold" fill="#7c3a1c">scratch/fit_capsules.py</text>
<text x="175" y="571" text-anchor="middle" font-size="12" fill="#8a5433">CAD 网格 → 实时胶囊模型拟合</text>
<line x1="175" y1="174" x2="175" y2="204" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#ar)"/>
<line x1="175" y1="292" x2="175" y2="322" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#ar)"/>
<line x1="175" y1="394" x2="175" y2="424" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#ar)"/>
<!-- ② 数据契约 -->
<g>
<rect x="365" y="426" width="250" height="56" rx="8" fill="#fff8ea" stroke="#c99418" stroke-width="1.4"/>
<text x="490" y="449" text-anchor="middle" font-size="13.5" font-weight="bold" fill="#7a5a08">travel.json / travel_detail.json</text>
<text x="490" y="468" text-anchor="middle" font-size="11.5" fill="#8f6d1c">单关节硬限位(滑块)+ 组合折叠明细</text>
<rect x="365" y="528" width="250" height="56" rx="8" fill="#fff8ea" stroke="#c99418" stroke-width="1.4"/>
<text x="490" y="551" text-anchor="middle" font-size="13.5" font-weight="bold" fill="#7a5a08">collide.json</text>
<text x="490" y="570" text-anchor="middle" font-size="11.5" fill="#8f6d1c">1995 胶囊 + adj 机械区间 + 硬边标记</text>
<rect x="365" y="324" width="250" height="56" rx="8" fill="#fff8ea" stroke="#c99418" stroke-width="1.4"/>
<text x="490" y="347" text-anchor="middle" font-size="13.5" font-weight="bold" fill="#7a5a08">torque_balance.json · strength.json</text>
<text x="490" y="366" text-anchor="middle" font-size="11.5" fill="#8f6d1c">逐关节力矩负载率 / 截面安全系数</text>
<rect x="365" y="222" width="250" height="56" rx="8" fill="#fff8ea" stroke="#c99418" stroke-width="1.4"/>
<text x="490" y="245" text-anchor="middle" font-size="13.5" font-weight="bold" fill="#7a5a08">mesh_arm.json(网格)</text>
<text x="490" y="264" text-anchor="middle" font-size="11.5" fill="#8f6d1c">AB.export → build_viewer.py 打包</text>
</g>
<line x1="310" y1="455" x2="363" y2="455" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#ar)"/>
<line x1="310" y1="559" x2="363" y2="559" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#ar)"/>
<line x1="310" y1="355" x2="363" y2="355" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#ar)"/>
<line x1="310" y1="255" x2="363" y2="252" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#ar)"/>
<text x="336" y="446" text-anchor="middle" font-size="11" fill="#5c7185">写出</text>
<text x="336" y="550" text-anchor="middle" font-size="11" fill="#5c7185">写出</text>
<!-- ③ 查看器 -->
<rect x="670" y="120" width="270" height="96" rx="8" fill="#edf3fc" stroke="#1f4f8f" stroke-width="1.6"/>
<text x="805" y="145" text-anchor="middle" font-size="15" font-weight="bold" fill="#153a6e">arm-ik-solver.js(ArmIK)</text>
<text x="805" y="166" text-anchor="middle" font-size="12.5" fill="#3a5a82">闭式支 + (θ4,θ5) 扫描 + 牛顿收口</text>
<text x="805" y="184" text-anchor="middle" font-size="12.5" fill="#3a5a82">DLS 兜底 · 实时胶囊碰撞 · 动态限位</text>
<text x="805" y="202" text-anchor="middle" font-size="12.5" fill="#3a5a82">纯 JS 无依赖 · 主线程 / Worker 同源</text>
<rect x="670" y="248" width="270" height="76" rx="8" fill="#edf3fc" stroke="#1f4f8f" stroke-width="1.6"/>
<text x="805" y="273" text-anchor="middle" font-size="15" font-weight="bold" fill="#153a6e">ik-glue.js(交互层)</text>
<text x="805" y="294" text-anchor="middle" font-size="12.5" fill="#3a5a82">拖拽 / 箭头姿态 / 实时追踪 / Worker</text>
<text x="805" y="312" text-anchor="middle" font-size="12.5" fill="#3a5a82">滑块动态限位夹取(边界缓存)</text>
<rect x="670" y="356" width="270" height="70" rx="8" fill="#edf3fc" stroke="#1f4f8f" stroke-width="1.6"/>
<text x="805" y="381" text-anchor="middle" font-size="15" font-weight="bold" fill="#153a6e">three.js 场景图</text>
<text x="805" y="402" text-anchor="middle" font-size="12.5" fill="#3a5a82">6 级嵌套 pivot(与 FK 链逐位一致)</text>
<text x="805" y="420" text-anchor="middle" font-size="12.5" fill="#3a5a82">FK 渲染 + 残差复核(双通道对账)</text>
<line x1="615" y1="460" x2="668" y2="168" stroke="#7c5cd6" stroke-width="1.8" marker-end="url(#arb)"/>
<line x1="615" y1="556" x2="668" y2="300" stroke="#7c5cd6" stroke-width="1.8" marker-end="url(#arb)"/>
<line x1="615" y1="250" x2="668" y2="392" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#ar)"/>
<line x1="805" y1="216" x2="805" y2="246" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#ar)"/>
<line x1="805" y1="324" x2="805" y2="354" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#ar)"/>
<text x="636" y="508" font-size="11.5" fill="#7c5cd6">限位 / 碰撞注入求解器与 UI</text>
<text x="621" y="262" font-size="11" fill="#5c7185" stroke="#ffffff" stroke-width="3" paint-order="stroke">几何网格</text>
<!-- human_layout → solver 直接注入 -->
<path d="M 310 130 C 480 96, 560 100, 668 138" fill="none" stroke="#1f6f43" stroke-width="1.8" stroke-dasharray="6 4" marker-end="url(#ar)"/>
<text x="470" y="84" text-anchor="middle" font-size="11.5" fill="#1f6f43">关节 rear/d 常量经 build_viewer 注入两处(CAD 与网页同源)</text>
<!-- 底部工作流 -->
<rect x="40" y="620" width="900" height="56" rx="8" fill="#f4f6f8" stroke="#9db0c0" stroke-width="1.2"/>
<text x="490" y="643" text-anchor="middle" font-size="12.5" fill="#3d5164">改几何的同步纪律:arm_model.py 重建 → AB.export 写 mesh_arm.json → build_viewer.py 重打包 → 渲染出图确认 →</text>
<text x="490" y="663" text-anchor="middle" font-size="12.5" fill="#3d5164">几何变更必须重跑 sweep_v2.py(行程/碰撞权威);collide.json 由 fit_capsules.py 重新拟合。</text>
</svg>
<figcaption><b>图 1</b> | 系统总览:一个事实源、两个碰撞模型、一条数据链。几何变更必须沿底部同步链走完整,缺一步网页就是旧的、行程就是错的。</figcaption>
</figure>
<p>关键纪律是<b>单一事实源</b>:6 根关节轴的位置 <code>rear</code> 与方向 <code>d</code> 只在 <code>human_layout.py</code> 里定义一次,CAD 装配、离线碰撞扫描、网页求解器三处都从这里取值——网页端由 <code>build_viewer.py</code> 打包时把同一组常量注入 HTML,因此"CAD 里看到的"和"网页里算的"逐位一致,不存在第二套会漂移的关节表。</p>
<p>两个碰撞模型各司其职:<b>离线精确模型</b>(sweep_v2.py)对真实 CAD 实体做布尔求交,慢(每姿态秒级)但精确,负责产出权威行程数据;<b>在线近似模型</b>(collide.json 胶囊体)把每件零件拟合成一组胶囊,快(每次 ~5ms)但近似,负责求解过程中的实时避碰。几何变更时的同步链:</p>
<pre><code>arm_model.py 重建 → AB.export 写 mesh_arm.json → build_viewer.py 重打包
→ 渲染出图确认 → sweep_v2.py 重扫(travel.json)→ fit_capsules.py 重拟合(collide.json)</code></pre>
<figure><img src="shots/01_iso.png" alt="实机等轴测渲染">
<figcaption><b>图 2</b> | 实机全貌(查看器渲染,零位姿态):6 电机人形构型,上臂 158mm / 前臂 110mm,13 件打印件 + 6 台 60mm 电机模组。</figcaption>
</figure>
</section>
<!-- ================= 第二篇 ================= -->
<section id="ch2">
<h2><span class="no">第二篇</span>运动学建模与正运动学(FK)</h2>
<h3><span class="no">2.1</span>关节拓扑</h3>
<p>对标智元灵犀 X1 的人形构型:肩部两轴(抬举+俯仰)→ 大臂自转 → 肘俯仰 → 腕俯仰 → 腕自转。所有关节轴都是坐标轴方向的(±X/±Y/±Z),这是后面一切降阶解法的前提:</p>
<table>
<tr><th>关节</th><th>名称</th><th>轴方向</th><th>轴位置(CAD 系)</th><th>驱动</th></tr>
<tr><td>J1</td><td>肩抬举</td><td>+X(滚转)</td><td>x=−51, z=610</td><td>M1 → P04 肩叉</td></tr>
<tr><td>J2</td><td>肩俯仰</td><td>+Y(俯仰)</td><td>x=19.5, z=490</td><td>M2 → P05 大臂旋转座</td></tr>
<tr><td>J3</td><td>大臂自转</td><td>−Z(滚转)</td><td>x=19.5, z=445</td><td>M3 → P06 大臂骨架</td></tr>
<tr><td>J4</td><td>肘部俯仰</td><td>+Y(俯仰)</td><td>x=19.5, z=332</td><td>M4 → P07 小臂骨架</td></tr>
<tr><td>J5</td><td>腕部俯仰</td><td>+Y(俯仰)</td><td>x=19.5, z=222</td><td>M5 → P08 腕偏转座</td></tr>
<tr><td>J6</td><td>腕自转</td><td>−Z(滚转)</td><td>x=19.5, z=176</td><td>M6 → P09 工具法兰</td></tr>
</table>
<figure><svg xmlns="http://www.w3.org/2000/svg" viewBox="0 0 980 760" font-family="'Segoe UI','PingFang SC','Microsoft YaHei',sans-serif">
<rect x="0" y="0" width="980" height="760" fill="#ffffff"/>
<defs>
<marker id="axr" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="6.5" markerHeight="6.5" orient="auto-start-reverse">
<path d="M0,0 L10,5 L0,10 z" fill="#d64545"/>
</marker>
<marker id="axb" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="6.5" markerHeight="6.5" orient="auto-start-reverse">
<path d="M0,0 L10,5 L0,10 z" fill="#3572c9"/>
</marker>
<marker id="dim" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="6" markerHeight="6" orient="auto-start-reverse">
<path d="M0,0 L10,5 L0,10 z" fill="#8a6d3b"/>
</marker>
</defs>
<text x="490" y="34" text-anchor="middle" font-size="21" font-weight="bold" fill="#16324f">运动链与关节轴(正视简化示意)</text>
<text x="490" y="58" text-anchor="middle" font-size="13" fill="#5c7185">沿 −Y 看:X 向右、Z 向上、Y 指向纸内(⊗)。零位 θ=0,臂竖直下垂。</text>
<!-- 地面与台架 -->
<line x1="70" y1="700" x2="910" y2="700" stroke="#8a9aa8" stroke-width="3"/>
<rect x="150" y="676" width="290" height="24" fill="#dfe8f0" stroke="#7ba3c7" stroke-width="1.2"/>
<rect x="228" y="80" width="52" height="598" fill="#dfe8f0" stroke="#7ba3c7" stroke-width="1.2"/>
<rect x="276" y="78" width="288" height="26" fill="#dfe8f0" stroke="#7ba3c7" stroke-width="1.2"/>
<text x="254" y="666" text-anchor="middle" font-size="11.5" fill="#5c7185">立柱 P01(台架)</text>
<text x="295" y="694" text-anchor="middle" font-size="11.5" fill="#5c7185">底座 P02</text>
<!-- 臂主干 -->
<rect x="592" y="86" width="16" height="522" rx="8" fill="#c3d2e0" stroke="#7ba3c7" stroke-width="1"/>
<rect x="560" y="72" width="80" height="36" rx="8" fill="#dbe7f3" stroke="#7ba3c7" stroke-width="1.4"/>
<!-- J1: X 轴 -->
<line x1="508" y1="90" x2="692" y2="90" stroke="#d64545" stroke-width="2.4" marker-start="url(#axr)" marker-end="url(#axr)"/>
<circle cx="600" cy="90" r="7" fill="#7c5cd6"/>
<text x="702" y="82" font-size="13" font-weight="bold" fill="#16324f">J1 肩抬举</text>
<text x="702" y="100" font-size="12" fill="#5c7185">轴 +X(滚转,前后方向)</text>
<!-- J2: Y ⊗ -->
<circle cx="600" cy="210" r="10" fill="#ffffff" stroke="#1f9d63" stroke-width="2.2"/>
<line x1="593" y1="203" x2="607" y2="217" stroke="#1f9d63" stroke-width="2.2"/>
<line x1="607" y1="203" x2="593" y2="217" stroke="#1f9d63" stroke-width="2.2"/>
<text x="620" y="202" font-size="13" font-weight="bold" fill="#16324f">J2 肩俯仰</text>
<text x="620" y="220" font-size="12" fill="#5c7185">轴 +Y(⊗,与 J4/J5 平行同向)</text>
<!-- J3: Z 自转 -->
<line x1="600" y1="225" x2="600" y2="352" stroke="#3572c9" stroke-width="1.6" stroke-dasharray="7 5" marker-start="url(#axb)" marker-end="url(#axb)"/>
<ellipse cx="600" cy="255" rx="26" ry="9" fill="none" stroke="#3572c9" stroke-width="1.6"/>
<circle cx="600" cy="255" r="5.5" fill="#7c5cd6"/>
<text x="640" y="250" font-size="13" font-weight="bold" fill="#16324f">J3 大臂自转</text>
<text x="640" y="268" font-size="12" fill="#5c7185">轴 −Z(臂自身轴线)</text>
<!-- J4 -->
<circle cx="600" cy="368" r="10" fill="#ffffff" stroke="#1f9d63" stroke-width="2.2"/>
<line x1="593" y1="361" x2="607" y2="375" stroke="#1f9d63" stroke-width="2.2"/>
<line x1="607" y1="361" x2="593" y2="375" stroke="#1f9d63" stroke-width="2.2"/>
<text x="620" y="360" font-size="13" font-weight="bold" fill="#16324f">J4 肘部俯仰</text>
<text x="620" y="378" font-size="12" fill="#5c7185">轴 +Y(⊗)</text>
<!-- J5 -->
<circle cx="600" cy="478" r="10" fill="#ffffff" stroke="#1f9d63" stroke-width="2.2"/>
<line x1="593" y1="471" x2="607" y2="485" stroke="#1f9d63" stroke-width="2.2"/>
<line x1="607" y1="471" x2="593" y2="485" stroke="#1f9d63" stroke-width="2.2"/>
<text x="620" y="470" font-size="13" font-weight="bold" fill="#16324f">J5 腕部俯仰</text>
<text x="620" y="488" font-size="12" fill="#5c7185">轴 +Y(⊗,与 J4 同向 ⇒ R4·R5 可合并)</text>
<!-- J6 -->
<line x1="600" y1="492" x2="600" y2="596" stroke="#3572c9" stroke-width="1.6" stroke-dasharray="7 5" marker-start="url(#axb)" marker-end="url(#axb)"/>
<ellipse cx="600" cy="524" rx="24" ry="8" fill="none" stroke="#3572c9" stroke-width="1.6"/>
<circle cx="600" cy="524" r="5.5" fill="#7c5cd6"/>
<text x="640" y="520" font-size="13" font-weight="bold" fill="#16324f">J6 腕自转</text>
<text x="640" y="538" font-size="12" fill="#5c7185">轴 −Z(滚转,TCP 在轴线上 ⇒ 不动位置)</text>
<!-- TCP -->
<circle cx="600" cy="607" r="6" fill="#e08a1e"/>
<text x="620" y="612" font-size="13" font-weight="bold" fill="#b06a10">TCP</text>
<text x="620" y="630" font-size="11.5" fill="#5c7185">工具法兰安装面中心(J6 组打印件最低面)</text>
<!-- 左侧尺寸链 -->
<g stroke="#8a6d3b" stroke-width="1.3">
<line x1="480" y1="90" x2="480" y2="210" marker-start="url(#dim)" marker-end="url(#dim)"/>
<line x1="480" y1="210" x2="480" y2="255" marker-start="url(#dim)" marker-end="url(#dim)"/>
<line x1="480" y1="255" x2="480" y2="368" marker-start="url(#dim)" marker-end="url(#dim)"/>
<line x1="480" y1="368" x2="480" y2="478" marker-start="url(#dim)" marker-end="url(#dim)"/>
<line x1="480" y1="478" x2="480" y2="524" marker-start="url(#dim)" marker-end="url(#dim)"/>
<line x1="470" y1="90" x2="490" y2="90"/><line x1="470" y1="210" x2="490" y2="210"/>
<line x1="470" y1="255" x2="490" y2="255"/><line x1="470" y1="368" x2="490" y2="368"/>
<line x1="470" y1="478" x2="490" y2="478"/><line x1="470" y1="524" x2="490" y2="524"/>
</g>
<g font-size="11.5" fill="#8a6d3b" text-anchor="end">
<text x="472" y="154">C0 = 120</text>
<text x="472" y="236">45</text>
<text x="472" y="315">113</text>
<text x="472" y="427">110(前臂)</text>
<text x="472" y="505">46</text>
</g>
<g font-size="11" fill="#8a9aa8">
<text x="498" y="122">z=610</text><text x="498" y="226">z=490</text><text x="498" y="271">z=445</text>
<text x="498" y="384">z=332</text><text x="498" y="494">z=222</text><text x="498" y="540">z=176</text>
</g>
<!-- 右侧段名 -->
<g font-size="12" fill="#3d5164">
<text x="800" y="154" text-anchor="middle">肩关节组</text>
<line x1="770" y1="92" x2="770" y2="212" stroke="#b9c8d6" stroke-width="1.2"/>
<text x="800" y="295" text-anchor="middle">大臂 158</text>
<line x1="770" y1="212" x2="770" y2="370" stroke="#b9c8d6" stroke-width="1.2"/>
<text x="800" y="428" text-anchor="middle">前臂 110</text>
<line x1="770" y1="370" x2="770" y2="480" stroke="#b9c8d6" stroke-width="1.2"/>
<text x="800" y="560" text-anchor="middle">腕组 46 + 法兰</text>
<line x1="770" y1="480" x2="770" y2="606" stroke="#b9c8d6" stroke-width="1.2"/>
</g>
<!-- 图例 -->
<rect x="70" y="560" width="300" height="120" rx="8" fill="#f4f6f8" stroke="#9db0c0" stroke-width="1.2"/>
<text x="86" y="586" font-size="13" font-weight="bold" fill="#16324f">图例</text>
<line x1="86" y1="606" x2="126" y2="606" stroke="#d64545" stroke-width="2.4" marker-start="url(#axr)" marker-end="url(#axr)"/>
<text x="136" y="610" font-size="12" fill="#3d5164">X 轴(水平,前后)</text>
<circle cx="106" cy="630" r="8" fill="#ffffff" stroke="#1f9d63" stroke-width="2"/>
<line x1="101" y1="625" x2="111" y2="635" stroke="#1f9d63" stroke-width="2"/>
<line x1="111" y1="625" x2="101" y2="635" stroke="#1f9d63" stroke-width="2"/>
<text x="136" y="634" font-size="12" fill="#3d5164">Y 轴(指向纸内)</text>
<line x1="106" y1="648" x2="106" y2="668" stroke="#3572c9" stroke-width="2" marker-start="url(#axb)" marker-end="url(#axb)"/>
<text x="136" y="662" font-size="12" fill="#3d5164">Z 轴(竖直,J3/J6 实为 −Z)</text>
<rect x="70" y="700" width="0" height="0" fill="none"/>
<text x="490" y="730" text-anchor="middle" font-size="12.5" fill="#3d5164">结构前提(求解器启动即断言):J2/J4/J5 平行同向且同一 x 平面(X_ARM≈19.5)、J1 ∥ X、J3/J6 ∥ Z、TCP 落在 J6 轴线上。</text>
<text x="490" y="750" text-anchor="middle" font-size="12.5" fill="#3d5164">这五条性质让六轴问题可以降阶为「二维冗余扫描 + 闭式支」,任何一条不成立时求解器会打出 WARN 并回落通用路径。</text>
</svg>
<figcaption><b>图 3</b> | 运动链与关节轴(正视简化示意)。求解器启动即断言五条结构前提:J2/J4/J5 平行同向且同一 x 平面、J1∥X、J3/J6∥Z、TCP 在 J6 轴线上——任何一条被改型破坏都会显式报警并回落通用路径,而不是悄悄算错。</figcaption>
</figure>
<figure><img src="shots/03_side.png" alt="实机侧视">
<figcaption><b>图 4</b> | 实机侧视(零位):立柱 P01 在左,臂竖直下垂——与图 3 的简化示意逐点对应。</figcaption>
</figure>
<h3><span class="no">2.2</span>正运动学:枢轴链</h3>
<p>每个关节是"绕一根不经过原点的轴"的旋转,写成枢轴变换;整条链就是六次左乘:</p>
<pre><code>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</code></pre>
<figure><svg xmlns="http://www.w3.org/2000/svg" viewBox="0 0 980 440" font-family="'Segoe UI','PingFang SC','Microsoft YaHei',sans-serif">
<rect x="0" y="0" width="980" height="440" fill="#ffffff"/>
<defs>
<marker id="fb" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="6.5" markerHeight="6.5" orient="auto-start-reverse">
<path d="M0,0 L10,5 L0,10 z" fill="#3572c9"/>
</marker>
</defs>
<text x="490" y="34" text-anchor="middle" font-size="21" font-weight="bold" fill="#16324f">正运动学:枢轴变换与链式累积</text>
<!-- 左:单关节枢轴 -->
<text x="230" y="66" text-anchor="middle" font-size="14.5" font-weight="bold" fill="#1f4f8f">单关节:绕“不在原点的轴”旋转</text>
<line x1="230" y1="110" x2="230" y2="300" stroke="#3572c9" stroke-width="1.6" stroke-dasharray="7 5"/>
<rect x="212" y="160" width="36" height="120" rx="10" fill="#e7edf3" stroke="#9db0c0" stroke-width="1.4" stroke-dasharray="5 4"/>
<rect x="212" y="160" width="36" height="120" rx="10" fill="#dbe9fb" stroke="#3572c9" stroke-width="1.6" transform="rotate(32 230 160)"/>
<ellipse cx="230" cy="160" rx="46" ry="14" fill="none" stroke="#3572c9" stroke-width="1.8" marker-end="url(#fb)"/>
<circle cx="230" cy="160" r="7" fill="#7c5cd6"/>
<text x="296" y="148" font-size="12.5" fill="#3d5164">枢轴点 P + 轴方向 d</text>
<text x="296" y="166" font-size="12.5" fill="#3d5164">θ=0 位形(虚线)→ θ 位形(蓝)</text>
<text x="296" y="184" font-size="12.5" fill="#3d5164">关节角 θi = 绕轴 d 的有符号转角</text>
<rect x="60" y="318" width="400" height="104" rx="8" fill="#f4f6f8" stroke="#9db0c0" stroke-width="1.2"/>
<text x="80" y="344" font-size="13" fill="#16324f" font-family="Consolas,monospace">A(θ) = T(P) · R(d,θ) · T(−P)</text>
<text x="80" y="368" font-size="13" fill="#16324f" font-family="Consolas,monospace">x' = R·(x − P) + P</text>
<text x="80" y="396" font-size="12" fill="#5c7185">先平移枢轴点到原点 → 绕轴转 θ → 平移回去。关节表只存</text>
<text x="80" y="412" font-size="12" fill="#5c7185">P(rear)与 d(单位轴向量),六个关节共 12 个常量。</text>
<!-- 右:链 -->
<text x="735" y="66" text-anchor="middle" font-size="14.5" font-weight="bold" fill="#1f4f8f">六链累积:World = A₁·A₂·A₃·A₄·A₅·A₆·TCP</text>
<g font-size="14" font-weight="bold" text-anchor="middle">
<rect x="510" y="130" width="52" height="44" rx="6" fill="#dbe9fb" stroke="#3572c9" stroke-width="1.5"/>
<text x="536" y="157" fill="#153a6e">A1</text>
<rect x="578" y="130" width="52" height="44" rx="6" fill="#dbe9fb" stroke="#3572c9" stroke-width="1.5"/>
<text x="604" y="157" fill="#153a6e">A2</text>
<rect x="646" y="130" width="52" height="44" rx="6" fill="#dbe9fb" stroke="#3572c9" stroke-width="1.5"/>
<text x="672" y="157" fill="#153a6e">A3</text>
<rect x="714" y="130" width="52" height="44" rx="6" fill="#dbe9fb" stroke="#3572c9" stroke-width="1.5"/>
<text x="740" y="157" fill="#153a6e">A4</text>
<rect x="782" y="130" width="52" height="44" rx="6" fill="#dbe9fb" stroke="#3572c9" stroke-width="1.5"/>
<text x="808" y="157" fill="#153a6e">A5</text>
<rect x="850" y="130" width="52" height="44" rx="6" fill="#dbe9fb" stroke="#3572c9" stroke-width="1.5"/>
<text x="876" y="157" fill="#153a6e">A6</text>
</g>
<g font-size="11" fill="#5c7185" text-anchor="middle">
<text x="536" y="190">θ1</text><text x="604" y="190">θ2</text><text x="672" y="190">θ3</text>
<text x="740" y="190">θ4</text><text x="808" y="190">θ5</text><text x="876" y="190">θ6</text>
</g>
<g stroke="#5a7d9a" stroke-width="1.5">
<line x1="472" y1="152" x2="508" y2="152"/>
<line x1="562" y1="152" x2="576" y2="152"/>
<line x1="630" y1="152" x2="644" y2="152"/>
<line x1="698" y1="152" x2="712" y2="152"/>
<line x1="766" y1="152" x2="780" y2="152"/>
<line x1="834" y1="152" x2="848" y2="152"/>
<line x1="902" y1="152" x2="918" y2="152"/>
</g>
<text x="462" y="157" text-anchor="end" font-size="13" font-weight="bold" fill="#16324f">World</text>
<text x="960" y="157" text-anchor="end" font-size="13" font-weight="bold" fill="#b06a10">TCP</text>
<text x="735" y="216" text-anchor="middle" font-size="12" fill="#5c7185">每级只读父级位姿与自己 —— 与 three.js 场景图 6 级嵌套 pivot 逐位一致</text>
<rect x="510" y="240" width="420" height="192" rx="8" fill="#f4f6f8" stroke="#9db0c0" stroke-width="1.2"/>
<text x="530" y="266" font-size="13" font-weight="bold" fill="#16324f">两套实现,互为对账:</text>
<text x="530" y="290" font-size="12.5" fill="#3d5164">· fkChain():严格 4×4 同质矩阵链 —— 对照基准,慢而直白</text>
<text x="530" y="312" font-size="12.5" fill="#3d5164">· fk():按结构常量展开三角函数的闭式快路径 —— 求解器</text>
<text x="530" y="332" font-size="12.5" fill="#3d5164"> 热路径专用,每姿态仅 ~20 次乘加</text>
<text x="530" y="360" font-size="12.5" fill="#3d5164">· 自检:5000 个随机位形,两实现的 TCP 差 ≤ 5.7e-13 mm</text>
<text x="530" y="384" font-size="12.5" fill="#3d5164">· 工具长度 tool 沿 J6 轴外伸:TCP_z 随 tool 平移(ax=J6 轴)</text>
<text x="530" y="406" font-size="11.5" fill="#8a9aa8">指纹校验:预置自检值绑定(限位表+常量+源码)的 FNV-1a 指纹,</text>
<text x="530" y="422" font-size="11.5" fill="#8a9aa8">模型或求解器一变即提示重算。</text>
</svg>
<figcaption><b>图 5</b> | 枢轴变换与链式累积:两套实现互为对账——fkChain() 严格 4×4 矩阵链做基准,fk() 展开三角函数做热路径;5000 随机位形最大差 5.7e-13 mm。</figcaption>
</figure>
<p>三个对算法影响深远的结构性质:</p>
<ul>
<li><b>J4 ∥ J5 同向</b> ⇒ <code>R4·R5 = R_y(θ4+θ5)</code>,腕部两个俯仰在"工具轴指向"上合并成一个未知数 s = θ4+θ5;</li>
<li><b>TCP 在 J6 轴线上</b> ⇒ θ6 只提供绕工具轴的滚转,<b>完全不改变 TCP 位置</b>——位置任务与姿态任务由此解耦;</li>
<li><b>工具轴(法兰 +Z)= (R1…R5)·ẑ</b>,与 θ6 无关——姿态任务只剩 2 个方程。</li>
</ul>
</section>
<!-- ================= 第三篇 ================= -->
<section id="ch3">
<h2><span class="no">第三篇</span>逆运动学:闭式支 + 二维冗余扫描 + 牛顿收口</h2>
<h3 id="s31"><span class="no">3.1</span>问题形式与降阶</h3>
<p>6 自由度机械臂,任务是给定 TCP 目标点(位置任务,3 个方程)或"目标点 + 工具姿态 {方位角 az, 仰角 el, 滚转 roll}"(位姿任务,再加 3 个方程)。臂本身有 6 个未知数,位置任务下存在 <b>2 维冗余</b>。</p>
<p>核心思路(<code>branch()</code>):<b>把 (θ4, θ5) 当作冗余参数</b>。一旦给定这两个角,腕心相对量 V 完全确定(u=−L·sinθ5, w=A54−L·cosθ5, dx=u·c4+w·s4, dz=A43−u·s4+w·c4),剩下的 θ1..θ3 可以用平面三角学<b>一步闭式解出</b>(每格至多 4 支:θ3 的 ±σ 与 ±(π−σ)),位置精确到浮点误差,不需要任何迭代。</p>
<figure><svg xmlns="http://www.w3.org/2000/svg" viewBox="0 0 980 580" font-family="'Segoe UI','PingFang SC','Microsoft YaHei',sans-serif">
<rect x="0" y="0" width="980" height="580" fill="#ffffff"/>
<defs>
<marker id="ga" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="6.5" markerHeight="6.5" orient="auto-start-reverse">
<path d="M0,0 L10,5 L0,10 z" fill="#8a9aa8"/>
</marker>
</defs>
<text x="490" y="34" text-anchor="middle" font-size="21" font-weight="bold" fill="#16324f">闭式支 branch():给定 (θ4,θ5),θ1..θ3 一步解出(简化示意)</text>
<!-- 左panel -->
<text x="255" y="66" text-anchor="middle" font-size="14.5" font-weight="bold" fill="#1f4f8f">第 1 步:(θ4,θ5) ⇒ 腕心平面坐标 V</text>
<line x1="150" y1="120" x2="150" y2="330" stroke="#3572c9" stroke-width="1.6" stroke-dasharray="7 5"/>
<text x="150" y="352" text-anchor="middle" font-size="11.5" fill="#5c7185">J4 轴(+Y,⊗)</text>
<circle cx="150" cy="140" r="8" fill="#ffffff" stroke="#1f9d63" stroke-width="2"/>
<line x1="144" y1="134" x2="156" y2="146" stroke="#1f9d63" stroke-width="2"/>
<line x1="156" y1="134" x2="144" y2="146" stroke="#1f9d63" stroke-width="2"/>
<line x1="150" y1="140" x2="278" y2="188" stroke="#3572c9" stroke-width="5" stroke-linecap="round"/>
<circle cx="278" cy="188" r="8" fill="#ffffff" stroke="#1f9d63" stroke-width="2"/>
<line x1="272" y1="182" x2="284" y2="194" stroke="#1f9d63" stroke-width="2"/>
<line x1="284" y1="182" x2="272" y2="194" stroke="#1f9d63" stroke-width="2"/>
<text x="292" y="186" font-size="11.5" fill="#5c7185">J5 轴</text>
<line x1="278" y1="188" x2="330" y2="252" stroke="#3572c9" stroke-width="4" stroke-linecap="round"/>
<circle cx="330" cy="252" r="6" fill="#e08a1e"/>
<text x="344" y="258" font-size="12" font-weight="bold" fill="#b06a10">腕心方向(L = L5+tool)</text>
<path d="M 150 170 A 30 30 0 0 0 178 150.5" fill="none" stroke="#7c5cd6" stroke-width="1.6"/>
<text x="186" y="178" font-size="12" fill="#7c5cd6">θ4</text>
<path d="M 262 216 A 30 30 0 0 1 296 214" fill="none" stroke="#7c5cd6" stroke-width="1.6"/>
<text x="272" y="232" font-size="12" fill="#7c5cd6">θ5</text>
<text x="150" y="120" font-size="11.5" fill="#5c7185">肘(J4)</text>
<rect x="50" y="386" width="420" height="152" rx="8" fill="#f4f6f8" stroke="#9db0c0" stroke-width="1.2"/>
<text x="68" y="412" font-size="12.5" fill="#16324f" font-family="Consolas,monospace">u = −L·sinθ5 w = A54 − L·cosθ5</text>
<text x="68" y="436" font-size="12.5" fill="#16324f" font-family="Consolas,monospace">dx = u·cosθ4 + w·sinθ4</text>
<text x="68" y="460" font-size="12.5" fill="#16324f" font-family="Consolas,monospace">dz = A43 − u·sinθ4 + w·cosθ4</text>
<text x="68" y="484" font-size="12.5" fill="#16324f" font-family="Consolas,monospace">V = (dx, dz + A32)</text>
<text x="68" y="514" font-size="11.5" fill="#5c7185">J4∥J5 同向 ⇒ R4·R5 = R(θ4+θ5):腕心只由这两个角决定,</text>
<text x="68" y="530" font-size="11.5" fill="#5c7185">姿态任务里工具轴对齐也因此只差 2 个未知数。</text>
<!-- 右panel -->
<text x="735" y="66" text-anchor="middle" font-size="14.5" font-weight="bold" fill="#1f4f8f">第 2 步:肩部圆交 + 肘部四支 ⇒ θ1/θ2/θ3</text>
<!-- 俯视:J1 轴与 φ -->
<circle cx="600" cy="130" r="9" fill="#ffffff" stroke="#d64545" stroke-width="2"/>
<circle cx="600" cy="130" r="2" fill="#d64545"/>
<text x="576" y="112" font-size="11.5" fill="#5c7185">J1 轴(+X,⊙)</text>
<circle cx="700" cy="96" r="5" fill="#e08a1e"/>
<text x="712" y="94" font-size="11.5" fill="#b06a10">目标(柱坐标 R, φ)</text>
<line x1="600" y1="130" x2="700" y2="96" stroke="#e08a1e" stroke-width="1.6" stroke-dasharray="5 4"/>
<path d="M 640 117 A 42 42 0 0 0 636 108" fill="none" stroke="#7c5cd6" stroke-width="1.6"/>
<text x="648" y="106" font-size="12" fill="#7c5cd6">φ</text>
<text x="566" y="158" font-size="12" fill="#16324f" font-family="Consolas,monospace">θ1 = φ − atan2(cc, b)</text>
<!-- 正视:肩 C0 偏置与肘三角 -->
<circle cx="620" cy="290" r="9" fill="#ffffff" stroke="#1f9d63" stroke-width="2"/>
<line x1="614" y1="284" x2="626" y2="296" stroke="#1f9d63" stroke-width="2"/>
<line x1="626" y1="284" x2="614" y2="296" stroke="#1f9d63" stroke-width="2"/>
<text x="560" y="294" text-anchor="end" font-size="11.5" fill="#5c7185">J1(正视)</text>
<circle cx="620" cy="382" r="9" fill="#ffffff" stroke="#1f9d63" stroke-width="2"/>
<line x1="614" y1="376" x2="626" y2="388" stroke="#1f9d63" stroke-width="2"/>
<line x1="626" y1="376" x2="614" y2="388" stroke="#1f9d63" stroke-width="2"/>
<text x="560" y="386" text-anchor="end" font-size="11.5" fill="#5c7185">J2</text>
<line x1="620" y1="299" x2="620" y2="373" stroke="#8a6d3b" stroke-width="1.3" marker-start="url(#ga)" marker-end="url(#ga)"/>
<text x="632" y="341" font-size="11.5" fill="#8a6d3b">C0=120</text>
<line x1="620" y1="382" x2="700" y2="322" stroke="#3572c9" stroke-width="4" stroke-linecap="round"/>
<line x1="700" y1="322" x2="820" y2="290" stroke="#3572c9" stroke-width="4" stroke-linecap="round"/>
<circle cx="820" cy="290" r="6" fill="#e08a1e"/>
<text x="834" y="294" font-size="12" font-weight="bold" fill="#b06a10">腕心(由 V 决定)</text>
<path d="M 676 342 A 44 44 0 0 0 666 314" fill="none" stroke="#7c5cd6" stroke-width="1.6"/>
<text x="688" y="318" font-size="12" fill="#7c5cd6">σ</text>
<text x="690" y="368" font-size="11.5" fill="#5c7185">肘部三角:σ=acos(√m2)</text>
<text x="690" y="386" font-size="11.5" fill="#5c7185">θ3 四支:±σ 与 ±(π−σ)</text>
<text x="690" y="404" font-size="11.5" fill="#5c7185">(肘上/肘下 × 同号/反号镜像族)</text>
<rect x="510" y="428" width="440" height="110" rx="8" fill="#f4f6f8" stroke="#9db0c0" stroke-width="1.2"/>
<text x="528" y="454" font-size="12.5" fill="#16324f" font-family="Consolas,monospace">n = (R²+A²−Vx²−Vz²−C0²) / (2·C0),cc = C0+n</text>
<text x="528" y="478" font-size="12.5" fill="#16324f" font-family="Consolas,monospace">θ2 = atan2((λA−κn)/det, (κA+λn)/det) κ=Vx·c3, λ=Vz</text>
<text x="528" y="502" font-size="12.5" fill="#16324f" font-family="Consolas,monospace">b = −Vx·sinθ3 (θ1 由柱角 φ 反推)</text>
<text x="528" y="526" font-size="11.5" fill="#8a5433">退化防线:|Vx|&lt;1e-7、det&lt;1e-12、m2 越界 → 该支无解,直接落选</text>
<text x="490" y="560" text-anchor="middle" font-size="12.5" fill="#3d5164">示意省略了 X0/Y0 基准平移与 tool 常量;四支解的位置全部精确命中目标 ——「扫描代替数值搜索」的根基。</text>
</svg>
<figcaption><b>图 6</b> | 闭式支 branch():给定 (θ4,θ5),θ1..θ3 一步解出。四支解的位置全部精确命中目标——「扫描代替数值搜索」的根基。</figcaption>
</figure>
<p>于是"求 IK"变成了"在 (θ4,θ5) 二维平面上挑最好的格点"——<b>扫描代替数值搜索</b>。每个格点的解天然精确,扫描的全部意义在于挑出代价(关节行程)最小的那支构型。</p>
<h3 id="s32"><span class="no">3.2</span>姿态参数化:方位角 / 仰角 / 滚转</h3>
<p>姿态任务用三个角描述工具姿态(<code>poseMat()</code> 构造工具系旋转矩阵,<code>poseAng()</code> 逆分解)。三个关键性质撑起了姿态算法:</p>
<ul>
<li>θ6 只提供绕工具轴的滚转,<b>工具轴 = (R1…R5)·ẑ 与 θ6 无关</b>——姿态任务因此只剩 2 个方程;</li>
<li>滚转由 <code>rollClose()</code> 闭式补出:θ6 = −atan2(N[1,0], N[0,0]),其中 N = R5ᵀ·R_target;</li>
<li>推论:<b>全姿态偏差 oerr ≥ 工具轴夹角</b>(20 万随机样本验证零违反)——粗扫用一次点积预筛(1−cos2°)就挡掉所有必被拒的候选,免掉矩阵链 + acos。</li>
</ul>
<figure><svg xmlns="http://www.w3.org/2000/svg" viewBox="0 0 980 440" font-family="'Segoe UI','PingFang SC','Microsoft YaHei',sans-serif">
<rect x="0" y="0" width="980" height="440" fill="#ffffff"/>
<defs>
<marker id="zb" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="6.5" markerHeight="6.5" orient="auto-start-reverse">
<path d="M0,0 L10,5 L0,10 z" fill="#3572c9"/>
</marker>
<marker id="zo" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="6.5" markerHeight="6.5" orient="auto-start-reverse">
<path d="M0,0 L10,5 L0,10 z" fill="#e08a1e"/>
</marker>
</defs>
<text x="490" y="34" text-anchor="middle" font-size="21" font-weight="bold" fill="#16324f">工具姿态参数化:方位角 az · 仰角 el · 滚转 roll</text>
<!-- 左:球面示意 -->
<circle cx="240" cy="220" r="130" fill="#f4f8fb" stroke="#b9c8d6" stroke-width="1.4"/>
<ellipse cx="240" cy="220" rx="130" ry="40" fill="none" stroke="#b9c8d6" stroke-width="1" stroke-dasharray="4 3"/>
<line x1="240" y1="220" x2="240" y2="70" stroke="#3572c9" stroke-width="2" marker-end="url(#zb)"/>
<text x="252" y="80" font-size="12.5" fill="#2a4a6e">CAD +Z</text>
<line x1="240" y1="220" x2="400" y2="220" stroke="#8a9aa8" stroke-width="1.4"/>
<text x="406" y="224" font-size="12" fill="#5c7185">方位基线</text>
<!-- 工具轴 -->
<line x1="240" y1="220" x2="296" y2="96" stroke="#e08a1e" stroke-width="2.6" marker-end="url(#zo)"/>
<text x="306" y="94" font-size="12.5" font-weight="bold" fill="#b06a10">工具轴</text>
<text x="306" y="112" font-size="11.5" fill="#b06a10">(法兰 +Z 外伸)</text>
<!-- el 弧 -->
<path d="M 240 128 A 92 92 0 0 1 277 136" fill="none" stroke="#7c5cd6" stroke-width="1.7"/>
<text x="92" y="140" font-size="12.5" fill="#7c5cd6">el(仰角,自 +Z 抬起)</text>
<text x="92" y="158" font-size="12.5" fill="#7c5cd6">0…180°</text>
<line x1="216" y1="136" x2="258" y2="131" stroke="#7c5cd6" stroke-width="1" stroke-dasharray="3 3"/>
<!-- roll 环绕工具轴 -->
<ellipse cx="270" cy="153" rx="20" ry="9" fill="none" stroke="#d64545" stroke-width="1.7" transform="rotate(-24 270 153)"/>
<text x="92" y="196" font-size="12.5" fill="#b03030">roll(绕轴滚转)</text>
<text x="92" y="214" font-size="12.5" fill="#b03030">= θ6,绕工具轴</text>
<line x1="212" y1="198" x2="256" y2="160" stroke="#d64545" stroke-width="1" stroke-dasharray="3 3"/>
<!-- az 弧(水平面内) -->
<path d="M 362 220 A 122 38 0 0 0 330 182" fill="none" stroke="#1f9d63" stroke-width="1.7"/>
<text x="320" y="262" font-size="12.5" fill="#1f7a4d">az(绕 Z 方位角,−180…180°)</text>
<circle cx="240" cy="220" r="4" fill="#16324f"/>
<text x="160" y="248" font-size="12" fill="#5c7185">法兰中心</text>
<!-- 右:公式 -->
<rect x="470" y="70" width="470" height="180" rx="8" fill="#f4f6f8" stroke="#9db0c0" stroke-width="1.2"/>
<text x="488" y="96" font-size="13" font-weight="bold" fill="#16324f">poseMat(az, el, roll) → 工具系旋转矩阵(列 = x/y/z)</text>
<text x="488" y="122" font-size="12.5" fill="#16324f" font-family="Consolas,monospace">z = (sin el·cos az, sin el·sin az, cos el)</text>
<text x="488" y="146" font-size="12.5" fill="#16324f" font-family="Consolas,monospace">x0 = (−sin az, cos az, 0) (子午面基,恒⊥z)</text>
<text x="488" y="170" font-size="12.5" fill="#16324f" font-family="Consolas,monospace">x = R(z, roll)·x0  y = z × x</text>
<text x="488" y="200" font-size="11.5" fill="#5c7185">poseAng() 为其逆分解;el ∈ [0,180],az/roll ∈ (−180,180]。</text>
<text x="488" y="220" font-size="11.5" fill="#5c7185">界面以「工具朝外」描述;求解器内部法兰法向朝臂内,</text>
<text x="488" y="236" font-size="11.5" fill="#5c7185">两者相差绕工具 x 轴 180°(RX180),入栈/出栈各翻一次。</text>
<rect x="470" y="266" width="470" height="140" rx="8" fill="#fdf6ec" stroke="#c99418" stroke-width="1.3"/>
<text x="488" y="292" font-size="13" font-weight="bold" fill="#7a5a08">为什么姿态任务只剩 2 个未知数</text>
<text x="488" y="316" font-size="12" fill="#6b5312">· θ6 只提供绕工具轴的滚转,不改变工具轴 ⇒ 工具轴 = (R1…R5)·ẑ</text>
<text x="488" y="338" font-size="12" fill="#6b5312">· 滚转由 rollClose 闭式补出:θ6 = −atan2(N[1,0], N[0,0]),N = R5ᵀ·Rtar</text>
<text x="488" y="360" font-size="12" fill="#6b5312">· 推论:全姿态偏差 oerr ≥ 工具轴夹角(20 万随机样本验证零违反)——</text>
<text x="488" y="380" font-size="12" fill="#6b5312"> 粗扫用一次点积预筛(1−cos2°)挡掉必被拒的候选,省掉矩阵链+acos</text>
</svg>
<figcaption><b>图 7</b> | 工具姿态参数化 az/el/roll:界面以"工具朝外"描述,求解器内部法兰法向朝臂内,相差绕工具 x 轴 180°(RX180),入栈/出栈各翻一次。</figcaption>
</figure>
<h3 id="s33"><span class="no">3.3</span>求解流水线</h3>
<figure><svg xmlns="http://www.w3.org/2000/svg" viewBox="0 0 980 920" font-family="'Segoe UI','PingFang SC','Microsoft YaHei',sans-serif">
<rect x="0" y="0" width="980" height="920" fill="#ffffff"/>
<defs>
<marker id="pa" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="6.5" markerHeight="6.5" orient="auto-start-reverse">
<path d="M0,0 L10,5 L0,10 z" fill="#5a7d9a"/>
</marker>
<marker id="pao" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="6.5" markerHeight="6.5" orient="auto-start-reverse">
<path d="M0,0 L10,5 L0,10 z" fill="#b06a10"/>
</marker>
</defs>
<text x="490" y="34" text-anchor="middle" font-size="21" font-weight="bold" fill="#16324f">逆运动学求解流水线(solve → solveDyn)</text>
<!-- 主流程 -->
<g>
<rect x="265" y="60" width="450" height="64" rx="8" fill="#dbe9fb" stroke="#3572c9" stroke-width="1.6"/>
<text x="280" y="84" font-size="12.5" font-weight="bold" fill="#153a6e">① 输入</text>
<text x="280" y="102" font-size="12" fill="#2a4a6e">目标 TCP (x,y,z) · 工具长 tool · 可选姿态 R(az,el,roll)</text>
<text x="280" y="118" font-size="12" fill="#2a4a6e">ref = 当前姿态:取「本步行程最小」的构型</text>
<rect x="265" y="148" width="450" height="64" rx="8" fill="#f4f8fb" stroke="#7ba3c7" stroke-width="1.4"/>
<text x="280" y="172" font-size="12.5" font-weight="bold" fill="#16324f">② 冗余维准备:(θ4,θ5) 网格 + 逐行可行段探测</text>
<text x="280" y="190" font-size="12" fill="#3d5164">步长 h=0.25°;先按探测步 hp 找每行有解的连续段(位置 2° / 姿态 0.5°)</text>
<text x="280" y="206" font-size="12" fill="#3d5164">段端外扩 PAD=2° 兜住段界不确定性;段内仍按 h 精扫,密度不降</text>
<rect x="265" y="236" width="450" height="64" rx="8" fill="#f4f8fb" stroke="#7ba3c7" stroke-width="1.4"/>
<text x="280" y="260" font-size="12.5" font-weight="bold" fill="#16324f">③ 全域粗扫:每格 branch() 闭式解 θ1..θ3(≤4 支)</text>
<text x="280" y="279" font-size="12" fill="#3d5164">位置由闭式支精确命中(零残差),扫描只优化冗余维;候选有界保留 4000</text>
<rect x="265" y="324" width="450" height="64" rx="8" fill="#fdf6ec" stroke="#c99418" stroke-width="1.3"/>
<text x="280" y="348" font-size="12.5" font-weight="bold" fill="#7a5a08">④ [姿态任务] 孤立根枚举:残差变号括号 → 阻尼高斯-牛顿收根</text>
<text x="280" y="367" font-size="12" fill="#6b5312">切向相切的根不变号,按每支 ‖e‖ 最小的节点补种;精确根直接进候选池</text>
<rect x="265" y="412" width="450" height="64" rx="8" fill="#f4f8fb" stroke="#7ba3c7" stroke-width="1.4"/>
<text x="280" y="436" font-size="12.5" font-weight="bold" fill="#16324f">⑤ 局部加密 + 模式搜索:32 种子 × 窗口 ±2h(步长 h/8)</text>
<text x="280" y="455" font-size="12" fill="#3d5164">8 邻域模式搜索步长逐轮减半至 2e-4°,把每条谷底走到最低</text>
<rect x="265" y="500" width="450" height="64" rx="8" fill="#fdf6ec" stroke="#c99418" stroke-width="1.3"/>
<text x="280" y="524" font-size="12.5" font-weight="bold" fill="#7a5a08">⑥ [姿态任务] 二维牛顿收口 + escalating 加压</text>
<text x="280" y="542" font-size="12" fill="#6b5312">对工具轴切向偏差做 (θ4,θ5) 牛顿迭代,轴残差 →1e-12、位置保持精确</text>
<text x="280" y="558" font-size="12" fill="#6b5312">仍无 oerr≤0.05° 达标解则姿态权重 ×100 重扫(最多 3 轮)</text>
<rect x="265" y="588" width="450" height="64" rx="8" fill="#f4f8fb" stroke="#7ba3c7" stroke-width="1.4"/>
<text x="280" y="612" font-size="12.5" font-weight="bold" fill="#16324f">⑦ 选解:cost = Σ w·|wrap180(θ−ref)| + 1e-6·Σ|θ|</text>
<text x="280" y="630" font-size="12" fill="#3d5164">姿态任务先取 oerr≤0.05° 达标集,集内再比代价</text>
<text x="280" y="646" font-size="12" fill="#3d5164">精确根只在代价 ≤ 达标最优+2° 时接管(防漏检多付几十度)</text>
<rect x="265" y="676" width="450" height="64" rx="8" fill="#eef8f1" stroke="#1f6f43" stroke-width="1.4"/>
<text x="280" y="700" font-size="12.5" font-weight="bold" fill="#14472c">⑧ solveDyn 碰撞后处理(装了胶囊模型时)</text>
<text x="280" y="718" font-size="12" fill="#2c5c44">解先过全姿态碰撞校验:不安全 → 候选表找 ≤1.5×cost+20° 的安全解</text>
<text x="280" y="734" font-size="12" fill="#2c5c44">贴边轴二分动态边界放宽(≥5° 才值得重解,重解 cost 不升才采用)</text>
<rect x="265" y="764" width="450" height="58" rx="8" fill="#dbe9fb" stroke="#3572c9" stroke-width="1.6"/>
<text x="280" y="788" font-size="12.5" font-weight="bold" fill="#153a6e">输出:q[6] · 残差 err / oerr · 本步行程 moveSum · 候选表 list</text>
<text x="280" y="806" font-size="12" fill="#2a4a6e">徽章分级:已到达 / 姿态到达·位置最近 / 混合折中 / 不可达·最近位姿</text>
</g>
<g stroke="#5a7d9a" stroke-width="1.8">
<line x1="490" y1="124" x2="490" y2="146" marker-end="url(#pa)"/>
<line x1="490" y1="212" x2="490" y2="234" marker-end="url(#pa)"/>
<line x1="490" y1="300" x2="490" y2="322" marker-end="url(#pa)"/>
<line x1="490" y1="388" x2="490" y2="410" marker-end="url(#pa)"/>
<line x1="490" y1="476" x2="490" y2="498" marker-end="url(#pa)"/>
<line x1="490" y1="564" x2="490" y2="586" marker-end="url(#pa)"/>
<line x1="490" y1="652" x2="490" y2="674" marker-end="url(#pa)"/>
<line x1="490" y1="740" x2="490" y2="762" marker-end="url(#pa)"/>
</g>
<!-- 右侧:快路 -->
<rect x="740" y="236" width="215" height="196" rx="10" fill="#ffffff" stroke="#b06a10" stroke-width="1.6" stroke-dasharray="7 5"/>
<text x="755" y="262" font-size="13" font-weight="bold" fill="#b06a10">solveWarm 实时快路</text>
<text x="755" y="284" font-size="11.5" fill="#6b5312">拖拽中:只在上帧解附近的</text>
<text x="755" y="302" font-size="11.5" fill="#6b5312">(θ4,θ5) 递进窗口 ±4→14→40°</text>
<text x="755" y="320" font-size="11.5" fill="#6b5312">同支跟随(Σ|Δθ|≤25°)+ 残差</text>
<text x="755" y="338" font-size="11.5" fill="#6b5312">≤1e-6 + 姿态牛顿收口</text>
<text x="755" y="362" font-size="11.5" font-weight="bold" fill="#b06a10">任一校验不过 → null</text>
<text x="755" y="380" font-size="11.5" fill="#6b5312">自动回落全量扫描(③)</text>
<text x="755" y="400" font-size="11" fill="#8a9aa8">快路只负责「能快时快」,</text>
<text x="755" y="416" font-size="11" fill="#8a9aa8">松手后全量求解才是终值</text>
<path d="M 738 268 C 727 240, 727 130, 719 96" fill="none" stroke="#b06a10" stroke-width="1.5" stroke-dasharray="6 4" marker-end="url(#pao)"/>
<path d="M 847 434 C 847 480, 700 560, 721 600" fill="none" stroke="#b06a10" stroke-width="1.5" stroke-dasharray="6 4" marker-end="url(#pao)"/>
<text x="742" y="470" font-size="11" fill="#b06a10">校验不过回落</text>
<!-- 左侧:兜底 -->
<rect x="25" y="324" width="215" height="230" rx="10" fill="#ffffff" stroke="#8a5a8f" stroke-width="1.6" stroke-dasharray="7 5"/>
<text x="40" y="350" font-size="13" font-weight="bold" fill="#7a3a80">兜底:目标不可达时</text>
<text x="40" y="372" font-size="11.5" fill="#5c3a60">closest() 位置 DLS:160 迭代 ×</text>
<text x="40" y="390" font-size="11.5" fill="#5c3a60">160 次 LCG 确定性重启(同输入</text>
<text x="40" y="408" font-size="11.5" fill="#5c3a60">逐位一致)+ anchor 回拉 0.03</text>
<text x="40" y="426" font-size="11.5" fill="#5c3a60">压退化漂移 + 零空间抛光收敛</text>
<text x="40" y="450" font-size="11.5" fill="#5c3a60">closest6() 位置+姿态 6 维联合</text>
<text x="40" y="468" font-size="11.5" fill="#5c3a60">DLS(姿态按 20mm/rad 折算)</text>
<text x="40" y="492" font-size="11.5" fill="#5c3a60">closestOri() 姿态优先:闭式轴</text>
<text x="40" y="510" font-size="11.5" fill="#5c3a60">对齐 + (θ3,s,θ4) 坐标下降,</text>
<text x="40" y="528" font-size="11.5" fill="#5c3a60">姿态精确、位置取最近</text>
<text x="40" y="548" font-size="11" fill="#8a9aa8">距离差 ≤0.5mm 时优先锚定解</text>
<path d="M 263 620 C 180 620, 140 590, 133 556" fill="none" stroke="#8a5a8f" stroke-width="1.5" stroke-dasharray="6 4" marker-end="url(#pao)"/>
<text x="148" y="648" font-size="11" fill="#8a5a8f">0 采样点 / 不可达</text>
<path d="M 133 322 C 140 220, 200 130, 262 96" fill="none" stroke="#8a5a8f" stroke-width="1.5" stroke-dasharray="6 4" marker-end="url(#pao)"/>
<text x="120" y="200" font-size="11" fill="#8a5a8f">返回最近位姿</text>
<text x="490" y="860" text-anchor="middle" font-size="12.5" fill="#3d5164">热路径性能手段:轴对齐特化 rotAxis(免 Rodrigues 通用式)· sin/cos 单槽缓存 · 关节限位数组预取 · 残差 fk 复核延后到入选候选统一补算</text>
<text x="490" y="884" text-anchor="middle" font-size="12.5" fill="#3d5164">全量扫描 40~430 ms(后台 Worker);实时快路毫秒级 —— 这是拖拽能跟手的原因</text>
</svg>
<figcaption><b>图 8</b> | 逆运动学求解流水线:主流程七步 + solveWarm 实时快路 + 不可达兜底。全量扫描 40~430ms(后台 Worker),快路毫秒级——这是拖拽能跟手的原因。</figcaption>
</figure>
<ol>
<li><b>逐行可行段探测</b>:位置闭式支只在平面三角有解时存在,限位矩形里只有 7~27% 的格点能过判据。先用探测步 hp 找每行的连续可行段(位置任务 2°、姿态任务 0.5°——姿态任务的可行带实测最窄 <b>0.98°</b>,2° 探测会整条漏掉造成"误报不可达"),段端外扩 PAD=2°,段内仍按原步长 h(默认 0.25°)精扫:密度不降、成本大降。</li>
<li><b>全域粗扫</b>:每格 <code>branch()</code> 出 ≤4 支 → 限位筛(NaN 候选用否定式写法保证落选)→ 打分,候选有界保留 4000。</li>
<li><b>孤立根枚举</b>(姿态任务):不再靠网格密度去撞窄带——逐行对姿态残差两个分量找<b>变号括号</b>,从括号中点沿该支跑阻尼高斯-牛顿收根;切向相切的根不变号,按每支 ‖e‖ 最小的节点补种。</li>
<li><b>局部加密 + 模式搜索</b>:32 个种子各开 ±2h 窗口(步长 h/8,小步长档 h/4),再对最优的 24 个候选做 8 邻域模式搜索(步长逐轮减半至 2e-4°)。</li>
<li><b>姿态收口</b>:姿态达标带是 (θ4,θ5) 平面上的斜窄谷(局部灵敏度可达 ~10°/°),模式搜索只能停在谷壁(实测最好 0.106°)。直接对"工具轴在目标轴法平面上的切向偏差"做 <b>(θ4,θ5) 二维牛顿迭代</b>(每步重解闭式位置支、按 θ1..θ3 最近匹配保持同支),轴残差压到 <b>1e-12</b> 量级、位置全程保持精确;尚无 oerr≤0.05° 的解时姿态权重 ×100 重扫(最多 3 轮)。</li>
<li><b>选解</b>:<code>cost = Σ w·|wrap180(θ−ref)| + 1e-6·Σ|θ|</code>。ref 是当前姿态——判据是「<b>所有电机需要转过的角度之和最小</b>」(这一步行程最短),连续轴取等价角防绕远。姿态任务先取 oerr≤0.05° 的达标集、集内再比代价,"省几度关节角、丢 0.2° 姿态"的交易被显式禁止。</li>
<li><b>solveDyn 碰撞后处理</b>:见第六篇。</li>
</ol>
<figure><svg xmlns="http://www.w3.org/2000/svg" viewBox="0 0 980 620" font-family="'Segoe UI','PingFang SC','Microsoft YaHei',sans-serif">
<rect x="0" y="0" width="980" height="620" fill="#ffffff"/>
<defs>
<marker id="ta" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="6.5" markerHeight="6.5" orient="auto-start-reverse">
<path d="M0,0 L10,5 L0,10 z" fill="#8a9aa8"/>
</marker>
<marker id="to" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="6.5" markerHeight="6.5" orient="auto-start-reverse">
<path d="M0,0 L10,5 L0,10 z" fill="#e08a1e"/>
</marker>
</defs>
<text x="490" y="34" text-anchor="middle" font-size="21" font-weight="bold" fill="#16324f">冗余维 (θ4,θ5) 平面:可行带、加密窗口与牛顿收口</text>
<!-- 绘图区 -->
<rect x="110" y="80" width="740" height="430" fill="#f8fafc" stroke="#b9c8d6" stroke-width="1.2"/>
<!-- 可行带(斜窄带,宽度做了夸张) -->
<polygon points="245,452 830,168 830,132 245,416" fill="#d9f2e5" opacity="0.9"/>
<text x="585" y="215" font-size="12" fill="#1f7a4d" transform="rotate(-26 585 215)">可行带:branch 判据筛掉 73~93% 的格点</text>
<!-- 不可行散点 -->
<g fill="#c3ccd4" font-size="10">
<text x="200" y="140">×</text><text x="330" y="120">×</text><text x="500" y="110">×</text>
<text x="700" y="128">×</text><text x="790" y="180">×</text><text x="170" y="300">×</text>
<text x="300" y="260">×</text><text x="430" y="210">×</text><text x="640" y="150">×</text>
<text x="220" y="470">×</text><text x="480" y="480">×</text><text x="700" y="460">×</text>
<text x="820" y="430">×</text><text x="360" y="420">×</text><text x="560" y="440">×</text>
</g>
<!-- 可行带内格点 -->
<g fill="#2f9d68">
<circle cx="270" cy="428" r="2.4"/><circle cx="310" cy="410" r="2.4"/><circle cx="352" cy="390" r="2.4"/>
<circle cx="395" cy="368" r="2.4"/><circle cx="438" cy="348" r="2.4"/><circle cx="480" cy="328" r="2.4"/>
<circle cx="522" cy="308" r="2.4"/><circle cx="565" cy="288" r="2.4"/><circle cx="608" cy="268" r="2.4"/>
<circle cx="650" cy="248" r="2.4"/><circle cx="692" cy="228" r="2.4"/><circle cx="735" cy="206" r="2.4"/>
<circle cx="778" cy="186" r="2.4"/><circle cx="812" cy="168" r="2.4"/>
<circle cx="292" cy="418" r="2.4"/><circle cx="375" cy="378" r="2.4"/><circle cx="459" cy="338" r="2.4"/>
<circle cx="543" cy="298" r="2.4"/><circle cx="628" cy="256" r="2.4"/><circle cx="712" cy="216" r="2.4"/>
<circle cx="796" cy="178" r="2.4"/>
</g>
<!-- 行可行段 -->
<g stroke="#3572c9" stroke-width="5" opacity="0.75">
<line x1="228" y1="440" x2="360" y2="376"/>
<line x1="415" y1="352" x2="548" y2="288"/>
<line x1="602" y1="264" x2="735" y2="200"/>
</g>
<text x="238" y="486" font-size="12" fill="#2a4a6e">逐行可行段 rowSpans(探测步 hp,段端外扩 PAD=2°):</text>
<text x="238" y="504" font-size="12" fill="#2a4a6e">每行通常是单一连续段,段内仍按 h 精扫 —— 密度不降、成本大降</text>
<!-- 加密窗口 -->
<rect x="478" y="278" width="92" height="92" fill="none" stroke="#e08a1e" stroke-width="1.8" stroke-dasharray="6 4"/>
<text x="580" y="368" font-size="12" fill="#b06a10">加密窗口 ±2h(步长 h/8,</text>
<text x="580" y="386" font-size="12" fill="#b06a10">小步长档 h/4)罩住种子</text>
<!-- 模式搜索折线 -->
<polyline points="500,340 524,318 512,296 540,282 560,296 578,282" fill="none" stroke="#e08a1e" stroke-width="1.8" marker-end="url(#to)"/>
<text x="700" y="330" font-size="12" fill="#b06a10">8 邻域模式搜索:</text>
<text x="700" y="348" font-size="12" fill="#b06a10">步长减半,停在窄谷谷壁</text>
<!-- 牛顿收口点 -->
<g>
<path d="M 596 244 l 4.5 10.5 11 1 -8.5 7.5 2.5 11 -9.5 -6 -9.5 6 2.5 -11 -8.5 -7.5 11 -1 z" fill="#d64545"/>
<line x1="602" y1="254" x2="640" y2="286" stroke="#d64545" stroke-width="1" stroke-dasharray="3 3"/>
<text x="648" y="296" font-size="12.5" font-weight="bold" fill="#b03030">牛顿收口点</text>
<text x="648" y="314" font-size="12" fill="#b03030">轴残差 →1e-12(位置保持精确)</text>
</g>
<!-- 坐标轴 -->
<g stroke="#8a9aa8" stroke-width="1.4">
<line x1="110" y1="510" x2="850" y2="510" marker-start="url(#ta)"/>
<line x1="110" y1="510" x2="110" y2="80" marker-end="url(#ta)"/>
<line x1="130" y1="510" x2="130" y2="516"/><line x1="245" y1="510" x2="245" y2="516"/>
<line x1="411" y1="510" x2="411" y2="516"/><line x1="576" y1="510" x2="576" y2="516"/>
<line x1="741" y1="510" x2="741" y2="516"/><line x1="830" y1="510" x2="830" y2="516"/>
<line x1="104" y1="500" x2="110" y2="500"/><line x1="104" y1="396" x2="110" y2="396"/>
<line x1="104" y1="302" x2="110" y2="302"/><line x1="104" y1="207" x2="110" y2="207"/>
<line x1="104" y1="100" x2="110" y2="100"/>
</g>
<g font-size="11.5" fill="#5c7185">
<text x="126" y="530">−111°</text><text x="238" y="530">−90°</text><text x="404" y="530">−60°</text>
<text x="569" y="530">−30°</text><text x="734" y="530">0°</text><text x="812" y="530">+16°</text>
<text x="66" y="504">−93°</text><text x="66" y="400">−60°</text><text x="66" y="306">−30°</text>
<text x="76" y="211">0°</text><text x="80" y="104">+34°</text>
<text x="840" y="552" font-size="13" font-weight="bold" fill="#16324f">θ4(J4 肘部俯仰)</text>
<text x="66" y="70" font-size="13" font-weight="bold" fill="#16324f">θ5(J5 腕俯仰)</text>
</g>
<text x="490" y="582" text-anchor="middle" font-size="12.5" fill="#3d5164">每个格点的位置解由闭式支精确给出 —— 扫描不是在「逼近」目标,而是在冗余维里「挑构型」;</text>
<text x="490" y="602" text-anchor="middle" font-size="12.5" fill="#3d5164">姿态任务的达标带实测最窄 0.98°,2° 探测步会整条漏掉(误报不可达),故姿态任务探测步降到 0.5°。</text>
</svg>
<figcaption><b>图 9</b> | 冗余维 (θ4,θ5) 平面:可行带、逐行可行段、加密窗口与牛顿收口点。每个格点的位置解由闭式支精确给出——扫描不是在「逼近」目标,而是在冗余维里「挑构型」。</figcaption>
</figure>
<h3 id="s34"><span class="no">3.4</span>兜底:最近可达位姿</h3>
<p>目标不可达时不报错了事,而是返回最近可达位姿,并按任务语义分三种:</p>
<ul>
<li><code>closest()</code>:位置 DLS(阻尼最小二乘)——160 迭代 × 160 次 <b>LCG 确定性重启</b>(固定种子,同一目标逐位可复现,消灭"随机倒摆")+ anchor 回拉 0.03(压准退化构型下 J3 的无用漂移,实测 82°)+ <b>零空间抛光</b>(到目标距离不增的前提下把 Σ|θ| 滑到最小);</li>
<li><code>closest6()</code>:位置 + 姿态 6 维联合 DLS(姿态旋转矢量按 20 mm/rad 折算进误差向量);</li>
<li><code>closestOri()</code>:姿态优先——利用 J4∥J5 的降阶结构(工具轴 = Rx(θ1)·Ry(θ2)·Rz(−θ3)·Ry(s)·ẑ),(θ3, s, θ4) 三维粗扫 + 坐标下降,<b>姿态精确、位置取最近</b>。</li>
</ul>
<h3 id="s35"><span class="no">3.5</span>混合优先(位置/姿态滑杆)</h3>
<p>位置与姿态在远目标上不可兼得(实测同一目标:位置 0mm 时姿态差 44°,姿态 0° 时位置差 116mm)。<code>mix</code> 模式用两端锚点(位置优先解 / 姿态优先解)的误差各自归一,按 <code>J(q) = w·(e/e_ref)² + (1−w)·(o/o_ref)²</code> 加权折中,种子取两锚点 + 关节空间插值 + ref,6 维阻尼高斯-牛顿 + 单轴抛光;J 同量级带内取行程更小者防横跳。滑杆 0%/100% 直接走两条专用算法,结果逐位一致;拖动中走 mixWarm 快路,位移超限(疑换盆地)回落全量。</p>
<h3 id="s36"><span class="no">3.6</span>实时续解(warm 路径)</h3>
<p>拖拽时目标每帧只挪几毫米,全域粗扫(0.25° 下 6~30 万点,40~430ms)是 100ms 级延迟的唯一来源。<code>solveWarm</code> 只在上一帧解附近的 (θ4,θ5) 递进窗口(±4°→14°→40°)里走<b>同一闭式支</b>:位置残差 ≤1e-6、关节跳 ≤25°(防换支/倒摆)、姿态牛顿收口——任一校验不过就返回 null 交回全量。快路只负责"能快的时候快",松手后的全量求解才是最终结果。</p>
<h3 id="s37"><span class="no">3.7</span>工作空间剖面</h3>
<p>工作空间由纯 FK 采样刻画(<code>scratch/gen_workspace_svg.cjs</code>,与查看器「可达空间」同口径:travel.json + adj 机械区间内随机位形、J6=0、未计自碰撞)。J1 整周连续,可达集对 J1 轴旋转对称,所以 (x, r) 二维剖面(r = 离 J1 轴的距离)就是完整工作空间:rmax = 517mm 恰好是<b>零位臂竖直下垂的 TCP</b>,前伸边界 x≈+416mm。</p>
<figure><svg xmlns="http://www.w3.org/2000/svg" viewBox="0 0 980 560" font-family="'Segoe UI','PingFang SC','Microsoft YaHei',sans-serif">
<rect x="0" y="0" width="980" height="560" fill="#ffffff"/>
<text x="490" y="30" text-anchor="middle" font-size="21" font-weight="bold" fill="#16324f">运动学工作空间剖面((x, r) 平面,真实 FK 采样 60000 位形)</text>
<text x="490" y="54" text-anchor="middle" font-size="13" fill="#5c7185">r = 离 J1 轴的距离;J1 整周连续 ⇒ 可达集对该轴旋转对称,本剖面即完整工作空间(未计自碰撞与胶囊干涉)</text>
<rect x="110" y="75.60000000000002" width="651" height="414" fill="#f8fafc" stroke="#b9c8d6" stroke-width="1.2"/>
<line x1="124.8" y1="490" x2="124.8" y2="75.60000000000002" stroke="#e4ebf1" stroke-width="1"/><text x="124.8" y="506" text-anchor="middle" font-size="11" fill="#5c7185">-400</text>
<line x1="198.8" y1="490" x2="198.8" y2="75.60000000000002" stroke="#e4ebf1" stroke-width="1"/><text x="198.8" y="506" text-anchor="middle" font-size="11" fill="#5c7185">-300</text>
<line x1="272.8" y1="490" x2="272.8" y2="75.60000000000002" stroke="#e4ebf1" stroke-width="1"/><text x="272.8" y="506" text-anchor="middle" font-size="11" fill="#5c7185">-200</text>
<line x1="346.8" y1="490" x2="346.8" y2="75.60000000000002" stroke="#e4ebf1" stroke-width="1"/><text x="346.8" y="506" text-anchor="middle" font-size="11" fill="#5c7185">-100</text>
<line x1="420.8" y1="490" x2="420.8" y2="75.60000000000002" stroke="#e4ebf1" stroke-width="1"/><text x="420.8" y="506" text-anchor="middle" font-size="11" fill="#5c7185">0</text>
<line x1="494.8" y1="490" x2="494.8" y2="75.60000000000002" stroke="#e4ebf1" stroke-width="1"/><text x="494.8" y="506" text-anchor="middle" font-size="11" fill="#5c7185">100</text>
<line x1="568.8" y1="490" x2="568.8" y2="75.60000000000002" stroke="#e4ebf1" stroke-width="1"/><text x="568.8" y="506" text-anchor="middle" font-size="11" fill="#5c7185">200</text>
<line x1="642.8" y1="490" x2="642.8" y2="75.60000000000002" stroke="#e4ebf1" stroke-width="1"/><text x="642.8" y="506" text-anchor="middle" font-size="11" fill="#5c7185">300</text>
<line x1="716.8" y1="490" x2="716.8" y2="75.60000000000002" stroke="#e4ebf1" stroke-width="1"/><text x="716.8" y="506" text-anchor="middle" font-size="11" fill="#5c7185">400</text>
<line x1="110" y1="416" x2="761.2" y2="416" stroke="#e4ebf1" stroke-width="1"/><text x="102" y="420" text-anchor="end" font-size="11" fill="#5c7185">100</text>
<line x1="110" y1="342" x2="761.2" y2="342" stroke="#e4ebf1" stroke-width="1"/><text x="102" y="346" text-anchor="end" font-size="11" fill="#5c7185">200</text>
<line x1="110" y1="268" x2="761.2" y2="268" stroke="#e4ebf1" stroke-width="1"/><text x="102" y="272" text-anchor="end" font-size="11" fill="#5c7185">300</text>
<line x1="110" y1="194" x2="761.2" y2="194" stroke="#e4ebf1" stroke-width="1"/><text x="102" y="198" text-anchor="end" font-size="11" fill="#5c7185">400</text>
<line x1="110" y1="120" x2="761.2" y2="120" stroke="#e4ebf1" stroke-width="1"/><text x="102" y="124" text-anchor="end" font-size="11" fill="#5c7185">500</text>
<path d="M399 398h.1M299 261h.1M180 441h.1M375 119h.1M664 269h.1M384 349h.1M631 368h.1M470 406h.1M648 232h.1M206 392h.1M351 416h.1M443 346h.1M691 439h.1M681 346h.1M350 376h.1M599 240h.1M673 369h.1M396 115h.1M652 350h.1M532 214h.1M712 316h.1M321 236h.1M212 361h.1M298 297h.1M261 209h.1M559 230h.1M647 366h.1M442 108h.1M199 263h.1M292 343h.1M678 258h.1M245 435h.1M307 175h.1M529 183h.1M344 160h.1M604 347h.1M352 197h.1M182 407h.1M204 301h.1M641 304h.1M214 324h.1M212 448h.1M224 206h.1M419 297h.1M715 370h.1M546 176h.1M455 129h.1M182 274h.1M163 404h.1M349 485h.1M631 421h.1M370 212h.1M554 252h.1M264 238h.1M688 290h.1M532 165h.1M568 477h.1M221 285h.1M259 187h.1M429 385h.1M702 371h.1M453 143h.1M593 214h.1M169 402h.1M447 245h.1M458 158h.1M158 370h.1M292 243h.1M520 133h.1M288 341h.1M579 408h.1M163 339h.1M717 389h.1M364 250h.1M483 144h.1M433 154h.1M618 187h.1M230 201h.1M708 296h.1M269 331h.1M409 426h.1M611 430h.1M340 222h.1M692 436h.1M679 403h.1M197 424h.1M215 321h.1M206 458h.1M191 455h.1M242 254h.1M553 194h.1M229 232h.1M470 164h.1M403 303h.1M531 143h.1M296 202h.1M223 433h.1M291 258h.1M515 295h.1M613 324h.1M243 310h.1M356 171h.1M686 452h.1M375 129h.1M287 324h.1M441 358h.1M577 171h.1M592 368h.1M463 263h.1M223 265h.1M189 345h.1M549 213h.1M196 350h.1M172 376h.1M222 460h.1M698 334h.1M173 398h.1M525 170h.1M182 450h.1M637 418h.1M340 179h.1M274 376h.1M273 246h.1M401 240h.1M520 184h.1M317 186h.1M213 386h.1M603 339h.1M336 263h.1M416 204h.1M201 374h.1M376 215h.1M211 330h.1M521 127h.1M559 344h.1M619 217h.1M410 282h.1M583 201h.1M324 209h.1M229 335h.1M325 442h.1M582 261h.1M489 200h.1M577 468h.1M307 280h.1M431 387h.1M260 290h.1M185 433h.1M180 279h.1M535 340h.1M636 252h.1M456 322h.1M427 365h.1M468 318h.1M258 329h.1M681 294h.1M608 429h.1M377 182h.1M671 378h.1M552 146h.1M579 424h.1M452 166h.1M573 298h.1M522 145h.1M197 280h.1M655 383h.1M212 423h.1M437 293h.1M458 222h.1M397 155h.1M368 173h.1M697 333h.1M569 329h.1M225 429h.1M393 255h.1M417 169h.1M297 367h.1M553 155h.1M530 210h.1M553 166h.1M182 260h.1M533 256h.1M563 167h.1M479 126h.1M526 186h.1M433 169h.1M447 129h.1M571 277h.1M318 226h.1M566 208h.1M244 392h.1M578 445h.1M426 213h.1M382 277h.1M633 358h.1M349 403h.1M266 203h.1M568 188h.1M265 370h.1M577 172h.1M252 281h.1M654 367h.1M640 292h.1M333 360h.1M334 334h.1M593 223h.1M687 361h.1M648 338h.1M512 204h.1M388 152h.1M173 440h.1M605 227h.1M354 303h.1M291 379h.1M567 271h.1M494 265h.1M668 231h.1M457 365h.1M269 216h.1M507 242h.1M236 393h.1M188 257h.1M511 347h.1M712 344h.1M371 265h.1M613 222h.1M435 374h.1M347 233h.1M605 252h.1M587 226h.1M694 292h.1M233 452h.1M447 188h.1M597 232h.1M228 395h.1M482 349h.1M297 281h.1M445 433h.1M236 249h.1M416 463h.1M171 450h.1M282 323h.1M293 328h.1M455 203h.1M194 418h.1M646 257h.1M580 195h.1M367 292h.1M251 222h.1M525 282h.1M603 206h.1M338 140h.1M296 240h.1M429 371h.1M302 368h.1M214 291h.1M572 152h.1M273 278h.1M712 478h.1M524 129h.1M425 327h.1M433 307h.1M454 159h.1M570 326h.1M449 401h.1M359 246h.1M607 257h.1M507 242h.1M645 280h.1M453 172h.1M643 266h.1M459 204h.1M215 334h.1M345 342h.1M373 207h.1M288 220h.1M644 383h.1M164 456h.1M189 475h.1M440 249h.1M640 240h.1M166 407h.1M256 380h.1M231 343h.1M517 196h.1M568 266h.1M404 347h.1M288 361h.1M187 434h.1M480 161h.1M643 400h.1M416 486h.1M311 348h.1M357 340h.1M159 454h.1M417 424h.1M467 139h.1M342 291h.1M225 276h.1M201 483h.1M337 280h.1M316 201h.1M600 224h.1M311 168h.1M391 261h.1M279 403h.1M463 159h.1M318 228h.1M150 335h.1M673 335h.1M722 385h.1M675 347h.1M568 298h.1M231 399h.1M158 376h.1M438 341h.1M608 223h.1M313 148h.1M353 121h.1M193 411h.1M550 374h.1M489 230h.1M202 274h.1M332 273h.1M295 250h.1M304 264h.1M613 211h.1M291 432h.1M599 422h.1M537 266h.1M678 283h.1M215 444h.1M309 386h.1M338 318h.1M195 383h.1M214 388h.1M484 319h.1M681 437h.1M338 401h.1M152 382h.1M507 247h.1M161 398h.1M584 335h.1M502 166h.1M467 142h.1M433 207h.1M229 403h.1M592 169h.1M311 194h.1M378 163h.1M185 378h.1M255 459h.1M179 265h.1M268 403h.1M431 205h.1M268 208h.1M163 413h.1M357 129h.1M659 267h.1M433 193h.1M608 253h.1M538 232h.1M519 178h.1M503 154h.1M251 340h.1M309 165h.1M407 203h.1M405 437h.1M394 133h.1M154 379h.1M578 322h.1M36
<line x1="110" y1="490" x2="761.2" y2="490" stroke="#8a9aa8" stroke-width="1.6"/>
<text x="761.2" y="524" text-anchor="end" font-size="13" font-weight="bold" fill="#16324f">x:CAD 前后方向 (mm)</text>
<text x="64" y="282.8" font-size="13" font-weight="bold" fill="#16324f" transform="rotate(-90 64 282.8)">r:离 J1 轴距离 (mm)</text>
<line x1="435.23" y1="75.60000000000002" x2="435.23" y2="490" stroke="#b9a8d6" stroke-width="1" stroke-dasharray="5 4"/>
<circle cx="435.23" cy="401.2" r="3.2" fill="#7c5cd6"/><text x="444.23" y="405.2" font-size="11" fill="#5c3a90" stroke="#ffffff" stroke-width="3" paint-order="stroke">J2 r=120</text><circle cx="435.23" cy="367.9" r="3.2" fill="#7c5cd6"/><text x="444.23" y="371.9" font-size="11" fill="#5c3a90" stroke="#ffffff" stroke-width="3" paint-order="stroke">J3 r=165</text><circle cx="435.23" cy="284.28" r="3.2" fill="#7c5cd6"/><text x="444.23" y="288.28" font-size="11" fill="#5c3a90" stroke="#ffffff" stroke-width="3" paint-order="stroke">J4 r=278</text><circle cx="435.23" cy="202.88" r="3.2" fill="#7c5cd6"/><text x="444.23" y="206.88" font-size="11" fill="#5c3a90" stroke="#ffffff" stroke-width="3" paint-order="stroke">J5 r=388</text><circle cx="435.23" cy="168.84000000000003" r="3.2" fill="#7c5cd6"/><text x="444.23" y="172.84000000000003" font-size="11" fill="#5c3a90" stroke="#ffffff" stroke-width="3" paint-order="stroke">J6 r=434</text><circle cx="435.23" cy="107.42000000000002" r="3.2" fill="#7c5cd6"/><text x="444.23" y="111.42000000000002" font-size="11" fill="#5c3a90" stroke="#ffffff" stroke-width="3" paint-order="stroke">TCP r=517</text>
<circle cx="728.4054753823943" cy="107.5961182888991" r="4" fill="#d64545"/>
<text x="716.4054753823943" y="129.5961182888991" text-anchor="end" font-size="12" font-weight="bold" fill="#b03030" stroke="#ffffff" stroke-width="4" paint-order="stroke">rmax = 517(离 J1 轴最远,与预置自检一致)</text>
<text x="490" y="542" text-anchor="middle" font-size="12.5" fill="#3d5164">采样口径与查看器「可达空间」一致:限位内随机位形(travel.json + adj 机械区间),J6=0;边界 x ∈ [-377, 416]</text>
</svg>
<figcaption><b>图 10</b> | 工作空间 (x, r) 剖面:真实 FK 采样 60000 位形(确定性 LCG,可复现)。紫色点为零位各关节轴到 J1 轴的距离;采样结果与查看器预置自检(400000 位形、rmax=517、x −378…415)一致。</figcaption>
</figure>
<p>自检成绩(预置值,指纹校验):</p>
<table>
<tr><th>项目</th><th>样本</th><th>结果</th></tr>
<tr><td>FK 双实现对账</td><td>5000</td><td>最大差 5.7e-13 mm</td></tr>
<tr><td>位置 IK 重构</td><td>200/200 命中</td><td>最大残差 9.9e-13 mm</td></tr>
<tr><td>姿态 IK 重构</td><td>400 组</td><td>398 命中,最大残差 1.79°,平均 77.8 ms/次</td></tr>
<tr><td>运动学可达空间</td><td>400 000 位形</td><td>x −378…415 / y −388…642 / z 94…1125 mm</td></tr>
</table>
<div class="note">姿态 IK 的 2 组未达 0.05° 达标线,以近似解如实上报 <code>poseTier=approx</code> 并给出真实残差,不虚报精确。</div>
<figure><img src="shots/08_pose.png" alt="IK 求解姿态">
<figcaption><b>图 11</b> | 一次 IK 求解后的实机姿态:所有滑块、残差徽章与候选构型表均由上述流水线驱动。</figcaption>
</figure>
</section>
<!-- ================= 第四篇 ================= -->
<section id="ch4">
<h2><span class="no">第四篇</span>交互层:把求解器接进 30fps</h2>
<p class="lead">ik-glue.js 负责三件事:调度、拖拽语义、动画与对账。</p>
<p><b>调度</b>。全量求解(40~430ms)放进 Blob Worker 后台跑,主线程只收结果渲染。每个请求按 id 记账,"是否最新一帧"用第二参交给回调——被插队作废的请求也会被放行,保证实时追踪的门闩(liveBusy/livePending)不因一次竞态永久停摆。Worker 不可用时自动回落主线程同步求解,行为逐位一致。</p>
<p><b>拖拽语义</b>。黄球 = 移动目标(Shift 只上下);姿态控制开启后出现工具箭头:拖箭头头部 = 弧球式瞄准(指针射线与"以目标为球心、箭长为半径"的球求交,方向即工具轴指向),拖箭杆/捏环/粉色侧球 = 绕轴滚转(指针在垂直于工具轴的平面上的转角)。"实时追踪"模式下拖动即算即动,松手全量精修。</p>
<p><b>动画与对账</b>。解算结果经 <code>animTo</code> 动画写入:连续轴(J1/J3/J6)按 <code>wrap180</code> 走<b>短弧</b>(+151.9°→−167° 若按数值插值会反向扫 318.9°,走短弧只有 41.1°),末帧落回求解器给的代表角——与插值终点恰差 360° 整数倍,同一物理角、视觉零跳变;直接写入会取消在飞动画,实时解不会被入场动画覆写。每帧渲染后还用<b>场景图真实矩阵链</b>复算一次 TCP 位置与姿态偏差(<code>sceneOrientErr</code>),和求解器的闭式残差双通道对账——任何"求解器以为对了、场景画错了"的不同步都会立刻暴露在界面上。</p>
</section>
<!-- ================= 第五篇 ================= -->
<section id="ch5">
<h2><span class="no">第五篇</span>碰撞检测 I:离线权威扫描(sweep_v2.py)</h2>
<p class="lead">离线扫描回答的问题是:"每个关节到底能转到多少度?"判定必须精确——它直接对真实 CAD 实体做 OCCT 布尔求交,一切预筛都设计成保守证书:只能"确证分离"才允许跳过精确布尔。</p>
<figure><svg xmlns="http://www.w3.org/2000/svg" viewBox="0 0 980 750" font-family="'Segoe UI','PingFang SC','Microsoft YaHei',sans-serif">
<rect x="0" y="0" width="980" height="750" fill="#ffffff"/>
<defs>
<marker id="fa" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="6.5" markerHeight="6.5" orient="auto-start-reverse">
<path d="M0,0 L10,5 L0,10 z" fill="#5a7d9a"/>
</marker>
</defs>
<text x="490" y="34" text-anchor="middle" font-size="21" font-weight="bold" fill="#16324f">离线碰撞检测(sweep_v2.py):逐层过滤漏斗</text>
<text x="490" y="58" text-anchor="middle" font-size="13" fill="#5c7185">目标:把「每姿态都做 NURBS 布尔」压到只剩真正贴 face 的零件对 —— 精确性一分不减,调用量掉几个数量级</text>
<g font-size="12.5">
<rect x="120" y="76" width="600" height="46" rx="8" fill="#dbe9fb" stroke="#3572c9" stroke-width="1.4"/>
<circle cx="145" cy="99" r="12" fill="#3572c9"/><text x="145" y="104" text-anchor="middle" fill="#ffffff" font-weight="bold">1</text>
<text x="168" y="95" font-weight="bold" fill="#153a6e">全对全候选对</text>
<text x="168" y="113" fill="#2a4a6e">19 件 ≈ 171 对(9 打印件 + 6 电机 + 台架 P01/P02),按姿态刚体变换</text>
<line x1="420" y1="122" x2="420" y2="136" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#fa)"/>
<rect x="140" y="138" width="560" height="46" rx="8" fill="#e6eef7" stroke="#5a86b5" stroke-width="1.3"/>
<circle cx="165" cy="161" r="12" fill="#5a86b5"/><text x="165" y="166" text-anchor="middle" fill="#ffffff" font-weight="bold">2</text>
<text x="188" y="157" font-weight="bold" fill="#153a6e">跳过刚性安装对</text>
<text x="188" y="175" fill="#2a4a6e">MOUNT_PAIRS 12 对:电机 ↔ 被驱动件由螺钉锁死,无相对运动</text>
<line x1="420" y1="184" x2="420" y2="198" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#fa)"/>
<rect x="160" y="200" width="520" height="46" rx="8" fill="#e6eef7" stroke="#5a86b5" stroke-width="1.3"/>
<circle cx="185" cy="223" r="12" fill="#5a86b5"/><text x="185" y="228" text-anchor="middle" fill="#ffffff" font-weight="bold">3</text>
<text x="208" y="219" font-weight="bold" fill="#153a6e">组跨度规则</text>
<text x="208" y="237" fill="#2a4a6e">该姿态中没有活动关节跨越的对跳过(零位时全部必查——静止不代表不干涉)</text>
<line x1="420" y1="246" x2="420" y2="260" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#fa)"/>
<rect x="180" y="262" width="480" height="46" rx="8" fill="#e6eef7" stroke="#5a86b5" stroke-width="1.3"/>
<circle cx="205" cy="285" r="12" fill="#5a86b5"/><text x="205" y="290" text-anchor="middle" fill="#ffffff" font-weight="bold">4</text>
<text x="228" y="281" font-weight="bold" fill="#153a6e">轴区间预剪枝(单关节扫描专用,精确安全)</text>
<text x="228" y="299" fill="#2a4a6e">沿关节轴的轴向/径向区间永不相交的对,任何角度都不可能碰撞</text>
<line x1="420" y1="308" x2="420" y2="322" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#fa)"/>
<rect x="200" y="324" width="440" height="46" rx="8" fill="#e6eef7" stroke="#5a86b5" stroke-width="1.3"/>
<circle cx="225" cy="347" r="12" fill="#5a86b5"/><text x="225" y="352" text-anchor="middle" fill="#ffffff" font-weight="bold">5</text>
<text x="248" y="343" font-weight="bold" fill="#153a6e">AABB 分离</text>
<text x="248" y="361" fill="#2a4a6e">8 角点变换后取包围盒,0.1mm 余量;分离即跳过</text>
<line x1="420" y1="370" x2="420" y2="384" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#fa)"/>
<rect x="220" y="386" width="400" height="46" rx="8" fill="#e6eef7" stroke="#5a86b5" stroke-width="1.3"/>
<circle cx="245" cy="409" r="12" fill="#5a86b5"/><text x="245" y="414" text-anchor="middle" fill="#ffffff" font-weight="bold">6</text>
<text x="268" y="405" font-weight="bold" fill="#153a6e">包围球分离</text>
<text x="268" y="423" fill="#2a4a6e">球心随组变换、半径旋转不变;球分离 ⇒ 零件必分离</text>
<line x1="420" y1="432" x2="420" y2="446" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#fa)"/>
<rect x="230" y="444" width="400" height="66" rx="8" fill="#fdf2e2" stroke="#c99418" stroke-width="1.4"/>
<circle cx="255" cy="477" r="12" fill="#c99418"/><text x="255" y="482" text-anchor="middle" fill="#ffffff" font-weight="bold">7</text>
<text x="278" y="468" font-weight="bold" fill="#7a5a08">GJK 凸包预筛(保守证书)</text>
<text x="278" y="487" fill="#6b5312">0.5mm 凸包 ⊕ 0.8mm 球:确证分离才允许跳过</text>
<text x="278" y="500" fill="#6b5312" font-size="11">可能干涉/数值退化一律回落精确布尔——结果与纯布尔一致</text>
<line x1="420" y1="510" x2="420" y2="516" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#fa)"/>
<rect x="255" y="518" width="350" height="66" rx="8" fill="#fdf2e2" stroke="#c99418" stroke-width="1.4"/>
<circle cx="280" cy="551" r="12" fill="#c99418"/><text x="280" y="556" text-anchor="middle" fill="#ffffff" font-weight="bold">8</text>
<text x="303" y="542" font-weight="bold" fill="#7a5a08">顶点侵入实锤(反方向 shortcut)</text>
<text x="303" y="561" fill="#6b5312">一方网格顶点(≤240 采样)落入对方实体 &gt;1mm</text>
<text x="303" y="574" fill="#6b5312" font-size="11">⇒ 直接判干涉,免 4~12s 布尔;只证「撞」不证「自由」</text>
<line x1="420" y1="584" x2="420" y2="590" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#fa)"/>
<rect x="275" y="592" width="300" height="52" rx="8" fill="#fbe9e7" stroke="#c0533a" stroke-width="1.5"/>
<circle cx="300" cy="618" r="12" fill="#c0533a"/><text x="300" y="623" text-anchor="middle" fill="#ffffff" font-weight="bold">9</text>
<text x="323" y="612" font-weight="bold" fill="#7c2a18">OCCT 精确布尔 Common</text>
<text x="323" y="630" fill="#8a3a26">实体交体积 &gt; 2mm³ 记干涉(附干涉质心)</text>
<text x="323" y="640" fill="#8a3a26" font-size="11">SetRunParallel(True) · 多进程分发姿态</text>
</g>
<path d="M 745 480 C 800 480, 820 540, 820 590 C 820 620, 790 630, 566 618" fill="none" stroke="#c0533a" stroke-width="1.5" stroke-dasharray="6 4" marker-end="url(#fa)"/>
<text x="806" y="470" text-anchor="middle" font-size="12" fill="#7c2a18">布尔每对 0.5~12s</text>
<text x="806" y="488" text-anchor="middle" font-size="12" fill="#7c2a18">前面每层都在为它省调用</text>
<rect x="60" y="660" width="860" height="66" rx="8" fill="#f4f6f8" stroke="#9db0c0" stroke-width="1.2"/>
<text x="490" y="684" text-anchor="middle" font-size="12.5" fill="#3d5164">扫描策略:单关节双向粗扫 2°(小批量、首个卡死角即停)+ 二分逼近 0.2° → travel.json(滑块硬限位)</text>
<text x="490" y="704" text-anchor="middle" font-size="12.5" fill="#3d5164">组合姿态收敛 6°(一个关节保持极限、扫其余,两轮迭代)→ 只写 travel_detail.json,不压缩滑块 | workers = min(12, CPU−2, 内存/2.5GB)</text>
</svg>
<figcaption><b>图 12</b> | 离线碰撞检测的九层过滤漏斗:目标是把「每姿态都做 NURBS 布尔」压到只剩真正贴 face 的零件对——精确性一分不减,调用量掉几个数量级。</figcaption>
</figure>
<p>逐层拆解(每姿态的判定路径 <code>test_posture()</code>):</p>
<ol>
<li><b>全对全</b>:19 件(9 打印件 + 6 电机 + 台架 P01/P02)≈ 171 对。台架默认<b>参与</b>碰撞——臂撞立柱就是臂撞躯干,是真约束(<code>--no-stand</code> 仅用于诊断某个限位是臂自身折叠还是躯干挡的)。</li>
<li><b>跳过刚性安装对</b>:12 对电机↔被驱动件由螺钉锁死(<code>MOUNT_PAIRS</code>),无相对运动。</li>
<li><b>组跨度规则</b>:某姿态下没有活动关节跨越的零件对相对静止,跳过;但<b>零位(无活动关节)时必须全查</b>——相对位姿不变不代表不干涉。</li>
<li><b>轴区间预剪枝</b>(单关节扫描专用):按关节轴算每对零件的轴向/径向区间,永不相交的对在该关节任何角度都不可能碰撞——对单关节扫描是<b>精确安全</b>的。</li>
<li><b>AABB</b>:8 角点变换后取包围盒,0.1mm 余量。</li>
<li><b>包围球</b>:球心随组刚体变换、半径旋转不变,仍精确保守。</li>
<li><b>GJK 凸包预筛</b>:零件按 0.5mm 容差网格化取顶点云 → scipy 凸包精简(电机 15 万顶点 → 千余点),GJK 带 0.8mm 安全外扩(&gt; 网格内缩 0.5mm,保证"确证分离"判据保守)。"可能干涉/数值退化/迭代超限"一律回落精确布尔——<b>结果与纯布尔完全一致</b>,<code>--no-gjk</code> 可关闭做对比校验。</li>
<li><b>顶点侵入实锤</b>(反方向捷径):一方的网格顶点(≤240 采样点)落入对方实体内部 &gt;1mm 即实锤干涉,免掉 4~12s 的 NURBS 布尔。注意方向性:它能证"撞",不能证"自由"——无命中则照旧走布尔。</li>
<li><b>OCCT 精确布尔</b> <code>BRepAlgoAPI_Common</code>:交体积 &gt; 2mm³ 记干涉并附干涉质心;布尔失败记入 errors 不装作没看见。</li>
</ol>
<p><b>扫描策略</b>:</p>
<ul>
<li><b>单关节硬限位</b>:2° 粗扫(小批量并行、首个卡死角即停)+ 二分逼近到 0.2°,正反两向——travel.json 的来源。</li>
<li><b>组合姿态收敛</b>:一个关节保持在极限、扫其余关节(6° 步长),两轮迭代收敛出"组合折叠约束"。这些约束<b>只写 travel_detail.json、不压缩滑块</b>——否则手臂会被静态限位锁死;查看器改用动态模型按当前姿态实时处理(第六篇)。</li>
<li><b>并行</b>:mp.Pool 每个工作进程独立建模(杜绝 STEP/网格过期),workers = min(12, CPU−2, 可用内存/2.5GB)。</li>
</ul>
<p>当前权威数据(travel.json / travel_detail.json,2026-10-03 扫描;collide.json 胶囊拟合 2026-09-29):</p>
<table>
<tr><th>轴</th><th>单关节硬限位</th><th>滑块范围</th><th>备注</th></tr>
<tr><td>J1</td><td>−180…+180</td><td>±180 连续</td><td>整周无干涉</td></tr>
<tr><td>J2</td><td>−106.25…+10.38</td><td><b>−105…+9</b></td><td>含 1° 安全余量</td></tr>
<tr><td>J3</td><td>−180…+180</td><td>±180 连续</td><td>整周无干涉</td></tr>
<tr><td>J4</td><td>−112.62…+17.88</td><td><b>−111…+16</b></td><td></td></tr>
<tr><td>J5</td><td>−94.75…+35.75</td><td><b>−93…+34</b></td><td></td></tr>
<tr><td>J6</td><td>−180…+180</td><td>±180 连续</td><td>电机行程 ±360°</td></tr>
</table>
<p>组合折叠明细共 99 条(典型:工具法兰 P09 撞立柱 P01_A 限制了 J3/J4/J5 的组合极限)。</p>
<figure><img src="shots/05_exploded.png" alt="零件爆炸图">
<figcaption><b>图 13</b> | 全零件爆炸图:离线扫描的"全对全"就是在这些零件(13 打印件 + 6 电机 + 台架)之间两两做布尔。</figcaption>
</figure>
</section>
<!-- ================= 第六篇 ================= -->
<section id="ch6">
<h2><span class="no">第六篇</span>碰撞检测 II:实时胶囊模型与动态限位</h2>
<h3><span class="no">6.1</span>模型:CAD → 胶囊</h3>
<p>离线布尔每姿态秒级,放进 30fps 的交互里不可能。<code>scratch/fit_capsules.py</code> 把每件零件的 CAD 网格拟合为一组<b>胶囊体</b>(线段端点 a,b + 半径 r),写进 <code>docs/collide.json</code>:共 <b>1995 枚胶囊</b>,归属 7 个运动组(g0=台架固定 … g6=J6 组),端点是零位 CAD 世界坐标,组 g 的胶囊随 A1…Ag 嵌套旋转——与 FK 链同一累积变换,与场景图逐位一致。胶囊是外凸近似,判"干涉"略偏保守(含 margin=0.3mm 外扩),判"自由"需离线布尔背书;换来的是每次 <code>collideFree()</code> 只需 <b>~5ms</b>(零位最坏情形)。</p>
<figure><svg xmlns="http://www.w3.org/2000/svg" viewBox="0 0 980 640" font-family="'Segoe UI','PingFang SC','Microsoft YaHei',sans-serif">
<rect x="0" y="0" width="980" height="640" fill="#ffffff"/>
<defs>
<marker id="ca" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="6.5" markerHeight="6.5" orient="auto-start-reverse">
<path d="M0,0 L10,5 L0,10 z" fill="#5a7d9a"/>
</marker>
</defs>
<text x="490" y="34" text-anchor="middle" font-size="21" font-weight="bold" fill="#16324f">实时碰撞模型:CAD 网格 → 胶囊体 + 两级宽相位</text>
<!-- 左:胶囊化手臂示意 -->
<text x="235" y="64" text-anchor="middle" font-size="14.5" font-weight="bold" fill="#1f4f8f">手臂按运动组胶囊化(fit_capsules.py 拟合)</text>
<line x1="90" y1="590" x2="400" y2="590" stroke="#8a9aa8" stroke-width="2.5"/>
<rect x="150" y="110" width="34" height="478" rx="6" fill="#e2e8ee" stroke="#9db0c0" stroke-width="1"/>
<text x="167" y="102" text-anchor="middle" font-size="11" fill="#5c7185">台架 g0(root 固定)</text>
<rect x="196" y="128" width="52" height="86" rx="26" fill="#aecbeb" stroke="#3572c9" stroke-width="1.4"/>
<text x="256" y="150" font-size="11.5" fill="#153a6e">g1 · J1 组(肩吊臂)</text>
<rect x="196" y="226" width="52" height="70" rx="26" fill="#b7dcC8" stroke="#1f9d63" stroke-width="1.4"/>
<text x="256" y="248" font-size="11.5" fill="#14573a">g2 · J2 组(肩叉)</text>
<rect x="196" y="306" width="52" height="96" rx="26" fill="#a8d5d8" stroke="#1f7a8f" stroke-width="1.4"/>
<text x="256" y="336" font-size="11.5" fill="#14525e">g3 · J3 组(大臂骨架)</text>
<rect x="196" y="412" width="52" height="78" rx="26" fill="#c9d9a8" stroke="#6f8f2f" stroke-width="1.4"/>
<text x="256" y="436" font-size="11.5" fill="#4a5f1c">g4 · J4 组(小臂骨架)</text>
<rect x="196" y="498" width="52" height="42" rx="21" fill="#f2d5a8" stroke="#c98a18" stroke-width="1.4"/>
<text x="256" y="512" font-size="11.5" fill="#7a5308">g5 · J5 组(腕偏转座)</text>
<rect x="196" y="546" width="0" height="0" fill="none"/>
<rect x="198" y="548" width="48" height="20" rx="10" fill="#f0bcb0" stroke="#c0533a" stroke-width="1.4"/>
<text x="256" y="564" font-size="11.5" fill="#7c2a18">g6 · J6 组(工具法兰)</text>
<text x="235" y="608" text-anchor="middle" font-size="11.5" fill="#5c7185">胶囊 = 线段端点 a,b + 半径 r,端点是零位 CAD 世界坐标;</text>
<text x="235" y="626" text-anchor="middle" font-size="11.5" fill="#5c7185">组 g 的胶囊随 A1..Ag 嵌套旋转(与 FK 链同一累积变换)。</text>
<!-- 右上:宽相位级联 -->
<text x="700" y="64" text-anchor="middle" font-size="14.5" font-weight="bold" fill="#1f4f8f">两级宽相位 → 胶囊精确距</text>
<rect x="470" y="80" width="470" height="58" rx="8" fill="#e6eef7" stroke="#5a86b5" stroke-width="1.3"/>
<text x="486" y="102" font-size="12.5" font-weight="bold" fill="#153a6e">第 1 级 · 零件包围球(19 球)</text>
<text x="486" y="122" font-size="11.5" fill="#2a4a6e">球心距 &gt; rad_A+rad_B+rmax_A+rmax_B+margin 才筛除(阈值推导防漏判)</text>
<line x1="705" y1="138" x2="705" y2="152" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#ca)"/>
<rect x="470" y="154" width="470" height="58" rx="8" fill="#e6eef7" stroke="#5a86b5" stroke-width="1.3"/>
<text x="486" y="176" font-size="12.5" font-weight="bold" fill="#153a6e">第 2 级 · 簇球(每零件胶囊按主轴分桶,每簇 ≤4 枚)</text>
<text x="486" y="196" font-size="11.5" fill="#2a4a6e">主轴 = 胶囊中点的幂迭代方向(5 轮);再细分实测得不偿失</text>
<line x1="705" y1="212" x2="705" y2="226" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#ca)"/>
<rect x="470" y="228" width="470" height="58" rx="8" fill="#dbe9fb" stroke="#3572c9" stroke-width="1.4"/>
<text x="486" y="250" font-size="12.5" font-weight="bold" fill="#153a6e">第 3 级 · 胶囊对:线段-线段距离(Ericson 标准式)</text>
<text x="486" y="270" font-size="11.5" fill="#2a4a6e">d(轴_a, 轴_b) &lt; r_i + r_j + margin(0.3mm) ⇒ 干涉;无 visit 回调时首对即短路</text>
<!-- 胶囊距小图 -->
<g transform="translate(490,306)">
<rect x="0" y="18" width="120" height="34" rx="17" fill="#cfe3f5" stroke="#3572c9" stroke-width="1.4"/>
<line x1="17" y1="35" x2="103" y2="35" stroke="#3572c9" stroke-width="1.2" stroke-dasharray="4 3"/>
<rect x="210" y="0" width="110" height="30" rx="15" fill="#f2d5a8" stroke="#c98a18" stroke-width="1.4"/>
<line x1="225" y1="15" x2="305" y2="15" stroke="#c98a18" stroke-width="1.2" stroke-dasharray="4 3"/>
<line x1="110" y1="30" x2="212" y2="18" stroke="#c0533a" stroke-width="1.8" marker-start="url(#ca)" marker-end="url(#ca)"/>
<text x="128" y="12" font-size="12" font-weight="bold" fill="#c0533a">d(轴间距)</text>
<text x="120" y="86" font-size="12" fill="#16324f" font-family="Consolas,monospace">自由 ⇔ d ≥ r_i + r_j + margin</text>
<text x="120" y="106" font-size="11" fill="#5c7185">胶囊并集 ⊇ 零件本体 ⇒ 「自由」可信、「干涉」需离线布尔确认</text>
</g>
<!-- 右下:规则 -->
<rect x="470" y="428" width="470" height="128" rx="8" fill="#fdf6ec" stroke="#c99418" stroke-width="1.3"/>
<text x="486" y="452" font-size="12.5" font-weight="bold" fill="#7a5a08">三条关键规则</text>
<text x="486" y="474" font-size="12" fill="#6b5312">· 只检 |Δg|≥2 的零件对(同组相对静止、零位已保证不干涉);</text>
<text x="486" y="492" font-size="12" fill="#6b5312"> 相邻对 P04×P05/P06×P07/P07×P08 同心跨轴,塞进模型零位必误报</text>
<text x="486" y="510" font-size="12" fill="#6b5312"> ⇒ 改用 adj 一维自由区间表(CAD 实测、与姿态无关)</text>
<text x="486" y="528" font-size="12" fill="#6b5312">· adj:J2 [−107°,+128°] · J4 [−112°,+112°] · J5 [−96°,+96°]</text>
<text x="486" y="546" font-size="12" fill="#6b5312">· 硬边(lo 侧相邻件自碰撞)静态值即真值;可放宽侧(撞立柱)按姿态实时二分</text>
<rect x="470" y="566" width="470" height="58" rx="8" fill="#eef8f1" stroke="#1f6f43" stroke-width="1.3"/>
<text x="486" y="588" font-size="12.5" font-weight="bold" fill="#14472c">性能:1995 胶囊 · collideFree ≈ 5ms/次(零位最坏)</text>
<text x="486" y="608" font-size="11.5" fill="#2c5c44">预分配缓冲 + 代际戳缓存(免 GC)· 轴对齐特化旋转 · 热循环标量直传免建数组</text>
</svg>
<figcaption><b>图 14</b> | 实时胶囊模型与两级宽相位:只检 |Δg|≥2 的零件对,相邻打印件对改用 adj 一维自由区间表(CAD 实测,由 scratch/probe_adj.py 扫描生成;collide.json 缺 adj 时查看器显式告警并退回静态限位)。</figcaption>
</figure>
<h3><span class="no">6.2</span>宽相位与热路径</h3>
<ul>
<li><b>只检 |Δg|≥2</b>:同组相对静止(零位单实体检查已保证不干涉);相邻组的对(P04×P05 / P06×P07 / P07×P08)同心跨在同一关节轴上,胶囊轴距≈0,塞进模型会在零位误报(实测 P04×P03 超报 15mm)——改用 <b>adj 一维自由区间表</b>:J2 [−107°,+128°]、J4 [−112°,+112°]、J5 [−96°,+96°]。</li>
<li><b>两级宽相位</b>:零件包围球(19 球)→ 簇球(每零件的胶囊按主轴投影分桶、每簇 ≤4 枚,主轴由幂迭代 5 轮定向)→ 只有邻近簇才逐对变换胶囊端点做<b>线段-线段距离</b>(Ericson《Real-Time Collision Detection》标准式,手写标量直传版,免建数组)。宽相位阈值带 rmax+margin 是有推导的:早期版本漏了 rmax,把 P01_C×P06 在 J1=20°/J2=49° 的 1618mm³ 深撞整对筛掉。</li>
<li><b>性能工程</b>:预分配缓冲 + 代际戳缓存(一次判定 ~2500 个小数组的 GC 开销占 13% → 归零)、轴对齐特化 <code>rotAxis</code>(六个关节轴都是 ±X/±Y/±Z,Rodrigues 通用式退化为每轴 4 个非零元素)、簇球再细分实测得不偿失(62 万次球对测试 27ms/check)。</li>
</ul>
<h3><span class="no">6.3</span>动态限位:静态值只是零位快照</h3>
<p>travel.json 的单关节限位是"其它轴全零"时的一次性扫描,对"撞固定立柱"这类跨 ≥2 组的约束<b>系统性过紧</b>:实测 J2 正向静态 +9.75°(垂臂零位),J1=+90° 时真边界放到 +108.75°。查看器的滑块边界因此分三层:</p>
<figure><svg xmlns="http://www.w3.org/2000/svg" viewBox="0 0 980 560" font-family="'Segoe UI','PingFang SC','Microsoft YaHei',sans-serif">
<rect x="0" y="0" width="980" height="560" fill="#ffffff"/>
<defs>
<marker id="da" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="6.5" markerHeight="6.5" orient="auto-start-reverse">
<path d="M0,0 L10,5 L0,10 z" fill="#5a7d9a"/>
</marker>
<marker id="dor" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="6.5" markerHeight="6.5" orient="auto-start-reverse">
<path d="M0,0 L10,5 L0,10 z" fill="#e08a1e"/>
</marker>
</defs>
<text x="490" y="34" text-anchor="middle" font-size="21" font-weight="bold" fill="#16324f">动态限位:静态值只是零位快照,真边界由碰撞模型按姿态二分</text>
<!-- 数轴(J5 例) -->
<text x="490" y="70" text-anchor="middle" font-size="14" font-weight="bold" fill="#1f4f8f">以 J5 腕俯仰 + 向为例(数值取自实机数据)</text>
<line x1="120" y1="200" x2="880" y2="200" stroke="#3d5164" stroke-width="2.5" marker-start="url(#da)" marker-end="url(#da)"/>
<g font-size="11.5" fill="#5c7185" text-anchor="middle">
<text x="120" y="222">−360°</text><text x="500" y="222">0°</text><text x="880" y="222">+360°(电机限位)</text>
<line x1="500" y1="194" x2="500" y2="206" stroke="#3d5164" stroke-width="1.5"/>
<line x1="120" y1="194" x2="120" y2="206" stroke="#8a9aa8" stroke-width="1.5"/>
<line x1="880" y1="194" x2="880" y2="206" stroke="#8a9aa8" stroke-width="1.5"/>
</g>
<!-- adj 机械区间 -->
<rect x="399" y="188" width="202" height="24" rx="4" fill="#dbe9fb" opacity="0.85" stroke="#3572c9" stroke-width="1.2"/>
<text x="500" y="136" text-anchor="middle" font-size="12" fill="#2a4a6e">adj 机械区间(相邻件 CAD 实测):J5 = [−96°, +96°]</text>
<!-- 静态扫描限位 -->
<line x1="536" y1="180" x2="536" y2="220" stroke="#1f9d63" stroke-width="3"/>
<text x="536" y="250" text-anchor="middle" font-size="12" fill="#1f7a4d">零位静态扫描 +34°(撞立柱 P01_A)</text>
<!-- 动态边界 -->
<line x1="601" y1="180" x2="601" y2="220" stroke="#e08a1e" stroke-width="3"/>
<text x="672" y="272" text-anchor="middle" font-size="12" font-weight="bold" fill="#b06a10">当前姿态动态边界 +96°</text>
<!-- 当前角 -->
<path d="M 521 194 l 6 12 h -12 z" fill="#16324f"/>
<text x="489" y="172" text-anchor="end" font-size="12" font-weight="bold" fill="#16324f">当前 θ</text>
<!-- 二分括号 -->
<g stroke="#e08a1e" stroke-width="1.4" fill="none" stroke-dasharray="5 4">
<path d="M 521 216 C 521 230, 601 230, 601 224" marker-end="url(#dor)"/>
<path d="M 561 216 C 561 227, 581 227, 581 224" marker-end="url(#dor)"/>
</g>
<text x="490" y="292" text-anchor="middle" font-size="12" fill="#b06a10">dynLimit:从当前角向限位方向二分「collideFree」翻转点(tol 0.25°)</text>
<rect x="120" y="304" width="760" height="58" rx="8" fill="#f4f6f8" stroke="#9db0c0" stroke-width="1.2"/>
<text x="490" y="326" text-anchor="middle" font-size="12.5" fill="#3d5164">为什么静态限位不可信:它来自「其它轴全零」的一次性扫描。实测 J2 正向静态 +9.75°(垂臂零位撞立柱),</text>
<text x="490" y="346" text-anchor="middle" font-size="12.5" fill="#3d5164">J1=+90° 时动态边界放到 +108.75° —— 撞立柱这类「跨 ≥2 组」的约束必须按姿态重算,滑块才不锁死。</text>
<!-- solveDyn 流程 -->
<text x="490" y="394" text-anchor="middle" font-size="14.5" font-weight="bold" fill="#1f4f8f">solveDyn:静态解 → 碰撞校验 → 贴边轴放宽重解(最多 2 轮)</text>
<g font-size="11.5">
<rect x="70" y="408" width="150" height="50" rx="8" fill="#e6eef7" stroke="#5a86b5" stroke-width="1.3"/>
<text x="145" y="429" text-anchor="middle" font-weight="bold" fill="#153a6e">静态限位求解</text>
<text x="145" y="447" text-anchor="middle" fill="#2a4a6e">solveTo(全流水线)</text>
<line x1="220" y1="433" x2="240" y2="433" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#da)"/>
<rect x="242" y="408" width="150" height="50" rx="8" fill="#e6eef7" stroke="#5a86b5" stroke-width="1.3"/>
<text x="317" y="429" text-anchor="middle" font-weight="bold" fill="#153a6e">全姿态碰撞校验</text>
<text x="317" y="447" text-anchor="middle" fill="#2a4a6e">不安全→候选表选安全解</text>
<line x1="392" y1="433" x2="412" y2="433" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#da)"/>
<rect x="414" y="408" width="160" height="50" rx="8" fill="#e6eef7" stroke="#5a86b5" stroke-width="1.3"/>
<text x="494" y="429" text-anchor="middle" font-weight="bold" fill="#153a6e">贴边轴检测(0.5°内)</text>
<text x="494" y="447" text-anchor="middle" fill="#2a4a6e">非连续轴 · 非硬边</text>
<line x1="574" y1="433" x2="594" y2="433" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#da)"/>
<rect x="596" y="408" width="150" height="50" rx="8" fill="#fdf2e2" stroke="#c99418" stroke-width="1.3"/>
<text x="671" y="429" text-anchor="middle" font-weight="bold" fill="#7a5a08">dynLimit 二分边界</text>
<text x="671" y="447" text-anchor="middle" fill="#6b5312">放宽 ≥5° 才值得重解</text>
<line x1="746" y1="433" x2="766" y2="433" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#da)"/>
<rect x="768" y="408" width="150" height="50" rx="8" fill="#eef8f1" stroke="#1f6f43" stroke-width="1.3"/>
<text x="843" y="429" text-anchor="middle" font-weight="bold" fill="#14472c">放宽后重解(粗档)</text>
<text x="843" y="447" text-anchor="middle" fill="#2c5c44">cost 不升才采用</text>
</g>
<rect x="120" y="482" width="760" height="62" rx="8" fill="#fdf6ec" stroke="#c99418" stroke-width="1.3"/>
<text x="490" y="506" text-anchor="middle" font-size="12.5" fill="#6b5312">滑块夹取 clampAxis:缓存 [轴, 其余轴 1° 量化姿态] → {lo, hi};拖同一轴直接取缓存,</text>
<text x="490" y="526" text-anchor="middle" font-size="12.5" fill="#6b5312">拖过边界才重算一次二分(~40ms)。当前姿已干涉时退回机械区间 —— 否则 dynLimit 会把界外值当边界,滑块再也回不来。</text>
</svg>
<figcaption><b>图 15</b> | 动态限位:adj 机械区间 → dynLimit 二分(tol 0.25°)→ solveDyn 放宽重解;滑块夹取带边界缓存,当前姿已干涉时退回机械区间。</figcaption>
</figure>
<ol>
<li><b>机械区间</b> = adj 表(相邻件实测,必然无自碰撞);</li>
<li><b>动态边界</b> = <code>dynLimit()</code> 从当前角向限位方向<b>二分</b>"collideFree"翻转点(tol 0.25°),上限受电机行程 ±360° 与 adj 封顶;"硬边"(lo 侧相邻件自碰撞,配置无关)直接取静态值不放宽;</li>
<li><b>滑块夹取</b> <code>clampAxis()</code> 带边界缓存:<code>[轴, 其余轴 1° 量化姿态] → {lo,hi}</code>,拖同一轴直接取缓存,拖过边界才重算一次二分(~40ms);当前姿已干涉时退回机械区间——否则 dynLimit 会把界外值当边界,滑块再也回不来。</li>
</ol>
<p>求解侧对应的是 <b>solveDyn</b>:静态解先过全姿态碰撞校验(不安全→按代价升序在候选表里找 ≤1.5×cost+20° 的安全解);解贴在限位边界(0.5° 内)时对非连续、非硬边的轴二分动态边界,放宽 ≥5° 才值得重解(粗档步长 ×2,省 3~4 倍),新解 cost 不升才采用——最坏情况就是静态行为,无回退风险。这就是"动态限位放宽重解"徽章的来源。</p>
<figure><img src="shots/06_wrist.png" alt="腕部特写">
<figcaption><b>图 16</b> | 腕部特写:P08 腕偏转座 + P09 工具法兰 + J6 电机——P07×P08 这类相邻件对的对位精度由 adj 表与布尔验证共同背书。</figcaption>
</figure>
</section>
<!-- ================= 第七篇 ================= -->
<section id="ch7">
<h2><span class="no">第七篇</span>力矩与强度校核(简述)</h2>
<p class="lead">运动学之外,同一张关节表还驱动两份物理校核,保证"能转到位"之外还"转得动、扛得住"。</p>
<ul>
<li><b>力矩</b>(docs/torque_balance.json + torque_report.md):零件质量与质心全部取自 CAD(bom.json),在可达姿态网格上搜索每个关节的<b>最大重力力矩</b>与 Σ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%)——即<b>持臂静态工况超出热衰减后的连续额定</b>,这是后续减速比/选型迭代的输入。</li>
<li><b>强度</b>(docs/strength.json):对 15 个关键截面(吊臂根座、吊臂颊板、肩叉腹板等)按梁理论算弯曲应力与挠度,输出安全系数 SF(当前最小截面:吊臂根座 @z=480,SF≈9.1)。</li>
</ul>
</section>
<!-- ================= 第八篇 ================= -->
<section id="ch8">
<h2><span class="no">第八篇</span>自检体系与复现命令</h2>
<p class="lead">这套系统最重要的工程习惯是:每个算法都带数值自检,且自检值绑定代码指纹(FNV-1a over 限位表+运动学常量+求解器源码),改模型或改求解器后指纹失配、页面自动标红要求重算。</p>
<table>
<tr><th>自检</th><th>工具</th><th>通过线</th></tr>
<tr><td>FK 双实现对账</td><td>ArmIK.selfTest()</td><td>≤1e-9 mm(实测 5.7e-13)</td></tr>
<tr><td>位置 IK 重构</td><td>同上</td><td>逐支精确命中(实测 9.9e-13)</td></tr>
<tr><td>姿态 IK 重构</td><td>poseSelfTest()</td><td>oerr&lt;0.05° 且位置&lt;1e-3</td></tr>
<tr><td>线段距离解析式</td><td>暴力密集采样对比(scratch 自检脚本)</td><td>逐位一致</td></tr>
<tr><td>螺钉孔对位</td><td>scratch/fit_check.py 布尔干涉</td><td>0 干涉(轴线取样法会误报,弃用)</td></tr>
<tr><td>全行程零干涉</td><td>sweep_v2.py 全对全扫描</td><td>单关节推满 0 碰撞</td></tr>
</table>
<pre><code>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 变更后)</code></pre>
<h3><span class="no">附</span>文件索引</h3>
<table>
<tr><th style="width:330px">文件</th><th>角色</th></tr>
<tr><td>src/human_layout.py</td><td>关节轴/拓扑单一事实源</td></tr>
<tr><td>src/arm_model.py · arm_build.py</td><td>参数化 CAD 与导出</td></tr>
<tr><td>src/sweep_v2.py</td><td>离线碰撞与行程权威工具</td></tr>
<tr><td>src/shot.py</td><td>渲染出图(docs/shots/)</td></tr>
<tr><td>src/finalize_travel.py</td><td>从 sweep 明细重建 travel.json 的维护工具</td></tr>
<tr><td>web/arm-ik-solver.js</td><td>六轴 IK 求解器(纯 JS,浏览器/Node 双端)</td></tr>
<tr><td>web/ik-glue.js</td><td>交互层:拖拽/Worker/动态限位/动画</td></tr>
<tr><td>scratch/fit_capsules.py</td><td>CAD 网格 → 胶囊模型拟合(collide.json)</td></tr>
<tr><td>scratch/probe_adj.py</td><td>相邻打印件 1D 自由区间实测(adj 表)</td></tr>
<tr><td>scratch/compute_precomputed.cjs</td><td>重算预置自检值与代码指纹</td></tr>
<tr><td>scratch/gen_workspace_svg.cjs</td><td>工作空间剖面图生成(本文图 10)</td></tr>
<tr><td>docs/travel.json · travel_detail.json</td><td>滑块硬限位 / 组合折叠明细</td></tr>
<tr><td>docs/collide.json</td><td>1995 胶囊 + adj 机械区间 + 硬边表</td></tr>
<tr><td>docs/algo_svg/</td><td>本文全部算法示意图源文件</td></tr>
</table>
<footer>
机器人手臂 · 运动学模拟与碰撞算法详解 | 生成于 2026-10-03 | 文字与 docs/运动学与碰撞算法详解.md 同步,算法图源文件在 docs/algo_svg/(改后重跑 scratch/build_algo_doc_html.py 刷新本页)。
本页无任何外部依赖(无 CDN/外链脚本),实机渲染图引用 docs/shots/ 相对路径——连同 docs/ 目录一起拷贝即可完整显示。
</footer>
</section>
</main>
</div>
<script>
/* 目录滚动高亮(IntersectionObserver,无依赖) */
(function(){
var links = Array.prototype.slice.call(document.querySelectorAll('nav.toc a[data-spy]'));
var map = {};
links.forEach(function(a){ map[a.getAttribute('href').slice(1)] = a; });
var obs = new IntersectionObserver(function(es){
es.forEach(function(e){
if (e.isIntersecting) {
links.forEach(function(a){ a.classList.remove('active'); });
var a = map[e.target.id]; if (a) a.classList.add('active');
}
});
}, {rootMargin:'-20% 0px -70% 0px'});
Object.keys(map).forEach(function(id){
var el = document.getElementById(id); if (el) obs.observe(el);
});
})();
</script>
</body>
</html>