各位同學大家好，那我們現在要開始進入軌跡規劃比較核心的內容 那在這一門課裡面，我們要教各位同學兩個軌跡規劃的方法 那第一個就是這一頁所 show 的所謂的 cubic polynomials，等於說是一個三次多項式的方式來規劃軌跡 那第二個是 linear function with parabolic blends，等於說基本上 是一個一次式跟二次式的多項式的一個復合架構 那兩個各有各的好處跟壞處，這個我們在最後軌跡規劃完之後我會做一個說明 那我們現在就針對這個第一個方法，我們開始來看一些細節 那我們想要的狀態，等於說是像下面來的這個圖所示，我們假設一開始 我們有個 initial condition, 就是所謂的 t0 可能軌跡，需要到某個點，這是圖中有畫一個點 那 final 在這裡，它也要到達某個點 那在中間，假設我就有很多很多的 via points, 所以各位同學我們可以看到說我這裡定義了很多個 via points 那要注意的是說我這裡所規劃的 θi 事實上是一個泛用的架構 它不一定要是 joint space 下面的 joint angle 它也可以是 cartesian 下面的 x, y, z 或者 cartesian 下面的三種姿態 是都可以，等於說這只是示意說我今天反正要針對某個自由度 或某個轉角或某個移動，開始做軌跡的規劃，那我們本來 先已知的條件就是這圖中所謂的藍點，就是我們需要通過的 那這里等於說，我們就開始說在針對這些藍點，我們就開始想辦法 拉一些線段把它串起來，那在這裡我們想要做的是像這樣子 我們等於說，今天在規劃的時候 我們的假想說我們今天，每兩個相鄰的點 我們就找一個函數把它串起來 那為了方便，我們等於說每兩個相鄰的點，就用一段自己的函數去串聯 那當然說為了方便，函數肯定都是同一個類型 但是每一個段的函數，用的是不同的參數去做 所以各位同學可以看到說，我這裡就舉例說我今天 針對 t i 到 t i+1 的這一段時間內，因為就是各有一個 via point 我就要找一個函數，把這兩個點串起來，就等於說這個圖中所畫的，就是紅框框裡面所串的 那因為我們又希望說這一段軌跡 串起來軌跡，跟下一段的軌跡，和這一段的軌跡跟上一段的軌跡 它都要滿足 smooth 的條件，那要滿足 smooth 的條件，間接代表說我今天 這個頭尾兩個點的位置跟速度條件要定義清楚 等於說位置條件也就是我們本來 via points 的條件，所以這個是本來就知道的 那速度條件變成說也要定義清楚，我們才能確保說我們 t i 到 t i+1 這一段的軌跡，能夠跟它前一段在速度上面是相連 還有跟它的後一段，速度上面是相連 所以這樣看起來知道說，我們這兩個點的邊界 總共需要提供四個條件才可以，兩個是位置的條件 兩個是速度的條件，總共有四個條件 那因為是四個條件的關係 那在 polynomial 下面我們就知道說我們必須要用三次的多項式 才夠，因為三次的 polynomial 它有四個參數可以讓我們選擇 所以等於說我們有四個參數來選擇，我們這裡有四個條件，才能夠有辦法 都找到一個對應，能夠把它解出來，那這個右方 其實就是一個簡表，跟各位同學說，我們今天從一次、 二次、 三次、 四次 到十次式的那個英文字是什麼，這個是常常看到，那 cubic 是三次式嘛，也是我們以前的立方體 所以這裡是三，寫在這裡。 那二次式也常常聽到是 quadratic 那四次式以上就比較少聽到了，quartic 這些以上都比較少聽到，那這個列在旁邊 供各位同學作參考，好 那我們現在就來針對剛剛講的那個三次式，來舉一個簡單的例子來 算算看，我們看這裡說，就是這裡列出來就是一個 polynomial 的三次式，那等於說 a0 加上 a₁t 加上 a₂t 加上 a₃t 那等於說它加過來後面的 t 的那個 階數越來越高，一次方、 二次方到三次方，所以等於說 這是一個三次的 polynomial 那它有四個未知數， a0 到 a₃ 那我們今天等於說，我們今天在做規劃的時候 針對每一個區段，就像剛剛有講的，我們的 t 假設我們這次做的是 t i 到 t i+1, 針對這一個區段 我們是事實上有做一個時間上面平移的動作，我們定義的 t~ 就等於 t 减 ti 那這樣變成一個 ~ 的好處是什麼呢？大家可以看到說 換成這個 t~ 的坐標之後，當我的時間在 t i 的時候 t~ 等於 0 嘛，因為等於說是 t i 減 t i 就等於 0 那在 t i+1 的時候，等於說 t i+1 減 t i，我們把它定義成 Δt 就等於說它是這兩個的時間差，那它是一個大於 0 ,它是一個正數 那經過這樣的轉換之後，可以看到说，我们原本的 t 的範圍是從 t i 到 t i+1 轉換過來之後，我的範圍就變成說從 0 到 Δt 那這個 0 其實會有很大的好處，我們等下往下看就可以看得到 我們可以看到說，我們今天轉換到新的坐標之後 我的邊界條件，就等於說邊界條件等於說我的 θ 要 代 t 等於 t i 的時候，那在 ~ 下面，t 代 t i 之後，就代表 t~ 等於 0 那等於說 t~ 的 0 代到我們上面的通式，代表說後面三個有 t 的項統統都變成 0 只剩下是一個 a0，所以各位同學可以清楚地看到說我今天的 a0 其實就 不用再去解，那等於說是借由我們第一個邊界條件就可以直接把 a0 找出來 那第二個邊界是 t 等於 t i+1 的時候 那 t 等於 t i+1 的時候，在 t~ 的坐標下面，我們知道說它是一個 Δt 的量值 所以等於說它的多項式的四個項是的確都存在，就是 a0 加上 a₁Δt 加上 a₂Δt² 加上 a₃Δt³ 這是四個項都存在的 那這是兩個位置的邊界條件，等於說 t 等於 t i 的時候，要滿足左側的邊界 它要到達 θ i 的這個位置 那 t 等于 t i+1 的时候，在右侧的边界条件，它要等於 θ i+1 的這個位置，那我們就把我們多項式串在一起 那當然說，現在a0, a ₁, a ₂, a ₃ 還沒有解 那 a ₁, a ₂, a ₃ 是未知，那 a0 是因為等於 θi 是已經是已知了 那我們下來看一下速度的條件 速度條件代表說我們今天是把 θ 這個通式去對 t~ 去做一個微分 那做一個微分之後， a0 就不見 只剩下 a ₁ 加上 a₂t~ 加上 a₃t~² 那這個時候當我們代 t ₁ 等於 t i 的時候，在 t~ 下面是 0 就等於說我們的 θi dot 這個速度，左側速度的條件就會剛好等於 a ₁ 所以等於說這個時候就會發現說，我們這個坐標平移的好處 等於說我們今天 a0 跟 a ₁ 都不用在聯立求解的狀態下面直接可以得知 等於說我們需要解的只有 a ₂ 跟a ₃ 了，那這個是左側的速度條件 那右側的速度條件就像是下面寫的這一個，是 θi+1 的 dot 那我們一微分之後，我們也知道說右側就剩下三個項，就像剛剛講的是a ₁ 加上 2a₂t~ 加上3a ₃t ~² 那 t~ 這時候代 t i+1 的時候又變成了 Δt 所以變成是 a ₁ 加上 2a ₂Δt 加上 3a ₃Δt ² 那這個就等於說我們這個四個條件配合 我們的三次的多項式所寫出來的方程式 那這裡可以看出來說，我們這裡理論上是一個 四元的 多項式，等於說我們有四個方程式，然後有四個未知數 但是我們今天這四個未知數裡面，有兩個其實借由其中兩個方程式，已經解出來 那當我們把這兩個已知的解，代入到剩下的兩個式子之後 我們事實上是二元一次的聯立解，就等於說我們是只要解兩個未知數 a ₂ 跟 a ₃ 就可以了。 所以等於說，這樣子的操作下面 就像下一頁講的，我們今天把它聯立重解之後，就可以把 a₂, a₃ 的 量值把它算出來。 那間接代表說我今天這四個 位置跟速度的條件，就可以把我今天中間的相連的三次式的四個係數都算出來 那這個就是我們今天用幾何的，用代數的一個聯立解的方式來做 那今天這一個聯立解的方式，我們也可以把它更有系統地 把它寫成矩陣的方式來解，就像是這裡一樣 那各位同學可以看到說我今天矩陣的一側 左側這邊，就是我們本來的 位置跟速度的條件，所以你看上面兩個是位置的，是 θi 跟 θi+1 然後是兩個速度的條件，θi dot 跟 θi+1 的 dot 那我們等號的右側就是剛剛 之前兩頁 PPT 所 show 的三次式的一個式子 那我們剛才知道說，我們今天第一個 θi 剛好會等於 a0，所以我們第一個 row 裡面就衹有 第一個數字裡頭衹是 1 0 0 0，就代表說它等號過來就是一個 1 乘上 a0，等於 a0 那第二個 row 裡面，我們 看到說它是右側的位置的邊界條件，是 θi+1 那剛剛由我們的三次式也知道說，它應該是等於是 a1 加上 a2 Δt 加上，抱歉，是 a0+a1 Δt a2 Δt 平方 + a3 Δt 三方，衹是現在 這個 Δt 的部分跟 a 的部分，我們以矩陣來寫，所以就拆開來成一個 row 跟一個 column，那下面的速度條件剛才知道說 θ1 dot 等於 a1 所以等於說這個 row 就變成了 0 1 0 0，那 column 一樣是四個數字 那 θi+1 dot 的條件，我們知道說剛剛微分之後的三次多項式變成二次的 然後會降階，所以 a0 效果就不見，剩下 a1 a2 a3 的 所以我們寫成矩陣之後，等於說我們現在的已知是 最左側的這個 column，類似於我們的位置跟速度的邊界條件，這個是已知 那中間的這個矩陣，我們發現說它單純是時間的函數 是 Δt 的函數，就等於說我們今天這個 polynomial 的左邊界跟右邊界 的時間差是多少，所以這個也是已知，它單純是時間的函數 那最右側的這個 column，a0 a1 a2 a3 這四個數字是未知的 這也是我們要求解的，所以等於說我們今天如果把它，矩陣把它簡化寫起來，是一個大 Θ 等於 T 跟 A 的相乘，那今天這個 A 的這個向量是未知的 那我們等於說，要未知 我們知道說，我們需要算 inverse，才能夠解出來說這個 A 是什麽 那要 inverse 要存在，我們也知道說必須先算出它的 determinant 到底有沒有等於 0，它必須要 determinant 不等於 0，它才有所謂的反矩陣 所以等於說，我們今天要先確認說今天 T 的這個矩陣到底它本身的特質是怎麽樣 那我們今天如果說把這個 T 的這個矩陣的 determinant 算出來之後，我們發現說它很漂亮地或者説很簡單地 剛好會等於 -Δt 的四次方 這間接代表著說，衹要我的 Δt 不等於 0 我這個 determinant 就不等於 0，就代表說這個矩陣有反矩陣存在，等於說我們就可以做 inverse 這個也是合理的，因爲 Δt=0 代表什麽？ 代表說我這個左邊界跟右邊界要發生在同一個時間點 那這個當然是不可能的嘛，就等於說我一開始假設在 θ=2 然後希望說經過某個時間之後，θ 到 3，那 Δt 不管是多少秒，衹要有一個秒差在 它就會等於說，我給它一個時間，它就可以跑過去，那今天如果 θ=2 跟 θ=3 都發生在同一個 時間點，我們知道說這個設定上面是有誤的，這是不可能發生的 所以等於說提前這個 determinant 條件其實還蠻合理的，衹要是 Δt 在 它就有辦法規劃出一個三次式，去滿足我們這個 Δt 以及這個邊界條件 好，所以等於說我們今天確認 T 可以 inverse 之後 我們就要求 A，那 A 其實很簡單的，就是 T 的 T 的 inverse 跟 Θ，大 Θ 去相乘 那這個因爲中間衹是單純的 t 的函數，我們就順便把 這個中間這個 t 的 4×4 的 inverse 的解析解寫給 各位同學，就像下面這樣子。 那各位同學也看到說，今天這個矩陣的前兩個 前兩個 row 非常的簡單，那後面兩個 row 是比較複雜 那這也對應到我們剛剛的結論，對不對，因爲我們剛才知道說 我們在原本，在代數解裡面就發現說，欸，我今天 a0 理論是要等於 θi，那我今天的 a1 要等於 要等於 θi dot，那剛剛方程式有看到這樣的特性 剛剛代數解有看到這樣的特性，我們現在的矩陣解也有看到這樣子的特性 那當然說 a2 a3 就比較複雜一點，它同時是這 四個位置跟速度條件的參數，然後今天在方程式裡面也可以看到 那這個矩陣解的這個，當我們把它乘開之後，這個 row 跟 column 乘開之後 我們發現就會得到跟剛剛代數解一樣的答案
