逆向运动学（IK）

逆向运动学：给定末端执行器的目标位姿，反算每个关节的角度。

IK 比 FK 难得多，因为：

• 存在多解（肘关节可以向上或向下）
• 可能无解（目标超出工作空间）
• 可能有无穷多解（冗余自由度）

常见方法：

方法	原理	优缺点
解析法	几何三角推导，直接算出每个关节角	快、准，但只适用于特定构型
数值法（雅可比迭代）	用雅可比矩阵伪逆迭代逼近	通用，但慢、可能不收敛
优化法	用非线性优化求解	灵活，但计算量大

推荐资源：

• Modern Robotics 第 6 章 Inverse Kinematics
• 解析法示例：搜索 “3-DOF planar arm inverse kinematics geometry”

本项目的情况

本项目用的是解析法，因为前 3 个关节（Yaw + Pitch1 + Pitch2）构成一个平面三连杆，可以用几何法直接求解：

已知：目标 world 坐标 (x, y, z)，去掉 L1 基座高度后得到腕部坐标 (x_w, y_w, z_w)

1. q0 = atan2(y_w, x_w)                        ← 基座旋转朝向目标
2. 在 XZ 平面用余弦定理求 q1、q2（肘向下分支）    ← 大臂 + 小臂
3. q3 = φ - q1 - q2                             ← 腕部调整姿态
4. q4 = 0（初始值）                              ← 末端 roll，后续用视觉对齐

代码位置：src/arm2_task/src/kinematics_engine.cpp 中的 solveIK() 和 solvePlanar3Link()

为什么选解析法？ 因为我们 5 个关节中有 3 个是平面连杆，几何结构简单，解析法又快又不会发散。数值法反而可能因为初始猜测不好而收敛到错误解。