4. 机械臂运动学
移动操作机器人要在仿真与实机中完成抓取、放置、按钮操作等任务，机械臂运动学是连接“任务空间目标”和“关节空间指令”的桥梁。本节围绕四个核心问题展开：已知关节角如何求末端位姿（正运动学）、已知末端位姿如何求关节角（逆运动学）、速度层面如何映射（雅可比矩阵）、以及哪些位形会让求解失效（奇异位形）。理解这四点，才能在仿真中稳定生成轨迹，并在实机上避免抖动、失控或关节超限。
一、正运动学：从关节角到末端位姿
正运动学是确定性的前向计算：给定各关节角度 q = [q1, q2, ..., qn]，通过连杆参数和关节变换链，求出末端执行器在基坐标系下的位置与姿态。常用方法是DH参数法或改进DH法，把每个连杆用一个4×4齐次变换矩阵描述，再依次相乘。
# 以UR5为例，正运动学核心逻辑（伪代码）
T = I
for i in range(6):
T = T @ DH_transform(a[i], alpha[i], d[i], theta[i] + q[i])
pos = T[:3, 3] # 末端位置
rot = T[:3, :3] # 末端姿态旋转矩阵
在仿真中，正运动学通常由URDF和KDL/IKFast自动完成，但理解其计算链有助于排查“仿真末端位置与预期不符”的问题。常见原因包括：DH参数符号错误、关节零位偏移未标定、工具坐标系TCP未正确设置。正运动学没有多解问题，只要参数正确，结果唯一。
二、逆运动学：解析解法与数值解法
逆运动学是运动学中最具挑战的部分：给定末端目标位姿，求满足条件的关节角。它可能无解、有唯一解或多解。对于6自由度串联臂，若满足Pieper准则（三轴相交或三轴平行），可得到解析解；否则通常采用数值迭代。
- 解析解法：通过几何关系分离变量，直接求出有限组关节角。速度快、精度高，但依赖机械臂构型，通用性差。
- 数值解法：以当前关节角为初值，迭代逼近目标。常用雅可比伪逆法、阻尼最小二乘法（DLS）、牛顿-拉夫森法。通用性强，但可能收敛到局部解或不收敛。
- 优化解法：把逆运动学写成带约束的优化问题，加入关节限位、避障、姿态偏好等代价项，适合移动操作机器人在复杂场景中使用。
选择建议：仿真阶段优先使用解析解或IKFast生成离线轨迹；实机部署时若目标频繁变化，使用数值解并配合关节限位与速度限幅。移动底盘与机械臂协同任务中，常把底盘位姿作为浮动基座，逆运动学需在基座坐标系下求解。
三、雅可比矩阵：速度层面的映射
雅可比矩阵 J(q) 描述关节速度到末端速度的线性映射：ẋ = J(q) · q̇。它是逆运动学数值迭代、力控制、奇异分析的基础。对于n自由度机械臂，末端6维速度对应6×n的雅可比矩阵。
# 数值逆运动学迭代（阻尼最小二乘）
while error > tol:
J = compute_jacobian(q)
e = target_pose - forward_kinematics(q)
dq = J.T @ inv(J @ J.T + lambda^2 * I) @ e
q = q + alpha * dq
雅可比矩阵的每一列表示对应关节单位速度引起的末端运动旋量。通过分析雅可比的秩、条件数，可以判断当前位形是否接近奇异。在仿真中，很多规划器会实时计算雅可比条件数，当条件数超过阈值时降低速度或改变路径。
四、奇异位形：不可忽视的失效边界
奇异位形是雅可比矩阵降秩的位形，此时某些方向上的末端运动无法由关节速度实现，逆解可能无穷多或不存在。典型奇异包括：腕部奇异（两轴共线）、肘部奇异（手臂完全伸直）、肩部奇异（腕心位于基座轴线上）。
| 奇异类型 | 典型位形 | 现象 | 应对策略 |
|---|---|---|---|
| 腕部奇异 | 第4、6轴共线 | 姿态微调导致关节速度剧增 | 避免腕部完全对齐，加入姿态偏置 |
| 肘部奇异 | 手臂完全伸直 | 末端无法沿臂方向继续外伸 | 保持肘部微弯，限制工作空间边界 |
| 肩部奇异 | 腕心落在基座轴线上 | 肩部关节解不唯一或跳变 | 偏移基座轴线，使用阻尼最小二乘 |
实机风险：接近奇异位形时，数值逆解可能输出极大的关节速度指令，导致实机剧烈抖动、触发保护或损坏减速器。部署前应在仿真中扫描工作空间，标记奇异区域，并在控制器中加入速度限幅和奇异回避策略。
五、仿真到实机的关键检查清单
- 确认URDF/DH参数与实机一致，特别是关节零位和工具TCP。
- 验证正运动学：随机取关节角，对比仿真与实机末端位姿。
- 验证逆运动学：给定目标位姿，检查解的关节限位、可达性和多解选择。
- 检查雅可比条件数：在典型任务轨迹上扫描，标记接近奇异的区段。
- 加入阻尼最小二乘或速度限幅，避免奇异附近指令发散。
- 实机首次运行使用低速、小范围运动，逐步扩大工作空间。
总结来看，正运动学是“算得出”，逆运动学是“求得到”，雅可比矩阵是“连得上”，奇异位形是“避得开”。在移动操作机器人的仿真与实机部署中，建议先用正运动学验证模型，再用逆运动学生成轨迹，用雅可比矩阵监控速度与奇异，最后在实机上加入限幅和保护。只有把四者串成闭环，才能让机械臂在移动平台上稳定、安全地完成操作任务。