3. 运动学基础：正运动学与逆运动学建模

运动学是机器人学的基石，它研究的是机器人关节空间与末端执行器位姿之间的映射关系，而不涉及产生运动的力和力矩。在运动控制算法开发中，无论是轨迹规划、力控还是视觉伺服，都离不开运动学模型的支持。本章以六轴串联机械臂和差速驱动移动机器人为例，深入剖析正运动学与逆运动学的数学本质、建模方法、代码实现路径以及工程调试中的关键陷阱。理解本章内容，是后续学习动力学控制、轨迹跟踪和运动规划的前提。

3.1 正运动学建模：从关节空间到笛卡尔空间

正运动学解决的是"给定关节角度，求末端执行器的位姿"这一核心问题。对于六轴机械臂，我们通常采用Denavit-Hartenberg（D-H）参数法建立连杆坐标系间的齐次变换矩阵。每个连杆由四个参数描述：连杆长度 a_i-1、连杆转角 α_i-1、连杆偏距 d_i 和关节角 θ_i。相邻坐标系间的变换矩阵为：

T_i = Rot(z, θ_i) * Trans(z, d_i) * Trans(x, a_{i-1}) * Rot(x, α_{i-1})

将六个变换矩阵依次相乘，即可得到末端执行器相对于基座标系的位姿矩阵 T_06。该矩阵包含旋转矩阵 R（3×3）和平移向量 P（3×1），完整描述了末端在三维空间中的位置和姿态。对于移动机器人，正运动学则表现为航迹推演（Dead Reckoning）：给定左右轮速度 v_L、v_R 和采样周期 Δt，通过积分计算位姿增量：

Δx = v * cos(θ) * Δt
Δy = v * sin(θ) * Δt
Δθ = ω * Δt

其中 v = (v_R + v_L)/2，ω = (v_R - v_L)/L，L 为轮距。这种模型在短距离内精度较高，但会随时间累积漂移。

3.2 逆运动学建模：从笛卡尔空间回到关节空间

逆运动学是运动控制中更具挑战性的问题：给定期望的末端位姿，求解满足条件的关节角组合。对于六轴机械臂，逆运动学通常存在多重解（如肘部上/下、腕部翻转等），且在某些位形下会出现奇异——即雅可比矩阵秩亏，导致关节速度趋于无穷大。常用的求解方法包括：

• 解析法：通过代数或几何方法直接求解，计算速度快、精度高，但仅适用于满足Pieper准则的机构（相邻三轴交于一点）。
• 数值迭代法：如牛顿-拉夫森法或雅可比伪逆法，通用性强，但需要良好的初值且计算量较大。
• 几何法：针对特定构型（如球腕结构）将问题分解为位置逆解和姿态逆解，直观且高效。

对于差速移动机器人，逆运动学表现为速度分解：给定目标线速度 v 和角速度 ω，反解左右轮速度：

v_R = v + (ω * L) / 2
v_L = v - (ω * L) / 2

这一过程看似简单，但在实际控制中需要处理轮速饱和、打滑补偿以及奇异状态（如原地旋转时 v=0 但 ω≠0）等问题。

3.3 案例拆解：六轴机械臂的逆运动学求解与调试

我们以一款典型的六轴协作机械臂（D-H参数如表1所示）为例，演示逆运动学求解的完整流程。该机械臂采用球腕结构，满足Pieper准则，因此可以使用解析法求解。

场景描述：在某焊接应用中，机器人需要将末端焊枪移动到工件上方某点 P(450, 120, 350)（单位mm），且焊枪姿态要求为：Z轴垂直向下（即接近向量指向 -Z 基座标方向），工具坐标系与基座标系无额外旋转。初始关节角为 θ = [0, -30°, 60°, 0, 90°, 0]°。

求解过程：

1. 位置逆解：首先根据目标位置 P 和腕部偏移 d6=80mm，计算出腕心位置 P_w = P - d6 * Z_tool。由于姿态要求垂直向下，Z_tool = [0, 0, -1]，因此 P_w = [450, 120, 430]。
2. 求解 θ1：θ1 = atan2(P_w.y, P_w.x) = atan2(120, 450) ≈ 14.93°。
3. 求解 θ2、θ3：将问题投影到平面，利用余弦定理求解肘部位置。计算得到两组解：肘部向上（θ2≈-45.2°, θ3≈78.5°）和肘部向下（θ2≈-134.8°, θ3≈-78.5°）。根据工作空间限制，选择肘部向上解。
4. 求解 θ4-θ6：根据姿态矩阵的剩余部分，利用欧拉角分解得到 θ4≈0°, θ5≈-90°, θ6≈0°。
5. 验证：将求得的关节角代入正运动学，计算末端位姿与目标对比，位置误差小于 1e-6 mm。

调试中的关键问题：在实际调试中，我们发现当目标点接近工作空间边界时，解析解会出现跳变。例如，当 P 点 x 坐标从 450mm 变为 460mm 时，θ1 从 14.93° 跳变到 -165.07°（即 194.93° 的等价表示）。这是因为 atan2 返回的角度范围是 (-π, π]，而机械臂的关节限位是 ±170°。解决方案是在求解后增加关节角归一化处理，并选择与当前关节角最接近的解作为最终输出。

3.4 移动机器人运动学建模与航迹推演实战

差速移动机器人的运动学模型虽然简单，但在实际应用中需要处理传感器噪声、轮子打滑和离散化误差等问题。我们以一个室内巡检机器人为例，其轮距 L=0.35m，轮子半径 r=0.05m，编码器分辨率为 1024 线/圈，控制周期 20ms。

航迹推演实现：

// 伪代码：航迹推演
void odometry_update(float dt) {
    float vL = (enc_L_delta / 1024.0f) * 2 * M_PI * r / dt;
    float vR = (enc_R_delta / 1024.0f) * 2 * M_PI * r / dt;
    float v = (vR + vL) / 2.0f;
    float w = (vR - vL) / L;
    
    x += v * cos(theta) * dt;
    y += v * sin(theta) * dt;
    theta += w * dt;
    
    // 角度归一化到 [-PI, PI]
    theta = atan2(sin(theta), cos(theta));
}

实测数据与分析：在光滑瓷砖地面上直线行驶 10m，航迹推演结果如下：

可以看出，随着行驶距离增加，累积误差显著增大。主要误差来源包括：轮子打滑（启动和停止时）、编码器量化误差以及地面不平整导致的轮径变化。针对这些问题，我们采取了以下措施：

• 轮径校准：通过行驶已知距离，反向计算实际轮径，将 r 从 0.05m 修正为 0.0493m。
• 打滑检测：比较编码器速度与IMU角速度，当偏差超过阈值时降低速度指令。
• 定期重置：在巡检点使用激光雷达或视觉标记进行定位修正。

3.5 常见误区与建模复盘

在运动学建模与调试过程中，工程师常犯以下错误：

1. D-H参数表错误：不同教材对 D-H 参数的定义存在差异（标准D-H vs. 改进D-H），混用会导致坐标系错乱。建议在代码注释中明确标注所采用的约定，并编写正运动学验证函数。
2. 忽略关节限位：逆运动学求解出的角度可能超出关节物理限位，必须在求解后立即检查并处理。
3. 奇异位形处理不当：在奇异点附近，即使关节角微小变化也会导致末端速度剧变。工程上应通过阻尼最小二乘法（DLS）或奇异鲁棒逆解来平滑过渡。
4. 移动机器人轮距测量误差：轮距 L 的误差直接影响旋转精度，应使用精密测量工具多次测量取平均值。
5. 坐标系定义不统一：机械臂基座标系、工具坐标系、移动机器人车体坐标系之间的转换关系必须明确，建议建立统一的坐标变换树。

建模复盘：运动学建模不是一次性的工作，而是需要持续迭代和验证的过程。建议在代码中内置正运动学自检功能：每次逆运动学求解后，自动调用正运动学验证位姿误差，并记录日志。对于移动机器人，定期进行"回字形"行驶测试，评估航迹推演的累积误差，并据此调整模型参数。运动学模型的精度直接决定了上层控制算法的性能上限，值得投入足够的时间进行精细调校。