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