好,各位同學大家好。
在我們解釋完了 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 的方式去把軌跡規劃的細節把它描繪出來,這是第一個部分