Learning Objectives
- 親手算出兩軸手臂的正運動學與逆運動學
- 說明一個位置為什麼有兩組關節角度，以及工作空間的形狀
- 認出奇異點，說明手臂接近伸直時為什麼難控制
- 分辨關節空間與笛卡兒空間的移動，用逐點逆解讓手走直線
手要到杯子那裡，馬達要轉幾度
杯子放在桌上，相機告訴你它在手臂基座前方 35 公分、左邊 20 公分。可是手臂的馬達不懂公分，它只能轉關節角度。所以機器手臂一定要會兩種換算：
| 問題 | 輸入 | 輸出 | 名稱 |
|---|---|---|---|
| 關節這樣轉，手會在哪裡 | 關節角度 | 末端位置 | 正運動學（forward kinematics） |
| 手要到那裡，關節要轉幾度 | 末端位置 | 關節角度 | 逆運動學（inverse kinematics） |
這一課用最簡單的手臂：平面上兩段，長度 L1 ＝ 0.3 公尺、L2 ＝ 0.25 公尺，像人的上臂和前臂都貼在桌面上。q1 是第一段相對 x 軸的角度，q2 是第二段相對第一段的角度，逆時針為正（和 REP-103 的右手定則一致）。
正運動學很直接，一段一段接起來。肘的位置是第一段的終點，第二段的方向是兩個角度相加：
肘 ＝ (L1 × cos q1, L1 × sin q1)
x ＝ L1 × cos q1 ＋ L2 × cos(q1 ＋ q2)
y ＝ L1 × sin q1 ＋ L2 × sin(q1 ＋ q2)
用數字走一次：q1 ＝ 30°、q2 ＝ 45°，第二段的方向是 75°。
x ＝ 0.3 × cos 30° ＋ 0.25 × cos 75° ＝ 0.2598 ＋ 0.0647 ＝ 0.3245
y ＝ 0.3 × sin 30° ＋ 0.25 × sin 75° ＝ 0.1500 ＋ 0.2415 ＝ 0.3915
一步一步算正運動學
程式裡的角度一律是弧度，math.radians 把度換成弧度。第三組就是上面算的 30°、45°；第四組只把 q2 改成負的，看末端差多少。
逆運動學：一個位置，兩組解
反過來算比較麻煩，因為有兩個未知數而且都在 cos、sin 裡面。兩軸手臂有漂亮的解法，用幾何：基座、肘、末端是一個三角形，三個邊長分別是 L1、L2 和基座到末端的距離 r。三邊都知道，夾角就能用餘弦定理算出來：
r² ＝ x² ＋ y²
cos q2 ＝（r² − L1² − L2²）÷（2 × L1 × L2）
q2 ＝ ± acos(cos q2)
q1 ＝ atan2(y, x) − atan2(L2 × sin q2, L1 ＋ L2 × cos q2)
用剛才的目標算 (0.35, 0.2)：
r² ＝ 0.1225 ＋ 0.04 ＝ 0.1625
cos q2 ＝（0.1625 − 0.09 − 0.0625）÷（2 × 0.3 × 0.25）＝ 0.01 ÷ 0.15 ＝ 0.0667
q2 ＝ ±86.2°
正負號就是兩組解。 同一個三角形可以往上翻或往下翻，就像你的手碰桌上同一個點，手肘可以抬高也可以放低。q2 為正時，肘在基座到末端連線的順時針側（目標在 x 軸正方向時就是下面，肘下）；q2 為負時在另一側（肘上）。兩組解末端完全相同，但手臂佔的空間不同：旁邊有障礙物時，只有一組能用。
一步一步算逆運動學，再用正運動學驗算
逆解算完一定要驗算：把角度代回正運動學，應該回到目標。最後一行試一個到不了的點。
工作空間與奇異點
工作空間（workspace）是末端碰得到的範圍。兩軸手臂是一個環形：外圈半徑 L1 ＋ L2 ＝ 0.55（伸直），內圈半徑 |L1 − L2| ＝ 0.05（完全折疊）。圈外太遠，圈內太近，都到不了，公式的表現就是 cos q2 超出 −1 到 1，直接呼叫 acos 會丟出例外。程式一定要先檢查，手臂收到無效的指令時應該拒絕並回報，而不是亂動。
奇異點（singularity）出現在 sin q2 ＝ 0：手臂完全伸直或完全折疊，兩組解在這裡合而為一。兩個關節各自造成的末端移動變成同一條線上的方向，所以有一個方向動不了。實務上危險的不是剛好踩在奇異點，而是接近它：
| q2 | 末端以 0.1 m/s 往最難的方向走，關節要轉多快 |
|---|---|
| 90° | 32.8 °/s |
| 45° | 60.4 °/s |
| 20° | 132.9 °/s |
| 10° | 264.8 °/s |
| 5° | 529.1 °/s |
| 2° | 1322.3 °/s |
末端的速度只有 0.1 m/s，並不快，關節卻要轉到每秒超過三圈半。下面練習的手臂關節最快約 86 °/s（1.5 rad/s），所以 q2 ＝ 20° 時就跟不上了。真實手臂的控制器通常會在接近奇異點時減速或停下來。
真實的手臂還要考慮更多
這一課的手臂是簡化的：沒有重力、沒有關節角度限制、兩段手臂不會互相碰撞，模擬世界裡關節只有速度上限（1.5 rad/s），沒有力矩與慣性。
真實手臂常有 6 或 7 個關節，才能同時決定手的位置和朝向。DH 參數（Denavit–Hartenberg）是描述每個關節座標系的標準寫法，把這一課的「一段一段接起來」系統化成矩陣相乘；逆運動學多半沒有這麼漂亮的公式，要用數值方法迭代。ROS 2 上常用的 MoveIt 2 把運動學、路徑規劃與避免碰撞都包好了，它預設的逆運動學外掛就是用雅可比矩陣迭代的數值解法。
手臂比輪型機器人更容易傷到旁邊的人：它的末端速度可能很快，而且關節空間的動作不容易預測。真實系統必須有緊急停止、速度與力量限制，以及人進入範圍時的處理。
從 A 到 B：關節空間與笛卡兒空間
手要從 A 移到 B，有兩種完全不同的走法：
- 關節空間（joint space）：算出 A、B 的關節角度，每個關節等比例從起點轉到終點。計算簡單，關節運動平順，但末端走的是一條曲線，事先不容易想像它會掃過哪裡。
- 笛卡兒空間（Cartesian space）：把 A 到 B 的直線切成很多點，每個點都算一次逆運動學。末端真的走直線，代價是每個點都要算，直線可能穿過工作空間外面，也可能經過奇異點附近，關節要轉得非常快。
拿杯子遠距離移動時，關節空間就夠了；把筆在紙上畫直線、把插頭對準插座時，要用笛卡兒空間。
下一個練習用 /line_error 量末端偏離直線多遠。程式從 /joint_states 拿到實際角度，用正運動學算出末端，再算它到 A、B 連線的距離，所以這是實際的軌跡，不是計畫。
實作題：正運動學與逆運動學
補上 fk 和 ik。ik 回傳兩組解的串列，第一組 q2 為正、第二組 q2 為負；到不了回傳 []。程式會把每組解代回 fk，印出的 x、y 應該等於目標。
第三個目標太遠，第四個目標太靠近基座。
確認一下
L1 ＝ 0.3、L2 ＝ 0.25，q1 ＝ 0°、q2 ＝ 90°。末端在哪裡？
目標在工作空間裡，逆運動學算出兩組解。程式固定選第一組，會有什麼問題？
程式對 (0.7, 0) 直接呼叫 math.acos(c)，這時 c 是 2.25。會怎樣？
要讓手臂拿著筆沿著尺畫一條直線，應該用哪種方式？
手臂接近伸直時，控制器看到關節速度指令突然變得非常大。比較合理的處理是？
重點整理
- 正運動學：角度算位置，一段一段接起來，只有一個答案。
- 逆運動學：位置算角度，兩軸手臂用餘弦定理，通常有兩組解（肘上、肘下）。
- 工作空間是環形，圈外圈內到不了。呼叫
acos之前要先檢查。 - 奇異點在 sin q2 ＝ 0：手臂伸直或折疊，有一個方向動不了，附近的關節速度會暴增。
- 關節空間走曲線，笛卡兒空間逐點逆解走直線；指令太快時關節跟不上，實際軌跡就會偏離。
- 這一課的手臂沒有重力、力矩與碰撞；更多軸要用 DH 參數描述，常見工具是 MoveIt 2。
下一課回到輪型機器人的感測器：光達和 IMU 量到的到底是什麼，又會在哪裡騙你。