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

3、运动学基础：四足机器人正运动学与逆运动学建模，D-H参数法，腿部工作空间分析

四足机器人的运动控制，本质上是对其腿部末端（足端）在三维空间中位置与姿态的精确管理。无论是实现稳定的对角小跑步态，还是完成跨越障碍的复杂动作，控制系统都必须首先回答两个核心问题：“我的脚应该放在哪里？”（逆运动学）和 “当前关节角度下，我的脚实际在哪里？”（正运动学）。本章将系统讲解四足机器人运动学的基础建模方法，重点围绕D-H参数法、正/逆运动学求解流程以及腿部工作空间的工程化分析展开，为后续的步态规划与力控制奠定坚实的数学与几何基础。

上图展示了本章的核心知识框架：从物理腿结构抽象出D-H参数模型，进而通过正运动学获得足端位姿，再基于此进行工作空间分析，最终服务于逆运动学求解。这四个环节环环相扣，构成了四足机器人运动控制的基础闭环。下面我们将逐一深入剖析。

3.1 核心原理：从物理腿到数学模型的抽象

四足机器人的每条腿通常由3-4个旋转关节串联组成。以最常见的3自由度（3-DOF）腿部构型为例，其关节配置一般为：髋关节横滚（Roll）、髋关节俯仰（Pitch）和膝关节俯仰（Pitch）。这种构型使得足端能够在三维空间中灵活运动，同时保持结构紧凑和重量分布合理。

运动学建模的第一步，是将物理关节抽象为数学坐标系之间的变换关系。这里我们采用经典的Denavit-Hartenberg（D-H）参数法。D-H法通过四个参数（连杆长度aᵢ₋₁、连杆扭转角αᵢ₋₁、连杆偏距dᵢ、关节角θᵢ）唯一确定相邻两个关节坐标系之间的齐次变换矩阵。对于旋转关节，θᵢ是变量，其余三个参数为常量。

以一条简化的腿部为例，其D-H参数表可定义如下：

关节 i	aᵢ₋₁ (mm)	αᵢ₋₁ (rad)	θᵢ (变量)	关节范围
1 (髋横滚)	0	0	θ₁	-30° ~ +30°
2 (髋俯仰)	L₁ (例如 50mm)	-π/2	θ₂	-45° ~ +90°
3 (膝俯仰)	L₂ (例如 200mm)	0	θ₃	-120° ~ 0°

其中，L₁为髋关节到膝关节的连杆长度，L₂为膝关节到足端的连杆长度。通过将相邻关节的变换矩阵A₁、A₂、A₃依次相乘，即可得到从基座坐标系到足端坐标系的齐次变换矩阵 T₀³ = A₁ · A₂ · A₃。该矩阵的左上角3×3子矩阵表示足端的姿态（旋转矩阵R），右上角3×1子向量表示足端的位置（平移向量P）。

工程要点：在建立D-H模型时，务必注意坐标系原点的选取和X轴方向的约定。不同的约定会导致参数符号和数值的差异，但最终计算出的足端位置是唯一的。建议在项目初期就固定使用一种约定（如标准D-H或改进D-H），并在代码注释中明确记录，避免后续团队协作时产生混乱。

3.2 正运动学求解：从关节空间到笛卡尔空间

正运动学（Forward Kinematics, FK）的任务是：给定三个关节角度（θ₁, θ₂, θ₃），计算足端在基座坐标系下的三维坐标（x, y, z）。这是所有运动学计算的基础，也是验证逆运动学结果正确性的“金标准”。

根据上述D-H参数表，我们可以推导出正运动学的解析表达式。设髋关节基座位于原点，X轴指向机器人前方，Y轴指向左侧，Z轴垂直向上。经过矩阵连乘并化简，足端位置可表示为：

x = cos(θ₁) * [L₂ * cos(θ₂ + θ₃) + L₁ * cos(θ₂)]
y = sin(θ₁) * [L₂ * cos(θ₂ + θ₃) + L₁ * cos(θ₂)]
z = -L₂ * sin(θ₂ + θ₃) - L₁ * sin(θ₂)

注意，这里的z轴方向为负值表示足端在基座下方（即地面方向）。在实际工程中，我们通常将基座坐标系定义在髋关节中心，而足端在站立时z坐标为负值。

正运动学的工程实现步骤：

参数初始化：在控制代码中定义连杆长度L₁、L₂以及关节角度偏移量（即关节零位对应的实际角度）。
角度输入：从电机编码器读取当前关节角度，并转换为弧度制。
矩阵计算：按照D-H变换矩阵公式，依次计算A₁、A₂、A₃。
结果提取：将T₀³矩阵中的平移向量部分提取出来，即为足端位置。
验证与标定：在机器人上电后，手动将腿摆至已知姿态（如直立状态），对比FK计算结果与实际测量值，进行零位标定。

实战建议：在嵌入式平台上，直接进行3×3矩阵乘法会消耗较多计算资源。对于3自由度腿，建议直接使用上述解析公式，避免矩阵运算。这可以将单腿FK计算时间从微秒级降至纳秒级，对于1kHz控制频率而言至关重要。

3.3 腿部工作空间分析：可达域与灵巧度

工作空间（Workspace）是指足端在基座坐标系下所有可达位置的集合。它是衡量机器人腿部运动能力的重要指标，直接影响步态规划中的足端落点选择。工作空间分析通常在正运动学基础上，通过蒙特卡洛法（Monte Carlo Method）进行数值求解。

蒙特卡洛法分析步骤：

随机采样：在关节角度范围内，使用均匀分布随机生成N组关节角度组合（N通常取10000~50000）。
正解计算：对每组角度执行正运动学计算，得到对应的足端位置点。
空间绘制：将所有位置点在三维空间中绘制出来，形成点云图，即为工作空间的可视化表示。
边界提取：通过凸包算法或体素化方法，提取工作空间的边界曲面，用于后续的碰撞检测和轨迹规划。

工作空间的形状和大小主要受以下因素影响：

影响因素	影响方式	工程优化方向
连杆长度 L₁, L₂	长度越大，工作空间半径越大，但惯量增加	在结构强度与动态性能间平衡
关节角度范围	范围越广，可达域越大，但可能产生奇异位形	根据步态需求设定合理限位
关节配置方式	横滚-俯仰-俯仰构型在侧向和前后方向分布不同	根据机器人用途选择构型
机体高度	基座高度影响工作空间在Z轴上的分布	结合机身设计确定安装位置

在实际工程中，我们不仅关注工作空间的“大小”，更关注其“形状”。例如，四足机器人站立时，足端工作空间应覆盖一个以髋关节投影为中心、半径约等于腿长的半球区域。在行走时，我们希望足端在身体前后方向有较大的活动范围，而在侧向方向可以适当缩小，以提高能效。

风险提示：工作空间边界附近的区域通常伴随关节速度趋近于无穷大的问题（即奇异位形）。例如，当膝关节完全伸直（θ₃=0）时，腿部处于奇异状态，此时足端在某个方向上的微小移动需要极大的关节角速度变化。在步态规划中，应避免将足端落点规划在工作空间边界附近，并设置安全裕度（通常保留10%~15%的边界余量）。

3.4 逆运动学求解：从笛卡尔空间到关节空间

逆运动学（Inverse Kinematics, IK）是运动控制的核心环节：给定期望的足端位置（x, y, z），求解对应的关节角度（θ₁, θ₂, θ₃）。对于3自由度腿部，我们可以采用解析法直接求解，计算速度快且精度高。

解析法求解步骤：

求解θ₁（髋横滚角）：根据几何关系，θ₁ = atan2(y, x)。该角度决定了足端在水平面上的投影方向。
求解等效平面内角度：将问题投影到腿部摆动平面内。定义r = sqrt(x² + y²)为水平距离，则从髋关节到足端的空间距离为 d = sqrt(r² + z²)。
求解θ₂（髋俯仰角）：利用余弦定理，计算髋关节到足端连线与大腿之间的夹角α，再计算该连线与水平面的夹角β，则 θ₂ = β - α（注意符号约定）。
求解θ₃（膝俯仰角）：同样利用余弦定理，计算膝关节处的夹角γ，则 θ₃ = π - γ（根据关节零位定义调整）。

具体公式如下（假设z轴向下为正，且θ₂、θ₃在零位时腿完全伸直向下）：

r = sqrt(x² + y²)
d = sqrt(r² + z²)
cos_γ = (L₁² + L₂² - d²) / (2 * L₁ * L₂)
γ = acos(clamp(cos_γ, -1, 1))
θ₃ = -(π - γ)   // 膝关节弯曲角度

α = atan2(L₂ * sin(γ), L₁ + L₂ * cos(γ))
β = atan2(z, r)
θ₂ = β - α

θ₁ = atan2(y, x)

需要注意的是，上述公式假设腿部在零位时垂直向下。实际机器人可能存在安装角度偏移，需要在公式中加入偏移量修正。

数值稳定性处理：在计算acos时，由于浮点误差，cos_γ可能略超出[-1, 1]范围。务必使用clamp函数将其限制在有效区间内，否则会产生NaN结果导致控制崩溃。此外，当d接近0（足端靠近髋关节正下方）时，atan2(y, x)可能不稳定，需要特殊处理。

3.5 案例拆解：基于运动学模型的足端轨迹跟踪实现

下面我们通过一个完整的工程案例，展示如何将上述运动学知识应用于实际控制中。

背景与目标：某四足机器人需要实现“原地踏步”动作，即足端在身体两侧交替抬起和落下。要求足端轨迹为矩形波，抬腿高度50mm，步长100mm，周期1s。控制器运行在1kHz频率下。

关键输入参数：

连杆长度：L₁ = 50mm，L₂ = 200mm
关节限位：θ₁ ∈ [-30°, 30°]，θ₂ ∈ [-45°, 90°]，θ₃ ∈ [-120°, 0°]
初始站立姿态：θ₁ = 0°，θ₂ = -30°，θ₃ = 60°（此时足端位于身体正下方）

实施步骤：

轨迹规划：在笛卡尔空间定义足端期望位置P_des(t)。抬腿阶段（0~0.5s），足端从初始位置P₀线性插值到抬升位置P₁（z方向+50mm）；落地阶段（0.5~1s），从P₁回到P₀。
逆运动学求解：在每个控制周期（1ms），将P_des(t)代入IK解析公式，计算目标关节角度θ_des。
平滑处理：对θ_des进行二阶低通滤波，避免加速度突变引起机械振动。
前馈+反馈控制：将θ_des发送至底层PD控制器，同时根据关节角速度计算前馈力矩，提高跟踪精度。
正运动学验证：在每个周期，读取实际关节角度，通过FK计算实际足端位置P_act，与P_des比较，计算跟踪误差。

结果与复盘：实测足端跟踪误差在X/Y方向小于2mm，Z方向小于1mm，满足设计要求。但在调试过程中发现，当足端接近工作空间边界时（如抬腿最高点），跟踪误差明显增大至5mm。分析原因是该位置接近膝关节奇异位形，导致关节速度需求过高。解决方案是调整轨迹规划，将抬腿高度从50mm降低至40mm，并优化了轨迹过渡曲线，最终误差控制在2mm以内。

经验总结：运动学模型是控制的基础，但实际系统还受到动力学、执行器带宽和结构柔性的影响。在工程中，建议将工作空间分析结果直接嵌入轨迹规划器，实时检查目标点是否在可达域内，并预留安全边界。同时，定期使用FK对IK结果进行交叉验证，是发现标定漂移和机械磨损的有效手段。

3.6 常见误区与总结复盘

在运动学建模与调试过程中，工程师常遇到以下问题：

误区/问题	现象	根因分析	解决方案
D-H参数符号错误	FK计算结果与实测位置偏差大	坐标系方向约定不一致	使用标准D-H约定，并用已知姿态验证
关节零位未标定	机器人站立姿态歪斜	编码器零位与机械零位不重合	上电后执行零点校准程序
IK多解选择不当	膝关节反弯（膝外翻）	未考虑关节限位和构型约束	在IK求解后增加限位检查与构型选择逻辑
工作空间边界未避让	足端轨迹跟踪时抖动	接近奇异位形	在轨迹规划中增加边界收缩因子
浮点精度问题	偶发NaN导致控制中断	acos输入越界	使用clamp函数并添加异常检测

本章总结：运动学建模是四足机器人控制的第一道门槛。正运动学提供了从关节到足端的“正向映射”，是感知和验证的基础；逆运动学提供了从足端到关节的“反向求解”，是轨迹执行的核心；工作空间分析则界定了机器人的物理能力边界，是安全运行的保障。三者相辅相成，构成了一个完整的运动学闭环。在实际项目中，建议按照“建模→验证→分析→求解→再验证”的流程推进，每一步都建立明确的量化指标，这样才能为后续的动力学控制和步态规划打下坚实基础。