81 lines
6.6 KiB
XML
81 lines
6.6 KiB
XML
|
<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|<1e-7、det<1e-12、m2 越界 → 该支无解，直接落选</text>
|
|||
|
|
|
|||
|
|
<text x="490" y="560" text-anchor="middle" font-size="12.5" fill="#3d5164">示意省略了 X0/Y0 基准平移与 tool 常量；四支解的位置全部精确命中目标 ——「扫描代替数值搜索」的根基。</text>
|
|||
|
|
</svg>
|