2. 机器人运动学与动力学基础

阻抗/导纳控制的本质是调节机器人末端执行器与外部环境之间的动态关系，使其呈现期望的“质量-阻尼-刚度”特性。要实现这一目标，必须首先建立机器人末端位姿与关节运动之间的映射关系（运动学），以及关节力矩与末端力之间的传递关系（静力学与动力学）。本章将系统梳理齐次变换、雅可比矩阵、牛顿-欧拉方程和拉格朗日方程这四大数学工具，为后续阻抗/导纳控制器的设计提供完整的模型基础。

2.1 齐次变换：位置与姿态的统一描述

核心原理：齐次变换矩阵是机器人运动学的基础工具，它将旋转矩阵（3×3）与位置向量（3×1）组合为4×4矩阵，从而用统一的矩阵乘法描述坐标系的平移和旋转。对于n自由度串联机器人，从基座到末端的齐次变换矩阵可表示为：

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

其中每个T_(i-1)i由DH参数（连杆长度a、连杆转角α、关节距离d、关节角θ）决定。对于旋转关节，θ为变量；对于移动关节，d为变量。齐次变换矩阵的逆矩阵T⁻¹描述了从末端坐标系到基座坐标系的逆向映射，其计算可利用旋转矩阵的正交性简化：

T⁻¹ = [Rᵀ  -Rᵀ·p]
          [0    1   ]

判断方法与操作流程：在实际应用中，建立齐次变换矩阵需遵循以下步骤：首先，根据机器人结构确定各关节坐标系的原点和轴向；其次，填写DH参数表；然后，逐关节计算变换矩阵并连乘得到末端位姿。验证正解正确性的常用方法是：将各关节角置零，检查末端位姿是否与机械零位一致。

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

核心原理：雅可比矩阵J(θ)建立了关节速度θ̇与末端速度v（包括线速度v和角速度ω）之间的线性映射关系：

v = [v_x, v_y, v_z, ω_x, ω_y, ω_z]ᵀ = J(θ)·θ̇

雅可比矩阵的列向量对应每个关节对末端速度的贡献。对于旋转关节i，其雅可比列向量为：

J_i = [z_(i-1) × (p_e - p_(i-1))]
      [        z_(i-1)        ]

其中z_(i-1)为关节i的旋转轴方向，p_e为末端位置，p_(i-1)为关节i坐标系原点位置。根据虚功原理，关节力矩τ与末端力F之间存在对偶关系：

τ = Jᵀ(θ)·F

这一关系是阻抗控制中力/力矩转换的核心桥梁。

判断方法与操作流程：计算雅可比矩阵的流程为：第一步，确定各关节轴方向向量z_i；第二步，确定各关节坐标系原点到末端的位置向量；第三步，逐列计算旋转关节和移动关节对应的雅可比列；第四步，组装完整雅可比矩阵。在控制中需特别注意雅可比矩阵的奇异性——当det(J)=0时，机器人处于奇异位形，某些方向的速度无法实现，此时力映射也会产生无穷大关节力矩。

案例拆解：以六自由度工业机器人（如ABB IRB 120）为例，在典型装配任务中，末端需要沿Z轴施加50N的接触力。若当前关节角为θ=[0°, -30°, 60°, 0°, 45°, 0°]，计算得到雅可比矩阵的转置Jᵀ，其第3列（对应Z方向力）映射到各关节的力矩为：

τ = Jᵀ·[0, 0, 50, 0, 0, 0]ᵀ = [0, -12.5, 8.7, 0, -3.2, 0] N·m

该结果表明，末端Z方向的接触力主要由关节2和关节3承担，关节5产生较小的补偿力矩。若关节2的力矩限制为20N·m，则当前位形下可安全施加的最大Z向力约为80N。通过这种分析，可以在执行任务前预判力控制可行性。

2.3 牛顿-欧拉方程：递推形式的动力学建模

核心原理：牛顿-欧拉法基于刚体动力学的基本定律，通过“向外递推”计算各连杆的速度和加速度，再通过“向内递推”计算各关节的力和力矩。对于连杆i，其角速度、角加速度、线加速度递推公式为：

ω_(i+1) = R_i^(i+1)·ω_i + θ̇_(i+1)·z_(i+1)
α_(i+1) = R_i^(i+1)·α_i + θ̈_(i+1)·z_(i+1) + θ̇_(i+1)·(R_i^(i+1)·ω_i) × z_(i+1)
a_(i+1) = R_i^(i+1)·(a_i + α_i × p_(i+1) + ω_i × (ω_i × p_(i+1)))

向内递推计算关节力矩：

f_i = R_(i+1)^i·f_(i+1) + m_i·a_ci
n_i = R_(i+1)^i·n_(i+1) + I_i·α_i + ω_i × (I_i·ω_i) + p_ci × f_i
τ_i = n_iᵀ·z_i

判断方法与操作流程：牛顿-欧拉法的计算复杂度为O(n)，适合实时控制。实现步骤为：初始化基座的速度和加速度（通常为0和重力加速度g）；向外递推计算每个连杆的运动学量；向内递推计算每个关节的力和力矩。该方法可直接得到关节力矩τ，无需计算复杂的惯性矩阵M(θ)。

2.4 拉格朗日方程：能量视角的动力学建模

核心原理：拉格朗日方程基于系统能量（动能K和势能P）建立动力学模型：

L = K - P
d/dt(∂L/∂θ̇) - ∂L/∂θ = τ

对于n自由度机器人，展开后得到标准形式：

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

其中M(θ)为n×n惯性矩阵，C(θ,θ̇)为科氏力和离心力矩阵，G(θ)为重力项。拉格朗日法得到的模型具有明确的物理意义，且M(θ)对称正定，这一性质在阻抗控制稳定性分析中至关重要。

判断方法与操作流程：建立拉格朗日模型的步骤为：第一步，选择广义坐标（通常为关节角θ）；第二步，计算系统总动能K = Σ(1/2·m_i·v_ciᵀ·v_ci + 1/2·ω_iᵀ·I_i·ω_i)；第三步，计算系统总势能P = Σ(m_i·g·h_i)；第四步，代入拉格朗日方程求导得到动力学方程。对于复杂机器人，符号推导繁琐，通常借助符号计算工具（如MATLAB Symbolic Toolbox）完成。

案例拆解：考虑二连杆平面机器人，连杆长度l₁=l₂=0.5m，质量m₁=m₂=5kg，质心位于连杆中点。在关节空间进行阻抗控制时，期望关节阻抗为M_d=diag(2,2)kg·m²，B_d=diag(10,10)N·m·s/rad，K_d=diag(50,50)N·m/rad。拉格朗日法得到惯性矩阵：

M(θ) = [m₁l₁²/3 + m₂(l₁² + l₂²/3 + l₁l₂cosθ₂)   m₂(l₂²/3 + l₁l₂cosθ₂/2)]
        [m₂(l₂²/3 + l₁l₂cosθ₂/2)                  m₂l₂²/3                  ]

当θ₂=90°时，M(1,2)=m₂·l₂²/3=0.417kg·m²。若忽略科氏项，关节空间阻抗控制律为：

τ = M(θ)·M_d⁻¹·[B_d·(θ̇_d-θ̇) + K_d·(θ_d-θ)] + C(θ,θ̇)θ̇ + G(θ)

仿真结果表明，在阶跃期望位置指令下，系统响应超调量约5%，调节时间约0.8s，与设计指标一致。若将M_d增大至5kg·m²，系统响应变慢但稳定性裕度增加。

2.5 模型在阻抗/导纳控制中的综合应用

核心原理：阻抗控制与导纳控制的本质区别在于信息流方向。阻抗控制以位置为输入、力为输出，控制器呈现“阻抗”特性；导纳控制以力为输入、位置为输出，控制器呈现“导纳”特性。两种方式都需要精确的机器人模型作为基础：

• 阻抗控制（位置源）：需要逆动力学模型计算前馈力矩，利用M(θ)、C(θ,θ̇)、G(θ)补偿机器人自身动力学，使末端呈现期望阻抗。
• 导纳控制（力源）：需要正运动学模型将关节位置映射为末端位姿，并利用雅可比矩阵将末端期望速度映射为关节速度指令。

判断方法与操作流程：选择阻抗还是导纳控制，取决于机器人接口类型和任务需求。若机器人提供力矩接口（如协作机器人），优先选择阻抗控制；若机器人仅提供位置/速度接口（如传统工业机器人），则选择导纳控制。实现流程为：

1. 建立齐次变换矩阵，实现末端位姿与关节位置的相互转换；
2. 计算雅可比矩阵，实现速度与力的双向映射；
3. 利用牛顿-欧拉法或拉格朗日法建立动力学模型；
4. 根据控制架构（阻抗/导纳）设计控制律；
5. 在仿真环境中验证模型正确性和控制性能。