知道目標角度之後，手臂為什麼還是會把零件「撞壞」？

知道目標角度之後，手臂為什麼還是會把零件「撞壞」？

入門篇我們解決了「末端該到哪、各關節該轉幾度」的問題：正向／逆向運動學給了座標，雅可比連起了速度。但如果你真的把一支六軸手臂搬到產線上，照著逆向運動學算出的角度直接命令它去「把軸插進孔裡」，結果往往是——零件被撞歪、夾爪卡死、甚至力矩過載跳機。

問題出在哪裡？逆向運動學只告訴你「終點」的關節角，卻沒說從起點到終點該怎麼走；它假設世界是剛性的、感測是完美的、模型是準確的。但真實的裝配任務裡，孔位有 $\pm 0.1\,\text{mm}$ 的公差、零件與孔壁會發生接觸（contact）、相機回傳的位置帶著雜訊。本篇要補上入門篇刻意略過、卻是真正決定「裝得進去 vs 撞壞」的三層：軌跡生成（trajectory generation）、力／順應控制（force / compliance control）、以及狀態估計（state estimation）。這三層才是「把零件精準放進孔裡」這句話背後真正困難的部分。

軌跡生成：從「到哪裡」到「怎麼走」

逆向運動學給的是離散的關節角目標，但馬達需要的是每一個時刻的位置、速度與加速度指令 $\big(\mathbf{q}(t),\ \dot{\mathbf{q}}(t),\ \ddot{\mathbf{q}}(t)\big)$。直接在兩個關節角之間「跳過去」會產生無限大的加速度，物理上等同於要求無限大的力矩（回想 $\boldsymbol{\tau} = \mathbf{M}\ddot{\mathbf{q}} + \cdots$）。我們必須生成一條平滑（smooth） 的軌跡。

最常用的是五次多項式（quintic polynomial） 插值。為什麼是五次？因為我們要在起點與終點同時指定位置、速度與加速度三個邊界條件，兩端共六個約束，恰好需要六個係數：

$$ q(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3 + a_4 t^4 + a_5 t^5 $$

對應的速度與加速度為：

$$ \dot{q}(t) = a_1 + 2a_2 t + 3a_3 t^2 + 4a_4 t^3 + 5a_5 t^4 $$ $$ \ddot{q}(t) = 2a_2 + 6a_3 t + 12a_4 t^2 + 20a_5 t^3 $$

若要求在 $t=0$ 與 $t=T$ 時速度、加速度皆為零（起步與停止都平順），代入邊界條件可解出漂亮的閉式係數。令總位移 $\Delta q = q_f - q_0$，則：

$$ a_0 = q_0,\quad a_1 = 0,\quad a_2 = 0 $$ $$ a_3 = \frac{10\,\Delta q}{T^3},\quad a_4 = -\frac{15\,\Delta q}{T^4},\quad a_5 = \frac{6\,\Delta q}{T^5} $$

這條曲線的加速度在兩端為零、中段才達到峰值，避開了會激發機構振動的「急動（jerk，即加速度的微分 $\dddot{q}$）」突跳。事實上「平滑」的工程定義往往就是有界的 jerk——這也是為什麼高階軌跡規劃會進一步用七次多項式或 S 形速度曲線（S-curve）來限制 jerk。

值得連到物理直覺：五次多項式的加速度峰值出現在 $t = T/2$，大小為：

$$ \ddot{q}_{\max} = \frac{10\,|\Delta q|}{\sqrt{3}\,T^2} \approx \frac{5.77\,|\Delta q|}{T^2} $$

這個式子告訴你一件很實際的事——想讓動作快一倍（$T$ 減半），所需的加速度與力矩會變成四倍（$T^2$ 在分母）。這正是工業手臂的循環時間（cycle time）為什麼那麼難壓縮：受限的不是「算不出路徑」，而是馬達能輸出的峰值力矩。

接觸的世界：為什麼純位置控制注定失敗

入門篇的控制器都是位置控制（position control）：給定目標角，控制器拚命把誤差壓到零。在自由空間裡這沒問題，但裝配的瞬間，零件碰到了孔壁。

想像孔位實際座標與相機估計差了 $0.2\,\text{mm}$，而你的位置控制器剛度極高（增益 $K_p$ 很大）。當零件被孔壁擋住、無法到達指令位置時，控制器看到「位置誤差還沒消除」，於是持續加大力矩去推。產生的接觸力近似於一個等效彈簧：

$$ F = K_{\text{eff}} \cdot \delta x $$

其中 $\delta x$ 是位置誤差、$K_{\text{eff}}$ 是系統的等效剛度。剛度越高、誤差越大，接觸力就越暴力——這就是零件被撞壞的力學根源。純位置控制在接觸任務裡的致命缺陷是：它把「位置不到位」誤判為「該更用力」。

解法是改變思維：在「會接觸的方向」上，我們不該控制位置，而該控制力或控制順應性（compliance）。

阻抗控制：讓手臂像一條可調的彈簧

阻抗控制（impedance control） 的核心思想由 Neville Hogan 提出：不要直接命令位置或力，而是命令末端對外力的動態反應——讓它表現得像一個「質量–彈簧–阻尼」系統。期望的動態關係寫成：

$$ \mathbf{M}_d\,\ddot{\tilde{\mathbf{x}}} + \mathbf{D}_d\,\dot{\tilde{\mathbf{x}}} + \mathbf{K}_d\,\tilde{\mathbf{x}} = \mathbf{F}_{\text{ext}} $$

其中 $\tilde{\mathbf{x}} = \mathbf{x} - \mathbf{x}_d$ 是末端相對期望位置的偏差，$\mathbf{F}_{\text{ext}}$ 是環境施加的外力，$\mathbf{M}_d$、$\mathbf{D}_d$、$\mathbf{K}_d$ 分別是我們自行設計的期望慣量、阻尼、剛度矩陣。

這條式子的威力在於：當零件碰到孔壁（$\mathbf{F}_{\text{ext}} \neq \mathbf{0}$）時，手臂不會死命硬推，而是像彈簧一樣順著力的方向退讓 $\tilde{\mathbf{x}} = \mathbf{K}_d^{-1}\mathbf{F}_{\text{ext}}$，自動把零件「滑」進孔裡。把 $\mathbf{K}_d$ 調小，手臂就「軟」；調大就「硬」。工程師因此可以針對不同方向設定不同剛度——例如插孔方向（z 軸）保持較硬以維持插入力，而垂直平面（x、y）刻意調軟以容許對位誤差被孔的倒角（chamfer）自動修正。

要在關節層實現這個期望阻抗，控制力矩透過雅可比轉置（入門篇學過的 $\boldsymbol{\tau} = \mathbf{J}^\top \mathbf{F}$ 對偶）映射回去：

$$ \boldsymbol{\tau} = \mathbf{J}^\top(\mathbf{q})\Big[\mathbf{K}_d\,(\mathbf{x}_d - \mathbf{x}) + \mathbf{D}_d\,(\dot{\mathbf{x}}_d - \dot{\mathbf{x}})\Big] + \mathbf{g}(\mathbf{q}) $$

注意這裡仍保留重力補償 $\mathbf{g}(\mathbf{q})$，否則手臂會因自重下垂破壞設定的剛度。這正是為什麼力控裝配的手臂通常需要關節力矩感測器或精確的動力學模型——光靠位置回授是做不到的。

力／位置混合控制與順應裝置

更系統化的框架是 Mason 提出的力／位置混合控制（hybrid force/position control）：把任務空間用一個選擇矩陣（selection matrix） $\mathbf{S}$ 拆成兩組正交的子空間——在「自由方向」控制位置，在「受限方向」控制力。

$$ \boldsymbol{\tau} = \mathbf{J}^\top\Big[\underbrace{\mathbf{S}\,\mathbf{F}_{\text{pos}}}_{\text{位置控制方向}} + \underbrace{(\mathbf{I}-\mathbf{S})\,\mathbf{F}_{\text{force}}}_{\text{力控制方向}}\Big] $$

以軸插孔為例：垂直插入的 z 方向受孔壁限制，用力控維持穩定插入力（如 $5\,\text{N}$）；而 x、y 平面是自由的，用位置控做對位搜尋。$\mathbf{S}$ 就是這個「哪個方向管位置、哪個方向管力」的開關。

在硬體上，還有一個經典的純機械解：遠端順應中心（Remote Center of Compliance, RCC）。它是一組巧妙排列的彈性元件，使夾爪的「順應中心」落在零件尖端。當零件斜著碰到孔的倒角時，接觸力產生的力矩會讓 RCC 被動地 旋轉與平移，把零件自動對正——完全不需要感測器或運算。這是「用機構智慧取代控制智慧」的優雅範例，至今仍廣泛用於高速裝配線。

看一個例子

設一支力控手臂執行軸插孔，z 方向（插入方向）的期望剛度 $K_z = 5000\,\text{N/m}$，x、y 方向刻意調軟成 $K_{xy} = 500\,\text{N/m}$。相機把孔位估計成 $(x_d, y_d)$，但實際孔位在 x 方向偏了 $\delta x = 0.4\,\text{mm} = 0.0004\,\text{m}$。

若用高剛度位置控制（假設 $K_{\text{eff}} = 50000\,\text{N/m}$），零件撞上孔壁產生的側向力為：

$$ F_x = K_{\text{eff}} \cdot \delta x = 50000 \times 0.0004 = 20\,\text{N} $$

對小零件而言，$20\,\text{N}$ 的橫向力足以使其變形或卡死。

改用阻抗控制、x 方向剛度設成 $K_{xy} = 500\,\text{N/m}$ 後，同樣偏差只產生：

$$ F_x = K_{xy} \cdot \delta x = 500 \times 0.0004 = 0.2\,\text{N} $$

側向力降為原本的 $1/100$。零件得以沿孔的倒角輕柔滑入，而非被硬推。同時 z 方向維持較高剛度以保證 $5\,\text{N}$ 的插入力——剛柔並濟，正是力控裝配的精髓。

插入力的軌跡核對： 若期望插入力 $F_z^d = 5\,\text{N}$、z 方向阻尼 $D_z = 200\,\text{N·s/m}$，當接觸初瞬末端以 $\dot{z} = 0.01\,\text{m/s}$ 接近，瞬時接觸力約：

$$ F_z \approx K_z \cdot \Delta z + D_z \cdot \dot{z} $$

阻尼項 $D_z \dot{z} = 200 \times 0.01 = 2\,\text{N}$ 的存在能吸收接觸瞬間的衝擊能量，避免「砰」一下的硬碰撞——這就是為什麼期望阻抗一定要包含阻尼 $\mathbf{D}_d$ 而非只有剛度。

狀態估計：相機與編碼器都在說謊

前面所有控制律都假設我們知道 $\mathbf{x}$、$\dot{\mathbf{x}}$、$\mathbf{q}$。但真實感測器都帶雜訊：相機位置估計可能抖動 $\pm 0.3\,\text{mm}$、關節編碼器有量化誤差、力感測器漂移。若直接把含雜訊的量測拿去微分求速度，雜訊會被放大到完全不可用。

解法是狀態估計（state estimation），最經典的工具是 Kalman 濾波器（Kalman filter）。它的思想是：把「物理模型的預測」與「感測器的量測」依各自的不確定度做最佳加權融合。系統模型寫成：

$$ \mathbf{x}_{k} = \mathbf{A}\,\mathbf{x}_{k-1} + \mathbf{B}\,\mathbf{u}_{k} + \mathbf{w}_k,\qquad \mathbf{w}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}) $$ $$ \mathbf{z}_{k} = \mathbf{H}\,\mathbf{x}_{k} + \mathbf{v}_k,\qquad \mathbf{v}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{R}) $$

其中 $\mathbf{Q}$ 是過程雜訊協方差、$\mathbf{R}$ 是量測雜訊協方差。濾波分兩步循環。預測（predict）：

$$ \hat{\mathbf{x}}_k^- = \mathbf{A}\,\hat{\mathbf{x}}_{k-1} + \mathbf{B}\,\mathbf{u}_k $$ $$ \mathbf{P}_k^- = \mathbf{A}\,\mathbf{P}_{k-1}\,\mathbf{A}^\top + \mathbf{Q} $$

更新（update），核心是 Kalman 增益（Kalman gain） $\mathbf{K}_k$：

$$ \mathbf{K}_k = \mathbf{P}_k^- \mathbf{H}^\top \big(\mathbf{H}\,\mathbf{P}_k^- \mathbf{H}^\top + \mathbf{R}\big)^{-1} $$ $$ \hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + \mathbf{K}_k \big(\mathbf{z}_k - \mathbf{H}\,\hat{\mathbf{x}}_k^-\big) $$ $$ \mathbf{P}_k = (\mathbf{I} - \mathbf{K}_k \mathbf{H})\,\mathbf{P}_k^- $$

Kalman 增益的物理意義非常直觀：當量測很準（$\mathbf{R}$ 小）時，$\mathbf{K}_k$ 趨近於把量測完全採信；當量測很吵（$\mathbf{R}$ 大）時，$\mathbf{K}_k$ 趨近於零，系統更信任物理模型的預測。 它在「相信感測器」與「相信物理」之間自動找到最佳平衡點——這也是 Kalman 濾波被稱為「最小均方誤差估計」的原因。

對機器人裝配而言，這層讓我們得以融合相機（絕對位置但慢且吵）、編碼器（快但只有關節角）、力感測器（接觸資訊）三種異質訊號，得到一個比任何單一感測器都更可靠的末端位姿估計。沒有這層，前面的阻抗控制再漂亮，餵進去的位置都是錯的。

重點回顧

1. 逆向運動學只給終點，不給路徑。軌跡生成用五次多項式等平滑曲線指定每一時刻的 $\mathbf{q}, \dot{\mathbf{q}}, \ddot{\mathbf{q}}$，並以「有界 jerk」為平滑的工程標準；動作加速一倍，所需力矩變四倍。
2. 純位置控制在接觸任務必敗：它把「位置不到位」誤判為「該更用力」，使接觸力 $F = K_{\text{eff}}\,\delta x$ 失控放大。
3. 阻抗控制讓手臂變成可調彈簧 $\mathbf{M}_d\ddot{\tilde{\mathbf{x}}} + \mathbf{D}_d\dot{\tilde{\mathbf{x}}} + \mathbf{K}_d\tilde{\mathbf{x}} = \mathbf{F}_{\text{ext}}$，把對位誤差方向調軟、插入方向調硬，零件得以順著倒角滑入。
4. 力／位置混合控制用選擇矩陣 $\mathbf{S}$ 拆分自由方向（控位置）與受限方向（控力）；硬體上的 RCC 則用純機構被動完成對正。
5. 狀態估計是控制的前提：Kalman 濾波器以 $\mathbf{Q}$、$\mathbf{R}$ 為權重融合物理預測與感測量測，Kalman 增益自動在「信感測」與「信模型」間取得最佳平衡。

深入探討（研究所視角）

把上述三層再往前推，會碰到當代機器人研究最活躍的幾個戰場：

接觸豐富任務的最佳化規劃。 當任務涉及反覆接觸（如多孔裝配、組裝家具），系統動態會在「接觸」與「分離」之間切換，形成混合動態系統（hybrid dynamical system）。直接用梯度法規劃會卡在不可微的接觸切換點。當前主流是把接觸建模成互補約束（complementarity constraints）——法向力 $\lambda_n \geq 0$ 與穿透距離 $\phi \geq 0$ 滿足 $\lambda_n \cdot \phi = 0$（接觸時力非零、分離時距離非零，兩者不會同時為正），並以此構成接觸隱含軌跡最佳化（contact-implicit trajectory optimization）。這是讓機器人「自己想出該怎麼推、怎麼夾」的數學基礎。

模型預測控制（MPC）。 入門篇的計算力矩控制只看「當下誤差」，而 MPC 在每個時步求解一個有限時域的最佳化問題：

$$ \min_{\mathbf{u}_{0:N-1}} \ \sum_{k=0}^{N-1} \Big(\|\mathbf{x}_k - \mathbf{x}_k^{\text{ref}}\|_{\mathbf{Q}}^2 + \|\mathbf{u}_k\|_{\mathbf{R}}^2\Big) \quad \text{s.t. 動力學與約束} $$

它能在規劃時就把力矩上限、關節極限、避障與接觸力上限當作硬約束直接納入，是當前足式機器人與動態操作的主力框架，代價是每個控制週期都要解一個非線性最佳化（因此計算即時性是核心挑戰）。

估計層的延伸。 線性 Kalman 濾波對非線性系統（機器人運動學充滿三角函數）並不適用，實務改用擴展 Kalman 濾波（EKF） 或無跡 Kalman 濾波（UKF）；在視覺定位裡則演化為因子圖（factor graph） 與滑動視窗最佳化，這也是現代 SLAM 與 visual-inertial odometry 的骨幹。

學習與物理先驗的融合。 近年強化學習與模仿學習在接觸密集裝配（如 NIST 的 peg-in-hole benchmark）取得突破，但純資料驅動的策略樣本效率低且難保證安全。最有前景的方向是殘差學習（residual learning）：以本篇的阻抗控制作為已知的、安全的基準策略，僅用神經網路學習「模型沒涵蓋的那一小部分修正量」。這呼應了入門篇結尾的判斷——無論方法多新，運動學、動力學與估計理論始終是不可繞過的物理先驗。能把經典骨架與資料驅動方法接起來的人，才真正站在這個領域的前沿。