第四章,軌跡規(guī)劃

1、第4章機(jī)器人軌跡規(guī)劃本章在操作臂運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)的基礎(chǔ)上，討論在關(guān)節(jié)空間和笛卡爾空間中機(jī)器人運(yùn)動(dòng) 的軌跡規(guī)劃和軌跡生成方法。所謂軌跡，是指操作臂在運(yùn)動(dòng)過(guò)程中的位移、速度和加速度。 而軌跡規(guī)劃是根據(jù)作業(yè)任務(wù)的要求，計(jì)算出預(yù)期的運(yùn)動(dòng)軌跡。首先對(duì)機(jī)器人的任務(wù)，運(yùn)動(dòng) 路徑和軌跡進(jìn)行描述，軌跡規(guī)劃器可使編程手續(xù)簡(jiǎn)化，只要求用戶輸入有關(guān)路徑和軌跡的 若干約束和簡(jiǎn)單描述，而復(fù)雜的細(xì)節(jié)問(wèn)題則由規(guī)劃器解決。例如，用戶只需給出手部的目 標(biāo)位姿，讓規(guī)劃器確定到達(dá)該目標(biāo)的路徑點(diǎn)、持續(xù)時(shí)間、運(yùn)動(dòng)速度等軌跡參數(shù)。并且，在 計(jì)算機(jī)內(nèi)部描述所要求的軌跡，即選擇習(xí)慣規(guī)定及合理的軟件數(shù)據(jù)結(jié)構(gòu)。最后，對(duì)內(nèi)部描 述的軌跡、實(shí)時(shí)計(jì)算機(jī)器

2、人運(yùn)動(dòng)的位移、速度和加速度，生成運(yùn)動(dòng)軌跡。4.1機(jī)器人軌跡規(guī)劃概述一、機(jī)器人軌跡的概念機(jī)器人軌跡泛指工業(yè)機(jī)器人在運(yùn)動(dòng)過(guò)程中的運(yùn)動(dòng)軌跡，即運(yùn)動(dòng)點(diǎn)的位移、速度和加速 度。機(jī)器人在作業(yè)空間要完成給定的任務(wù)，其手部運(yùn)動(dòng)必須按一定的軌跡(trajectory)進(jìn)行。軌跡的生成一般是先給定軌跡上的若干個(gè)點(diǎn)，將其經(jīng)運(yùn)動(dòng)學(xué)反解映射到關(guān)節(jié)空間，對(duì)關(guān)節(jié) 空間中的相應(yīng)點(diǎn)建立運(yùn)動(dòng)方程，然后按這些運(yùn)動(dòng)方程對(duì)關(guān)節(jié)進(jìn)行插值，從而實(shí)現(xiàn)作業(yè)空間 的運(yùn)動(dòng)要求，這一過(guò)程通常稱(chēng)為軌跡規(guī)劃。工業(yè)機(jī)器人軌跡規(guī)劃屬于機(jī)器人低層規(guī)劃，基 本上不涉及人工智能的問(wèn)題，本章僅討論在關(guān)節(jié)空間或笛卡爾空間中工業(yè)機(jī)器人運(yùn)動(dòng)的軌 跡規(guī)劃和軌跡生成方法。機(jī)

3、器人運(yùn)動(dòng)軌跡的描述一般是對(duì)其手部位姿的描述，此位姿值可與關(guān)節(jié)變量相互轉(zhuǎn)換。 控制軌跡也就是按時(shí)間控制手部或工具中心走過(guò)的空間路徑。二、軌跡規(guī)劃的一般性問(wèn)題通常將操作臂的運(yùn)動(dòng)看作是工具坐標(biāo)系T相對(duì)于工件坐標(biāo)系S的一系列運(yùn)動(dòng)。這種描述方法既適用于各種操作臂，也適用于同一操作臂上裝夾的各種工具。對(duì)于移動(dòng)工作臺(tái)(例如傳送帶)，這種方法同樣適用。這時(shí)，工作坐標(biāo) S 位姿隨時(shí)間而變化。例如，圖4.1所示將銷(xiāo)插入工件孔中的作業(yè)可以借助工具坐標(biāo)系的一系圖4.1機(jī)器人將銷(xiāo)插入工件孔中的作業(yè)描述列位姿Pi（i=i, 2，n）來(lái)描述。這種描述方法不僅符合機(jī)器人用戶考慮問(wèn)題的思路，而 且有利于描述和生成機(jī)器人的運(yùn)動(dòng)軌

4、跡。用工具坐標(biāo)系相對(duì)于工件坐標(biāo)系的運(yùn)動(dòng)來(lái)描述作業(yè)路徑是一種通用的作業(yè)描述方法。 它把作業(yè)路徑描述與具體的機(jī)器人、手爪或工具分離開(kāi)來(lái)，形成了模型化的作業(yè)描述方法， 從而使這種描述既適用于不同的機(jī)器人，也適用于在同一機(jī)器人上裝夾不同規(guī)格的工具。 在軌跡規(guī)劃中，為敘述方便，也常用點(diǎn)來(lái)表示機(jī)器人的狀態(tài)，或用它來(lái)表示工具坐標(biāo)系的 位姿，例如起始點(diǎn)、終止點(diǎn)就分別表示工具坐標(biāo)系的起始位姿及終止位姿。對(duì)點(diǎn)位作業(yè)（pick and place operation）的機(jī)器人（如用于上、下料），需要描述它的起始狀 態(tài)和目標(biāo)狀態(tài)，即工具坐標(biāo)系的起始值 TO。目標(biāo)值Tf。在此，用“點(diǎn)”這個(gè)詞表示工具 坐標(biāo)系的位置和姿態(tài)

5、（簡(jiǎn)稱(chēng)位姿），例如起始點(diǎn)和目標(biāo)點(diǎn)等。對(duì)于另外一些作業(yè)，如弧焊和曲面加工等，不僅要規(guī)定操作臂的起始點(diǎn)和終止點(diǎn)，而且要指明兩點(diǎn)之間的若干中間點(diǎn)（稱(chēng)路徑點(diǎn)），必須沿特定的路徑運(yùn)動(dòng)（路徑約束）。這類(lèi)稱(chēng)為連續(xù)路徑運(yùn)動(dòng)（continuous Path motion）或輪廓運(yùn)動(dòng)（contour motion），而前者稱(chēng)點(diǎn)到點(diǎn)運(yùn)動(dòng) （PTP = pointtopoint motion）。在規(guī)劃?rùn)C(jī)器人的運(yùn)動(dòng)時(shí).還需要弄清楚在其路徑上是否存在障礙物（障礙約束）。路徑約束和障礙約束的組合將機(jī)器人的規(guī)劃與控制方式劃分為四類(lèi)、如表4-1所示。表4.1機(jī)器人的規(guī)劃與控制方式障礙約束有無(wú)路徑約束有離線無(wú)碰撞路徑規(guī)則+在線路

6、徑跟蹤離線路徑規(guī)劃+在線路徑跟蹤無(wú)位置控制+在線障礙探測(cè)和避障1位置控制越徑約束F14門(mén) T fto路徑設(shè)定軌跡規(guī)劃器I 時(shí)，則修正最后一步的步長(zhǎng)A0,并以如表示，日7 7，故平面圓弧位置插補(bǔ)為Xj 1 =Xj cos ：v -Y sin 二口Y 1 =丫 cos v Xj sin 二口2. 空間圓弧插補(bǔ)問(wèn)題?？臻g圓弧是指三維空間任一平面內(nèi)的圓弧，此為空間一般平面的圓弧 空間圓弧插補(bǔ)可分三步來(lái)處理：(1) 把三維問(wèn)題轉(zhuǎn)化成二維，找出圓弧所在平面。(2) 利用二維平面插補(bǔ)算法求出插補(bǔ)點(diǎn)坐標(biāo)(Xi+i, Yi+i)。 把該點(diǎn)的坐標(biāo)值轉(zhuǎn)變?yōu)榛A(chǔ)坐標(biāo)系下的值，如圖4.7所示。通過(guò)不在同一直線上的三點(diǎn)P

7、i、P2、P3可確定一個(gè)圓及三點(diǎn)間的圓弧，其圓心為Or,半徑為R,圓弧所在平面與基礎(chǔ)坐標(biāo)系平面的交線分別為AB、BC、CA。建立圓弧平面插補(bǔ)坐標(biāo)系，即把OrXrYrZr坐標(biāo)系原點(diǎn)與圓心 Or重合，設(shè)OrXrYrZr平面為圓弧所在平面，且保持 Zr為外法線方向。這樣，一個(gè)三維問(wèn)題就轉(zhuǎn)化成平面問(wèn)題，可 以應(yīng)用平面圓弧插補(bǔ)的結(jié)論。求解兩坐標(biāo)系(圖4.7)的轉(zhuǎn)換矩陣。令TR表示由圓弧坐標(biāo) OrXrYrZr至基礎(chǔ)坐標(biāo)系OX0Y0Z0的轉(zhuǎn)換矩陣。若Zr軸與基礎(chǔ)坐標(biāo)系Zo軸的夾角為G, Xr軸與基礎(chǔ)坐標(biāo)系的夾角為則可完成下述步驟：將XrYrZr的原點(diǎn)Or放到基礎(chǔ)原點(diǎn) 0上；繞Zr軸轉(zhuǎn)使Xo與Xr平行；再繞

8、Xr軸轉(zhuǎn)：角，使Zo與Zr平行。這三步完成了 XrYrZr向XoYoZo的轉(zhuǎn)換，故總轉(zhuǎn)換矩陣應(yīng)為T(mén) R 二 T (Xr,Yor,Zr) R(Zc) R(X,：J式中:-si n 日 cos 日si nTcos 日XOr1cosG cosacosGsin aYorsin acosaZOr001一COS J sin日00Or在基礎(chǔ)坐標(biāo)系下的坐標(biāo)值。(4.1)-cos0si nO0(Xor COS0 +Yg sind )T丄一 r-sin Ocos 日cosT cosasi na (XorsinGcosa +YdR co的cosasina )lxsi nTsi not-cosBsin acosa (

9、 XoRsinTsin a+YoR cosBsin a+ZoRcosa )0001 一XOr、Yr ZoR 為圓心欲將基礎(chǔ)坐標(biāo)系的坐標(biāo)值表示在OrXrYrZr坐標(biāo)系，則要用到 Tr的逆矩陣Pi Pi+1= Vts(4.2)Pi Pi+1= Vts(4.2)三、定時(shí)插補(bǔ)與定距插補(bǔ)由上述可知，機(jī)器人實(shí)現(xiàn)一個(gè)空間軌跡的過(guò)程即是實(shí)現(xiàn)軌跡離散的過(guò)程，如果這些離 散點(diǎn)間隔很大，則機(jī)器人運(yùn)動(dòng)軌跡與要求軌跡可能有較大誤差。只有這些插補(bǔ)得到的離散 點(diǎn)彼此距離很近，才有可能使機(jī)器人軌跡以足夠的精確度逼近要求的軌跡。模擬CP控制實(shí)際上是多次執(zhí)行插補(bǔ)點(diǎn)的 PTP控制，插補(bǔ)點(diǎn)越密集，越能逼近要求的軌跡曲線。插補(bǔ)點(diǎn)要多么

10、密集才能保證軌跡不失真和運(yùn)動(dòng)連續(xù)平滑呢？可采用定時(shí)插補(bǔ)和定距插 補(bǔ)方法來(lái)解決。1.定時(shí)插補(bǔ)從圖4.3所示的軌跡控制過(guò)程知道，每插補(bǔ)出一軌跡點(diǎn)的坐標(biāo)值，就要轉(zhuǎn)換成相應(yīng)的關(guān) 節(jié)角度值并加到位置伺服系統(tǒng)以實(shí)現(xiàn)這個(gè)位置，這個(gè)過(guò)程每隔一個(gè)時(shí)間間隔ts完成一次。為保證運(yùn)動(dòng)的平穩(wěn)，顯然 ts不能太長(zhǎng)。由于關(guān)節(jié)型機(jī)器人的機(jī)械結(jié)構(gòu)大多屬于開(kāi)鏈?zhǔn)?，剛度不高，ts一般不超過(guò)25 ms(40 Hz)，這樣就產(chǎn)生了 ts的上限值。當(dāng)然ts越小越好，但它的下限值受到計(jì)算量限制，即對(duì)于機(jī)器人的控制，計(jì)算機(jī)要在ts時(shí)間里完成一次插補(bǔ)運(yùn)算和一次逆向運(yùn)動(dòng)學(xué)計(jì)算。對(duì)于目前的大多數(shù)機(jī)器人控制器，完成這樣一次計(jì)算約需幾毫秒。這樣產(chǎn)生了

11、ts的下限值。當(dāng)然，應(yīng)當(dāng)選擇ts接近或等于它的下限值，這樣可保證較高的軌跡精度和平滑的運(yùn)動(dòng)過(guò)程。以一個(gè)XOY平面里的直線軌跡為例說(shuō)明定時(shí)插補(bǔ)的方法。設(shè)機(jī)器人需要的運(yùn)動(dòng)軌跡為直線，運(yùn)動(dòng)速度為v(mm/s)，時(shí)間間隔為ts (ms)，則每個(gè)ts間隔內(nèi)機(jī)器人應(yīng)走過(guò)的距離為Pi Pi+1= Vts(4.2)可見(jiàn)兩個(gè)插補(bǔ)點(diǎn)之間的距離正比于要求的運(yùn)動(dòng)速度，兩點(diǎn)之間的軌跡不受控制，只有插補(bǔ)點(diǎn)之間的距離足夠小，才能滿足一定的軌跡精度要求。機(jī)器人控制系統(tǒng)易于實(shí)現(xiàn)定時(shí)插補(bǔ)，例如采用定時(shí)中斷方式每隔ts中斷一次進(jìn)行一次插補(bǔ)，計(jì)算一次逆向運(yùn)動(dòng)學(xué)，輸出一次給定值。由于ts僅為幾毫秒，機(jī)器人沿著要求軌跡的速度一般不會(huì)很高

12、，且機(jī)器人總的運(yùn)動(dòng)精度不如數(shù)控機(jī)床、加工中心高，故大多數(shù)工業(yè) 機(jī)器人采用定時(shí)插補(bǔ)方式。當(dāng)要求以更高的精度實(shí)現(xiàn)運(yùn)動(dòng)軌跡時(shí)，可采用定距插補(bǔ)。2.定距插補(bǔ)由式（4.2）可知v是要求的運(yùn)動(dòng)速度，它不能變化，如果要兩插補(bǔ)點(diǎn)的距離 Pi Pi+1恒為一 個(gè)足夠小的值，以保證軌跡精度， ts就要變化。也就是在此方式下，插補(bǔ)點(diǎn)距離不變，但t要隨著不同工作速度 v的變化而變化。這兩種插補(bǔ)方式的基本算法相同，只是前者固定ts,易于實(shí)現(xiàn)，后者保證軌跡插補(bǔ)精度，但ts要隨之變化，實(shí)現(xiàn)起來(lái)比前者困難。四、關(guān)節(jié)空間插補(bǔ)如上所述，路徑點(diǎn)（結(jié)點(diǎn)）通常用工具坐標(biāo)系以相對(duì)于工作坐標(biāo)系位姿來(lái)表示。為了求得在關(guān)節(jié)空間形成所要求的軌跡

13、，首先用運(yùn)動(dòng)學(xué)反解將路徑點(diǎn)轉(zhuǎn)換成關(guān)節(jié)矢量角度值，然后 對(duì)每個(gè)關(guān)節(jié)擬合一個(gè)光滑函數(shù)，使之從起始點(diǎn)開(kāi)始，依次通過(guò)所有路徑點(diǎn)，最后到達(dá)目標(biāo) 點(diǎn)。對(duì)于每一段路徑，各個(gè)關(guān)節(jié)運(yùn)動(dòng)時(shí)間均相同，這樣保證所有關(guān)節(jié)同時(shí)到達(dá)路徑點(diǎn)和終止點(diǎn)，從而得到工具坐標(biāo)系應(yīng)有的位置和姿態(tài)。但是，盡管每個(gè)關(guān)節(jié)在同一段路徑中的運(yùn) 動(dòng)時(shí)間相同，各個(gè)關(guān)節(jié)函數(shù)之間卻是相互獨(dú)立的?？傊?，關(guān)節(jié)空間法是以關(guān)節(jié)角度的函數(shù)來(lái)描述機(jī)器人的軌跡的，關(guān)節(jié)空間法不必在直角坐標(biāo)系中描述兩個(gè)路徑點(diǎn)之間的路徑形狀，計(jì)算簡(jiǎn)單、容易。再者，由于關(guān)節(jié)空間與直角坐標(biāo)空間之間并不是連續(xù)的對(duì)應(yīng)關(guān)系，因而不會(huì)發(fā)生機(jī)構(gòu)的奇異性問(wèn)題。在關(guān)節(jié)空間中進(jìn)行軌跡規(guī)劃，需要給定機(jī)器人在起始

14、點(diǎn)、終止點(diǎn)手臂的形位。對(duì)關(guān)節(jié)進(jìn)行插值時(shí)，應(yīng)滿足一系列約束條件，例如抓取物體時(shí)，手部運(yùn)動(dòng)方向（初始點(diǎn)），提升物體離開(kāi)的方向（提升點(diǎn)），放下物體（下放點(diǎn)）和停止點(diǎn)等結(jié)點(diǎn)上的位姿、速度和加速度的要求； 與此相應(yīng)的各個(gè)關(guān)節(jié)位移、速度、加速度在整個(gè)時(shí)間間隔內(nèi)連續(xù)性要求；其極值必須在各 個(gè)關(guān)節(jié)變量的容許范圍之內(nèi)等。在滿足所要求的約束條件下，可以選取不同類(lèi)型的關(guān)節(jié)插 值函數(shù)生成不同的軌跡。本節(jié)著重討論關(guān)節(jié)軌跡的插值方法“1.三次多項(xiàng)式插值在操作臂運(yùn)動(dòng)的過(guò)程中，由于相應(yīng)于起始點(diǎn)的關(guān)節(jié)角度6是已知的而終止點(diǎn)的關(guān)節(jié)角f可以通過(guò)運(yùn)動(dòng)學(xué)反解得到，因此，運(yùn)動(dòng)軌跡的描述，可用起始點(diǎn)關(guān)節(jié)角與終止點(diǎn)關(guān)節(jié)角 度的一個(gè)平滑插值函

15、數(shù)二(t)來(lái)表示。v(t)在to=O時(shí)刻的值是起始關(guān)節(jié)角度 如 終端時(shí)刻tf的值是終止關(guān)節(jié)角度-f。顯然，有許多平滑函數(shù)可作為關(guān)節(jié)插值函數(shù)，如圖43所示。為實(shí)現(xiàn)單個(gè)關(guān)節(jié)的平穩(wěn)運(yùn)動(dòng)，軌跡函數(shù)r(t)至少需要滿足四個(gè)約束條件，即兩端點(diǎn)位置約束和兩端點(diǎn)速度約束。端點(diǎn)位置約束是指起始位姿和終止位姿分別所對(duì)應(yīng)的關(guān)節(jié)角度。V(t)在時(shí)刻to=0時(shí)的值是起始關(guān)節(jié)角度 飛，在終端時(shí)刻tf時(shí)的值是終止關(guān)節(jié)角度f(wàn) 即二 Of二 tf T為滿足關(guān)節(jié)運(yùn)動(dòng)速度的連續(xù)性要求，兩外還有兩個(gè)約束條件，即在起始點(diǎn)和終止點(diǎn)的 關(guān)節(jié)速度要求。在但前的情況下，可簡(jiǎn)單地設(shè)定為零，即0 =0tf =0上面給出的四個(gè)約束條件可以惟一地確定

16、一個(gè)三次多項(xiàng)式丁 t 二a0 a1t a2t2 a3t3運(yùn)動(dòng)過(guò)程中的關(guān)節(jié)速度和加速度則為4t =a*i +2a?t +3a?t t再 t = 2a2 6a3t為求得三次多項(xiàng)式的系數(shù)ao, a a2和a3，代以給定的約束條件，有方程組二0 - a04 na。tf - a?# - ast；0 = q(4.7)0 ai 2a2tf 3a3tf求解該方程組，可得a22(f -二0tf2 一 a3 :對(duì)于起始速度及終止速度為零的關(guān)節(jié)運(yùn)動(dòng), 函數(shù)為一占佝一日0 )tf(4.8)滿足連續(xù)平穩(wěn)運(yùn)動(dòng)要求的三次多項(xiàng)式插值M) r 律(訃 ft2tf2聳(W0)t3tf3(4.9)由式(4.9)可得關(guān)節(jié)角速度和角加

17、速度的表達(dá)式為舟(t)2(半- V0)t3(呂-二0)tftfO4 QF(t)2(Vf iJ。)- 3(詁 16八tftf(4.10)三次多項(xiàng)式插值的關(guān)節(jié)運(yùn)動(dòng)軌跡曲線如圖4.8所示。由圖可知，其速度曲線為拋物線,相應(yīng)的加速度曲線為直線。這里再次指出：這組解只適用于關(guān)節(jié)起始、終止速度為零的運(yùn)動(dòng)情況。對(duì)于其他情況， 后面另行討論。例4.1設(shè)有一臺(tái)具有轉(zhuǎn)動(dòng)關(guān)節(jié)的機(jī)器人，其在執(zhí)行一項(xiàng)作業(yè)時(shí)關(guān)節(jié)運(yùn)動(dòng)歷時(shí)2 s。根據(jù)需要，其上某一關(guān)節(jié)必須運(yùn)動(dòng)平穩(wěn)，并具有如下作業(yè)狀態(tài)：初始時(shí)，關(guān)節(jié)靜止不動(dòng)，位 置0)=0 運(yùn)動(dòng)結(jié)束時(shí)6=90。,此時(shí)關(guān)節(jié)速度為 0。試根據(jù)上述要求規(guī)劃該關(guān)節(jié)的運(yùn)動(dòng)。解 根據(jù)要求，可以對(duì)該關(guān)節(jié)采

18、用三次多項(xiàng)式插值函數(shù)來(lái)規(guī)劃其運(yùn)動(dòng)。已知00=0 6f=90 , tf =2 s，代入式(4.8)可得三次多項(xiàng)式的系數(shù)a=0.0, ai =0.0, a2=22.5, a3 = _67.5由式(4.5)和式(4.6)可確定該關(guān)節(jié)的運(yùn)動(dòng)軌跡，即r t i=22.5t267.5t34 t ；=45.0 t 202. 5 t2月 t =45 405.0 t2.過(guò)路徑點(diǎn)的三次多項(xiàng)式插值一般情況下要求規(guī)劃過(guò)路徑點(diǎn)的軌跡。如圖4.9所示，機(jī)器人作業(yè)除在 A、B點(diǎn)有位姿要求外，在路徑點(diǎn)C、D也有位姿要求。對(duì)于這種情況，假如末端執(zhí)行器在路徑點(diǎn)停留， 即各路徑點(diǎn)上速度為 0,則軌跡規(guī)劃可連續(xù)直接使用前面介紹的三次

19、多項(xiàng)式插值方法；但若末端執(zhí)行器只是經(jīng)過(guò)，并不停留，就需要將前述方法推廣。實(shí)際上，可以把所有路徑點(diǎn)也看作是“起始點(diǎn)”或“終止點(diǎn)”，求解逆運(yùn)動(dòng)學(xué)，得到相應(yīng)的關(guān)節(jié)矢量值。然后確定所要求的三次多項(xiàng)式插值函數(shù)，把路徑點(diǎn)平滑地連接起來(lái)。但 是，在這些“起始點(diǎn)”和“終止點(diǎn)“的關(guān)節(jié)運(yùn)動(dòng)速度不再是零。設(shè)路徑點(diǎn)上的關(guān)節(jié)速度已知，在某段路徑上，起始點(diǎn)為和-0，終止點(diǎn)為Q和勺f，這時(shí)，確定三次多項(xiàng)式系數(shù)的方法與前所述完全一致，只是速度約束條件變?yōu)? 0 勺tf #利用約束條件確定三次多項(xiàng)式系數(shù)，有下列方程組=逐0 =80+3 +a?tf +a3tf 1求解方程組，得ai(i 0,1,2,3)為a。=二 031 0a2

20、2Wf 00ftftftfa3壬-ftfdf =q +2a2tf +3a3t：日。)*他弋)tf(4.13)實(shí)際上，由上式確定的三次多項(xiàng)式描述了起始點(diǎn)和終止點(diǎn)具有任意給定位置和速度的運(yùn)動(dòng)軌跡，是式的推廣。當(dāng)路徑點(diǎn)上的關(guān)節(jié)速度為0，即勺0 =豈=0時(shí)，式(4.13)與式(4.8)完全相同，這就說(shuō)明了由式(4.13)確定的三次多項(xiàng)式描述了起始點(diǎn)和終止點(diǎn)具有任意給定位置和速度約束條件的運(yùn)動(dòng)軌跡。剩下的問(wèn)題就是 如何來(lái)確定路徑點(diǎn)上的關(guān)節(jié)速度，可由以下三種方法規(guī)定:(1) 根據(jù)工具坐標(biāo)系在直角坐標(biāo)空間中的瞬時(shí)線速度和角速度來(lái)確定每個(gè)路徑點(diǎn)的關(guān) 節(jié)速度。對(duì)于方法（1），利用操作臂在此路徑點(diǎn)上的逆雅可比，把

21、該點(diǎn)的直角坐標(biāo)速度映射” 為所要求的關(guān)節(jié)速度。當(dāng)然，如果操作臂的某個(gè)路徑點(diǎn)是奇異點(diǎn)，這時(shí)就不能任意設(shè)置速 度值。按照方法（1）生成的軌跡雖然能滿足用戶設(shè)置速度的需要，但是逐點(diǎn)設(shè)置速度畢竟要 耗費(fèi)很大的工作量。因此。機(jī)器人的控制系統(tǒng)最好具有方法（2）或（3）的功能，或者二者兼而有之。（2）在直角坐標(biāo)空間或關(guān)節(jié)空間中采用適當(dāng)?shù)膯l(fā)式方法，由控制系統(tǒng)自動(dòng)地選擇路 徑點(diǎn)的速度。對(duì)于方法（2）系統(tǒng)采用某種啟發(fā)式方法自動(dòng)選取合適的路徑點(diǎn)速度。圖45表示一種啟發(fā)式選擇路徑點(diǎn)速度的方式。圖中（0為起始點(diǎn)；0D為終止點(diǎn)，0a, 0B和6C是路徑點(diǎn)，用細(xì)實(shí)線表示過(guò)路徑點(diǎn)時(shí)的關(guān)節(jié)運(yùn)動(dòng)速度。這里所用的啟發(fā)式信息從概念

22、到計(jì)算方法都很簡(jiǎn)單， 即，假設(shè)用宣線段把這些路徑點(diǎn)依次連接起來(lái)，如果相鄰線段的斜率在路徑點(diǎn)處改變將號(hào)， 則把速度選定為零；如果相鄰線段不改變符號(hào)，則選取路徑點(diǎn)兩例的線段斜率的平均值作 為該點(diǎn)的速度。因此，根據(jù)規(guī)定的路徑點(diǎn)，系統(tǒng)就能夠按此規(guī)則自動(dòng)生成相應(yīng)的路徑點(diǎn)速 度。tst圖45路徑點(diǎn)上il度的自動(dòng)生成（3）為了保證每個(gè)路徑點(diǎn)上的加速度連續(xù)，由控制系統(tǒng)按此要求自動(dòng)地選擇路徑點(diǎn)的速度。對(duì)于方法（3）,為了保證路徑點(diǎn)處的加速度連續(xù)，可以設(shè)法用兩條三次曲線在路徑點(diǎn)處按一定規(guī)則聯(lián)接起來(lái)，拼湊成所要求的軌跡。其約束條件是：聯(lián)接處不僅速度連續(xù)，而且 加速度也連續(xù)，下面具體地說(shuō)明這種方法。設(shè)所經(jīng)過(guò)的路徑點(diǎn)處

23、的關(guān)節(jié)角度為（，與該點(diǎn)相鄰的前后兩點(diǎn)的關(guān)節(jié)角分別為（和0。設(shè)其路徑點(diǎn)處的關(guān)節(jié)加速度連續(xù)。如果路徑點(diǎn)用三次多項(xiàng)式連接，試確定多項(xiàng)式的所有系 數(shù)。該機(jī)器人路徑可分為 4到入段及 入到弓段兩段，可通過(guò)由兩個(gè)三次多項(xiàng)式組成的樣 條函數(shù)連接。設(shè)從 T0到內(nèi)的三次多項(xiàng)式插值函數(shù)為.23% t = Qo at ai2t 玄忍而從2到弓的三次多項(xiàng)式插值函數(shù)為23二211 二 a?o a?it *22七*23七上述兩個(gè)三次多項(xiàng)式的時(shí)間區(qū)間分別是0 , tf1和0 , tf2，若要保證路徑點(diǎn)處的速度及加速度均連續(xù)，即存在下列約束條件扌 tfi二(0)3 tfi(0)根據(jù)約束條件建立的方程組為0 = ai06 =a

24、i0 七iitfi *ai2tfi *ai3tfi6 =a20殆-a 20 a2itf2 a22tf2 a23tf 2=a1120 =a2i +2a22tf 2 +3a23tf22aii +2ai2tfi +3ai3tfi = a2i2ai2 6ai3tfi =2比2上述約束條件組成含有8個(gè)未知數(shù)的8個(gè)線性方程。對(duì)于tfi=tf2=tf的情況，這個(gè)方程組的解為aioaiiai2=日0=0i2dv3dg心。ai3a20a21一4tf33咼一3鳥(niǎo)a224tf-32tf28氏-5日g -3J在更一般的情況下，包含許多路徑點(diǎn)的機(jī)器人軌跡可用多個(gè)三次多項(xiàng)式表示。包括各路徑點(diǎn)處加速度連續(xù)的約束條件構(gòu)成的方

25、程組能表示成矩陣的形式，由于系數(shù)矩陣是三角陣，路徑點(diǎn)的速度易于求出。a234t；3. 高階多項(xiàng)式插值若對(duì)于運(yùn)動(dòng)軌跡的要求更為嚴(yán)格，約束條件增多，三次多項(xiàng)式就不能滿足需要，須用 更高階的多項(xiàng)式對(duì)運(yùn)動(dòng)軌跡的路徑段進(jìn)行插值。例如，對(duì)某段路徑的起始點(diǎn)和終止點(diǎn)都規(guī)定了關(guān)節(jié)的位置、速度和加速度，則要用一 個(gè)五次多項(xiàng)式進(jìn)行插值，即(4.14)丁 t 二 a0 a1t a2t2 a3t3 a4t4 a5t5多項(xiàng)式的系數(shù)a。，a!，a5必須滿足6個(gè)約束條件佻=aoQ =a0 +a1tf +a2t； +a3t； +a4t： +a5t；ft =ai& =印 +2a2tf +3a3t： +4a4tf3 +5a5t：

26、曲=2a2件15)G =2a2 +6a3tf +12a4tf +20a5tf4. 用拋物線過(guò)渡的線性插值在關(guān)節(jié)空間軌跡規(guī)劃中，對(duì)于給定起始點(diǎn)和終止點(diǎn)的情況選擇線性函數(shù)插值較為簡(jiǎn)單,如圖4.10所示。然而，單純線性插值會(huì)導(dǎo)致起始點(diǎn)和終止點(diǎn)的關(guān)節(jié)運(yùn)動(dòng)速度不連續(xù)，且加 速度無(wú)窮大，顯然，在兩端點(diǎn)會(huì)造成剛性沖擊。如圖為此應(yīng)對(duì)線性函數(shù)插值方案進(jìn)行修正，在線性插值兩端點(diǎn)的鄰域內(nèi)設(shè)置一段拋物線形 緩沖區(qū)段。由于拋物線函數(shù)對(duì)于時(shí)間的二階導(dǎo)數(shù)為常數(shù)，即相應(yīng)區(qū)段內(nèi)的加速度恒定，這 樣保證起始點(diǎn)和終止點(diǎn)的速度平滑過(guò)渡，從而使整個(gè)軌跡上的位置和速度連續(xù)。線性函數(shù) 與兩段拋物線函數(shù)平滑地銜接在一起形成的軌跡稱(chēng)為帶有拋物

27、線過(guò)渡域的線性軌跡4.11所示。4.12v h。為了構(gòu)造這段運(yùn)動(dòng)軌跡，假設(shè)兩端的拋物線軌跡具有相 同的持續(xù)時(shí)間ta，具有大小相同而符號(hào)相反的恒加速度 對(duì)于這種路徑規(guī)劃存在有多個(gè)解，其軌跡不惟一。如圖 所示。但是，每條路徑都對(duì)稱(chēng)于時(shí)間中點(diǎn)th和位置中點(diǎn)要保證路徑軌跡的連續(xù)、光滑，即要求拋物線軌跡的終 點(diǎn)速度必須等于線性段的速度，故有下列關(guān)系lh a式中：乙為對(duì)應(yīng)于拋物線持續(xù)時(shí)間 ta的關(guān)節(jié)角度。飛的值可以按式(4.17)求出:2(4.17)設(shè)關(guān)節(jié)從起始點(diǎn)到終止點(diǎn)的總運(yùn)動(dòng)時(shí)間為tf，則tf = 2th，并注意到1(4.18)丸=2仃十)則由式(4.16)至(4.18)得4Jta 將tft a 十冷

28、日0 )=0(4 19)一般情況下，北、f tf是已知條件，這樣，據(jù)式(4.16)可以選擇相應(yīng)的V和ta，得到相 應(yīng)的軌跡。通常的做法是先選定加速度舟的值，然后按式(4.19)求出相應(yīng)的ta :t嗣2tf2 _4畝日f(shuō) _出)a 22雜(4.20)由式(4.20)可知，為保證ta有解，加速度值 令必須選得足夠大，即t 2tf(4.21)當(dāng)式(4.21)中的等號(hào)成立時(shí)，軌跡線性段的長(zhǎng)度縮減為零， 整個(gè)軌跡由兩個(gè)過(guò)渡域組成， 這兩個(gè)過(guò)渡域在銜接處的斜率 (關(guān)節(jié)速度)相等；加速度專(zhuān)的取值愈大，過(guò)渡域的長(zhǎng)度會(huì)變得 愈短，若加速度趨于無(wú)窮大，軌跡又復(fù)歸到簡(jiǎn)單的線性插值情況。例4.3 樂(lè)q和tf的定義同例

29、4.1，若將已知條件改為0=15 &=75 tf =3 s,試設(shè)計(jì)兩條帶有拋物線過(guò)渡的線性軌跡。解 根據(jù)題意，按式(4.21)定出加速度的取值范圍，為此，將已知條件代入式(4.21) 中,有 v 26.67s21)設(shè)計(jì)第一條軌跡對(duì)于第一條軌跡，如果選舟1 = 42 /s2，由式件17)算出過(guò)渡時(shí)間ta1，則tai342232 -4 42(75 -15)=()s =0.59 s22 42用式(4.17)和件佝計(jì)算過(guò)渡域終了時(shí)的關(guān)節(jié)位置范1和關(guān)節(jié)速度勺，得賂=15(1 42 0.592) =22.3引=ta1 =(42 0.59) ；/s =24.78； /s據(jù)上面計(jì)算得出的數(shù)值可以繪出如圖4.1

30、3(a)所示的軌跡曲線。0,6 1,2 1上 2.4 3.0 間礙r諳妙云0.6 U L.S 2.4 30逋應(yīng)時(shí)間依 50505 0 506 3-2211KN*翅潘妊I I06 4 2 0 2 4 3 2 1 T-2 :.一毛魁駕呂辰7K.242.id-36 -42 二抑速度時(shí)間佝世擋時(shí)間t.fs速度時(shí)間應(yīng)(a)加速度較大時(shí)的位移、速度、加速度曲線-2.41ai11 .6 1.2- 264 2 O2 4 A2L T 亠亠 7(b)加速度較小時(shí)的位移、速度、加速度曲線圖4.13帶有拋物線過(guò)渡的線性插值2)設(shè)計(jì)第二條軌跡對(duì)于第二條軌跡，若選擇 劃 F，可求出ta2(-,27232 -4 27 (

31、75 -15 )2漢27)s =1.33 s乙2 =15 (丄 27 1.332)； =38.8821 乂ta2 =(27 1.33) ；/s =35.91/s 相應(yīng)的軌跡曲線如圖 4.13(b)所示。用拋物線過(guò)渡的線性函數(shù)插值進(jìn)行軌跡規(guī)劃的物理概念非常清楚，即如果機(jī)器人每一 關(guān)節(jié)電動(dòng)機(jī)采用等加速、等速和等減速運(yùn)動(dòng)規(guī)律，則關(guān)節(jié)的位置、速度、加速度隨時(shí)間變 化的曲線如圖4.13所示。若某個(gè)關(guān)節(jié)的運(yùn)動(dòng)要經(jīng)過(guò)一個(gè)路徑點(diǎn)，則可采用帶拋物線過(guò)渡域的線性路徑方案。如 圖4.14所示，關(guān)節(jié)的運(yùn)動(dòng)要經(jīng)過(guò)一組路徑點(diǎn)，用關(guān)節(jié)角度 可、和*表示其中三個(gè)相鄰的路徑點(diǎn)，以線性函數(shù)將每?jī)蓚€(gè)相鄰路徑點(diǎn)之間相連，而所有路徑點(diǎn)附近都采用拋物線過(guò)渡。應(yīng)該注意到：各路徑段采用拋物線過(guò)渡域線性函數(shù)所進(jìn)行的規(guī)劃，機(jī)器人的運(yùn)動(dòng)關(guān)節(jié) 并不能真正到達(dá)那些路徑點(diǎn)。即使選取的加速度充分大，實(shí)際路徑也只是十分接近理想路 徑點(diǎn)，如圖4.14所示。圖4.14多段帶有拋物線過(guò)渡域的線性軌跡4.4機(jī)器人手部路徑的軌跡規(guī)劃操作對(duì)象的描述由前述可知，任一剛體相對(duì)參考系的位