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 + 前饋、計算力矩控制）通過補償系統非線性，顯著提高軌跡跟蹤精度，是機器人實現復雜任務（如焊接、抓取、導航）的核心技術。實際應用中需權衡模型精度與計算效率，并結合自適應/魯棒控制處理不確定性。