機(jī)器人軌跡規(guī)劃

機(jī)器人軌跡規(guī)劃第1頁，課件共31頁，創(chuàng)作于2023年2月2

軌跡規(guī)劃問題通常是將軌跡規(guī)劃器看成“黑箱”，接受表示路徑約束的輸入變量，輸出為起點(diǎn)和終點(diǎn)之間按時(shí)間排列的操作機(jī)中間形態(tài)（位姿,速度和加速度）序列。路徑約束動(dòng)力學(xué)約束路徑設(shè)定軌跡規(guī)劃器11.2機(jī)器人軌跡規(guī)劃的一般性問題

由初始點(diǎn)運(yùn)動(dòng)到終止點(diǎn)，所經(jīng)過的由中間形態(tài)序列構(gòu)成的空間曲線稱為路徑。第2頁，課件共31頁，創(chuàng)作于2023年2月3

規(guī)劃操作機(jī)的軌跡有兩種常用的方法：第一種方法：要求使用者在沿軌跡選定的位置點(diǎn)上(稱為結(jié)節(jié)或插值點(diǎn))顯式地給定廣義坐標(biāo)位置、速度和加速度的一組約束(例如，連續(xù)性和光滑程度等)。然后，軌跡規(guī)劃器從插值和滿足插值點(diǎn)約束的函數(shù)中選定參數(shù)化軌跡。顯然，在這種方法中，約束的給定和操作機(jī)軌跡規(guī)劃是在關(guān)節(jié)坐標(biāo)系中進(jìn)行的。第二種方法：使用者以解析函數(shù)顯式地給定操作機(jī)必經(jīng)之路徑，例如，笛卡爾坐標(biāo)中的直線路徑。然后，軌跡規(guī)劃器在關(guān)節(jié)坐標(biāo)或笛卡幾坐標(biāo)中確定一條與給定路徑近似的軌跡。在這種方法中，路徑約束是在笛卡爾坐標(biāo)中給定的。第3頁，課件共31頁，創(chuàng)作于2023年2月4

在第一種方法中，約束的給定和操作機(jī)軌跡規(guī)劃在關(guān)節(jié)坐標(biāo)系中進(jìn)行。由于對(duì)操作機(jī)手部沒有約束，使用者難于跟蹤操作機(jī)手部運(yùn)行的路徑。因此，操作機(jī)手部可能在沒有事先警告的情況下與障礙物相碰。在第二種方法中，路徑約束在笛卡爾坐標(biāo)中給定，而關(guān)節(jié)驅(qū)動(dòng)器是在關(guān)節(jié)坐標(biāo)中受控制的。因此，為了求得一條逼近給定路徑的軌跡，必須用函數(shù)近似把笛卡爾坐標(biāo)中的路徑約束變換為關(guān)節(jié)坐標(biāo)中的路徑約束，再確定滿足關(guān)節(jié)坐標(biāo)路徑約束的參數(shù)化軌跡。軌跡規(guī)劃既可在關(guān)節(jié)變量空間中進(jìn)行，也可在笛卡爾空間進(jìn)行。對(duì)于關(guān)節(jié)變量空間的規(guī)劃，要規(guī)劃關(guān)節(jié)變量的時(shí)間函數(shù)及其前二階時(shí)間導(dǎo)數(shù)，以便描述操作機(jī)的預(yù)定運(yùn)動(dòng)。在笛卡爾空間規(guī)劃中，要規(guī)劃操作機(jī)手部位置、速度和加速度的時(shí)間函數(shù)，而相應(yīng)的關(guān)節(jié)位置、速度和加速度可根據(jù)手部信息導(dǎo)出。第4頁，課件共31頁，創(chuàng)作于2023年2月5

面向笛卡爾空間方法的優(yōu)點(diǎn)是概念直觀，而且沿預(yù)定直線路徑可達(dá)到相當(dāng)?shù)臏?zhǔn)確性。可是由于現(xiàn)代還沒有可用笛卡爾坐標(biāo)測(cè)量操作機(jī)手部位置的傳感器，所有可用的控制算法都是建立在關(guān)節(jié)坐標(biāo)基礎(chǔ)上的。因此，笛卡爾空間路徑規(guī)劃就需要在笛卡爾坐標(biāo)和關(guān)節(jié)之間進(jìn)行實(shí)時(shí)變換，這是一個(gè)計(jì)算量很大的任務(wù)，常常導(dǎo)致較長的控制間隔。由笛卡爾坐標(biāo)向關(guān)節(jié)坐標(biāo)的變換是病態(tài)的，因而它不是一一對(duì)應(yīng)的映射。如果在軌跡規(guī)劃階段要考慮操作機(jī)的動(dòng)力學(xué)特性，就要以笛卡爾坐標(biāo)給定路徑約束，同時(shí)以關(guān)節(jié)坐標(biāo)給定物理約束(例如，每個(gè)關(guān)節(jié)電機(jī)的力和力矩、速度和加速度權(quán)限)。這就會(huì)使最后的優(yōu)化問題具有在兩個(gè)不同坐標(biāo)系中的混合約束。在笛卡爾空間進(jìn)行軌跡規(guī)劃的特點(diǎn)：第5頁，課件共31頁，創(chuàng)作于2023年2月6

在關(guān)節(jié)變量空間的規(guī)劃有三個(gè)優(yōu)點(diǎn)：直接用運(yùn)動(dòng)時(shí)的受控變量規(guī)劃軌跡；軌跡規(guī)劃可接近實(shí)時(shí)地進(jìn)行；關(guān)節(jié)軌跡易于規(guī)劃。伴隨的缺點(diǎn)是難于確定運(yùn)動(dòng)中各桿件和手的位置，但是，為了避開軌跡上的障礙．常常又要求知道一些桿件和手位置。

由于面向笛卡爾空間的方法有前述鐘種缺點(diǎn)，使得面向關(guān)節(jié)空間的方法被廣泛采用。它把笛卡爾結(jié)點(diǎn)變換為相應(yīng)的關(guān)節(jié)坐標(biāo)，并用低次多項(xiàng)式內(nèi)插這些關(guān)節(jié)結(jié)點(diǎn)。這種方法的優(yōu)點(diǎn)是計(jì)算較快，而且易于處理操作機(jī)的動(dòng)力學(xué)約束。但當(dāng)取樣點(diǎn)落在擬合的光滑多項(xiàng)式曲線上時(shí)，面向關(guān)節(jié)空間的方法沿笛卡爾路徑的準(zhǔn)確性會(huì)有損失。第6頁，課件共31頁，創(chuàng)作于2023年2月7一般說來，生成關(guān)節(jié)軌跡設(shè)定點(diǎn)的基本算法是很簡單的：循環(huán)：等待下一個(gè)控制間隔；

時(shí)刻操作機(jī)關(guān)節(jié)所處的位置；若，則退出循環(huán)；轉(zhuǎn)向循環(huán)。

從上述算法可以看出，要計(jì)算的是在每個(gè)控制間隔中必須更新的軌跡函數(shù)(或軌跡規(guī)劃器)h(t)。因此，對(duì)規(guī)劃的軌跡要提出四個(gè)限制。第一，必須便于用迭代方式計(jì)算軌跡設(shè)定點(diǎn)；第二，必須求出并明確給定中間位置；第三，必須保證關(guān)節(jié)變量及其前二階時(shí)間導(dǎo)數(shù)的連續(xù)性，使得規(guī)劃的關(guān)節(jié)軌跡是光滑的；最后，必須減少額外的運(yùn)動(dòng)(例如，“游移”)。11.3關(guān)節(jié)插值軌跡第7頁，課件共31頁，創(chuàng)作于2023年2月8

若某關(guān)節(jié)(例如關(guān)節(jié)i)的關(guān)節(jié)軌跡使用p個(gè)多項(xiàng)式，則要滿足初始和終止條件(關(guān)節(jié)位置、速度和加速度)，并保證這些變量在多項(xiàng)式銜接處的連續(xù)性，因而需要確定3(p十1)個(gè)系數(shù)。若給定附加的中間條件(例如位置)，則對(duì)每個(gè)中間條件需要增加一系數(shù)。通常，可以給定兩個(gè)中間位置，一個(gè)靠近初始位置；另一個(gè)靠近終止位置。這樣做，除了可以較好地控制運(yùn)動(dòng)外，還能保證操作機(jī)末端以適當(dāng)?shù)姆较螂x開起點(diǎn)和接近終點(diǎn)。因此，對(duì)于連接初始位置和終止位置的每個(gè)關(guān)節(jié)變量，一個(gè)七次多項(xiàng)式就足夠了，或者用兩段四次軌跡加一段三次軌跡（4—3—4），也可以用兩段三次軌跡加一段五次軌跡（3—5—3），或五段三次軌跡（3—3—3—3—3）。第8頁，課件共31頁，創(chuàng)作于2023年2月9

為了控制操作機(jī)，在規(guī)劃運(yùn)動(dòng)軌跡之前，需要給定機(jī)器人在初始點(diǎn)和終止點(diǎn)的手臂形態(tài)。在規(guī)劃機(jī)器人關(guān)節(jié)插值運(yùn)動(dòng)軌跡時(shí)，需要注意下述幾點(diǎn)：1、抓住一個(gè)物體時(shí)，手的運(yùn)動(dòng)方向應(yīng)該指向離開物體支承表面的方向。否則，手可能與支承面相碰。2、若沿支承面的法線方向從初始點(diǎn)向外給定一個(gè)離開位置(提升點(diǎn))，并要求手(即手部坐標(biāo)系的原點(diǎn))經(jīng)過此位置，這種離開運(yùn)動(dòng)就是允許的。如果還給定由初始點(diǎn)運(yùn)動(dòng)到離開位置的時(shí)間，我們就可以控制提起物體運(yùn)動(dòng)的速度。3、對(duì)于手臂運(yùn)動(dòng)提升點(diǎn)的要求同樣也適用于終止位置運(yùn)動(dòng)的下放點(diǎn)(即必須先運(yùn)動(dòng)到支承表面外法線方向上的某點(diǎn)，再慢慢下移至終止點(diǎn))。這樣，可獲得和控制正確的接近方向。4、對(duì)手臂的每一次運(yùn)動(dòng)，都設(shè)定上述四個(gè)點(diǎn)：初始點(diǎn)，提升點(diǎn)，下放點(diǎn)和終止點(diǎn)。第9頁，課件共31頁，創(chuàng)作于2023年2月105、位置約束

(a)初始點(diǎn)：給定速度和加速度(一般為零)；

(b)提升點(diǎn)：中間點(diǎn)運(yùn)動(dòng)的連續(xù)；

(c)下放點(diǎn)：同提升點(diǎn)；

(d)終止點(diǎn)：給定速度和加速度(一般為零)。第10頁，課件共31頁，創(chuàng)作于2023年2月116、除上述約束外，所有關(guān)節(jié)軌跡的極值不能超出每個(gè)關(guān)節(jié)變量的物理和幾何極限。7、時(shí)間的考慮

(a)軌跡的初始段和終止段：時(shí)間由手接近和離開支承表面的速率決定；也是由關(guān)節(jié)電機(jī)特性決定的某個(gè)常數(shù)。

(b)軌跡的中間點(diǎn)或中間段：時(shí)間由各關(guān)節(jié)的最大速度和加速度決定，將使用這些時(shí)間中的一個(gè)最長時(shí)間(即用最低速關(guān)節(jié)確定的最長時(shí)間來歸一化)。

在關(guān)節(jié)軌跡的典型約束條件之下，我們所要研究的是選擇一種n次(或小于n次)的多項(xiàng)式函數(shù)，使得在各結(jié)點(diǎn)(初始點(diǎn)，提升點(diǎn)，下放點(diǎn)和終止點(diǎn))上滿足對(duì)位置、速度和加速度的要求，并使關(guān)節(jié)位置、速度和加速度在整個(gè)時(shí)間間隔[t0,tf]中保持連續(xù)。第11頁，課件共31頁，創(chuàng)作于2023年2月121.位置(給定)2．速度(給定，通常為零)3.加速度(給定，通常為零)4.提升點(diǎn)位置(給定)5．提升點(diǎn)位置（與前一段軌跡連續(xù)）

6.速度（與前一段軌跡連續(xù)）

7．加速度（與前一段軌跡連續(xù)）

8.下放點(diǎn)位置(給定)9．下放點(diǎn)位置（與前一段軌跡連續(xù)）

11.速度（與前一段軌跡連續(xù)）

11.加速度（與前一段軌跡連續(xù)）

12.位置(給定)13.速度(給定，通常為零)14.加速度(給定，通常為零)終止位置中間位置初始位置

規(guī)劃關(guān)節(jié)插值軌跡的約束條件：第12頁，課件共31頁，創(chuàng)作于2023年2月13一種方法是為每個(gè)關(guān)節(jié)規(guī)定一個(gè)七次多項(xiàng)式函數(shù)式中，未知系數(shù)aj可由已知的位置和連續(xù)條件確定。但用這種高次多項(xiàng)式內(nèi)插給定的結(jié)點(diǎn)也許不能令人滿意，因?yàn)樗臉O值難求，而且容易產(chǎn)生額外的運(yùn)動(dòng)。另一種辦法是將整個(gè)關(guān)節(jié)空間軌跡分割成幾段，在每段軌跡中用不同的低次多項(xiàng)式來插值。有幾種分割軌跡的方法，每種方法的特性各不相同。4—3—4軌跡每個(gè)關(guān)節(jié)有下面三段軌跡：第一段由初始點(diǎn)到提升點(diǎn)的軌跡用四次多項(xiàng)式表示。第二段(或中間段）由提升點(diǎn)到下放點(diǎn)的軌跡用三次多項(xiàng)式表示。最后一段由下放點(diǎn)到終止點(diǎn)的軌跡由四次多項(xiàng)式表示。第13頁，課件共31頁，創(chuàng)作于2023年2月14

3—5—3軌跡每個(gè)關(guān)節(jié)也有下面三段軌跡：第一段用三次多項(xiàng)式，第二段用五次多項(xiàng)式，最后一段用三次多項(xiàng)式。

(3—3—3—3—3)軌跡再增加二個(gè)中間點(diǎn)，且對(duì)五段軌跡都使用三次多項(xiàng)式樣條函數(shù)。注意，上述討論對(duì)每個(gè)關(guān)節(jié)軌跡都是有效的，即每個(gè)關(guān)節(jié)軌跡可分割成三段或五段。11.44—3—4關(guān)節(jié)軌跡的計(jì)算

由于在每段軌跡中要確定N條關(guān)節(jié)軌跡，引用歸一化時(shí)間變量是方便的，它使我們能用同樣的方法處理每個(gè)關(guān)節(jié)每段軌跡的方程。時(shí)間變化范圍均由(各段軌跡的初始時(shí)間)變到(各段軌跡的終止時(shí)間)。第14頁，課件共31頁，創(chuàng)作于2023年2月15定義下列變量：

軌跡是由多項(xiàng)式序列hi(t)構(gòu)成的，這些多項(xiàng)式合起來形成關(guān)節(jié)j的軌跡。在每段軌跡中關(guān)節(jié)變量的多項(xiàng)式用歸一化時(shí)間表示為:第15頁，課件共31頁，創(chuàng)作于2023年2月16這些關(guān)節(jié)軌跡分段多項(xiàng)式所應(yīng)滿足的邊界條件為:第16頁，課件共31頁，創(chuàng)作于2023年2月174—3—4關(guān)節(jié)軌跡的邊界條件如圖示。第17頁，課件共31頁，創(chuàng)作于2023年2月18這些多項(xiàng)式對(duì)實(shí)際時(shí)間t的一階和二階導(dǎo)數(shù)?？蓪懗傻谝欢诬壽E的基本多項(xiàng)式是四次的第18頁，課件共31頁，創(chuàng)作于2023年2月19對(duì)于t=0(這段軌跡的初始位置)，要滿足此位置的邊界條件，則第19頁，課件共31頁，創(chuàng)作于2023年2月20用求出的這些未知量得到對(duì)于t=1(這段軌跡的終點(diǎn))，在此位置，我們放寬插值多項(xiàng)式必須準(zhǔn)確通過該點(diǎn)的要求，僅要求在此位置的速度和加速度必須與下一段軌跡起點(diǎn)的速度和加速度連續(xù)。此處的速度和加速度為第二段軌跡的基本多項(xiàng)式是三次的第20頁，課件共31頁，創(chuàng)作于2023年2月21對(duì)于t=0(提升點(diǎn))，此點(diǎn)的速度和加速度分別為由于此點(diǎn)的速度和加速度必須分別與前一段軌跡終點(diǎn)的速度和加速度連續(xù)，故可得第21頁，課件共31頁，創(chuàng)作于2023年2月22或或

對(duì)于t=1(下放點(diǎn))，該點(diǎn)的速度和加速度必須與下一段軌跡起點(diǎn)處的速度和加速度連續(xù)?？煞謩e求得此位置的速度和加速度為第22頁，課件共31頁，創(chuàng)作于2023年2月23末段軌跡的基本多項(xiàng)式是四次的如果在上式中以代替t，我們就把歸一化時(shí)間t由

移至。第23頁，課件共31頁，創(chuàng)作于2023年2月24可得對(duì)于

(這段軌跡的終點(diǎn))，要滿足軌跡終點(diǎn)的邊界條件，即對(duì)于

(這段軌跡的起點(diǎn))，要滿足軌跡起點(diǎn)的邊界條件，即第24頁，課件共31頁，創(chuàng)作于2023年2月25在此下放點(diǎn)的速度和加速度連續(xù)性條件是：或第25頁，課件共31頁，創(chuàng)作于2023年2月26可以求出相連軌跡段間關(guān)節(jié)角之差為

軌跡多項(xiàng)式其余未知系數(shù)可由以上的速度、加速度連續(xù)約束條件的聯(lián)立方程解出，用矩陣矢量符號(hào)改寫這些方程，可得其中第26頁，課件共31頁，創(chuàng)作于2023年2月27或C矩陣的結(jié)構(gòu)便于計(jì)算未知系數(shù)，若時(shí)間間隔ti

為正值，C的逆矩陣總