工業(yè)機器人軌跡規(guī)劃

1、第第3 3章章 機器人軌跡規(guī)劃機器人軌跡規(guī)劃3.1 機器人軌跡規(guī)劃概述機器人軌跡規(guī)劃概述3.1.1 機器人軌跡的概念機器人軌跡的概念 機器人軌跡泛指工業(yè)機器人在運動過程中的運動軌跡，即運動點的位移、速度和加速度。 機器人在作業(yè)空間要完成給定的任務，其手部運動必須按一定的軌跡進行。軌跡的生成一般是先給定軌跡上的若干個點，將其經運動學反解映射到關節(jié)空間，對關節(jié)空間中的相應點建立運動方程，然后按這些運動方程對關節(jié)進行插值，從而實現作業(yè)空間的運動要求，這一過程通常稱為軌跡規(guī)劃。本章僅討論在關節(jié)空間或笛卡爾空間中工業(yè)機器人運動的軌跡規(guī)劃和軌跡生成方法。 機器人運動軌跡的描述一般是對其手部位姿的描述，此位

2、姿值可與關節(jié)變量相互轉換。控制軌跡也就是按時間控制手部或工具中心走過的空間路徑。3.1.2 軌跡規(guī)劃的一般性問題軌跡規(guī)劃的一般性問題 機器人的作業(yè)可以描述成工具坐標系T相對于工件坐標系S的一系列運動。 例如，圖3.1所示將銷插入工件孔中的作業(yè)可以借助工具坐標系的一系列位姿Pi (i=1，2，n)來描述。這種描述方法不僅符合機器人用戶考慮問題的思路，而且有利于描述和生成機器人的運動軌跡。圖3.1 機器人將銷插入工件孔中的作業(yè)描述 用T相對于S的運動來描述作業(yè)路徑是一種通用的作業(yè)描述方法。它把作業(yè)路徑描述與具體的機器人、手爪或工具分離開來，形成了模型化的作業(yè)描述方法，從而使這種描述既適用于不同的機

3、器人，也適用于在同一機器人上裝夾不同規(guī)格的工具。有了這種描述方法就可以把如圖3.2所示的機器人從初始狀態(tài)運動到終止狀態(tài)的作業(yè)看做是工具坐標系從初始位置T0變化到終止位置Tf的坐標變換。顯然，這種變換與具體機器人無關。一般情況下，這種變換包含了工具坐標系位置和姿態(tài)的變化。圖3.2 機器人的初始狀態(tài)和終止狀態(tài) 在軌跡規(guī)劃中，為敘述方便，也常用點來表示機器人的狀態(tài)，或用它來表示工具坐標系的位姿。 更詳細地描述運動時不僅要規(guī)定機器人的起始點和終止點，而且要給出介于起始點和終止點之間的中間點，也稱路徑點。這時，運動軌跡除了位姿約束外，還存在著各路徑點之間的時間分配問題。 機器人的運動應當平穩(wěn)，不平穩(wěn)的運

4、動將加劇機械部件的磨損，并導致機器人的振動和沖擊。為此，要求所選擇的運動軌跡描述函數必須連續(xù)，而且它的一階導數(速度)，有時甚至二階導數(加速度)也應該連續(xù)。 軌跡規(guī)劃既可以在關節(jié)空間中進行，也可以在直角坐標空間中進行。在關節(jié)空間中進行軌跡規(guī)劃是指將所有關節(jié)變量表示為時間的函數，用這些關節(jié)函數及其一階、二階導數描述機器人預期的運動；在直角坐標空間中進行軌跡規(guī)劃是指將手爪位姿、速度和加速度表示為時間的函數，而相應的關節(jié)位置、速度和加速度由手爪信息導出。3.1.3 軌跡的生成方式軌跡的生成方式(1) 示教-再現運動。這種運動由人手把手示教機器人，定時記錄各關節(jié)變量，得到沿路徑運動時各關節(jié)的位移時間

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

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

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

8、使各軸協(xié)調運動。(3) 各關節(jié)最大加速度限制(1) 直線、圓弧、曲線等距插補。(2) 起停線速度、線加速度給定，各關節(jié)速度、加速度限制3.2.2 機器人軌跡控制過程機器人軌跡控制過程 機器人的基本操作方式是示教-再現。對于有規(guī)律的軌跡，僅示教幾個特征點，計算機就能利用插補算法獲得中間點的坐標，如直線需要示教兩點，圓弧需要示教三點，通過機器人逆向運動學算法由這些點的坐標求出機器人各關節(jié)的位置和角度(1, , n)，然后由后面的角位置閉環(huán)控制系統(tǒng)實現要求的軌跡上的一點。繼續(xù)插補并重復上述過程，從而實現要求的軌跡。機器人軌跡控制過程如圖3.3所示。圖3.3 機器人軌跡控制過程3.3 3.3 機器人軌

9、跡插值計算機器人軌跡插值計算 給出各個路徑結點后，軌跡規(guī)劃的任務包含解變換方程，進行運動學反解和插值計算。在關節(jié)空間進行規(guī)劃時，需進行的大量工作是對關節(jié)變量的插值計算。3.3.1 直線插補直線插補 直線插補和圓弧插補是機器人系統(tǒng)中的基本插補算法。 空間直線插補是在已知該直線始末兩點的位置和姿態(tài)的條件下，求各軌跡中間點(插補點)的位置和姿態(tài)。下面介紹位置插補原理，如圖3.4所示。已知直線始末兩點的坐標值P0(X0，Y0，Z0)、Pe(Xe，Ye，Ze)及姿態(tài)。這些已知的位置和姿態(tài)通常是通過示教方式得到的。設v為要求的沿直線運動的速度；ts為插補時間間隔。直線長度ts間隔內行程d =vts；插補總

10、步數N為L/d+1的整數部分；各軸增量各插補點坐標值3.3.2 圓弧插補圓弧插補一、平面圓弧插補一、平面圓弧插補 平面圓弧是指圓弧平面與基坐標系的三大平面之一重合，以XOY平面圓弧為例。已知不在一條直線上的三點P1、P2、P3及這三點對應的機器人手端的姿態(tài)，如圖3.5及圖3.6所示。圖3.5 由已知的三點P1、P2、P3決定的圓弧圖3.6 圓弧插補 設v為沿圓弧運動速度；ts為插補時時間隔。類似直線插補情況計算出：(1) 由P1、P2、P3決定的圓弧半徑R。(2) 總的圓心角=1+2，即(3) ts時間內角位移量=tsv/R，據圖3.4所示的幾何關系求各插補點坐標。(4) 總插補步數(取整數)

11、 N = / + 1 對Pi+1 點的坐標，有式中：Xi=R cosi；Yi=Rsini。 同理有 由i+1=i +可判斷是否到插補終點。若i+1，則繼續(xù)插補下去；當 i+1 時，則修正最后一步的步長，并以 表示， ，故平面圓弧位置插補為二、空間圓弧插補二、空間圓弧插補 空間圓弧是指三維空間任一平面內的圓弧，此為空間一般平面的圓弧問題。 空間圓弧插補可分三步來處理：(1) 把三維問題轉化成二維，找出圓弧所在平面。(2) 利用二維平面插補算法求出插補點坐標(Xi+1, Yi+1)。(3) 把該點的坐標值轉變?yōu)榛A坐標系下的值，如圖3.7所示。圖3.7 基礎坐標與空間圓弧平面的關系 通過不在同一直

12、線上的三點P1、P2、P3可確定一個圓及三點間的圓弧，其圓心為OR,半徑為R，圓弧所在平面與基礎坐標系平面的交線分別為AB、BC、CA。 建立圓弧平面插補坐標系，即把ORXRYRZR坐標系原點與圓心OR重合，設ORXRYRZR平面為圓弧所在平面，且保持ZR為外法線方向。這樣，一個三維問題就轉化成平面問題，可以應用平面圓弧插補的結論。求解兩坐標系(圖3.7)的轉換矩陣。令T TR表示由圓弧坐標ORXRYRZR至基礎坐標系OX0Y0Z0的轉換矩陣。若ZR軸與基礎坐標系Z0軸的夾角為，XR軸與基礎坐標系的夾角為，則可完成下述步驟：將XRYRZR的原點OR放到基礎原點O上；繞ZR軸轉，使X0與XR平行

13、；再繞XR軸轉角，使Z0與ZR平行。這三步完成了XRYRZR向X0Y0Z0的轉換，故總轉換矩陣應為 欲將基礎坐標系的坐標值表示在ORXRYRZR坐標系，則要用到T TR的逆矩陣：3.3.3 3.3.3 定時插補與定距插補定時插補與定距插補 由上述可知，機器人實現一個空間軌跡的過程即是實現軌跡離散的過程，如果這些離散點間隔很大，則機器人運動軌跡與要求軌跡可能有較大誤差。只有這些插補得到的離散點彼此距離很近，才有可能使機器人軌跡以足夠的精確度逼近要求的軌跡。模擬CP控制實際上是多次執(zhí)行插補點的PTP控制，插補點越密集，越能逼近要求的軌跡曲線。 插補點要多么密集才能保證軌跡不失真和運動連續(xù)平滑呢？可

14、采用定時插補和定距插補方法來解決。一、定時插補一、定時插補 從軌跡控制過程知道，每插補出一軌跡點的坐標值，就要轉換成相應的關節(jié)角度值并加到位置伺服系統(tǒng)以實現這個位置，這個過程每隔一個時間間隔ts完成一次。為保證運動的平穩(wěn)，顯然ts不能太長。 由于關節(jié)型機器人的機械結構大多屬于開鏈式，剛度不高，ts一般不超過25 ms(40 Hz)，這樣就產生了ts的上限值。當然ts越小越好，但它的下限值受到計算量限制，即對于機器人的控制，計算機要在ts時間里完成一次插補運算和一次逆向運動學計算。對于目前的大多數機器人控制器，完成這樣一次計算約需幾毫秒。這樣產生了ts的下限值。當然，應當選擇ts接近或等于它的下

15、限值，這樣可保證較高的軌跡精度和平滑的運動過程。 以一個XOY平面里的直線軌跡為例說明定時插補的方法。 設機器人需要的運動軌跡為直線，運動速度為v(mm/s)，時間間隔為ts (ms)，則每個ts間隔內機器人應走過的距離為Pi Pi+1= v ts可見兩個插補點之間的距離正比于要求的運動速度，兩點之間的軌跡不受控制，只有插補點之間的距離足夠小，才能滿足一定的軌跡精度要求。 機器人控制系統(tǒng)易于實現定時插補，例如采用定時中斷方式每隔ts中斷一次進行一次插補，計算一次逆向運動學，輸出一次給定值。由于ts僅為幾毫秒，機器人沿著要求軌跡的速度一般不會很高，且機器人總的運動精度不如數控機床、加工中心高，故

16、大多數工業(yè)機器人采用定時插補方式。當要求以更高的精度實現運動軌跡時，可采用定距插補。二、定距插補二、定距插補 v是要求的運動速度，它不能變化，如果要兩插補點的距離Pi Pi+1恒為一個足夠小的值，以保證軌跡精度，ts就要變化。也就是在此方式下，插補點距離不變，但ts要隨著不同工作速度v的變化而變化。 這兩種插補方式的基本算法相同，只是前者固定ts，易于實現，后者保證軌跡插補精度，但ts要隨之變化，實現起來比前者困難。3.3.4 關節(jié)空間插補關節(jié)空間插補 在關節(jié)空間中進行軌跡規(guī)劃，需要給定機器人在起始點和終止點手臂的位形。對關節(jié)進行插值時應滿足一系列的約束條件，例如抓取物體時手部的運動方向(初始

17、點)、提升物體離開的方向(提升點)、放下物體(下放點)和停止點等結點上的位姿、速度和加速度的要求；與此相應的各個關節(jié)位移、速度、加速度在整個時間間隔內的連續(xù)性要求以及其極值必須在各個關節(jié)變量的容許范圍之內等。滿足所要求的約束條件之后，可以選取不同類型的關節(jié)插值函數，生成不同的軌跡。常用的關節(jié)空間插補有以下方法：1、三次多項式插值2、過路徑點的三次多項式插值3、高階多項式插值4、用拋物線過渡的線性插值一、三次多項式插值一、三次多項式插值 在機器人運動過程中，若末端執(zhí)行器的起始和終止位姿已知，由逆向運動學即可求出對應于兩位姿的各個關節(jié)角度。末端執(zhí)行器實現兩位姿的運動軌跡描述可在關節(jié)空間中用通過起始

18、點和終止點關節(jié)角的一個平滑軌跡函數 (t)來表示。 為實現系統(tǒng)的平穩(wěn)運動，每個關節(jié)的軌跡函數 (t)至少需要滿足四個約束條件，即兩端點位置約束和兩端點速度約束。 端點位置約束是指起始位姿和終止位姿分別所對應的關節(jié)角度。 (t)在時刻t0=0時的值是起始關節(jié)角度0，在終端時刻tf時的值是終止關節(jié)角度f，即 為滿足關節(jié)運動速度的連續(xù)性要求，起始點和終止點的關節(jié)速度可簡單地設定為零，即 000ft上面給出的四個約束條件可以惟一地確定一個三次多項式運動過程中的關節(jié)速度和加速度則為 2123232326taa ta ttaa t 將其代以給定的約束條件，可得三次多項式的系數a0，a1，a2和a3，則對于

19、起始速度及終止速度為零的關節(jié)運動，滿足連續(xù)平穩(wěn)運動要求的三次多項式插值函數為關節(jié)角速度和角加速度的表達式為 20023002366612ffffffffttttttttt 三次多項式插值的關節(jié)運動軌跡曲線如圖3.8所示。由圖可知，其速度曲線為拋物線，相應的加速度曲線為直線。圖3.8 三次多項式插值的關節(jié)運動軌跡二、過路徑點的三次多項式插值二、過路徑點的三次多項式插值 若所規(guī)劃的機器人作業(yè)路徑在多個點上有位姿要求，如圖3.9所示，機器人作業(yè)除在A、B點有位姿要求外，在路徑點C、D也有位姿要求。對于這種情況，假如末端執(zhí)行器在路徑點停留，即各路徑點上速度為0，則軌跡規(guī)劃可連續(xù)直接使用前面介紹的三次多

20、項式插值方法；但若末端執(zhí)行器只是經過，并不停留，就需要將前述方法推廣。 對于機器人作業(yè)路徑上的所有路徑點可以用求解逆運動學的方法先得到多組對應的關節(jié)空間路徑點，進行軌跡規(guī)劃時，把每個關節(jié)上相鄰的兩個路徑點分別看做起始點和終止點，再確定相應的三次多項式插值函數，把路徑點平滑連接起來。一般情況下，這些起始點和終止點的關節(jié)運動速度不再為零。圖3.9 機器人作業(yè)路徑點 設路徑點上的關節(jié)速度已知，這時，確定三次多項式系數的方法與前所述完全一致，只是速度約束條件變?yōu)?00fft 利用約束條件可確定三次多項式系數a0，a1，a2和a3，和前面有所不同。但當路徑點上的關節(jié)速度為0時，確定的三次多項式與上一種方

21、法完全相同，這就說明了這里所確定的三次多項式描述了起始點和終止點具有任意給定位置和速度約束條件的運動軌跡。三、高階多項式插值三、高階多項式插值 若對于運動軌跡的要求更為嚴格，約束條件增多，三次多項式就不能滿足需要，須用更高階的多項式對運動軌跡的路徑段進行插值。 例如，對某段路徑的起始點和終止點都規(guī)定了關節(jié)的位置、速度和加速度，則要用一個五次多項式進行插值，即多項式的系數a0，a1，a5必須滿足6個約束條件00234501234501234123450223234523452261220fffffffffffffffaaata ta ta ta taaa ta ta ta taaa ta ta

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

23、線性插值軌跡圖3.11 帶有拋物線過渡域的線性軌跡 設兩端的拋物線軌跡具有相同的持續(xù)時間ta ，具有大小相同而符號相反的恒加速度 。對于這種路徑規(guī)劃存在有多個解，其軌跡不惟一。如圖3.12所示。但是，每條路徑都對稱于時間中點th和位置中點 h。要保證路徑軌跡的連續(xù)、光滑，即要求拋物線軌跡的終點速度必須等于線性段的速度，故有下列關系圖3.12 軌跡的多解性與對稱性haahattt2012aat設關節(jié)從起始點到終止點的總運動時間為tf ，則tf=2th，并注意到012hf則200af aftt t 一般情況下，0、f、tf是已知條件，這樣，據前式可以選擇相應的加速度 和ta ，得到相應的軌跡。通常

24、的做法是先選定加速度 的值，然后)求出相應的ta ：2 20422fffattt 由上式知，為保證有解，加速度值 必須選得足夠大，即024fft 當上式中的等號成立時，軌跡線性段的長度縮減為零，整個軌跡由兩個過渡域組成，這兩個過渡域在銜接處的斜率(關節(jié)速度)相等；加速度 的取值愈大，過渡域的長度會變得愈短，若加速度趨于無窮大，軌跡又復歸到簡單的線性插值情況。 用拋物線過渡的線性函數插值進行軌跡規(guī)劃的物理概念非常清楚，即如果機器人每一關節(jié)電動機采用等加速、等速和等減速運動規(guī)律，則關節(jié)的位置、速度、加速度隨時間變化的曲線如圖所示。 若某個關節(jié)的運動要經過一個路徑點，則可采用帶拋物線過渡域的線性路徑方案。如圖3.14所示，關節(jié)的運動要經過一組路徑點，用關節(jié)角度