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

3. 运动学与动力学基础

人形机器人的全身控制，本质上是对高维刚体系统在约束与外力作用下的运动与力进行精确调控。无论是双足行走、全身协同操作，还是受到扰动后的动态平衡恢复，其底层都依赖于对运动学与动力学的深刻理解。本章作为从基础理论迈向全身控制算法的桥梁，将系统梳理刚体运动学、正逆运动学求解、雅可比矩阵的构造与应用，以及牛顿-欧拉与拉格朗日两种主流动力学建模方法，并重点引入人形机器人特有的浮基动力学建模思想。这些内容构成了后续全身动力学控制（WBC）、模型预测控制（MPC）以及状态估计的数学基石。

上图展示了本章的知识体系结构。运动学部分解决“机器人各部分在哪里、以多快速度运动”的问题，而动力学部分则回答“需要多大的力/力矩才能实现期望的运动”。浮基动力学建模则是将人形机器人区别于固定基座机械臂的核心环节，它将机器人本体视为一个漂浮在空间中的基座，通过足底或手部与环境的接触力来驱动全身运动。下面我们将逐一深入剖析。

3.1 刚体运动学与姿态描述

3.1.1 核心原理：位置、姿态与齐次变换

刚体运动学描述的是物体在空间中的位置和姿态随时间的变化关系，不涉及产生运动的力。对于人形机器人，每个连杆（如大腿、小腿、躯干）都可以视为一个刚体。描述一个刚体的完整位姿需要6个独立参数：3个位置参数（x, y, z）和3个姿态参数（通常用欧拉角、旋转矩阵或四元数表示）。

在实际工程中，我们使用齐次变换矩阵 T 将旋转和平移统一在一个4×4矩阵中：

T = [ R   p ]
    [ 0   1 ]

其中 R 是3×3旋转矩阵，p 是3×1位置向量。对于人形机器人，我们通常在世界坐标系（World Frame）和机器人本体坐标系（Body Frame）之间进行变换。例如，躯干坐标系相对于世界坐标系的变换矩阵 ^WT_B 描述了机器人当前的位置和朝向。

工程经验：在全身控制中，姿态表示优先选择四元数而非欧拉角。欧拉角存在万向锁问题，且插值不自然；四元数虽然直观性差，但计算稳定、无奇异，适合用于状态估计和控制反馈。

3.1.2 操作流程：从关节角到末端位姿

正运动学（Forward Kinematics, FK）是指已知各关节角度，求解末端执行器（如手、脚）在世界坐标系中的位姿。对于人形机器人，通常采用改进的DH参数法或指数积公式（POE）建立运动学模型。POE公式基于旋量理论，更适合处理树状结构的浮基机器人。

定义坐标系：为每个连杆建立固连坐标系，确定基座坐标系（通常设在腰部或骨盆中心）。
确定关节类型：人形机器人主要包含旋转关节（髋、膝、肩、肘）和少量直线关节（如有）。
计算变换矩阵：逐级相乘得到末端相对于基座的变换矩阵 ^BT_E。
叠加浮基变换：最终末端在世界坐标系中的位姿为 ^WT_E = ^WT_B · ^BT_E。

逆运动学（Inverse Kinematics, IK）则是已知期望的末端位姿，反解关节角度。由于人形机器人是冗余系统（全身自由度通常超过30个），IK通常采用数值优化方法（如阻尼最小二乘法、Levenberg-Marquardt算法）或解析+数值混合方法。在全身控制中，IK往往不是独立求解，而是作为动力学优化问题的一部分。

3.1.3 案例拆解：双足支撑时的运动学约束

考虑人形机器人双足站立时的情形。此时双脚均与地面接触，形成闭链约束。若仅使用正运动学，我们可以计算出任意时刻双脚的位置和姿态。但若给定一个期望的躯干运动轨迹，需要同时满足左脚和右脚的接触约束，此时IK问题变为一个带约束的优化问题。

约束类型	数学表达	物理意义
位置约束	p_foot = p_fixed	脚掌与地面接触点不滑动
姿态约束	R_foot = R_ground	脚掌平面与地面平行
速度约束	v_foot = 0	接触点瞬时速度为零

在实际控制器中，我们通常将双脚的接触约束作为硬约束（必须满足），而将躯干跟踪、重心高度等作为软约束（最小化误差）。这种分层处理方式在后续的全身控制（WBC）中会详细展开。

3.2 雅可比矩阵：速度与力的映射

3.2.1 核心原理：从关节空间到任务空间

雅可比矩阵 J 是机器人学中最重要的概念之一。它建立了关节速度 q̇ 与末端笛卡尔速度 v 之间的线性映射：

v = J(q) · q̇

其中 v 是6维向量（3个线速度 + 3个角速度），q̇ 是n维关节速度向量。雅可比矩阵的维度为6×n。对于人形机器人，由于存在浮基，雅可比矩阵需要分为浮动基座部分和关节部分：

v = J_B · v_B + J_q · q̇

其中 v_B 是基座的空间速度（6维），J_B 和 J_q 分别是对应的雅可比子矩阵。

雅可比矩阵的另一重要性质是力/力矩对偶关系：

τ = J^T · F

即末端施加的力 F 通过雅可比矩阵的转置映射为关节力矩 τ。这一关系在全身力控制中至关重要——我们通过控制关节力矩来间接控制末端与环境的接触力。

3.2.2 判断方法：奇异位形与可操作性

当雅可比矩阵不满秩时，机器人处于奇异位形。此时机器人会丧失某个方向上的运动能力，且关节速度可能趋于无穷大。对于人形机器人，常见的奇异位形包括：

肘部完全伸展：手臂失去径向运动能力。
膝盖完全伸直：腿部失去弯曲方向的控制能力，这在行走中非常危险。
髋关节内旋极限：影响腿部摆动自由度。

风险提示：在行走控制中，必须避免膝关节完全锁死。一旦进入奇异位形，不仅控制力矩会急剧增大，还可能导致机构损坏或摔倒。工程上通常会在关节限位之前设置软约束，并在优化目标中加入奇异规避项。

3.2.3 实战场景：全身力控制中的雅可比应用

在全身动力学控制中，我们经常需要将期望的末端力（如脚底反作用力）映射为关节力矩。假设我们期望左脚产生一个垂直向上的力 F_z = 500N，同时保持脚掌水平。通过雅可比矩阵的转置，我们可以计算出所需的关节力矩：

τ_leg = J_contact^T · F_desired

但这里有一个关键问题：人形机器人是冗余驱动的，多个关节组合可以产生相同的末端力。因此，我们需要在满足力约束的前提下，优化关节力矩的分配，使得各关节力矩尽量小且均匀，避免某个电机过载。这通常通过二次规划（QP）求解。

3.3 动力学建模：牛顿-欧拉与拉格朗日方法

3.3.1 核心原理：两种建模路径的对比

动力学建模描述的是关节力矩与运动（加速度、速度、位置）之间的关系。两种主流方法各有优劣：

特性	牛顿-欧拉法	拉格朗日法
基本原理	牛顿第二定律 + 欧拉方程	能量守恒（动能-势能）
推导方式	递推形式（外推-内推）	闭式方程（全局）
计算复杂度	O(n)，效率高	O(n³)，矩阵运算多
物理直观性	强，每步有明确物理意义	较弱，偏数学化
适用场景	实时控制、逆动力学	系统分析、仿真验证

牛顿-欧拉法通过从基座到末端的外推计算各连杆的速度和加速度，再从末端到基座的内推计算各关节的力和力矩。其递推形式非常适合计算机实现，计算效率高，是实时控制的首选。

拉格朗日法通过定义拉格朗日函数 L = T - V（动能减势能），然后应用欧拉-拉格朗日方程得到动力学方程。最终形式为：

M(q)q̈ + C(q, q̇)q̇ + G(q) = τ

其中 M(q) 是惯性矩阵，C(q, q̇) 是科氏力和离心力矩阵，G(q) 是重力项。这种形式便于进行系统分析和控制律设计。

3.3.2 操作流程：构建人形机器人动力学模型

以牛顿-欧拉法为例，构建人形机器人逆动力学模型的步骤如下：

运动学初始化：从基座开始，计算每个连杆的角速度、角加速度、线加速度。
外推（Outward）：从基座到末端，利用关节运动计算每个连杆的惯性力和力矩。
内推（Inward）：从末端到基座，计算每个关节所需的驱动力矩，同时考虑外部接触力。
叠加浮基效应：由于基座是浮动的，需要额外计算基座本身的加速度和惯性效应。

  关键点：对于人形机器人，浮基的存在使得动力学方程变为：

  M(q)q̈ + C(q,q̇)q̇ + G(q) = τ + J_c^T F_c

  其中 J_c 是接触点的雅可比矩阵，F_c 是接触力（如地面反作用力）。这个方程是全身控制的核心模型。

3.3.3 案例拆解：单腿支撑相的动力学校验

假设人形机器人处于单腿支撑状态（另一条腿正在摆动）。我们已知期望的关节轨迹 q(t)，需要计算所需的关节力矩。这是一个典型的逆动力学问题。

输入数据：

机器人质量分布：大腿 8kg，小腿 3kg，躯干 30kg，摆动腿质量 11kg。
期望运动：支撑腿髋关节角度从 10° 到 -5° 变化，持续 0.8s。
地面反作用力：通过力传感器实时测量，假设垂直力 600N，水平力 20N。

分析过程：使用牛顿-欧拉递推算法，从支撑脚开始向上计算。首先计算支撑腿各连杆的惯性力，然后考虑躯干和摆动腿的耦合效应。关键步骤是正确处理摆动腿产生的反作用力矩对躯干的影响。

结果：计算得到支撑腿髋关节需要输出约 85Nm 的力矩，膝关节约 45Nm。与实测电机电流换算的力矩对比，误差在 5% 以内，验证了模型的准确性。

复盘：模型误差主要来源于关节摩擦和柔性传动部件的未建模动态。在后续控制中，可以通过增加摩擦补偿项或使用扰动观测器来抑制这些误差。

3.4 浮基动力学建模

3.4.1 核心原理：浮动基座的特殊性

与固定基座机械臂不同，人形机器人没有物理锚点。其基座（通常定义在骨盆或腰部）在空间中自由漂浮，具有6个自由度（3个平移 + 3个旋转）。这使得动力学方程的结构发生根本变化：

[ M_B   M_Bq ] [ q̈_B ]   [ C_B ]   [ G_B ]   [ 0   ]   [ J_cB^T ]
[ M_qB  M_q  ] [ q̈   ] + [ C_q ] + [ G_q ] = [ τ   ] + [ J_cq^T ] F_c

其中 q̈_B 是基座的6维加速度，q̈ 是关节加速度。注意基座部分没有直接的驱动力矩（τ_B = 0），只能通过关节力矩和接触力间接控制基座运动。

这一特性决定了人形机器人控制的本质：我们无法直接“固定”躯干，只能通过调整关节和接触力来间接控制躯干的姿态和位置。这也是为什么人形机器人的平衡控制如此具有挑战性。

3.4.2 建模方法：从固定基座到浮基的扩展

浮基动力学建模通常采用以下步骤：

定义广义坐标：选择基座的6个自由度（位置 + 姿态）加上所有关节角度，构成完整的广义坐标向量。
计算质量矩阵：使用复合刚体算法（Composite Rigid Body Algorithm）计算6+n维的质量矩阵。
处理接触约束：将足底接触视为约束，通过拉格朗日乘子或约束雅可比矩阵引入接触力。
化简与降维：利用接触约束消去部分自由度，得到降阶的动力学方程。

在实际工程中，我们通常不直接使用完整的浮基动力学方程进行控制，而是采用质心动力学（Centroidal Dynamics）进行简化。质心动力学将整个机器人等效为一个位于质心处的单刚体，大大降低了计算复杂度，同时保留了平衡控制的核心特性。

3.4.3 实战场景：双足行走中的浮基动力学应用

在双足行走控制中，浮基动力学模型被用于计算所需的足底接触力。假设机器人正在以 1.0 m/s 的速度行走，步频为 2 步/秒。控制器需要实时计算每个脚底的三维接触力，以保证：

质心轨迹跟踪期望的ZMP（零力矩点）轨迹。
躯干姿态保持水平。
摆动腿按规划轨迹运动。

通过浮基动力学模型，我们可以将上述目标转化为一个优化问题：

minimize  || q̈_desired - q̈ ||² + || F_c ||²
subject to 动力学方程约束
            接触力在摩擦锥内
            关节力矩在限幅内

这个优化问题通常以 1kHz 的频率求解，输出关节力矩指令。实际运行中，还需要考虑通信延迟、传感器噪声和执行器带宽等因素，因此控制器中会加入鲁棒项和滤波环节。

常见误区：初学者容易将浮基动力学与固定基座动力学混用，直接使用固定基座的逆动力学公式计算关节力矩。这会导致躯干姿态发散，因为忽略了基座加速度对关节力矩的影响。务必在建模时显式包含基座的6个自由度。

3.5 总结与工程要点

本章内容是人形机器人全身控制的数学基础。核心要点总结如下：

模块	关键公式/方法	工程应用
刚体运动学	齐次变换矩阵、四元数	状态估计、轨迹规划
正/逆运动学	DH参数、数值IK	末端定位、步态生成
雅可比矩阵	v = J·q̇, τ = J^T·F	力控制、奇异规避
牛顿-欧拉动力学	递推算法	实时逆动力学计算
拉格朗日动力学	M(q)q̈ + C + G = τ	系统分析、控制器设计
浮基动力学	含接触力的扩展方程	全身控制、平衡控制

在实际开发中，建议采用模块化建模思路：先建立精确的浮基动力学模型用于离线仿真验证，再根据实时性要求简化模型用于在线控制。同时，务必通过实验数据（如力传感器、IMU、关节编码器）对模型参数进行辨识和校准，因为模型误差是控制性能的主要瓶颈之一。

  工程建议：在全身控制器的初始调试阶段，建议先使用拉格朗日法建立完整的动力学模型进行仿真，验证算法的正确性；然后切换到牛顿-欧拉法实现实时计算。同时，保留一个简化的质心动力学模型作为备份，用于快速原型验证和故障排查。