本課程主要在學習機械手臂在分析面的運動學和軌跡規劃。 學習目標： 1.瞭解物體在空間中運動的描述方式 2.瞭解多關節機械手臂的順逆運動學 3.瞭解運動軌跡的規劃方式。
5-5_多段Cubic Polynomials
Loading...
來自 National Taiwan University 的課程
機器人學一 (Robotics 1)
26 個評分
從本節課中
機械手臂軌跡規劃 (一)
1.瞭解如何以Cubic polynomials來規劃位置、速度、加速度連續的運動軌跡
2.以平面三個自由度機械手臂為例進行演算
與講師見面
林沛群教授(Professor)
機械工程學系(Department of Mechanical Engineering)
在我們剛剛前一段的影片中，我們是假設兩側的位置跟速度條件都是已知，
這樣等於說我們有四個已知條件之後，我們就去解這個三次的多項式的四個未知數。
那今天在真實的狀況下面，我們常常會看到說我今天如果 這是中間的一個
via point，就等於說我今天也是中間的 via point 的時候，
我位置可能是知道的，就等於說我每個 θi、 θi+1 是知道，
那我要怎麼定義中間 via points 的速度呢？要怎麼去選擇？
那等於說那個是事先所不知道的，那這個部分總共有好 幾個選擇的方式。
那第一個就是所謂等於說就直接定義了， 就等於說我們今天可能那個
via point 下面，如果說我們 今天本身的需求上面是有速度的需求的話，
我們就可以用這個方式直接先定義，那這個時候通常會發生在說，可能今天這個手臂 要通過那個
via 時候，需要以某個速度去穿過，它可能那個 感測器才可以讀，或者說它可以拍照或者怎麼樣的。
在某些這些特定的狀況下面，我們是可以去直接去定義。
那一般是不建議這樣做，因為這樣等於說除非是有需求，不然理論上應該讓系統自己把
適合的速度搞出來會比較簡單，那是直接定義的部分。
那第二個是我們就用一個簡單的法則去生成中間的速度。
那下面這個就是一個舉例，好，各位同學可以看到說這個圖上面， 我就有中間很多個
via points 對不對，等於說這裡有藍色的 就是它的 via points。
那我們定義的方式其實這裡是一個很簡單的 rule，就說
如果說我一開始的速度跟後段的速度是反向的， 比如說這裡的 ti、
ti-1 這個，這個部分， 從 ti-2 這個點到 ti-1，它的速度是正的，因為它的
θ 是增加的， 然後 ti-1 到 ti 的部分，速度變成負的，因為它的 θ 是減少。
假設我們的前後段速度反向， 那我們其實比較簡單的設定是說，那我們就假設說，它剛好在
通過 ti-1 的時候，它的速度是零，就等於說把這個點設成是水平的，
然後再去做規劃，這是一個 前後速度有變號的時候設定方法。
那假設如果前後的速度是沒有變號，只是有差異，比如說這裡 ti+1
的這個點， 我們從 ti 到 ti+1，它是緩上，我們等於說θ是慢慢增加，
但是過了 ti+1，从 ti+1 到 ti+2 之後，它的變動比較快，它是比較陡，陡上的。
基本上這兩個都是正值，那這個時候我們在 t i+1
的這個點上，速度要怎麼設定？基本上我們就是這兩個速度的平均。
所以各位同學可以看到說我後面的 θi+1 dot 就是直接是前面的速度
加上後面的速度平均，相加除以 2，來得到說 θi+1 dot 的這個速度是什麼。
那這個是第二個設定的方式， 那這是這兩個方式，那這兩個方式的好處是說，
今天在這些設定之下，我們今天來看每一段的時候， 等於說每一段的邊界條件就都已經已知了。
就等於說我今天一設定完，即使我今天想做 ti-1 到 ti
這一段， 我們的 θi-1 知道，θi-1 dot 知道， θi 知道，θi dot
也知道，就等於說可以直接就抓出來， 每一段每一段分開算，等於說這是一個很好的一個
一個條件，等於說可以讓我們每一段，每一段地分開來算。
這兩個設定的方式等於說直接定義，或者自動生成，都有這樣子的一個好處， 它可以獨立處理。
那這還有另外一個方式是說 我今天一路上，我們等於說一路上從前一段到下一段，
我們當然知道說本來的速度是希望是連續的，因為速度連續才是 圓滑的一個 function。
那今天由於說我們今天如果說，我們本來這個速度的條件不設定，
我們去多增加一個設定的方式是說，我設定說把這個 free
的這個參數拿來想成說 我希望過程中那個 θ dot
是一個特別剛好的 θ dot，剛好讓前段跟後段的加速度也相同，
那這樣等於說今天在這樣子的設定之下，我的軌跡的未知速度、 加速度就都會是連續的。
那這樣會讓系統運作的更好，因為加速度是連續的時候，
間接代表手臂的出力，手臂的出力可能就會比較連續一點，不會說加速度本來說從零
忽然開始有個轉折到某個定值，這樣子的話，代表說手臂要從本來不出力，瞬間出力之類的。
这等于说加速度連續之後，手臂的運作事實上是會更順暢的。
那這個方式等於說這是一個好處，它的好處就是說我今天 θi
dot，等於說某個速度不特別指定，我讓它系統自己去跑， 它會跑出一個適合的
θi dot， 那在這個適合θi下，我們的前段跟后段的加速度會是一樣。
那等於說我們這個方法來做的話，事實上是把這一個我們本來 需要設定的這個變數，變成是一個更好的一個利用方式。
那這個的壞處是說，因為等於說今天 它的邊界上面的這個
θ dot 是未知的，它必須跟前一段是有關，
就變成說它跟前一段就要合在一起解，那前一段又變成跟前前段有關，它跟前前段也要一起解。
往後的方式也是一樣，我們 θi+1 的 dot
是未知，它必須要跟下一段等於說 ti+1 到 ti+2 這一段一起解，
那以此類推就知道說，整個手臂 的所有的 θi dot 跟
θi dobule dot 都要全部一起解， 所以這個的壞處就是說今天這個
Polynomial 就變成是要是更 多個 Polynomial
去做聯立，而不是像剛剛一個三次式，三次式可以分開來做。
好，我們現在就來看看前面剛剛講的那個中間的 加速度可以自己調控的這個方式的話，我們要怎麼去做規劃？
那我們就舉一個比較簡單的例子，就像下面這裡寫的我們今天就是有一個 initial
在 t0，有一個 final 在 tf，然後中間只有一個 via point，我們設定成
t1， 那等於說我們這裡是有三個需要通過的點，在 t0 的時候，軌跡需要到
θ0， 在 t1 的時候，軌跡要到 θ1，在 tf 的時候，軌跡需要到
tf，我們只有三個點， 那三個點間接代表說我今天有兩個三次式需要去
生成出來，等於說總共就會有八個未知數。
那等於說我們今天中間的這個點的速度跟加速度，就希望用 我們剛剛講的方式去生成出來，那我們現在就來看看這要怎麼做？
首先我們先看第一段，就是等於說從 t0
到 t1 的這一段，時間上是這一段， 那跟剛剛講的一樣，我們先來對多項式去做一個平移。
所以我們設定一個 t tilde，它定義成 t-to，
那在這個平移之後，我們等於說本來的時間是從 to 到 t1，那現在是變成是從
0 到 Δt1， 那下面在這個左下角這個圖，我有畫出來說這個
Δt1 的 範圍到底是哪一段？那在這個設定之後，
我們等於說就開始建立一個第一段的三次多項式， 那這時候就有兩個下標了，前面的第一個下標代表是第一段，所以是
a10， 11、 12、 13，那第二個數字的 0、 1、 2、
3 代表 着 t 的冪次，是從 t 的 0 次到 1 次，到 2 次，到 3 次。
用這個方式去做一個對應，那這是我們第一段的部分， 那第二段的部分是從
t1 到 tf， 那等於說 t1 到 tf ，我們一樣設定一個
t tilde，是t-t1, 所以等於說我們今天在 t1 的時候，它會一樣跑到 0。
那在 tf 設定進來的時候，它會跑成 Δt2，
那有左下角的這個圖也可以看得出來，說這個 Δt2 的一個長度， 那我們設定的第二個三次多項式是
θ2， 那像剛剛講的第一個下標是第幾段的多項式，所以是第二段，
那第二個下標就是 t 的冪次，就等於說 0 次，1 次，2 次，3
次，我們等於說在這裡 總共有八個未知數，從 a10 到
a13， a20 到 a23，總共四加四有八個未知數需要求解。
這是我們一開始的設定，這裡剛剛講了有八個未知數。
好，那我們現在就開始把我們已知的條件 代入然後開始列出方程式。
我們首先我們知道說有兩段之後，每一段都各有兩個未知的邊界條件，
所以兩段加起來應該有四個條件。
那這個條件列出來就像是剛這個圖中所寫的， 我們前面兩個條件是第一段的條件，等於說在
θ0 的 一開始 t0 的時候，它需要到達 θ0 這個點，
那因爲我們在 t tilde 的座標下面，它就等於剛好等於 a10。
這也是爲什麼要平移的原因，因爲這樣等於說在 a10 直接就解出來了 那在
θ1 的時候，所謂的 via point 針對左側的 polynomial，它是右端點，這個
a10 11 12 13 是左側 的 polynomial 嘛，那它是右端點
θ1，所以它是等於全部，這個有一個 Δt1 這個 4 個項都有，但是這個
via point θ1 對於右側的 polynomial 來講，它是一個起點
對不對，那等於說它是 t1 它平移之後就變成 0，所以等於是剛好也等於
a20 所以對右側的 polynomial 來講，它是一個起點 那右側的
polynomial 的終點，就是我們所謂的 final point，它會到達 θf
它就等於說，等於說在右側那一個就把 Δt2 代進來
所以今天針對我們的位置條件，我們就可以寫出 4
個 多項式，那這個是一個聯立的狀況，是可以一起聯立來求解的
那我們來看一下開始跟 最後的速度條件，就是所謂的
boundary 等於說是我們定義了 θ0 dot，一開始的跟最後
tf 的時候的速度，所以是 θf dot 那我們在這裏是暫時先設定成
0，那真實上這兩個其實是 不一定是要是 0，如果說我們的軌跡的一開始是從
另外別的軌跡相接過來，我們事實上是可以給一開始 跟最後存在的某個速度的量值，它沒有一定是要
0 那這裏我們只是假想說它一開始是靜止的，最後也是靜止的，所以我們等於說先設定成
0 那今天 θ0 dot 等於說剛好是第一個式子的去微分，所以會得到
a11 那 θf dot 等於說是第二個式子微分之後代 Δt2
所以等於說是剩下 3 個項，那這裏也等於說我們的 頭尾的 boundary
等於說 t0 跟 tf 的 boundary 給了我們兩個條件 好，所以這裏有
6 個條件，那剩下的條件，就是我們剛剛想講的 我們藉由這個
free 的參數，希望說我們在 via point 上面 速度跟加速度都是連續
我們先來看速度的部分，就等於說我們今天 θ1 dot
的這個速度 由左側的多項式來算，跟由右側的多項式來算，都要是同一個值
才代表說它的速度上是連續嘛，那中間的這個部分就是由左側的多項式來
來算出來的速度，它基本上就是 θ1 的那個多項式對
t 去微分之後 代 Δt1 進來，所以等於說我們今天是代 Δt1 就等於說就剩下
3 個項，左側的部分是這樣 那針對右側的多項式的話，它也是本來的
θ2 的那個 函式對 t 去微分之後，然後代 t~
等於 0 所以就剩下 a21 一個項，所以這個是速度的
連續性所需要的條件，那我們真實在用的時候 事實上是用掉了是右邊的這個等號，左邊的這個等號是沒有使用到嘛，因爲我們事實上是
暫時還不知道 θ1 dot 是什麼，我們要的是右邊的這個
等號，讓左側的多項式跟右邊的多項式在 via point
的那個瞬間，它的速度是連續的 那加速度的部分的推導事實上也是相似的 我們假設加入就是一個 θ1
double dot，在 via point 上面 那從左側的多項式來看的話，它是那個
θ1 去微分兩次，然後再代 Δt1 進來
它微分兩次之後，前面兩個係數都 都不見了，然後是本來的二次項 就變成了這裏
2 的 a12，就變成一個常數 那本來的三次項，2
跟 3 都微下來了，所以變成 6a13 然後再乘上
Δt1，因爲對第一個多項式來講，它是右邊界，所以是代 Δt1
那對第二個多項式來講它是左邊界，所以兩次的 微分之後，又代
t~ 等於 0，所以等於說是剩下 2 的 a22，所以再等於說這個就是我們加速度的連續性的條件
那這個部分也像剛剛講的，我們用到的是右側的這個等號，因爲左側這邊我們不知道 θ1 double
dot 是什麼 我們藉由右側的等號就可以知道說，這兩個相等可以强迫左右兩段多項式在
via point 上面加速度是相連。
那我們今天這個等號，如果到時候爲了 方便計算，我們通常會把這個
a21 譬如說移到左側，另外一側變成 0 這個 2a22
移到左側，然後另外一側變成 0 ，就變成了兩個等於 0 的等式，這樣後續就可以
進行運算 好，那這裏這個
這樣條件寫完了之後，我們現在就可以看到說，我們今天的問題上面 我們總共有
8 個方程式，位置有 4 個 初始跟最終的邊界的速度條件有兩個
那中間 via point，一個 via point 就有兩個條件，一個是速度的，一個是加速度的，所以我們今天是
8 個方程式了，那照剛剛講的我們剛剛有 8 個未知數，那現在有
8 個方程式 那我們就可以直接進行聯立的求解
那這個是第一個解法，就等於說我們事實上是 直接用代數的方式去解聯立方程，然後就可以直接把它解出來
那這下面的解法事實上是爲了整個過程的簡化，我們就假設 Δt1
跟 Δt2 是 一樣的，就等於說我們今天的 via point 是在我們的
t0 跟 tf 的正中間 那在這個時候等於說方程式本身會比較簡化，那解出來解就像是下方這樣，大家可以看到說
a10 像剛剛想的一樣等於 θ0，a20 像剛剛想的一樣是
θ1 那 a11 理論上是一開始的速度，所以我們知道一開始速度設定是
0，所以它還是 0，等於說今天 8 個未知數裏面，有
5 個才是真正需要去解的 有 3 個可以在我們剛剛的 8
個條件裏面比較輕易地先找到，所以是 5 個一次方程式要去求解，它基本上是
可以算的，只是大家可以看到說，這裏算起來已經開始有一些複雜度 那這個是代數的解法，那今天這
8 個 方程式就如同剛剛的一個 polynomial 的 4 個方程式一樣
我們也可以用矩陣的方式來做，那矩陣的方式基本上就是把它排成 一個 8x8
的矩陣的等式 就像是這個圖中所秀的一樣。
那這裏我們可以看到說其實前面的 4 個條件
是位置的條件，中間的再來的第 5 個第 6個
θ0 dot 跟 θf dot 是 速度的最邊界條件，等於說是一開始跟最後
那最後的兩個 row 是我們剛剛講的在 via point
上面的速度的連續跟加速度的連續 那我們從前面
4 個未知的條件也可以知道說 我們今天代 t~ 等於
0 的時候是各自是 第一個多項式的左邊界跟第二個多項式的左邊界，所以各自要等於
θ0 跟 θ1，等於說是 a10 跟 a20 就直接很快地可以算出來
那我們整段軌跡的一開始的速度條件 θ0 dot，
因爲是第一段的左邊界的微分 所以等於說我們直接等於 a11，所以這裏是一個
01，統統 都是 0 的狀態，所以從這個矩陣也可以看出來說，今天有
3 個參數是直接就知道答案的 那剩下來才是需要求解的。
那這裏也可以看到說，我今天最後的兩個條件就像是我們剛剛講的
今天我們左側的速度跟右側的速度條件會搬到方程式的同一側 另外一側等於
0，所以才會看到這個第 7 個數字它是 0，因爲等於說我們把換成另外一側
我們的 a21 已經搬到跟 a11 什麼的在同一側
加速度也一樣，我們今天的 a22 的這個加速度的這個項已經在這個係數上
已經搬到另外一側，等於說是跟其他 a1 什麼的在同一側 然後它等於
0，所以這邊最後一個數字也才會是 0 那我們今天也看到說今天這是一個
8x8 的聯立要解 我們有 8 個未知數在最右側
a10 到 a23 有 8 個未知數要解 中間的這個
8x8 T 矩陣 它是已知的，因爲我們的 Δt1 跟
Δt2是已知，那跟剛剛一樣的結果，就是說今天中間這個矩陣 單純是時間的函數。
那最右側就是我們一開始所給的 邊界條件，包含位置的條件跟速度的條件。
那各位同學也可以看到說我們 所已知的 θ0 θ1
θf 還有我們的左右的速度的條件都用到了，通通都放進來了
還剩下中間 via 的速度跟加速度不知道，所以等於說也是在裏面需要求解才可以算出來的
那今天這個矩陣我們要求解要求得這個 a，也像剛剛一樣我們需要去找到它的
inverse 所以等於說我們必須先確認說中間這個
T 的矩陣是可以 有反矩陣，等於說它的 determinant 必須要不等於
0 那這裏我們可以看出來說今天這個的 determinant 會剛好等於後面的這個式子
看起來本來數字很多，化簡之後其實是非常的簡潔 它基本上是
4 倍的 Δt1 的四次方乘以 Δt2 的三次方加上 4
倍的反過來 那我們就可以發現說什麼時候 determinant
會不等於 0 呢 等於說基本上只要 t1 t2 本身不等於
0 就可以了 那這個的想法跟剛剛在單一的矩陣 Δt 不等於 0 其實是
有異曲同工之妙，事實上是背後是一樣的物理，就等於說 我們不可以設定
via point 跟 initial 是一樣的時間 或設定今天這個 via point 跟 final
是一樣的時間，因爲設定成一樣的時間 的時候，代表說那個時間它必須要到達
θ1 也要同時到達 θf，那等於說這個幾何上或者物理上是不合理的
就等於說我們今天只要 Δt1 跟 Δt2 是有值 代表說我的
via point 真正的時間點是落在兩個的中間 那理論上就可以解。
那當然說我們今天在設定的時候 Δt 理論上是都要正的啦
那當然說在數學上面 Δt 我們也可以把它想成是負的
所以在這裏如果說 Δt1 不等於 Δt2
的話 它基本上它也是可以算的，它也是會有解
那在這裏等於說這只是一個在數學上面看的條件，我們就不特別去 去強調它，因爲等於說我們一般在設定的時候
via point 會在中間 那這裏一看完就知道說，今天其實這個矩陣本身就有很強韌的特性說，它本身會存在着
inverse 所以我們就可以藉由一個 a 就等於 t 的inverse 乘以 θ 的方式
把我們的 8 個未知數 a，也就是所謂的兩個 polynomial 的
8 個係數把它算出來 那就等於說這就作爲一個例子，讓各位同學知道說在這個方法下面我們要怎麼去操作
