機械手臂為什麼能精準把零件放進孔裡？

機械手臂為什麼能精準把零件放進孔裡？

想像一條工業機械手臂（robotic manipulator），要把一顆螺絲精準地放進距離自己 80 公分外的孔位。馬達只知道每個關節「轉了幾度」，但工程師關心的卻是末端那支夾爪（end-effector）此刻在三維空間的哪個座標、朝向哪個方向。從「關節角度」到「空間位置」之間的這層翻譯，正是機器人運動學（kinematics）要解決的核心問題；而要讓手臂沿著我們想要的路徑平順移動，又必須加上控制（control）這一層。本篇就從這個看似簡單、實則牽涉大量幾何與線性代數的任務出發，把運動學與機器人控制的骨架建立起來。

從關節空間到工作空間：座標轉換的語言

機器人最自然的描述變數是各個關節的角度或位移，我們把它們收集成一個向量，稱為關節空間（joint space）的座標：

$$ \mathbf{q} = \begin{bmatrix} q_1 & q_2 & \cdots & q_n \end{bmatrix}^\top $$

對於旋轉關節（revolute joint），$q_i$ 是轉角 $\theta_i$；對於平移關節（prismatic joint），$q_i$ 是滑移距離 $d_i$。而我們真正在意的末端位姿（pose），則活在工作空間（task space） 或稱笛卡兒空間（Cartesian space）裡，需要同時描述位置與姿態。

要在三維空間中同時記錄「平移」與「旋轉」，工程上採用 $4\times4$ 的齊次變換矩陣（homogeneous transformation matrix）：

$$ \mathbf{T} = \begin{bmatrix} \mathbf{R} & \mathbf{p} \\ \mathbf{0}^\top & 1 \end{bmatrix} = \begin{bmatrix} r_{11} & r_{12} & r_{13} & p_x \\ r_{21} & r_{22} & r_{23} & p_y \\ r_{31} & r_{32} & r_{33} & p_z \\ 0 & 0 & 0 & 1 \end{bmatrix} $$

其中 $\mathbf{R} \in SO(3)$ 是 $3\times3$ 的旋轉矩陣（rotation matrix），描述姿態；$\mathbf{p} \in \mathbb{R}^3$ 是位置向量。旋轉矩陣的兩個關鍵性質是正交且行列式為 1：

$$ \mathbf{R}^\top \mathbf{R} = \mathbf{I}, \qquad \det(\mathbf{R}) = 1 $$

齊次矩陣最迷人的地方在於它的可組合性：若 $\mathbf{T}_1^0$ 描述座標系 1 相對於座標系 0，$\mathbf{T}_2^1$ 描述座標系 2 相對於座標系 1，則直接矩陣相乘即可串接：

$$ \mathbf{T}_2^0 = \mathbf{T}_1^0 \, \mathbf{T}_2^1 $$

這正是整條手臂能被「一節一節接起來」的數學基礎，也呼應了物理裡剛體運動可以分解為平移與旋轉的觀念。

正向運動學：給我關節角，還你末端位姿

正向運動學（forward kinematics, FK） 回答的問題是：已知所有關節變數 $\mathbf{q}$，末端的位姿 $\mathbf{T}$ 是多少？這個問題有唯一解，且可由連桿座標系逐節串接得到。

業界最通用的描述方式是 Denavit–Hartenberg（DH）參數法。它用四個參數 $(a_i, \alpha_i, d_i, \theta_i)$ 描述相鄰兩連桿之間的幾何關係：$a_i$ 為連桿長度、$\alpha_i$ 為連桿扭角、$d_i$ 為連桿偏移、$\theta_i$ 為關節角。每一節的變換矩陣寫成：

$$ \mathbf{A}_i = \begin{bmatrix} \cos\theta_i & -\sin\theta_i \cos\alpha_i & \sin\theta_i \sin\alpha_i & a_i \cos\theta_i \\ \sin\theta_i & \cos\theta_i \cos\alpha_i & -\cos\theta_i \sin\alpha_i & a_i \sin\theta_i \\ 0 & \sin\alpha_i & \cos\alpha_i & d_i \\ 0 & 0 & 0 & 1 \end{bmatrix} $$

整條 $n$ 軸手臂的正向運動學就是把每節串起來：

$$ \mathbf{T}_n^0(\mathbf{q}) = \mathbf{A}_1 \mathbf{A}_2 \cdots \mathbf{A}_n $$

以最簡單的平面二連桿手臂（兩個旋轉關節，連桿長 $l_1$、$l_2$，關節角 $\theta_1$、$\theta_2$）為例，末端位置有漂亮的閉式解：

$$ x = l_1 \cos\theta_1 + l_2 \cos(\theta_1 + \theta_2) $$ $$ y = l_1 \sin\theta_1 + l_2 \sin(\theta_1 + \theta_2) $$

注意第二項用的是 $\theta_1 + \theta_2$，因為第二節的角度是相對於第一節「累加」上去的——這正是齊次矩陣串接在背後默默幫我們處理好的角度疊加。

逆向運動學：給我目標位置，還你關節角

實務中我們通常反過來：「我要末端到 $(x, y, z)$、朝某個方向」，請問各關節該轉幾度？這就是逆向運動學（inverse kinematics, IK），難度遠高於 FK，原因有三：

1. 非線性：方程式中充滿三角函數，無法簡單線性求解。
2. 多解：同一個目標位置，手臂可能有「肘朝上」與「肘朝下」等多組解。
3. 無解或奇異：若目標超出工作空間（workspace），則根本沒有解。

仍以平面二連桿為例，給定目標 $(x, y)$，可用餘弦定理求出 $\theta_2$：

$$ \cos\theta_2 = \frac{x^2 + y^2 - l_1^2 - l_2^2}{2 l_1 l_2} $$

$$ \theta_2 = \pm \arccos\!\left( \frac{x^2 + y^2 - l_1^2 - l_2^2}{2 l_1 l_2} \right) $$

正負號分別對應肘朝上與肘朝下兩組解。接著回代求 $\theta_1$：

$$ \theta_1 = \operatorname{atan2}(y, x) - \operatorname{atan2}\big(l_2 \sin\theta_2,\; l_1 + l_2 \cos\theta_2\big) $$

這裡務必使用 $\operatorname{atan2}$ 而非 $\arctan$，因為前者能正確處理四個象限，避免角度落在錯誤的範圍。對於六軸以上的複雜手臂，閉式解往往不存在，只能改用下一節要談的雅可比矩陣做數值迭代求解。

雅可比矩陣：把速度連起來

當手臂要沿著連續軌跡運動時，我們關心的不只是位置，更是速度。雅可比矩陣（Jacobian matrix） $\mathbf{J}(\mathbf{q})$ 正是連接關節速度與末端速度的橋樑：

$$ \dot{\mathbf{x}} = \mathbf{J}(\mathbf{q}) \, \dot{\mathbf{q}} $$

其中 $\dot{\mathbf{x}}$ 是末端的線速度與角速度（合稱 twist），$\dot{\mathbf{q}}$ 是各關節角速度。雅可比是位姿對關節變數的偏微分：

$$ J_{ij} = \frac{\partial x_i}{\partial q_j} $$

對平面二連桿手臂，位置雅可比為：

$$ \mathbf{J} = \begin{bmatrix} -l_1 \sin\theta_1 - l_2 \sin(\theta_1 + \theta_2) & -l_2 \sin(\theta_1 + \theta_2) \\ \;\;\,l_1 \cos\theta_1 + l_2 \cos(\theta_1 + \theta_2) & \;\;\,l_2 \cos(\theta_1 + \theta_2) \end{bmatrix} $$

若要從末端期望速度反推關節速度，需對雅可比求逆：

$$ \dot{\mathbf{q}} = \mathbf{J}^{-1}(\mathbf{q}) \, \dot{\mathbf{x}} $$

但當 $\det(\mathbf{J}) = 0$ 時，矩陣不可逆，這個位形稱為奇異點（singularity）。在奇異點上，手臂會「失去一個自由度」，需要極大的關節速度才能產生微小的末端運動，控制器可能因此飽和甚至失控。雅可比也決定了力矩與外力的關係（靜力學對偶）：

$$ \boldsymbol{\tau} = \mathbf{J}^\top(\mathbf{q}) \, \mathbf{F} $$

也就是說，同一個雅可比既描述速度傳遞，也描述力的傳遞——這個對偶關係在機器人力控制（force control）中極為關鍵。

動力學與控制：讓手臂真的動起來

知道「該轉到哪」還不夠，馬達輸出的是力矩（torque），必須透過動力學把力矩與運動連起來。機器人的運動方程式可由拉格朗日法導出，寫成標準形式：

$$ \mathbf{M}(\mathbf{q}) \ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}}) \dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau} $$

其中 $\mathbf{M}(\mathbf{q})$ 是慣量矩陣、$\mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})\dot{\mathbf{q}}$ 是科氏力與離心力項、$\mathbf{g}(\mathbf{q})$ 是重力項，$\boldsymbol{\tau}$ 是關節力矩。這條方程式本質上是牛頓第二定律 $\mathbf{F} = m\mathbf{a}$ 在多連桿系統的推廣，只是質量變成了會隨位形改變的慣量矩陣。

最直觀的控制策略是比例-微分（PD）控制，讓實際關節角 $\mathbf{q}$ 追上期望角 $\mathbf{q}_d$：

$$ \boldsymbol{\tau} = \mathbf{K}_p (\mathbf{q}_d - \mathbf{q}) + \mathbf{K}_d (\dot{\mathbf{q}}_d - \dot{\mathbf{q}}) $$

$\mathbf{K}_p$ 是比例增益（像彈簧把手臂拉向目標），$\mathbf{K}_d$ 是微分增益（像阻尼器抑制振盪）。但 PD 控制忽略了重力與耦合，會有穩態誤差。更精緻的做法是計算力矩控制（computed-torque control），利用動力學模型把非線性項先抵消掉：

$$ \boldsymbol{\tau} = \mathbf{M}(\mathbf{q})\big[\ddot{\mathbf{q}}_d + \mathbf{K}_d \dot{\mathbf{e}} + \mathbf{K}_p \mathbf{e}\big] + \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) $$

其中誤差 $\mathbf{e} = \mathbf{q}_d - \mathbf{q}$。這個控制律巧妙地把非線性系統「線性化」成一組獨立的二階誤差方程 $\ddot{\mathbf{e}} + \mathbf{K}_d \dot{\mathbf{e}} + \mathbf{K}_p \mathbf{e} = \mathbf{0}$，只要把 $\mathbf{K}_p$、$\mathbf{K}_d$ 選成正定，誤差就會指數收斂到零。

看一個例子

設一支平面二連桿手臂，連桿長 $l_1 = 0.5\,\text{m}$、$l_2 = 0.3\,\text{m}$，目前關節角 $\theta_1 = 30^\circ$、$\theta_2 = 45^\circ$。

正向運動學——末端在哪裡？

$$ x = 0.5\cos 30^\circ + 0.3\cos 75^\circ = 0.5(0.866) + 0.3(0.259) = 0.510\,\text{m} $$ $$ y = 0.5\sin 30^\circ + 0.3\sin 75^\circ = 0.5(0.5) + 0.3(0.966) = 0.540\,\text{m} $$

末端目前位於 $(0.510,\ 0.540)\,\text{m}$。

逆向運動學——若想讓末端到 $(0.6,\ 0.4)$ 呢？

先求 $\theta_2$：

$$ \cos\theta_2 = \frac{0.6^2 + 0.4^2 - 0.5^2 - 0.3^2}{2(0.5)(0.3)} = \frac{0.36 + 0.16 - 0.25 - 0.09}{0.30} = \frac{0.18}{0.30} = 0.6 $$

$$ \theta_2 = \arccos(0.6) = 53.1^\circ \quad (\text{取肘朝上解}) $$

再求 $\theta_1$：

$$ \theta_1 = \operatorname{atan2}(0.4, 0.6) - \operatorname{atan2}\big(0.3\sin 53.1^\circ,\; 0.5 + 0.3\cos 53.1^\circ\big) $$ $$ = 33.7^\circ - \operatorname{atan2}(0.240,\ 0.680) = 33.7^\circ - 19.4^\circ = 14.3^\circ $$

所以要把末端送到 $(0.6, 0.4)$，需把關節角設為 $\theta_1 \approx 14.3^\circ$、$\theta_2 \approx 53.1^\circ$。

檢查奇異點： 當 $\theta_2 = 0^\circ$ 或 $180^\circ$ 時，$\sin\theta_2 = 0$，此時雅可比的行列式 $\det(\mathbf{J}) = l_1 l_2 \sin\theta_2 = 0$，手臂完全伸直或完全摺疊，落入奇異位形。本例 $\theta_2 = 53.1^\circ$ 遠離奇異點，控制上是安全的。

重點回顧

1. 運動學分兩個方向：正向運動學（FK）由關節角算末端位姿，有唯一解；逆向運動學（IK）由目標位姿反推關節角，可能多解、無解或落在奇異點。
2. 齊次變換矩陣 $4\times4$ 同時封裝旋轉 $\mathbf{R}$ 與平移 $\mathbf{p}$，並以矩陣相乘串接各連桿，DH 參數法是業界標準的系統化描述方式。
3. 雅可比矩陣 $\mathbf{J}$ 連接關節速度與末端速度（$\dot{\mathbf{x}} = \mathbf{J}\dot{\mathbf{q}}$），其轉置 $\mathbf{J}^\top$ 又連接力與力矩；$\det(\mathbf{J}) = 0$ 處為奇異點，須避開。
4. 逆向運動學算角度時務必用 $\operatorname{atan2}$，才能正確涵蓋四個象限。
5. 控制層面：PD 控制簡單但有穩態誤差；計算力矩控制利用動力學模型抵消非線性項，能達成誤差指數收斂。

深入探討（研究所視角）

進入研究層級，運動學與控制會延伸出幾個更深的方向：

冗餘機器人與虛逆。 當關節自由度 $n$ 大於任務維度 $m$（例如七軸手臂執行六維任務），雅可比是長方形矩陣，$\mathbf{J}^{-1}$ 不存在。此時改用 Moore–Penrose 虛逆（pseudo-inverse）：

$$ \dot{\mathbf{q}} = \mathbf{J}^{+} \dot{\mathbf{x}} + (\mathbf{I} - \mathbf{J}^{+}\mathbf{J})\,\dot{\mathbf{q}}_0 $$

其中 $\mathbf{J}^{+} = \mathbf{J}^\top(\mathbf{J}\mathbf{J}^\top)^{-1}$。第一項給出最小範數解，第二項是零空間（null space） 投影，可在不影響末端運動的前提下，額外執行避障、避奇異或關節極限等次要任務——這是冗餘機器人靈巧度的數學來源。

奇異點的工程處理。 在奇異點附近直接求虛逆會因條件數爆炸而產生極大關節速度。實務上採用阻尼最小平方法（damped least squares, DLS）：

$$ \dot{\mathbf{q}} = \mathbf{J}^\top (\mathbf{J}\mathbf{J}^\top + \lambda^2 \mathbf{I})^{-1} \dot{\mathbf{x}} $$

阻尼因子 $\lambda$ 在奇異點附近犧牲少許精度以換取數值穩定，是工業控制器的標準防護。

姿態表示的選擇。 旋轉矩陣有 9 個元素卻只有 3 個自由度，存在冗餘。歐拉角（Euler angles）會遭遇萬向鎖（gimbal lock），因此進階應用多採用四元數（quaternion） 或李群 $SE(3)$ 的指數座標（exponential coordinates），後者也催生了現代的旋量理論（screw theory） 與乘積指數公式（product of exponentials, PoE），讓運動學描述更為簡潔且無奇異參數化問題。

控制的前沿。 從計算力矩控制再往前，包含對模型不確定性具強健性的滑模控制（sliding mode control）、能線上估計未知參數的自適應控制（adaptive control），以及處理接觸與外力的阻抗／導納控制（impedance / admittance control）。近年資料驅動方法（如強化學習與模仿學習）也逐漸用於難以精確建模的柔性與接觸密集任務，但無論方法多新，背後仍以本篇的運動學與動力學方程作為物理先驗。理解這套經典骨架，才有能力判斷哪些是真正的進步、哪些只是換了包裝。