2、机器人运动学与动力学基础：齐次变换、雅可比矩阵、牛顿-欧拉与拉格朗日动力学方程

工业机器人惯性参数辨识的核心目标，是获取关节空间或笛卡尔空间下描述连杆质量、质心位置、惯性张量等物理量的精确数值。这些参数无法直接测量，必须通过运动激励与力/力矩观测间接估计。而这一切的前提，是建立机器人运动学与动力学的完备数学描述。本章从齐次变换出发，建立位置与姿态的传递规则；随后引入雅可比矩阵，将关节速度映射为末端速度；最后对比牛顿-欧拉递推与拉格朗日能量法两种动力学建模路径，为后续惯性参数线性化与最小二乘辨识奠定基础。

上图展示了本章知识的内在逻辑：齐次变换解决“位姿如何描述与传递”，雅可比矩阵解决“速度与力的映射”，而动力学方程则回答“力矩与运动、惯性参数之间的关系”。三者层层递进，共同构成惯性参数辨识的理论基石。

2.1 齐次变换：位姿描述与坐标传递

2.1.1 核心原理

齐次变换矩阵是一个 4×4 矩阵，将旋转矩阵 R（3×3）与平移向量 p（3×1）统一在一个矩阵中，形式为：

T = [ R   p ]
    [ 0   1 ]

其中 R 描述坐标系之间的姿态关系，p 描述原点之间的平移关系。对于 n 自由度串联机器人，从基座到末端的齐次变换矩阵通过链式乘法获得：

T_0n = T_01 · T_12 · … · T_(n-1)n

每个相邻连杆变换 T_(i-1)i 通常由 DH 参数（连杆长度 a、连杆转角 α、关节距离 d、关节角 θ）构造。DH 参数法的核心思想是用四个参数唯一确定相邻坐标系间的变换关系，其中旋转关节的 θ 为变量，移动关节的 d 为变量。

2.1.2 操作流程

1. 为每个连杆建立固连坐标系，遵循 DH 约定（Z 轴沿关节轴线，X 轴沿公垂线方向）。
2. 确定四个 DH 参数：a_i、α_i、d_i、θ_i。
3. 构造相邻连杆变换矩阵 A_i = Rot(z, θ_i) · Trans(z, d_i) · Trans(x, a_i) · Rot(x, α_i)。
4. 将各 A_i 依次相乘得到末端位姿 T_0n。

2.1.3 案例拆解：六轴机器人正运动学计算

以典型六轴串联工业机器人为例，其 DH 参数表如下：

当给定一组关节角（如 θ = [0°, 30°, -60°, 0°, 90°, 0°]），通过逐级相乘 A₁A₂A₃A₄A₅A₆，即可得到末端法兰盘的位姿矩阵。该矩阵中的旋转部分可用于提取末端姿态（如欧拉角），平移部分直接给出末端位置坐标。在实际辨识实验中，正运动学用于将关节角数据转换为末端轨迹，为激励轨迹设计提供依据。

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

2.2.1 核心原理

雅可比矩阵 J(q) 是关节速度到末端笛卡尔速度的线性映射：

v = J(q) · q̇

其中 v = [v_x, v_y, v_z, ω_x, ω_y, ω_z]ᵀ 为末端线速度与角速度向量，q̇ 为关节速度向量。雅可比矩阵的维度为 6×n（n 为关节数）。对于六轴机器人，J 为 6×6 方阵。雅可比矩阵的构造方法分为几何法与解析法：几何法通过各关节运动对末端速度的贡献逐列构造，解析法则对正运动学方程求偏导。

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

τ = Jᵀ · F

其中 τ 为关节力矩向量，F 为末端施加的广义力（力与力矩）。这一关系在辨识中用于将末端传感器测得的力/力矩转换到关节空间。

2.2.2 判断方法与操作流程

• 几何雅可比构造：对于旋转关节 i，其对末端线速度的贡献为 z_{i-1} × (p_n - p_{i-1})，对角速度的贡献为 z_{i-1}。其中 z_{i-1} 为关节 i 的 Z 轴方向，p_n 为末端位置，p_{i-1} 为关节 i 原点位置。
• 奇异位形判断：计算 det(J)，当行列式接近零时，机器人处于奇异位形，此时某些方向的速度无法实现，辨识激励轨迹应避开这些位形。
• 力映射应用：若末端装有六维力/力矩传感器，关节力矩 τ = JᵀF，该值可直接与动力学模型中的力矩预测值对比。

2.2.3 实战场景：利用雅可比进行可观测性分析

在惯性参数辨识中，激励轨迹的设计需要保证观测矩阵满秩。观测矩阵的构建依赖于雅可比矩阵及其导数。具体而言，当采用有限傅里叶级数作为激励轨迹时，需要计算每个采样时刻的雅可比矩阵，并组装成回归矩阵。若某些关节角组合导致雅可比矩阵降秩，则对应的参数不可辨识。因此，在轨迹优化阶段，通常将雅可比条件数作为优化目标之一，确保机器人运动远离奇异位形。

2.3 牛顿-欧拉动力学方程

2.3.1 核心原理

牛顿-欧拉法基于牛顿第二定律与欧拉方程，通过两次递推完成动力学计算：

• 向外递推（1→n）：从基座开始，逐连杆计算角速度、角加速度、线加速度，进而计算每个连杆的惯性力与惯性力矩。
• 向内递推（n→1）：从末端执行器开始，逐关节计算约束力与驱动力矩，最终得到各关节所需力矩。

对于连杆 i，其牛顿方程与欧拉方程分别为：

f_i = m_i · a_ci
n_i = I_ci · α_i + ω_i × (I_ci · ω_i)

其中 m_i 为连杆质量，I_ci 为连杆质心处的惯性张量，a_ci 为质心线加速度，α_i 为角加速度，ω_i 为角速度。

2.3.2 操作流程

1. 初始化基座的运动状态（通常为静止或匀速）。
2. 向外递推：利用关节运动（q, q̇, q̈）计算各连杆的角速度、角加速度、线加速度。
3. 计算各连杆的惯性力与惯性力矩。
4. 向内递推：从末端开始，考虑外力/外力矩，逐关节计算约束力与驱动力矩。

2.3.3 案例拆解：二连杆平面机器人的牛顿-欧拉计算

考虑一个平面二连杆机器人，连杆 1 和连杆 2 的质量分别为 m₁、m₂，质心位于连杆中点，惯性张量分别为 I₁、I₂。给定关节角 q = [30°, 45°]，角速度 q̇ = [0.5, 0.3] rad/s，角加速度 q̈ = [0.2, 0.1] rad/s²。通过牛顿-欧拉递推，可得到关节 1 和关节 2 的驱动力矩 τ₁、τ₂。该计算过程虽然繁琐，但每一步物理意义明确，且易于编程实现。在实际辨识中，牛顿-欧拉法常被用于生成动力学回归矩阵的数值形式。

2.4 拉格朗日动力学方程

2.4.1 核心原理

拉格朗日法基于能量守恒原理，定义拉格朗日函数 L = K - P（动能减势能），然后通过欧拉-拉格朗日方程推导动力学方程：

d/dt(∂L/∂q̇) - ∂L/∂q = τ

对于 n 自由度机器人，拉格朗日方程可整理为标准形式：

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

其中 M(q) 为惯性矩阵，C(q, q̇) 为科氏力与离心力矩阵，G(q) 为重力项。该形式在惯性参数辨识中具有重要价值，因为 M、C、G 均可表示为惯性参数的线性函数。

2.4.2 线性化与回归方程

将动力学方程改写为关于惯性参数向量 θ 的线性回归形式：

τ = Y(q, q̇, q̈) · θ

其中 Y 为回归矩阵（n×m），θ 为 m 维惯性参数向量（包含质量、质心位置、惯性张量分量）。这一线性化是参数辨识的核心步骤。对于六轴机器人，惯性参数总数通常为 10×6 = 60 个（每个连杆 10 个参数），但通过重组可减少到最小惯性参数集（通常为 30-40 个）。

2.4.3 案例拆解：单连杆摆的拉格朗日建模

以单连杆摆为例，设连杆质量 m，质心距关节距离 l_c，惯性张量 I，关节角 θ。动能 K = ½Iθ̇² + ½m(l_cθ̇)²，势能 P = mgl_c(1-cosθ)。代入拉格朗日方程得：

(I + ml_c²)θ̈ + mgl_c·sinθ = τ

定义 θ₁ = I + ml_c²（等效惯量），θ₂ = ml_c（等效质量矩），则方程变为 τ = θ₁θ̈ + θ₂g·sinθ。这正是线性回归形式 Y = [θ̈, g·sinθ]，θ = [θ₁, θ₂]ᵀ。通过采集多组 (τ, θ̈, sinθ) 数据，即可用最小二乘法估计 θ₁、θ₂。

2.5 两种动力学建模方法的对比与选择

在实际惯性参数辨识中，两种方法最终都能得到 τ = Y(q, q̇, q̈)·θ 的线性形式。牛顿-欧拉法更适合编程实现，拉格朗日法更适合理论推导与最小参数集分析。许多现代辨识工具（如 MATLAB Robotics Toolbox）内部采用牛顿-欧拉递推生成回归矩阵，但对外提供统一的线性化接口。

2.6 本章总结与复盘

本章建立了机器人运动学与动力学的基础框架，为惯性参数辨识提供了必要的数学工具。齐次变换解决了位姿描述问题，雅可比矩阵建立了速度与力的映射，牛顿-欧拉法与拉格朗日法分别从力平衡和能量角度推导了动力学方程。核心要点如下：

• 齐次变换矩阵 T 是位姿传递的基本载体，DH 参数是串联机器人建模的标准方法。
• 雅可比矩阵 J 不仅用于速度映射，还通过 τ = JᵀF 实现力映射，并可用于奇异位形判断。
• 牛顿-欧拉法通过两次递推高效计算关节力矩，适合编程实现。
• 拉格朗日法从能量角度推导动力学方程，自然呈现线性回归结构 τ = Y·θ，是参数辨识的理论基础。
• 两种建模方法最终统一于线性化形式，关键在于正确构造回归矩阵 Y 并确定最小惯性参数集。

在下一阶段的辨识工作中，我们将基于本章的动力学线性模型，设计激励轨迹、采集数据并进行参数估计。理解本章内容是确保辨识结果准确可靠的前提。