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

3、运动学基础：正运动学与逆运动学、DH参数法、腿部工作空间分析

各位同学，欢迎来到运动学这一章。说实话，很多做四足机器人的朋友，一开始都急着调PID、跑步态，结果腿卡在地上或者走着走着就劈叉了。为什么？因为运动学没搞明白。

运动学是什么？说白了，就是研究“关节怎么动，脚才能到那儿”的学问。它不关心力，不关心扭矩，只关心位置和姿态。但就是这么个“纯几何”的问题，搞不好，你的狗子就是个瘸子。

我个人习惯把运动学分成两块：正运动学和逆运动学。这两兄弟，一个顺着推，一个反着算，是腿部控制的基础。再加上DH参数法这个标准工具，以及工作空间分析这个实用技能，今天这一章，我们就把四足机器人的腿给彻底聊透。

3.1 正运动学：从关节空间到操作空间

正运动学，就是已知每个关节的角度，求脚端在空间中的位置。听起来很简单对吧？但实际做起来，坐标系一多，人就容易晕。

我刚开始做四足的时候，就犯过一个低级错误：把大腿和小腿的旋转方向搞反了，结果仿真里腿直接穿模到身体里去了。嗯，从那以后，我每次建坐标系都会在纸上画三遍。

对于一条标准的四足机器人腿部，我们通常有三个关节：髋关节横滚（Hip Roll）、髋关节俯仰（Hip Pitch）和膝关节俯仰（Knee Pitch）。正运动学就是把这三级连杆的变换矩阵乘起来。

    核心公式：

    T = T0→1 · T1→2 · T2→3

    其中 Ti→i+1 是相邻坐标系间的齐次变换矩阵。

举个例子，假设大腿长度 L1，小腿长度 L2，髋关节俯仰角 θ1，膝关节俯仰角 θ2。那么脚端在髋关节坐标系下的位置 (x, y, z) 可以这样算：

x = L1 * sin(θ1) + L2 * sin(θ1 + θ2)
y = 0  (假设横滚角为0)
z = -L1 * cos(θ1) - L2 * cos(θ1 + θ2)

你看，其实就是个平面几何问题。但一旦加上横滚角，就变成了三维空间里的旋转，这时候矩阵乘法就逃不掉了。

我的小技巧： 在写正运动学代码之前，先用MATLAB或者Python的符号计算库（比如SymPy）把矩阵乘法的结果推导出来，然后手动化简。这样能避免很多手算错误。

3.2 DH参数法：标准化的坐标系建立方法

说到建立坐标系，就不得不提DH参数法。Denavit和Hartenberg这两位老前辈，给我们留下了一套标准流程。你只要按照这个流程走，任何串联机构的运动学都能搞定。

DH参数法有四个参数：

参数	符号	含义
连杆长度	a_i	沿x_i轴，从z_i-1到z_i的距离
连杆扭角	α_i	绕x_i轴，从z_i-1到z_i的转角
连杆偏距	d_i	沿z_i-1轴，从x_i-1到x_i的距离
关节转角	θ_i	绕z_i-1轴，从x_i-1到x_i的转角

对于四足机器人的腿部，我一般这样分配DH参数：

关节1（髋横滚）： a=0, α=-90°, d=0, θ=θ₁
关节2（髋俯仰）： a=L1, α=0°, d=0, θ=θ₂
关节3（膝俯仰）： a=L2, α=0°, d=0, θ=θ₃

有了这张表，你就能写出通用的变换矩阵。我曾经在项目里用DH参数法同时处理了四条腿，代码复用率极高，改个参数就能适配不同尺寸的机器人。

注意： DH参数有两种约定——标准DH和改进DH。区别在于坐标系建在连杆的前端还是后端。我个人习惯用改进DH（Craig版本），因为它在处理树形结构时更直观。选一种，然后坚持用下去，别混着用。

3.3 逆运动学：从操作空间回到关节空间

逆运动学是正运动学的逆过程：已知脚端位置，求关节角度。这是实际控制中最常用的——你告诉机器人“脚放到这里”，它得算出每个电机转多少度。

对于四足机器人的腿部，逆运动学通常有解析解。为什么？因为腿的结构相对简单，几何关系清晰。我见过有人用数值法（比如牛顿迭代）去解，说实话，没必要，还容易不收敛。

以最常见的两自由度腿部（髋俯仰+膝俯仰）为例，逆解公式如下：

// 已知脚端位置 (x, z)，求 θ1 和 θ2
// L1: 大腿长度, L2: 小腿长度

// 先求膝关节角度（余弦定理）
cos_θ2 = (x² + z² - L1² - L2²) / (2 * L1 * L2)
θ2 = atan2(±√(1 - cos_θ2²), cos_θ2)  // 通常取负值（膝关节向后弯）

// 再求髋关节角度
β = atan2(-x, -z)  // 注意符号取决于坐标系定义
γ = atan2(L2 * sin(θ2), L1 + L2 * cos(θ2))
θ1 = β - γ

这里有个关键点：膝关节角度通常有两个解（肘朝上 vs 肘朝下）。对于四足机器人，我们一般取“膝关节向后弯”的那个解，因为这样更符合生物力学，也更容易控制。

避坑指南： 我曾经在逆解里忘了处理 atan2 的象限问题，结果腿在某个区域突然反向旋转了180度。调试了一整天，最后发现是符号写反了。所以，写完逆解代码，一定要在仿真里画个圆轨迹跑一遍，看看关节角度是否连续变化。

3.4 腿部工作空间分析

工作空间，就是脚端能到达的所有点的集合。这个分析非常重要，它直接决定了你的机器人能走多高、跨多远、能不能在斜坡上站稳。

工作空间受三个因素限制：

关节角度限位： 每个电机都有物理限位，比如膝关节不能完全伸直
连杆长度： 大腿和小腿的长度决定了最大伸展距离
自碰撞： 腿不能打到身体或其他腿

我一般用蒙特卡洛法来可视化工作空间：随机生成大量关节角度组合，计算对应的脚端位置，然后画成点云。这样能直观地看到哪些区域是可达的，哪些是盲区。

// 伪代码：蒙特卡洛工作空间分析
for i = 1 to 10000:
    θ1 = random(θ1_min, θ1_max)
    θ2 = random(θ2_min, θ2_max)
    θ3 = random(θ3_min, θ3_max)
    [x, y, z] = forward_kinematics(θ1, θ2, θ3)
    if not self_collision_check(x, y, z):
        plot_point(x, y, z)

下面这张图展示了典型的四足机器人腿部工作空间：

从这张图你能看出几个关键信息：

工作空间不是完美的球形，而是被关节限位切掉了一部分
靠近身体的位置（上极限）可达区域较小，因为腿不能完全收拢
站立点通常在工作空间的中下部，这样既有向下踩的余量，也有向上抬的空间

重要提醒： 工作空间分析一定要在实物上验证。仿真里看着好好的位置，实物可能因为电机线缆拉扯、结构干涉等原因到不了。我吃过这个亏——仿真里规划了一条完美的轨迹，结果实物上一跑，线缆卡住了膝关节。

3.5 运动学在实物部署中的常见问题

最后，我总结几个在实物部署中经常遇到的运动学问题：

问题	现象	解决方案
坐标系定义不一致	仿真和实物腿的方向相反	统一使用右手定则，在代码里加坐标系可视化
关节零位偏移	腿站直时关节角度不为0	每次上电做一次零点校准
逆解多解选择错误	腿突然反向弯曲	根据当前关节角度选择最近的解
工作空间边界抖动	靠近极限时腿剧烈抖动	在逆解中加入软限位，预留安全边界

嗯，运动学这部分就讲到这里。说白了，正逆运动学就是四足机器人的“数学骨架”，DH参数法是建这个骨架的标准工具，工作空间分析则是告诉你这个骨架能摆出什么姿势。这三样东西搞扎实了，后面的步态规划、力控制才有根基。