1. 從機器人為什么會撞到自己的胳膊說起很多人第一次接觸機器人學是從一個看似很蠢的問題開始的為什么機械臂抓東西的時候明明目標就在眼前它卻經常擺出一個擰巴到不行的姿勢甚至差點撞到自己我當年在實驗室里調一臺六軸機械臂就遇到過這種尷尬——手動示教時它能很順暢地走到位一切換成程序下發(fā)位姿末端就直奔一個詭異的角度去把旁邊的人嚇得夠嗆。問題的根子幾乎都出在空間描述和坐標變換上。這兩個詞聽起來很學術但說白了就是一件事機器人得先搞清楚東西在哪自己朝哪才談得上動。你告訴它把杯子拿起來這句話在人腦子里對應的是一個三維位置加一個姿態(tài)而機器人控制器里真正接收到的是一串數。這串數怎么來的、怎么換算、參考的是哪個坐標系、旋轉變換有沒有搞反——全在這一層決定。這篇內容適合誰如果你是剛翻開機器人學教材、被旋轉矩陣和齊次變換繞暈的學生或者是轉行做機器人應用、天天跟示教器和位姿數據打交道的工程師再或者只是想搞明白機器人到底是怎么知道自己在哪的好奇者這一篇都能用得上。我不打算把教材原樣復述一遍——那些公式書上有。我要做的是把為什么要有這些數學工具、每個量到底代表什么、實操中哪里最容易翻車講清楚讓你看完能自己動手把一個位姿算出來而不是只會背公式。整篇內容會圍繞幾條主線展開空間里一個點、一個剛體到底該怎么描述旋轉矩陣為什么長這樣、怎么用它齊次變換矩陣是怎么把旋轉平移打包在一起的以及多坐標系串聯時為什么順序一錯結果就面目全非。每一條我都會配上一個能直接算、能直接驗證的例子你看完可以自己拿筆或者拿代碼跑一遍。先說一個反直覺的結論機器人學里最難的往往不是動力學、不是控制算法而是最開頭這幾頁幾何。后面再復雜的規(guī)劃、再花哨的強化學習最后都要落到一個末端相對于基座的位姿是多少上。這一層要是含糊后面全是空中樓閣。2. 位置和姿態(tài)把在哪和朝哪拆開看2.1 位置描述只需要三個數但有前提描述空間中一個點的位置最直觀的辦法就是在某個坐標系里給出三個坐標。比如以機器人基座為原點建立坐標系 {A}那么末端執(zhí)行器上某個點的位置可以寫成一個 3×1 的列向量$$^A P \begin{bmatrix} p_x \ p_y \ p_z \end{bmatrix}$$這里的左上標 A 非常關鍵它表示這個位置是在坐標系 A 里量的。我在帶新人的時候發(fā)現大部分人最開始會忽略這個上標覺得是多余的裝飾。等做到兩個、三個坐標系串聯位姿突然算不對回頭一看全是上標寫錯、參考系搞混惹的禍。一個位置向量單獨存在是沒有意義的必須綁定一個參考坐標系。你可以這樣理解我說離我三步遠你得先知道我是誰、面朝哪。坐標系就是那個我。位置描述的本質是在一個已知的參照系里用三個正交方向上的分量鎖定一個點。這里有個常見誤解要提前澄清三個數并不總是長度。在笛卡爾坐標系里它們是長度分量但換成柱坐標、球坐標三個數的含義就變了。機器人學里絕大部分場景默認用笛卡爾坐標所以位置就是三個長度分量。但你要清楚這只是一種約定不是天然如此。2.2 姿態(tài)描述才是真正的難點所在位置好辦三個數搞定。姿態(tài)就麻煩了——剛體在空間里可以任意朝向怎么用數字把它轉了多少、往哪轉描述清楚這是機器人學里最容易讓人掉進去的坑。先建立直覺一個剛體的姿態(tài)本質上是固連在它身上的坐標系相對于參考坐標系的朝向關系。所以描述姿態(tài)等價于描述一個坐標系的三個軸分別指向參考坐標系的什么方向。設有一個固連在剛體上的坐標系 {B}它的三個單位軸向量在參考坐標系 {A} 中的表示分別是 $^A X_B$、$^A Y_B$、$^A Z_B$每個都是 3×1 的列向量。把這三大列并排放在一起就得到了旋轉矩陣$$^A_B R \begin{bmatrix} ^A X_B ^A Y_B ^A Z_B \end{bmatrix} \begin{bmatrix} r_{11} r_{12} r_{13} \ r_{21} r_{22} r_{23} \ r_{31} r_{32} r_{33} \end{bmatrix}$$這個 3×3 矩陣是整個機器人學的地基之一。它有三個必須記住的性質我把它整理成一張表因為這三條直接決定了后面所有推導成不成立性質數學表達實際含義正交性各列兩兩點積為 0三個軸互相垂直坐標系不變形單位性每列的模長為 1軸向量是單位向量只表方向不表長度行列式為 1$\det(R) 1$排除鏡像只表示純旋轉最后那條要特別強調。行列式等于 1 而不是 -1意味著這個變換是正常旋轉不會把左手系變成右手系、不會產生鏡像。很多調試時的詭異現象——比如機器人明明該往左轉卻看起來像照鏡子——追到根上就是某個旋轉矩陣行列式算成了負的或者某處不小心用了轉置。2.3 一個能立刻上手的數值例子光說抽象性質沒感覺來一個具體的。為了讓例子不至于太瑣碎我打算在下面幾節(jié)里用同一個場景貫穿始終機器人基座坐標系 {A}末端工具坐標系 {B}我們要求出從 A 到 B 的變換。這樣你讀起來能一直掛在同一個語境里。假設 {B} 相對于 {A} 只繞 Z 軸轉了 90 度位置沒動。那么 {B} 的 X 軸在 {A} 里指向原來的 Y 方向即 $(0,1,0)^T${B} 的 Y 軸指向 $-X$ 方向即 $(-1,0,0)^T${B} 的 Z 軸和 {A} 的 Z 軸重合即 $(0,0,1)^T$。并起來$$^A_B R \begin{bmatrix} 0 -1 0 \ 1 0 0 \ 0 0 1 \end{bmatrix}$$這正好是繞 Z 軸轉 90 度的標準旋轉矩陣形式。你可以拿它驗算把 {B} 里的點 $(1,0,0)^T$ 變換到 {A}得到 $^A_B R \cdot (1,0,0)^T (0,1,0)^T$也就是轉到 A 的 Y 方向上——和繞 Z 轉 90 度完全吻合。這種自己代進去驗一遍的習慣比死記矩陣格式可靠得多。3. 旋轉矩陣為什么長這樣怎么用它3.1 從軸角到矩陣的構造邏輯旋轉矩陣不是憑空寫出來的它來自一個很樸素的想法旋轉不改變長度也不改變夾角。任何滿足這兩條的變換都是旋轉或者旋轉加反射被行列式條件排除掉。正交矩陣恰好刻畫了保持長度和夾角這件事所以旋轉的數學語言自然就是正交矩陣。具體構造上最常用的是基本旋轉——繞 X、Y、Z 軸分別轉一個角度得到三個基礎矩陣任何復雜旋轉都可以拆成它們的組合。繞 Z 軸轉 $\theta$ 的矩陣是$$R_z(\theta) \begin{bmatrix} \cos\theta -\sin\theta 0 \ \sin\theta \cos\theta 0 \ 0 0 1 \end{bmatrix}$$繞 Y 軸轉 $\theta$注意符號習慣:$$R_y(\theta) \begin{bmatrix} \cos\theta 0 \sin\theta \ 0 1 0 \ -\sin\theta 0 \cos\theta \end{bmatrix}$$繞 X 軸轉 $\theta$$$R_x(\theta) \begin{bmatrix} 1 0 0 \ 0 \cos\theta -\sin\theta \ 0 \sin\theta \cos\theta \end{bmatrix}$$這三個矩陣我建議你不要背而是記住構造方法確定哪個軸不動那個軸對應的行列上就是 1 和 0剩下兩個軸構成的平面里就是普通的二維旋轉。繞 Z 軸轉時 Z 軸不動所以第三行第三列是 1左上角 2×2 是二維旋轉矩陣。這個方法一記三個矩陣永遠不用查表。注意繞 Y 軸的矩陣里$\sin\theta$ 的符號位置和另外兩個不一樣。這不是筆誤而是右手系下 Y 軸旋轉的固有形式。很多人在這一步栽跟頭把三個矩陣統(tǒng)一成同一種模式結果算出的姿態(tài)總是差一個鏡像。3.2 復合旋轉順序決定結果單個軸旋轉好辦麻煩的是先繞 X 轉、再繞 Y 轉這種復合。這里有一條鐵律旋轉矩陣的乘法不可交換$R_1 R_2 \neq R_2 R_1$。這不是數學上的小瑕疵而是物理現實——你拿一本書先繞豎直軸轉 90 度再繞水平軸轉 90 度和反過來做書的朝向完全不同。不信就用手邊的手機試一下兩秒鐘就能驗證。所以在寫代碼或做推導時順序信息必須顯式保留。常見的約定有兩類固定軸旋轉如 XYZ 歐拉角每次都繞參考系固定的軸和動軸旋轉如 ZYZ 歐拉角每次都繞剛體自己的當前軸轉。這兩類即使角度數值一樣結果也完全不同。機器人學教材里那個著名的萬向節(jié)死鎖問題根源就在某些歐拉角組合會在特定姿態(tài)下丟失一個自由度。我用一個簡化例子說明為什么順序重要。設 $R_1 R_x(90°)$$R_2 R_z(90°)$。先算 $R_1 R_2$表示先做 $R_2$ 再做 $R_1$因為矩陣作用在列向量上是從右往左$$R_x(90°) R_z(90°) \begin{bmatrix} 1 0 0 \ 0 0 -1 \ 0 1 0 \end{bmatrix} \begin{bmatrix} 0 -1 0 \ 1 0 0 \ 0 0 1 \end{bmatrix} \begin{bmatrix} 0 -1 0 \ 0 0 -1 \ 1 0 0 \end{bmatrix}$$再算 $R_2 R_1$$$R_z(90°) R_x(90°) \begin{bmatrix} 0 -1 0 \ 1 0 0 \ 0 0 1 \end{bmatrix} \begin{bmatrix} 1 0 0 \ 0 0 -1 \ 0 1 0 \end{bmatrix} \begin{bmatrix} 0 0 1 \ 1 0 0 \ 0 1 0 \end{bmatrix}$$兩個結果明顯不同。這不是巧合是必然。所以凡是看到旋轉矩陣相乘第一步永遠是確認乘法順序第二步才是算數。3.3 旋轉矩陣的逆轉置就夠但方向別搞反旋轉矩陣有一 機器人空間描述與坐標變換：旋轉矩陣、齊次變換與位姿計算入門
1. 從機器人為什么會撞到自己的胳膊說起很多人第一次接觸機器人學是從一個看似很蠢的問題開始的為什么機械臂抓東西的時候明明目標就在眼前它卻經常擺出一個擰巴到不行的姿勢甚至差點撞到自己我當年在實驗室里調一臺六軸機械臂就遇到過這種尷尬——手動示教時它能很順暢地走到位一切換成程序下發(fā)位姿末端就直奔一個詭異的角度去把旁邊的人嚇得夠嗆。問題的根子幾乎都出在空間描述和坐標變換上。這兩個詞聽起來很學術但說白了就是一件事機器人得先搞清楚東西在哪自己朝哪才談得上動。你告訴它把杯子拿起來這句話在人腦子里對應的是一個三維位置加一個姿態(tài)而機器人控制器里真正接收到的是一串數。這串數怎么來的、怎么換算、參考的是哪個坐標系、旋轉變換有沒有搞反——全在這一層決定。這篇內容適合誰如果你是剛翻開機器人學教材、被旋轉矩陣和齊次變換繞暈的學生或者是轉行做機器人應用、天天跟示教器和位姿數據打交道的工程師再或者只是想搞明白機器人到底是怎么知道自己在哪的好奇者這一篇都能用得上。我不打算把教材原樣復述一遍——那些公式書上有。我要做的是把為什么要有這些數學工具、每個量到底代表什么、實操中哪里最容易翻車講清楚讓你看完能自己動手把一個位姿算出來而不是只會背公式。整篇內容會圍繞幾條主線展開空間里一個點、一個剛體到底該怎么描述旋轉矩陣為什么長這樣、怎么用它齊次變換矩陣是怎么把旋轉平移打包在一起的以及多坐標系串聯時為什么順序一錯結果就面目全非。每一條我都會配上一個能直接算、能直接驗證的例子你看完可以自己拿筆或者拿代碼跑一遍。先說一個反直覺的結論機器人學里最難的往往不是動力學、不是控制算法而是最開頭這幾頁幾何。后面再復雜的規(guī)劃、再花哨的強化學習最后都要落到一個末端相對于基座的位姿是多少上。這一層要是含糊后面全是空中樓閣。2. 位置和姿態(tài)把在哪和朝哪拆開看2.1 位置描述只需要三個數但有前提描述空間中一個點的位置最直觀的辦法就是在某個坐標系里給出三個坐標。比如以機器人基座為原點建立坐標系 {A}那么末端執(zhí)行器上某個點的位置可以寫成一個 3×1 的列向量$$^A P \begin{bmatrix} p_x \ p_y \ p_z \end{bmatrix}$$這里的左上標 A 非常關鍵它表示這個位置是在坐標系 A 里量的。我在帶新人的時候發(fā)現大部分人最開始會忽略這個上標覺得是多余的裝飾。等做到兩個、三個坐標系串聯位姿突然算不對回頭一看全是上標寫錯、參考系搞混惹的禍。一個位置向量單獨存在是沒有意義的必須綁定一個參考坐標系。你可以這樣理解我說離我三步遠你得先知道我是誰、面朝哪。坐標系就是那個我。位置描述的本質是在一個已知的參照系里用三個正交方向上的分量鎖定一個點。這里有個常見誤解要提前澄清三個數并不總是長度。在笛卡爾坐標系里它們是長度分量但換成柱坐標、球坐標三個數的含義就變了。機器人學里絕大部分場景默認用笛卡爾坐標所以位置就是三個長度分量。但你要清楚這只是一種約定不是天然如此。2.2 姿態(tài)描述才是真正的難點所在位置好辦三個數搞定。姿態(tài)就麻煩了——剛體在空間里可以任意朝向怎么用數字把它轉了多少、往哪轉描述清楚這是機器人學里最容易讓人掉進去的坑。先建立直覺一個剛體的姿態(tài)本質上是固連在它身上的坐標系相對于參考坐標系的朝向關系。所以描述姿態(tài)等價于描述一個坐標系的三個軸分別指向參考坐標系的什么方向。設有一個固連在剛體上的坐標系 {B}它的三個單位軸向量在參考坐標系 {A} 中的表示分別是 $^A X_B$、$^A Y_B$、$^A Z_B$每個都是 3×1 的列向量。把這三大列并排放在一起就得到了旋轉矩陣$$^A_B R \begin{bmatrix} ^A X_B ^A Y_B ^A Z_B \end{bmatrix} \begin{bmatrix} r_{11} r_{12} r_{13} \ r_{21} r_{22} r_{23} \ r_{31} r_{32} r_{33} \end{bmatrix}$$這個 3×3 矩陣是整個機器人學的地基之一。它有三個必須記住的性質我把它整理成一張表因為這三條直接決定了后面所有推導成不成立性質數學表達實際含義正交性各列兩兩點積為 0三個軸互相垂直坐標系不變形單位性每列的模長為 1軸向量是單位向量只表方向不表長度行列式為 1$\det(R) 1$排除鏡像只表示純旋轉最后那條要特別強調。行列式等于 1 而不是 -1意味著這個變換是正常旋轉不會把左手系變成右手系、不會產生鏡像。很多調試時的詭異現象——比如機器人明明該往左轉卻看起來像照鏡子——追到根上就是某個旋轉矩陣行列式算成了負的或者某處不小心用了轉置。2.3 一個能立刻上手的數值例子光說抽象性質沒感覺來一個具體的。為了讓例子不至于太瑣碎我打算在下面幾節(jié)里用同一個場景貫穿始終機器人基座坐標系 {A}末端工具坐標系 {B}我們要求出從 A 到 B 的變換。這樣你讀起來能一直掛在同一個語境里。假設 {B} 相對于 {A} 只繞 Z 軸轉了 90 度位置沒動。那么 {B} 的 X 軸在 {A} 里指向原來的 Y 方向即 $(0,1,0)^T${B} 的 Y 軸指向 $-X$ 方向即 $(-1,0,0)^T${B} 的 Z 軸和 {A} 的 Z 軸重合即 $(0,0,1)^T$。并起來$$^A_B R \begin{bmatrix} 0 -1 0 \ 1 0 0 \ 0 0 1 \end{bmatrix}$$這正好是繞 Z 軸轉 90 度的標準旋轉矩陣形式。你可以拿它驗算把 {B} 里的點 $(1,0,0)^T$ 變換到 {A}得到 $^A_B R \cdot (1,0,0)^T (0,1,0)^T$也就是轉到 A 的 Y 方向上——和繞 Z 轉 90 度完全吻合。這種自己代進去驗一遍的習慣比死記矩陣格式可靠得多。3. 旋轉矩陣為什么長這樣怎么用它3.1 從軸角到矩陣的構造邏輯旋轉矩陣不是憑空寫出來的它來自一個很樸素的想法旋轉不改變長度也不改變夾角。任何滿足這兩條的變換都是旋轉或者旋轉加反射被行列式條件排除掉。正交矩陣恰好刻畫了保持長度和夾角這件事所以旋轉的數學語言自然就是正交矩陣。具體構造上最常用的是基本旋轉——繞 X、Y、Z 軸分別轉一個角度得到三個基礎矩陣任何復雜旋轉都可以拆成它們的組合。繞 Z 軸轉 $\theta$ 的矩陣是$$R_z(\theta) \begin{bmatrix} \cos\theta -\sin\theta 0 \ \sin\theta \cos\theta 0 \ 0 0 1 \end{bmatrix}$$繞 Y 軸轉 $\theta$注意符號習慣:$$R_y(\theta) \begin{bmatrix} \cos\theta 0 \sin\theta \ 0 1 0 \ -\sin\theta 0 \cos\theta \end{bmatrix}$$繞 X 軸轉 $\theta$$$R_x(\theta) \begin{bmatrix} 1 0 0 \ 0 \cos\theta -\sin\theta \ 0 \sin\theta \cos\theta \end{bmatrix}$$這三個矩陣我建議你不要背而是記住構造方法確定哪個軸不動那個軸對應的行列上就是 1 和 0剩下兩個軸構成的平面里就是普通的二維旋轉。繞 Z 軸轉時 Z 軸不動所以第三行第三列是 1左上角 2×2 是二維旋轉矩陣。這個方法一記三個矩陣永遠不用查表。注意繞 Y 軸的矩陣里$\sin\theta$ 的符號位置和另外兩個不一樣。這不是筆誤而是右手系下 Y 軸旋轉的固有形式。很多人在這一步栽跟頭把三個矩陣統(tǒng)一成同一種模式結果算出的姿態(tài)總是差一個鏡像。3.2 復合旋轉順序決定結果單個軸旋轉好辦麻煩的是先繞 X 轉、再繞 Y 轉這種復合。這里有一條鐵律旋轉矩陣的乘法不可交換$R_1 R_2 \neq R_2 R_1$。這不是數學上的小瑕疵而是物理現實——你拿一本書先繞豎直軸轉 90 度再繞水平軸轉 90 度和反過來做書的朝向完全不同。不信就用手邊的手機試一下兩秒鐘就能驗證。所以在寫代碼或做推導時順序信息必須顯式保留。常見的約定有兩類固定軸旋轉如 XYZ 歐拉角每次都繞參考系固定的軸和動軸旋轉如 ZYZ 歐拉角每次都繞剛體自己的當前軸轉。這兩類即使角度數值一樣結果也完全不同。機器人學教材里那個著名的萬向節(jié)死鎖問題根源就在某些歐拉角組合會在特定姿態(tài)下丟失一個自由度。我用一個簡化例子說明為什么順序重要。設 $R_1 R_x(90°)$$R_2 R_z(90°)$。先算 $R_1 R_2$表示先做 $R_2$ 再做 $R_1$因為矩陣作用在列向量上是從右往左$$R_x(90°) R_z(90°) \begin{bmatrix} 1 0 0 \ 0 0 -1 \ 0 1 0 \end{bmatrix} \begin{bmatrix} 0 -1 0 \ 1 0 0 \ 0 0 1 \end{bmatrix} \begin{bmatrix} 0 -1 0 \ 0 0 -1 \ 1 0 0 \end{bmatrix}$$再算 $R_2 R_1$$$R_z(90°) R_x(90°) \begin{bmatrix} 0 -1 0 \ 1 0 0 \ 0 0 1 \end{bmatrix} \begin{bmatrix} 1 0 0 \ 0 0 -1 \ 0 1 0 \end{bmatrix} \begin{bmatrix} 0 0 1 \ 1 0 0 \ 0 1 0 \end{bmatrix}$$兩個結果明顯不同。這不是巧合是必然。所以凡是看到旋轉矩陣相乘第一步永遠是確認乘法順序第二步才是算數。3.3 旋轉矩陣的逆轉置就夠但方向別搞反旋轉矩陣有一