所以我們現在就來繼續看一個在空間中的手臂的 IK 的一個運算。 那這個時候就會用到所謂的 Pieper 的 Solution, 那我們剛剛之前有講過說,如果說今天手臂 它其中的三個軸,中間相連的三個軸, 如果有交在同一個點,它的軸線方向交同一個點的話, 我們說這個手臂會有解析解,那下方這個圖裡面,其實各位可以看到 說這就是一個目前常常看到的手臂,這個叫做 PUMA 嘛,之前有舉例過, 那它的六個軸線比較像是這樣,第一個軸是向上轉的, 第二個軸是一個側轉,第三個軸也是一個側轉的動作。 那各位同學就可以發現說我今天經過這三個軸的這個配置方式, 就可以讓我的手腕,比如說這個黑色的部分在空間中 動來動去,所以大家可以想象說我今天假設說我的手腕有一個位置上的需求,譬如說有一個 x y z, 某個位置的需求,我就可以藉由這三個軸的轉動讓我的手腕到達那個我想要的點, 那這裡後面的三軸呢,四五六基本上它就會交在一個點上面。 這就是用這個四五六軸來交點的方式,來讓我們的這個六軸手臂有解析解, 那這個三軸的轉動交在同一個點,它主要的負責是在轉動的部分。 這等於說前三軸,像剛剛講的,是負責移動。 后三軸是讓這個手腕產生適當的姿態。 它三個旋轉它會產生適當的姿態,那事實上這個架構就跟 我們人的手事實上是很像的,我們人的手腕這邊 它也有三個轉動,它有三個轉動, 所以因為這三個轉動是在幾乎在手的末端, 所以我們今天手要做一個動作的時候就變得比較容易,就等於說在手腕這邊可以有一些蠻靈活- 的操作。 這是一個手臂設計的一個原則。 那這裡我們就要開始來解解看這邊, 那我們也知道說今天4到6它都交在同一個軸,所以我們間接 也知道說今天6對0,就是說6的 frame 的原點對0的向量, 基本上會和4的 frame 的原點對0是同一個,因為4、 5、 6都交在同一個點上。 那麼後續在開始解我們的IK的時候,基本上第一步就是要用到像 這樣子的特性,那麼現在就來看一看說這個該怎麼解,我們先來解移動的部分。 所以各位同學可以先想說我今天有一個需求在手腕上面,等於說我手腕希望它 到達某個 x y z 這個位置,那到達這個位置之後我等於說 想要利用這個位置的條件來反算出θ1,θ2,θ3各是什麼? 那我們之前也知道說今天針對一個空間中的手臂, 這個θ1,θ2,θ3事實上會在三角函數以很複雜的方式, 復合在一起,那麼現在在所謂的代數法,就希望說這個 這個復合的方式能夠我們一步一步把它拆解開來。 那在第一步之前,我們先把我們之前常用到的 transformation matrix 的標準公式列出來這裡給大家做參考。 那這個 transformation matrix i 到 i-1 的轉換, 是依據這個 Craig 定義的方式所推導出來的,那各位同學如果想要用 standard 的方式也是可以, 後續的步驟也相類似,那會有一點點差異,我解出來的解事實上是可以 去互相去比對的,那在這個T的i到 i-1的式子裡面我們事實上裡面已經有把符號稍微簡化一點。 就等於說我的 cosθ1 我已經這裡是把它簡寫成 cθ1, 對不對?然後這裡的 cosα1 或者說cosαi-1 我就把它寫成c的αi-1,這裡等於說我們今天 知道說這個 transformation matrix 裡面有四個參數嘛,就是有θ有α有a 有d,a 跟 d 是長度的 參數,然後θ跟α是角度的,那角度的用到了cos sin 的部分, 在這裡都簡寫成 c 跟 s,那由於後續很多的角度會在 更多的東西乘在一起,所以我們後續還有再針對θ的部分 再進一步的簡化,所以今天cθi 我們在後續的內容中就會簡化成ci, sθ就簡化成si,所以就等於說各位同學如果看到 ci 代表了它就是cosθi, 看到si 就代表它是sinθi,那因為這個,這個 ci,si 已經保留給θ用了,所以我們在α的部分就不改了,就等於說我們之後看到的 在α那邊也一樣是cα1,2,3 或是sα1,2,3之類的,那邊我們就不動了。 那我們接下來就照著剛剛講的, 我今天是給了一個所謂在第四個 frame 上面原點的條件,就等於說我們給了一個手腕的坐標,希望它到達空間中的某個xyz上面, 那我們今天也知道說,今天這個第四個 frame 對0的這個 向量我們基本上可以把它展開成右邊的這個形式, 就等於說我之前是有4對3的 表達法,那3再經過3個 transformation matrix 把它轉到零,所以等於說 P的4的原點對3是一個4×1的向量。 然後前面有三個4×4的 transformation matrix 就讓我們可以把 P的表達法從第三個 frame 轉換到第0個 frame。 那我們P的4對3的這個向量基本上如果 我們從上面的這個 transformation matrix 來看的話,就等於說我們i代4的時候 對不對,我們今天如果i代4的話,上面這個標準式裡面代給我們的是T的4 對3,那麼T的4對3,我們也知道說它的第4個column 本身就 是P的4的原點對3,所以等於說我這裡需要的這個綠色的部分, 就是P的4的原點對3,基本上就是上面這個矩陣的 第四個column,然後 i 代4,所以就等於說漸漸我們就可以算出 來說我的第四個column代下來之後為什麼第一個是a3,因為是a的 4-1,然後第二個是-S的-sin的α3跟d4嘛。 對不對?然後第三個是cos的α3跟 d4,就像下面寫的這樣子,所以就等於說我們這個綠色的部分是照著我們這個 標準的公式裡面代i等於4,然後取最後一個column去得到。 那這個等於說這個基本上裡面都是常數,對不對,它只有a 跟 d 跟α,它是一個 在手臂設計知道之後,dh 知道之後這個本身這個column就是一個常數。 那我們這時候就把T的3對2把它乘進來。 那我們知道說t的3對2用到的是θ3的角度,所以 等於說我們今天T的3對2跟後面這個綠色相乘之後 基本上這一個向量就會變成是θ3的函數。 那我們這θ3的函數我們把它命名成是f。 那這裡的 f這個函數我們是可以把它算出來的。 它基本上就是T 的3對2這個,跟剛剛的這個綠色的部分相乘。 那展開來之後就是像下面寫的這個式子這樣子,那發現說下面的這個式子裡面, 的確只有出現c3跟s3, 那剩下的都是常數,所以我們可以確認說我們今天的f 僅僅是θ3的函數。 那有了這一步之後呢,我們可以依法炮製再往下走。 再乘開一個T,就等於說我們把T的2對1,理論上它是θ2的函數, 把它跟後面的f相乘,乘開之後 我們就會得到一個新的g的函數,那我們g因為知道說剛剛的f是θ3的函數, 那今天T2對1是θ2的函數,所以相乘 之後的g會是θ2和θ3的函數。 那乘開來之後我們就知道說我們的g就像是像下面這樣子寫的一樣。 那各位同學裡面也可以看到說,這裡面就會出現了 cos跟sin的2和f,那我們的 f本身就裡面有s2、 s3跟c3,所以就等於說我們的g本身是 θ2跟θ3的函數,所以乘到這一步我們就可以得到我們的g 的表達法是這樣子,那這個同時 我們也可以知道說我們今天再把它跟1對0乘開之後, 就可以得到最右側的這一個向量。 那這個向量基本上,這裡可以看到說因為1對0本身裡面有 θ1,那θ1就是在裡面展開來是由cosθ1跟 sinθ1,所以一展開之後基本上我們等於說就把cos、 sin的θ1 把它乘開來就長成像這樣子的形式,那這樣的形式之後,我們等於說我們就找到了 x、 y、 z 就等於說這個是我們想要到達的 跟我們 θ1、 θ2、 θ3 的表達法在這裡 衹是我們這個表達法裡面,我們又一層一層把它包起來 沒有說直接看到 θ 1 2 3 乘在一起,那太複雜,我們是 利用一個 f 的函數把 θ3 包起來,再利用一個 g 的函數 同時包 θ2 跟 θ3,然後再把 θ1 留在外面 這個方式讓我們有一個比較簡單的對應,等一下才有辦法依序地分開解 好,那麽所解的方式的第一步是說,我們有了 xyz 的條件 那我們現在就把 xyz 去取一個平方和 基本上就等於說,我們想要算到算出說 這個手腕的那個點的那個長度平方,相對于我的坐標原點的長度平方 那這個平方和,我們如果説把它照著這個 右上的第二個這個矩陣來看的話,基本上我要的就是 g1 g2 g3 的平方和 那 g1 g2 g3 本身又是下面黑色的這一串,可以把它乘開來 那當我把他乘開來,再化簡之後,我們就發現說,今天這一串乘開來跟化簡之後 可以得到像這第二行的這個形式 那第二行的這個形式,我們就可以再進一步利用一個所謂的 k 函數 把它包起來,等於說看起來這個方程式就比較簡潔 那包起來之後,我們就可以看到說,這裡會有一個 c2 s2 跟 k 嘛,那也看到說,欸,今天在這個包起來之後 我剛剛本來相乘裡面,本來的 θ2 跟 θ3 就可以重整成一個看起來很乾净的形式,就等於說我這裡的 k 衹跟 θ3 有關,那我這裡的 θ2 就等於說單純地顯示出衹有一個 cos 跟一個 sin 在這裡 那這個 a 是我們的長度的常數,那同時今天這個 r 也把 θ1 的效果消除掉了,所以等於說我們今天做了這一步 的用意就是説,我今天就連結了我今天本來 xyz 的需求 可是本來 xyz 是 θ1 θ2 θ3 的函數,經過找到這個向量和的方式 就把 θ1 的效果先消掉,所以等於說它單純衹是 θ2 跟 θ3 的函數,這樣看起來就比較簡潔 那第二個條件是一個高度的條件,就是我們把剛剛 xyz 的 z 取出來 那因爲 z 本身就衹跟 g3 有關,衹跟 g3 有關,所以 z 它本身也是 θ2 跟 θ3 的函數 那等於說這樣匯合之和,我們發現它剛剛也 用到了跟剛剛在算 r 時候一樣的 k1 跟 k2 的部分 但是多了一個 k4,所以我們可以是重新定義出來 那今天 k4 看出來,也知道說它裡面衹有一個 f3,所以它衹是 θ3 的函數 那等於說,我們今天藉由這樣子的一個取長度跟取 z 的方式 讓我們可以把本來 xyz 的條件,三個條件 會整成兩個條件,會整成一個 r 的條件,一個 z 的條件,那這個 r 跟 z 這兩個條件下都僅僅是 θ2 跟 θ3 的函數 這樣等於說我們 θ1 就不用管了,我們可以用 θ2 跟 θ3 來設法去解解開 所以整合起來就變成像這樣子,那我們知道說 我們在這個整合之後,我們 θ2 的 是外顯在外面,對不對,這裡有 c2 跟 s2,那 θ3 就隱藏在 k 裡面,所以 k 裡面 k1 到 k4 裡面藏有 θ3 那 θ2 是外顯在外面。 那針對這兩個複合的式子,我們就可以分開去討論 譬如說,今天如果説是 a1 等於 0 的時候 等於說 a1 等於 0 的時候,我的 r 就非常單純地 等於 k3,那因爲 k3 是僅僅是 θ3 的函數,所以就等於說 我今天這個 r,這是已知嘛,因爲等於說我們 xyz 是已知 所以 r 是已知,就可以等於說今天是單純是有一個 θ3 的一個三角函數的式子 等於說這個今天這個式子裡面衹有 θ3 的一個函數,那它 θ3 可能會藏在 cos 跟 sin 裡面,所以等於說我們這個還需要 用三角函數去解,那第二個是如果說 sα1 等於 0,就等於說我們今天 如果說手臂的設計上,因爲一些幾何條件讓 α1 等於 0 的話,我們就發現說我們的 z 就等於 k4 那 k4 本身也僅僅是 θ3 的函數,所以就等於說我今天也是一樣,最後可以展開來之後 z4 已知,展開來之後的右側就變成是 cos 跟,cos θ3 跟 sin θ3 的組合 所以這等於說也是一個單一變數的一個三角函數的方程式 那第三個,如果說這兩個都 并不是 0,那我們可以再藉由一個相加相乘的方式,因爲這裡面有 cos 跟 sin,所以我們把 k3 k4 搬到另外一側之後 取平方和,再消,最後可以得到一個像是一個 橢圓的一個,一個式子。 那在這個式子之下 我們等於說裡面是複合的,但是不論是什麽樣的式子,我們都可以 把 θ3 找出來,因爲各位同學也可以看到說,我們今天在最下方的這個式子 我們就是已經把 cos 跟 sin 的 θ2 都消掉了 所以這個式子也是僅僅是 θ3 的函數,因爲右側衹有 k 所以我們知道說今天不論這三個 case 的哪一個,我們都得到了一個 θ3 的三角函數的方程式 那這個時候就可以套用我們之前教的把 θ3 的這個三角函數的方程式 用 tan(θ3/2) 等於 u 的方式把它代換到多項式上面 那最後這個方程式就會變成是一個多項式的一個方程式 然後我們就藉由解多項式的方式找到 u 然後再藉由兩倍的 tan(-1) 的方式就可以解出我們的 θ3 是什麽 θ3 解完了之後,我們看可以 把剛剛的條件再拿來使用,譬如說我們用一個 r 的條件 那因爲這個條件 θ3 知道,所以 k1 k2 k3 都知道了,就剩下 c2 跟 s2,所以變成說今天是變成了是一個 θ2 的三角函數的方程式 我們如法炮製把 θ2 換成 u 然後也可以,就可以再解多項式的方式來找出 θ2 那最後我們也是一樣相似的方式,這是一個 θ1 的三角函數 一樣把它藉由多項式的轉換,就可以把 θ1 解出來 那等於說,我們今天這個步驟這樣跑完之後,我們在移動的部分 給了 xyz 的條件,我們就可以找出 這個手臂所需對應的 θ1 θ2 跟 θ3