第三章 機器人軌跡規(guī)劃

1、3 機器人軌跡規(guī)劃 規(guī)劃意味著在行動之前決定行動的進(jìn)程。一個規(guī)劃是一個行動過程的描述。 自動規(guī)劃是一種重要的問題求解技術(shù),它從某個特定的問題狀態(tài)出發(fā),尋求一系列行為動作,并建立一個操作序列,直到求得目標(biāo)狀態(tài)為止。與一般問題求解相比，自動規(guī)劃更注重于求解過程。此外規(guī)劃要解決的問題往往是真實問題，而不是抽象的數(shù)學(xué)模型問題。機器人規(guī)劃是機器人學(xué)的一個重要研究領(lǐng)域，研究機器人各種控制求解問題（控制方案、過程等）。 工業(yè)機器人軌跡規(guī)劃屬于機器人低層規(guī)劃，基本上不涉及人工智能的問題。 3.1 3.1 機器人軌跡規(guī)劃概述機器人軌跡規(guī)劃概述 3.2 3.2 插補方式分類和軌跡控制插補方式分類和軌跡控制 3.3

2、 3.3 機器人軌跡插值計算機器人軌跡插值計算 3.4 3.4 機器人手部路徑的軌跡規(guī)劃機器人手部路徑的軌跡規(guī)劃3. 1 機器人軌跡規(guī)劃概述機器人軌跡規(guī)劃概述3. 1 .1 機器人軌跡的概念機器人軌跡的概念 機器人軌跡泛指工業(yè)機器人在運動過程中的位移、速度和加速度。也可以說是機器人運動構(gòu)件的位姿和位姿變化情況。多數(shù)是指末端執(zhí)行件（手部）的位姿和位姿變化情況。 機器人運動軌跡的描述一般是對其手部位姿變化的描述。控制軌跡也就是按時間控制手部走過的空間路徑。 在軌跡規(guī)劃中，為敘述方便，也常用點來表示機器人在某一時刻的狀態(tài)，或某一時刻的軌跡，或用它來表示末端執(zhí)行件的位姿，例如起始點、終止點就分別表示末

3、端執(zhí)行件的起始位姿及終止位姿。3. 1. 2 軌跡規(guī)劃的一般性問題軌跡規(guī)劃的一般性問題 機器人在作業(yè)空間要完成給定的任務(wù)，其手部運動必須按一定的軌跡進(jìn)行。軌跡規(guī)劃是根據(jù)作業(yè)任務(wù)的要求，計算出預(yù)期的運動軌跡。 軌跡的生成一般是先給定軌跡上的若干個點，將其經(jīng)運動學(xué)反解映射到關(guān)節(jié)空間，對關(guān)節(jié)空間中的相應(yīng)點建立運動方程，然后按這些運動方程對關(guān)節(jié)進(jìn)行插值，用于關(guān)節(jié)運動的控制，從而實現(xiàn)作業(yè)空間的運動要求，這一過程通常稱為軌跡規(guī)劃軌跡規(guī)劃。 軌跡規(guī)劃的幾個一般性問題 機器人的作業(yè)可以看作是工具坐標(biāo)系T 相對于工件坐標(biāo)系S 的一系列運動。如圖示，將銷插入工件孔中的作業(yè)，可以借助工具坐標(biāo)系的一系列位姿Pi (i

4、=1，2，n)來描述。 用工具坐標(biāo)系相對于工件坐標(biāo)系的運動來描述作業(yè)路徑是一種通用的作業(yè)描述方法。它把作業(yè)路徑描述與具體的機器人、手爪或工具分離開來，形成了模型化的作業(yè)描述方法，從而使這種描述既適用于不同的機器人，也適用于在同一機器人上裝夾不同規(guī)格的工具。把圖示的機器人從初始狀態(tài)運動到終止?fàn)顟B(tài)的作業(yè)看做是工具坐標(biāo)系從初始位置T0 變化到終止位置Tf 的坐標(biāo)變換。更詳細(xì)地描述運動時不僅要規(guī)定機器人的起始點和終止點，而且要給出介于起始點和終止點之間的中間點，也稱路徑點。這時，運動軌跡除了位姿約束外，還存在著各路徑點之間的時間分配問題。機器人的運動應(yīng)當(dāng)平穩(wěn)，不平穩(wěn)的運動將加劇機械部件的磨損，并導(dǎo)致機

5、器人的振動和沖擊。為此，要求所選擇的運動軌跡描述函數(shù)必須連續(xù)，且它的描述函數(shù)必須連續(xù)，且它的一階導(dǎo)數(shù)一階導(dǎo)數(shù)( (速度速度) )，有時二階導(dǎo)數(shù)，有時二階導(dǎo)數(shù)( (加速度加速度) )也應(yīng)該連也應(yīng)該連續(xù)。續(xù)。 軌跡規(guī)劃既可以在關(guān)節(jié)空間中進(jìn)行，也可以在直角坐標(biāo)空間中進(jìn)行。在關(guān)節(jié)空間中進(jìn)行軌跡規(guī)劃是指將所有關(guān)節(jié)變量表示為時間的函數(shù)，用這些關(guān)節(jié)函數(shù)及其一階、二階導(dǎo)數(shù)描述機器人預(yù)期的運動；在直角坐標(biāo)空間中進(jìn)行軌跡規(guī)劃是指將手爪位姿、速度和加速度表示為時間的函數(shù)，而相應(yīng)的關(guān)節(jié)位置、速度和加速度由手爪信息導(dǎo)出。3. 1. 3 軌跡的生成方式1)示教-再現(xiàn)運動運動由人手把手示教機器人，定時記錄各關(guān)節(jié)變量，得到沿

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

7、或逼近結(jié)點，此軌跡可按一定的原則優(yōu)化，如加速度平滑得到直角空間的位移時間函數(shù) X(t)或關(guān)節(jié)空間的位移時間函數(shù) q(t)；在結(jié)點之間如何進(jìn)行插補，即根據(jù)軌跡表達(dá)式在每一個采樣周期實時計算軌跡上點的位姿和各關(guān)節(jié)變量值。 3)以上生成的軌跡是機器人位置控制的給定值，可以據(jù)此并根據(jù)機器人的動態(tài)參數(shù)設(shè)計一定的控制規(guī)律。4) 規(guī)劃機器人的運動軌跡時，尚需明確其路徑上是否存在障礙約束的組合。一般將機器人的規(guī)劃與控制方式分為四種情況。障 礙 約 束有無路徑約束有離線無碰撞路徑規(guī)劃+在線路徑跟蹤離線路徑規(guī)劃+在線路徑跟蹤無位置控制+在線障礙探測和避障位置控制表3.1 機器人的規(guī)劃與控制方式3. 2 插補方式分

8、類與軌跡控制插補方式分類與軌跡控制3. 2. 1 插補方式分類插補方式分類 有兩種方式：點位控制(PTP控制)和連續(xù)軌跡控制(CP控制)。 點位控制(PTP控制)通常沒有路徑約束，多以關(guān)節(jié)坐標(biāo)運動表示。點位控制只要求滿足起終點位姿，在軌跡中間只有關(guān)節(jié)的幾何限制、最大速度和加速度約束。 連續(xù)軌跡控制(CP控制)有路徑約束，因此要對路徑進(jìn)行設(shè)計。路徑控制不插補關(guān)節(jié)插補(平滑)空間插補點位控制PTP1) 各軸獨立快速到達(dá)。2) 各關(guān)節(jié)最大加速度限制(1) 各軸協(xié)調(diào)運動定時插補。(2) 各關(guān)節(jié)最大加速度限制連續(xù)路徑控制CP(1) 在空間插補點間進(jìn)行關(guān)節(jié)定時插補。(2) 用關(guān)節(jié)的低階多項式擬合空間直線使

9、各軸協(xié)調(diào)運動。(3) 各關(guān)節(jié)最大加速度限制(1) 直線、圓弧、曲線等距插補。(2) 起停線速度、線加速度給定，各關(guān)節(jié)速度、加速度限制路徑控制不插補關(guān)節(jié)插補(平滑)空間插補點位控制PTP1) 各軸獨立快速到達(dá)。2) 各關(guān)節(jié)最大加速度限制(1) 各軸協(xié)調(diào)運動定時插補。(2) 各關(guān)節(jié)最大加速度限制連續(xù)路徑控制CP(1) 在空間插補點間進(jìn)行關(guān)節(jié)定時插補。(2) 用關(guān)節(jié)的低階多項式擬合空間直線使各軸協(xié)調(diào)運動。(3) 各關(guān)節(jié)最大加速度限制(1) 直線、圓弧、曲線等距插補。(2) 起停線速度、線加速度給定，各關(guān)節(jié)速度、加速度限制3. 2. 2 機器人軌跡控制過程機器人軌跡控制過程 機器人的基本操作方式是示教

10、-再現(xiàn)，即首先教機器人如何做，機器人記住了這個過程，于是它可以根據(jù)需要重復(fù)這個動作。操作過程中，不可能把空間軌跡的所有點都示教一遍使機器人記住，這樣太繁瑣，也浪費很多計算機內(nèi)存。實際上，對于有規(guī)律的軌跡，僅示教幾個特征點，計算機就能利用插補算法獲得中間點的坐標(biāo)，如直線需要示教兩點，圓弧需要示教三點，通過機器人逆向運動學(xué)算法由這些點的坐標(biāo)求出機器人各關(guān)節(jié)的位置和角度 (1, , n) ，然后由后面的角位置閉環(huán)控制系統(tǒng)實現(xiàn)要求的軌跡上的一點。繼續(xù)插補并重復(fù)上述過程，從而實現(xiàn)要求的軌跡。圖3.3 機器人軌跡控制過程3.3 機器人軌跡插值計算給出各個路徑結(jié)點后，軌跡規(guī)劃的任務(wù)包含解變換方程，進(jìn)行運動學(xué)

11、反解和插值計算。在關(guān)節(jié)空間進(jìn)行規(guī)劃時，需進(jìn)行大量工作是對關(guān)節(jié)變量的插值計算。 直線插補和圓弧插補是機器人系統(tǒng)中的基本插補算法。對于非直線和圓弧軌跡，可以采用直線或圓弧逼近，以實現(xiàn)這些軌跡。 3.3.1 直線插補 空間直線插補是在已知該直線始末兩點的位置和姿態(tài)的條件下，求各軌跡中間點(插補點)的位置和姿態(tài)。由于在大多數(shù)情況下，機器人沿直線運動時其姿態(tài)不變，所以無姿態(tài)插補，即保持第一個示教點時的姿態(tài)。當(dāng)然在有些情況下要求變化姿態(tài)，這就需要姿態(tài)插補。可仿照下面介紹的位置插補原理處理。如圖示。已知直線始末兩點的坐標(biāo)值P0(X0，Y0，Z0)、Pe(Xe，Ye，Ze)及姿態(tài)，其中P0、Pe是相對于基坐標(biāo)

12、系的位置。設(shè)v為要求的沿直線運動的速度；ts為插補時間間隔。 直線長度 L ； ts 間隔內(nèi)行程 d = vts； 插補總步數(shù)N為L/d+1的整數(shù)部分； 兩插補點間各軸增量 各插補點坐標(biāo)值 （插補實時計算） 式中：i=0，1，2，N。 e0e0e0/XXXNYYYNZZZN111iiiiiiXXi XYYi YZZi Z 姿態(tài)插補可仿照下面介紹的位置插補原理處理。如圖3.4示。已知直線始末兩點的坐標(biāo)值P0(X0，Y0，Z0)、Pe(Xe，Ye，Ze)及姿態(tài)，其中P0、Pe是相對于基坐標(biāo)系的位置。這些已知的位置和姿態(tài)通常是通過示教方式得到的。設(shè)v為要求的沿直線運動的速度；ts為插補時間間隔。 直

13、線長度 ts 間隔內(nèi)行程 d = vts； 插補總步數(shù)N為L/d+1的整數(shù)部分；兩插補點間各軸增量 各插補點坐標(biāo)值 （插補實時計算） 式中：i=0，1，2，N。 e0e0e0/XXXNYYYNZZZN111iiiiiiXXi XYYi YZZi Z 202020)()()(ZZYYXXLeee圖3.4 空間直線插補3. 3. 2 平面圓弧插補平面圓弧插補 平面圓弧是指圓弧平面與基坐標(biāo)系的三大平面之一重合，以XOY平面圓弧為例。已知不在一條直線上的三點P1、P2、P3及這三點對應(yīng)的機器人手端的姿態(tài)，如圖示。 設(shè)v為沿圓弧運動速度；ts為插補時間間隔。計算出： 1) 由P1、P2、P3決定的圓弧半

14、徑R。 2) 總的圓心角=1+2，即 3) ts時間內(nèi)角位移量 =tsv/R，。 4) 總插補步數(shù)(取整數(shù)) N = / + 1 插補遞推公式 111cossincossiniiiiiiiiXXYYYX i， 221212122232322arcsin()() / 22arcsin()() / 2XXYYRXXYYR3. 3. 3 定時插補與定距插補定時插補與定距插補 機器人實現(xiàn)一個空間軌跡的過程即是實現(xiàn)軌跡離散的過程，如果這些離散點間隔很大，則機器人運動軌跡與要求軌跡可能有較大誤差。只有這些插補得到的離散點彼此距離很近，才有可能使機器人軌跡以足夠的精確度逼近要求的軌跡。模擬 CP 控制實際上

15、是多次執(zhí)行插補點的 PTP 控制，插補點越密集，越能逼近要求的軌跡曲線。一、 定時插補 （1） 從軌跡控制過程知道，每插補出一軌跡點的坐標(biāo)值，就要轉(zhuǎn)換成相應(yīng)的關(guān)節(jié)角度值并加到位置伺服系統(tǒng)以實現(xiàn)這個位置，這個過程每隔一個時間間隔 ts 完成一次。為保證運動的平穩(wěn)，顯然ts不能太長。（2） 關(guān)節(jié)型機器人剛度不高(固有頻率40 Hz左右) ，ts 一般不超過25 ms（上限值）。當(dāng)然ts越小越好，但它的下限值受到計算量限制，計算機要在 ts時間里完成一次插補運算和一次逆向運動學(xué)計算。對于目前的大多數(shù)機器人控制器，完成這樣一次計算約需幾毫秒（下限值）。應(yīng)選擇 ts 接近它的下限值，可保證較高的軌跡精度

16、和平滑的運動過程。 （3）機器人控制系統(tǒng)易于實現(xiàn)定時插補，機器人速度一般不會很高，且運動精度不如數(shù)控機床，故大多數(shù)工業(yè)機器人采用定時插補方式。當(dāng)要求以更高的精度實現(xiàn)運動軌跡時，可采用定距插補。二、定距插補 v是要求的運動速度，如果要兩插補點的距離恒為一個足夠小的值，以保證軌跡精度，ts 就要變化。也就是在此方式下，插補點距離不變，但ts 要隨著不同工作速度 v 的變化而變化。 這兩種插補方式的基本算法相同，只是前者易于實現(xiàn)，后者保證軌跡插補精度，但實現(xiàn)起來比前者困難。3. 3. 4 關(guān)節(jié)空間插補關(guān)節(jié)空間插補 在關(guān)節(jié)空間中進(jìn)行軌跡規(guī)劃，需要給定機器人在起始點起始點和終止點終止點手臂的位形。對關(guān)節(jié)

17、進(jìn)行插值時應(yīng)滿足一系列的約束條件，例如抓取物體時手部的運動方向 (初始點) 、提升物體離開的方向 (提升點) 、放下物體 (下放點) 和停止點等結(jié)點上的位姿、速度和加速度的要求；與此相應(yīng)的各個關(guān)節(jié)位移、速度、加速度在整個時間間隔內(nèi)的連續(xù)性要求以及其極值必須在各個關(guān)節(jié)變量的容許范圍之內(nèi)等。滿足所要求的約束條件之后，可以選取不同類型的關(guān)節(jié)插值函數(shù)，生成不同的軌跡。3. 3. 4 關(guān)節(jié)空間插補關(guān)節(jié)空間插補 常用的關(guān)節(jié)空間插補有以下方法(自己看書P8997)： 三次多項式插值、過路徑點的三次多項式插值、高階多項式插值、用拋物線過渡的線性插值 。（NURBS曲線-Non Uniform Rational B-spline非均勻有理B樣條曲線）3.4 機器人手部路徑的軌跡規(guī)劃3.4.1 操作對象的描述 由前述可知，任一剛體相對參考系的位姿是用與它固接的坐標(biāo)系來描述的。剛體上相對于固接坐標(biāo)系的任一點用相應(yīng)的位置矢量P表示；任一方向用方向余弦表示。給出剛體的幾何圖形及固接坐標(biāo)系后，只要規(guī)定固接坐標(biāo)系的位姿，便可重構(gòu)該剛體在空間的位姿。如圖示的螺栓，其軸線與固接坐標(biāo)系的Z軸重合。螺栓頭部直徑為32 mm，中心取為坐標(biāo)原點，螺栓長80 mm，直徑20 mm，則可根據(jù)