Scara清洁机械臂的详细设计及分析

整机结构（侧视图 + 俯视图）：

侧视图： 俯视图：
 Z(↑) Y(↑)
 │ │
 │ J5(↘) ── L6 ── [末端] │
 │ ↗ │
 │ J6(Ry) ← J5(Ry) ← J4(Rz) │
 │ 俯仰链 │
 │ ↗ │
 │ J4(Rz) ── J3 ── [工具旋转] │
 │ ↗ │
 │ J3(Pz) ← J2(Rz) ← J1(Rz) │
 │ ↗ │
 │ J2(Rz) │
 │ ↗ │
 └─── J1(Rz) └──── X(→)
 底座

J5/J6 俯仰链的运动逻辑：
─────────────────────────────────────────────────────
当末端需伸入内壁底部（目标在基座前方+下方）时：
├─ J5：向下俯仰（θ₅ < 0），让末端向内弯折
├─ J6：继续向下俯仰（θ₆ < 0），进一步弯折
└─ θ₅ + θ₆ 的总角度决定末端朝向

举例：J5 = -45°，J6 = -45° → 总俯仰 -90°（末端竖直向下）
举例：J5 = -60°，J6 = -30° → 总俯仰 -90°（末端竖直向下）

两种分配方式效果：
├─ J5大+J6小：力矩由J5承担多，J6轻载
└─ J5小+J6大：J5轻载，J6力矩大（末端力矩传递）
→ 推荐：J5承担主要俯仰角度（60%），J6辅助（40%）

末端位置 (X, Y, Z) 推导：

水平分量（SCARA定位链贡献）：
R_horizontal = L1·cos(θ₁) + L2·cos(θ₁+θ₂) + L_base·方向修正

X = [L1·cos(θ₁) + L2·cos(θ₁+θ₂)] · cos(θ₁)
 - L_base·sin(θ₁方向) [J2轴心的水平偏移]

Y = [L1·cos(θ₁) + L2·cos(θ₁+θ₂)] · sin(θ₁)
 + L_base·cos(θ₁方向) [J2轴心的水平偏移]

垂直分量（Z轴升降 + 俯仰链贡献）：
Z = h_base + d₃_var - L_wrist
 - L5·sin(|θ₅|) - L6·sin(|θ₅+θ₆|) [俯仰链末端Z坐标]

俯仰链水平伸出量：
R_pitch = L5·cos(θ₅) + L6·cos(θ₅+θ₆)

末端总伸出半径：
R_total = R_horizontal + R_pitch·cos(φ_wrist) [φ_wrist=J4朝向]

末端朝向（绕Z轴，工具滚动）：
ψ = θ₁ + θ₂ + θ₄ [三个旋转关节的累积角度]

末端俯仰角（绕Y轴）：
φ_pitch = θ₅ + θ₆ [两个俯仰关节的总角度]

正运动学本质：
├─ 输入：6个关节值 (θ₁, θ₂, d₃, θ₄, θ₅, θ₆)
├─ 输出：末端位置(X,Y,Z) + 朝向(ψ, φ_pitch)
└─ 计算：均为解析公式，无迭代，瞬时求解

逆运动学求解（给定末端目标位置+朝向，求6个关节值）：

Step 1：SCARA平面定位（J1, J2, J3）
─────────────────────────────────────────────────────
已知：末端目标 (X, Y, Z)，Z轴基准 h_base

Step 1.1 计算J3升降量：
d₃_var = Z - h_base + L_wrist + L5·sin(|θ₅|) + L6·sin(|θ₅+θ₆|)
→ 先忽略俯仰链高度贡献（第一次估计）
→ d₃_var_estimate = Z - h_base + L_wrist

Step 1.2 计算水平定位半径：
R_target = sqrt(X² + Y²) - 俯仰链水平伸出量（迭代求解）

Step 1.3 SCARA逆运动学（几何解）：
┌─────────────────────────────────────┐
│ Y │
│ ↑ │
│ │ L2 │
│ │ θ₁←──●───←θ₂ │
│ │ ↙ ╲___╱ │
│ │ L1 ↘ R_target（目标） │
│ │ │
│ └────────────────────────────────→ X│
└─────────────────────────────────────┘

├─ r = sqrt(X² + Y²) [目标到原点的距离]
├─ φ = atan2(Y, X) [目标方向角]
│
├─ cos(θ₂) = (r² - L1² - L2²) / (2·L1·L2)
│ → θ₂ = ±arccos(...) [肘上/肘下两种构型]
│
└─ θ₁ = φ - atan2(L2·sin(θ₂), L1 + L2·cos(θ₂))

此即完整的SCARA逆运动学解析解——初中几何水平！
代码量：约30行Python

Step 2：手腕Z轴旋转（J4）
─────────────────────────────────────────────────────
给定：工具期望朝向角 ψ_desired

θ₄ = ψ_desired - θ₁ - θ₂
→ J4的角度 = 工具朝向 - J1角度 - J2角度

即：三个Z轴旋转关节的累积角度决定工具绕Z轴朝向

Step 3：俯仰链求解（J5, J6）
─────────────────────────────────────────────────────
已知：
├─ 俯仰链末端的目标俯仰角 φ_target（向下为负）
├─ 俯仰链末端水平伸出量 R_pitch（需要达到内壁目标点）
└─ 连杆长度：L5 = 150mm, L6 = 60mm

求解策略（几何法）：
├─ 总俯仰角：φ_total = θ₅ + θ₆ ≈ φ_target
│
├─ 角度分配：
│ └─ θ₅ ≈ φ_total × 0.65（J5主要弯折）
│ └─ θ₆ ≈ φ_total × 0.35（J6辅助弯折）
│
└─ 验证水平伸出量：
 R_pitch = L5·cos(θ₅) + L6·cos(θ₅+θ₆)
 → 调整θ₅/θ₆比例使R_pitch达到目标

此求解也可通过解析几何直接计算（两自由度串联链）

减速机选型三原则：
─────────────────────────────────────────────────────
原则1：负载决定扭矩需求
 → 负载越大 → 需要减速比越高 → 减速机规格越大

原则2：精度决定减速机类型
 → 定位精度要求高 → 选谐波（零背隙）
 → 姿态调整精度要求低 → 选行星（成本低）

原则3：成本按需配置
 → 不必每个关节都用高精度减速机
 → 精度"够用"即可，过度配置是浪费

J1底座旋转减速机选型：
─────────────────────────────────────────────────────
受力分析（最关键关节）：
├─ 承载重量：整臂重量 ≈ 8kg × 9.8 ≈ 78N（重力方向）
├─ 偏心距：J2距J1轴心 L_base = 100mm
│ └─ 静态弯矩 = 78N × 0.1m = 7.8 N·m
├─ 动态惯性力矩：加速/减速时 ≈ 5–10 N·m
├─ 安全系数：2.0
└─ 综合需求：额定输出扭矩 ≥ 20 N·m

精度要求：
├─ J1底座旋转直接决定末端X-Y平面坐标
├─ 目标末端定位精度：±1mm
├─ J1关节角精度需求：约 ±0.1°
├─ 背隙要求：< 1 arcmin（J1背隙会直接放大为末端误差）
└─ 结论：必须用高精度减速机（谐波）

推荐选型：谐波减速器 + 无刷电机 一体化关节
├─ 减速比：1:30（平衡力矩和速度）
│ └─ 电机额定扭矩 0.5 N·m → 减速后输出 15 N·m
│ └─ 电机峰值扭矩 1.5 N·m → 减速后输出 45 N·m（满足20N·m需求）
├─ 精度：±15 arcsec（0.004°）→ 远优于 ±0.1° 需求
├─ 防护等级：IP65
├─ 成本：谐波减速器约 1500–3000元
└─ 推荐型号：来福 LF-17-30 或 绿的 LHS-17

为什么用谐波而不是行星？
├─ J1是整臂最关键的定位关节
├─ 背隙误差会直接传递到末端位置
├─ 谐波减速器天然零背隙，无需算法补偿
└─ 虽贵但可靠，是J1的最佳选择

J2肩旋转减速机选型：
─────────────────────────────────────────────────────
受力分析：
├─ 承载重量：去除J1后剩余重量 ≈ 7kg
├─ 偏心距：J3升降轴心距J2轴心 L2 = 300mm（连杆长度）
│ └─ 静态弯矩 = 70N × 0.3m = 21 N·m ⚠️ 很大！
├─ 但注意：J2和J1共同承载，实际偏心距需向量合成
│ └─ 实际J2承受的力矩 ≈ 10–15 N·m（需精确计算）
├─ 安全系数：2.0
└─ 综合需求：额定输出扭矩 ≥ 15 N·m

精度要求：
├─ J2与J1联动决定末端X-Y坐标
├─ 精度需求同J1：±0.1°关节精度
├─ 背隙要求：< 1 arcmin
└─ 结论：同样需要高精度减速机（谐波）

推荐选型：谐波减速器 + 无刷电机 一体化关节
├─ 减速比：1:20 或 1:30
├─ 精度：±15 arcsec（零背隙）
├─ 成本：谐波减速器约 1200–2500元
└─ 推荐型号：来福 LF-14-30 或 绿的 LHS-14

J1 vs J2 减速机对比：
├─ J1：需要承受整臂旋转力矩 → 选更大型号（LF-17）
├─ J2：力矩略小 → 可选稍小型号（LF-14）
└─ 成本差约 500–800元/J1

J3 Z轴升降关节选型：
─────────────────────────────────────────────────────
运动方式：直线运动（Pz），不是旋转

推力需求计算：
├─ 负载重量：整臂 ≈ 8kg → 重力 = 78N
├─ 运动加速度：0.2m/s² → 惯性力 = 1.6N
├─ 摩擦力（导轨）：约 5–10N
├─ 安全系数：2.0
└─ 所需推力 = (78 + 10) × 2 ≈ 180N

行程需求：
├─ 座圈高度：约 400mm
├─ 内壁最深处：约 100mm（低于地面）
├─ 安全余量：100mm
└─ 所需行程：0–500mm

定位精度：
├─ Z轴决定末端高度，精度需求 ±1mm
├─ 滚珠丝杠精度：±0.05mm/300mm（标准级）
└─ 完全满足需求

推荐选型：一体化直线关节（旋转电机 + 滚珠丝杠）
├─ 类型：BLDC电机 + 滚珠丝杠 + 内置驱动
├─ 额定推力：≥ 200N（有充分裕量）
├─ 行程：500mm（标准规格可选）
├─ 定位精度：±0.1mm（丝杠精度）
├─ 自锁：滚珠丝杠不自锁，需制动器保持位置
│ └─ 解决：末端增加抱闸，或重力方向恒定无需抱闸
├─ 防护等级：IP65
├─ 成本：一体化直线关节约 2000–4000元
└─ 推荐型号：步科FD2L-S / 雷赛LMS系列

为什么不用其他方案？
├─ 气缸：精度差，无法闭环控制 ❌
├─ 电动推杆：精度和刚性不足 ❌
└─ 丝杠直驱一体化关节：精度高+刚性好+体积小 ✅

J4手腕Z轴旋转减速机选型：
─────────────────────────────────────────────────────
J4的作用：调整工具绕Z轴（自身轴线）的朝向
├─ 即工具的"滚动"角度
├─ 不影响末端位置（纯姿态调整）
└─ 不承受外部负载

受力分析：
├─ 末端负载：末端执行器 ≈ 500g–1kg
├─ 偏心距：L4 ≈ 50mm（J4轴心到末端质心）
│ └─ 静态扭矩 = 10N × 0.05m = 0.5 N·m
├─ 清洁时的反作用力：刷头接触时约 5–10N
│ └─ 反作用扭矩 = 10N × 0.05m = 0.5 N·m
├─ 安全系数：1.5
└─ 综合需求：额定输出扭矩 ≥ 1.5 N·m

精度要求：
├─ J4只影响工具朝向，不影响位置精度
├─ 工具朝向误差 ±5° 内均可接受
├─ 对应J4关节角误差 ±5°
└─ 结论：精度要求低，可用普通减速机

推荐选型：行星减速器一体化关节（中低规格）
├─ 减速比：1:10 或 1:15
├─ 额定扭矩：≥ 3 N·m（有裕量）
├─ 背隙：< 10 arcmin（可接受，姿态容差大）
├─ 响应速度：要求较高（J4调整频繁）
├─ 防护等级：IP65
├─ 成本：一体化关节约 800–1500元
└─ 推荐型号：步科FD1L / 雷赛LMC306

为什么不用谐波（J4 vs J1/J2）？
├─ J4精度要求比J1/J2低一个数量级
├─ J4不影响末端位置，只影响姿态
├─ 姿态误差±5°完全可接受
├─ 行星减速器成本仅为谐波的 1/3–1/5
└─ 用谐波在J4上是严重的过度配置

J5俯仰第一节减速机选型：
─────────────────────────────────────────────────────
J5的作用：俯仰链第一节，承受L6+末端工具的重量
├─ 是俯仰链的主要力矩承担者
└─ 决定俯仰链的基础角度

受力分析：
├─ 末端负载 = L6(60mm) + 末端执行器(500g) ≈ 600g
├─ 偏心距 = L6 = 60mm
│ └─ 静态扭矩 = 6N × 0.06m = 0.36 N·m
├─ 清洁反作用力：约 10N × 0.06m = 0.6 N·m
├─ J6传递的扭矩：J6的负载 × L6 = 约 0.3 N·m（需合成）
├─ 安全系数：2.0
└─ 综合需求：额定输出扭矩 ≥ 2 N·m

精度要求：
├─ J5+J6共同决定末端俯仰角
├─ 允许末端俯仰误差 ±3°
├─ J5关节角误差 ±3° 内可接受
├─ 对应精度要求：中等
└─ 结论：行星减速器足够，无需谐波

推荐选型：行星减速器一体化关节
├─ 减速比：1:10（平衡力矩和速度）
├─ 额定扭矩：≥ 5 N·m（有裕量）
├─ 背隙：< 8 arcmin（姿态容差内）
├─ 防护等级：IP65
├─ 成本：一体化关节约 600–1200元
└─ 推荐型号：步科FD1.5L / 雷赛LMC308

为什么J5/J6的减速机可以比J1/J2小很多？
├─ J1/J2承受整臂重量（8kg级）
├─ J5/J6只承受俯仰链末端重量（0.6kg级）
├─ 力矩需求差 10–15 倍
└─ 相应地，减速机规格也差一个数量级

J6俯仰末节减速机选型：
─────────────────────────────────────────────────────
J6的作用：俯仰链末节，直接安装末端执行器
├─ 承受末端执行器重量（最轻的负载）
├─ 调整末端俯仰角（最终姿态）
└─ 力矩最小

受力分析：
├─ 末端执行器重量 ≈ 500g = 5N
├─ 偏心距：末端质心距J6轴心 ≈ 30mm
│ └─ 静态扭矩 = 5N × 0.03m = 0.15 N·m
├─ 清洁反作用力：约 10N × 0.03m = 0.3 N·m
├─ 安全系数：1.5
└─ 综合需求：额定输出扭矩 ≥ 0.5 N·m

精度要求：
├─ J6是末端执行器的直接安装关节
├─ 但末端执行器通过弹簧机构有被动顺应
├─ J6角度误差 ±5° 内完全可接受
├─ 结论：精度要求最低，可用微型减速机

推荐选型A（推荐）：微型行星减速一体化关节
├─ 减速比：1:5 或 1:10
├─ 额定扭矩：≥ 1 N·m
├─ 体积：直径 ≤ 40mm（轻量化）
├─ 防护等级：IP65
├─ 成本：一体化关节约 400–800元
└─ 推荐型号：雷赛LMC204 / 步科FD06

推荐选型B（备选）：空心杯电机 + 微型减速器
├─ 空心杯电机：直径20–30mm，扭矩 0.05–0.1 N·m
├─ 微型行星减速：减速比1:5 → 输出 0.25–0.5 N·m
├─ 成本：更低（约 200–500元）
└─ 适合：成本极度敏感 + 精度要求极低的场景

为什么J6是6个关节中减速机规格最小的？
├─ J6是最后一个关节，力矩臂最短
├─ 只承受末端执行器重量（0.5kg级）
├─ 负载仅为J1的 1/20–1/30
└─ 相应地，减速机成本也最低

减速机成本分布（饼图说明）：
─────────────────────────────────────────────────────
J1底座：2000–3500元 ████████████████ 约35%
J2肩： 1500–2500元 █████████████ 约27%
J3升降：2000–4000元 ██████████████████ 约25%
J4手腕： 800–1500元 █████ 约7%
J5俯仰1：600–1200元 ████ 约5%
J6俯仰2： 400–800元 ██ 约1%

减速机总计：约 7300–13500元

成本分布规律：
├─ SCARA底座（J1+J2+J3）：占减速机总成本 87%
│ └─ 谐波×2 + 直线丝杠×1 = 高精度高成本
├─ 俯仰腕部（J4+J5+J6）：占减速机总成本 13%
│ └─ 行星×3 = 低精度低成本
└─ 结论：按需配置，不过度设计，成本合理分配

连杆长度设计（满足清洁工作空间）：
─────────────────────────────────────────────────────
工作空间需求分析：
├─ 内壁最远点：约550mm（从基座中心算）
├─ 最深处（内壁底部）：低于地面约100mm
├─ 座圈高度：约400mm（地面到座圈面）
└─ 俯仰链长度：L5 + L6 = 150 + 60 = 210mm

SCARA连杆设计：
├─ L1（第1连杆）：280mm
│ └─ J2轴心到J3轴心的水平投影距离
├─ L2（第2连杆）：250mm
│ └─ J3轴心到J4轴心的水平投影距离
└─ L_base（肩偏移距）：100mm

俯仰链设计：
├─ L5（第1俯仰连杆）：150mm
│ └─ J5轴心到J6轴心的长度
├─ L6（第2俯仰连杆）：60mm
│ └─ J6轴心到末端执行器安装面的长度

工作空间验证：
├─ 最大水平伸出 = L1 + L2 + L5 = 280 + 250 + 150 = 680mm ✅
│ └─ 大于最远点550mm，有130mm裕量
│
├─ 最大下降深度（Z轴）：
│ └─ J3下降500mm - L5·sin(90°) - L6·sin(90°)
│ └─ = 500 - 150 - 60 = 290mm（从基座高度算）
│ └─ 超过内壁底部（地面下100mm）✅
│
└─ 最小工作半径 = |L1 - L2| + L5 + L6 = 30 + 150 + 60 = 240mm
 └─ 需通过J3的Z轴升降补偿小范围近距离作业

工作空间（侧视图）：
─────────────────────────────────────────────────────
 Z轴 (mm)
 ↑
 500 │ ─ ─ ─ J3最大上升 ─ ─ ─ ─ ─ ─ ─ ─ ─ ─ ─
 │ ╲
 400 │ ╲ ← 座圈高度
 │ ╲
 290 │ ╲ ← 俯仰链末端最深点
 │ ╲
 150 │ ╲ ← J5/J6折叠状态
 │ ╲
 0 │ ╲
 └────────────────────────────────────────── X轴
 0 200 400 600

 ● = 基座中心（J1轴心）

 绿色区域 = 清洁可达范围
 (径向100–550mm，高度0–450mm)

俯视图（X-Y平面）：
─────────────────────────────────────────────────────
 Y (mm)
 │
 680 │ ╭───────────────────╮
 │ ╱ ╲
 500 │ ╱ 工作空间 ╲
 │ │ │
 340 │ │ ● ← 基座 │
 │ │ │
 240 │ │ （最小半径约束） │
 │ ╲ ╱
 0 │ ╰────────────────────╯
 └────────────────────────────────── X (mm)
 0 200 400 600 800

J5/J6 俯仰链的三种典型运动模式：
─────────────────────────────────────────────────────
模式A：伸入内壁（最关键）
├─ J5：θ₅ < 0（向下俯身）
├─ J6：θ₆ < 0（继续向下弯折）
├─ 末端：超过基座平面，向内部延伸
└─ 应用：擦拭内壁下半部分

模式B：上方作业
├─ J5：θ₅ ≈ 0（水平或微俯）
├─ J6：θ₆ ≈ 0（末端水平指向前方）
├─ 末端：在正上方，俯视作业
└─ 应用：消毒液喷洒、座圈上表面擦拭

模式C：侧面作业
├─ J5：θ₅ > 0（向后仰起避开障碍）
├─ J6：θ₆ 调整（精细定位）
├─ 末端：偏转避开边缘
└─ 应用：外壁侧面擦拭

角度和约束：
├─ θ₅ 范围：-90° ~ +90°
├─ θ₆ 范围：-90° ~ +90°
├─ 推荐分配：θ₅ 承担 60%，θ₆ 承担 40%
└─ 总俯仰角：θ₅ + θ₆ ∈ [-135°, +135°]

俯仰链的重力补偿策略（算法简化关键）：
─────────────────────────────────────────────────────
问题：J5/J6俯仰时，重力导致末端位置偏移

传统方案（主动重力补偿）：
├─ 需要精确的动力学模型
├─ 需要实时计算重力向量
├─ 控制算法复杂
└─ 对算法能力要求高 ❌

本方案（机械+算法简化）：
├─ 末端执行器安装弹性弹簧机构
│ └─ 刷头通过弹簧连接于J6末端
│ └─ 弹簧可吸收 ±10mm 的位置误差
│
├─ 视觉反馈校正（可选）
│ └─ 末端RGB-D相机实时检测末端相对的位置
│ └─ 每隔3–5秒校正一次（J1/J2微调）
│
└─ 结果：重力补偿从"必须精确"降级为"尽量做好"
 残余误差由弹簧吸收，清洁效果不受影响 ✅

J6末端接口（工具安装面）：
─────────────────────────────────────────────────────
机械接口：
├─ 安装面：圆形法兰，直径 φ50mm
├─ 安装螺钉：6×M4（均布）
├─ 定位销：2×φ4mm（保证重复定位精度 ±0.05mm）
├─ 工具旋转自由度：J4已承担工具绕Z轴旋转
└─ 工具重量限制：≤ 1kg

电气接口：
├─ 供电：DC 24V（工具电机）
├─ 信号：工具类型ID（2位编码）
│ └─ 00：内壁刷头
│ └─ 01：座圈擦拭头
│ └─ 10：消毒液喷洒头
│ └─ 11：外壁擦拭头
└─ 通信：工具自带控制器（由主控统一管理）

液体接口：
├─ 清洁液供给：J6轴心通孔
├─ 流量控制：主控板电磁阀
└─ 接头规格：快插接头（防漏）

两层控制架构（一体化关节方案）：
─────────────────────────────────────────────────────
Layer 1：上位机（Jetson NX / 树莓派5）
├─ 功能：
│ ├─ 逆运动学求解（6个关节角）
│ ├─ 轨迹规划（梯形/S曲线）
│ ├─ 清洁任务调度
│ ├─ 视觉处理（可选）
│ └─ CAN总线指令发送
├─ 通信：CAN总线（向6个关节发指令）
└─ 代码量：约 800–1500行

Layer 2：一体化关节（6个关节各自内置）
├─ 功能：
│ ├─ 电机FOC矢量控制
│ ├─ 位置环/速度环PID
│ ├─ 力矩限制（过载保护）
│ ├─ 关节状态上报
│ └─ 温度/电流监控
├─ 接口：CAN总线收指令、回报状态
└─ 用户只需发：目标角度（或速度）

硬件连接：
J1 ──[CAN]── J2 ──[CAN]── J3 ──[CAN]── J4 ──[CAN]── J5 ──[CAN]── J6
 │ │
 └────────────────── DC 48V 电源总线 ──────────────────────┘

仅需两根总线：CAN信号线 + 电源线
布线复杂度降低 90%

# 逆运动学求解（核心算法，约50行）

import numpy as np

def inverse_kinematics(x_target, y_target, z_target, tool_angle):
 """
 6自由度机械臂逆运动学求解
 输入：目标末端位置(x,y,z) + 工具朝向角
 输出：6个关节值 [θ1, θ2, d3, θ4, θ5, θ6]
 """
 # 已知参数
 L_base = 100 # 肩偏移(mm)
 L1 = 280 # SCARA第1连杆(mm)
 L2 = 250 # SCARA第2连杆(mm)
 L5 = 150 # 俯仰第1连杆(mm)
 L6 = 60 # 俯仰第2连杆(mm)
 h_base = 200 # 基座高度(mm)

 # === Step 1: SCARA平面定位 (J1, J2, J3) ===
 r = np.sqrt(x_target**2 + y_target**2)

 # J2角度（肘角，几何解）
 cos_theta2 = (r**2 - L1**2 - L2**2) / (2 * L1 * L2)
 cos_theta2 = np.clip(cos_theta2, -1, 1) # 数值保护
 theta2 = np.arccos(cos_theta2)

 # J1角度（底座朝向）
 phi = np.arctan2(y_target, x_target)
 k = (r**2 - L1**2 - L2**2) / (2 * L1)
 alpha = np.arctan2(y_target, x_target)
 theta1 = phi - np.arctan2(L2 * np.sin(theta2), L1 + L2 * np.cos(theta2))

 # J3升降量（Z轴）
 d3 = z_target - h_base

 # === Step 2: 手腕Z轴旋转 (J4) ===
 theta4 = tool_angle - theta1 - theta2

 # === Step 3: 俯仰链求解 (J5, J6) ===
 # 总俯仰角度（根据目标高度和内壁位置计算）
 z_pitch = z_target - (h_base + d3) # 俯仰链需要覆盖的Z变化
 r_pitch = np.sqrt(r**2 - z_pitch**2) # 水平伸出量

 # 俯仰链逆运动学（几何解）
 # θ5 + θ6 ≈ arctan2(z_pitch, r_pitch)
 total_pitch = np.arctan2(z_pitch, r_pitch)

 # 角度分配（6:4）
 theta5 = total_pitch * 0.6
 theta6 = total_pitch * 0.4

 return [theta1, theta2, d3, theta4, theta5, theta6]

# 使用示例
joints = inverse_kinematics(x_target=300, y_target=0, z_target=200, tool_angle=0)
print(f"关节角度: {[f'{j:.2f}°' for j in joints[:4]]}, 升降: {joints[2]:.1f}mm")