1. 机器人控制的本质：通过关节扭矩执行轨迹

机器人控制的核心目标是让机器人关节精确跟踪期望轨迹 $(q_d,{\dot{q}}_d,{\ddot{q}}_d)$。为此，控制器需根据当前状态 $(q,\dot{q})$计算每个关节所需的扭矩 $\tau$，驱动电机产生该扭矩，使机器人产生预期运动。

关键点：

• 扭矩 $\tau$ 是控制输入，直接作用于机器人关节（如电机输出力矩）；
• 轨迹 $(q_d,{\dot{q}}_d,{\ddot{q}}_d)$ 是期望输出，由路径规划与时间参数化算法生成（如PRM/RRT + 多项式插值）；
• 控制算法的任务是弥合实际状态 $(q,\dot{q})$ 与期望轨迹的偏差，计算最优扭矩 $\tau$。

2. 机器人动力学模型：连接扭矩与运动的桥梁

机器人动力学模型描述了力/扭矩与运动之间的关系，是控制算法设计的基础。根据输入输出方向，分为两种形式：

2.1 正动力学（Forward Dynamics, FD）

• 数学表达式：$\ddot{q}=FD(\tau ;q,\dot{q})$
• 物理意义：已知当前状态 $(q,\dot{q})$ 和施加的扭矩 $\tau$，计算机器人的加速度 $\ddot{q}$。
• 应用场景：
  • 仿真：给定控制输入 $\tau$，预测机器人的运动轨迹（如MATLAB/SimMechanics中的动力学仿真）；
  • 自适应控制：在线估计系统参数（如质量、惯性矩阵）时需使用正动力学模型。

计算流程：

1. 从逆动力学方程 $\tau =M(q)\ddot{q}+C(q,\dot{q})\dot{q}+g(q)$ 中解出 $\ddot{q}$：
   $\ddot{q}=M(q{)}^{-1}\left(\tau -C(q,\dot{q})\dot{q}-g(q)\right)$
2. 通过数值积分（如欧拉法、Runge-Kutta法）得到速度 $\dot{q}$ 和位置 $q$。

2.2 逆动力学（Inverse Dynamics, ID）

• 数学表达式：$\tau =ID(\ddot{q};q,\dot{q})=M(q)\ddot{q}+C(q,\dot{q})\dot{q}+g(q)$
• 物理意义：已知当前状态 $(q,\dot{q})$ 和期望加速度 $\ddot{q}$，计算为实现该加速度所需的扭矩 $\tau$。
• 应用场景：
  • 前馈控制：直接计算抵消动力学效应的扭矩（见下文PD + 前馈控制）；
  • 轨迹规划：在生成轨迹时考虑动力学约束（如最大可实现加速度）。

方程各部分含义：

• 惯性矩阵项 $M(q)\ddot{q}$：抵抗加速度的惯性力，与关节位置 $q$ 相关（如机械臂伸展时惯性增大）；
• 科里奥利力与离心力项 $C(q,\dot{q})\dot{q}$：由关节间相对运动产生的力（如旋转手臂时的离心效应）；
• 重力项 $g(q)$：抵抗重力的力（如支撑机械臂自重所需的扭矩）。

3. 基于动力学模型的轨迹跟踪控制方法

3.1 PD控制（比例-微分控制）

• 控制律：$\tau =K_p(q_d-q)+K_d({\dot{q}}_d-\dot{q})$
• 特点：仅基于位置误差 $e_q=q_d-q$ 和速度误差 $e_{\dot{q}}={\dot{q}}_d-\dot{q}$计算扭矩，不依赖动力学模型。
• 局限性：
  • 无法补偿重力、惯性等非线性效应，跟踪精度有限；
  • 需增大增益 $K_p,K_d$ 提高响应速度，但可能导致系统不稳定。

3.2 PD + 前馈控制（PD + Feedforward）

• 控制律：$\tau =K_p{e}_q+K_d{e}_{\dot{q}}+ID({\ddot{q}}_d;q,\dot{q})$
• 工作原理：
  1. PD部分：提供基础反馈控制，抑制误差；
  2. 前馈部分：通过逆动力学模型 $ID$ 计算期望加速度 ${\ddot{q}}_d$ 所需的理论扭矩，补偿系统非线性。
• 优势：
  • 结合反馈与前馈，显著提高跟踪精度（尤其在高速运动时）；
  • 可处理重力、惯性等复杂动力学效应。

3.3 计算力矩控制（Computed Torque Control）

• 控制律：
  $\tau =M(q){\ddot{q}}_r+C(q,\dot{q})\dot{q}+g(q)$
  其中 ${\ddot{q}}_r={\ddot{q}}_d+K_p{e}_q+K_d{e}_{\dot{q}}$为修正后的期望加速度。
• 物理意义：
  • 通过逆动力学计算扭矩，将系统动态特性转化为单位质量系统（即 $\ddot{q}={\ddot{q}}_r$）；
  • 误差动态 ${\ddot{e}}_q+K_d{\dot{e}}_q+K_p{e}_q=0$可设计为二阶系统，通过选择 $K_p,K_d$调整响应速度与稳定性。
• 优势：
  • 理论上可实现精确线性化和完全跟踪（需精确动力学模型）；
  • 适用于高精度任务（如机器人手术、精密装配）。

4. 动力学模型的挑战与解决方案

4.1 模型不确定性

• 问题：实际机器人参数（如质量分布、摩擦系数）与建模值存在偏差，导致控制精度下降。
• 解决方案：
  • 自适应控制：在线估计不确定参数（如 $M(q)$、$C(q,\dot{q})$）并更新控制律；
  • 鲁棒控制：设计对参数扰动不敏感的控制器（如滑模控制）。

4.2 计算复杂度

• 问题：逆动力学计算涉及矩阵求逆和大量非线性运算，实时性要求高。
• 解决方案：
  • 高效算法：使用递归牛顿-欧拉算法（RNEA）降低计算复杂度（从 $O(n^3)$ 降至 $O(n)$，$n$ 为关节数）；
  • 硬件加速：利用GPU/FPGA并行计算动力学模型。

5. 应用案例：工业机械臂轨迹跟踪

• 场景：机械臂需精确跟踪焊接轨迹，速度 $1m/s$，定位误差 < 0.1mm。
• 控制方案：
  1. 轨迹生成：PRM规划路径，5阶多项式时间参数化生成 $(q_d,{\dot{q}}_d,{\ddot{q}}_d)$；
  2. 控制输入计算：
     $$\tau =M(q)({\ddot{q}}_d+K_p{e}_q+K_d{e}_{\dot{q}})+C(q,\dot{q})\dot{q}+g(q)$$
  3. 执行：电机根据 ( \tau ) 输出力矩，编码器实时反馈 $(q,\dot{q})$。
• 效果：
  • 前馈项补偿重力和惯性力，PD项抑制外界干扰；
  • 高速运动时误差仍可控制在0.05mm以内。

总结：动力学模型与控制的协同作用

机器人动力学模型（正/逆动力学）是连接控制输入（扭矩 $\tau$）与期望输出（轨迹 $q_d$）的桥梁。基于逆动力学的控制方法（如PD + 前馈、计算力矩控制）通过补偿系统非线性，显著提高轨迹跟踪精度，是机器人实现复杂任务（如焊接、抓取、导航）的核心技术。实际应用中需权衡模型精度与计算效率，并结合自适应/鲁棒控制处理不确定性。