好，各位同學大家好。

在我們解釋完了 Cubic Spline 的一些基本的設定之後我們現在就來做一個練習。

那這個練習基本上是一個平面上的三個旋轉自由度的手臂因為它有三個自由度，所以我們知道說平面上面任何的

x、 y 跟 theta 都到得了，因為平面上基本就是三個自由度

那這個手臂的設定就像是這個右下角的這個圖 show 出來的一樣。

這個手臂就有三截嘛，是一個三個旋轉的關節，就跟我們這門課一直在常常用的這個設定是一樣的

那這裡因為要做一些量化的計算，所以我就把手臂的長度也給各位同學就長度基本上是第一截是

4 ，第二截是 3 ，第三截是 1。

那等於說是兩個跟最後一個短短的手。

那記得各位同學，我們在上課的時候講過說我們今天在這個操作上面，我們常常是用前面的自由度來產生移動。

最後一個自由度來產生轉動。

所以這也是為什麼在這裡的設定，我的 l3 故意設定的比較短它等於說讓各位同學也看到說它就是一個手的姿態。

那各位同學也可以把它想成說這就像一個人的手臂，一個前一個上臂、一個前臂跟一個手掌，這個相對的幾何

那我們這裡的規範是希望說，我今天這有一個軌跡要規劃

我希望說這個手臂到達空間中的這四個紅點。

我一開始是在這個最左側的這個紅點，然後通過中間上面的這兩個點，最後要停在右側的點

那我這個表裡面就列出來說，這個手臂需要到達的時間歷程在 t 等於 0 的時候，在 (-4,

0) 嘛，就是這裡的第一個點在 t 等於 2 的時候，要走到這個 (0,

3) ，那 t 等於 4 的時候，要到達 (3, 3) 那 t 等於 7

的時候，等於說這個時間有再拉長一點，比較慢一點，它需要到達一個 (4,

0) 的這個位置那在這裡的這個到達的位置，我們其實是讓問題簡化一點，就等於說這裡定義的這四組 (X,

Y) 是手臂的二、三截的這個交界處需要到達的點

那以手來講的話，就等於說我規範的是手腕要到達的位置，是在這裡。

等於說我們的 (X, Y) 在求解的時候，只需要去解出 theta 1 跟 theta

2 的狀態就好就不需要去考慮 theta 3 ，那事實上如果針對以後的需求

我希望說我規範的點是在手掌的正中心其實也是可以。

因為各位同學可以想像說我今天這個 l3 的姿態

就是這 theta ，事實上就是它的 l3 的姿態，對 D 的姿態，就是我這裡所謂的

theta 的條件就是等於說希望手是以什麼樣的姿態去擺就等於說我這裡

theta 的條件基本上就是 l3 的這個手在空間中的絕對姿態。

所以我們今天如果說一開始定義的是這個手的末端點，就是 l3

的末端點需要到達什麼位置的話我們其實只要再配合這個姿態跟

l3 的桿長就可以反推出 l3 跟 l2 中間的這個

joint ，所謂的手腕的這個關節需要到達的位置

然後之後就可以照著像這裡一樣的反算方式，藉由這個手腕的位置去算出 theta 1 跟 theta 2 的角度。

等於說也是可以用同樣的方式去做，只是要再多做一步而已。

那這裡為了簡化，我們就希望說我們的 X1、 X2 ，我們的 (X,

Y) 就直接定義在這個手腕的位置，讓各位同學直接可以 theta 1、 theta 2

去求解然後 theta 3 跟 l3 的作用，就在我們最後的這個

theta 裡面去反映出來那這是我們這樣的設定。

那各位同學也可以發現說我在這個圖形裡面有畫出這個紫色的圈圈

那基本上這個圈圈其實就代表著這個手腕可以到達的位置的極限範圍。

大家可以看到說我今天這個圈圈的位置其實在半徑等於 7 的位置

就等於說今天手伸直了之後，這個手腕最遠就可以到達跟圓心半徑 7

的地方畫一個圓，這是最遠可以到達的那最近的到達的，就是半徑為 1

的圓，因為第一截是 4 ，第二截是 3 ，即使整個手臂整個內折到最裡面

它等於說這個手腕能夠離圓心最近，也是在半徑等於 1

的時候所以基本上是這兩個紫色圈圈所定義出來的中間範圍，就是這個手腕能夠到達的

位置，我們一般也稱之為這個手臂的 work space。

等於說中間可以到達的位置是在這兩個圈圈的中間。

所以這也間接知道說我們今天所規劃的點，至少一定要在這個這兩個圈圈的中間，否則它

inverse 算一定會算不出來，算出來就會得到我們 cos 到時候要解出來

的時候，cos 的範圍的值會在 -1 跟 1 之外，那等於說是沒辦法求解的

如果手在這個點，選擇在這兩個圈圈的範圍之間，我們其實

cos 到時候就可以，inverse 的時候就可以解得出來，這是我們的定義

那我們現在就針對我們今天所教的那幾個方法開始來一一演練第一個是說，我們是在

Cartesian-space 下面先做軌跡規劃。

那我們教過了 polynomial ，那在這一個例題裡面我們有兩個

via points，所以等於說我們今天基本上，我們的軌跡會有三段的

polynomial 去組成那我們現在就來稍微看一下。

好，我們的第一步就是因為我們現在是要在 Cartesian-space

下面做軌跡規劃那我們剛剛已經有所謂的需求點，就等於說我們針對

X ，針對 Y 針對我們的角度 θ ，這三個點

都各自有四個條件，對不對？因為等於說各自有，需要通過 initial

通過兩個 via ，跟通過 final，對不對？所以等於說我們可以想像說

我們今天每一個都有四個條件，所以剛剛的那個 table

裡面才會是 4 乘 3 嘛，等於說有 12 個數字需要去滿足

那我們知道說，針對每一個，我們就先來看 X

好了，我們今天針對一個這個 X 這個自由度，它要通過四個點

間接代表說，它裡面需要三段的 polynomial 才夠那每一段的

polynomial 又有四個未知數所以等於說針對 X ，我們就有 12

個未知數要求解針對 Y 也有 12 個，針對 Z 也有，針對 theta

也有 12 個那等於說我們今天如果寫成通解的話就比較像是這樣。

就等於說大家可以想像說，這是這是比較通解的寫法，就等於說我今天這個大

Θ 可能是可以代 X、 Y 或者是 θ 都可以。

那在這個寫法下下面的時候我的 T ，因為我的

A 等於說我就有 12 個未知數所以我今天這個

A 的向量等於說有 12 個排在一起，所以是 12 乘 1

的向量我的 T 基本上就變成是 12 乘 12 的矩陣。

那我的 Θ ，就是我的 X 的條件基本上要列成 12 乘 1

那我們現在就把它展開來看，基本上就變成是像這樣子的一個型態。

那這個矩陣的內容我就不再依次的細講，因為這個在跟剛剛一個 via point

是很像的，只是因為今天變成了多了一個 via point ，所以矩陣的維度就增加，剛剛一個

via point 的時候是 8 乘 8 兩個 via point 就變成是 12 乘 12

，對不對？所以就我們也看到說今天裡面的排列方式跟剛剛是很像只是維度在增加。

那這裡也可以看到就像剛剛一樣前面六個條件是位置的條件

第一個跟第二個是第一段的位置條件

第三個跟第四個是第二段的，然後第五個跟第六個是第三段的，所以前面六個是位置的條件

那再來第七第八個元素，是一開始的速度跟最後 final 的速度的條件。

在這裡我們也一樣假設一開始跟 final 的速度是 0 或者說是 0。

那這裡是把它先把符號先寫出來，所以這是 final 速度的條件

那最後的這四個 row 就是我們這兩個 via

point 速度跟加速度的連續性的條件前面的兩個是第一個

via point 的速度跟加速度最後的兩個 row 是第二個 via

point 的速度跟加速度所以這樣整個合在一起之後，就等於說是針對

X 的部分，我們把三段 polynomial 一起求解所以這是從

a10 到 a33 ，我們總共有 12 個未知數，就一起求

所以才會說今天如果 via point 越多，這個矩陣的維度就會越來越大

那今天在這個設定方式之下，我們也知道說今天我們總共有 X、 Y、

θ 三個變數都要去求，那等於說我們今天每一個都有 12 個數字。

所以今天這個 Θ 可以代 X 的，代 Y 的，也可以代 θ 的所以間接代表說我們今天會有

36 個未知數需要求那我們是可以寫成像這樣的 12 乘 1

等於 T 的 12 乘 12 乘上 A 的 12 乘 1 ，也可以寫成

12 乘 3 跟 A 的 12 乘 3 ，兩個寫法都可以那中間的 T

矩陣基本上是一模一樣，因為它基本上只是 Δt 的函數所以今天，我們今天針對 x,

針對 y ,針對 θ ,它的 Δt 的設定要一模一樣嘛，才能夠確保說它會在同一個時間點到達中間的

via point 所以等於說中間的這個矩陣是三個變數下面，x,

y ,θ 下面都可以共用的所以中間基本上，我們真實在運算上的時候只要算一次，把中間這個

t 的矩陣算出來之後我們隨著不同的邊界跟速度條件

不同參數下面的，代進去之後，我們直接可以求出不同 x,

y, θ 下面所对应的 polynomial 的系数是什么所以這是第一步，我們就要把它做一個求解

那這是剛剛講過的，我們有 6 個位置條件 2 個速度條件跟 4

個速度跟加速度的連續性好，那這個等於說，就像剛剛講的，我們今天把它整個

串在一起的時候，就像是像這樣的形式，等於說大家如果說真正做更同步的矩陣運算的話，我可以把 x,

y 跟 θ 排在不同的 column 下面那 A 也會變成是有三個 column，所以今天A

12*3 就間接知道說它裡面有 16 個未知數需要去求解

那等於說經過一次的 inverse 可以把 36 個全部都算出來

好，那這個算出來之後，等於說我們今天所有 polynomial

的係數都知道了，那麼實際上就有辦法，我們等於說

另一個方面來講，等於說我們今天 x 對 t 的軌跡知道 y

對 t 的軌跡知道，θ 對 t 的軌跡也會知道，我們就事實上可以拿來作圖

那這個作圖的內容，就像是下面所 show 的，最左邊下角的這個是

x 對 t 的，對不對？那等於說我們今天的確，我們的軌跡會從

-4 跑到 0 ，跑到 3 ，跑到 4

那我們時間上面的刻度也是對的，從 0 秒，2 秒，4 秒到 7 秒所以我們今天的 x,

y, 跟 θ，這個 D(t) 代表的是 θ 都會依據我們的需求，去通過我們本來設定的 initial,

final 跟 via points 所以等於說我們設定的軌跡基本上是連續的而且是對的

那從幾何上也可以看出來說，它的確是一個圓滑的線段，它經過所有的 via points 的時候，至少眼睛看起來

速度上是連續的，加速度很難直接看，至少看起來速度是連續的那今天我們又可以把這個 x,

y 整並起來，把 t 消掉之後去畫成 x 對 y

的圖，我們這裡就可以比較精確地看到說到底這個軌跡，或者說這個手腕的部位，在空間中到底是怎麼跑

好，它等於說，它會以這樣子的一個方向開始出發通過第

2 點之後，就開始一個緩緩的轉折，以便它在第 3

點跟第 4 點的時候它的方向跟速度會有些變動，所以等於說它是這樣沖上去開始一個緩緩的轉折，再向下動。

所以各位同學可以看到說，雖然說我們本來規劃的都是三次式

可是當它整體一起考量了之後，考量了中間 via point 速度跟加速度連續之後

整個的軌跡其實是非常的單純，它非常的簡單，它並沒有像三次式會想像中的會有很多的轉折，它反而是在第一段

跟第三段，都是像是一個，幾乎是直線，只是有一些輕微度的傾斜的線

那各位同學，如果去對應到我們今天本來的軌跡，大家也看到說，今天雖然說，譬如說以

x 對 t 來看雖然說三段都是三次式，可以看到說它其實軌跡的本身

基本上都非常地接近線性，只有後面的轉折會比較需要那

y 的部分，因為它是上去再下來

所以你會發現說，它整個串起來，基本上其實也像是一個很簡潔的一個多項式的圖形，我們前面是就是往上沖

第三段是往回跑，所以第二段就要有一個沖過頭再一個降回來的一個過程才會讓我們在這個

via point 上面的軌跡是比較相連的，所以第二段等於說是 y

的部分就是這樣沖上去，然後一個緩降，然後就沖下來那 θ 的部分因為有快有慢，所以它會發現

在中間就會有一個比較多的轉折，然後就這樣子動下來所以這個是我們針對第二個軌跡

規劃的時候所看出來的一個軌跡的形態好，那剛剛算完了所有的在

Cartesian 下面的軌跡之後那我們也知道說，我們知道所有的係數

為了安全，我們還是會先做一個 IK，把各個 joint 上面的軌跡去找出來

那 joint 上面的軌跡找出來之後，我們其實可以寫一個簡單的一個

simulation 的 code，去看看說這個軌跡是不是有照著我們規劃的在跑

我們現在手臂是在一開始的姿態，就像剛剛講的這樣子，那我們一旦啟動之後

它手臂就開始往前跑，那最後一段應該跑 3 秒，所以各位同學可以發現說最後一段它真得跑的比較慢

它這個手臂就可以照著我們本來想要的方式就一直往前沖，而且一路上是幾何上是可以求解得出來

代表說我今天這個角度的設定也是可行的那等於說借著這個影片，大家就可以知道說我們今天就照著我們

預設的方式，在 Cartesian-space 下面去做一個軌跡規劃然後我們用 cubic

spline 的方式去把軌跡規劃的細節把它描繪出來，這是第一個部分

5-6_General Cubic Polynomials 2 Example 1

機器人學一 (Robotics 1)

与讲师见面