104 lines
9.1 KiB
XML
104 lines
9.1 KiB
XML
<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>
|