2. 机器人运动学基础：位姿描述与齐次变换，D-H参数法建立运动学模型，正运动学与逆运动学求解
各位同学，欢迎来到第二章。这一章是整个轨迹规划的基石。说白了，你要让机器人动起来，首先得知道它现在在哪、胳膊怎么伸的。这就是运动学要解决的问题。
我个人习惯把运动学比作「给机器人拍X光片」——我们不关心它为什么有力气，只关心骨头之间的相对位置和角度。嗯，这个比喻可能不太严谨，但你记住这个感觉就好。
2.1 位姿描述：位置与姿态
机器人学里，描述一个物体在空间中的状态，需要两样东西：位置和姿态。合起来叫位姿。
- 位置：用三维坐标 (x, y, z) 表示。比如机器人末端在空间中的某个点。
- 姿态：用旋转矩阵、欧拉角或四元数表示。描述这个物体是正着、歪着还是倒着。
我在项目中遇到过一个问题：一个六轴机器人抓取工件时，明明位置对了，但姿态偏了0.5度，结果工件卡在夹具里出不来。你看，姿态的精度有时候比位置还关键。
重要概念：一个刚体在空间中的自由度是6个（3个平移 + 3个旋转）。所以描述位姿，最少需要6个独立参数。
2.2 齐次变换矩阵
为什么要用齐次变换？你想想看，如果位置用3x1向量，旋转用3x3矩阵，那你要做一次坐标变换，就得分开算旋转和平移，很麻烦。齐次变换矩阵把两者统一成4x4矩阵，一次乘法搞定所有。
齐次变换矩阵 T 的形式如下：
T = [ R p ]
[ 0 1 ]其中 R 是 3x3 旋转矩阵，p 是 3x1 位置向量。最后一行是 [0 0 0 1]，这是齐次坐标的固定格式。
举个例子，坐标系 {B} 相对于坐标系 {A} 的描述：
^A_T_B = [ ^A_R_B ^A_p_B ]
[ 0 1 ]这个矩阵可以连续乘。比如从基座到末端，中间经过关节1、2、3... 每个关节一个变换矩阵，乘起来就是末端相对于基座的位姿。这就是正运动学的核心思想。
我的小技巧：写代码时，我习惯把齐次变换矩阵封装成一个类，包含旋转矩阵和平移向量。这样调试的时候，可以单独打印旋转部分和平移部分，方便定位问题。
2.3 D-H参数法：建立运动学模型
D-H参数法，全称Denavit-Hartenberg参数法。这是机器人学里最经典的运动学建模方法。我刚开始学的时候觉得它很绕，后来发现其实就是四个参数描述两个相邻关节之间的关系。
四个参数分别是：
| 参数 | 符号 | 含义 |
|---|---|---|
| 连杆长度 | ai | 沿xi轴，从zi到zi+1的距离 |
| 连杆扭角 | αi | 绕xi轴，从zi到zi+1的转角 |
| 连杆偏距 | di | 沿zi轴，从xi-1到xi的距离 |
| 关节转角 | θi | 绕zi轴，从xi-1到xi的转角 |
对于旋转关节，θi是变量；对于移动关节，di是变量。其他三个参数是固定的，由机器人结构决定。
我曾经在调试一个SCARA机器人时，发现D-H参数表里一个α的符号写反了，结果正运动学算出来的末端位置差了十几毫米。查了两天才找到问题。嗯，D-H参数表一定要反复核对。
相邻关节的变换矩阵公式：
T_i = Rot(z, θ_i) * Trans(z, d_i) * Trans(x, a_i) * Rot(x, α_i)展开后：
T_i = [ cosθ_i -sinθ_i*cosα_i sinθ_i*sinα_i a_i*cosθ_i ]
[ sinθ_i cosθ_i*cosα_i -cosθ_i*sinα_i a_i*sinθ_i ]
[ 0 sinα_i cosα_i d_i ]
[ 0 0 0 1 ]注意：D-H参数有两种约定——标准D-H和改进D-H。区别在于坐标系建立的位置不同。我个人习惯用标准D-H，但如果你用ROS，它默认是改进D-H。混用会出大问题，切记！
2.4 正运动学求解
正运动学，就是已知关节角度（或位移），求末端位姿。说白了，你告诉机器人每个关节转多少度，它告诉你手在哪。
对于n轴机器人，正运动学公式很简单：
^0_T_n = T_1 * T_2 * T_3 * ... * T_n其中T_i是第i个关节的D-H变换矩阵。乘出来的结果是一个4x4齐次变换矩阵，包含了末端的位置和姿态。
举个例子，一个六轴机器人，给定六个关节角度θ1到θ6，代入D-H参数表，依次计算T1到T6，然后乘起来。就这么简单。
代码实现也很直接：
def forward_kinematics(theta, dh_params):
# dh_params: [a, alpha, d, theta_offset]
T = np.eye(4)
for i in range(len(theta)):
a, alpha, d, offset = dh_params[i]
theta_i = theta[i] + offset
Ti = dh_transform(a, alpha, d, theta_i)
T = T @ Ti
return T我在项目中用这个函数做过实时正运动学计算，在100Hz的控制周期里跑，完全没问题。计算量其实很小。
2.5 逆运动学求解
逆运动学就麻烦多了。已知末端位姿，求关节角度。说白了，你告诉机器人手要放到哪，它自己算每个关节该转多少。
为什么麻烦？因为逆运动学通常有多个解，甚至无解。比如一个六轴机器人，同一个末端位姿可能有8组关节角度解。你想想看，肘关节可以朝上也可以朝下，手腕可以翻转也可以不翻转，组合起来就多了。
求解方法主要有两种：
- 解析法：通过几何或代数推导出封闭解。速度快，但只适用于满足Pieper准则的机器人（相邻三个关节轴交于一点）。大多数工业六轴机器人满足这个条件。
- 数值法：用迭代优化求解，比如牛顿-拉夫森法。通用性强，但速度慢，且可能不收敛。
我个人偏好解析法。为什么？因为稳定。数值法在奇异点附近容易发散，而解析法只要公式推导正确，结果就是确定的。
以典型的六轴工业机器人为例，逆运动学求解步骤大致如下：
- 从末端位姿反推腕部中心位置（因为最后三个关节决定姿态）
- 根据腕部中心位置求解前三个关节角度（通常有2-4组解）
- 根据前三个关节角度和末端姿态，求解后三个关节角度
- 组合所有可能的解，剔除超出关节限位的解
- 从剩余解中选择最优解（比如关节运动量最小的那个）
避坑指南：我曾经在逆运动学求解时，没有处理奇异点。结果机器人在某个位姿附近突然关节速度突变，差点撞到工件。后来我加了一个奇异点检测，当接近奇异位形时，切换到数值法并限制关节速度。嗯，这个坑踩得值。
2.6 运动学在实际项目中的应用
讲完理论，说说实际怎么用。运动学在工业机器人里无处不在：
- 示教编程：操作员拖动示教器，系统实时计算正运动学，显示末端位姿。
- 离线编程：在软件里规划路径，用逆运动学把路径点转换成关节角度序列。
- 轨迹插补：在笛卡尔空间插补时，每个插补点都要做一次逆运动学，得到关节角度。
- 碰撞检测：用正运动学实时计算各连杆的位置，判断是否与环境碰撞。
我记得有一次做焊接机器人项目，焊缝是一条空间曲线。我们在笛卡尔空间均匀采样了1000个点，每个点做逆运动学求解。结果发现有些点无解——因为超出了机器人的工作空间。后来调整了工件摆放位置才解决。所以，做轨迹规划前，一定要先确认目标位姿在机器人的可达范围内。
实用建议：写运动学代码时，先把正运动学写好并验证正确。然后用正运动学生成大量随机关节角度对应的末端位姿，作为逆运动学测试的样本。这样能快速发现逆运动学代码的bug。
好了，这一章的内容就到这里。运动学是机器人学的「内功」，基础打牢了，后面学轨迹规划、动力学、控制才会顺手。下一章我们讲轨迹插补的具体算法，到时候会大量用到今天学的齐次变换和运动学求解。