好,我們在之前的課程教了各位同學如何 去做linear
function跟有parabolic blends的這個軌跡規劃之後
那我們現在就來舉個例子,來說明說,這個真實上在 操作的時候要怎麼去做這個規劃。
那我們採用的例子就跟之前在做 cubic polynomials的時候一樣的例子,大家可以看到說,我們這裡有一個平面的三個旋-
轉自由度的手臂 那一樣,第一節的長度是4,第二節是3,第三節是1
那第三節其實,我們只是示意地來秀出它的一個姿態 那我們一樣用四個點,就是一個initial,
兩個via points 還有一個final point,我們希望說,我們的手臂能夠到達這四個點
那我們的時間,設定跟之前一樣,0秒、 2秒、 4秒、 7秒
那點的位置也跟之前一樣,所以有X的需求,有Y的需求跟 θ的需求,那θ就是第三桿對D的
角度,然後對D坐標,代表的是指對D坐標的X軸的角度 那我們為了簡化,為了讓我們的IK比較簡單一點,我們的這個X,
Y的點 指的就是一、 二桿,等於說joint 3的這個點,這就等於說是
第三桿的這個起點,並不是第三桿末端點的那個手的點,是第三桿的
這個起點,所以我們在做IK的時候,只要同時算出θ1跟θ2就好了 好,這是一個比較簡單的設計。
那之前也講過說,如果說我們今天給的點是 第三桿的末端點,那因為我們有整個第三桿的姿態需求
我們也可以借由這個姿態需求跟桿長,先把
第三桿的起始點的位置算出來,等於說,我們只是要從我們給的坐標 再多做一步個轉換就可以。
那這裡是目前,為了讓大家比較簡單一點 所以我們就把,我們的X,
Y的需求直接定義在 l1, l2, l2的末端點,或者說是第三桿的起點
那我們現在要用的就是我們剛教的方法 我們之前教了cubic polynomials,
那我們之前也直接試了在 Joint-space跟Cartesian-space下面,兩個的規劃都做了
那我們這裡,就是要我們新教的linear function with parabolic blends這個方式來做
那由於linear function主要說是為了在幾何空間下面有一個直線的
軌跡,所以這裡我們就不像之前,兩個space下面都試著規劃,我們就只有 做一個在Cartesian
space下面的軌跡規劃,這是我們現在要做的題目是這樣子 那首先,我們就知道說,我們已經有了
這三個自由度,我們等於說,現在在Cartesian下面去做規劃,所以等於說 我們是要在X,
Y跟θ下面做規劃,就不是θ1,θ2,θ3 那我們原本定義的initial,
final跟via points本來就是定義在 X,
Y跟θ下面,所以我們就不需要先做一個IK的動作,我們可以直接
就以我們這裡給定的這幾個點,開始各自去做規劃
那等於說我們是,譬如說,我們要先考慮X的時候,我們就把X這個column抓出來
我們知道說一開始在-4,然後在0,到3,到4 那等於說,就是X,
Y跟θ三個自由度是分開做 那我們首先第一步,要做Parabolic的這個方式的時候
我們第一步就要先找出,這個過程中
所有線段的速度跟加速度 那我們現在知道說,我們總共有四個點
所以我們知道說,四個點中間有三個直線段,所以 中間會有三個linear
function的速度段要求解 那我們有四個點,所以我們會有四個
二次項的段,就等於說,從靜止開始,就開始要先到二次段,然後到
直線段,到二次段,到最後,也是以二次段收尾 所以是四個二次段,裡面內夾著三個直線段
那我們現在就是要把這些段落的速度跟加速度算出來 這邊就是我像剛剛講的,我們一開始
在第一個點,就有一個一開始的加速度段,中間是接一個速度段,然後再一個加速度段
然後再一個等速段,加速段,等速段,最後是一個加速度段,或者說一個
減速段,讓它到靜止,那么一開始的速度,V0這邊也設成是0嘛,等於說
一開始是靜止,最後一個Vf的速度也設成是0,等於說它最後停在這個位置 所以等於說,我們總共有七個段落要做處理。
那我們的第一步就是先把 一路上的這幾個速度段的速度先算出來
那我們就以這個X來舉例好了,我們一開始知道說
V0,等於說第一段的X的速度是0,最後一段 Vf的速度是0,那中間的這三個V1,
V2 跟V3就要我們去算,那算法其實就是照定義來講的話
就是由後面的那個點的位置,減掉前面的那個點的 位置除上後面那個點的時間,減掉前面那個點的時間
簡單來講,就是δx除以δt,所以我們來看說
今天,我們的中間線段是這樣子的一個
算法,頭尾段其實也是,就等於說是一個δx除上δt
只是頭尾段,我們剛剛有講過說,因為頭尾段的時間上 我們速度段這個,等於說一開始的時候,這個時間要有一點內移
我們才能說,在今天在二次項的時候是一個圓滑的線段,所以等於說,頭尾段
的時間上面,除了本來的時間差之外,還要減掉我們內移的時間
那我們先來看中間段好了,可能大家比較不會混淆 那我們現在可以看到說,我們今天中間這一段的速度,就是所謂的
V2好了,我們的V2,就等於說,我們是從第三個點
減掉第二個點的位置,去除上第三個點的時間
減掉第二個點的時間,所以你可以看到說,我們第三個點的 位置在3,然後
第二個點的位置在0,所以分子就是3減0 那分母是,第三個點的時間是4
然後第二個點的時間是2,分母是4減2,所以是 3除以2,所以我們知道說,我們的V2就等於1.5,就像是這裡寫的
這個是,我們的規劃是這樣,就等於說 三個自由度就都是這樣算,剛剛舉的是X的
例子嘛,所以是3減0除上2,那如果是Y的例子就是3減3除上2,所以是0
那θ的例子就是30減掉45除上2,所以是-7.5,就是等於說中間
段都是一樣的算法,基本上分子就是我們需要的自由度,因為分母都是時間,那三個
自由度所需要的時間是一樣,所以分母是共用的,那我們就來看頭尾段
我們一樣以X來舉例好了,我們一開始要從-4
去走到0,所以我們是0減掉-4,我們是0去減掉
-4,那始時間,等於說是從2減掉0之外 還要減掉我們一開始第一個點,內移所需要的
時間的一半,就等於說,第一個點Parabolic需要0.5秒,我們需要它的一半,所- 以是2減0減掉
2分之0.5,所以第一個點這樣算出來之後,它就事實上就變成是 4除以1.75,4除以
1.75,所以最後得到的是一個2.29的數字 那最後一段其實是一樣,也等於說是4減3
然後是7減4,再減掉0.5,就等於說是,用這個方式就可以把我們所需要的速度都
算出來,那算完之後,我們可以知道說 第一個row就像我們的設定,它是靜止的,所以都是0
最後一個row是最後的速度,也是靜止的,那中間三個就是照著我們右側的這三個方程式
去把中間的那些速度段的直線等速的速度都算出來
那有了速度之後,我們也知道說,這些速度的差異 就是我們所需要的加速度,就等於說,一開始
V0等於0,然後經過我們的0.5秒之後,要到達V1,是2.29好了
那我們就知道說,那中間的這個加速度就是V1減 V0除上這個0.5秒,就可以得到這個a0
所以等於說,我們接下來算的加速度就直接是上面的相鄰的相減
再除上我們加速段所需要的0.5秒 所以加速段是,三個都是,四個加速度的分母都是0.5秒
這是我們先設定的,因為這裡我要先設定時間,也像是我之前講的,就說 我們今天借由一個設定時間的方式可以讓三個自由度
各自都能夠對應到一樣的 直線跟二次線的曲段,不會說,今天有了自由度還在直線上有自由度已經到
二次段,那我們在這樣的設定之下呢,我們其實就是相減除以0.5
所以像第一個,就是2.29減0除以0.5,就是2.29的兩倍,所以是4.58了,等- 於說基本上是有四捨五入過
那再來就是1.5減2.29再除以0.5,就基本上用這個方式去算出來
那所以各位同學可以看到說,我今天其實從速度到加速度,也是相鄰的相減再除上
一樣的時間就可以把它算出來了,那我們這個算完之後,其實間接就代表著
我們軌跡中所需要的速度跟加速度都變成是已知了
就等於說,我們其實方程式的係數就都知道 那變成說,下一步的關鍵就在於說,我要怎麼把整段的方程式
依據絕對的世界時間把它寫出來,那我們現在就來看,這個要怎麼做