第四章軌跡規(guī)劃

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

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

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

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

5、和姿態(tài)（簡稱位姿），例如起始點和目標點等。對于另外一些作業(yè)，如弧焊和曲面加工等，不僅要規(guī)定操作臂的起始點和終止點，而且要指明兩點之間的若干中間點（稱路徑點），必須沿特定的路徑運動（路徑約束）。這類稱為連續(xù)路徑運動（continuous Path motion）或輪廓運動（contour motion），而前者稱點到點運動 （PTP = pointtopoint motion）。在規(guī)劃機器人的運動時.還需要弄清楚在其路徑上是否存在障礙物（障礙約束）。路徑約束和障礙約束的組合將機器人的規(guī)劃與控制方式劃分為四類、如表4-1所示。表4.1機器人的規(guī)劃與控制方式障礙約束路徑約束離線路徑規(guī)劃+在線路徑跟蹤

6、 位置控制離線無碰撞路徑規(guī)則+在線路徑跟蹤 位置控制+在線障礙探測和避障路徑純束" 路徑設定一軌述規(guī)勿IB -*|廠單 Q） , 0U ）動力學約東本章主要討論連續(xù)路徑的無障礙的軌跡規(guī)劃方法。軌跡規(guī)劃器可形象地看成為一個黑箱（圖4 2），其輸入包括路徑的“設定”和“約束”，輸出的是操作臂末端手部的“位姿序列”，表示手部在各離散時刻的中間形位。操作臂最常用的軌跡規(guī)劃方法有兩種：第一種方法要求用戶對于選定的軌跡結(jié)點（插值點）上的位姿、速度和加速度給出一組顯式約束（例如連續(xù)性和光滑程度等 ），軌跡規(guī)劃器從一類函數(shù) （例如n次多項式）中選取參數(shù)化 軌跡，對結(jié)點進行插值，并滿足約束條件。第二種

7、方法要求用戶給出運動路徑的解析式；如直角坐標空間中的直線路徑，軌跡規(guī) 劃器在關節(jié)空間或直角坐標空間中確定一條軌跡來逼近預定的路徑。在第一種方法中，約束的設定和軌跡規(guī)劃均在關節(jié)空間進行。由于對操作臂手部(直角坐標形位 )沒有施加任何約束，用戶很難弄清手部的實際路徑，因此可能會發(fā)生與障礙物相 碰。第二種方法的路徑約束是在直角坐標空間中給定的、而關節(jié)驅(qū)動器是在關節(jié)空間中受 控的。因此，為了得到與給定路徑十分接近的軌跡，首先必須采用某種函數(shù)逼近的方法將 直角坐標路徑約束轉(zhuǎn)化為關節(jié)坐標路徑約束，然后確定滿足關節(jié)路徑約束的參數(shù)化路徑。軌跡規(guī)劃既可在關節(jié)空間也可在直角空間中進行但是所規(guī)劃的軌跡函數(shù)都必須連續(xù)

8、 和平滑，使得操作臂的運動平穩(wěn)。 在關節(jié)空間進行規(guī)劃時、是將關節(jié)變量表示成時間的函 數(shù)，并規(guī)劃它的一階和二階時間導數(shù)； 在直角空間進行規(guī)劃是指將手部位姿、速度和加速 度表示為時間的函數(shù)。而相應的關節(jié)位移、速度和加速度由手部的信息導出。通常通過運 動學反解得出關節(jié)位移、用逆稚可比求出關節(jié)速度，用逆雅可比及其導數(shù)求解關節(jié)加速度。用戶根據(jù)作業(yè)給出各個路徑結(jié)點后規(guī)劃器的任務包含：解變換方程、進行運動學反 解和插值運算等；在關節(jié)空間進行規(guī)劃時，大量工作是對關節(jié)變量的插值運算。下面討論 關節(jié)軌跡的插值計算。三、軌跡的生成方式運動軌跡的描述或生成有以下幾種方式：(1) 示教 -再現(xiàn)運動。這種運動由人手把手示

9、教機器人，定時記錄各關節(jié)變量，得到沿 路徑運動時各關節(jié)的位移時間函數(shù)q(t);再現(xiàn)時，按內(nèi)存中記錄的各點的值產(chǎn)生序列動作。(2) 關節(jié)空間運動。 這種運動直接在關節(jié)空間里進行。 由于動力學參數(shù)及其極限值直接 在關節(jié)空間里描述，所以用這種方式求最短時間運動很方便。(3) 空間直線運動。這是一種直角空間里的運動，它便于描述空間操作，計算量小，適 宜簡單的作業(yè)。(4) 空間曲線運動。這是一種在描述空間中用明確的函數(shù)表達的運動，如圓周運動、螺 旋運動等。四、軌跡規(guī)劃涉及的主要問題 為了描述一個完整的作業(yè)，往往需要將上述運動進行組合。通常這種規(guī)劃涉及到以下 幾方面的問題：(1) 對工作對象及作業(yè)進行描述

10、，用示教方法給出軌跡上的若干個結(jié)點(knot)。(2) 用一條軌跡通過或逼近結(jié)點， 此軌跡可按一定的原則優(yōu)化， 如加速度平滑得到直角空間的位移時間函數(shù) X(t)或關節(jié)空間的位移時間函數(shù)q(t);在結(jié)點之間如何進行插補，即根據(jù)軌跡表達式在每一個采樣周期實時計算軌跡上點的位姿和各關節(jié)變量值。(3) 以上生成的軌跡是機器人位置控制的給定值， 可以據(jù)此并根據(jù)機器人的動態(tài)參數(shù)設 計一定的控制規(guī)律。(4) 規(guī)劃機器人的運動軌跡時， 尚需明確其路徑上是否存在障礙約束的組合。 一般將機 器人的規(guī)劃與控制方式分為四種情況，如表 4.1 所示。4.2插補方式分類與軌跡控制一、插補方式分類點位控制(PTP控制)通常

11、沒有路徑約束，多以關節(jié)坐標運動表示。點位控制只要求滿足 起終點位姿，在軌跡中間只有關節(jié)的幾何限制、最大速度和加速度約束；為了保證運動的連 續(xù)性，要求速度連續(xù)，各軸協(xié)調(diào)。連續(xù)軌跡控制(CP控制)有路徑約束，因此要對路徑進行設計。路徑控制與插補方式分類如表 4.2所示。表4.2路徑控制與插補方式分類路徑控制不插補關節(jié)插補(平滑)空間插補點位控制PTP(1) 各軸獨立快速到達。(2) 各關節(jié)最大加速度 限制(1) 各軸協(xié)調(diào)運動定時插補。(2) 各關節(jié)最大加速度限制連續(xù)路徑控制CP(1) 在空間插補點間進行關 節(jié)定時插補。(2) 用關節(jié)的低階多項式擬合 空間直線使各軸協(xié)調(diào)運動。(3) 各關節(jié)最大加速度

12、限制(1) 直線、圓弧、 曲線等距插補。(2) 起停線速 度、線加速度給 定，各關節(jié)速度、 加速度限制、機器人軌跡控制過程機器人的基本操作方式是示教 -再現(xiàn)，即首先教機器人如何做， 機器人記住了這個過程, 于是它可以根據(jù)需要重復這個動作。操作過程中，不可能把空間軌跡的所有點都示教一遍 使機器人記住，這樣太繁瑣，也浪費很多計算機內(nèi)存。實際上，對于有規(guī)律的軌跡，僅示 教幾個特征點，計算機就能利用插補算法獲得中間點的坐標，如直線需要示教兩點，圓弧 需要示教三點，通過機器人逆向運動學算法由這些點的坐標求出機器人各關節(jié)的位置和角 度(1,n)，然后由后面的角位置閉環(huán)控制系統(tǒng)實現(xiàn)要求的軌跡上的一點。繼續(xù)插

13、補并重 復上述過程，從而實現(xiàn)要求的軌跡。機器人軌跡控制過程如圖 4.3所示。圖4.3機器人軌跡控制過程4.3機器人軌跡插值計算給出各個路徑結(jié)點后，軌跡規(guī)劃的任務包含解變換方程，進行運動學反解和插值計算。 在關節(jié)空間進行規(guī)劃時，需進行的大量工作是對關節(jié)變量的插值計算。一、直線插補直線插補和圓弧插補是機器人系統(tǒng)中的基本插補算法。對于非直線和圓弧軌跡，可以 采用直線或圓弧逼近，以實現(xiàn)這些軌跡?？臻g直線插補是在已知該直線始末兩點的位置和姿態(tài)的條件下，求各軌跡中間點(插補點)的位置和姿態(tài)。由于在大多數(shù)情況下，機器人沿直線運動時其姿態(tài)不變，所以無姿態(tài)插 補，即保持第一個示教點時的姿態(tài)。當然在有些情況下要求

14、變化姿態(tài)，這就需要姿態(tài)插補,可仿照下面介紹的位置插補原理處理，也可參照圓弧的姿態(tài)插補方法解決，如圖 4.4所示。已知直線始末兩點的坐 標值 Po(Xo, YO, ZO)、Pe(Xe，Ye, Ze)及姿態(tài)，其中 Po、Pe 是相對于基坐標系的位置。這些已知的位置和姿態(tài)通常是通過示教方式得到的。設 V為要求的沿直線運動的速度； ts為插補時間間隔。為減少實時計算量，示教完成后，可求出： 2 22"L '、.： Xe XoYe YoZe Zo .ts間隔內(nèi)行程d = Vts；插補總步數(shù)N為L/d+1的整數(shù)部分;各軸增量X Xe Xo /NY Ye Yo /NZ Ze Zo /N各插

15、補點坐標值Xi 1 Xi i XYi i Y i 丫 乙 1 Zi i Z式中：i=o, 1, 2，,、圓弧插補1平面圓弧插補平面圓弧是指圓弧平面與基坐標系的三大平面之一重合，以XOY平面圓弧為例。已知不在一條直線上的三點Pi、P2、P3及這三點對應的機器人手端的姿態(tài)，如圖4.5及圖4.6所設v為沿圓弧運動速度；ts為插補時時間隔。類似直線插補情況計算出：(1) 由Pi、P2、P3決定的圓弧半徑 R。(2) 總的圓心角 =1+ 2，即卩2 2222 arccos (X3X2)Y3Y22R /2R(3) ts時間內(nèi)角位移量帶tsv/R,據(jù)圖4.4所示的幾何關系求各插補點坐標。(4) 總插補步數(shù)(

16、取整數(shù))N = / 0+ 1對Pi+1點的坐標，有Xi 1 Rcos( i ) Rcos i cos Rsin i sin Xi cosYi sin式中：Xi=R cos 0; Yi=Rsin 0。同理有Yi 1Rsin( j ) Rsin j cos Rcos i sin Y cos Xi sin由0+1=0i +0可判斷是否到插補終點。若0+1，則繼續(xù)插補下去；當0+1時，貝U修正最后一步的步長0,并以 表示，i ,故平面圓弧位置插補為Xi 1 Xi cos Y sinYi 1 Y cos Xi sin 2 21 arccos (X2Xi)Y2Y12R /2R2. 空間圓弧插補空間圓弧是指

17、三維空間任一平面內(nèi)的圓弧，此為空間一般平面的圓弧問題。 空間圓弧插補可分三步來處理：(1) 把三維問題轉(zhuǎn)化成二維，找出圓弧所在平面。(2) 利用二維平面插補算法求出插補點坐標(Xi+i, Yi+i)。 把該點的坐標值轉(zhuǎn)變?yōu)榛A坐標系下的值，如圖4.7所示。通過不在同一直線上的三點Pi、P2、P3可確定一個圓及三點間的圓弧，其圓心為Or,半徑為R,圓弧所在平面與基礎坐標系平面的交線分別為AB、BC、CA。建立圓弧平面插補坐標系，即把OrXrYrZr坐標系原點與圓心 Or重合，設OrXrYrZr平面為圓弧所在平面，且保持Zr為外法線方向。這樣，一個三維問題就轉(zhuǎn)化成平面問題，可以應用平面圓弧插補的結(jié)

18、論。求解兩坐標系(圖4.7)的轉(zhuǎn)換矩陣。令Tr表示由圓弧坐標OrXrYrZr至基礎坐標系OXoYoZo的轉(zhuǎn)換矩陣。若Zr軸與基礎坐標系 Zo軸的夾角為，Xr軸與基礎坐標系的夾角為0,則可完成下述步驟：將XrYrZr的原點Or放到基礎原點 0上；繞Zr軸轉(zhuǎn)0使Xo與Xr平行；再繞 Xr軸轉(zhuǎn)角，使Zo與Zr平行。這三步完成了 XrYrZr向XoYoZo的轉(zhuǎn)換，故總轉(zhuǎn)換矩陣應為cossin0XORcosYOR sinTR1sin coscos cossinXOR sincosYOR cos cosZOR sinsin sincos sincosXOR sinsinYOR cos sinZOR cos

19、0001OrXrYrZr坐標系，則要用到 T r的逆矩陣TRT(XOR,YOR,ZOR)R(Z,)R(X, )cossin cossin cosXORsincos coscos sinYOR0sincosZOR0001(4.1)式中： XOR、,Yor、Zor為圓心Or在基礎坐標系下的坐標值。欲將基礎坐標系的坐標值表示在三、定時插補與定距插補由上述可知，機器人實現(xiàn)一個空間軌跡的過程即是實現(xiàn)軌跡離散的過程，如果這些離 散點間隔很大，則機器人運動軌跡與要求軌跡可能有較大誤差。只有這些插補得到的離散 點彼此距離很近， 才有可能使機器人軌跡以足夠的精確度逼近要求的軌跡。 模擬 CP 控制實 際上是多次

20、執(zhí)行插補點的 PTP 控制，插補點越密集，越能逼近要求的軌跡曲線。插補點要多么密集才能保證軌跡不失真和運動連續(xù)平滑呢？可采用定時插補和定距插 補方法來解決。1.定時插補從圖 4.3 所示的軌跡控制過程知道， 每插補出一軌跡點的坐標值， 就要轉(zhuǎn)換成相應的關 節(jié)角度值并加到位置伺服系統(tǒng)以實現(xiàn)這個位置，這個過程每隔一個時間間隔ts 完成一次。為保證運動的平穩(wěn)，顯然 ts不能太長。由于關節(jié)型機器人的機械結(jié)構(gòu)大多屬于開鏈式，剛度不高，ts一般不超過25 ms(40 Hz)，這樣就產(chǎn)生了 ts的上限值。當然ts越小越好，但它的下限值受到計算量限制，即對于機器人的控制，計算機要在 ts 時間里完成一次插補運

21、算和一次逆向運動學計算。對于目前的大多 數(shù)機器人控制器，完成這樣一次計算約需幾毫秒。這樣產(chǎn)生了ts 的下限值。當然，應當選擇ts接近或等于它的下限值，這樣可保證較高的軌跡精度和平滑的運動過程。以一個 XOY 平面里的直線軌跡為例說明定時插補的方法。設機器人需要的運動軌跡為直線，運動速度為v(mm/s)，時間間隔為ts (ms)，則每個ts間隔內(nèi)機器人應走過的距離為可見兩個插補點之間的距離正比于要求的運動速度，兩點之間的軌跡不受控制，只有 插補點之間的距離足夠小，才能滿足一定的軌跡精度要求。機器人控制系統(tǒng)易于實現(xiàn)定時插補，例如采用定時中斷方式每隔ts 中斷一次進行一次插補，計算一次逆向運動學，輸

22、出一次給定值。由于 ts 僅為幾毫秒，機器人沿著要求軌跡 的速度一般不會很高，且機器人總的運動精度不如數(shù)控機床、加工中心高，故大多數(shù)工業(yè) 機器人采用定時插補方式。當要求以更高的精度實現(xiàn)運動軌跡時，可采用定距插補。2.定距插補由式 （4.2）可知 v 是要求的運動速度，它不能變化，如果要兩插補點的距離PiPi+1 恒為一個足夠小的值，以保證軌跡精度，ts就要變化。也就是在此方式下，插補點距離不變，但ts要隨著不同工作速度 v 的變化而變化。這兩種插補方式的基本算法相同，只是前者固定ts,易于實現(xiàn)，后者保證軌跡插補精度， 但ts要隨之變化，實現(xiàn)起來比前者困難。四、關節(jié)空間插補如上所述, 路徑點 （

23、結(jié)點 ）通常用工具坐標系以相對于工作坐標系位姿來表示。為了求得在關節(jié)空間形成所要求的軌跡,首先用運動學反解將路徑點轉(zhuǎn)換成關節(jié)矢量角度值,然后 對每個關節(jié)擬合一個光滑函數(shù),使之從起始點開始,依次通過所有路徑點,最后到達目標 點。對于每一段路徑,各個關節(jié)運動時間均相同,這樣保證所有關節(jié)同時到達路徑點和終 止點,從而得到工具坐標系應有的位置和姿態(tài)。但是,盡管每個關節(jié)在同一段路徑中的運 動時間相同,各個關節(jié)函數(shù)之間卻是相互獨立的?？傊?關節(jié)空間法是以關節(jié)角度的函數(shù)來描述機器人的軌跡的,關節(jié)空間法不必在直 角坐標系中描述兩個路徑點之間的路徑形狀,計算簡單、容易。再者,由于關節(jié)空間與直 角坐標空間之間并不

24、是連續(xù)的對應關系,因而不會發(fā)生機構(gòu)的奇異性問題。在關節(jié)空間中進行軌跡規(guī)劃,需要給定機器人在起始點、終止點手臂的形位。對關節(jié) 進行插值時,應滿足一系列約束條件,例如抓取物體時,手部運動方向（初始點 ） ,提升物體離開的方向 （提升點 ）,放下物體 （下放點 ）和停止點等結(jié)點上的位姿、速度和加速度的要求； 與此相應的各個關節(jié)位移、速度、加速度在整個時間間隔內(nèi)連續(xù)性要求；其極值必須在各 個關節(jié)變量的容許范圍之內(nèi)等。在滿足所要求的約束條件下,可以選取不同類型的關節(jié)插 值函數(shù)生成不同的軌跡。本節(jié)著重討論關節(jié)軌跡的插值方法“1.三次多項式插值在操作臂運動的過程中,由于相應于起始點的關節(jié)角度0 是已知的而終

25、止點的關節(jié)角f可以通過運動學反解得到，因此，運動軌跡的描述，可用起始點關節(jié)角與終止點關節(jié)角 度的一個平滑插值函數(shù)（t）來表示。在to=0時刻的值是起始關節(jié)角度0,終端時刻tf的值是終止關節(jié)角度為實現(xiàn)單個關節(jié)的平穩(wěn)運動，軌跡函數(shù)（t）至少需要滿足四個約束條件，即兩端點位置約束和兩端點速度約束。端點位置約束是指起始位姿和終止位姿分別所對應的關節(jié)角度。（t）在時刻to=0時的值是起始關節(jié)角度 0,在終端時刻tf時的值是終止關節(jié)角度f，即tff為滿足關節(jié)運動速度的連續(xù)性要求，兩外還有兩個約束條件，即在起始點和終止點的 關節(jié)速度要求。在但前的情況下，可簡單地設定為零，即&0 0&tf0上

26、面給出的四個約束條件可以惟一地確定一個三次多項式t a。 ait a2t2 a3t3運動過程中的關節(jié)速度和加速度則為&t a1 2a2t 3a3t2為求得三次多項式的系數(shù)&2a2 6a3ta0，ai，a2和a3，代以給定的約束條件，有方程組f a 0aitfa2tf2-3a3tf0a10 ai 2a?tf3a3tf(4.7)求解該方程組，可得0aoa3a22t"2a33tfo(4.8)對于起始速度及終止速度為零的關節(jié)運動，滿足連續(xù)平穩(wěn)運動要求的三次多項式插值 函數(shù)為3(t) o ( ftf2o)t2!( ftf3o)t3(4.9)由式(4.9)可得關節(jié)角速度和角加速度

27、的表達式為&t)令 ftf&&t)4( f tf6o)t 3 ( ftf12o)( ftfo)t2o)t(4.io)三次多項式插值的關節(jié)運動軌跡曲線如圖4.8所示。由圖可知，其速度曲線為拋物線,相應的加速度曲線為直線。這里再次指出：這組解只適用于關節(jié)起始、終止速度為零的運動情況。對于其他情況，后面另行討論。圖4.8三次多項式插值的關節(jié)運動軌跡例4.1設有一臺具有轉(zhuǎn)動關節(jié)的機器人，其在執(zhí)行一項作業(yè)時關節(jié)運動歷時2 s。根據(jù)需要，其上某一關節(jié)必須運動平穩(wěn)，并具有如下作業(yè)狀態(tài)：初始時，關節(jié)靜止不動，位置 0o=o ° ;運動結(jié)束時0f=9o ° ,此時關節(jié)

28、速度為 o。試根據(jù)上述要求規(guī)劃該關節(jié)的運動。解根據(jù)要求，可以對該關節(jié)采用三次多項式插值函數(shù)來規(guī)劃其運動。已知9o=o ° Of=9o ° , tf =2 s，代入式(4.8)可得三次多項式的系數(shù)ao=o.o, ai =o.o, a2=22.5, a3 = -67.5由式(4.5)和式(4.6)可確定該關節(jié)的運動軌跡，即t 22.5t267.5t3&t45.0 t 202. 5 t2&&t45 405.0 t2.過路徑點的三次多項式插值一般情況下要求規(guī)劃過路徑點的軌跡。如圖4.9所示，機器人作業(yè)除在 A、B點有位姿要求外，在路徑點C、D也有位姿要求。對

29、于這種情況，假如末端執(zhí)行器在路徑點停留， 即各路徑點上速度為 0,則軌跡規(guī)劃可連續(xù)直接使用前面介紹的三次多項式插值方法；但若末端執(zhí)行器只是經(jīng)過，并不停留，就需要將前述方法推廣。實際上，可以把所有路徑點也看作是“起始點”或“終止點”，求解逆運動學，得到相應的關節(jié)矢量值。然后確定所要求的三次多項式插值函數(shù)，把路徑點平滑地連接起來。但 是，在這些“起始點”和“終止點“的關節(jié)運動速度不再是零。設路徑點上的關節(jié)速度已知，在某段路徑上，起始點為0)和&，終止點為Q和&，這時，確定三次多項式系數(shù)的方法與前所述完全一致，只是速度約束條件變?yōu)?amp;0 &&tf&(4.

30、11)圖4.9機器人作業(yè)路徑點利用約束條件確定三次多項式系數(shù)，有下列方程組0a。f a0 aitf& aia2tf2& ai 2a?tf 3ast；(4.12)求解方程組，得ai(i0,1,2,3)為 a。a1a2a3tf2f&tf&tf2(4.13)實際上，由上式確定的三次多項式描述了起始點和終止點具有任意給定位置和速度的& &運動軌跡，是式的推廣。當路徑點上的關節(jié)速度為0,即卩0= f =0時，式(4.13)與式(4.8)完全相同，這就說明了由式(4.13)確定的三次多項式描述了起始點和終止點具有任意給定位置和速度約束條件的運動軌跡。剩下的問

31、題就是 如何來確定路徑點上的關節(jié)速度，可由以下三種方法規(guī)定:(1) 根據(jù)工具坐標系在直角坐標空間中的瞬時線速度和角速度來確定每個路徑點的關 節(jié)速度。對于方法（1），利用操作臂在此路徑點上的逆雅可比，把該點的直角坐標速度"映射” 為所要求的關節(jié)速度。當然，如果操作臂的某個路徑點是奇異點，這時就不能任意設置速 度值。按照方法（1）生成的軌跡雖然能滿足用戶設置速度的需要，但是逐點設置速度畢竟要或的功能，或者二者兼而耗費很大的工作量。因此。機器人的控制系統(tǒng)最好具有方法 有之。由控制系統(tǒng)自動地選擇路徑（2）在直角坐標空間或關節(jié)空間中采用適當?shù)膯l(fā)式方法， 點的速度。對于方法（2）系統(tǒng)采用某種啟

32、發(fā)式方法自動選取合適的路徑點速度。圖45表示一種啟發(fā)式選擇路徑點速度的方式。圖中（0為起始點；0D為終止點，0A, 0B和0C是路徑點，用細實線表示過路徑點時的關節(jié)運動速度。這里所用的啟發(fā)式信息從概念到計算方法都很簡單， 即，假設用宣線段把這些路徑點依次連接起來，如果相鄰線段的斜率在路徑點處改變將號， 則把速度選定為零；如果相鄰線段不改變符號，則選取路徑點兩例的線段斜率的平均值作 為該點的速度。因此，根據(jù)規(guī)定的路徑點，系統(tǒng)就能夠按此規(guī)則自動生成相應的路徑點速 度。8tn圖6弋路徑點上連度的自動生成（3）為了保證每個路徑點上的加速度連續(xù)，由控制系統(tǒng)按此要求自動地選擇路徑點的速度。對于方法（3）,

33、為了保證路徑點處的加速度連續(xù)，可以設法用兩條三次曲線在路徑點處按一定規(guī)則聯(lián)接起來，拼湊成所要求的軌跡。其約束條件是：聯(lián)接處不僅速度連續(xù)，而且 加速度也連續(xù)，下面具體地說明這種方法。設所經(jīng)過的路徑點處的關節(jié)角度為0v,與該點相鄰的前后兩點的關節(jié)角分別為00和0）0設其路徑點處的關節(jié)加速度連續(xù)。如果路徑點用三次多項式連接，試確定多項式的所有系 數(shù)。該機器人路徑可分為0到v段及v到g段兩段，可通過由兩個三次多項式組成的樣條函數(shù)連接。設從1 tai0ai1t0到V的三次多項式插值函數(shù)為Mt2ai3t3而從v到g的三次多項式插值函數(shù)為上述兩個三次多項式的時間區(qū)間分別是0，tfi和0，tf2，若要保證路徑

34、點處的速度及加速度均連續(xù)，即存在下列約束條件& tfi&(0)1 tfi2(0)上述約束條件組成含有根據(jù)約束條件建立的方程組為0ai0vai0aiitfiai2tfiai3tfiv屜ga 20a2itf 2a22tf2a23tf 20Qi0a2i2a22tf23a23t2 f 2ai122ai2tfi3ai3tfia212ai26ai3tfi2a228個未知數(shù)的8個線性方程。對于tfi=tf2=tf的情況，這個方程組的解為ai00aii0a20a2i4tfa223g 3。2tf2a23在更一般的情況下，包含許多路徑點的機器人軌跡可用多個三次多項式表示。包括各 路徑點處加速度連續(xù)

35、的約束條件構(gòu)成的方程組能表示成矩陣的形式，由于系數(shù)矩陣是三角 陣，路徑點的速度易于求出。3. 高階多項式插值若對于運動軌跡的要求更為嚴格，約束條件增多，三次多項式就不能滿足需要，須用 更高階的多項式對運動軌跡的路徑段進行插值。例如，對某段路徑的起始點和終止點都規(guī)定了關節(jié)的位置、速度和加速度，則要用一 個五次多項式進行插值，即taa1ta2t2a3it3 a4t4a5t5(4.14)多項式的.系數(shù)ao,ai,as必須滿足6個約束條件0a0fa oaitfa2tfa3tfa4tfastf&0ai&fai2a?tf3af 4a4t； 5ast：&2a2&2a26a；3

36、tf 12a4tf20astf(4.15)4. 用拋物線過渡的線性插值在關節(jié)空間軌跡規(guī)劃中，對于給定起始點和終止點的情況選擇線性函數(shù)插值較為簡單，如圖4.10所示。然而，單純線性插值會導致起始點和終止點的關節(jié)運動速度不連續(xù)，且加 速度無窮大，顯然，在兩端點會造成剛性沖擊。如圖為此應對線性函數(shù)插值方案進行修正，在線性插值兩端點的鄰域內(nèi)設置一段拋物線形 緩沖區(qū)段。由于拋物線函數(shù)對于時間的二階導數(shù)為常數(shù)，即相應區(qū)段內(nèi)的加速度恒定，這 樣保證起始點和終止點的速度平滑過渡，從而使整個軌跡上的位置和速度連續(xù)。線性函數(shù) 與兩段拋物線函數(shù)平滑地銜接在一起形成的軌跡稱為帶有拋物線過渡域的線性軌跡4.11所示。為

37、了構(gòu)造這段運動軌跡，假設兩端的拋物線軌跡具有相 同的持續(xù)時間ta，具有大小相同而符號相反的恒加速度 對于這種路徑規(guī)劃存在有多個解，其軌跡不惟一。如圖 所示。但是，每條路徑都對稱于時間中點th和位置中點要保證路徑軌跡的連續(xù)、光滑，即要求拋物線軌跡的終 點速度必須等于線性段的速度，故有下列關系&&o4.12ho&h_aa t tth ta (4.16)式中：a為對應于拋物線持續(xù)時間ta的關節(jié)角度。a的值可以按式(4.17)求出：_!&t2a 0a2(4.17)設關節(jié)從起始點到終止點的總運動時間為tf，則tf = 2th，并注意到h 1( 0 f )2(4.18)則由

38、式(4.16)至(4.18)得 筆辱仏f 00(4.佝一般情況下，0、f、tf是已知條件，這樣，據(jù)式(4.16)可以選擇相應的 晦口 ta ,得到相應的軌跡。通常的做法是先選定加速度&的值，然后按式(4.19)求出相應的ta：0、taf o2&&(4.20)由式(4.20)可知，為保證ta有解，加速度值 &必、須選得足夠大，即4 f 0tf(4.21)當式(4.21)中的等號成立時，軌跡線性段的長度縮減為零， 整個軌跡由兩個過渡域組成， 這兩個過渡域在銜接處的斜率 (關節(jié)速度)相等；加速度&的取值愈大，過渡域的長度會變得 愈短，若加速度趨于無窮大，軌跡又

39、復歸到簡單的線性插值情況。例4.3。、 f和tf的定義同例4.1，若將已知條件改為60=15° , Of =75 °tf =3 s,試設計兩條帶有拋物線過渡的線性軌跡。解1)設計第一條軌跡對&&0、根據(jù)題意1422 324 叱 15)o.59s2 42用式(4.17)和件佝計算過渡域終了時的關節(jié)位置al和關節(jié)速度&，得(241 2 ooa1 15 (- 42 0.59 )22.32&0o,&&a1 (42 0.59) /s 24.78 /s據(jù)上面計算得出的數(shù)值可以繪出如圖4.13(a)所示的軌跡曲線。M 1.2 1JS 1.4

40、 3.05 Q 5 o 5 o3 3- -z 2 I- I -養(yǎng)童幡乍-12-24-A1-1L»1 .啦112.41.0 '加講奧IM制曲(a)0.6 1.2 1 It 2.40 o moo o o o? 7 5 4 3 2 I加速度較大時的位移、速度、加速度曲線+劇 5 O 5111更'1任箱廂it 坨訊" 1/t血速fft旳間他/ -F- 4 2 C 2 4旳* W- 4 3 1 I I i-一- 廠M=閔琶8霍(b)加速度較小時的位移、速度、加速度曲線 圖4.13帶有拋物線過渡的線性插值2)設計第二條軌跡 對于3272324 27 ( 7515 )、第ta2(-)s 1.33 s22 27條a2151(27 1.332)038.88o2軌&2&2 a2(27 1.33)o/s 35.91°/s相應的軌跡曲線如圖 4.13(b)所示。， 用拋物線過渡的線性函數(shù)插值進行軌跡規(guī)劃的物理概念非常清楚，即如果機器人