📚 课程目录第3章机器人运动学基础

3. 机器人运动学基础

在移动机械臂视觉抓取系统中，运动学是连接“看到目标”与“抓住目标”的数学桥梁。视觉模块给出的是相机坐标系下的目标位姿，而机械臂控制器需要的是各关节角度或末端执行器在基座坐标系下的位姿。若运动学建模不准确，即使视觉检测精度再高，抓取也会出现系统性偏差。本章围绕坐标系变换、DH参数、正逆运动学、雅可比矩阵以及移动底盘与机械臂联合建模五个核心知识点展开，为后续手眼标定、轨迹规划和抓取控制奠定基础。

一、坐标系变换：从像素到关节空间的桥梁

移动机械臂系统中至少涉及像素坐标系、图像坐标系、相机坐标系、末端执行器坐标系、机械臂基座坐标系、移动底盘坐标系和世界坐标系。任意两个坐标系之间的变换都可以用齐次变换矩阵统一描述：

T = [ R  t ]
    [ 0  1 ]

其中 R 为 3×3 旋转矩阵，t 为 3×1 平移向量。
点 P 在坐标系 B 下的坐标可表示为：
P_B = T_AB · P_A

旋转矩阵满足正交性 R·Rᵀ=I 且 det(R)=1。实际工程中常用欧拉角、旋转向量或四元数表示姿态，再转换为旋转矩阵参与链式运算。坐标系变换的核心价值在于：把视觉观测统一到机械臂基座坐标系，使规划与控制在同一参考系下进行。

建议在代码中始终使用 4×4 齐次矩阵进行链式相乘，避免分别处理旋转和平移导致维度混乱。链式顺序为：世界→底盘→机械臂基座→末端→相机→目标。

二、DH参数：用四个参数描述相邻连杆关系

Denavit-Hartenberg（DH）参数法用四个参数描述相邻连杆坐标系之间的变换关系，是串联机械臂建模的经典工具。标准DH参数包括：连杆长度 aᵢ、连杆扭角 αᵢ、关节偏距 dᵢ、关节转角 θᵢ。对于旋转关节，θᵢ 为变量；对于移动关节，dᵢ 为变量。

相邻连杆变换矩阵为：

A_i = Rot_z(θ_i) · Trans_z(d_i) · Trans_x(a_i) · Rot_x(α_i)

展开后：
A_i = [ cθ  -sθcα   sθsα   a·cθ ]
      [ sθ   cθcα  -cθsα   a·sθ ]
      [ 0    sα     cα     d    ]
      [ 0    0      0      1    ]

建立DH参数表时，需要为每个关节指定 z 轴（关节轴线）和 x 轴（相邻 z 轴公垂线方向）。下表给出一个典型六自由度机械臂的DH参数示例：

关节 i	aᵢ (mm)	αᵢ (°)	dᵢ (mm)	θᵢ 范围 (°)
1	0	90	150	-170 ~ 170
2	260	0	0	-120 ~ 120
3	60	90	0	-150 ~ 150
4	0	-90	240	-180 ~ 180
5	0	90	0	-120 ~ 120
6	0	0	80	-360 ~ 360

三、正运动学与逆运动学

正运动学是已知各关节变量，求末端执行器位姿。对于六自由度机械臂，末端位姿为各连杆变换矩阵的连乘：

T_0_6 = A_1 · A_2 · A_3 · A_4 · A_5 · A_6

正运动学计算简单、结果唯一，适合实时反馈和可视化。逆运动学则是已知末端目标位姿，反求各关节角度。逆解通常存在多解、无解或奇异情况。工程上常用解析法（适用于满足Pieper准则的构型）和数值法（如雅可比迭代、阻尼最小二乘）。

逆运动学多解时，需根据关节限位、避障、能量最优或与当前位姿最近原则选择合理解。直接取第一组解可能导致机械臂大幅翻转甚至碰撞。

四、雅可比矩阵：速度映射与奇异分析

雅可比矩阵 J(q) 描述关节速度与末端速度之间的线性映射关系：

ẋ = J(q) · q̇

其中 ẋ 为末端六维速度 [v; ω]，
q̇ 为关节速度向量。

雅可比矩阵的每一列表示对应关节单位速度对末端线速度和角速度的贡献。当 J 不满秩时，机械臂处于奇异位形，某些方向无法运动，逆解速度趋于无穷。实际系统中可通过条件数、最小奇异值或可操作度指标监测奇异。

在视觉伺服中，雅可比矩阵还用于图像特征速度到关节速度的映射，即图像雅可比与机器人雅可比的复合。移动机械臂的雅可比需要同时考虑底盘运动和机械臂关节运动。

五、移动机械臂运动学建模

移动机械臂不是固定基座机械臂的简单扩展。底盘运动使机械臂基座位姿随时间变化，因此需要建立“底盘—机械臂”联合运动学模型。常用方法是将底盘位姿 (x, y, φ) 与机械臂关节角 q 组合为广义坐标：

ξ = [x, y, φ, q1, q2, q3, q4, q5, q6]ᵀ

末端位姿：
T_world_ee = T_world_base(x,y,φ) · T_base_ee(q)

对时间求导可得联合雅可比：

ẋ_ee = J_base · ξ̇_base + J_arm · q̇

其中 J_base 反映底盘运动对末端速度的影响，J_arm 为机械臂雅可比。若底盘为差速驱动，还需加入非完整约束：

ẋ·sinφ - ẏ·cosφ = 0

该约束意味着底盘不能横向平移，规划时必须考虑。移动抓取中，通常先调整底盘使目标进入机械臂工作空间，再锁定底盘进行精细抓取，以降低联合控制的复杂度。

六、典型误区与工程复盘

DH参数符号约定混乱：标准DH与改进DH的坐标系定义不同，混用会导致变换矩阵错误。建议在项目中统一约定并写入文档。
忽略底盘非完整约束：直接按全向移动规划底盘速度，实际执行时轨迹偏离，导致抓取失败。
逆解不检查关节限位：数学上有解但超出物理限位，控制器会报错或产生危险动作。
雅可比奇异未处理：在奇异位形附近求逆会放大噪声，建议使用阻尼最小二乘或伪逆。
坐标系变换链顺序错误：左乘与右乘含义不同，必须明确“在哪个坐标系下描述”。

工程建议：在移动机械臂抓取系统中，先建立世界→底盘→基座→末端→相机的完整变换链，再分别验证正运动学、逆运动学和雅可比矩阵。建议用已知位姿的标定板做闭环验证，误差应控制在毫米级，否则后续视觉抓取精度无法保证。

运动学建模是移动机械臂视觉抓取系统的“骨架”。坐标系变换解决“在哪里”的问题，DH参数解决“怎么描述”的问题，正逆运动学解决“怎么到达”的问题，雅可比矩阵解决“怎么运动”的问题，而移动机械臂联合建模则解决“底盘与机械臂如何协同”的问题。只有这五个环节都准确无误，视觉抓取才能从仿真走向真实场景。