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