2. 机器人学基础
双臂视觉抓取与交接操作的所有算法，最终都要落到“机械臂末端在哪儿、朝哪儿、怎么动”这几个问题上。本章把机器人学中最核心的数学工具串成一条线：用刚体变换描述位姿，用正运动学由关节角算出末端位姿，用逆运动学由目标位姿反解关节角，用雅可比矩阵把关节速度映射为末端速度，再结合工作空间与双臂构型，判断“这个抓取和交接任务在几何上是否可行”。
一、刚体变换：位姿的统一描述
刚体在三维空间中的状态由位置和姿态共同决定，称为位姿。位置用三维平移向量 p = [x, y, z]ᵀ 表示，姿态用 3×3 旋转矩阵 R 表示，二者组合成 4×4 齐次变换矩阵：
T = [ R p ]
[ 0 1 ]
其中 R 满足 RᵀR = I 且 det(R) = +1
齐次变换的价值在于“串联”：若基座到肩部为 T₁，肩部到肘部为 T₂，肘部到末端为 T₃，则末端相对基座的位姿就是 T = T₁·T₂·T₃。旋转矩阵虽然直观，但存在万向节死锁和插值困难，因此工程中常用欧拉角、轴角或四元数表示姿态。四元数 q = (w, x, y, z) 无奇异、插值平滑，是视觉抓取中传递目标姿态的首选。
二、正运动学与逆运动学
正运动学（FK）是“已知关节角，求末端位姿”，本质是沿连杆链依次相乘变换矩阵，计算确定、无多解。逆运动学（IK）是“已知末端目标位姿，求关节角”，它才是抓取任务中的难点：
- 多解性：同一个末端位姿可能对应“肘上/肘下”“腕翻转”等多组关节角，需要按避障、关节限位、行程最短来选解。
- 可解性：目标点若超出臂长可达范围，或姿态超出腕部转动能力，则无解。
- 求解方式：解析法速度快、精度高，但依赖特定构型；数值法（如雅可比迭代、阻尼最小二乘）通用性强，但需迭代且可能陷入局部解。
在双臂交接中，IK 不仅要让末端到达抓取点，还要保证两只手在交接瞬间的位姿满足“可对接”约束，因此常把交接点作为共同目标，分别对两条臂求解并检查解的存在性与连续性。
三、雅可比矩阵：关节速度到末端速度的桥梁
雅可比矩阵 J(q) 描述关节微小运动与末端微小运动之间的线性关系：
ẋ = J(q) · q̇
其中 ẋ = [v; ω] 为末端线速度与角速度
q̇ 为关节速度向量
雅可比矩阵的三大用途：一是速度控制与轨迹跟踪；二是通过 τ = JᵀF 把末端接触力换算为关节力矩；三是判断奇异位形——当 J 降秩时，某些方向末端无法运动，逆解会发散、关节速度会突变。工程上常用阻尼最小二乘（DLS）缓解奇异：
q̇ = Jᵀ (J Jᵀ + λ² I)⁻¹ ẋ
λ 为阻尼系数，越大越稳定但跟踪精度略降
四、工作空间与双臂系统构型
工作空间是末端可达点的集合，分为“可达工作空间”和“灵巧工作空间”（能以任意姿态到达的区域）。双臂系统的构型直接决定交接是否顺畅，常见有三类：
| 构型 | 布局特点 | 交接优势 | 主要约束 |
|---|---|---|---|
| 对置式 | 两臂相对安装，工作空间在中间重叠 | 交接路径短、可视性好 | 中间区域易碰撞 |
| 并列式 | 两臂同侧平行布置 | 结构紧凑、标定简单 | 重叠区小，交接需绕行 |
| 仿人式 | 肩部左右分布，类似人类双臂 | 姿态自然、适合复杂操作 | 运动学耦合强，规划复杂 |
判断交接可行性的核心是“重叠工作空间 ∩ 灵巧工作空间 ∩ 无碰撞区域”。若交接点落在重叠区边缘，虽然位置可达，但姿态调整余量小，容易在交接瞬间失稳。
五、案例：从视觉目标到双臂交接的完整链路
假设视觉系统给出目标物体位姿 T_obj，交接规划流程如下：
- 把
T_obj转换到机器人基座坐标系，得到抓取位姿T_grasp。 - 对主动臂做 IK，检查解是否存在、是否在关节限位内、是否远离奇异位形。
- 在重叠工作空间内选取交接点
T_handoff，要求两臂都能以合理姿态到达。 - 分别对两臂做 IK，并沿时间轴检查雅可比条件数与关节速度上限。
- 若交接点不可行，则回退调整物体姿态或交接点位置，重新求解。
六、复盘：常见错误与排查清单
- 坐标系混用：视觉结果未统一到基座坐标系，导致 IK 目标点整体偏移。
- 忽略多解选择：直接取第一组解，可能让手臂撞到自身或工作台。
- 奇异位形未检查：末端到位但关节速度异常，实机抖动或报警。
- 工作空间判断过粗：只判断“点是否可达”，未判断“姿态是否可达”。
- 双臂耦合未考虑：两臂各自可行，但中间区域发生碰撞。
掌握刚体变换、正逆运动学、雅可比矩阵、工作空间与双臂构型这五个基础工具后，后续的视觉标定、抓取规划与交接控制才有可靠的几何与运动学依据。它们是双臂系统从“能看见”走向“能抓住、能交接”的底层支撑。